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=mgl2l2l1m1=mg
4、(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.2 3.2 牛顿牛顿欧拉方程
8、法欧拉方程法机器人及其控制原理 第第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惯量积。惯量积。CzzCyzCxzCyzCyyC
10、xyCxzCxyCxxCIIIIIIIIII3.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 牛顿牛顿欧拉方程欧拉方程机器人及其控制原理 第第3 3章章 机
11、器人动力学机器人动力学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杆件速度和加速度杆件速度和加速度iiqq ,1111,iiiiav iiiiav,ii
13、qq ,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 ,Oiii-1关节关节iXi-1Z i-1Oi-1XiZi1111 iii
14、iav 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、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式111
15、1 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 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其
16、控制原理 第第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 iiziiiiiziiiiiieqsReqsR iiqq ,Oiii-1关节
17、关节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 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向
18、递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式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-1XiZi1111 iiiiav iiiiav )()()1(111111
19、111iiziiiiiiiiiziiiiiipeqsRpReqsvRvii 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 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3
20、章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式 )1()(1111111111iiiziiiiiiziiiziiiiiiiiieqsRpeqseqsRaRa 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法关节关节iiiqq ,Oiii-1Xi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav 3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式 )()()1()(111111111111
21、1111iiziiiziiiiiziiiiiiziiiziiiiiiiiipeqseqsReqsRpeqseqsRaRaiiiii 关节关节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-1XiZi1111 iiiiav iiiiav )1(2)()()1()(111
22、1111111111111111 iiiiiiziiiiiiiziiiziiiiiziiiiiiziiiziiiiiiiiieqsRpeqseqsReqsRpeqseqsRaRa 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式关节关节iiiqq ,Oiii-1Xi-1Z i-1Oi-1XiZi1111 iiiiav iiiiav )1()1(2)()()(1111111111111111111 iiiiiiziiiiz
23、iiiiiiiziiiziiiiiiiziiiziiiiiiiiieqsReqsRpeqseqsRpeqseqsRaRa 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件质心上的速度和加速度杆件质心上的速度和加速度iiC )(iiiCiiCiiCrraa iCi iXiZiOiiiiiav iCriCa3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件
24、质心上的惯性力和惯性力矩杆件质心上的惯性力和惯性力矩惯性力惯性力：惯性力矩惯性力矩：iiCiCamF )(iCiiCCiiiIIM iXiZiOiiiiiav iCriCFiCM3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：（第二种杆件坐标系）（第二种杆件坐标系）杆件速度和加速度杆件速度和加速度递推计算公式递推计算公式 建立相邻两个建立相邻两个杆件的坐标系：杆件的坐标系：i-1i-1、iiii-1关节关节iXi-1Z i-1Oi-1XiZiOi3.2 3.2 牛顿牛顿欧拉方
25、程法欧拉方程法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 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3
26、3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速度递推计算公式杆件速度和加速度递推计算公式分析分析：I I、坐标系、坐标系:相邻杆件位姿矩阵相邻杆件位姿矩阵IIII、关节速度和加速度、关节速度和加速度 的矢量化：的矢量化：iizizieqeq ,10111iiiiiipRMii-1关节关节iXi-1Z i-1Oi-1XiZiOi1111 iiiiav iiiiav iizizieqeq ,3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、正向递推、正向递推：杆件速度和加速
27、度递推计算公式杆件速度和加速度递推计算公式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-1XiZiOi1111 iiiiav iiiiav iizizieqeq ,izii
28、iiiiiiieqspvRv )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.2.2 递推计算公式递推计算公式机器人及其控制原理 第第3 3章章 机器人动
29、力学机器人动力学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 3章章 机器人动力学机器人动力学2 2、反向递推、反向递推：关节承受的力和力矩
31、递推计算公式关节承受的力和力矩递推计算公式已知已知：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、反向递推、反向递推：关节承受的力和力矩关节承受的力和力矩递推计算公式递推计算公式 以以i-1i-1杆件为研究对象，杆件为研究对象，由达朗贝尔原理可得：由达
33、朗贝尔原理可得：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杆件为研究对象，杆件为研究对象，由达朗贝尔原理可得：由达朗贝尔原理可得：)()()()()(11111111111111111iiCiiiiiiCCCii
34、iiiiiiiCiiiiCprRfRpFrMmRmfRRfrmRmMiiiiii 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(1111111 iizTiizTiiiefsems 111 iizTief 111 izTi
35、iem 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 00000,0gav gx0z0Tgg,0,00 o03.2 3.2 牛顿牛顿欧拉方程法欧拉方程
36、法机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学1 1、递推初始条件、递推初始条件反向递推反向递推机器人手部负载：机器人手部负载：11,nhnhmmff3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法l1l3l2hhmf3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、递推应用条件、递推应用条件已知机器人的关节变量已知机器人的关节变量 及其速度及其速度 和加速度和加速度 ；已知任一杆件已知任一杆件i相对于与自身坐标系相对于与自身坐标系ii方向相同的方向相同的坐标系坐标系CCi i 所描述的惯性张量所描述的惯性张量 及其质心在自身坐
37、标及其质心在自身坐标系系ii中的位置矢量中的位置矢量 （可用实验等方法确定）；（可用实验等方法确定）；已知相邻杆件的位姿矩阵及必要的初始数据。已知相邻杆件的位姿矩阵及必要的初始数据。qq q iCIiCr3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、递推应用条件、递推应用条件第二种坐标系下递推算法第二种坐标系下递推算法正向递推正向递推:)()()1(2)1()()1()(1111111111111111111iCiiCCCiCCiiCiiCziiiiiziiiiiiiiiiiiiziiiiiz
38、iiiiiiziiiiiiiiiziiiiiiiiiiiiiiiiiiiiIIMamFrraaeqsReqsppaRaeqsReqsReqspvRveqsR 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学2 2、递推应用条件、递推应用条件第二种坐标系下递推算法第二种坐标系下递推算法反向递推反向递推:)1(11111111111111111iiiiiiziiziiiiiiiiCCCiiiiCiiiiefsemsfRpFrMmRmFfRf 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应
39、用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学例：例：已知二自由度机器人如已知二自由度机器人如图所示，机器人两个杆件的图所示，机器人两个杆件的长度分别为长度分别为 和和 ，且其质，且其质量量 和和 都集中在杆件的端都集中在杆件的端头。若用第二种方法建立机头。若用第二种方法建立机器人的坐标系，当机器人各器人的坐标系，当机器人各个关节的位移个关节的位移 、速度、速度 和加速度和加速度 已知时，试用已知时，试用牛顿牛顿欧拉递推算法计算欧拉递推算法计算各关节的驱动力矩。各关节的驱动力矩。12m1m2xy关节关节1 1关节关节2 21l2l1m2m21,21,21,3.2 3
40、.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：建立坐标系如图所示。建立坐标系如图所示。相邻杆件的位姿矩阵为：相邻杆件的位姿矩阵为：12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2 100000000000111101 csscM 100000000002212212 cslscM3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推 已知关节速度和加速度已知关节速度和
41、加速度分别为分别为 ，由于考，由于考虑杆件的重量，所以机座的虑杆件的重量，所以机座的运动参数（初始条件）设为：运动参数（初始条件）设为：2121,0,000000gav 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推的的速速度度和和加加速速度度：杆杆件件时时1,1 i12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2gTzzeeR,0,01001111010111 3.2 3.2 牛顿牛
42、顿欧拉方程法欧拉方程法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)正向递推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g01001000100101)(aRppaRa Tgcgsgcssc000
43、10000111111 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则线线加加速速度度：已已知知,0,0,11TClr 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)(1111111CCCrraa 0)()(0000000000011121111111111 lgclgsllgcgs3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)
44、正向递推正向递推则则惯惯性性力力为为：，已已知知杆杆件件质质量量为为的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件11m12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g 011111211111111 lmgcmlmgsmamFCC3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则惯惯性性力力矩矩为为：，已已知知杆杆件件惯惯性性张张量量的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件011 CI12m1m2x0y0关节关节1 1关节关节2 2x1x2
45、y1y2g0)(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 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推12m1m2x0
46、y0关节关节1 1关节关节2 2x1x2y1y2gTzzcssceReR,0,0 0000100002121222221212121222 3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)()(11122112111211212CCrpaRppaRa 00)()(1000012221121112221121111121112222 gcslclgsclsllgclgscssc 3.2 3.2 牛顿牛顿
47、欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则线线加加速速度度：已已知知,0,0,22TClr 12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g)(2222222CCCrraa 122122121222112111222112110)(0)(00 llgcslclgsclsl 0)()(2121222112112212122211211 lgcslcllgsclsl3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第
48、3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则惯惯性性力力为为：，已已知知杆杆件件质质量量为为的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件22m 0)()(212212222112211222122122221122112222 lmgcmslmclmlmgsmclmslmamFCC12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(1)(1)正向递推正向递推则则惯惯性性力力矩矩为为：，已已知知杆杆件
49、件惯惯性性张张量量的的惯惯性性力力和和惯惯性性力力矩矩：杆杆件件022 CI0)(222222 CCCIIM12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推 由于机器人手部无负载，由于机器人手部无负载，所以初始条件为：所以初始条件为：12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g033 mf3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制
50、原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推 关节关节2 2受的力：受的力：12m1m2x0y0关节关节1 1关节关节2 2x1x2y1y2g223232CCFFfRf 0)()(212212222112211222122122221122112 lmgcmslmclmlmgsmclmslm3.2 3.2 牛顿牛顿欧拉方程法欧拉方程法3.2.3 递推算法应用递推算法应用机器人及其控制原理 第第3 3章章 机器人动力学机器人动力学解：解：(2)(2)反向递推反向递推 关节关节2 2受的力矩：受的力矩：12m1m2x0y0关节关节1 1关节关节2 2x1x2y
