何勱航，孫付平，肖 凱，車通宇，苗岳旺

自適應(yīng)前后向平滑算法在組合導(dǎo)航中的應(yīng)用

何勱航1，孫付平2，肖 凱2，車通宇1，苗岳旺1

（1. 61363部隊(duì)，西安 710054；2. 信息工程大學(xué)，鄭州 450001）

當(dāng)全球衛(wèi)星導(dǎo)航系統(tǒng)（GNSS）的導(dǎo)航信號(hào)受到部分遮擋或干擾，導(dǎo)致新息向量誤差增大，會(huì)造成GNSS/INS（慣性導(dǎo)航系統(tǒng)）單向?yàn)V波誤差增大，濾波誤差將進(jìn)一步干擾平滑效果，針對(duì)這一問(wèn)題，提出一種自適應(yīng)前后向平滑算法，算法在單向?yàn)V波時(shí)采用自適應(yīng)卡爾曼濾波，通過(guò)自適應(yīng)調(diào)節(jié)新息向量的方差協(xié)方差矩陣，減小誤差的不利影響。一組跑車實(shí)驗(yàn)數(shù)據(jù)計(jì)算表明，自適應(yīng)平滑算法精度要優(yōu)于常規(guī)的平滑算法，可以有效減小誤差。自適應(yīng)前后向平滑算法三維位置誤差的均值和標(biāo)準(zhǔn)差分別為25.03和8.94 cm，比未自適應(yīng)處理精度分別提升21.66%和43.74%，對(duì)速度和姿態(tài)精度也有一定提升。提出的算法和實(shí)驗(yàn)結(jié)論可為組合導(dǎo)航數(shù)據(jù)后處理提供參考。

前后向平滑; 自適應(yīng)濾波; 組合導(dǎo)航; 位置精度; 數(shù)據(jù)后處理

0 引言

在全球衛(wèi)星導(dǎo)航系統(tǒng)（global navigation satellite system, GNSS）和慣性導(dǎo)航系統(tǒng)（inertial navigation system, INS）組合導(dǎo)航定位中，當(dāng)衛(wèi)星信號(hào)受到部分遮擋或者干擾時(shí)，可以依靠慣性導(dǎo)航提供短時(shí)的導(dǎo)航信息；但是隨著遮擋干擾時(shí)間越長(zhǎng)，誤差增大。保證在GNSS觀測(cè)質(zhì)量不佳時(shí)依舊獲得連續(xù)高精度的導(dǎo)航信息一直是組合導(dǎo)航的研究熱點(diǎn)[1-2]。

常見(jiàn)的方法分為2種：1）神經(jīng)網(wǎng)絡(luò)訓(xùn)練和輸出；2）平滑算法。文獻(xiàn)[3-6]分別采用高斯回歸過(guò)程、神經(jīng)網(wǎng)絡(luò)算法、支持向量機(jī)等多種模型作為訓(xùn)練方法，當(dāng)GNSS信號(hào)良好時(shí)，對(duì)神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)中的各類參數(shù)進(jìn)行訓(xùn)練，當(dāng)GNSS遮擋或失效時(shí)，以此作為輸出進(jìn)行量測(cè)更新，避免誤差發(fā)散。但是載體當(dāng)運(yùn)動(dòng)狀態(tài)多變時(shí)，訓(xùn)練難度較大；因神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)參數(shù)變化較大，輸出結(jié)果不穩(wěn)定[3-6]。平滑算法可以有效提高定位精度，主要包括前后向平滑（forward-backward-smoothing, FBS）算法和勞赫-通-斯特里貝爾（Rauch-Tung-Striebel, RTS）平滑算法。RTS平滑技術(shù)利用了相同歷元間的動(dòng)力學(xué)約束，可以有效提高速度和姿態(tài)的后處理精度[7-9]。而前后向平滑算法可以依靠前、后向?yàn)V波抵消部分誤差影響。前后向平滑算法中，要進(jìn)行2次獨(dú)立的正向、反向?yàn)V波，傳統(tǒng)的正向?yàn)V波是利用歷史信息預(yù)測(cè)當(dāng)前狀態(tài)值，受當(dāng)前觀測(cè)值影響較大，逆向?yàn)V波的原理相同，一般采用正向?yàn)V波末尾歷元信息作為起算依據(jù)[10]。文獻(xiàn)[11]首次在國(guó)內(nèi)提出了逆向慣性導(dǎo)航解算方法，但是沒(méi)有給出誤差方程[11]。文獻(xiàn)[12]將前后向平滑算法應(yīng)用于精密單點(diǎn)定位（precise point positioning, PPP）和INS緊組合后處理中，并且給出了詳細(xì)的地心地固系的前后向誤差方程，取得了良好的效果[12]。文獻(xiàn)[13]結(jié)合FBS平滑和RTS平滑的優(yōu)缺點(diǎn)，提出了一種二者融合的最優(yōu)平滑算法[13]。前期的平滑算法研究中，主要采用常規(guī)的卡爾曼濾波技術(shù)，沒(méi)有引入自適應(yīng)算法來(lái)避免單向?yàn)V波誤差發(fā)散的問(wèn)題，并沒(méi)有研究對(duì)比自適應(yīng)濾波對(duì)前后向平滑的提升效果。

在平滑過(guò)程中，當(dāng)GNSS信號(hào)受干擾或部分遮擋時(shí)，量測(cè)誤差對(duì)前向或后向?yàn)V波都會(huì)帶來(lái)影響，也必然會(huì)影響平滑的結(jié)果。所以本文提出一種自適應(yīng)前后向平滑算法，在前向、后向?yàn)V波中都加入自適應(yīng)濾波算法，以此減小觀測(cè)量誤差對(duì)濾波的影響。

1 組合導(dǎo)航濾波模型

1.1 前向?yàn)V波狀態(tài)方程

本文采用當(dāng)?shù)氐乩硐底鳛閷?dǎo)航坐標(biāo)系，狀態(tài)方程依靠慣性導(dǎo)航誤差方程進(jìn)行構(gòu)建。首先，前向慣性導(dǎo)航微分方程為

同理推導(dǎo)前向慣性導(dǎo)航誤差方程，以前向微分方程為依據(jù)，構(gòu)建前向?yàn)V波狀態(tài)方程為

1.2 后向?yàn)V波狀態(tài)方程

后向慣性導(dǎo)航微分方程是后向慣性導(dǎo)航解算和構(gòu)建后向誤差方程的基礎(chǔ)，結(jié)構(gòu)與前向微分方程相同，只須對(duì)若干參數(shù)取相反數(shù)，后向慣性導(dǎo)航微分方程為

1.3 量測(cè)方程

本文采用位置速度為觀測(cè)量的松組合模式，前后向?yàn)V波模型的量測(cè)方程一致，可以表示為

2 前后向自適應(yīng)平滑算法

2.1 前后向平滑算法

前后向平滑算法主要分為3個(gè)步驟：

2.2 自適應(yīng)濾波

在前、后向?yàn)V波中引入自適應(yīng)算法，通過(guò)自適應(yīng)算法調(diào)節(jié)觀測(cè)噪聲方差陣，以此減弱GNSS受部分遮擋或干擾時(shí)較大的粗差對(duì)定位結(jié)果的影響，即：

進(jìn)一步對(duì)比自適應(yīng)平滑和前向、后向以及前后向平滑。正向?yàn)V波時(shí)，當(dāng)GNSS受到部分遮擋或干擾，導(dǎo)致組合的誤差增大，當(dāng)GNSS觀測(cè)值恢復(fù)正常后誤差重新收斂；逆向?yàn)V波同理。前后向平滑可以減小誤差發(fā)散程度，自適應(yīng)平滑算法通過(guò)在單向?yàn)V波中加入自適應(yīng)算法，可在單向?yàn)V波中減弱GNSS誤差影響，從而進(jìn)一步提高平滑的精度。

3 實(shí)驗(yàn)與結(jié)果分析

圖1 實(shí)驗(yàn)跑車軌跡

3.1 位置誤差平滑效果分析

令組合導(dǎo)航初始時(shí)刻為0，實(shí)驗(yàn)大約2.5 h，采樣率為1 s，共計(jì)8000多個(gè)歷元，對(duì)采用前8000個(gè)歷元進(jìn)行數(shù)據(jù)分析，在4000～4100 s的時(shí)刻段添加10 m的隨機(jī)位置誤差。主要目的是為了模擬GNSS觀測(cè)值出現(xiàn)誤差時(shí)對(duì)前后向?yàn)V波的影響，圖2為4種濾波模式的位置誤差。對(duì)比圖2可以直觀看出，使用自適應(yīng)平滑算法后位置誤差顯著減小，加入10 m誤差時(shí)間段內(nèi)的前向、后向以及前后向平滑算法的位置誤差受其影響導(dǎo)致誤差增大，但是自適應(yīng)平滑算法在單向?yàn)V波中已經(jīng)通過(guò)自適應(yīng)算法減弱了誤差影響，所以平滑后誤差沒(méi)有增大，仍然保持良好精度。

圖2 4種濾波方式的位置誤差

對(duì)4種模式的位置誤差進(jìn)行統(tǒng)計(jì)，采用均值誤差（mean error, ME）和標(biāo)準(zhǔn)差（standard deviation, SD）對(duì)比和分析，如表1所示。相比較于單向?yàn)V波以及平滑算法，本文提出的自適應(yīng)前后向平滑算法可以進(jìn)一步提高位置精度和穩(wěn)定性。對(duì)比平滑算法和自適應(yīng)平滑算法的三維位置誤差，自適應(yīng)平滑算法的位置誤差均值由31.95減小到25.03 cm，標(biāo)準(zhǔn)差由15.89減小到8.94 cm，精度分別提升21.66%和43.74%。

表1 位置誤差統(tǒng)計(jì)表 cm

3.2 速度和姿態(tài)平滑效果分析

進(jìn)一步對(duì)比速度和姿態(tài)的精度，圖3和圖4為速度和姿態(tài)的平滑算法和自適應(yīng)平滑算法的誤差。由圖可看出，相比平滑算法，自適應(yīng)平滑算法的速度和姿態(tài)精度更優(yōu)。

表2為速度和姿態(tài)平滑和自適應(yīng)平滑的誤差均值統(tǒng)計(jì)情況。由表2可得出結(jié)論，采用自適應(yīng)平滑算法，速度誤差均值都要小于平滑算法，姿態(tài)精度略優(yōu)于平滑算法。

圖3 平滑和自適應(yīng)平滑速度誤差

圖4 平滑和自適應(yīng)平滑姿態(tài)誤差

表2 速度和姿態(tài)誤差均值統(tǒng)計(jì)表

4 結(jié)束語(yǔ)

本文提出了一種適用于GNSS/INS組合數(shù)據(jù)后處理的自適應(yīng)前后向平滑算法，算法在單向?yàn)V波中通過(guò)新息向量自適應(yīng)調(diào)節(jié)觀測(cè)量方差和協(xié)方差陣，以此減小GNSS誤差帶來(lái)的影響，最后再進(jìn)行平滑，可以得到比常規(guī)前后向平滑算法更高精度的導(dǎo)航結(jié)果。一組實(shí)驗(yàn)數(shù)據(jù)計(jì)算和分析結(jié)果表明，自適應(yīng)平滑算法的位置誤差均值和標(biāo)準(zhǔn)差為25.03和8.94 cm，相比較于平滑算法，精度分別提高21.66%和43.74%，速度和姿態(tài)精度均有一定的提高。本文研究的是基于結(jié)構(gòu)域的松組合模型，如何將自適應(yīng)平滑算法應(yīng)用到基于觀測(cè)值域的緊組合模型中，值得進(jìn)一步深入研究。

[1] GAO Zhouzheng, GE Maorong, LI You, et al. Modeling of multi-sensor tightly aided BDS triple-frequency precise point positioning and initial assessments[J]. Information Fusion, 2020, 55(1): 184-198.

[2] 袁夢(mèng)飛, 闞秀, 曹樂(lè), 等. 自適應(yīng)精英遺傳算法的快遞車路徑規(guī)劃[J]. 導(dǎo)航定位學(xué)報(bào), 2021, 9(6): 104-111.

[3] 張夢(mèng)得, 李開(kāi)龍, 胡柏青, 等. 輔助信息源短時(shí)失效下的捷聯(lián)慣性基組合導(dǎo)航算法[J]. 兵工學(xué)報(bào), 2020, 41(10): 2008-2015.

[4] YAO Yiqing, XU Xiaosu, ZHU Chenchen, et al. A hybrid fusion algorithm for GPS/INS integration during GPS outages[J]. Measurement, 2017, 103(2): 42-51.

[5] TAN Xinglong, WANG Jian, JIN Shuanggen, et al. GA-SVR and pseudo-position-aided GPS/INS integration during GPS outage[J]. Journal of Navigation, 2015, 68(4): 1-19.

[6] SUN Rui, ZHANG Zixuan, CHENG Qi, et al. Pseudorange error prediction for adaptive tightly coupled GNSS/IMU navigation in urban areas[J]. Gps Solutions, 2022, 26(1): 1-13.

[7] 郝萬(wàn)亮, 孫付平, 劉帥. 基于載波相位DGNSS/INS緊組合的平滑后處理算法[J]. 大地測(cè)量與地球動(dòng)力學(xué), 2015, 35(6): 1031-1035.

[8] 朱鋒. GNSS/SINS/視覺(jué)多傳感器融合的精密定位定姿方法與關(guān)鍵技術(shù)[D]. 武漢：武漢大學(xué), 2019: 106-109.

[9] LIU Hang, NASSAR S, EL-SHEIMY N. Two-filter smoothing for accurate INS/GPS land-vehicle navigation in urban centers[J]. IEEE Trans. Vehicular Technology, 2010, 59(9): 4256-4267.

[10] 楊洋, 李賓, 袁泉, 等. MEMS慣導(dǎo)管內(nèi)坐標(biāo)測(cè)量的改進(jìn)RTS平滑濾波算法[J]. 傳感技術(shù)學(xué)報(bào), 2021, 34(2): 189-195.

[11] 于飛飛. 基于正逆向?qū)Ш浇馑愕慕萋?lián)羅經(jīng)動(dòng)基座初始對(duì)準(zhǔn)方法研究[D]. 哈爾濱: 哈爾濱工業(yè)大學(xué), 2019. DOI：10. 27061/d. cnki. ghgdu. 2019. 000854.

[12] 劉帥, 孫付平, 李海峰, 等. 前后向平滑算法在精密單點(diǎn)定位/INS緊組合數(shù)據(jù)后處理中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2015, 23(1): 85-91.

[13] ZHANG Xiaohong, ZHU Feng, TAO Xianlu, et al. New optimal smoothing scheme for improving relative and absolute accuracy of tightly coupled GNSS/SINS integration[J]. GPS Solutions, 2017, 21(3): 861-872.

[14] 劉帥. GPS/INS組合導(dǎo)航算法研究與實(shí)現(xiàn)[D]. 鄭州: 信息工程大學(xué), 2012: 38-41.

[15] 嚴(yán)恭敏. 捷聯(lián)慣導(dǎo)算法與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 2019: 159-161.

Adaptive forward-backward-smoothing algorithm with application to integrated navigation

HE Maihang1, SUN Fuping2, XIAO Kai2, CHE Tongyu1, MIAO Yuewang1

（1. Troops 61363, Xi'an 710054, China; 2. Information Engineering University, Zhengzhou 450001, China）

When the signal of the global navigation satellite system (GNSS) is partially occluded or interfered, the error of the innovation vector will increase, which will increase the one-way filtering error of the GNSS/INS (inertial navigation system), and the filtering error will further interfere with the smoothing effect. In order to solve this problem, an adaptive forward and backward smoothing algorithm is proposed. The algorithm adopts adaptive Kalman filtering in one-way filtering, and adjusts the variance covariance matrix of the innovation vector adaptively to reduce the adverse effect of errors. A vehicle experimental data calculations show that the accuracy of the adaptive smoothing algorithm is better than the conventional smoothing algorithm, which can effectively reduce the error. The mean and standard deviation of the three-dimensional position error of the adaptive forward and backward smoothing algorithm are 25.03 cm and 8.94 cm, relatively, which are 21.66% and 43.74% improved than the non-adaptive processing accuracy, and the speed and attitude accuracy are also slightly improved. The proposed algorithm and experimental conclusions provide valuable reference for post-processing of integrated navigation.

forward and backward smoothing; adaptive filtering; integrated navigation; position accuracy; data post-processing

P228

A

2095-4999(2022)06-0151-06

何勱航，孫付平，肖凱，等. 自適應(yīng)前后向平滑算法在組合導(dǎo)航中的應(yīng)用[J]. 導(dǎo)航定位學(xué)報(bào), 2022, 10(6): 151-156.（HE Maihang, SUN Fuping, XIAO Kai, et al. Adaptive forward-backward-smoothing algorithm with application to integrated navigation[J]. Journal of Navigation and Positioning, 2022, 10(6): 151-156.）

10.16547/j.cnki.10-1096.20220620.

2022-03-09

國(guó)家自然科學(xué)基金項(xiàng)目（42104034，42174047）。

何勱航（1996—），男，浙江衢州人，碩士，助理工程師，研究方向?yàn)榻M合導(dǎo)航。

孫付平（1964—），男，河南長(zhǎng)葛人，博士，教授，研究方向?yàn)榫芏ㄎ患敖M合導(dǎo)航。