机器人动力学

1、机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学3.1 3.1 引言引言3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.3 3.3 拉格朗日方程法拉格朗日方程法 习题习题机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学动力学研究的问题动力学研究的问题： 机器人各个机器人各个关节的运动关节的运动与与关节关节需要的驱动力（矩）需要的驱动力（矩）之间的关系。之间的关系。正问题正问题：已知关节运动，求：已知关节运动，求 关节驱动力（矩）。关节驱动力（矩）。逆问题逆问题：已知关节驱动力（矩），：已知关节驱动力（矩）， 求关节运动。求关节运动。机器人及其控制原理 第第3 3章章 机

2、器人动力学机器人动力学数学模型数学模型： 关节运动关节运动位移、速度、加速度变化位移、速度、加速度变化 关节驱动力（矩）关节驱动力（矩）驱动力或驱动力矩驱动力或驱动力矩i动力学方程动力学方程： ， i=1，n正问题正问题：已知：已知 ，求，求i。逆问题逆问题：已知：已知i ，求，求 。),(iiiiqqqf iiiqqq ,iiiqqq ,iiiqqq ,机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学3.1 3.1 引言引言3.1.1 静力学分析静力学分析3.1.2 动力学分析动力学分析机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学l2l13.1.1 静力学分析静力

3、学分析机器人各个关节处于静止状态。机器人各个关节处于静止状态。当负载为一重物时：当负载为一重物时：关节承受的力和力矩：关节承受的力和力矩：关节需要的驱动力（矩）：关节需要的驱动力（矩）：mgf3=mgf2=mgf1=mgm2=mgl2m1=mg(l1+l2)1=02=mgl23=mg3.1 3.1 引言引言机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学机器人各个关节处于静止状态。机器人各个关节处于静止状态。考虑杆件自重时：考虑杆件自重时：关节承受的力和力矩：关节承受的力和力矩：关节需要的驱动力（矩）：关节需要的驱动力（矩）：mgf3=mgf2=mgf1=mgm2=mgl2l2l1

4、m1=mg(l1+l2)1=02=mgl23=mgm3gm2gm1g3.1 3.1 引言引言3.1.1 静力学分析静力学分析机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学 机器人各个关节处于运动状态。机器人各个关节处于运动状态。 当负载为一重物时：当负载为一重物时：关节承受的力和力矩：关节承受的力和力矩：关节需要的驱动力（矩）：关节需要的驱动力（矩）：3.1 3.1 引言引言3.1.2 动力学分析动力学分析f3f2f1m2l2l1m112311, 22, 33,dd m33dmmg 121)( llm 2121)( llm 机器人及其控制原理 第第3 3章章 机器人动力学机器人动

5、力学牛顿牛顿欧拉方程法原理欧拉方程法原理：将机器人的每个杆件看成刚体：将机器人的每个杆件看成刚体，并确定每个杆件，并确定每个杆件质心的位置质心的位置和表征其质量分布的和表征其质量分布的惯性惯性张量矩阵张量矩阵。当确定机器人坐标系后，根据机器人关节速。当确定机器人坐标系后，根据机器人关节速度和加速度，则可先由机器人机座开始向手部杆件度和加速度，则可先由机器人机座开始向手部杆件正向正向递推递推出每个杆件在自身坐标系中的速度和加速度，再用出每个杆件在自身坐标系中的速度和加速度，再用牛顿牛顿欧拉方程得到机器人每个杆件上的惯性力和惯欧拉方程得到机器人每个杆件上的惯性力和惯性力矩，然后再由机器人末端关节开

6、始向第一个关节性力矩，然后再由机器人末端关节开始向第一个关节反反向递推向递推出机器人每个关节上承受的力和力矩，最终得到出机器人每个关节上承受的力和力矩，最终得到机器人每个关节所需要的驱动力（矩），这样就确定了机器人每个关节所需要的驱动力（矩），这样就确定了机器人关节的驱动力（矩）与关节位移、速度和加速度机器人关节的驱动力（矩）与关节位移、速度和加速度之间的函数关系，即建立了机器人的动力学方程。之间的函数关系，即建立了机器人的动力学方程。 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学牛顿牛顿欧拉方程法递推过程欧拉方程法递推过程：正向递推

7、正向递推： 已知机器人各个关节的速度和加速度已知机器人各个关节的速度和加速度 从从1n递推出机器人每个杆件在自身坐标系中的速度和加递推出机器人每个杆件在自身坐标系中的速度和加速度速度 机器人每个杆件质心上的速度和加速度机器人每个杆件质心上的速度和加速度 再用牛顿再用牛顿欧拉方程得到机器人每个杆件质心上欧拉方程得到机器人每个杆件质心上的惯性力和惯性力矩。的惯性力和惯性力矩。反向递推反向递推： 根据正向递推的结果根据正向递推的结果 从从n1递推出机器人递推出机器人每个关节上承受的力和力矩每个关节上承受的力和力矩 得到机器人每个关节得到机器人每个关节所需要的驱动力（矩）。所需要的驱动力（矩）。 3.

8、2 3.2 牛顿牛顿欧拉方程法欧拉方程法机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学3.2.1 牛顿牛顿欧拉方程欧拉方程3.2.2 递推计算公式递推计算公式3.2.3 递推算法应用递推算法应用3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学3.2.1 牛顿牛顿欧拉方程欧拉方程1 1、牛顿方程、牛顿方程惯性力惯性力矢量。矢量。质心上的线加速度。质心上的线加速度。 CCamF 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、欧拉方程、欧拉方程惯性力矩惯性力矩矢量。

9、矢量。质心上的惯性张量矩阵。质心上的惯性张量矩阵。 )( CCIIM CzzCyzCxzCyzCyyCxyCxzCxyCxxCIIIIIIIIII3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.1 牛顿牛顿欧拉方程欧拉方程机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、欧拉方程、欧拉方程惯性力矩惯性力矩惯性张量矩阵简介：惯性张量矩阵简介：a.a.坐标系：与杆件坐标系同向，位于杆件质心上。坐标系：与杆件坐标系同向，位于杆件质心上。b.b.元素名称：元素名称： Icxx，Icyy，Iczz惯量矩；惯量矩； Icxy=Icyx,Icyz=Iczy,Iczx=Icxz惯量积。惯

10、量积。 CzzCyzCxzCyzCyyCxyCxzCxyCxxCIIIIIIIIII3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.1 牛顿牛顿欧拉方程欧拉方程机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、欧拉方程、欧拉方程惯性力矩惯性力矩惯性张量矩阵计算：惯性张量矩阵计算：理论计算方法：理论计算方法：实验测试法：实验测试法： 惯量摆仪器。惯量摆仪器。 VCxxdvzyI )(22 VCyydvxzI )(22 VCzzdvyxI )(22 VCxydvxyI VCxzdvxzI VCyzdvyzI 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.1 牛顿牛顿欧拉

11、方程欧拉方程机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：已知机器人各个关节的速度和加速度：已知机器人各个关节的速度和加速度： 从从1n递推出机器人每个杆件在自身坐标系递推出机器人每个杆件在自身坐标系 中的速度和加速度；中的速度和加速度； 机器人每个杆件质心上的速度和加速度；机器人每个杆件质心上的速度和加速度； 机器人每个杆件质心上的惯性力和惯性力矩。机器人每个杆件质心上的惯性力和惯性力矩。3.2.2 递推计算公式递推计算公式3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递

12、推：（第一种杆件坐标系）（第一种杆件坐标系）杆件速度和加速度杆件速度和加速度递推计算公式递推计算公式 建立相邻两个建立相邻两个杆件的坐标系：杆件的坐标系：i-1i-1、i i Oiii-1关节关节iXi-1Z i-1Oi-1XiZi3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学 1 1、正向递推、正向递推： 杆件速度和加速度杆件速度和加速度 递推计算公式递推计算公式已知已知：i-1i-1杆件速度和加速度杆件速度和加速度 i i关节速度和加速度关节速度和加速度计算计算：i i杆件速度和加速度杆件速度和加

13、速度iiqq ,1111, iiiiav iiiiav, iiqq ,ii-1关节关节iXi-1Z i-1Oi-1XiZiOi1111 iiiiav iiiiav 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学 1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式分析分析：I I、坐标系、坐标系: :相邻杆件位姿矩阵相邻杆件位姿矩阵IIII、关节速度和加速度、关节速度和加速度 的矢量化：的矢量化：11, iizizieqeq 10111iiiiiipRMiiqq ,Oi

14、ii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式 11iiiiR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向

15、递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式1111 iziiiiiiiieqsRR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式)(111 iziiiiiieqsR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 3.2 3.2 牛顿牛

16、顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式 )(111iziiiiiieqsR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式)()(111111 iizi

17、iiiiziiiiiieqsReqsR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav )1(111iziiiiiieqsvRv3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器

18、人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav )()1(111111iiiiiziiiiiipReqsvRvi 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1Xi

19、Zi1111 iiiiav iiiiav )()()1(111111111iiziiiiiiiiiziiiiiipeqsRpReqsvRvii 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 11)1()(11111 iiziiiiiiziiiiiiieqsRpeqsvRv 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程

20、法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式 )1()(1111111111iiiziiiiiiziiiziiiiiiiiieqsRpeqseqsRaRa 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法关节关节iiiqq ,Oiii-1Xi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件

21、速度和加速度递推计算公式 )()()1()(1111111111111111iiziiiziiiiiziiiiiiziiiziiiiiiiiipeqseqsReqsRpeqseqsRaRaiiiii 关节关节iiiqq ,Oiii-1Xi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式关节关节iiiqq ,Oiii-1Xi-1Z i-1Oi-1XiZi1

22、111 iiiiav iiiiav )1(2)()()1()(1111111111111111111 iiiiiiziiiiiiiziiiziiiiiziiiiiiziiiziiiiiiiiieqsRpeqseqsReqsRpeqseqsRaRa 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式关节关节iiiqq ,Oiii-1Xi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav )1()1(2)()(

23、)(1111111111111111111 iiiiiiziiiiziiiiiiiziiiziiiiiiiziiiziiiiiiiiieqsReqsRpeqseqsRpeqseqsRaRa 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件质心上的速度和加速度杆件质心上的速度和加速度iiC )(iiiCiiCiiCrraa iCi iXiZiOiiiiiav iCriCa3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第

24、3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件质心上的惯性力和惯性力矩杆件质心上的惯性力和惯性力矩惯性力惯性力：惯性力矩惯性力矩：iiCiCamF )(iCiiCCiiiIIM iXiZiOiiiiiav iCriCFiCM3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：（第二种杆件坐标系）（第二种杆件坐标系）杆件速度和加速度杆件速度和加速度递推计算公式递推计算公式 建立相邻两个建立相邻两个杆件的坐标系：杆件的坐标系：i-1i-1、iiii-1关节关节i

25、Xi-1Z i-1Oi-1XiZiOi3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式已知已知：i-1i-1杆件速度和加速度杆件速度和加速度 i i关节速度和加速度关节速度和加速度计算计算：i i杆件速度和加速度杆件速度和加速度iiqq ,1111, iiiiav iiiiav, ii-1关节关节iXi-1Z i-1Oi-1XiZiOi1111 iiiiav iiiiav iiqq ,3.2 3.2 牛顿牛顿欧拉方程法欧

26、拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式分析分析：I I、坐标系、坐标系: :相邻杆件位姿矩阵相邻杆件位姿矩阵IIII、关节速度和加速度、关节速度和加速度 的矢量化：的矢量化：iizizieqeq , 10111iiiiiipRMii-1关节关节iXi-1Z i-1Oi-1XiZiOi1111 iiiiav iiiiav iizizieqeq ,3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第

27、3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式ii-1关节关节iXi-1Z i-1Oi-1XiZiOi1111 iiiiav iiiiav iizizieqeq ,iziiiiiieqsR 11 iiziiiiiziiiiiieqsReqsR 1111 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式ii-1关节关节iXi-1Z i-1Oi-1Xi

28、ZiOi1111 iiiiav iiiiav iizizieqeq ,iziiiiiiiiieqspvRv )1()(1111 iiziiiiiiiiiiiziiiiiiiiieqsRpReqspaRa )1(2)()1()(1111111111 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件质心上的速度和加速度杆件质心上的速度和加速度iiC )(iiiCiiCiiCrraa iCi iXiZiOiiiiiav iCriCa3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3

29、.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件质心上的惯性力和惯性力矩杆件质心上的惯性力和惯性力矩惯性力惯性力：惯性力矩惯性力矩：iiCiCamF )(iCiiCCiiiIIM iXiZiOiiiiiav iCriCFiCM3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、反向递推、反向递推：已知机器人各个杆件的惯性力和惯性力矩：已知机器人各个杆件的惯性力和惯性力矩：从从n1递推出机器人每个关节承受的力和力矩；递推出机

30、器人每个关节承受的力和力矩；机器人每个关节的驱动力或驱动力矩。机器人每个关节的驱动力或驱动力矩。3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、反向递推、反向递推：（第二种杆件坐标系）（第二种杆件坐标系）关节承受的力和力矩关节承受的力和力矩递推计算公式递推计算公式 建立相邻两个建立相邻两个杆件的坐标系：杆件的坐标系：i-1i-1、iiii-1关节关节iXi-1Z i-1Oi-1XiZiOi3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3

31、3章章 机器人动力学机器人动力学2 2、反向递推、反向递推：关节承受的力和力矩递推计算公式关节承受的力和力矩递推计算公式已知已知：i-1i-1杆件的惯性力杆件的惯性力 和惯性力矩和惯性力矩 i i关节承受的力和力矩关节承受的力和力矩计算计算：i-1i-1关节承受的力和关节承受的力和 力矩力矩11, iiCCMFiimf,11, iimfii-1关节关节iXi-1Z i-1Oi-1XiZiOi11 iimfiimf1 iCr1 iCF1 iCM3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、反向递

32、推、反向递推：关节承受的力和力矩关节承受的力和力矩递推计算公式递推计算公式分析分析：I I、坐标系、坐标系: :相邻杆件位姿矩阵相邻杆件位姿矩阵IIII、i-1i-1杆件受力分析：杆件受力分析：iiiiiimRfR11, 10111iiiiiipRMii-1关节关节iXi-1Z i-1Oi-1XiZiOi11 iimfiimf1 iCr1 iCF1 iCMiiifR1 iiimR1 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、反向递推、反向递推：关节承受的力和力矩关节承受的力和力矩递推计算公

33、式递推计算公式 以以i-1i-1杆件为研究对象，杆件为研究对象，由达朗贝尔原理可得：由达朗贝尔原理可得：111111)( iiCiiiiiiiiCFfRffRfFi-1关节关节iXi-1Z i-1Oi-111 iimf1 iCr1 iCF1 iCMiiifR1 iiimR1 iip1 Oi3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、反向递推、反向递推：关节承受的力和力矩关节承受的力和力矩递推计算公式递推计算公式 以以i-1i-1杆件为研究对象，杆件为研究对象，由达朗贝尔原理可得：由达朗贝尔原

34、理可得：)()()()()(11111111111111111iiCiiiiiiCCCiiiiiiiiiCiiiiCprRfRpFrMmRmfRRfrmRmMiiiiii i-1关节关节iXi-1Z i-1Oi-111 iimf1 iCr1 iCF1 iCMiiifR1 iiimR1 iip1 Oi1 iR3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、反向递推、反向递推：关节驱动力（矩）关节驱动力（矩）平移关节：平移关节：回转关节：回转关节：则关节驱动力（矩）为：则关节驱动力（矩）为：)1(1

35、111111 iizTiizTiiiefsems 111 iizTief 111 izTiiem i-1关节关节iXi-1Z i-1Oi-111 iimf3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学3.2.3 递推算法应用递推算法应用1 1、递推初始条件、递推初始条件正向递推正向递推机座机座0 0的速度和加速度：的速度和加速度：* *考虑杆件自重或手部负载为重物时：考虑杆件自重或手部负载为重物时： 为描述在机座坐标系为描述在机座坐标系00中的标准重力加速度。中的标准重力加速度。0g00000 av

36、00000, 0gav gx0z0Tgg, 0 , 00 o03.2 3.2 牛顿牛顿欧拉方程法欧拉方程法机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、递推初始条件、递推初始条件反向递推反向递推机器人手部负载：机器人手部负载：11, nhnhmmff3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法l1l3l2hhmf3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、递推应用条件、递推应用条件已知机器人的关节变量已知机器人的关节变量 及其速度及其速度 和加速度和加速度 ； 已知任一杆件已知任一杆件i相对于与自身坐标系相对于与

37、自身坐标系ii方向相同的方向相同的坐标系坐标系CCi i 所描述的惯性张量所描述的惯性张量 及其质心在自身坐标及其质心在自身坐标系系ii中的位置矢量中的位置矢量 （可用实验等方法确定）；（可用实验等方法确定）； 已知相邻杆件的位姿矩阵及必要的初始数据。已知相邻杆件的位姿矩阵及必要的初始数据。qq q iCIiCr3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、递推应用条件、递推应用条件第二种坐标系下递推算法第二种坐标系下递推算法正向递推正向递推: : )()()1(2)1()()1()(1111

38、111111111111111iCiiCCCiCCiiCiiCziiiiiziiiiiiiiiiiiiziiiiiziiiiiiziiiiiiiiiziiiiiiiiiiiiiiiiiiiiIIMamFrraaeqsReqsppaRaeqsReqsReqspvRveqsR 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、递推应用条件、递推应用条件第二种坐标系下递推算法第二种坐标系下递推算法反向递推反向递推: : )1(11111111111111111iiiiiiziiziiiiiiiiCCCi

39、iiiCiiiiefsemsfRpFrMmRmFfRf 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学例：例：已知二自由度机器人如已知二自由度机器人如图所示，机器人两个杆件的图所示，机器人两个杆件的长度分别为长度分别为 和和 ，且其质，且其质量量 和和 都集中在杆件的端都集中在杆件的端头。若用第二种方法建立机头。若用第二种方法建立机器人的坐标系，当机器人各器人的坐标系，当机器人各个关节的位移个关节的位移 、速度、速度 和加速度和加速度 已知时，试用已知时，试用牛顿牛顿欧拉递推算法计算欧拉递推算法计算各

40、关节的驱动力矩。各关节的驱动力矩。 12m1m2xy关节关节1 1关节关节2 21l2l1m2m21, 21, 21, 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：建立坐标系如图所示。建立坐标系如图所示。相邻杆件的位姿矩阵为：相邻杆件的位姿矩阵为：12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2 100000000000111101 csscM 100000000002212212 cslscM3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机

41、器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解： (1)(1)正向递推正向递推 已知关节速度和加速度已知关节速度和加速度分别为分别为 ，由于考，由于考虑杆件的重量，所以机座的虑杆件的重量，所以机座的运动参数（初始条件）设为：运动参数（初始条件）设为：2121, 0 , 000000gav 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推的的速速度度和和加加速速度度：杆杆件件时时1,1

42、i12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2gTzzeeR, 0 , 01001111010111 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2gTzzzeeReR, 0 , 0100111101010101111 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递

43、推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g01001000100101)(aRppaRa Tgcgsgcssc00010000111111 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则线线加加速速度度：已已知知,0 , 0 ,11TClr 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)(1111111CCCrraa 0)()(0000000000011121111111111 lgclgsllgcgs3.

44、2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则惯惯性性力力为为：，已已知知杆杆件件质质量量为为的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件11m12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g 011111211111111 lmgcmlmgsmamFCC3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则惯惯性性力力矩

45、矩为为：，已已知知杆杆件件惯惯性性张张量量的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件011 CI12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g0)(111111 CCCIIM3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推的的速速度度和和加加速速度度：杆杆件件时时2,2 i12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2gTzcssceR, 0 , 000001000021212222212122 3.2 3.2 牛顿牛顿欧拉方

46、程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2gTzzcssceReR, 0 , 0 0000100002121222221212121222 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)()(11122112111211212CCrp

47、aRppaRa 00)()(1000012221121112221121111121112222 gcslclgsclsllgclgscssc 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则线线加加速速度度：已已知知,0 , 0 ,22TClr 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)(2222222CCCrraa 122122121222112111222112110)(0)(00 llgcslclgsclsl 0)()(21212

48、22112112212122211211 lgcslcllgsclsl3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则惯惯性性力力为为：，已已知知杆杆件件质质量量为为的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件22m 0)()(212212222112211222122122221122112222 lmgcmslmclmlmgsmclmslmamFCC12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法

49、3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则惯惯性性力力矩矩为为：，已已知知杆杆件件惯惯性性张张量量的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件022 CI0)(222222 CCCIIM12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推 由于机器人手部无负载，由于机器人手部无负载，所以初始条件为：所以初始条件为

50、： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g033 mf3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推 关节关节2 2受的力：受的力： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g223232CCFFfRf 0)()(212212222112211222122122221122112 lmgcmslmclmlmgsmclmslm3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控

51、制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推 关节关节2 2受的力矩：受的力矩： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g22222323233232CCCCCFrfRpFrMmRm )(0021222122222121221212 lmgclmsllmcllm3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推 关节关节2 2的驱动力矩：的驱动力矩： 12m1m2x0y0关节关节1 1关节关节2 2x1x

52、2y1y2g)(12222212122122221212222 gclmsllmlmcllmemzT 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推 关节关节1 1受的力：受的力： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g12121CFfRf 0)()()()(10000212212221121122212212221121122222 lmgcslclmlmgsclslmcssc 011111211111 lmgcmlmgsm3.2 3.

53、2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推整理可得关节整理可得关节1 1受的力：受的力： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g12121CFfRf 011111211111 lmgcmlmgsm 0)()()()(221222221221211222122222122122112 clmslmgcmlmslmclmgsmlm 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器

54、人动力学机器人动力学解：解：(2)(2)反向递推反向递推 关节关节1 1受的力矩：受的力矩： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g21212212111fRpFrmRmCC 111121100 gclmlm )(0021222122222121221212 lmgclmsllmcllm 2212222212121121212)()(00 clmsllmgclmlm 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推关节关节1 1的驱动力矩：

55、的驱动力矩： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)(2)(2)(1121122221221221221222212222122122222121111gclmmgclmsllmsllmcllmlmcllmlmlmmemzT 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推关节关节1 1和和2 2的驱动力矩为：的驱动力矩为： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g )()(2)(2)(1222221212212

56、2221212211211222212212212212222122221221222221211 gclmsllmlmcllmgclmmgclmsllmsllmcllmlmcllmlmlmm 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学将得到的关节驱动所需力矩简写为如下形式：将得到的关节驱动所需力矩简写为如下形式：当机器人有当机器人有n个关节时，上式可推广为普遍形式：个关节时，上式可推广为普遍形式： 2212212122222221211222121212112111222122211112121

57、111)()(DDDDDDDMDDDDDDDM ), 2 , 1(111niGqqHqDFnjnkikjijknjjiji 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学将上式进一步简化为如下所示的矩阵形式：将上式进一步简化为如下所示的矩阵形式： 上式也称为机器人的上式也称为机器人的动力学模型动力学模型。式中：式中： 是机器人动力学模型中的是机器人动力学模型中的惯性力项惯性力项； 表示机器人操作机的质量矩阵，它是表示机器人操作机的质量矩阵，它是nn阶的对称矩阵；阶的对称矩阵； 是是n1阶矩阵，表示机器

58、人动力学模型中阶矩阵，表示机器人动力学模型中非线性的非线性的耦合力项耦合力项，包括离心力（自耦力）和哥氏，包括离心力（自耦力）和哥氏力（互耦力）；力（互耦力）； 也是也是n1阶矩阵，表示机器人动力学模型中阶矩阵，表示机器人动力学模型中的的重力项重力项。 )(),()(qGqqHqqDF qqD )()(qD),(qqH )(qG3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学拉格朗日方程的一般形式为：拉格朗日方程的一般形式为：式中，式中， 广义力，它可以是力，也可以是力矩；广义力，它可以是力，也可以是力

59、矩； 系统选定的广义坐标；系统选定的广义坐标； 广义坐标对时间的一阶导数，即速度；广义坐标对时间的一阶导数，即速度； 拉格朗日函数，又称为拉格朗日算子，拉格朗日函数，又称为拉格朗日算子，它被定义为系统的动能与势能之差它被定义为系统的动能与势能之差L= =T- -U。niqLqLdtdFiii, 2 , 1 iFiqiq L3.3 3.3 拉格朗日方程法拉格朗日方程法机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学 对给定的机器人，可以按以下几个步骤建立拉对给定的机器人，可以按以下几个步骤建立拉格朗日动力学方程：格朗日动力学方程： （1 1）选取完全并独立的广义坐标：）选取完全并独立

60、的广义坐标： （2 2）选定广义力：）选定广义力： （3 3）求出系统的动能）求出系统的动能T和势能和势能U，并用其构造拉格，并用其构造拉格 朗日函数朗日函数L=T-U； （4 4）将以上结果代入拉格朗日方程式中，即可求）将以上结果代入拉格朗日方程式中，即可求 得机器人的动力学方程。得机器人的动力学方程。;,21niqqqq ;,21niFFFF 3.3 3.3 拉格朗日方程法拉格朗日方程法机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学例：例：已知二关节机器人如图所示，机器人的两个已知二关节机器人如图所示，机器人的两个连杆长度分别为连杆长度分别为l1和和l2，质量分别为，质量分别