基于UWB室內(nèi)送餐機(jī)器人定位信息系統(tǒng)①

吳 鵬,于世東

1(沈陽師范大學(xué) 軟件學(xué)院,沈陽 110036)

2(中國科學(xué)院 沈陽計(jì)算技術(shù)研究所,沈陽 110168)

目前基于移動位置服務(wù)LBS 發(fā)展迅速,室外定位技術(shù)已經(jīng)可以滿足日常人們生活生產(chǎn)的需要,其定位精度在民用上達(dá)到米級,室外定位系統(tǒng)在室外空曠環(huán)境中使用時,信號狀態(tài)良好,但在室內(nèi)進(jìn)行定位,由于受到建筑結(jié)構(gòu)等環(huán)境因素的影響,信號微弱進(jìn)行定位困難,定位精度嚴(yán)重下降.因此對于室內(nèi)定位的研究就非常必要,現(xiàn)在主要的定位技術(shù)有紅外定位、藍(lán)牙定位、UWB 定位、WLAN 定位、RFID 定位、ZigBee定位、超聲波定位以及LED 可見光定位等[1].本文采用UWB 是一種無載波通信技術(shù),利用納秒至微秒級的非正弦波窄脈沖傳輸數(shù)據(jù),UWB 發(fā)射功率很小,應(yīng)用范圍很廣[2],被用來應(yīng)用在近距離高速數(shù)據(jù)傳輸,目前利用其亞納秒級超窄脈沖來做近距離精確室內(nèi)定位.

1 系統(tǒng)結(jié)構(gòu)設(shè)計(jì)

本機(jī)器人定位系統(tǒng)整體結(jié)構(gòu)圖如圖1所示,在室內(nèi)場所具有不少于3 個的固定基站,基站與機(jī)器人采用超寬帶高時間分辨率信號進(jìn)行通信來確定機(jī)器人的位置,同時通過無線網(wǎng)絡(luò)將機(jī)器人的位置信息傳遞到遠(yuǎn)程服務(wù)器上去,終端設(shè)備可以在任何時間和地點(diǎn)查看機(jī)器人的位置和狀態(tài),也可以下發(fā)指令對機(jī)器人進(jìn)行控制和狀態(tài)調(diào)整.

圖1 系統(tǒng)結(jié)構(gòu)圖

系統(tǒng)的超寬帶無線收發(fā)模塊選用半導(dǎo)體公司DecaWave 推出的DWM1000 模塊,基站與路由器的無線網(wǎng)絡(luò)通信采用ESP8266 網(wǎng)絡(luò)WiFi 模塊,服務(wù)器采用IIS 提供的Web 服務(wù),終端設(shè)備可以是計(jì)算機(jī),手機(jī),平板等可上網(wǎng)設(shè)備.

2 TWR 測距算法及改進(jìn)

雙向測距(TWR)是通過用無線電波的速度與飛行時間的乘積來確定兩個物體之間的距離,信號的飛行時間是測量距離的關(guān)鍵[3-9].圖2給出了雙向測距過程的原理.

圖2 TWR 測距

在T1時刻目標(biāo)A 發(fā)送超寬帶無線信息給目標(biāo)B,該信息帶有A 的發(fā)送時刻標(biāo)簽.在T2時刻目標(biāo)B 接收到該由A 發(fā)來的信息,進(jìn)行處理,同時記錄下當(dāng)前B 自己本身的時刻.在T3時刻目標(biāo)B 發(fā)送超寬帶無線信息給目標(biāo)A,該信息帶有B 接收A 時的時間標(biāo)簽和發(fā)送時的時間標(biāo)簽,同時將T1時刻A 發(fā)送的時間標(biāo)簽一同發(fā)送給B.在T4時刻目標(biāo)A 接收到目標(biāo)B 發(fā)送的信息.

在這里首先給出幾個時間定義及公式：

其中,Tf為在目標(biāo)A 與目標(biāo)B 之間無線信號傳輸所用的時間.

其中,Tdb為目標(biāo)B 處理目標(biāo)A 發(fā)送的信息時間,其可以通過目標(biāo)B 的發(fā)送時間標(biāo)簽與接收時間標(biāo)簽之差獲得.

其中,Tra是目標(biāo)A 發(fā)送信息到其接收目標(biāo)B 返回信息的時間間隔,該間隔可以通過目標(biāo)A 接收時刻的時間標(biāo)簽與其發(fā)送時刻的時間標(biāo)簽之差獲得.

目標(biāo)A 計(jì)算與目標(biāo)B 之間的距離為：

其中,D為目標(biāo)A 與B 的距離,C 為無線電波的傳輸速度.那么就可也計(jì)算出目標(biāo)之間的距離.在此算法中Tra是由目標(biāo)A 的時鐘計(jì)算得來,其中Tdb是根據(jù)目標(biāo)B 提供的時鐘得來,這樣就存在目標(biāo)A 與目標(biāo)B 時鐘不同步偏差造成的誤差.根據(jù)式(4)可以看出存在目標(biāo)A 與目標(biāo)B 的時鐘不同步的問題,由于時鐘不同步會一起測量誤差,圖3給出改進(jìn)的原理圖.

圖3 改進(jìn)TWR 測距

在圖3中增加了一次目標(biāo)A 做處理后再次發(fā)送給目標(biāo)B 的應(yīng)答信號,該信號中帶有T1,T2,T3,T4,T5的時間標(biāo)簽,由圖3可以得到目標(biāo)A 與目標(biāo)B 的距離公式：

其中,Tf為在目標(biāo)A 與目標(biāo)B 之間無線信號傳輸所用的時間.

其中,Tda為目標(biāo)A 處理目標(biāo)B 發(fā)送的信息時間,其可以通過目標(biāo)A 的發(fā)送時間標(biāo)簽與接收時間標(biāo)簽之差獲得.

其中,Trb是目標(biāo)B 發(fā)送信息到其接收目標(biāo)A 返回信息的時間間隔,該間隔可以通過目標(biāo)B 接收時刻的時間標(biāo)簽與其發(fā)送時刻的時間標(biāo)簽之差獲得.

目標(biāo)A 計(jì)算與目標(biāo)B 之間的距離為：

式(4)與式(8)相加可以得到如下公式：

上面的公式解決了目標(biāo)A 與目標(biāo)B 時鐘不同步偏差造成的誤差,因?yàn)樵谟?jì)算距離的時候都使用的自身時鐘進(jìn)行計(jì)算.下面分析式(4)與式(9)自身時鐘頻率偏差對測距產(chǎn)生的影響,由于存在時鐘偏差,式(4)的帶偏差的實(shí)際公式是：

其中,Ae是目標(biāo)A 的時鐘頻率誤差,Be是目標(biāo)B 的時鐘頻率誤差,實(shí)際的誤差由式(10)與式(4)做差得到：

實(shí)際的誤差由式(12)與式(9)做差得到：

若設(shè)Tda=Tdb+T?,則式(13)變換為：

由于是室內(nèi)定位距離計(jì)數(shù)單位是米,因此Tf的數(shù)量級是納秒級,在式(11)與式(14)中Ae是目標(biāo)A 的時鐘頻率誤差,Be是目標(biāo)B 的時鐘頻率誤差其數(shù)量級是微秒級的誤差,在計(jì)算過程中相差103的數(shù)量級,因此式(11)與式(14)中起到?jīng)Q定作用的是第二項(xiàng).因?yàn)門?在實(shí)際中是遠(yuǎn)小于Tdb,故T?(Ae?Be)C/4遠(yuǎn)小于Tdb(Ae?Be)C/2,因此式(14)的誤差遠(yuǎn)小于式(11)的誤差,計(jì)算誤差得到了改善.

3 定位與WiFi 數(shù)據(jù)傳輸

通過上面改進(jìn)的TWR 雙向測距可以獲得室內(nèi)機(jī)器人與基站的距離,由于是室內(nèi)測距,為了方便計(jì)算將基站的高度與機(jī)器人的UWB 標(biāo)簽放在同一高度,這樣就可以如圖4的TOA 原理計(jì)算機(jī)器人的位置[10-14].

圖4 TOA 定位

由圖4可以得到方程組：

其中,xi是基站的橫坐標(biāo),yi是基站的縱坐標(biāo),Di是機(jī)器人與基站的距離,解方程組可以得到機(jī)器人的坐標(biāo).由于在室內(nèi)3 個基站可以覆蓋千平方米的范圍,可以將基站放于平面的坐標(biāo)原點(diǎn),和x,y軸上,這樣更便于計(jì)算,如果將基站1 定位原點(diǎn),基站2 在x軸,基站3 在y軸上,方程組(15)變成方程組(16)運(yùn)算速度大大加快.

如果條件不允許將基站與機(jī)器人UWB 標(biāo)簽放于同一高度,那么只需要給出基站的高度可以將基站通過數(shù)學(xué)公式轉(zhuǎn)換到與機(jī)器人的UWB 標(biāo)簽在同一高度如圖5所示.

圖5 位置換算

圖5中Dt是基站與機(jī)器人標(biāo)簽實(shí)測距離,Hb是基站離地面距離,H是機(jī)器人UWB 標(biāo)簽離地面距離,從圖中看基站與機(jī)器人不在同一高度,通過式(17)可以將實(shí)測距離換算成同一高度的距離,這樣就提高了運(yùn)算速度.

在實(shí)際應(yīng)用中在參數(shù)設(shè)置界面給出基站的離地距離Hb,Hr是機(jī)器人UWB 標(biāo)簽離地面距離,由于機(jī)器人高度一定,所以可以自行標(biāo)定,如果機(jī)器人高度變化了,也可以手動標(biāo)定.這樣就可以采用式(17),進(jìn)行位置換算,減小計(jì)算量.

數(shù)據(jù)傳輸?shù)椒?wù)器采用無線以太網(wǎng)模式,其整體結(jié)構(gòu)如圖6所示.

圖6 通信結(jié)構(gòu)

圖6中DW100 模塊將時差的標(biāo)簽數(shù)據(jù)通過SPI 接口傳送到MCU,這里MCU 是中央處理器,其進(jìn)行機(jī)器人位置的計(jì)算,然后將計(jì)算機(jī)結(jié)果通過UART接口傳送給ESP8266,再由其傳輸?shù)椒?wù)器進(jìn)行存貯處理.ESP8266 模塊是采用TCP/IP 協(xié)議進(jìn)行無線透傳的,其要傳輸數(shù)據(jù)給Web 服務(wù)器需要使用HTTP 協(xié)議,所以在TCP/IP 協(xié)議的數(shù)據(jù)部分是HTTP 報頭與其內(nèi)部的數(shù)據(jù),也就是說透傳的內(nèi)容是HTTP 數(shù)據(jù)報.報文中包括時間、機(jī)器人的坐標(biāo)、狀態(tài)等內(nèi)容[15].

4 實(shí)驗(yàn)與結(jié)論

實(shí)驗(yàn)場地在長30 米寬20 米的室內(nèi)進(jìn)行,取了30 個隨機(jī)位置,通過測量隨機(jī)位置的坐標(biāo)和通過本系統(tǒng)的測得位置的坐標(biāo)進(jìn)行對比.

圖7是通過改進(jìn)的算法測得的數(shù)據(jù),在圖中隨機(jī)位置的實(shí)際坐標(biāo)用黑色點(diǎn)表示,改進(jìn)算法測量的隨機(jī)位置坐標(biāo)用灰色點(diǎn)表示,圓圈表示誤差在6 cm 的范圍.

圖7 改進(jìn)算法的位置檢測

圖8是未改進(jìn)的算法測得的數(shù)據(jù),在圖中隨機(jī)位置的實(shí)際坐標(biāo)用黑色點(diǎn)表示,未改進(jìn)算法測量的隨機(jī)位置的坐標(biāo)用灰色點(diǎn)表示,圓圈表示誤差在8 cm 的范圍.從圖7與圖8的對比中可以看到,圖7的定位基本都在圓圈范圍內(nèi),定位精度要高于圖8所示.

圖8 未改進(jìn)算法的位置檢測

通過兩組對比實(shí)驗(yàn),從每組中各隨機(jī)抽取7 個實(shí)驗(yàn)數(shù)據(jù)并計(jì)算偏差與偏差的平均值,在表1中可以看到算法改進(jìn)組的誤差明顯低于算法未改進(jìn)組的誤差.

表1 誤差表(單位：cm)

本文對UWB 室內(nèi)送餐機(jī)器人定位信息系統(tǒng)進(jìn)行了說明,給出了改進(jìn)的TWR 算法,并給與證明.其避免各基站時鐘不同步問題,減小了基站自身的時鐘偏移誤差所帶來的定位誤差.在進(jìn)行TOA 位置計(jì)算時,調(diào)整了基站與UWB 標(biāo)簽的位置,給出了在兩者不在同一水平面是的調(diào)整算法,簡化了計(jì)算.經(jīng)實(shí)際測試,此系統(tǒng)定位精度達(dá)到6 cm 以內(nèi),很好地提高了定位精度,同時本系統(tǒng)還可以通過網(wǎng)絡(luò)查看位置與狀態(tài),滿足室內(nèi)送餐機(jī)器人的應(yīng)用.