李燦 沈強 秦偉偉 段志強 汪立新

摘 要： 為了提高模型不準確、大擾動等情況下MIMU/BDS組合導航精度，提出了平滑變結構-卡爾曼組合濾波的信息融合方法。介紹了坐標轉換方法，建立了組合導航系統(tǒng)的狀態(tài)空間方程和觀測方程。為了防止老舊觀測數據引起算法發(fā)散，在卡爾曼濾波算法中融入隨殘差自適應變化的漸消因子，從而構造了自適應卡爾曼濾波算法。將卡爾曼濾波的精度優(yōu)勢與平滑變結構濾波的魯棒性優(yōu)勢融合，提出了平滑變結構-卡爾曼組合濾波算法。經仿真驗證，在模型不準確、大擾動等情況下，組合算法的位置融合誤差和速度融合誤差均小于卡爾曼濾波和自適應卡爾曼濾波算法;經跑車試驗驗證，在衛(wèi)星遮擋情況下，組合導航系統(tǒng)的位置融合精度和速度融合精度仍然較高，實現了衛(wèi)星可觀數量不足情況下的精確導航。

關鍵詞：MIMU/BDS組合導航;模型不準確;自適應漸消因子;平滑變結構濾波;組合濾波算法

中圖分類號：TJ765; V448 ?文獻標識碼： A? 文章編號：1673-5048（2021）03-0051-08

0 引? 言

不同的導航手段中，北斗衛(wèi)星的定位精度高、實時性好，但是衛(wèi)星極易被遮擋;MIMU可以提供全部導航信息，且不受任何外界干擾，但是其導航誤差隨時間積累。將北斗衛(wèi)星與MIMU導航進行信息融合，實現MIMU/BDS的優(yōu)勢互補，可以提高導航定位的連續(xù)性和定位精度[1]，具有重要的研究價值。

多源傳感器信息融合技術相比于單一傳感器系統(tǒng)，具有更高的定位精度、更高的定位穩(wěn)定性，兼具了各子導航系統(tǒng)的優(yōu)勢。信息融合算法可大致分為3類：基于概率模型的融合算法、基于參數識別的融合算法、新型信息融合算法。具體地講，信息融合算法包括貝葉斯估計法[2]、物理建模法、卡爾曼濾波算法[3]、神經網絡算法[4]、支持向量機、因子圖法[5]等?；趨底R別的融合算法主要包括卡爾曼濾波及其改進算法、平滑變結構濾波、粒子濾波[6]等，其中卡爾曼濾波是一種最優(yōu)狀態(tài)估計算法，信息融合精度較高，但是當出現模型不準確、外界擾動較大時，卡爾曼濾波算法容易發(fā)散，即算法的魯棒性較差[7]。平滑變結構濾波是一種次優(yōu)濾波方法，其濾波精度略低于卡爾曼濾波，但是在應對模型不準確、大擾動等情況時具有較好的魯棒性[8]。本文為了兼顧濾波的精度和魯棒性，將卡爾曼濾波的精度優(yōu)勢和平滑變結構濾波的魯棒性優(yōu)勢進行融合，提出了AKF和SVSF的組合濾波方法，有效提高了模型不準確、大擾動等異常情況下的組合導航精度和魯棒性。

1 MIMU/BDS組合導航建模

1.1 常用坐標系定義

MIMU在車輛上的安裝方式為捷聯(lián)方式，加速計與陀螺儀的測量信息經過坐標變換才能獲得車輛的導航信息。求解過程中用到的坐標系包括地心坐標系（i系）、地球坐標系（e系）、導航坐標系（n系）和載體坐標系（b系），在此對各坐標系的定義方法進行明確。

地心坐標系（i系）：原點位于地球中心O，Xi是地球公轉的黃道平面與赤道平面的交線， Yi按右手規(guī)則定義，Zi的方向與地球的極軸方向一致。

地球坐標系（e系）：原點位于地球中心，坐標系與地球固連。其中Ze沿地球極軸方向，Xe沿格林尼治子午面與地球赤平面的交線，Ye根據右手定則確定。地球坐標系相對于慣性坐標系繞Zi以角速度Ω轉動。

導航坐標系（n系）：原點位于車輛質心位置，本文使用東北天坐標系作為導航坐標系，即Xn，Yn，Zn分別為東向、北向和天向。

載體坐標系（b系）：Xb，Yb，Zb分別為載體坐標系的橫滾軸、俯仰軸和偏航軸。

坐標系間的變換通過平移變換和旋轉變換實現，其中旋轉變換使用歐拉角法，即通過3次旋轉實現坐標系間的姿態(tài)重合。由于歐拉角法已經成熟，這里不再詳細介紹，將坐標系A向坐標系B間的變換矩陣記為CBA。

2.3 平滑變結構-卡爾曼組合濾波

分析式（19）可知，平滑有界層寬度跟模型不確定性、系統(tǒng)噪聲、量測噪聲、觀測誤差等直接相關，因此， 其可以用來衡量模型不確定性和噪聲大小。本文將平滑變結構濾波與卡爾曼濾波組合的思路為：設置一個有界層ψlim，當ψk+1<ψlim時，說明系統(tǒng)模型較為準確、噪聲較小，為了保證濾波精度，使用最優(yōu)狀態(tài)估計方法——卡爾曼濾波;當ψk+1≥ψlim時，說明系統(tǒng)模型誤差較大、噪聲較大，為了保證濾波穩(wěn)定性和魯棒性，使用次優(yōu)狀態(tài)估計方法——平滑變結構濾波。組合算法示意圖如圖2所示。

平滑變結構-卡爾曼組合濾波（記為SVSKF）的迭代過程為

（1）? 計算狀態(tài)向量一步預測值，觀測向量一步預測值、一步預測誤差方差陣、量測誤差;

（2）? 計算平滑有界層寬度，如式（19）;

（3）? 若ψk+1<ψlim，則使用AKF增益KAKFk+1;若ψk+1≥ψlim，則使用SVSF增益KSVSFk+1;

（4） 計算后驗狀態(tài)估計值，更新濾波誤差方差陣、后驗量測估計值、后驗量測誤差，如式（20）。

需要說明的是，有界層ψlim是人為設置，具體方法為：以狀態(tài)量εE為例，根據εE的實際變化范圍，設置εE有界層的可取范圍，其中可取范圍略大于實際變化范圍。而后以濾波誤差為指標，得到εE有界層取不同值時的濾波誤差，以圖形方式畫出濾波誤差隨εE有界層取值的變化趨勢，選擇濾波誤差最小時對應的εE有界層值為最終確定的εE有界層取值。

3 試驗驗證

為了驗證平滑變結構-卡爾曼組合濾波算法在系統(tǒng)受到大擾動、模型不準確、衛(wèi)星數量不足等情況的魯棒性、穩(wěn)定性和導航精度，本文設計了2組仿真試驗和1組跑車試驗。

3.1 仿真試驗一

設置系統(tǒng)仿真參數：車輛初始位姿參數設置為經度34°，緯度108°，高度380 m，水平姿態(tài)角0°，航向角0°;初始誤差參數設置為定位誤差10 m，速度誤差0.1 m/s，姿態(tài)角初始誤差1°，航向初始誤差2°;慣性器件參數設置為陀螺儀常值漂移10 （°）/h，角度隨機游走1 （°）/h，加速度計零偏10-4g，隨機游走誤差4×10-5g/h，更新頻率100 Hz; BDS接收機參數設置為定位誤差10 m，測速誤差0.1 m/s，數據更新頻率1 Hz。

場景設置：假設車輛起始時刻以5 m/s的速率作勻速直線運動，之后以1 m/s2加速度行駛100 s，最后勻速行駛。在對準過程中，忽略高度變化。設定1 200 s的“S”型標準軌跡，如圖3所示。

在衛(wèi)星接收機收星穩(wěn)定且大于4顆的情況下驗證算法的組合導航精度，為了說明該算法的優(yōu)劣性，采用AKF算法、傳統(tǒng)KF算法和本文的SVSKF算法融合效果進行對比，結果如圖4所示。

由圖4可知，在衛(wèi)星接收機收星穩(wěn)定且大于4顆的情況下，SVSKF和AKF算法的位置融合誤差、速度融合誤差相差不大，位置融合誤差在2 m內，速度融合誤差在0.1 m/s內。而KF算法的位置融合誤差和速度融合誤差較大，位置最大誤差為10 m，速度最大誤差為1 m/s。這是因為圖3的標準軌跡中存在大角轉彎的情況，此時存在非線性耦合情況，使KF算法的導航信息融合精度較差，而AKF和SVSKF算法由于增益可以隨新息的大小進行自適應，使算法快速收斂，因此導航精度較高。

3.2 仿真試驗二

在仿真試驗二中，將標準軌跡改為直線，其余的系統(tǒng)參數和場景設置與3.1節(jié)一致。

在1 200 s仿真過程加入3種擾動量：（1）在400～460 s期間設置模型不準確擾動，方法為在狀態(tài)轉移矩陣對角元素中隨機選擇3個元素，施加其自身幅值10%的隨機干擾;（2）在600～660 s期間設定大的白噪聲，即將系統(tǒng)噪聲和量測噪聲的幅值均調大3倍;（3）在1 000～1 020 s期間設定可觀測衛(wèi)星為3顆。為了說明該算法的優(yōu)劣性，采用AKF算法、傳統(tǒng)KF算法和本文的SVSKF算法融合效果進行對比，結果如圖5所示。

由圖5可以直觀看出，3種擾動下KF算法的位置融合誤差和速度融合誤差均較大，無法應用于實際。而AKF和SVSKF算法融合精度較高，且SVSKF算法的融合精度略高于AKF算法。以模型不準確為例，在統(tǒng)計模型不準確情況下，傳統(tǒng)KF，AKF，SVSKF三種算法的位置融合誤差和速度融合誤差如表1所示，其中均方根誤差的統(tǒng)計時間為400～600 s。

分析表1中數據可知，在模型不準確的情況下，KF算法的位置融合誤差和速度融合誤差均較大，AKF算法的融合誤差次之，SVSKF算法的位置融合精度和速度融合精度均較高。這是因為模型不準確導致的狀態(tài)估計值與實際值偏差較大，KF算法的信息融合能力有限，使得融合誤差較大。AKF算法可以根據觀測值殘差自適應調整遺忘因子，增強算法的收斂性，使融合誤差迅速減小，快速恢復到期望值附近。SVSKF算法能夠根據系統(tǒng)狀態(tài)的變化在AKF算法增益和SVSF算法增益間切換，兼具AKF算法的精確性和SVSF算法的魯棒性，因此融合精度最高。

3.3 跑車試驗

本次試驗目的為驗證SVSKF算法在實際跑車過程中的信息融合精度。選取實際的組合系統(tǒng)作為試驗對象進行跑車試驗。北斗時代生產的BDST-MGI760-MIMU微機械慣性測量裝置詳細參數如表2所示。

MIMU數據更新率為100 Hz，采樣周期為0.01 s。BDS采樣頻率為1 Hz，采樣周期為1 s。衛(wèi)星接收機利用MIMU自帶的BDS衛(wèi)星接收模塊，采用雙天線差分定位，試驗裝置如圖6（a）～（b）所示。車輛的參考位置使用高精度的激光慣組/GPS組合導航系統(tǒng)，如圖6（c）所示。

組合導航試驗運動載體為汽車，在校園灌木叢較多的區(qū)域進行跑車試驗，主要驗證衛(wèi)星信號受干擾情況下組合定位系統(tǒng)的跟蹤情況。汽車的運行軌跡如圖7所示。

MIMU/BDS組合定位系統(tǒng)運行10 min后采集數據，路況覆蓋了衛(wèi)星正常和衛(wèi)星受干擾的情況，起點處具有較大的灌木叢區(qū)域和建筑群，衛(wèi)星觀測信號弱;終點在開闊區(qū)域，衛(wèi)星定位精度高。為了增強試驗可信性，車輛圍繞設定軌跡跑4圈，采用本文算法得到的位置和速度如圖8所示。由于車輛在平地上試驗，因此對高度融合信息不進行分析與討論。

分析圖8（a）可知，在整個試驗過程中汽車位置的經度和緯度信息融合精度較高，融合位置能夠精確地定位到參考位置，且在衛(wèi)星信號良好與被遮擋的切換過程中，位置融合信息也能夠平穩(wěn)過渡，波動極小。說明在衛(wèi)星可觀測數量不足的情況下，SVSKF組合濾波方法依然能夠有效地進行信息融合，得到較高精度的位置信息。分析圖8（b）中東向速度誤差和北向速度誤差可知，速度融合信息能夠精確地跟蹤參考速度，東向最大瞬時速度誤差為1.2 m/s，均方根誤差為0.02 m/s;北向最大瞬時速度誤差為0.3 m/s，均方根誤差為0.01 m/s，即速度融合誤差和波動均較小。這是因為組合濾波器能根據狀態(tài)量的估計誤差自主選擇濾波方法，當狀態(tài)量誤差較小，出現在有界層范圍內時，使用殘差自適應卡爾曼濾波，保證較高的濾波精度;當狀態(tài)誤差較大，出現在有界層范圍外時，使用平滑變結構濾波，提高算法的魯棒性。綜合仿真結果和跑車試驗結果可知，SVSKF組合濾波方法能夠有效解決大擾動、模型不準確和衛(wèi)星可觀測數量不足等情況下的MIMU/BDS組合導航的精度和魯棒性問題。

參考文獻：

[1] 陳德潘， 陳帥， 樊龍江， 等. MIMU/CSAC/BDS組合導航微系統(tǒng)的硬件設計[J].航天控制， 2018， 36（3）： 71-78.

Chen Depan，? Chen Shuai，? Fan Longjiang，? et al. The Hardware Design of MIMU/CSAC/BDS Integrated Navigation Micro System [J]. Aerospace Control，? 2018， 36（3）： 71-78.（in Chinese）

[2] 戴海發(fā)， 卞鴻巍， 馬恒， 等. 全源定位與導航的統(tǒng)一理論框架構建[J].導航定位與授時， 2018， 5（6）： 9-16.

Dai Haifa，? Bian Hongwei，? Ma Heng，? et al. Unified Theoretical Framework Construction of All Source Positioning and Navigation [J]. Navigation Positioning and Timing，? 2018， 5（6）： 9-16.（in Chinese）

[3] 劉軍， 劉克誠， 田甜， 等. 基于自適應卡爾曼濾波算法的緊組合導航系統(tǒng)的研究[J]. 電子測量技術， 2019，? 42（5）： 52-55.

Liu Jun，? Liu Kecheng，? Tian Tian，? et al. Research on Tight Integrated Navigation System Based on Adaptive Kalman Filter Algorithm [J]. Electric Measurement Technology，? 2019， 42（5）： 52-55.（in Chinese）

[4] 劉宇，? 惠鴻飛，? 路永樂，? 等. 基于遺傳算法優(yōu)化神經網絡的多源信息融合室內定位方法[J]. 中國慣性技術學報， 2020， 28（1）： 67-73.

Liu Yu，? Hui Hongfei，? Lu Yongle，? et al. Multi-Source Information Fusion Indoor Positioning Method Based on Genetic Algorithm to Optimize Neural Network [J]. Journal of Chinese Inertial Techno-logy，? 2020， 28（1）： 67-73.（in Chinese）

[5] 尹慧琳， 伍淑莉， 王亞偉， 等. 基于層次因子圖的智能車環(huán)境感知和態(tài)勢認知模型[J]. 控制與決策， 2020， 35（10）： 2528-2534.

Yin Huilin，? Wu Shuli，? Wang Yawei，? et al. An Intelligent Vehicle Environment Perception and Situation Cognition Model Based on Hierarchical Factor Graph [J]. Control and Decision，? 2020， 35（10）： 2528-2534.（in Chinese）

[6] 薛麗， 潘歡， 魏文輝. 自適應高斯粒子濾波在導航中的應用[J]. 計算機仿真， 2020， 37（1）： 121-125.

Xue Li，? Pan Huan，? Wei Wenhui. Adaptive Gaussian Particle Filtering for Navigation [J]. Computer Simulation，? 2020， 37（1）： 121-125.（in Chinese）

[7] 蔡體菁， 顏穎， 王新宇. 捷聯(lián)航空重力測量中的卡爾曼濾波[J]. 壓電與聲光， 2019， 41（3）： 436-439.

Cai Tijing，? Yan Ying，? Wang Xinyu. Kalman Filtering of Strapdown Airborne Gravity Measurement [J]. Piezoelectrics & Acoustooptics，? 2019， 41（3）： 436-439.（in Chinese）

[8] 施常勇， 張麗敏. 平滑變結構濾波在航天器組合導航中的應用[J].中國慣性技術學報， 2017， 25（4）： 478-482.

Shi Changyong，? Zhang Limin. Application of SVSF in Spacecraft Integrated Navigation System [J]. Journal of Chinese Inertial Technology，? 2017， 25（4）： 478-482.（in Chinese）

[9] Susi M，?? Borio D. Kalman Filtering with Noncoherent Integrations for Galileo E6-B Tracking[J]. Navigation， 2020，? 67（3）： 601-618.

[10] 薛海建， 郭曉松， 周召發(fā).基于自適應多重漸消因子卡爾曼濾波的SINS初始對準方法[J].系統(tǒng)工程與電子技術， 2017， 39（3）： 620-626.

Xue Haijian，? Guo Xiaosong，? Zhou Zhaofa. SINS Initial Alignment Method Based on Adaptive Multiple Fading Factors Kalman Filter [J]. Systems Engineering and Electronics，? 2017， 39（3）： 620-626.（in Chinese）

MIMU/BDS Integrated Navigation Technology Based on

Smooth Variable Structure-Adaptive Kalman Filter

Li Can，? Shen Qiang*，? Qin Weiwei，? Duan Zhiqiang，? Wang Lixin

（Rocket Force University of Engineering，? Xian 710025，China）

Abstract：

In order to improve the accuracy of MIMU/BDS integrated navigation with uncertain models and large disturbances，? a smoothing variable structure-Kalman combined filter information fusion method is proposed. The coordinate transformation method is introduced，? and the state space equation and the observation equation of the integrated navigation system are established. In order to prevent the algorithm divergence caused by the old observation data，? the adaptive fading factor which varies with the residual error is incorporated into the Kalman filter algorithm，? and the adaptive Kalman filter algorithm is constructed. Combining the accuracy advantage of Kalman filter with the robustness advantage of smooth variable structure filter，? a smooth variable structure-Kalman combined filter algorithm is constructed. It is verified by simulation that location and speed fusion error of combined algorithm are smaller than Kalman filter and adaptive Kalman filter under the condition of uncertain model and large disturbances. The experiment shows that location fusion accuracy and speed fusion accuracy are also high under the condition of satellite occlusion，? which means the accurate navigation with insufficient number of satellites is achieved by utilizing the combined filter algorithm.

Key words： MIMU/BDS integrated navigation; uncertain model; adaptive fading factor; smooth variable structure filter; combined filter algorithm

收稿日期：2020-10-27

項目基金：陜西省自然科學基礎研究計劃項目（2020JQ-491; 2020JM-357）

作者簡介：李燦（1989-），男，山東兗州人，碩士，助教，研究方向為慣性系統(tǒng)及儀器。

通訊作者：沈強（1989-），男，山東高密人，博士，講師，研究方向為慣性系統(tǒng)及儀器。