机器人动力学方程
机器人动力学方程是描述机器人力和运动之间的关系的方程。只描述力和运动的关系,不考虑产生运动的力和扭矩。
欧拉 - 拉格朗日方程
欧拉-拉格朗日方程(OL)描述了处于完整约束下,并且约束力满足虚功原理的机械系统的力和运动随时间的变化。
有两种推导方法,先介绍使用牛顿第二定律的推导方法。
根据牛顿第二定律,某质点的运动方程是:
m
y
¨
=
f
−
m
g
m\ddot y = f-mg
my¨=f−mg
先对时间求导,再对
y
˙
\dot y
y˙求偏导,方程左侧可以写为:
m
y
¨
=
d
d
t
(
m
y
˙
)
=
d
d
t
∂
∂
y
˙
(
1
2
m
y
˙
2
)
=
d
d
t
∂
K
∂
y
˙
m \ddot y = {d \over dt}(m \dot y)={d \over dt}{\partial \over \partial \dot y}({1 \over 2}m \dot y^2)={d \over dt}{\partial K \over \partial \dot y}
my¨=dtd(my˙)=dtd∂y˙∂(21my˙2)=dtd∂y˙∂K
其中
K
=
1
2
m
y
˙
2
K = {1 \over 2}m \dot y^2
K=21my˙2,是动能。
接着将重力表示为:
m
g
=
∂
∂
y
(
m
g
y
)
=
∂
P
∂
y
mg = {\partial \over \partial y}(mgy) = {\partial P \over \partial y}
mg=∂y∂(mgy)=∂y∂P
其中
P
=
m
g
y
P = mgy
P=mgy表示重力势能。
定义拉格朗日算子
L
L
L,表示系统动能与势能之差:
L
=
K
−
P
=
1
2
m
y
˙
2
−
m
g
y
L = K-P ={1 \over 2}m \dot y^2-mgy
L=K−P=21my˙2−mgy
并且有:
∂
L
∂
y
˙
=
∂
K
∂
y
˙
,
∂
L
∂
y
=
−
∂
P
∂
y
{\partial L\over \partial \dot y} = {\partial K \over \partial \dot y},{\partial L \over \partial y} =-{\partial P \over \partial y}
∂y˙∂L=∂y˙∂K,∂y∂L=−∂y∂P
则初始的质点运动方程可化为:
d
d
t
∂
K
∂
y
˙
=
f
−
∂
P
∂
y
{d \over dt}{\partial K \over \partial \dot y} = f-{\partial P \over \partial y}
dtd∂y˙∂K=f−∂y∂P
即:
d
d
t
∂
L
∂
y
˙
−
∂
L
∂
y
=
f
{d \over dt}{\partial L \over \partial \dot y}-{\partial L \over \partial y} = f
dtd∂y˙∂L−∂y∂L=f
此方程被称为欧拉-拉格朗日方程。
推广到n自由度的系统,得到:
d
d
t
∂
L
∂
q
˙
k
−
∂
L
∂
q
k
=
τ
k
{d \over dt}{\partial L \over \partial \dot q_k}-{\partial L \over \partial q_k} = \tau_k
dtd∂q˙k∂L−∂qk∂L=τk
其中
τ
k
\tau_k
τk是与广义坐标
q
k
q_k
qk相关的力。
动能与势能
欧拉-拉格朗日方程可以直接用来推导动力学方程,前提是我们能够以一组广义坐标来表示该系统的动能和势能。如果要让这能够得到实际应用,那么我们就必须能够针对一个n连杆机器人计算出他的动能和势能。接下来将推到刚性连杆机器人动能和势能的表达式。
动能表示
刚体的动能可表示为平移动能和关于质心的旋转动能之和:
K
=
1
2
m
v
T
v
+
1
2
ω
T
Z
ω
K = {1 \over 2}mv^Tv+{1\over 2}\omega^T Z\omega
K=21mvTv+21ωTZω
Z
Z
Z表示物体的惯性张量,是一个3*3的对称矩阵。
Z = R I R T Z = RIR^T Z=RIRT,R是附体坐标系与惯性坐标系之间的姿态变换。
I
I
I是附体坐标系中的惯性张量,仅取决于物体的形状和质量分布,与物体运动无关。
速度
v
v
v和角速度
ω
\omega
ω需要转置,因为要考虑多个维度的方向。连杆上任意一点的现速度和角速度可通过雅可比矩阵和关节速度(关节变量的导数)来表示:
v
i
=
J
v
i
q
q
˙
ω
i
=
J
ω
i
q
q
˙
v_i = J_{v_i}q\dot q \\ \omega_i = J_{\omega_i}q\dot q
vi=Jviqq˙ωi=Jωiqq˙
机器人总动能可表示为:
K
=
1
2
q
˙
T
∑
i
=
1
n
[
m
i
(
J
v
i
(
q
)
)
T
J
v
i
(
q
)
+
(
J
ω
i
(
q
)
)
T
R
i
(
q
)
I
i
(
R
i
(
q
)
)
T
J
ω
i
(
q
)
]
q
˙
K = {1 \over 2}\dot q^T \sum_{i=1}^n[m_i\ {(J_{v_i}(q))}^T \ J_{v_i}(q) \ + \ (J_{\omega_i}(q))^T \ R_{i}(q) \ I_i \ (R_i(q))^T \ J_{\omega_i}(q)]\dot q
K=21q˙Ti=1∑n[mi (Jvi(q))T Jvi(q) + (Jωi(q))T Ri(q) Ii (Ri(q))T Jωi(q)]q˙
用
D
(
q
)
D(q)
D(q)来表示机器人的惯性矩阵:
D
(
q
)
=
∑
i
=
1
n
[
m
i
(
J
v
i
(
q
)
)
T
J
v
i
(
q
)
+
(
J
ω
i
(
q
)
)
T
R
i
(
q
)
I
i
(
R
i
(
q
)
)
T
J
ω
i
(
q
)
]
K
=
1
2
q
˙
T
D
(
q
)
q
˙
D(q) \ = \ \sum_{i=1}^n[m_i\ {(J_{v_i}(q))}^T \ J_{v_i}(q) \ + \ (J_{\omega_i}(q))^T \ R_{i}(q) \ I_i \ (R_i(q))^T \ J_{\omega_i}(q)] \\ K = {1 \over 2}\dot q^T D(q)\dot q
D(q) = i=1∑n[mi (Jvi(q))T Jvi(q) + (Jωi(q))T Ri(q) Ii (Ri(q))T Jωi(q)]K=21q˙TD(q)q˙
机器人惯性矩阵
D
(
q
)
D(q)
D(q)有如下特点:
- 只与机器人构型有关
- 对称且正定
- 动能总是非负的
势能表示
在刚体动力学情形下,势能总是来源于重力。假设物体质量集中在质心,计算第
i
i
i个连杆的势能:
P
i
=
m
i
g
T
r
c
i
P_i = m_ig^Tr_{ci}
Pi=migTrci
g
g
g是惯性坐标系中的重力向量,
r
c
i
r_{ci}
rci是连杆
i
i
i的质心坐标。机器人总势能为:
P
=
∑
i
=
1
n
P
i
=
∑
i
=
1
n
m
i
g
T
r
c
i
P = \sum_{i=1}^n P_i = \sum_{i=1}^n m_ig^Tr_{ci}
P=i=1∑nPi=i=1∑nmigTrci
在m、g保持不变的情况下,机器人势能只与广义坐标
r
c
i
r_{ci}
rci有关。
运动方程
上面我们得到了如下结果:
系统动能是关于广义速度(坐标微分)的二次函数:
K
=
1
2
q
˙
T
D
(
q
)
q
˙
=
1
2
∑
i
,
j
n
d
i
,
j
(
q
)
q
˙
i
q
˙
j
K = {1 \over 2}\dot q^T D(q)\dot q = {1 \over 2}\sum_{i,j}^n d_{i,j}(q) \dot q_i \dot q_j
K=21q˙TD(q)q˙=21i,j∑ndi,j(q)q˙iq˙j
系统势能是关于广义坐标的函数,且与广义速度无关:
P
=
∑
i
=
1
n
m
i
g
T
r
c
i
P = \sum_{i=1}^n m_ig^Tr_{ci}
P=i=1∑nmigTrci
欧拉-拉格朗日算子为:
L
=
K
−
P
=
1
2
∑
i
,
j
n
d
i
,
j
(
q
)
q
˙
i
q
˙
j
−
P
(
q
)
L = K-P ={1 \over 2}\sum_{i,j}^n d_{i,j}(q) \dot q_i \dot q_j-P(q)
L=K−P=21i,j∑ndi,j(q)q˙iq˙j−P(q)
欧拉-拉格朗日方程为:
d
d
t
∂
L
∂
q
˙
k
−
∂
L
∂
q
k
=
τ
k
{d \over dt}{\partial L \over \partial \dot q_k}-{\partial L \over \partial q_k} = \tau_k
dtd∂q˙k∂L−∂qk∂L=τk
其中:
∂
L
∂
q
˙
k
=
∑
j
d
k
j
q
˙
j
d
d
t
∂
L
∂
q
˙
k
=
∑
j
d
k
j
q
¨
j
+
∑
j
d
d
t
d
k
j
q
˙
j
=
∑
j
d
k
j
q
¨
j
+
∑
i
,
j
∂
d
k
j
∂
q
i
q
˙
i
q
˙
j
∂
L
∂
q
k
=
1
2
∑
i
,
j
∂
d
i
,
j
∂
q
k
q
˙
i
q
˙
j
−
∂
P
∂
q
k
{\partial L \over \partial \dot q_k}= \sum_jd_{kj}\dot q_j\\ {d \over dt}{\partial L \over \partial \dot q_k} = \sum_jd_{kj}\ddot q_j+\sum_{j}{d \over dt}d_{kj}\dot q_j \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \sum_jd_{kj}\ddot q_j+\sum_{i,j}{\partial d_{kj}\over \partial q_i}\dot q_i\dot q_j \\ {\partial L \over \partial q_k} = {1 \over 2}\sum_{i,j} {\partial d_{i,j} \over \partial q_k} \dot q_i \dot q_j - {\partial P \over \partial q_k}
∂q˙k∂L=j∑dkjq˙jdtd∂q˙k∂L=j∑dkjq¨j+j∑dtddkjq˙j =j∑dkjq¨j+i,j∑∂qi∂dkjq˙iq˙j∂qk∂L=21i,j∑∂qk∂di,jq˙iq˙j−∂qk∂P
因此对于每个
k
=
1
,
2
,
.
.
.
n
k=1,2,...n
k=1,2,...n,欧拉-拉格朗日方程可以写成:
∑
j
d
k
j
q
¨
j
+
∑
i
,
j
{
∂
d
k
j
∂
q
i
−
1
2
∂
d
i
,
j
∂
q
k
}
q
˙
i
q
˙
j
−
∂
P
∂
q
k
=
τ
k
\sum_jd_{kj}\ddot q_j+\sum_{i,j}\{ {\partial d_{kj}\over \partial q_i} -{1\over 2}{\partial d_{i,j} \over \partial q_k}\} \dot q_i\dot q_j - {\partial P \over \partial q_k}= \tau_k
j∑dkjq¨j+i,j∑{∂qi∂dkj−21∂qk∂di,j}q˙iq˙j−∂qk∂P=τk
即:
∑
j
d
k
j
q
¨
j
+
∑
i
,
j
1
2
{
∂
d
k
j
∂
q
i
+
∂
d
k
j
∂
q
i
−
∂
d
i
,
j
∂
q
k
}
q
˙
i
q
˙
j
−
∂
P
∂
q
k
=
τ
k
\sum_jd_{kj}\ddot q_j+\sum_{i,j} {1\over 2}\{ {\partial d_{kj}\over \partial q_i} + {\partial d_{kj}\over \partial q_i} -{\partial d_{i,j} \over \partial q_k}\} \dot q_i\dot q_j - {\partial P \over \partial q_k}= \tau_k
j∑dkjq¨j+i,j∑21{∂qi∂dkj+∂qi∂dkj−∂qk∂di,j}q˙iq˙j−∂qk∂P=τk
定义Christoffel symbol:
c
i
j
k
=
c
j
i
k
=
1
2
{
∂
d
k
j
∂
q
i
+
∂
d
k
j
∂
q
i
−
∂
d
i
,
j
∂
q
k
}
c_{ijk} = c_{jik} ={1\over 2}\{ {\partial d_{kj}\over \partial q_i} + {\partial d_{kj}\over \partial q_i} -{\partial d_{i,j} \over \partial q_k} \}
cijk=cjik=21{∂qi∂dkj+∂qi∂dkj−∂qk∂di,j}
定义广义重力:
g
k
=
∂
P
∂
q
k
g_k = {\partial P \over \partial q_k}
gk=∂qk∂P
最终得到欧拉-拉格朗日方程:
∑
j
d
k
j
(
q
)
q
¨
j
+
∑
i
,
j
c
i
j
k
(
q
)
q
˙
i
q
˙
j
−
g
k
(
q
)
=
τ
k
\sum_jd_{kj}(q)\ddot q_j+\sum_{i,j} c_{ijk}(q) \dot q_i\dot q_j - g_k(q)= \tau_k
j∑dkj(q)q¨j+i,j∑cijk(q)q˙iq˙j−gk(q)=τk
方程左侧三项分别为:
- 广义坐标的二阶导数:惯性项
- 广义坐标一阶导数的二次型:离心力项+哥氏力项
- 广义位置(0阶导数)重力项
方程可简写为:
D
(
q
)
q
¨
+
C
(
q
,
q
˙
)
q
˙
+
g
(
q
)
=
τ
D(q)\ddot q+C(q,\dot q)\dot q+g(q) = \tau
D(q)q¨+C(q,q˙)q˙+g(q)=τ
推导平面2关节机器人的动力学模型
现在考虑下图中带有两个转动关节的平面机械臂。
要使用刚刚得到的欧拉-拉格朗日方程,就要与关节位置和关节速度相关的三个量: D ( q ) , C ( q , q ˙ ) , g ( q ) D(q),C(q,\dot q),g(q) D(q),C(q,q˙),g(q)。
首先使用雅可比矩阵来计算动能,计算平移速度:
v
c
1
=
J
v
c
1
q
˙
v
c
2
=
J
v
c
2
q
˙
J
v
c
1
=
[
−
l
c
s
i
n
(
q
1
)
0
l
c
1
c
o
s
(
q
1
)
0
0
0
]
J
v
c
2
=
[
−
l
1
s
i
n
(
q
1
)
−
l
c
2
s
i
n
(
q
1
+
q
2
)
−
l
c
2
s
i
n
(
q
1
+
q
2
)
l
1
c
o
s
(
q
1
)
+
l
c
2
c
o
s
(
q
1
+
q
2
)
l
c
2
c
o
s
(
q
1
+
q
2
)
0
0
]
v_{c1} = J_{v_{c1}}\dot q \\ v_{c2} = J_{v_{c2}}\dot q \\ J_{v_{c1}} = \begin{bmatrix}-l_c sin(q_1) & 0 \\ l_{c1}cos(q_1) & 0\\ 0 & 0 \\\end{bmatrix} \\ J_{v_{c2}} = \begin{bmatrix}-l_1sin(q_1)-l_{c2}sin(q_1+q_2) & -l_{c2}sin(q_1+q_2) \\ l_1cos(q_1)+l_{c2}cos(q_1+q_2) & l_{c2}cos(q_1+q_2)\\ 0 & 0 \\\end{bmatrix}
vc1=Jvc1q˙vc2=Jvc2q˙Jvc1=⎣⎡−lcsin(q1)lc1cos(q1)0000⎦⎤Jvc2=⎣⎡−l1sin(q1)−lc2sin(q1+q2)l1cos(q1)+lc2cos(q1+q2)0−lc2sin(q1+q2)lc2cos(q1+q2)0⎦⎤
平移部分对应的动能为:
1
2
m
1
v
c
1
T
v
c
1
+
1
2
m
2
v
c
2
T
v
c
2
=
1
2
q
˙
{
m
1
J
v
c
1
T
J
v
c
1
+
m
2
J
v
c
2
T
J
v
c
2
}
q
˙
{1\over 2}m_1 v^T_{c1}v_{c1}+{1\over2}m_2v^T_{c2}v_{c2} = {1\over 2}\dot q\{ m_1J^T_{v_{c1}}J_{v_{c1}} +m_2J^T_{v_{c2}}J_{v_{c2}} \}\dot q
21m1vc1Tvc1+21m2vc2Tvc2=21q˙{m1Jvc1TJvc1+m2Jvc2TJvc2}q˙
接下来考虑角速度项:
ω
1
=
q
˙
1
k
ω
2
=
(
q
˙
1
+
q
˙
2
)
k
\omega_1 = \dot q_1k \\ \omega_2 = (\dot q_1+ \dot q_2)k
ω1=q˙1kω2=(q˙1+q˙2)k
由于
ω
i
\omega_i
ωi与每个关节坐标系的z轴对齐,旋转动能可以简单表示为
1
2
I
i
ω
i
2
{1\over 2}I_i\omega_i^2
21Iiωi2,其中
I
i
I_i
Ii是转动惯量,它的轴线穿过连杆i的质心且平行于
z
i
z_i
zi轴。因此,就广义坐标而言,整个系统的旋转动能为:
1
2
q
˙
T
{
I
1
[
1
0
0
0
]
+
I
2
[
1
1
1
1
]
}
q
˙
{1\over 2}\dot q^T \{ I_1 \begin{bmatrix}1 &0 \\ 0&0 \end{bmatrix} + I_2 \begin{bmatrix} 1&1\\1&1 \end{bmatrix}\}\dot q
21q˙T{I1[1000]+I2[1111]}q˙
惯性矩阵:
D
(
q
)
=
m
1
J
v
c
1
T
J
v
c
1
+
m
2
J
v
c
2
T
J
v
c
2
+
[
I
1
+
I
2
I
2
I
2
I
2
]
=
[
d
11
d
12
d
21
d
22
]
D(q) = m_1J^T_{v_{c1}}J_{v_{c1}} +m_2J^T_{v_{c2}}J_{v_{c2}} + \begin{bmatrix} I_1+I_2&I_2 \\ I_2&I_2 \end{bmatrix} = \begin{bmatrix} d_{11}&d_{12} \\ d_{21}&d_{22} \end{bmatrix} \\
D(q)=m1Jvc1TJvc1+m2Jvc2TJvc2+[I1+I2I2I2I2]=[d11d21d12d22]
计算得:
d
11
=
m
1
l
c
1
2
+
m
2
(
l
1
2
+
l
c
2
2
+
2
l
1
l
c
2
c
o
s
(
q
2
)
)
+
I
1
+
I
2
d
12
=
d
21
=
m
2
(
l
c
2
2
+
l
1
l
c
2
c
o
s
(
q
2
)
)
+
I
2
d
22
=
m
2
l
c
2
2
+
I
2
d_{11} = m_1l_{c1}^2 + m_2(l_1^2+l^2_{c2}+2l_1l_{c2}cos(q_2))+I_1+I_2\\ d_{12} = d_{21} = m_2(l^2_{c2}+l_1l_{c2}cos(q_2))+I_2 \\ d_{22} = m_2l^2_{c2}+I_2
d11=m1lc12+m2(l12+lc22+2l1lc2cos(q2))+I1+I2d12=d21=m2(lc22+l1lc2cos(q2))+I2d22=m2lc22+I2
我们已经得到了惯性矩阵,接下来计算Christoffel符号
c
i
j
k
c_{ijk}
cijk:
c
111
=
1
2
∂
d
11
∂
q
1
=
0
c
121
=
c
211
=
1
2
∂
d
11
∂
q
2
=
−
m
2
l
1
l
c
2
s
i
n
(
q
2
)
=
h
c
221
=
∂
d
12
∂
q
2
−
1
2
∂
d
11
∂
q
1
=
h
c
112
=
∂
d
21
∂
q
1
−
1
2
∂
d
11
∂
q
2
=
−
h
c
122
=
c
212
=
1
2
∂
d
22
∂
q
1
=
0
c
222
=
1
2
∂
d
22
∂
q
2
=
0
c_{111}= {1 \over 2}{\partial d_{11}\over \partial q_1} = 0 \\ c_{121}= c_{211}= {1 \over 2}{\partial d_{11}\over \partial q_2} = -m_2l_1l_{c2}sin(q_2) = h \\ c_{221}= {\partial d_{12}\over \partial q_2}-{1 \over 2}{\partial d_{11}\over \partial q_1} = h \\ c_{112}= {\partial d_{21}\over \partial q_1}-{1 \over 2}{\partial d_{11}\over \partial q_2} = -h \\ c_{122}= c_{212}= {1 \over 2}{\partial d_{22}\over \partial q_1} = 0\\ c_{222}= {1 \over 2}{\partial d_{22}\over \partial q_2} = 0 \\
c111=21∂q1∂d11=0c121=c211=21∂q2∂d11=−m2l1lc2sin(q2)=hc221=∂q2∂d12−21∂q1∂d11=hc112=∂q1∂d21−21∂q2∂d11=−hc122=c212=21∂q1∂d22=0c222=21∂q2∂d22=0
接下来计算势能,机械臂的势能等于两个连杆势能之和。
P
1
=
m
1
g
l
c
1
s
i
n
(
q
1
)
P
2
=
m
2
g
(
l
2
s
i
n
(
q
1
)
+
l
c
2
s
i
n
(
q
1
+
q
2
)
)
P
=
P
1
+
P
2
=
(
m
1
l
c
1
+
m
2
l
1
)
g
s
i
n
(
q
1
)
+
m
2
l
c
2
g
s
i
n
(
q
1
+
q
2
)
P_1 = m_1gl_{c1}sin(q_1) \\ P_2 = m_2g(l_{2}sin(q_1)+l_{c2}sin(q_1+q_2))\\ P = P_1+P_2 =(m_1l_{c1}+m_2l_1)gsin(q_1)+m_2l_{c2}gsin(q_1+q_2)
P1=m1glc1sin(q1)P2=m2g(l2sin(q1)+lc2sin(q1+q2))P=P1+P2=(m1lc1+m2l1)gsin(q1)+m2lc2gsin(q1+q2)
之前的广义重力
g
k
g_k
gk可变为:
g
1
=
∂
P
∂
q
1
=
(
m
1
l
c
1
+
m
2
l
1
)
g
c
o
s
(
q
1
)
+
m
2
l
c
2
g
c
o
s
(
q
1
+
q
2
)
g
2
=
∂
P
∂
q
2
=
m
2
l
c
2
g
c
o
s
(
q
1
+
q
2
)
g_1 = {\partial P \over\partial q_1 } = (m_1l_{c1}+m_2l_1)gcos(q_1)+m_2l_{c2}gcos(q_1+q_2) \\ g_2 = {\partial P \over\partial q_2 } = m_2l_{c2}gcos(q_1+q_2)
g1=∂q1∂P=(m1lc1+m2l1)gcos(q1)+m2lc2gcos(q1+q2)g2=∂q2∂P=m2lc2gcos(q1+q2)
最后可以写出系统的动力学方程:
d
11
q
¨
1
+
d
12
q
¨
2
+
c
121
q
˙
1
q
˙
2
+
c
211
q
˙
2
q
˙
1
+
c
221
q
˙
2
2
+
g
1
=
τ
1
d
21
q
¨
1
+
d
22
q
¨
2
+
c
112
q
˙
1
2
+
g
2
=
τ
2
d_{11}\ddot q_1+d_{12} \ddot q_2+c_{121}\dot q_1 \dot q_2 + c_{211}\dot q_2 \dot q_1 +c_{221}\dot q_2^2+g_1=\tau_1 \\ d_{21} \ddot q_1+d_{22}\ddot q_2+c_{112}\dot q_1^2+g_2 = \tau_2
d11q¨1+d12q¨2+c121q˙1q˙2+c211q˙2q˙1+c221q˙22+g1=τ1d21q¨1+d22q¨2+c112q˙12+g2=τ2
在这种情况下,原方程矩阵
C
(
q
,
q
˙
)
C(q, \dot q)
C(q,q˙)由下式给出:
C
=
[
h
q
˙
2
h
q
˙
2
+
h
q
˙
1
−
h
q
˙
1
0
]
C = \begin{bmatrix}h\dot q_2 & h \dot q_2+h\dot q_1 \\ -h\dot q_1 & 0\end{bmatrix}
C=[hq˙2−hq˙1hq˙2+hq˙10]