基于UKF數(shù)據(jù)融合的勘測機器人定位方法

徐萌, 孔令華

（1.福建工程學院機械與汽車工程學院，福州 350118；2.數(shù)字福建工業(yè)制造物聯(lián)網(wǎng)實驗室，福州 350118）

0 引言

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

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

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

1 多傳感器定位系統(tǒng)

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

圖1 地下空間定位系統(tǒng)

1.1 SSMR里程計定位

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

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

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

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

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

圖3 機器人軌跡推算

1.2 UKF數(shù)據(jù)融合算法

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

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

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

式中：F及H分別為狀態(tài)轉(zhuǎn)移函數(shù)及觀測函數(shù)；ωk為系統(tǒng)噪聲，ωk～N（0，Q）；vk為量測噪聲，vk～N（0，R）。

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

2 實驗及結(jié)果分析

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

2.1 實驗平臺

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

圖4 輪式勘檢機器人平臺

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

2.2 實驗設(shè)置

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

2.3 定位精度

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

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

圖6 定位軌跡圖

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

圖7 X軸定位誤差箱圖

圖8 Y軸定位誤差箱圖

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

表1 各定位方法精度 m

3 結(jié)論

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