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

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

$$\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] \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也可以为任何数。

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