基于互补滤波的飞行器姿态解算.docx
《基于互补滤波的飞行器姿态解算.docx》由会员分享，可在线阅读，更多相关《基于互补滤波的飞行器姿态解算.docx（13页珍藏版）》请在冰豆网上搜索。
基于互补滤波的飞行器姿态解算
基于互补滤波的飞行器姿态解
姿态解算
、主线
姿态表示方式：
矩阵表示，轴角表示，欧拉角表示，四元数表示。
惯性测量单元IMU（InertialMeasurementUnit）：
MPU6050芯片，包含陀螺仪和加速度计，分别测量三轴加速度和三轴角速度。
注意，传感器所测数据是原始数据，包含了噪声，无法直接用于飞行器的姿态解算，因此需要对数据进行滤波。
滤波算法：
非线性互补滤波算法，卡尔曼滤波算法，Mahony互补滤波算法。
二、知识点补充
加速度计和陀螺仪
加速度计：
加速度计,可以测量加速度，包括外力加速度和重力加速度，因此，当被测物体在静止或匀速运动（匀速直线运动）的时候，加速度计仅仅测量的是重力加速度，而重力加速度与R坐标系（绝对坐标系）是固连的，通过这种关系，可以得到加速度计所在平面与地面的角度关系
也就是横滚角和俯仰角。
把加速度传感器水平静止放在桌子上，它的Z轴输出的是1g的加速度。
因为它Z轴方向被重力向下拉出了一个形变。
可惜的是，加速度传感器不会区分重力加速度与外力加速度。
所以，当系统在三维空间做变速运动时，它的输出就不正确了，或者说它的输出不能表明物体的姿态和运动状态。
陀螺仪：
陀螺仪测量角速度。
陀螺仪模型如图1所示，陀螺仪的每个通道检测一个轴的旋转。
上图中，Rxz是R在XZ面上的投影，与Z轴的夹角为Axz。
Ryz是R在ZY面上的投影，与Z轴的夹角为Ayz。
陀螺仪就是测量上面定义角度的变化率，换句话说，它会输出一个与上面这些角度变化率线性相关的值。
加速度计工作原理介绍（摘自网络）大多数加速度计可归为两类：
数字和模拟。
数字加速度计可通过I2C，SPI或USART方式
获取信息，而模拟加速度计的输出是一个在预定范围内的电压值，你需要用ADC（模拟量转数字量）模块将其转换为数字值。
不管使用什么类型的ADC模块，都会得到一个在一定范围内的数值。
例如一个10位ADC模块的输出值范围在0-1023间。
假设我们从10位ADC模块得到了以下的三个轴的数据：
AdcRx586,AdcRy630,AdcRz561
每个ADC模块都有一个参考电压，假设在我们的例子中，它是3.3V。
要将一个10位的ADC值转成电压值，我们使用下列公式：
VoltsRxAdcRxVref
1023
将3个轴的值代入上式，得到:
VoltsRx586
VoltsRy630
VoltsRz561
3.3
1023
1.89V
3.3
1023
3.3
1023
2.03V
1.81V
每个加速度计都有一个零加速度的电压值，这个电压值对应于加速度为0g。
通过计算相对0g电压的偏移量我们可以得到一个有符号的电压值。
比方说，0g电压值VzeroG1.65V，通过下面的方式可以得到相对0g电压的偏移量:
DeltaVoltsRx1.89V1.65V0.24V
DeltaVoltsRy2.03V1.65V0.38V
DeltaVoltsRz1.81V1.65V0.16V
现在我们得到了加速度计的电压值，但它的单位
还不是g（9.8m/s2）,最后的转换，我们还需要引入加速度计的灵敏度，单位通常是mV/g。
比如，加速度计的灵敏度Sensitivity478.5mV/g0.4785V/g。
灵敏度值可以在加速度计说明书中找到。
要获得最后的单位为g的加速度，我们使用下列公式计算：
Rx
DeltaVoltsRx
Sensitivity
Sensitivity
AdcRy
Ry
Vref
1023
VzeroG
Sensitivity
Rx0.240.47850.5g
Ry0.380.47850.79g
Rz0.160.47850.33g
综上，可以把以上步骤用以下公式表达
Vref
AdcRxVzeroG
Rx
1023
Vref
AdcRzVzeroG
Rz
1023
Sensitivity
现在我们得到了惯性力矢量的三个分量，如果设
认为这个方向就是重力矢量的方向。
（自此明白
了文献［1］中所说只使用加速度计获得的角度是
基于飞行器在匀速飞行或静止的条件下得到的）
我们感兴趣的角度是向量R和X,Y,Z轴之间的夹角，那就令这些角度为Axr，Ayr，Azr。
观察由R和Rx组成的直角三角形
Axr
Rx
Ayr巴
八Rz
COS
—
cos
cosAzr——
R
R
R
图2中，R2
RX2
RY2
RZ2
，那么,
角度即为
Axr
Rx
arccos—
Ayr
Ry
arccos——
ARz
Azrarccos—
R
R
R
三、互补滤波算法
加速度计是极易受外部干扰的传感器（如机械振动），但是测量值的误差不随时间的变化。
陀螺仪输出的角速度可以积分得到角度，动态性能好，受外部干扰小，但积分会造成误差累积。
可以看出，它们优缺点互补，结合起来才能有好的效果。
经典互补滤波算法（ClassicalComplementaryFilter）
[2]
。
经典互补滤波算法基本原理是充分利用加速度计提供的低频角度信号和陀螺仪提供的高频角速度信号，对加速度计进行低通滤波，对陀螺仪进行高通滤波，分别滤出相应的干扰信号，为两者的有效融合提供了很好的解决方案
图3经典互补滤波算法一频域形式原理图
⑵
融合后姿态角估计值为
?
sgKp
a
sKpssKp
其中，g为陀螺仪测量的角速度,度计测量的角度值，Kp为比例系数，
Fi（s）
a为加速
為为
高通滤波器，
先为低通滤波器。
F2（s）1Fi（s）
图4经典互补滤波算法一时域形式原理图
可得时域微分
[2]
对?
I」a进行反拉氏变换，sKpssKp
形式为
改进后的互补滤波算法（Explicit
ComplementaryFilter）
经典互补滤波算法实现简单，但是估算精度角度较低，文献［3］提出了一种改进算法（ECF），
在经典互补滤波算法的补偿环节加入积分器,
消除陀螺仪漂移常值误差，原理框图如下图
图5改进后的互补滤波算法
[2]
时域微分形式为
四、四旋翼飞行器姿态解算流程
四旋翼姿态解算整体流程框图如下
图5四元数表示的姿态解算整体流程框图⑷其中，Gyroscope为陀螺仪，_b是陀螺仪输出的测量数据，Accelerometer是加速度计，在
Airspeed模块以及fb作用下，加速度输出测量数据P。
第一，对加速度计测出来的数据进行归一化处理：
£；
处理團；
作为标准
第二，将重力加速度旋转到机体坐标，得到重力加速度在三个轴上的加速度分量，加速度。
0
C；0
1
cc
cs
s
22
q°q
2（q&2
2（qQ3
2（qQ3
2（q°q1
22
qoq1
0
0
1
s
c
s
22q2q3q°q3）q°q2）
qoq2）q2q3）
22q2q3
sscsssc
2加2
22
q。
qi
2（q°q
q°q3）
22q2q3
q2q3）
sscss
sccss
cc
2（q°q2
2qq3
22
q。
q
q^）
q°q）
22
q2q3
0
0
1
T
其中Cb为世界坐标系到机体坐标系的变换矩阵。
（文献［4］图中此处原文中有错误，已经在截图中进行了修改）
第三，归一化后的加速度计值与标准加速度做叉积运算，求得误差向量e；
第四，对误差进行滤波，改进后的互补滤波算法（ECF）可以表示为以下四元数形式：
q技p「）
kpek|e，
ev\?
第四，求解四元数微分方程&技p「），求得最新的qo,q1.q2.q3；
第五，代入以下公式求得姿态角。
arctan2（種弊J
qoqq2q3
arcsin2（qQ3q°q2）
arctan
q°qiq2q3
参考文献
[1]郭晓鸿，杨忠，陈喆，等.EKF和互补滤波器在飞行姿态确定中的应用[J].传感器与微系统,2011,30（11）:
149-152.
[2]傅忠云,朱海霞,孙金秋,等.基于惯性传感器MPU6050的滤波算法研究[J].压电与声光，
2015（2015年05）:
821-825,829.
[3]MahonyR,HamelT,PflimlinJM.
Nonlinearcomplementaryfiltersonthespecialorthogonalgroup[J].IEEETransactionsonautomaticcontrol,2008,53（5）:
1203-1218.
[4]EustonM,CooteP,MahonyR,etal.Acomplementaryfilterforattitude
estimationofafixed-wing
UAV[C]//IntelligentRobotsandSystems,2008.IROS2008.IEEE/RSJInternationalConferenceon.IEEE,2008:
340-345.
附程序：
voidIMUupdate（floatgx,floatgy,floatgz,floatax,floatay,floataz）
{
floatnorm;
floatvx,vy,vz;
floatex,ey,ez;
floatq0q0=q0*q0;
floatq0q1=q0*q1;
floatq0q2=q0*q2;
floatqlql=q1*q1;
floatq1q3=q1*q3;
floatq2q2=q2*q2;
floatq2q3=q2*q3;
floatq3q3=q3*q3;
if（ax*ay*az==O）
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=qOqO-q1q1-q2q2+q3q3;
//第三步：
在机体坐标系下做向量叉积得到补偿数据
ex=ay*vz-az*vy;
ey=az*vx-ax*vz;
ez=ax*vy-ay*vx;
//第四步：
对误差进行PI计算，补偿角速度
exlnt=exlnt+ex*Ki;
eylnt=eylnt+ey*Ki;
ezlnt=ezlnt+ez*Ki;
gx=gx+Kp*ex+exlnt;
gy=gy+Kp*ey+eylnt;
gz=gz+Kp*ez+ezlnt;
//第五步：
按照四元数微分公式进行四元数更新
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;
升级会员 