[飞控]刚体运动学-欧拉角模型

刚体运动学和刚体动力学，我以前也搞不清楚，但是最近全权老师的实验课，给了我很棒的解释。

刚体运动学模型研究的是与力无关的过程，只研究位置、速度、姿态、角速度等参量，常以质点为模型。

例如路程的计算公式：x=vt

刚体动力学模型就是要研究物理的受力，物体受力后会有什么变化：

例如：F=ma

所以，之前的欧拉方程描述的是机体角速度和角加速度的关系，属于动力学模型。

运动学模型，对于多旋翼来说主要关心的就是机体角速度与姿态变化率的关系。

为什么呢？因为，我们传感器只能测量机体角速度，但是我们控制需要知道姿态变化率。

如果有了机体角速度与姿态变化率的关系，我们就可以通过传感器数据得到姿态。

我们之前反复提过，姿态有很多种表达，所以你在全权老师的书上也会看到不同形式的刚体运动学模型。今天就简单介绍一下基于欧拉角的运动学模型：

$$\begin{array} { l } ^ e\dot { \mathbf { p } } = ^e \mathbf { v } \\ \dot { \mathbf { \Theta } } = \mathbf { W } {^ b \omega } \end{array}$$

显然第一个公式非常位置的微分=速度

$$\begin{aligned} \left[ \begin{array} { c } {\dot p_x } \\ { \dot p_y } \\ { \dot p_z } \end{array} \right] = \left[ \begin{array} { c } { v_x } \\ { v_p } \\ { v_z } \end{array} \right] \\ \end{aligned}$$

第二个公式，让我们考虑飞机从A状态转动到C状态的过程：

这个过程如果用机体角速度描述：

$$\left[ \begin{array} { c } { \omega _ { x } } \\ { \omega _ { y } } \\ { \omega _ { z } } \end{array} \right]$$

但是这个过程，如果用姿态变化率描述是什么结果呢？

假设从地理坐标系到机体坐标系三次转动顺序为

$$\psi \rightarrow \theta \rightarrow \gamma$$

第一次旋转是yaw，绕z轴旋转，这个过程的欧拉角速度为：

$$\dot { \psi }$$

然后进行第二次旋转是pitch，绕y轴旋转，这个过程的欧拉角速度为：

$$\dot { \theta }$$

最后，第三次旋转是roll,绕x轴旋转,这个过程的欧拉角速度为：

$$\dot { \gamma }$$

仔细回想一下这个过程，你站在C的角度上看，yaw的欧拉角速度是经过了两次旋转后你才看见的，而pitch的欧拉角速度是经过一次旋转才被你看见的，只有roll 的欧拉角速度是被你直接观察到的。

所以，机体坐标系相对于地理坐标系的角速率向量可以表示为：

$$\left[ \begin{array} { c } { \omega _ { x } } \\ { \omega _ { y } } \\ { \omega _ { z } } \end{array} \right] = \left[ \begin{array} { c } { \dot { \gamma } } \\ { 0 } \\ { 0 } \end{array} \right] + \boldsymbol { C } _ { \gamma } \left[ \begin{array} { l } { 0 } \\ { \dot { \theta } } \\ { 0 } \end{array} \right] + \boldsymbol { C } _ { \gamma } \boldsymbol { C } _ { \theta } \left[ \begin{array} { l } { 0 } \\ { 0 } \\ { \dot { \psi } } \end{array} \right]$$

其中：

$$\boldsymbol { C } _ { \gamma } = \left[ \begin{array} { c c c } { 1 } & { 0 } & { 0 } \\ { 0 } & { \cos \gamma } & { \sin \gamma } \\ { 0 } & { - \sin \gamma } & { \cos \gamma } \end{array} \right] \\ \boldsymbol C _ { \theta } = \left[ \begin{array} { c c c } { \cos \theta } & { 0 } & { - \sin \theta } \\ { 0 } & { 1 } & { 0 } \\ { \sin \theta } & { 0 } & { \cos \theta } \end{array} \right] \\ \boldsymbol { C } _ { \gamma } = \left[ \begin{array} { c c c } { 1 } & { 0 } & { 0 } \\ { 0 } & { \cos \gamma } & { \sin \gamma } \\ { 0 } & { - \sin \gamma } & { \cos \gamma } \end{array} \right]$$

所以，姿态变化率与机体角速度：

$$\left[ \begin{array} { c } { w _ { x } } \\ { w _ { y } } \\ { w _ { z } } \end{array} \right] = \left[ \begin{array} { c c c } { 1 } & { 0 } & { - \sin \theta } \\ { 0 } & { \cos \phi } & { \sin \phi \cos \theta } \\ { 0 } & { - \sin \phi } & { \cos \phi \cos \theta } \end{array} \right] \left[ \begin{array} { c } { \dot { \phi } } \\ { \dot { \theta } } \\ { \dot { \psi } } \end{array} \right]$$

对矩阵求逆，可得：

$$\left[ \begin{array} { c } { \dot { \phi } } \\ { \dot { \theta } } \\ { \dot { \psi } } \end{array} \right] = \left[ \begin{array} { c c c } { 1 } & { \tan \theta \sin \phi } & { \tan \theta \cos \phi } \\ { 0 } & { \cos \phi } & { - \sin \phi } \\ { 0 } & { \sin \phi / \cos \theta } & { \cos \phi / \cos \theta } \end{array} \right]\left[ \begin{array} { c } { w _ { x } } \\ { w _ { y } } \\ { w _ { z } } \end{array} \right]$$

令：

$$W \triangleq \left[ \begin{array} { c c c } { 1 } & { \tan \theta \sin \phi } & { \tan \theta \cos \phi } \\ { 0 } & { \cos \phi } & { - \sin \phi } \\ { 0 } & { \sin \phi / \cos \theta } & { \cos \phi / \cos \theta } \end{array} \right]$$

姿态为：

$$\mathbf { \Theta } = \left[ \begin{array} { c } { { \phi } } \\ { { \theta } } \\ { { \psi } } \end{array} \right]$$

姿态变化率为：

$$\left[ \begin{array} { c } { \dot { \phi } } \\ { \dot { \theta } } \\ { \dot { \psi } } \end{array} \right]$$

即：

$$\dot { \mathbf { \Theta } } = \mathbf { W } {^ b \omega }$$

一阶差分方法进行近似离散化：

$$\left\{ \begin{array} { l } { \gamma _ { k } = \gamma _ { k - 1 } + \omega _ { x k } T _ { s } + \frac { \sin \gamma _ { k - 1 } \sin \theta _ { k - 1 } } { \cos \theta _ { k - 1 } } \omega _ { y k } T _ { s } + \frac { \cos \gamma _ { k - 1 } \sin \theta _ { k - 1 } } { \cos \theta _ { k - 1 } } \omega _ { z k } T _ { s } } \\ { \theta _ { k } = \theta _ { k - 1 } + \cos \gamma _ { y - 1 } \omega _ { y k } T _ { s } - \sin \gamma _ { k - 1 } \omega _ { z k } T _ { s } } \\ { \psi _ { k } = \psi _ { k - 1 } + \frac { \sin \gamma _ { k - 1 } } { \cos \theta _ { k - 1 } } \omega _ { y k } T _ { s } + \frac { \cos \gamma _ { k - 1 } } { \cos \theta _ { k - 1 } } \omega _ { z k } T _ { s } } \end{array} \right.$$

其中：

$$\omega _ { b k } = \left[ \begin{array} { l l l } { \omega _ { x k } } & { \omega _ { y k } } & { \omega _ { z k } } \end{array} \right] ^ { T }$$

为机载 MEMS 陀螺仪实时输出的机体坐标系下的三轴角速率，Ts 为采样周期。只需给定三个姿态角初值，就可通过式递推计算得到 k 时刻的姿态角。

(ps:可以看到刚体的运动学模型通常用在迭代更新上，是不是突然想到卡尔曼里通常也会有模型更新的概念。)