第6章_机器人动力学

1、1 第第6 6章章 机器人动力学机器人动力学 机器人的运动是通过在关节轴上施加驱动力来实现的。机器人运动与驱动机器人的运动是通过在关节轴上施加驱动力来实现的。机器人运动与驱动 力的关系称为机器人动力学，是本章要讨论的主要问题。力的关系称为机器人动力学，是本章要讨论的主要问题。 机器人动力学问题分为两类：机器人动力学问题分为两类： 一类是已知作用在机器人上驱动力随时间的变规律，求机器人的运动规一类是已知作用在机器人上驱动力随时间的变规律，求机器人的运动规 律（位置、速度和加速度轨迹），称为机器人正动力学问题；律（位置、速度和加速度轨迹），称为机器人正动力学问题； 另一类是已知机器人随时间的运动规

2、律，求期望的驱动力，称为机器人另一类是已知机器人随时间的运动规律，求期望的驱动力，称为机器人 逆动力学问题。逆动力学问题。 n刚体定轴转动与惯性矩刚体定轴转动与惯性矩 刚体定轴转动微分方程：刚体定轴转动微分方程： I 其中其中I称为绕固定轴的惯性矩（也称为转动惯量），称为绕固定轴的惯性矩（也称为转动惯量）， 是作用在固定轴上的是作用在固定轴上的 合外力矩。合外力矩。 质量为质量为m的质点，其在直线上运动的动力学问题可以用牛顿第二定律描述：的质点，其在直线上运动的动力学问题可以用牛顿第二定律描述： mvf或者或者mxf 比较（比较（6-16-1）和（）和（6-26-2）式可以发现，刚体定轴转动和

3、质点的直线运动的动力）式可以发现，刚体定轴转动和质点的直线运动的动力 学方程的形式是完全相同的。因此，学方程的形式是完全相同的。因此，I I可以看成刚体定轴转动的惯性质量。可以看成刚体定轴转动的惯性质量。 （6-2） （6-1） 2 图图6-1 6-1 圆盘绕过圆心轴惯性矩圆盘绕过圆心轴惯性矩 下面以图下面以图6-1所示质量为所示质量为M半径为半径为R的均匀圆盘绕过的均匀圆盘绕过 圆心的圆心的Z轴的惯性矩计算问题给出惯性矩的定义：轴的惯性矩计算问题给出惯性矩的定义： 2 V Ir dm （6-3）式给出了任意刚体绕固定轴惯性矩的定义，其中）式给出了任意刚体绕固定轴惯性矩的定义，其中dm是微元体

4、质量，是微元体质量，r是是 微元体到转轴的距离，微元体到转轴的距离，V是刚体的体积，因此（是刚体的体积，因此（6-3）表示在整个体积上积分。）表示在整个体积上积分。 对于图对于图6-1所示均匀圆盘，面密度所示均匀圆盘，面密度 =M/( R2)，取极坐标微元体，则，取极坐标微元体，则 44 2 222 2 00 1 22 442 R V RMR Ir dmrrdrdMR R 图图6-2 6-2 匀质杆绕质心惯性矩匀质杆绕质心惯性矩 例例6-1 如图如图6-2 所示匀质杆，质量为所示匀质杆，质量为M，杆长为，杆长为L， 计算绕质心的惯性矩。计算绕质心的惯性矩。 解：匀质杆的线密度解：匀质杆的线密度

5、 =M/L，取微元体，取微元体 dx，则，则 3 /2/2 22 /20 3 2 (/ 2) 22 3 1 2 3 812 LL L L Ix dmxdx ML ML L 3 平行移轴定理：平行移轴定理：刚体绕任意平行于质心轴的惯性矩为刚体绕任意平行于质心轴的惯性矩为 2C IIMd 其中其中CI 表示刚体绕质心轴的惯性矩，表示刚体绕质心轴的惯性矩，M为刚体质量，为刚体质量，d为两轴之间的距离。为两轴之间的距离。 若已知刚体绕质心轴的惯性矩，则刚体绕任意平行轴的惯性矩可以非常方便若已知刚体绕质心轴的惯性矩，则刚体绕任意平行轴的惯性矩可以非常方便 地利用平行移轴定理（地利用平行移轴定理（6-56

6、-5）进行计算。）进行计算。 （6-5） 例如，计算图例如，计算图6-26-2所示匀质杆绕杆端点的惯性矩，根据平行移轴定理，所示匀质杆绕杆端点的惯性矩，根据平行移轴定理， 2222 11 ( ) 1223 C L IIMdMLMML 可以验证，与采用积分方法计算的结果相同。可以验证，与采用积分方法计算的结果相同。 n刚体的惯性张量刚体的惯性张量 对于在三维空间自由运动的刚体，存在无穷多对于在三维空间自由运动的刚体，存在无穷多 个可能转轴，计算绕所有转轴的惯性矩显然是个可能转轴，计算绕所有转轴的惯性矩显然是 不现实的。不现实的。 图图6-3 6-3 空间刚体的惯性张量空间刚体的惯性张量 因此需要

7、考虑这样的问题：因此需要考虑这样的问题： 是否存在一个量，它能够表示刚体绕任意转轴的是否存在一个量，它能够表示刚体绕任意转轴的 惯性矩？惯性矩？ 答案是肯定的，该量就是刚体的答案是肯定的，该量就是刚体的惯性张量惯性张量。 它描述了刚体的三维质量分布，若惯性张量在某坐标系下表示出来，它它描述了刚体的三维质量分布，若惯性张量在某坐标系下表示出来，它 是一个是一个3 3阶对称矩阵。阶对称矩阵。 定义了固连的坐标系定义了固连的坐标系 A A ，在坐标系，在坐标系 A A 中惯性张量为：中惯性张量为： 4 xxxyxz A xyyyyz xzyzzz III IIII III 惯性张量是一个对称矩阵，各

8、元素的值为，惯性张量是一个对称矩阵，各元素的值为， 22 22 22 xxxy VV yyxz VV zzyz VV IyzdvIxy dv IxzdvIxz dv IxydvIyz dv 其中其中dv表示单元体，表示单元体， 表示单元体密度，单元体的位置表示单元体密度，单元体的位置Ar =x y zT。 惯性张量中惯性张量中Ixx，Iyy和和Izz称为惯性矩，交叉项称为惯性矩，交叉项Ixy，Ixz和和Iyz称为惯性积。称为惯性积。 惯性张量中元素的数值与坐标系的选择有关，一般存在某个坐标系，使得交惯性张量中元素的数值与坐标系的选择有关，一般存在某个坐标系，使得交 叉项全为叉项全为0 0。 称

9、其坐标轴为惯性主轴，该坐标系称为惯性主轴坐标系。称其坐标轴为惯性主轴，该坐标系称为惯性主轴坐标系。 对于质量均匀分布的规则物体，惯性主轴就是物体的对称轴。对于质量均匀分布的规则物体，惯性主轴就是物体的对称轴。 例例6-2 6-2 如图如图6-46-4所示质量均匀分布的长方形刚体，密所示质量均匀分布的长方形刚体，密 度为度为r r，质量为，质量为MM，计算其惯性张量。，计算其惯性张量。 解：单元体解：单元体dv=dxdydz，根据（，根据（6-8）得：）得： （6-8） 5 2222 000 2232 000 33 22 /3 ()() 333 HLW xx V HLH Iyzdvyzdxdyd

10、z WyzdydzWLLzdz HLLHM WLH 同理可以得到另外两个惯性矩，同理可以得到另外两个惯性矩， 2222 (),() 33 yyzz MM IWHIWL 下面计算惯性积，下面计算惯性积， 2 00000 2222 0 /2 444 HLWHL xy V H Ixy dvxydxdydzyWdydz W LW L HM dzWL 同理可以得到另外两个惯性积，同理可以得到另外两个惯性积， , 44 xzyz MM IWHIHL 对于惯性张量的计算问题，平对于惯性张量的计算问题，平 行移轴定理也是成立的，下面行移轴定理也是成立的，下面 给出其中两个表达式，其余的给出其中两个表达式，其余

11、的 四个表达式与此类似：四个表达式与此类似： 22 () () AC zzzzcc AC xyxycc IIM xy IIM x y 6 22 () () AC zzzzcc AC xyxycc IIM xy IIM x y 式中是式中是xc yc zcT是刚体质心在是刚体质心在A坐标系下的坐标。需要说明的是，在使用平行坐标系下的坐标。需要说明的是，在使用平行 移轴定理时，移轴定理时，A坐标系和质心坐标系坐标系和质心坐标系C的姿态必须相同。的姿态必须相同。 例例6-3 6-3 如图如图6-46-4所示质量均匀分布的长方形刚体在质心坐标系（原点位于质心，所示质量均匀分布的长方形刚体在质心坐标系（

12、原点位于质心， 坐标系姿态与原坐标系姿态相同）下表示的惯性张量。坐标系姿态与原坐标系姿态相同）下表示的惯性张量。 解：根据平行移轴定理计算，其中解：根据平行移轴定理计算，其中 1 2 c c c xW yL zH 因此得，因此得， 22222222 ()()()() () 32212 ()()0 422 CA zzzzcc CA xyxycc MLWM IIM xyLWMLW MLW IIM x yWLM 其它四个值可以采用类似的方法获得。在质心坐标系其它四个值可以采用类似的方法获得。在质心坐标系C下，刚体的惯性张量为下，刚体的惯性张量为 22 22 22 00 00 12 00 C HL M

13、 IWH LW 结果是对角矩阵，此时坐标系结果是对角矩阵，此时坐标系C的坐标轴是刚体的的坐标轴是刚体的 惯性主轴。惯性主轴。 其中其中MM表示刚体质量，表示刚体质量，F F表示作用在刚体上的合外力矢量，表示作用在刚体上的合外力矢量， 表示质心速度矢量。表示质心速度矢量。 7 n刚体的牛顿刚体的牛顿- -欧拉方程欧拉方程 在动力学分析过程中，把刚体的运动分解为质心的平移运动和绕质心的转动在动力学分析过程中，把刚体的运动分解为质心的平移运动和绕质心的转动 。一般将连体坐标系的原点固定在刚体的质心，这样坐标原点的运动描述刚。一般将连体坐标系的原点固定在刚体的质心，这样坐标原点的运动描述刚 体的平移运

14、动，坐标系的转动描述刚体绕质心的旋转运动。体的平移运动，坐标系的转动描述刚体绕质心的旋转运动。 刚体质心的平动用牛顿第二定律描述刚体质心的平动用牛顿第二定律描述 c MF v c v 刚体绕质心的转动用欧拉方程描述刚体绕质心的转动用欧拉方程描述 CC IIN 其中其中CI表示刚体在质心坐标系表示刚体在质心坐标系C下表示的惯性张量，下表示的惯性张量，N表示作用在刚体上的合表示作用在刚体上的合 外力矩矢量，外力矩矢量， 表示角速度矢量，表示角速度矢量， 表示角加速度矢量。表示角加速度矢量。 式（式（6-116-11）和（）和（6-126-12）一起称为刚体的牛）一起称为刚体的牛 顿顿- -欧拉方程

15、。分析机械臂的动力学问题时欧拉方程。分析机械臂的动力学问题时 ，首先对每个连杆列出牛顿，首先对每个连杆列出牛顿- -欧拉方程，同欧拉方程，同 时需要分析连杆间的速度、加速度传递关时需要分析连杆间的速度、加速度传递关 系以及力的传递关系。系以及力的传递关系。 6-11） （6-12） 8 n拉格朗日方程拉格朗日方程 上节介绍牛顿上节介绍牛顿- -欧拉方程是采用几何矢量方法建立每个连杆的动力学方程，方程欧拉方程是采用几何矢量方法建立每个连杆的动力学方程，方程 中会出现约束力项。拉格朗日方程采用解析方法建立系统的动力学方程，在理中会出现约束力项。拉格朗日方程采用解析方法建立系统的动力学方程，在理 想

16、约束条件下，动力学方程中不出现约束力项。想约束条件下，动力学方程中不出现约束力项。 定义拉格朗日函数定义拉格朗日函数L=K-P 其中其中K是系统动能，是系统动能，P是系统势能，系统动力学方程为是系统势能，系统动力学方程为 (),1,2, i ii dLL Fin dtqq (6-14) 其中其中qi是描述系统位置的坐标，称为广义坐标，是描述系统位置的坐标，称为广义坐标，Fi是作用在是作用在qi上的广义力。分量上的广义力。分量 形式的方程形式的方程(6-14)也可以写成矢量形式如下：也可以写成矢量形式如下： () dLL dt F q q (6-15) 式（式（6-146-14）或（）或（6-1

17、56-15）称为第二类拉格朗日方程。）称为第二类拉格朗日方程。 9 例例6-4 如图如图6-6所示单摆由一根无质量杆末端连接一集中质所示单摆由一根无质量杆末端连接一集中质 量量m，杆长为，杆长为l，其上作用力矩，其上作用力矩 ，建立系统的动力学方程。，建立系统的动力学方程。 解：解： 牛顿牛顿- -欧拉方法欧拉方法 单摆运动可以简化为刚体的定轴转动，其动力学方程为单摆运动可以简化为刚体的定轴转动，其动力学方程为IN 转动惯量和合外力矩计算如下，转动惯量和合外力矩计算如下， 2 Iml sinNmgl 因此，系统的动力学为因此，系统的动力学为 2 sinmlmgl 拉格朗日方程拉格朗日方程 选择

18、选择 为描述单摆位置的广义坐标，为描述单摆位置的广义坐标， sin ,cos cos,sin xlyl xlyl 系统的动能系统的动能 22222222222 11 cossin 2222 mm Kmvxyllml 取坐标原点为势能零点，则系统的势能取坐标原点为势能零点，则系统的势能 cosPmgymgl 系统的拉格朗日函数系统的拉格朗日函数 22 1 cos 2 LKPmlmgl 10 (),1,2, i ii dLL Fin dtqq (6-14) 221 cos 2 Lmlmgl 根据根据614614式计算相应的导数式计算相应的导数 22 dLd mlml dtdt sin L mgl

19、代入到拉格朗日方程得系统的动力学方程代入到拉格朗日方程得系统的动力学方程 2 sinmmg 计算结果与采用牛顿计算结果与采用牛顿 欧拉方法计算的结果相同。欧拉方法计算的结果相同。 例例6-5 如图如图6-7所示两连杆平面机械臂。连杆所示两连杆平面机械臂。连杆 长都分别为长都分别为L1和和L2，连杆质量分别为，连杆质量分别为m1和和m2，质，质 心到杆端点距离分别为心到杆端点距离分别为Lc1和和Lc2，两杆绕质心转动，两杆绕质心转动 惯量分别为惯量分别为Ic1和和Ic2，两个关节上作用驱动力矩，两个关节上作用驱动力矩 1 和和 2，建立系统的动力学方程，建立系统的动力学方程 非定轴转动刚体的动能

20、表示为质心平移动能和非定轴转动刚体的动能表示为质心平移动能和 绕质心转动动能之和绕质心转动动能之和。 22 11 22 cc KmvI 其中其中vc是质心速度的大小，是质心速度的大小， 是刚体的角速度。是刚体的角速度。 11 两杆的动能分别为两杆的动能分别为 2222 1111122222 1111 , 2222 cccc KmvIKm vI 选择选择 1 1 和和 2 2为描述连杆位置的广义坐标，先为描述连杆位置的广义坐标，先 用广义坐标表示质心的位置，用广义坐标表示质心的位置， 11 111 1 21 12 1221 12 12 , , CcCC CCCC xL cyL s xLcL cy

21、LsL s 再对时间求导得到质心的速度再对时间求导得到质心的速度 11 1 111 1 1 21 1 12 121221 1 12 1212 , , CCCC CCCC xL syL c xL sL syLcL c 两杆的转动角速度分别为两杆的转动角速度分别为 11212 , 因此，两杆的动能为，因此，两杆的动能为， 22222 11111 11111 111 222 CCCCC KmxyIIm L 2 22 2222212 11 22 CCC KmxyI 12 22 222 21121212 2 112212 11 2 22 CCC mLLL L cI 系统的总动能可以表示为系统的总动能可以

22、表示为 11112 1212 2122 2 11 22 T MM KKKM MM qq 其中其中 222 111211212122 2 2112222122 2 2222 2 () CCCCC CCC CC MIIm LmLLL L c MMIm LL L c MImL 取固定在基座处的坐标原点为势能零点，系统的总势能为，取固定在基座处的坐标原点为势能零点，系统的总势能为， 11 121 22 12 () CC Pm gL sm g LsL s 系统的拉格朗日函数为系统的拉格朗日函数为LKP 直接代入到拉格朗日方程直接代入到拉格朗日方程614614，即可得到系统的动力学方程。当然，导数，即可得

23、到系统的动力学方程。当然，导数 的计算过程是比较复杂的。下面分析方程的结构，的计算过程是比较复杂的。下面分析方程的结构， 222222 22121221222222212212 11 2 22 CCCCCCCC ImLLL L cImLIm Lm L L c 13 1 2 T dLLdKPK dtdt dPK M dt MP MM qqqqq q q qq q qqq qq 拉格朗日方程可以表示为拉格朗日方程可以表示为 ( )( , )Mq qC q qG 616 其中其中M是对称正定质量矩阵，是对称正定质量矩阵，C是离心力和柯氏力项，是离心力和柯氏力项，G是重力项。是重力项。C和和G如下：如

24、下： 1 , 2 TMP M CqqqG qq 下面计算各量的具体值，下面计算各量的具体值， 1212 2212 22 12212 2112 2 212 2 2 2 1 02 CC C C m L L sm L L s K m L L s m L L s 11 1 0 2 TKM qq 2 212 21 22 11 212 22212 22 212 22 212 22 22 212 2 1 2 2 212 100 C CC C C C m L L s m L L sm L L s Mm L L s m L L s m L L s q 2 122 212 2 2 1 2 1 , 2 T C M

25、Mm L L s Cqqq q 11 122 12 22 12 CC C m L cm L cg P m L c g G q 14 图图6868小车倒立摆系统小车倒立摆系统 例例6-6 如图如图6-8所示在竖直平面内运动的小车所示在竖直平面内运动的小车 倒立摆系统，假设小车质量倒立摆系统，假设小车质量M，倒立摆连杆长度，倒立摆连杆长度 为为L，末端集中质量为，末端集中质量为m，重力加速度为，重力加速度为g，忽略，忽略 连杆质量和转动惯量。选择图中给定的坐标系，连杆质量和转动惯量。选择图中给定的坐标系， 用广义坐标用广义坐标(x和和 ) 表示小车中心和倒立摆集中质表示小车中心和倒立摆集中质 量的

26、位置，计算系统的动能和势能，并采用拉格量的位置，计算系统的动能和势能，并采用拉格 朗日方法建立系统的动力学方程。朗日方法建立系统的动力学方程。 解：先用广义坐标表示两个集中质量的位置，然解：先用广义坐标表示两个集中质量的位置，然 后计算其速度后计算其速度 0 M M xx y 0 M M xx y sin cos m m xxL yL cos sin m m xxL yL 系统的动能为系统的动能为 222 22222 222 11 () 22 11 ()(2cos ) 22 11 ()cos 22 MMM mmm Mm KM xyMx Km xym xLLx KKKMm xmLmLx 15 取

27、取y=0为势能零点，则为势能零点，则 P=mgLcos ，将，将L=K-P 代入拉格朗日方程得：代入拉格朗日方程得： 222 11 ()cos 22 KMm xmLmLx 0 dLL dt qq 2 2 ()cossin cossin0 Mm xmLmLF mLxmLmLg u双足机器人动力学双足机器人动力学 日本的类人机器人日本的类人机器人ASIMO 和和HRP-2P代代 表了双足机器人研究的最高水平。机器人表了双足机器人研究的最高水平。机器人 一般采用零力矩点（一般采用零力矩点（Zero Moment Point, ZMP）方法规划机器人关节的轨迹，然后）方法规划机器人关节的轨迹，然后 通

28、过控制伺服电机使机器人的关节跟踪规通过控制伺服电机使机器人的关节跟踪规 划的轨迹。该方法的主要缺点是计算非常划的轨迹。该方法的主要缺点是计算非常 复杂，同时和人类行走相比需要消耗非常复杂，同时和人类行走相比需要消耗非常 多的能量。多的能量。 图图6-9所示的被动行走（所示的被动行走（Passive Dynamic Walking，PDW）机器人不需要）机器人不需要 外部的能量输入，可以在重力的作用下稳外部的能量输入，可以在重力的作用下稳 定地走下小的斜坡。定地走下小的斜坡。 图图6969两连杆双足机器人两连杆双足机器人 16 在行走的每一步，机器人从重力势能的变化中获在行走的每一步，机器人从重

29、力势能的变化中获 取能量，脚与地面的冲击作用耗散机器人的能量。取能量，脚与地面的冲击作用耗散机器人的能量。 图图6969两连杆双足机器人两连杆双足机器人 如果初始条件和坡度的组合适当，每一如果初始条件和坡度的组合适当，每一 步获取的能量和耗散的能量恰好平衡，就可步获取的能量和耗散的能量恰好平衡，就可 以得到稳定的双足被动行走步态。以得到稳定的双足被动行走步态。 双足被动行走步态设计通常采用牛顿迭代双足被动行走步态设计通常采用牛顿迭代 算法，对于给定的坡度和机器人模型，确定算法，对于给定的坡度和机器人模型，确定 机器人运动的初始条件。机器人运动的初始条件。 双足机器人行走过程可以分为腿摆动期和斜

30、面冲击两个阶段。双足机器人行走过程可以分为腿摆动期和斜面冲击两个阶段。 在腿摆动阶段，支撑腿与斜面交点可以简化为铰链，支撑腿绕该点转动，在腿摆动阶段，支撑腿与斜面交点可以简化为铰链，支撑腿绕该点转动， 摆动腿从支撑腿后方摆动到其前面。摆动腿从支撑腿后方摆动到其前面。 在冲击阶段，摆动腿下落与地面发生冲击，同时完成支撑腿和摆动腿的转换。在冲击阶段，摆动腿下落与地面发生冲击，同时完成支撑腿和摆动腿的转换。 腿摆动阶段的动力学方程：腿摆动阶段的动力学方程： ( )( , )0Mq qC q qG 617 17 u地面瞬时冲击地面瞬时冲击 假设地面冲击作用是瞬时的，采用下面假设假设地面冲击作用是瞬时的

31、，采用下面假设 冲击是完全塑性的冲击是完全塑性的 冲击的同时完成支撑腿与摆动腿的转换冲击的同时完成支撑腿与摆动腿的转换 冲击时腿与地面之间没有滑动冲击时腿与地面之间没有滑动 根据动量矩守恒可以得到冲击前后的速度关系根据动量矩守恒可以得到冲击前后的速度关系 ( )( ) QQ 618 其中其中 12()/2 ，公式中的，公式中的-和和+分别代表冲击前和冲击后的相应量。分别代表冲击前和冲击后的相应量。 定义摆动腿末端到行走平面的距离定义摆动腿末端到行走平面的距离 12 cos()cos() tip yl 对时间求导可以得到对时间求导可以得到 2211 cos()cos() tip yl 冲击时应该

32、满足以下条件：冲击时应该满足以下条件： 1)1)摆动腿在支撑腿之前。摆动腿在支撑腿之前。 2) 2) 摆动腿与地面接触。摆动腿与地面接触。 3 3 摆动腿的末端向下运动。摆动腿的末端向下运动。 21 0 tip y 0 tip y 则则 是一个周期解。是一个周期解。 18 u周期步态与极限环周期步态与极限环 定义状态变量定义状态变量 , Tx 双足机器人的行走模型双足机器人的行走模型617 620617 620组成一个混合系统：组成一个混合系统： 考虑没有力矩输入的被动行走，考虑没有力矩输入的被动行走， 0 ( ) (0) () swing events xf x xx xh x 623 这里

33、这里events表示腿与地面冲击，表示腿与地面冲击，-和和+分别代表冲击前和冲击后的状态变量。分别代表冲击前和冲击后的状态变量。 双足机器人的周期步态对应系统的周期解，双足机器人的周期步态对应系统的周期解，( )()ttTxx，T是步态的周期。是步态的周期。 系统系统623623的解可以表示为的解可以表示为 0 ( )(, )ttx x，如果存在，如果存在x* *和时间和时间T T 使得使得 *( *, )Tx x( )( *, )ttx x 对于对于2 2维状态空间，周期解对应于一个闭环，如果周期解是孤立的称为极限环。维状态空间，周期解对应于一个闭环，如果周期解是孤立的称为极限环。 若从极限环附近出发的解收敛到极限环，则称极限环是稳定的。双足若从极限环附近出发的解收敛到极