机械臂运动学

机械臂运动学基础
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轴的夹角。

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

而移动关节di 是可变的，θi 是恒定的。

为了统一，表示为
i i i
q d θ⎧=⎨⎩转动关节移动关节
运用Denavit-Hartenberg （DH ）方法，可以将相邻的两个坐标系之间的变换关系表示为一个
4x4的齐次变换矩阵
1
cos sin cos sin sin cos sin cos cos cos sin sin 0sin cos 0
1i i i i i i i i i i
i i
i i i i i
i
i a a A d θθαθαθθ
θαθαθαα--⎡⎤⎢⎥-⎢
⎥=⎢⎥
⎢⎥⎣⎦
上式表示出了坐标系i 相对于坐标系i-1的关系。

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

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

重复利用上式,得到
011
12
()n n n T A A A K q -==
机械臂末端位姿在笛卡尔坐标系中有6个自由度，3个平移，3个旋转。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

而数值法不具有些特点。

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

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

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

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

n 个自由度的机械臂的末端位姿由n 个关节变量所决定，这n 个关节变量统称为n 维关节矢量，记为q 所有的关节矢量构成的空间称为关节空间。

机械臂末端的位姿x 是在直角坐标空间中描述的，因此，称该空间为操作空间或作业定向空间。

机器人各关节驱动器的位置统称为驱动矢量s ，由这些矢量组成的空间称为驱动空间。

3、Jacobian 矩阵
机械臂的Jacobian 矩阵表示机械臂的操作空间与关节空间之间速度的线性映射关系，对于一个n-轴的机械臂，笛卡尔坐标系中的基座速度是
0n x J q θ=
末端速度是
n
n t t n x J q θ=
其中x 是6个元素的向量。

对于6个关节机械臂Jacobian 矩阵是方阵，如果它是可逆的，则可以由机械臂的末端速度求出各个关节的速度。

Jacobian 矩阵在运动的奇异点的位置是不可逆的。

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

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

首先来看一个两自由度的平面机械手，容易求得
1121211121211212111212c c cos(),cos()s s sin(),sin()
x l l c c y l l s s θθθθθθ=+==+⎧⎨
=+==+⎩
两边微分后写成矩阵形式
121212x x d dx d dy y
y θθθθθθ∂∂⎡⎤
⎢⎥∂∂⎡⎤
⎡⎤⎢⎥=⎢⎥⎢⎥∂∂⎢⎥⎣⎦
⎣⎦⎢⎥∂∂⎣⎦
11212
2121112122122s s s c c c l l l d dx l l l d dy θθ---⎡⎤⎡⎤
⎡⎤=⎢⎥⎢⎥⎢⎥+⎣⎦⎣⎦⎣⎦
，简写成 dx=Jd θ，式中J 就称为机械手的雅可比
（Jacobian ）矩阵，它由函数x ，y 的偏微分组成，反映了关节微小位移d θ与机械臂末端微小运动dx 之间的关系。

dx/dt=Jd θ/dt,
因此机械手的雅可比矩阵定义为它的操作空间速度
与关节空间速度的线性变换。

dx/dt 称为手爪在操作空间中的广义速度，简称操作速度， d θ/dt 为关节速度。

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

由11212
21211212212s s s c c c l l l J l l l ---⎡⎤
=⎢
⎥+⎣⎦
可以看出，J 阵的值随末端位置的不同而不同，即θ1和θ2的改驱动空间 关节空间 工作空间
正向运动学
运动学逆解
变会导致J 的变化。

对于关节空间的某些形位，机械手的雅可比矩阵的秩减少，这些形位称为操作臂（机械手）的奇异形位。

上例机械手雅可比矩阵的行列式为：
122det()cos()J l l θ=，当θ2=0°或θ2=180°时，机械手的雅可比行列式为0，矩阵的秩为1，
因此处于奇异状态。

在奇异形位时，机械手在操作空间的自由度将减少。

如果机械手的雅可比J 是满秩的方阵，相应的关节速度即可求出，即1
J x θ-=，上例平面2R 机械手的逆雅可比矩阵2122121
1121211212122
1l c l s J
l c l c l s l s l l s -⎡⎤
=
⎢⎥----⎣⎦
，显然，当θ2趋于0°（或180°）时，机械手接近奇异形位，相应的关节速度将趋于无穷大。

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

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

例如给定变换T 为：
1112131421
2223243132333441
424344t t t t t t t t T t t t t t t t t ⎡⎤⎢⎥⎢⎥=⎢⎥
⎢⎥⎣⎦ 若它的元素是变量x 的函数，则变换T 的微分为:
13111214232122243132333443414244t t t t x x x x t t t t x x x x dT dx t t t t x x x x t t t t x
x
x
x ∂∂∂∂⎡⎤⎢⎥∂∂∂∂⎢⎥∂∂∂∂⎢⎥⎢⎥∂∂∂∂=⎢
⎥∂∂∂∂⎢⎥⎢⎥∂∂∂∂⎢⎥
∂∂∂∂⎢⎥∂∂∂∂⎣⎦
下面讨论机械臂的微分运动，设机器人某一杆件相对于基坐标系的位姿为T ，经过微运动后该杆件相对基坐标系的位姿变为T+dT ，若这个微运动是相对于基坐标系（静系）进行的(左乘)，总可以用微小的平移和旋转来表示，即
(,,)(,)x y z T dT Trans d d d Rot k d T θ+=
所以有
44(,,)(,)x y z dT Trans d d d Rot k d I T θ⨯⎡⎤=-⎣⎦
根据齐次变换的对称性，若微运动是相对某个杆件坐标系i （动系）进行的(右乘)，则T+dT
可以表示为
(,,)(,)x y z T dT T Trans d d d Rot k d θ+=⋅
所以有
44(,,)(,)x y z dT T Trans d d d Rot k d I θ⨯⎡⎤=-⎣⎦
令44(,,)(,)x y z Trans d d d Rot k d I θ⨯∆=-为微分算子，则相对基系有dT=Δ0T ，相对i 系有dT=T Δi 。

这里Δ的下标不同是由于微运动相对不同坐标系进行的。

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

微分平移变换与一般平移变换一样，其变换矩阵为:
1000
10(,,)0010001dx dy Trans dx dy dz dz ⎡⎤⎢⎥⎢
⎥=⎢⎥
⎢⎥
⎣⎦
由于微分旋转θ→0 ，所以sinθ→dθ，cosθ→1将它们代入旋转变换通式中得微分旋转表达式:
1010(,)100
1z y z
x y x k d k d k d k d Rot k d k d k d θθθ
θθθθ-⎡⎤⎢⎥-⎢⎥=⎢⎥-⎢⎥⎣⎦
于是得微分算子44(,,)(,)x y z Trans d d d Rot k d I θ⨯∆=-，即
0000
0z y z
x y x k d k d dx k d k d dy k d k d dz θθθθθθ-⎡⎤⎢⎥-⎢⎥∆=⎢⎥-⎢⎥⎣⎦
微分旋转的无序性，当θ→0 时，有sinθ→dθ，cosθ→1．若令δx=dθx ，δy=dθy ，δz=dθz ，则绕三个坐标轴的微分旋转矩阵分别为
10
00010(,)01000
01x
Rot x x x δδδ⎡⎤⎢⎥-⎢
⎥=⎢⎥
⎢
⎥
⎣⎦1
000100(,)0100
001y Rot y y y δδδ⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥⎣⎦ 100100(,)00
100001z
z Rot z z δδδ-⎡⎤
⎢⎥⎢⎥=⎢⎥
⎢
⎥⎣⎦
略去2次项，得到
1
0010010010(,)(,)101000010001y y
x y x
x Rot x x Rot y y y x y x δδδδδδδδδδδδ⎡⎤⎡⎤
⎢⎥⎢⎥--⎢
⎥⎢
⎥∴==⎢⎥⎢⎥--⎢
⎥⎢⎥⎣⎦⎣⎦ 1
010*******(,)(,)10100
001000
1x y y y
x
x Rot y y Rot x x y x y x δδδδδδδδδδδδ⎡⎤⎡⎤⎢⎥⎢⎥--⎢
⎥⎢
⎥==⎢⎥⎢⎥--⎢
⎥⎢⎥⎣⎦⎣⎦
两者结果相同，可见这里左乘与右乘等效。

结论：微分旋转其结果与转动次序无关，这是与
有限转动（一般旋转）的一个重要区别。

同理可得
1
010(,)(,)(,)100001z y z x
Rot x x Rot y y Rot z z y x δδδδδδδδδ-⎡⎤
⎢⎥-⎢
⎥=⎢⎥-⎢
⎥⎣⎦
若Rot （δx ，δy ，δz ） 和Rot （δx’，δy’，δz’） 表示两个不同的微分旋转，则两次连续转动的结果为：
1(')'0'1(')
0(,,)(',',')(')'100001z z y y z z x x Rot x y z Rot x y z y y x x δδδδδδδδδδδδδδδδδδ-++⎡
⎤
⎢⎥+-+⎢
⎥=⎢⎥-++⎢
⎥⎣
⎦
上式表明：任意两个微分旋转的结果为绕每个轴转动的元素的代数和，即微分旋转是可加的。

由等效转轴和等效转角与(,)(,)(,)Rot x x Rot y y Rot z z δδδ等效，有
(,)(,)(,)(,)Rot k d Rot x x Rot y y Rot z z θδδδ=
1
010*********
10001z y z
x y x k d k d z y
k d k d z x k d k d y x θθδδθθδδθθδδ--⎡⎤⎡⎤
⎢⎥⎢⎥--⎢⎥⎢
⎥=⎢⎥⎢⎥--⎢⎥⎢⎥⎣⎦⎣⎦
所以有kxdθ=δx ， kydθ=δy ， kzdθ=δz ，将它们代入Δ得
00
00
0z y x z
x
y y x
z d d d δδδδδδ-⎡⎤⎢⎥-⎢⎥∆=⎢⎥-⎢⎥⎣⎦
可见，微分变换由两个部分组成δ微分转动矢量，d 微分平移矢量,
合称为微分运动矢量，
可表示为(,,,,,)T
x y z x y z D d d d δδδ=
例：已知一个坐标系001101
00501000
001A ⎡⎤⎢⎥⎢
⎥=⎢⎥
⎢⎥
⎣⎦ ，相对固定系的微分平移矢量d=[1 0 0.5]，微分旋转矢量δ=[0 0.1 0 ]，求微分变换dA 。

00
00.110
000000.1000.50
00
0z y x z
x
y y x
z d d d δδδδδδ-⎡⎤⎡⎤⎢⎥⎢⎥-⎢⎥⎢⎥∆==⎢⎥⎢⎥--⎢⎥⎢⎥⎣⎦⎣⎦
000.110011000.1010
000100500000.1000.50100000.10.50
000010000dA A ⎡⎤⎡⎤⎡⎤
⎢⎥⎢⎥⎢⎥⎢
⎥⎢⎥⎢⎥∴=∆==⎢⎥⎢⎥⎢⎥
---⎢⎥⎢⎥⎢⎥
⎣⎦⎣⎦⎣⎦
下面讨论两坐标系之间的微分关系，设第一个坐标系为i 系，第二个坐标系为j 系不失一般
性，假定j 系就是固定的0系。

00
1x x x x y y y y i z
z z z n o a p n o a p T n o a p ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦ 因为 00000000z y dx z x dy y x dz δδδδδδ-⎡⎤⎢⎥-⎢
⎥∆=⎢⎥-⎢⎥⎣⎦，0
00
0i i i i i
i i i i
i z y dx z x dy y x dz δδδδδδ-⎡⎤⎢⎥-⎢⎥∆=⎢⎥-⎢⎥⎣⎦
所以00
0i i i T T ∆=∆，0100i i i T T -∆=∆，整理得到
(())(())(())ix ix iy iy iz iz d n p d n d o p d o d a p d a δδδδδδδδδ
=⨯+==⨯+==⨯+= 000000()()()()()()()()()0000000
x y
z x y z i x y z x y z i x y z x
y
z i x y z i x y z i x
y
z i n n n p n p n p n dx dx o o o p o p o p o dy dy a a a p a p a p a dz dz n n n x x o o o y y a a a z z δδδδδδ⨯⨯⨯⎡⎤⎡⎤⎡⎤
⎢⎥⎢⎥⎢⎥⨯⨯⨯⎢⎥⎢⎥⎢⎥
⎢⎥⨯⨯⨯⎢⎥⎢⎥
=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥
⎢⎥⎢⎥⎢⎥⎢⎥⎢
⎥⎢⎥⎣⎦⎣⎦⎣⎦ 对于任何三维矢量 p =[p x , p y , p z ],其反对称矩阵s(p) 定义为：
0()00z y z
x y x
p p s p p p p p ⎡⎤-⎢⎥=-⎢⎥⎢⎥-⎣
⎦
记
0x
x x i y
y y z
z
z n o a R n o a n o a ⎡⎤
⎢⎥=⎢⎥⎢⎥⎣⎦
上式简写成
000
00()0T
T i i i i T
i i d R R s p R δ⎡⎤-⎡⎤=⎢⎥⎢⎥⎣⎦⎣⎦
类似地，任意两坐标系{A}和{B}之间广义速度的坐标变换为：
()0B B A B A A A BO B B A A R RS P V V R ωω⎡⎤⎡⎤⎡⎤-=⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦，()0A A
B A B B B AO A
A B B R RS P V V R ωω⎡⎤⎡⎤⎡⎤
-=⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦
⎣⎦ 例：已知一个坐标系0
01101
00501000001A ⎡⎤⎢⎥⎢
⎥=⎢⎥
⎢⎥
⎣⎦
，相对固定系的微分平移矢量d=[1 0 0.5]，微分旋转矢量δ=[0 0.1 0 ]，求A 系中等价的微分平移矢量d A 和微分旋转矢量δA 。

解：将d=[1 0 0.5] 和δ=[0 0.1 0 ]代入
(())(())(())ix ix iy iy iz iz d n p d n d o p d o d a p d a δδδδδδδδδ
=⨯+==⨯+==⨯+= 得到[]
[]00 5.10.100T
T
A A d δ==。

3、robotics工具箱中的运动学求解函数使用
函数fkine的调用格式
tr =fkine (ROBOT, Q)
ROBOT表示机械臂对象，Q机械臂关节坐标值。

函数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矩阵的伪逆，这是数值求解方法，对于特定机械臂逆运动学解(如果可能)应该尽量使用解析解。

但是这种方法可以得到奇异点上的解，零空间中的关节角度可以任取。

函数transl的调用格式
tr= transl (X, Y, Z)
tr= transl( [X Y Z] )
返回机械臂末端坐标X, Y, Z对应的齐次表换表示
[X Y Z]' = transl(T)
返回齐次表换表示中的平移值，作为一个3元素的列向量
[X Y Z] = TRANSL(TG)
从笛卡尔坐标系的轨迹TG中得到X, Y 和Z 的值。

函数ctraj的调用格式
tc= ctraj(T0, T1, N)
tc = ctraj(T0, T1, R)
返回从T0到T1笛卡尔坐标系的轨迹TC N表示轨迹中的点数。

在第1中情况下，轨迹中的点在T0到T1中等距离分配。

在第2中情况下，向量R给出轨迹中每个点的距离，R中的元素取值为[0 1]。

一个轨迹是4x4xN 矩阵，最后一个下标表示点索引。

函数trinterp的调用格式
tr = trinterp (T0, T1, R)
返回从T0到T1齐次变换的插值矩阵。

R中的元素取值为[0 1]。

旋转变换使用4元素球坐标线性插值。

puma560
qr=[0 pi/2 -pi/2 0 0 0]
x=fkine(p560,qr)
xyz=transl(x)’
q=ikine(p560,x)
q=[-0.0000 1.5238 -1.4768 -0.0000 -0.0470 0.0000]%注意逆解不唯一
transl(fkine(p560,q))'%验证末端位置
ans= 0.0203 -0.1500 0.8636
x0=[0.3 0.2 0.5]
x1=[0.3 0.2 -0.5]
tc=ctraj(transl(x0),transl(x1),5)
qc=ikine(p560,tc)
qc=mod(qc,2*pi)
%验证末端位置
transl(fkine(p560,qc(1,:)))'
transl(fkine(p560,qc(2,:)))'
T0 = transl([0 0 0]); T1 = transl([-1 2 1]);
t= [0:0.056:10];
r = jtraj(0, 1, t);
TC = ctraj(T0, T1, r);
plot(t, transl(TC));
tc=ctraj(transl(x0),transl(x1),[0 0.5 1])
qc=ikine(p560,tc)
qc=mod(qc,2*pi)
%验证末端位置
transl(fkine(p560,qc(1,:)))'
transl(fkine(p560,qc(2,:)))'
transl(fkine(p560,qc(3,:)))'
%Jacobian 矩阵
q0=[0.7854 0.7854 0.7854 0.78540.7854 0.7854]
q1=[0.7854 0.7854 0.7854 0.78540.7854 0.7854]+0.01
r0=fkine(p560,q0)
r1=fkine(p560,q1)
%Jn=jacobn(p560,q0)
J0= jacob0(p560,q0)
x_dot=[0.01 0.01 0.05 0.03 0.01 0.02]'
q_dot=inv(J0)*x_dot
The manipulator Jacobian matrix Jn maps differential changes in joint space to differential Cartesian motion of the end-effector (end-effector coords).
The manipulator Jacobian matrix J0 maps differential changes in joint space to differential Cartesian motion (world coord frame) of the end-effector.。