MIMU/GNSS緊組合精密單點定位協方差成形自適應濾波方法

劉 斌，穆榮軍，馬新普，蔣金龍，崔乃剛

（1. 哈爾濱工業大學 航天學院，哈爾濱 150001；2. 航天科工集團第四研究院 第九總體設計部，武漢 100191）

MIMU/GNSS緊組合精密單點定位協方差成形自適應濾波方法

劉 斌1，穆榮軍1，馬新普2，蔣金龍2，崔乃剛1

（1. 哈爾濱工業大學 航天學院，哈爾濱 150001；2. 航天科工集團第四研究院 第九總體設計部，武漢 100191）

針對精密單點定位應用需求，研究了MIMU/GNSS緊組合協方差成形自適應濾波方法。給出了“位臵濾波器+速度濾波器”的分布式濾波器設計方案以降低計算復雜度；推導了地理系緊組合導航系統模型，并把偽距測量不一致性偏差擴展至系統狀態向量中予以估計，從而提高系統對于動態環境下偽距測量偏差抖動的適應能力。協方差成形自適應濾波算法利用 Frobenius范數來衡量系統殘差噪聲水平建模狀態與實際狀態的匹配程度，并以最小化 Frobenius范數作為優化指標，動態調節濾波增益，以此來提高狀態估計精度、平穩性與魯棒性。地面靜態試驗表明：緊組合協方差成形自適應濾波器定位誤差均值與均值穩定性均優于標準卡爾曼濾波器，定位精度提高了約50%，能夠提供亞米級單點定位導航服務。相較于集中式濾波器設計方案，分布式濾波器方案計算復雜度降低了63.5%。

緊組合；慣性導航；衛星導航；協方差成形；精密單點定位；自適應濾波

SINS/GNSS緊組合導航方法已經被廣泛研究并應用于車輛與飛行器系統中。在現有的研究工作中，松散組合與緊組合導航系統能夠在無遮擋條件下提供相同精度的導航定位服務[1]，且對于松散組合系統而言，衛星接收機內部采用最小二乘迭代器或卡爾曼濾波器進行導航解算，通常情況下系統收斂性與動態性能均優于緊組合導航系統[2-3]。但在復雜應用環境下，例如，衛星可見性較差、受外界干擾導致接收機性噪比減弱以及衛星星座頻繁切換時，采用緊組合系統具有一定優勢[4-5]。

無人駕駛飛行器（Unmanned Aerial Vehicle，UAV）在很多應用場合需要高精度單點定位服務，且UAV工作環境一般較為復雜[6-8]。大的姿態機動導致衛星接收機可見衛星星座的頻繁切換，以及復雜應用環境下衛星星座的遮擋和多路徑效應的影響，導致采用松散組合方式一般無法滿足系統精度與數據平穩性要求[9-11]。采用緊密組合方式，并對傳統緊組合導航系統模型與濾波方法進行改進，以提高系統對于復雜環境的適應性，將可能成為解決UAV精密單點定位問題的一種有效解決途徑。

本文主要對改進的SINS/GNSS緊組合導航方法進行研究，在系統模型中引入偽距偏差不一致性作為待估計狀態向量，通過合理設計濾波器參數以補償不同仰角衛星偽距測量偏差不一致性與偽距偏差抖動對于濾波平穩性與精度的影響。與此同時，為提高系統對于外界干擾的抑制能力，增強系統魯棒性與快速收斂性，在傳統卡爾曼濾波方法基礎上，增加協方差成形環節，綜合實測與預測信息，把協方差分為固定與可變兩部分，通過比對系統殘差建模狀態與實測狀態的匹配程度，以最小化協方差Frobenius范數[12]為優化指標，完成可變協方差部分自適應增益因子計算，實現對濾波器的自適應調節，以此來保證濾波估計精度與平穩性。

采用低成本微機械慣性測量單元（Miniature Inertial Measurement Unit, MIMU）與低成本多頻衛星接收機組建地面實驗系統，開展緊組合導航單點定位地面靜態試驗，對緊組合導航方法進行性能分析與精度評估驗證。

1 改進的緊組合導航系統模型

傳統緊組合系統模型建立在WGS-84坐標系下[13-14]，而常用慣性導航系統模型建立在地理坐標系下[15]，且對于地面應用來說，地理系模型更加有利于分析系統可觀測性以及直觀評價系統性能。為方便工程應用，在傳統模型基礎上，推導給出地理系下緊組合導航系統模型。

不同仰角導航衛星信號的信噪比差異，以及多路徑等影響，導致接收機偽距測量偏差的一致性較差，從而極大地降低了系統定位精度。為解決該問題，引入偽距一致性偏差作為狀態向量的增廣予以估計，以此來提高系統對于觀測信息擾動的適應能力，增強系統精度與魯棒性。

1.1 改進的緊組合導航系統結構

采用“位置濾波器單元+速度濾波器單元”的分散式系統結構，以解決傳統集中式濾波器高維矩陣計算量過大的問題。利用速度反饋修正以阻尼慣性導航系統位置積分偏差的發散，保持系統狀態模型精度，分散式緊組合系統結構見圖1。

圖1 SINS/GNSS緊組合導航系統結構Fig.1 Structure of SINS/GNSS tightly-coupled inertial navigation system

1.2 位置通道緊組合系統模型

狀態模型以慣性導航誤差傳播模型為基礎，引入衛星偽距偏差不一致性作為擴展的狀態向量以提高系統穩定性與精度；觀測模型以衛星導航偽距模型泰勒級數展開得到。

1.2.1 狀態模型

狀態向量xp選取為

離散化后的狀態方程為

1.2.2 觀測模型

地心地固坐標系緊組合系統偽距觀測模型為：

地理系與地心地固系位置轉換關系為：

忽略緯度變化對卯酉圈半徑RN的影響，對式(7)在(,,)

Lλh處進行泰勒級數展開，有：

寫為矩陣形式有：

聯合式(5)與式(10)，并且考慮每顆導航衛星測量偽距偏差的不一致性偏差，得到地理系下觀測方程為

1.3 速度通道緊組合系統模型

狀態模型根據慣性導航誤差傳播特性得到，觀測模型利用偽距率模型泰勒級數展開得到。

1.3.1 狀態模型

狀態向量xv選取為

離散化后的狀態方程為

1.3.2 觀測模型

地心系緊組合系統偽距率觀測模型為

地理系速度偏差可通過坐標變換矩陣直接轉換至地心系，即：

把式(17)代入式(16)中，得到地理系下偽距率對速度偏差的觀測方程為

2 協方差成形自適應卡爾曼濾波方法

慣性導航系統誤差傳播模型可以采用如下線性系統進行表示：

系統殘差ek為

則系統測量殘差方差Sk-1,k為

其中，Rk為測量噪聲方差。

把狀態預測協方差矩陣Pk-1,k寫成如下形式：

其中，α為自適應增益因子。

把式(22)代入式(21)中，有：

由式(22)可知，可以改變自適應增益因子α，實現對濾波器的自適應調節。系統實際殘差方差的均值可以通過N點采樣得到，即：

式(23)是卡爾曼濾波估計的殘差協方差矩陣，式(24)是實測計算得到的殘差協方差矩陣。以α為優化變量，可以得到使得式(23)與式(24)間偏差最小的代價函數，代價函數采用Frobenius范數表示，定義為

把式(22)與式(23)代入式(25)中，有：

其中，

式(26)對α進行微分，考慮到β為反對稱矩陣，有：

使得代價函數J(α) 最小，則有：

由于自適應增益矩陣α為對角矩陣，聯合式(27)與式(29)，得到自適應增益矩陣α的計算公式為：

其中，diag[·]表示取對角線元素。

3 地面靜態試驗與分析

地面靜態試驗系統主要由微機械慣性測量單元、衛星導航接收機、數據采集計算機、數據融合處理計算機構成，系統結構、傳感器指標以及硬件系統實物圖如圖2所示。

開展地面靜態搜星試驗以考核衛星接收機的偽距測量精度與穩定度，偽距測量偏差及與衛星仰角間的關系見圖3。

由圖3給出的靜態搜星結果可以看出，高仰角衛星的測量偽距精度（6 m）明顯優于低仰角衛星（10 m），且高低仰角衛星測量偽距的誤差特性不一致。若采用標準卡爾曼濾波方法進行組合導航計算，偽距偏差的不一致性會影響導航定位精度（不滿足高斯白噪聲特性）。單次緊組合單點定位試驗結果見圖4。

圖2 地面靜態試驗系統結構與硬件實物圖Fig.2 Structure and hardware of ground static test system

圖3 衛星仰角-偽距偏差曲線Fig.3 Elevation angle vs. pseudorange deviation of satellite

由圖4給出的單次試驗數據可知，在定位數據平穩性與精度上，協方差成形自適應濾波器均優于標準卡爾曼濾波器，表明協方差成形自適應濾波算法具有更好的抗干擾能力。重復開展99次試驗，得到緊組合精度統計結果見圖5和圖6。

圖5和圖6的精度統計結果表明：協方差成形自適應濾波器的定位均值精度與偏置穩定性均優于標準卡爾曼濾波器，定位精度提高了約50%。在無差分輔助的靜態情況下，緊組合協方差成形自適應濾波器能夠提供亞米級（水平1σ：0.65 m；高程1σ：0.82 m）的導航定位服務，能夠有效解決UAV的高精度單點定位問題。

集中式與分布式緊組合濾波器的計算復雜度分析見表1。由表1可知，采用分布式濾波器結構較集中式濾波器結構，計算復雜度降低了63.5%。

表1 濾波器每萬步計算耗時(實測)Tab.1 Time consuming per 104steps of filtering calculation

圖5 水平定位精度統計結果Fig.5 Statistical results of horizontal positioning accuracy

圖6 高程定位精度統計結果Fig.6 Statistical results of vertical positioning accuracy

4 結 論

本文針對MIMU/GNSS緊組合精密單點定位自適應濾波方法開展研究，取得了以下三個較為典型的研究成果：

1）針對工程應用中對計算實時性要求，給出了“位置濾波器+速度濾波器”的分布式濾波器結構設計方案，與傳統的集中式濾波器方案相比，計算實時性提高了63.5%。

2）推導給出了地理系下緊組合導航系統模型，與傳統的WGS-84系模型相比，更加有利于分析系統可觀測性等性能；把不同仰角衛星偽距測量偏差不一致程度擴展至狀態向量中予以估計，提高了系統對于測量偽距偏差抖動的適應能力，進而有效保證狀態估計的平穩性。

3）在標準卡爾曼濾波方法基礎上，把狀態預測協方差矩陣劃分為固定部分與可變部分，通過對比殘差協方差建模狀態與實測狀態的匹配程度，以最小化Frobenius范數為優化代價函數，動態計算狀態預測協方差的自適應增益矩陣，實現對濾波器的自適應調節，提高濾波器對于測量抖動以及外界干擾的抑制能力，保證濾波估計的平穩性，提高濾波估計精度。地面靜態試驗表明：采用協方差成形自適應濾波方法能夠提高定位精度約50%，在靜態條件下能夠獲取亞米級定位精度。

（References）：

[1] Rabbou M A, El-Rabbany A. Tightly coupled integration of GPS precise point positioning and MEMS-based inertial systems[J]. GPS Solutions, 2015, 19(4): 601-609.

[2] Montillet J P, Yu K. Modified leaky LMS algorithms applied to satellite positioning[C]//2014 IEEE 80th Vehicular Technology Conference (VTC2014-Fall). 2014: 1-5.

[3] Nguyen T T T, Ta T H, Nguyen H L T, et al. An adaptive bandwidth notch filter for GNSS narrowband interference mitigation[J]. REV Journal on Electronics and Communications, 2015, 4(3-4): 59-68.

[4] Chiang K W, Lin C A, Peng K Y. The performance analysis of an AKF based tightly-coupled INS/GNSS sensor fusion scheme with non-holonomic constraints for land vehicular applications[C]//Applied Mechanics and Materials. Trans Tech Publications, 2013, 284: 1956-1960.

[5] 楊濤, 趙子陽, 李醒飛, 等. 多星座 GNSS/INS緊耦合方法[J]. 中國慣性技術學報, 2015, 23(1): 38-42. Yang Tao, Zhao Zi-yang, Li Xing-fei, et al. Tightly-cou pled integration method for multi-constellation GNSS/INS[J]. Journal of Chinese Inertial Technology, 2015, 23(1): 38-42.

[6] Jiang F, Swindlehurst A L. Dynamic UAV relay positioning for the ground-to-air uplink[C]//2010 IEEE Globecom Workshops. 2010: 1766-1770.

[7] Caliskan F, Hacizade C. Sensor and actuator FDI applied to an UAV dynamic model[J]. IFAC Proceedings Volumes, 2014, 47(3): 12220-12225.

[8] Rahimi A M, Ruschel R, Manjunath B S. UAV Sensor fusion with latent-dynamic conditional random fields in coronal plane estimation[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2016: 4527-4534.

[9] Falco G, Einicke G A, Malos J T, et al. Performance analysis of constrained loosely coupled GPS/INS integration solutions[J]. Sensors, 2012, 12(11): 15983-16007. [10] Nguyen V H. Loosely coupled GPS/INS integration with Kalman filtering for land vehicle applications[C]//2012 International Conference on Control, Automation and Information Sciences. 2012: 90-95.

[11] Hirokawa R, Ebinuma T. A low-cost tightly coupled GPS/INS for small UAVs augmented with multiple GPS antennas[J]. Navigation, 2009, 56(1): 35-44.

[12] Custódio A L, Rocha H, Vicente L N. Incorporating minimum Frobenius norm models in direct search[J]. Computational Optimization and Applications, 2010, 46(2): 265-278.

[13] 李增科, 高井祥, 姚一飛, 等. GPS/INS緊耦合導航中多路徑效應改正算法及應用[J]. 中國慣性技術學報, 2014, 22(6) : 782-787. Li Zeng-ke, Gao Jing-xiang, Yao Yi-fei, et al. GPS/INS tightly-coupled navigation with multipath correction algorithm[J]. Journal of Chinese Inertial Technology, 2014, 22(6) : 782-787.

[14] 王琰, 張傳定, 胡小工, 等. 衛星分群的抗差 Kalman濾波在GPS/BDS融合精密單點定位中的應用[J]. 中國慣性技術學報, 2016, 24(6): 769-774. Wang Yan, Zhang Chuan-ding, Hu Xiao-gong, et al. Robust Kalman filtering based on different satellite types and it's application in GPS/BDS precise point positioning [J]. Journal of Chinese Inertial Technology, 2016, 24(6): 769-774.

[15] 鄭辛, 付夢印. SINS/GPS緊耦合組合導航[J]. 中國慣性技術學報, 2011, 19(1): 33-37. Zheng Xin, Fu Meng-yin. SINS GPS tightly-coupled integrated navigation[J]. Journal of Chinese Inertial Technology, 2011, 19(1): 33-37.

Covariance shaping adaptive filter method for tightly-coupled GNSS precise single-point positioning inertial navigation

LIU Bin1, MU Rong-jun1, MA Xin-pu2, JIANG Jin-long2, CUI Nai-gang1
(1. School of Astronautics, Harbin Institute of Technology, Harbin 150001, China;
2. China Aerospace Science and Industry Corporation, Wuhan 100191, China)

According to the application requirement of the precision single-point positioning, the adaptive filtering method for MIMU/GNSS tightly-coupled system based on covariance shaping is studied. The distributed filter design of the “position filter + speed filter” is given to reduce the computational complexity. The model of MIMU/GNSS tightly-coupled system in geography system is deduced, and the pseudo-range measurement inconsistency is extended into the system state vector to improve the system’s ability to suppress the pseudo-range deviation jitter in dynamic environments. The covariance shaping adaptive filtering algorithm utilizes the Frobenius norm to measure the matching degree between the modeling and actuality of residual noise, and dynamically adjust the filter gain to minimize the Frobenius norm which is taken as the optimization index, in order to improve the state estimation accuracy, stability and robustness. The ground static test shows that the positioning error’s mean values and the mean-value’s stability of the covariance shaping adaptive filter are both better than those of the standard Kalman filter, and the positioning accuracy is increased by approximately 50%, and the sub-level single-point positioning navigation services can be provided. Compared with the centralized filter design scheme, the computational complexity of the distributed filter scheme is reduced by 63.5%.

tight combination; inertial navigation; satellite navigation; covariance shaping; precise point positioning; adaptive filtering

V249. 322；U666. 11

A

1005-6734(2017)02-0221-06

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

2017-01-04；

2017-03-28

國家高技術研究發展計劃（863計劃）（2015AA7026083）

劉斌（1989—），男，博士研究生，主要從事慣性導航初始對準、組合導航研究。E-mail: xiaobin_hit@163.com

聯 系 人：崔乃剛（1965—），男，教授，博士生導師。E-mail: Cui_Naigang@163.com