机械臂运动学.

机械臂运动学基础
1、机械臂的运动学模型
机械臂运动学研究的是机械臂运动，而不考虑产生运动的力。

运动学研究机械臂的位置，速度和加速度。

机械臂的运动学的研究涉及到的几何和基于时间的内容，特别是各个关节彼此之间的关系以及随时间变化规律。

典型的机械臂由一些串行连接的关节和连杆组成。

每个关节具有一个自由度，平移或旋转。

对于具有n个关节的机械臂，关节的编号从1到n,有n +1个连杆，编号从0到n。

连杆0是机械臂的基础，一般是固定的，连杆n上带有末端执行器。

关节i连接连杆i和连杆i-1。

一个连杆可以被视为一个刚体, 确定与它相邻的两个关节的坐标轴之间的相对位置。

一个连杆可以用两个参数描述, 连杆长度和连杆扭转, 这两个量定义了与它相关的两个坐标轴在空间的相对位置。

而第一连杆和最后一个连杆的参数没有意义,一般选择为0。

一个关节用两个参数描述, 一是连杆的偏移, 是指从一个连杆到下一个连杆沿的关节轴线的距离。

二是关节角度,指一个关节相对于下一个关节轴的旋转角度。

为了便于描述的每一个关节的位置, 我们在每一个关节设置一个坐标系, 对于一个关节链, Denavit 和Hartenberg 提出了一种用矩阵表示各个关节之间关系的系统方法。

对于转动关节i，规定它的转动平行于坐标轴Z i-i，坐标轴X i-i对准从Z i-i到乙的法线方向，如果Z i-i与
Z i相交，则X i-1取Z i-1 Xz i的方向。

连杆，关节参数概括如下：
连杆长度a i 沿着x i 轴从Z i-i 和Z i 轴之间的距离;
连杆扭转a i从Z i-1轴到Zi轴相对X i-1轴夹角；
连杆偏移d i从坐标系i-1的原点沿着Z i-1轴到X i轴的距离；
关节角度Q i X i-1轴和X i轴之间关于Z i-1轴的夹角。

对于一个转动关节Q i是关节变量，d i是常数。

而移动关节d i是可变的，也是恒定的。

为了
统一，表示为
■ i转动关节
q = 移动关节
运用Denavit-Hartenberg （DH ）方法，可以将相邻的两个坐标系之间的变换关系表示为
一个4x4的齐次变换矩阵
-Cosq-sind cos%si nd si n 闪 a cosq'
sin耳cosQ COS^i一cosd sins a i sin 4
i .
A =
0si n^i cos%d i
000 1 一
上式表示出了坐标系i相对于坐标系i-1的关系。

即
八
0 0i A
T = l i A A
其中T表示坐标系i相对于世界坐标系0的位置与姿态，简称位姿。

2、正向和反向运动学
对于一个n-轴刚性连接的机械臂，正向运动学的解给出的是最后一个连杆坐标系的位置和姿态。

重复利用上式，得到
七=0几人川心代=K（q）
机械臂末端位姿在笛卡尔坐标系中有6个自由度，3个平移，3个旋转。

所以，一般来说具
有6个自由度的机械臂可以使末端实现任意的位姿。

总的机械臂变换In —般简写为T n，对6个自由度的机械臂简写为T6。

对于任意的机械臂, 无论其它有多少个关节，具有什么结构，正向运动学解都是可以得到的。

在机械臂的路径规划中，用到的是反向运动学的解q = K’CT n），它给出了特定的末端位
姿对应的机械臂的关节角度。

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

对于 6 自由度的机器人而言，运动学逆解非常复杂，一般没有封闭解。

只有在某些特殊情况下才可能得到封闭解。

不过，大多数工业机器人都满足封闭解的两个充分条件之一（Pieper 准则）
（1）三个相邻关节轴交于一点
（2）三个相邻关节轴相互平行
如果机械臂多于 6 个关节，称关节为冗余的，这时解是欠定的。

如果对于机械臂某个特别的位姿，解不存在，称这个位姿为奇异位姿。

机械臂的奇异性可能是由于机械臂中某些坐标轴的重合，或位置不能达到引起的。

机械臂的奇异位姿分为两类：
（1）边界奇异位姿，当机械臂的关节全部展开或折起时，使得末端处于操作空间的边界或边界附近，雅克比矩阵奇异，机械臂的运动受到物理结构的约束，这时机械臂的奇异位姿称为边界奇异位姿。

（2）内部奇异位姿，两个或两个以上的关节轴线重合时，机械臂各个关节的运动相互抵消，不产生操作运动，这时机械臂的奇异位姿称为内部奇异位姿。

机械臂运动学逆解的方法可以分为两类：封闭解和数值解、在进行逆解时总是力求得到封闭解。

因为封闭解的计算速度快，效率高，便于实时控制。

而数值解法不具有这些特点。

机械臂运动学的封闭逆解可通过两种途径得到：代数法和几何法。

一般而言，非零连杆参数越多，到达某一目标的方式也越多，即运动学逆解的数目也越多。

在从多重解中选择解时，应根据具体情况，在避免碰撞的前提下通常按“ 最短行程”准则来选择。

同时还应当兼顾“ 多移动小关节，少移动大关节”的原则。

n 个自由度的机械臂的末端位姿由n 个关节变量所决定，这n 个关节变量统称为n 维关节矢量，记为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矩阵在机械臂的奇异位姿上是不可逆的。

在实际应用中，
当机械臂的末端位置接近奇异位置时，Jacobia n矩阵是病态的，可能导致关节速度不能正
确地得到。

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

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

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

雅可比矩阵可以看成是从关节空间到操作空间运动速度的传动比，
同时也可用来表示两空间
之间力的传递关系。

对于冗余自由度机械臂， 其雅可比矩阵是长方矩阵， 因J 满秩且方程个 数少于未知数个数， 所以有无穷多个解， 这时，一般是求其中的最小范数解，或采用加权最 小范数解也就是说使| q T Dq|最小的解，其中D 是对称正定加权矩阵。

此时的解是使机械臂 在能量消耗最小的情况下的解。

这时，逆速度问题便转为：求 q 满足q = J 」x 且使L q T Dq 最小。

实际上等同于求性能
指标L 在约束条件 q
°X 下的极值。

应用
Lagrange 乘子法，以上极值为题的解是
q 二D‘J T (JD 」J T )」X ，当D=I 时，雅可比矩阵是J • = J T (JJ T )_1，称为雅可比矩阵的
伪逆。

阵的特性，根据右图中的几何关系容易求得
^l 1 c 1 l 2 c 12 G 二 cos(m), G 2 二 cos(m 二2) y =l i3 丄晁 S =sin(m),$2 二 sin(弓 二2)
两边微分后写成矩阵形式
即 dx _ -l 1s ^ _l 2 s 12
T2S12
||_dy .
HJ 1
C / l 2C 12 LS.IL d l
F 面通过一个两自由度的平面机械臂说明雅可比矩
I ^x _ "dx ]
讯 ]dy . Jy
01
-X I
简写成dx=Jd 0,式中J 就称为机械臂的雅可比(Jacobian )矩阵，它由函数 x , y 的偏微 分组成，反映了关节微小位移 d 0与机械臂末端微小运动 dx 之间的关系。

将两边同除以dt
dt 得到：dx/dt=Jd
0/dt, 因此机械臂的雅可比矩阵也可以看做是操作空间中的速度与关
节空间中速度的线性变换。

dx/dt 称为末端在操作空间中的广义速度，简称操作速度，
d 0
/dt 为关节速度。

可以看出，雅可比矩阵的每一列表示其它关节不动而某一关节以单位速度 运动产生的末端速度。

l l S i -^2 S 12
由J 二
|U i
C i I 2 C 12
改变会导致J 的变化。

称为机械臂的奇异位姿。

上例机械臂雅可比矩阵的行列式为：
det(J)二l 1l 2 sin (二2),当02=0
或02=180。

时，机械臂的雅可比行列式为 0,矩阵的秩为1，这时机械臂处于奇异位姿。

机 械臂在操作空间的自由度将减少。

如果机械臂的雅可比 J 是满秩的方阵，相应的关节速度即可求出，即 扌二J'x ，上例平面
180 °)时，机械臂接近奇异位姿，相应的关节速度将趋于无穷大。

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

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

例如给定变换
tn t |2 t |3 t |4
t 21 t 22 t 23 t 24 t 31
t 32 t 33 t 34
t
42
t
43
t
44
若它的元素是变量x 的函数，则变换T 的微分为：
—12 S 12 |
可以看出，J 阵的值随末端位置的不同而不同，即
01和02的
I 2 C |2
对于关节空间的某些位姿， 机械臂的雅可比矩阵的秩减少，这些位姿 2R 机械臂的逆雅可比矩阵 J 」二
丄［8
l 1l 2S
^
l 1
C ^l 2
C
12
—l 1S ^ — l 2S 12
_
I 2S2
,显然，当02趋于0 ° (或
以及解决两个不同坐标系之间的微 T 为:
1
F 面讨论机械臂的微分运动， 设机械臂某一连杆相对于基坐标系的位姿为 该连杆相对基坐标系的位姿变为
T+dT ，若这个微运动是相对于基坐标系（静系）进行的（左
乘），总可以用微小的平移和旋转来表示，即
T dT 二Trans （d x ,d y ,d z ）Rot （k,dr ）T
所以有
dT - ||Trans （d x ,d y ,d z ）Rot （k,d" - I 4 4 T
根据齐次变换的对称性， 若微运动是相对某个连杆坐标系 i （动系）进行的（右乘），则T+dT 可
以表示为
T dT 二T Trans(d x ,d y ,d z )Rot(k,^)
所以有
dT =T Trans(d x ,d y ,d z )Rot(k,d" - I 4 4
令厶二Trans(d x ,d y ,d z )Rot(k,dR-l 4 4为微分算子，则相对基系有
dT= A OT ,相对i 系
有dT=T A i o 这里△的下标不同是由于微运动相对不同坐标系进行的。

在机械臂运动学中 微分变换分为微分平移和微分旋转两类。

微分平移变换与一般平移变换一样，其变换矩阵为
0 0 dx
1 0 dy 0 1 dz
0 0 1
-
'ctn 盘21
点t|2 泳 戲
22
盘13
次
Ct23
dT =
ex ex
盘31
戲32
员
33
ex
ex
也1 戲42 戲
43
一 ex ex
泳dx
盘34
ex
T ,经过微运动后
Trans(dx,dy, dz)二
-：
t 14
CX
孜24
盘44
cX 一
由于微分旋转时 0^ 0，所以sin Q^d 0, cos 0^1将它们代入旋转变换通式中得微分旋转
表达式： Rot （k,dr ）二 -1 k z d0 i-kyd 6 0 -k z dr 1 k x dr 0
k y dn -k x d v 1 01 0 于是得到微分算子.：=Trans （d x ,d y ,d z ）Rot （k,dv ）- ■ 0 -k z d6 k y d 日 dx] k z d 日 0 —kxd^ dy _k y d 日 k x d^ 0 dz 0
0 0 0 也=
F 面分别说明。

I 4 4，即 微分旋转与有限旋转相比，有一些特殊的性质, (1 )微分旋转的无序性，当 0T 0时，有 sin 0f 0, cos 若令8x=d 0x , S y=d
S z=d 0z ,则绕三个坐标轴的微分旋转矩阵分别为 Rot （x,、.x ） a ■1 0 0 卫 0 1 、.x 0
0 -、x 1 0
01 0
0 1 略去2次项，得到
-Rot （x,、x ）Rot （ y,、y ）
Rot （y,、y ）Rot （x,、x ）=
0 0 1 一 ' 0 0 -
-1 0 b
01
1
-1
0 6y
01
y 1
丿X 0
0 1
-6 y
1 0
—d y 6x
1 0
0 0
0 1 _ 1 _ 0
1
一
- 1
y
01
-
• 1 0 6y
01
0 1 -5x 0
0 1 一
0 —0 y 6x 1 0
—0 y 6x 1 0
- 0
0 1
一
1
_ 0
1
一
Rot （y,、.y ） a 结论:
微分旋转其结果与转动次序无关, 0 1 0 01
0 0 0 0 1 、y 0 1 -、z 1
■1 0 -5y ■1 6z 0 Rot （Z,、.Z ）= 01
0 0
1
两者结果相同，可见这里左乘与右乘等效。

这是与
有限转动(一般旋转)的一个重要区别。

(2 )微分旋转的可加性，考虑两个微分旋转复合后的效果
1 --:：'z 、y
0 d z 1 -d x 0 Rot(x, 6x) Rot( y, 6y)Rot(z, 6z)=
-b y d x 1
.0
0 1一
连续转动的结果为:
上式表明：任意两个微分旋转的结果为绕每个轴转动的元素的代数和, 由等效转轴和等效转角与 Rot( x r x) Rot( yr y)Rot(z 「z)等效，有
Rot(k, d 力二 Rot(x,、x)Rot(y,、y)Rot(z,、z) 1 -k z d v k z d 日
1
-k y d 日 k x d & -0
所以有 kxd 0= 8x , kyd 0= 8y ,
丿z
H y
d x I
| 6
-氏
d y
」y 我x 0 d z
.0
0 0 0 一
可见，微分变换由两个部分组成 S 微分转动矢量，d 微分平移矢量
合称为微分运动矢量，
- 0
1 0 0 1 0 10] 5
例：已知一个坐标系 A =
0 1 0 0
- 0 0 0 1
一
可表示为 D = (d x , d y ,d
z r x^ y r z ) 微分旋转矢量S =[0 0.1
0 ]，求微分变换dA 。

，相对固定系的微分平移矢量 d= [1 0 0.5],
若 Rot ( 8x, S y , 8z) 和 Rot ( 3x '
Sy ' , S z '） 表示两个不同的微分旋转,
则两次
Rot(、x,、y,、z) RotC x ：、y',、z')=
1
z :. z'
_(、y :. y')
-（：Z 丄眉
z'）
1
即微分旋转是可加的。

k y d 日 01
- -1 」z
5y 01
-k x d 日 0
1
-5 x 0 1 0
-d y
1 0 0
1 一
1 0 0
1
一
kzd 0= S z ,将它们代入△得
z -y 」z
J dx i
一 0
0 0.1 1 1 0 」x d y
0 0
眞 0 d z
-0.1 0 0 0.5 0 0 0 _ 1 0 0 0 0
一
■0
1 10 0
'0 0 0.1
0 0 0 二 dA = A A = -0.1 0 0 0 0 0
1 0 0
0 1 0
0.5 0 1 0 _0 0 0.1 0 0 0
0 0 -0.1 0
F 面讨论两坐标系之间的微分关系，设第一个坐标系为 i 系，第二个坐标系为 1 1 0 -0.5
0 一
j 系不失一般
性，假定j 系就是固定的0系。

O x a x 0T n z 「° P x P y
P z 1
-0 —0 z dx] '0 —° z dx ] d z 0 -5x dy ，也i — 岀 0 一o x dy
-5 y 6x 0 dz
」Y i &X i 0 dz -0 0 0 0 一 1 1 0 0 0 0 一 = 0TA,也i = 打， 整理得到
因为礼 a y a z 0 o y O z 0 所以厶00
T d ix = nkC P) d) * = n 」 d iy =0L((「 P) d) Jy =o 」
d iz = aL((「 P )
d)、：iz = a 」： dx i I |dy i d 乙 k i
母j O x a x 「° n y n z (P n)x (P n)y O y O z (P o)x (P 0)y
a y a z (P a)x (P a)y
0 0 n x n y
0 0 O x O y
0 0 a x a y
a n)J _dx 0 一 (P O)z (P a)z n z
O z
a z 对于任何三维矢量 p=[p x , P y , P z ]，其反对称矩阵S(P)定义为: 0 -P z P y S(P )
= I P z 0 "P x '-P y P x 0 一
dy 0 dz 0
卩 1=；0R T
—0R
T
S (0P O )1
o
0R T
一
；B vL
；R -BRS(A P B °)1
「
V ] ；A V L£R -：RS(B P A。

)
」B =」八
.1 A ，」A =」八
」B 「J -0
A R
」L j
B R
八 J
解：将d= [1 0 0.5]和 冃0
0.1 0 ]代入
d ix = nkC P) d) * = n 」 d iy =0kC ，P) d)=o 」 d iz =ak(、： p) d) 、：iz=a 上
得到 d A -〔0 0 5.11 :• A - 0.1 0 0丨 o
4、机械臂轨迹规划
机械臂的轨迹规划可以在关节空间也可以在笛卡尔空间中进行， 或者说机械臂轨迹规划是指 在关节空间或者笛卡尔空间中研究机械臂轨迹生成方法。

简言之，机械臂轨迹规划是运动学 逆解的实际应用，它描述了机械臂在多维空间中的运动路线 o 在知道末端位姿的前提下， 通过运动学逆解得到各个关节在相应时刻的转动量或者平移量， 合理的规划指的是规划出的 角位移曲线、角速度曲线以及角加速度曲线， 可以有效地减少了机械臂在运动过程中的冲击 和振动，使机械臂的工作寿命得以延
n x O x a x
0 _ i R = n y
O y
a y
O z a z 一
上式简写成
- 0 0 1 10]
1 0 0 5
例：已知一个坐标系 A =
0 1 0 0 ，相对固定系的微分平移矢
量
- 0 0 0 1 _
d= [1 0 0.5],
微分旋转矢量S =[0 类似地，任意两坐标系 ｛A ｝和｛B ｝之间广义速度的坐标变换为:
0.1
0 ]，求A 系中等价的微分平移矢量
d A 和微分旋转矢量
S A 。

长。

械臂可以分为点到点作业(Point-to-Point Motion ) 和连续路径作业(Continuous-Path
Motion ) 。

点到点的运动指的是机械臂在运动过程中，只要求在某些点上有准确的位置和姿态，相邻的点不做要求。

连续运动要求机械臂严格的沿特定的曲线运动。

机械臂的关节角位移变化率比较小，能够有效地防止了机械臂工作时的振动和冲击。

机械臂关节角速度和角加速度变化均平顺连续，从而有效避免了机械部件的磨损，能够保证整个机械臂系统的长期、稳定的运行，满足机械臂的工作要求。

5、robotics 工具箱中的相关函数
link 建立一个连杆对象,例如对于本次竞赛的机械臂，根据连杆参数得到
L{1}=link([pi/2001200 0]);
L{2}=link([pi/20000 0]);
L{3}=link([-pi/200140.80 pi]);
L{4}=link([-pi/271.8000 pi/2 ]);
L{5}=link([+pi/271.8000 pi]);
L{6}=link([-pi/20000 pi/2]);
L{7}=link([000129.60 0]);
robot 建立一个机械臂对象
R= robot(L)
noname (7 axis, RRRRRRR)
grav = [0.00 0.00 9.81]standard D&H parameters
alpha A theta D R/P
1.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 1.0000129.6000
-0.0000 1.00000.0000-0.0000
-1.0000-0.00000.0000-20.8000
000 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)
输入变量
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.0000129.6000
-0.0000 1.00000.0000-0.0000
-1.0000-0.00000.0000-20.8000
000 1.0000
transl 计算平移变换
tr= transl (X, Y, Z)
返回机械臂末端坐标X, Y, Z 对应的齐次表换矩阵
tr=transl(129.6,0,20.8)
tr =
1.000000129.6000
0 1.000000
00 1.000020.8000
000 1.0000
[X Y Z]' = transl(T)
返回齐次表换表示中的平移值，作为一个 3 元素的列向量xyz=transl(tr)'
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.00000.00000.0000
-0.0000-0.0000 1.0000108.8000
000 1.0000
tr1=fkine(R,[pi/4 pi/6 0 pi/3 0 0 0])
tr1 =
0.6124-0.70710.353695.6008
0.61240.70710.353695.6008
-0.5000-0.00000.8660110.3005
000 1.0000
tc(:,:,1) =
1.000000-0.0000
0 1.000000.0000
00 1.0000108.8000
000 1.0000
tc(:,:,2) =
0.8976-0.38220.219847.8004
0.35710.92260.145847.8004
-0.2585-0.05230.9646109.5503
000 1.0000
tc(:,:,3) =
0.6124-0.70710.353695.6008
0.61240.70710.353695.6008
-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)
QD0, QD1 轨迹中的点数是N，或者是一个时间向量T。

插值使用7次多项式，边界速度由
指定，缺省时边界速度和加速度为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)
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 矩阵进行奇异值分解。

附录 rpy 角与 euler 角 （1 ） rpy 角
rpy 角是描述船舶航行时的姿态的一种方法，
将船的行驶方向作为 Z 轴，则绕Z 轴旋转称为
滚动（Roll ）角冬将绕丫轴（与海面平行）方向的旋转称为俯仰（Pitch ）角取X 轴与海 面垂直方向，将绕 X 轴的旋转称为偏转（Yaw ）角丫。

机械臂末端的定义类似，故习惯上称 为rpy 角。

描述运动坐标系的规则是：首先使运动坐标系的初始方位与固定坐标系重合，
将运动坐标系
绕固定坐标系 X 轴转动Y ,再将运动坐标系绕固定坐标系 丫轴转动3,最后将运动坐标系绕
固定坐标系Z 轴转动a 。

因为三次转动都是相对固定坐标系的，所以相应的旋转矩阵
rpy （,：,：）二rot （ z, ： ）rot （y, ： ）rot （x,）
O x a x 01
.V R
\
n y
O y a
y 0
rpy （「H ）=
c
n z o z a z 0 .0
1 一
上式中有3个未知数，9个方程，其中6个不独立，因此可利用其中 3个解出未知数。

cos （ ■）= n ： - n y ，如果cos （ 3）不为零，则可以得到
「g
-sa 0 O ［「c B 0 S0 0V1 0 0
0〕
s
g 0 0 0 1 0 0 0
& -sY 0 —0
0 1 0 -sP 0 C0 0 0 S? c Y
0 「0
―。

0 0
1
一
将三个矩阵相乘得到
「c^cP casPs? 一尹 c
gsP c 01
sPs? -EC?
少 sPc? 0 rpy （j, * d
创s?
cPc? 0
-0
1 一 它表示绕固定坐标系的三个轴依次旋转得到的旋转矩阵，称为绕固定轴
XYZ 旋转的rpy 方
法。

下面讨论逆问题：从给定的旋转矩阵得到绕固定轴
XYZ 旋转的rpy 角丫3 a 令
P =atan2( -n z ，J n ； +n ：),口 =atan2(o x , n x ), Y =atan2(a y , a z )
(2 )绕运动系ZYX 转动的euler 角
描述运动坐标系的规则是： 运动坐标系的初始方位与参考坐标系重合， 首先将运动坐标系绕
Z 轴转动a,再将运动坐标系绕
Y 轴转动3,最后将运动坐标系绕
X 轴转动丫。

这种描述方法
中各次的转动都是相对运动系的，而不是相对固定坐标系的。

相应的旋转矩阵为
euler （： , :, ） = rot （x^ ）rot （y, ：）rot （z,）
Pca -sa 0 0「cB
0 sP 0］
「
0 0 01
S- d
0 0
0 1
0 0
0 & -s / 0 0
0 1 0 -sP 0 cP 0 0 s? c 0 「°
0 1 一
-0
0 0 1一
0 0 0 1
一
将三个矩阵相乘得到
euler （-知：，）
c ：
c ： S ^c P I -s P
g S - S - S --^ c
—
c =c cP s?
01
结果与绕固定轴XYZ 旋转相同, 且旋转角度对应相等。

因此，用 另外一种常用的euler 角方法是
这是因为绕固定轴旋转的顺序与绕运动轴旋转的顺序相反, ZYX euler 角与XYZ rpy 角的描述方法是等价的。

ZYZ 方法，首先使运动坐标系与参考坐标系重合，将运动
-0 -St 0 0「
c B 0 sP 0］「
-s 0 01 s
0 0 0 1 0 0 s? c 0 0 0 0 1 0 1-s P 0 cP 0 0 0 1 0 「° 0 0 1 一 •0 0 0 1一
0 0 0 1一
坐标系绕Z 轴转动a,再将运动坐标系绕 Y 轴转动3,最后将运动坐标系绕 Z 轴转动丫。

euler （： , ：, ） =rot （z, ：
）rot （y, ：
）rot （z,） 将三个矩阵相乘得到
C -：心 C -S -^S
euler （:,：,）=
_C -：：S ：S _S -：i C -s -：：s E“s ' c -^c 01 0 0 1
如果sin( 3)不为零，ZYZ方式euler角逆解是
2
:=atan2(. a：
+ a y, a z),a =atan2(0z,n Z)7 =atan2(a y, _a x)
21。