基于Linux微计算机BBB的飞控开发之IMU姿态解算

1. 介绍

最近在做Linux平台的飞控开发，过程中还是遇到了不少的问题，当然也学到很多东西。在我博客的四旋翼无人机开发专栏里我会分享我制作的每一个过程，希望能够帮到同样在制作飞控的小伙伴。硬件平台是Beaglebone Blue（BBB）。在看本章之前或之后，读者也可以看一看我写的另外一篇文章《基于Linux微计算机BBB的飞控开发之IMU数据获取与预处理》，看如何获取mpu9250的数据。

2. 坐标变换

2.1 坐标系

在研究无人机姿态时我们需要考虑两个坐标系，一个是地理坐标系（E系），一个是物体坐标系（b系），E系是固定不变的，而b系是随着无人机的姿态变化的。

2.1.1 E系

首先我们先定义E系，x指向北方，y指向西方，z指向天空，如图1。当然也有其它定义方式，这是为了与物体坐标系的定义相统一，减少不必要的坐标转换。
图1

2.1.2 b系

接着我们定义b系，如图2。这里x与y轴的位置与我们熟知的并不一样，看起来有点难受。我也尝试过交换x轴与y轴，但是最终还是以失败告终，至于为什么我也不太清楚，实在吃不消了，再不学课程就要挂科了`(>﹏<)′。
图2

2.1.3 IMU坐标系

最后我们再看一下mpu9250的坐标系，如图3。可以看到加速度计与陀螺仪的坐标系我们定义的b系一致，而对磁力计只需将x数据与y数据互换，z数据乘-1，就可以将mpu9250的坐标系转换到b系。
图3

2.2 坐标转换

我们知道E系是静止的，而b系是相对于E系进行旋转的。那么E系中的一个向量如何使用b系的坐标基来表示呢，比如重力加速度向量 $\vec G$ ，在E系中为（0,0，-1），在b系中是什么呢？也就是E系如何变换b系的问题。下面从两个角度来给出解决方法。

2.2.1 四元数求解变换矩阵

四元数是什么呢？四元数就是对三维旋转问题的一个数学描述。定义为 $\vec Q$ = $q_0$ + $q_1\vec i$ + $q_2\vec j$ + $q_3\vec k$，这里 $\vec i$、 $\vec j$、 $\vec k$分别为x、y、z轴上的单位向量。不过我们还是先来看看它的三角式 $\vec Q$ = $cos\frac{\theta} {2}$ + $\vec u * sin$ $\frac{\theta} {2}$，这里 $\vec u$（ $\vec u$ = $l\vec i$ + $m\vec j$ + $n\vec k$）表示的旋转轴方向上的向量（一般是单位向量），表示绕 $\vec u$向量顺时针旋转 $\theta$角。把 $\vec u$带入 $\vec Q$，可以得到 $\vec Q$ = $cos\frac{\theta} {2}$ + $( l\vec i + m\vec j + n\vec k) * sin\frac{\theta} {2}$ = cos $\frac{\theta} {2}$ + $l\vec i * sin\frac{\theta} {2}$ + $m\vec j * sin\frac{\theta} {2}$ + $n\vec k * sin\frac{\theta} {2}$，令 $q_0$ = $cos\frac{\theta} {2}$， $q_1$ = $lsin\frac{\theta} {2}$， $q_2$ = $msin\frac{\theta} {2}$， $q_3$ = $nsin\frac{\theta} {2}$，也就是最上面那个定义式。
了解四元数了，那么它跟变换矩阵有什么关系呢？可以证明E系到b系的变换矩阵 $C_E^b$ = $\begin{pmatrix} 1 - 2(q_2^2 + q_3^2)&2(q_0 * q_3 + q_1 * q_2)&2(q_1 * q_3 - q_0 * q_2)&\\ 2(q_1 * q_2 - q_0 * q_3)&1 - 2(q_2^2 + q_3^2)&2(q_0 * q_1 + q_2 * q_3)&\\ 2(q_0 * q_2 + q_1 * q_3)&2(q_2 * q_3 - q_0 * q_1)&1 - 2(q_2^2 + q_1^2)&\end{pmatrix}$
注意这里的四元数是规范四元数，即 $q_0^2$ + $q_1^2$ + $q_2^2$ + $q_3^2$ = 1，一般可以使用 $\frac {\vec Q} {\sqrt {q_0^2 + q_1^2 + q_2^2 + q_3^2}}$来规范化四元数。
同样可以得到b系到E系的逆变换矩阵 $C_b^E$ = $(C_E^b)^T$ = $\begin{pmatrix} 1 - 2(q_2^2 + q_3^2)&2(q_1 * q_2 - q_0 * q_3)&2(q_0 * q_2 + q_1 * q_3)&\\ 2(q_0 * q_3 + q_1 * q_2)&1 - 2(q_2^2 + q_3^2)&2(q_2 * q_3 - q_0 * q_1)&\\ 2(q_1 * q_3 - q_0 * q_2)&2(q_0 * q_1 + q_2 * q_3)&1 - 2(q_2^2 + q_1^2)&\end{pmatrix}$
在回去看看我们在第二部分开头提出的问题，是不是就可以解决了，只要我们知道四元数我们就可以使用这个公式计算b系中的重力向量 $\vec Q`$ = $C_E^b \vec Q$

2.2.2 欧拉角求解变换矩阵

三维坐标系中的旋转问题也可以使用欧拉角来描述，旋转可以等效成先绕z轴旋转 $\psi$(yaw)角度，然后绕y轴旋转 $\gamma$(pitch)角度，最后绕x轴旋转 $\phi$(roll)角度。
绕z旋转的变换矩阵 $C_z$ = $\begin{pmatrix} cos\psi&sin\psi&0\\ -sin\psi&cos\psi&0\\ 0&0&1& \end{pmatrix}$
绕y旋转的变换矩阵 $C_y$ = $\begin{pmatrix} cos\gamma&0&-sin\gamma&\\ 0&1&0&\\ sin\gamma&0&cos\gamma& \end{pmatrix}$
绕x旋转的变换矩阵 $C_x$ = $\begin{pmatrix} 1&0&0&\\ 0&cos\phi&sin\phi&\\ 0&-sin\phi&cos\phi& \end{pmatrix}$
那么可以计算E系到b系的变换矩阵 $C_E^b$ = $C_xC_yC_z$ = $\begin{pmatrix} cos\gamma cos\psi&cos\gamma sin\psi&-sin\gamma&\\ -cos\phi sin\psi + sin\phi sin\gamma sin\psi&cos\phi cos\psi + sin\phi sin\gamma sin\psi&sin\phi cos\gamma&\\ sin\phi sin\psi + cos\phi sin\gamma cos\psi&-sin\phi sin\psi + cos\phi sin\gamma sin\psi&cos\phi cos\gamma& \end{pmatrix}$
而b系到E系的变换矩阵 $C_b^E$ = $(C_E^b)^T$ = $\begin{pmatrix} cos\gamma cos\psi&-cos\phi sin\psi + sin\phi sin\gamma sin\psi&sin\phi sin\psi + cos\phi sin\gamma cos\psi&\\ cos\gamma sin\psi&cos\phi cos\psi + sin\phi sin\gamma sin\psi&-sin\phi sin\psi + cos\phi sin\gamma sin\psi&\\ -sin\gamma&sin\phi cos\gamma&cos\phi cos\gamma& \end{pmatrix}$
到这里，开头提出的问题我们也可以使用这里的矩阵来计算，但是显然 $sin$与 $cos$的计算复杂度与简单乘法完全不是层次，所以在进行坐标转换时还是使用2.2.1提出的矩阵来计算比较简单。

3. Mahony算法解算姿态

前面我们说欧拉角和四元数都可以用来描述三维旋转，并且提出了两种方式对应的变换矩阵。其实我们只要能够求出欧拉角和四元数中的一个就可以完成姿态解算。实际上下面我们是直接求解四元数，最终会用四元数计算欧拉角。四元数便于解算姿态，而欧拉角看起来更明白，在控制上使用起来更简单。下面介绍的就是Mahony算法，当然还有其它算法，但是在大部分飞控软件都是使用的这种Mahony算法，可能是比较简单吧。Ardupoilot就是用的这个算法，另外在使用了Mahony算法之后他还用了卡尔曼滤波（EKF），但我已经无力再学了。实际上，我把Ardopoilt的EKF关掉之后，发现姿态解算的效果并没有显著的差别。

3.1 使用四元数计算欧拉角

我们到过来说吧，假如我们已经计算出了四元数，那怎么用四元数计算欧拉角呢？
第二部分我们已经给出了两种计算变换矩阵的方法。如 $C_b^E$ = $\begin{pmatrix} 1 - 2(q_2^2 + q_3^2)&2(q_1 * q_2 - q_0 * q_3)&2(q_0 * q_2 + q_1 * q_3)&\\ 2(q_0 * q_3 + q_1 * q_2)&1 - 2(q_2^2 + q_3^2)&2(q_2 * q_3 - q_0 * q_1)&\\ 2(q_1 * q_3 - q_0 * q_2)&2(q_0 * q_1 + q_2 * q_3)&1 - 2(q_2^2 + q_1^2)&\end{pmatrix}$ = $\begin{pmatrix} cos\gamma cos\psi&-cos\phi sin\psi + sin\phi sin\gamma sin\psi&sin\phi sin\psi + cos\phi sin\gamma cos\psi&\\ cos\gamma sin\psi&cos\phi cos\psi + sin\phi sin\gamma sin\psi&-sin\phi sin\psi + cos\phi sin\gamma sin\psi&\\ -sin\gamma&sin\phi cos\gamma&cos\phi cos\gamma& \end{pmatrix}$
上面的矩阵应该各项相等，即：
$-sin\gamma$ = $2(q_1 * q_3 - q_0 * q_2)$
$cos\gamma cos\psi$ = $1 - 2(q_2^2 + q_3^2)$
$cos\gamma sin\psi$ = $2(q_0 * q_3 + q_1 * q_2)$
$sin\phi cos\gamma$ = $2(q_0 * q_1 + q_2 * q_3)$
$cos\phi cos\gamma$ = $1 - 2(q_2^2 + q_1^2)$
就可以解得欧拉角 $\gamma(pitch)、\phi(roll)、\psi(yaw)$：
$\gamma$ = $-arcsin(2(q_1 * q_3 - q_0 * q_2))$
$\phi$ = $arctan(\frac {2(q_0 * q_1 + q_2 * q_3)} {1 - 2(q_2^2 + q_1^2)})$
$\psi$ = $arctan(\frac {2(q_0 * q_3 + q_1 * q_2)} {1 - 2(q_2^2 + q_3^2)})$

3.2 计算四元数

上面的计算都是在四元数已知的条件下，那么如何求解四元数呢？
这时候mpu9250就派上用场了。

3.2.1 四元数微分

我们对四元数 $\vec Q$ = $cos\frac{\theta} {2} + \vec u * sin\frac{\theta} {2}$求微分：
$\frac {d\vec Q} {dt} = -\frac {d\theta} {2dt} * sin\frac {\theta} {2} + \frac {d\vec u} {dt} * sin\frac {\theta} {2} + \vec u * \frac {d\theta} {2dt} * cos\frac {\theta} {2}$
因为 $\vec u* \vec u$ = -1，所以上式可以继续化为：
$\frac {d\vec Q} {dt} = \vec u * \vec u * \frac {d\theta} {2dt} * sin\frac {\theta} {2} + \frac {d\vec u} {dt} * sin\frac {\theta} {2} + \vec u * \frac {d\theta} {2dt} * cos\frac {\theta} {2}$
= $\vec u * \frac {d\theta} {2dt} * (cos\frac{\theta} {2} + \vec u * sin\frac{\theta} {2}) + \frac {d\vec u} {dt} * sin\frac {\theta} {2}$
= $\vec u * \frac {d\theta} {2dt} * \vec Q + \frac {d\vec u} {dt} * sin\frac {\theta} {2}$
我们知道飞机在某一段很短的时刻是绕着同一个轴在旋转的，也就是说在某一一段时间内 $\vec u$是一个常量，导数为0。转轴发生的变化是阶跃式的变化，可以忽略不计，所以 $\frac {d\vec u} {dt}$ = 0。
另外 $\vec u * \frac {d\theta} {dt}$的实际意义就是将绕 $\vec u$旋转的角速度分解为E系中的x、y、z轴的角速度，我们记 $\vec \omega_E$ = $\vec u * \frac {d\theta} {dt}$
则上式可以改写为：
$\frac {d\vec Q} {dt} = \frac {1} {2}\vec \omega^E * \vec Q$
可以证明 $\vec \omega^b = \vec Q^* * \vec \omega^E * \vec Q$，且 $\vec \omega^b = \omega_x^b * \vec i + \omega_y^b * \vec j + \omega_z^b * \vec z$
所以上式可以继续改写为：
$\frac {d\vec Q} {dt} = \frac {1} {2} \vec Q * \vec \omega^b = \frac {1} {2} \begin{pmatrix} 0&-\omega_x^b &-\omega_y^b &-\omega_z^b \\ \omega_x^b &0&\omega_z^b &-\omega_y^b \\ \omega_y^b &-\omega_z^b &0&\omega_x^b \\ \omega_z^b &\omega_y^b &-\omega_x^b &0\end{pmatrix} \begin{pmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{pmatrix}$
好了，到这里我们已经求出四元数微分了，下面我们积分一下就可以成功的求解出四元数了。

3.2.2 由四元数微分求解四元数

这里就很简单了，我们只需要把上面的微分方程两边积分一下就好了，得到下面的公式：
$\vec Q_{t + \Delta t} = \vec Q_t + \frac {1} {2} * \begin{pmatrix} 0&-\omega_x^b &-\omega_y^b &-\omega_z^b \\ \omega_x^b &0&\omega_z^b &-\omega_y^b \\ \omega_y^b &-\omega_z^b &0&\omega_x^b \\ \omega_z^b &\omega_y^b &-\omega_x^b &0\end{pmatrix} * \begin{pmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{pmatrix}_t$
即：
$\begin{pmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{pmatrix}_{t + \Delta t} = \begin{pmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{pmatrix}_t + \frac {1} {2} \begin{pmatrix} 0&-\omega_x^b &-\omega_y^b &-\omega_z^b \\ \omega_x^b &0&\omega_z^b &-\omega_y^b \\ \omega_y^b &-\omega_z^b &0&\omega_x^b \\ \omega_z^b &\omega_y^b &-\omega_x^b &0\end{pmatrix} \begin{pmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{pmatrix}_t * \Delta t$
这里的 $\omega_x^b、\omega_y^b、\omega_z^b$就是b系下各轴的角速度（弧度制）。
一开始我们假设四元数的初始值 $\vec Q_0 = \begin{pmatrix} 1\\ 0 \\ 0 \\ 0\end{pmatrix}$，然后用上面的公式一直迭代下去就可以获得每个时刻的四元数。当然求解完，一定要记得规范化一下。由于误差的存在可能会导致四元数不再是规范四元数。

ok，到这里我们已经完成了几乎所有的工作。回想一下，我们由mpu9250的陀螺仪的角速度计算出四元数之后，我们就可以计算出欧拉角，也可以用变换矩阵计算b系中的重力向量。

4. 陀螺仪的修正

我们虽然已经求出了四元数，但是其实我们在实际求解的过程中不是准确无误的。首先陀螺仪由于自身特性和噪声影响会产生一定的误差；其次积分的时候 $\Delta t$并不可能无穷小，实际上会取0.002s或者0.0025这些数值，在 $\Delta t$内角速度只能测量一次或至少有限次，所以这样就会产生一定的误差。这些误差短时间内不会有什么影响，但是由于积分效应，误差会越来越大。因而我们要去对这些误差进行一些修正。这里我们其实是使用PI控制器进行修正。如下图。陀螺仪属于正反馈，而磁力计与加速度计属于负反馈，我们对负反馈的误差进行比例放大和积分，修正陀螺仪的输出数据，进而修正四元数。下面提供了两种修正方式。
图4（该图来自于_Summer__的博客）

4.1 加速度计补偿

在我的另外一篇文章《基于Linux微计算机BBB的飞控开发之IMU数据获取与预处理》里面我们已经获取了加速度计的数据。我们假设加速度计数据向量 $\vec A_{raw} = \begin{pmatrix} a_x\\ a_y\\ a_z\end{pmatrix}$，我们把它规范化一下，即： $\vec A = \frac {\vec A_{raw}} {\sqrt {a_x^2 + a_y^2 + A_z^2}}$。在静止或匀速直线运动状态下， $\vec A$向量就是b系下的重力向量（这里的重力向量实际上是竖直指向天空的，而不是竖直指向地下的，这跟我们的理解不一样，读者要特别注意）。可以说加速度计算出的重力向量是比较精确的，我们把它作为一个实际值看待。
前面说到由变换矩阵可以将E系中的重力向量 $\vec G(0,0,1)$（为什么是1不是-1的原因在上一段提到了）变换b系中，即 $\vec G` = C_E^b \vec G$， $\vec G`$就是b系下的重力向量，只不过这是一个理论值，是由四元数求解出来的，是有一定误差的。
那么现在，我们用理论值与实际值作比较，求解出误差向量 $\vec E$，求解方法就是叉乘。即：
$\vec E = \vec G` * \vec A$
读者有没有疑问，为什么叉乘可以计算出误差？实际上，我们想一下， $\vec E$是不是就是垂直于 $\vec A 、\vec G`$，而 $\vec E$的模是不是就是 $\vec A 、\vec G`$两个向量围成的平行四边形面积。两个单位向量形成的面积一定程度上可以描述两个向量之间的差距，这个最终体现在 $\vec E$的模上。再想一想，既然 $\vec E$垂直于这两个向量，那么我是不是只要让 $\vec G`$绕着这个 $\vec E$旋转一个与 $\vec E$的模相关的角度就可以使得 $\vec G`$与 $\vec A$重合。那么绕着 $\vec E$旋转一个与 $\vec E$的模相关的角度如何实现呢？实际上 $\vec E$在b系各个坐标轴的投影长度就对应了各个轴角速度应该减去的修正值。绕单位向量 $\vec n$旋转 $\theta$可以使用这个公式分解为各个轴的角速度： $\omega_x^b * \vec i + \omega_y^b * \vec j + \omega_z^b * \vec k = \vec n * \theta$。
描述修正过程的公式如下：
$\vec G = \begin{pmatrix} 0\\ 0\\ 1\end{pmatrix}$
$\vec A_{raw} = \begin{pmatrix} a_x^b\\ a_y^b\\ a_z^b\end{pmatrix}$
$\vec \omega_{raw}^b = \begin{pmatrix} \omega_x^b\\ \omega_y^b\\ \omega_z^b\end{pmatrix}$
$\vec A = \frac {\vec A_{raw}} {\sqrt {a_x^2 + a_y^2 + A_z^2}}$
$\vec G` = C_E^b \vec G$
$\vec E = \vec G` * \vec A$
$\vec \omega_{correction}^b = \vec \omega_{raw}^b - kp * \vec E - ki * \sum_{i=0} ^{n} \vec E$
最终 $\vec \omega_{correction}^b$作为四元数积分的输入值。就可以达到修正的目的。当然， $kp 与ki$的取值对修正的效果是有一定影响的。注意只有在静止或匀速直线运动下才可以修正，加速和圆周运动下，加速度计中混入了其它加速度，加速度计数据向量并不是垂直指向地下的，也就是 $4000< |\vec A_{raw}| < 4150$，因为加速度计在静止状态测出的重力向量的模为4096，所以笔者设置了这样一个范围，读者可以根据需要调整范围。

4.2 磁力计补偿

加速度只能修补 $pitch 和roll$，至于为什么读者可以将IMU绕着E系的z轴旋转,看加速度计数据向量是否有明显变化，加速度计是无法反应yaw的。所以还需要磁力计来专门修正yaw。磁力计补偿的原理与加速度补偿类似，仅有一点点区别，下面直接以公式说明修正方法。
因为磁场是从北指向南，而我们定义的E系的x轴是指向北，所以磁力在E系xoy上的投影向量：
$\vec M = \begin{pmatrix} -1\\ 0\end{pmatrix}$
磁力计数据向量：
$\vec m^b = \begin{pmatrix} m_x^b\\ m_y^b\\ m_z^b\end{pmatrix}$
我们把 $\vec m^b$转换到E系：
$\vec m^E = C_b^E\begin{pmatrix} m_x^E\\ m_x^E\\ m_x^E\end{pmatrix}$
取投影值：
$\vec m_{xy}^E = \begin{pmatrix} m_x^E\\ m_x^E\end{pmatrix}$
与实际值 $\vec M$做叉乘:
$error = \vec m_{xy}^E * \vec M = sin<\vec m_{xy}^E,\vec M> = sin(yaw_{error})$
则：
$yaw_{error} = arcsin(\vec m_{xy}^E * \vec M)$
这里得到的是实际的yaw与四元数计算出来的yaw得偏差，要修正这个偏差，实际上就是要绕重力向量旋转 $yaw_{error}$这样一个角度，在加速度修正的基础上继续做出如下修正:
$\vec \omega_{correction}^b = \vec \omega_{correction}^b - kp * yaw_{error}$
这里 $kp$与加速度计的 $kp$并不一样，具体还需要实际的测试调整。
哇，终于写到最后了，最后我们只需要用这个修正的角速度去计算四元数就可以达到修正陀螺仪误差的作用。
上面的计算过程我经过实测。理论上我并不是很懂，最近才接触飞控的，所以有什么错的地方还望读者指正。