机械臂运动学基础

机械臂运动学基础1 、机械臂的运动学模型机械臂运动学研究的是机械臂运动，而不考虑产生运动的力。运动学研究机械臂的位置，速度和加速度。 机械臂的运动学的研究涉及到的几何和基于时间的内容，特别是各个关节彼此之间的关系以及随时间变化规律。典型的机械臂由一些串行连接的关节和连杆组成。每个关节具有一个自由度，平移或旋转。对于具有 n 个关节的机械臂，关节的编号从1 到 n ，有 n +1 个连杆，编号从 0到 n。连杆0 是机械臂的基础， 一般是固定的， 连杆 n 上带有末端执行器。 关节 i 连接连杆 i和连杆 i-1 。一个连杆可以被视为一个刚体，确定与它相邻的两个关节的坐标轴之间的相对位置。一个连杆可以用两个参数描述， 连杆长度和连杆扭转， 这两个量定义了与它相关的两个坐标轴在空间的相对位置。而第一连杆和最后一个连杆的参数没有意义，一般选择为0 。一个关节用两个参数描述， 一是连杆的偏移， 是指从一个连杆到下一个连杆沿的关节轴线的距离。二是关节角度，指一个关节相对于下一个关节轴的旋转角度。为了便于描述的每一个关节的位置，我们在每一个关节设置一个坐标系，对于一个关节链，Denavit 和 Hartenberg提出了一种用矩阵表示各个关节之间关系的系统方法。对于转动关节 i，规定它的转动平行于坐标轴zi-1 ，坐标轴 xi-1 对准从 zi-1 到 zi 的法线方向，如果 zi-1 与z相交，则 xi-1取 zi-1z的方向。连杆，关节参数概括如下：ii连杆长度 a i沿着 xi 轴从 zi-1和 zi 轴之间的距离 ;连杆扭转 i从 zi-1 轴到 zi 轴相对 xi-1 轴夹角 ;连杆偏移 d i从坐标系 i-1 的原点沿着 zi-1 轴到 xi 轴的距离 ;关节角度 ixi-1 轴和 xi 轴之间关于 zi-1 轴的夹角。1对于一个转动关节i 是关节变量， d i 是常数。而移动关节d i 是可变的， i 是恒定的。为了统一，表示为qii转动关节di移动关节运用Denavit-Hartenberg（ DH ）方法，可以将相邻的两个坐标系之间的变换关系表示为一个 4x4 的齐次变换矩阵cos isin i cosisini siniai cos ii 1 Asin icos i cos icos isin iai sin ii0sin icosdii0001上式表示出了坐标系i 相对于坐标系i-1 的关系。即0Ti0Ti 1 i 1 Ai其中 0Ti 表示坐标系 i 相对于世界坐标系 0的位置与姿态，简称位姿。2 、正向和反向运动学对于一个n- 轴刚性连接的机械臂，正向运动学的解给出的是最后一个连杆坐标系的位置和姿态。重复利用上式,得到0Tn0 A1 1 A2n 1 AnK (q)机械臂末端位姿在笛卡尔坐标系中有6 个自由度， 3 个平移， 3 个旋转。所以，一般来说具有 6 个自由度的机械臂可以使末端实现任意的位姿。总的机械臂变换0Tn 一般简写为Tn ，对 6 个自由度的机械臂简写为T6 。对于任意的机械臂，无论其它有多少个关节，具有什么结构，正向运动学解都是可以得到的。在机械臂的路径规划中，用到的是反向运动学的解qK 1( 0Tn ) ，它给出了特定的末端位姿对应的机械臂的关节角度。一般来说， 反向运动学的解不是唯一的，对具有某种结构的机械臂，封闭解可能不存在。2对于6 自由度的机器人而言，运动学逆解非常复杂，一般没有封闭解。只有在某些特殊情况下才可能得到封闭解。不过，大多数工业机器人都满足封闭解的两个充分条件之一（ Pieper准则）（ 1 ）三个相邻关节轴交于一点（ 2 ）三个相邻关节轴相互平行如果机械臂多于6 个关节，称关节为冗余的，这时解是欠定的。如果对于机械臂某个特别的位姿， 解不存在， 称这个位姿为奇异位姿。机械臂的奇异性可能是由于机械臂中某些坐标轴的重合，或位置不能达到引起的。机械臂的奇异位姿分为两类：(1) 边界奇异位姿，当机械臂的关节全部展开或折起时，使得末端处于操作空间的边界或边界附近， 雅克比矩阵奇异，机械臂的运动受到物理结构的约束，这时机械臂的奇异位姿称为边界奇异位姿。(2) 内部奇异位姿，两个或两个以上的关节轴线重合时，机械臂各个关节的运动相互抵消，不产生操作运动，这时机械臂的奇异位姿称为内部奇异位姿。机械臂运动学逆解的方法可以分为两类： 封闭解和数值解、 在进行逆解时总是力求得到封闭解。因为封闭解的计算速度快，效率高，便于实时控制。而数值解法不具有这些特点。机械臂运动学的封闭逆解可通过两种途径得到：代数法和几何法。一般而言，非零连杆参数越多，到达某一目标的方式也越多，即运动学逆解的数目也越多。在从多重解中选择解时， 应根据具体情况， 在避免碰撞的前提下通常按“ 最短行程 ”准则来选择。同时还应当兼顾“ 多移动小关节，少移动大关节 ”的原则。n 个自由度的机械臂的末端位姿由n 个关节变量所决定，这n 个关节变量统称为n 维关节3矢量， 记为 q 。所有的关节矢量构成的空间称为关节空间 。机械臂末端的位姿用6 个变量描述， 3 个平移 (x,y,z) 和 3 个旋转 (x,y,z) ，记 x= (x,y,z,x,y,z)， x 是机械臂末端在基坐标空间中的坐标，所有的矢量x 构成的空间称为操作空间或作业定向空间。工作空间是操作臂的末端能够到达的空间范围，即末端能够到达的目标点集合。值得指出的是， 工作空间应该严格地区分为两类：(1) 灵活（工作）空间指机械臂末端能够以任意方位到达的目标点集合。因此，在灵活空间的每个点上，手爪的指向可任意规定。(2) 可达（工作）空间指机械臂末端至少在一个方位上能够到达的目标点集合。机械臂各关节驱动器的位置组成的矢量称为驱动矢量s，由这些矢量构成的空间称为驱动空间。正向运动学驱动空间关节空间工作空间运动学逆解3 、 Jacobian矩阵机械臂的 Jacobian矩阵表示机械臂的操作空间与关节空间之间速度的线性映射关系，对于一个 n 轴的机械臂，机械臂末端在基坐标系中的速度是xJq 其中 x 是 6 个元素的向量。对于 6 个关节机械臂Jacobian矩阵是方阵，如果它是可逆的，则可以由机械臂的末端速度求出各个关节的速度。Jacobian矩阵在机械臂的奇异位姿上是不可逆的。在实际应用中，当机械臂的末端位置接近奇异位置时，Jacobian矩阵是病态的，可能导致关节速度不能正确地得到。上式解决的是正速度问题，即已知 q 和 q 求末端执行器的速度x 。对于逆速度解问题，由上4式可以得到速度逆解公式为qJ 1x ，注意到此时需要求雅可比矩阵的逆，由线性方程组理论知上式对任意的x ， q 都有解的必要条件是雅可比矩阵的秩rank(J)=6，这意味着机械臂的自由度数n 6 。这也说明了具有冗余自由度的机械臂，在末端位姿固定的条件下，能使关节在一个较大的关节空间的子空间中运动，有效地避开障碍或奇异位姿，并把关节位移限制在允许范围内，从而具有更大的运动灵活性。雅可比矩阵可以看成是从关节空间到操作空间运动速度的传动比，同时也可用来表示两空间之间力的传递关系。对于冗余自由度机械臂，其雅可比矩阵是长方矩阵，因 J 满秩且方程个数少于未知数个数，所以有无穷多个解，这时，一般是求其中的最小范数解，或采用加权最小范数解也就是说使qT Dq 最小的解， 其中 D 是对称正定加权矩阵。此时的解是使机械臂在能量消耗最小的情况下的解。这时，逆速度问题便转为：求q 满足 qJ 1x 且使 L1 qT Dq 最小。实际上等同于求性能2指标L 在约束条件qJ 1 x下的极值。应用Lagrange乘子法，以上极值为题的解是qD 1J T (JD 1 J T ) 1 x ，当 D= I 时，雅可比矩阵是JJ T ( JJ T ) 1 ，称为雅可比矩阵的伪逆。下面通过一个两自由度的平面机械臂说明雅可比矩阵的特性，根据右图中的几何关系容易求得xl1 c1l2 c12c1cos( 1), c12cos(yl1 s1l 2 s12s1sin( 1 ), s12sin(1 2 )12 )两边微分后写成矩阵形式xxdx12ddyyyd1dxl1 s1 l2 s12l2 s12d即l1 c1 l 2 c12l2 c12d2dy12125简写成 dx=Jd ， 式中 J 就称为机械臂的雅可比（ Jacobian ）矩阵，它由函数x，y 的偏微分组成， 反映了关节微小位移d 与机械臂末端微小运动 dx 之间的关系。 将两边同除以dtdt 得到：dx/dt=Jd/dt,因此机械臂的雅可比矩阵也可以看做是操作空间中的速度与关节空间中速度的线性变换。dx/dt 称为末端在操作空间中的广义速度，简称操作速度，d /dt为关节速度。 可以看出， 雅可比矩阵的每一列表示其它关节不动而某一关节以单位速度运动产生的末端速度。由 Jl1 s1 l2 s12l2 s12 可以看出， J 阵的值随末端位置的不同而不同，即1 和2 的l1 c1 l 2 c12l2 c12改变会导致J 的变化。 对于关节空间的某些位姿，机械臂的雅可比矩阵的秩减少，这些位姿称为机械臂的奇异位姿。 上例机械臂雅可比矩阵的行列式为：det(J )2 =0 l1l2 sin( 2 ) ，当 或 =180 时，机械臂的雅可比行列式为0 ，矩阵的秩为1 ，这时机械臂处于奇异位姿。机2械臂在操作空间的自由度将减少。如果机械臂的雅可比 J是满秩的方阵，相应的关节速度即可求出，即J 1 x ，上例平面2R 机械臂的逆雅可比矩阵J 11l2 c12l2 s12l1c1 l 2c12，显然，当 2 趋于 0 （或l1l 2 s2l1s1 l 2s12180 ）时，机械臂接近奇异位姿，相应的关节速度将趋于无穷大。为了补偿机器人末端执行器位姿与目标物体之间的误差， 以及解决两个不同坐标系之间的微位移关系问题，需要讨论机器人连杆在作微小运动时的位姿变化。假设一变换的元素是某个变量的函数， 对该变换的微分就是该变换矩阵各元素对该变量的偏导数所组成的变换矩阵乘以该变量的微分。例如给定变换T 为：t11t12t13t14t21t 22t23t24Tt32t33t34t31t41t 42t43t44若它的元素是变量x 的函数，则变换T 的微分为 :6t11t12t13t14xxxxt21t22t23t 24dTxxxxdxt31t32t33t34xxxxt41t42t43t 44xxxx下面讨论机械臂的微分运动，设机械臂某一连杆相对于基坐标系的位姿为T ，经过微运动后该连杆相对基坐标系的位姿变为T+dT ，若这个微运动是相对于基坐标系（静系） 进行的 (左乘) ，总可以用微小的平移和旋转来表示，即TdTTrans(dx , dy , dz ) Rot(k ,d)T所以有dTTrans(d x, d y , dz ) Rot(k , d )I 44T根据齐次变换的对称性， 若微运动是相对某个连杆坐标系i（动系） 进行的 ( 右乘 )，则 T+dT可以表示为TdTT Trans( dx , d y ,d z) Rot(k , d)所以有dTT Trans( dx ,d y , dz) Rot(k , d)I 44令Trans(dx, d y , d z)Rot( k , d) I 44 为微分算子，则相对基系有dT= 0T ，相对 i 系有 dT=T i 。 这里 的下标不同是由于微运动相对不同坐标系进行的。在机械臂运动学中微分变换分为微分平移和微分旋转两类。微分平移变换与一般平移变换一样，其变换矩阵为:100dx010dyTrans (dx, dy, dz)001dz00017由于微分旋转时 0，所以 sin d， cos 1将它们代入旋转变换通式中得微分旋转表达式 :1kzdky d0Rot(k , d )kzd1kxd0kydkxd100001于是得到微分算子Trans(dx ,d y , dz)Rot( k , d ) I 44 ，即0kzdkyddxkzd0kxddykydkx d0dz0000微分旋转与有限旋转相比，有一些特殊的性质，下面分别说明。（1 ）微分旋转的无序性，当 0 时，有 sin d， cos 1 若令 x=d x，y=d y ，z=d z，则绕三个坐标轴的微分旋转矩阵分别为100010y01z 0001x 00100z100Rot(x, x)x10Rot( y, y)y 010Rot(z, z)01000000100010001略去 2 次项，得到10y010y0Rot(x,x y1x001x0x)Rot( y, y)x10yx10y000100011x yy010y0Rot ( y,01x001x0y) Rot(x, x)x10yx10y00010001两者结果相同， 可见这里左乘与右乘等效。结论：微分旋转其结果与转动次序无关，这是与有限转动（一般旋转）的一个重要区别。（2 ）微分旋转的可加性，考虑两个微分旋转复合后的效果81zy0z1x0Rot( x, x) Rot( y, y)Rot( z, z)x10y0001若 Rot （ x，y ，z） 和 Rot （x， y ， z）表示两个不同的微分旋转，则两次连续转动的结果为：1( zz)yy 0zz1( xx )0Rot( x, y, z) Rot( x , y , z )y )xx 10( y0001上式表明：任意两个微分旋转的结果为绕每个轴转动的元素的代数和，即微分旋转是可加的。由等效转轴和等效转角与Rot( x,x) Rot( y,y)Rot( z,z) 等效，有Rot(k , d )Rot( x,x) Rot( y,y)Rot( z, z)1kzdky d01zy0kz d1kxd0z1x 0k ydkxd10yx1000010001所以有 kxd = x， kyd = y ， kzd = z， 将它们代入得0zydxz0xd yyx0d z0000可见，微分变换由两个部分组成微分转动矢量 ，d 微分平移矢量 ,合称为微分运动矢量，可表示为 D (d x, dy , dz , x,y , z )T001101005例：已知一个坐标系A，相对固定系的微分平移矢量d= 100.5 ，01000001微分旋转矢量=00.10 ，求微分变换dA 。90zydx000.11z0xdy0000yx0dz0.1000.500000000000.110011000.101dAA0000100500000.1000.50100000.10.5000000010000下面讨论两坐标系之间的微分关系，设第一个坐标系为i系，第二个坐标系为j 系不失一般性，假定 j 系就是固定的 0系。nxoxaxpx0nyoyaypyiTnzozazpz00010zy dx0ziyidxi因为z0x dy， izi0xidyi0yx0dzyixi0dzi00000000所以00iT0iT i ，i0iT100iT ，整理得到dixn (p)d )diyo (p)d )diza (p)d )ixiyiznoadxinxnynz( p n)x( p n) y( pn) zdx0dyioxoyoz( p o)x( p o ) y( po) zdy0dziaxayaz( p a)x( p a) y( pa) zdz0xi000nxnynzx0yi000oxoyozy0z000axayazzi0对于任何三维矢量p = p x, p y, p z,其反对称矩阵s(p)定义为：0pzpys( p)pz0pxpypx010记nxoxax0i Rnyoyaynzozaz上式简写成di0RT0RT s( 0p)iii 0i00i RT类似地，任意两坐标系A 和 B之间广义速度的坐标变换为：BVAB RAB RS( A PBO )AV，AVBARBA RS( B PAO )BVB0ABRAA0BA RB00110100500.5 ，例：已知一个坐标系A10，相对固定系的微分平移矢量d= 1000001微分旋转矢量 =00.1 0 ，求 A 系中等价的微分平移矢量d A 和微分旋转矢量A 。解：将 d= 10 0.5 和 =00.1 0 代入dixn (p)d )ixdiyo (p)d )iydiza (p)d )iz得到 dA 0 0 5.1TTA0.1 0 0。noa4 、机械臂轨迹规划机械臂的轨迹规划可以在关节空间也可以在笛卡尔空间中进行，或者说机械臂轨迹规划是指在关节空间或者笛卡尔空间中研究机械臂轨迹生成方法。简言之， 机械臂轨迹规划是运动学逆解的实际应用，它描述了机械臂在多维空间中的运动路线。 在知道末端位姿的前提下，通过运动学逆解得到各个关节在相应时刻的转动量或者平移量，合理的规划指的是规划出的角位移曲线、 角速度曲线以及角加速度曲线，可以有效地减少了机械臂在运动过程中的冲击和振动，使机械臂的工作寿命得以延长。11械臂可以分为点到点作业(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)12fkine串联机械臂正向运动学计算tr =fkine (ROBOT, Q)ROBOT 表示机械臂对象，Q 机械臂关节坐标值。tr =fkine (R, 0 0 0 pi/2 0 0 0)tr =0.0000-0.00001.0000129.6000-0.00001.00000.0000-0.0000-1.0000-0.00000.0000-20.80000001.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)13输入变量ROBOT 表示机械臂对象，TG 是 4x4xN机械臂末端变换矩阵。输出变量qt 是一组 (N 个 )TG 对应的关节坐标。一行对应一个输入变换，每一步的初始值取上一步的值。求解使用机械臂Jacobian矩阵的伪逆，这是数值求解方法，对于特定机械臂逆运动学解( 如果可能 )应该尽量使用解析解。但是这种方法可以得到奇异点上的解，零空间中的关节角度可以任取。q=ikine(R,tr)q =0.00000.00000.00000.7854-0.0000-0.78540.0000注意：对于机械臂末端的一个位置与姿态，逆运动学计算不是唯一的，验证tr=fkine(R,q)tr =0.0000-0.00001.0000129.6000-0.00001.00000.0000-0.0000-1.0000-0.00000.0000-20.80000001.0000transl计算平移变换tr= transl (X, Y, Z)返回机械臂末端坐标X, Y, Z 对应的齐次表换矩阵tr=transl(129.6,0,20.8)tr =1.000000129.600001.000000001.000020.80000001.0000X Y Z = transl(T)返回齐次表换表示中的平移值，作为一个3 元素的列向量xyz=transl(tr)14xyz =129.6000020.8000ctraj计算工作空间中两点T0,T1之间的轨迹tc= ctraj(T0, T1, N)tc = ctraj(T0, T1, R)返回从 T0 到 T1 笛卡尔坐标系的轨迹TCN 表示轨迹中的点数。在第1 中情况下，轨迹中的点在 T0到 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.600815-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 次多项式， 边界速度由QD0, 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)16diff2tr微分表示转换为齐次变换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 by DT = tr2jac(T) * D;DQ= pinv(J) * DT;vel = 1 0 0 0 0 0; % translational motion in the X direction qvel = pinv(J) * vel;ans =-0.00000.0000-0.00000.0039-0.0000-0.00390.0000% 这是计算工作空间轨迹和求逆解的另外一种方法。但是，如果Jacobian 矩阵奇异时% 会失效。如果机械臂的自由度大于6，即是冗余的，采用 Jacobian 矩阵伪逆计算，或% 对 Jacobian 矩阵进行奇异值分解。17附录rpy 角与 euler角（ 1 ）rpy 角rpy 角是描述船舶航行时的姿态的一种方法，滚动（ Roll ）角 ，将绕 Y 轴 (与海面平行面垂直方向，将绕X 轴的旋转称为偏转（将船的行驶方向作为Z 轴，则绕 Z 轴旋转称为)方向的旋转称为俯仰（Pitch ）角 ，取 X 轴与海Yaw ）角 。机械臂末端的定义类似，故习惯上称为 rpy 角。描述运动坐标系的规则是：首先使运动坐标系的初始方位与固定坐标系重合，将运动坐标系绕固定坐标系X 轴转动 ，再将运动坐标系绕固定坐标系Y 轴转动 ，最后将运动坐标系绕固定坐标系Z 轴转动 。因为三次转动都是相对固定坐标系的，所以相应的旋转矩阵rpy ( , , ) rot ( z,) rot ( y,)rot ( x,)cs00c0s01000sc0001000cs00010s0c00sc0000100010001将三个矩阵相乘得到c cc ssscc s c0s cs ssccs s c0rpy ( , , )c sc c0s0001它表示绕固定坐标系的三个轴依次旋转得到的旋转矩阵，称为绕固定轴XYZ 旋转的 rpy 方法。下面讨论逆问题：从给定的旋转矩阵得到绕固定轴XYZ 旋转的 rpy 角。令nxoxax0rpy( , ,nyoyay0)ozaz0nz0001上式中有 3个未知数， 9 个方程，其中6 个不独立，因此可利用其中3 个解出未知数。cos( )nx2ny2 ，如果 cos( )不为零，则可以得到18a tan 2( n,n2n2),a tan 2(ox, nx),a tan 2(ay, a )zxyz（ 2 ）绕运动系 ZYX 转动的 euler 角描述运动坐标系的规则是：运动坐标系的初始方位与参考坐标系重合，首先将运动坐标系绕Z 轴转动 ，再将运动坐标系绕Y 轴转动 ，最后将运动坐标系绕X 轴转动 。这种描述方法中各次的转动都是相对运动系的，而不是相对固定坐标系的。相应的旋转矩阵为euler( , , ) rot ( x,)rot ( y,)rot (z,)cs00c0s01000sc0001000cs0001 0s0c00sc0000100010001将三个矩阵相乘得到c cc s ss cc s c0s cs s sc cs s c0euler ( , , )csc c0s0001结果与绕固定轴XYZ 旋转相同， 这是因为绕固定轴旋转的顺序与绕运动轴旋转的顺序相反，且旋转角度对应相等。因此，用ZYX euler角与 XYZ rpy角的描述方法是等价的。另外一种常用的euler角方法是ZYZ 方法，首先使运动坐标系与参考坐标系重合，将运动坐标系绕Z 轴转动 ，再将运动坐标系绕Y 轴转动 ，最后将运动坐标系绕Z 轴转动 。euler( , ) rot (