机器人动力学 拉格朗日方程

山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
L
2

m2 gl1l2sin2
1 (1

2 ) m2 gl2
cos(1 2 )
L
2

m2l22 (1
2 ) m2l1l2 cos2 1
d dt
l1212 l22 (12 212 22 ) 2l1l2 cos(12 12 )
所以，M2动能为：
T2

1 2
m2[l1212
l22 (12

212
22 )

2l1l2
cos2 (12
12 )]
势能为：
力或广义力矩
L—系统的动能 Ek 和位能 Ep之差，称为拉格朗日
函数，即： L Ek Ep
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
例3：对例2所示两杆平面机器人用拉格朗日方 法建立动力学方程。
解：
1、动能和势能
连杆1的动能为：
T1

1 2
m1(l1
1)2
设Y0=0为零势面，则连杆1的
势能为：
V1 m1gl1 sin1
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
质量m2的位置表示为： x2 l1 cos1 l2 cos(1 2 )
y2 l1 sin1 l2 sin(1 2 )
4.3 机器人拉格朗日动力学方程简介
上式进一步写成
n
nn
Qi Di, j qk Iai qi
Di, j,k qk qm Di
j 1
j 1 k 1
式中
Di, j

p
n max(i
,
j
)
Trace

Tp q j
Hp
TpT qi

Di, j,k
速度分量为： x2 l1sin11 l2sin(1 2 )(1 2 )
y2 l1cos11 l2cos(1 2 )(1 2 )
则质量M2的速度平方为：
x22 y22 （l1sin11 l2sin(1 2 )(1 2 )）2 （l1cos11 l2cos(1 2 )(1 2 )）2
因为 V ( ,) 中仅有速度和位形，上 述方程也称状态空间方程。 特点：
多变量、时变、非线性、强耦合。
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
拉格朗日方程是基于能量项（动能 T、势能V）对系统变量及时间的微分 而建立的。
对于简单系统拉格朗日方程法相较 于牛顿—欧拉方程法更显复杂，然而随 着系统复杂程度的增加，拉格朗日方程 法建立系统运动微分方程变得相对简单。

(m1

m2 )gl1
cos1

m2 gl2
cos(1
2)
d dt
L
1
[(m1
m2 )l12
m2l22
2m2l1l2
cos2 ]1
[m2l22

m2l1l2
cos2 ]2
2m2l1l2 sin2 1 2 m2l1l2 sin2 22
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
系统拉格朗日方程为：
Qi

d dt
L

qi

L qi
i 1, 2,...n
式中： n ——系统的广义坐标数
qi ——第i个广义坐标 qi ——第i个广义速度
Qi ——作用在第i个广义坐标上的广义
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
末端执行器上无作用力，所以： 基座静止，因此：
0 0, 0 0 考虑到引力，我们使用：
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
应用递推公式有： 向前：1杆件：
山东大学机械工程学院机电工程研究所2010/09/02
V2 m2 gl1 sin1 m2 gl2 sin(1 2 )
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
2、拉格朗日函数
L T1 V1 T2 V2

1 2
(m1

m2 )l1212

1 2
m2l22 (1
2 )2
4.3 机器人拉格朗日动力学方程简介
4、计算拉格朗日函数
L Ekt E p

1n 2 i 1
i j 1
i k 1
Trace

Ti q j
Hi
TiT qk
q j qk
1 2
n i 1
Ia i
qi2
n
mi gTTi ri
i 1
L Ek Ep
k
j 1
Trace

Tj qk
Hj
T j T qi
qk
Iai
qi
n

j i
k
j 1
j
Trace
m1

2T j qk qm
Hj
T
T j
qi
qk qm
n
mj gT
j i
Tj qi
ri
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
代入：
1 [(m1 m2 )l12 m2l22 2m2l1l2 cos2 ]1 [m2l22 m2l1l2 cos2 ]2 2m2l1l2 sin2 1 2 m2l1l2 sin2 22 （m1 m2）gl1 cos1 m2gl2 cos(1 2 )
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
整理得：
1 [(m1 m2 )l12 m2l22 2m2l1l2 cos2 ]1 [m2l22 m2l1l2 cos2 ]2 2m2l1l2 sin2 1 2 m2l1l2 sin2 22 m2 gl2 cos(1 2 ) （m1 m2）gl1 cos1

p
n max(i
,
j,k
)
Trace

2Tp q jqk
Hp
TpT qi

Di

n
mp gT
pi
Tp qi
rp
山东大学机械工程学院机电工程研究所2010/09/02
i j 1
i

Trace
k 1

2Ti q j qk
Hi
TiT q p
q j qk
n
mi gT
i p
Ti q p
ri
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
作用在关节上的广义力为：
Qi

n j i
2 （m2l221 m2l1l2 cos2）1 m2l222 m2l1l2 sin2 12 m2gl2 cos(1 2 )
比较例2与例3可知，用牛顿-欧拉法与拉格朗 日法得到的结果是相同的。
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介 求出速度的平方：
v2 v v
Trace v vT

Trace
i j 1
Ti q j
q
j
i

r

k 1
Ti qk
qk
T
rT


Trace
i j 1
H
i

Ti qk
T


q
j
qk


1n 2 i 1
i j 1
i k 1
Trace
Ti q j
Hi
TiT qk
q j qk
3、求系统位能
n
n
EP Ep i mi gTTi ri
i 1
i 1
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
惯性力
2杆件：
惯性力矩
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
向后递推： 2杆件：
1杆件：
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
取力矩的Z分量，得到关节力矩：
i k 1
Ti q j
r rT

Ti qk
T



q
j
qk

山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
• 2、求系统动能
Ek

n
Eki
i 1

1 2
n
i
Trace
i Ti
i 1
j1 k 1 q j

m2l1l2
cos2 (12
12 )
(m1 m2 )gl1 sin1 m2 gl2 sin(1 2 )
3、动力学方程
L
1wk.baidu.com

(m1

m2 )l121
m2l22 (1
2 ) m2l1l2
cos2 (21
2 )
L
1
牛顿—欧拉方程实例
例2：如图所示为两杆平面机器人，为 了简单起见，我们假设每个杆件的质量集
中于杆件的前尾部，其大小为m1和m2。
解：每个杆件的质量中心 矢量为：
Pc1 l1 Xˆ1, Pc2 l2 Xˆ 2
由于点质量假设， 每个杆件相对质心的惯 性张量为零，即：
Ic1 0, Ic2 0
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
5、代入拉格朗日方程
d L
dt

qi


L qi

i
n
p
i

Trace
k 1

Ti qk
Hi
TiT q p
qk
Ia p
qp
n

i p
2 （m2l221 m2l1l2 cos2）1 m2l222 m2l1l2 sin2 12 m2 gl2 cos(1 2 )
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
通常，机器人的动力学方程常写为抽象的形式，
4.3 机器人拉格朗日动力学方程简介
步骤总结：
1、机械臂上一点速度 设杆件i上一点ri，它在基坐标系中的位
置为：
r Ti ri
其中，Ti是{i}坐标系相对基础坐标系的齐次变 换矩阵。
那么，该点的速度为：
vi

dr dt


i j 1
Ti q j
q j


ri
山东大学机械工程学院机电工程研究所2010/09/02
令：


1 2

离心力
科氏力
山东大学机械工程学院机电工程研究所2010/09/02
机器人机构动力学方程 有： M () V ( ,) G() Q
其中： 为广义坐标向量，Q 为广义力向量。 称 M ()为惯量阵，V ( ,) 是离心力、科 氏力等相关部分，G() 为重力部分。
L
2

m2l221
m2l222
m2l1l2 cos2
1
m2l1l2sin2
1 2
用于的广关义节坐上标的为驱动1和力距2 对1和应的2 广。义外力为作
d dt
L
1
-
L
1
1
d dt
L
2
- L
2
2
山东大学机械工程学院机电工程研究所2010/09/02