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

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

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

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

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

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

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

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

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

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

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

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

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

1.2 因子圖模型的求解

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

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

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

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

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

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

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

在過去幾年中，交互式多模型算法已經(jīng)廣泛地應(yīng)用于車輛定位等領(lǐng)域[12,13]，在這類研究中，可能的狀態(tài)變量由多個不同的模型表示，這些模型根據(jù)不同的條件建立。交互式多模型算法通過計算各個模型的權(quán)重值來獲取最終的狀態(tài)估計，因此具有自適應(yīng)的特點。然而，傳統(tǒng)的交互式多模型算法大多采用卡爾曼濾波來對模型進(jìn)行估計，這類算法未能充分考慮歷史觀測信息，對視距/非視距模型不能起到較好的平滑作用?；诖?，在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個因子圖模型輸出的邊緣化協(xié)方差矩陣。

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

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

模型j的更新概率：

歸一化常數(shù)：

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

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

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

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

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

2.1 先驗因子節(jié)點

2.2 IMU因子節(jié)點

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

因此，運動模型可表示為：

速度模型可表示為：

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

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

2.3 IMM因子節(jié)點

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

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

3 實驗結(jié)果與分析

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

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

圖4 測試平臺與實驗場景Fig.4 Experimental platform scene

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

圖5 融合軌跡對比Fig.5 Fusion trajectory comparison

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

表1 四種算法定位性能對比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算法由于充分考慮了歷史測量信息，對UWB非視距誤差的抑制效果更好，因此有著更高的定位精度。而在FG算法融合前首先對UWB原始測量信息進(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算法對UWB的非視距誤差有更高的抑制作用，較好地實現(xiàn)對車輛的精準(zhǔn)定位。AIMM-FG算法比EKF算法在定位精度（RMS）上提升了40%以上。

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

圖7 速度估計誤差Fig.7 Velocity estimation errors

4 結(jié) 論

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

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

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