基于UKF數據融合的勘測機器人定位方法

徐萌, 孔令華

（1.福建工程學院機械與汽車工程學院，福州 350118；2.數字福建工業制造物聯網實驗室，福州 350118）

0 引言

在過去的20年中，對隧道、城市地下管廊、復雜洞穴環境等各類地下環境進行自主探索需求不斷增長。具備自主定位導航功能機器人可應用于礦洞搜索救援、災難響應及基礎設施監測等領域[1]。由于衛星導航系統無法在地下正常工作，傳統的IMU及里程計等傳感器在長時間工作后會出現累計誤差。因此必須提出一種能夠提高移動機器人定位精度的導航方案。

目前，基于UWB（ultra－wideband）的定位系統最高定位精度可以達到厘米級[2]。其具有良好的信號隱蔽性、極低功耗、抗干擾能力強等特點，在室內定位中受到了越來越多的關注[3]。由于UWB屬于超高頻段，只適合視距傳播條件，存在障礙物阻擋時，其測距精度會大打折扣。因此在復雜環境中僅使用某一種傳感器很難在長時間內維持較高精度定位。眾多學者開始研究UWB與其他傳感器相結合的導航方法。例如，Zhang R. G.等[4]介紹了一種利用UWB、IMU、GNSS結合的室內外定位系統，可有效防止進出室內時丟失信號帶來的影響；文獻[5]中提出了基于卷積神經網絡（CNN）的超寬帶組合導航方法，可有效減小NLOS誤差影響。文獻[6]中利用2個UWB傳感器及陀螺儀測量室內無人飛艇的姿態，并使用擴展卡爾曼濾波融合姿態信息，實現UAV自主導航；A. Marquez等[7]提出了利用IMU及UWB的傳感器融合算法，實現精確的位置估計，提高了魯棒性；Zhang W.等[8]設計了基于UWB與慣性測量單元的糧倉機器人定位系統，彌補了UWB傳感器定位刷新頻率低的問題，突破了對運動速度的限制，提高了定位精度。

本文采用UWB定位及IMU、里程計等傳感器相結合的方式，通過UWB定位標簽測量機器人位姿，與來自IMU及里程計的航向角及推算的位置信息，利用無跡卡爾曼濾波法做融合處理，得到了更為精確的航向角及位姿。該方法有效減少了UWB傳感器因刷新頻率低出現的數據延遲及里程計因隨機漂移出現的累計誤差。解決了在地下環境下無法準確獲取航向及位置信息的問題，為勘測機器人實現高精度定位及導航提供了幫助。

1 多傳感器定位系統

針對地下管廊、人防工程等需要定期巡檢的地下工程，設計一種多傳感器定位系統，如圖1所示。該系統由SSMR（skid－steering mobile robot）四輪驅動移動機器人、UWB定位系統、計算終端組成。SSMR上裝備IMU、里程計、定位標簽傳感器。可根據使用場景的范圍大小配置不同數量的定位基站，其中最小定位系統應該至少由U0、U1、U2、U3四個定位基站組成。

圖1 地下空間定位系統

1.1 SSMR里程計定位

本文中使用的SSMR移動機器人，通過4臺電動機驅動，并通過編碼器獲取位姿信息。該型機器人運動學示意圖如圖2所示。假設機器人幾何中心與質心重合，以質心COG為原點建立機器人參考坐標系（x，y，z）。對機器人在世界坐標系（X，Y，Z）下水平運動進行分析。設機器人在COG坐標原點速度為v=［vx，vy，ωz］T。SSMR機器人相對世界坐標系狀態方程為q=［X，Y，θ］T，其中X和Y代表COG點在世界坐標系中的坐標，θ為機器人參考坐標系X軸與世界坐標系X軸之間夾角，即機器人航向角。因此機器人在世界坐標系下的速度為q=［X˙，Y˙，θ˙］T。

圖2 SSMR移動機器人運動學示意圖

SSMR移動機器人在轉彎時，通過控制同側輪速相同實現圍繞ICR為圓心的運動，其實質為兩側轉速不同的差速運動。參考文獻[9]中的方法對四輪驅動機器人進行等效簡化。則四輪驅動移動機器人簡化為虛擬左右驅動輪的二輪差速運動，其中虛擬左右驅動輪速分別為vL、vR。vi（i=1，2，3，4）為實際驅動輪速度。轉彎時同側輪速相同，即滿足式（2）：

基于虛擬左右驅動輪速簡化正運動模型可表示為

由式（1）、式（3）可以求出△t時刻SSMR機器人狀態q=［X，Y，θ］T。如圖3所示中（Xn，Yn，θn）為若干△t時間后的機器人狀態，通過式（4）可計算機器人相對于初始狀態的位姿。

圖3 機器人軌跡推算

1.2 UKF數據融合算法

機器人里程計由位姿增量積分得到，工作精度高但存在累計誤差。而超寬帶傳感器可以提供全局定位。二者相互融合可以彌補里程計存在累計誤差的缺陷，提高定位系統魯棒性。

由SSMR機器人軌跡推算模型可知該系統為非線性系統。針對非線性系統，無跡卡爾曼濾波（Unscented Kalman Filter，UKF）、擴展卡爾曼濾波（Extended Kalman Filter，EKF）均可解決非線性濾波問題。EKF算法對系統非線性部分進行一階Taylor級數展開，這樣線性化處理不可避免模型誤差，同時會增加系統計算復雜度與成本[10]。而UKF則通過無跡變換UT（Unscented Transform）來得到非線性變換后的的概率分布。即對隨機變量按分布進行sigma點采樣，對采樣點進行非線性變換以后獲得后驗分布。因此該算法可有效解決EKF算法估計精度低、穩定性差的問題[11]。

假設SSMR機器人以恒定線速度及角速度在二維平面上運動。以里程計測量信息作為系統輸入，以UWB定位數據作為系統觀測，將兩種數據進行濾波融合。系統狀態變量Xk=（x，y，θ）T定義為k時刻機器人在世界坐標系位姿。Zk=（x，y）T作為k時刻觀測量。

式中：F及H分別為狀態轉移函數及觀測函數；ωk為系統噪聲，ωk～N（0，Q）；vk為量測噪聲，vk～N（0，R）。

UKF算法中步驟1）完成系統初始化，步驟2）、3）根據SSMR運動學模型，由上一時刻狀態預測當前時刻狀態。步驟4）利用傳感器測量數據對預測值進行更新，得到最優狀態估計。多傳感器數據濾波融合的過程就是步驟2）、3）、4）的反復迭代[12]。

2 實驗及結果分析

首先選擇在實訓中心大廳走廊搭建定位系統，該處處于室內，空間寬闊，適合模擬城市地下管廊等環境。遙控勘檢機器人沿矩形路線一周，并在ROS操作環境中通過bag形式記錄數據。

2.1 實驗平臺

本文采用的機器人為自主研發越野輪式移動機器人。采用四輪驅動、差速轉向驅動方式。機器人上位機采用英特爾NUC（I5處理器），并裝有ROS操作系統，下位機使用Pixhawk控制器，如圖4所示。

圖4 輪式勘檢機器人平臺

UWB定位系統選用Nooploop公司型號為LinkTrack定位、測距、數傳一體化模塊。內嵌三軸陀螺儀、三軸加速度計。測距、定位頻率為200 Hz。定位范圍為80 m，二維定位精度為10 cm。

2.2 實驗設置

實驗區域長寬約 為29.5 m ×12.5 m，如圖5所示。在該實驗環境中按長寬29.5 m×9 m矩形設置4部定位基站，并分別命名為U0、U1、U2、U3。機器人以初始位置作為原點，運動軌跡沿（0，0）、（9，0）、（9，22.5）、（0，22.5）坐標運行一周。其中可能遮擋UWB信號的為中間鏤空護欄。

2.3 定位精度

以勘測巡檢機器人初始位置為坐標原點，機器人在實驗區域，沿著矩形軌跡繞場一周，并最終回到原點。通過不同傳感器定位方法得到的軌跡如圖6所示。從圖中可以看出，僅使用里程計定位時，定位軌跡比較平滑，但是隨著時間變化機器人在第一個轉彎處航向估計就發生了較大的偏差，導致機器人位姿估計發生漂移，整個運動軌跡無法形成閉環。使用里程計與慣導結合定位，該方法可以通過慣導估計航向，彌補里程計因機器人打滑所產生的航向累計誤差。使用超寬帶定位修正里程計定位方法，該方法不僅可以避免航向估計誤差，更可以通過全局定位減少里程計累計誤差。由圖中可以直觀看出兩種方法均能實現軌跡回環。

如圖6所示，由圖中可以看出機器人里程計定位在運行到25 s左右經過第一個轉彎點便出現偏差，并且在X 軸方向上的偏差隨時間推移而增大。

圖6 定位軌跡圖

圖7、圖8分別為X 軸方向定位誤差及Y 軸方向定位誤差，由圖中可以看出本文所提出的UWB組合定位方法相比于里程計定位具有更高的定位精度

圖7 X軸定位誤差箱圖

圖8 Y軸定位誤差箱圖

由表1中數據分析可知，在位置估計上里程計定位誤差約為4.4629 m, 而里程計慣導融合定位及超寬帶融合定位均能達到厘米級定位，其中超寬帶融合定位比里程計慣導定位精度提升40%，且方差值更小，具有魯棒性。

表1 各定位方法精度 m

3 結論

實驗結果表明，機器人在29.5 m×12.5 m的無GPS環境中作業時，定位誤差可達到厘米級。所提出的UKF融合濾波算法定位算法相對于單獨使用里程計定位精度有一定的提升。在地下空間等缺少GPS高精度導航環境下，該方法可有效提高勘測機器人定位精度，實現機器人在地下環境的自主導航。