Mahony姿态解算算法详解

本文将介绍Mahony姿态解算算法，不涉及磁力计部分，程序源代码见：https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

姿态的表示方法

欧拉角

物体姿态最直观的表示方法就是欧拉角，一个坐标系相对于另一个坐标系的转动，可以通过围绕三个正交轴的转动来表示，这三个转动角被称为一组欧拉角。常用的欧拉角组是横滚角($roll$，$\varphi$)、俯仰角($pitch$，$\theta$)、偏航角($yaw$，$\psi$)，分别表示绕导航坐标系中$X$、$Y$、$Z$轴的旋转，导航坐标系满足右手坐标系，食指方向为$X$轴正方向，中指为$Y$轴正方向，大拇指为$Z$轴正方向，这三个旋转角对应的旋转矩阵如下：
$$\begin{array}{rl}R(\varphi )& =\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & sin\varphi \\ 0& -sin\varphi & cos\varphi \end{array}\right]\\ R(\theta )& =\left[\begin{array}{ccc}cos\theta & 0& -sin\theta \\ 0& 1& 0\\ sin\theta & 0& cos\theta \end{array}\right]\\ R(\psi )& =\left[\begin{array}{ccc}cos\psi & sin\psi & 0\\ -sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]\end{array}$$
由于矩阵乘法不满足交换律，所以旋转顺序不同，得到的姿态也不同，一般按照"$ZYX$"的顺序进行旋转，得到：
$$\begin{array}{rl}R& =R(\psi )R(\theta )R(\varphi )\\ & =\left[\begin{array}{ccc}cos\theta cos\phi & -cos\varphi sin\psi +sin\varphi sin\theta cos\psi & sin\varphi sin\psi +cos\varphi sin\theta cos\psi \\ cos\theta sin\psi & cos\varphi cos\psi +sin\varphi sin\theta sin\psi & -sin\varphi cos\psi +cos\varphi sin\theta sin\psi \\ -sin\theta & sin\varphi cos\theta & cos\varphi cos\theta \end{array}\right]\end{array}$$
虽然欧拉角的表示方式很直观，但是由于万向节死锁的存在，一般不用欧拉角来计算姿态。

四元数

四元数(Quaternion)是一种能表示三维物体转动的四维超复数。一般的复数可表示为$a+ib$，而四元数包含一个实部，三个虚部，用$q_0+q_{1}i+q_{2}j+q_{3}k$表示，用向量表示为：
$$q=\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]\left[\begin{array}{c}1\\ i\\ j\\ k\end{array}\right]$$
假设坐标系$A$，绕该坐标系中的一个轴$\hat{r}=[r_x,r_y,r_z]$进行旋转，旋转角度为$\theta$，得到坐标系$B$，如下图：

我们可以用如下四元数来表示从$A$系到$B$系的旋转，也即$A$系绕$\hat{r}$轴的正方向(右手准则)转动角度$\theta$后与$B$系重合。注：使用四元数来表示刚体旋转，必须使四元数的模等于1
$${}_A^{B}q=\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\end{array}\right]=\left[\begin{array}{cccc}cos\frac{\theta }{2}& r_{x}sin\frac{\theta }{2}& r_{y}sin\frac{\theta }{2}& r_{z}sin\frac{\theta }{2}\end{array}\right]$$
相反的旋转可用$q_B^A$表示：
$${}_B^{A}q={}_A^B{q}^{-1}=\frac{{}_A^B{q}^{\ast }}{||{}_A^{B}q||}$$
其中$q^{\ast }$表示共轭，$||q||$表示四元数的模，用于表示姿态的四元数模长固定为1，所以：
$${}_B^{A}q={}_A^B{q}^{\ast }=\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\end{array}\right]$$
也即相反的旋转可以用四元数的共轭表示。

使用$\otimes$来表示四元数乘法，对于四元数$a$、$b$，四元数乘积为：
$$\begin{array}{rl}a\otimes b& =\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\\ a_3\end{array}\right]\otimes \left[\begin{array}{c}{b}_0\\ b_1\\ b_2\\ b_3\end{array}\right]\\ & =\left[\begin{array}{c}{a}_0{b}_0-a_1{b}_1-a_2{b}_2-a_3{b}_3\\ a_0{b}_1+a_1{b}_0+a_2{b}_3-a_3{b}_2\\ a_0{b}_2-a_1{b}_3+a_2{b}_0+a_3{b}_1\\ a_0{b}_3+a_1{b}_2-a_2{b}_1+a_3{b}_0\end{array}\right]\\ & =\left[\begin{array}{cccc}{b}_0& -b_1& -b_2& -b_3\\ b_1& b_0& b_3& -b_2\\ b_2& -b_3& b_0& b_1\\ b_3& b_2& -b_1& b_0\end{array}\right]\cdot \left[\begin{array}{c}{a}_0\\ a_1\\ a_2\\ a_3\end{array}\right]\\ & =\left[\begin{array}{cccc}{a}_0& -a_1& -a_2& -a_3\\ a_1& a_0& -a_3& a_2\\ a_2& a_3& a_0& -a_1\\ a_3& -a_2& a_1& a_0\end{array}\right]\cdot \left[\begin{array}{c}{b}_0\\ b_1\\ b_2\\ b_3\end{array}\right]\end{array}$$
令$a=[a_s,a_v{]}^T$，$b=[b_s,b_v{]}^T$，下标$s$表示实部，下标$v$表示虚部，则两者的乘积可表示为矩阵形式：
$$\begin{array}{rl}a\otimes b& =\left[\begin{array}{cc}{b}_s& -b_v^T\\ b_v& b_{s}I-(b_v\times )\end{array}\right]\left[\begin{array}{c}{a}_s\\ a_v\end{array}\right]\\ & =\left[\begin{array}{cc}{a}_s& -a_v^T\\ a_v& a_{s}I+(a_v\times )\end{array}\right]\left[\begin{array}{c}{b}_s\\ b_v\end{array}\right]\end{array}$$
其中$v\times$表示向量$v$的反对称矩阵，如下：
$$v\times =\left[\begin{array}{ccc}0& -v_3& v_2\\ v_3& 0& -v_1\\ -v_2& v_1& 0\end{array}\right]$$
假设${}_B^{A}q$和${}_C^{B}q$分别表示$B$系→$A$系、$C$系→$B$系的旋转，则$C$系→$A$系的旋转可表示为：
$${}_C^{A}q={}_B^{A}q\otimes {}_C^{B}q$$
通过如下算子即可将$B$系中的向量${}^{B}v$转换为$A$系中的向量${}^{A}v$：
$$\left[\begin{array}{c}0\\ {}^{A}v\end{array}\right]={}_B^{A}q\otimes \left[\begin{array}{c}0\\ {}^{B}v\end{array}\right]\otimes {}_B^A{q}^{\ast }$$
令${}_B^{A}q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}{q}_s\\ q_v\end{array}\right]$，${}_B^A{q}^{\ast }=\left[\begin{array}{c}{q}_0\\ -q_1\\ -q_2\\ -q_3\end{array}\right]=\left[\begin{array}{c}{q}_s\\ -q_v\end{array}\right]$，带入四元数乘法的矩阵表示形式，得到：
$$\begin{array}{rl}{}^{A}v& ={}_B^{A}q\otimes \left[\begin{array}{c}0\\ {}^{A}v\end{array}\right]\otimes {}_B^A{q}^{\ast }\\ & =\left[\begin{array}{cc}{q}_s& q_v^T\\ -q_v& q_{s}I+(q_v\times )\end{array}\right]\left[\begin{array}{cc}{q}_s& -q_v^T\\ q_v& q_{s}I+(q_v\times )\end{array}\right]\left[\begin{array}{c}0\\ {}^{A}v\end{array}\right]\\ & =\left[\begin{array}{cc}1& 0^T\\ 0& q_v{q}_v^T+q_s^{2}I+2q_s(q_v\times )+(q_v\times {)}^2\end{array}\right]\left[\begin{array}{c}0\\ {}^{A}v\end{array}\right]\end{array}$$
其中，矩阵$R_{3\times 3}=q_v{q}_v^T+q_s^{2}I+2q_s(q_v\times )+(q_v\times {)}^2$就是旋转矩阵的四元数表示形式，展开如下：
$$R=\left[\begin{array}{ccc}2(q_0^2+q_1^2)-1& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 2(q_0^2+q_2^2)-1& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 2(q_0^2+q_3^2)-1\end{array}\right]$$
向量${}^{A}v$通过左乘$R$，也能够转换为${}^{B}v$，即：${}^{B}v=R\cdot {}^{A}v$

通过如下公式可将四元数转换为欧拉角：
$$\begin{array}{rl}\varphi & =Atan2(2q_2{q}_3-2q_0{q}_1,2q_0^2+2q_3^2-1)\\ \theta & =-si{n}^{-1}(2q_1{q}_3+2q_0{q}_2)\\ \psi & =Atan2(2q_1{q}_2-2q_0{q}_3,2q_0^2+2q_1^2-1)\end{array}$$
其中

• $\varphi$：横滚角(roll)
• $\theta$：俯仰角(pitch)
• $\psi$：偏航角(yaw)

姿态解算原理

基于角速度积分得到的姿态

对角速度进行积分，可以得到角度，四元数微分方程的推导过程较为复杂，本文直接给出，如下：

$${}_b^n\dot{q}=\frac{1}{2}{}_b^{n}q\otimes \left[\begin{array}{c}0\\ {}^b\omega \end{array}\right]$$
其中，角标$b$表示机体(body)坐标系，角标$n$表示导航(navigation)坐标系，$\omega$表示角速度。使用前向差分法对其进行离散化：
$$\dot{q}=\frac{q_t-q_{t-1}}{△t}=\frac{1}{2}{q}_{t-1}\otimes \left[\begin{array}{c}0\\ \omega \end{array}\right]$$
离散后的方程如下：
$$\begin{array}{rl}{{}_b^{n}q}_{\omega ,t}& ={{}_b^{n}q}_{t-1}+\frac{1}{2}{{}_b^{n}q}_{t-1}\otimes \left[\begin{array}{c}0\\ {}^b\omega \end{array}\right]△t\end{array}$$

${}^b\omega$表示机体坐标系下的角速度，也就是陀螺仪的测量值。

基于加速度计观测得到的姿态

对角速度积分可以得到机体的姿态，但是这个姿态会随着时间漂移，还需要引入其他观测量来对积分得到的姿态进行校正，如使用加速度计测量得到的三轴加速度。

前面说到，旋转矩阵也是一种姿态的表示方法，它是一个正交矩阵，即满足：$R^T=R^{-1}$。除了使用四元数表示外，它还可以用欧拉角表示：
KaTeX parse error: Expected 'EOF', got '&' at position 3: R&̲=\begin{bmatrix…
其中：$\varphi$表示横滚角(roll)，$\theta$表示俯仰角(pitch)，$\psi$表示偏航角(yaw)

如果用$R_b^n$来表示$b$系到$n$系的旋转矩阵，则$b$系中的向量${}^{b}v$通过左乘$R_b^n$即可转换为$n$系中的向量${}^{n}v$，即：
$${}^{n}v=R_b^n\cdot {}^{b}v$$
那么如何用加速度计的三轴观测值得到机体的姿态呢？我们知道，当IMU在导航坐标系($n$系)静止时，加速度计的输出为：
$${}^{n}a=\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$$
其中$g$为重力加速度。此时加速度计在机体坐标系($b$系)中的输出为：
$${}^{b}a=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$$
将${}^{n}a$、${}^{b}a$带入$\{\begin{array}{l}{}^{b}a=R_n^b\cdot {}^{n}a\\ R_n^b={R_b^n}^T\end{array}$，得：
$$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=R_n^b\cdot \left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{c}-gsin\theta \\ gcos\theta sin\varphi \\ gcos\theta cos\varphi \end{array}\right]$$
解得：
$$\begin{array}{rl}\varphi & =arctan(\frac{a_y}{a_z})\\ \theta & =arcsin(-\frac{a_x}{g})\end{array}$$
由于偏航角$\psi$的旋转轴与重力方向平行，所以使用加速度计没办法获得偏航角$\psi$，还需要其他的传感器，如磁力计，这里不展开叙述。

以上只是说明使用加速度计测量值计算姿态的原理，但是在实际应用中并没有这么简单的，因为载体是会运动的，此时加速度计在$n$系的输出值并不恒等于重力加速度。

互补滤波

前面说到，陀螺仪解算得到的姿态具有良好的高频特性，但是会随着时间漂移，而加速度计解算得到的姿态具有良好的低频特性，不会随着时间漂移，但是载体剧烈运动时，往往不能解算出真实的姿态。这时可以将陀螺仪的高频特性和加速度计的低频特性相融合，得到高频、低频特性都很好的算法。

当然这里的融合不是简单地将陀螺仪和加速度计解算得到的姿态进行相加求平均，而是要用互补滤波，以下将介绍Mahony互补滤波算法。

误差定义

互补滤波是一种基于误差反馈的滤波器，首先要定义误差，Mahony算法中，用向量叉积来表示误差。向量叉积的模长如下：
$$|a\times b|=|a||b|sin\theta$$
若$a$、$b$均为单位向量，则$|a\times b|=sin\theta$，使用小角度假设，则：
$$|a\times b|=sin\theta \approx \theta$$
两个向量差距越大，它们的叉积模长也越大，因此可以用向量的叉积来表示向量间的误差。接下来需要构造两个向量。

• 加速度归一化向量

对加速度计的测量值进行归一化，得到：加速度的归一化向量
$$a^{\prime }=\left[\begin{array}{c}{a}_x^{\prime }\\ a_y^{\prime }\\ a_z^{\prime }\end{array}\right]=\frac{1}{|a|}\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$$
Mahony源码中的相应程序为：

// 归一化加速度测量值
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

• 当前姿态估计得到重力方向向量

上一节中，当载体处于静止状态时，加速度计在$n$系的输出恒为$\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$。如果带入四元数形式的旋转矩阵，则有
$$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=R_n^b\cdot \left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=g\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 2(q_0^2+q_3^2)-1\end{array}\right]$$
我们用$v=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$来表示重力方向向量：
$$v=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\\ 2(q_2{q}_3+q_0{q}_1)\\ 2(q_0^2+q_3^2)-1\end{array}\right]$$
相应程序如下：

// 估计重力方向向量
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;

注意此处计算的向量是原始向量的一半，这样做的目的是减少乘法运算量，所以在程序开头的宏定义中，滤波系数$K_p$、$K_i$均为两倍的比例和积分增益。

#define twoKpDef	(2.0f * 0.5f)	// 2 * 比例增益
#define twoKiDef	(2.0f * 0.0f)	// 2 * 积分增益

• 向量叉积得到误差

误差可以表示为：
$$Err=a^{\prime }\times v$$
相应的程序为：

// 误差就是两个向量的叉积
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);

误差补偿(补偿角速度)

使用PI控制算法来进行误差补偿，补偿量就是角速度的测量偏差，如下：
$${\omega }_{bias}=K_p\cdot Err+K_i\cdot \int Err$$
其中$K_p$为比例系数，$K_i$为积分系数。$K_p$可以表示对传感器的信任程度，$K_p$越大，越信任加速度计的测量值，反之$K_p$越小，越信任陀螺仪的数据。$K_i$用于消除静态误差，也就是消除陀螺仪的零偏。

最后将补偿量加到角速度测量值上则可得到修正后的角速度。相应的程序为：

// 计算误差补偿，并应用到角速度测量值上
if(twoKi > 0.0f) {
    integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// 误差积分
    integralFBy += twoKi * halfey * (1.0f / sampleFreq);
    integralFBz += twoKi * halfez * (1.0f / sampleFreq);
    gx += integralFBx;	// 应用积分反馈项
    gy += integralFBy;
    gz += integralFBz;
}
else {
    integralFBx = 0.0f;	// 防止积分饱和
    integralFBy = 0.0f;
    integralFBz = 0.0f;
}

// 应用比例反馈项
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;

姿态递推

通过前面的PI算法，可以得到修正后的角速度值。再根据前文提到的四元数微分方程对姿态进行递推即可得到融合后的姿态
$$\begin{array}{rl}{{}_b^{n}q}_{\omega ,t}& ={{}_b^{n}q}_{t-1}+\frac{1}{2}{{}_b^{n}q}_{t-1}\otimes {}^b\omega △t\\ & ={{}_b^{n}q}_{t-1}+\frac{1}{2}△t\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]\end{array}$$
相应的程序如下：

// 利用四元数微分方程进行迭代
gx *= (0.5f * (1.0f / sampleFreq));
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx); 

// 四元数归一化，用于表示姿态的四元数，其模长必须为1
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;

此时姿态还是用四元数来表示的，不是很直观，通过前文提到的公式可以将四元数转换为欧拉角：
$$\begin{array}{rl}\varphi & =Atan2(2q_2{q}_3-2q_0{q}_1,2q_0^2+2q_3^2-1)\\ \theta & =-si{n}^{-1}(2q_1{q}_3+2q_0{q}_2)\\ \psi & =Atan2(2q_1{q}_2-2q_0{q}_3,2q_0^2+2q_1^2-1)\end{array}$$

调用方法

在调用Mahony解算程序前，建议先对陀螺仪和加速度计零偏进行估计。将IMU静止一段时间，这段时间角速度和加速度的均值就是陀螺仪和加速度计的零偏。程序如下：

if(!ready) 
{
    // 计算零偏
    gyro_offsets_sum[0] += imu.gyro[0];
    gyro_offsets_sum[1] += imu.gyro[1];
    gyro_offsets_sum[2] += imu.gyro[2];
    accel_offsets_sum[0] += imu.accel[0];
    accel_offsets_sum[1] += imu.accel[1];
    accel_offsets_sum[2] += imu.accel[2];
    offset_count++;
    if(offset_count > 3000)
    {
        gyroOffset[0] = gyro_offsets_sum[0] / offset_count;
        gyroOffset[1] = gyro_offsets_sum[1] / offset_count;
        gyroOffset[2] = gyro_offsets_sum[2] / offset_count;
        accelOffset[0] = accel_offsets_sum[0] / offset_count;
        accelOffset[1] = accel_offsets_sum[1] / offset_count;
        accelOffset[2] = accel_offsets_sum[2] / offset_count;
        accelOffset[2]-= 9.81; // 去除重力加速度常量

        offset_count = 0;
        gyro_offsets_sum[0] = 0;
        gyro_offsets_sum[1] = 0;
        gyro_offsets_sum[2] = 0;
        ready = true;
    }
    return;
}

// 去除陀螺仪零偏
imu.gyro[0] -= gyroOffset[0];
imu.gyro[1] -= gyroOffset[1];
imu.gyro[2] -= gyroOffset[2];
// 去除加速度计零偏
imu.accel[0] -= accelOffset[0];
imu.accel[1] -= accelOffset[1];
imu.accel[2] -= accelOffset[2];

// 调用Mahony解算程序
MahonyAHRSupdateIMU(imu.gyro[0], imu.gyro[1], imu.gyro[2], 
                 	imu.accel[0], imu.accel[1], imu.accel[2]);

参考

https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

https://www.cnblogs.com/WangHongxi/p/12357230.html