刚体转动的姿态动力学与姿态运动学

以下是常用公式及其简单描述，这些内容在航天器控制、导航等方面是通用的。从教材上摘录整理下来的，仅供大家参考，如果要学习详细内容建议翻教材。如有错误，欢迎指出。

1. 姿态动力学

• 描述角速度变化率与所受到的力矩之间的关系，由这些式子无法求出姿态角，只能推出角速度变化
• 所有变量代表本体系$b$相对惯性系$i$在惯性系$b$下的表达[3] page.23，如角速度$w\equiv w_{ib}^b$

角动量定义为$\mathbf{h}=J\omega$，角动量定理为
$$\frac{\mathrm{d}\mathbf{h}}{\mathrm{d}t}=\frac{\partial \mathbf{h}}{\partial t}+\omega \times \mathbf{h}=\dot{J}\omega +J\dot{\omega }+\omega \times (J\omega )=\mathbf{M}$$
如果不考虑对象转动惯量的变化，然后利用叉乘的法则也可以写作
$$J\dot{\omega }-\mathbf{h}\times \omega =\mathbf{M}$$
力矩表示的是刚体到的力矩在本体系下的表达，即$M_b^b$。将角速度代入$\omega =[{\omega }_x,{\omega }_y,{\omega }_z{]}^{\mathrm{T}}$
$$\{\begin{array}{c}{\dot{h}}_x+h_z{w}_y-h_y{w}_z=M_x\\ {\dot{h}}_y+h_x{w}_z-h_z{w}_x=M_y\\ {\dot{h}}_z+h_y{w}_x-h_x{w}_y=M_z\end{array}$$
当三轴为惯量主轴时，转动惯量为对角阵$J=diag[J_x,J_y,J_z]$，则
$$\{\begin{array}{c}{J}_x{\dot{w}}_x+(J_z-J_y)w_z{w}_y=M_x\\ J_y{\dot{w}}_y+(J_x-J_z)w_x{w}_z=M_y\\ J_z{\dot{w}}_z+(J_y-J_x)w_x{w}_y=M_z\end{array}$$

2. 姿态运动学

• 体现姿态与角速度之间的关系
• 按照四元数形式描述，四元数描述的是本体系$b$相对于惯性系$i$的姿态，也就是从惯性系$i$旋转到本体系$b$所需要的姿态$q_{ib}$
• 由于描述姿态的是四元数，所以微分方程左边是${\dot{q}}_{ib}$

若四元数定义为[4]
$$\begin{array}{l}\mathbf{q}=q_0+\mathbf{i}{q}_1+\mathbf{j}{q}_2+\mathbf{k}{q}_3=[q_1,q_{\mathbf{r}}]\\ q_0^2+q_1^2+q_2^2+q_3^2=1\end{array}$$
姿态运动学方程为
$$\dot{q}=\frac{1}{2}\Omega \left({\omega }_{ib}^b\right)\cdot q$$
其中4阶反对称矩阵
$$\Omega \left(\omega \right)=\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]$$
它代表的是体系相对于惯性系的角速度在体系下的表达，也就是陀螺仪能敏感到的数据。姿态运动学也可以写作四元数的4阶反对称矩阵乘以角速度的形式，即：
$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes {\mathbf{\omega }}_{\mathrm{r}}$$

其中${\mathbf{\omega }}_{\mathrm{r}}=[0,w_r^{\mathrm{T}}]$展开为
$$\left[\begin{array}{l}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$$

3. 姿态旋转矩阵

3.1姿态四元数转到姿态旋转矩阵

公式如下
$$\begin{array}{rl}{\mathbf{R}}_i^b(q_{ib})& \\ =& (q_0^2-q_{\mathbf{r}}\cdot q_{\mathbf{r}})I_2+2q_r{q}_r^T-2q_0[q_r^{\times }]\\ =& \left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& -2q_0{q}_3+2q_1{q}_2& 2q_0{q}_2+2q_1{q}_3\\ 2q_0{q}_3+2q_1{q}_2& q_0^2-q_1^2+q_2^2-q_3^2& -2q_0{q}_1+2q_2{q}_3\\ -2q_0{q}_2+2q_1{q}_3& 2q_0{q}_1+2q_2{q}_3& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\\ =& \left[\begin{array}{lll}2\left(q_0^2+q_1^2\right)-1& 2\left(q_1{q}_2+q_0{q}_3\right)& 2\left(q_1{q}_3-q_0{q}_2\right)\\ 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_0^2+q_2^2\right)-1& 2\left(q_2{q}_3+q_0{q}_1\right)\\ 2\left(q_1{q}_3+q_0{q}_2\right)& 2\left(q_2{q}_3-q_0{q}_1\right)& 2\left(q_0^2+q_3^2\right)-1\end{array}\right]\end{array}$$

其中3阶反对称矩阵
$$[r\times ]=\left[\begin{array}{ccc}0& -r_3& r_2\\ r_3& 0& -r_1\\ -r_2& r_1& 0\end{array}\right]$$

3.2 姿态四元数转到三轴姿态角

姿态四元数转到三轴姿态角，可以先转换到姿态旋转矩阵，再转到姿态角。参考[1]第91页并作出订正，航天器3-1-2旋转的过程，从惯性系转到本体系的姿态旋转矩阵应该这样构建:

1. $O{x}_i{y}_i{z}_i\to 绕O{z}_i("3")旋转\psi 角\to O{x}_1{y}_1{z}_1$
2. $O{x}_1{y}_1{z}_1\to 绕O{x}_1("1")旋转\theta 角\to O{x}_2{y}_2{z}_2$
3. $O{x}_2{y}_2{z}_2\to 绕O{y}_2("2")旋转\gamma 角\to O{x}_b{y}_b{z}_b$

而姿态角xyz方向俯仰$\theta$-滚转$\gamma$-偏航$\psi$，即x方向为俯仰角，y方向为滚转角，z方向为偏航角，这个和文献[2]的本体系相对应。

按照的312(zxy)旋转的姿态旋转矩阵为
$$\begin{array}{l}{\mathbf{R}}_i^b=R_2^b{R}_1^2{R}_i^1=R_y(\gamma )R_x(\theta )R_z(\psi )\\ =\left[\begin{array}{ccc}\cos \gamma & 0& -\sin \gamma \\ 0& 1& 0\\ \sin \gamma & 0& \cos \gamma \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & \sin \theta \\ 0& -\sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \sin \psi & 0\\ 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{ccc}\cos \psi \cos \gamma +\sin \psi \sin \theta \sin \gamma & -\sin \psi \cos \gamma +\cos \psi \sin \theta \sin \gamma & -\cos \theta \sin \gamma \\ \sin \psi \cos \theta & \cos \psi \cos \theta & \sin \theta \\ \cos \psi \sin \gamma -\sin \psi \sin \theta \cos \gamma & -\sin \psi \sin \gamma -\cos \psi \sin \theta \cos \gamma & \cos \theta \cos \gamma \end{array}\right]\end{array}$$

如果已知四元数，就可以得到姿态转移矩阵，相当于已知方程左边，由这个对应关系可以反解三轴姿态角
$$\begin{array}{l}\theta ={\sin }^{-1}\left(R_{23}\right)\\ \gamma ={\tan }^{-1}\left(-\frac{R_{13}}{R_{33}}\right)\\ \psi ={\tan }^{-1}\left(\frac{R_{21}}{R_{22}}\right)\end{array}$$

3.3 3-1-2旋转的姿态运动学

推导过程需要考虑姿态旋转矩阵的先后转动顺序，不是简单相加，如$w=\dot{\theta }\mathbf{i}+\dot{\gamma }\mathbf{j}+\dot{\psi }\mathbf{k}$，这样是错的。正确过程是
$$\begin{gathered} w_{ib}\equiv \left[\begin{array}{c}{w}_x\\ w_y\\ w_z\end{array}\right]=\dot{\gamma }\mathbf{j}+R_y(\gamma )\dot{\theta }\mathbf{i}+R_y(\gamma )R_x(\theta )\dot{\psi }\mathbf{k} \\ \begin{array}{l}{\omega }_x=\dot{\theta }\cos \gamma -\dot{\psi }\cos \theta \sin \gamma \\ {\omega }_y=\dot{\gamma }+\dot{\psi }\sin \theta \\ {\omega }_z=\dot{\theta }\sin \gamma +\dot{\psi }\cos \gamma \cos \theta \end{array} \end{gathered}$$

4. 姿态旋转矩阵的姿态运动学方程

姿态旋转矩阵$R_i^b$代表由惯性系转换到本体系。如果用姿态旋转矩阵来描述姿态，在导航算法中也是常用的，这样虽然内存占用大，但是足够精确也方便使用。这样，矩阵形式的姿态运动学方程为
$$\frac{\mathrm{d}{R}_i^b}{\mathrm{d}t}=R_i^b[w_{ib}^b\times ]\text{\hspace{1em}(1)}$$
注意到角速度矢量仍然遵循坐标系变换法则，即
$$w_{ib}^p=R_b^p{w}_{ib}^b$$
其中p为任意坐标系。向量的叉乘矩阵遵循类似的法则[3] page.25
$$[w_{ib}^p\times ]=R_b^p[w_{ib}^b\times ]R_p^b\text{e.g},[w_{ib}^i\times ]=R_b^i[w_{ib}^b\times ]R_i^b$$
文献[3]page 44(2-97)式指出，另一种矩阵形式的姿态运动学方程为
$$\frac{\mathrm{d}{R}_i^b}{\mathrm{d}t}=-[w_{ib}^b\times ]R_i^b\text{\hspace{1em}(2)}$$
考虑
$$[w_{ib}^i\times ]=R_b^i[w_{ib}^b\times ]R_i^b\Rightarrow [w_{ib}^b\times ]=R_i^b[w_{ib}^i\times ]R_b^i$$
代入上式并化简可得
$$\frac{\mathrm{d}{R}_i^b}{\mathrm{d}t}=-[w_{ib}^b\times ]R_i^b=-R_i^b[w_{ib}^i\times ]R_b^i\cdot R_i^b=R_i^b[w_{bi}^i\times ]\text{\hspace{1em}(3)}$$

于是一共有3中形式的表达，其中第一种是最常用的。

5. ref

[1]. 周军, 刘莹莹. 航天器姿态与轨道控制原理[M]. 西安: 西北工业大学出版社, 2016.

[2]. 李新国, 方群. 有翼导弹飞行动力学[M]. 西北工业大学出版社, 2005.

[3]. Noureldin A, Karamat T B, Georgy J. Fundamentals of inertial navigation, satellite-based positioning and their integration[M]. 2013: 1-313.
[4]. 方群，李新国，朱战霞等[M]. 西北工业大学出版社，2015. 章节：6.5.4