文獻(xiàn)標(biāo)識碼: A
DOI:10.16157/j.issn.0258-7998.2017.05.005
中文引用格式: 盧靖宇,,余文濤,,趙新,等. 基于超寬帶的移動機器人室內(nèi)定位系統(tǒng)設(shè)計[J].電子技術(shù)應(yīng)用,,2017,,43(5):25-28.
英文引用格式: Lu Jingyu,Yu Wentao,,Zhao Xin,,et al. Design of indoor positioning system for mobile robot based on ultra-wideband[J].Application of Electronic Technique,2017,,43(5):25-28.
0 引言
移動機器人定位是其自主導(dǎo)航中的最基本環(huán)節(jié),也是移動機器人完成任務(wù)必須解決的問題,。對定位的要求是定位精度高(亞米級精度),實時性好,。目前移動機器人室內(nèi)定位主要分為兩大類:(1)相對定位法,,即航跡推算法[1]。利用機器人所裝備的各種傳感器獲取機器人的運動動態(tài)信息,,通過遞推累計公式獲得機器人相對初始狀態(tài)的估計位置,。使用的傳感器主要是碼盤和慣性傳感器。但是他們都有一個共同的缺點:存在累積誤差,,隨著行駛時間,、距離的不斷增加,誤差也不斷增大,,不適合長時間長距離的精確定位,。(2)絕對定位法,即機器人通過獲取外界一些位置等已知的參照信息,,通過計算自己與參照信息之間的相互關(guān)系解算出自己的位置,。絕對定位法主要采用同步定位與建圖[2](Simultaneous Localization and Mapping,SLAM),、視覺定位方法以及基于信標(biāo)定位等方法,。其中SLAM定位法和視覺定位數(shù)據(jù)量大,價格昂貴,目前只適宜試驗研究使用,。而且,,這兩種方法只適用于一些結(jié)構(gòu)簡單的環(huán)境,對移動機器人一般工作的復(fù)雜室內(nèi)環(huán)境并不能提供很好的定位精度,。因此,,本系統(tǒng)選用基于UWB的信標(biāo)定位的方法來實現(xiàn)復(fù)雜室內(nèi)環(huán)境下的移動機器人室內(nèi)定位。
UWB信號具有超高分辨率,、抗多徑效應(yīng),、穿透力強以及結(jié)構(gòu)簡單的優(yōu)點,成為目前室內(nèi)高精度定位的最佳技術(shù)[3],。常用的UWB定位方式為基于到達(dá)時間法(Time of Arrival,,TOA)以及到達(dá)時間差法(Time Difference of Arrival,TDOA),。但是,,這TOA法需要標(biāo)簽與基站之間時鐘同步,TDOA法需要基站與基站之間的時鐘同步,,這增大了系統(tǒng)設(shè)計難度,。
本文基于UWB技術(shù),,采用decaWave公司生產(chǎn)的DWM1000模塊,應(yīng)用非對稱雙邊雙向測距(Asymmetric Double Sided Two-Way Range,,ADS-TWR)技術(shù)進(jìn)行定位,,無需基站與標(biāo)簽之間和基站與基站之間時鐘同步,大大縮減系統(tǒng)設(shè)計難度,。針對實際應(yīng)用中由非視距傳播引起的測量誤差,,采用卡爾曼濾波算法對測距進(jìn)行優(yōu)化,提高定位精度,。最終實現(xiàn)一種高精度,、高實時性的移動機器人室內(nèi)定位系統(tǒng)。
1 系統(tǒng)總體設(shè)計
移動機器人室內(nèi)定位系統(tǒng)主要由UWB無線傳感器網(wǎng)絡(luò)和上位機顯示軟件兩部分組成,,系統(tǒng)示意圖如圖1所示,。系統(tǒng)硬件部分包括基站和安裝在移動機器人頂端的標(biāo)簽。其中基站分為普通基站(基站2,、基站3和基站4)和通信基站(基站1),。標(biāo)簽和基站均由單片機和DWM1000組成的通信模塊構(gòu)成,并由軟件配置模塊的角色(標(biāo)簽或者基站),。DWM1000可以精確地測量UWB信號發(fā)送和接收的時間點,,通過ADS-TWR 技術(shù)測得標(biāo)簽到各個基站之間的距離,利用UWB通信功能,,各基站將距離信息發(fā)送給通信基站,,最終通過WiFi將距離信息傳送給上位機進(jìn)行定位并顯示。通信基站的硬件結(jié)構(gòu)圖如圖2所示(標(biāo)簽和普通基站沒有WiFi模塊),。
2 基于ADS-TWR技術(shù)的測距及優(yōu)化
無線定位系統(tǒng)定位的準(zhǔn)確性取決于測距的精度,。UWB定位系統(tǒng)測距誤差的來源除了非視距引起的誤差外,還包括晶振的時鐘漂移引起的誤差,。晶振時鐘漂移會影響對信號發(fā)送和接收時間點的測量,,繼而影響測距的準(zhǔn)確性。UWB定位系統(tǒng)最簡單的測距方式是單程測距(One Way Ranging,,OWR),,但是這對節(jié)點之間的時鐘同步要求極其嚴(yán)格,而雙程測距(Two Way Ranging,,TWR)雖能消除節(jié)點之間未能完全同步的影響,,但無法消除晶振時鐘漂移的影響。而對稱雙邊雙向測距(Symmetry Double Sided Two-Way Range,,SDS-TWR)可以消除晶振時鐘漂移的影響,,但是要求信號回復(fù)時間嚴(yán)格相等,這樣大大降低了定位的實時性[4],。在此采用ADS-TWR測距技術(shù),。
ADS-TWR測距過程如圖3所示,,圖中pollTX、pollRX,、answerTX,、answerRX、finalTX,、finalRX代表UWB信號離開標(biāo)簽和基站天線的時間點,。測距過程如下:首先標(biāo)簽向基站請求幀;基站收到請求幀后啟動計時,,經(jīng)過延時Treply1后向標(biāo)簽發(fā)送應(yīng)答幀;標(biāo)簽收到應(yīng)答幀后啟動計時,,并將發(fā)送和接收信號時的時間點寫入終止幀,,經(jīng)延時Treply2后發(fā)送給基站;基站收終止幀后表示測距結(jié)束,。
距離計算公式如式(1)和(2)所示:
其中,,kt和 ka為標(biāo)簽和基站時鐘偏移系數(shù),二者都接近于1,。對于20 ppm的時鐘(最壞規(guī)格的時鐘),,則kt和ka都可以是0.999 98或1.000 02。對于相對較大的測距范圍如100 m,,Ttof僅為333 ns,,飛行時間測量誤差為6.7 ps,換算為距離誤差僅為2.2 mm,。因此,,ADS-TWR測距能很好地抑制時鐘漂移的影響。
ADS-TWR測距不要求應(yīng)答時間Treply1和Treply2相等,。因此,,在標(biāo)簽與多個基站進(jìn)行通信時,可以通過設(shè)定各節(jié)點的應(yīng)答時間來減少標(biāo)簽測距的時間,,保證定位系統(tǒng)的實時性,。
圖4為本文所采用的多基站測距機制。標(biāo)簽向4個基站發(fā)送請求幀,,基站接收到請求幀后按照設(shè)定的應(yīng)答時間依次向標(biāo)簽發(fā)送應(yīng)答幀,,標(biāo)簽接收到應(yīng)答幀后,將4個基站用于計算距離參數(shù)寫入終止幀,,并發(fā)送給所有基站,;各基站接收到終止幀后測距結(jié)束?;纠檬?1)和(2)計算出距離,,然后通過UWB發(fā)送給通信基站,。實際應(yīng)用中,對于四基站定位系統(tǒng),,通過優(yōu)化各基站應(yīng)答時間,,可使單輪測距時間能控制在2 ms左右,完全可以滿足移動機器人定位對實時性的要求,。
3 基于卡爾曼濾波的定位算法
移動機器人在室內(nèi)活動,,不可避免地受到遮擋的影響。標(biāo)簽與基站之間由于存在人或座椅等遮擋物,,這時UWB信號不能進(jìn)行直線傳播,,而是利用衍射、透射和反射的方式到達(dá)接收端,,就是非視距傳播(NLOS),。這時系統(tǒng)對飛行時間的測量就是會出現(xiàn)誤差,而定位精度也就有了誤差,。非視距誤差受到室內(nèi)環(huán)境影響,,是一個實時變化的值。由于非視距傳播增加了信號的傳播時間和傳播距離,,因此非視距誤差服從一個正均值的隨機過程[5],。
假設(shè)t時刻標(biāo)簽到基站的距離為di(t),ri(t)表示二者之間的真實距離,,Ni(t)表示觀測時由環(huán)境因素引起的非視距誤差,,ni(t)為零均值高斯噪聲,則可用下式表示它們之間的關(guān)系:
本文采用卡爾曼濾波算法對距離進(jìn)行優(yōu)化,,對NLOS誤差值進(jìn)行迭代處理,,以此來減小定位誤差。首先利用狀態(tài)向量方程對距離信號進(jìn)行卡爾曼濾波,,估計出NLOS誤差值Ni(t),,然后從最初的測距值di(t)中除去非視距誤差,得到精確地距離值[6],。系統(tǒng)的狀態(tài)方程和測量方程如下:
其中,,Δt為采樣間隔;ωd(t)和ωN(t)分別為測量過程中的噪聲誤差分量,;β為實驗參數(shù),;vi(t)為測量誤差。再給出t時刻的狀態(tài)向量和估計誤差的協(xié)方差的初始值之后,,就可以通過迭代運算對不同時刻的狀態(tài)向量做出估計,。由于非視距誤差具有非負(fù)性,因此在迭代過程中如Ni(t)出現(xiàn)負(fù)值就強制置零。
根據(jù)基站坐標(biāo)和標(biāo)簽到基站的距離可以得到以下方程組:
其中(xi,,yi,,zi)為基站i的坐標(biāo),di為經(jīng)過卡爾曼濾波優(yōu)化后標(biāo)簽到基站i的距離,。用第i個式子減去第j個式子得:
4 系統(tǒng)測試
實驗地點為12 m×8 m×6 m的南開大學(xué)微納加工實驗室,,該環(huán)境下NLOS干擾現(xiàn)象比較嚴(yán)重。為減小人工對真實路徑測量所帶來的誤差,,本文實驗結(jié)果與實驗室內(nèi)QUALISYS視頻式三維運動采集系統(tǒng)的定位結(jié)果進(jìn)行對比,。通過在標(biāo)簽上粘貼被動標(biāo)記獲得QUALISYS系統(tǒng)的定位結(jié)果,其定位精度可達(dá)到亞毫米級,。試驗場景照片如圖5所示,,標(biāo)簽安裝在移動機器人的頂端。
系統(tǒng)測試分為兩部分,,分別對靜態(tài)點和動態(tài)點進(jìn)行定位實驗,。在試驗場地內(nèi)隨機選擇10個點作為定點實驗位置,分別做定位實驗 ,,每個點采集500次定位數(shù)據(jù)。實驗結(jié)果如圖6所示,,其中方框代表 QUALISYS系統(tǒng)測出的結(jié)果,,小黑點為本系統(tǒng)定位結(jié)果。
利用均方根誤差(RSME)對實驗數(shù)據(jù)進(jìn)行分析,,計算結(jié)果如表1所示,。可以看出,,在室內(nèi)NLOS環(huán)境下,,基于卡爾曼濾波的定位方法在定點定位中誤差能控制在13 cm以內(nèi)。
進(jìn)一步根據(jù)移動機器人實際定位的需求,,隨機選擇一個標(biāo)簽以1 m/s的速度在沿著設(shè)定路線進(jìn)行移動,,測試結(jié)果如圖7所示,其中深色軌跡為QUALISYS系統(tǒng)的定位結(jié)果,,淺色軌跡為所設(shè)計系統(tǒng)定位結(jié)果,。可以看到標(biāo)簽偏離目標(biāo)估計的最大距離為20 cm,,保證了移動機器人移動時的定位精度,。
5 結(jié)語
本文針對目前移動機器人室內(nèi)定位方式靈活性差和精度不高的問題,利用UWB技術(shù),,設(shè)計了一種高精度移動機器人室內(nèi)定位系統(tǒng),。一方面采用ADS-TWR測距技術(shù)保證定位系統(tǒng)的實時性和測距精度;另一方面采用卡爾曼濾波方法進(jìn)行定位,,濾除非視距誤差,,保證系統(tǒng)的定位精度,。實驗表明,系統(tǒng)具有高精度,、高穩(wěn)定性的特點,,滿全能滿足移動機器人室內(nèi)定位的需求。
參考文獻(xiàn)
[1] CHUNG H,,OJEDA L,,BORENSTEIN J.Accurate mobile robot dead-reckoning with a precision-calibrated fiber-optic gyroscope[J].Robotics & Automation IEEE Transactions on,2001,,17(1):80-84.
[2] 王煒強.基于視覺定位的地圖構(gòu)建方法研究[D].浙江:浙江大學(xué),,2010.
[3] 房秉毅.基于超寬帶技術(shù)的室內(nèi)定位系統(tǒng)[J].電子技術(shù)應(yīng)用,2006,,32(7):124-127.
[4] 陶偲.基于UWB的室內(nèi)SDS-TWR測距算法優(yōu)化和定位算法融合的研究[D].武漢:華中師范大學(xué),,2016.
[5] 張宴龍.室內(nèi)定位關(guān)鍵技術(shù)研究[D].合肥:中國科學(xué)技術(shù)大學(xué),2014.
[6] ZHANG L,,ZHANG H,,CUI X R,et al.Ultra wideband indoor positioning using Kalman filters[C].Advanced Materials Research,,2012,,433-440:4207-4213.
作者信息:
盧靖宇1,2,,余文濤1,,2,趙 新1,,2,,孫廣毅1,2
(1.南開大學(xué) 計算機與控制工程學(xué)院,,天津300350,;2.天津市智能機器人技術(shù)重點實驗室,天津300350)