基于交互多模型距離平滑的UWB/IMU因子圖組合導(dǎo)航方法

李 旭，孔 鑫，劉錫祥,2，宋 翔，徐啟敏

（1. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096；2. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京 210096；3. 南京曉莊學(xué)院 電子工程學(xué)院，南京 211171）

近年來(lái)隨著交通系統(tǒng)的發(fā)展，對(duì)于高級(jí)駕駛員輔助系統(tǒng)（ADAS）、智能交通系統(tǒng)（ITS）等應(yīng)用來(lái)說(shuō)，獲取精準(zhǔn)可靠的車輛位置是至關(guān)重要的[1]。然而，現(xiàn)有的車輛高度依賴于GNSS技術(shù)，當(dāng)車輛行駛至隧道、地下停車場(chǎng)等封閉區(qū)域時(shí)，由于衛(wèi)星信號(hào)受到遮擋從而無(wú)法對(duì)車輛進(jìn)行定位[2]。

當(dāng)衛(wèi)星信號(hào)不可用時(shí)，單一的慣性測(cè)量單元（IMU）無(wú)法對(duì)車輛進(jìn)行精準(zhǔn)的定位，可將IMU與其它傳感器集成起來(lái)提高定位的精度[3]。目前，隨著通信技術(shù)的發(fā)展，基于無(wú)線傳感網(wǎng)絡(luò)的定位技術(shù)受到了廣泛關(guān)注。其中，UWB因其出色的穿透障礙物的能力和厘米級(jí)的測(cè)距精度而表現(xiàn)出高精度定位的巨大潛力。然而，由于交通場(chǎng)景的復(fù)雜性，無(wú)線信號(hào)會(huì)不可避免地產(chǎn)生反射和折射從而導(dǎo)致非視距效應(yīng)，UWB的測(cè)距精度也會(huì)因此降低。為了解決UWB傳輸?shù)姆且暰鄦?wèn)題，文獻(xiàn)[4]提出了自回歸滑動(dòng)平均模型來(lái)識(shí)別UWB的非視距測(cè)量量，然后引入模糊邏輯算法來(lái)自適應(yīng)地調(diào)整對(duì)每個(gè)UWB測(cè)量量的依賴性。這類方法分類區(qū)間過(guò)于復(fù)雜，需要確定的參數(shù)過(guò)多，對(duì)非視距觀測(cè)量的抑制效果不太明顯。文獻(xiàn)[5]利用無(wú)線信號(hào)的隨機(jī)特征來(lái)檢測(cè)非視距節(jié)點(diǎn)并將其刪除，但容易造成可用節(jié)點(diǎn)過(guò)少，影響最終的定位精度。

在組合導(dǎo)航領(lǐng)域中，為了解決多傳感器同步的問(wèn)題，大部分方法都是采用傳統(tǒng)的聯(lián)邦卡爾曼濾波的方法[6]。這類方法通過(guò)數(shù)據(jù)同步處理能夠融合不同頻率的傳感器，但卻需要丟棄一部分測(cè)量值，從而造成信息的浪費(fèi)。另一方面，雖然卡爾曼濾波在線性的情況下是最優(yōu)的，但大部分傳感器模型都是非線性的，在非線性的情況下擴(kuò)展卡爾曼濾波邊緣化所有過(guò)去狀態(tài)僅估計(jì)當(dāng)前狀態(tài)來(lái)獲取實(shí)時(shí)估計(jì)，在精度上有所下降[7]。其他的UKF[8]、PF[9]等濾波算法雖然具有較高的估計(jì)精度，但卻是以復(fù)雜的運(yùn)算量為代價(jià)的。

對(duì)于GNSS拒止環(huán)境下，基于UWB/IMU的車輛組合導(dǎo)航，經(jīng)常會(huì)出現(xiàn)因非視距現(xiàn)象而出現(xiàn)的UWB測(cè)量值異常情況，以及傳感器數(shù)據(jù)不同步的問(wèn)題。針對(duì)上述問(wèn)題，本文提出了一種基于UWB/IMU的“即插即用”因子圖融合優(yōu)化方法?；谝蜃訄D的算法能充分考慮歷史測(cè)量信息，在定位精度上優(yōu)于擴(kuò)展卡爾曼濾波[10]，并且該算法能實(shí)現(xiàn)“即插即用”的功能，因?yàn)樾碌膫鞲衅髦皇翘砑拥綀D中的新因子；同樣的，若由于信號(hào)丟失或故障導(dǎo)致傳感器失效，系統(tǒng)也會(huì)避免添加相應(yīng)的因子。為了解決UWB信號(hào)非視距干擾的問(wèn)題，本文在文獻(xiàn)[11]建立的視距與非視距模型的基礎(chǔ)上，提出了一種基于因子圖的改進(jìn)交互式多模型算法來(lái)抑制UWB的非視距誤差，并將IMU輸出的測(cè)量值與改進(jìn)交互式多模型輸出的多個(gè)UWB距離測(cè)量值添加到因子圖模型中，從而形成一種即插即用的因子圖組合導(dǎo)航方法。實(shí)車測(cè)試結(jié)果證明，所提出的方法能夠兼容多個(gè)傳感器信息，同時(shí)，該方法相較于傳統(tǒng)的交互式多模型算法在定位精度上有所提升。

1 基于交互式多模型的距離平滑算法

1.1 UWB視距/非視距因子圖模型

本文采用改進(jìn)的交互式多模型算法對(duì)UWB原始距離測(cè)量值進(jìn)行平滑，以減弱因非視距傳播所導(dǎo)致的距離誤差。由文獻(xiàn)[11]可得，UWB距離的狀態(tài)方程和觀測(cè)方程為：

根據(jù)上述模型，即可建立相應(yīng)的因子圖模型，如圖1所示。

圖1 基于因子圖的UWB視距/非視距模型Fig.1 UWB LOS/NLOS model based on factor graph

在圖1中，包含了一種變量節(jié)點(diǎn)和三種類型的因子節(jié)點(diǎn)。變量節(jié)點(diǎn)包括了車載標(biāo)簽與路側(cè)第k個(gè)基站間的真實(shí)距離與相對(duì)速度，因子節(jié)點(diǎn)包括先驗(yàn)因子fPrior，狀態(tài)轉(zhuǎn)移因子 fs和距離因子 fr'。其中，先驗(yàn)因子的誤差函數(shù)可表示為：

由建立好的因子圖模型和誤差函數(shù)方程，即可得到視距/非視距下的距離估計(jì)值，最終的目標(biāo)函數(shù)為：

1.2 因子圖模型的求解

對(duì)于1.1節(jié)的因子圖模型，我們通過(guò)建立各因子的誤差函數(shù)并把狀態(tài)估計(jì)轉(zhuǎn)換成基于最大后驗(yàn)估計(jì)的非線性最小二乘問(wèn)題[11]：

為了簡(jiǎn)單起見，在這里我們略去了之前的先驗(yàn)信息。通過(guò)對(duì)式(6)中的測(cè)量函數(shù)進(jìn)行一階泰勒展開來(lái)線性化，則狀態(tài)轉(zhuǎn)移因子的誤差函數(shù)可進(jìn)一步表示為：

通過(guò)將誤差函數(shù)線性化后，式(6)可轉(zhuǎn)化為如下函數(shù)：

其中I為單位矩陣。通過(guò)馬氏距離變換公式：

可以將式(9)的雅可比矩陣合并為一個(gè)大矩陣A，向量ak和ck合并到右側(cè)向量b中，因此式(9)可轉(zhuǎn)換為如下的標(biāo)準(zhǔn)最小二乘問(wèn)題：

在數(shù)據(jù)量較大的情況下，式(12)可通過(guò)矩陣分解來(lái)求解。假設(shè)A可QR分解為A=QR，則有 QTA=[ R0]T且 QTb=[ d e]T。由于R為上三角矩陣，因此可通過(guò)回代法求得，則由線性化點(diǎn)更新得到的新的估計(jì)值為算法具體的求解過(guò)程可參考文獻(xiàn)[11]。

1.3 改進(jìn)的交互式多模型算法

在過(guò)去幾年中，交互式多模型算法已經(jīng)廣泛地應(yīng)用于車輛定位等領(lǐng)域[12,13]，在這類研究中，可能的狀態(tài)變量由多個(gè)不同的模型表示，這些模型根據(jù)不同的條件建立。交互式多模型算法通過(guò)計(jì)算各個(gè)模型的權(quán)重值來(lái)獲取最終的狀態(tài)估計(jì)，因此具有自適應(yīng)的特點(diǎn)。然而，傳統(tǒng)的交互式多模型算法大多采用卡爾曼濾波來(lái)對(duì)模型進(jìn)行估計(jì)，這類算法未能充分考慮歷史觀測(cè)信息，對(duì)視距/非視距模型不能起到較好的平滑作用?；诖?，在1.1節(jié)建立的UWB視距/非視距因子圖模型的基礎(chǔ)上，本節(jié)提出了一種基于改進(jìn)交互式多模型的距離平滑算法，如圖2所示。

圖2 改進(jìn)的交互多模型框架與馬爾可夫轉(zhuǎn)換模型Fig.2 Improved interacting multiple model framework and Markov switching model

改進(jìn)的交互式多模型距離平滑算法流程如下：

1）輸入交互： ? i, j ∈［ 1 ,2］，1為視距模型，2為非視距模型， k?［1 …K］。

混合權(quán)重：

歸一化因子：

其中，Pk,i為第i個(gè)因子圖模型輸出的邊緣化協(xié)方差矩陣。

2）因子圖平滑：具體的因子圖模型求解方法見第1.2節(jié)。

3）模型概率更新：似然函數(shù)為：

模型j的更新概率：

歸一化常數(shù)：

4）數(shù)據(jù)融合：

2 基于因子圖的UWB/IMU融合框架

隨著車載傳感器的增加，傳統(tǒng)基于濾波的方法不具備對(duì)異步導(dǎo)航信息即插即用的功能。因此，本文提出了一種基于因子圖的多傳感器融合方法，如圖3所示。

在圖3中，Xn和Vn分別代表n時(shí)刻車輛的位置和速度。其中，分別代表n時(shí)刻車輛的東向位置、北向位置和天向位置;分別代表n時(shí)刻車輛的東向速度、北向速度和天向速度。 fPrior代表先驗(yàn)因子， fMM代表運(yùn)動(dòng)模型因子， fv代表速度因子， fIMM代表交互多模型因子。

圖3 基于因子圖的UWB/IMU融合框架Fig.3 The framework of UWB/IMU based on factor graph

2.1 先驗(yàn)因子節(jié)點(diǎn)

2.2 IMU因子節(jié)點(diǎn)

IMU因子包含了運(yùn)動(dòng)模型因子與速度因子，通過(guò)接收來(lái)自IMU的測(cè)量值定義這兩種因子。其中， fb與 ωb分別代表載體坐標(biāo)系下載體的比力和角速度。

因此，運(yùn)動(dòng)模型可表示為：

速度模型可表示為：

其中，ae、an、au分別為捷聯(lián)慣導(dǎo)算法轉(zhuǎn)換的載體在東北天坐標(biāo)系的加速度。

則IMU因子的誤差函數(shù)可表示為：

2.3 IMM因子節(jié)點(diǎn)

IMM因子的量測(cè)值來(lái)自交互式多模型輸出的平滑后的UWB距離信息，因此IMM因子的量測(cè)方程可表示為：

根據(jù)各傳感器的工作頻率實(shí)時(shí)地在因子圖中插入相關(guān)的因子，根據(jù)各因子的代價(jià)函數(shù)實(shí)現(xiàn)變量的遞推與更新。根據(jù)各傳感器可用性的變化，最終實(shí)現(xiàn)車輛組合導(dǎo)航的“即插即用”。

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

本文開展了實(shí)際的實(shí)驗(yàn)測(cè)試，用于驗(yàn)證所提出算法的性能。測(cè)試系統(tǒng)包括采集頻率為50 Hz的車載UWB標(biāo)簽、4個(gè)路側(cè)UWB基站100 Hz的IMU和SCOUT 2.0移動(dòng)機(jī)器人底盤。陀螺儀的零偏穩(wěn)定性為0.3°/h，角度隨機(jī)游走系數(shù)為0.05°/；加速度計(jì)的零偏穩(wěn)定性為50gμ ，速度隨機(jī)游走系數(shù)為0.01m/s/。

此外，考慮到在實(shí)際交通場(chǎng)景中進(jìn)行實(shí)驗(yàn)存在一定安全隱患，且車輛實(shí)際運(yùn)行過(guò)程中的參考軌跡難以獲得，因此在測(cè)試平臺(tái)上額外搭載了高精度的光纖組合導(dǎo)航系統(tǒng)KY-INS 300來(lái)作為運(yùn)行軌跡的參考值。本文的實(shí)驗(yàn)場(chǎng)景為室外停車場(chǎng)，為了增加非視距效應(yīng)，實(shí)驗(yàn)中人為地降低了UWB基站的高度。測(cè)試平臺(tái)與實(shí)驗(yàn)場(chǎng)景如圖4所示。

圖4 測(cè)試平臺(tái)與實(shí)驗(yàn)場(chǎng)景Fig.4 Experimental platform scene

在整個(gè)實(shí)驗(yàn)過(guò)程中，高精度的光纖組合導(dǎo)航系統(tǒng)僅作為參考值，并沒有參與融合定位。根據(jù)不同時(shí)段UWB與IMU測(cè)量值的可用性，多源融合導(dǎo)航系統(tǒng)采用不同的傳感器配置方案。我們分別對(duì)擴(kuò)展卡爾曼濾波（EKF）、因子圖（FG）、采用卡爾曼濾波的傳統(tǒng)交互式多模型距離平滑的因子圖（IMM-FG）以及本文提出的基于改進(jìn)交互式多模型距離平滑的因子圖（AIMM-FG）四種算法進(jìn)行了對(duì)比。圖5和圖6分別展示了這四種算法的融合軌跡對(duì)比圖以及歐氏距離誤差對(duì)比圖，表1羅列了這四種融合定位結(jié)果的歐氏距離誤差統(tǒng)計(jì)數(shù)據(jù)，包括水平位置的均方根誤差值和最大誤差值。

圖5 融合軌跡對(duì)比Fig.5 Fusion trajectory comparison

圖6 歐氏距離誤差對(duì)比Fig.6 Euclidean distance error comparison

表1 四種算法定位性能對(duì)比Tab.1 Positioning performance comparison of four algorithms

首先比較EKF和FG兩種算法的定位效果，從圖6和表1可以看出，EKF在水平位置上的最大誤差和均方根誤差分別為18.35 m和4.63 m，而FG在水平位置上的最大誤差和均方根誤差分別為15.82 m和3.53 m，相比于EKF誤差分別下降了13.79%和23.76%。由此可見，F(xiàn)G算法由于充分考慮了歷史測(cè)量信息，對(duì)UWB非視距誤差的抑制效果更好，因此有著更高的定位精度。而在FG算法融合前首先對(duì)UWB原始測(cè)量信息進(jìn)行抑制的IMM-FG和AIMM-FG算法在定位精度上又有了一定的提升。從表1可以看出，IMM-FG算法在水平位置上的均方根位置降到了2.81 m，相比于FG誤差下降了20.39%；而AIMM-FG算法在水平位置上的均方根位置降到了2.32 m，相比于IMM-FG誤差又下降了17.44% 。由此可見，本文提出的AIMM-FG算法相比于傳統(tǒng)的IMM-FG算法對(duì)UWB的非視距誤差有更高的抑制作用，較好地實(shí)現(xiàn)對(duì)車輛的精準(zhǔn)定位。AIMM-FG算法比EKF算法在定位精度（RMS）上提升了40%以上。

圖7 展示了四種算法的速度估計(jì)誤差，從圖中可以看出本文所提出的方法對(duì)車輛速度的估計(jì)具有更高的精度和更好的收斂效果。

圖7 速度估計(jì)誤差Fig.7 Velocity estimation errors

4 結(jié) 論

本文提出了一種基于交互多模型距離平滑的UWB/IMU因子圖組合導(dǎo)航方法，該方法能夠?qū)WB和IMU等傳感器的非同步測(cè)量數(shù)據(jù)進(jìn)行實(shí)時(shí)融合，從而實(shí)現(xiàn)傳感器“即插即用”的功能。

首先，利用交互式多模型自適應(yīng)的特性以及因子圖算法能夠充分考慮歷史測(cè)量信息的特性，本文提出了一種改進(jìn)的交互式多模型算法來(lái)對(duì)UWB原始測(cè)量值進(jìn)行平滑，從而抑制非視距誤差。其次，通過(guò)因子圖融合交互式多模型輸出的UWB距離信息與IMU測(cè)量值來(lái)達(dá)到異步傳感器的實(shí)時(shí)融合，并在確保實(shí)時(shí)性的基礎(chǔ)上進(jìn)一步提升了定位精度。

本文在純非視距情況下開展了實(shí)車測(cè)試，結(jié)果表明該方法能有效兼容各傳感器的測(cè)量值，同時(shí)相比于EKF算法，本文的方法在定位精度（RMS）上提升了40%以上。