UWB與里程計融合的室內移動機器人定位設計

徐慶坤，王天皓，宋中越

（天津中德應用技術大學新能源系，天津300350）

1 引言

隨著傳感器與智能技術的發(fā)展，室內自主移動機器人定位算法也被持續(xù)地提出與改進[1]。近年來，室內定位中應用比較廣泛的傳感器有UWB、激光雷達、攝像頭、IMU、里程計、超聲波等。UWB（Ultra Wideband）[2，3]，即超寬帶技術，此技術可追溯至19世紀，如今已發(fā)展為一種新興的、全新的無載波無線通信技術。它集無線通信與實時感知定位于一體，與其他室內定位技術相比，它具有傳輸速率高、帶寬極寬、功耗低、輻射小、抗干擾能力強、定位精度高、安全可靠性高等優(yōu)點[4-5]，已成為性價比較高定位方法。但由于其具有超高的時間分辨率，因此，往往會受到非視距（non-line of sight，簡稱NLOS）[6-8]誤差的影響，它是影響UWB定位精度與可靠性的主要來源。

為了有效抑制UWB定位的非視距誤差，提高室內移動機器人的定位精度與可靠性，眾多學者提出了很多算法。文獻[9]提出一種以Chan算法為基礎的非視距誤差鑒別和消除的TDOA定位算法，首先利用Chan算法定位獲得定位目標位置的初始估計，再通過TDOA測量誤差鑒別非視距，最后通過將修正后的測量值代入Chan算法并獲得定位目標的精確位置。文獻[10]提出了一種基于UWB與激光雷達組合的室內定位方法，充分利用UWB的超高的分辨率、良好的抗多徑能力以及激光雷達高效、高精度等特點，在復雜環(huán)境下將兩種傳感器數(shù)據(jù)進行EKF融合，從而獲得厘米級定位。

綜上所述，基于UWB與里程計兩種不同類型的傳感器，充分利用UWB定位精度高以及里程計短時定位效果好等特點，將二者進行融合，各取所長，從而獲得高可靠性、高精度的定位效果。視距（line of sight，簡稱LOS）環(huán)境下，利用UWB高精度定位數(shù)據(jù)消除里程計長距離長時間所產生的累積誤差；非視距環(huán)境下，利用里程計航跡推演算法獲得機器人的定位信息來彌補UWB非視距引起的中斷區(qū)域影響。

2硬件系統(tǒng)設計

本系統(tǒng)硬件由定位基站與定位單元兩部分組成，其中定位基站由A0、A1、A2、A3組成，定位基站采用DecaWave公司出品的DW1000芯片作為UWB的收發(fā)器，它是一款厘米級的低功耗低成本的室內定位芯片，可用于雙向測距或時差定位系統(tǒng)。定位單元安裝于移動機器人平臺上，主要由主控單元、編碼器、標簽T0與標簽T1組成。編碼器選用增量式編碼器，脈沖數(shù)為1024，集電極輸出方式。標簽T0、T1與定位基站（A0、A1、A2、A3）的硬件組成完全一致，僅在軟件上有所區(qū)別。主控單元選用32位STM32F103系列ARM微控制器作為定位系統(tǒng)的處理單元，它采用Cortex-M3內核，CPU最高速度可達72MHz。移動機器人定位系統(tǒng)硬件結構，如圖1所示。

圖1定位系統(tǒng)硬件框圖Fig.1 Hardware Diagram of Positioning System

3定位技術分析

3.1 UWB定位分析

UWB定位相比于藍牙或WIFI定位的方法不同，它不是基于信號強度（RSSI）方法實現(xiàn)定位，而是通過對無線信號的發(fā)送時刻與接收時刻進行精確捕獲，進而計算得出基站與標簽之間的精準距離，并通過相應算法解算獲得定位信息。目前比較流行的兩種定位方法為到達時間差法（TDOA，Time Difference of Arrival）與飛行時間測距法（TOF，Time Of Flight）[11-13]。TDOA利用到達時間差方法進行定位，定位標簽僅需對外發(fā)射一次UWB信號，根據(jù)各基站接收到信號的時間差即可計算出距離信息，這種方法要求各基站必須為嚴格的時間同步，若不同步，此方法無效。

本系統(tǒng)采用TOF測距方法實現(xiàn)室內移動機器人的定位。TOF是一種雙向測距技術，其雙向測距算法流程，如圖2所示。從圖中可看出，標簽定時發(fā)送帶有時間戳Tsp的輪詢信息，基站收到信息后記錄時間Trp，并將帶有時間戳Tsr的響應信息發(fā)送至標簽，標簽接收到信息后記錄時間Trr，并將帶有時間戳Tsf以及以上時間戳的信息發(fā)送至基站，基站接收到信息后通過式（1）與式（2）計算得出標簽與基站間的距離。

式中：Tof—單程飛行時間；（Tsr-Trp）—基站的反應時間；（Tsf-Trr）—標簽的反應時間。

式中：D—標簽與基站間的距離；C—光速。

TOF測距方法的誤差來源主要為隨機誤差與NLOS誤差，其中隨機誤差服從高斯分布，影響不大，可通過濾波算法進行有效濾除；而NLOS誤差具有偶發(fā)性，常常由于人為遮擋或者穿墻等因素的影響，致使測量數(shù)據(jù)較大偏離數(shù)據(jù)真值，甚至在受到嚴重遮擋時，會造成數(shù)據(jù)丟失而無法定位，因此，為了提高UWB定位的精度與可靠性，必須對NLOS誤差進行抑制。

圖2雙向測距算法流程圖Fig.2 Flow Chart of Two-way Ranging Algorithm

3.2里程計定位分析

里程計（Odometry）是一種根據(jù)傳感器所檢測到的數(shù)據(jù)以估計物體的位置與時間的關系的方法[14-16]，目前已廣泛應用于移動機器人定位系統(tǒng)。目前里程計的航跡推演定位方法主要基于光電編碼器來實現(xiàn)，通過在采樣周期內采集光電編碼器脈沖的變化量計算車輪的轉動圈數(shù)，已知車輪的周長，進而間接地計算機器人的移動距離與方向角的變化量。

這種方法實現(xiàn)相對簡單，但是這種定位方法存在以下缺點：（1）當移動機器人如遇到比較光滑的特殊地形，會造成車輪打滑空轉現(xiàn)象，導致里程計出現(xiàn)無效的累積情況；（2）里程計航跡推演算法由于存在累積誤差，不適合移動機器人長距離的定位場合。

3.3 UWB與里程計融合定位機制

針對上述室內移動機器人定位傳感器存在可靠性差，隨時間推移誤差累積增大，復雜環(huán)境下非視距效應等問題，提出了一種基于UWB與里程計融合的定位機制，融合定位算法流程，如圖3所示。

圖3 融合定位算法流程圖Fig.3 Flow Chart of Fusion Location Algorithm

（1）為了解決經緯度、環(huán)境障礙物等現(xiàn)場環(huán)境因素對UWB測距值的干擾影響，首先需要對UWB基站與標簽進行一次校準標定；針對TOF測距法存在的隨機誤差問題，采用卡爾曼濾波算法對測距值進行濾波處理；

（2）針對TOF測距法存在的非視距誤差問題，采用融合里程計方法來消除非視距對移動機器人定位的影響。首先利用NLOS誤差鑒別方法進行鑒別，如鑒別結果為非視距場景，將采用里程計航跡推演的定位結果對UWB測距進行補償；

（3）為解決里程計所產生的固然累積誤差，通過NLOS誤差鑒別方法鑒別現(xiàn)場環(huán)境，如鑒別結果為視距場景，將UWB高精度定位結果對組合里程計進行累積誤差矯正。

4 融合算法

4.1 卡爾曼濾波算法

卡爾曼（Kalman）濾波是最佳的線性濾波器[17，18]，實現(xiàn)簡單，廣泛應用于導航、定位、傳感器數(shù)據(jù)融合等領域。本文采用其對UWB測距值進行處理，有效地過濾了隨機誤差。卡爾曼濾波器的狀態(tài)方程與觀測方程分別為：

式中：k—離散時刻，A與H分別為系統(tǒng)的轉移矩陣與觀測矩陣；x→k—k時刻輸出的狀態(tài)；x→k-1—k-1時刻輸出的狀態(tài)；B—系統(tǒng)的控制矩陣；u→k—k時刻的控制量；ω→—系統(tǒng)的過程噪聲；z→k—k時刻的測量值；v→k—系統(tǒng)的觀測噪聲。卡爾曼濾波算法的迭代過程為：

式中：P k|k-1—k-1時刻預測誤差協(xié)方差矩陣；P k-1|k-1—k-1時刻估計誤差的協(xié)方差矩陣；Q—系統(tǒng)的過程噪聲的協(xié)方差，可由誤差方差代替；K—卡爾曼增益，用于修正預測值；R—測量噪聲對應的協(xié)方差矩陣，它反映了傳感器的精度；I—單位矩陣。

4.2 里程計航跡推演算法

在做移動機器人定位導航或者方向控制時，通常會用到航跡推演。根據(jù)機器人輪子的編碼器測得四個輪子的速度以及運動底盤的運動模型，可解算出機器人的線速度（Vx，Vy）與角速度ω，再將其進行積分處理獲得機器人的坐標。

航跡推演算法，如圖4所示。將機器人的起始點作為坐標系的原點，起始航向作為坐標系的Y方向，在較小的時間間隔d t下，根據(jù)幾何算法可近似得出此時機器人的坐標（x1，y1）與航向角β。推導公示，如式（10）與式（11）所示。

圖4 航跡推演示意圖Fig.4 Diagram of Odometry

將公示化簡得：

4.3 里程計累積誤差矯正

里程計航跡推演算法的一個缺點是推導出的位置值誤差與不確定性是累積的，會隨著時間增加而增長，因此其定位精度取決于初始位置與航向估計，而UWB定位屬于絕對定位方法，在視距情況下它具有較高的定位精度。對于位置累積誤差，本文采用累積誤差矯正方法來清除里程計的位置累積誤差，首先根據(jù)NLOS鑒別方法鑒別環(huán)境情況，如為視距場景，那么將UWB的定位坐標作為里程計的初始位置。對于航向估計累積誤差，本文采用雙標簽航向解析方法來清除里程計的航向誤差累積，首先求解UWB標簽T0與標簽T1的定位坐標，然后對坐標進行航向解析，最后將航向結果作為里程計的初始航向估計。

4.4 UWB非視距補償方法

卡爾曼濾波迭代算法可有效地抑制隨機誤差對測距值的影響，但對于較大的NLOS誤差，如標簽與基站之間存在障礙物等干擾時，其抑制效果明顯降低，因此，在對UWB基站進行環(huán)境布局時應充分做好測試環(huán)節(jié)，保證UWB定位標簽不能出現(xiàn)長時間丟失信號狀態(tài)。在此前提下，本文提出一種非視距補償方法。

通過引入NLOS誤差鑒別法來鑒別現(xiàn)場環(huán)境。首先，常態(tài)下取，式中，→zk—實際觀測值—狀態(tài)向量的預測值。通過將二者的差值e與閾值TH進行比較，如果e小于閾值TH，可認為此時為LOS場景；如果e大于閾值TH，或者丟失的時間大于設定的失效時間時可認為此時為NLOS場景，此時將里程計獲得的定位坐標經進行反解算，從而得到標簽與各基站的距離信息，再將各距離信息作為卡爾曼濾波的測距值。

5 定位實驗

本文實驗安排在工業(yè)實訓中心E1-5樓車間進行。實驗平臺選用自主開發(fā)的倉儲物流機器人平臺，如圖5所示。它選用四個麥克納姆輪作為機器人的車輪，每個車輪分別安裝了電機與編碼器；選用UWB Mini 3s模塊作為本實驗的定位產品，它采用DW1000芯片作為UWB收發(fā)芯片，通訊距離在無遮擋情況下可達80m，實驗中使用三個基站（Anchor）與兩個標簽（Tag）組成，測試時將基站固定在三腳架上，距離地面1.5m以上。測試實驗場地布局，如圖6所示。各基站分別固定在距離地面1.5m的三腳架上，并布置在測試區(qū)域的角落。

圖5 移動機器人平臺Fig.5 Mobile Robot Platform

圖6 實驗場地Fig.6 Experiment Site

5.1 UWB測距數(shù)據(jù)標定

由于現(xiàn)場環(huán)境不同，受經緯度、海拔等因素的干擾，須對UWB模塊測距值進行校準，校準只需在現(xiàn)場對1個基站（Anchor）與1個標簽（Tag）進行校準一次即可。具體標定方法為：基站A0固定不動，將安裝有標簽T0的移動機器人通過手動遙控方式從1m位置移動至10m位置，步長為1m，共選取10個點，在每個點的位置處測量100組測距數(shù)據(jù)，再經求平均值得到該點的測距值，以此類推，共得到10組UWB測距值，再利用線性方程y=k x+b進行數(shù)據(jù)擬合，最終獲得修正系數(shù)，測距數(shù)據(jù)擬合直線，如圖7所示。

圖7 測距數(shù)據(jù)擬合圖Fig.7 Fitting Diagram of Experiment Data

5.2 組合定位實驗

實驗選取測試場地中P1、P2、P3三點進行運動測試，測試路線，如圖6所示。實驗過程中，通過手動控制機器人以0.5m∕s的速度從P1點勻速前進至P2點位置，之后右轉彎前進至P3點位置。實驗的測試結果，如圖8所示。

圖8 融合定位實驗Fig.8 Fusion Positioning Experiment

紅色加號為UWB原始定位軌跡，黑色圓圈為實際位置軌跡，如圖8（a）所示。從圖中可看出：UWB定位在視距情況下，定位精度較高，當經過立柱障礙物時，由于受到NLOS影響，致使圖8（a）中A與B區(qū)域點位嚴重偏離實際位置，由表可看出其最大誤差達到1.77m，平均誤差達到了0.38m。藍色叉號為里程計定位軌跡，如圖8（b）所示。從圖中可看出：從起點P1開始至P2位置這段軌跡定位效果較好，但在經過一段時間拐彎后，累積誤差開始顯現(xiàn)出來，由表可看出最大誤差0.49m，平均誤差為0.29m，如果路線更長時間更久誤差會隨之加大。粉色方框為利用本文融合UWB與里程計算法計算的軌跡，如圖8（c）所示。在LOS環(huán)境下的軌跡與UWB定位精度相當，但在NLOS環(huán)境下，通過NLOS誤差鑒別與融合算法有效地濾除了NLOS誤差的影響，即使出現(xiàn)嚴重的測距信息丟失現(xiàn)象，仍能通過里程計進行航跡推演，另外在LOS環(huán)境下，UWB測距對里程計進行實時修正，有效地抑制了航跡推演的累積誤差，靜態(tài)點測試數(shù)據(jù)，如表1所示。由表可看出最大誤差為0.31m，平均誤差為0.2m。因此，本文所采用的融合算法有效地抑制了NLOS誤差的影響，提高了室內移動機器人定位精度與可靠性。

表1 靜態(tài)點測試數(shù)據(jù)（mm）Tab.1 Experiments Data of Static Point

6 結語

設計了一種基于UWB與里程計融合的定位系統(tǒng)，通過在傳統(tǒng)的UWB定位算法基礎上引入NLOS誤差鑒別方法，如鑒別為LOS場景，將根據(jù)UWB測距值進行卡爾曼濾波處理，濾除掉隨機干擾誤差，再經最小二乘法獲得機器人的定位坐標，而機器人的位姿本文采用雙標簽坐標解算法，最終獲得機器人的坐標及位姿，同時將獲得的高精度定位結果對里程計進行累積誤差修正；如鑒別為NLOS場景，將里程計獲得的定位坐標通過反解算獲得測距值對UWB定位進行補償。實驗結果表明，設計的定位系統(tǒng)有效地提高了基于UWB定位方法的定位精度與可靠性，尤其是當UWB信號受到嚴重影響無法獲得測距值時，仍可解算出機器人的定位數(shù)據(jù)，對于室內移動機器人定位系統(tǒng)的設計場合具有一定的參考意義。