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

l1212 l22 (12 212 22 ) 2l1l2 cos(12 12 )
所以，M2动能为：
T2

1 2
m2[l1212
l22 (12

212
22 )

2l1l2
cos2 (12
12 )]
势能为：
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
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 )

p
n max(i
,
j,k
)
Trace

2Tp q jqk
Hp
TpT qi

Di

n
mp gT
pi
Tp qi
rp
山东大学机械工程学院机电工程研究所2010/09/02
山东大学机械工程学院机电工程研究所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
山东大学机械工程学院机电工程研究所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
牛顿—欧拉方程实例
惯性力
2杆件：
惯性力矩
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
向后递推： 2杆件：
1杆件：
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
取力矩的Z分量，得到关节力矩：
速度分量为： 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
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
所2010/09/02
4.3 机器人拉格朗日动力学方程简介
作用在关节上的广义力为：
Qi

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

1 2
m1(l1
令：


1 2

离心力
科氏力
山东大学机械工程学院机电工程研究所2010/09/02
机器人机构动力学方程 有： M () V ( ,) G() Q
其中： 为广义坐标向量，Q 为广义力向量。 称 M ()为惯量阵，V ( ,) 是离心力、科 氏力等相关部分，G() 为重力部分。
因为 V ( ,) 中仅有速度和位形，上 述方程也称状态空间方程。 特点：
多变量、时变、非线性、强耦合。
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
拉格朗日方程是基于能量项（动能 T、势能V）对系统变量及时间的微分 而建立的。
对于简单系统拉格朗日方程法相较 于牛顿—欧拉方程法更显复杂，然而随 着系统复杂程度的增加，拉格朗日方程 法建立系统运动微分方程变得相对简单。
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
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 机器人拉格朗日动力学方程简介 求出速度的平方：
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
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
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
末端执行器上无作用力，所以： 基座静止，因此：
0 0, 0 0 考虑到引力，我们使用：
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
应用递推公式有： 向前：1杆件：
山东大学机械工程学院机电工程研究所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
2 （m2l221 m2l1l2 cos2）1 m2l222 m2l1l2 sin2 12 m2 gl2 cos(1 2 )
山东大学机械工程学院机电工程研究所2010/09/02
牛顿—欧拉方程实例
通常，机器人的动力学方程常写为抽象的形式，
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
山东大学机械工程学院机电工程研究所2010/09/02
4.3 机器人拉格朗日动力学方程简介
系统拉格朗日方程为：
Qi

d dt
L

qi

L qi
i 1, 2,...n
式中： n ——系统的广义坐标数
qi ——第i个广义坐标 qi ——第i个广义速度
Qi ——作用在第i个广义坐标上的广义

(m1

m2 )gl1
cos1