第九讲(1)-机器人动力学--拉格朗日方程

山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
末端执行器上无作用力，所以： 基座静止，因此： 0 0 0 0, 考虑到引力，我们使用：
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
应用递推公式有： 向前：1杆件：
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
通常，机器人的动力学方程常写为抽象的形式，
令：
1 2
离心力
科氏力
山东大学机械工程学院机电工程研究所2010/09/02
机器人机构动力学方程 有：
V ( , ) G() Q M ()
为广义坐标向量， Q 为广义力向量。 其中：
2
V1 m1 gl1 sin 1
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
质量m2的位置表示为： 速度分量为：
x2 l1 cos1 l2 cos(1 2 ) y2 l1 sin 1 l2 sin(1 2 )
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
整理得：
[m l 2 m l l cos ] 1 [( m1 m2 )l12 m2l22 2m2l1l2 cos 2 ] 1 2 2 2 1 2 2 2 m l l sin 2 2m2l1l2 sin 2 1 2 2 1 2 2 2 m2 gl2 cos(1 2 ) （m1 m2）gl1 cos1
L Ek E p
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
例3：对例2所示两杆平面机器人用拉格朗日方 法建立动力学方程。 解： 1、动能和势能 连杆1的动能为： 1 T1 m1 (l1 1 ) 2 设Y0=0为零势面，则连杆1的 势能为：
qq j k
3、求系统位能
EP E p i mi gT Ti ri
i 1 i 1 n n
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
4、计算拉格朗日函数
L Ekt E p Ti TiT 1 n i i Trace Hi 2 i 1 j 1 k 1 qk q j
3、动力学方程
L 2 2 ) m l l cos (2 ) ( m m ) l m l ( 1 2 1 1 2 2 1 2 2 1 2 2 1 2 1 L (m1 m2 ) gl1 cos1 m2 gl2 cos(1 2 ) 1
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
2、拉格朗日函数
L T1 V1 T2 V2 1 1 2 2 2 ) 2 m l l cos ( 2 (m1 m2 )l1 1 m2l2 (1 2 2 1 2 2 1 1 2) 2 2 (m1 m2 ) gl1 sin 1 m2 gl2 sin(1 2 )
d L L Qi dt qi qi
i 1, 2,...n
式中： n
Qi ——作用在第i个广义坐标上的广义
力或广义力矩 L—系统的动能 Ek 和位能 E p之差，称为拉格朗日 函数，即：
qi i ——第i个广义速度 q
——系统的广义坐标数 ——第i个广义坐标
2 l 2 ( 2 2 2 2 l12 1 2 1 1 2 2 ) 2l1l 2 cos( 1 1 2)
所以，M2动能为：
T2
势能为： V2 m2 gl1 sin 1 m2 gl2 sin(1 2 )
1 2 l 2 ( 2 2 2 ) 2l l cos ( 2 m2 [l12 1 2 1 1 2 2 1 2 2 1 1 2 )] 2
d L L 1 dt 1 1 d L L 2 dt 2 2
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
代入：
[m l 2 m l l cos ] 1 [( m1 m2 )l12 m2l22 2m2l1l2 cos 2 ] 1 2 2 212 2 2 2m l l sin m l l sin 2
4.3 机器人拉格朗日动力学方程简介
• 2、求系统动能
T i i Ti Ti 1 Ek Eki Trace Hi q j qk 2 i 1 q i 1 qk j 1 k 1 j n n
Ti TiT 1 n i i Trace Hi 2 i 1 j 1 k 1 qk q j
2 1 2 2 1 2 2 1 2 2 2
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
L ( ) m gl cos( ) m2 gl1l2 sin 2 1 1 2 2 2 1 2 2 L 2 ) m l l cos m l ( 2 2 1 2 2 1 2 2 1 2
d L 2 2 [m l 2 m l l cos ] [( m m ) l m l 2 m l l cos ] 1 2 1 2 2 2 1 2 2 1 2 2 2 1 2 2 2 dt 1 m l l sin 2 2m l l sin
牛顿—欧拉方程实例
惯性力
惯性力矩
2杆件：
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
向后递推： 2杆件：
1杆件：
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
取力矩的Z分量，得到关节力矩：
比较例2与例3可知，用牛顿-欧拉法与拉格朗 日法得到的结果是相同的。
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介 步骤总结： 1、机械臂上一点速度 设杆件i上一点ri，它在基坐标系中的位 置为：
r Ti ri
其中，Ti是{i}坐标系相对基础坐标系的齐次变 换矩阵。 那么，该点的速度为：
T i Ti T r qk r k 1 qk
T i i T T Ti i Trace r r q j qk q j qk j 1 k 1
山东大学机械工程学院机电工程研究所2010/09/02
q Ia p qp k q q j k
2Ti TiT Trace Hi q p i p j 1 k 1 q j qk Ti mi g ri q p i p
n T
山东大学机械工程学院机电工程研究所2010/09/02
i T dr i j ri vi q dt q j 1 j
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介 求出速度的平方：
v2 v v
T Trace v v
i T i Trace qj q j j 1
d L 2 m l 2 m l l cos m l l sin m l 2 2 1 2 2 2 2 1 2 2 1 2 1 2 2 1 2 dt 2
广义坐标为 1和 2 对应的广义外力为作 用于的关节上的驱动力距 1和 2 。
拉格朗日方程是基于能量项（动能 T、势能V）对系统变量及时间的微分 而建立的。 对于简单系统拉格朗日方程法相较 于牛顿—欧拉方程法更显复杂，然而随 着系统复杂程度的增加，拉格朗日方程 法建立系统运动微分方程变得相对简单。
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介 系统拉格朗日方程为：
Baidu Nhomakorabea
, ) 是离心力、科 称 M ()为惯量阵， V ( G() 为重力部分。 氏力等相关部分， , ) 中仅有速度和位形，上 因为 V ( 述方程也称状态空间方程。 特点： 多变量、时变、非线性、强耦合。
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
2 m l 2 m l l sin 2 2 （m2l2 m l l cos ） 1 2 1 2 2 1 2 2 2 2 1 2 2 1 m2 gl2 cos(1 2 )
山东大学机械工程学院机电工程研究所2010/09/02
n 1 n I a i qi2 mi g T Ti ri 2 i 1 i 1
q q j k
L Ek E p
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
5、代入拉格朗日方程
d L dt qi
n i Ti L TiT Trace Hi q p qi i p k 1 qk n i i
212 2 1 2 212 2 2
（m1 m2）gl1 cos1 m2 gl2 cos(1 2 )
m l l cos ） m l 2 m l l sin 2 2 （m2l22 1 21 2 2 1 2 2 2 2 1 2 2 1 m2 gl2 cos(1 2 )
牛顿—欧拉方程实例
例2：如图所示为两杆平面机器人，为 了简单起见，我们假设每个杆件的质量集 中于杆件的前尾部，其大小为m1和m2。 解：每个杆件的质量中心 矢量为：
ˆ , P l X ˆ Pc1 l1 X 1 c2 2 2
由于点质量假设， 每个杆件相对质心的惯 性张量为零，即：
I c1 0, I c 2 0
l sin ( )( ) 2 l1sin1 x 1 2 1 2 1 2 l cos( )( ) l cos y
2 1 1 1 2 1 2 1 2
则质量M2的速度平方为：
1 1 1 2
2 2 2 l sin ( )( )） 2 2 x y （l1sin1 1 2 1 2 1 2 2 l cos( )( )） （l cos 1 2 1 2