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

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

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

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

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

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

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

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

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

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

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

1 改進(jìn)的緊組合導(dǎo)航系統(tǒng)模型

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

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

1.1 改進(jìn)的緊組合導(dǎo)航系統(tǒng)結(jié)構(gòu)

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

圖1 SINS/GNSS緊組合導(dǎo)航系統(tǒng)結(jié)構(gòu)Fig.1 Structure of SINS/GNSS tightly-coupled inertial navigation system

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

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

1.2.1 狀態(tài)模型

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

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

1.2.2 觀測(cè)模型

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

地理系與地心地固系位置轉(zhuǎn)換關(guān)系為：

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

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

寫為矩陣形式有：

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

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

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

1.3.1 狀態(tài)模型

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

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

1.3.2 觀測(cè)模型

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

地理系速度偏差可通過坐標(biāo)變換矩陣直接轉(zhuǎn)換至地心系，即：

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

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

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

系統(tǒng)殘差ek為

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

其中，Rk為測(cè)量噪聲方差。

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

其中，α為自適應(yīng)增益因子。

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

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

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

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

其中，

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

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

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

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

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

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

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

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

圖2 地面靜態(tài)試驗(yàn)系統(tǒng)結(jié)構(gòu)與硬件實(shí)物圖Fig.2 Structure and hardware of ground static test system

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

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

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

集中式與分布式緊組合濾波器的計(jì)算復(fù)雜度分析見表1。由表1可知，采用分布式濾波器結(jié)構(gòu)較集中式濾波器結(jié)構(gòu)，計(jì)算復(fù)雜度降低了63.5%。

表1 濾波器每萬(wàn)步計(jì)算耗時(shí)(實(shí)測(cè))Tab.1 Time consuming per 104steps of filtering calculation

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

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

4 結(jié) 論

本文針對(duì)MIMU/GNSS緊組合精密單點(diǎn)定位自適應(yīng)濾波方法開展研究，取得了以下三個(gè)較為典型的研究成果：

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

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

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

（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] 楊濤, 趙子陽(yáng), 李醒飛, 等. 多星座 GNSS/INS緊耦合方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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緊耦合導(dǎo)航中多路徑效應(yīng)改正算法及應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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融合精密單點(diǎn)定位中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 鄭辛, 付夢(mèng)印. SINS/GPS緊耦合組合導(dǎo)航[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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

國(guó)家高技術(shù)研究發(fā)展計(jì)劃（863計(jì)劃）（2015AA7026083）

劉斌（1989—），男，博士研究生，主要從事慣性導(dǎo)航初始對(duì)準(zhǔn)、組合導(dǎo)航研究。E-mail: xiaobin_hit@163.com

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