基于卡爾曼濾波的UWB與里程計融合定位方法

黃俊杰，李海濱，賈翠玲

(1.內(nèi)蒙古工業(yè)大學(xué)材料科學(xué)與工程學(xué)院，內(nèi)蒙古呼和浩特 010051；2.內(nèi)蒙古工業(yè)大學(xué)工程訓(xùn)練教學(xué)部，內(nèi)蒙古呼和浩特 010051)

0 前言

隨著物聯(lián)網(wǎng)技術(shù)的快速發(fā)展，工廠、商店、醫(yī)院等都在走向智能化，人們對定位的穩(wěn)定性和精度的要求變得越來越高。在室外環(huán)境中，依靠全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System，GNSS)如GPS、北斗等與慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)的組合導(dǎo)航可以達到很高的定位精度，但是由于GNSS信號容易受復(fù)雜環(huán)境影響，在室內(nèi)無法提供有效的定位數(shù)據(jù)，因此開發(fā)可靠的室內(nèi)定位系統(tǒng)(Indoor Positioning System，IPS)具有很高的實用價值。

超寬帶(Ultra-Wideband，UWB)是利用極窄脈沖傳輸?shù)凸β薀o線信號的新興通信技術(shù)，時間分辨率高，定位精度可以達到厘米級別，在穿透性、抗多徑能力和抗干擾性方面都有著很好的表現(xiàn)，加上高的傳輸功率和低功耗的特點，所以UWB廣泛應(yīng)用于室內(nèi)高精度定位。但是同其他無線定位系統(tǒng)一樣,數(shù)據(jù)抖動和非視距環(huán)境下的數(shù)據(jù)失真也是UWB系統(tǒng)存在的主要問題。從動輪式里程計固定在機器人上，結(jié)合陀螺儀的角度信息可以對機器人的位置信息進行實時計算，具有結(jié)構(gòu)簡單、成本低、短時精度高等優(yōu)點。從動輪式里程計在短時內(nèi)有很好的定位效果，但是隨著時間增長會產(chǎn)生較大的累計誤差，所以單一的里程計模式不適合長時間定位。文獻[9]提出了一種結(jié)合馬爾可夫鏈和指紋匹配算法的UWB技術(shù)，是一種解決NLOS(Non-Line of Sight)誤差問題的有效方法，但是需要先對UWB的數(shù)據(jù)在指紋數(shù)據(jù)庫中進行匹配，計算量過大。文獻[10]提出了一種基于UWB、里程計和RGB-D融合的室內(nèi)定位方法，使用了加權(quán)平均的思想，成功將誤差抑制在10 cm以內(nèi)，但并沒有對UWB處于嚴重非視距狀態(tài)下的情況進行處理。文獻[11]提出了一種基于改進自適應(yīng)加權(quán)數(shù)據(jù)融合的粒子濾波定位算法，與擴展卡爾曼融合算法相比其定位精度提高了15%以上。文獻[12]通過求解LiDAR(Light Detection and Ranging)定位信息與粒子之間的幾何距離來更新粒子的權(quán)重,從而彌補UWB的非視距誤差。但是這2種方法都沒有對嚴重非視距狀態(tài)進行判斷，而且沒有對IMU(Inertial Measurement Unit)和LiDAR的累計誤差進行判斷和消除，導(dǎo)致系統(tǒng)長時間運行效果不好。

針對上述問題，本文作者提出一種基于卡爾曼濾波的UWB和里程計數(shù)據(jù)融合的定位方法。該方法將里程計短時精度高和UWB長時誤差有界性的優(yōu)點進行融合，有望在單片機上實現(xiàn)長時高精度定位。作者先以里程計的數(shù)據(jù)作為參考量對UWB數(shù)據(jù)進行非視距情況的判斷；然后使用卡爾曼濾波算法對里程計和UWB數(shù)據(jù)進行不同信賴度下的融合；再使用融合后的數(shù)據(jù)對里程計數(shù)據(jù)進行累計誤差的判斷和修正，來減小里程計的累計誤差，提高系統(tǒng)長時間運行的精度和穩(wěn)定性，以實現(xiàn)長時高精度定位。

1 傳感器原理及誤差產(chǎn)生原因

1.1 超寬帶定位

1.1.1 定位算法原理

UWB基站和標簽使用雙向飛行時間法(Two Way-Time of Flight, TW-TOF)可以得到基站和標簽之間的距離，如圖1所示。

圖1 TW-TOF測距方法 圖2 三邊定位算法原理

標簽在時刻發(fā)射請求性質(zhì)的脈沖信號，基站在時刻接收到信號，在時刻發(fā)射一個響應(yīng)性質(zhì)的脈沖信號，標簽在時刻接收到，由此可以計算出信號總飛行時間和處理時間。無線電傳播速度為光速，則可以得到兩模塊的距離。

(1)

在超寬帶定位過程中，采用三基站一標簽的定位方法，使用三邊定位算法得到標簽的坐標信息，如圖2所示。

假設(shè)標簽的坐標是(,)，第個基站的坐標是(,)，基站到標簽的距離是，此時二維模型下標簽坐標滿足式(2)：

(2)

將式(2)各公式進行平方后得到一個非線性方程組，用方程組中的前-1個方程減去第個方程，整理后得到線性方程組=

其中：

(3)

(4)

使用最小二乘法求解方程可以得到標簽坐標：

=()

(5)

1.1.2 非視距誤差產(chǎn)生原因

當UWB信號傳播遭到障礙物的阻擋而無法直線傳輸時，就會出現(xiàn)UWB定位的非視距問題。在非視距傳播情況下，信號只能穿透障礙物或者借助反射、折射和衍射的方式進行傳輸，使得到的信息含有大量失真信號，嚴重影響三邊定位算法的計算精度。

1.2 輪式里程計定位

輪式里程計定位系統(tǒng)由圖3所示的正交從動輪式里程計和一個JY61六軸陀螺儀組成，輪式里程計由2個正交的全向輪和2個光電編碼器組成，由陀螺儀來提供姿態(tài)角。

圖3 正交從動輪式里程計

將里程計固定到移動機器人底盤上，里程計的2個全向輪相對于移動機器人車體中心的垂直距離分別為和。計算單個采樣周期內(nèi)機器人的位移坐標增量，然后積分，從而得到機器人相對于參考坐標系的位姿狀態(tài)。如圖4所示，在一個采樣周期內(nèi)，2個光電編碼器測得2個全向輪旋轉(zhuǎn)軌跡的增量為d和d。機器人相對于機器人車體坐標系的位移增量分別記為d和d,可以由d和d以及機器人旋轉(zhuǎn)角與、等參數(shù)表示。機器人相對于參考坐標系的位移增量分別記為d和d，可由d和d及機器人旋轉(zhuǎn)角表示。機器人的旋轉(zhuǎn)角度為可由JY61六軸陀螺儀給出。

圖4 機器人移動姿態(tài)

最后將d和d進行積分，得到機器人相對于參考坐標系的位姿信息(,,)。

(6)

輪式里程計的數(shù)據(jù)具有信號頻率高、短時精度高的特點，但是如果不經(jīng)過校核，容易產(chǎn)生較大的累計誤差。

2 基于卡爾曼濾波的數(shù)據(jù)融合定位系統(tǒng)

輪式里程計配合JY61六軸陀螺儀通過公式(6)可以得到機器人位姿信息，短時精度高，但是長時會有較大的累計誤差，數(shù)據(jù)可信度降低。UWB可以得到一個沒有累計誤差的坐標信息，視距情況下可維持10 cm左右的定位精度，但在非視距環(huán)境下會出現(xiàn)數(shù)據(jù)失真導(dǎo)致數(shù)據(jù)可信度降低?；诶锍逃嫸虝r精度高和UWB長時誤差的有界性，將2種信息進行有機融合以達到長時高精度定位的目的。首先用輪式里程計所得數(shù)據(jù)作為參考量，對UWB進行狀態(tài)判斷，判斷UWB是否處于視距環(huán)境中；再使用卡爾曼濾波算法對里程計和UWB進行不同信賴度下的融合；然后使用融合得到的數(shù)據(jù)對里程計數(shù)據(jù)進行累計誤差的判斷與修正，從而降低里程計的累計誤差,提高系統(tǒng)定位精度。數(shù)據(jù)融合的計算流程如圖5所示。

圖5 數(shù)據(jù)融合的計算流程

2.1 判斷UWB狀態(tài)

先對UWB數(shù)據(jù)進行一次一階低通濾波，以初步減小數(shù)據(jù)的抖動，然后分兩層對UWB數(shù)據(jù)狀態(tài)進行判斷。

第一層先對UWB的單點數(shù)據(jù)進行判斷，需要獲知UWB基站是否得到標簽的距離信息，因為在定位中可能出現(xiàn)障礙物引起基站無法得到標簽信息的情況，使三邊定位算法不能正常使用，導(dǎo)致UWB數(shù)據(jù)失效。通過第一層的判斷可以排除無法得到單點數(shù)據(jù)的情況，如能得到單點數(shù)據(jù)則進入第二層判斷。

在第二層中對UWB數(shù)據(jù)做是否進入非視距狀態(tài)與非視距程度的判斷。用當前的UWB數(shù)據(jù)對里程計的數(shù)據(jù)求距離偏差，在里程計的數(shù)據(jù)上設(shè)定多層環(huán)形區(qū)域，通過判斷UWB所屬區(qū)域?qū)Ψ且暰喑潭冗M行分類量化，為后續(xù)數(shù)據(jù)融合提供依據(jù)。將UWB所處狀態(tài)分為3類，則有式(7)所示分類量化結(jié)果。

(7)

式中：為第類視距狀態(tài),其中=(1,2,3)；表示狀態(tài)閾值；為距離偏差值。表示UWB處于視距狀態(tài)，因為文中實驗使用的UWB系統(tǒng)的定位精度為10 cm，所以取值可在15 cm左右，但是不能小于UWB本身的定位精度。表示UWB處于輕度非視距狀態(tài)，這個非視距狀態(tài)的數(shù)據(jù)尚可以使用，具有一定的可信度，表示允許使用的非視距狀態(tài)誤差距離的最大值。表示UWB已經(jīng)處于嚴重的非視距狀態(tài)，數(shù)據(jù)已經(jīng)完全不可信。經(jīng)過上述兩層判斷，實現(xiàn)了以里程計為參考量的UWB非視距程度的分類量化。

在上述工作基礎(chǔ)上，將對里程計數(shù)據(jù)和分類量化后的UWB數(shù)據(jù)進行數(shù)據(jù)融合。

2.2 基于卡爾曼濾波算法的數(shù)據(jù)融合

卡爾曼濾波的數(shù)學(xué)模型包括預(yù)測模型和觀測模型2個部分,離散化的形式如式(8):

(8)

(9)

(10)

(11)

(12)

(13)

在上述卡爾曼濾波算法公式中已省略對單位矩陣的書寫。

用式(7)分類量化的結(jié)果設(shè)置參數(shù)，表示UWB數(shù)據(jù)處于視距狀態(tài)，則和可以設(shè)置為相同的值，即設(shè)里程計和UWB值具有相同的可信度，這對參數(shù)使用、表示。表示UWB處于輕度非視距狀態(tài)，則通過增大、減少來提高系統(tǒng)對里程計的信賴度，這對參數(shù)用、來表示。表示UWB處于嚴重非視距狀態(tài)，UWB數(shù)據(jù)失去作用，此時系統(tǒng)以里程計數(shù)據(jù)為準，設(shè)置為單位矩陣，設(shè)置為零矩陣，此時=、=。綜上所述，將UWB的視距狀態(tài)與和的參數(shù)取值歸納為式(14)。

(14)

在UWB處于不同狀態(tài)下使用式(14)改變預(yù)測值噪聲的協(xié)方差和觀測值噪聲的協(xié)方差，從而改變?nèi)诤舷到y(tǒng)對觀測和預(yù)測的信賴程度，進而達到自適應(yīng)的效果，提高了系統(tǒng)的穩(wěn)定性。

2.3 里程計累計誤差的判斷和修正

使用融合后的數(shù)據(jù)來判斷里程計的累計誤差。以得到的融合后的數(shù)據(jù)為圓心設(shè)定一個圓形許用區(qū)域，通過判斷里程計數(shù)據(jù)所在區(qū)域?qū)锍逃嬂塾嬚`差進行判斷。如果里程計的數(shù)據(jù)連續(xù)多次超出圓形許用區(qū)域，則表明里程計已經(jīng)產(chǎn)生了較大的累計誤差。設(shè)定的圓形許用區(qū)域半徑越大則對里程計的累計誤差的容忍度越大，圓形許用區(qū)域半徑越小則對里程計的修正會越靈敏。但實驗使用的UWB的定位精度為10 cm，視距情況下的數(shù)據(jù)融合對里程計和UWB信賴度相同，融合后的數(shù)據(jù)在兩者之間，所以圓形許用區(qū)域的最小半徑不能小于5 cm。

(Corrected)=(orignal)+

(15)

再用修正的里程計的數(shù)據(jù)參與數(shù)據(jù)融合，從而提高系統(tǒng)長時定位的精度。

3 仿真與實驗驗證與分析

3.1 仿真實驗

仿真實驗使用開源機器人仿真軟件Webots，仿真機器人由全向運動底盤、輪式里程計和UWB定位系統(tǒng)組成。其中全向運動底盤和輪式里程計通過在Webots中直接建模生成，UWB定位系統(tǒng)使用軟件中的GPS傳感器通過設(shè)定傳感器的噪聲來模擬產(chǎn)生不同狀態(tài)的UWB信號。仿真機器人模擬完成如圖6所示的蛇形運動，先向軸正方向運動3 m，然后向軸正方向運動1 m，向軸負向運動3 m，軸正向運動1 m，如此反復(fù)，得到的數(shù)據(jù)如圖6—圖7所示。

圖6 理想軌跡與UWB數(shù)據(jù)形成的軌跡 圖7 理想軌跡與里程計數(shù)據(jù)形成的軌跡

圖6為UWB數(shù)據(jù)的軌跡圖，可以看出：機器人從(0，2)到(2，2)的運動期間設(shè)定UWB為非視距狀態(tài)，具體表現(xiàn)為UWB數(shù)據(jù)抖動幅度變大。圖7為里程計數(shù)據(jù)的軌跡，里程計在運行過程中產(chǎn)生了累計誤差，隨著時間的增加誤差也在增加。

圖8為最終融合得到的數(shù)據(jù)，系統(tǒng)成功判斷出UWB的狀態(tài)，并進行了不同信賴度的融合，得到穩(wěn)定的數(shù)據(jù)。并且在(3.057,2.625)m點處融合后的數(shù)據(jù)判斷出里程計的累計誤差，并對里程計數(shù)據(jù)進行了修正。然后使用修正過的里程計數(shù)據(jù)繼續(xù)與UWB進行融合，減小了融合后數(shù)據(jù)的累計誤差，提高了系統(tǒng)長時間運行的穩(wěn)定性。

圖8 理想軌跡與最終融合后數(shù)據(jù)的軌跡

圖9為每個UWB原始數(shù)據(jù)與理想位置坐標的誤差和每個融合后數(shù)據(jù)與理想位置坐標的誤差對比，可以看出：融合后的數(shù)據(jù)相對于UWB原始數(shù)據(jù)的抖動和誤差都明顯減小，而且在非視距情況融合后的數(shù)據(jù)依然有很好的穩(wěn)定性和準確性。融合后的數(shù)據(jù)誤差會隨著時間的增加逐漸變大，這是由于里程計的累計誤差逐漸增大所致，但當里程計的累計誤差達到一定程度時，又會通過融合后的數(shù)據(jù)對里程計數(shù)據(jù)進行修正，從而將里程計數(shù)據(jù)的累計誤差控制在一定范圍之內(nèi)。UWB原始數(shù)據(jù)的平均誤差為0.101 5 m，融合后的平均誤差為0.048 3 m，融合后的數(shù)據(jù)誤差減少了52.41%，達到了良好的預(yù)期效果。

圖9 UWB原始數(shù)據(jù)的誤差和融合后數(shù)據(jù)的誤差對比

3.2 實驗驗證與分析

為進一步驗證文中方法在單片機系統(tǒng)中的應(yīng)用效果，擬開展實物機器人定位實驗研究。實驗采用基于Decawave公司的DW1000芯片的UWB產(chǎn)品，該產(chǎn)品定位范圍為50 m，組成的系統(tǒng)定位誤差為10 cm左右。采用的輪式里程計為自制的正交從動式輪式里程計如圖3所示，平均運行10 m會產(chǎn)生5 cm的累計誤差。JY61六軸陀螺儀采用TDK公司的MPU6050姿態(tài)傳感器，通過JY61的處理器讀取MPU6050的測量數(shù)據(jù)然后通過串口輸出，靜態(tài)測量精度穩(wěn)定在0.05°，動態(tài)精度為0.1°。實驗場景為9 m×21 m的實驗室，由3個UWB基站組成一個等腰三角形，所有基站在距地面高為2.40 m的同一平面上，UWB標簽置于機器人頂端。機器人實物系統(tǒng)如圖10所示。

圖10 實驗機器人的圖片

實驗分為5組，每組做5次實驗，每組實驗分別為單獨使用輪式里程計、在視距情況下單獨使用UWB、在視距情況下進行文中方法的融合、在存在8 m非視距情況下單獨使用UWB、在存在8 m非視距情況下進行文中方法的融合，以所得實驗數(shù)據(jù)分別作為定位坐標進行運動控制。利用UWB標簽加蓋來產(chǎn)生非視距情況。

理想實驗軌跡與仿真時的相同，一次實驗共運行20 m，平均運行時間為59.82 s，其中2～10 m區(qū)間段為產(chǎn)生非視距的區(qū)域。機器人以stm32f103單片機為控制器，進行數(shù)據(jù)處理和機器人控制。機器人每10 ms進行一次閉環(huán)糾偏,每次的糾偏距離是機器人當前坐標與終點和起點直線的垂直距離，即當前機器人偏離目標位置的距離。機器人將用這個偏離的距離作為輸入量，使用PID算法對機器人進行軌跡糾偏，以保證機器人回到目標路徑上?？偧m偏距離可以反映機器人一段時間內(nèi)運動的平穩(wěn)性，總糾偏距離越小表明機器人運動越平穩(wěn)，糾偏距離為零表示機器人保持在理想路徑之上。將所得實驗數(shù)據(jù)列于表1。

表1 得到的5組數(shù)據(jù)

由表1可以看出:單獨使用里程計的數(shù)據(jù)運動20 m平均產(chǎn)生了100 mm的終點偏差，這個誤差是長距離運動導(dǎo)致的累計誤差，且這個誤差會隨著運動距離的增加而增加，但里程計數(shù)據(jù)穩(wěn)定，總糾偏距離只有5.696 m，機器人運行平穩(wěn)。

使用純UWB數(shù)據(jù)進行定位的機器人運動過程中抖動極大，尤其是在非視距狀態(tài)下UWB數(shù)據(jù)極不穩(wěn)定，終點偏差分別為112、123 mm，在視距狀態(tài)下運動20 m平均產(chǎn)生了152.844 m的總糾偏距離，在存在8 m的非視距狀態(tài)下運動更是平均產(chǎn)生了292.265 m的總糾偏距離，這表明單獨使用UWB數(shù)據(jù)不適合機器人定位。

使用文中算法在一直處于視距狀態(tài)下運動20 m終點偏差為37 mm，平均總糾偏距離為28.224 m，機器人運行較平穩(wěn)。在存在8 m的非視距環(huán)境中運動20 m，終點偏差為51 mm，平均總糾偏距離為27.764 m，系統(tǒng)成功判斷出UWB是否處于非視距狀態(tài)并自動進行了不同信賴度的數(shù)據(jù)融合，大幅減小了UWB數(shù)據(jù)的抖動，并且將里程計數(shù)據(jù)的累計誤差控制在一定范圍內(nèi)，得到的數(shù)據(jù)穩(wěn)定且精度較高。相比單獨使用UWB的數(shù)據(jù)，在視距環(huán)境中穩(wěn)定性提高了81.5%，精度提高了67.1%；在存在8 m非視距的環(huán)境中穩(wěn)定性提高了90.5%，精度提高了59.2%。由此可見，使用文中融合方法可以大幅提高定位系統(tǒng)的穩(wěn)定性和精度。

4 結(jié)論

本文作者利用里程計的短時精度高和UWB的長時誤差有界性，提出了一種基于卡爾曼濾波的數(shù)據(jù)融合方法，成功地將2種信息有機融合達到了長時高精度定位的目的。通過軟件仿真和實驗驗證表明：在UWB一直處于視距環(huán)境中融合后的數(shù)據(jù)相比單獨使用UWB的數(shù)據(jù)穩(wěn)定性提高了81.5%，精度提高了67.1%；在存在8 m非視距環(huán)境中，相對于單獨使用UWB的數(shù)據(jù)穩(wěn)定性提高了90.5%，精度提高了59.2%。系統(tǒng)運行穩(wěn)定，實現(xiàn)了長時間連續(xù)可靠的定位，具有較大的工程應(yīng)用價值。