劉關(guān)迪 趙 璐

(南京工業(yè)大學(xué)計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院 江蘇 南京 211816)

0 引 言

隨著技術(shù)的發(fā)展和應(yīng)用場(chǎng)景的擴(kuò)展,可移動(dòng)設(shè)備的智能跟隨需求越來(lái)越大。然而目前主流采用的紅外傳感器、超聲傳感器等定位方式存在著無(wú)法鎖定跟隨目標(biāo)，易偏離目標(biāo)失去定位的問題[1]。這是因?yàn)槭覂?nèi)環(huán)境下障礙物較多，傳感器對(duì)目標(biāo)的測(cè)距易被障礙物阻擋，導(dǎo)致測(cè)量錯(cuò)誤；而GPS信號(hào)功率低、接收要求高，室內(nèi)定位時(shí)建筑物的阻擋又減弱了信號(hào)，導(dǎo)致GPS的定位精度低。因此，設(shè)計(jì)并實(shí)現(xiàn)一種可以精確定位目標(biāo)、準(zhǔn)確自動(dòng)跟隨的系統(tǒng)很有必要。

本文使用UWB(Ultra Wideband)技術(shù)實(shí)現(xiàn)精確定位，以樹莓派小車為移動(dòng)設(shè)備，設(shè)計(jì)并實(shí)現(xiàn)一個(gè)實(shí)時(shí)自動(dòng)跟隨系統(tǒng)，并進(jìn)一步對(duì)系統(tǒng)的可優(yōu)化性進(jìn)行討論。

1 UWB技術(shù)簡(jiǎn)介

UWB是一種無(wú)載波通信技術(shù)，其脈沖寬度僅為納秒級(jí)或亞納秒級(jí)，擁有數(shù)據(jù)傳輸速率高、發(fā)射平均功率譜密度低等多種優(yōu)點(diǎn)。其在定位方面有著兩方面優(yōu)勢(shì)：

1)由于其窄脈沖特點(diǎn)，定位精度可達(dá)厘米級(jí)甚至更高。

2)UWB通信采用調(diào)時(shí)序列，時(shí)間分辨率高，具有抗多徑能力和一定的穿透能力，適用于高速、低速等不同移動(dòng)環(huán)境[2-3]。

因此，UWB技術(shù)憑借其穿透性，可以很好地消除室內(nèi)障礙物對(duì)定位的影響；其信息傳送能力良好，測(cè)距準(zhǔn)確，精度高。該技術(shù)的使用解決了傳統(tǒng)室內(nèi)定位方法遇到的困難。近年來(lái)，國(guó)內(nèi)外逐漸將UWB技術(shù)應(yīng)用于近距離精確室內(nèi)定位，IEEE 802.15.4a標(biāo)準(zhǔn)采用UWB為定位首選技術(shù)[4-5]。

1.1 室內(nèi)定位模塊

UWB用于室內(nèi)定位時(shí)，需要配置定位標(biāo)簽和定位基站兩種模塊。

(1)定位標(biāo)簽。定位標(biāo)簽用于配置在需要定位的目標(biāo)端，是UWB定位的主模塊。其體積小，功耗低，可制成或集成于各類穿戴設(shè)備中，便于定位目標(biāo)攜帶。定位標(biāo)簽為有源電子標(biāo)簽，每個(gè)標(biāo)簽具有唯一的ID，可以將目標(biāo)與ID一對(duì)一綁定。

(2)定位基站。定位基站是UWB定位的從模塊。基站可以通過(guò)飛行時(shí)間測(cè)距法等多種方法，確定標(biāo)簽的位置，并將數(shù)據(jù)傳輸至網(wǎng)絡(luò)控制器及定位引擎軟件，定位精度達(dá)到厘米級(jí)。

1.2 測(cè)距原理

UWB測(cè)距使用飛行時(shí)間測(cè)距法(Time of Flight)。該技術(shù)需要在異步收發(fā)機(jī)(Transceiver)之間有準(zhǔn)確的時(shí)間同步[6-7]，利用射頻信號(hào)在異步收發(fā)機(jī)間的飛行時(shí)間TTOF轉(zhuǎn)換為兩點(diǎn)之間的距離d：

d=c·TTOF

(1)

式中：c為射頻信號(hào)的傳播速度，可認(rèn)為其等同于真空中的光速。

飛行時(shí)間測(cè)距法測(cè)距流程[8]如圖1所示，其中：TX表示發(fā)送；RX表示接收。設(shè)備A在TsA時(shí)刻發(fā)送請(qǐng)求脈沖信號(hào)，設(shè)備B在TrB時(shí)刻接收該信號(hào)，并于TsB時(shí)刻發(fā)射響應(yīng)信號(hào)，設(shè)備A在TrA時(shí)刻接收該響應(yīng)信號(hào)。設(shè)定設(shè)備A發(fā)送脈沖信號(hào)和接收到響應(yīng)信號(hào)的時(shí)間間隔為Tround，設(shè)備B從接收到請(qǐng)求脈沖信號(hào)到發(fā)射響應(yīng)信號(hào)的時(shí)間間隔為Treply，則:

圖1 TOF測(cè)距過(guò)程

(2)

2 系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

如圖2所示，系統(tǒng)由兩部分組成：用于實(shí)現(xiàn)自動(dòng)跟隨的樹莓派小車和跟隨目標(biāo)定位模塊。跟隨目標(biāo)攜帶一塊UWB主模塊作為標(biāo)簽，小車配置兩塊UWB從模塊作為基站。通過(guò)實(shí)時(shí)測(cè)量?jī)蓚€(gè)基站與標(biāo)簽之間的距離，結(jié)合數(shù)據(jù)處理來(lái)判斷目標(biāo)方向，實(shí)現(xiàn)跟隨。

圖2 系統(tǒng)硬件結(jié)構(gòu)示意圖

2.1 UWB模塊選型

系統(tǒng)的UWB模塊選用SWM1000射頻模塊。SWM1000模塊是基于Decawave的DW1000收發(fā)器的集成電路，其集成了天線、電源管理和時(shí)鐘電源系統(tǒng)，可用于高精度室內(nèi)測(cè)距與定位。

2.2 跟隨小車的結(jié)構(gòu)設(shè)計(jì)

跟隨小車由樹莓派控制，小車的四個(gè)輪子各配有一個(gè)步進(jìn)電機(jī)，樹莓派通過(guò)驅(qū)動(dòng)不同電機(jī)實(shí)現(xiàn)小車的前進(jìn)、后退和轉(zhuǎn)彎；通過(guò)控制PWM(Pulse Width Modulation)輸出值的變化實(shí)現(xiàn)小車速度的變化。

作為基站的兩塊SWM1000從模塊對(duì)稱安裝于小車后方左、右兩端，與樹莓派USB接口通過(guò)TTL轉(zhuǎn)USB轉(zhuǎn)換器相連。

2.3 跟隨距離的確定

跟隨目標(biāo)上的標(biāo)簽與跟隨小車上的兩個(gè)基站綁定、配對(duì)后建立通信通道。單次測(cè)距由標(biāo)簽主動(dòng)發(fā)起，基站一直處于等待監(jiān)聽狀態(tài)，并在收到標(biāo)簽發(fā)來(lái)的測(cè)距開始消息后與標(biāo)簽交互完成一次距離測(cè)量[9]。獲得與標(biāo)簽距離后，基站將測(cè)量距離通過(guò)USB接口發(fā)送至樹莓派。

如圖3所示，設(shè)定兩個(gè)基站A、B各自與y軸的距離為常數(shù)ds。左側(cè)基站A距離目標(biāo)標(biāo)簽E長(zhǎng)度為d1，右側(cè)基站B距離目標(biāo)標(biāo)簽E長(zhǎng)度為d2，小車中心O距離目標(biāo)標(biāo)簽E的距離為x，設(shè)目標(biāo)標(biāo)簽E、小車中心O與x軸負(fù)方向構(gòu)成夾角為θ。

圖3 跟隨小車定位示意圖

可以得到：

(3)

從而可以求解出未知量：

(4)

由此可見，小車與跟隨目標(biāo)的距離的計(jì)算僅與d1、d2和dS有關(guān)，與小車在當(dāng)前距離下的具體朝向無(wú)關(guān)。

2.4 水平距離的計(jì)算

為了不影響跟隨目標(biāo)的正常移動(dòng)，當(dāng)跟隨設(shè)備距離跟隨目標(biāo)一定距離時(shí)，跟隨設(shè)備暫停接近，從而實(shí)現(xiàn)與目標(biāo)保持距離的要求。這需要計(jì)算標(biāo)簽與跟隨小車的水平距離。

跟隨目標(biāo)與跟隨設(shè)備的距離如圖4所示。設(shè)定小車左側(cè)基站為A，右側(cè)基站為B，基站A、B連線的中點(diǎn)為O，標(biāo)簽距離地面高度為DE=h。已知兩個(gè)從模塊至主模塊的實(shí)時(shí)距離AE=d1，BE=d2,兩基站至中點(diǎn)的距離AO=BO=dS。OE即式(4)所求x。

圖4 基站與標(biāo)簽空間位置示意圖

DE長(zhǎng)度為標(biāo)簽距地高度，需要從外界獲得，因?yàn)椴煌母叨葧?huì)影響跟隨設(shè)備與跟隨目標(biāo)水平距離OD的計(jì)算。

跟隨設(shè)備距離目標(biāo)的水平距離即為OD。易得：

x2=OD2+DE2

(5)

跟隨小車與目標(biāo)的水平停止距離可由外部輸入設(shè)定，設(shè)該距離為k，即OD的理論最小值為k。小車實(shí)際運(yùn)行時(shí)，只要在OD≤k的情況下，就會(huì)暫停進(jìn)一步靠近，從而實(shí)現(xiàn)跟隨小車實(shí)時(shí)與目標(biāo)保持一定距離的要求。

3 跟隨實(shí)現(xiàn)的誤差討論

3.1 理想狀態(tài)的運(yùn)動(dòng)

在理想化的情況下，假設(shè)小車當(dāng)前方向偏移其理想正對(duì)目標(biāo)方向至多π/2，小車可以通過(guò)自身的左右旋轉(zhuǎn)，以達(dá)到兩基站測(cè)距值d1=d2，即達(dá)到兩個(gè)基站與標(biāo)簽距離相等的方向。此時(shí)跟隨小車正對(duì)目標(biāo)，直行即可。若d1d2時(shí)向右旋轉(zhuǎn)，直至d1=d2。

3.2 實(shí)際狀況的推導(dǎo)

在現(xiàn)實(shí)情況下，d1=d2的精準(zhǔn)情況幾乎難以穩(wěn)定保持，因?yàn)樾≤囍毙袝r(shí)不能保證其沒有任何角度的偏差，UWB在定位時(shí)也存在一定的誤差[10]。因此，可以設(shè)定誤差容忍區(qū)間，跟隨小車的運(yùn)動(dòng)偏移情況只要在誤差容忍區(qū)間內(nèi)，就選擇前進(jìn)，如果超出，則在運(yùn)動(dòng)中糾偏，從而實(shí)現(xiàn)小車在運(yùn)動(dòng)過(guò)程中不斷調(diào)整方向的需求。

如圖4所示，令?yuàn)A角α為小車?yán)硐胝龑?duì)目標(biāo)方向與小車當(dāng)前朝向所形成的夾角。為簡(jiǎn)化討論，設(shè)定α∈[0,π/2]，設(shè)D點(diǎn)的坐標(biāo)為(xD,yD,0)。

通過(guò)空間解析幾何分析，可以得到：

(6)

解得：

(7)

由式(7)可知，α的計(jì)算僅能確定小車當(dāng)前車頭方向相對(duì)于理想正對(duì)目標(biāo)方向的偏移角度，但不能指出其具體偏移方向，即無(wú)法確定是左偏還是右偏。因此，需要同時(shí)結(jié)合d1和d2的大小關(guān)系判斷小車具體的偏移方向。

在系統(tǒng)實(shí)際應(yīng)用中，可設(shè)定小車距跟隨目標(biāo)任意距離處的誤差容忍度數(shù)為γ。當(dāng)α<γ，即小車運(yùn)動(dòng)偏移情況在誤差容忍區(qū)間內(nèi)，則直行；當(dāng)α≥γ時(shí)，小車通過(guò)自身的偏移轉(zhuǎn)動(dòng)，調(diào)節(jié)車頭方向，使α重新回歸誤差容忍區(qū)間內(nèi)，實(shí)現(xiàn)運(yùn)動(dòng)中跟隨。

因此，小車的運(yùn)動(dòng)邏輯如下：

由式(5)得，OD≤k時(shí)，小車暫停進(jìn)一步靠近；當(dāng)OD>k時(shí)，小車運(yùn)動(dòng)情況如表1所示。

表1 雙基站定位跟隨系統(tǒng)根據(jù)測(cè)距情況的運(yùn)動(dòng)調(diào)整邏輯表

綜上所述，雙基站定位跟隨系統(tǒng)需要通過(guò)設(shè)定獲得跟隨設(shè)備與目標(biāo)的水平停止距離k，標(biāo)簽距地高度h，誤差容忍度數(shù)γ。在以上的條件下，跟隨小車均能找到目標(biāo)方向并進(jìn)行調(diào)整，實(shí)現(xiàn)自動(dòng)跟隨的效果。

4 系統(tǒng)的優(yōu)化方式

4.1 雙基站定位跟隨系統(tǒng)的缺陷與解決措施

3.2節(jié)的討論均是以假設(shè)小車的理想正對(duì)目標(biāo)方向與小車當(dāng)前朝向偏差?yuàn)A角至多π/2，即α∈[0,π/2]的情況為基礎(chǔ)的。由此擴(kuò)展到α∈[0,π]，在這種情況下，小車可能處于相對(duì)于跟隨目標(biāo)的任意一種朝向。

當(dāng)α∈(0,π/2)時(shí)，α的值即為式(7)所求解值。當(dāng)α∈(π/2,π)時(shí)，通過(guò)空間解析幾何分析：

(8)

在實(shí)際情況下，由于α∈[0,π]，在一組確定的d1、d2情況下，計(jì)算過(guò)程中會(huì)出現(xiàn)小車正對(duì)目標(biāo)和背對(duì)目標(biāo)都有相應(yīng)α解的問題，從而無(wú)法判斷小車是否正向跟隨目標(biāo)。

如圖5所示，取小車車頭前進(jìn)方向?yàn)閥軸正方向，根據(jù)d1、d2求得的偏移角度可以是標(biāo)簽在E點(diǎn)時(shí)，利用式(7)計(jì)算出的偏移角α，也可以是標(biāo)簽在E′點(diǎn)時(shí)，利用式(8)計(jì)算出的偏移角α′。因此，小車是正對(duì)還是背對(duì)跟隨目標(biāo)無(wú)法通過(guò)d1、d2、dS的關(guān)系確定，也無(wú)法通過(guò)三者的關(guān)系確定選擇使用式(7)還是式(8)的計(jì)算式來(lái)計(jì)算α。

圖5 偏移角度α的多種取值情況

為了解決這個(gè)問題，可以定義β為小車?yán)硐胝龑?duì)目標(biāo)方向與當(dāng)前朝向所在的直線所成的非鈍角，即β∈[0,π/2]。當(dāng)α∈[0,π/2]時(shí)，β=α；α∈(π/2,π]時(shí)，β=π-α。因此，

(9)

β的計(jì)算式在α∈[0,π/2]與α∈(π/2,π]之間是統(tǒng)一的，其不受小車是否正對(duì)跟隨目標(biāo)而造成的計(jì)算影響。當(dāng)獲得一組d1、d2時(shí)，可通過(guò)式(9)直接計(jì)算出β的取值。

當(dāng)d1>d2且β≥γ時(shí)，無(wú)論小車正對(duì)還是背對(duì)目標(biāo)，都需要向左偏轉(zhuǎn)；當(dāng)d1

當(dāng)β<γ時(shí)，如果仍參考表1的運(yùn)動(dòng)邏輯，那么跟隨小車無(wú)論車頭是正對(duì)還是背對(duì)跟隨目標(biāo)，均會(huì)執(zhí)行向前運(yùn)動(dòng)的操作，從而可能出現(xiàn)直行背離跟隨目標(biāo)的情況。而只要始終保持該角度范圍，這種背離會(huì)持續(xù)下去，形成“完美背離”現(xiàn)象。

對(duì)于這種情況，可以先讓小車直行運(yùn)動(dòng)，計(jì)算一段時(shí)間內(nèi)的x累計(jì)變化。如果該段時(shí)間內(nèi)x的值是持續(xù)變大的，則判斷為小車正在背離運(yùn)行目標(biāo)，因此需要調(diào)轉(zhuǎn)車頭；相反，如果x的值在直行過(guò)程中是逐漸變小的，則可以判斷小車正在接近目標(biāo)，方向是正確的。

修正后的運(yùn)動(dòng)邏輯如下：

當(dāng)x≤k時(shí)，小車暫停進(jìn)一步靠近；當(dāng)x>k時(shí)，小車運(yùn)動(dòng)情況如表2所示。

表2 修正后雙基站定位跟隨系統(tǒng)的運(yùn)動(dòng)調(diào)整邏輯表

4.2 三基站定位跟隨的優(yōu)化與實(shí)現(xiàn)

4.1節(jié)中的方法雖然可以解決問題，但是如果在計(jì)算累計(jì)變化的過(guò)程中，跟隨目標(biāo)做出的是變速幅度較大的移動(dòng)，跟隨小車就容易出現(xiàn)判斷錯(cuò)誤。同時(shí)，雙基站定位系統(tǒng)依賴于用戶自行輸入的標(biāo)簽高度。一方面，這需要保證系統(tǒng)的高度設(shè)置要跟隨標(biāo)簽的高度變化而進(jìn)行修改。另一方面，如果標(biāo)簽高度的輸入出現(xiàn)誤差，會(huì)影響跟隨小車到目標(biāo)的水平距離計(jì)算，導(dǎo)致系統(tǒng)跟隨效果變差。

為解決這些問題，可以引入第三個(gè)基站，構(gòu)建三基站定位系統(tǒng)。第三個(gè)基站安裝于小車的頭部，三個(gè)基站構(gòu)成一個(gè)等邊三角形。

如圖6所示建立坐標(biāo)系，A、B、C為三個(gè)基站，三角形ABC為等邊三角形。E為標(biāo)簽，標(biāo)簽E投影至xoy平面于D。AB=AC=BC=2dS,H是等邊三角形ABC的中心。O為AB中點(diǎn)。設(shè)定A基站、B基站、C基站與標(biāo)簽的距離分別為d1、d2、d3。設(shè)小車?yán)硐胝龑?duì)目標(biāo)方向與小車當(dāng)前朝向所形成的夾角，即圖中DH與y軸正方向所成角為α(α∈[0,π])。

圖6 三基站定位示意圖

令E點(diǎn)坐標(biāo)為(xE,yE,zE)，通過(guò)空間解析幾何求解，可以得到：

(10)

解得：

(11)

α=∠DHC=

(12)

由式(11)可以得到，DH的長(zhǎng)度與ED無(wú)關(guān)，即標(biāo)簽E的高度對(duì)DH的計(jì)算沒有影響，所以三基站定位跟隨系統(tǒng)不需要外部輸入標(biāo)簽高度。

之所以在雙基站定位跟隨系統(tǒng)中出現(xiàn)“完美背離”的情況，是因?yàn)橄到y(tǒng)僅通過(guò)d1和d2的關(guān)系，無(wú)法區(qū)分跟隨小車正對(duì)或背對(duì)目標(biāo)的狀態(tài)。而三基站系統(tǒng)中α∈[0,π]，只要獲得一組d1、d2、d3的值，就可以通過(guò)式(12)計(jì)算出α的值，從而明確地判斷小車相對(duì)目標(biāo)方向的具體偏移度數(shù)。

仍設(shè)定跟隨小車與目標(biāo)的水平停止距離為k；跟隨小車距標(biāo)簽任意距離處，誤差容忍度數(shù)為γ。三基站定位跟隨系統(tǒng)中小車運(yùn)動(dòng)邏輯如下：

由式(11)得，DH≤k時(shí)，小車暫停進(jìn)一步靠近；當(dāng)DH>k時(shí)，小車運(yùn)動(dòng)情況如表3所示。

表3 三基站定位跟隨系統(tǒng)根據(jù)測(cè)距情況的運(yùn)動(dòng)調(diào)整邏輯表

綜上所述，三基站定位跟隨系統(tǒng)只需設(shè)定跟隨小車與目標(biāo)的水平停止距離k，誤差容忍度數(shù)γ，即可實(shí)現(xiàn)小車的自動(dòng)跟隨。其不僅減少輸入?yún)?shù)的個(gè)數(shù)，同時(shí)解決雙基站定位系統(tǒng)中“完美背離”的情況，更易處理分析。

5 實(shí)驗(yàn)數(shù)據(jù)與分析

實(shí)驗(yàn)進(jìn)行于東西朝向4 m，南北朝向6 m的房間內(nèi)部。被跟隨者與小車各自額外攜帶一塊UWB定位標(biāo)簽，房間四個(gè)角落分別安裝一塊UWB定位基站。通過(guò)四個(gè)定位基站對(duì)兩個(gè)定位標(biāo)簽測(cè)距即可計(jì)算出被跟隨者與小車位于室內(nèi)的坐標(biāo)。

被跟隨者采用正常步速運(yùn)動(dòng)。設(shè)定雙基站定位跟隨系統(tǒng)最小保持距離k=0.4 m，標(biāo)簽距地高度h=1.5 m，最大誤差容忍度數(shù)γ=π/6。其運(yùn)動(dòng)路徑與雙基站定位跟隨小車運(yùn)動(dòng)路徑如圖7所示。

圖7 雙基站定位跟隨中被跟隨者與跟隨小車運(yùn)動(dòng)路徑

設(shè)定三基站定位跟隨系統(tǒng)最小保持距離k=0.4 m，最大誤差容忍度數(shù)γ=π/6，無(wú)須設(shè)定標(biāo)簽距地高度。被跟隨者與三基站定位跟隨小車的運(yùn)動(dòng)路徑如圖8所示。

圖8 三基站定位跟隨中被跟隨者與跟隨小車運(yùn)動(dòng)路徑

可見，無(wú)論是雙基站定位還是三基站定位，均可實(shí)現(xiàn)小車的室內(nèi)自動(dòng)跟隨，識(shí)別準(zhǔn)確，不易跟丟。三基站相較雙基站定位跟隨方法，在保證準(zhǔn)確跟隨的同時(shí)，減少參數(shù)的輸入，并避免“完美背離”的發(fā)生。

6 結(jié) 語(yǔ)

本文以UWB定位為基礎(chǔ)，提出可以實(shí)時(shí)路徑糾偏，實(shí)現(xiàn)智能跟隨的自動(dòng)跟隨設(shè)備。由跟隨過(guò)程中的誤差討論，提出并改進(jìn)雙基站跟隨小車的跟隨方式，并進(jìn)一步提出三基站定位跟隨的優(yōu)化方法。在實(shí)驗(yàn)過(guò)程中，跟隨小車已成功實(shí)現(xiàn)智能定位和跟隨。本系統(tǒng)具有識(shí)別準(zhǔn)確度高、跟隨中糾偏等多種優(yōu)勢(shì)，具有良好的應(yīng)用價(jià)值。