基于自适应UKF微型航姿系统噪声在线估计

刘　宇，刘　琼，周　帆，李云梅，向高林

(重庆邮电大学 光电信息感测与传输技术重庆市重点实验室，重庆 400065)



基于自适应UKF微型航姿系统噪声在线估计

刘宇，刘琼，周帆，李云梅，向高林

(重庆邮电大学 光电信息感测与传输技术重庆市重点实验室，重庆 400065)

摘要：针对先验噪声与系统真实噪声不符引起标准无迹卡尔曼(unscented Kalman filter，UKF)性能退化的情况，提出一种应用于非线性时变状态和参数联合估计的自适应UKF(adaptive unscented Kalman filter，AUKF)算法。根据新的协方差矩阵与相应估计值之间存在的误差，构建成本函数。采用梯度下降法进行在线预估，对噪声的协方差进行在线更新并反馈给标准的UKF。实验和仿真分析表明，与标准UKF相比，自适应UKF算法在精度上有较大的提高。对于时变噪声协方差不确定时，自适应UKF噪声在线估计的鲁棒性得到明显改善，验证了自适应UKF噪声在线估计模型的准确性和可行性。

关键词：无迹卡尔曼；自适应UKF；联合估计；成本函数；梯度下降算法；鲁棒性

0引言

由于微型航姿系统具有时变性和复杂性的特点，系统为了得到精确的姿态角信息，准确估计状态和参数成为一个关键。因为传统的噪声估计方法有诸如仅适应线性系统、跟踪突变的延时性等缺陷，无法满足当今的工程应用，所以，对航姿系统的噪声进行实时估计成为提高微型姿态仪精度的关键技术。近些年来，人们在在线建模领域取得了骄人的成绩，参考模型控制领域正在成为一个重要的研究方向[1]。在非线性系统，尽管扩展卡尔曼滤波(extend Kalman filter，EKF)[2]是运用比较普遍的估计器，但它在估计过程中仍然有一些缺陷，比如动态状态的可导性，对偏差和散度敏感。无迹卡尔曼(unscented Kalman filter，UKF)直接用非线性动态方程替代线性方程。UKF和EKF具有相同的复杂度，都是O(L3)阶[3]。由于UKF使用的非线性方程并没有线性化，因此，不需要估计雅可比行列式或者Hessians矩阵就能实现二阶精度(EKF的精度是一阶)。 然而UKF也是卡尔曼滤波器的扩充，它和卡尔曼滤波一样，只有在知道一些假定信息作为先验信息的情况下[4-6]才能实现优异的性能。这些先验信息包括：①精确的参考模型；②噪声分布的完整信息；③正确的初始条件。但是，因为先验信息会被时变和不确定的环境所影响，所以，不能保证很强的稳定性。克服上述缺陷的一种有效方式是使用自适应算法。国内外在自适应滤波领域已有许多研究[7-9]。Hu等[7]提出了限制卡尔曼滤波(Kalman filter，KF)记忆的方法，这种方式能根据最优化条件来自适应调整遗忘因子。Maybeck[8]设计一个自适应KF，通过最大似然估计的方法估计错误统计的协方差矩阵。Loebis等[9]使用了模糊逻辑技术来更新传感器噪声方差。但在实际中，每次要立即确定协方差的增量值是困难的。本文提出了自适应无迹卡尔曼滤波，它通过新的协方差矩阵与它相应的估计之间的误差建立了成本函数，然后将更新好的协方差反馈到该系统，实现系统噪声的在线估计。

1UKF滤波算法

离散系统动态空间模型可以描述为

(1)

(2)

(1)—(2) 式中：φk|k-1为tk-1—tk时刻的一步转移矩阵；Γk|k-1为系统噪声驱动阵；wk为系统噪声；ek为协方差的高斯白噪声；h(xk)为量测矩阵。(1)式和(2)式分别描述了系统的状态模型和测量模型，状态向量xk和测量向量yk所包含坐标向量的数量，可根据实际的微型航姿系统三维姿态数据确定。由于噪声对系统的影响，必须把噪声项添加到状态项。

增广状态表示为

(3)

增广状态的方差表示为

(4)

1)经过无损变换(unscented transform，UT)，k时刻的sigma点和权值选取。

(5)

(6)

(7)

(8)

(9)

(10)

2)计算预测估计值。

(11)

3)计算预测协方差。

(12)

4)计算测量值。

(13)

5)计算预测测量值。

(14)

(15)

7)计算卡尔曼增益。

(16)

8)更新误差协方差。

(17)

9)更新状态。

(18)

UKF其实是卡尔曼滤波的扩展，它以UT变换为核心，非线性传递均值和协方差。进行UT变换时可以采用不同的采样策略，包括确定相应权值，sigma点的个数以及位置。

2自适应UKF在线估计算法

2.1成本指标函数

因为新息来源于真实值的测量值，目前大多数算法都是基于新息的自适应滤波算法，也就是以最小新息(测量值和估计值之差)为目标，这只能得到最小新息，并不能反映系统的统计特性，本文主要是以最小化新息方差的真实值和估计值差作为自适应滤波的指标函数，用滤波更新中产生的真实值和估计值差值来更新滤波器中的参数。

(19)

(19)式中：N是估计窗口的尺度因子；vk是新息，也称为残差。vk可以描述为

(20)

(20)式中：yk是在k时刻的测量值；yk-1是UKF的估计值。可以通过标准UKF更新公式知道，新息方差的UKF估计值为

(21)

自适应参数R的调整标准是使指标函数最小，因ΔSk对系统统计特性的变化更加敏感，自适应的成本指标函数表示为

(22)

2.2自适应UKF

实际上，方差矩阵Q和R是决定精度和稳定性的因素，因此，选择协方差的对角元素去自适应更新过程噪声Q。不失一般性，R也可以通过该方法更新。Whittake准则是一种比较常规的自适应参数更新算法，方法简单，计算量少。此方法就是基于梯度下降算法，将成本函数下降最快的方向作为参数更新的方向。即

(23)

(24)

根据(23)式，可以得到递归机制

(25)

(25)式中，T为恒定时间间隔。(25)式需要结合常规的UKF来实现自适应UKF。由(22)式可得

(26)

(27)

根据(21)式，可以推导出

(28)

1)初始化。

(29)

sigma点的导数为

(30)

(31)

2)测量更新。

(32)

(32)式中，h为转化矩阵。

3)梯度预测。

(33)

(34)

2.3自适应UKF算法复杂度和收敛性分析

对于一个n维系统，标准UKF与自适应UKF的计算量在同一个数量阶，它们的算法复杂度都是O(n3)，自适应UKF算法收敛性可以根据滤波器收敛性判别依据进行讨论，如果滤波器发散，误差协方差阵无界，这时实际估计误差往往比理论预计的误差大很多倍。这时理论预测误差远远小于实际误差，根据这样的原理，可以利用信息序列构建一个变量，来判断滤波器是否发散。发散的判别依据为

(35)

(35)式的左边是信息的平方和，反映的是实际估计误差，它只能通过测试数据得到；右边与信息序列的迹相关，反映的是理论预计误差。当不等式成立时，滤波器收敛，如果不等式不成立，就说明理论误差远远小于实际误差，说明该滤波器发散，这时要及时地调整新息协方差阵。

3系统试验及分析

3.1仿真平台

本论文提出的自适应UKF运用在自主研制的微型航姿系统中，如图1所示。微型航姿系统主要由三轴陀螺仪、三轴加速度和三轴磁强计组成，根据惯性导航的相关理论，采用东北天导航坐标系描述载体姿态信息重要模型。

3.2微型航姿系统的测量的系统结构

本文提出的是基于自适应UKF噪声的在线估计，系统利用加速度和磁力计计算出姿态，将陀螺的输出反馈到动态解算过程中，利用自适应UKF实现对磁力计和加速度的噪声估计，同时利用磁力计和加速度计对陀螺的累积误差进行在线预估。微型航姿系统的数学模型如图2所示。

图1　自主研制的微型姿态仪Fig.1　Self-developed mini-AHRS

图2　微型航姿系统测量的系统结构Fig.2　Measurement system structure of mini-AHRS

3.3微型姿态仪的状态模型

在惯性坐标系中，定义姿态角为(φ，θ，ψ)，其中，φ为航向角，θ为俯仰角，ψ是横滚角。通过姿态角的导数，对磁力计和加速度动态测量的误差进行补偿，此时，在对自适应UKF的状态模型进行设计时，采用陀螺仪的数据。在k+1时刻的姿态角可以通过前时刻角速度在采样时间内的积分得到，表示为

(36)

(37)

(36)式的左边是通过磁力计和加速度进行解算的姿态角，把这个解算出来的姿态角作为自适应UKF算法的观测值。观测模型表示为

(38)

由于时间的积累，根据UKF的状态方程，将前一个时刻计算的结果作为下一个时刻角速度，对其做一个简单的预估计，但是估计中会带来积累误差，表示为

(39)

(40)

4仿真验证

为了验证自适应UKF算法的效果，实验对自主研制的微型航姿系统进行数据采集，将算法运用到该系统中。采样时间为30 s，采样频率为50 Hz。为了验证该算法的优越性，将自适应UKF算法和普通的UKF算法进行对比，分别做了静态和动态实验。由于横滚角和俯仰角在算法的原理上一样，为避免赘述，所以本实验主要对航向和俯仰进行验证。期望误差为0时，静态和动态俯仰角和航向角偏差如图3所示。图3中曲线表明：静态实验中，标准UKF值呈现出在期望值附近振荡，自适应UKF值呈现出与期望值逼近的趋势。采用标准UKF时，航向角偏差为±1°，静态俯仰角偏差为±0.5；采用自适应UKF时，静态航向角偏差为0.3°，俯仰角偏差为0.2°。动态实验中，采用标准UKF时，航向角偏差为±2°，俯仰角偏差为±0.75°；采用自适应UKF时，航向角偏差在0.8°，俯仰角偏差在0.4°。实验验证表明，自适应UKF算法在动静态实验中都比标准UKF算法对误差估计的精度高。UKF不能得到较好的估计性能；自适应UKF算法可以很好地在线估计噪声。

图3　期望误差为0时动静态姿态角偏差Fig.3　Dynamic and static attitude angle deviation

本实验采用加速度计，磁强计和陀螺仪，静态时，直接把微型航姿系统放在双轴转台上。动态采用的是摇摆实验，标准UKF和自适应UKF算法对传感器数据融合后，不同期望误差下静态动态仿真结果如图4所示。由图4可知，静态实验中，标准UKF值与自适应UKF值都在期望值上下振荡，但自适应UKF值与期望值非常逼近。图4表明，自适应UKF较标准UKF对噪声的在线估计精度高，在静态的自适应程度较动态高。对噪声的精确估计后，使微型航姿系统的静态俯仰角精度为±0.2°，航向角精度为±0.3°，动态俯仰角精度为±0.4°，动态航向角精度为±0.8°，已经满足了工程应用的需要。

5结束语

本论文针对传统UKF由于假定的先验噪声与实际系统噪声不符导致精度下降，提出了一种基于自适应UKF(adaptive unscented Kalman filter，AUKF)算法，构建以新的协方差矩阵与相应估计值误差为成本函数，将更新的协方差反馈到实测系统，对自主研制的微型航姿系统的噪声进行在线估计。实验仿真表明，动静态精度都有较大的提高，验证了在线估计模型的正确性和可行性，具有很好的工程应用价值。

由图4a及图4b可知，静态实验中，标准UKF值及自适应UKF值都在期望值上下振荡，但自适应UKF值与期望值非常逼近。

参考文献：

[1]徐慧娟，吴美平，罗兵.EKF和UKF INS/GPS组合导航中的应用分析[J].航天控制，2006，26(6)：7-10.

XU Huijuan, WU Meiping, LUO Bing. Analysis of Extended and Unscented Kalman Filtering for INS/GPS Integration[J]. Aerospace Control,2006,26(6)：7-10.

[2]张欣，白越，赵常均.多旋翼姿态解算中的改进自适应扩展Kalman算法[J].光学精密工程，2014，22(12):3385-3390.

ZHANG Xin, BAI Yue, ZHAO Changjun. Improved adaptive extended Kalman algorithm for attitude estimation of multi-rotor UAV[J].Optics and Precision Engineering, 2014,22 (12)：3385-3390.

[3]JULIER S, UHLMANN J.Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE, 2004，92(3):401- 421.

[4]周卫东， 乔相伟， 吉宇人.基于新息和残差的自适应UKF 算法[J]. 宇航学报，2010,31(7):1798-1804.

ZHOU Weidong, QIAO Xiangwei,JI Yuren.An Innovation and Residual-Based Adaptive UKF Algorithm[J]. Journal of Astronautics，2010,31(7)：1798-1804.

[5]刘锡祥，徐晓苏，冯爱国.有限记忆量测噪声在线估计的Kalman改进算法[J].东南大学学报:自然科学版，2010,40(4)：766-770.

LIU Xixiang, XU Xiaosu, FENG Aiguo.Improved algorithm for limited memory on-line estimating Kalman filter [J].Journal of Southeast University:Natual Science Edition，2010, 40(4)：766-770.

[6]乔相伟，周卫东，吉宇人.用四元数状态切换无迹卡尔曼滤波器估计的飞行器姿态[J].控制理论与应用，2012,29(1):97-103.

QIAO Xiangwei, ZHOU Weidong, JI Yuren. Aircraft attitude estimation based on quaternion stateswitching unscented Kalman filter[J].Control Theory & Applications，2012,29(1)：97-103.

[7]HU C W,CHEN W, CHEN Y Q.Adaptive Kalman filtering for vehicle navigation[J].Journal of Global Positioning Systems，2003,2(1):42- 47.

[8]PESONEN U,STECK J，ROKHSAZ K. Adaptive neural network inverse controller for general aviation safety[J].Journal of Guidance, Control, and Dynamics，2004,27(3):434- 443.

[9]LOEBIS D，SUTTON R, CHUDLEY J.Adaptive tuning of a Kalman filter via fuzzy logicfor an intelligent AUV navigation system[J].Control Engineering Practice，2004,12(12):1531-1539.

Online noise estimation of mini-AHRS based on adaptive UKF algorithm

LIU Yu, LIU Qiong, ZHOU Fan, LI Yunmei, XIANG Gaolin

(Chongqing Municipal Level Key Laboratory of Photoelectronic Information Sensing & Transmitting Technology,Chongqing University of Posts and Telecommunications, Chongqing 400065,P.R.China)

Abstract:Considering that the prior noise of a normal unscented Kalman filter does not agree with its real behavior，an adaptive unscented Kalman filter algorithm applied to nonlinear joint estimation of both time-varying states and parameters is proposed. Firstly, a cost function is built based on the error between the covariance matrices of innovation and their corresponding estimations. Then the gradient descent method for online forecast is used. Finally, the noise covariance is online updated, the updated covariance feedback to the standard UKF.Experimental and simulation analysis indicates that adaptive UKF provides higher estimation precision than the nomal UKF algorithm. For time-varying noise covariance is uncertain,adaptive UKF online noise estimation robustness is improved significantly, and the accuracy and feasibility of online adaptive UKF noise estimation model is verified.

Keywords：unscented Kalman filter; adaptive unscented Kalman filter; joint estimation;cost function;gradient descent algorithm;robustness

DOI：10.3979/j.issn.1673-825X.2016.03.001

收稿日期：2015-06-23

修订日期：2016-02-29通讯作者：刘琼739767483@qq.com

基金项目：国家自然科学基金(51175535)；MEMS振动传感与微姿态组合测井技术国际联合研究中心科技平台与基地建设(cstc2014gjhz0038)

Foundation Items：The National Natural Science Foundation of China(51175535);The Science and Technology Platform and Construction of International Joint Research Center(cstc2014gjhz0038)

中图分类号：TN212

文献标志码：A

文章编号：1673-825X(2016)03-0285-06

作者简介：

刘宇(1972-)，男，重庆人，教授，博士，硕士生导师，长期从事微振动惯性传感器、光机电一体化科学技术研发和MEMS惯性传感技术研究。E-mail：liuyu@cqupt.edu.cn。

刘琼(1989-)，男，湖南衡阳人，硕士研究生，主要从事MEMS惯性传感技术研究。E-mail：739767483@qq.com。

(编辑：王敏琦)