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

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

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

例如路程的计算公式: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:可以看到刚体的运动学模型通常用在迭代更新上,是不是突然想到卡尔曼里通常也会有模型更新的概念。)