基于UWB技術的室內智能跟隨系統的實現與優化

劉關迪 趙 璐

(南京工業大學計算機科學與技術學院 江蘇 南京 211816)

0 引 言

隨著技術的發展和應用場景的擴展,可移動設備的智能跟隨需求越來越大。然而目前主流采用的紅外傳感器、超聲傳感器等定位方式存在著無法鎖定跟隨目標，易偏離目標失去定位的問題[1]。這是因為室內環境下障礙物較多，傳感器對目標的測距易被障礙物阻擋，導致測量錯誤；而GPS信號功率低、接收要求高，室內定位時建筑物的阻擋又減弱了信號，導致GPS的定位精度低。因此，設計并實現一種可以精確定位目標、準確自動跟隨的系統很有必要。

本文使用UWB(Ultra Wideband)技術實現精確定位，以樹莓派小車為移動設備，設計并實現一個實時自動跟隨系統，并進一步對系統的可優化性進行討論。

1 UWB技術簡介

UWB是一種無載波通信技術，其脈沖寬度僅為納秒級或亞納秒級，擁有數據傳輸速率高、發射平均功率譜密度低等多種優點。其在定位方面有著兩方面優勢：

1)由于其窄脈沖特點，定位精度可達厘米級甚至更高。

2)UWB通信采用調時序列，時間分辨率高，具有抗多徑能力和一定的穿透能力，適用于高速、低速等不同移動環境[2-3]。

因此，UWB技術憑借其穿透性，可以很好地消除室內障礙物對定位的影響；其信息傳送能力良好，測距準確，精度高。該技術的使用解決了傳統室內定位方法遇到的困難。近年來，國內外逐漸將UWB技術應用于近距離精確室內定位，IEEE 802.15.4a標準采用UWB為定位首選技術[4-5]。

1.1 室內定位模塊

UWB用于室內定位時，需要配置定位標簽和定位基站兩種模塊。

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

(2)定位基站。定位基站是UWB定位的從模塊。基站可以通過飛行時間測距法等多種方法，確定標簽的位置，并將數據傳輸至網絡控制器及定位引擎軟件，定位精度達到厘米級。

1.2 測距原理

UWB測距使用飛行時間測距法(Time of Flight)。該技術需要在異步收發機(Transceiver)之間有準確的時間同步[6-7]，利用射頻信號在異步收發機間的飛行時間TTOF轉換為兩點之間的距離d：

d=c·TTOF

(1)

式中：c為射頻信號的傳播速度，可認為其等同于真空中的光速。

飛行時間測距法測距流程[8]如圖1所示，其中：TX表示發送；RX表示接收。設備A在TsA時刻發送請求脈沖信號，設備B在TrB時刻接收該信號，并于TsB時刻發射響應信號，設備A在TrA時刻接收該響應信號。設定設備A發送脈沖信號和接收到響應信號的時間間隔為Tround，設備B從接收到請求脈沖信號到發射響應信號的時間間隔為Treply，則:

圖1 TOF測距過程

(2)

2 系統設計與實現

如圖2所示，系統由兩部分組成：用于實現自動跟隨的樹莓派小車和跟隨目標定位模塊。跟隨目標攜帶一塊UWB主模塊作為標簽，小車配置兩塊UWB從模塊作為基站。通過實時測量兩個基站與標簽之間的距離，結合數據處理來判斷目標方向，實現跟隨。

圖2 系統硬件結構示意圖

2.1 UWB模塊選型

系統的UWB模塊選用SWM1000射頻模塊。SWM1000模塊是基于Decawave的DW1000收發器的集成電路，其集成了天線、電源管理和時鐘電源系統，可用于高精度室內測距與定位。

2.2 跟隨小車的結構設計

跟隨小車由樹莓派控制，小車的四個輪子各配有一個步進電機，樹莓派通過驅動不同電機實現小車的前進、后退和轉彎；通過控制PWM(Pulse Width Modulation)輸出值的變化實現小車速度的變化。

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

2.3 跟隨距離的確定

跟隨目標上的標簽與跟隨小車上的兩個基站綁定、配對后建立通信通道。單次測距由標簽主動發起，基站一直處于等待監聽狀態，并在收到標簽發來的測距開始消息后與標簽交互完成一次距離測量[9]。獲得與標簽距離后，基站將測量距離通過USB接口發送至樹莓派。

如圖3所示，設定兩個基站A、B各自與y軸的距離為常數ds。左側基站A距離目標標簽E長度為d1，右側基站B距離目標標簽E長度為d2，小車中心O距離目標標簽E的距離為x，設目標標簽E、小車中心O與x軸負方向構成夾角為θ。

圖3 跟隨小車定位示意圖

可以得到：

(3)

從而可以求解出未知量：

(4)

由此可見，小車與跟隨目標的距離的計算僅與d1、d2和dS有關，與小車在當前距離下的具體朝向無關。

2.4 水平距離的計算

為了不影響跟隨目標的正常移動，當跟隨設備距離跟隨目標一定距離時，跟隨設備暫停接近，從而實現與目標保持距離的要求。這需要計算標簽與跟隨小車的水平距離。

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

圖4 基站與標簽空間位置示意圖

DE長度為標簽距地高度，需要從外界獲得，因為不同的高度會影響跟隨設備與跟隨目標水平距離OD的計算。

跟隨設備距離目標的水平距離即為OD。易得：

x2=OD2+DE2

(5)

跟隨小車與目標的水平停止距離可由外部輸入設定，設該距離為k，即OD的理論最小值為k。小車實際運行時，只要在OD≤k的情況下，就會暫停進一步靠近，從而實現跟隨小車實時與目標保持一定距離的要求。

3 跟隨實現的誤差討論

3.1 理想狀態的運動

在理想化的情況下，假設小車當前方向偏移其理想正對目標方向至多π/2，小車可以通過自身的左右旋轉，以達到兩基站測距值d1=d2，即達到兩個基站與標簽距離相等的方向。此時跟隨小車正對目標，直行即可。若d1d2時向右旋轉，直至d1=d2。

3.2 實際狀況的推導

在現實情況下，d1=d2的精準情況幾乎難以穩定保持，因為小車直行時不能保證其沒有任何角度的偏差，UWB在定位時也存在一定的誤差[10]。因此，可以設定誤差容忍區間，跟隨小車的運動偏移情況只要在誤差容忍區間內，就選擇前進，如果超出，則在運動中糾偏，從而實現小車在運動過程中不斷調整方向的需求。

如圖4所示，令夾角α為小車理想正對目標方向與小車當前朝向所形成的夾角。為簡化討論，設定α∈[0,π/2]，設D點的坐標為(xD,yD,0)。

通過空間解析幾何分析，可以得到：

(6)

解得：

(7)

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

在系統實際應用中，可設定小車距跟隨目標任意距離處的誤差容忍度數為γ。當α<γ，即小車運動偏移情況在誤差容忍區間內，則直行；當α≥γ時，小車通過自身的偏移轉動，調節車頭方向，使α重新回歸誤差容忍區間內，實現運動中跟隨。

因此，小車的運動邏輯如下：

由式(5)得，OD≤k時，小車暫停進一步靠近；當OD>k時，小車運動情況如表1所示。

表1 雙基站定位跟隨系統根據測距情況的運動調整邏輯表

綜上所述，雙基站定位跟隨系統需要通過設定獲得跟隨設備與目標的水平停止距離k，標簽距地高度h，誤差容忍度數γ。在以上的條件下，跟隨小車均能找到目標方向并進行調整，實現自動跟隨的效果。

4 系統的優化方式

4.1 雙基站定位跟隨系統的缺陷與解決措施

3.2節的討論均是以假設小車的理想正對目標方向與小車當前朝向偏差夾角至多π/2，即α∈[0,π/2]的情況為基礎的。由此擴展到α∈[0,π]，在這種情況下，小車可能處于相對于跟隨目標的任意一種朝向。

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

(8)

在實際情況下，由于α∈[0,π]，在一組確定的d1、d2情況下，計算過程中會出現小車正對目標和背對目標都有相應α解的問題，從而無法判斷小車是否正向跟隨目標。

如圖5所示，取小車車頭前進方向為y軸正方向，根據d1、d2求得的偏移角度可以是標簽在E點時，利用式(7)計算出的偏移角α，也可以是標簽在E′點時，利用式(8)計算出的偏移角α′。因此，小車是正對還是背對跟隨目標無法通過d1、d2、dS的關系確定，也無法通過三者的關系確定選擇使用式(7)還是式(8)的計算式來計算α。

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

為了解決這個問題，可以定義β為小車理想正對目標方向與當前朝向所在的直線所成的非鈍角，即β∈[0,π/2]。當α∈[0,π/2]時，β=α；α∈(π/2,π]時，β=π-α。因此，

(9)

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

當d1>d2且β≥γ時，無論小車正對還是背對目標，都需要向左偏轉；當d1

當β<γ時，如果仍參考表1的運動邏輯，那么跟隨小車無論車頭是正對還是背對跟隨目標，均會執行向前運動的操作，從而可能出現直行背離跟隨目標的情況。而只要始終保持該角度范圍，這種背離會持續下去，形成“完美背離”現象。

對于這種情況，可以先讓小車直行運動，計算一段時間內的x累計變化。如果該段時間內x的值是持續變大的，則判斷為小車正在背離運行目標，因此需要調轉車頭；相反，如果x的值在直行過程中是逐漸變小的，則可以判斷小車正在接近目標，方向是正確的。

修正后的運動邏輯如下：

當x≤k時，小車暫停進一步靠近；當x>k時，小車運動情況如表2所示。

表2 修正后雙基站定位跟隨系統的運動調整邏輯表

4.2 三基站定位跟隨的優化與實現

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

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

如圖6所示建立坐標系，A、B、C為三個基站，三角形ABC為等邊三角形。E為標簽，標簽E投影至xoy平面于D。AB=AC=BC=2dS,H是等邊三角形ABC的中心。O為AB中點。設定A基站、B基站、C基站與標簽的距離分別為d1、d2、d3。設小車理想正對目標方向與小車當前朝向所形成的夾角，即圖中DH與y軸正方向所成角為α(α∈[0,π])。

圖6 三基站定位示意圖

令E點坐標為(xE,yE,zE)，通過空間解析幾何求解，可以得到：

(10)

解得：

(11)

α=∠DHC=

(12)

由式(11)可以得到，DH的長度與ED無關，即標簽E的高度對DH的計算沒有影響，所以三基站定位跟隨系統不需要外部輸入標簽高度。

之所以在雙基站定位跟隨系統中出現“完美背離”的情況，是因為系統僅通過d1和d2的關系，無法區分跟隨小車正對或背對目標的狀態。而三基站系統中α∈[0,π]，只要獲得一組d1、d2、d3的值，就可以通過式(12)計算出α的值，從而明確地判斷小車相對目標方向的具體偏移度數。

仍設定跟隨小車與目標的水平停止距離為k；跟隨小車距標簽任意距離處，誤差容忍度數為γ。三基站定位跟隨系統中小車運動邏輯如下：

由式(11)得，DH≤k時，小車暫停進一步靠近；當DH>k時，小車運動情況如表3所示。

表3 三基站定位跟隨系統根據測距情況的運動調整邏輯表

綜上所述，三基站定位跟隨系統只需設定跟隨小車與目標的水平停止距離k，誤差容忍度數γ，即可實現小車的自動跟隨。其不僅減少輸入參數的個數，同時解決雙基站定位系統中“完美背離”的情況，更易處理分析。

5 實驗數據與分析

實驗進行于東西朝向4 m，南北朝向6 m的房間內部。被跟隨者與小車各自額外攜帶一塊UWB定位標簽，房間四個角落分別安裝一塊UWB定位基站。通過四個定位基站對兩個定位標簽測距即可計算出被跟隨者與小車位于室內的坐標。

被跟隨者采用正常步速運動。設定雙基站定位跟隨系統最小保持距離k=0.4 m，標簽距地高度h=1.5 m，最大誤差容忍度數γ=π/6。其運動路徑與雙基站定位跟隨小車運動路徑如圖7所示。

圖7 雙基站定位跟隨中被跟隨者與跟隨小車運動路徑

設定三基站定位跟隨系統最小保持距離k=0.4 m，最大誤差容忍度數γ=π/6，無須設定標簽距地高度。被跟隨者與三基站定位跟隨小車的運動路徑如圖8所示。

圖8 三基站定位跟隨中被跟隨者與跟隨小車運動路徑

可見，無論是雙基站定位還是三基站定位，均可實現小車的室內自動跟隨，識別準確，不易跟丟。三基站相較雙基站定位跟隨方法，在保證準確跟隨的同時，減少參數的輸入，并避免“完美背離”的發生。

6 結 語

本文以UWB定位為基礎，提出可以實時路徑糾偏，實現智能跟隨的自動跟隨設備。由跟隨過程中的誤差討論，提出并改進雙基站跟隨小車的跟隨方式，并進一步提出三基站定位跟隨的優化方法。在實驗過程中，跟隨小車已成功實現智能定位和跟隨。本系統具有識別準確度高、跟隨中糾偏等多種優勢，具有良好的應用價值。