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