基于超寬帶和航位推算的室內機器人UKF定位算法

王 芳,李 楠,劉汝佳,呂 翀

(航天科工智能機器人有限責任公司,北京 100074)

基于超寬帶和航位推算的室內機器人UKF定位算法

王 芳,李 楠,劉汝佳,呂 翀

(航天科工智能機器人有限責任公司,北京 100074)

超寬帶是一種傳輸速率快、功耗低的新型無線通信技術,可提供亞米級定位精度,近年來超寬帶定位在機器人領域的應用日益廣泛。在超寬帶信號有效區域邊緣或信號受到遮擋時,超寬帶定位精度急劇下降。為此提出了一種基于超寬帶定位和航位推算的UKF組合定位方法,可有效克服上述問題,從而為室內機器人定位提供一種穩定可靠的解決方案。

機器人；定位；UKF；超寬帶；航位推算

0 引言

2013年比爾·蓋茨預測機器人與自動化技術將成為科技發展的一大趨勢,機器人或將成為后移動時代一件改變世界的大事。繼機器人在工業現場獲得大規模應用后,服務機器人日漸增多,機器人與我們的生活日益密切。服務機器人主要工作在室內環境,在室內的自主定位和導航是服務機器人關鍵技術之一。

目前應用于機器人的室內定位技術包括：基于視覺傳感器或激光雷達的SLAM技術,基于Wi-Fi、藍牙、ZigBee的定位技術,以及超寬帶(Ultra Wide Band,UWB)定位技術等。SLAM技術不依賴外部信息源,具有較強的自主性,對環境改動最小,但在人員密集的動態環境下SLAM技術難以奏效。超寬帶定位是一種基于TOF(Time of Flight)原理的定位方式,相比于Wi-Fi、藍牙、ZigBee等需要檢測信號強度的方法,定位更為可靠,近年來在室內定位導航應用方面獲得關注[1-3]。對于超寬帶信號受到遮擋時,或機器人超出超寬帶有效定位區域時,超寬帶定位誤差會急劇增大,此時可通過與航位推算(Dead Reckoning,DR)、慣性等信息進行組合獲得較為精確的定位結果。為此,本文提出了一種基于超寬帶和航位推算的UKF定位算法,從而為室內機器人導航提供一種較為精確可靠的定位方式。

1 超寬帶定位

超寬帶技術是一種新型的無線通信技術,它通過對急劇變化的沖擊脈沖進行直接調制,使信號具有GHz量級的帶寬。超寬帶技術解決了困擾傳統無線技術多年的有關傳播方面的重大難題,具有對信道衰落不敏感、發射信號功率譜密度低、抗截獲能力強等特點,能提供分米級定位精度。

本文采用清研訊科公司研制的LocalSense?自解算超寬帶定位系統,該系統工作原理如圖1所示。4個定位基站是定位系統的基準錨點,安裝在試驗區域的固定位置,為系統確定了空間坐標基準。一般情況下,坐標原點選在墻角或者走廊入口,或者具有明顯標志性的節點；坐標系的X軸一般與墻體或走廊的延伸方向一致；Y軸則與X軸垂直,由此建立機器人定位導航坐標系。自解算標簽安裝在移動機器人上,是定位系統的計算核心,該標簽實時獲取周圍基站的位置,并根據反饋結果計算本身的位置,亦即機器人的位置。在信號無遮擋的條件下,該系統定位精度可達10～15cm。

圖1 超寬帶定位原理Fig.1 UWB localization system

超寬帶定位系統是基于電磁脈沖飛行時間的系統,基本算法采用了三點定位曲線相交的原理。在室內環境下,由于空間尺寸較小,無線系統多徑效應強,高精度定位部署困難；而由于建筑支柱、隔斷墻體、玻璃、家具等遮擋物體的存在,更加惡化了無線定位的工作環境。因此,機器人在室內環境下自主移動不能單純依靠超寬帶定位系統,應與其他定位方式進行融合。

2 航位推算

本文采用的機器人平臺如圖2所示。該機器人由2個直流伺服電機驅動左右主動輪,通過左右輪差速實現轉向運動,前后2個萬向輪僅起支撐作用。本文以安裝在2個伺服電機軸端的光電編碼器輸出進行航位推算。

圖2 機器人平臺Fig.2 Robot platform

記2個編碼器精度,即每轉脈沖數(PPR)為P,驅動電機的減速比為η,車輪直徑為D(單位：m),兩車輪輪間距為w(單位：m)。令L=π·D·η/P,為航位推算的比例系數。設k-1至k時刻左右車輪編碼器輸出的脈沖增量為Nl和Nr,則k時刻根據DR機器人的位置(xdk,ydk)和航向φdk的遞推公式如式(1)～式(3)所示。

(1)

(2)

(3)

初始裝訂時,以機器人當前位置的超寬帶定位數據作為航位推算的初始位置,以當前航向下電子羅盤輸出的航向作為初始航向。該電子羅盤集成了三軸磁傳感器、三軸MEMS陀螺、三軸加速度計,可以對航向、水平姿態進行測量。已事先將電子羅盤航向進行校準,以便將電子羅盤的航向統一到超寬帶定位系統坐標系下,即導航所用航向為與X軸正方向夾角,取值范圍0°～360°。

由于2個電機輸出的轉速經減速器減速時存在間隙,而且車輪與地面之間不可避免地存在打滑現象,尤其在轉彎時滑動更為明顯。因此航位推算的軌跡為理想的輸出軌跡,全程連續光滑,在轉彎時航向誤差有明顯增大的趨勢。機器人在室內運行時同時采集超寬帶數據和航位推算數據,將兩組數據在同一坐標系輸出,如圖3所示。因此,在本文研究中不能以航位推算的結果作為導航的基準,僅作為組合定位的參考,用以預測機器人運行軌跡的趨勢。

圖3 超寬帶定位與航位推算對比Fig.3 The comparison of UWB localization and DR

3 UKF組合定位算法

無跡Kalman濾波(Unscented Kalman Filter,UKF)、擴展Kalman濾波(Extended Kalman Filter,EKF)、粒子濾波(Particle Filter,PF)都可以解決非線性濾波問題。EKF通過將非線性模型線性化后進行濾波,因此不可避免地存在模型誤差,導致濾波器次優甚至發散。UKF則在Kalman濾波算法的框架上,通過UT(Unscented Transform)變換,以一組Sigma點表示隨機狀態的先驗分布,經過非線性變換以后獲得后驗分布。UKF直接基于非線性模型進行解算,近似非線性函數的概率密度分布,而非對非線性模型近似,避免了模型誤差。粒子濾波同樣通過采樣方法,近似隨機樣本在狀態空間傳播的概率密度函數。粒子濾波基于蒙特卡羅方法(Monte Carlo methods),是一種順序重要性采樣方法,狀態變量無需滿足高斯分布,但計算量較大,粒子數過少將導致估計精度下降。UKF基于狀態變量滿足高斯分布的假設,為確定性采樣,樣本數與狀態維數相關,與粒子濾波器相比采樣點數少,同時又能保證逼近精度。

近年來隨著UKF技術引入組合導航領域,基于UKF的組合定位方法研究取得了較大進展[4-8],尤其是在車輛或輪式機器人DR導航中。從式(1)～式(3)可見航位推算模型是非線性的,若采用EKF方法,需要對模型進行線性化, 即在當前狀態估值處進行Taylor 級數展開,并忽略高階項,從而導致模型信息丟失, 而且噪聲的存在使之進一步惡化。為此本文選用UKF方法進行組合定位。

3.1 狀態方程和觀測方程

基于式(1)～式(3),UWB/DR組合定位算法的狀態方程如下：

cos (φ(k)+Δφ(k))+ωx(k)

(4)

sin (φ(k)+Δφ(k))+ωy(k)

(5)

wφ(k)

(6)

Δφ(k)=Δφ(k-1)+ωΔφ(k)

(7)

濾波狀態X=[x,y,φ,Δφ],其中(x,y)為機器人的位置坐標；φ+Δφ為機器人當前航向,包括根據航位推算獲得的機器人航向φ,和由于傳動間隙和打滑造成的航位推算的航向偏差Δφ。ω=[ωx,ωy,ωφ,ωΔφ]是高斯白噪聲,Cov[w(k),w(l)]=Q·δkl。NR、NL分別為左右輪驅動電機的編碼器脈沖,可看作狀態方程的控制變量。由式(4)～式(7)可以看出狀態方程是非線性的,狀態方程可記為

X(k)=f(X(k-1),NR(k),NL(k))+w(k)

(8)

選取觀測方程如下

Z(k)=H·X(k)+v(k)

(9)

測量值Z=[z1,z2,z3]分別為超寬帶測量的XY軸坐標,和電子羅盤測量的航向(已轉換到超寬帶定位坐標系下)。矩陣H定義如下

(10)

測量噪聲v=[v1,v2,v3]為高斯白噪聲,Cov[v(k),v(l)]=R·δkl。

3.2UKF方法

將UKF計算過程分為Sigma點計算、一步預測和測量更新三部分,濾波的過程就是這三部分運算的反復迭代[9-10]。

1)初始化

(11)

(12)

根據上述公式,在導航初始位置采集超寬帶坐標、電子羅盤航向的均值、方差,作為狀態初值和對應的誤差方差,航向偏差Δφ的初值和方差可取較小值。

系統噪聲矩陣Q可通過航位推算實驗統計得到,量測噪聲R可根據超寬帶定位系統及電子羅盤產品參數及測量統計得到。

由于UKF算法避免了模型線性化造成的高階截斷誤差,收斂速度和穩態精度也優于EKF,對初始條件和系統噪聲的不確定性具有較好的魯棒。

2)Sigma點計算

(11)

其中,λ為尺度因子,其計算公式如下

λ=α2(n+β)-n

(12)

利用各采樣點計算的權重設定如下：

(13)

3)一步預測

將上述Sigma點代入非線性狀態方程(8),得到更新后的Sigma點

(14)

根據更新后的Sigma點計算狀態、狀態方差和測量的一步預測值,公式如下：

(15)

(16)

(17)

(18)

4)測量更新

(19)

(20)

(21)

(22)

(23)

4 實驗驗證

將上述UKF組合定位算法在機器人平臺上編程實現,并在室內環境下進行試驗驗證。UKF濾波周期選為200ms,超寬帶、里程計、電子羅盤等的采樣周期為50ms。

在圖3所示的實驗中,航位推算由于齒輪間隙、輪子打滑等原因,導致在轉彎后航向誤差增大較快。采用UKF組合定位算法,實驗結果如圖4所示,藍色曲線為UKF結果。可以看出濾波后能夠很好修正航位推算的航向誤差,從而保證了機器人定位精度。

圖4 實驗數據1Fig.4 Experimental data Ⅰ

圖5所示實驗中,在坐標(900,450)附近存在較大金屬物體,超寬帶信號被遮擋,導致超寬帶定位誤差急劇增大。通過對比2次采樣間超寬帶位置增量和里程增量,很容易判別超寬帶數據異常。在超寬帶數據異常期間以航位推算位置作為濾波輸出。由于UKF補償航位推算的航向誤差,因此采用短時間采用航位推算可以保持定位精度。

圖5 實驗數據2Fig.5 Experimental data Ⅱ

5 結論

本文提出的UKF組合定位方法能夠有效融合超寬帶定位、航位推算、電子羅盤等多源信息,從而為室內移動機器人提供了一種較為穩定、可靠的方案。通過UKF可根據超寬帶定位信息、羅盤航向信息估計出航位推算的航向失準角,從而在超寬帶信號異常時,可通過航位推算、維持機器人定位精度。

[1]LiuB,WuX,ReddyV,etal.ARTLS/DRbasedlocalizationsystemarchitectureforindoormobilerobots[C]//IEEEInternationalSymposiumonaWorldofWireless,MobileandMultimediaNetworks, 2010:1-6.

[2] 何貴斌. 超寬帶導航技術及應用構想[J]. 中國新通信, 2012(8): 43-48.

[3] 張令文, 楊剛. 超寬帶室內定位關鍵技術[J]. 數據采集與處理, 2013, 28(6): 706-713.

[4] 張紅梅,鄧正隆.UKF方法在陸地車輛組合導航中的應用[J]. 中國慣性技術學報, 2004, 12(4):20-23.

[5]ZhouZ,GaoY,ChenJ.UnscentedKalmanfilterforSINSalignment[J].JournalofSystemsEngineeringandElectronics, 2007, 18(2): 327-333.

[6] 陸海勇. 捷聯慣性導航系統中UKF濾波技術的應用研究[D]. 南京: 南京航空航天大學, 2009.

[7] 王維鋒, 徐萬里, 聶揮宇. 基于UKF的北斗/INS組合導航[J]. 四川兵工學報, 2013, 34(8): 96-99.

[8] 郭澤, 繆玲娟, 趙洪松. 一種改進的強跟蹤UKF算法及其在SINS大方位失準角初始對準中的應用[J]. 航空學報, 2014, 35(1): 203-214.

[9]ZhouZhanxin,GaoYannan,ChenJiabin.UnscentedKalmanfilterforSINSalignment[J].JournalofSystemsEngineeringandElectronics, 2007, 18(2): 327-333.

[10] 申文斌,裴海龍.改進的UnscentedKalman濾波算法[J]. 計算機工程與科學, 2011,33(4):192-197.

Indoor Robot UKF Localization Algorithm Based on UWB and DR

WANG Fang, LI Nan, LIU Ru-jia, LV Chong

(Aerospace Science & Industry Intelligent Robot Company Limited,Beijing 100074,China)

UWB (Ultra Wide Band) is a new type of wireless communication technology with characteristics of rapid transmission, low power consumption and so on. By means of UWB, sub-meter level positioning can be achieved. Recently, positioning based on UWB has been widely used in the field of robots. Given that the positioning accuracy decreases rapidly at the edge of effective area of UWB, or in the situation of the signal block, a new method of UKF(Unscented Kalman Filter) combined positioning based on UWB and dead reckoning is proposed. It affords a stable and reliable approach for indoor robot localization for the sake of overcoming the shortcomings of UWB positioning mentioned above.

Robot; Localization; UKF; UWB; DR

2016-09-07；

2016-10-09

王芳(1977-),女,高級工程師,主要從事移動機器人定位導航和環境感知方面的研究。 E-mail: zhwfang765@sohu.com

10.19306/j.cnki.2095-8110.2017.02.005

TP242

A

2095-8110(2017)02-0026-05