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

倪偲001

已于 2022-05-17 10:00:35 修改

阅读量6.1k

点赞数 14

分类专栏：飞行动力学与制导文章标签：线性代数矩阵机器学习

于 2020-08-27 17:02:59 首次发布

本文链接：https://blog.csdn.net/NICAI001/article/details/108264506

版权

飞行动力学与制导专栏收录该内容

6 篇文章 14 订阅

订阅专栏

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

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

1. 姿态动力学

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

角动量定义为 $\mathbf h=J\mathbf \omega$ ，角动量定理为
$\frac{\mathrm d\mathbf h}{\mathrm dt}=\frac{\mathrm\partial\mathbf h}{\mathrm\partial t}+\mathbf\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}$
$\left\{\begin{matrix} \dot h_x+h_zw_y-h_yw_z=M_x\\ \dot h_y+h_xw_z-h_zw_x=M_y\\ \dot h_z+h_yw_x-h_xw_y=M_z \end{matrix}\right.$
当三轴为惯量主轴时，转动惯量为对角阵 $J=diag[J_x,J_y,J_z]$ ，则
$\left\{\begin{matrix} J_x\dot w_x+ (J_z-J_y)w_zw_y=M_x\\ J_y\dot w_y+ (J_x-J_z)w_xw_z=M_y\\ J_z\dot w_z+ (J_y-J_x)w_xw_y=M_z \end{matrix}\right.$

2. 姿态运动学

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

若四元数定义为[4]
$\begin{array}{l} {\boldsymbol{q}}=q_{0}+\bold i q_{1}+\bold j q_{2}+\bold 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{\boldsymbol{q}}=\frac{1}{2} \boldsymbol{q} \otimes \boldsymbol{\omega}_{\mathrm{r}}$

其中 $\boldsymbol{\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{aligned} \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^× ]\\ =&\left[\begin{array}{ccc} q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & -2 q_{0} q_{3}+2 q_{1} q_{2} & 2 q_{0} q_{2}+2 q_{1} q_{3} \\ 2 q_{0} q_{3}+2 q_{1} q_{2} & q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2} & -2 q_{0} q_{1}+2 q_{2} q_{3} \\ -2 q_{0} q_{2}+2 q_{1} q_{3} & 2 q_{0} q_{1}+2 q_{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{aligned}$

其中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旋转的过程，从惯性系转到本体系的姿态旋转矩阵应该这样构建:

$Ox_iy_iz_i\rightarrow绕Oz_i("3")旋转\psi角\rightarrow Ox_1y_1z_1$
$Ox_1y_1z_1\rightarrow绕Ox_1("1")旋转\theta角\rightarrow Ox_2y_2z_2$
$Ox_2y_2z_2\rightarrow绕Oy_2("2")旋转\gamma角\rightarrow Ox_by_bz_b$

而姿态角xyz方向俯仰 $\theta$ -滚转 $\gamma$ -偏航 $\psi$ ，即x方向为俯仰角，y方向为滚转角，z方向为偏航角，这个和文献[2]的本体系相对应。
在这里插入图片描述
按照的312(zxy)旋转的姿态旋转矩阵为
$\begin{array}{l} \mathbf R_{i}^{b}=R_2^bR_1^2R_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$ ，这样是错的。正确过程是
$w_{ib}\equiv\begin{bmatrix}w_x\\w_y\\w_z\end{bmatrix}= \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}$

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

姿态旋转矩阵 $R_i^b$ 代表由惯性系转换到本体系。如果用姿态旋转矩阵来描述姿态，在导航算法中也是常用的，这样虽然内存占用大，但是足够精确也方便使用。这样，矩阵形式的姿态运动学方程为
$\frac{\mathrm d R_i^b}{\mathrm d t}=R_i^b[w_{ib}^b\times] \tag 1$
注意到角速度矢量仍然遵循坐标系变换法则，即
$w_{ib}^p=R_b^pw_{ib}^b$
其中p为任意坐标系。向量的叉乘矩阵遵循类似的法则[3] page.25
$[w_{ib}^p\times]=R_b^p[w_{ib}^b\times]R_p^b\quad \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\tag 2$
考虑
$[w_{ib}^i\times]=R_b^i[w_{ib}^b\times]R_i^b⇒[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]\tag 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