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

吳 鵬,于世東

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

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

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

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

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

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

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

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

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

圖2 TWR 測(cè)距

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

在這里首先給出幾個(gè)時(shí)間定義及公式：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

由于是室內(nèi)定位距離計(jì)數(shù)單位是米,因此Tf的數(shù)量級(jí)是納秒級(jí),在式(11)與式(14)中Ae是目標(biāo)A 的時(shí)鐘頻率誤差,Be是目標(biāo)B 的時(shí)鐘頻率誤差其數(shù)量級(jí)是微秒級(jí)的誤差,在計(jì)算過(guò)程中相差103的數(shù)量級(jí),因此式(11)與式(14)中起到?jīng)Q定作用的是第二項(xiàng).因?yàn)門(mé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ù)傳輸

通過(guò)上面改進(jìn)的TWR 雙向測(cè)距可以獲得室內(nèi)機(jī)器人與基站的距離,由于是室內(nèi)測(cè)距,為了方便計(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 個(gè)基站可以覆蓋千平方米的范圍,可以將基站放于平面的坐標(biāo)原點(diǎn),和x,y軸上,這樣更便于計(jì)算,如果將基站1 定位原點(diǎn),基站2 在x軸,基站3 在y軸上,方程組(15)變成方程組(16)運(yùn)算速度大大加快.

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

圖5 位置換算

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

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

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

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

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

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

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

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

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

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

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

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

表1 誤差表(單位：cm)

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