[飞控]刚体运动学-旋转矩阵模型

上一篇文章我们介绍了欧拉角速度与机体角速度的关系，但是姿态除了欧拉角形式，还有我们最喜欢的旋转矩阵形式，让我们来分析一下基于旋转矩阵模型:

$$\begin{array} { l } { \dot { \mathbf { p } } = ^ { \mathrm { e } } \mathbf { v } } \\ { \dot { \mathbf { R } } = \mathbf { R } \left[ ^ { b } \mathbf { \boldsymbol\omega } \right] _ { \times } } \end{array}$$

为什么旋转矩阵的模型是这种形式？

假设地理系(e)中有一个固定的向量 r, 且机体系(b) 相对于 e 系转动的角速度为 web ,则固定矢量 r 在两坐标系下投影的转换关系（即坐标变换），为：

$$\boldsymbol { r } ^ { e } = \boldsymbol { R } _ { b } ^ { e } \boldsymbol { r } ^ { b }$$

两边同时微分，得:

$$\dot { \boldsymbol { r } } ^ { e } = \boldsymbol { R } _ { b } ^ { e } \dot { \boldsymbol { r } } ^ { b } + \dot { \boldsymbol { R } } _ { b } ^ { e } \boldsymbol { r } ^ { b }$$

（Q1:这里是对谁微分？）

因为 r 是 e 系中的固定矢量，所以：

$$\dot { \boldsymbol r } ^ { e } = \mathbf { 0 }$$

由于 b 系相对于 e 系的角速度为:

$$\boldsymbol{\omega}_{eb}$$

则在b 系上观察r的角速度应为:

$$-\boldsymbol\omega_{eb},（-\boldsymbol\omega_{eb}=\boldsymbol\omega_{be}）$$

并且已知：

$$\dot { \mathbf { r } } ^ { b } = - \mathbf {\boldsymbol \omega } _ { e b } ^ { b } \times \mathbf { r } ^ { b }$$ $$0 = \mathbf R _ { b } ^ { e } \left( - \boldsymbol\omega _ { e b } ^ { b } \times \mathbf r ^ { b } \right) + \dot {\mathbf R } _ { b } ^ { e } \mathbf r ^ { b }$$

即：

$$\dot {\mathbf R } _ { b } ^ { e } \mathbf r ^ { b } =\mathbf R _ { b } ^ { e } \left( \boldsymbol\omega _ { e b } ^ { b } \times \right) \mathbf r ^ { b }$$

由于上式对于任意 e 系固定矢量 r 都成立，任选三个不共面的非零矢量 r1 、r2 和 r3 ，则有：

$$\dot {\mathbf R } _ { b } ^ { e } \left[ \begin{array} { l l l } {\mathbf r _ { 1 } ^ { b } } & {\mathbf r _ { 2 } ^ { b } } & {\mathbf r _ { 3 } ^ { b } } \end{array} \right] =\mathbf R _ { b } ^ { e } \left( \boldsymbol\omega _ { e b } ^ { b } \times \right) \left[ \begin{array} { l l l } { \mathbf r _ { 1 } ^ { b } } & {\mathbf r _ { 2 } ^ { b } } & { \mathbf r _ { 3 } ^ { b } } \end{array} \right]$$

显然矩阵[r1b r2b r3b]可逆，所以必定有:

$$\dot {\mathbf R } _ { b } ^ { e } =\mathbf R _ { b } ^ { e } \left( \boldsymbol\omega _ { e b } ^ { b } \times \right)$$

即：

$$\frac { \mathrm{d} \mathbf{R} _ { b } ^ { e } } { \mathrm{d} t } = \mathbf{R} _ { b } ^ { e } [ ^b \boldsymbol\omega ] _ { \times }$$

这就是旋转矩阵模型的微分方程，这个公式表示了机体角速度与姿态导数之间的关系。（Q2:从这个微分方程判断，旋转矩阵能否测量全姿态？）

其中：

$$[^b \boldsymbol\omega]_\times \triangleq \left[ \begin{array} { c c c } { 0 } & { -w_z } & { w_y } \\ { w_z } & { 0 } & { - w_a } \\ { -w_y } & { w_x } & { 0 } \end{array} \right]$$

简单给出旋转矩阵模型的递推计算公式：

[tm-1,tm]时间段的角增量为：

$$\boldsymbol { \varDelta \theta } _ { m } = \int _ { t _ { m - 1 } } ^ { t _ { m } } \boldsymbol { \omega } _ { e b } ^ { b } ( t ) \mathrm { d } t$$

记模值为：

$$\Delta \theta _ { m } = \left| \boldsymbol { \Delta } \boldsymbol { \theta } _ { m } \right|$$ $$\mathbf R _ { b ( m ) } ^ { e } = \mathbf R _ { b ( m - 1 ) } ^ { e } \mathbf R _ { b ( m ) } ^ { b ( m - 1 ) }$$ $$\mathbf R _ { b ( m ) } ^ { b ( m - 1 ) } = \mathbf I + \frac { \sin \Delta \theta _ { m } } { \Delta \theta _ { m } } \left( \boldsymbol{\Delta \theta} _ { m } \times \right) + \frac { 1 - \cos \Delta \theta _ { m } } { \Delta \theta _ { m } ^ { 2 } } \left( \boldsymbol{ \Delta \theta _ { m } }\times \right) ^ { 2 }$$

同样用这个微分方程可以通过迭代更新得到飞机当前的姿态，不过显然这个公式过于复杂所以不常用。

补充：角速度与线速度的关系

在三维空间中， 圆周运动所在的平面可以任意选取， 我们可以将角速度拓展成一个矢量 ω， 其方向垂直于该平面并由右手定则 确定． 令坐标系的原点在圆周运动的轴上， 用位矢 r 表示点 P 的位置， 则圆周运动的半径为 r⊥=rsin⁡θ， 其中 θ 是 r 与 ω 的夹角． 所以圆周运动速度的大小为v=ωrsin⁡θ． 根据矢量叉乘的几何定义

$$\mathbf { v } = \boldsymbol { \omega } \times \mathbf { r }$$

所以：

$${ \cfrac{ \mathrm{d} \mathbf v}{\mathrm{d} t} } = \boldsymbol { \omega } \times \mathbf { r }$$

Q1:两边都是t的函数，两边对t微分，四则运算求导公式。

Q2:旋转矩阵可以测量全姿态，因为这个微分方程恒成立，R可以为任何旋转矩阵，W也可以为任何数。

感兴趣的小伙伴可以留意一下不同姿态表达形式的微分方程，非常有意思。