Modern Robotics单个刚体的动力学
基本公式
考虑刚体由质量为 m i m_i mi的一系列质点 i i i组成,总质量为 m = ∑ i m i \text m=\sum_i\text m_i m=∑imi。物体坐标系原点建立在刚体质心处 r i = ( x i , y i , z i ) r_i=(x_i,y_i,z_i) ri=(xi,yi,zi),则有 ∑ i m i r i = 0 \sum_i\text m_ir_i=0 ∑imiri=0.
设一个刚体的运动旋量为
V
b
=
(
ω
b
,
v
b
)
\mathcal V_b=(\omega_b,v_b)
Vb=(ωb,vb),质点
m
i
\text m_i
mi的位置坐标为
p
i
(
t
)
p_i(t)
pi(t), 在惯性坐标系{b}的最初位置为
r
i
r_i
ri。注意这些物理量都是在物体坐标系中描述,那么有
p
˙
i
=
v
b
+
ω
b
×
p
i
p
¨
i
=
v
˙
b
+
d
d
t
ω
b
×
p
i
+
ω
b
×
d
d
t
p
i
=
v
˙
b
+
ω
˙
b
×
p
i
+
ω
b
×
(
v
b
+
ω
b
×
p
i
)
\dot p_i=v_b+\omega_b\times p_i\\ \ddot p_i=\dot v_b+\frac{d}{dt}\omega_b\times p_i+\omega_b\times\frac{d}{dt}p_i\\ =\dot v_b+\dot \omega_b\times p_i+\omega_b\times(v_b+\omega _b\times p_i)
p˙i=vb+ωb×pip¨i=v˙b+dtdωb×pi+ωb×dtdpi=v˙b+ω˙b×pi+ωb×(vb+ωb×pi)
把
r
i
r_i
ri代替
p
i
p_i
pi,并使用反对称矩阵表示叉乘,那么有
p
¨
i
=
v
˙
b
+
[
ω
˙
b
]
r
i
+
[
ω
b
]
v
b
+
[
ω
b
]
2
r
i
\ddot p_i=\dot v_b+[\dot \omega_b]r_i+[\omega_b]v_b+[\omega_b]^2r_i
p¨i=v˙b+[ω˙b]ri+[ωb]vb+[ωb]2ri
根据牛顿第二定律,对于一个质点有
f
i
=
m
i
p
¨
i
f_i=m_i\ddot p_i
fi=mip¨i,作用在质点
m
i
m_i
mi的力为
f
i
=
m
i
(
v
˙
b
+
[
ω
˙
b
]
r
i
+
[
ω
b
]
v
b
+
[
ω
b
]
2
r
i
)
f_i=\text m_i(\dot v_b+[\dot \omega_b]r_i+[\omega_b]v_b+[\omega_b]^2r_i)
fi=mi(v˙b+[ω˙b]ri+[ωb]vb+[ωb]2ri)
这意味着力矩为
m
i
=
[
r
i
]
f
i
.
m_i=[r_i]f_i.
mi=[ri]fi.
作用在刚体的总力和总力矩用力旋量
F
b
\mathcal F_b
Fb表示为
F
b
=
[
m
b
f
b
]
=
[
∑
i
m
i
∑
i
f
i
]
\mathcal F_b=\left[ \begin{matrix} m_b\\f_b \end{matrix}\right]=\left[ \begin{matrix} \sum _im_i\\\sum_if_i \end{matrix}\right]
Fb=[mbfb]=[∑imi∑ifi]
下面化简
f
b
,
m
b
f_b,m_b
fb,mb,注意到
∑
i
m
i
r
i
=
0
\sum _i\text m_ir_i=0
∑imiri=0(因此,
∑
i
m
i
[
r
i
]
=
0
\sum_i\text m_i[r_i]=0
∑imi[ri]=0), 对于
a
,
b
∈
R
3
,
[
a
]
=
−
[
a
]
T
,
[
a
]
b
=
−
[
b
]
a
,
[
a
]
[
b
]
=
(
[
b
]
[
a
]
)
T
a,b\in \mathbb R^3,[a]=-[a]^T,[a]b=-[b]a,[a][b]=([b][a])^T
a,b∈R3,[a]=−[a]T,[a]b=−[b]a,[a][b]=([b][a])T.这些关系在下面化简中会用到。
平动动力学
f
b
=
∑
i
m
i
(
v
˙
b
+
[
ω
˙
b
]
r
i
+
[
ω
b
]
v
b
)
+
[
ω
b
]
2
r
i
=
∑
i
m
i
(
v
˙
b
+
[
ω
b
]
v
b
)
−
∑
i
m
i
[
r
i
]
ω
˙
b
+
∑
i
m
i
[
r
i
]
[
ω
b
]
ω
b
=
m
(
v
˙
b
+
[
ω
b
]
v
b
)
.
(8.22)
f_b=\sum _i\text m_i(\dot v_b+[\dot\omega_b]r_i+[\omega_b]v_b)+[\omega_b]^2r_i\\ =\sum_i\text m_i(\dot v_b+[\omega_b]v_b)-\sum_i\text m_i[r_i]\dot\omega_b+\sum_i\text m_i[r_i][\omega_b]\omega _b\\ =\text m(\dot v_b+[\omega_b]v_b).\tag{8.22}
fb=i∑mi(v˙b+[ω˙b]ri+[ωb]vb)+[ωb]2ri=i∑mi(v˙b+[ωb]vb)−i∑mi[ri]ω˙b+i∑mi[ri][ωb]ωb=m(v˙b+[ωb]vb).(8.22)
转动动力学
m
b
=
∑
i
m
i
[
r
i
]
(
v
˙
b
+
[
ω
˙
b
]
r
i
+
[
ω
b
]
v
b
+
[
ω
b
]
T
r
i
)
=
∑
i
m
i
[
r
i
]
v
˙
b
+
∑
i
m
i
[
r
i
]
[
ω
b
]
v
b
+
∑
i
m
i
[
r
i
]
(
[
ω
˙
b
]
r
i
+
[
ω
b
]
T
r
i
)
=
∑
i
m
i
(
−
[
r
i
]
2
ω
˙
b
−
[
r
i
]
T
[
ω
b
]
T
[
r
i
]
ω
b
)
=
(
−
∑
i
m
i
[
r
i
]
2
)
ω
˙
b
+
[
ω
b
]
(
−
∑
i
m
i
[
r
i
]
2
)
=
I
b
ω
˙
b
+
[
ω
b
]
I
b
ω
b
m_b=\sum_i\text m_i[r_i](\dot v_b+[\dot\omega_b]r_i+[\omega_b]v_b+[\omega_b]^Tr_i)\\ =\sum_i\text m_i[r_i]\dot v_b+\sum_i\text m_i[r_i][\omega_b]v_b+\sum_i\text m_i[r_i]([\dot\omega_b]r_i+[\omega_b]^Tr_i)\\ =\sum_i\text m_i(-[r_i]^2\dot\omega_b-[r_i]^T[\omega_b]^T[r_i]\omega_b)\\ =\left (-\sum_i\text m_i[r_i]^2 \right)\dot\omega_b+[\omega_b]\left(-\sum_i\text m_i[r_i]^2 \right)\\ =\mathcal I_b\dot\omega_b+[\omega_b]\mathcal I_b\omega_b
mb=i∑mi[ri](v˙b+[ω˙b]ri+[ωb]vb+[ωb]Tri)=i∑mi[ri]v˙b+i∑mi[ri][ωb]vb+i∑mi[ri]([ω˙b]ri+[ωb]Tri)=i∑mi(−[ri]2ω˙b−[ri]T[ωb]T[ri]ωb)=(−i∑mi[ri]2)ω˙b+[ωb](−i∑mi[ri]2)=Ibω˙b+[ωb]Ibωb
即
m
b
=
I
b
ω
˙
b
+
[
ω
b
]
I
b
ω
b
(8.23)
m_b=\mathcal I_b\dot\omega_b+[\omega_b]\mathcal I_b\omega_b \tag{8.23}
mb=Ibω˙b+[ωb]Ibωb(8.23)
这里
I
b
=
−
∑
i
m
i
[
r
i
]
2
∈
R
3
×
3
\mathcal I_b=-\sum_i\text m_i[r_i]^2\in\mathbb R^{3\times 3}
Ib=−∑imi[ri]2∈R3×3 是旋转惯量矩阵。方程(8.23)称为旋转刚体的欧拉方程。
写出
I
b
I_b
Ib的每个元素,有
I
b
=
[
∑
m
i
(
y
i
2
+
z
i
2
)
−
∑
m
i
x
i
y
i
−
∑
m
i
x
i
z
i
−
∑
m
i
x
i
y
i
∑
m
i
(
x
i
2
+
z
i
2
)
−
∑
m
i
y
i
z
i
−
∑
m
i
x
i
z
i
−
∑
m
i
y
i
z
i
∑
m
i
(
x
i
2
+
y
i
2
)
]
=
[
I
x
x
I
x
y
I
x
z
I
x
y
I
y
y
I
y
z
I
x
z
I
y
z
I
z
z
]
\mathcal I_b =\left[\begin{matrix} \sum \text m_i(y_i^2+z_i^2)& -\sum\text m_ix_iy_i &-\sum\text m_ix_iz_i \\ -\sum\text m_ix_iy_i & \sum \text m_i(x_i^2+z_i^2) &-\sum\text m_iy_iz_i \\ -\sum\text m_ix_iz_i & -\sum \text m_iy_iz_i &\sum \text m_i(x_i^2+y_i^2) \end{matrix} \right]\\ =\left[\begin{matrix} \mathcal I_{xx}&\mathcal I_{xy} &\mathcal I_{xz}\\ \mathcal I_{xy}&\mathcal I_{yy} &\mathcal I_{yz}\\ \mathcal I_{xz}&\mathcal I_{yz} &\mathcal I_{zz} \end{matrix} \right]
Ib=⎣⎡∑mi(yi2+zi2)−∑mixiyi−∑mixizi−∑mixiyi∑mi(xi2+zi2)−∑miyizi−∑mixizi−∑miyizi∑mi(xi2+yi2)⎦⎤=⎣⎡IxxIxyIxzIxyIyyIyzIxzIyzIzz⎦⎤
上面求和形式可以写成积分形式,物体体积为
B
\mathcal B
B, 体积微元
d
V
dV
dV, 质点
m
i
\text m_i
mi以密度函数替换
ρ
(
x
,
y
,
z
)
\rho (x,y,z)
ρ(x,y,z),
{
I
x
x
=
∫
B
(
y
2
+
z
2
)
ρ
(
x
,
y
,
z
)
d
V
I
y
y
=
∫
B
(
x
2
+
z
2
)
ρ
(
x
,
y
,
z
)
d
V
I
z
z
=
∫
B
(
x
2
+
y
2
)
ρ
(
x
,
y
,
z
)
d
V
I
z
z
=
∫
B
(
x
2
+
y
2
)
ρ
(
x
,
y
,
z
)
d
V
I
x
y
=
∫
B
x
y
ρ
(
x
,
y
,
z
)
d
V
I
x
z
=
∫
B
x
z
ρ
(
x
,
y
,
z
)
d
V
I
y
z
=
∫
B
y
z
ρ
(
x
,
y
,
z
)
d
V
(8.24)
\begin{cases} \mathcal I_{xx}=\int_\mathcal B(y^2+z^2)\rho(x,y,z)dV\\ \mathcal I_{yy}=\int_\mathcal B(x^2+z^2)\rho(x,y,z)dV\\ \mathcal I_{zz}=\int_\mathcal B(x^2+y^2)\rho(x,y,z)dV\\ \mathcal I_{zz}=\int_\mathcal B(x^2+y^2)\rho(x,y,z)dV\\ \mathcal I_{xy}=\int_\mathcal Bxy\rho(x,y,z)dV\\ \mathcal I_{xz}=\int_\mathcal Bxz\rho(x,y,z)dV\\ \mathcal I_{yz}=\int_\mathcal Byz\rho(x,y,z)dV \end{cases} \tag{8.24}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Ixx=∫B(y2+z2)ρ(x,y,z)dVIyy=∫B(x2+z2)ρ(x,y,z)dVIzz=∫B(x2+y2)ρ(x,y,z)dVIzz=∫B(x2+y2)ρ(x,y,z)dVIxy=∫Bxyρ(x,y,z)dVIxz=∫Bxzρ(x,y,z)dVIyz=∫Byzρ(x,y,z)dV(8.24)
给定一个惯量矩阵
I
b
\mathcal I_b
Ib, 惯量主轴由
I
b
\mathcal I_b
Ib的特征向量和特征值给定。设特征向量为
v
1
,
v
2
,
v
3
v_1,v_2,v_3
v1,v2,v3,特征值为
λ
1
,
λ
2
,
λ
3
\lambda_1, \lambda_2, \lambda_3
λ1,λ2,λ3. 那么惯量主轴的方向为特征向量
v
1
,
v
2
,
v
3
v_1,v_2,v_3
v1,v2,v3的方向,在这些轴的转动惯量值为主惯量矩,分别是
λ
1
,
λ
2
,
λ
3
>
0
\lambda_1, \lambda_2 , \lambda_3>0
λ1,λ2,λ3>0 .
假设在坐标系{b}中,一个惯量矩阵表示为
I
b
\mathcal I_b
Ib, 坐标系{c}可由旋转矩阵
R
b
c
R_{bc}
Rbc描述,那么该惯量矩阵在坐标系{c}中表示为
I
c
\mathcal I_c
Ic。 由于旋转刚体的动能与选择的坐标系无关,因此有
1
2
ω
c
T
I
c
ω
c
=
1
2
ω
b
T
I
b
ω
b
=
1
2
ω
c
T
(
R
b
c
T
I
b
R
b
c
)
ω
c
.
\frac{1}{2}\omega_c^T\mathcal I_c\omega_c=\frac{1}{2}\omega_b^T\mathcal I_b\omega_b\\ =\frac{1}{2}\omega_c^T(R_{bc}^T\mathcal I_bR_{bc})\omega_c.
21ωcTIcωc=21ωbTIbωb=21ωcT(RbcTIbRbc)ωc.
也就是说
I
c
=
R
b
c
T
I
b
R
b
c
.
(8.26)
\mathcal I_c=R_{bc}^TI_bR_{bc}. \tag{8.26}
Ic=RbcTIbRbc.(8.26)
如果{b}的坐标轴与惯量主轴不平行,我们可以在经过旋转变换
R
b
c
R_{bc}
Rbc的坐标系{c}中表示该惯量矩阵, 从而可以把惯量矩阵对角化。
R
b
c
R_{bc}
Rbc 的列就是
I
b
\mathcal I_b
Ib的向量。
有时惯量矩阵不在质心的坐标系表示更方便,例如在关节处的坐标系表示。那么可以通过斯坦纳定理(平行轴定理)转换。
定理:惯量矩阵
I
q
\mathcal I_q
Iq定义在坐标系{q}中,各轴与坐标系{b}平行,q为坐标系{b}中坐标为
q
=
(
q
x
,
q
y
,
q
z
)
q=(q_x,q_y,q_z)
q=(qx,qy,qz)的一点,定义在质心{b}的坐标系的惯量矩阵为
I
b
\mathcal I_b
Ib,那么
I
q
=
I
b
+
m
(
q
T
q
I
−
q
q
T
)
,
(8.27)
\mathcal I_q=\mathcal I_b+\text m(q^TqI-qq^T),\tag{8.27}
Iq=Ib+m(qTqI−qqT),(8.27)
这里
I
I
I是
3
×
3
3\times 3
3×3的单位矩阵,
m
\text m
m为刚体的质量。
力旋量
上面的式(8.22)和式(8.23)可以写为
[
m
b
f
b
]
=
[
I
b
0
0
m
I
]
[
ω
˙
b
v
˙
b
]
+
[
[
ω
b
]
0
0
[
ω
b
]
]
[
I
b
0
0
m
I
]
[
ω
b
v
b
]
(8.29)
\left[ \begin{matrix} m_b \\ f_b \end{matrix}\right] =\left[ \begin{matrix} \mathcal I_b & 0 \\ 0 &\text mI \end{matrix}\right] \left[ \begin{matrix} \dot \omega_b \\ \dot v_b \end{matrix}\right] +\left[ \begin{matrix} [\omega_b ]& 0\\ 0 & [\omega_b] \end{matrix}\right] \left[ \begin{matrix} \mathcal I_b & 0 \\ 0 &\text mI \end{matrix}\right] \left[ \begin{matrix} \omega_b \\ v_b \end{matrix}\right]\tag{8.29}
[mbfb]=[Ib00mI][ω˙bv˙b]+[[ωb]00[ωb]][Ib00mI][ωbvb](8.29)
这里
I
I
I是
3
×
3
3\times 3
3×3的单位阵。另外
[
v
]
v
=
v
×
v
=
0
,
[
v
]
T
=
−
[
v
]
[v]v=v\times v=0,[v]^T=-[v]
[v]v=v×v=0,[v]T=−[v],那么式(8.29)可以写为如下等效形式
[
m
b
f
b
]
=
[
I
b
0
0
m
I
]
[
ω
˙
b
v
˙
b
]
+
[
[
ω
b
]
[
v
b
]
0
[
ω
b
]
]
[
I
b
0
0
m
I
]
[
ω
b
v
b
]
=
[
I
b
0
0
m
I
]
[
ω
˙
b
v
˙
b
]
−
[
[
ω
b
]
0
[
v
b
]
[
ω
b
]
]
T
[
I
b
0
0
m
I
]
[
ω
b
v
b
]
(8.30)
\left[ \begin{matrix} m_b \\ f_b \end{matrix}\right] =\left[ \begin{matrix} \mathcal I_b & 0 \\ 0 &\text mI \end{matrix}\right] \left[ \begin{matrix} \dot \omega_b \\ \dot v_b \end{matrix}\right] +\left[ \begin{matrix} [\omega_b ]& [v_b]\\ 0 & [\omega_b] \end{matrix}\right] \left[ \begin{matrix} \mathcal I_b & 0 \\ 0 &\text mI \end{matrix}\right] \left[ \begin{matrix} \omega_b \\ v_b \end{matrix}\right]\\ =\left[ \begin{matrix} \mathcal I_b & 0 \\ 0 &\text mI \end{matrix}\right] \left[ \begin{matrix} \dot \omega_b \\ \dot v_b \end{matrix}\right] -\left[ \begin{matrix} [\omega_b ]& 0\\ [v_b] & [\omega_b] \end{matrix}\right]^T \left[ \begin{matrix} \mathcal I_b & 0 \\ 0 &\text mI \end{matrix}\right] \left[ \begin{matrix} \omega_b \\ v_b \end{matrix}\right]\tag{8.30}
[mbfb]=[Ib00mI][ω˙bv˙b]+[[ωb]0[vb][ωb]][Ib00mI][ωbvb]=[Ib00mI][ω˙bv˙b]−[[ωb][vb]0[ωb]]T[Ib00mI][ωbvb](8.30)
这样方程中的每一项可以用一个6维的空间量表示:
(1)向量
(
ω
b
,
v
b
)
,
(
m
b
,
f
b
)
(\omega_b,v_b),(m_b,f_b)
(ωb,vb),(mb,fb)分别用物体旋量
V
b
\mathcal V_b
Vb和力旋量
F
b
\mathcal F_b
Fb表示,
V
b
=
[
ω
b
v
b
]
,
F
b
=
[
m
b
f
b
]
(8.31)
\mathcal V_b=\left[\begin {matrix} \omega_b\\ v_b \end{matrix}\right], \mathcal F_b=\left[\begin {matrix} m_b\\ f_b \end{matrix}\right]\tag{8.31}
Vb=[ωbvb],Fb=[mbfb](8.31)
(2)空间惯量矩阵
G
b
∈
R
6
×
6
\mathcal G_b\in \mathbb R^{6\times 6}
Gb∈R6×6定义为
G
b
=
[
I
b
0
0
m
I
]
(8.32)
\mathcal G_b= \left[\begin{matrix} \mathcal I_b &0\\ 0 &\text mI \end{matrix}\right]\tag{8.32}
Gb=[Ib00mI](8.32)
这时候动能用空间惯量矩阵可以表示为
E
=
1
2
ω
b
T
I
b
ω
b
+
1
2
m
v
b
T
v
b
=
1
2
V
b
T
G
b
V
b
.
(8.33)
E=\frac{1}{2}\omega_b^T\mathcal I_b\omega_b+\frac{1}{2}\text mv_b^Tv_b=\frac{1}{2}\mathcal V_b^T\mathcal G_b\mathcal V_b.\tag{8.33}
E=21ωbTIbωb+21mvbTvb=21VbTGbVb.(8.33)
(3)空间动量
P
b
∈
R
6
×
6
\mathcal P_b\in\mathbb R^{6\times 6}
Pb∈R6×6定义为
P
b
=
[
I
b
m
v
b
]
=
[
I
b
0
0
m
I
]
[
ω
b
v
b
]
=
G
b
V
b
.
(8.34)
\mathcal P_b=\left[\begin{matrix} \mathcal I_b\\ \text mv_b \end{matrix}\right] =\left[\begin{matrix} \mathcal I_b&0\\ 0 & \text mI \end{matrix}\right] \left[\begin{matrix} \omega_b\\ v_b \end{matrix}\right] =\mathcal G_b\mathcal V_b.\tag{8.34}
Pb=[Ibmvb]=[Ib00mI][ωbvb]=GbVb.(8.34)
观察发现式(8.30)右边第二项是 P b \mathcal P_b Pb左乘下面的矩阵构成
− [ [ ω b ] 0 [ v b ] [ ω b ] ] T . (8.35) -\left[\begin{matrix} [\omega_b]&0\\ [v_b]&[\omega_b] \end{matrix}\right]^T.\tag{8.35} −[[ωb][vb]0[ωb]]T.(8.35)
注意,两个矢量的叉乘可以用反对称矩阵表示
[ ω 1 × ω 2 ] = [ ω 1 ] [ ω 2 ] − [ ω 2 ] [ ω 1 ] . (8.36) [\omega_1\times\omega_2]=[\omega_1][\omega_2]-[\omega_2][\omega_1].\tag{8.36} [ω1×ω2]=[ω1][ω2]−[ω2][ω1].(8.36)
式(8.35)中的矩阵可以看成是6维旋量叉乘运算的一般化。给定两个旋量 V 1 = ( ω 1 , v 1 ) , V 2 = ( ω 2 , v 2 ) \mathcal V_1=(\omega_1,v_1),\mathcal V_2=(\omega_2,v_2) V1=(ω1,v1),V2=(ω2,v2), 进行
[
V
1
]
[
V
2
]
−
[
V
2
]
[
V
1
]
=
[
[
ω
1
]
v
1
0
0
]
[
[
ω
2
]
v
2
0
0
]
−
[
[
ω
2
]
v
2
0
0
]
[
[
ω
1
]
v
1
0
0
]
=
[
[
ω
1
]
[
ω
2
]
−
[
ω
2
]
[
ω
1
]
[
ω
1
]
v
2
−
[
ω
2
]
v
1
0
0
]
=
[
[
ω
′
]
[
v
′
]
0
0
]
,
[\mathcal V_1][\mathcal V_2]-[\mathcal V_2][\mathcal V_1]= \left[\begin{matrix} [\omega_1]&v_1\\ 0& 0 \end{matrix}\right] \left[\begin{matrix} [\omega_2]&v_2\\ 0& 0 \end{matrix}\right]- \left[\begin{matrix} [\omega_2]&v_2\\ 0& 0 \end{matrix}\right] \left[\begin{matrix} [\omega_1]&v_1\\ 0& 0 \end{matrix}\right]\\ =\left[\begin{matrix} [\omega_1][\omega_2]-[\omega_2][\omega_1]&[\omega_1]v_2-[\omega_2]v_1\\ 0& 0 \end{matrix}\right]\\ =\left[\begin{matrix} [\omega ']&[v']\\ 0& 0 \end{matrix}\right],
[V1][V2]−[V2][V1]=[[ω1]0v10][[ω2]0v20]−[[ω2]0v20][[ω1]0v10]=[[ω1][ω2]−[ω2][ω1]0[ω1]v2−[ω2]v10]=[[ω′]0[v′]0],
还可以用向量写成更简洁的形式
[
ω
′
v
′
]
=
[
[
ω
1
]
0
[
v
1
]
[
ω
1
]
]
[
ω
2
v
2
]
\left[\begin{matrix} \omega'\\ v' \end{matrix}\right]= \left[\begin{matrix} [\omega_1]&0\\ [v_1]&[\omega_1] \end{matrix}\right] \left[\begin{matrix} \omega_2\\ v_2 \end{matrix}\right]
[ω′v′]=[[ω1][v1]0[ω1]][ω2v2]
一般地两个旋量的叉乘称为李括号.
定义:给定两个旋量
V
1
=
(
ω
1
,
v
1
)
,
V
2
=
(
ω
2
,
v
2
)
\mathcal V_1=(\omega_1,v_1),\mathcal V_2=(\omega_2,v_2)
V1=(ω1,v1),V2=(ω2,v2),
V
1
\mathcal V_1
V1和
V
2
\mathcal V_2
V2的李括号写成
[
ad
V
1
]
V
2
[\text{ad}_{\mathcal V_1} ]\mathcal V_2
[adV1]V2 或者
ad
V
1
(
V
2
)
\text{ad} _{\mathcal V_1}(\mathcal V_2)
adV1(V2), 定义为
[
[
ω
1
]
0
[
v
1
]
[
ω
1
]
]
[
ω
2
v
2
]
=
[
ad
V
1
]
V
1
=
ad
V
1
(
V
2
)
∈
R
6
,
(8.37)
\left[\begin{matrix} [\omega_1]&0\\ [v_1]&[\omega_1] \end{matrix}\right] \left[\begin{matrix} \omega_2\\ v_2 \end{matrix}\right]= [\text{ad}_{\mathcal V_1}]\mathcal V_1=\text{ad}_{\mathcal V_1}(\mathcal V_2)\in\mathbb R^6,\tag{8.37}
[[ω1][v1]0[ω1]][ω2v2]=[adV1]V1=adV1(V2)∈R6,(8.37)
其中
[
ad
V
]
=
[
[
ω
]
0
[
v
]
[
ω
]
]
∈
R
6
×
6
[\text{ad}_\mathcal V]=\left[\begin{matrix} [\omega]&0\\ [v]&[\omega] \end{matrix}\right]\in\mathbb R^{6\times 6}
[adV]=[[ω][v]0[ω]]∈R6×6
定义:给一个运动旋量
V
=
(
ω
,
v
)
\mathcal V =(\omega,v)
V=(ω,v)和力旋量
F
=
(
m
,
f
)
\mathcal F=(m,f)
F=(m,f), 定义映射
ad
V
T
(
F
)
=
[
ad
V
]
T
(
F
)
=
[
[
ω
]
0
[
v
]
[
ω
]
]
T
[
m
f
]
=
[
−
[
ω
]
m
−
[
v
]
f
−
[
ω
]
f
]
.
(8.39)
\text{ad}^T_{\mathcal V}(\mathcal F)=[\text{ad}_{\mathcal V}]^T(\mathcal F)=\left[\begin{matrix} [\omega]&0\\ [v]&[\omega] \end{matrix}\right]^T \left[\begin{matrix} m\\ f \end{matrix}\right]= \left[\begin{matrix} -[\omega]m-[v]f\\ -[\omega]f \end{matrix}\right].\tag{8.39}
adVT(F)=[adV]T(F)=[[ω][v]0[ω]]T[mf]=[−[ω]m−[v]f−[ω]f].(8.39)
使用上述符号和定义,单个刚体的动力学方程可以写成
F
b
=
G
b
V
˙
−
ad
V
b
T
(
P
b
)
=
G
b
V
˙
−
[
ad
V
b
]
T
G
b
V
b
(8.40)
\mathcal F_b=\mathcal G_b\dot{\mathcal V}-\text{ad}^T_{\mathcal V_b}(\mathcal P_b)\\ =\mathcal G_b\dot{\mathcal V}-[\text{ad}_{\mathcal V_b}]^T\mathcal G_b\mathcal V_b \tag{8.40}
Fb=GbV˙−adVbT(Pb)=GbV˙−[adVb]TGbVb(8.40)
在其他坐标系的动力学
式(8.40)依赖于使用的质心坐标系{b}。在另一个坐标系{a}中的动力学方程,可以通过动能不依赖于坐标系的性质推导出来
1
2
=
V
a
T
G
a
V
a
=
1
2
V
b
T
G
b
V
b
=
1
2
(
[
A
d
T
b
a
]
V
a
)
T
G
b
[
A
d
T
b
a
]
V
a
\frac{1}{2}=\mathcal {V_a^TG_aV_a}=\frac{1}{2}\mathcal {V_b^TG_bV_b}\\ =\frac{1}{2}([Ad_{T_{ba}}]\mathcal V_a)^T\mathcal {G_b}[Ad_{T_{ba}}]\mathcal V_a
21=VaTGaVa=21VbTGbVb=21([AdTba]Va)TGb[AdTba]Va
其中Ad是伴随表示. 也就是说{a}坐标系的空间惯量矩阵
G
a
\mathcal G_a
Ga与
G
b
\mathcal G_b
Gb的关系可以用下式表示
G
a
=
[
A
d
T
b
a
]
T
G
b
[
A
d
T
b
a
]
.
(8.42)
\mathcal G_a=[Ad_{T_{ba}}]^T\mathcal G_b[Ad_{T_{ba}}].\tag{8.42}
Ga=[AdTba]TGb[AdTba].(8.42)
使用空间惯量矩阵
G
a
\mathcal G_a
Ga,在{b}坐标系的对应的运动方程(8.40)在{a}坐标系可等效地写成
F
a
=
G
a
V
˙
a
−
[
ad
V
a
]
T
G
a
V
a
,
(8.43)
\mathcal F_a=\mathcal {G_a\dot V_a}-[\text{ad}_{\mathcal V_a }]^T\mathcal{G_aV_a} ,\tag{8.43}
Fa=GaV˙a−[adVa]TGaVa,(8.43)
其中
F
a
,
V
a
\mathcal{F_a,V_a}
Fa,Va是在坐标系{a}中描述的力旋量和运动旋量.
参考文献
[1] Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017