基于互补滤波的飞行器姿态解算Word文档下载推荐.docx
《基于互补滤波的飞行器姿态解算Word文档下载推荐.docx》由会员分享,可在线阅读,更多相关《基于互补滤波的飞行器姿态解算Word文档下载推荐.docx(13页珍藏版)》请在冰点文库上搜索。
陀螺仪就是测量上面定义角度的变化率,换句话说,它会输出一个与上面这些角度变化率线性相关的值。
加速度计工作原理介绍(摘自网络)大多数加速度计可归为两类:
数字和模拟。
数字加速度计可通过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
1.89V
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
AdcRy
Ry
Vref
VzeroG
Rx0.240.47850.5g
Ry0.380.47850.79g
Rz0.160.47850.33g
综上,可以把以上步骤用以下公式表达
AdcRxVzeroG
AdcRzVzeroG
Rz
1023
现在我们得到了惯性力矢量的三个分量,如果设
认为这个方向就是重力矢量的方向。
(自此明白
了文献[1]中所说只使用加速度计获得的角度是
基于飞行器在匀速飞行或静止的条件下得到的)
我们感兴趣的角度是向量R和X,Y,Z轴之间的夹角,那就令这些角度为Axr,Ayr,Azr。
观察由R和Rx组成的直角三角形
Axr
Ayr巴
八Rz
COS
—
cos
cosAzr——
R
图2中,R2
RX2
RY2
RZ2
,那么,
角度即为
arccos—
Ayr
arccos——
ARz
Azrarccos—
三、互补滤波算法
加速度计是极易受外部干扰的传感器(如机械振动),但是测量值的误差不随时间的变化。
陀螺仪输出的角速度可以积分得到角度,动态性能好,受外部干扰小,但积分会造成误差累积。
可以看出,它们优缺点互补,结合起来才能有好的效果。
经典互补滤波算法(ClassicalComplementaryFilter)
[2]
。
经典互补滤波算法基本原理是充分利用加速度计提供的低频角度信号和陀螺仪提供的高频角速度信号,对加速度计进行低通滤波,对陀螺仪进行高通滤波,分别滤出相应的干扰信号,为两者的有效融合提供了很好的解决方案
图3经典互补滤波算法一频域形式原理图
⑵
融合后姿态角估计值为
?
sgKp
a
sKpssKp
其中,g为陀螺仪测量的角速度,度计测量的角度值,Kp为比例系数,
Fi(s)
a为加速
為为
高通滤波器,
先为低通滤波器。
F2(s)1Fi(s)
图4经典互补滤波算法一时域形式原理图
可得时域微分
对?
I」a进行反拉氏变换,sKpssKp
形式为
改进后的互补滤波算法(Explicit
ComplementaryFilter)
经典互补滤波算法实现简单,但是估算精度角度较低,文献[3]提出了一种改进算法(ECF),
在经典互补滤波算法的补偿环节加入积分器,
消除陀螺仪漂移常值误差,原理框图如下图
图5改进后的互补滤波算法
时域微分形式为
四、四旋翼飞行器姿态解算流程
四旋翼姿态解算整体流程框图如下
图5四元数表示的姿态解算整体流程框图⑷其中,Gyroscope为陀螺仪,_b是陀螺仪输出的测量数据,Accelerometer是加速度计,在
Airspeed模块以及fb作用下,加速度输出测量数据P。
第一,对加速度计测出来的数据进行归一化处理:
£
;
处理團;
作为标准
第二,将重力加速度旋转到机体坐标,得到重力加速度在三个轴上的加速度分量,加速度。
C;
0
1
cc
cs
s
22
q°
q
2(q&
2
2(qQ3
2(q°
q1
qoq1
c
22q2q3q°
q3)q°
q2)
qoq2)q2q3)
22q2q3
sscsssc
2加2
q。
qi
q
q3)
q2q3)
sscss
sccss
q2
2qq3
q^)
q)
q2q3
T
其中Cb为世界坐标系到机体坐标系的变换矩阵。
(文献[4]图中此处原文中有错误,已经在截图中进行了修改)
第三,归一化后的加速度计值与标准加速度做叉积运算,求得误差向量e;
第四,对误差进行滤波,改进后的互补滤波算法(ECF)可以表示为以下四元数形式:
q技p「)
kpek|e,
ev\?
第四,求解四元数微分方程&
技p「),求得最新的qo,q1.q2.q3;
第五,代入以下公式求得姿态角。
arctan2(種弊J
qoqq2q3
arcsin2(qQ3q°
arctan
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;