第4章 机器人动力学

则由式（4－10）可以得到驱动力如下
L2 A J FA L2
T
L1 f x L2 f x 0 0 L2 f x
L1 0 L1 f y f 0 y 0
L2 B J FB L2
T
从求解的结果看到，在这里驱动力的大小为 手爪力的大小和手爪力到作用线距离的乘积。
三、惯性矩的确定 动力学不仅与驱动力有关，还与绕质心的惯 性矩有关。下面以一质点的运动为例，了解惯性 矩的物理意义。 如图4－4所示，若将力F 作用到质量为m 的质点时的平移运动，看作是运动方向的标量， 则可以表示为：
下面求解一下图4－10所示的1自由度机械手 的运动方程式，在这里，由于关节轴制约连杆的 运动，所以可以将式（4－30）的运动方程式看 作是绕固定轴的运动。
图4－10 1自由度机械手
假设绕关节轴的惯性矩为 I ，取垂直纸面的方 向为轴 z ，则得到
0 0 I I
图4－3 求生成手爪力或的驱动力
由关节角给出如下姿态
L1 sin 1 L2 sin(1 2 ) L2 sin(1 2 ) L2 J L1 cos1 L2 cos(1 2 ) L2 cos(1 2 ) L1 L2 0
图4－7 运动学、静力学、动力学的关系
第三节 机器人动力学方程式
一、机器人的动能与位能 1．动能 为了导出多关节机器人的运动方程式，首先 要了解机器人的动能和位能。先看图4－8所表示的 第i个连杆的运动能量。
图4－8 第i个连杆的旋转速度和重心的平移速度
刚体的运动能量，是由该刚体的平移构 成的运动能量，与该刚体的旋转而构成的 运动能量之和表示的。因此，图4－8中表 示的连杆的运动能量，可以用下式表示：
i
二、机器人动力学方程的建立举例
1．牛顿─欧拉方程式 首先，以单一刚体为例，如图4－9所示，其 运动方程式可用下式表示
c Fc mv
（4－29）
( I c) N I c
（4－30）
图4－9 单一刚体
式（4－29）和式（4－30）分别被称为牛 顿运动方程式及欧拉运动方程式。式中，m 是刚 33 Ic 的 I R 体的质量； c 是绕重心 C 的惯性矩阵， 各元素表示对应的力矩元素和角加速度元素间的 N 是惯性矩 惯性矩；Fc 是作用于重心的平动力； vc 是重心的平移速度； 是角速度。 ；
1 n ( i )T ( i ) ( i )T (i ) T J L q T J A ) K (mi q JL q Ii J A q 2 i 1
（4－25）
（4－26）
令
( i )T ( i ) ( i )T (i ) H (mi J L JL JA Ii J A ) i 1 n
由于这一公式对任意的 下式成立
T T
都成立，因此得到
F J 0 （4－9）
进一步整理，把式中第二项移到等式右边，并 取两边的转置，则可得到下面的机械手静力学关 系式
JTF
（4－10）
上式表示了机械手在静止状态为产生手爪力 F 的驱动力 。
为了加深理解，下面分别求解图4－3所示的2自 由度机械手在图示位置时，生成手爪力FA f x 0T T 或 FB 0 f y 的驱动力 A 或 B 。图示 为1 0(rad) ， 2 / 2(rad) 时的姿态。
i 1
(i ) vci J L q
K Ki
n
(i ) பைடு நூலகம் i J A q
（4－21） （4－22）
(i ) 相关的雅可比矩阵，J A 是与i第个连杆转动速度相 关的雅可比矩阵。为了区别于与指尖速度相关的 雅可比矩阵，在上面标明了注角（i）。
(i ) J 式中， L 是与 i第个连杆重心位置的平移速度
将式（4－12）、（4－13）代入式（4－11 ），得到 2 m r N （4－14） 2 I m r 如 ，则式（4－14）就改写为 （4－15） 上式是质点绕固定轴进行回转运动时的运动方 程式。与式（4－11）比较 I 相当于平移运动时 的质量，在旋转运动中称为惯性矩。
N I
对于动力学来说，除了与连杆长度 Li 有关之外 ，还与各连杆的质量 mi ，绕质量中心的惯性矩 I Ci ，连杆的质量中心与关节轴的距离 L 有关。如 Ci 图4－6所示。
图4－6 与动力学有关的各量
运动学、静力学和动力学中各变量的关系如 图4－7所示。图中用虚线表示的关系可通过实线 关系的组合表示，这些也可作为动力学的问题来 处理。
因此得到
LA FB FA LB
（4－4）
当力FA向下取正值时，FB则为负值，由于FB 的正方向定义为向上，所以这时表明FB的方向是 向下的，即此时FA和FB的方向都朝下。
二、机器人静力学关系式的推导
利用前面的虚功原理来推导机器人的静力 学关系式。 如图4－2所示的机械手，要产生图（a） 所示的虚位移，推导出图（b）所示各力之间 的关系式。这一推导方法本身也适用于一般的 情况。
F m x
（4－11）
x 表示加速度。 式中，
若把这一运动看作是质量可以忽略的棒长为 的回转运动，则得到加速度和力的关系式为
r
r x
N F r
（4－12） （4－13）
， N 和是绕轴回转的角加速度和惯性 式中 矩。
图4－4 质点平移运动作为回转运动的解析
0 0 0 0 0 I 0 I 0
（4－31）
0 N 0 c cos m gL
（4－32）
I c R 33是在第3行第3列上 式中，g是重力常数； 具有绕关节轴惯性矩的惯性矩阵。把这些公式代 入式（4－30），提取只有z分量的回转，则得到 mgL cos （4－33） I c 该式为1自由度机械手的欧拉运动方程式，其中： I I c mLc （4－34） 对于一般形状的连杆，在式（4－31）中，由于I 除第3分量以外其他分量皆不为0，所以 I 的第1、2分量成了改变轴方向的力矩，但在固定 轴的场合，与这个力矩平衡的约束力生成式（4－ 32）的第1、2分量，不产生运动。
图4－2
机械手的虚位移和施加的力
假设 ： T m1 手爪的虚位移为 r r1 ,, rm , R T n1 , , , R 关节的虚位移为 1 n T m1 手爪力为 F f1 ,, f m , R T n1 关节驱动力为 1 ,, n , R 如果施加在机械手上的力作为手爪力 的反力（用-F来表示）时，机械手的虚功 可表示为：
I dI r 2 dV
（4－18）
四、运动学、静力学、动力学的关系
如图4－5所示，在机器人的手爪接触环境 时，手爪力 F 的驱动力 的关系起重要作用 ，在静止状态下处理这种关系称为静力学（
statics）。
图4－5 手爪力的关节驱动力
在考虑控制时，就要考虑在机器人的动作中 ，关节驱动力 会产生怎样的关节位置 、关 节速度 ，处理这种关系称为 、关节加速度 动力学（dynamics）。
第四章 机器人的动力学初步
第一节 前 言 机器人动力学是研究机器人运动数学 方程的建立。其实际动力学模型可以根据 已知的物理定律(例如牛顿或拉格朗日力学 定律)求得。
机器人运动方程的求解可分为两种不同性质的问题：
正动力学问题。即机器人各执行器的驱 动力或力矩为已知，求解机器人关节变量在 关节变量空间的轨迹或末端执行器在笛卡尔 空间的轨迹，这称为机器人动力学方程的正 面求解，简称为正动力学问题。
x A LA
x B LB （4－2）
式中， 是绕杠杆支点的虚位移。把式（ 4－2）代入式（4－1）消去 x A 、x B ，可得 到下式
（4－3） 由于公式（4－3）对任意的都成立，所以 有下式成立
( FA LA FB LB ) 0
FA LA FB LB 0
则机器人的运动能量公式（4－25）写为 1 T （4－27） Hq K q 2 这里H表示的称为机器人的惯性矩阵。
2．势能 机器人的势置能量和运动能量一样，也是由 各连杆的位置能量的总和给出，因此可用下式表 示： n P mi g T r0,C i 1 （4－28） 式中，g 表示重力加速度，它是一个在基准坐 r0,Ci 表示从基准坐标系原 标系上表示的三维向量。 点，到 i 个连杆的重心位置的位置向量。
（4－19） 式中，Ki为连杆的运动能量，mi为质量 ，vci为在基准坐标系上表示的重心的平移 速度向量，Ii为在基准坐标系上表示的连杆 的转动惯量，i为在基准坐标系上表示的 转动速度向量。
1 1 T T K i mi vcivci i I ii 2 2
因为机器人的全部运动能量 K ，由各连杆的 运动能量的总和表示，所以得到 （4－20） n 为机器人的关节总数。其次我们来考 式中， 虑把作为机器人各关节速度的函数。这里 vci 与 i 分别表示如下：
（ 4 － 5）
W (F ) r
T T
为此，如果应用虚功原理，则得到 这里，手爪的虚位移 r 和关节的虚位移 之间的关系，用雅克比矩阵表示为
(F ) r 0
T T
（4－6）
r J
（4－7）
把式（4－7）代入式（4－6），提出公因数 ，可得到下式 （ 4－ 8） ( T F T J ) 0
下面看一个例子来理解一下实际上如何使 用虚功原理。如图4－1所示，已知作用在杠杆 一端的力FA，试用虚功原理求作用于另一端的 力FB。假设杠杆长度LA，LB已知。
图4－1
杠杆及作用在它两端上的力
按照虚功原理，杠杆两端受力所作的虚功 应该是
FAx A FBx B 0
（ 4－ 1）
式中 x A ，x B ，是杠杆两端的虚位 移。而就虚位移来讲，下式成立
机器人运动方程的求解可分为两种不同性质的问题：
逆动力学问题。即机器人在关节变量空 间的轨迹已确定，或末端执行器在笛卡尔空 间的轨迹已确定（轨迹已被规划），求解机 器人各执行器的驱动力或力矩，这称为机器 人动力学方程的反面求解，简称为逆动力学 问题。
第二节 机器人的静力学
一、虚功原理 在介绍机器人静力学之前，首先要说明一下 静力学中所需要的虚功原理（principle of virtual work）。 约束力不作功的力学系统实现平衡的必要且 充分条件是对结构上允许的任意位移（虚位移） 施力所作功之和为零。这里所指的虚位移（ virtual displacement）是描述作为对象的系统 力学结构的位移，不同于随时间一起产生的实际 位移。为此用“虚”一词来表示。而约束力（ force of constraint）是使系统动作受到制约的 力。
对于质量连续分布的物体，求解其惯性矩， 可以将其分割成假想的微小物体，然后再把每个 微小物体的惯性矩加在一起。这时，微小物体的 质量 dm 及其微小体积 dV 的关系，可用密度 dm dV 表示为 （4－16 ） 所以，微小物体的惯性矩 dI ，依据式 I m r2 ，可以写成 dI dmr2 r 2 dV （4－17） 因此，整个物体的惯性矩通过积分求得如下：
(i ) (i ) (i ) （4-23） JL JL J 0 0 1 Li
(i ) A


J
J

(i ) A1
J 0 0
(i ) Ai

（4-24）
在式（4－23）和式（4－24）中，包含着0分量 ，这是因为第i个连杆的运动与其以后的关节运动 是无关的。
现在将式（4－21）和式（4－22）代进式（ 4－19）和式（4－20），机器人的运动能量公 式可以写成