第4章机器人动力学

1、 工工业业机机器器人人第第4 4章章 机器人动力学机器人动力学4.1 4.1 引言引言4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法4.3 4.3 拉格朗日方程法拉格朗日方程法 习题习题 工工业业机机器器人人动力学研究的问题动力学研究的问题： 机器人各个机器人各个关节的运动关节的运动与与关节关节需要的驱动力（矩）需要的驱动力（矩）之间的关系。之间的关系。正问题正问题：已知关节运动，求：已知关节运动，求 关节驱动力（矩）。关节驱动力（矩）。逆问题逆问题：已知关节驱动力（矩），：已知关节驱动力（矩）， 求关节运动。求关节运动。第第4 4章章 机器人动力学机器人动力学 工工业业机机器器人人数学模型数学

2、模型： 关节运动关节运动位移、速度、加速度变化位移、速度、加速度变化 关节驱动力（矩）关节驱动力（矩）驱动力或驱动力矩驱动力或驱动力矩i动力学方程动力学方程： ， i=1，n正问题正问题：已知：已知 ，求，求i。逆问题逆问题：已知：已知i ，求，求 。),(iiiiqqqf iiiqqq ,iiiqqq ,iiiqqq ,第第4 4章章 机器人动力学机器人动力学 工工业业机机器器人人4.1 4.1 引言引言4.1.1 静力学分析静力学分析4.1.2 动力学分析动力学分析第第4 4章章 机器人动力学机器人动力学 工工业业机机器器人人4.1.1 静力学分析静力学分析机器人各个关节处于静止状态。机器

3、人各个关节处于静止状态。当负载为一重物时：当负载为一重物时：关节承受的力和力矩：关节承受的力和力矩：关节需要的驱动力（矩）：关节需要的驱动力（矩）：4.1 4.1 引言引言第第4 4章章 机器人动力学机器人动力学mgf3=mgf2=mgf1=mgm2=mgl2l2l1m1=mg(l1+l2)1=02=mgl23=mgm3=0 工工业业机机器器人人机器人各个关节处于静止状态。机器人各个关节处于静止状态。考虑杆件自重时：考虑杆件自重时：关节承受的力和力矩：关节承受的力和力矩：关节需要的驱动力（矩）：关节需要的驱动力（矩）：4.1 4.1 引言引言第第4 4章章 机器人动力学机器人动力学4.1.1

4、静力学分析静力学分析mgf3=mgf2=mgf1=mgm2=mgl2l2l1m1=mg(l1+l2)1=02=mgl23=mgm3gm2gm1gm3=0 工工业业机机器器人人 机器人各个关节处于运动状态。机器人各个关节处于运动状态。 当负载为一重物时：当负载为一重物时：关节承受的力和力矩：关节承受的力和力矩：关节需要的驱动力（矩）：关节需要的驱动力（矩）：f3f2f1m2l2l1m112311, 22, 33,dd m33dmmg 121)( llm 2121)( llm 4.1 4.1 引言引言第第4 4章章 机器人动力学机器人动力学4.1.2 动力学分析动力学分析 工工业业机机器器人人牛顿

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

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

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

8、2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学 工工业业机机器器人人4.2.1 牛顿牛顿欧拉方程欧拉方程4.2.2 递推计算公式递推计算公式4.2.3 递推算法应用递推算法应用4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学 工工业业机机器器人人4.2.1 牛顿牛顿欧拉方程欧拉方程1 1、牛顿方程、牛顿方程惯性力惯性力矢量。矢量。质心上的线加速度。质心上的线加速度。 CCamF 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学 工工业业机机器器人人2 2、欧拉方程、欧拉方程惯性力矩惯性力矩

9、矢量。矢量。质心上的惯性张量矩阵。质心上的惯性张量矩阵。 )( CCIIM CzzCyzCxzCyzCyyCxyCxzCxyCxxCIIIIIIIIII4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.1 牛顿牛顿欧拉方程欧拉方程 工工业业机机器器人人2 2、欧拉方程、欧拉方程惯性力矩惯性力矩惯性张量矩阵：惯性张量矩阵：a.a.坐标系：与杆件坐标系同向，位于杆件质心上。坐标系：与杆件坐标系同向，位于杆件质心上。b.b.元素名称：元素名称： Icxx，Icyy，Iczz惯量矩；惯量矩； Icxy=Icyx,Icyz=Iczy,Iczx=Icxz惯量积。惯

10、量积。 CzzCyzCxzCyzCyyCxyCxzCxyCxxCIIIIIIIIII4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.1 牛顿牛顿欧拉方程欧拉方程 工工业业机机器器人人2 2、欧拉方程、欧拉方程惯性力矩惯性力矩惯性张量矩阵：惯性张量矩阵：理论计算方法：理论计算方法：实验测试法：实验测试法： 惯量摆仪器。惯量摆仪器。 VCxxdvzyI )(22 VCyydvxzI )(22 VCzzdvyxI )(22 VCxydvxyI VCxzdvxzI VCyzdvyzI 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机

11、器人动力学4.2.1 牛顿牛顿欧拉方程欧拉方程 工工业业机机器器人人1 1、正向递推、正向递推：已知机器人各个关节的速度和加速度：已知机器人各个关节的速度和加速度： 从杆件从杆件1n递推出机器人每个杆件在自身坐标递推出机器人每个杆件在自身坐标 系中的速度和加速度；系中的速度和加速度； 机器人每个杆件质心上的速度和加速度；机器人每个杆件质心上的速度和加速度； 机器人每个杆件质心上的惯性力和惯性力矩。机器人每个杆件质心上的惯性力和惯性力矩。4.2.2 递推计算公式递推计算公式4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学 工工业业机机器器人人1 1、正向递推、

12、正向递推：（第一种杆件坐标系）（第一种杆件坐标系）杆件速度和加速度杆件速度和加速度递推计算公式递推计算公式 建立相邻两个建立相邻两个杆件的坐标系：杆件的坐标系：i-1i-1、i i Oiii-1关节关节iXi-1Z i-1Oi-1XiZi4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人 1 1、正向递推、正向递推： 杆件速度和加速度杆件速度和加速度 递推计算公式递推计算公式已知已知：i-1i-1杆件速度和加速度杆件速度和加速度 i i关节速度和加速度关节速度和加速度计算计算：i i杆件速度和加速度杆件

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

14、q ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式 11iiiiR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、

15、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式1111 iziiiiiiiieqsRR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式)(111 iziiiiiieqsR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 4.2

16、 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式 )(111iziiiiiieqsR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式)()(1

17、11111 iiziiiiiziiiiiieqsReqsR iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav )1(111iziiiiiieqsvRv4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机

18、器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav )()1(111111iiiiiziiiiiipReqsvRvi 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式iiqq ,Oiii-1关节关节iXi

19、-1Z i-1Oi-1XiZi1111 iiiiav iiiiav )()()1(111111111iiziiiiiiiiiziiiiiipeqsRpReqsvRvii 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式iiqq ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 11)1()(11111 iiziiiiiiziiiiiiieqsRpeqsvRv 4.2 4.2

20、 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式 )1()(1111111111iiiziiiiiiziiiziiiiiiiiieqsRpeqseqsRaRa 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学关节关节iiiqq ,Oiii-1Xi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：

21、杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式 )()()1()(1111111111111111iiziiiziiiiiziiiiiiziiiziiiiiiiiipeqseqsReqsRpeqseqsRaRaiiiii 关节关节iiiqq ,Oiii-1Xi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式关节关节iiiqq ,Oiii-1X

22、i-1Z i-1Oi-1XiZi1111 iiiiav iiiiav )1(2)()()1()(1111111111111111111 iiiiiiziiiiiiiziiiziiiiiziiiiiiziiiziiiiiiiiieqsRpeqseqsReqsRpeqseqsRaRa 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件质心上的速度和加速度杆件质心上的速度和加速度iiC )(iiiCiiCiiCrraa iCi iXiZiOiiiiiav iCriCa4.2 4

23、.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件质心上的惯性力和惯性力矩杆件质心上的惯性力和惯性力矩惯性力惯性力：惯性力矩惯性力矩：iiCiCamF )(iCiiCCiiiIIM iXiZiOiiiiiav iCriCFiCM4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：（第二种杆件坐标系）（第二种杆件坐标系）杆件速度和加速度杆件速度和加速度递推计算公式递推

24、计算公式 建立相邻两个建立相邻两个杆件的坐标系：杆件的坐标系：i-1i-1、iiii-1关节关节iXi-1Z i-1Oi-1XiZiOi4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式已知已知：i-1i-1杆件速度和加速度杆件速度和加速度 i i关节速度和加速度关节速度和加速度计算计算：i i杆件速度和加速度杆件速度和加速度iiqq ,1111, iiiiav iiiiav, ii-1关节关节iXi-1Z i-1Oi-

25、1XiZiOi1111 iiiiav iiiiav iiqq ,4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式分析分析：I I、坐标系、坐标系: :相邻杆件位姿矩阵相邻杆件位姿矩阵IIII、关节速度和加速度、关节速度和加速度 的矢量化：的矢量化：iizizieqeq , 10111iiiiiipRMii-1关节关节iXi-1Z i-1Oi-1XiZiOi1111 iiiiav iiiiav iizizieqeq ,4

26、.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式ii-1关节关节iXi-1Z i-1Oi-1XiZiOi1111 iiiiav iiiiav iizizieqeq ,iziiiiiieqsR 11 iiziiiiiziiiiiieqsReqsR 1111 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推

27、：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式ii-1关节关节iXi-1Z i-1Oi-1XiZiOi1111 iiiiav iiiiav iizizieqeq ,iziiiiiiiiieqspvRv )1()(1111 iiziiiiiiiiiiiziiiiiiiiieqsRpReqspaRa )1(2)()1()(1111111111 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件质心上的速度和加速度杆件质心上的速度和加速度iiC )(iiiCiiCi

28、iCrraa iCi iXiZiOiiiiiav iCriCa4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人1 1、正向递推、正向递推：杆件质心上的惯性力和惯性力矩杆件质心上的惯性力和惯性力矩惯性力惯性力：惯性力矩惯性力矩：iiCiCamF )(iCiiCCiiiIIM iXiZiOiiiiiav iCriCFiCM4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人2 2、反向递推、反向递推：已知机器人各个杆

29、件的惯性力和惯性力矩：已知机器人各个杆件的惯性力和惯性力矩：从关节从关节n1递推出机器人每个关节承受的力和力矩；递推出机器人每个关节承受的力和力矩；机器人每个关节的驱动力或驱动力矩。机器人每个关节的驱动力或驱动力矩。4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人2 2、反向递推、反向递推：（第二种杆件坐标系）（第二种杆件坐标系）关节承受的力和力矩关节承受的力和力矩递推计算公式递推计算公式 建立相邻两个建立相邻两个杆件的坐标系：杆件的坐标系：i-1i-1、iiii-1关节关节iXi-1Z i-1Oi-

30、1XiZiOi4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人2 2、反向递推、反向递推：关节承受的力和力矩递推计算公式关节承受的力和力矩递推计算公式已知已知：i-1i-1杆件的惯性力杆件的惯性力 和惯性力矩和惯性力矩 i i关节承受的力和力矩关节承受的力和力矩计算计算：i-1i-1关节承受的力和关节承受的力和 力矩力矩11, iiCCMFiimf,11, iimfii-1关节关节iXi-1Z i-1Oi-1XiZiOi11 iimfiimf1 iCr1 iCF1 iCM4.2 4.2 牛顿牛顿欧拉方

31、程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人2 2、反向递推、反向递推：关节承受的力和力矩关节承受的力和力矩递推计算公式递推计算公式分析分析：I I、坐标系、坐标系: :相邻杆件位姿矩阵相邻杆件位姿矩阵IIII、i-1i-1杆件受力分析：杆件受力分析：iiiiiimRfR11, 10111iiiiiipRMii-1关节关节iXi-1Z i-1Oi-1XiZiOi11 iimfiimf1 iCr1 iCF1 iCMiiifR1 iiimR1 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.

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

33、和力矩关节承受的力和力矩递推计算公式递推计算公式 以以i-1i-1杆件为研究对象，杆件为研究对象，由达朗贝尔原理可得：由达朗贝尔原理可得：)()()()()(11111111111111111iiCiiiiiiCCCiiiiiiiiiCiiiiCprRfRpFrMmRmfRRfrmRmMiiiiii i-1关节关节iXi-1Z i-1Oi-111 iimf1 iCr1 iCF1 iCMiiifR1 iiimR1 iip1 Oi1 iR4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人2 2、反向递推、反

34、向递推：关节驱动力（矩）关节驱动力（矩）平移关节：平移关节：回转关节：回转关节：则关节驱动力（矩）为：则关节驱动力（矩）为：)1(1111111 iizTiizTiiiefsems 111 iizTief 111 izTiiem i-1关节关节iXi-1Z i-1Oi-111 iimf4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.2 递推计算公式递推计算公式 工工业业机机器器人人4.2.3 递推算法应用递推算法应用1 1、递推初始条件、递推初始条件正向递推正向递推机座机座0 0的速度和加速度：的速度和加速度：* *考虑杆件自重或手部负载为重物时：考

35、虑杆件自重或手部负载为重物时： 为描述在机座坐标系为描述在机座坐标系00中的标准重力加速度。中的标准重力加速度。0g00000 av 00000, 0gav gx0z0Tgg), 0 , 0(0 o04.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学 工工业业机机器器人人1 1、递推初始条件、递推初始条件反向递推反向递推机器人手部负载：机器人手部负载：11, nhnhmmff4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学l1l3l2hhmf4.2.3 递推算法应用递推算法应用 工工业业机机器器人人2 2、递推应用条件、递推

36、应用条件已知机器人的关节变量已知机器人的关节变量 及其速度及其速度 和加速度和加速度 ； 已知任一杆件已知任一杆件i相对于与自身坐标系相对于与自身坐标系ii方向相同的方向相同的坐标系坐标系CCi i 所描述的惯性张量所描述的惯性张量 及其质心在自身坐标及其质心在自身坐标系系ii中的位置矢量中的位置矢量 （可用实验等方法确定）；（可用实验等方法确定）； 已知相邻杆件的位姿矩阵及必要的初始数据。已知相邻杆件的位姿矩阵及必要的初始数据。qq q iCIiCr4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人2

37、 2、递推应用条件、递推应用条件第二种坐标系下递推算法第二种坐标系下递推算法正向递推正向递推: : )()()1(2)1()()1()(1111111111111111111iCiiCCCiCCiiCiiCziiiiiziiiiiiiiiiiiiziiiiiziiiiiiziiiiiiiiiziiiiiiiiiiiiiiiiiiiiIIMamFrraaeqsReqsppaRaeqsReqsReqspvRveqsR 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人2 2、递推应用条件、递推应用条件第二种

38、坐标系下递推算法第二种坐标系下递推算法反向递推反向递推: : )1(11111111111111111iiiiiizTiizTiiiiiiiiCCCiiiiCiiiiefsemsfRpFrMmRmFfRf 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人例：例：已知二自由度机器人如已知二自由度机器人如图所示，机器人两个杆件的图所示，机器人两个杆件的长度分别为长度分别为 和和 ，且其质，且其质量量 和和 都集中在杆件的端都集中在杆件的端头。若用第二种方法建立机头。若用第二种方法建立机器人的坐标系，当机器

39、人各器人的坐标系，当机器人各个关节的位移个关节的位移 、速度、速度 和加速度和加速度 已知时，试用已知时，试用牛顿牛顿欧拉递推算法计算欧拉递推算法计算各关节的驱动力矩。各关节的驱动力矩。 12m1m2xy关节关节1 1关节关节2 21l2l1m2m21, 21, 21, 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：建立坐标系如图所示。建立坐标系如图所示。相邻杆件的位姿矩阵为：相邻杆件的位姿矩阵为：12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2 10000000000011

40、1101 csscM 100000000002212212 cslscM4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解： (1)(1)正向递推正向递推 已知关节速度和加速度已知关节速度和加速度分别为分别为 ，由于考，由于考虑杆件的重量，所以机座的虑杆件的重量，所以机座的运动参数（初始条件）设为：运动参数（初始条件）设为：2121, Tgav)0 , 0(00000 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器

41、人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推的的速速度度和和加加速速度度：杆杆件件时时1,1 i12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2gTzzeeR), 0 , 0(1001111010111 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2gTzzzeeReR), 0 , 0(100111101010101

42、111 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g01001000100101)(aRppaRa Tgcgsgcssc0,0010000111111 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推则则线线加加速速度度：已已知知,)0 , 0 ,(11TClr 12m

43、1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)(1111111CCCrraa 0)()(0000000000011121111111111 lgclgsllgcgs4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推则则惯惯性性力力为为：，已已知知杆杆件件质质量量为为的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件11m12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g 011111211111111 lmgcmlmgsmamFCC4.2

44、4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推则则惯惯性性力力矩矩为为：，已已知知杆杆件件惯惯性性张张量量的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件011 CI12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g0)(111111 CCCIIM4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推的的速速度度和和加加速速度度：杆杆件

45、件时时2,2 i12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2gTzcssceR), 0 , 0(00001000021212222212122 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2gTzzcssceReR), 0 , 0(0000100002121222221212121222 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3

46、 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)()(11122112111211212CCrpaRppaRa 00)()(1000012221121112221121111121112222 gcslclgsclsllgclgscssc 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推则则线线加加速速度度：已已知知,)0 , 0 ,(22TClr 12m1m2x

47、0y0关节关节1 1关节关节2 2x1x2y1y2g)(2222222CCCrraa 122122121222112111222112110)(0)(00 llgcslclgsclsl 0)()(2121222112112212122211211 lgcslcllgsclsl4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推则则惯惯性性力力为为：，已已知知杆杆件件质质量量为为的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件22m 0)()(212212222112211

48、222122122221122112222 lmgcmslmclmlmgsmclmslmamFCC12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(1)(1)正向递推正向递推则则惯惯性性力力矩矩为为：，已已知知杆杆件件惯惯性性张张量量的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件022 CI0)(222222 CCCIIM12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g4.2 4.2 牛顿牛顿欧拉方程法欧拉方程

49、法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(2)(2)反向递推反向递推 由于机器人手部无负载，由于机器人手部无负载，所以初始条件为：所以初始条件为： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g033 mf4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(2)(2)反向递推反向递推 关节关节2 2受的力：受的力： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g223232CCFFfRf

50、0)()(212212222112211222122122221122112 lmgcmslmclmlmgsmclmslm4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(2)(2)反向递推反向递推 关节关节2 2受的力矩：受的力矩： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g22222323233232CCCCCFrfRpFrMmRm )(0021222122222121221212 lmgclmsllmcllm4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章

51、章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(2)(2)反向递推反向递推 关节关节2 2的驱动力矩：的驱动力矩： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)(12222212122122221212222 gclmsllmlmcllmemzT 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(2)(2)反向递推反向递推 关节关节1 1受的力：受的力： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g1

52、2121CFfRf 0)()()()(10000212212221121122212212221121122222 lmgcslclmlmgsclslmcssc 011111211111 lmgcmlmgsm4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(2)(2)反向递推反向递推整理可得关节整理可得关节1 1受的力：受的力： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g12121CFfRf 011111211111 lmgcmlmgsm 0)()()()(221222

53、221221211222122222122122112 clmslmgcmlmslmclmgsmlm 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(2)(2)反向递推反向递推 关节关节1 1受的力矩：受的力矩： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g21212212111fRpFrmRmCC )(0021222122222121221212 lmgclmsllmcllm 111121100 gclmlm 2212222212121121212)()(00 clm

54、sllmgclmlm 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人解：解：(2)(2)反向递推反向递推关节关节1 1的驱动力矩：的驱动力矩： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)(2)(2)(1121122221221221221222212222122122222121111gclmmgclmsllmsllmcllmlmcllmlmlmmemzT 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应

55、用 工工业业机机器器人人解：解：(2)(2)反向递推反向递推关节关节1 1和和2 2的驱动力矩为：的驱动力矩为： 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g )()(2)(2)(12222212122122221212211211222212212212212222122221221222221211 gclmsllmlmcllmgclmmgclmsllmsllmcllmlmcllmlmlmm 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.3 递推算法应用递推算法应用 工工业业机机器器人人将例中得到的关节驱动所需力矩简写为如

56、下形式：将例中得到的关节驱动所需力矩简写为如下形式：当机器人有当机器人有n个关节时，上式可推广为普遍形式：个关节时，上式可推广为普遍形式： 2212212122222221211222121212112111222122211112121111)()(DDDDDDDMDDDDDDDM ), 2 , 1(111niGqqHqDFnjnkikjijknjjiji 4.2 4.2 牛顿牛顿欧拉方程法欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.4 动力学模型动力学模型 工工业业机机器器人人将上式进一步简化为如下所示的矩阵形式：将上式进一步简化为如下所示的矩阵形式： 上式也称为机器人的上式

57、也称为机器人的动力学模型动力学模型。式中：式中： 是机器人动力学模型中的是机器人动力学模型中的惯性力项惯性力项； 表示机器人操作机的质量矩阵，它是表示机器人操作机的质量矩阵，它是nn阶的对称矩阵；阶的对称矩阵； 是是n1阶矩阵，表示机器人动力学模型中阶矩阵，表示机器人动力学模型中非线性的非线性的耦合力项耦合力项，包括离心力（自耦力）和哥氏，包括离心力（自耦力）和哥氏力（互耦力）；力（互耦力）； 也是也是n1阶矩阵，表示机器人动力学模型中阶矩阵，表示机器人动力学模型中的的重力项重力项。 )(),()(qGqqHqqDF qqD )()(qD),(qqH )(qG4.2 4.2 牛顿牛顿欧拉方程法

58、欧拉方程法第第4 4章章 机器人动力学机器人动力学4.2.4 动力学模型动力学模型 工工业业机机器器人人拉格朗日方程的一般形式为：拉格朗日方程的一般形式为：式中，式中， 广义力，它可以是力，也可以是力矩；广义力，它可以是力，也可以是力矩； 系统选定的广义坐标；系统选定的广义坐标； 广义坐标对时间的一阶导数，即速度；广义坐标对时间的一阶导数，即速度； 拉格朗日函数，又称为拉格朗日算子，拉格朗日函数，又称为拉格朗日算子，它被定义为系统的动能与势能之差它被定义为系统的动能与势能之差L= =T- -U。niqLqLdtdFiii, 2 , 1 iFiqiq L4.3 4.3 拉格朗日方程法拉格朗日方程

59、法第第4 4章章 机器人动力学机器人动力学 工工业业机机器器人人 对给定的机器人，可以按以下几个步骤建立拉对给定的机器人，可以按以下几个步骤建立拉格朗日动力学方程：格朗日动力学方程： （1 1）选取完全并独立的广义坐标：）选取完全并独立的广义坐标： （2 2）选定广义力：）选定广义力： （3 3）求出系统的动能）求出系统的动能T和势能和势能U，并用其构造拉格，并用其构造拉格 朗日函数朗日函数 L=T-U； （4 4）将以上结果代入拉格朗日方程式中，即可求）将以上结果代入拉格朗日方程式中，即可求 得机器人的动力学方程。得机器人的动力学方程。;,21niqqqq ;,21niFFFF 第第4 4章

60、章 机器人动力学机器人动力学4.3 4.3 拉格朗日方程法拉格朗日方程法 工工业业机机器器人人例：例：已知二关节机器人如图所示，机器人的两个已知二关节机器人如图所示，机器人的两个连杆长度分别为连杆长度分别为l1和和l2，质量分别为，质量分别为m1和和m2，且集，且集中在各连杆的端部。若将机器人直接悬挂在加速中在各连杆的端部。若将机器人直接悬挂在加速度为度为g的重力场中，试用拉格朗日方程建立该机器的重力场中，试用拉格朗日方程建立该机器人的动力学方程。人的动力学方程。解：解： 选取连杆绕关节的转角选取连杆绕关节的转角为变量为变量1和和2 ，则系统的广义，则系统的广义坐标就可以选为坐标就可以选为 ，