（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－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来表示）时，机械手的虚功 可表示为：
则由式（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
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第个连杆重心位置的平移速度
因此得到
LA FB FA LB
（4－4）
当力FA向下取正值时，FB则为负值，由于FB 的正方向定义为向上，所以这时表明FB的方向是 向下的，即此时FA和FB的方向都朝下。
二、机器人静力学关系式的推导
利用前面的虚功原理来推导机器人的静力 学关系式。 如图4－2所示的机械手，要产生图（a） 所示的虚位移，推导出图（b）所示各力之间 的关系式。这一推ห้องสมุดไป่ตู้方法本身也适用于一般的 情况。
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 － 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
(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），机器人的运动能量公 式可以写成
第四章 机器人的动力学初步
第一节 前 言 机器人动力学是研究机器人运动数学 方程的建立。其实际动力学模型可以根据 已知的物理定律(例如牛顿或拉格朗日力学 定律)求得。
机器人运动方程的求解可分为两种不同性质的问题：
正动力学问题。即机器人各执行器的驱 动力或力矩为已知，求解机器人关节变量在 关节变量空间的轨迹或末端执行器在笛卡尔 空间的轨迹，这称为机器人动力学方程的正 面求解，简称为正动力学问题。
由于这一公式对任意的 下式成立
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) 时的姿态。
对于动力学来说，除了与连杆长度 Li 有关之外 ，还与各连杆的质量 mi ，绕质量中心的惯性矩 I Ci ，连杆的质量中心与关节轴的距离 L 有关。如 Ci 图4－6所示。
图4－6 与动力学有关的各量
运动学、静力学和动力学中各变量的关系如 图4－7所示。图中用虚线表示的关系可通过实线 关系的组合表示，这些也可作为动力学的问题来 处理。
下面求解一下图4－10所示的1自由度机械手 的运动方程式，在这里，由于关节轴制约连杆的 运动，所以可以将式（4－30）的运动方程式看 作是绕固定轴的运动。
图4－10 1自由度机械手
假设绕关节轴的惯性矩为 I ，取垂直纸面的方 向为轴 z ，则得到
0 0 I I
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
将式（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
则机器人的运动能量公式（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 个连杆的重心位置的位置向量。
对于质量连续分布的物体，求解其惯性矩， 可以将其分割成假想的微小物体，然后再把每个 微小物体的惯性矩加在一起。这时，微小物体的 质量 dm 及其微小体积 dV 的关系，可用密度 dm dV 表示为 （4－16 ） 所以，微小物体的惯性矩 dI ，依据式 I m r2 ，可以写成 dI dmr2 r 2 dV （4－17） 因此，整个物体的惯性矩通过积分求得如下：
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－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）是使系统动作受到制约的 力。
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 是重心的平移速度； 是角速度。 ；
图4－3 求生成手爪力或的驱动力