周奇勛, 王 茜, 劉全龍, 蔡紫薇

(西安科技大學(xué)礦用電機(jī)驅(qū)動(dòng)技術(shù)研究所， 西安 710054)

機(jī)器人運(yùn)動(dòng)的能量來(lái)自于自身攜帶的電池。為了使機(jī)器人能夠可持續(xù)工作，當(dāng)機(jī)器人攜帶的電池電量低時(shí)，能夠準(zhǔn)確找到機(jī)器人充電樁位置完成自主充電這一過(guò)程就變得尤為重要。常見(jiàn)的室內(nèi)定位技術(shù)有基準(zhǔn)系統(tǒng)(reference point system，RPS)激光定位、視覺(jué)同步定位與建圖(visual simultaneous localization and mapping，VSLAM)、WiFi定位[1]、射頻識(shí)別(radio frequency idenfication，RFID)定位[2]、超聲波定位[3]等。RPS激光定位中激光探頭成本價(jià)格高，工作時(shí)激光探頭不停地旋轉(zhuǎn)導(dǎo)致壽命有限；VSLAM圖像位移定位算法復(fù)雜且對(duì)硬件資源要求高；WiFi定位對(duì)環(huán)境依賴(lài)性較大，定位精度低，可達(dá)到米級(jí)別；RFID定位布局難度大，對(duì)環(huán)境依賴(lài)性也較大；而超聲波定位具有抗干擾強(qiáng)、布局簡(jiǎn)單、定位精度高和成本低等優(yōu)勢(shì)[4-5]。

文獻(xiàn)[6]將測(cè)量誤差精度提高到不超過(guò)3 cm，但需要在定位區(qū)域分散安裝4個(gè)定位基點(diǎn)，再結(jié)合三邊測(cè)距原理得到移動(dòng)節(jié)點(diǎn)的坐標(biāo)(x,y)，文獻(xiàn)[7]建立誤差修正模型提高了定位精度，但定位算法采用了最小二乘法，需要系統(tǒng)中安裝多個(gè)參考節(jié)點(diǎn)，還加入了匯聚節(jié)點(diǎn)，增加了系統(tǒng)實(shí)現(xiàn)難度。

現(xiàn)采用由兩個(gè)發(fā)送節(jié)點(diǎn)和三個(gè)接收節(jié)點(diǎn)組成的被動(dòng)式移動(dòng)架構(gòu)(2T3R passive mobile architecture，2T3R-PMA)定位系統(tǒng)，定位節(jié)點(diǎn)固定在機(jī)器人充電樁上，能夠?qū)崿F(xiàn)機(jī)器人的自主充電功能；通過(guò)測(cè)量模型除了得到機(jī)器人的位置坐標(biāo)(x,y)，還可得到其相對(duì)于機(jī)器人充電樁的方向角。

1 2T3R-PMA定位原理與測(cè)量模型

采用的超聲波測(cè)距方法為到達(dá)時(shí)間差(time difference of arrival，TDOA)方法[7-8]。TDOA方法的示意圖如圖1所示。機(jī)器人充電樁同時(shí)發(fā)送無(wú)線(xiàn)同步信號(hào)和超聲波信號(hào)，由于無(wú)線(xiàn)信號(hào)傳播速率(v1)遠(yuǎn)遠(yuǎn)大于超聲波傳播速率(v2)，可忽略不計(jì)無(wú)線(xiàn)信號(hào)的傳播時(shí)間，即t1近似為0，Δt即為超聲波的傳播時(shí)間。

t1為無(wú)線(xiàn)信號(hào)的傳播時(shí)間；t2為超聲波信號(hào)的傳播時(shí)間；Δt為兩者差值圖1 TDOA方法示意圖Fig.1 Schematic diagram of TDOA method

2T3R-PMA的定位思想為機(jī)器人攜帶電池電量低時(shí)，通過(guò)無(wú)線(xiàn)模塊向機(jī)器人充電樁定位基站發(fā)送“定位”無(wú)線(xiàn)信號(hào)，機(jī)器人充電樁接收到“定位”無(wú)線(xiàn)信號(hào)后，兩個(gè)發(fā)送節(jié)點(diǎn)Ti(i=1,2)，周期性地輪流發(fā)送帶有發(fā)送節(jié)點(diǎn)標(biāo)識(shí)碼的無(wú)線(xiàn)同步信號(hào)和超聲波信號(hào)。機(jī)器人接收到帶有標(biāo)識(shí)碼的無(wú)線(xiàn)同步信號(hào)開(kāi)始計(jì)時(shí)；三個(gè)接收節(jié)點(diǎn)Rj(j=1,2,3)均接收到超聲波信號(hào)后，停止計(jì)時(shí)，利用TDOA方法可得到超聲波信號(hào)從發(fā)送節(jié)點(diǎn)Ti到接收節(jié)點(diǎn)Rj的傳播時(shí)間，乘以當(dāng)前聲速，進(jìn)而估算出Ti與Rj之間的距離。

2T3R-PMA定位系統(tǒng)示意圖如圖2所示。對(duì)機(jī)器人充電樁來(lái)說(shuō)，以?xún)蓚€(gè)發(fā)送節(jié)點(diǎn)的中點(diǎn)Oc為坐標(biāo)系原點(diǎn)，直線(xiàn)T1T2為xc軸；對(duì)機(jī)器人來(lái)說(shuō)，以R2作為相對(duì)坐標(biāo)系的原點(diǎn)Or，直線(xiàn)R1R2R3為xr軸。xr與xc的夾角θ為機(jī)器人的方位角。其中2a為機(jī)器人充電樁上兩個(gè)發(fā)送節(jié)點(diǎn)之間的距離，2b為機(jī)器人上R1與R3接收節(jié)點(diǎn)之間的距離，取a=10 cm，b=30 cm。

圖2 2T3R-PMA定位系統(tǒng)示意圖Fig.2 Schematic diagram of 2T3R-PMA positioning system

假定發(fā)送節(jié)點(diǎn)和接收節(jié)點(diǎn)安裝在同一高度上，在2T3R-PMA定位系統(tǒng)中，設(shè)機(jī)器人的位置信息為(x,y,θ)T，接收節(jié)點(diǎn)R1、R2、R3的坐標(biāo)分別為(x1,y1)、(x2,y2)和(x3,y3)，發(fā)送節(jié)點(diǎn)到接收節(jié)點(diǎn)的距離公式可以表示為

(1)

式(1)中：(xj,yj)為接收節(jié)點(diǎn)的坐標(biāo)；d1j為2#發(fā)射節(jié)點(diǎn)到j(luò)#接收節(jié)點(diǎn)的距離，cm；d2j為2#發(fā)射節(jié)點(diǎn)到j(luò)#接收節(jié)點(diǎn)的距離，cm。

任意兩個(gè)接收節(jié)點(diǎn)xm、xn與兩個(gè)發(fā)送節(jié)點(diǎn)ym、yn構(gòu)成的測(cè)量模型可表示為

(2)

由式(1)和式(2)可得x和y的表達(dá)式為

(3)

(4)

(5)

機(jī)器人方向角θ的表達(dá)式為

(6)

(7)

(8)

接收節(jié)點(diǎn)與m，n，k的取值對(duì)應(yīng)表如表1所示。

表1 接收節(jié)點(diǎn)與m，n，k的取值對(duì)應(yīng)表

2 定位誤差分析及修正

2.1 算法誤差

算法誤差產(chǎn)生的主要原因是d1j和d2j距離值的不準(zhǔn)確。而導(dǎo)致距離值不準(zhǔn)確的原因有兩種：①由于超聲波存在反射現(xiàn)象，機(jī)器人得到的d1j和d2j有可能為反射后的不準(zhǔn)確數(shù)據(jù);②當(dāng)機(jī)器人相對(duì)于機(jī)器人充電樁的方向角|θ|過(guò)大或者機(jī)器人與其充電樁距離較近時(shí)，會(huì)導(dǎo)致兩邊接收節(jié)點(diǎn)R1或R3接收不到超聲波信號(hào)。

當(dāng)d1j和d2j距離值數(shù)據(jù)不準(zhǔn)確時(shí)，直觀(guān)地反映在測(cè)量模型公式上存在兩種情況：①式(4)中ym或yn的根號(hào)下出現(xiàn)負(fù)值情況;②式(6)～式(8)中的sinθ、cosθ和tanθ的值出現(xiàn)大于1的情況。

以R1接收節(jié)點(diǎn)(即m=1)為例，假設(shè)d11=41.6 cm和d21=63.2 cm，各個(gè)中間變量的值如表2所示。

表2 d11=41.6 cm和d21=63.2 cm時(shí)各變量的值

此時(shí)y1的第一個(gè)根號(hào)下出現(xiàn)了負(fù)值，為-440.25，這時(shí)會(huì)計(jì)算出復(fù)數(shù)值，造成最終結(jié)果y的誤差。誤差修正的辦法為在程序中設(shè)置濾值條件，舍棄不符合條件的距離值，保證不會(huì)出現(xiàn)根號(hào)下為負(fù)值的情況。假設(shè)每個(gè)接收節(jié)點(diǎn)都能接收到兩個(gè)發(fā)送節(jié)點(diǎn)的超聲波信號(hào)，以R1為例，T1、T2和R1三個(gè)節(jié)點(diǎn)一起組成△T1T2R1，三角形各個(gè)邊長(zhǎng)2a、d11和d21滿(mǎn)足三角形構(gòu)成條件|d11-d21|<2a。同理，R2和R3的約束條件分別為|d12-d22|<2a和|d13-d23|<2a。

以R1和R2節(jié)點(diǎn)(即m=1，n=2，k=b/2=15)為例，假設(shè)d11=348.86 cm，d21=347.44 cm，d12=361.42 cm和d22=362.35 cm，可得

此時(shí)得出的余弦值無(wú)法用于計(jì)算方位角θ。誤差修正的方法為在程序中設(shè)置判斷條件，若得到的正弦、余弦或者正切值出現(xiàn)大于1的情況，舍棄該值，用其他值計(jì)算θ。

當(dāng)R1、R2和R3均收到超聲波信號(hào)時(shí)，采用兩兩組合的方式均代入測(cè)量模型公式，最終結(jié)果取平均值。當(dāng)R1節(jié)點(diǎn)接收不到超聲波信號(hào)，就直接采用R2和R3兩節(jié)點(diǎn)的數(shù)據(jù)進(jìn)行計(jì)算。同理，當(dāng)R3節(jié)點(diǎn)接收不到信號(hào)的情況下，用R1和R2兩節(jié)點(diǎn)的數(shù)據(jù)計(jì)算。

2.2 測(cè)量誤差

測(cè)量誤差主要由硬件測(cè)量時(shí)間和程序執(zhí)行時(shí)間等組成[7]。

TDOA測(cè)距方法默認(rèn)無(wú)線(xiàn)信號(hào)與超聲波信號(hào)同時(shí)發(fā)出，實(shí)際上無(wú)線(xiàn)信號(hào)通過(guò)串口通信直接發(fā)出，超聲波信號(hào)需脈沖信號(hào)驅(qū)動(dòng)發(fā)射電路才能發(fā)出，導(dǎo)致超聲波發(fā)出時(shí)刻滯后于無(wú)線(xiàn)信號(hào)發(fā)出時(shí)刻。機(jī)器人充電樁上無(wú)線(xiàn)信號(hào)和超聲波信號(hào)發(fā)出時(shí)序圖如圖3所示。

t3為無(wú)線(xiàn)信號(hào)的發(fā)出時(shí)刻；t4為超聲波信號(hào)的發(fā)出時(shí)刻；Δt1為超聲波信號(hào)滯后無(wú)線(xiàn)信號(hào)發(fā)出的時(shí)間圖3 機(jī)器人充電樁上信號(hào)發(fā)出時(shí)序圖Fig.3 Time sequence diagram of signal sending on robot charging pile

機(jī)器人是否接收到超聲波信號(hào)的判斷依據(jù)是接收芯片輸出管腳是否產(chǎn)生下降沿。將接收芯片輸出管腳設(shè)置成外部中斷來(lái)監(jiān)測(cè)是否有下降沿，輸出管腳產(chǎn)生下降沿到外部中斷響應(yīng)有一定的時(shí)間延遲。

測(cè)量誤差通過(guò)分段拋物線(xiàn)插值來(lái)補(bǔ)償，分段拋物線(xiàn)插值的補(bǔ)償效果比曲線(xiàn)擬合和線(xiàn)性插值效果更好、更精確。拋物線(xiàn)插值的原理為已知三點(diǎn)(x0,y0)、(x1,y1)和(x2,y2)，可構(gòu)造出二次拉格朗日插值多項(xiàng)式為

(9)

2.3 溫度漂移

超聲波傳播速度會(huì)受到空氣中溫度、濕度、壓強(qiáng)等因素的影響，其中受溫度的變化影響最大[6-12]。超聲波在不同溫度下的傳播速度如表3所示，由表3中數(shù)據(jù)可以看出，超聲波傳播速度在-30 ℃時(shí)為313.1 m/s，而在30 ℃為349.0 m/s，假設(shè)在-30 ℃環(huán)境下測(cè)量卻用30 ℃的傳播速度進(jìn)行計(jì)算，傳播速度誤差為(349.0-313.1)/313.1=11.47%。

表3 不同溫度下對(duì)應(yīng)的超聲波傳播速度

對(duì)表3中數(shù)據(jù)進(jìn)行二次插值得到傳播速度與溫度表達(dá)式為

v=0.000 7t2+0.567 5t+v0

(10)

式(10)中：v為當(dāng)前傳播速度，m/s；t為當(dāng)前環(huán)境溫度，℃；v0=331.895 4 m/s。當(dāng)t=0 ℃,代入式(10)得到v=331.895 4，與實(shí)際0 ℃對(duì)應(yīng)傳播速度的誤差為|331.895 4-332.1|/332.1=0.62%，誤差明顯減少。所以對(duì)于溫度漂移的修正辦法為通過(guò)溫度傳感器檢測(cè)溫度，代入上述公式獲得當(dāng)前傳播速度。

3 定位裝置的設(shè)計(jì)與實(shí)現(xiàn)

2T3R-PMA定位系統(tǒng)組成如圖4所示，機(jī)器人充電樁具備發(fā)射超聲波和915 MHz無(wú)線(xiàn)通信功能；機(jī)器人具備接收超聲波、915 MHz無(wú)線(xiàn)通信和通過(guò)RS422與上位機(jī)通信等功能，為了定位的準(zhǔn)確性，還增加了溫度傳感器實(shí)時(shí)檢測(cè)溫度。

3.1 硬件設(shè)計(jì)

3.1.1 超聲波發(fā)射電路

超聲波發(fā)射電路如圖5所示，T1為中心頻率 40 kHz 的防水超聲波探頭，工作原理為CPU產(chǎn)生40 kHz的脈沖方波經(jīng)過(guò)升壓中周進(jìn)行放大，將電壓由3.3 V放大到140 V左右來(lái)驅(qū)動(dòng)探頭發(fā)射超聲波。

圖4 2T3R-PMA定位系統(tǒng)組成Fig.4 2T3R-PMA positioning system composition

圖5 超聲波發(fā)射電路Fig.5 Ultrasonic transmitting circuit

圖6 超聲波接收電路Fig.6 Ultrasonic receiving circuit

3.1.2 超聲波接收電路

超聲波接收電路如圖6所示，采用CX20106A芯片對(duì)超聲波信號(hào)進(jìn)行放大濾波，該芯片具有抗干擾能力強(qiáng)、靈敏度高等特性[8]。引腳7為是否接收到超聲波信號(hào)的指示引腳，未接收到超聲波時(shí)，該引腳為高電平；一旦接收到超聲波，該引腳立刻輸出低電平。

3.1.3 915 MHz無(wú)線(xiàn)模塊

機(jī)器人與其充電樁的信息交互通過(guò)AS10-M4463D-915SMA無(wú)線(xiàn)模塊來(lái)實(shí)現(xiàn)。該模塊采用全雙工SPI接口與STM32通信，工作頻段為915 MHz，避免了室內(nèi)其他2.4 GHz的干擾，保證了機(jī)器人與其充電樁通信的成功率和實(shí)時(shí)性。

3.1.4 溫度傳感器

采用的溫度傳感器為DALLAS公司的DS1624芯片，該溫度傳感器的分辨率為0.031 25 ℃，測(cè)量精度高，操作使用方便，能直接給CPU反饋溫度值。

3.2 軟件設(shè)計(jì)

2T3R-PMA定位系統(tǒng)軟件設(shè)計(jì)流程如圖7所示。程序開(kāi)始后，機(jī)器人給溫度傳感器發(fā)送測(cè)溫指令，用公式計(jì)算出當(dāng)前聲速，再通過(guò)與機(jī)器人充電樁的通信，得到超聲波傳播時(shí)間，進(jìn)而計(jì)算出探頭節(jié)點(diǎn)間距離，帶入測(cè)量模型中計(jì)算出機(jī)器人當(dāng)前的位置信息(x,y,θ)T。

圖7 2T3R-PMA定位系統(tǒng)軟件設(shè)計(jì)流程圖Fig.7 2T3R-PMA positioning system software design flow chart

4 實(shí)驗(yàn)結(jié)果分析

基于2T3R-PMA定位系統(tǒng)的定位實(shí)驗(yàn)就是機(jī)器人通過(guò)跟其充電樁的信息交互計(jì)算得到自己的位置信息，該系統(tǒng)定位的最遠(yuǎn)距離為6 m。該實(shí)驗(yàn)通過(guò)模擬充電樁保持不動(dòng)，不斷改變機(jī)器人的位置來(lái)進(jìn)行，實(shí)物模擬圖如圖8所示，通過(guò)測(cè)量?jī)x器來(lái)確定機(jī)器人與充電樁的距離和角度。

圖8 實(shí)物模擬圖Fig.8 Physical simulation diagram

實(shí)驗(yàn)數(shù)據(jù)如表4所示，表中測(cè)量距離為采樣100次的平均值。由表4數(shù)據(jù)可知，x的最大誤差為2.25 cm，y的最大誤差為2 cm，θ的最大誤差為3°，x和y的平均誤差均在2 cm以?xún)?nèi)。

表4 2T3R-PMA定位系統(tǒng)的實(shí)驗(yàn)數(shù)據(jù)

5 結(jié)論

為降低機(jī)器人定位系統(tǒng)安裝難度和提高定位精度，在以TDOA方法進(jìn)行超聲波測(cè)距的基礎(chǔ)上，設(shè)計(jì)了一種2T3R-PMA定位系統(tǒng)。實(shí)驗(yàn)結(jié)果表明該系統(tǒng)將機(jī)器人充電樁作為定位基站使得系統(tǒng)安裝更容易實(shí)現(xiàn)，三個(gè)接收節(jié)點(diǎn)的設(shè)計(jì)能夠有效降低定位誤差，具有一定的參考應(yīng)用價(jià)值。