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

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

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

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

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

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

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

緊組合；慣性導航；衛(wèi)星導航；協(xié)方差成形；精密單點定位；自適應濾波

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

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

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

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

1 改進的緊組合導航系統(tǒng)模型

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

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

1.1 改進的緊組合導航系統(tǒng)結構

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

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

1.2 位置通道緊組合系統(tǒng)模型

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

1.2.1 狀態(tài)模型

狀態(tài)向量xp選取為

離散化后的狀態(tài)方程為

1.2.2 觀測模型

地心地固坐標系緊組合系統(tǒng)偽距觀測模型為：

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

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

Lλh處進行泰勒級數(shù)展開，有：

寫為矩陣形式有：

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

1.3 速度通道緊組合系統(tǒng)模型

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

1.3.1 狀態(tài)模型

狀態(tài)向量xv選取為

離散化后的狀態(tài)方程為

1.3.2 觀測模型

地心系緊組合系統(tǒng)偽距率觀測模型為

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

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

2 協(xié)方差成形自適應卡爾曼濾波方法

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

系統(tǒng)殘差ek為

則系統(tǒng)測量殘差方差Sk-1,k為

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

把狀態(tài)預測協(xié)方差矩陣Pk-1,k寫成如下形式：

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

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

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

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

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

其中，

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

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

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

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

3 地面靜態(tài)試驗與分析

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

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

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

圖2 地面靜態(tài)試驗系統(tǒng)結構與硬件實物圖Fig.2 Structure and hardware of ground static test system

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

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

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

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

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

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

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

4 結 論

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

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

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

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

（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] 王琰, 張傳定, 胡小工, 等. 衛(wèi)星分群的抗差 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

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

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

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