基于被動式移動架構的室內機器人二維定位方法

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

(西安科技大學礦用電機驅動技術研究所， 西安 710054)

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

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

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

1 2T3R-PMA定位原理與測量模型

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

t1為無線信號的傳播時間；t2為超聲波信號的傳播時間；Δt為兩者差值圖1 TDOA方法示意圖Fig.1 Schematic diagram of TDOA method

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

2T3R-PMA定位系統(tǒng)示意圖如圖2所示。對機器人充電樁來說，以兩個發(fā)送節(jié)點的中點Oc為坐標系原點，直線T1T2為xc軸；對機器人來說，以R2作為相對坐標系的原點Or，直線R1R2R3為xr軸。xr與xc的夾角θ為機器人的方位角。其中2a為機器人充電樁上兩個發(fā)送節(jié)點之間的距離，2b為機器人上R1與R3接收節(jié)點之間的距離，取a=10 cm，b=30 cm。

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

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

(1)

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

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

(2)

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

(3)

(4)

(5)

機器人方向角θ的表達式為

(6)

(7)

(8)

接收節(jié)點與m，n，k的取值對應表如表1所示。

表1 接收節(jié)點與m，n，k的取值對應表

2 定位誤差分析及修正

2.1 算法誤差

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

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

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

表2 d11=41.6 cm和d21=63.2 cm時各變量的值

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

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

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

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

2.2 測量誤差

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

TDOA測距方法默認無線信號與超聲波信號同時發(fā)出，實際上無線信號通過串口通信直接發(fā)出，超聲波信號需脈沖信號驅動發(fā)射電路才能發(fā)出，導致超聲波發(fā)出時刻滯后于無線信號發(fā)出時刻。機器人充電樁上無線信號和超聲波信號發(fā)出時序圖如圖3所示。

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

機器人是否接收到超聲波信號的判斷依據(jù)是接收芯片輸出管腳是否產生下降沿。將接收芯片輸出管腳設置成外部中斷來監(jiān)測是否有下降沿，輸出管腳產生下降沿到外部中斷響應有一定的時間延遲。

測量誤差通過分段拋物線插值來補償，分段拋物線插值的補償效果比曲線擬合和線性插值效果更好、更精確。拋物線插值的原理為已知三點(x0,y0)、(x1,y1)和(x2,y2)，可構造出二次拉格朗日插值多項式為

(9)

2.3 溫度漂移

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

表3 不同溫度下對應的超聲波傳播速度

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

v=0.000 7t2+0.567 5t+v0

(10)

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

3 定位裝置的設計與實現(xiàn)

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

3.1 硬件設計

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

超聲波發(fā)射電路如圖5所示，T1為中心頻率 40 kHz 的防水超聲波探頭，工作原理為CPU產生40 kHz的脈沖方波經過升壓中周進行放大，將電壓由3.3 V放大到140 V左右來驅動探頭發(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芯片對超聲波信號進行放大濾波，該芯片具有抗干擾能力強、靈敏度高等特性[8]。引腳7為是否接收到超聲波信號的指示引腳，未接收到超聲波時，該引腳為高電平；一旦接收到超聲波，該引腳立刻輸出低電平。

3.1.3 915 MHz無線模塊

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

3.1.4 溫度傳感器

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

3.2 軟件設計

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

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

4 實驗結果分析

基于2T3R-PMA定位系統(tǒng)的定位實驗就是機器人通過跟其充電樁的信息交互計算得到自己的位置信息，該系統(tǒng)定位的最遠距離為6 m。該實驗通過模擬充電樁保持不動，不斷改變機器人的位置來進行，實物模擬圖如圖8所示，通過測量儀器來確定機器人與充電樁的距離和角度。

圖8 實物模擬圖Fig.8 Physical simulation diagram

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

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

5 結論

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