1、第四章 机器人的动力学 第一节 前 言 机器人动力学是研究机器人运动数学方程的建立。其实际动力学模型可以根据已知的物理定律(例如牛顿或拉格朗日力学定律)求得。 前面我们所研究的机器人运动学都是在稳态下进前面我们所研究的机器人运动学都是在稳态下进行的，没有考虑机器人运动的动态过程。实际上，行的，没有考虑机器人运动的动态过程。实际上，机器人的动态性能不仅与运动学相对位置有关，机器人的动态性能不仅与运动学相对位置有关，还与机器人的结构形式、质量分布、执行机构的还与机器人的结构形式、质量分布、执行机构的位置、传动装置等因案有关。机器人动态性能由位置、传动装置等因案有关。机器人动态性能由动力学方程描述，
2、动力学是考虑上述因素，研究动力学方程描述，动力学是考虑上述因素，研究机器人运动与关节力机器人运动与关节力(力矩力矩)间的动态关系。描述间的动态关系。描述这种动态关系的微分方程称为机器人动力学方程。这种动态关系的微分方程称为机器人动力学方程。机器人动力学要解决两类问题：机器人动力学要解决两类问题： 动力学动力学正问题正问题和和逆问题逆问题。 正动力学问题。即机器人各执行器的驱动力或力矩为已知，求解机器人关节变量在关节变量空间的轨迹或末端执行器在笛卡尔空间的轨迹，这称为机器人动力学方程的正面求解，简称为正动力学问题。 逆动力学问题。即机器人在关节变量空间的轨迹已确定，或末端执行器在笛卡尔空间的轨迹
3、已确定（轨迹已被规划），求解机器人各执行器的驱动力或力矩，这称为机器人动力学方程的反面求解，简称为逆动力学问题。 简单的讲：简单的讲： 动力学正问题是动力学正问题是根据关节驱动力矩或力，计根据关节驱动力矩或力，计算机器人的运动算机器人的运动(关节位移、速度和加速度关节位移、速度和加速度)； 动力学逆问题是动力学逆问题是已知轨迹对应的关节位移、已知轨迹对应的关节位移、速度和加速度，求出所需要的关节力矩或力。速度和加速度，求出所需要的关节力矩或力。 不考虑机电控制装置的惯性、摩擦、间隙、不考虑机电控制装置的惯性、摩擦、间隙、饱和等因素时，饱和等因素时，n 自由度机器人动力方程为自由度机器人动力方程
4、为n个二个二阶耦合非线性微分方程。方程中包括惯性力阶耦合非线性微分方程。方程中包括惯性力/力矩、力矩、哥氏力哥氏力/力矩、离心力力矩、离心力/力矩及重力力矩及重力/力矩，是一个力矩，是一个耦合的非线性多输入多输出系统。对机器人动力耦合的非线性多输入多输出系统。对机器人动力学的研究，所采用的方法很多，有拉格朗日学的研究，所采用的方法很多，有拉格朗日(Lagrange)方法、牛顿一欧拉方法、牛顿一欧拉(NewtonEuler)、高斯高斯(Gauss)、凯恩、凯恩(Kane)、旋量对偶数、罗伯、旋量对偶数、罗伯逊一魏登堡逊一魏登堡(RobersonWittenburg)等方法。等方法。 研究机器人动
5、力学的目的是多方面的研究机器人动力学的目的是多方面的。 动力学正问题与机器人的仿真有关；动力学正问题与机器人的仿真有关； 逆问题是为了实时控制的需要，利用动力学模型，实现逆问题是为了实时控制的需要，利用动力学模型，实现最优最优控制，以期达到良好的动态性能和控制，以期达到良好的动态性能和最优最优指标指标。在设计中在设计中需根据连杆质量、运动学和动力学需根据连杆质量、运动学和动力学参数参数、传动机构特征和负、传动机构特征和负载大小进行动态仿真，从而决定机器人的结构参数和传动方载大小进行动态仿真，从而决定机器人的结构参数和传动方案，验算设计方案的合理性和可行性，以及结构优化程度案，验算设计方案的合理
6、性和可行性，以及结构优化程度。 在离线编程时，为了估计机器人高速运动引起的动载荷在离线编程时，为了估计机器人高速运动引起的动载荷和路径偏差，要进行路径控制仿真和动态模型仿真和路径偏差，要进行路径控制仿真和动态模型仿真。这些都这些都需要以机器人动力学模型为基础需要以机器人动力学模型为基础。研究机器人动力学的目的研究机器人动力学的目的第二节 机器人的静力学 机器人静力学研究机器人静止或者缓慢运机器人静力学研究机器人静止或者缓慢运动时作用在手臂上的力和力矩问题，特别动时作用在手臂上的力和力矩问题，特别是当手端与外界环境有接触力时，各关节是当手端与外界环境有接触力时，各关节力矩与接触力的关系。力矩与接
7、触力的关系。一、虚功原理 在介绍机器人静力学之前，首先要说明一下静力学中所需要的虚功原理（principle of virtual work）。 约束力不作功的力学系统实现平衡的必要且充分条件是对结构上允许的任意位移（虚位移）施力所作功之和为零。这里所指的虚位移（virtual displacement）是描述作为对象的系统力学结构的位移，不同于随时间一起产生的实际位移。为此用“虚”一词来表示。而约束力（force of constraint）是使系统动作受到制约的力。 下面看一个例子来理解一下实际上如何使用虚功原理。如图41所示，已知作用在杠杆一端的力 ，试用虚功原理求作用于另一端的力 。假
8、设杠杆长度 ， 已知。 AFBFALBL图41 杠杆及作用在它两端上的力 按照虚功原理，杠杆两端受力所作的虚功应该是 （41） 式中 ， ，是杠杆两端的虚位移。而就虚位移来讲，下式成立 （42） 0BBAAxFxFAALxBBLxAxBx 式中， 是绕杠杆支点的虚位移。把式（42）代入式（41）消去 、 ，可得到下式 （43） 由于公式（43）对任意的都成立，所以有下式成立 0)(BBAALFLFAxBx0BBAALFLF 因此得到 （44） 当力 向下取正值时， 则为负值，由于 的正方向定义为向上，所以这时表明 的方向是向下的，即此时 和 的方向都朝下。 AAABFLLFAFBFBFBFBF
9、AF二、机器人静力学关系式的推导 利用前面的虚功原理来推导机器人的静力学关系式。 如图42所示的机械手，要产生图（a）所示的虚位移，推导出图（b）所示各力之间的关系式。这一推导方法本身也适用于一般的情况。图42 机械手的虚位移和施加的力 假设 ：手爪的虚位移为 关节的虚位移为 手爪力为 关节驱动力为 如果施加在机械手上的力作为手爪力的反力（ 用来表示）时，机械手的虚功可表示为： （45） 11,mTmRrrr11,nTnR11,mTmRffF11,nTnRFrFWTT)( 为此，如果应用虚功原理，则得到 （46） 这里，手爪的虚位移 和关节的虚位移 之间的关系，用雅克比矩阵表示为 （47） 把
10、式（47）代入式（46），提出公因数 ，可得到下式 （48） 0)(rFTTrJr 0)(JFTT 由于这一公式对任意的 都成立，因此得到下式成立 （49） 进一步整理，把式中第二项移到等式右边，并取两边的转置，则可得到下面的机械手静力学关系式 （410） 上式表示了机械手在静止状态为产生手爪力 的驱动力 。 0JFTTFJTF 为了加深理解，下面分别求解图43所示的2自由度机械手在图示位置时，生成手爪力 或 的驱动力 或 。图示 为 ， 时的姿态。图43 求生成手爪力或的驱动力 TxAfF0TyBfF0AB)(01rad)(2/2rad由关节角给出如下姿态 则由式（410）可以得到驱动力如下
11、 从求解的结果看到，在这里驱动力的大小为手爪力的大小和手爪力到作用线距离的乘积。 0)cos()cos(cos)sin()sin(sin1222122121121221211LLLLLLLLLJxxxATAfLfLfLLLFJ22212000001212yyBTBfLfLLLFJ三、惯性矩的确定 动力学不仅与驱动力有关，还与绕质心的惯性矩有关。下面以一质点的运动为例，了解惯性矩的物理意义。 如图44所示，若将力 作用到质量为 的质点时的平移运动，看作是运动方向的标量，则可以表示为： （411） 式中， 表示加速度。FmFxm x 若把这一运动看作是质量可以忽略的棒长为 的回转运动，则得到加速度
12、和力的关系式为 （412） （413）图44 质点平移运动作为回转运动的解析 式中 ， 和是绕轴回转的角加速度和惯性矩。 r rx rNF N 将式（412）、（413）代入式（411），得到 （414）如 ，则式（414）就改写为 （415） 上式是质点绕固定轴进行回转运动时的运动方程式。与式（411）比较 相当于平移运动时的质量，在旋转运动中称为惯性矩。 Nmr 22mrI NI I 对于质量连续分布的物体，求解其惯性矩，可以将其分割成假想的微小物体，然后再把每个微小物体的惯性矩加在一起。这时，微小物体的质量 及其微小体积 的关系，可用密度 表示为 （416） 所以，微小物体的惯性矩 ，依
13、据式 ，可以写成 （417） 因此，整个物体的惯性矩通过积分求得如下： （418）dmdVdVdmdI2mrI dVrdmrdI22dVrdII2四、运动学、静力学、动力学的关系 如图45所示，在机器人的手爪接触环境时，手爪力 的驱动力 的关系起重要作用，在静止状态下处理这种关系称为静力学（statics）。图45 手爪力的关节驱动力 F 在考虑控制时，就要考虑在机器人的动作中，关节驱动力 会产生怎样的关节位置 、关节速度 、关节加速度 ，处理这种关系称为动力学（dynamics）。 对于动力学来说，除了与连杆长度 有关之外，还与各连杆的质量 ，绕质量中心的惯性矩 ，连杆的质量中心与关节轴的距
14、离 有关。如图46所示。图46 与动力学有关的各量 iLimCiICiL 运动学、静力学和动力学中各变量的关系如图47所示。图中用虚线表示的关系可通过实线关系的组合表示，这些也可作为动力学的问题来处理。图47 运动学、静力学、动力学的关系 第三节 机器人动力学方程式 一、机器人的动能与位能 1动能 为了导出多关节机器人的运动方程式，首先要了解机器人的动能和位能。先看图48所表示的第个连杆的运动能量。图48 第个连杆的旋转速度和重心的平移速度 刚体的运动能量，是由该刚体的平移构成的运动能量，与该刚体的旋转而构成的运动能量之和表示的。因此，图48中表示的连杆的运动能量，可以用下式表示： （419）
15、 式中， 为连杆的运动能量， 为质量， 为在基准坐标系上表示的重心的平移速度向量， 为在基准坐标系上表示的连杆的转动惯量， 为在基准坐标系上表示的转动速度向量。iiTiciTciiiIvvmK2121iKimciviIi 因为机器人的全部运动能量 ，由各连杆的运动能量的总和表示，所以得到 （420） 式中， 为机器人的关节总数。其次我们来考虑把作为机器人各关节速度的函数。这里 与 分别表示如下： （421） （422）KniiKK1nciviqJviLci)(qJiAi)( 式中， 是与 第个连杆重心位置的平移速度相关的雅可比矩阵， 是与 第个连杆转动速度相关的雅可比矩阵。为了区别于与指尖速度
16、相关的雅可比矩阵，在上面标明了注角（ ）。 （423） （4-24） 在式（423）和式（424）中，包含着0分量，这是因为第 个连杆的运动与其以后的关节运动是无关的。 )(iLJi)(iAJii00)()(1)(iLiiLiLJJJ00)()(1)(iAiiAiAJJJi 现在将式（421）和式（422）代进式（419）和式（420），机器人的运动能量公式可以写成 （425）令 （426）则机器人的运动能量公式（425）写为 （427）这里 表示的称为机器人的惯性矩阵。 )(21)()()()(1qJIJqqJJqmKiAiTiATiLTiLTini)()()()()(1iAiTiAiLTi
17、LiniJIJJJmHqHqKT21H2势能 机器人的势置能量和运动能量一样，也是由各连杆的位置能量的总和给出，因此可用下式表示： （428） 式中， 表示重力加速度，它是一个在基准坐标系上表示的三维向量。 表示从基准坐标系原点，到 个连杆的重心位置的位置向量。 iCTniirgmP, 01giCr, 0i二、机器人动力学方程的建立举例 1牛顿欧拉方程式 首先，以单一刚体为例，如图49所示，其运动方程式可用下式表示 （429） （430） 图49 单一刚体 ccFvmNIIcc)( 式（429）和式（430）分别被称为牛顿运动方程式及欧拉运动方程式。式中， 是刚体的质量； 是绕重心 的惯性矩阵
18、， 的各元素表示对应的力矩元素和角加速度元素间的惯性矩； 是作用于重心的平动力； 是惯性矩； 是重心的平移速度； 是角速度。 m33RIcCcIcFNcv 下面求解一下图410所示的1自由度机械手的运动方程式，在这里，由于关节轴制约连杆的运动，所以可以将式（430）的运动方程式看作是绕固定轴的运动。图410 1自由度机械手 假设绕关节轴的惯性矩为 ，取垂直纸面的方向为轴 ，则得到 （431） （432）Iz II000000000IIcos00cmgLN 式中， 是重力常数； 是在第3行第3列上具有绕关节轴惯性矩的惯性矩阵。把这些公式代入式（430），提取只有 分量的回转，则得到 （433）
19、该式为1自由度机械手的欧拉运动方程式，其中： （434） 对于一般形状的连杆，在式（431）中，由于 除第3分量以外其他分量皆不为0，所以 的第1、2分量成了改变轴方向的力矩，但在固定轴的场合，与这个力矩平衡的约束力生成式（432）的第1、2分量，不产生运动。 g33 RIczcoscmgLI ccmLIIII2拉格朗日方程式 拉格朗日运动方程式一般表示为 （435）式中， 是广义坐标， 是广义力。 拉格朗日运动方程式也可以表示为 （436）这里， 是拉格朗日算子， 是动能， 是势能。 qLqLdtdqPKLLKP 现在再以前面推导的1自由度机械手为例，利用拉格朗日运动方程式来具体求解，假设
20、为广义坐标，则得到 由于 所以用 置换式（435）中的广义坐标 后，可得到下式 （437）该式与前面推导的结果完全一致。 221IK sincmgLP sin212cmgLILILcoscmgLLqcoscmgLI 下面推导2自由度机械手的运动方程式，如图411所示。在推导时，把 ， 当作广义坐标， ， 当作广义力，求拉格朗日算子，代入式（435）的拉格朗日运动方程式即可。 （4-38） （439）图411 2自由度机械手 121221111112121CCTCIppmK1111sinCgLmP （440） （441） 式中， 是第 个连杆质量中心的位置向量。 （442） （443） （444
21、） （445） 根据理论力学的知识，各连杆的动能可用质量中心平移运动的动能和绕质量中心回转运动的动能之和来表示。 22122222)(2121CCTCIppmK)sin(sin2121122CLLgmPTCiyCixCipppi111cosLpxC111sinLpyC)cos(cos212112cxCLLp)sin(sin212112cyCLLp 由式（442）（445），得到式（438），（440）中的质量中心速度和为 （446） （447） 利用式（438）（441）和式（446）、（447），通过下式 （448） 212211CCTCLpp221212122122212122cos)(2
22、)(CCCTCLLLLpp2121PPKKL 可求出拉格朗日算子 ，把它代入式（435）的拉格朗日运动方程式，整理后可得到 （449）式中 （450） （451）L)(),()(gcM 22211211)(MMMMM21),(ccc21)(ggg222122212121111)cos2(CCCCCILLLLmILmM （452） （453） （454） （455） （456） （457） （458）222122212)cos(CCCILLLmM1221MM222222CCILmM221222121sin)2(CLLmc2212122sin)(CLLmc )cos(coscos2121121111CCLLgmgLmg)cos(21222CgLmg 是惯性力； 是离心力； 表示加在机械手上的重力项， 是重力加速度常数。 对于多于3个自由度的机械手，也可用同样的方法推导出运动方程式，但随着自由度的增多演算量将大量增加。 与此相反，着眼于每一个连杆的运动，求其运动方程式的牛顿欧拉法，即便对于多自由度的机械手其计算量也不增加，其算法易于编程。只是其运动方程式就不是式（449）的形式，由于推导出的是一些列公式的组合，要注意惯性矩阵等的选择和求解问题。进一步的问题请参考相关文献资料。 )(M),( c)(gg
