机械臂运动学

机械臂运动学基础1、机械臂的运动学模型 机械臂运动学研究的是机械臂运动，而不考虑产生运动的力。运动学研究机械臂的位置，速 度和加速度。机械臂的运动学的研究涉及到的几何和基于时间的内容，特别是各个关节彼此 之间的关系以及随时间变化规律。典型的机械臂由一些串行连接的关节和连杆组成。每个关节具有一个自由度，平移或旋转。 对于具有n个关节的机械臂，关节的编号从1到n ,有n +1个连杆，编号从0到n。连杆 0是机械臂的基础,一般是固定的，连杆n上带有末端执行器。关节i连接连杆i和连杆i-1。 一个连杆可以被视为一个刚体，确定与它相邻的两个关节的坐标轴之间的相对位置。一个连 杆可以用两个参数描述，连杆长度和连杆扭转，这两个量定义了与它相关的两个坐标轴在空 间的相对位置。而第一连杆和最后一个连杆的参数没有意义，一般选择为0。一个关节用两 个参数描述，一是连杆的偏移，是指从一个连杆到下一个连杆沿的关节轴线的距离。二是关 节角度，指一个关节相对于下一个关节轴的旋转角度。为了便于描述的每一个关节的位置，我们在每一个关节设置一个坐标系， 对于一个关节链， Denavit和Hartenberg提出了一种用矩阵表示各个关节之间关系的系统方法。对于转动关 节i，规定它的转动平行于坐标轴1，坐标轴x“对准从zi-住1zi的法线方向，如果z“与 Zi相交，则X取zi-1 xzi的方向。连杆，关节参数概括如下： 连杆长度ai沿着谷轴从zi-；L和Zj轴之间的距离 连杆扭转a）从Z轴到zi轴相对xi-1轴夹角； 连杆偏移dj从坐标系i-1的原点沿着Zj_i轴到Xi轴的距离； 关节角度ei xi-1轴和Xj轴之间关于zi-1轴的夹角。对于一个转动关节q是关节变量，dj是常数。而移动关节di是可变的，q是恒定的。为了统一，表示为qi = ldii转动关节 移动关节运用Denavit-Hartenberg( DH )方法，可以将相邻的两个坐标系之间的变换关系表示为-个 4x4的齐次变换矩阵cos 0isin0i0-sin 0 cos aiicos0 cosaiisinai0sin0 sinaii-cos0 sinaiicosai0a cos0iia sin0iidi1上式表示出了坐标系i相对于坐标系i-1的关系。即oT = oT i-iAii -1 i其中oT表示坐标系i相对于世界坐标系0的位置与姿态，简称位姿。i2、正向和反向运动学对于一个n-轴刚性连接的机械臂，正向运动学的解给出的是最后一个连杆坐标系的位置和姿态。重复利用上式,得到oT = oA iA n-iA = K (q)n12n机械臂末端位姿在笛卡尔坐标系中有6个自由度，3个平移，3个旋转。所以，一般来说具有6个自由度的机械臂可以使末端实现任意的位姿。总的机械臂变换oT 般简写为Tn ,对6个自由度的机械臂简写为T6。对于任意的机械臂， nn6无论其它有多少个关节，具有什么结构，正向运动学解都是可以得到的。在机械臂的路径规划中，用到的是反向运动学的解q = K-i(oT )，它给出了特定的末端位 n姿对应的机械臂的关节角度。一般来说，反向运动学的解不是唯一的，对具有某种结构的机械臂，封闭解可能不存在。对于 6 自由度的机器人而言，运动学逆解非常复杂，一般没有封闭解。只有在某些特殊情 况下才可能得到封闭解。不过大多数工业机器人都满足封闭解的两个充分条件之一（Pieper准则）（ 1）三个相邻关节轴交于一点（ 2）三个相邻关节轴相互平行如果机械臂多于 6 个关节，称关节为冗余的，这时解是欠定的。如果对于机械臂某个特别 的位姿，解不存在，称这个位姿为奇异位姿。机械臂的奇异性可能是由于机械臂中某些坐标 轴的重合，或位置不能达到引起的。机械臂的奇异位姿分为两类：（1）边界奇异位姿，当机械臂的关节全部展开或折起时，使得末端处于操作空间的边界或边 界附近，雅克比矩阵奇异，机械臂的运动受到物理结构的约束，这时机械臂的奇异位姿称为 边界奇异位姿。（2）内部奇异位姿，两个或两个以上的关节轴线重合时，机械臂各个关节的运动相互抵消， 不产生操作运动，这时机械臂的奇异位姿称为内部奇异位姿。机械臂运动学逆解的方法可以分为两类：封闭解和数值解、在进行逆解时总是力求得到封闭 解。因为封闭解的计算速度快，效率高，便于实时控制。而数值解法不具有这些特点。机械 臂运动学的封闭逆解可通过两种途径得到：代数法和几何法。一般而言，非零连杆参数越多，到达某一目标的方式也越多，即运动学逆解的数目也越多。 在从多重解中选择解时，应根据具体情况，在避免碰撞的前提下通常按“最短行程”准则来 选择。同时还应当兼顾“多移动小关节，少移动大关节”的原则。n个自由度的机械臂的末端位姿由n个关节变量所决定，这n个关节变量统称为n维关节矢量，记为q。所有的关节矢量构成的空间称为关节空间。机械臂末端的位姿用6个变量描 述，3个平移（x,y,z）和3个旋转（叭 叫,oz），记x=（x,y,乙一叫oz），x是机械臂末端在基x y zx y z坐标空间中的坐标，所有的矢量 x 构成的空间称为操作空间或作业定向空间。工作空间是 操作臂的末端能够到达的空间范围，即末端能够到达的目标点集合。值得指出的是，工作空 间应该严格地区分为两类：（1）灵活（工作）空间 指机械臂末端能够以任意方位到达的目标点集合。因此，在灵活空 间的每个点上，手爪的指向可任意规定。（2）可达（工作）空间 指机械臂末端至少在一个方位上能够到达的目标点集合。机械臂各关节驱动器的位置组成的矢量称为驱动矢量s，由这些矢量构成的空间称为驱动空 间。正向运动学3、Jacobian 矩阵机械臂的 Jacobian 矩阵表示机械臂的操作空间与关节空间之间速度的线性映射关系，对于一个n轴的机械臂，机械臂末端在基坐标系中的速度是x二Jq其中x是6个元素的向量。 对于6个关节机械臂Jacobian矩阵是方阵，如果它是可逆的，则可以由机械臂的末端速度 求出各个关节的速度。 Jacobian 矩阵在机械臂的奇异位姿上是不可逆的。在实际应用中， 当机械臂的末端位置接近奇异位置时， Jacobian 矩阵是病态的，可能导致关节速度不能正 确地得到。上式解决的是正速度问题，即已知q和q求末端执行器的速度x。对于逆速度解问题，由上 式可以得到速度逆解公式为 q J-i x ，注意到此时需要求雅可比矩阵的逆，由线性方程组 理论知上式对任意的x，q都有解的必要条件是雅可比矩阵的秩rank(J)=6,这意味着机械 臂的自由度数nA 6。这也说明了具有冗余自由度的机械臂，在末端位姿固定的条件下，能使关节在一个较大的关 节空间的子空间中运动，有效地避开障碍或奇异位姿，并把关节位移限制在允许范围内，从 而具有更大的运动灵活性。雅可比矩阵可以看成是从关节空间到操作空间运动速度的传动比，同时也可用来表示两空间 之间力的传递关系。对于冗余自由度机械臂，其雅可比矩阵是长方矩阵，因J满秩且方程个 数少于未知数个数，所以有无穷多个解，这时，一般是求其中的最小范数解，或采用加权最 小范数解也就是说使|qTDq|最小的解，其中D是对称正定加权矩阵。此时的解是使机械臂 在能量消耗最小的情况下的解。这时，逆速度问题便转为：求q满足 q J-1 x且使L - qTDq最小。实际上等同于求性能2指标L在约束条件q J-1 x下的极值。应用Lagrange乘子法，以上极值为题的解是q D-1 Jt (JD-i Jt)-ix ,当D=I时，雅可比矩阵是J+ - Jt (JJt)-i，称为雅可比矩阵的 伪逆。下面通过一个两自由度的平面机械臂说明雅可比矩阵的特性，根据右图中的几何关系容易求得x l c +1 c1 1 2 12 y 1 s +1 s1 1 2 12c cos(0 ),c cos(0 +0 )1 1 12 1 2s sin(0 ), s sin(0 +0 )1 1 12 1 2dxdxdxdTd0 1I12I1IIdyIIdydyII d0 IId01dT22两边微分后写成矩阵形式dx 1-1 s 1 sI I 1 12 12dy I I 1 c +1 c1 1 2 12-1 s2 121c2 12d0 11d0 II2简写成dx=Jde ,式中J就称为机械臂的雅可比(Jacobian )矩阵，它由函数x , y的偏微 分组成，反映了关节微小位移de与机械臂末端微小运动dx之间的关系。将两边同除以dt dt得到：dx/dt=Jde/dt,因此机械臂的雅可比矩阵也可以看做是操作空间中的速度与关 节空间中速度的线性变换。 dx/dt 称为末端在操作空间中的广义速度，简称操作速度， de /dt 为关节速度。可以看出，雅可比矩阵的每一列表示其它关节不动而某一关节以单位速度l s l sl s1 1 2 12 2 12 l c +1 c l c1 1 2 12 2 12运动产生的末端速度。可以看出， J 阵的值随末端位置的不同而不同，即01 和02 的改变会导致J的变化。对于关节空间的某些位姿，机械臂的雅可比矩阵的秩减少，这些位姿 称为机械臂的奇异位姿。上例机械臂雅可比矩阵的行列式为：det(J)二II sin(e )，当02=01 2 2 2或02=180时，机械臂的雅可比行列式为0 ，矩阵的秩为 1，这时机械臂处于奇异位姿。机械臂在操作空间的自由度将减少。如果机械臂的雅可比J是满秩的方阵，相应的关节速度即可求出，即8 = J-1 x，上例平面2R机械臂的逆雅可比矩阵J-1 =Ils l c l c1221 cl s2 12 2 12l s l s1 1 2 12 1 1 2 12，显然，当02趋于 0(或180)时，机械臂接近奇异位姿，相应的关节速度将趋于无穷大。为了补偿机器人末端执行器位姿与目标物体之间的误差，以及解决两个不同坐标系之间的微 位移关系问题，需要讨论机器人连杆在作微小运动时的位姿变化。假设一变换的元素是某个变量的函数，对该变换的微分就是该变换矩阵各元素对该变量的偏 导数所组成的变换矩阵乘以该变量的微分。例如给定变换T为:ttt111213ttt212223ttt313233ttt414243t14t24t34t44若它的元素是变量x的函数，则变换T的微分为:dtStdtStStdxStStF面讨论机械臂的微分运动，设机械臂某一连杆相对于基坐标系的位姿为T .经过微运动后该连杆相对基坐标系的位姿变为T+dT,若这个微运动是相对于基坐标系（静系）进行的（左 乘），总可以用微小的平移和旋转来表示，即T + dT - Trans(d , d , d )Rot(k, do )Txyz所以有dT - Trans (d , d , d )Rot(k, d0) -1Lx y z4x4 -根据齐次变换的对称性，若微运动是相对某个连杆坐标系i （动系）进行的（右乘），则T+dT 可以表示为T + dT - T - Trans(d , d , d )Rot(k, do )xyz所以有dT - T Trans(d , d , d )Rot(k, dO) -1x y z4x4令A = Trans(d , d , d )Rot(k, do ) -1 为微分算子，则相对基系有dT=A0T ,相对i系x y z4x 4有dT=TAi。这里A的下标不同是由于微运动相对不同坐标系进行的。在机械臂运动学中微分变换分为微分平移和微分旋转两类。微分平移变换与一般平移变换一样，其变换矩阵为:Trans(dx, dy, dz) -100001000010dxdydz1由于微分旋转时0fO，所以sinefde , cose1将它们代入旋转变换通式中得微分旋转表达式:Rot(k,dO) =1k dO-k dOy0-k dOz1k dOx0k dOy-k dOx100001于是得到微分算子 A = Trans(d , d , d )Rot(k, d0 ) -1xyz一 0kdOz-k dOy0-k dOz 0k dOx04x4k dOy c-k dOx00dxdydz0微分旋转与有限旋转相比，有一些特殊的性质，下面分别说明。(1)微分旋转的无序性，当e fO时，有sinede , cose fl.若令6 x=de x , 6 y=de y ,1000_ 106y0_1-6 z00 01-6 x0Rot(y,6y) =0100Rot(z,6 z) =6 z10006x106 y0100010_0001_ 0001 _ 00015 z=de z，则绕三个坐标轴的微分旋转矩阵分别为Rot (x, 6 x)=略去2 次项，得到Rot(x, 6 x)Rot(y, 6 y)=Rot (y, 6 y) Rot (x, 6 x)=16 x6 y-6 y0016x010-6y06x6y16x010016y-6 x0 -0-6 y6x10_ 0001 _106y0 -01-6 x0-6 y6x10_ 0001 _000100016y-6 x106y-6x10两者结果相同，可见这里左乘与右乘等效。结论：微分旋转其结果与转动次序无关，这是与 有限转动（一般旋转）的一个重要区别。（ 2）微分旋转的可加性，考虑两个微分旋转复合后的效果1-5z5 y 0Rot (x, 5 x) Rot (y, 5 y) Rot (z, 5 z)=5z-5 y15x-5 x 010若Rot （6 x, 6 y , 6 z）和Rot （6 x, 6 y, 6 z）表示两个不同的微分旋转，则两次连续转动的结果为：Rot(5x,5 y,5z)Rot(5x,5 y,5z) =5z + 5z-(5 y+5 y)-(5z+5z)5 y+5 y01-(5x+5x)05 x+ 5x 10001上式表明：任意两个微分旋转的结果为绕每个轴转动的元素的代数和,即微分旋转是可加的。由等效转轴和等效转角与Rot（x，5 x） Rot （y，5 y）Rot （z，5 z）等效，有一 1一 k dOk dO0 -一 1-5 z5y0 -k d0z1y- k dO05 z1-5 x0z一 k d0k dOx10-5 y5x10y_ 0x001_ 0001 _Rot (k, d0) = Rot (x, 5 x) Rot (y, 5 y) Rot (z, 5 z)所以有kxde=6x， kycto=6y，kzde=6z，将它们代入得5-5zy0-5z05x05d-5y dxxy0dz00可见，微分变换由两个部分组成6微分转动矢量，d微分平移矢量，合称为微分运动矢量，xyzxyz_ 00110_例：已知一个坐标系 A =10050100_ 0001 _可表示为 D = (d , d , d ,5 ,5 ,5 )T微分旋转矢量6=0 0.10 ，求微分变换dA。，相对固定系的微分平移矢量d=1 0 0.5，一 0-88d 一一 000.11 8z0-8yxd0000z-88x0yd-0.1000.5yxz_ 0000 _ 0000 _A=一 000.11 一-00110 -_00.101 一000010050000-0.1000.5010000-0.1-0.5_ 0000 _ 0001_0000 _下面讨论两坐标系之间的微分关系，设第一个坐标系为i系，第二个坐标系为j系不失一般dA = AA =性，假定j系就是固定的0系。0T =inxnynz0oxoyoz0axayaz0pxpypz1因为 A =008z-8 y0-8 z08x08y-8 x00dxdydz008zi-8 y0-8 zi08xi08yi-8 xidxi dyi dzi0所以 A 0T = 0TA0 i i iA = 0T - 1 A ii00iT ，整理得到dixdiydiz=n(8x p)+d)=o(8x p)+ d)=a(8x p) + d)8ix8 iy = 8 iz=adxidyi dzi8xi8yi8 zinx ox ax000nyoyay000nzozaz000(pxn)x(pxo)x(pxa)nxoxax(p x n)y(pxo)y(pxa)yn -y o y a y(pxn)z(pxo)z( p x a)znzozazdx0dy0dzo8 x08 y 008 z0对于任何三维矢量p=px，py，pz,其反对称矩阵S(p)定义为：pz-py-pz0py-px0上式简写成nxoxax0 R =noaiyyynoazzzdi5i0RT-bV BBRA0AVAVA/A- BRS ( AP )A BOBRAARB0- ARS(BP )B AOARBBVB-oRts(op )ii 00RTi类似地，任意两坐标系A和B之间广义速度的坐标变换为：_00110 -例：已知一个坐标系 A =10050100_0001 _微分旋转矢量S =0 0.1得到d0A5.11dixdiydiz=n(5x p)+d) =o(x p) + d)=a(5x p) + d)5 =o.1 0 olA55=管ixiy，相对固定系的微分平移矢量 d=1 0 0.5，0 ，求A系中等价的微分平移矢量dA和微分旋转矢量* 解：将 d=1 0 0.5和 6 =0 0.1 0 代入4、机械臂轨迹规划机械臂的轨迹规划可以在关节空间也可以在笛卡尔空间中进行，或者说机械臂轨迹规划是指 在关节空间或者笛卡尔空间中研究机械臂轨迹生成方法。简言之，机械臂轨迹规划是运动学 逆解的实际应用，它描述了机械臂在多维空间中的运动路线 。 在知道末端位姿的前提下， 通过运动学逆解得到各个关节在相应时刻的转动量或者平移量，合理的规划指的是规划出的 角位移曲线、角速度曲线以及角加速度曲线，可以有效地减少了机械臂在运动过程中的冲击 和振动，使机械臂的工作寿命得以延长。 械臂可以分为点到点作业 (Point-to-Point Motion ) 和连续路径作业 (Continuous-PathMotion ) 。点到点的运动指的是机械臂在运动过程中，只要求在某些点上有准确的位置和姿态，相邻的点不做要求。连续运动要求机械臂严格的沿特定的曲线运动。 机械臂的关节角位移变化率比较小，能够有效地防止了机械臂工作时的振动和冲击。机械臂 关节角速度和角加速度变化均平顺连续， 从而有效避免了机械部件的磨损，能够保证整个 机械臂系统的长期、稳定的运行，满足机械臂的工作要求。5、robotics 工具箱中的相关函数link 建立一个连杆对象,例如对于本次竞赛的机械臂，根据连杆参数得到L1=link(pi/20012000);L2=link(pi/200000);L3=link(-pi/200140.80pi);L4=link(-pi/271.8000pi/2 );L5=link(+pi/271.8000pi);L6=link(-pi/20000pi/2);L7=link(000129.600);robot 建立一个机械臂对象R= robot(L)noname (7 axis, RRRRRRR)grav = 0.00 0.00 9.81standard D&H parametersalphaAthetaDR/P1.57080120R(std)1.5708.00R(std)-1.57080140.8R(std)-1.570871.80R(std)1.570871.80R(std)-1.570800R(std)00129.6R(std)drivebot 用滑块控制的机械臂图形drivebot(R,ones(1,7)*pi)plot 机械臂的图形显示plot(R,pi/2 pi/2 0 0 0 0 0)fkine 串联机械臂正向运动学计算tr =fkine (ROBOT, Q)ROBOT表示机械臂对象，Q机械臂关节坐标值。tr =fkine (R, 0 0 0 pi/2 0 0 0)tr =0.0000 -0.0000-0.0000 1.0000-1.0000 -0.0000001.0000 129.60000.0000 -0.00000.0000 -20.80000 1.0000ikine串联机械臂逆向运动学计算q = ikine(ROBOT, T)q = ikine(ROBOT, T, Q)q = ikine(ROBOT, T, Q, M)输入变量ROBOT表示机械臂对象，T机械臂末端变换矩阵。输出变量 q 机械臂关节的角度(单位是弧度)，一般来说逆运动学的解不是唯一的，取决于初 始值Q ,缺省时是0向量。如果机械臂的自由度(DOF)小于6 ,由于解空间的维数大于机械 臂的自由度，这时需要第4个输入量M来确定笛卡尔坐标(手腕对应的坐标系)中的哪些量 在求解中被忽略。M中有6个元素，分别表示沿着x,y,z方向的平移和相对于x轴，y轴， z 轴的旋转，值是 0(忽略)或 1。非零元素的个数应该等于机械臂的自由度。例如，对典型 的有5个自由度的机械臂，一般是忽略相对手腕坐标的转动，这时M = 1 1 1 1 1 0。另外一种用法是qt = ikine(ROBOT, TG)qt = ikine (ROBOT, TG, Q) qt = ikine (ROBOT, TG, Q, M)输入变量ROBOT表示机械臂对象，TG是4x4xN机械臂末端变换矩阵。输出变量qt是一组（N个）TG对应的关节坐标。一行对应一个输入变换，每一步的初始值取上一步的 值。求解使用机械臂Jacobian矩阵的伪逆，这是数值求解方法，对于特定机械臂逆运动学 解（如果可能）应该尽量使用解析解。但是这种方法可以得到奇异点上的解，零空间中的关节 角度可以任取。q=ikine（R,tr）q =0.0000 0.0000 0.0000 0.7854 -0.0000 -0.7854 0.0000注意：对于机械臂末端的一个位置与姿态，逆运动学计算不是唯一的，验证tr=fkine(R,q)1.0000 129.60000.0000 -0.00000.0000 -20.80000 1.0000tr =0.0000 -0.0000 -0.0000 1.0000 -1.0000 -0.0000 00 transl 计算平移变换tr= transl (X, Y, Z)返回机械臂末端坐标X, Y, Z对应的齐次表换矩阵tr=transl(129.6,0,20.8)tr =1.000000129.600001.000000001.000020.8000000 1.0000X Y Z = transl(T)返回齐次表换表示中的平移值，作为一个3 元素的列向量xyz=transl(tr)xyz =129.6000 0 20.8000ctraj计算工作空间中两点T0,T1之间的轨迹tc= ctraj(T0, T1, N)tc = ctraj(T0, T1, R)返回从TO到T1笛卡尔坐标系的轨迹TC N表示轨迹中的点数。在第1中情况下，轨迹中的点在TO到T1中等距离分配。在第2中情况下，向量R给出轨迹中每个点的距离，R 中的元素取值为0 1。一个轨迹是4x4xN矩阵，最后一个下标表示点索引。旋转插值使用 四元球形线性插值。tr0=fkine(R,0 0 0 0 0 0 0)tr0 =1.0000-0.0000-0.0000-0.00000.00001.00000.00000.0000-0.0000-0.00001.0000108.80000001.0000tr1=fkine(R,pi/4 pi/6 0 pi/3 0 0 0)tr1 =0.6124-0.70710.353695.60080.61240.70710.353695.6008-0.5000-0.00000.8660110.30050001.0000tc(:,:,1) =1.000000-0.000001.000000.0000001.0000108.80000001.0000tc(:,:,2) =0.8976-0.38220.219847.80040.35710.92260.145847.8004-0.2585-0.05230.9646109.55030001.0000tc(:,:,3) =0.6124-0.70710.353695.60080.61240.70710.353695.6008-0.5000-0.00000.8660110.30050001.0000transl(tc)ans =-0.00000.0000108.800047.800447.8004109.550395.600895.6008110.3005jtraj计算关节中两点Q0,Q1之间的轨迹Q QD QDD = jtraj(Q0, Q1, N)Q QD QDD = jtraj (Q0, Q1, N, QD0, QD1)Q QD QDD = jtraj (Q0, Q1, T)Q QD QDD = jtraj (Q0, Q1, T, QD0, QD1)轨迹中的点数是N ,或者是一个时间向量T。插值使用7次多项式，边界速度由QDO, QD1 指定，缺省时边界速度和加速度为 0。q0=pi pi pi pi pi pi pi;q1=pi pi/2 0 0 0 pi/2 0;tr0=fkine(R,pi pi pi pi pi pi pi);tr1=fkine(R,pi pi/2 0 0 0 pi/2 0);QT,QD,QDD=jtraj(q0,q1,30);figuresubplot(2,2,1),plot(R,QT)subplot(2,2,2),plot(QT),grid on,legend(q1,q2,q3,q4,q5,q6,q7,Location, NorthWest)subplot(2,2,3),plot(QD),grid onsubplot(2,2,4),plot(QDD),grid on%注意：其中有一些曲线重合jacob0 计算机械臂在基坐标系中 Jacobian 矩阵J = jacob0(ROBOT, Q)tr2jac 计算机械臂在基坐标系中 Jacobian 矩阵J = TR2JAC(T)diff2tr 微分表示转换为齐次变换tr = diff2tr(D)返回表示微分平移与旋转的齐次变换矩阵,矩阵中包含一个反对称的旋转子矩阵。 tr2diff 转换为齐次变换转换为微分表示D =tr2diff(T)D = tr2diff(T1, T2) 第一种形式将齐次表换矩阵表示转换为6-元素向量微分表示。第二种形式返回6-元素向量，表示从T1到T2的在基坐标系中需要的微分移动。J = jacob0(R, q1)% Jacobian and differential motion demonstration% A differential motion can be represented by a 6-element vector with elements% dx dy dz drx dry drz% where the first 3 elements are a differential translation, and the last 3% are a differential rotation. When dealing with infinitisimal rotations,% the order becomes unimportant. The differential motion could be written% in terms of compounded transforms% transl(dx,dy,dz) * trotx(drx) * troty(dry) * trotz(drz)% but a more direct approach is to use the function diff2tr()D = .1 .2 0 -.2 .1 .1diff2tr(D)T=fkine(R,q1)% then the differential motion in the second frame would be given byDT = tr2jac(T) * D;DQ= pinv(J) * DT;vel = 1 0 0 0 0 0; % translational motion in the X directionqvel = pinv(J) * vel;ans =-0.0000 0.0000 -0.0000 0.0039 -0.0000 -0.0039 0.0000%这是计算工作空间轨迹和求逆解的另外一种方法。但是，如果Jacobia n矩阵奇异时%会失效。如果机械臂的自由度大于6，即是冗余的，采用Jacobian矩阵伪逆计算，或 %对Jacobian矩阵进行奇异值分解。附录rpy角与euler角（1）rpy 角rpy角是描述船舶航行时的姿态的一种方法，将船的行驶方向作为Z轴侧绕Z轴旋转称为 滚动（Roll）角a，将绕Y轴（与海面平行）方向的旋转称为俯仰（Pitch ）角p，取X轴与海 面垂直方向，将绕X轴的旋转称为偏转（Yaw）角丫。机械臂末端的定义类似，故习惯上称 为rpy角。描述运动坐标系的规则是：首先使运动坐标系的初始方位与固定坐标系重合，将运动坐标系 绕固定坐标系X轴转动y，再将运动坐标系绕固定坐标系Y轴转动p，最后将运动坐标系绕 固定坐标系Z轴转动a。因为三次转动都是相对固定坐标系的，所以相应的旋转矩阵rpy（Y,卩,a） = rot （z, a ）rot （y,卩）rot （x, y）ca-sa00 cPsaca0o|1 00010 11 - sP000111 00sP0_100100|0cy-sy0cP0|0sycy0010000001将三个矩阵相乘得到cacPcasPsy - sacycasPcy0sacP-sPsasPsy -cacyc sysasPcy0cPcy00001它表示绕固定坐标系的三个轴依次旋转得到的旋转矩阵，称为绕固定轴XYZ旋转的rpy方 法。下面讨论逆问题：从给定的旋转矩阵得到绕固定轴XYZ旋转的rpy角YPa。令noa0xxxnoa0yyynoa0zzz0001rpy （y,卩,a）=上式中有3个未知数，9个方程，其中6个不独立，因此可利用其中3个解出未知数。，如果cos（p）不为零，则可以得到P = a tan 2(-n , Jn2 + n2),a = a tan 2(o , n ),y = a tan 2(a , a )乙 x yxxy z(2 )绕运动系ZYX转动的euler角描述运动坐标系的规则是：运动坐标系的初始方位与参考坐标系重合，首先将运动坐标系绕Z轴转动a ,再将运动坐标系绕Y轴转动B ,最后将运动坐标系绕X轴转动丫。这种描述方法 中各次的转动都是相对运动系的，而不是相对固定坐标系的。相应的旋转矩阵为euler(a, P , y ) = rot(x,a)rot(y, P)rot(z,y )0_T1000II0cy-sy0I0sycy1 III0000001_ca-sa00IT cP0sPsaca00II010=0010II-sP0cp0001II000将三个矩阵相乘得到cacP casPsy - sacy casPcy 0euler(a,P,y)=sacP-sPsasPsy - cacycPsysasPcycPcy0001结果与绕固定轴XYZ旋转相同，这是因为绕固定轴旋转的顺序与绕运动轴旋转的顺序相反， 且旋转角度对应相等。因此，用ZYX euler角与XYZ rpy角的描述方法是等价的。另外一种常用的euler角方法是ZYZ方法，首先使运动坐标系与参考坐标系重合，将运动坐标系绕Z轴转动a，再将运动坐标系绕Y轴转动B，最后将运动坐标系绕Z轴转动丫。euler(a,P,y)=rot(z,a)rot(y,P)rot(z,y)Tca-sa00 T cP0sP0_Tcy-sy00_saca00III 0100IIsycy00II0010III-sP0cP0I0010II0001III0001 III0001 II将三个矩阵相乘得到cacPcy - sasy -casPsy - sacy casP 0euler(a,P,y)=sa cP cy + ca sy-sPcy-sa sP sy + ca cysP sysa sP 0cP00 0 0 1P = a tan 2(如果sing不为零，ZYZ方式euler角逆解是,a ),a = a tan 2(o , n ),y = a tan 2(a , -a )zz Zy x