孟 陽，高社生，高兵兵，王 維

（西北工業(yè)大學 自動化學院，西安 710072）

基于UKF的INS/GNSS/CNS組合導航最優(yōu)數(shù)據(jù)融合方法

孟 陽，高社生，高兵兵，王 維

（西北工業(yè)大學 自動化學院，西安 710072）

為了提高INS/GNSS/CNS組合系統(tǒng)的導航精度，提出了一種基于UKF的多傳感器最優(yōu)數(shù)據(jù)融合方法。該方法具有兩層融合結構，第一層中，GNSS和CNS分別通過兩個局部UKF濾波器與INS組合，以并行的方式獲得INS/GNSS和INS/CNS子系統(tǒng)的局部最優(yōu)狀態(tài)估值；第二層中，根據(jù)線性最小方差準則推導出一種矩陣加權數(shù)據(jù)融合算法，對局部狀態(tài)估值進行融合，獲取系統(tǒng)狀態(tài)的全局最優(yōu)估計。提出的方法無需采用方差上界技術對局部狀態(tài)進行去相關處理，克服了聯(lián)邦卡爾曼濾波(FKF)及其優(yōu)化形式存在的缺陷。仿真結果表明，相比于FKF，提出方法的導航精度可至少提高36.4%；相比于UKF-FKF，其導航精度也可至少提高21.0%。

INS/GNSS/CNS組合系統(tǒng)；數(shù)據(jù)融合；聯(lián)邦卡爾曼濾波；無跡卡爾曼濾波

INS/GNSS/CNS組合導航系統(tǒng)大多采用聯(lián)邦卡爾曼濾波（Federated Kalman Filter，F(xiàn)KF）綜合處理各導航子系統(tǒng)的信息[1-3]。理論上，F(xiàn)KF具有全局最優(yōu)或近于最優(yōu)性，同時具有很強的容錯能力[4-5]。然而，F(xiàn)KF僅適用于線性系統(tǒng)，當組合導航系統(tǒng)具有非線性特征時，F(xiàn)KF的數(shù)據(jù)融合精度急劇下降。

無跡卡爾曼濾波（Unscented Kalman Filter，UKF）是一種新興的非線性濾波算法。與擴展卡爾曼濾波(EKF)相比，UKF濾波精度更高，收斂性更好；與粒子濾波(PF)相比，UKF實現(xiàn)簡單，計算量小[6]。因此，學者們將FKF與UKF相結合，用于處理具有非線性特征的INS/GNSS/CNS組合系統(tǒng)中的數(shù)據(jù)融合問題[7]。但是，由于INS/GNSS和INS/CNS子系統(tǒng)的狀態(tài)具有相關性，該算法與FKF類似，仍存在兩個主要問題[3]：子濾波器采用過程噪聲方差的上界而不是其本身進行濾波解算，給全局估計帶來一定的保守性；方差上界技術要求各子系統(tǒng)的狀態(tài)估計誤差在初始時刻不相關，這在工程實際中很難實現(xiàn)。

為了避免方差上界技術的使用對數(shù)據(jù)融合精度的影響，文獻[8]提出了一種基于信息矩陣的分散式卡爾曼濾波。然而，由于涉及過多局部與全局狀態(tài)估值的協(xié)方差陣的逆運算，該算法實時性差。在線性最小方差準則下，文獻[9]建立了一種適用于高斯系統(tǒng)的多傳感器最優(yōu)數(shù)據(jù)融合算法，然而該算法并未提供局部狀態(tài)估值間互協(xié)方差矩陣的計算方法。文獻[3]提出了一種基于矩陣加權的多傳感器數(shù)據(jù)方法，對文獻[9]中提出的算法進行了完善，然而，與文獻[8]和[9]所提出的算法類似，該方法僅可用于線性系統(tǒng)中的數(shù)據(jù)融合，不適用于具有非線性特征的組合導航系統(tǒng)。

本文提出了一種基于 UKF的多傳感器最優(yōu)數(shù)據(jù)融合算法，以解決具有非線性特征的INS/GNSS/CNS組合系統(tǒng)的信息融合問題。與前人算法不同，該算法具有兩層融合結構：在第一層中，GNSS和CNS分別通過兩個局部UKF濾波器與INS組合，構成INS/GNSS、INS/CNS子系統(tǒng)，并得到系統(tǒng)狀態(tài)的局部最優(yōu)估值；在第二層中，根據(jù)線性最小方差準則推導出一種矩陣加權數(shù)據(jù)融合算法，對局部狀態(tài)估值進行融合，進而獲取系統(tǒng)狀態(tài)的全局最優(yōu)估計。該數(shù)據(jù)融合算法無需采用方差上界技術對局部狀態(tài)進行去相關處理，克服了FKF及其優(yōu)化形式存在的缺陷。仿真結果驗證了該算法的有效性。

1 INS/GNSS/CNS組合系統(tǒng)數(shù)學模型

INS/GNSS/CNS組合導航的基本原理是利用GNSS提供的高精度速度、位置信息和CNS提供的高精度姿態(tài)信息，對INS的速度、位置和姿態(tài)誤差進行修正，最終實現(xiàn)載體連續(xù)的高精度導航。

1.1 系統(tǒng)狀態(tài)方程

將INS的誤差方程和慣性測量元件（IMU）的誤差方程相結合，建立了INS/GNSS/CNS組合系統(tǒng)的狀態(tài)方程。

選取東北天地理坐標系（g系）為導航坐標系（n系），記慣性坐標系為i系，地球坐標系為e系，載體坐標系為b系，INS模擬的實際計算平臺坐標系為n′系，則以Euler平臺誤差角表示的INS非線性姿態(tài)誤差模型和速度誤差模型為[10]

式中：φ=（φ,φ,φ）T和δvn=(δv,δv,δv )T分別

式中：vE和vN為載體沿東向和北向的速度；L和h為載體的緯度和高度，δL和δh為載體的緯度誤差和高度誤差；RM和RN分別為沿地球子午圈和卯酉圈的主曲率半徑。

INS的位置誤差模型為

通常情況下，陀螺常值漂移 εb和加速度計常值偏置?b可用隨機常數(shù)來描述[10]，即

定義系統(tǒng)狀態(tài)：

根據(jù)所選取的系統(tǒng)狀態(tài)，結合式(1)～(6)，可建立如下INS/GNSS/CNS組合系統(tǒng)狀態(tài)方程：

1.2 INS/GNSS子系統(tǒng)量測方程

將INS和GNSS輸出的位置與速度的差值作為量測信息，則INS/GNSS子系統(tǒng)的量測方程可表示為

1.3 INS/CNS子系統(tǒng)量測方程

星敏感器輸出的赤經(jīng)、赤緯和旋角經(jīng)過解算可以得到載體的高精度姿態(tài)信息。將INS和CNS輸出的姿態(tài)的差值作為量測信息，則INS/CNS子系統(tǒng)的量測方程可表示為

式中：H2(k)=[I3×3,03×12]；v2(k)為量測噪聲，對應于星敏感器的量測誤差。

2 基于UKF的最優(yōu)數(shù)據(jù)融合方法

如圖1所示，提出的數(shù)據(jù)融合方法具有兩層結構：第一層中，GNSS和CNS分別通過兩個局部濾波器與INS組合，以并行的方式獲得INS/GNSS和INS/CNS子系統(tǒng)的局部最優(yōu)狀態(tài)估值；第二層中，采用矩陣加權數(shù)據(jù)融合算法對得到的局部狀態(tài)估值進行融合，以獲取INS/GNSS/CNS組合系統(tǒng)的全局最優(yōu)狀態(tài)估計。此外，由式(8)可以看到，INS/GNSS/CNS組合系統(tǒng)的狀態(tài)方程具有非線性特征，因而在局部濾波過程中采用UKF來處理系統(tǒng)狀態(tài)的估計問題。

圖1 基于UKF的多傳感器最優(yōu)數(shù)據(jù)融合結構圖Fig.1 Multi-sensor optimal data fusion based on UKF

2.1 分布式局部狀態(tài)估計

對式(8)進行離散化處理，可得離散形式的INS/GNSS/CNS組合系統(tǒng)狀態(tài)方程：

式中：X(k)∈Rn為系統(tǒng)狀態(tài)；f(·)為非線性函數(shù)；w(k)為過程噪聲，通常假設為方差Q(k)≥0的零均值高斯白噪聲。

INS/GNSS和INS/CNS子系統(tǒng)的量測方程可表示為

式中：Z(k)∈Rmi為第i個局部濾波器的量測量；

iHi(k)為量測矩陣；vi(k)為量測噪聲，通常假設為方差Ri(k)＞0的零均值高斯白噪聲。這里假定兩個子系統(tǒng)的量測噪聲互不相關，且均與過程噪聲不相關，即

INS/GNSS和INS/CNS子系統(tǒng)利用UKF解算得到各子系統(tǒng)的局部最優(yōu)狀態(tài)估值。對于第i(i=1,2)個局部濾波器，UKF算法描述如下：

步驟2 時間更新。經(jīng)非線性函數(shù)f(·)變換后的Sigma點為

狀態(tài)預測值及其協(xié)方差可更新為

步驟3 量測更新。由于INS/GNSS和INS/CNS子系統(tǒng)的量測方程均是線性的，因此量測更新過程與卡爾曼濾波相同：

步驟4 返回步驟1，進行下一時刻濾波解算。

完成兩個分布式局部濾波器的并行計算之后，可以得到局部最優(yōu)狀態(tài)估值及其誤差協(xié)方差陣采用2.2節(jié)提出的矩陣加權數(shù)據(jù)融合算法將得到的局部狀態(tài)估值進行融合，即可獲得系統(tǒng)狀態(tài)的全局最優(yōu)估計。

2.2 系統(tǒng)狀態(tài)全局最優(yōu)估計

步驟2 根據(jù)式(23)計算系統(tǒng)狀態(tài)的全局最優(yōu)估值：

下面對式(21)～(23)進行推導：

1） 記第i(i=1,2)個局部濾波器的估計誤差為

因此，式(21)得證。

2） 借助于式(13)所選擇的 Sigma點，采用 UT變換的思想計算可得：

因此，式(22)得證。

其中，wi(i=1,2)為待確定的權值矩陣。

在線性最小方差準則下，系統(tǒng)狀態(tài)的全局最優(yōu)估計應滿足以下條件：

對式(23)求期望，條件(i)顯然滿足。

下面討論如何在條件(ii)下求取系統(tǒng)狀態(tài)的全局最優(yōu)估計。

由式(27)和(28)可得：

將式(31)代入其中，有：

3 仿真實驗及分析

通過Monte Carlo仿真對提出的基于UKF的最優(yōu)數(shù)據(jù)融合方法在INS/GNSS/CNS組合系統(tǒng)中的性能進行了評估，并與FKF和文獻[7]提出的基于UKF的FKF（記為UKF-FKF）進行了比較。由于INS/GNSS/CNS組合系統(tǒng)的狀態(tài)方程具有非線性特征，因此在FKF的局部濾波過程中，需要對系統(tǒng)狀態(tài)方程進行線性化處理。

假設飛機作機動飛行，包括平飛、爬升、側(cè)滾、轉(zhuǎn)彎、加速、減速、下降等多種飛行狀態(tài)，其飛行軌跡如圖2所示。飛機的初始位置為東經(jīng)108.997°，北緯34.246°，高度5000 m；初始速度為150 m/s，方向正北；初始姿態(tài)為航向角0°、俯仰角0°和橫滾角0°，即與載體坐標系平行。采用中等精度的航空慣導，其中：陀螺常值漂移為0.01 (°)/h，白噪聲為0.001 (°)/h；加速度計常值偏置為10-4g，白噪聲為5×10-5g。GNSS的水平位置誤差均方根為3 m，高度誤差均方根為5 m，速度誤差均方根為0.05 m/s。CNS的姿態(tài)誤差均方根為5′。INS采樣周期為0.05 s，GNSS和CNS的采樣周期均為1 s，仿真時間為1000 s。Monte Carlo仿真次數(shù)為50次。

圖2 飛機飛行軌跡Fig.2 Flight trajectory of the aircraft

對于提出的最優(yōu)數(shù)據(jù)融合方法，初始誤差協(xié)方差陣和過程噪聲方差設置為

對于FKF和UKF-FKF，為應用方差上界技術消除兩個局部狀態(tài)間的相關性，將增大為式(37)和(38)所示初始值的兩倍。

三種算法中，量測噪聲方差設置為

圖3～圖5為采用FKF、UKF-FKF和提出的數(shù)據(jù)融合方法計算得到的姿態(tài)、速度和位置誤差的均方根誤差(Root Mean Squared Error，RMSE)曲線?？梢钥吹剑現(xiàn)KF的RMSE最大，意味著FKF得到的導航精度最低。這是因為在局部濾波過程中，F(xiàn)KF不但采用了方差上界技術，而且對系統(tǒng)狀態(tài)方程進行了線性化處理，使得數(shù)據(jù)融合精度嚴重降低。UKF-FKF在局部濾波過程中應用UKF進行濾波解算，避免了系統(tǒng)狀態(tài)方程線性化所導致的截斷誤差，提高了FKF的精度。然而，相比于提出的數(shù)據(jù)融合方法，其數(shù)據(jù)融合精度依然稍低。原因在于，UKF-FKF在局部濾波過程中仍采用了方差上界技術，導致兩個局部濾波器始終處于次優(yōu)的工作模式，因而該算法僅能輸出次優(yōu)的全局估計。相反地，提出的數(shù)據(jù)融合方法由線性最小方差準則直接推導得到，避免了子濾波器中方差上界技術的使用。因而，無論局部狀態(tài)估值是否相關，提出的方法都可得到系統(tǒng)狀態(tài)的全局最優(yōu)估計。

圖3 姿態(tài)誤差的RMSE曲線Fig.3 RMSE of attitude errors

圖4 速度誤差的RMSE曲線Fig.4 RMSE of velocity errors

圖5 位置誤差的RMSE曲線Fig.5 RMSE of position errors

表1給出了三種算法在100～1000 s內(nèi)得到的姿態(tài)誤差、速度誤差和位置誤差的RMSE的均值。由表1的統(tǒng)計結果可知：相比于FKF，提出方法得到的導航誤差至少減小了36.4%；即便與UKF-FKF相比，其導航誤差也至少減小了21.0%。

仿真結果表明，提出的數(shù)據(jù)融合方法有效克服了FKF及其優(yōu)化形式在INS/GNSS/CNS組合系統(tǒng)應用中由于采用方差上界技術而導致融合精度不高的缺陷，顯著提高了INS/GNSS/CNS組合系統(tǒng)的導航性能。

表1 導航誤差的RMSE均值比較Tab.1 Comparison on mean RMSE of navigation errors

4 結 論

本文提出了一種基于UKF的多傳感器最優(yōu)數(shù)據(jù)融合方法以解決INS/GNSS/CNS組合系統(tǒng)中的信息融合問題。提出的方法具有兩層融合結構：第一層中，GNSS和CNS分別通過兩個局部UKF濾波器與INS組合，構成INS/GNSS、INS/CNS子系統(tǒng)，并得到系統(tǒng)狀態(tài)的局部最優(yōu)估值；第二層中，根據(jù)線性最小方差準則推導出一種矩陣加權數(shù)據(jù)融合算法，對局部狀態(tài)估值進行融合，進而獲取系統(tǒng)狀態(tài)的全局最優(yōu)估計。提出的數(shù)據(jù)融合方法無需對局部狀態(tài)估值進行去相關處理，克服了FKF及其優(yōu)化形式所存在的缺陷。仿真結果表明：相比于FKF，提出方法的導航精度至少提高了36.4%；相比于UKF-FKF，其導航精度也至少提高了21.0%。

（References）：

[1] 周衛(wèi)東, 王巧云. 基于矢量信息分配的 INS/GNSS/CNS組合導航系統(tǒng)[J]. 哈爾濱工業(yè)大學學報, 2015, 47(4): 99-103. Zhou Wei-dong, Wang Qiao-yun. The INS/GNSS/CNS integrated navigation system based on vector information distribution[J]. Journal of Harbin Institute of Technology, 2015, 47(4):99-103.

[2] Hu G G, Gao S S, Zhong Y M, et al. Modified federated Kalman filter for INS/GNSS/CNS integration[C]//Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, 2016, 230(1): 30-44.

[3] Hu G G, Gao S S, Zhong Y M, et al. Matrix weighted multi-sensor data fusion for INS/GNSS/CNS integration [J]. Proceedings of the Institution of Mechanical Engineers Part g-Journal of Aerospace Engineering, 2016, 230(6): 1011-1026.

[4] Deng H, Liu G B, Chen H M, et al. The application of federated Kalman filtering in SINS/GPS/CNS intergrated navigation system[J]. International Journal of Wireless and Microwave Technologies, 2012, 2(2): 12-19.

[5] Xiong Z, Chen J, Wang R, et al. A new dynamic vector formed information sharing algorithm in federated filter [J]. Aerospace Science and Technology, 2013, 29(1): 37-46.

[6] 趙巖, 高社生, 馮鵬程. 臨近空間偽衛(wèi)星抗風場干擾自主導航[J]. 中國慣性技術學報, 2013, 21(3): 359-364. Zhao Yan, Gao She-sheng, Feng Peng-cheng. Autonomous navigation of near space pseudolite under wind field disturbance[J]. Journal of Chinese Inertial Technology, 2013, 21(3): 359-364.

[7] Huang J, Yan B. Federated filter based on dynamic information allocation with unscented Kalman filter[C]//International Conference on Electrical, Computer Engineering and Electronics. 2015: 911-916.

[8] Chang K C, Zhi T, Saha R K. Performance evaluation of track fusion with information matrix filter[J]. IEEE Transactions on Aerospace and Electronic Systems, 2002, 38(2): 455-466.

[9] Gao S S, Zhong Y M, Shirinzadeh B. Random weighting estimation for fusion of multi-dimensional position data[J]. Information Sciences, 2010, 180(24): 4999-5007.

[10] 孫楓, 唐李軍. 基于cubature Kalman filter的INS/GPS組合導航濾波算法[J]. 控制與決策, 2012, 27(7): 1032-1036. Sun Feng, Tang Li-jun. INS/GPS integrated navigation filter algorithm based on cubature Kalman filter[J]. Control and Decision, 2012, 27(7): 1032-1036.

UKF-based optimal data fusion method for integrated INS/GNSS/CNS

MENG Yang, GAO She-sheng, GAO Bing-bing, WANG Wei
(School of Automatics, Northwestern Polytechnical University, Xi’an 710072 , China)

An UKF-based multi-sensor optimal data fusion algorithm is presented to improve the navigation accuracy of integrated INS/GNSS/CNS system. It’s fusion structure has two levels: on the first level, GNSS and CNS are respectively integrated with INS by two local UKFs to obtain the local optimal state estimates in parallel; on the second level, a matrix weighted data fusion algorithm is derived based on the rule of linear minimum variance to fuse the local state estimates for generating the global optimal state estimation. Since the proposed method refrains using the upper bound technique to eliminate the correlation between local states, it overcomes the limitations of the federated Kalman filter (FKF) and its improved forms. Simulation results demonstrate that the navigation accuracy achieved by the proposed method is at least 36.4% higher than that by FKF and 21.0% higher than that by UKF-FKF.

INS/GNSS/CNS integration; data fusion; federated Kalman filter; unscented Kalman filter

V249.3

：A

2016-07-26；

：2016-10-28

國家自然科學基金（61174193）；航天科技創(chuàng)新基金（2014-HT-XGD）資助課題

孟陽（1985—），男，博士研究生，從事組合導航與非線性濾波算法研究。E-mail: horizon131@126.com

聯(lián) 系 人：高社生（1956—），男，教授，博士生導師，研究方向為導航、制導與控制。E-mail: gshshnpu@163.com

1005-6734(2016)06-0746-06

10.13695/j.cnki.12-1222/o3.2016.06.009