【姿态解算与滤波算法】

姿态解算

一、主线

姿态表示方式：矩阵表示，轴角表示，欧拉角表示，四元数表示。

惯性测量单元IMU（Inertial Measurement Unit）：MPU6050芯片，包含陀螺仪和加速度计，分别测量三轴加速度和三轴角速度。注意，传感器所测数据是原始数据，包含了噪声，无法直接用于飞行器的姿态解算，因此需要对数据进行滤波。

滤波算法：非线性互补滤波算法，卡尔曼滤波算法，Mahony互补滤波算法。

二、知识点补充

加速度计和陀螺仪

加速度计：加速度计,可以测量加速度,包括外力加速度和重力加速度,因此，当被测物体在静止或匀速运动（匀速直线运动）的时候,加速度计仅仅测量的是重力加速度,而重力加速度与 R 坐标系（绝对坐标系）是固连的,通过这种关系,可以得到加速度计所在平面与地面 的角度关系 也就是横滚角和俯仰角。把加速度传感器水平静止放在桌子上，它的Z轴输出的是1g的加速度。因为它Z轴方向被重力向下拉出了一个形变。可惜的是，加速度传感器不会区分重力加速度与外力加速度。所以，当系统在三维空间做变速运动时，它的输出就不正确了，或者说它的输出不能表明物体的姿态和运动状态。

陀螺仪：陀螺仪测量角速度。陀螺仪模型如图所示，陀螺仪的每个通道检测一个轴的旋转。

图2[引自网络] 

上图中，Rxz是R在XZ面上的投影，与Z轴的夹角为Axz。Ryz是R在ZY面上的投影，与Z轴的夹角为Ayz。陀螺仪就是测量上面定义角度的变化率，换句话说，它会输出一个与上面这些角度变化率线性相关的值。

加速度计工作原理介绍（摘自网络）

大多数加速度计可归为两类：数字和模拟。数字加速度计可通过I2C，SPI或USART方式获取信息，而模拟加速度计的输出是一个在预定范围内的电压值，你需要用ADC（模拟量转数字量）模块将其转换为数字值。不管使用什么类型的ADC模块，都会得到一个在一定范围内的数值。例如一个10位ADC模块的输出值范围在0-1023间。假设我们从10位ADC模块得到了以下的三个轴的数据：

每个ADC模块都有一个参考电压，假设在我们的例子中，它是3.3V。要将一个10位的ADC值转成电压值，我们使用下列公式：

将3个轴的值代入上式，得到：

每个加速度计都有一个零加速度的电压值，这个电压值对应于加速度为0g。通过计算相对0g电压的偏移量我们可以得到一个有符号的电压值。比方说，0g电压值VzeroG=1.65V，通过下面的方式可以得到相对0g电压的偏移量:

现在我们得到了加速度计的电压值，但它的单位还不是g(9.8m/s^2)，最后的转换，我们还需要引入加速度计的灵敏度，单位通常是mV/g。比如，加速度计的灵敏度Sensitivity = 478.5mV/g。灵敏度值可以在加速度计说明书中找到。要获得最后的单位为g的加速度，我们使用下列公式计算：

综上，可以把以上步骤用以下公式表达

现在我们得到了惯性力矢量的三个分量，如果设备除了重力外不受任何外力影响，那我们就可以认为这个方向就是重力矢量的方向。（自此明白了文献[1]中所说只使用加速度计获得的角度是基于飞行器在匀速飞行或静止的条件下得到的）

图2[引自网络]

我们感兴趣的角度是向量R和X，Y，Z轴之间的夹角，那就令这些角度为Axr，Ayr，Azr。观察由R和Rx组成的直角三角形

图2中，那么，角度即为

三、互补滤波算法

加速度计是极易受外部干扰的传感器（如机械振动），但是测量值的误差不随时间的变化。陀螺仪输出的角速度可以积分得到角度，动态性能好，受外部干扰小，但积分会造成误差累积。可以看出，它们优缺点互补，结合起来才能有好的效果。

经典互补滤波算法（Classical Complementary Filter）

经典互补滤波算法基本原理是充分利用加速度计提供的低频角度信号和陀螺仪提供的高频角速度信号，对加速度计进行低通滤波，对陀螺仪进行高通滤波，分别滤出相应的干扰信号，为两者的有效融合提供了很好的解决方案[2]。

图3 经典互补滤波算法—频域形式原理图[2]

融合后姿态角估计值为

其中，为陀螺仪测量的角速度，为加速度计测量的角度值，

为比例系数，为高通滤波器，

为低通滤波器。

图4 经典互补滤波算法—时域形式原理图[2]

对

进行反拉氏变换，可得时域微分形式为

    改进后的互补滤波算法（Explicit Complementary Filter）

经典互补滤波算法实现简单，但是估算精度角度较低，文献[3]提出了一种改进算法（ECF），在经典互补滤波算法的补偿环节加入积分器，以消除陀螺仪漂移常值误差，原理框图如下图。

图5 改进后的互补滤波算法[2]

时域微分形式为

 

参考文献

[1] 郭晓鸿,杨忠,陈喆,等. EKF和互补滤波器在飞行姿态确定中的应用[J]. 传感器与微系统,2011,30(11):149-152.

[2] 傅忠云,朱海霞,孙金秋,等. 基于惯性传感器 MPU6050 的滤波算法研究[J]. 压电与声光, 2015 (2015 年 05): 821-825,829.

[3] Mahony R, Hamel T, Pflimlin J M. Nonlinear complementary filters on the special orthogonal group[J]. IEEE Transactions on automatic control, 2008, 53(5): 1203-1218.

[4] Euston M, Coote P, Mahony R, et al. A complementary filter for attitude estimation of a fixed-wing UAV[C]//Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on. IEEE, 2008: 340-345.

附程序：

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

{

    float norm;

    float vx, vy, vz;

    float ex, ey, ez;

    float q0q0 = q0*q0;

    float q0q1 = q0*q1;

    float q0q2 = q0*q2;

    float q1q1 = q1*q1;

    float q1q3 = q1*q3;

    float q2q2 = q2*q2;

    float q2q3 = q2*q3;

    float q3q3 = q3*q3;

    if(ax*ay*az==0)

        return;

    // 第一步：对加速度数据进行归一化

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

    ax = ax / norm;

    ay = ay / norm;

    az = az / norm;

    // 第二步：DCM矩阵旋转

    vx = 2*(q1q3 - q0q2);

    vy = 2*(q0q1 + q2q3);

    vz = q0q0 - q1q1 - q2q2 + q3q3 ;

    // 第三步：在机体坐标系下做向量叉积得到补偿数据

    ex = ay*vz - az*vy ;

    ey = az*vx - ax*vz ;

    ez = ax*vy - ay*vx ;

    // 第四步：对误差进行PI计算，补偿角速度

    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;

    // 第五步：按照四元数微分公式进行四元数更新

    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;

}