基于超寬帶和航位推算的室內(nèi)機(jī)器人UKF定位算法

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

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

基于超寬帶和航位推算的室內(nèi)機(jī)器人UKF定位算法

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

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

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

機(jī)器人；定位；UKF；超寬帶；航位推算

0 引言

2013年比爾·蓋茨預(yù)測(cè)機(jī)器人與自動(dòng)化技術(shù)將成為科技發(fā)展的一大趨勢(shì),機(jī)器人或?qū)⒊蔀楹笠苿?dòng)時(shí)代一件改變世界的大事。繼機(jī)器人在工業(yè)現(xiàn)場(chǎng)獲得大規(guī)模應(yīng)用后,服務(wù)機(jī)器人日漸增多,機(jī)器人與我們的生活日益密切。服務(wù)機(jī)器人主要工作在室內(nèi)環(huán)境,在室內(nèi)的自主定位和導(dǎo)航是服務(wù)機(jī)器人關(guān)鍵技術(shù)之一。

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

1 超寬帶定位

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

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

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

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

2 航位推算

本文采用的機(jī)器人平臺(tái)如圖2所示。該機(jī)器人由2個(gè)直流伺服電機(jī)驅(qū)動(dòng)左右主動(dòng)輪,通過左右輪差速實(shí)現(xiàn)轉(zhuǎn)向運(yùn)動(dòng),前后2個(gè)萬向輪僅起支撐作用。本文以安裝在2個(gè)伺服電機(jī)軸端的光電編碼器輸出進(jìn)行航位推算。

圖2 機(jī)器人平臺(tái)Fig.2 Robot platform

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

(1)

(2)

(3)

初始裝訂時(shí),以機(jī)器人當(dāng)前位置的超寬帶定位數(shù)據(jù)作為航位推算的初始位置,以當(dāng)前航向下電子羅盤輸出的航向作為初始航向。該電子羅盤集成了三軸磁傳感器、三軸MEMS陀螺、三軸加速度計(jì),可以對(duì)航向、水平姿態(tài)進(jìn)行測(cè)量。已事先將電子羅盤航向進(jìn)行校準(zhǔn),以便將電子羅盤的航向統(tǒng)一到超寬帶定位系統(tǒng)坐標(biāo)系下,即導(dǎo)航所用航向?yàn)榕cX軸正方向夾角,取值范圍0°～360°。

由于2個(gè)電機(jī)輸出的轉(zhuǎn)速經(jīng)減速器減速時(shí)存在間隙,而且車輪與地面之間不可避免地存在打滑現(xiàn)象,尤其在轉(zhuǎn)彎時(shí)滑動(dòng)更為明顯。因此航位推算的軌跡為理想的輸出軌跡,全程連續(xù)光滑,在轉(zhuǎn)彎時(shí)航向誤差有明顯增大的趨勢(shì)。機(jī)器人在室內(nèi)運(yùn)行時(shí)同時(shí)采集超寬帶數(shù)據(jù)和航位推算數(shù)據(jù),將兩組數(shù)據(jù)在同一坐標(biāo)系輸出,如圖3所示。因此,在本文研究中不能以航位推算的結(jié)果作為導(dǎo)航的基準(zhǔn),僅作為組合定位的參考,用以預(yù)測(cè)機(jī)器人運(yùn)行軌跡的趨勢(shì)。

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

3 UKF組合定位算法

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

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

3.1 狀態(tài)方程和觀測(cè)方程

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

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

(4)

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

(5)

wφ(k)

(6)

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

(7)

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

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

(8)

選取觀測(cè)方程如下

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

(9)

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

(10)

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

3.2UKF方法

將UKF計(jì)算過程分為Sigma點(diǎn)計(jì)算、一步預(yù)測(cè)和測(cè)量更新三部分,濾波的過程就是這三部分運(yùn)算的反復(fù)迭代[9-10]。

1)初始化

(11)

(12)

根據(jù)上述公式,在導(dǎo)航初始位置采集超寬帶坐標(biāo)、電子羅盤航向的均值、方差,作為狀態(tài)初值和對(duì)應(yīng)的誤差方差,航向偏差Δφ的初值和方差可取較小值。

系統(tǒng)噪聲矩陣Q可通過航位推算實(shí)驗(yàn)統(tǒng)計(jì)得到,量測(cè)噪聲R可根據(jù)超寬帶定位系統(tǒng)及電子羅盤產(chǎn)品參數(shù)及測(cè)量統(tǒng)計(jì)得到。

由于UKF算法避免了模型線性化造成的高階截?cái)嗾`差,收斂速度和穩(wěn)態(tài)精度也優(yōu)于EKF,對(duì)初始條件和系統(tǒng)噪聲的不確定性具有較好的魯棒。

2)Sigma點(diǎn)計(jì)算

(11)

其中,λ為尺度因子,其計(jì)算公式如下

λ=α2(n+β)-n

(12)

利用各采樣點(diǎn)計(jì)算的權(quán)重設(shè)定如下：

(13)

3)一步預(yù)測(cè)

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

(14)

根據(jù)更新后的Sigma點(diǎn)計(jì)算狀態(tài)、狀態(tài)方差和測(cè)量的一步預(yù)測(cè)值,公式如下：

(15)

(16)

(17)

(18)

4)測(cè)量更新

(19)

(20)

(21)

(22)

(23)

4 實(shí)驗(yàn)驗(yàn)證

將上述UKF組合定位算法在機(jī)器人平臺(tái)上編程實(shí)現(xiàn),并在室內(nèi)環(huán)境下進(jìn)行試驗(yàn)驗(yàn)證。UKF濾波周期選為200ms,超寬帶、里程計(jì)、電子羅盤等的采樣周期為50ms。

在圖3所示的實(shí)驗(yàn)中,航位推算由于齒輪間隙、輪子打滑等原因,導(dǎo)致在轉(zhuǎn)彎后航向誤差增大較快。采用UKF組合定位算法,實(shí)驗(yàn)結(jié)果如圖4所示,藍(lán)色曲線為UKF結(jié)果?？梢钥闯鰹V波后能夠很好修正航位推算的航向誤差,從而保證了機(jī)器人定位精度。

圖4 實(shí)驗(yàn)數(shù)據(jù)1Fig.4 Experimental data Ⅰ

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

圖5 實(shí)驗(yàn)數(shù)據(jù)2Fig.5 Experimental data Ⅱ

5 結(jié)論

本文提出的UKF組合定位方法能夠有效融合超寬帶定位、航位推算、電子羅盤等多源信息,從而為室內(nèi)移動(dòng)機(jī)器人提供了一種較為穩(wěn)定、可靠的方案。通過UKF可根據(jù)超寬帶定位信息、羅盤航向信息估計(jì)出航位推算的航向失準(zhǔn)角,從而在超寬帶信號(hào)異常時(shí),可通過航位推算、維持機(jī)器人定位精度。

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

[2] 何貴斌. 超寬帶導(dǎo)航技術(shù)及應(yīng)用構(gòu)想[J]. 中國(guó)新通信, 2012(8): 43-48.

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

[4] 張紅梅,鄧正隆.UKF方法在陸地車輛組合導(dǎo)航中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2004, 12(4):20-23.

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

[6] 陸海勇. 捷聯(lián)慣性導(dǎo)航系統(tǒng)中UKF濾波技術(shù)的應(yīng)用研究[D]. 南京: 南京航空航天大學(xué), 2009.

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

[8] 郭澤, 繆玲娟, 趙洪松. 一種改進(jìn)的強(qiáng)跟蹤UKF算法及其在SINS大方位失準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用[J]. 航空學(xué)報(bào), 2014, 35(1): 203-214.

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

[10] 申文斌,裴海龍.改進(jìn)的UnscentedKalman濾波算法[J]. 計(jì)算機(jī)工程與科學(xué), 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-),女,高級(jí)工程師,主要從事移動(dòng)機(jī)器人定位導(dǎo)航和環(huán)境感知方面的研究。 E-mail: zhwfang765@sohu.com

10.19306/j.cnki.2095-8110.2017.02.005

TP242

A

2095-8110(2017)02-0026-05