摘要 首先推导角速度公式和角加速度公式,并举一个例子说明角速度公式的细节,然后从加速度公式出发推导转动惯量矩阵 J 和姿态动力学方程 Jw’=w×Jw,并解释为什么需要进行一次坐标变换。最后总结从力矩到姿态四元数的完整过程,并对无力矩输入时的进动现象进行仿真。
旋转坐标系下的速度和加速度
本体系绕世界系以角速度
ω
⃗
\vec\omega
ω 旋转,设世界系下位置向量
r
⃗
\vec r
r 的一阶导和二阶导分别为
w
r
⃗
˙
^w\dot{\vec r}
wr˙ 和
w
r
⃗
¨
^w\ddot{\vec r}
wr¨,本体系下位置向量的一阶导和二阶导分别为
b
r
⃗
˙
^b\dot{\vec r}
br˙ 和
b
r
⃗
¨
^b\ddot{\vec r}
br¨,满足
w
r
⃗
=
R
b
r
⃗
w
r
⃗
˙
=
R
b
r
⃗
˙
+
ω
⃗
×
w
r
⃗
w
r
⃗
¨
=
R
b
r
⃗
¨
+
2
ω
⃗
×
R
b
r
⃗
˙
+
ω
⃗
×
(
ω
⃗
×
w
r
⃗
)
+
ω
⃗
˙
×
w
r
⃗
(1)
\begin{aligned} ^w\vec r =& R\ {^b\vec r} \\ ^w\dot{\vec r} =& R\ {^b\dot{\vec r}}+ \vec\omega\times{^w\vec r} \\ ^w\ddot{\vec r} =& R\ {^b\ddot{\vec r}} +2\vec\omega\times R\ {^b\dot{\vec r}} +\vec\omega\times(\vec\omega\times{^w\vec r}) +\dot{\vec\omega}\times{^w\vec r} \\ \end{aligned}\tag{1}
wr=wr˙=wr¨=R brR br˙+ω×wrR br¨+2ω×R br˙+ω×(ω×wr)+ω˙×wr(1)
式中所有的
R
R
R 都是从本体系到世界系的旋转矩阵,完整应写作
b
w
R
{^w_bR}
bwR;所有的
ω
\omega
ω 都在世界系下表示,完整应写作
w
ω
{^w\omega}
wω:
w
r
⃗
=
b
w
R
b
r
⃗
w
r
⃗
˙
=
b
w
R
b
r
⃗
˙
+
w
ω
⃗
×
w
r
⃗
w
r
⃗
¨
=
b
w
R
b
r
⃗
¨
+
2
w
ω
⃗
×
b
w
R
b
r
⃗
˙
+
w
ω
⃗
×
(
w
ω
⃗
×
w
r
⃗
)
+
w
ω
⃗
˙
×
w
r
⃗
\begin{aligned} ^w\vec r =& {^w_bR}\ {^b\vec r} \\ ^w\dot{\vec r} =& {^w_bR}\ {^b\dot{\vec r}}+ {^w\vec\omega}\times{^w\vec r} \\ ^w\ddot{\vec r} =& {^w_bR}\ {^b\ddot{\vec r}} +2{^w\vec\omega}\times {^w_bR}\ {^b\dot{\vec r}} +{^w\vec\omega}\times({^w\vec\omega}\times{^w\vec r}) +{^w\dot{\vec\omega}}\times{^w\vec r} \\ \end{aligned}
wr=wr˙=wr¨=bwR brbwR br˙+wω×wrbwR br¨+2wω×bwR br˙+wω×(wω×wr)+wω˙×wr
证明:
d
d
t
(
w
r
⃗
)
=
d
d
t
(
R
b
r
⃗
)
w
r
⃗
˙
=
R
˙
b
r
⃗
+
R
b
r
⃗
˙
=
R
b
r
⃗
˙
+
w
ω
⃗
×
R
b
r
⃗
=
R
b
r
⃗
˙
+
w
ω
⃗
×
w
r
⃗
\begin{aligned} \frac{\text d}{\text dt}(^w\vec r) =& \frac{\text d}{\text dt}({R}\ {^b\vec r}) \\ ^w\dot{\vec r} =& \dot R\ {^b\vec r}+R\ {^b\dot{\vec r}} \\ =& R\ {^b\dot{\vec r}}+{^w\vec\omega}\times R\ {^b\vec r} \\ =& R\ {^b\dot{\vec r}}+{^w\vec\omega}\times{^w\vec r} \\ \end{aligned}
dtd(wr)=wr˙===dtd(R br)R˙ br+R br˙R br˙+wω×R brR br˙+wω×wr
此处以及后面的
w
ω
⃗
^w\vec\omega
wω 都是在世界系下的坐标,下面简记作
ω
⃗
\vec\omega
ω。
此处补充一些个人理解。
R
˙
=
ω
⃗
×
R
\dot R=\vec\omega\times R
R˙=ω×R 是因为
R
R
R 可以简单地看作三个向量绕旋转轴
ω
⃗
\vec\omega
ω 转动,不涉及坐标变换;而
w
r
⃗
˙
≠
ω
⃗
×
w
r
⃗
^w\dot{\vec r}\neq\vec{\omega} \times{^w\vec r}
wr˙=ω×wr 是因为此时向量
r
⃗
\vec r
r 不是绕旋转轴
ω
⃗
\vec\omega
ω 转动,而是个复合运动,
ω
⃗
\vec\omega
ω 是本体系(旋转坐标系)相对于世界系的旋转角速度,
r
⃗
\vec r
r 在本体系下还有
r
⃗
2
\vec r_2
r2 的角速度,当
r
⃗
\vec r
r 在本体系下的角速度为0时两种情况等价,可以理解成有个本体系固定在
r
⃗
\vec r
r 上。
对式(1)的第二行再次求导
d
d
t
(
w
r
⃗
˙
)
=
d
d
t
(
R
b
r
⃗
˙
+
ω
⃗
×
w
r
⃗
)
w
r
⃗
¨
=
R
˙
b
r
⃗
˙
+
R
b
r
⃗
¨
+
ω
⃗
˙
×
w
r
⃗
+
ω
⃗
×
w
r
⃗
˙
=
R
b
r
⃗
¨
+
ω
⃗
×
R
b
r
⃗
˙
+
ω
⃗
×
(
R
b
r
⃗
˙
+
ω
⃗
×
w
r
⃗
)
+
ω
⃗
˙
×
w
r
⃗
=
R
b
r
⃗
¨
+
2
ω
⃗
×
R
b
r
⃗
˙
+
ω
⃗
×
(
ω
⃗
×
w
r
⃗
)
+
ω
⃗
˙
×
w
r
⃗
\begin{aligned} \frac{\text d}{\text dt}(^w\dot{\vec r}) =& \frac{\text d}{\text dt}(R\ {^b\dot{\vec r}}+\vec\omega\times{^w\vec r}) \\ ^w\ddot{\vec r} =& \dot R\ {^b\dot{\vec r}} +R\ {^b\ddot{\vec r}} +\dot{\vec\omega}\times{^w\vec r} +\vec\omega\times{^w\dot{\vec r}} \\ =& R\ {^b\ddot{\vec r}} +\vec\omega\times R\ {^b\dot{\vec r}} +\vec\omega\times(R\ {^b\dot{\vec r}}+\vec\omega\times{^w\vec r}) +\dot{\vec\omega}\times{^w\vec r} \\ =& R\ {^b\ddot{\vec r}} +2\vec\omega\times R\ {^b\dot{\vec r}} +\vec\omega\times(\vec\omega\times{^w\vec r}) +\dot{\vec\omega}\times{^w\vec r} \\ \end{aligned}
dtd(wr˙)=wr¨===dtd(R br˙+ω×wr)R˙ br˙+R br¨+ω˙×wr+ω×wr˙R br¨+ω×R br˙+ω×(R br˙+ω×wr)+ω˙×wrR br¨+2ω×R br˙+ω×(ω×wr)+ω˙×wr
当匀速转动时
ω
⃗
˙
=
0
\dot{\vec\omega}=0
ω˙=0,最后一项可以省略。
举例
如图所示,世界系用
O
x
w
y
w
z
w
Ox_wy_wz_w
Oxwywzw 表示,本体系用
O
x
b
y
b
z
b
Ox_by_bz_b
Oxbybzb 表示。一开始时,本体系与世界系重合,向量
r
⃗
\vec r
r 与y轴正向重合,然后本体系沿世界系x轴逆时针旋转,角速度为
w
ω
⃗
1
^w\vec\omega_1
wω1;向量
r
⃗
\vec r
r 沿本体系z轴顺时针旋转,角速度为
b
ω
⃗
2
^b\vec\omega_2
bω2
w
ω
⃗
1
=
[
1
0
0
]
,
b
ω
⃗
2
=
[
0
0
−
1
]
^w\vec\omega_1=\begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix},\ ^b\vec\omega_2=\begin{bmatrix} 0 \\ 0 \\ -1 \end{bmatrix}
wω1=
100
, bω2=
00−1
注意两个角速度向量分别在世界系和本体系下表示。向量
r
⃗
\vec r
r 在本体系下的坐标为
b
r
⃗
=
[
sin
t
cos
t
0
]
^b\vec r=\begin{bmatrix} \sin t \\ \cos t \\ 0 \end{bmatrix}
br=
sintcost0
本体系到世界系的旋转矩阵
b
w
R
^w_bR
bwR 为
b
w
R
=
[
1
0
0
0
cos
t
−
sin
t
0
sin
t
cos
t
]
,
w
b
R
=
[
1
0
0
0
cos
t
sin
t
0
−
sin
t
cos
t
]
{^w_bR}=\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & -\sin t \\ 0 & \sin t & \cos t \end{bmatrix},\ ^b_wR=\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & \sin t \\ 0 & -\sin t & \cos t \end{bmatrix}
bwR=
1000costsint0−sintcost
, wbR=
1000cost−sint0sintcost
向量
r
⃗
\vec r
r 在世界系下的坐标为
w
r
⃗
=
b
w
R
b
r
⃗
=
[
1
0
0
0
cos
t
−
sin
t
0
sin
t
cos
t
]
[
sin
t
cos
t
0
]
=
[
sin
t
cos
2
t
sin
t
cos
t
]
{^w\vec r}={^w_bR}\ {^b\vec r}= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & -\sin t \\ 0 & \sin t & \cos t \end{bmatrix} \begin{bmatrix} \sin t \\ \cos t \\ 0 \end{bmatrix} =\begin{bmatrix} \sin t \\ \cos^2 t \\ \sin t\cos t \end{bmatrix}
wr=bwR br=
1000costsint0−sintcost
sintcost0
=
sintcos2tsintcost
两边同时求导得到角速度公式
w
v
⃗
=
w
r
⃗
˙
=
b
w
R
b
r
⃗
˙
+
w
ω
⃗
1
×
w
r
⃗
[
cos
t
−
sin
2
t
cos
2
t
]
=
[
1
0
0
0
cos
t
−
sin
t
0
sin
t
cos
t
]
[
cos
t
−
sin
t
0
]
+
[
1
0
0
]
×
[
sin
t
cos
2
t
sin
t
cos
t
]
=
[
cos
t
−
sin
t
cos
t
−
sin
2
t
]
+
[
0
−
sin
t
cos
t
cos
2
t
]
\begin{aligned} ^w\vec v = {^w\dot{\vec r}} =& {^w_bR}\ {^b\dot{\vec r}} + {^w\vec\omega_1} \times {^w\vec r} \\ \begin{bmatrix} \cos t \\ -\sin 2t \\ \cos 2t \end{bmatrix} =& \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & -\sin t \\ 0 & \sin t & \cos t \end{bmatrix} \begin{bmatrix} \cos t \\ -\sin t \\ 0 \end{bmatrix} +\begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix} \times\begin{bmatrix} \sin t \\ \cos^2 t \\ \sin t\cos t \end{bmatrix} \\ =& \begin{bmatrix} \cos t \\ -\sin t\cos t \\ -\sin^2t \end{bmatrix} +\begin{bmatrix} 0 \\ -\sin t\cos t \\ \cos^2 t \end{bmatrix} \\ \end{aligned}
wv=wr˙=
cost−sin2tcos2t
==bwR br˙+wω1×wr
1000costsint0−sintcost
cost−sint0
+
100
×
sintcos2tsintcost
cost−sintcost−sin2t
+
0−sintcostcos2t
于是可以得到,世界系下的速度向量转换到本体系下并不等于本体系下位置向量的导数。
b
v
⃗
=
w
b
R
w
v
⃗
=
[
1
0
0
0
cos
t
sin
t
0
−
sin
t
cos
t
]
[
cos
t
−
sin
2
t
cos
2
t
]
=
[
cos
t
−
sin
t
cos
t
]
≠
b
r
⃗
˙
=
[
cos
t
−
sin
t
0
]
\begin{aligned} & ^b\vec v = {^b_wR}\ {^w\vec v} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & \sin t \\ 0 & -\sin t & \cos t \end{bmatrix} \begin{bmatrix} \cos t \\ -\sin 2t \\ \cos 2t \end{bmatrix} =\begin{bmatrix} \cos t \\ -\sin t \\ \cos t \end{bmatrix} \\ \neq& {^b\dot{\vec r}} = \begin{bmatrix} \cos t \\ -\sin t \\ 0 \end{bmatrix} \end{aligned}
=bv=wbR wv=
1000cost−sint0sintcost
cost−sin2tcos2t
=
cost−sintcost
br˙=
cost−sint0
姿态动力学方程
M
⃗
=
J
ω
⃗
˙
+
ω
⃗
×
J
ω
⃗
\vec M=J\dot{\vec\omega}+\vec\omega\times J\vec\omega
M=Jω˙+ω×Jω
参考资料[3]的第2章是我觉得所有资料里讲这个方程讲的最清楚的,网上看什么资料都不如看这本英文书,这篇博客就当是我的笔记了,强烈建议有条件还是看原文。我把原文大致翻译了一下:
刚体姿态动力学详细推导与翻译
角动量定理
d
H
⃗
d
t
=
M
⃗
\frac{\text d\vec H}{\text dt}=\vec M
dtdH=M
其中
H
,
M
H,M
H,M 分别为刚体的角动量和作用在刚体上的力矩。
推导1
下面的推导来自文末参考资料[1],也是主流推导方法,主要思路是因为转动惯量矩阵在本体系保持不变,利用向量在本体系与世界系中的关系推导。但[1]不够详细,更详细的推导见参考资料[3]。
刚体角动量是刚体内所有质量微元相对基准点
S
S
S 的角动量之和
h
⃗
=
m
r
⃗
×
v
⃗
,
H
=
∭
V
r
⃗
×
r
⃗
˙
d
m
\vec h=m\vec r\times\vec v,\quad H=\iiint\limits_V\vec r\times\dot{\vec r}\text dm
h=mr×v,H=V∭r×r˙dm
其中
d
m
\text dm
dm 为刚体的质量微元。
r
⃗
×
v
⃗
=
r
⃗
×
(
ω
⃗
×
r
⃗
)
=
r
2
ω
⃗
−
(
r
⃗
⋅
ω
⃗
)
r
⃗
=
[
(
x
2
+
y
2
+
z
2
)
w
x
−
(
x
w
x
+
y
w
y
+
z
w
z
)
x
]
e
⃗
x
+
[
(
x
2
+
y
2
+
z
2
)
w
y
−
(
x
w
x
+
y
w
y
+
z
w
z
)
y
]
e
⃗
y
+
[
(
x
2
+
y
2
+
z
2
)
w
z
−
(
x
w
x
+
y
w
y
+
z
w
z
)
z
]
e
⃗
z
=
[
(
y
2
+
z
2
)
w
x
−
x
y
w
y
−
x
z
w
z
]
e
⃗
x
+
[
−
x
y
w
x
+
(
x
2
+
z
2
)
w
y
−
y
z
w
z
]
e
⃗
y
+
[
−
x
z
w
x
−
y
z
w
y
+
(
x
2
+
y
2
)
w
z
]
e
⃗
z
=
[
y
2
+
z
2
−
x
y
−
x
z
−
x
y
x
2
+
z
2
−
y
z
−
x
z
−
y
z
x
2
+
y
2
]
[
w
x
w
y
w
z
]
\begin{aligned} & \vec r\times\vec v = \vec r\times(\vec\omega\times\vec r) = r^2\vec\omega-(\vec r\cdot\vec\omega)\vec r \\ =& [(x^2+y^2+z^2)w_x-(xw_x+yw_y+zw_z)x]\vec e_x \\ +& [(x^2+y^2+z^2)w_y-(xw_x+yw_y+zw_z)y]\vec e_y \\ +& [(x^2+y^2+z^2)w_z-(xw_x+yw_y+zw_z)z]\vec e_z \\ =& [(y^2+z^2)w_x-xyw_y-xzw_z]\vec e_x \\ +& [-xyw_x+(x^2+z^2)w_y-yzw_z]\vec e_y \\ +& [-xzw_x-yzw_y+(x^2+y^2)w_z]\vec e_z \\ =&\begin{bmatrix} y^2+z^2 & -xy & -xz \\ -xy & x^2+z^2 & -yz \\ -xz & -yz & x^2+y^2 \\ \end{bmatrix} \begin{bmatrix} w_x \\ w_y \\ w_z \end{bmatrix} \\ \end{aligned}
=++=++=r×v=r×(ω×r)=r2ω−(r⋅ω)r[(x2+y2+z2)wx−(xwx+ywy+zwz)x]ex[(x2+y2+z2)wy−(xwx+ywy+zwz)y]ey[(x2+y2+z2)wz−(xwx+ywy+zwz)z]ez[(y2+z2)wx−xywy−xzwz]ex[−xywx+(x2+z2)wy−yzwz]ey[−xzwx−yzwy+(x2+y2)wz]ez
y2+z2−xy−xz−xyx2+z2−yz−xz−yzx2+y2
wxwywz
于是
H
=
∭
V
r
⃗
×
v
⃗
d
m
=
J
ω
⃗
H=\iiint\limits_V\vec r\times\vec v\text dm=J\vec\omega
H=V∭r×vdm=Jω
其中
J
J
J 为刚体的转动惯量矩阵
J
=
[
J
x
−
J
x
y
−
J
x
z
−
J
x
y
J
y
−
J
y
z
−
J
x
z
−
J
y
z
J
z
]
=
[
∭
(
y
2
+
z
2
)
d
m
−
∭
x
y
d
m
−
∭
x
z
d
m
−
∭
x
y
d
m
−
∭
(
x
2
+
z
2
)
d
m
−
∭
y
z
d
m
−
∭
x
z
d
m
∭
y
z
d
m
∭
(
x
2
+
y
2
)
d
m
]
\begin{aligned} J =& \begin{bmatrix} J_x & -J_{xy} & -J_{xz} \\ -J_{xy} & J_y & -J_{yz} \\ -J_{xz} & -J_{yz} & J_z \\ \end{bmatrix} \\ =& \begin{bmatrix} \iiint(y^2+z^2)\text dm & -\iiint xy\text dm & -\iiint xz\text dm \\ -\iiint xy\text dm & -\iiint(x^2+z^2)\text dm & -\iiint yz\text dm \\ -\iiint xz\text dm & \iiint yz\text dm & \iiint(x^2+y^2)\text dm \end{bmatrix} \end{aligned}
J==
Jx−Jxy−Jxz−JxyJy−Jyz−Jxz−JyzJz
∭(y2+z2)dm−∭xydm−∭xzdm−∭xydm−∭(x2+z2)dm∭yzdm−∭xzdm−∭yzdm∭(x2+y2)dm
可以看出
J
J
J 是刚体的属性,在与刚体固连的本体系下不随姿态改变。
这里需要注意区分世界系和与刚体固连的本体系,也就是为什么需要进行一次坐标变换。在世界系中,刚体上各点的坐标不是定值,所以刚体的转动惯量矩阵在世界系下不是定值,只有在本体系下才是定值,所以这里需要转换坐标系。
下面推导坐标系变换的最后一步,同时加上坐标系角标来清楚地标明每个变量属于哪个坐标系下的描述。
w
M
⃗
=
w
h
⃗
˙
w
M
⃗
=
R
b
h
⃗
˙
+
w
ω
⃗
×
w
h
⃗
R
−
1
w
M
⃗
=
b
h
⃗
˙
+
R
−
1
w
ω
⃗
×
R
−
1
w
h
⃗
b
M
⃗
=
b
h
⃗
˙
+
b
ω
⃗
×
b
h
⃗
b
M
⃗
=
J
b
w
⃗
˙
+
b
ω
⃗
×
J
b
ω
⃗
\begin{aligned} {^w\vec M} =& {^w\dot{\vec h}} \\ {^w\vec M} =& R\ {^b\dot{\vec h}}+{^w\vec\omega}\times{^w\vec h} \\ R^{-1}\ {^w\vec M} =& {^b\dot{\vec h}} +R^{-1}\ {^w\vec\omega}\times R^{-1}\ {^w\vec h} \\ {^b\vec M} =& {^b\dot{\vec h}}+{^b\vec\omega}\times{^b\vec h} \\ {^b\vec M} =& J{^b\dot{\vec w}}+{^b\vec\omega}\times J\ {^b\vec\omega} \\ \end{aligned}
wM=wM=R−1 wM=bM=bM=wh˙R bh˙+wω×whbh˙+R−1 wω×R−1 whbh˙+bω×bhJbw˙+bω×J bω
根据上面结果可以看出,姿态动力学方程
M
⃗
=
J
w
⃗
˙
+
w
⃗
×
J
w
⃗
\vec M=J\dot{\vec w}+\vec w\times J\vec w
M=Jw˙+w×Jw 中的所有量,包括角速度向量,都在本体系下描述。
推导2
下面的推导来自文末参考资料[2],主要思路是所有变量都在世界系中处理。但我觉得这种方法不对。
推导1中得到转动惯量矩阵
J
J
J 后,
J
J
J 在本体系下保持不变,但在世界系下是变量,可以记作
J
(
t
)
=
∑
i
∑
j
J
i
j
a
⃗
i
(
t
)
a
⃗
j
(
t
)
T
J(t)=\sum_i\sum_j J_{ij}\vec a_i(t)\vec a_j(t)^\text T
J(t)=i∑j∑Jijai(t)aj(t)T
其中
J
i
j
J_{ij}
Jij 是本体系下保持不变的转动惯量矩阵元素,
a
⃗
i
,
a
⃗
j
\vec a_i,\vec a_j
ai,aj 表示本体系的坐标轴,例如当两个系重合时
a
⃗
2
a
⃗
3
T
=
[
0
1
0
]
[
0
0
1
]
=
[
0
0
0
0
0
1
0
0
0
]
\vec a_2\vec a_3^\text T= \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix} \begin{bmatrix} 0 & 0 & 1 \end{bmatrix} =\begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{bmatrix}
a2a3T=
010
[001]=
000000010
但
a
⃗
i
,
a
⃗
j
\vec a_i,\vec a_j
ai,aj 在旋转时是个变量,所以
M
=
d
H
d
t
=
d
d
t
(
J
(
t
)
w
⃗
(
t
)
)
=
J
˙
(
t
)
w
⃗
(
t
)
+
J
(
t
)
w
⃗
˙
(
t
)
=
∑
i
∑
j
J
i
j
(
a
⃗
˙
i
a
⃗
j
T
+
a
⃗
i
a
⃗
˙
j
T
)
w
⃗
+
J
w
˙
=
∑
i
∑
j
J
i
j
[
(
w
⃗
×
a
⃗
i
)
a
⃗
j
T
w
⃗
+
a
⃗
i
(
w
⃗
×
a
⃗
j
)
T
w
⃗
]
+
J
w
˙
=
∑
i
∑
j
(
w
⃗
×
J
i
j
a
⃗
i
)
a
⃗
j
T
w
⃗
+
J
w
˙
=
w
⃗
×
(
∑
i
∑
j
J
i
j
a
⃗
i
a
⃗
j
T
)
w
⃗
+
J
w
˙
=
w
⃗
(
t
)
×
J
(
t
)
w
⃗
(
t
)
+
J
(
t
)
w
˙
(
t
)
\begin{aligned} M =& \frac{\text dH}{\text dt} = \frac{\text d}{\text dt}(J(t)\vec w(t)) \\ =& \dot J(t)\vec w(t) + J(t)\dot{\vec w}(t) \\ =& \sum_i\sum_j J_{ij}(\dot{\vec a}_i\vec a_j^\text T +\vec a_i\dot{\vec a}_j^\text T)\vec w+J\dot w \\ =& \sum_i\sum_j J_{ij}\big[(\vec w\times\vec a_i)\vec a_j^\text T\vec w +\vec a_i(\vec w\times\vec a_j)^\text T\vec w\big]+J\dot w \\ =& \sum_i\sum_j (\vec w\times J_{ij}\vec a_i)\vec a_j^\text T\vec w+J\dot w \\ =& \vec w\times\left(\sum_i\sum_j J_{ij}\vec a_i\vec a_j^\text T\right)\vec w +J\dot w \\ =& \vec w(t)\times J(t)\vec w(t)+J(t)\dot w(t) \end{aligned}
M=======dtdH=dtd(J(t)w(t))J˙(t)w(t)+J(t)w˙(t)i∑j∑Jij(a˙iajT+aia˙jT)w+Jw˙i∑j∑Jij[(w×ai)ajTw+ai(w×aj)Tw]+Jw˙i∑j∑(w×Jijai)ajTw+Jw˙w×(i∑j∑JijaiajT)w+Jw˙w(t)×J(t)w(t)+J(t)w˙(t)
其中
(
w
⃗
×
a
⃗
j
)
T
w
⃗
=
0
(\vec w\times\vec a_j)^\text T\vec w=0
(w×aj)Tw=0。
举例
这个例子描述了没有外部力矩输入时角速度却一直在变化的现象,这一现象在参考资料[3]的第13章中有详细描述。
设转动惯量矩阵
J
J
J、角速度初始值
w
⃗
0
\vec w_0
w0 分别为
J
=
[
2
0
0
0
2
0
0
0
1
]
,
w
⃗
0
=
[
1
0
1
]
J=\begin{bmatrix} 2 & 0 & 0 \\ 0 & 2 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix},\ \vec w_0=\begin{bmatrix} 1 \\ 0 \\ 1 \end{bmatrix}
J=
200020001
, w0=
101
当力矩为0时,由姿态动力学方程得
w
⃗
˙
=
−
J
−
1
(
w
⃗
×
J
w
⃗
)
=
−
[
0.5
0
0
0
0.5
0
0
0
1
]
(
[
w
x
w
y
w
z
]
×
[
2
w
x
2
w
y
w
z
]
)
=
−
[
0.5
0
0
0
0.5
0
0
0
1
]
[
−
w
y
w
z
w
x
w
z
0
]
[
w
˙
x
w
˙
y
w
˙
z
]
=
[
0.5
w
y
−
0.5
w
x
0
]
\begin{aligned} \dot{\vec w} =& -J^{-1}(\vec w\times J\vec w) \\ =& -\begin{bmatrix} 0.5 & 0 & 0 \\ 0 & 0.5 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} (\begin{bmatrix} w_x \\ w_y \\ w_z \end{bmatrix} \times\begin{bmatrix} 2w_x \\ 2w_y \\ w_z \end{bmatrix}) \\ =& -\begin{bmatrix} 0.5 & 0 & 0 \\ 0 & 0.5 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} -w_yw_z \\ w_xw_z \\ 0 \end{bmatrix} \\ \begin{bmatrix} \dot w_x \\ \dot w_y \\ \dot w_z \end{bmatrix} =& \begin{bmatrix} 0.5w_y \\ -0.5w_x \\ 0 \end{bmatrix} \end{aligned}
w˙===
w˙xw˙yw˙z
=−J−1(w×Jw)−
0.50000.50001
(
wxwywz
×
2wx2wywz
)−
0.50000.50001
−wywzwxwz0
0.5wy−0.5wx0
解得
w
x
(
t
)
=
cos
0.5
t
w
y
(
t
)
=
−
sin
0.5
t
w
z
(
t
)
=
1
\begin{aligned} w_x(t) =& \cos 0.5t \\ w_y(t) =& -\sin 0.5t \\ w_z(t) =& 1 \\ \end{aligned}
wx(t)=wy(t)=wz(t)=cos0.5t−sin0.5t1
附录:向量叉乘的乘法分配律
补充一个在推导过程中遇见的一个公式。当
A
A
A 是正交矩阵时,
A
(
b
⃗
×
c
⃗
)
=
(
A
b
⃗
)
×
(
A
c
⃗
)
A(\vec b\times\vec c)=(A\vec b)\times(A\vec c)
A(b×c)=(Ab)×(Ac)
A
A
A 不是正交矩阵时暂时没发现什么有意思的性质。前面还有一个公式
A
(
A
−
1
b
⃗
×
b
⃗
)
A(A^{-1}\vec b\times\vec b)
A(A−1b×b)
也没找到能合并
A
A
A 和
A
−
1
A^{-1}
A−1 的方法。
证明:
(
A
b
⃗
)
×
(
A
c
⃗
)
=
[
(
b
x
a
⃗
1
)
+
(
b
y
a
⃗
2
)
+
(
b
z
a
⃗
3
)
]
×
[
(
c
x
a
⃗
1
)
+
(
c
y
a
⃗
2
)
+
(
c
z
a
⃗
3
)
]
=
(
b
x
a
⃗
1
)
×
(
c
y
a
⃗
2
)
+
(
b
x
a
⃗
1
)
×
(
c
z
a
⃗
3
)
+
(
b
y
a
⃗
2
)
×
(
c
x
a
⃗
1
)
+
(
b
y
a
⃗
2
)
×
(
c
z
a
⃗
3
)
+
(
b
z
a
⃗
3
)
×
(
c
x
a
⃗
1
)
+
(
b
z
a
⃗
3
)
×
(
c
y
a
⃗
2
)
=
(
b
x
c
y
−
b
y
c
x
)
a
⃗
3
+
(
b
z
c
x
−
b
x
c
z
)
a
⃗
2
+
(
b
y
c
z
−
b
z
c
y
)
a
⃗
1
=
A
(
b
⃗
×
c
⃗
)
\begin{aligned} (A\vec b)\times(A\vec c) =& [(b_x\vec a_1)+(b_y\vec a_2)+(b_z\vec a_3)]\times [(c_x\vec a_1)+(c_y\vec a_2)+(c_z\vec a_3)] \\ =& (b_x\vec a_1)\times(c_y\vec a_2) +(b_x\vec a_1)\times(c_z\vec a_3) +(b_y\vec a_2)\times(c_x\vec a_1) \\ &+(b_y\vec a_2)\times(c_z\vec a_3) +(b_z\vec a_3)\times(c_x\vec a_1) +(b_z\vec a_3)\times(c_y\vec a_2) \\ =& (b_xc_y-b_yc_x)\vec a_3 +(b_zc_x-b_xc_z)\vec a_2 +(b_yc_z-b_zc_y)\vec a_1 \\ =& A(\vec b\times\vec c) \end{aligned}
(Ab)×(Ac)====[(bxa1)+(bya2)+(bza3)]×[(cxa1)+(cya2)+(cza3)](bxa1)×(cya2)+(bxa1)×(cza3)+(bya2)×(cxa1)+(bya2)×(cza3)+(bza3)×(cxa1)+(bza3)×(cya2)(bxcy−bycx)a3+(bzcx−bxcz)a2+(bycz−bzcy)a1A(b×c)
参考
- 章仁为.卫星轨道姿态动力学与控制[M].北京航空航天大学出版社.1998.
- 刚体动力学中的欧拉方程是如何推导出来的?-知乎
- de Ruiter. Spacecraft dynamics and control: an introduction. 2013.
进动现象仿真
仿真一下前文提到的角速度向量在本体系下绕圈的现象,这一现象使用了本体极锥和空间极锥的概念(不知道是不是这么翻译),在参考资料[3]中有详细介绍。
仿真结果见视频 https://www.bilibili.com/video/BV1Ck4y1F7Ba/。视频中白色线条代表刚体,黄色和橙黄色分别代表本体系的z轴和xy轴,红色线条表示角速度向量,绿色线条表示角动量向量。视频前半段摄像头在世界系下,后半段跟随刚体转动。
基于 simucpp 和 raylib 的部分代码如下,完整代码见 https://gitee.com/xd15zhn/atttitudemotion。
/**************************************************************
// 验证刚体自由转动时的进动现象
simucpp版本:2.1.14
**************************************************************/
int main(void) {
Mat J(3, 3, vecdble{2, 0, 0, 0, 2, 0, 0, 0, 1});
Mat Jinv = J.inv();
/* 初始化仿真器 */
Simulator sim1;
auto intQ = new MStateSpace(&sim1, BusSize(4, 1), true, "intQ");
auto intW = new MStateSpace(&sim1, BusSize(3, 1), true, "intW");
auto misoQ = new MFcnMISO(&sim1, BusSize(4, 1), "misoQ");
auto misoW = new MFcnMISO(&sim1, BusSize(3, 1), "misoW");
auto muxtau = new Mux(&sim1, BusSize(3, 1), "muxtau");
UInput **intau = new UInput*[3];
for (uint32_t i = 0; i < 3; i++) {
intau[i] = new UInput(&sim1, "inw"+to_string(i));
sim1.connectU(intau[i], muxtau, BusSize(i, 0));
}
sim1.connectM(intQ, misoQ);
sim1.connectM(intW, misoQ);
sim1.connectM(misoQ, intQ);
sim1.connectM(intW, misoW);
sim1.connectM(muxtau, misoW);
sim1.connectM(misoW, intW);
sim1.Set_SimStep(0.01);
intQ->Set_InitialValue(Mat(vecdble{1, 0, 0, 0}));
intW->Set_InitialValue(Mat(vecdble{1, 0, 1}));
intau[0]->Set_Function([](double t){ return 0; });
intau[1]->Set_Function([](double t){ return 0; });
intau[2]->Set_Function([](double t){ return 0; });
misoQ->Set_Function([](Mat *u){
// u[0]为姿态四元数(4x1),u[1]为本体系下的角速度向量(3x1)
Mat W(4, 4, vecdble{ // 角速度向量w1对应的四元数乘法左矩阵(4x4)
0, -u[1].at(0, 0), -u[1].at(1, 0), -u[1].at(2, 0),
u[1].at(0, 0), 0, u[1].at(2, 0), -u[1].at(1, 0),
u[1].at(1, 0), -u[1].at(2, 0), 0, u[1].at(0, 0),
u[1].at(2, 0), u[1].at(1, 0), -u[1].at(0, 0), 0,
});
return 0.5*W*u[0];
});
misoW->Set_Function([&J, &Jinv](Mat *u){
Vector3d omega = u[0]; // 本体系下的角速度向量(3x1)
Vector3d tau = Mat(3, 1, vecdble{ // 本体系下的力矩向量(3x1)
u[1].at(0, 0), u[1].at(1, 0), u[1].at(2, 0)
});
Vector3d ans = Jinv*(tau - (omega & (J*omega)));
return Mat(ans);
});
sim1.Initialize();
sim1.Simulate_FirstStep();
Mat curq, curw, curR, omegaI, hmom;
/*初始化场景*/
Camera camera;
SetWindowMonitor(1);
SetConfigFlags(FLAG_MSAA_4X_HINT);
SetConfigFlags(FLAG_FULLSCREEN_MODE);
SetTargetFPS(50);
InitGraph(0, 0, "Universe");
Init_Camera(&camera);
/*场景循环*/
while (!WindowShouldClose()) {
curq = intQ->Get_OutValue(); // 姿态四元数
curR = Quaternion_to_Rotation(curq); // 旋转矩阵
curw = intW->Get_OutValue(); // 本体系下的角速度向量
omegaI = curR * curw; // 世界系下的角速度向量
hmom = curR * (J*curw); // 世界系下的角动量
if (Shift_View())
SetReferenceFrame(Mat33_to_Matrix(curR));
else
SetReferenceFrame(MatrixIdentity());
Update_Camera(&camera);
BeginDrawing();
ClearBackground(BLACK);
BeginMode3D(camera);
DrawGrid(120, 2);
Draw_Body(curR); // 画刚体和固连坐标系
DrawLine3D(Vector3Zero(), Mat_to_Vector3(omegaI), RED); // 画世界系下的角速度向量
DrawLine3D(Vector3Zero(), Mat_to_Vector3(hmom), GREEN); // 画世界系下的角动量向量
EndMode3D();
EndDrawing();
if (IsKeyDown(KEY_Q)) {
sim1.Simulation_Reset();
}
if (Pause()) continue;
for (int i = 0; i < 5; i++)
sim1.Simulate_OneStep();
}
CloseGraph();
return 0;
}