[飞控]刚体运动学-四元数模型

根据之前的文章
我们已经知道刚体的运动学模型，可以分成两个部分，
平移和旋转，平移过程用于描述速度与位置之间的关系，
旋转过程用于描述机体角速与姿态变化率之间的关系。
平移过程比较简单都是位置的微分等于速度。
但是旋转过程比较复杂，因为旋转有不同的描述方法，所以旋转部分的微分方程可以有不同的形式:欧拉角微分方程，旋转矩阵微分方程
，四元数微分方程。

所以研究刚体运动学的重点，在于旋转的微分方程。

按照之前的文章，首先会推导微分方程，但是四元数的推导比较复杂，以后单独讲。以《多旋翼飞行器设计与控制》给出的四元数模型为例，基于四元数的微分方程:

$$\begin{aligned} \dot { q } _ { 0 } & = - \frac { 1 } { 2 } \mathbf { q } _ { \mathrm { v } } ^ { \mathrm { T } } \cdot ^ { \mathrm { b } } \boldsymbol { \omega } \\ \dot { \mathbf { q } } _ { \mathrm { v } } & = \frac { 1 } { 2 } \left( q _ { 0 } \mathbf { I } _ { 3 } + \left[ \mathbf { q } _ { \mathrm { v } } \right] _ { \times } \right) ^ { b } \boldsymbol { \omega } \end{aligned}$$

可以简化写成：

$$\\ \dot {\boldsymbol Q } _ { b } ^ { e } = \frac { 1 } { 2 } \boldsymbol { Q } _ { b } ^ { e } \circ \boldsymbol { \omega } _ { e b } ^ { b }$$

怎么四元数的微分方程长这样？跟之前提到的过的不一样呀。

让我们尝试一下，能不能把它转换成更加直观的一些。

首先这个公式，右边是个四元数乘法，四元数乘法是怎么样的呢？
如果有两个四元数P=[p0,p1,p2,p3],Q=[q0,q1,q2,q3]。

我们可以把四元数转换成矩阵形式再进行矩阵乘法这样就比较简单了(注意这里的顺序)。

$$\boldsymbol P \circ \boldsymbol Q = \left[ \begin{array} { c c c c } { 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 } { p _ { 0 } } \\ { p _ { 1 } } \\ { p _ { 2 } } \\ { p _ { 3 } } \end{array} \right] = \boldsymbol { M } _ { Q } ^ { \prime } \boldsymbol { P }$$

但是我们的机体角速度是三维向量怎么办呢？没关系转化成0标量四元数就行(在向量前面添加一个0即可)，之前的矩阵就变成了：

$$\left(\boldsymbol p _ { v } * \right) = \left[ \begin{array} { c c c c } { 0 } & { - p _ { 1 } } & { - p _ { 2 } } & { - p _ { 3 } } \\ { p _ { 1 } } & { 0 } & { p _ { 3 } } & { - p _ { 2 } } \\ { p _ { 2 } } & { - p _ { 3 } } & { 0 } & { p _ { 1 } } \\ { p _ { 3 } } & { p _ { 2 } } & { - p _ { 1 } } & { 0 } \end{array} \right] = \left[ \begin{array} { c c } { 0 } & { -\boldsymbol p _ { v } ^ { \mathrm { T } } } \\ {\boldsymbol p _ { v } } & { - \left(\boldsymbol p _ { v } \times \right) } \end{array} \right]$$

把矩阵里的元素整理一下就可以得到等式右边。

有了这些计算基础，重新回到我们的微分方程，把展开 W_{eb}^{b} 写成0标量四元数【0，w_v^T】的矩阵形式：

$$\boldsymbol { \omega_v } = ^ { \mathrm { b } } \boldsymbol { \omega } =\left[\begin{array} { c } {w_x}\\{w_y}\\{w_z} \end{array} \right]=\left[\begin{array} { c } {0}\\{w_x}\\{w_y}\\{w_z} \end{array} \right]$$

这个角速度向量对应的四元数矩阵可以写成：

$$\dot {\boldsymbol Q } _ { b } ^ { e } =\left[ \begin{array} { c c } { 0 } & { -\boldsymbol \omega _ { v } ^ { \mathrm { T } } } \\ {\boldsymbol \omega _ { v } } & { - \left[\boldsymbol \omega _ { v } \times \right] } \end{array} \right] \frac { 1 } { 2 } \boldsymbol { Q } _ { b } ^ { e }$$

继续把 Q_e^b 展开成向量形式【q0 , q_v^T】

即：

$$\left[\begin{array} { c } {q_0}\\{\boldsymbol q_v} \end{array} \right] =\frac { 1 } { 2 }\left[ \begin{array} { c c } { 0 } & { -\boldsymbol \omega _ { v } ^ { \mathrm { T } } } \\ {\boldsymbol \omega _ { v } } & { - \left[\boldsymbol \omega _ { v } \times \right] } \end{array} \right] \left[\begin{array} { c } {q_0}\\{\boldsymbol q_v} \end{array} \right]$$

因为：

$$\left[\boldsymbol \omega _ { v } \times \right]\boldsymbol { q } _ { v } = \left[\boldsymbol q _ { v } \times \right]{\boldsymbol \omega } _ { v }$$

展开：

$$\begin{aligned} \dot { q } _ { 0 } & = - \frac { 1 } { 2 } \mathbf { q } _ { \mathrm { v } } ^ { \mathrm { T } } \cdot \boldsymbol { \omega_v } \\ \dot { \mathbf { q } } _ { \mathrm { v } } & = \frac { 1 } { 2 } \left( q _ { 0 } \mathbf { I } _ { 3 } + \left[ \mathbf { q } _ { \mathrm { v } } \right] _ { \times } \right) \boldsymbol { \omega _v } \end{aligned}$$

全部展开即：

$$\left[ \begin{array} { c } { \dot { q } _ { 0 } } \\ { \dot { q } _ { 1 } } \\ { \dot { q } _ { 2 } } \\ { \dot { q } _ { 3 } } \end{array} \right] = \frac { 1 } { 2 } \left[ \begin{array} { c c c c } { 0 } & { - w _ { x } } & { - w _ { y } } & { - w _ { z } } \\ { w _ { x } } & { 0 } & { w _ { z } } & { - w _ { y } } \\ { w _ { y } } & { - w _ { z } } & { 0 } & { w _ { x } } \\ { w _ { z } } & { w _ { y } } & { - w _ { x } } & { 0 } \end{array} \right] \left[ \begin{array} { c } { q _ { 0 } } \\ { q _ { 1 } } \\ { q _ { 2 } } \\ { q _ { 3 } } \end{array} \right]$$

你看其实就是我们很早之前就说过的四元数微分方程，这些都是四元数微分方程的不同形式而已。看论文的时候，为了排版更简洁，通常会使用最简洁的形式，但本质上不论运动学模型怎么变，都逃不出这几个微分方程，所以大家看论文的时候不用害怕。

注意：这里的四元数上下标采用的是国内常用的标准，参考文章《 旋转标准的统一》