改进型自适应无迹卡尔曼姿态算法*

侯江宽,马 珺*,贾华宇

(1.太原理工大学物理与光电工程学院,山西 晋中 030600;2.太原理工大学信息工程学院,山西 晋中 030600)

项目来源:山西省自然科学基金项目(2015011050)

2017-02-28修改日期2017-06-18

改进型自适应无迹卡尔曼姿态算法*

侯江宽1,马 珺1*,贾华宇2

(1.太原理工大学物理与光电工程学院,山西 晋中 030600;2.太原理工大学信息工程学院,山西 晋中 030600)

提出了一种改进型自适应无迹卡尔曼滤波姿态算法,能够有效的解决MEMS陀螺仪的漂移和噪声问题,同时减小运动加速度对加速度计的影响。将改进的自适应Sage-Husa算法与无迹卡尔曼滤波器相结合,使量测噪声统计特性在线更新,提高系统的抗干扰能力,避免扩展卡尔曼滤波的线性化误差,可以得到精确的全姿态角。每次迭代只更新3个欧拉角,提高了系统的解算速度。飞行实验和分析表明:改进算法能够有效的提高姿态解算精度,收敛速度快,自适应能力强,稳定可靠,具有较强的鲁棒性,在干扰消失时能够快速得到准确姿态角。

传感器;姿态解算;无迹卡尔曼滤波器;自适应Sage-Husa算法;多旋翼飞行器

获取准确的姿态是无人飞行器实现自主飞行的前提条件[1],由于MEMS(Micro Electro Mechanical Systems)传感器的高集成度、低成本,在微型飞行器上大量使用。多旋翼飞行器上的惯性测量单元主要由MEMS三轴陀螺仪、三轴加速度计、三轴磁强计组成。但是MEMS陀螺仪存在漂移问题,会导致累积误差,因此需要对陀螺仪输出的数据进行有效的处理才能获得稳定可靠的姿态信息。

目前常用的姿态解算方法有Robert Mahony的互补滤波姿态解算算法[2]及其改进算法[3]、Sebastian Madgwick提出的基于梯度下降法的姿态解算算法[7]、基于卡尔曼滤波器的姿态解算算法[8]以及粒子滤波姿态解算算法[9]。互补滤波算法是比较实用的一种算法,适用于对姿态精确度要求不高的小型飞行器。互补滤波姿态解算的改进主要集中于滤波器系数的自适应调节[5],以提高算法对于噪声的补偿,这种自适应调节主要是结合了模糊算法,精度并不高。Madgwick将互补滤波的状态更新方程由DCM(Direction Cosine Matrix)矩阵变为四元数微分方程,大大提高了互补滤波算法的解算速度。Kalman滤波是目前较为适合多传感器姿态融合的算法,但常规Kalman滤波算法需要系统噪声和量测噪声的统计特性已知[14]。然而在多旋翼飞行器的多传感器融合中,系统噪声和量测噪声的统计特性往往是未知的且时变的,这会使常规Kalman滤波估计精度降低,失去算法最优性,甚至导致滤波发散。扩展卡尔曼滤波姿态解算算法是对非线性函数进行线性化,但是在线性化过程中必须求解非线性函数的Jacobi矩阵,这给嵌入式微处理器带来了大量的计算负载,降低了系统的实时性,尤其对于复杂模型的系统,比较复杂且容易出错。另外,扩展卡尔曼滤波算法在系统达到稳定状态时,会丧失对于系统突变的跟踪能力,将导致状态估计不准甚至发散[11]。针对扩展卡尔曼滤波算法存在的问题,很多文献引入了高斯QEKF算法、高斯IEKF算法、强跟踪EKF算法[11]以及各种改进算法[12],但是这些算法都是次优滤波。

本文将自适应算法和无迹卡尔曼滤波相结合,使用确定的Sigma点集来描述非线性函数的统计特性。Sigma点集经过非线性函数传播之后,后验均值和方差可以达到2阶精度而扩展卡尔曼滤波算法只能达到1阶精度。不像扩展卡尔曼滤波算法对于非线性函数的近似,无迹卡尔曼滤波是对状态随机变量的近似,使用了真实非线性模型,由于不需要像二次滤波那样计算Hession和Jacobian矩阵,提高了解算速度。由于加速度计和罗盘良好的低频特性,使用加速度以及罗盘得到的姿态角作为观测量来修正陀螺仪得到姿态角,可以有效抑制陀螺仪漂移和量化误差。得益于自适应无迹卡尔曼滤波器响应速度快的特点,在导航坐标系补偿运动加速度对加速度计影响的前提,该算法可以可以进一步减少运动加速度对于加速度计的影响。另外该算法可以在线实时更新系统量测噪声,所以有更强的鲁棒性和更高的解算精度。

1 非线性姿态解算系统建模

本文使用的是NED(北、东、地)右手坐标系。姿态解算系统离散时间状态空间模型如式(1)所示:

(1)

DCM矩阵微分方程由于需要求解9个变量,且涉及大量三角运算,会增加处理器运算负载。使用欧拉角微分方程更新姿态角不需要重正交化,可以简化运算过程。因此我们使用欧拉角法来预测3个姿态角,非线性函数f(Xk-1)可以写为式(2):

(2)

式中:w=[wxwywz]T为三轴陀螺仪得到的机体角速度。当等式俯仰角为90°时方程具有奇异性,横滚角与俯仰角变得不确定。在实际工程中,通常将俯仰角限制在一定的范围以防止方程退化,如式(3)所示:

(3)

观测方程的量测向量Z可以由MEMS三轴加速度计和磁强计直接计算。式(1)中的观测方程可以写为式(4):

(4)

由式(5)描述的坐标系变换矩阵以及式(6)可以得到由加速度描述的横滚角和俯仰角,如式(7)所示:

(5)

(6)

(7)

式中:Ab=[axayaz]T表示三轴加速度计的输出值,An=[0 0g]T为重力加速度在参考坐标系下的向量表示。

偏航角可以由三轴磁强计获得,如式(8)所示:

Zψ=arctan(mx/my)+Δψ

(8)

式中:Mb=[mxmymz]T为磁强计在x轴、y轴和z轴的输出值,Δψ为真北与磁北的夹角。机体在转动过程中由于横滚角和俯仰角的变化,每次姿态解算之后需要对mx和my进行修正,如式(9)所示:

(9)

2 改进自适应无迹卡尔曼滤波算法

由上一部分得到的状态空间方程,我们可以利用加速度计和磁强计得到的横滚角、俯仰角以及偏航角来修正由陀螺仪更新后的姿态角。无迹卡尔曼滤波融合算法可以利用加速度计提供的准确的静态角度补偿陀螺仪的横滚角和俯仰角的漂移误差,通过引入自适应算法在线估计测量噪声,可以有效降低对于加速度计震动噪声的影响。下面介绍了完整的改进型无迹卡尔曼滤波算法的解算步骤。

①计算Sigma点集,由于状态变量为三维,所以取7个Sigma点。

(10)

②计算Sigma点集的权值

(11)

式中:下标m表示均值,c表示协方差,上标表示第几个采样点。参数λ=a2(n+κ)-n是缩放比例参数,用于降低总的预测误差。a的选取控制了采样点的分布状态,κ为另外一个缩放比例参数,用于保证矩阵(n+λ)P为半正定矩阵。参数β≥0是一个非负的权系数,可以用来合并方程中高阶项的动差。

③利用式(10)、式(11)获得Sigma点集及其对应的权值。

(12)

④计算Sigma点集的一步预测。

(13)

⑤计算系统状态量的一步预测及协方差矩阵,由Sigma点集的预测值加权求和得到,其中权值由式(11)得到。

(14)

(15)

⑥根据一步预测值,由UT变换产生新的Sigma点集。

(16)

⑦将⑥式预测的Sigma点集代入观测方程,得到预测的观测量。

(17)

⑧将⑦得到的Sigma点集的观测预测值,通过加权求和得到系统预测的均值及协方差。

(18)

(19)

(20)

(21)

(22)

⑨计算卡尔曼增益矩阵

(23)

⑩计算系统的状态更新和协方差更新

(24)

(25)

βk=βk-1(βk-1+b)-1,β1=I

(26)

式中:b=diag([bφbθbψ])为渐消记忆因子0

由于加速度计容易受到电机震动等的干扰。本文采用了低通FIR滤波器对加速度信号进行了处理,可以有效的抑制震动噪声。基于汉明窗的线性相位FIR滤波器的传递函数在Z域中可以用式(27)表示:

(27)

式中:N是滤波器阶数,hk是滤波器脉冲响应由截止频率决定。为了有效的获取加速度计的截止频率,本文使用离散傅里叶变换来计算谱振幅,如式(28)所示:

(28)

图1 姿态解算流程图

式中:DFT表示离散傅里叶变换,Macc(f)是频率(f)的谱振幅,macc(n)表示加速度计的采样序列。整个姿态解算流程如图1所示。

3 实验与分析

为了验证该姿态解算算法的有效性,选取了商用姿态板对算法进行了验证。该姿态板集成了MPU-6050芯片包括MEMS三轴加速度计和MEMS三轴角速度计,以及MEMS三轴磁强计HMC5883L芯片和一个气压高度计BPM180。姿态板使用了STM32F103T8作为处理核心,该处理器基于ARM CortexTM-M3内核,通过板子上的8M晶体和PLL,STM32可以运行在最高72M时钟。

实验使用本文提出改进型自适应无迹卡尔曼滤波算法(简记为:IAUKF)对姿态板采集的数据进行了姿态解算并与扩展卡尔曼滤波算法(EKF)以及Xsens 公司的MTi标准姿态曲线进行了对比。姿态板采集的9轴数据如图2所示。3种方法解算得到的横滚角、俯仰角以及偏航角分别如图3所示。

图2 传感器采集信号

从图3中可以看出,改进型自适应无迹卡尔曼滤波算法相比扩展卡尔曼滤波啊算具有更高的解算精度。为了进一步验证算法的数据融合效果,使用3D Optical Measurement系统对本文提出的算法(IAUKF)的静态(w<5°/s)和动态(w≥5°/s)RMS(Root-Mean-Square)误差进行了检测,实验结果如表1所示。

图3 IAUKF与EKF以及MTi解算效果对比

表1 静态和动态RMS误差

加速度计主要受到电机震动的影响,可以视为白噪声,为了验证观测噪声的在线更新效果,加入了白噪声作为干扰项,图4对比了自适应滤波前后的横滚角以及俯仰角姿态解算效果。

图4中可以看到,无迹卡尔曼滤波算法和改进型自适应滤波相结合以后在噪声扰动下更为稳定,具有更好的鲁棒性。为了验证本文提出的算法(IAUFK)的收敛速度将初始姿态与目标姿态设定偏差并与互补滤波(CF)以及扩展卡尔曼滤波(EKF)做了对比试验,如图5所示。

图4 噪声扰动下横滚角、俯仰角有无自适应对比

图5 3种算法的收敛性对比

从图中可以看到,扩展卡尔曼滤波算法收敛速度快于互补滤波算法,而本文提出的算法收敛速度比扩展卡尔曼滤波算法更快。图6展示了互补滤波算法、扩展卡尔曼滤波算法以及本文提出的算法的解算时间。

图6 3种算法运行1 000次每次需要的时间

从图6中可以看出,传统的互补滤波算法每次解算需要的时间最短,本文提出的算法和扩展卡尔曼滤波算法需要的时间近似。为了测试IAUKF算法对于陀螺仪漂移的补偿效果,将姿态板初始姿态置为零态,然后任意旋转姿态板,每隔3 min记录一次零态姿态角,补偿前后横滚角、俯仰角对比如图7所示。

从图7可以看出,在算法补偿前陀螺仪由于存在常值漂移、速率随机游走、角度随机游走以及各种噪声的影响,漂移严重。本文提出的算法可以有效的补偿陀螺漂移。

磁强计容易受到外部磁场干扰,偏航角误差会随着外部磁场变化,数据融合算法会对干扰起到一定的补偿作用,但是偏差依然存在。实验中加入一定强度外部磁场,偏航角产生一定误差,当外部磁场消失以后偏航角迅速得到修正,如图8所示。

图7 算法补偿前后横滚角、俯仰角

图8 磁强计收到干扰时的偏航角

4 结束语

本文提出的改进型自适应无迹卡尔曼滤波算法可以有效的补偿低成本MEMS传感器存在的姿态漂移以及噪声影响,可以有效提高姿态解算精度。该算法将改进型自适应Sage-Husa算法与无迹卡尔曼滤波算法相结合,可以在线更新状态方程的量测误差,同时避免了使用扩展卡尔曼滤波算法带来的高阶截断误差,然后使用加速度计以及磁强计作为量测信息校准陀螺仪解算欧拉角的误差,提高了基于惯性测量单元解算的准确性与可靠性。试验与仿真分析表明,该算法收敛速度快、解算速度与扩展卡尔曼滤波算法相当但是可以提供更高的姿态解算精度,静态RMS误差小于0.75°,动态RMS误差小于1.65°,可以满足对于高精度姿态控制的要求。但是,相对于传统互补滤波算法,该算法在解算速度上存在较大差距,接下来实验室将集中精力对于算法进行优化以提高算法的数据融合速度。

[1] 李迪,陈向坚,续志军. 增益自适应滑模控制器在微型飞行器飞行姿态控制中的应用[J]. 光学精密工程,2013,21(5):1183-1191.

[2] Mahony R,Hamel T,Pflimlin J M. Complementary Filter Design on the Special Orthogonal Group SO(3)[C]//Decision and Control,2005 and 2005 European Control Conference. Cdc-Ecc’05. IEEE Conference on,2006:1477-1484.

[3] Yanlin Y U,Jiakui Z U,Zeng G. The Research on Attitude Determination Algorithm Based on Complementary Filter for Unmanned Helicopter[J]. Helicopter Technique,2014.

[4] Wu J,Zhou Z,Chen J,et al. Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors[J]. IEEE Sensors Journal,2016,16(18):1-1.

[5] 王立,章政,孙平. 一种自适应互补滤波姿态估计算法[J]. 控制工程,2015(5):881-886.

[6] 孙金秋,游有鹏,傅忠云. 基于共轭梯度法和互补滤波相结合的姿态解算算法[J]. 传感技术学报,2014(4):524-528.

[7] Madgwick S O,Harrison A J,Vaidyanathan A. Estimation of IMU and MARG Orientation Using a Gradient Descent Algorithm.[C]//IEEE International Conference on Rehabilitation Robotics. IEEE Int Conf Rehabil Robot,2011:1-7.

[8] Zhang S,Yu S,Liu C,et al. A Dual-Linear Kalman Filter for Real-Time Orientation Determination System Using Low-Cost MEMS Sensors.[J]. Sensors,2016,16(2):264.

[9] Wang Y,Wai D,Tomizuka M. Steady-State Marginalized Particle Filter for Attitude Estimation[C]//ASME 2014 Dynamic Systems and Control Conference. American Society of Mechanical Engineers,2014:V002T25A002-V002T25A002.

[10] Wei G,Li J. Adaptive Kalman Filtering for the Integrated SINS/DVL System[J]. Journal of Computational Information Systems,2013,16(9):6443-6450.

[11] 宋康宁,丛爽,邓科,等. 自适应强跟踪卡尔曼滤波在陀螺稳定平台中的应用[J]. 中国科学技术大学学报,2015(1):17-22.

[12] Zou Bo,Zhang Hua,Jiang Jun. 多传感信息融合的改进扩展卡尔曼滤波定姿[J]. 计算机应用研究,2014,31(4):1035-1038.

[13] 王丁伟,祖家奎,黄海. 基于MAEKF算法的航姿参考系统设计[J]. 传感技术学报,2017,30(2):319-325.

[14] Hajiyev C,Soken H E. Robust Adaptive Kalman Filter for Estimation of UAV Dynamics in the Presence of Sensor/Actuator Faults[J]. Aerospace Science and Technology,2013,28(1):376-383.

[15] Makni A,Fourati H,Kibangou A Y. Adaptive Kalman Filter for MEMS-IMU Based Attitude Estimation Under External Acceleration and Parsimonious use of Gyroscopes[M]. 2014.

[16] 严恭敏,李四海,秦永元. 惯性仪器测试与数据分析[M]. 国防工业出版社,2012.

AnImprovedAdaptiveUnscentedKalmanFilterforAttitudeDetermination*

HOUJiangkuan1,MAJun1*,JIAHuayu2

(1.College of Physics and Optoelectronics,Taiyuan University of Science and Technology,Jinzhong Shanxi 030600,China; 2.College of Information Engineering,Taiyuan University of Science and Technology,Jinzhong Shanxi 030600,China)

An improved adaptive unscented Kalman filter for attitude determination is proposed,which can effectively solve the drift and noise problem of gyroscope and reduce the influence of motion acceleration on the accelerometer. Combining the improved adaptive Sage-Husa algorithm with the unscented Kalman filter,the statistic characteristic of the measurement noise is updated on-line,the anti-jamming capability of the system is improved,and the linearization error of the extended Kalman filter can be avoided. And we can get the precise Attitude angle. Only three Euler angles are updated each iteration which can improve the solution speed of the system. The flight experiment and analysis show that the improved algorithm can effectively improve the accuracy of attitude determination,improve convergence speed,strong adaptability,robustness and stability,and can get the accurate attitude angle quickly when the interference disappears.

sensor;attitude estimation;unscented Kalman filter;adaptive Sage-Husa algorithm;multi-rotor aircraft

TP27

A

1004-1699(2017)10-1518-07

10.3969/j.issn.1004-1699.2017.10.011

侯江宽(1991-),男,汉族,河北省石家庄市,硕士研究生,主要研究方向为导航与控制、智能控制,houjiangkuan0126@163.com;

马珺(1980-),女,汉族,山西省太原市人,博士研究生,副教授,硕士研究生导师,主要研究方向为新型传感器技术、智能控制理论,zymajun@126.com;

贾华宇(1977-),男,汉族,山西临汾人,博士研究生,副教授,硕士研究生导师,主要研究方向为数模混合集成电路设计。