基于粒子濾波的IMU/UWB組域內(nèi)自主導航定位研究*

劉 宇，賀竹川，路永樂，邸 克，文丹丹，鄒新海

(重慶郵電大學 智能傳感技術(shù)與微系統(tǒng)重慶市高校工程研究中心，重慶 400065)

0 引 言

現(xiàn)代科學技術(shù)的發(fā)展，導航定位技術(shù)已經(jīng)在社會的很多領(lǐng)域得到應(yīng)用[1]，對室內(nèi)外導航定位的需求也越來越多。目前，現(xiàn)行室外定位技術(shù)主要憑借全球定位系統(tǒng)(global positioning system,GPS)以及中國北斗衛(wèi)星導航系統(tǒng)(BeiDou navigation satellite system,BDS)進行定位，其定位精度能達到亞米級[2]。由于衛(wèi)星定位只適用于空曠無遮擋地帶，在信號干擾源多、環(huán)境復雜多變的室內(nèi)衛(wèi)星信號的強度和質(zhì)量急速下降，導致定位精度下降甚至無法完成定位。因此，工業(yè)界和科學界均在探求精度更高，可靠性更強的室內(nèi)定位技術(shù)。

目前，利用單一慣性測量單元(inertial measurement unit,IMU)設(shè)備對多個移動節(jié)點組成的班組進行室內(nèi)定位時，整個班組的整體性導航定位誤差會隨著時間的增長而累計，不適合于長時間的室內(nèi)定位導航[3,4]。超寬帶(ultra-wideband,UWB)定位能夠達到較高精度，但由于需要提前搭設(shè)參考基站，在遇到突發(fā)狀況時使用局限性較大，且UWB基站成本高，定位精度容易受到非視距(non line of sight,NLOS)噪聲的干擾[5]。

Tong H B[6]、陳隆亮等人[7]均利用UWB和IMU松耦合進行組合定位，將IMU和UWB分別定位結(jié)果進行融合濾波，該方法需要設(shè)置UWB定位基站，定位效率不高。Olsson F等人[8]提出了選擇以IMU為核心的基礎(chǔ)上，將UWB測距數(shù)據(jù)作為位置觀測值，加入到基于零速度的擴展卡爾曼濾波(EKF)算法中實現(xiàn)融合。然而，EKF本質(zhì)上是觀測方程的線性逼近，基于高度非線性的觀測模型很難對UWB傳感器實現(xiàn)很好的逼近。相比之下，當許多粒子分布在狀態(tài)空間近似于狀態(tài)概率分布時，粒子濾波可以解決大量的非線性或非高斯問題[9,10]。

本文選用粒子濾波算法將IMU測量數(shù)據(jù)以及UWB測距信息進行融合。將IMU數(shù)據(jù)作為粒子濾波的先驗信息，并將UWB測距值作為粒子的觀測值，對節(jié)點位置信息進行預測、更新，提高導航定位的精度。經(jīng)實驗證明，在利用粒子濾波融合的IMU和UWB室內(nèi)定位系統(tǒng)的定位精度，相較于單一IMU慣性導航的定位精度明顯提高。

1 IMU導航與UWB測距模型

1.1 IMU導航模型

IMU是測量物體運動的三軸角速度和三軸加速度的單元。本文將采用行人航跡推算(pedestrian dead reckoning，PDR)算法對IMU采集的移動節(jié)點的三軸角速度和三軸加速度進行解算[11]。采集無其他參考設(shè)備的環(huán)境下佩戴IMU運動人員在行進過程中的加速度、角速度等數(shù)據(jù)，并利用這些數(shù)據(jù)對行進人員進行步長與方向的推算，得到移動節(jié)點的坐標以及方位，從而達到對人員進行定位跟蹤的目的，其中,主要涉及的過程有,步態(tài)檢測、步長和方向計算。圖1所示為PDR算法示意。

圖1 PDR算法示意

PDR初始條件：已知初始位置S0(X0,Y0)，由IMU中內(nèi)置的加速度計對行人的步態(tài)進行檢測，再由陀螺儀對行人角度進行實時監(jiān)測，從而利用下列公式對路徑進行推算

(1)

根據(jù)以上PDR推算公式，可以得出任意時刻k的位置Sk(Xk,Yk)。

1.2 UWB測距模型

UWB測距具有穿透力強、功耗低、抗多徑能力強，安全性能高等優(yōu)勢。本文UWB測距將采用雙向飛行時間法(two way time of flight,TW-TOF)來完成。主要兩個異步收發(fā)機(transceiver)之間的信號飛行時間來測距。每個收發(fā)機啟動時會生成一條獨立的時間戳。如圖2所示，模塊A的發(fā)射機在其時間戳上的Ta1發(fā)射請求性質(zhì)的脈沖信號，模塊B接收機在其時間戳Tb1接收到該信號。模塊B在Tb2時刻發(fā)射一個響應(yīng)性質(zhì)的信號，被模塊A在自己的時間戳Ta2時刻接收。由此可以計算出脈沖信號在兩個模塊之間的飛行時間，從而確定飛行距離S

圖2 UWB測距原理

S=c[(Ta2-Ta1)-(Tb2-Tb1)]/2

(2)

將發(fā)送端發(fā)出的數(shù)據(jù)包和接收回應(yīng)的時間間隔記為TTOT,接收端收到數(shù)據(jù)包和發(fā)出回應(yīng)的時間間隔記為TTAT,那么數(shù)據(jù)包在空中單向飛行的時間TTOF可以簡化為

TTOF=c(TTOT-TTAT)/2

(3)

2 UWB/IMU協(xié)同導航定位模型

2.1 協(xié)同導航定位模型建立

IMU對短期內(nèi)位置變化的估計很準確，但對于長航時的誤差累積非常敏感。特別是移動的節(jié)點來說，不規(guī)則運動對其影響很大。因此，利用UWB的測距數(shù)據(jù)來補償IMU的誤差，從而產(chǎn)生一種新的、更準確、更可靠的室內(nèi)班組自主導航定位系統(tǒng)。

在該組合導航模型當中，將每個節(jié)點自身攜帶的IMU測量慣性數(shù)據(jù)作為系統(tǒng)的狀態(tài)量，當每個移動節(jié)點接收到IMU數(shù)據(jù)，則使用狀態(tài)轉(zhuǎn)移方程更新當前系統(tǒng)狀態(tài)。當接收到班組區(qū)域內(nèi)各節(jié)點之間的UWB測距數(shù)據(jù)，則使用該測距數(shù)據(jù)作為觀測值，將兩者融入一個概率框架之中，得到當前位置的聯(lián)合后驗概率分布，并根據(jù)IMU的信息對粒子進行更新后輸出狀態(tài)估計。最后，通過重新采樣來緩解粒子衰減的問題。其融合算法示意如圖3所示。

圖3 傳感器粒子濾波融合算法示意

2.2 粒子濾波融合算法

(4)

(5)

(6)

(7)

(8)

(9)

式中d(·)為節(jié)點i和節(jié)點j之間距離的平方根。在本文模型中，粒子濾波的步驟為三部分組成。

(10)

重采樣：當有效的樣本大小低于粒子數(shù)M的50%時，將生成一組新的粒子來替代之前的舊粒子樣本集。

根據(jù)仿真驗證，在粒子數(shù)為500時，系統(tǒng)的運行時間較低，定位產(chǎn)生的誤差也較低，綜合性能最優(yōu)。所以，本文試驗將粒子數(shù)M設(shè)定為500。

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

基于上述理論，為驗證該方法的合理性及其定位精度，設(shè)計了UWB和IMU相結(jié)合與單一IMU相對比的實驗方案。如圖4所示，用到的IMU設(shè)備為實驗室自行研制的IMU設(shè)備，UWB模塊選用Time Domain公司的PulsOn440。本文實驗場地選取重慶郵電大學室內(nèi)體育館中心較復雜的室內(nèi)環(huán)境，實驗的行走范圍設(shè)定為50 m×60 m的矩形路徑，實驗由3名人員分別佩戴導航定位設(shè)備在室內(nèi)行走。實驗人員將IMU設(shè)備固定于腰間，為了避免人體對UWB信號產(chǎn)生遮擋，將UWB設(shè)備固定于頭部。實驗一共分為2組，每組實驗圍繞該矩形路徑行走2圈，3名移動人員在起點處相隔1 m出發(fā)，并在實驗過程中保持該距離行走。

圖4 IMU和UWB組合導航定位系統(tǒng)

第一組實驗由3名移動人員組成移動班組佩戴IMU設(shè)備從相同起始點出發(fā)，移動人員的定位信息由采樣頻率為50 Hz的IMU進行采集，并用PDR算法進行解算，得到IMU導航定位結(jié)果。

第二組實驗由3名移動人員組成班組佩戴IMU和UWB導航定位設(shè)備從相同的起始點出發(fā)，其中，UWB的定位信息由串口相連接的IMU內(nèi)設(shè)CPU進行采集，然后用粒子濾波算法進行融合濾波,得到融合定位導航結(jié)果。圖5和圖6分別為IMU定位和IMU/UWB組合定位實際軌跡圖。對比可以得到，僅使用IMU進行導航定位的方案，受到IMU的累積誤差以及航向角偏差的的影響，整個小組行進的路線與真實路線偏差較大，在行進途中各人員雖然保持同步，但軌跡卻很快的發(fā)散，相互偏離。而由IMU/UWB組合的導航系統(tǒng)則與真實路線更加接近，人員軌跡基本保持同步。

圖5 單IMU慣性導航定位效果

圖6 IMU與UWB融合導航定位效果

由圖7和圖8得到，在僅用IMU導航的情況下，受累積影響，3個移動節(jié)點隨著行走時間累計，單個節(jié)點以及班組的整體誤差不斷在增加，實驗過程中單個節(jié)點在X和Y軸上的最大偏差可達到11.8 m和10.7 m，班組整體的組合軌跡偏差最大可達到6.4 m和6.3 m，整體路徑偏離真實路徑較大。而在添加了UWB測距量進行修正后，班組的整體組合軌跡重合度增大，單個節(jié)點和班組整體在X和Y軸上的最大偏差均小于2.1 m，軌跡的偏差明顯降低。

圖7 不同導航定位系統(tǒng)在X軸向的誤差曲線

圖8 不同導航定位系統(tǒng)在Y軸向的誤差曲線

由表1可知，在單一IMU導航的情況下，在X軸向上，3個移動節(jié)點組成的班組區(qū)域在行走期間的平均累積誤差為2.78 m，Y軸向上為3.08 m。在結(jié)合UWB后，班組區(qū)域平均累積誤差在X軸上縮小為1.07 m，在Y軸向上縮小為1 m。誤差較單一IMU慣性導航減小了61.5 %和67.5 %，有效地提高了班組的整體導航定位的精度。

表1 X軸和Y軸方向上不同系統(tǒng)的平均誤差 m

4 結(jié) 論

單一IMU慣性導航的長航時累積誤差會導致定位偏差，本文提出的無需外界輔助信號源，將IMU慣性信息和班組區(qū)域內(nèi)的UWB測距信息結(jié)合導航定位的方法，相對單一IMU慣性導航，增加了精確的距離觀測值，利用粒子濾波將2種數(shù)據(jù)進行融合，以提高導航定位的精度。通過實驗驗證，將IMU和UWB結(jié)合的方案與單一器件的定位誤差相比在X軸和Y軸上各自降低61.5 %和67.5 %，班組實際行走軌跡與參考軌跡重合，導航定位精度得到明顯的提升。