第六章 机器人运动学及动力学
6.1 引论
到现在为止我们对操作机的研究集中在仅考虑动力学上。我们研究了静力位置、静力和速度，但我们从未考虑过产生运动所需的力。本章中我们考虑操作机的运动方程式——由于促动器所施加的扭矩或作用在机械手上的外力所产生的操作机的运动之情况。
机构动力学是一个已经写出很多专著的领域。的确，人们可以花费以年计的时间来研究这个领域。显然，我们不可能包括它所应有的完整的内容。但是，某种动力学问题的方程式似乎特别适合于操作机的应用。特别是，那种能利用操作机的串联链性质的方法是我们研究的天然候选者。
有两个与操作机动力学有关的问题我们打算去解决。向前的动力学问题是计算在施加一
组关节扭矩时机构将怎样运动。也就是，已知扭矩矢量τ，计算产生的操作机的运动Θ、Θ 和Θ
。这个对操作机仿真有用，在逆运动学问题中，我们已知轨迹点Θ、Θ 和Θ ，我们欲求出所需要的关节扭矩矢量τ。这种形式的动力学对操作机的控制问题有用。
6.2 刚体的加速度
现在我们把对刚体运动的分析推广到加速度的情况。在任一瞬时，线速度矢量和角速度矢量的导数分别称为线加速度和角加速度。即
B B Q Q B
B Q Q
0V ()V ()d V V lim dt t t t t t
?→+?-==? （6-1） 和
A A
Q Q A
A Q Q
0()()d lim dt t t t t t
?→Ω+?-ΩΩ=Ω=? （6-2） 正如速度的情况一样，当求导的参坐标架被理解为某个宇宙标架{}U 时我们将用下面的记号
U A AORG V V = （6-3）
和
U A A
ω=Ω （6-4）
6.2.1 线加速度
我们从描述当原点重合时从坐标架{}A 看到的矢量B
Q 的速度
A
A B A A Q B Q B B V V B
R R Q =+Ω? （6-5）
这个方程的左手边描述A
Q 如何随时间而变化。所以，因为原点是重合的，我们可以重写（6-5）为
A A
B A A B B Q B B d ()V dt
B B R Q R R Q =+Ω? （6-6） 这种形式的方程式当推导对应的加速度方程时特别有用。
通过对（6-5）求导，我们可以推出当{}A 与{}B 的原点重合时从{}A 中看到的B
Q 的
加速度表达式
A
A B A A A A Q B Q B B B B
d d V (V )()dt dt
B B R R Q R Q =+Ω?+Ω? （6-7） 现在用（6-6）两次── 一次对第一项，一次对最后一项。（6-7）式的右侧成为：
A B A A A A B
Q B B Q B B
A
A
A
A B B
Q B B
V ()
+Ω?+Ω?+Ω?+Ω? B B B
B
R R V R Q R V R Q （6-8）
把相同两项合起来
A B A A A A B
Q B B Q B B
A
A
A
B B B
V 2 ()
+Ω?+Ω?+Ω?Ω? B B B
R R V R Q R Q （6-9）
最后，为了推广到原点不重合的情况，我们加上一项给出{}B 的原点的线加速度的项，得到下面的最后的一般公式
A B A A A A BORG B Q B B Q B B A
A
A
B B B
V 2 ()
++Ω?+Ω?+Ω?Ω? A
B B B
V R R V R Q
R Q （6-10）
对于我们将在本章上考虑的情况，我们总是有B
Q 为不变，或
B
Q Q
V 0== B V （6-11） 所以，（6-10）简化为
A
A A A A A Q BORG
B B B B B V ()=+Ω?Ω?+Ω? A B B V R Q R Q （6-12）
我们将用这一结果来计算操作机杆件的线加速度。
6.2.2 角加速度
考虑{}B 以A B Ω相对于{}A 转动的情况，而{}C 以B
C Ω相对于{}B 转动。为了计算
A
C Ω我们把矢量在坐标架{}A 中相加
A
A A B
C B B C Ω=Ω+ΩR （6-13）
求导后我们得到
A
A A
B
C B B C
d dt
Ω=Ω+Ω R （6-14） 现在，把（6-6）用到（6-14）的末项中去，我们得到
A
A A
B A A B
C B B C B B C
Ω=Ω+Ω+Ω?Ω R R （6-15） 我们将把这个结果用来求操作机杆件的角加速度。
6.3 质量分布
在单自由系统中，我们常常谈到刚体的质量。在绕一根简单轴转动的运动情况下，惯性
矩这一术语是大家所熟悉的。对一个在三维空间中自由运动的刚体，有无穷多个可能的转动轴。在绕一任意轴转动的情况下，我们需要有一个描绘刚体质量分布的方法。这里我们介绍为这个目的的
惯性张量，它可以看作为物体标量惯性矩的广义化。
我们现在将定义一组量，它给出刚体关于参考坐
标架的质量分布的信息。图6-1示出一个带着固结标架的刚体。虽然惯性张量可以相对于任何标架来定义，我们将最经常地考虑定义在与刚体相固结的标架中的惯性张量。在重要的地方我们将用一个前上标来指明给定惯性张量的参考标架。相对于标架{}A 的惯性张量是表示成下面的33?矩阵形式
x x
x y x z A
x y
y y y z x z y z
z z ????
=?????
?
I I I I I I I I I I （6-16） 式中，标量元素由下列公式给出
22x x 22y y 22z z x y x z y z ()()()ρν
ρν
ρν
ρν
ρν
ρν
=+=+=+===??????????????????V
V
V
V
V
V
I y z d I x z d I x y d I xy d I xz d I yz d （6-17）
其中刚体由微分体积单元νd 组成，包含密度为ρ的材料。每一体积单位的位置由矢量A
P 确定,如图6-1所示，而
????=??
????
A
x P y z 元素x x y y z z I I I ，，称为质量惯性矩。注意每种情况下我们是对质量单元ρνd 乘以对应轴垂直距离的平方来积分。带混合下标的称为质量惯性积。对一给定刚体这一组6个量将与
它们定义所在的标架的位置和方位有关。若我们可以任意选取参考标架的方位，有可能选成使惯性积为零。这样的标架的轴称作主轴，而对应得的惯性矩称作主惯性矩。
例 6-1
求出关于图6-2所示的坐标系的均匀密度ρ的矩形物体的惯性张量。 首先，我们计算x x I 。用体积单元ν=d dxdydz ，我们得到
22
22x x 000
3332
0()() ()()333ρρρρ
=+=+=+=+?
??
?
?
?h
l
w
h
l
h
I y z dxdydz y z w dydz
l hl w h lw z l w dz
22()3
=+m
l h （6-18）
式中，m 为物体的全部质量。交换各项，我们可以用观察求出y y I 和z z I
2
2y y ()3
=
+m I w h （6-19） 和
22z z
()3
=+m
I l w （6-20） 其次我们计算x y I
2
x y 0000
022
2 44
ρρρ====?
??
?
??
h
l w
h
l
h
w I xy dxdydz y dydz
w l m
dz wl （6-21）
交换各项我们得到
x z 4
m
I hw =
（6-22） 和
y z 4
m
I hl =
（6-23） 因此，这个物体的惯性张量为：
22
2
222()344()434
()
4
4
3A
m m
m l h wl hw m m m I wl w h hl m m m hw hl l w ??+???
?
??=+???
?
??+????
（6-24） 如说过的那样，惯性张量是参考标架的位置和方位的函数。一个众所周知的结论，平行轴定理，是一种在参考坐标系改变的情况下如何计算惯性张量的改变的方法。平行轴定理说明在原点位于质心处的标架的惯性张量在另一个参考标架中的惯性张量之间的关系。在定理中{}C 为位于物体质心处，{}A 为任意变换后的标架，定理陈述为两个方程式〔Shames 〕
22z z z z x y x y ()A C c c A
C
c c
I I m x y I I mx y =++=+ （6-25）
式中，c x 、c y 和c z 为质心在{}A 中的位置。其余的惯性矩和惯性积可以由（6-25）中x 、
y 、z 的置换计算出来。 例6-2
求出例6-1中描述的同一个物体的惯性张量，此时它是在原点位于物体质心处的坐标系中描述的。
我们应用平行轴定理（6-25），其中
12c c c x w y l z h ????
????=????????????
于是，我们求出
22
z z x y
()120
C
C
m I w l I =+= （6-26） 其余元素可以根据对称性求出。这个写在质心处的坐标架中的结果惯性张量为：
22
2222()00120()0
1200()12C m l h m I w h m l w ??+????
??=+??????+????
（6-27） 由于结果是对角阵，{}C 必定代表这个物体的主轴。 下面是某些关于惯性张量的其他结论： 1．
如果参考标架的两根轴形成物体质量分布的对称平面，则带垂直于这个平面坐标的
下标的惯性积将为零。
2． 惯性矩必须总为正值。惯性积则可正可负。
3． 在参考标架的方位改变中，三个惯性矩之和为不变量。
4．
惯性张量的固有值（特征值）是物体的主惯性矩。相应的固有矢量是主轴。
大多数操作机的杆件的几何和组成都有点复杂，所以在实用中应用（6-17）是很困难的。一个实用的选择是用测量装置（例如惯性摆）实际测量每个杆件的惯性矩而不是计算。
6.4 牛顿方程，欧拉方程
我们将把操作机的每个杆件考虑为刚体。如果我们知道杆件的质心的位置和惯性张量，则它的质量分布的特性是完全表示出来了。为了使这些杆件运动，我们必须使它们加速或减速。对于这种运动所需的力是期望的加速度和杆件质量分布的函数。牛顿方程和回转用的与它类似的欧拉方程描述了力、惯性和加速度之间是个什么样的关系。
6.4.1 牛顿方程
图6-3示出一刚体，其质心以加速度C
V 在加速运动。 在这种情况下，作用在质心的造成这个加速度的力F 可以由牛顿方程给出为：
v
C F m = （6-28） 式中，m 为刚体的总质量。
6.4.2 欧拉方程
图6-4示出一刚体以角速度ω回转着，角加速度为ω
在这样的情况下，为了产生这个运动必须施加在刚体上的力矩
可由欧拉方程给出：
C C N I I ω
ωω=+? （6-29） 式中，C I 为卸载标架{}C 中的刚体的惯性张量，{}C 的原点位于质心，如图6-4所示。
6.5 迭代牛顿－欧拉动力学公式
我们现在考虑计算对应于给定操作机轨迹的扭矩的问题。假定我们已知关节的位置，速
度和加速度（Θ、Θ 、Θ ）。根据这些知识以及机器人运动学的知识，质量分布的信息等，
我们可以计算出造成这个运动所需的关节扭矩。
6.5.1 向前迭代以计算速度和加速度
为了计算作用在杆件上的惯性力，需要计算在每个给定瞬时，操作机各杆件的回转速度，
质心的线加速度和回转加速度。这些计算将以迭代式的形式从杆1开始，逐次向外移动，一杆接一杆，直到杆n 。
在前面讨论了从杆件到杆件的回转速度的变换，给为：
11111?i
i i i i i i i i R z
ωωθ+++++=+ （6-30） 从（6-15）我们得到从杆件到杆件的角加速度的变换方程
1
11111111
1??i i i i i i i i i i i i i i i i R R z z ωωωθθ++++++++++=+?+ （6-31） 各个杆的线加速度根据（6-12）可以得到为：
1
1
111(())i i i i i i i i i i i i i i i i v R P P v ω
ωω+++++=?+??+ （6-32） 我们也将需要各个杆件质心的线加速度，它也可由应用（6-10）求出
()i
i i i i i i Ci i Ci i i Ci i v
P P v ωωω=?+??+ （6-33） 式中，我们假想一个标架{}i C 固结于各个杆件而它的原点位于杆件质心处，而且与杆件标架{}i 有相同的方位。
注意方程式应用于杆1特别简单，因为00
000ωω
== 。
6.5.2 作用在杆件上的力和扭矩
计算出各个杆件质心的线加速度和角加速度后，我们可以应用牛顿－欧拉方程（6-4节）来计算作用在各个杆件质心上的力和力矩。
i C i C i
C i i F mv
N I I ω
ωω==
+? （6-34）
式中，{}i C 的原点在各杆的质心处，而它的方位与杆件标架{}i 相同。
6.5.3 向后迭代以计算力和扭矩
计算出作用在各杆件上的力和扭矩后，现在剩下要做的是计算关节扭矩，它们将产生这
些作用在杆件上的净力和扭矩。
我们根据对典型杆件的分离体图（见图6-5）写出力和力矩的平衡方程式就可以计算出这些关节扭矩。各个杆件受到其相邻杆件所施加的作用力和扭矩，还承受一个惯性力和力矩。我们对这些邻杆的作用力规定特殊的符号
11i i f i i n i i =-=-杆件作用到杆件的力；
杆件作用到杆件的力矩。
把作用在杆件i 上的力加起来我们得到一个力平衡关系
111i
i i i i i i i F f R f +++=- （6-35）
把扭矩对于质心加起来再让它们等于零，我们得到扭矩平衡方程式
111()()i
i i i i i i i i i i C i i i C i i N n n P f P P f +++=-+-?--? （6-36）
用从力平衡关系（6-35）得来的结果再加上几个回转矩阵，我们可以把（6-36）写为：
1111111i
i i i i i i i i i i i i C i i i i i N n R n P F P R f +++++++=--?-? （6-37）
最后，我们可以整理力和扭矩方程式使它们成为由较高编号邻杆。
图6-5 典型杆件的分离体图
111i
i i i i i i i f R f F +++=+ （6-38）
1111111i
i i i i i i i i i i i i C i i i i i n N R n P F P R f +++++++=++?+? （6-39）
这些方程式一杆接一杆地求值，从杆n 开始向机器人的基础进行下去。这些向后力的迭代与前面介绍过的静力迭代相似，只是现在考虑了各杆的惯性力和力矩。
如同静力的情况那样，所需的关节扭矩可由取一杆作用于其邻杆的扭矩的Z 分量来求得
?i T i i i
n Z τ= （6-40） 注意对一个在自由空间运动的机器人，
1
1N N f ++和11N N n ++都让它为零，因此方程式的第
一个对杆n 的应用非常简单。如果机器人与周围环境有接触，由于接触而产生的力和扭矩以
1
1N N f ++和11N N n ++不等于零而包括在力平衡中。
6.5.4 迭代的牛顿－欧拉动力学算法
由关节的运动来计算关节扭矩的完整的算法由两部分组成。首先，杆件的速度和加速度从杆件1到杆件n 迭代地被计算出来，牛顿－欧拉方程式被用于各个杆件。其次，反力和反力矩以及促动器扭矩从杆件n 回到杆1递归地被计算出来。这些方程式概括如下： 向前 i ：05→
1
11111?i i i i i i i i i R z
ωωθ++++++=+ （6-41） 1
111111111??i i i i i i i i i i i i i i i i R R z z ωωωθθ++++++++++=+?+ （6-42） 1
1
111(())i i i i i i i i i i
i i i i i i v
R P P v ω
ωω+++++=?+??+ （6-43） 1
1
11111 11 111 11()i i i i i i i C i i C i i i C i i v
P P v ω
ωω++++++++++++++=?+??+ （6-44） 1
111 1i i i i C i F m v
+++++= （6-45） 1
111111111i i i i i i i i i i N I I ω
ωω++++++++++=+? （6-46） 向后 i ：61→
111i
i i i i i i i f R f F +++=+ （6-47） 1111111i
i i i i i i i i i i i i C i i i i i n N R n P F P R f +++++++=++?+? （6-48）
?i T i i i i
n Z τ= （6-49）
6.5.5 在动力学算法中包括重力
作用在杆件上的重力的影响可以很简单地让0
0v
G = 来包括进去，其中G 为重力矢量。这等于说机器人的基础以一个G 的加速度向上加速着。这个假想的向上加速度对杆件造成的影响，和重力将造成的完全相同。所以，不需额外的计算支出，重力的影响就被计算进去了。
6.6 封闭形式的动力学方程
方程式（6-41）到（6-49）给出了一个计算方案，在那里根据已知的关节位置，速度和
加速度等，我们可以计算所需的关节扭矩。和我们在前面推导计算雅可比雅可比的方程式一起，这些关系可以有两种用法：作为数值计算算法，或作为用来解析地推导符号方程式的算法。
用这些方程作为数值计算算法是吸引人的，因为方程式可以应用于任何带回转关节的机器人（对于带移动关节的机器人可以推导出一组类似的方程式）。一旦对特定的操作机定出了惯性张量，杆件的质量， C i P 矢量和矩阵1
i i
R +等，这些方程式可以直接用来计算对应于任
何运动的惯性扭矩。
但是，我们经常有兴趣于得到方程式结构的更好的了解。例如，重力项的形式是什么？重力影响的数量级是否和惯性力的相当？为了考察这种和那种问题，我们常常想写出逆动力
学的封闭形式。这些封闭形式的方程式可以对Θ、Θ
和Θ 符号地应用递归的牛顿－欧拉方程来推导出。它与我们前面推导雅可比的符号形式是所做的相似。
6.6.1 操作机动力学的乘积和形式
对于操作机的封闭形式的运动方程式可以写成这样的形式，作用在各个关节的扭矩表达
为乘积之和。
例6-3
计算图6-6所示2杆平面操作机的封闭形式的逆动力学方程式。为了简单起见，假设质量分布极简单：所有质量作为一个点质量位于各杆件的末梢。这意味写在质量中心处的各个杆件惯性张量为零矩阵（注意尽管各个杆件的惯性张量为零，我们仍将发现惯性项出现在动力学方程式中）。
首先我们确定将出现在递归牛顿－欧拉方程中的各种量
1
21 111 222211
1230033? 0 0 0 0 0C C P l X P l X P l X I I I f n ωω
==========
和
图6-6 带杆件末端的点质
量的2杆
111110.00.00.00.0 1.0i i i
i i i c s R s c +++++-????=??????
11
1
1
10.00.00.0
0.0 1.0i i i i i i c s R s c +++++??
??=-??????
杆件1的向前迭代
1111110?0Z ωθθ??
??==??
???? 1111110?0Z ωθθ??
??==??
???? 1
111
11110000
100c s gs v s c g gc ??????
??????=-=?
?????????????????
22111111
1111111100000C l gs l gs v l gc l gc θθθθ??-??-+????????????=++=?
??????????????
?+???????? 211111
1
1111110m l m gs F m l m gc θθ??-+??=+??????
1
1000N ????=??????
（6-50~a f ）
杆件2的向前迭代
2
21200ωθθ??
??=??
??+?? 2
21200ωθθ??
??=??
??+??
222
2
111112112 1 22
222211
1112112 1 2000
0100c s l gs l s l c gs v s c l gc l c l s gc θθθθθθ????-+-+????????=-+=++?????
????????????? 22212112112 1 2
222212112112 1 20()()0000C l l s l c gs v l l c l s gc θθθθθθθθ????-+-+????????=+++++?
?????????????????
22211221122122212
2
22211221122122212()()0m l s m l c m gs m l F m l c m l s m gc m l θθθθθθθθ??-+-+??=++++?????? 2
2000N ????=??????
（6-51~a f ）
杆件2的向后迭代
2
222f F =
2
22221221212212212221200()n m l l c m l l s m l gc m l θθθθ??
??=??
??++++??
（6-52~a b ）
杆件1的向后迭代
2222121212121222122
211111
1
212
2111
11212121212122212()01()0
100m l s m l c m gs m l c s m l m gs f s c m l m gc m l c m l s m gc m l θθθθθθθθθθ????-+-+--+????????=++++++???????????????
??? 1
12222122121221221222122111112221121221221112212212212120000()0 0()()n m l l c m l l s m l gc m l m l m l gc m l m l l s m l gs s m l l c m l gc c θθθθθθθθθθ????????=+????
????+++++????
??
??+??
??-+++++??
（6-53~a b ）
取出i
i n 的?Z
分量，我们求出关节扭矩
22122122122121221
22122221221222121211
()(2)() 2()m l m l l c m m l m l l s m l l s m l gc m m l gc τθθθθθθθθ=+++++--+++
222212212122222122212
()m l l c m l l s m l gc m l τθθθθ=++++ （6-54~a b ）
（6-54）式给出促动器扭矩作为关节位置，速度和加速度的函数表达式。读者可以想像，6自由度操作机的解析方程式会非常复杂。
6.6.2 操作机动力学方程式的普遍结构
当牛顿－欧拉方程对任意操作机符号地估值时，它们产生一个可写为下面形式的动力学方程式
()()()M V G τ=ΘΘ
+ΘΘ+Θ ， （6-55） 式中，()M Θ为操作机的n n ?惯性矩阵。
()V ΘΘ
，为离心和哥利奥里斯（Coriolis ）项的1n ?矢量。 ()G Θ为重力项的1n ?矢量。
()M Θ和()G Θ的每项都是与Θ有关的复杂函数，Θ为操作机所有关节的位置。
()V ΘΘ
，的每项为Θ和Θ 两者的复杂函数。 我们可以把出现在动力学方程中的各项分离为各种类型而形成操作机的质量矩阵、离心和哥氏（Coriolis ）矢量以及重力矢量等。
例6-4
例6-3中操作机的操作机质量矩阵是什么？
（6-55）式定义了操作机质量矩阵。它由所有乘Θ
的项组成，而且是Θ的函数。因此 222
22122211222122222
221222222()()l m l l m c l m m l m l l m c M l m l l m c l m ??
++++Θ=??+??
（6-56） 任何操作机质量矩阵都是对称的和和正定的，因而总是可以求逆的。
进一步考察（6-54），我们可以确定其他作用于2杆操作机上的动力效应。一个象
221222
m l l s θ 的项是离心力，因为它与关节速度的平方有关。一个诸如2122122m l l s θθ 的项称做哥利奥里斯（Coriolis ）力，它是由两个不同关节的速度乘积。最后，任何包括重力常数g 的项都是重力项。注意重力项仅与Θ有关，而与它的导数无关。
本章参考文献
1. 王庭树.机器人运动学及动力学.西安：西安电子科技大学出版社，1991
2.
Xu Liju ,Fan Shouwen and Chen Yong.Numeric-Symbolic Approach for Kinematic and Dynamic of Robotic Manipulators.Mechanism and Machine Theory ，1995,30(5):645-652 3.
Xu Liju,Fan Shouwen and Chen Yong.Analytical Model Algorithm for Dynamics of Robotic Manipulators.机械工程学报（英文版）,1995,l8(2):168-171
4. Xu Liju,Fan Shouwen and Li Hong.Analytical model method for dynamics of N-celled tetrahedron-tetrahedron variable geometry truss manipulators. Mechanism and Machine Theory,2001,36(11):1271-1279
5.
徐礼钜,范守文.基于并行计算的新型并联机床动力学解析模型.机械工程学报,2004,40(4):71-76
一、平面二连杆机器人手臂运动学 平面二连杆机械手臂如图1所示，连杆1长度1l ，连杆2长度2l 。建立如图1所示的坐标系，其中，),(00y x 为基础坐标系，固定在基座上，),(11y x 、),(22y x 为连体坐标系，分别固结在连杆1和连杆2上并随它们一起运动。关节角顺时针为负逆时针为正。 图1平面双连杆机器人示意图 1、用简单的平面几何关系建立运动学方程 连杆2末段与中线交点处一点P 在基础坐标系中的位置坐标： ) sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p （1） 2、用D-H 方法建立运动学方程 假定0z 、1z 、2z 垂直于纸面向里。从),,(000z y x 到),,(111z y x 的齐次旋转变换矩阵为： ?? ??? ???????-=100 010000cos sin 00sin cos 1 111 01θθ θθT （2） 从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为： ?? ??? ???????-=100 010000cos sin 0sin cos 2 212212 θθ θθl T （3） 从),,(000z y x 到),,(222z y x 的齐次旋转变换矩阵为：
? ???? ???????+++-+=?? ??? ? ? ?? ???-?????????????-=?=10000100sin 0)cos()sin(cos 0)sin()cos( 1000010 000cos sin 0sin cos 1000 010000cos sin 00sin cos 1121211121212212 2111 1120102θθθθθθθθθθθθθθθθ θθl l l T T T （4） 那么，连杆2末段与中线交点处一点P 在基础坐标系中的位置矢量为： ? ?? ? ? ???????=????????????++++=? ? ? ?? ? ?????????????? ?? ???+++-+=?=110)sin(sin )cos( cos 10010000100sin 0)cos()sin(cos 0)sin()cos( 212112121121121211121212 020p p p z y x l l l l l l l P T P θθθθθθθθθθθθθθθθ （5） 即， ) sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p （6） 与用简单的平面几何关系建立运动学方程（1）相同。 建立以上运动学方程后，若已知个连杆的关节角21θθ、，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。 3、平面二连杆机器人手臂逆运动学 建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂二连杆的关节角21θθ、，这叫机械臂的逆运动学。逆运动学可以用于对机械臂关节角和末端位置的控制。对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置 ),(p p y x 求相应关节角21θθ、的过程。推倒如下。 （1）问题 ) sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p 已知末端位置坐标),(p p y x ，求关节角21θθ、。 （2）求1θ
第二章 机器人运动学 机器人，尤其是其中最为常用的关节型机器人，由若干个关节所联系起来的一种开链，其一端固结在机座上，另一端安装有末端执行器。已知所有关节变量确定机器人末端执行器的位姿或者由末端手的位姿计算出每一个关节变量值是机器人运动学研究的主要内容。 本章主要介绍机器人运动学，首先介绍了 1.1齐次坐标与齐次变换 在描述刚体（如零件、工具或机械手）间关系时，要用到点、向量、坐标系、平移、旋转以及变换等概念，这些概念可用齐次矩阵来表示。 1.1.1空间点的表示 在指定的直角坐标系{}A 中，空间任一点P （图2-1）的位置可用13?的列矢量P A 表 示： [ ] z y x A p p p P = （2.1） 其中x p ，y p ，z p 为点P 的三个坐标分量，P A 的上标A 代表参考坐标系{}A ，称P A 为位置矢量。 图2-1位置表示 1.1.2空间向量的表示 将一个n 维空间的点用1+n 维坐标表示，则该1+n 维坐标即为n 维坐标的齐次坐标，即： [ ] T z y x A p p p P 1= （2.2） 在上式中加入一个比例因子w ，点P 表示为： [ ] T z y x A w c b a P = （2.3） 其中，w p a x x =，w p b y y =，w p c z z =。式2.2和2.3表示同一个点P 。 起始于原点，终止于P 点的空间向量也可以采用齐次矩阵形式表示：
[ ] T z y x w c b a P = （2.4） 若比例因子w 变化，向量的大小也会发生变化，w 大于1，向量所有的分量都变大，如果w 小于1，向量所有的分量都变小，w 等于1，各分量的大小保持不变。w 等于0表示该向量的方向，称为方向向量。如图2-2中，i 、j 、k 分别表示直角坐标系中X 、Y 、Z 坐标轴的单位矢量，用齐次坐标表示为： []T X 0001= []T Y 0010=[]T Z 0100= （2.5） 图2-2中所示的矢量u 的方向表示为： []T u 0cos cos cos γ β α= （2.6） 其中α、β、γ分别为矢量u 与坐标轴的夹角。 1.1.3刚体位姿的表示 为了研究机器人的运动，往往不仅要表示空间某个点的位置，而且需要表示刚体的姿态。指定一个坐标系与此刚体固接，再将此坐标系在空间表示出来，该坐标系称为动坐标系。如 图2-3所示，O '为刚体上任一点，Z Y X O ''' '为固接在刚体上的一个动坐标系，动坐标系的原点与固定坐标系原点之间做一个向量P 来表示动坐标系的位置，即为式2.2。动坐标系的姿态可由其坐标轴方向来表示，令n 、o 、a 分别为X '、Y '、Z '坐标轴的单位向量，每个向量都由其所在固定坐标系中的三个分量表示： [] [][ ] ?? ? ??===T z y x T z y x T z y x a a a a o o o o n n n n 000 （2.7） 动坐标系的位姿可以由三个表示方向的单位向量以及第四个位置向量来表示，式中前三个向量是0=w 的方向向量，表示该坐标系的三个单位向量n 、o 和a 的方向，而第四个 1=w 的向量表示动坐标系的原点相对于固定坐标系的位置。与单位向量不同，向量P 的长 度十分重要，因而比例因子为1。 []????? ???????==10 00 z z z z y y y y x x x x p a o n p a o n p a o n p a o n F （2.8） 由于动坐标系一直固接在该刚体上，只要动坐标系在空间表示出来，刚体相对于固定坐
国防科学技术大学 硕士学位论文 仿人机器人运动学和动力学分析 姓名：王建文 申请学位级别：硕士 专业：模式识别与智能系统 指导教师：马宏绪 20031101
能力；目前，ＡＳＩＭＯ代表着仿人机器人研究的最高水平，见图卜２。２０００年，索尼公司也推出了自己研制的仿人机器人ＳＤＲ一３Ｘ，２００２年又研制出了ＳＤＲ一４Ｘ，见图卜３。日本东京大学也一直在进行仿人机器人的研究，与Ｋａｗａｄａ工学院合作相继研制成功了Ｈ５、Ｈ６和Ｈ７仿人机器人，其中Ｈ６机器人高１．３７米，体重５５公斤，具有３５个自由度，目前正在开发名为Ｉｓａｍｕ的新一代仿人机器人，其身高１．５米，体重５５公斤，具有３２个自由度。日本科学技术振兴机构也在从事ＰＩＮＯ机器人的研究，ＰＩＮＯ高０．７５米，采用２９个电机驱动，见图卜４。日本Ｗａｓｅｄａ大学一直在从事仿人机器人研究计划，研制的ｗＬ系列仿人机器人和ＷＥＮＤＹ机器人在机器人界有很大的影响，至今已投入１００多万美元，仍在研究之中。Ｔｏｈｏｋｕ大学研制的Ｓａｉｋａ３机器人高１．２７米，重４７公斤，具有３０个自由度。美国的ＭＩＴ和剑桥马萨诸塞技术学院等单位也一直在从事仿人机器人研究。德国、英国和韩国等也有很多单位在进行类似的研究。 图卜１Ｐ２机器人图卜２ＡＳＩＭＯ机器人图１．３ＳＤＲ－４Ｘ机器人图１－４ＰＩＮＯ机器人 图卜５第一代机器人图ｌ－６第二代机器人图１．７第三代机器人图１—８第四代机器人 在国家“８６３”高技术计划和自然科学基金的资助下，国内也开展了仿人机器人的研究工作。目前，国内主要有国防科技大学、哈尔滨工业大学和北京理工大学等单位从事仿人机器人的研究。国防科技大学机器人实验室研制机器人已有１０余年的历史，该实验室在这期间分四阶段推出了四代机器人，其中，２０００年底推出的仿人机器入一“先行者”一是国内第一台仿人机器人。２００３年６月，又成功研制了一台具有新型机械结构和运动特性的仿人机器人，这台机器人身高１．５５米，体重６３．５公斤，共有３６个自由度，脚踝有力 第２页
精心整理 一、平面二连杆机器人手臂运动学 平面二连杆机械手臂如图1所示，连杆1长度1l ，连杆2长度2l ，连杆3长度为3l 。建立如图1所示的坐标系，其中，),(00y x 为基础坐标系，固定在基座上，),(11y x 、),(22y x 、33(,)x y 为连体坐标系，分别固结在连杆1、连杆2、连杆3上并随它们一起运动。关节角顺时针为负逆时针为正。 1 θ 图11112123123p p x y 2、用D-H 方法建立运动学方程 假定0z 、1z 、2z 垂直于纸面向外。从),,(000z y x 到),,(111z y x 的齐次旋转变换矩阵为： ?? ??? ???????-=100 010000cos sin 00sin cos 1 11101 θθ θθT （2）
从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为： ?? ??? ???????-=100 010000cos sin 0sin cos 2 212212θθ θθl T （3） 从222(,,)x y z 到333(,,)x y z 的齐次旋转变换矩阵为： 33212cos sin 0l T θθ-????=从(003T =003P =结论：（6）与用简单的平面几何关系建立运动学方程（1）相同。 补充：正解用于仿真，逆解用于控制 建立以上运动学方程后，若已知个连杆的关节角123θθθ、、，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。 3、平面二连杆机器人手臂逆运动学 二、平面二连杆机器人手臂的速度雅可比矩阵 速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。 上面的运动学方程两边对时间求导，得到下面的速度表达式：
第六章机器人操作臂动力学 动力学研究的是物体的运动和受力之间的关系。操作臂动力学有两个问题需要解决。 ①动力学正问题：根据关节运动力矩或力，计算操作臂的运动（关节位移，速 度和加速度） ②动力学逆问题：已知轨迹运动对应的关节位移，速度和加速度，求出所需要 的关节力矩或力。 机器人操作臂是个复杂的动力学系统，由多个连杆和多个关节组成，具有多个输入和多个输出，存在着错综复杂的耦合关系和严重的非线性。因此，对于机器人动力学的研究，引起了十分广泛的重视。所采用的方法很多，①有拉格朗日方法，②牛顿-欧拉方法，③高斯法，④凯恩方法，⑤旋量对偶数方法等等。在此重点介绍牛顿-欧拉方法，它是基于运动坐标和达朗贝尔原理来建立相应的运动方程。 研究机器人动力学的目的是多方面的，动力学正问题与操作臂仿真有关，逆问题是为实时控制的需要，利用动力学模型，实现最优控制，以期达到良好的动态性能和最优指标。 机器人动力学模型主要用于机器人的设计和离线编程。在设计中需根据连杆质量，运动学和动力学参数，传动机构特征和负载大小进行动态仿真，从而决定机器人的结构参数和传动方案，验算设计方案的合理性和可行性，以及结构优化程度。在离线编程时，为了估计机器人高速运动引起的动载荷和路径偏差，要进行路径控制仿真和动态模型的仿真。这些都必须以机器人动态模型为基础。 为了建立机器人动力学方程，在此首先讨论机器人运动的瞬时状态，对其进行速度分析和加速度分析，研究连杆的静力平衡，然后利用朗贝尔原理，将静力学平衡条件用于动力学。 §6-1连杆的速度和加速度 点的速度表示一般要涉及到两个坐标系： 要指明速度是相对于哪个坐标系的运动所造成的。
平面二自由度机械臂动力学分析 [摘要] 机器臂是一个非线性的复杂动力学系统。动力学问题的求解比较困难，而且需要较长的运算时间，因此，这里主要对平面二自由度机械臂进行动力学研究。本文采用拉格朗日方程在多刚体系统动力学的应用方法分析平面二自由度机械臂的正向动力学。经过研究得出平面二自由度机械臂的动力学方程，为后续更深入研究做铺垫。 [关键字] 平面二自由度 一、介绍 机器人是一个非线性的复杂动力学系统。动力学问题的求解比较困难，而且需要较长的运算时间，因此，简化解的过程，最大限度地减少工业机器人动力学在线计算的时间是一个受到关注的研究课题。 机器人动力学问题有两类： (1) 给出已知的轨迹点上的，即机器人关节位置、速度和加速度，求相应的关节力矩向量Q r。这对实现机器人动态控制是相当有用的。 (2) 已知关节驱动力矩，求机器人系统相应的各瞬时的运动。也就是说，给出关节力矩向量τ，求机器人所产生的运动。这对模拟机器人的运动是非常有用的。 二、二自由度机器臂动力学方程的推导过程 机器人是结构复杂的连杆系统，一般采用齐次变换的方法，用拉格朗日方程建立其系统动力学方程，对其位姿和运动状态进行描述。机器人动力学方程的具体推导过程如下： (1) 选取坐标系，选定完全而且独立的广义关节变量θr ，r=1, 2,…, n。 (2) 选定相应关节上的广义力F r：当θr是位移变量时，F r为力；当θr是角度变量时， F r为力矩。 (3) 求出机器人各构件的动能和势能，构造拉格朗日函数。 (4) 代入拉格朗日方程求得机器人系统的动力学方程。 下面以图1所示说明机器人二自由度机械臂动力学方程的推导过程。
第六章 机器人运动学及动力学 6.1 引论 到现在为止我们对操作机的研究集中在仅考虑动力学上。我们研究了静力位置、静力和速度，但我们从未考虑过产生运动所需的力。本章中我们考虑操作机的运动方程式——由于促动器所施加的扭矩或作用在机械手上的外力所产生的操作机的运动之情况。 机构动力学是一个已经写出很多专著的领域。的确，人们可以花费以年计的时间来研究这个领域。显然，我们不可能包括它所应有的完整的内容。但是，某种动力学问题的方程式似乎特别适合于操作机的应用。特别是，那种能利用操作机的串联链性质的方法是我们研究的天然候选者。 有两个与操作机动力学有关的问题我们打算去解决。向前的动力学问题是计算在施加一 组关节扭矩时机构将怎样运动。也就是，已知扭矩矢量τ，计算产生的操作机的运动Θ、Θ 和Θ 。这个对操作机仿真有用，在逆运动学问题中，我们已知轨迹点Θ、Θ 和Θ ，我们欲求出所需要的关节扭矩矢量τ。这种形式的动力学对操作机的控制问题有用。 6.2 刚体的加速度 现在我们把对刚体运动的分析推广到加速度的情况。在任一瞬时，线速度矢量和角速度矢量的导数分别称为线加速度和角加速度。即 B B Q Q B B Q Q 0V ()V ()d V V lim dt t t t t t ?→+?-==? （6-1） 和 A A Q Q A A Q Q 0()()d lim dt t t t t t ?→Ω+?-ΩΩ=Ω=? （6-2） 正如速度的情况一样，当求导的参坐标架被理解为某个宇宙标架{}U 时我们将用下面的记号 U A AORG V V = （6-3） 和 U A A ω=Ω （6-4）
6.2.1 线加速度 我们从描述当原点重合时从坐标架{}A 看到的矢量B Q 的速度 A A B A A Q B Q B B V V B R R Q =+Ω? （6-5） 这个方程的左手边描述A Q 如何随时间而变化。所以，因为原点是重合的，我们可以重写（6-5）为 A A B A A B B Q B B d ()V dt B B R Q R R Q =+Ω? （6-6） 这种形式的方程式当推导对应的加速度方程时特别有用。 通过对（6-5）求导，我们可以推出当{}A 与{}B 的原点重合时从{}A 中看到的B Q 的 加速度表达式 A A B A A A A Q B Q B B B B d d V (V )()dt dt B B R R Q R Q =+Ω?+Ω? （6-7） 现在用（6-6）两次── 一次对第一项，一次对最后一项。（6-7）式的右侧成为： A B A A A A B Q B B Q B B A A A A B B Q B B V () +Ω?+Ω?+Ω?+Ω? B B B B R R V R Q R V R Q （6-8） 把相同两项合起来 A B A A A A B Q B B Q B B A A A B B B V 2 () +Ω?+Ω?+Ω?Ω? B B B R R V R Q R Q （6-9） 最后，为了推广到原点不重合的情况，我们加上一项给出{}B 的原点的线加速度的项，得到下面的最后的一般公式 A B A A A A BORG B Q B B Q B B A A A B B B V 2 () ++Ω?+Ω?+Ω?Ω? A B B B V R R V R Q R Q （6-10） 对于我们将在本章上考虑的情况，我们总是有B Q 为不变，或 B Q Q V 0== B V （6-11） 所以，（6-10）简化为 A A A A A A Q BORG B B B B B V ()=+Ω?Ω?+Ω? A B B V R Q R Q （6-12） 我们将用这一结果来计算操作机杆件的线加速度。 6.2.2 角加速度 考虑{}B 以A B Ω相对于{}A 转动的情况，而{}C 以B C Ω相对于{}B 转动。为了计算 A C Ω我们把矢量在坐标架{}A 中相加
机器人运动学知识介绍 收藏 21:53|发布者: dynamics|查看数: 1125|评论数: 2| 来自: 东方早报 摘要: 现在你可能正拿着一本书，边看边翻页，并时不时回头，越过肩膀察看后面是否有红眼的恶意机器人。随着书页的翻动，你也许会在无意识里考虑这个问题。作为人类，在物理世界移动是如此自然，只需要一丁点的意识即可。而 ... 丹尼尔·威尔逊 现在你可能正拿着一本书，边看边翻页，并时不时回头，越过肩膀察看后面是否有红眼的恶意机器人。随着书页的翻动，你也许会在无意识里考虑这个问题。作为人类，在物理世界移动是如此自然，只需要一丁点的意识即可。而另一方面，机器人———就像最后一个选择踢球的孩子———为了避免伤到自己和别人，每一个动作都必须经过仔细考虑。机器 人专家管这个过程叫做“操作研究”。 前进和逆转 如果你醒来发现自己处在一具新的躯体中，拥有金属手臂，每只手只有三根手指，你会怎么样呢？如果不知道手臂的长度，拿东西会很困难；如果只有三根手指，那么你必须找到一个全新的抓取和握东西的方法；由于弯曲的金属手臂，你可能再也没有约会的机会。这些就是身处各地的孤独的机器人们所面临的重大问题。 运动学研究旨在解决机器人的手臂转向何方(动力学则为了解决移动的速度和劲道)。机器人运动学可分两类：前进和逆转。前进运动学的问题是机器人运用它对自身的了解(关节角度和手臂长度)来判断自己在三维空间中到底身处何方。这算是简单的部分，逆转运动学正好相反，它解决机器人如何移动才能达到合适的姿势(改变关节位置)这一问题。机器人在握你手之前，需要知道你手的大概方位，以及从这里移向那里的最优顺序。有时候，可能没有最好的解决方案(试试用你的右手碰你的右肘)。 对逆转运动学来说，大多数方案运用传感器(通常是视觉和力)来估计机器人身体的当前位置。只要有了这个，机器人就能够计划下一步行动(握手、问好或绞断你的脖子)。机器人的反应很敏捷，日本ATR实验室的类人机器人能够更新视觉，估计世界形势，并且在一秒钟里能够做60个动作。这些类人机器人已经能够跳舞，耍弄彩球，玩篮球和曲棍球。 扫描环境和选择动作的过程叫做反馈环路。新的信息被经常性地用于更新当前的决定。如果缺乏经常性更新，机器人的操作技能会变得糟糕。传感器的损伤(或非常不可信赖的传感器)会干扰这一重要的环路。比如以视觉为基础的跟踪遇到混乱的场景会大受干扰，或者浪费资源去跟踪一些无意义的目标(比如落叶等)。震动可以扰乱力传感器，即使它们位于机器人手臂的内部。虽然机器人能够反应得更快更精确，但它们总是依赖于不断更新的信息和持续改进的计划。
第二章机器人基础知识 2.3工业机器人运动学(二) 【内容提要】 本课主要学习工业机器人技术的运动学基础知识，涉及连杆参数及连杆坐标系的建立、连杆坐标系之间的变换矩阵、机器人运动学一般方程，并介绍了正逆向运动学以及实例。 知识要点： ?连杆坐标系 ?连杆坐标系之间的变换矩阵 ?机器人运动学一般方程 重点： ?掌握连杆坐标系之间的变换矩阵 ?掌握机器人运动学一般方程的建立 ?掌握机器人正逆向运动学的实例 难点： ?机器人运动学一般方程的建立 关键字： ?连杆参数、变换矩阵、运动学一般方程、
【本课内容相关资料】 2.3.4 工业机器人的连杆参数和齐次变换矩阵 1. 连杆参数及连杆坐标系的建立 工业机器人相邻连杆之间的关系，与连杆自身的特征和连杆之间的连接方式有关。因此，我们首先应该清楚如何对连杆的特征和连接方式进行描述。 1）连杆特征的描述 如图2.35所示，连杆i 两端有关节i 和i +1。该连杆的特征可以用两个参数来描述：一个是两个关节轴线沿公垂线的距离l i ，称为连杆长度；另一个是在垂直于公垂线的平面内两个轴线的夹角αi ，称为连杆扭角。这两个参数为表述连杆特征的尺寸参数。连杆长度l i 恒为非负数，但连杆扭角αi 可正、可负。αi 的正负是这样规定的：公垂线的正向规定为从 关节i 指向关节i +1，按右手法则从轴线i 绕公垂线转至轴线i +1，逆时针为正，瞬时针为负。两轴线平行时，αi ＝0；两轴线相交时，l i ＝0，此时扭角αi 为两轴线的夹角，正负与X i 轴选向有关。 2）连杆连接方式的描述 如图2.36所示，连杆i 与连杆i -1通过关节i 相连，因此，关节i 的轴线有两条公垂线与它垂直。两条公垂线的相对位置可用两个参数d i 和θi 来确定，其中d i 是沿关节i 轴线测量的两个公垂线与i 轴线交点的距离，当关节轴线相交时，d i 为i 轴线上两交点的距离；θi 是在关节i 轴线的垂直平面内两个公垂线的夹角，当公垂线不存在时，对旋转关节θi 仍然存在。d i 和θi 是表达相邻连杆连接关系的参数。d i 和θi 都可正、可负(详见表2.8)。 这样，相邻两个连杆之间的关系可以由四个参数所描述：其中两个参数（l i 和αi ）描述连杆i 的尺寸；另外两个参数（d i 和θi ）描述连杆i 和连杆i -1之间的连接关系。对于旋转关节，θi 是关节变量，其它三个参数固定不变；对于移动关节，d i 是关节变量，其它三个参数固定不变。(对照图2.36解释，一个关节即为一个自由度) 3）连杆坐标系的建立 D-H 法要求按下面规则建立连杆i 的坐标系{i }（简称i 系）： 1）坐标系{i }与连杆i 固连。Z i 轴与关节i +1的轴线重合，指向任意； 2） X i 轴与连杆i 的两个关节轴线的公垂线重合，方向从关节i 指向关节i +1。当l i 连杆i 关节+1 αi l i 图2.35 连杆尺寸参数l i 及αi