1、 基金项目： 国家自然科学基金资助项目 (No. 51175535); MEMS 振动传感与微姿态组合测井技术国际联合研究中心 科技平台与基地建设 (cstc2014gjhz0038)； 作者简介： 刘宇 (1972-),男 (汉 )，重庆人，硕士生导师，教授。长期从事振动惯性传感器，光纤传感器，惯性导航及传感器件的研究。 刘琼 (1989-),男 (汉 )，湖南人，硕士研究生，学生。主要研究振动惯性传感器，惯性导航及传感器件的研究。 基于自适应 UKF 微型航姿系统噪声在线估计 刘宇 刘琼 周帆 李云梅 向磊 （重庆邮电大学 光电信息感测与传输技术重庆市重点实验室，重庆 400065） 摘
2、要 ： 针对先验噪声与系统真实噪声不符引起标准无迹卡尔曼（ UKF）性能退化的情况， 提出了一种 应用于非线性时变状态和参数联合估计 的 自适应 UKF（ AUKF）算法 。 该算法首先基于 新的协方差矩阵与相应估计 值 之间 存在 的误差， 构建 成本函数。 然后 为了最小化成本函数，采用梯度下降法进行在线预估 。 最后对 噪声的协方差 进行在线更新，将 更新 后 的协方差反馈给标准的 UKF， 使更新后的 理论协方差接近真实噪声 。 通过实验仿真分析， 自适应 UKF 相比 标准 UKF，静态精度提高了40%，动态精度提高了 30%，验证了自适应 UKF 噪声在线估计模型的准确性和可行性。
3、 在实际测试系统中，对于时变噪声协方差不确定 时 ， 自适应 UKF 噪声在线估计的鲁棒性得到明显改善 。 关键词 ： UKF ； AUKF；联合估计 ； 成本函数；梯度下降算法； 鲁棒性 中图分类号： TN212 文献标识码： A Noise On-line Estimation of Mini-AHRS Based On Adaptive UKF Algorithm Yu LIU, Qiong LIU， （ Chongqing Municipal Level Key Laboratory of Photoelectronic Information Sensing AUKF; Joint
4、estimation;Cost function; Gradient descent;Robustness 引言 由于微型航姿系统的复杂性和时变性，为了让该系统得到精确的姿态角 新息 ，准确的估计状态和参数是很关键的。但传统的估计方法具有明显的缺陷，如仅适用于线性时不变系统、对目标模型的强依赖性、对突变状态跟踪的滞后性等，这些都大大限制其工程应用。所以对航姿系统的噪声进行实时的估计。将成为提高微型姿态仪精度的关键技术。近些年来，人们在 在线建模领域取得了骄人的成绩，参考模型控制领域正在成为一个重要的研究方向 1。对于非线性系统来说， 普遍运用 的估计器就是扩展卡尔曼滤波 (EKF)2。尽管它被
5、广泛应用，但它在估计过程中仍然有一些缺陷 ， 比如 动态状态的可导性，对偏差和散度敏感。无迹卡尔曼滤波器(UKF)，使用非线性动态方程直接代替了线性方程。UKF 与 EKF 相比，具有相同的复杂度，都是 O(L3)阶 3。因为 UKF 使用的非线性方程并没有线性化，因此不需要估计雅可比行列式或者 Hessians 矩阵就能实现二阶精度 (EKF 的精度是一阶 )。 然而 UKF也 是卡尔曼滤波器的 扩充 ， 它和卡尔曼滤波一样，它只有在知道一些假定信息作为先验信息的情况下 4-6，才能实现优异的性能。这些先验信息包括：1)精确参考模型； 2)噪声分布的完整信息； 3)正确的初始条件。但是，因为
6、先验信息会被时变和不确定的环境所影响，所以不能保证很强的稳定性。克服上述提 缺陷 的一种有效方式是使用自适应算法。国内外在自适应滤波领域已有许多研究。 Hu et al 7提出了限制 KF 记忆的方法，这种方式能根据最优化条件来自适应调整遗忘因子。 Maybeck8设计一个自适应 KF，通过最大似然估计的方 法估计错误统计的协方差矩阵。 Loebis et al9使用了模糊逻辑技术来更新传感器噪声方差。但是在实际中，每次要立即确定协方差的增量值是困难的。 在 本 论文中，提出了自适应无迹卡尔曼滤波，它通过新的协方差矩阵和它相应的估计之间的误差建立了成本函数，然后将更新好的协方差反馈到该系统，实
7、现系统噪声的在线估计。 1 UKF滤波算法 1.1 UKF滤波的算法原理 假设微型航姿系统的离散方程和观测方程分别为： 11111 kkkkkkk wXX ， （ 1） kkk eXhZ )( （ 2） 可以知道， UKF算法中的观测方程也是非线性函数。状态向量 kX 和观测向量 Zk 所包含坐标向量数量，根据实际的微型航姿系统三维姿态数据确定。 1.2 标准 UKF的计算步骤 (1) 利用初始状态估计，设计最初的 2n+1个iSgam 点： UKF会将噪声项添加到状态项中，即状态扩维，并将驱动噪声阵 Q扩到 P阵中。进行状态扩充的原因是考虑噪声对系统的影响，进行扩维以后的状态变量和其他协方差
8、矩阵： TTkTkTkak ewxx （ 3） 000000kakkkPPQR（ 4） 设 akx 为 L 维列向量，显然 L=n+p+q。 (2) 利用过程模型变换这些 iSgam 点，即对状态变量进行 U 变换： ),(),/()/1( kkukkxfkkx ii （ 5） 式中 f 为 U 变换的具体方程，步骤如下： ,0kkXX （ 6） 1,1) ( 1 , . . . , )( 1 , . . . , 2 )k k ikikk imX m X i mX X m X i m m （ （ ）（ （ ）（ 7） )/(0 mW m （ 8） )-1()/( 20 mW c （ 9） 0
9、. 5 / ( ) , ( 1 , . . . , 2 )mciiW W m i m (10) 式中 m 为 状态参数个数 , 2 mm （ ） 为尺度因子，其中的 表示 iSgam 点到 kX 的距离，一般将这个数值设置为 110-4 ， 为常数，一般设为 0或者为 3m ， 用于融入预报向量 kX 的验前信息，对于高斯分布， =2 最优。 ()kimX （ ）是矩阵平方根的第 i 列，可以通过 Cholesky分解获得。 (3) 计算预测估计值 2 ()0 ( 1 / ) ( 1 / )n miiiX k k W k k (11) (4) 计算预测协方差 2 ()0 ( 1 / ) ( 1
10、 / ) ( 1 )n TciiiP k k W k k X k k (12) (5) 通过测量方程计算测量值 Z ( 1 / ) ( 1 / )i k k h X k k (13) (6) 计算预测测量值 2()0 ( 1 / ) ( 1 / ) ( 1 / ) ( 1 / ) ( 1 / ) n ciiiTikZ k k W Z k kZ k k Z k k Z k k R (14) 式中，假设预报残差向量为( 1 / ) ( 1 / )kiV Z k k Z k k 可以 知道向量确定等价权 P ，进而求出。 (7) 计算 ( 1/ )X k k 和 ( 1/ )Z k k 的协方差xz
11、P 2 ()0( 1 / ) ( 1 / ) ( 1 / ) ( 1 / ) ( 1 / ) n cx z i iiTiP k k W k kX k k Z k k Z k k (15) (8) 计算卡尔曼增益 1( 1) xz zzK k P P (16) (9) 更新误差协方差 ( 1 / 1 ) ( 1 / ) ( 1 ) ( 1 )TxzP k k P k k K k P K k (17) (10) 更新状态 ( 1 / 1 ) ( 1 / )( 1 ) ( 1 ) ( 1 / ) X k k X k kK k Z k Z k k (18) UKF其实就是状态估计方法的一种，同过联合估
12、计系统参数，将模型参数作为动态变量，将其添加到真实的状态矢量，合并成增广状态矢量，利用自适应 UKF对状态和模型参数进行预测估计。 UKF方法其关键技术就是对 sigma点采样策略，包括sigma点个数的确定，位置的确定，和相应权值的确定，目前大多数研究表明，采用 2n+1个 Sigma点，它们服从 x分布，并且当前的状态值与方差和样本均值相同。 2 自适应 UKF滤波算法 2.1 成本指标函数 本文主要是将最小化真实值和新息方差的差作为成本指标函数，将滤波更新中产生的真实值和估计状态值差值来更新滤波器的参数。 11 k Tk k ki k NS v vN (19) 式中 N是估计窗口的尺度因
13、子， kv 是新息， kv可以这样描述： |1k k k kv y y (20) 式子中， ky 和 1ky 是滤波和预测的真实值。相对来说，我们可以通过标准 UKF知道： 2, | 1 | 1 , | 1 | 10 ( ) ( )n cTk i i k k k k i k k k kiS w y y R （ 21） 然后提出自适应的成本指标函数，以尽量减少成本函数。 22( ) ( ) k k k kV tr S tr S S （ 22） 2.2 自适应 UKF 实际上，方差矩阵 Q和 R是决定精度和稳定性的因素，因此，选择协方差的对角元素去自适应更新过程噪声 Q。不失一般性， R也可以通过
14、该方法更新。Whittake准则是一种比较常规的自适应参数更新算法，方法简单，计算量少。此方法就是基于梯度下降算法，将负梯度方向作为参数更新的方向。即： mkkmk qVq （ 23） 式中， mkq 为时刻噪声方差阵第 m行的对角线元素； k是控制收敛速度的自适应调节率，其符合传统的随机估计条件 k kk kk 20 (24) 根据式（ 23），我们可以得到下面递归机制： TqVqq mkkmkmk .1 (25) 式中， T是取样时间或者恒定的时间间隔。式（ 25）需 要结合常规的 UKF来实现自适应 UKF。由式（ 22）可得： 22( ) ( )()kkkm m mk k kkkkkm
15、mVStr S trq q qSStr S Sqq (26) 由于 ()k k kkkm m m mk k k kS S SSSq q q q ，我们可以根据式 (19)， (20)可以推导出： 1|1|11|1|11 ()1 ( ( )( ) )TkTk k kkkm m mi k Nk k kkkk Tk k kmi k N kTkkk k k mkS v vvvNq q qyyyN qyyyq (27) 根据式（ 21），可以推导出： 2 |1, | 1 | 10|1, | 1 | 1 ()()l kkcTki i k k k kmmikkkki k k k k mkyS wyqqyyq
16、 (28) 为了计算 |1 mkk kyq，必须先求先 UKF 的滤波方程的导数。参照标准 UKF 算法，对应的可以得到 |1 mkk kyq的迭代计算公式如下所述： （ 1） 初始化 0000mkmkqPqx(29) sigma 点的导数 ,1 11,1 11, 1 , . . ., 1 , . . . 2ik kkim m mk k kik kkim m mk k kx Px n i nq q qPx n i n nq q q （ ）（ ）*, | 1 ,1,1*2| 1 , | 10*2| 1 , | 1 | 1 *, | 1 | 10*, | 1 | 1*, | 1 | 1|.( )
17、.( )( ) .( )i k k ikikmmkknk k i k kmimmikknk k i k k k kcTi i k k k km m mik k ki k k k k Ti k k k k mmkkfxxqqxwqqPxwxq q qxxqq mkQq (30) 测量更新： , | 1|1, | 1 | 1|1, | 1 | 1, | 1 , | 1.( ) , 1 , . .( ) , 1 , . .2|.i k kkki k k k kim m mk k kkki k k k kim m mk k ki k k i k kxmmkkPxn i nq q qPxn i n nq
18、 q qhxqq (31) 梯度预测： 2| 1 , | 102, , | 1 | 1, | 1 | 10, | 1 | 1, | 1 | 1( ) .( ).( )kknk k i k kmimminxy i k k k kcTi i k k k km m mik k ki k k k k Ti k k k k mmkkywqqP xwyq q qyxqq （ ）(32) 1111 1 1|1| 1 | 1|1()()k k k kk k k kk k k kk k k k k kkkkky y y yy y y ymmkkx y y yky y x y y ym m mk k kxykk
19、TTkkx y km m m mk k k kk k k kkkk k k km m mk k kPPPPqqKP P Pq q qPPPKPKq q q qxyxKy y Kqq q q (33) 3 系统试验及分析 3.1 仿真平台 在 本 论文中，提出的自适应 UKF 运用在自主研制的微型航姿系统（参见图 1），微型航姿系统主要是由三轴陀螺仪、三轴加速度和三轴磁强计组成，根据惯性导航的相关理论，采用东北天导航坐标描述 载体姿态新息 重要模型。 图 1 自 主研制的微型姿态仪 3.2 微型航姿系统的 测量 的系统 结构 本文提出的是基于自适应 UKF 噪声的在线估计，系统利用加速度和磁力计
20、计算出姿态，将陀螺的输出反馈到动态解算过程中，利用自适应 UKF实现对磁力计和加速度的噪声估计，同时也用磁力计和加速度计对陀螺的累积误差进行在线预估。微型航姿系统的数学模型如图 2 所示。 三 轴 加 速 度三 轴 磁 力 计三 轴 陀 螺 仪自 适 应 U K F融 合输 出反 馈图 2 微型航姿系统 测量 的 系统结构 3.3 微型姿态仪的状态模型 在惯性坐标系中，定义姿态角为 ），（ ，通过姿态角的导数，对磁力计和加速度动态测量的误差进行补偿，此时，在对自适应 UKF 的状态模型进行设计时，采用陀螺仪的数据，但是在 K+1 时刻姿态角通过该时刻角速度在采样时间内的积分，如式（ 34）：
21、100( , ) ( , ) ( , ) 00T T Tk R x R x R y (34) 100( , ) ( , ) ( , ) 00T T Tk R x R x R y , 11 s in ta n c o s ta n0 c o s s in0 s in / c o s c o s / c o sbxbybz kwww (35) 式 (35),等式左边是姿态角，它是通过磁力计和加速度进行解算的，把这个解算出来的姿态角作为自适应 UKF 算法的观测值，式（ 36）观测模型。 1111 0 0 0 0 00000000 1 1 0 0 0xyz (36) 由于时间的积累，根据 UKF 的
22、状态方程，将前一个时刻计算的结果作为下一个时刻角速度，对其做一个简单的预估计，但是估计中会带来积累误差，如式（ 37）。 1 ( , )kkx x w (37) 本论文将磁力计和加速度计解算后的姿态角作为 观测值，用估算值通过加权计算去求得 1Zk ，然后经过式 (38)去补偿陀螺长时间带来的积累误差 , M 是卡尔曼的增益。 1 1 1 1()k k k kx x M z Z (38) 式 (37)是陀螺仪的输出，通过状态转移可以描述其中的动态过程，能够补偿通过磁力计和加速度动态测量的误差，通过反馈，能够克服长时间陀螺的积累误差。 4 仿真验证 为了验证自 适应 UKF 算法的效果，实验对自
23、主研制的微型航姿系统进行数据采集，将算法运用到该系统中。采样时间选择 30s，采样频率为 50HZ。为了验证该算法的优越性，将自适应 UKF 算法和普通的 UKF 算法进行对比，分别做了静态和动态实验。由于横滚角和俯仰角在算法的原理上一样，为避免赘述，所以本实验主要对航向和俯仰进行验证。图 3 分别是静态 和动态 俯仰角和航向角偏差。图中的蓝线为标准 UKF 算法获得的误差曲线，图中的红线是自适应 UKF 算法获得的误差曲线，黑色的线是期望得到的值。图中曲线表明， 静态实验，采用标准 UKF 航向角偏差在 0.5 ，静态俯仰角偏差在 1 ，然而自适应 UKF 算法静态航向角偏差在0.3 ，俯仰
24、角偏差在 0.2 。动态实验，标准 UKF航向角偏差在 2 ，俯仰角偏差在 0.75 。 自适应UKF 航向角偏差在 1 ，俯仰角偏差在 0.4 。实验验证表明， 自适应 UKF 算法在动静态实验中都比标准的 UKF 算法对误差估计的精度 要 高。 UKF 不能得到较好的估计性能；而自适应 UKF 算法可以很快地克服噪声特性的改变。 200 400 600 800 1000 1200 1400-1- 0 . 500 . 51采样点（个）静态俯仰角偏差(/s)标准 U K F 方法自适应 U F K 方法期望偏差 :0(a) 静态俯仰角偏差 200 400 600 800 1000 1200 1
25、400-2- 1 . 5-1- 0 . 500 . 511 . 52采样点（个）静态航向角偏差(/s)标准 U K F自适应 U K F期望偏差 :0(b) 静态航向角偏差 200 400 600 800 1000 1200 1400-2- 1 . 5-1- 0 . 500 . 511 . 52采样点（个）动态俯仰角偏差(/s)标准 U K F 方法自适应 U K F 方法期望偏差 :0(c) 动态俯仰角偏差 200 400 600 800 1000 1200 1400-3-2-10123采样点（个）动态航向角偏差(/s)标准 U K F 方法自适应 U K F 方法期望偏差 :0(d) 动态
26、航向角偏差 图 3 动静态姿态角偏差 本实验是使用加速度计，磁强计和陀螺仪，静态是直接把微型航姿系统放在双轴转台上。动态采用的是摇摆实验，采用标准 UKF 和自适应 UKF 算法对传感器数据融合后，静态动态仿真结果如图 (4)所示：图中表明，自适应 UKF 较标准 UKF 对噪声的在线估计精度高，在静态的自适应程度较动态高。对噪声的精确估计后，使微型航姿系统的静态俯仰角精度在 20. ，航向角精度在 0.5 。动态俯仰角精度 80. ，动态航向角精度 1 。已经满足 了 工程应用的需要。 200 400 600 800 1000 1200 1400-1- 0 . 500 . 51采样点（个）静
27、态俯仰角(/s)标准 U K F 方法自适应 U K F 方法期望值 :0(a) 静态俯仰图 200 400 600 800 1000 1200 1400696 9 . 5707 0 . 571采样点（个）静态航向角(/s)标准 U K F 方法自适应 U K F 方法期望值 : 7 0(b) 静态航向角 200 400 600 800 1000 1200 1400- 1 0 0- 5 0050100采样点（个）动态俯仰角(/s)标准 U K F 方法自适应 U K F 方法(c) 动态俯仰角 200 400 600 800 1000 1200 1400- 1 0 0- 5 0050100采样
28、点（个）动态俯仰角(/s)标准 U K F 方法自适应 U K F 方法(d) 动态航向角 图 4 动静态姿态角 结论 本论文 针对传统 UKF 因为假定的先验噪声 与实际系统噪声不符导致精度下降，提出 了一种 基于自适应无迹卡尔曼滤波器 (AUKF)， 构建以 新的协方差矩阵与相应估计 值误差为成本函数，将更新的协方差反馈到实测系统，对自主研制的微型航姿系统的噪声进行在线估计， 实验仿真 表明 ， 静态精度提高了 40%，动态精度提高了 30%，验证了在线估计模型的正确性和可行性。有很好的工程应用价值。 参考文献 ： 1 徐慧娟 ,吴美平 ,罗兵 .EKF 和 UKF INS/GPS 组合导
29、航中的应用分析 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 张欣，白越，赵常均 .多旋翼姿态解算中的改进自适应扩展 Kalman 算法 J. 光学精密工程， 2014 Zhang Xin, Bai Yue, Zhao ChangJun.Improved adaptive extended K
30、alman algorithm for attitude estimation ofmulti-rotor UAVJ.Optics and Precision Engineering,2014,22(12),3385-3390 3 S. Julier, J. Uhlmann, “Unscented filtering and nonlinear estimation,”Proceedings of the IEEE, 2004. 4 周卫东 , 乔相伟 , 吉宇人 .基于新息和残差的自适应 UKF 算法 J 宇航学报， 2010 5 刘锡祥，徐晓苏，冯爱国 .有限记忆量测噪声在线估计的Kalm
31、an 改进算法 J.东南大学学报， 2010 6 乔相伟 , 周卫东 , 吉宇人 .用四元数状态切换无迹卡尔曼滤波器估计的飞行器姿态 J.控制理论与应用 .2012 7 C.-W. Hu, W. Chen, Y.-Q. Chen, “Adaptive Kalman filtering for vehicle navigation,” Journal of Global Positioning Systems, 2003 8 P. Maybeck, 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 system 10 Abdelkrim Nemra， Nabil Aouf Robust INS /GPS Sensor Fusion for UVA Localization Using SDRE Nonlinear FilteringJ IEEE Sensors Journal， 2010 EKF 和 UKF INS/GPS 组合导航 中的应用分析
