1、基于互补滤波的飞行器姿态解算基于互补滤波的飞行器姿态解姿态解算、主线姿态表示方式：矩阵表示，轴角表示，欧拉角 表示，四元数表示。惯性测量单元 IMU （Inertial Measurement Unit）： MPU6050芯片，包含陀螺仪和加速度计， 分别测量三轴加速度和三轴角速度。注意，传感 器所测数据是原始数据，包含了噪声，无法直接 用于飞行器的姿态解算，因此需要对数据进行滤 波。滤波算法：非线性互补滤波算法，卡尔曼滤波 算法，Mahony互补滤波算法。二、知识点补充加速度计和陀螺仪加速度计：加速度计,可以测量加速度，包括外 力加速度和重力加速度，因此，当被测物体在静 止或匀速运动（匀速直
2、线运动）的时候，加速度 计仅仅测量的是重力加速度，而重力加速度与R 坐标系（绝对坐标系）是固连的，通过这种关系， 可以得到加速度计所在平面与地面 的角度关系也就是横滚角和俯仰角。把加速度传感器水平静 止放在桌子上，它的Z轴输出的是1g的加速度。 因为它Z轴方向被重力向下拉出了一个形变。可 惜的是，加速度传感器不会区分重力加速度与外 力加速度。所以，当系统在三维空间做变速运动 时，它的输出就不正确了，或者说它的输出不能 表明物体的姿态和运动状态。陀螺仪：陀螺仪测量角速度。陀螺仪模型如图 1所示，陀螺仪的每个通道检测一个轴的旋转。上图中，Rxz是R在XZ面上的投影，与 Z 轴的夹角为Axz。Ryz
3、是R在ZY面上的投影， 与Z轴的夹角为Ayz。陀螺仪就是测量上面定义 角度的变化率，换句话说，它会输出一个与上面 这些角度变化率线性相关的值。加速度计工作原理介绍（摘自网络） 大多数加速度计可归为两类：数字和模拟。 数字加速度计可通过I2C，SPI或USART方式获取信息，而模拟加速度计的输出是一个在预定 范围内的电压值，你需要用 ADC （模拟量转数 字量）模块将其转换为数字值。不管使用什么类 型的ADC模块，都会得到一个在一定范围内的 数值。例如一个10位ADC模块的输出值范围在 0-1023间。假设我们从10位ADC模块得到了 以下的三个轴的数据：AdcRx 586, AdcRy 630
4、, AdcRz 561每个ADC模块都有一个参考电压，假设在我们 的例子中，它是3.3V。要将一个10位的ADC 值转成电压值，我们使用下列公式：VoltsRx AdcRx Vref1023将3个轴的值代入上式，得到:VoltsRx 586VoltsRy 630VoltsRz 5613.310231.89V3.310233.310232.03V1.81V每个加速度计都有一个零加速度的电压值，这个 电压值对应于加速度为0g。通过计算相对0g电 压的偏移量我们可以得到一个有符号的电压值。比方说，0g电压值VzeroG 1.65V，通过下面的方式可 以得到相对0g电压的偏移量:DeltaVoltsR
5、x 1.89V 1.65V 0.24VDeltaVoltsRy 2.03V 1.65V 0.38VDeltaVoltsRz 1.81V 1.65V 0.16V现在我们得到了加速度计的电压值， 但它的单位还不是g(9.8m/s2),最后的转换，我们还需要引入加 速度计的灵敏度，单位通常是mV/g。比如，加速 度计的灵敏度Sensitivity 478.5mV /g 0.4785V / g。灵敏度值 可以在加速度计说明书中找到。要获得最后的单 位为g的加速度，我们使用下列公式计算：RxDeltaVoltsRxSen sitivitySen sitivityAdcRyRyVref1023VzeroG
6、Sen sitivityRx 0.24 0.4785 0.5gRy 0.38 0.4785 0.79gRz 0.16 0.4785 0.33g综上，可以把以上步骤用以下公式表达VrefAdcRx VzeroGRx1023VrefAdcRz VzeroGRz1023 Sen sitivity现在我们得到了惯性力矢量的三个分量， 如果设认为这个方向就是重力矢量的方向。(自此明白了文献1中所说只使用加速度计获得的角度是基于飞行器在匀速飞行或静止的条件下得到的）我们感兴趣的角度是向量 R和X, Y , Z轴之间 的夹角，那就令这些角度为 Axr，Ayr，Azr。观 察由R和Rx组成的直角三角形AxrR
7、xAyr巴八 RzCOScoscos Azr RRR图2中，R2RX2RY2RZ2，那么,角度即为AxrRxarccosAyrRyarccosA RzAzr arccosRRR三、互补滤波算法加速度计是极易受外部干扰的传感器（如机 械振动），但是测量值的误差不随时间的变化。 陀螺仪输出的角速度可以积分得到角度， 动态性 能好，受外部干扰小，但积分会造成误差累积。 可以看出，它们优缺点互补，结合起来才能有好 的效果。经典互补滤波算法 (Classical Complementary Filter )2。经典互补滤波算法基本原理是充分利用加 速度计提供的低频角度信号和陀螺仪提供的高 频角速度信号，
8、对加速度计进行低通滤波，对陀 螺仪进行高通滤波，分别滤出相应的干扰信号， 为两者的有效融合提供了很好的解决方案图3经典互补滤波算法一频域形式原理图融合后姿态角估计值为? s g K pas Kp s s Kp其中，g为陀螺仪测量的角速度, 度计测量的角度值，Kp为比例系数，Fi(s)a为加速為为高通滤波器，先为低通滤波器。F2(s) 1 Fi(s)图4经典互补滤波算法一时域形式原理图可得时域微分2对? Ia进行反拉氏变换， s Kp s s Kp形式为改进后的互补滤波算法（ExplicitComplementary Filter ）经典互补滤波算法实现简单，但是估算精度 角度较低，文献3提出了
9、一种改进算法（ECF），在经典互补滤波算法的补偿环节加入积分器,消除陀螺仪漂移常值误差，原理框图如下图图5改进后的互补滤波算法2时域微分形式为四、四旋翼飞行器姿态解算流程四旋翼姿态解算整体流程框图如下图5四元数表示的姿态解算整体流程框图 其中，Gyroscope为陀螺仪，_b是陀螺仪输出的 测量数据，Accelerometer是加速度计，在Airspeed模块以及fb作用下，加速度输出测量数 据P。第一，对加速度计测出来的数据进行归一化 处理：；处理團；作为标准第二，将重力加速度旋转到机体坐标，得到 重力加速度在三个轴上的加速度分量， 加速度。0C； 01c cc ss2 2q q2(q&22
10、(qQ32(qQ32(qq12 2qo q1001scs2 2 q2 q3 qq3) qq2)qoq2) q2q3)2 2 q2 q3s s c s s s c2加22 2q。 qi2(qqqq3)2 2 q2 q3q2q3)s s csss c cssc c2(qq22qq32 2q。 qq)qq)2 2q2 q3001T其中Cb为世界坐标系到机体坐标系的变换 矩阵。（文献4图中此处原文中有错误，已经在 截图中进行了修改）第三，归一化后的加速度计值与标准加速度 做叉积运算，求得误差向量e ；第四，对误差进行滤波，改进后的互补滤波 算法(ECF)可以表示为以下四元数形式： q 技 p)kpe
11、k| e ，e v ?第四，求解四元数微分方程&技p)，求得 最新的qo, q1.q2.q3 ；第五，代入以下公式求得姿态角。arctan 2(種弊Jqo q q2 q3arcsi n2(qQ3 qq2)arctanq qi q2 q3参考文献1郭晓鸿，杨忠，陈喆，等.EKF和互补滤波器 在飞行姿态确定中的应用 J.传感器与微系 统,2011,30(11):149-152.2傅忠云,朱海霞,孙金秋,等.基于惯性传感 器MPU6050的滤波算法研究J.压电与声光，2015 (2015 年 05): 821-825,829.3Mahony R, Hamel T, Pflimlin J M.Nonl
12、inear complementary filters on the special orthog onal groupJ. IEEE Transactions on automatic control, 2008, 53(5): 1203-1218.4Euston M, Coote P, Mahony R, et al. A compleme ntary filter for attitudeestimati on of a fixed-w ingUAVC/Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ Internatio
13、nal 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 qlql = q1*q1;float q1q3 = q1*q3;float q2q2 = q2*q2;float q2q3 = q2*q3;float
14、q3q3 = q3*q3;if(ax*ay*az=O)return;/第一步：对加速度数据进行归一化norm = sqrt(ax*ax + ay*ay + az*az);ax = ax / norm;ay = ay / norm;az = az / n orm;/第二步：DCM矩阵旋转vx = 2*(q1q3 - q0q2);vy = 2*(q0q1 + q2q3);vz = qOqO - q1q1 - q2q2 + q3q3 ;/第三步：在机体坐标系下做向量叉积得到 补偿数据ex = ay*vz - az*vy ;ey = az*vx - ax*vz ;ez = ax*vy - ay*vx
15、;/第四步：对误差进行PI计算，补偿角速 度exlnt = exlnt + ex * Ki;eylnt = eylnt + ey * Ki;ezlnt = ezlnt + ez * Ki;gx = gx + Kp*ex + exl nt;gy = gy + Kp*ey + eyl nt;gz = gz + Kp*ez + ezl nt;/第五步：按照四元数微分公式进行四元数 更新q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (qO*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
