《坐标转换模型-完整版》由会员分享,可在线阅读,更多相关《坐标转换模型-完整版(11页珍藏版)》请在金锄头文库上搜索。
1、坐标转换模型 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角, 使经过旋转后的
2、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; 在误差允许范围内可以将模型 简化为: (
3、同样七参数模型) 四参数模型是在七参数模型的特例,没有考虑坐标轴的旋转量,只考虑坐标 轴的平移。 总结: 类似布尔莎模型(以坐标原点为参考点),还有莫洛金斯基坐标模型(以目 标点为变换中心) 、武测转换模型和范士转换模型(以控制网参考点的站心地平 坐标系的三个坐标轴为旋转轴) ,这些坐标转换模型很容易实现相关坐标在不同 坐标系的转换, 但是参考位置的偏移向量的相关参数,在实际运用中这些参量是 很难测定的, 并且受地球重力等物理因素的影响,两个坐标系统即使经过相似变 换,仍可能存在较大的残差,所以这些模型适用于简单且规则模型中。 程序: clc clear all dX=input(please
4、 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)
5、; 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
6、)=(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(
7、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地的子午面。则
8、地 面点 P的大地坐标定义为 : 大地纬度是 B-PKp,与赤道面 WAE 的夹角,从赤道面 开始起算,以北为正 (0 度-90 度),称为北纬,以南为负 (0 度-90 度,称为南纬。 大地经度是 L-P 地的子午面 NGS 与起始子午面 NGS 所构成的二面角,以东为正 (0 度-180 度 ) ,称为东经,以西为负 (0 度-180 度) ,称为西经 ; 大地高 H-P地沿法 线方向到椭球体的P地P,从椭球面为起算标准,以外为正,以内为负。大地高H 与水准测量中的正常高HN或正高 HN有以下关系 H=HN+N=HV+%短半径 % a=input(please input value of
9、 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*d2 Y=(N+H)*c2*d1 Z=N*(1-e2)+H*c1 取 a=6378140 b=6356755.29 L=30 B=120 H=0 结果:
10、2) 空间坐标系转换为大地坐标 公式: 程序: clc clear all N=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=(a
11、2-b2)/b2)0.5% 第二偏心率 % n=X2+Y2 w=atand(Z*a/(n0.5*b) v=sin(w) u=cos(w) den=Z+e12*b*v3 mun=(n-e2*a*u3)0.5 L=atand(Y/X) B=atand(den/mun) H=n0.5/cos(B)-N 3.D-H 坐标系的建立 关节的运动会对机械臂末端执行器的位姿产生影响,因此在进行运动学分析 时,需要对每个关节指定一个参考坐标系。在机械臂运动学分析过程中,当需要 描述两坐标系间的位姿关系时, 可以通过使用齐次变换矩阵来描述一个坐标系对 另一坐标系的变换过程。 在机器人中,通常有两类关节:转动关节和
12、移动关节。不同于人类的关节, 一般机器人关节为一个自由度的关节,其目的是为了简化力学、 运动学和机器人 的控制。转动关节提供了一个转动自由度,移动关节提供一个移动自由度,各关 节间是以固定杆件相连接的。机器人杆件是连接两个关节的固定物体(机械)。 机器人杆件的主要目的是用来保持该关节与各相关末端关节一个固定的关系。 对于 D-H坐标系的建立思想, 首先给每个关节指定坐标系, 然后确定从一个 关节到下一个关节进行变化的步骤,这体现在两个相邻参考坐标系之间的变化, 将所有变化结合起来, 就确定了末端关节与基座之间的总变化,从而建立运动学 方程,进一步对其求解。 D-H坐标系的建立是通过推导坐标系间
13、的其次变换矩阵来表示坐标系间的 变换关系,当坐标系间的关系确定以后就可以建立运动学方程。机械臂是用旋转 关节或平移关节连接刚体连杠组成的,对连杆进行编号时按照从底座至末端执行 器的顺序由低到高进行编号, 固定的底座为杆件0, 与其相连的连杆是杆件1,它 们之间的关节编号为关节1; 与连杆 1 另一端相连的为连杆2, 二者之间的连接关 节为关节 2,以此类推。 1. 对于关节处坐标系的建立 1) Z轴应沿着第 1 个关节的运动轴 ; 2) X轴垂直于 Zi-1 和 Zi 轴,并且其指向为离开Zi-1 轴的方向 ; 3) Y轴根据右手定则确定。 2. 相关参数 由于泵车臂架各关节大部分是转动环节,
14、则只需考虑转动参量。即如果第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 相连,因此关节轴线
15、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轴对准。 这样就可以实
16、现从一个坐标系转换到另一个坐标系(其实转换过程不一定要 选取 x 轴和 z 轴,但 x 轴和 z 轴是建立坐标系首先判定的两个轴,符合建立D-H 坐标的思想,一旦两个坐标轴确立,另一个坐标轴也就确定了)。 其中对应的四个矩阵分别是: 1000 0100 0010 ai001 00aiTrans)( 1000 d100 0010 0001 d00Trans)( 1000 daicosaisin0 0aisin-aicos0 0001 aix Rot )()( )()( )( 则相邻两个关节点的变换矩阵是: (以四节臂为例) 分别将关节参数带入可得(节臂数越多,变换矩阵越复杂): 将参数带入,可得工作平台的正运动方程: 5. 注意事项 1)建立 D-H坐标系的时候,第一个关节点的坐标选取是在规定一个默认坐 标系的前提下建立的,而下一个坐标系的选取同样要依靠前一个坐标系才行: 2)在第一个关节点确定下来后,默认其初始高度h 为零,之后的节点坐标 是在其参考下给定的, 要求其实际