1、 基金项目:国家自然科学基金资助项目(No. 51175535); MEMS 振动传感与微姿态组合测井技术国际联合研究中心 科技平台与基地建设(cstc2014gjhz0038); 作者简介:刘宇(1972-),男(汉),重庆人,硕士生导师,教授。长期从事振动惯性传感器,光纤传感器,惯性导航及传感器件的研究。刘琼(1989-),男(汉),湖南人,硕士研究生,学生。主要研究振动惯性传感器,惯性导航及传感器件的研究。基于自适应 UKF 微型航姿系统噪声在线估计刘宇 刘琼 周帆 李云梅 向磊(重庆邮电大学 光电信息感测与传输技术重庆市重点实验室,重庆 400065)摘 要:针对先验噪声与系统真实噪声
2、不符引起标准无迹卡尔曼(UKF)性能退化的情况,提出了一种应用于非线性时变状态和参数联合估计的自适应 UKF(AUKF)算法。该算法首先基于新的协方差矩阵与相应估计值之间存在的误差,构建成本函数。然后为了最小化成本函数,采用梯度下降法进行在线预估。最后对噪声的协方差进行在线更新,将更新后的协方差反馈给标准的 UKF,使更新后的理论协方差接近真实噪声。通过实验仿真分析,自适应 UKF 相比标准 UKF,静态精度提高了 40%,动态精度提高了 30%,验证了自适应 UKF 噪声在线估计模型的准确性和可行性。在实际测试系统中,对于时变噪声协方差不确定时,自适应 UKF 噪声在线估计的鲁棒性得到明显改
3、善。关键词:UKF ;AUKF ;联合估计;成本函数;梯度下降算法;鲁棒性中图分类号:TN212 文献标识码: A Noise On-line Estimation of Mini-AHRS Based On Adaptive UKF AlgorithmYu LIU, Qiong LIU,(Chongqing Municipal Level Key Laboratory of Photoelectronic Information Sensing AUKF; Joint estimation;Cost function; Gradient descent;Robustness引言由于微型航姿系
4、统的复杂性和时变性,为了让该系统得到精确的姿态角新息,准确的估计状态和参数是很关键的。但传统的估计方法具有明显的缺陷,如仅适用于线性时不变系统、对目标模型的强依赖性、对突变状态跟踪的滞后性等,这些都大大限制其工程应用。所以对航姿系统的噪声进行实时的估计。将成为提高微型姿态仪精度的关键技术。近些年来,人们在在线建模领域取得了骄人的成绩,参考模型控制领域正在成为一个重要的研究方向 1。对于非线性系统来说,普遍运用的估计器就是扩展卡尔曼滤波(EKF )2。尽管它被广泛应用,但它在估计过程中仍然有一些缺陷,比如动态状态的可导性,对偏差和散度敏感。无迹卡尔曼滤波器(UKF),使用非线性动态方程直接代替了
5、线性方程。UKF 与 EKF 相比,具有相同的复杂度,都是 O(L3)阶 3。因为 UKF 使用的非线性方程并没有线性化,因此不需要估计雅可比行列式或者 Hessians 矩阵就能实现二阶精度(EKF 的精度是一阶)。 然而 UKF 也是卡尔曼滤波器的扩充,它和卡尔曼滤波一样,它只有在知道一些假定信息作为先验信息的情况下 4-6,才能实现优异的性能。这些先验信息包括:1)精确参考模型;2)噪声分布的完整信息;3) 正确的初始条件。但是,因为先验信息会被时变和不确定的环境所影响,所以不能保证很强的稳定性。克服上述提缺陷的一种有效方式是使用自适应算法。国内外在自适应滤波领域已有许多研究。Hu et
6、 al 7提出了限制 KF 记忆的方法,这种方式能根据最优化条件来自适应调整遗忘因子。Maybeck 8设计一个自适应 KF,通过最大似然估计的方法估计错误统计的协方差矩阵。Loebis et al9使用了模糊逻辑技术来更新传感器噪声方差。但是在实际中,每次要立即确定协方差的增量值是困难的。 在本论文中,提出了自适应无迹卡尔曼滤波,它通过新的协方差矩阵和它相应的估计之间的误差建立了成本函数,然后将更新好的协方差反馈到该系统,实现系统噪声的在线估计。1 UKF滤波算法1.1 UKF滤波的算法原理假设微型航姿系统的离散方程和观测方程分别为:111kkkwX,(1)kkehZ)((2)可以知道,UK
7、F算法中的观测方程也是非线性函数。状态向量 和观测向量 所包含坐标kXk向量数量,根据实际的微型航姿系统三维姿态数据确定。1.2 标准UKF 的计算步骤(1) 利用初始状态估计,设计最初的2n+1个点:UKF会将噪声项添加到状态项中,即状iSgam态扩维,并将驱动噪声阵Q扩到 P阵中。进行状态扩充的原因是考虑噪声对系统的影响,进行扩维以后的状态变量和其他协方差矩阵:TkTkaewx(3)0kaPQR(4)设 为 L 维列向量,显然 L=n+p+q。akx(2) 利用过程模型变换这些 点,即对状iSgam态变量进行 U 变换:),(/)/1(kuxfkxii(5)式中 为 U 变换的具体方程,步
8、骤如下:f,0kkX(6)(71, )(,.)2kkiki imX( ( )( ( ))/(0W(8))-1()/20c(9)(10).5/(),.,)mcii im式中 为状态参数个数, 为尺2( )度因子,其中的 表示 点到 的距离,一iSgakX般将这个数值设置为 , 为常数,一般10-4设为0或者为 , 用于融入预报向量 的验3mk前信息,对于高斯分布, 最优。=2是矩阵平方根的第 列,可以通过()kiX( ) iCholesky分解获得。(3) 计算预测估计值(11)2()0(1/1/)nmikWk(4) 计算预测协方差(12)2()0(1/)/)()n TciiiPkX(5) 通过
9、测量方程计算测量值 (13)Z(/)(1/)ikhk(6) 计算预测测量值(14)2()0(1/)/)/(1nciii TkkWZkZR式中,假设预报残差向量为可以知道向量确定等价(1/)(/)kiVkk权 ,进而求出。P(7) 计算 和 的协方差(1/)Xk(1/)ZkxzP(15)2()0(/)/)1/1ncxziii TWkZkk(8) 计算卡尔曼增益 (16)1()xzKP(9) 更新误差协方差(17)(1/)(1/)()()TxzPkkkKk(10) 更新状态(18)(/)(/)11XXKkZkUKF其实就是状态估计方法的一种,同过联合估计系统参数,将模型参数作为动态变量,将其添加到
10、真实的状态矢量,合并成增广状态矢量,利用自适应UKF对状态和模型参数进行预测估计。UKF方法其关键技术就是对sigma点采样策略,包括sigma 点个数的确定,位置的确定,和相应权值的确定,目前大多数研究表明,采用2n+1个Sigma点,它们服从x分布,并且当前的状态值与方差和样本均值相同。2 自适应UKF滤波算法2.1 成本指标函数本文主要是将最小化真实值和新息方差的差作为成本指标函数,将滤波更新中产生的真实值和估计状态值差值来更新滤波器的参数。(19)1kTkiNSv式中N是估计窗口的尺度因子, 是新息,k可以这样描述:kv(20)|1kkvy式子中, 和 是滤波和预测的真实值。相对来说,
11、我们可以通过标准UKF知道:2,|1|,|1|0()()nc TkikkikkiSwyyR (21)然后提出自适应的成本指标函数,以尽量减少成本函数。22()()kkkVtrStrS(22)2.2 自适应UKF实际上,方差矩阵Q和R是决定精度和稳定性的因素,因此,选择协方差的对角元素去自适应更新过程噪声Q。不失一般性, R也可以通过该方法更新。Whittake准则是一种比较常规的自适应参数更新算法,方法简单,计算量少。此方法就是基于梯度下降算法,将负梯度方向作为参数更新的方向。即:mkkqV(23)式中, 为时刻噪声方差阵第m 行的对角线kq元素;k是控制收敛速度的自适应调节率,其符合传统的随
12、机估计条件(24)kkk 20根据式(23),我们可以得到下面递归机制:(25)TqVqmkkm.1式中,T是取样时间或者恒定的时间间隔。式(25)需要结合常规的UKF来实现自适应UKF 。由式(22)可得:(26)22()()(k kkmmkkVStrtrqqStr由于 ,我们可()k kkmmSqq以根据式(19), (20)可以推导出:(27)1| |1|1|() ()TkTkkkmmmiNkikTmkSvqqy根据式(21),可以推导出:(28)2|1,|10|,|1|()()lkc TkiikkmmiikkkySwyqq为了计算 ,必须先求先 UKF 的滤|y波方程的导数。参照标准
13、UKF 算法,对应的可以得到 的迭代计算公式如下所述:|1mkkyq(1)初始化(29)0mkqPxsigma 点的导数,1 11, ,.,.2ik kk immik kk ixnnqqP ( )( )*,|1,1,*2| ,|10|1,|1*,|1|*,|*,|1|.().()().ik ikikmmniiikkikc Ti ikkmmikkikTikkfxqqxwPxxqqxmkQq (30)测量更新:(31),|1|1,|1|1|,|,|1,|1.(),.2ikkikk immkikk ii ixmmk kPxnnqqhqq梯度预测: 2|1,|10, ,|1,|1|,|1|,|1|()
14、.().knkikmiixyikc Ti ikkmikikTikkkwqqPxyyq( )(32)(33)111|1| |1|1()(kkkkkkkkkyymmx yyxymk xTTmmkk kmkPPqqKqKxyyq 3 系统试验及分析3.1 仿真平台在本论文中,提出的自适应 UKF 运用在自主研制的微型航姿系统(参见图 1),微型航姿系统主要是由三轴陀螺仪、三轴加速度和三轴磁强计组成,根据惯性导航的相关理论,采用东北天导航坐标描述载体姿态新息重要模型。图 1 自主研制的微型姿态仪3.2 微型航姿系统的测量的系统结构本文提出的是基于自适应 UKF 噪声的在线估计,系统利用加速度和磁力计计
15、算出姿态,将陀螺的输出反馈到动态解算过程中,利用自适应UKF 实现对磁力计和加速度的噪声估计,同时也用磁力计和加速度计对陀螺的累积误差进行在线预估。微型航姿系统的数学模型如图 2 所示。三轴加速度三轴磁力计三轴陀螺仪自适应 U K F融合输出反馈图 2 微型航姿系统测量的系统结构3.3 微型姿态仪的状态模型在惯性坐标系中,定义姿态角为 ,),( 通过姿态角的导数,对磁力计和加速度动态测量的误差进行补偿,此时,在对自适应 UKF 的状态模型进行设计时,采用陀螺仪的数据,但是在K+1 时刻姿态角通过该时刻角速度在采样时间内的积分,如式(34):(34)100(,)(,),TTkRxxRy1(,)(
16、,),00TTk,1sintacostanci/bxyzkw (35)式(35),等式左边是姿态角,它是通过磁力计和加速度进行解算的,把这个解算出来的姿态角作为自适应 UKF 算法的观测值,式(36)观测模型。(36)100xyz 由于时间的积累,根据 UKF 的状态方程,将前一个时刻计算的结果作为下一个时刻角速度,对其做一个简单的预估计,但是估计中会带来积累误差,如式(37)。(37)1(,)kkxw本论文将磁力计和加速度计解算后的姿态角作为观测值,用估算值通过加权计算去求得 ,1Zk然后经过式(38) 去补偿陀螺长时间带来的积累误差,是卡尔曼的增益。M(38)11()kkxMz式(37)是
17、陀螺仪的输出,通过状态转移可以描述其中的动态过程,能够补偿通过磁力计和加速度动态测量的误差,通过反馈,能够克服长时间陀螺的积累误差。4 仿真验证为了验证自适应 UKF 算法的效果,实验对自主研制的微型航姿系统进行数据采集,将算法运用到该系统中。采样时间选择 30s,采样频率为50HZ。为了验证该算法的优越性,将自适应 UKF算法和普通的 UKF 算法进行对比,分别做了静态和动态实验。由于横滚角和俯仰角在算法的原理上一样,为避免赘述,所以本实验主要对航向和俯仰进行验证。图 3 分别是静态和动态俯仰角和航向角偏差。图中的蓝线为标准 UKF 算法获得的误差曲线,图中的红线是自适应 UKF 算法获得的
18、误差曲线,黑色的线是期望得到的值。图中曲线表明,静态实验,采用标准 UKF 航向角偏差在,静态俯仰角偏差在 ,然而自适应0.51UKF 算法静态航向角偏差在 ,俯仰角偏差在0.3。动态实验,标准 UKF 航向角偏差在 ,0.2 2俯仰角偏差在 。自适应 UKF 航向角偏差在.75,俯仰角偏差在 。实验验证表明,自适应104UKF 算法在动静态实验中都比标准的 UKF 算法对误差估计的精度要高。UKF 不能得到较好的估计性能;而自适应 UKF 算法可以很快地克服噪声特性的改变。20 40 60 80 100 120 140-1-0.500.51个个个个个个个个个个(/s)个UKF个个个UFK个个
19、个:0(a) 静态俯仰角偏差20 40 60 80 100 120 140-2-1.5-1-0.500.511.52个个个个个个个个个个(/s)个UKF个个UKF个个:0(b) 静态航向角偏差20 40 60 80 100 120 140-2-1.5-1-0.500.511.52个个个个个个个个个个个个个(/s)个个UKF个个个个个UKF个个个个个个:0(c) 动态俯仰角偏差 20 40 60 80 100 120 140-3-2-10123个个个个个个个个个个(/s)个UKF个个个UKF个个个:0(d) 动态航向角偏差图 3 动静态姿态角偏差本实验是使用加速度计,磁强计和陀螺仪,静态是直接把
20、微型航姿系统放在双轴转台上。动态采用的是摇摆实验,采用标准 UKF 和自适应UKF 算法对传感器数据融合后,静态动态仿真结果如图(4)所示:图中表明,自适应 UKF 较标准UKF 对噪声的在线估计精度高,在静态的自适应程度较动态高。对噪声的精确估计后,使微型航姿系统的静态俯仰角精度在 ,航向角精度在20.。动态俯仰角精度 ,动态航向角精度0.58。已经满足了工程应用的需要。120 40 60 80 100 120 140-1-0.500.51个个个个个个个个(/s)个UKF个个个UKF个个个:0(a) 静态俯仰图 20 40 60 80 100 120 1406969.57070.571个个个
21、个个个个个(/s)个UKF个个个UKF个个个:70(b) 静态航向角20 40 60 80 100 120 140-10-5005010个个个个个个个个(/s)个UKF个个个UKF个(c) 动态俯仰角20 40 60 80 100 120 140-10-5005010个个个个个个个个(/s)个UKF个个个UKF个(d) 动态航向角图 4 动静态姿态角结论本论文针对传统 UKF 因为假定的先验噪声与实际系统噪声不符导致精度下降,提出了一种基于自适应无迹卡尔曼滤波器(AUKF),构建以新的协方差矩阵与相应估计值误差为成本函数,将更新的协方差反馈到实测系统,对自主研制的微型航姿系统的噪声进行在线估计
22、,实验仿真表明,静态精度提高了 40%,动态精度提高了 30%,验证了在线估计模型的正确性和可行性。有很好的工程应用价值。参考文献: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 IntegrationJ.A erospace Con trol,2006,26(6):7-10.2 张欣,白越,赵常均.多旋翼姿态解算中的改进
23、自适应扩展 Kalman 算法 J. 光学精密工程,2014Zhang Xin, Bai Yue, Zhao ChangJun.Improved adaptive extended Kalman algorithm for attitude estimation ofmulti-rotor UAVJ.Optics and Precision Engineering,2014,22(12),3385-33903 S. Julier, J. Uhlmann, “Unscented filtering and nonlinear estimation,”Proceedings of the IEEE
24、, 2004.4 周卫东, 乔相伟 , 吉宇人.基于新息和残差的自适应UKF 算法 J 宇航学报,20105 刘锡祥,徐晓苏,冯爱国.有限记忆量测噪声在线估计的 Kalman 改进算法 J.东南大学学报,20106 乔相伟, 周卫东 , 吉宇人.用四元数状态切换无迹卡尔曼滤波器估计的飞行器姿态J. 控制理论与应用 .20127 C.-W. Hu, W. Chen, Y.-Q. Chen, “Adaptive Kalman filtering for vehicle navigation,” Journal of Global Positioning Systems, 20038 P. Mayb
25、eck, Stochastic Models, Estimation, and Control, Vol. 2,Academic Press, New York, 2010.9 D. Loebis el at, “Adaptive tuning of a Kalman filter via fuzzy logic for an intelligent AUV navigation system10 Abdelkrim Nemra,Nabil Aouf Robust INS /GPS Sensor Fusion for UVA Localization Using SDRE Nonlinear FilteringJ IEEE Sensors Journal,2010EKF 和 UKF INS/GPS 组合导航中的应用分析