坐标转换模型.doc

坐标转换模型1. 空间直角坐标系间的转换模型（七参数模型）公式（布尔莎模型）：分析： (1)将O-XYZ中的长度单位缩放l+m倍，使其与O-XYZ的长度单位一致； (2)从X反向看向原点O，以O为旋转点，让O-XYZ绕X轴顺时针旋转Wx角，使经过旋转后的Y轴与O-XYZ平面平行； (3)从Y反向看向原点O，以O为旋转点，让O-XYZ绕Y轴顺时针旋转Wy角，使经过旋转后的X轴与O-XYZ平面平行。显然，此时Z轴也与Z轴平行； (4)从Z反向看向原点O，以O点为旋转点，O-XYZ绕Z轴顺时针旋转Wz角，使经过旋转后的X轴与X轴平行。显然，此时O-XYZ的三个坐标轴己与O-XYZ中相应的坐标轴平行； 原坐标为O-XYZ，转换到新坐标O-XYZ.(两坐标系都为空间直角坐标系）其中（dX dY dZ）为坐标原点的平移参数，即将坐标O-XYZ的原点分别沿三个坐标轴平移-dX,-dY,-dZ，使原坐标轴与O-XYZ的点重合。m为尺度参数，（w1 w2 w3）分别为坐标轴的旋转参量（角度），构成的旋转矩阵分别为：分别将R1 R2 R3代入上式，可得：当旋转角度w1 w2 w3很小时（=10),cos(w)=1,sin(w)=0;在误差允许范围内可以将模型简化为：(同样七参数模型） 四参数模型是在七参数模型的特例，没有考虑坐标轴的旋转量，只考虑坐标轴的平移。总结：类似布尔莎模型（以坐标原点为参考点），还有莫洛金斯基坐标模型（以目标点为变换中心）、武测转换模型和范士转换模型（以控制网参考点的站心地平坐标系的三个坐标轴为旋转轴），这些坐标转换模型很容易实现相关坐标在不同坐标系的转换，但是参考位置的偏移向量的相关参数，在实际运用中这些参量是很难测定的，并且受地球重力等物理因素的影响，两个坐标系统即使经过相似变换，仍可能存在较大的残差，所以这些模型适用于简单且规则模型中。程序： clc clear all dX=input(please input value of dX=); dY=input(please input value of dY=); dZ=input(please input value of dZ=); w1=input(please input value of w1=); w2=input(please input value of w2=); w3=input(please input value of w3=); m=input(please input value of m=); a=sind(w1); b=cosd(w1); c=sind(w2); d=cosd(w2); e=sind(w3); f=cosd(w3); r1=1 0 0;0 b a;0 -a b r2=d 0 -c;0 1 0;c 0 d r3=f e 0;-e f 0;0 0 1 X=input(please input value of X=); Y=input(please input value of Y=); Z=input(please input value of Z=); aa=dX;dY;dZ; bb=(1+m)*r3*r2*r1*X;Y;Z; cc=aa+bb; X1=cc(1) Y1=cc(2) Z1=cc(3)选取平移参数（dX dY dZ）=（1 1 1），旋转角度（w1 w2 w3）=（0 0 0),尺度参量m取1，选取原坐标中坐标点（X Y Z）=（1 1 1），则该坐标在新坐标中的坐标值（X1 Y2 Z2)=（3 3 3);根据结果可知，程序运行正确。2. 大地坐标与空间坐标系的相互转换（参数a b，跟选定的椭球有关）1） 大地坐标向空间坐标系的转换设P点的大地坐标为（L B H），其对应的球心直角坐标为（X Y Z），若P点在椭球面上，则H=0 ，根据图1所示的三角关系可列出方程：X=xcos（L）Y=xsin（L）Z=y由图2可见，TP是过P点子午线的切线，与x轴夹角是90+B：dy/dx=-cot(B)将子午圈椭圆方程x2/a2+y2/b2=1和第一偏心率公式e=（a2-b2)0.5/a带入上式：Dy/dx=-cot(B)，可得：将卯酉圈方程N=a/(1-e2*sin2(B)0.5带入上式得：即将大地坐标（L B H)带入，可得对应的球心坐标值（X Y Z）。e：第一偏心率N：卯酉圈的半径b：椭球短半径a：椭球长半径分析：大地坐标是世界公用的最方便的坐标系统，以大地经度L、地纬度B、大地高H来表示空间某一点的位置。0表示椭球中心，NGS为起始大地子午面，WAE为赤道面，地面点P的法线P-Kp交椭球面于P点，NGS为P地的子午面。则地面点P的大地坐标定义为:大地纬度是B-PKp，与赤道面WAE的夹角，从赤道面开始起算，以北为正(0度-90度)，称为北纬，以南为负(0度-90度，称为南纬。大地经度是L-P地的子午面NGS与起始子午面NGS所构成的二面角，以东为正(0度-180度 )，称为东经，以西为负(0度-180度)，称为西经;大地高H-P沿法线方向到椭球体的PP，从椭球面为起算标准，以外为正，以内为负。大地高H与水准测量中的正常高HN或正高HN有以下关系H=HN+N=HV+&式中&是高程异常，N是大地水准面差距。其中第一偏心率e和第二偏心率等候可以由长短半径求出！程序： clcclear allb=input(please input value of b=);%短半径%a=input(please input value of a=);%长半径%B=input(please input value of B=);L=input(please input value of L=);H=input(please input value of H=);c1=sind(B)c2=cosd(B)d1=sind(L)d2=cosd(L)e=(a2-b2)0.5/a%第一偏心率%N=a/(1-e2*c12)0.5%卯酉圈的半径%X=(N+H)*c2*d2Y=(N+H)*c2*d1Z=N*(1-e2)+H*c1取a=6378140 b=6356755.29 L=30 B=120 H=0结果：2)空间坐标系转换为大地坐标公式：程序：clcclear allN=input(please input value of N=);%卯酉圈的半径%b=input(please input value of b=);%短半径%a=input(please input value of a=);%长半径%X=input(please input value of X=);Y=input(please input value of Y=);Z=input(please input value of Z=);e=(a2-b2)0.5/a%第一偏心率%e1=(a2-b2)/b2)0.5%第二偏心率%n=X2+Y2w=atand(Z*a/(n0.5*b)v=sin(w)u=cos(w)den=Z+e12*b*v3mun=(n-e2*a*u3)0.5L=atand(Y/X)B=atand(den/mun)H=n0.5/cos(B)-N3. D-H坐标系的建立关节的运动会对机械臂末端执行器的位姿产生影响，因此在进行运动学分析时，需要对每个关节指定一个参考坐标系。在机械臂运动学分析过程中，当需要描述两坐标系间的位姿关系时，可以通过使用齐次变换矩阵来描述一个坐标系对另一坐标系的变换过程。在机器人中，通常有两类关节：转动关节和移动关节。不同于人类的关节，一般机器人关节为一个自由度的关节，其目的是为了简化力学、运动学和机器人的控制。转动关节提供了一个转动自由度，移动关节提供一个移动自由度，各关节间是以固定杆件相连接的。机器人杆件是连接两个关节的固定物体（机械）。机器人杆件的主要目的是用来保持该关节与各相关末端关节一个固定的关系。对于D-H坐标系的建立思想，首先给每个关节指定坐标系，然后确定从一个关节到下一个关节进行变化的步骤，这体现在两个相邻参考坐标系之间的变化，将所有变化结合起来，就确定了末端关节与基座之间的总变化，从而建立运动学方程，进一步对其求解。D-H坐标系的建立是通过推导坐标系间的其次变换矩阵来表示坐标系间的变换关系，当坐标系间的关系确定以后就可以建立运动学方程。机械臂是用旋转关节或平移关节连接刚体连杠组成的，对连杆进行编号时按照从底座至末端执行器的顺序由低到高进行编号，固定的底座为杆件0,与其相连的连杆是杆件1，它们之间的关节编号为关节1;与连杆1另一端相连的为连杆2，二者之间的连接关节为关节2，以此类推。1.对于关节处坐标系的建立 1) Z轴应沿着第1个关节的运动轴; 2) X轴垂直于Zi-1和Zi轴，并且其指向为离开Zi-1轴的方向; 3) Y轴根据右手定则确定。2.相关参数由于泵车臂架各关节大部分是转动环节，则只需考虑转动参量。即如果第i个关节为转动副，则为关节变量，其它的三个关节参量（l d ）为固定常量。在连杆坐标系中，（ l d ）合称为D-H参数，各参数的具体含义如下：1）连杆长度l：关节i轴线与关节i+1轴线之间的公垂线距离(在泵车臂架中l就是各长度）；2) 连杆扭角：关节i轴线与关节i+1轴线的夹角，指向是从关节i绕公垂线转至关节i+1的方向（在泵车臂架中由于各节臂的伸展是在同一平面上的，所以扭角为零）；3) 连杆偏移量（横距）d ：除第一个和最后一个连杆之外，其余相邻两连杆i-1和i通过关节i相连，因此关节轴线i有两条与之垂直的公法线，而这两条公法线之间的距离即为连杆偏移量（在泵车臂架中横距相对于臂架的长度可以忽略不计）；4) 关节角：关节z的相邻两条公垂线的夹角。3.相应的算法由于空间任一点p绕坐标系（X Y Z）轴的旋转矩阵如下：则我们可以通过下列几个运动由一个坐标系转换到另一个坐标系。1）绕Zn轴旋转角，它使得Xn+1和Xn互相平行；2）沿Zn轴平移dn+1距离，使得Xn和Xn+1共线；3）沿Xn轴平移an+1的距离，使Xn和Xn+1的原点重合；4）将Zn轴绕Xn+1轴旋转n+1，使得Zn轴与Zn+1轴对准。这样就可以实现从一个坐标系转换到另一个坐标系（其实转换过程不一定要选取x轴和z轴，但x轴和z轴是建立坐标系首先判定的两个轴，符合建立D-H坐标的思想，一旦两个坐标轴确立，另一个坐标轴也就确定了）。其中对应的四个矩阵分别是：则相邻两个关节点的变换矩阵是：（以四节臂为例）分别将关节参数带入可得（节臂数越多，变换矩阵越复杂）：将参数带入，可得工作平台的正运动方程：5.注意事项1）建立D-H坐标系的时候，第一个关节点的坐标选取是在规定一个默认坐标系的前提下建立的，而下一个坐标系的选取同样要依靠前一个坐标系才行：2）在第一个关节点确定下来后，默认其初始高度h为零，之后的节点坐标是在其参考下给定的，要求其实际高度，必须还要用初始高度h减去左边运算的Z，才是实际的高度。6.编程实现clcclear alla1=input(please input value of a1=);%关节1的变量%b1=input(please input value of b1=);%关节1的扭转角%c1=input(please input value of c1=);%关节1的横距%d1=input(please input value of d1=);%关节1的杆长%w1=sind(a1)w2=cosd(a1)v1=sin(b1)v2=cosd(b1)R1=w2 -w1 0 0;w1 w2 0