四元数姿态解算算法的解析与应用

只看该作者 · 2021-7-19 10:36

本帖最后由会笑的星星于 2021-7-19 10:40 编辑

因为工作需要，研究了一下使用四元数完成MPU6883的姿态解算，自己在研究这个解算算法的时候遇到了不少问题，查阅了不少资料，但是很多都讲的不够清楚或者有大量的数学推导，一时难以理解。但对于工程应用而言，数学上的细节我认为倒不是最重要的，更重要的是要知道的是这个算法的基本原理以及使用方法。因此，这篇**的目的就是在于从宏观上描述四元数姿态解算算法的原理以及应用，为了避开一些繁杂的细节，我会忽略掉数学证明的过程，直接给出结果。如果你想知道这些方程式是怎么来的，我把相关的链接放在文末，以便你研究。

得出MPU6883的姿态，实际上就是计算MPU6883的Pitch（俯仰角）、Roll（横滚角）、Yall（偏航角）（关于这些角度，你可以看其他资料）。要求解这几个角，我们先要搞清楚一件事，那就是MPU芯片测量的6轴数据是相对自己的坐标系（如图一），而MPU自身又随着飞行器的姿态变化而不停变化，这也意味着MPU的坐标系也在不停的变化。而对于我们位于地面上的观察者来说，我们所在参考系是静止的（如图二）。因此，在地面上，我们想要知道MPU相对于地面的姿态角，就必须要把MPU在自己的参考系中测量的值转换到地面参考系。要完成这种转换，最终得到相对地面参考系的姿态角，用到的一种方法就是四元数算法。

图一：MPU坐标系

图二：大地坐标系
所谓的四元数，简单理解就是利用被称为四元数的数学方法求得的四个实数（设为q0、q1、q2、q3），借助这个4个实数，我们能描述一个物体在空间中的旋转状态，进而能求得MPU的姿态角。下面就是利用四元数求解姿态角的公式：

复制

Pitch = arcsin(2 * q0* q2 - 2 * q1 * q3)

Roll = arctan((2 * q2 * q3 + 2 * q0 * q1) / (-2 * q1^2 - 2 * q2^2 + 1)) 

Yaw = arctan((2*q1^2 + 2*q0*q3) / (-2*q2^2 - 2*q3^2+1))

有了求角公式，剩下的问题就是如何求解q0，q1，q2，q3。而这一点我们的前辈早已解决，我们先看代码：

复制

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          

float exInt = 0, eyInt = 0, ezInt = 0;



void IMUupdate2(float gx, float gy, float gz, float ax, float ay, float az)

{

  float norm;

  float vx, vy, vz;

  float ex, ey, ez;

 

  // normalise the measurements

  norm = sqrt(ax * ax + ay * ay + az * az);

  ax = ax / norm;

  ay = ay / norm;

  az = az / norm;

 

  // estimated direction of gravity

  vx = 2.0 * (q1 * q3 - q0 * q2);

  vy = 2.0 * (q0 * q1 + q2 * q3);

  vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

 

  // error is sum of cross product between reference direction of field and direction measured by sensor

  ex = (ay * vz - az * vy);

  ey = (az * vx - ax * vz);

  ez = (ax * vy - ay * vx);

 

  // integral error scaled integral gain

  exInt = exInt + ex * Ki;

  eyInt = eyInt + ey * Ki;

  ezInt = ezInt + ez * Ki;

 

  // adjusted gyroscope measurements

  gx = gx + Kp * ex + exInt;

  gy = gy + Kp * ey + eyInt;

  gz = gz + Kp * ez + ezInt;

  

  // integrate quaternion rate and normalise

  q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;

  q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;

  q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;

  q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;

 

  // normalise quaternion

  norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

  q0 = q0 / norm;

  q1 = q1 / norm;

  q2 = q2 / norm;

  q3 = q3 / norm;      

}

这个算法的基本思路是以陀螺仪所测的角度为主，把由加速度得到角度误差补偿到由陀螺仪所得的角度值当中。之所以要这样做，是因为陀螺仪短期测量很准，但在长期测量时容易积累误差，而加速度则相反。

现在再来详细看看求解四元数算法的代码。

我们先看代码第四行。

复制

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

gx、gy、gz这里是以弧度为单位的角速度（rad/s）。其计算方法与你的选择的陀螺仪精度有关，比如你的陀螺仪精度选择为±250°/s，对应的灵敏度为131LSB，假设此时从陀螺仪中读出3个轴角速度对应的ADC分别为gx_adc_x、gy_adc_y、gz_adc_z。则陀螺仪每个轴的角速度为

复制

gx = gx_adc_x / 131 

gy = gy_adc_y / 131

gz = gz_adc_z / 131

以上求出的角速度是以°为单位的，这里，我们还需要把°转化为弧度。其转换方法如下：

复制

gx = gx * (π/180)

gy = gy * (π/180)

gz = gz * (π/180)

ax、ay、az这里可以是各个轴的加速度值（以x轴为例，ax = ac_adc_x / 加速度灵敏度），也可以是直接从加速度计中读取的adc值。推荐后一种方法，可以减少运算量。

接着看代码的11~14行。

复制

  norm = sqrt(ax * ax + ay * ay + az * az);

  ax = ax / norm;

  ay = ay / norm;

  az = az / norm;

这一部分的作用在于把ax、ay、az标准化，它有这么一个性质，就是对于空间向量（ax、ay、az），其模长为1。这个性质有什么作用呢？后面会讲到。

接着看代码17~19行。

复制

  vx = 2.0 * (q1 * q3 - q0 * q2);

  vy = 2.0 * (q0 * q1 + q2 * q3);

  vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

这里是利用上次计算的四元数估算地面坐标系的重力加速度(g = 9.8m/s^2)在MPU芯片坐标系下空间坐标（vx，vy，vz），它也满足一个性质 --- 模长为1。

我们前面说过，这个算法是利用了加速度的信息来补偿陀螺仪所得的角度。之所以如此，是因为算法假设MPU芯片所测的加速度值（空间向量(ax、ay、az)的模长）就是重力加速度（相对于MPU坐标系），且认为在没有误差的情况下，MPU加速度计所测的重力加速度应该与地面坐标系的重力加速度（经过坐标变换将重力加速度的地面坐标变换到MPU所在坐标系）在方向上是一致的。如果两者角度不一致，也就是说两者存在角度差，此时说明陀螺仪的角度测量有误差。因此，我们需要把由加速度得出的角度差按照一定的方式补偿到陀螺仪所测的角度当中。

那么，如何计算这个角度差呢？向量积就登场了 --- axb = |a||b|sinθ。a向量在代码中是陀螺仪所得的各个轴的加速度值，其向量为（ax，ay，az），b向量在代码中就是重力加速度的估算值，其向量为（vx，vy，vz）。我们刚刚说过，这两个向量的模长为1 --- |a|=|b|=1，当θ很小时，其值接近sinθ，因此，这里可以认为 axb = θ，根据向量积的运算规则（不知道的话自己百度吧），可以得到公式如下（代码22~24行）：

复制

  ex = (ay * vz - az * vy);

  ey = (az * vx - ax * vz);

  ez = (ax * vy - ay * vx);

由上述的代码，我们就能知道角度差，进而按照一定的方式补偿到陀螺仪所测的值当中。补偿方法见27~34行代码。

复制

  exInt = exInt + ex * Ki;

  eyInt = eyInt + ey * Ki;

  ezInt = ezInt + ez * Ki;

  

  gx = gx + Kp * ex + exInt;

  gy = gy + Kp * ey + eyInt;

  gz = gz + Kp * ez + ezInt;

这部分代码中，exInt 、eyInt、ezInt为角度误差的积分项， Kp*ex、Kp*ey、Kp*ez为角度误差比例项。之所以需要积分项以及比例项目，是因为如果只考虑比例项而不要积分项，那么角速度的值容易被当下误差值影响，比如偶然来的一个噪音就会影响到角速度的测量值。如果只考虑积分项而不考虑比例项，那么角速度的测量值对当下变化不会有任何反应，而其结果只由累计角度误差决定，假如此时误差值真的变化很快，所得到的角速度值显然就失真了。这一点，与PID算法中的思想是非常类似的。

上述代码中，在积分项（exInt 、eyInt、ezInt）中有一个积分系数Ki，这个值需要根据实际来调整。这个值过大，会导致积分项对角速度gx、gy、gz的补偿影响过大，如果太小，则影响又太小，一般而言，Ki < 0.01 [1]。在比例项(Kp * ex、Kp*ey、Kp*ez)中有一个系数Kp，同样需要根据实际来调整。这个系数如果设置过大，角速度的补偿值容易受到当前的加速度角度差（ex、ey、ez）影响，但好处在于陀螺仪所测的角度值能快速收敛到真实值，坏处在于容易受到当前角度差的影响，抗噪能力相对较弱，至于Kp到底选择一个什么值，则需要根据实际情况决定（我在一个项目中设置Kp=20，不过也有人推荐Kp < 1）。

我们根据27~34代码计算出了经过加速度角度差补偿的角速度，接下来要做的就是利用补偿后的角速度求得四元数，代码如下：

复制

  // integrate quaternion rate and normalise

  q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;

  q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;

  q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;

  q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;

代码中，halfT = 两次调用IMUupdate（）函数的时间间隔 / 2。如果要知道是为什么，需要去看推到过程。当然，你也可以不在意它，只要按照规则做就可以。

经过上面的过程，我们求得了四元数，但是还没有结束，我们还需要最后一步 --- 把求得的四元数q0、q1、q2、q3标准化，以为后续相关的四元数计算做好准备，代码如下。

复制

  norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

  q0 = q0 / norm;

  q1 = q1 / norm;

  q2 = q2 / norm;

  q3 = q3 / norm;

通过上述步骤，求出四元数，带入到姿态角公式中，就可以求出俯仰角、偏航角以及横滚角，如下所示。

复制

Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; 

Roll = atan2f(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; 

Yaw = atan2f(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q3+1) * 57.3;  //弧度转为角度，57.3 ≈ 180/π

关于四元数做姿态解算的算法就全部讲完，其中对于我们应用来说，最重要的是理解代码17~33行的基本原理，至于是否知道数学细节，并非十分重要。

在我应用这个算法时，出现了一个问题，求得的3个姿态角中，偏航角（YAW）会出现漂移，其他两个角则很稳定。本来怀疑是不是代码写错了，后来发现并不是。之所以出现YAW的漂移，是因为当MPU围绕着偏航角对应的坐标轴转动时，其在MPU坐标系中的重力加速度方向不会有任何改变，这就意味着，利用加速度的信息对角速度进行补偿就行不通了，此时角速度的值完全取决于陀螺仪的角速度测量值。因此，才出现了偏航角漂移的问题。要解决这个漂移问题，还需要地磁传感器的加入才行。

[1]关于四元数算法理解的补充：
https://blog.csdn.net/wubaobao1993/article/details/53130362?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.nonecase

[2]欧拉角以及四元数求解公式的推导：
https://blog.csdn.net/guanjianhe/article/details/95608801?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-8.non
ecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-8.nonecase

[3]为什么YAW会漂移？
https://zhuanlan.zhihu.com/p/24280315?from_voters_page=true

[4]为什么角速度需要加速度的补偿？
https://zhuanlan.zhihu.com/p/89592536

[5]一份四元数算法的代码参考
https://blog.csdn.net/qq_38288618/article/details/77320646

[6]陀螺仪、加速度计的基本原理见附件