基于平滑變結(jié)構(gòu)-卡爾曼濾波的MIMU/BDS組合導(dǎo)航技術(shù)

李燦 沈強(qiáng) 秦偉偉 段志強(qiáng) 汪立新

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

關(guān)鍵詞：MIMU/BDS組合導(dǎo)航;模型不準(zhǔn)確;自適應(yīng)漸消因子;平滑變結(jié)構(gòu)濾波;組合濾波算法

中圖分類(lèi)號(hào)：TJ765; V448 ?文獻(xiàn)標(biāo)識(shí)碼： A? 文章編號(hào)：1673-5048（2021）03-0051-08

0 引? 言

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

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

1 MIMU/BDS組合導(dǎo)航建模

1.1 常用坐標(biāo)系定義

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

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

地球坐標(biāo)系（e系）：原點(diǎn)位于地球中心，坐標(biāo)系與地球固連。其中Ze沿地球極軸方向，Xe沿格林尼治子午面與地球赤平面的交線，Ye根據(jù)右手定則確定。地球坐標(biāo)系相對(duì)于慣性坐標(biāo)系繞Zi以角速度Ω轉(zhuǎn)動(dòng)。

導(dǎo)航坐標(biāo)系（n系）：原點(diǎn)位于車(chē)輛質(zhì)心位置，本文使用東北天坐標(biāo)系作為導(dǎo)航坐標(biāo)系，即Xn，Yn，Zn分別為東向、北向和天向。

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

坐標(biāo)系間的變換通過(guò)平移變換和旋轉(zhuǎn)變換實(shí)現(xiàn)，其中旋轉(zhuǎn)變換使用歐拉角法，即通過(guò)3次旋轉(zhuǎn)實(shí)現(xiàn)坐標(biāo)系間的姿態(tài)重合。由于歐拉角法已經(jīng)成熟，這里不再詳細(xì)介紹，將坐標(biāo)系A(chǔ)向坐標(biāo)系B間的變換矩陣記為CBA。

2.3 平滑變結(jié)構(gòu)-卡爾曼組合濾波

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

平滑變結(jié)構(gòu)-卡爾曼組合濾波（記為SVSKF）的迭代過(guò)程為

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

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

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

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

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

3 試驗(yàn)驗(yàn)證

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

3.1 仿真試驗(yàn)一

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

場(chǎng)景設(shè)置：假設(shè)車(chē)輛起始時(shí)刻以5 m/s的速率作勻速直線運(yùn)動(dòng)，之后以1 m/s2加速度行駛100 s，最后勻速行駛。在對(duì)準(zhǔn)過(guò)程中，忽略高度變化。設(shè)定1 200 s的“S”型標(biāo)準(zhǔn)軌跡，如圖3所示。

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

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

3.2 仿真試驗(yàn)二

在仿真試驗(yàn)二中，將標(biāo)準(zhǔn)軌跡改為直線，其余的系統(tǒng)參數(shù)和場(chǎng)景設(shè)置與3.1節(jié)一致。

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

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

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

3.3 跑車(chē)試驗(yàn)

本次試驗(yàn)?zāi)康臑轵?yàn)證SVSKF算法在實(shí)際跑車(chē)過(guò)程中的信息融合精度。選取實(shí)際的組合系統(tǒng)作為試驗(yàn)對(duì)象進(jìn)行跑車(chē)試驗(yàn)。北斗時(shí)代生產(chǎn)的BDST-MGI760-MIMU微機(jī)械慣性測(cè)量裝置詳細(xì)參數(shù)如表2所示。

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

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

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

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

參考文獻(xiàn)：

[1] 陳德潘， 陳帥， 樊龍江， 等. MIMU/CSAC/BDS組合導(dǎo)航微系統(tǒng)的硬件設(shè)計(jì)[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ā)， 卞鴻巍， 馬恒， 等. 全源定位與導(dǎo)航的統(tǒng)一理論框架構(gòu)建[J].導(dǎo)航定位與授時(shí)， 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] 劉軍， 劉克誠(chéng)， 田甜， 等. 基于自適應(yīng)卡爾曼濾波算法的緊組合導(dǎo)航系統(tǒng)的研究[J]. 電子測(cè)量技術(shù)， 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] 劉宇，? 惠鴻飛，? 路永樂(lè)，? 等. 基于遺傳算法優(yōu)化神經(jīng)網(wǎng)絡(luò)的多源信息融合室內(nèi)定位方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào)， 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] 尹慧琳， 伍淑莉， 王亞偉， 等. 基于層次因子圖的智能車(chē)環(huán)境感知和態(tài)勢(shì)認(rèn)知模型[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] 薛麗， 潘歡， 魏文輝. 自適應(yīng)高斯粒子濾波在導(dǎo)航中的應(yīng)用[J]. 計(jì)算機(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)航空重力測(cè)量中的卡爾曼濾波[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] 施常勇， 張麗敏. 平滑變結(jié)構(gòu)濾波在航天器組合導(dǎo)航中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào)， 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ā).基于自適應(yīng)多重漸消因子卡爾曼濾波的SINS初始對(duì)準(zhǔn)方法[J].系統(tǒng)工程與電子技術(shù)， 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

項(xiàng)目基金：陜西省自然科學(xué)基礎(chǔ)研究計(jì)劃項(xiàng)目（2020JQ-491; 2020JM-357）

作者簡(jiǎn)介：李燦（1989-），男，山東兗州人，碩士，助教，研究方向?yàn)閼T性系統(tǒng)及儀器。

通訊作者：沈強(qiáng)（1989-），男，山東高密人，博士，講師，研究方向?yàn)閼T性系統(tǒng)及儀器。