0 引言

    移動(dòng)機(jī)器人定位是其自主導(dǎo)航中的最基本環(huán)節(jié)，也是移動(dòng)機(jī)器人完成任務(wù)必須解決的問(wèn)題。對(duì)定位的要求是定位精度高（亞米級(jí)精度），實(shí)時(shí)性好。目前移動(dòng)機(jī)器人室內(nèi)定位主要分為兩大類：(1)相對(duì)定位法，即航跡推算法^[1]。利用機(jī)器人所裝備的各種傳感器獲取機(jī)器人的運(yùn)動(dòng)動(dòng)態(tài)信息，通過(guò)遞推累計(jì)公式獲得機(jī)器人相對(duì)初始狀態(tài)的估計(jì)位置。使用的傳感器主要是碼盤(pán)和慣性傳感器。但是他們都有一個(gè)共同的缺點(diǎn)：存在累積誤差，隨著行駛時(shí)間、距離的不斷增加，誤差也不斷增大，不適合長(zhǎng)時(shí)間長(zhǎng)距離的精確定位。(2)絕對(duì)定位法，即機(jī)器人通過(guò)獲取外界一些位置等已知的參照信息，通過(guò)計(jì)算自己與參照信息之間的相互關(guān)系解算出自己的位置。絕對(duì)定位法主要采用同步定位與建圖^[2]（Simultaneous Localization and Mapping，SLAM）、視覺(jué)定位方法以及基于信標(biāo)定位等方法。其中SLAM定位法和視覺(jué)定位數(shù)據(jù)量大，價(jià)格昂貴,目前只適宜試驗(yàn)研究使用。而且，這兩種方法只適用于一些結(jié)構(gòu)簡(jiǎn)單的環(huán)境，對(duì)移動(dòng)機(jī)器人一般工作的復(fù)雜室內(nèi)環(huán)境并不能提供很好的定位精度。因此，本系統(tǒng)選用基于UWB的信標(biāo)定位的方法來(lái)實(shí)現(xiàn)復(fù)雜室內(nèi)環(huán)境下的移動(dòng)機(jī)器人室內(nèi)定位。

    UWB信號(hào)具有超高分辨率、抗多徑效應(yīng)、穿透力強(qiáng)以及結(jié)構(gòu)簡(jiǎn)單的優(yōu)點(diǎn)，成為目前室內(nèi)高精度定位的最佳技術(shù)^[3]。常用的UWB定位方式為基于到達(dá)時(shí)間法(Time of Arrival，TOA)以及到達(dá)時(shí)間差法(Time Difference of Arrival，TDOA)。但是，這TOA法需要標(biāo)簽與基站之間時(shí)鐘同步，TDOA法需要基站與基站之間的時(shí)鐘同步，這增大了系統(tǒng)設(shè)計(jì)難度。

    本文基于UWB技術(shù)，采用decaWave公司生產(chǎn)的DWM1000模塊，應(yīng)用非對(duì)稱雙邊雙向測(cè)距(Asymmetric Double Sided Two-Way Range，ADS-TWR)技術(shù)進(jìn)行定位，無(wú)需基站與標(biāo)簽之間和基站與基站之間時(shí)鐘同步，大大縮減系統(tǒng)設(shè)計(jì)難度。針對(duì)實(shí)際應(yīng)用中由非視距傳播引起的測(cè)量誤差，采用卡爾曼濾波算法對(duì)測(cè)距進(jìn)行優(yōu)化，提高定位精度。最終實(shí)現(xiàn)一種高精度、高實(shí)時(shí)性的移動(dòng)機(jī)器人室內(nèi)定位系統(tǒng)。

1 系統(tǒng)總體設(shè)計(jì)

    移動(dòng)機(jī)器人室內(nèi)定位系統(tǒng)主要由UWB無(wú)線傳感器網(wǎng)絡(luò)和上位機(jī)顯示軟件兩部分組成，系統(tǒng)示意圖如圖1所示。系統(tǒng)硬件部分包括基站和安裝在移動(dòng)機(jī)器人頂端的標(biāo)簽。其中基站分為普通基站(基站2、基站3和基站4)和通信基站(基站1)。標(biāo)簽和基站均由單片機(jī)和DWM1000組成的通信模塊構(gòu)成，并由軟件配置模塊的角色(標(biāo)簽或者基站)。DWM1000可以精確地測(cè)量UWB信號(hào)發(fā)送和接收的時(shí)間點(diǎn)，通過(guò)ADS-TWR 技術(shù)測(cè)得標(biāo)簽到各個(gè)基站之間的距離，利用UWB通信功能，各基站將距離信息發(fā)送給通信基站，最終通過(guò)WiFi將距離信息傳送給上位機(jī)進(jìn)行定位并顯示。通信基站的硬件結(jié)構(gòu)圖如圖2所示（標(biāo)簽和普通基站沒(méi)有WiFi模塊）。

2 基于ADS-TWR技術(shù)的測(cè)距及優(yōu)化

    無(wú)線定位系統(tǒng)定位的準(zhǔn)確性取決于測(cè)距的精度。UWB定位系統(tǒng)測(cè)距誤差的來(lái)源除了非視距引起的誤差外，還包括晶振的時(shí)鐘漂移引起的誤差。晶振時(shí)鐘漂移會(huì)影響對(duì)信號(hào)發(fā)送和接收時(shí)間點(diǎn)的測(cè)量，繼而影響測(cè)距的準(zhǔn)確性。UWB定位系統(tǒng)最簡(jiǎn)單的測(cè)距方式是單程測(cè)距（One Way Ranging，OWR），但是這對(duì)節(jié)點(diǎn)之間的時(shí)鐘同步要求極其嚴(yán)格，而雙程測(cè)距（Two Way Ranging，TWR）雖能消除節(jié)點(diǎn)之間未能完全同步的影響，但無(wú)法消除晶振時(shí)鐘漂移的影響。而對(duì)稱雙邊雙向測(cè)距（Symmetry Double Sided Two-Way Range，SDS-TWR）可以消除晶振時(shí)鐘漂移的影響，但是要求信號(hào)回復(fù)時(shí)間嚴(yán)格相等，這樣大大降低了定位的實(shí)時(shí)性^[4]。在此采用ADS-TWR測(cè)距技術(shù)。

    ADS-TWR測(cè)距過(guò)程如圖3所示，圖中poll_TX、poll_RX、answer_TX、answer_RX、final_TX、final_RX代表UWB信號(hào)離開(kāi)標(biāo)簽和基站天線的時(shí)間點(diǎn)。測(cè)距過(guò)程如下：首先標(biāo)簽向基站請(qǐng)求幀；基站收到請(qǐng)求幀后啟動(dòng)計(jì)時(shí)，經(jīng)過(guò)延時(shí)T_reply1后向標(biāo)簽發(fā)送應(yīng)答幀；標(biāo)簽收到應(yīng)答幀后啟動(dòng)計(jì)時(shí)，并將發(fā)送和接收信號(hào)時(shí)的時(shí)間點(diǎn)寫(xiě)入終止幀，經(jīng)延時(shí)T_reply2后發(fā)送給基站；基站收終止幀后表示測(cè)距結(jié)束。

    距離計(jì)算公式如式(1)和(2)所示：

其中，k_t和 k_a為標(biāo)簽和基站時(shí)鐘偏移系數(shù)，二者都接近于1。對(duì)于20 ppm的時(shí)鐘(最壞規(guī)格的時(shí)鐘)，則k_t和k_a都可以是0.999 98或1.000 02。對(duì)于相對(duì)較大的測(cè)距范圍如100 m，T_tof僅為333 ns，飛行時(shí)間測(cè)量誤差為6.7 ps，換算為距離誤差僅為2.2 mm。因此，ADS-TWR測(cè)距能很好地抑制時(shí)鐘漂移的影響。

    ADS-TWR測(cè)距不要求應(yīng)答時(shí)間T_reply1和T_reply2相等。因此，在標(biāo)簽與多個(gè)基站進(jìn)行通信時(shí)，可以通過(guò)設(shè)定各節(jié)點(diǎn)的應(yīng)答時(shí)間來(lái)減少標(biāo)簽測(cè)距的時(shí)間，保證定位系統(tǒng)的實(shí)時(shí)性。

    圖4為本文所采用的多基站測(cè)距機(jī)制。標(biāo)簽向4個(gè)基站發(fā)送請(qǐng)求幀，基站接收到請(qǐng)求幀后按照設(shè)定的應(yīng)答時(shí)間依次向標(biāo)簽發(fā)送應(yīng)答幀，標(biāo)簽接收到應(yīng)答幀后，將4個(gè)基站用于計(jì)算距離參數(shù)寫(xiě)入終止幀，并發(fā)送給所有基站；各基站接收到終止幀后測(cè)距結(jié)束?；纠檬?1)和(2)計(jì)算出距離，然后通過(guò)UWB發(fā)送給通信基站。實(shí)際應(yīng)用中，對(duì)于四基站定位系統(tǒng)，通過(guò)優(yōu)化各基站應(yīng)答時(shí)間，可使單輪測(cè)距時(shí)間能控制在2 ms左右，完全可以滿足移動(dòng)機(jī)器人定位對(duì)實(shí)時(shí)性的要求。

3 基于卡爾曼濾波的定位算法

    移動(dòng)機(jī)器人在室內(nèi)活動(dòng)，不可避免地受到遮擋的影響。標(biāo)簽與基站之間由于存在人或座椅等遮擋物，這時(shí)UWB信號(hào)不能進(jìn)行直線傳播，而是利用衍射、透射和反射的方式到達(dá)接收端，就是非視距傳播（NLOS）。這時(shí)系統(tǒng)對(duì)飛行時(shí)間的測(cè)量就是會(huì)出現(xiàn)誤差，而定位精度也就有了誤差。非視距誤差受到室內(nèi)環(huán)境影響，是一個(gè)實(shí)時(shí)變化的值。由于非視距傳播增加了信號(hào)的傳播時(shí)間和傳播距離，因此非視距誤差服從一個(gè)正均值的隨機(jī)過(guò)程^[5]。

    假設(shè)t時(shí)刻標(biāo)簽到基站的距離為d_i(t)，r_i(t)表示二者之間的真實(shí)距離，N_i(t)表示觀測(cè)時(shí)由環(huán)境因素引起的非視距誤差，n_i(t)為零均值高斯噪聲，則可用下式表示它們之間的關(guān)系：

    本文采用卡爾曼濾波算法對(duì)距離進(jìn)行優(yōu)化，對(duì)NLOS誤差值進(jìn)行迭代處理，以此來(lái)減小定位誤差。首先利用狀態(tài)向量方程對(duì)距離信號(hào)進(jìn)行卡爾曼濾波，估計(jì)出NLOS誤差值N_i(t)，然后從最初的測(cè)距值d_i(t)中除去非視距誤差，得到精確地距離值^[6]。系統(tǒng)的狀態(tài)方程和測(cè)量方程如下：

其中，Δt為采樣間隔；ω_d(t)和ω_N(t)分別為測(cè)量過(guò)程中的噪聲誤差分量；β為實(shí)驗(yàn)參數(shù)；v_i(t)為測(cè)量誤差。再給出t時(shí)刻的狀態(tài)向量和估計(jì)誤差的協(xié)方差的初始值之后，就可以通過(guò)迭代運(yùn)算對(duì)不同時(shí)刻的狀態(tài)向量做出估計(jì)。由于非視距誤差具有非負(fù)性，因此在迭代過(guò)程中如N_i(t)出現(xiàn)負(fù)值就強(qiáng)制置零。

    根據(jù)基站坐標(biāo)和標(biāo)簽到基站的距離可以得到以下方程組：

其中(x_i，y_i，z_i)為基站i的坐標(biāo)，d_i為經(jīng)過(guò)卡爾曼濾波優(yōu)化后標(biāo)簽到基站i的距離。用第i個(gè)式子減去第j個(gè)式子得：

4 系統(tǒng)測(cè)試

    實(shí)驗(yàn)地點(diǎn)為12 m×8 m×6 m的南開(kāi)大學(xué)微納加工實(shí)驗(yàn)室，該環(huán)境下NLOS干擾現(xiàn)象比較嚴(yán)重。為減小人工對(duì)真實(shí)路徑測(cè)量所帶來(lái)的誤差，本文實(shí)驗(yàn)結(jié)果與實(shí)驗(yàn)室內(nèi)QUALISYS視頻式三維運(yùn)動(dòng)采集系統(tǒng)的定位結(jié)果進(jìn)行對(duì)比。通過(guò)在標(biāo)簽上粘貼被動(dòng)標(biāo)記獲得QUALISYS系統(tǒng)的定位結(jié)果，其定位精度可達(dá)到亞毫米級(jí)。試驗(yàn)場(chǎng)景照片如圖5所示，標(biāo)簽安裝在移動(dòng)機(jī)器人的頂端。

    系統(tǒng)測(cè)試分為兩部分，分別對(duì)靜態(tài)點(diǎn)和動(dòng)態(tài)點(diǎn)進(jìn)行定位實(shí)驗(yàn)。在試驗(yàn)場(chǎng)地內(nèi)隨機(jī)選擇10個(gè)點(diǎn)作為定點(diǎn)實(shí)驗(yàn)位置，分別做定位實(shí)驗(yàn) ，每個(gè)點(diǎn)采集500次定位數(shù)據(jù)。實(shí)驗(yàn)結(jié)果如圖6所示，其中方框代表 QUALISYS系統(tǒng)測(cè)出的結(jié)果，小黑點(diǎn)為本系統(tǒng)定位結(jié)果。

    利用均方根誤差（RSME）對(duì)實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析，計(jì)算結(jié)果如表1所示?？梢钥闯?，在室內(nèi)NLOS環(huán)境下，基于卡爾曼濾波的定位方法在定點(diǎn)定位中誤差能控制在13 cm以內(nèi)。

    進(jìn)一步根據(jù)移動(dòng)機(jī)器人實(shí)際定位的需求，隨機(jī)選擇一個(gè)標(biāo)簽以1 m/s的速度在沿著設(shè)定路線進(jìn)行移動(dòng)，測(cè)試結(jié)果如圖7所示，其中深色軌跡為QUALISYS系統(tǒng)的定位結(jié)果，淺色軌跡為所設(shè)計(jì)系統(tǒng)定位結(jié)果?？梢钥吹綐?biāo)簽偏離目標(biāo)估計(jì)的最大距離為20 cm，保證了移動(dòng)機(jī)器人移動(dòng)時(shí)的定位精度。

5 結(jié)語(yǔ)

    本文針對(duì)目前移動(dòng)機(jī)器人室內(nèi)定位方式靈活性差和精度不高的問(wèn)題，利用UWB技術(shù)，設(shè)計(jì)了一種高精度移動(dòng)機(jī)器人室內(nèi)定位系統(tǒng)。一方面采用ADS-TWR測(cè)距技術(shù)保證定位系統(tǒng)的實(shí)時(shí)性和測(cè)距精度；另一方面采用卡爾曼濾波方法進(jìn)行定位，濾除非視距誤差，保證系統(tǒng)的定位精度。實(shí)驗(yàn)表明，系統(tǒng)具有高精度、高穩(wěn)定性的特點(diǎn)，滿全能滿足移動(dòng)機(jī)器人室內(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] 王煒強(qiáng).基于視覺(jué)定位的地圖構(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測(cè)距算法優(yōu)化和定位算法融合的研究[D].武漢：華中師范大學(xué)，2016.

[5] 張宴龍.室內(nèi)定位關(guān)鍵技術(shù)研究[D].合肥：中國(guó)科學(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.南開(kāi)大學(xué) 計(jì)算機(jī)與控制工程學(xué)院，天津300350；2.天津市智能機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室，天津300350）