机械臂运动学

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与zi相交，则xi-1取zi−1 ×zi的方向。连杆，关节参数概括如下：  连杆长度ai 沿着xi轴从zi-1和zi轴之间的距离;  连杆扭转αi 从zi-1轴到zi轴相对xi-1轴夹角;

 连杆偏移di 从坐标系i-1的原点沿着zi-1轴到xi轴的距离;  关节角度θi xi-1轴和xi轴之间关于zi-1轴的夹角。

1

对于一个转动关节θi是关节变量，di是常数。而移动关节di是可变的，θi是恒定的。为了统一，表示为

转动关节 qiid移动关节i运用Denavit-Hartenberg（DH）方法，可以将相邻的两个坐标系之间的变换关系表示为一个4x4的齐次变换矩阵

cosisinii1Ai00sinicosicosicosisini0sinisinicosisinicosi0aicosiaisini di1上式表示出了坐标系i相对于坐标系i-1的关系。即

0Ti0Ti1i1Ai

0其中Ti表示坐标系i相对于世界坐标系0的位置与姿态，简称位姿。

2、正向和反向运动学

对于一个n-轴刚性连接的机械臂，正向运动学的解给出的是最后一个连杆坐标系的位置和姿态。重复利用上式,得到

0Tn0A11A2n1AnK(q)

机械臂末端位姿在笛卡尔坐标系中有6个自由度，3个平移，3个旋转。所以，一般来说具有6个自由度的机械臂可以使末端实现任意的位姿。

0总的机械臂变换Tn一般简写为Tn，对6个自由度的机械臂简写为T6。对于任意的机械臂，

无论其它有多少个关节，具有什么结构，正向运动学解都是可以得到的。

10在机械臂的路径规划中，用到的是反向运动学的解qK(Tn)，它给出了特定的末端位

姿对应的机械臂的关节角度。一般来说，反向运动学的解不是唯一的，对具有某种结构的机械臂，封闭解可能不存在。

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轴的机械臂，机械臂末端在基坐标系中的速度是xJq其中x是6个元素的向量。对于6个关节机械臂Jacobian矩阵是方阵，如果它是可逆的，则可以由机械臂的末端速度求出各个关节的速度。Jacobian矩阵在机械臂的奇异位姿上是不可逆的。在实际应用中，当机械臂的末端位置接近奇异位置时，Jacobian矩阵是病态的，可能导致关节速度不能正确地得到。

上式解决的是正速度问题，即已知q和q求末端执行器的速度x。对于逆速度解问题，由上

4

式可以得到速度逆解公式为qJx，注意到此时需要求雅可比矩阵的逆，由线性方程组理论知上式对任意的x，q都有解的必要条件是雅可比矩阵的秩rank(J)=6，这意味着机械臂的自由度数n≥6。

这也说明了具有冗余自由度的机械臂，在末端位姿固定的条件下，能使关节在一个较大的关节空间的子空间中运动，有效地避开障碍或奇异位姿，并把关节位移限制在允许范围内，从而具有更大的运动灵活性。

雅可比矩阵可以看成是从关节空间到操作空间运动速度的传动比，同时也可用来表示两空间之间力的传递关系。对于冗余自由度机械臂，其雅可比矩阵是长方矩阵，因J满秩且方程个数少于未知数个数，所以有无穷多个解，这时，一般是求其中的最小范数解，或采用加权最小范数解也就是说使qTDq最小的解，其中D是对称正定加权矩阵。此时的解是使机械臂在能量消耗最小的情况下的解。

这时，逆速度问题便转为：求q满足qJx且使L1111TqDq最小。实际上等同于求性能2指标L在约束条件qJx下的极值。应用Lagrange乘子法，以上极值为题的解是

qD1JT(JD1JT)1x，当D=I时，雅可比矩阵是JJT(JJT)1，称为雅可比矩阵的

伪逆。

下面通过一个两自由度的平面机械臂说明雅可比矩阵的特性，根据右图中的几何关系容易求得

xl1c1l2c12c1cos(1),c12cos(12)yl1s1l2s12s1sin(1),s12sin(12)两边微分后写成矩阵形式

x1dxdyy1

x2d1 即 yd22dxl1s1l2s12dylclc11212l2s12d1 l2c12d25

简写成 dx=Jdθ，式中J就称为机械臂的雅可比（Jacobian）矩阵，它由函数x，y的偏微分组成，反映了关节微小位移dθ与机械臂末端微小运动dx之间的关系。 将两边同除以dt dt 得到：dx/dt=Jdθ/dt,

因此机械臂的雅可比矩阵也可以看做是操作空间中的速度与关

节空间中速度的线性变换。dx/dt称为末端在操作空间中的广义速度，简称操作速度，dθ/dt为关节速度。可以看出，雅可比矩阵的每一列表示其它关节不动而某一关节以单位速度运动产生的末端速度。

l1s1l2s12由Jl1c1l2c12l2s12可以看出，J阵的值随末端位置的不同而不同，即θ1和θ2的

l2c12改变会导致J的变化。对于关节空间的某些位姿，机械臂的雅可比矩阵的秩减少，这些位姿

det(J)l1l2sin(2)，称为机械臂的奇异位姿。上例机械臂雅可比矩阵的行列式为：当θ2=0°

或θ2=180°时，机械臂的雅可比行列式为0，矩阵的秩为1，这时机械臂处于奇异位姿。机械臂在操作空间的自由度将减少。

如果机械臂的雅可比J是满秩的方阵，相应的关节速度即可求出，即Jx，上例平面2R机械臂的逆雅可比矩阵J111l1l2s2l2c12lclc11212l2s12，显然，当θ2趋于0°（或

l1s1l2s12180°）时，机械臂接近奇异位姿，相应的关节速度将趋于无穷大。

为了补偿机器人末端执行器位姿与目标物体之间的误差，以及解决两个不同坐标系之间的微位移关系问题，需要讨论机器人连杆在作微小运动时的位姿变化。

假设一变换的元素是某个变量的函数，对该变换的微分就是该变换矩阵各元素对该变量的偏导数所组成的变换矩阵乘以该变量的微分。例如给定变换T为：

t11tT21t31t41t12t22t32t42t13t14t23t24 t33t34t43t44若它的元素是变量x的函数，则变换T的微分为:

6

t11xt21xdTt31xt41xt12xt22xt32xt42xt13xt23xt33xt43xt14xt24x

dxt34xt44x下面讨论机械臂的微分运动，设机械臂某一连杆相对于基坐标系的位姿为T，经过微运动后该连杆相对基坐标系的位姿变为T+dT，若这个微运动是相对于基坐标系（静系）进行的(左乘)，总可以用微小的平移和旋转来表示，即

TdTTrans(dx,dy,dz)Rot(k,d)T

所以有

dTTrans(dx,dy,dz)Rot(k,d)I44T

根据齐次变换的对称性，若微运动是相对某个连杆坐标系i（动系）进行的(右乘)，则T+dT可以表示为

TdTTTrans(dx,dy,dz)Rot(k,d)

所以有

dTTTrans(dx,dy,dz)Rot(k,d)I44

令Trans(dx,dy,dz)Rot(k,d)I44为微分算子，则相对基系有dT=Δ0T，相对i系有dT=TΔi 。这里Δ的下标不同是由于微运动相对不同坐标系进行的。在机械臂运动学中微分变换分为微分平移和微分旋转两类。

微分平移变换与一般平移变换一样，其变换矩阵为:

10Trans(dx,dy,dz)00

00dx10dy 01dz0017

由于微分旋转时θ→0 ，所以sinθ→dθ，cosθ→1将它们代入旋转变换通式中得微分旋转表达式:

1kdRot(k,d)zkyd0kzd1kxd0kydkxd1000 01于是得到微分算子Trans(dx,dy,dz)Rot(k,d)I44，即

0kdzkyd0kzd0kxd0kydkxd00dxdy dz0微分旋转与有限旋转相比，有一些特殊的性质，下面分别说明。

（1）微分旋转的无序性，当θ→0 时，有sinθ→dθ，cosθ→1．若令δx=dθx，δy=dθy，δz=dθz，则绕三个坐标轴的微分旋转矩阵分别为

01001xRot(x,x)0x10000100Rot(y,y)y0100y01zz100 1Rot(z,z)00100001000000 1001略去2次项，得到

0y1xy1xRot(x,x)Rot(y,y)yx1000010y001x0yx11000010y001x0yx1100000 0100 01xyy101xRot(y,y)Rot(x,x)yx1000两者结果相同，可见这里左乘与右乘等效。结论：微分旋转其结果与转动次序无关，这是与有限转动（一般旋转）的一个重要区别。

（2）微分旋转的可加性，考虑两个微分旋转复合后的效果

8

zy1z1xRot(x,x)Rot(y,y)Rot(z,z)yx100000 01若Rot（δx，δy，δz） 和Rot（δx’，δy’，δz’） 表示两个不同的微分旋转，则两次连续转动的结果为：

1(zz')yy'zz'1(xx')Rot(x,y,z)Rot(x',y',z')(yy')xx'100000 01上式表明：任意两个微分旋转的结果为绕每个轴转动的元素的代数和，即微分旋转是可加的。 由等效转轴和等效转角与Rot(x,x)Rot(y,y)Rot(z,z)等效，有

Rot(k,d)Rot(x,x)Rot(y,y)Rot(z,z)

1kdzkyd0kzd1kxd0kydkxd1001zyz01x0yx1100000 01所以有kxdθ=δx， kydθ=δy ， kzdθ=δz，将它们代入Δ得 0zy0z0yx00x0dxdy dz0可见，微分变换由两个部分组成δ微分转动矢量，d微分平移矢量,可表示为D(dx,dy,dz,x,y,z)

T 合称为微分运动矢量，

01例：已知一个坐标系A000110005 ，相对固定系的微分平移矢量d=[1 0 0.5]，100001微分旋转矢量δ=[0 0.1 0 ]，求微分变换dA。

9

0zy000dAA0.10000z0yx00x0000dx00dydz0.10000.100000010 0.5000.110100.5000011000.1010000500

100000.10.50010000下面讨论两坐标系之间的微分关系，设第一个坐标系为i系，第二个坐标系为j系不失一般性，假定j系就是固定的0系。

nxn0yiTnz0oxoyoz0axayaz0pxpy pz1zi0zydx00zz0xdy，ii因为 0yxyi0dz0000001000所以0iTiTi，iiT0iT，整理得到

yixi00xi0dxidyi dzi0dixn((p)d)ixndiyo((p)d)iyo diza((p)d)izadxinxdyoixdziaxxi0yi0zi0nyoyay000nzozaz000(pn)x(po)x(pa)xnxoxax(pn)y(po)y(pa)ynyoyay(pn)zdx0dy(po)z0(pa)zdz0 nzx0ozy0azz0对于任何三维矢量 p=[px, py, pz],其反对称矩阵s(p) 定义为：

0s(p)pzpy

pz0pxpypx 010

记

nx0iRnynz上式简写成

Tdi0iRi0oxoyozaxay azT00iRs(pi0) 0TiR类似地，任意两坐标系{A}和{B}之间广义速度的坐标变换为：

BBAABVARARS(APBO)AVAVBRBRS(BPAO)BVA，AB BBA0AR0BR01例：已知一个坐标系A000110005 ，相对固定系的微分平移矢量d=[1 0 0.5]，100001微分旋转矢量δ=[0 0.1 0 ]，求A系中等价的微分平移矢量dA和微分旋转矢量δA。 解：将d=[1 0 0.5] 和δ=[0 0.1 0 ]代入

dixn((p)d)ixndiyo((p)d)iyo diza((p)d)iza得到dA005.1TA0.100。

T4、机械臂轨迹规划

机械臂的轨迹规划可以在关节空间也可以在笛卡尔空间中进行，或者说机械臂轨迹规划是指在关节空间或者笛卡尔空间中研究机械臂轨迹生成方法。简言之，机械臂轨迹规划是运动学逆解的实际应用，它描述了机械臂在多维空间中的运动路线 。 在知道末端位姿的前提下，通过运动学逆解得到各个关节在相应时刻的转动量或者平移量，合理的规划指的是规划出的 角位移曲线、角速度曲线以及角加速度曲线，可以有效地减少了机械臂在运动过程中的冲击和振动，使机械臂的工作寿命得以延长。

11

械臂可以分为点到点作业 (Point-to-Point Motion ) 和连续路径作业 (Continuous-Path Motion ) 。点到点的运动指的是机械臂在运动过程中，只要求在某些点上有准确的位置和姿态，相邻的点不做要求。连续运动要求机械臂严格的沿特定的曲线运动。

机械臂的关节角位移变化率比较小，能够有效地防止了机械臂工作时的振动和冲击。机械臂关节角速度和角加速度变化均平顺连续， 从而有效避免了机械部件的磨损，能够保证整个机械臂系统的长期、稳定的运行，满足机械臂的工作要求。 5、robotics工具箱中的相关函数

link 建立一个连杆对象,例如对于本次竞赛的机械臂，根据连杆参数得到

L{1}=link([pi/2 0 0 120 0 0]); L{2}=link([pi/2 0 0 0 0 0]); L{3}=link([-pi/2 0 0 140.8 0 pi]); L{4}=link([-pi/2 71.8 0 0 0 pi/2 ]); L{5}=link([+pi/2 71.8 0 0 0 pi]); L{6}=link([-pi/2 0 0 0 0 pi/2]); L{7}=link([0 0 0 129.6 0 0]);

robot 建立一个机械臂对象

R= robot(L)

noname (7 axis, RRRRRRR) grav = [0.00 0.00 9.81] alpha

A

standard D&H parameters

D R R R R R R

R/P (std) (std) (std) (std) (std) (std) (std)

R

theta 0 0 0 0

1.5708 0 120 1.5708. 0 -1.5708 -1.5708 1.5708 -1.5708 0

0

140.8

71.8 71.8 0 0

129.6

drivebot 用滑块控制的机械臂图形

drivebot(R,ones(1,7)*pi)

plot 机械臂的图形显示

plot(R,[pi/2 pi/2 0 0 0 0 0])

12

fkine 串联机械臂正向运动学计算 tr =fkine (ROBOT, Q)

ROBOT表示机械臂对象，Q机械臂关节坐标值。

tr =fkine (R, [0 0 0 pi/2 0 0 0]) tr =

0.0000 -0.0000 1.0000 129.6000 -0.0000 1.0000 0.0000 -0.0000 -1.0000 -0.0000 0.0000 -20.8000 0 0 0 1.0000

ikine串联机械臂逆向运动学计算 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.0000 0.0000 0.0000 0.7854 -0.0000 -0.7854 0.0000

注意：对于机械臂末端的一个位置与姿态，逆运动学计算不是唯一的，验证

tr=fkine(R,q) tr =

0.0000 -0.0000 1.0000 129.6000 -0.0000 1.0000 0.0000 -0.0000 -1.0000 -0.0000 0.0000 -20.8000 0 0 0 1.0000

transl 计算平移变换 tr= transl (X, Y, Z)

返回机械臂末端坐标X, Y, Z对应的齐次表换矩阵

tr=transl(129.6,0,20.8) tr =

1.0000 0 0 129.6000 0 1.0000 0 0 0 0 1.0000 20.8000 0 0 0 1.0000

[X Y Z]' = transl(T)

返回齐次表换表示中的平移值，作为一个3元素的列向量

xyz=transl(tr)'

14

xyz =

129.6000 0 20.8000

ctraj 计算工作空间中两点T0,T1之间的轨迹 tc= ctraj(T0, T1, N)

tc = ctraj(T0, T1, R)

返回从T0到T1笛卡尔坐标系的轨迹 TC N表示轨迹中的点数。在第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.0000 0.0000 1.0000 0.0000 0.0000 -0.0000 -0.0000 1.0000 108.8000 0 0 0 1.0000 tr1=fkine(R,[pi/4 pi/6 0 pi/3 0 0 0]) tr1 =

0.6124 -0.7071 0.3536 95.6008 0.6124 0.7071 0.3536 95.6008 -0.5000 -0.0000 0.8660 110.3005 0 0 0 1.0000 tc(:,:,1) =

1.0000 0 0 -0.0000 0 1.0000 0 0.0000 0 0 1.0000 108.8000 0 0 0 1.0000 tc(:,:,2) =

0.8976 -0.3822 0.2198 47.8004 0.3571 0.9226 0.1458 47.8004 -0.2585 -0.0523 0.9646 109.5503 0 0 0 1.0000 tc(:,:,3) =

0.6124 -0.7071 0.3536 95.6008 0.6124 0.7071 0.3536 95.6008

15

-0.5000 -0.0000 0.8660 110.3005 0 0 0 1.0000 transl(tc) ans =

-0.0000 0.0000 108.8000 47.8004 47.8004 109.5503 95.6008 95.6008 110.3005

jtraj 计算关节中两点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); figure

subplot(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 on subplot(2,2,4),plot(QDD),grid on %注意：其中有一些曲线重合

jacob0 计算机械臂在基坐标系中 Jacobian 矩阵 J = jacob0(ROBOT, Q)

tr2jac 计算机械臂在基坐标系中 Jacobian 矩阵 J = TR2JAC(T)

16

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 .1]' diff2tr(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.0000 0.0000 -0.0000 0.0039 -0.0000 -0.0039 0.0000 %这是计算工作空间轨迹和求逆解的另外一种方法。但是，如果Jacobian矩阵奇异时 %会失效。如果机械臂的自由度大于6，即是冗余的，采用Jacobian矩阵伪逆计算，或% 对Jacobian矩阵进行奇异值分解。

17

附录 rpy角与euler角 （1）rpy角

rpy角是描述船舶航行时的姿态的一种方法，将船的行驶方向作为Z轴，则绕Z轴旋转称为滚动（Roll）角α，将绕Y轴(与海面平行)方向的旋转称为俯仰（Pitch）角β，取X轴与海面垂直方向，将绕X轴的旋转称为偏转（Yaw）角γ。机械臂末端的定义类似，故习惯上称为rpy角。

描述运动坐标系的规则是：首先使运动坐标系的初始方位与固定坐标系重合，将运动坐标系绕固定坐标系X轴转动γ，再将运动坐标系绕固定坐标系Y轴转动β，最后将运动坐标系绕固定坐标系Z轴转动α。因为三次转动都是相对固定坐标系的，所以相应的旋转矩阵

rpy(,,)rot(z,)rot(y,)rot(x,)cs00将三个矩阵相乘得到

sc0000c00010s0100s100c000100c000s1000sc000 01ccscrpy(,,)s0cssscssscccs0cscssccc000 01它表示绕固定坐标系的三个轴依次旋转得到的旋转矩阵，称为绕固定轴XYZ旋转的rpy方法。下面讨论逆问题：从给定的旋转矩阵得到绕固定轴XYZ旋转的rpy角γβα。令

nxnrpy(,,)ynz0oxoyoz0axayaz000 01上式中有3个未知数，9个方程，其中6个不独立，因此可利用其中3个解出未知数。

22cos()nxny，如果cos(β)不为零，则可以得到

18

22atan2(nz,nxny),atan2(ox,nx),atan2(ay,az)

（2）绕运动系ZYX转动的euler角

描述运动坐标系的规则是：运动坐标系的初始方位与参考坐标系重合，首先将运动坐标系绕Z轴转动α，再将运动坐标系绕Y轴转动β，最后将运动坐标系绕X轴转动γ。这种描述方法中各次的转动都是相对运动系的，而不是相对固定坐标系的。相应的旋转矩阵为

euler(,,)rot(x,)rot(y,)rot(z,)cs00将三个矩阵相乘得到

sc0000c00010s0100s100c000100c000s1000sc000 01ccsceuler(,,)s0cssscssscccs0cscssccc000 01结果与绕固定轴XYZ旋转相同，这是因为绕固定轴旋转的顺序与绕运动轴旋转的顺序相反，且旋转角度对应相等。因此，用ZYX euler角与XYZ rpy角的描述方法是等价的。 另外一种常用的euler角方法是ZYZ方法，首先使运动坐标系与参考坐标系重合，将运动坐标系绕Z轴转动α，再将运动坐标系绕Y轴转动β，最后将运动坐标系绕Z轴转动γ。

euler(,,)rot(z,)rot(y,)rot(z,)cs00将三个矩阵相乘得到

sc0000c00010s0100s100c000cs00010sc000000 1001cccssscccseuler(,,)sc0

cssscsssccss0csssc000 0119

如果sin(β)不为零，ZYZ方式euler角逆解是

22atan2(axay,az),atan2(oz,nZ),atan2(ay,ax)

20