基于UWB雙向飛行算法完成精準跟隨實驗的研究

劉卓婭，周俊華，胡樹杰，林凡舒，張馳，董映萍，龍永運

（1.珠?？萍紝W院，廣東珠海，519041；2.珠海城市職業(yè)技術學院，廣東珠海，519090）

0 引言

隨著智能時代的發(fā)展，各種智能機器人產(chǎn)品亦蜂擁而出，諸如自動跟隨行李箱、智能跟隨陪護機器人、智能跟隨快遞車、智能跟隨搬運機器人等，它們均體現(xiàn)出了智能跟隨技術在機器人領域中有著廣泛的應用。智能跟隨搬運機器人能有效解決人工勞動力，為人們的日常生活提供便利。本文研究設計了基于UWB技術的智能跟隨系統(tǒng)。該系統(tǒng)采用基于UWB的雙向飛行算法，能有效提高智能跟隨精準度，同時考慮到現(xiàn)實應用場景中，跟隨機器人執(zhí)行定位跟隨功能時會遇到各種障礙物，因此本系統(tǒng)設有基于超聲波傳感器的避障功能模塊。

1 總體設計

■1.1 系統(tǒng)流程設計

本系統(tǒng)總體分為兩部分，第一部分為是智能手環(huán)，第二部分是小跟搬主體。其中智能手環(huán)的主要功能為接收基站發(fā)送的信號并返回信號給基站，而小跟搬的主要功能分為兩個，一個是向智能手環(huán)發(fā)收信號并進行計算處理，實現(xiàn)跟隨的功能，另一個是避障功能，當小跟搬在跟隨的過程中判斷到出現(xiàn)障礙物的情況，停止跟隨進行避障操作，當避障完成后重新定位繼續(xù)跟隨。

系統(tǒng)總體流程如圖1所示，當系統(tǒng)初始化后，智能手環(huán)通過藍牙模塊與小跟搬進行同步后，首先小跟搬的基站發(fā)出信號給手環(huán)上的標簽，通過預設跟隨距離計算出跟隨手環(huán)標簽預基站之間的水平差，然后進行跟隨模式。當小跟搬上的超聲波模塊檢測到障礙物后，斷開跟隨模式進入避障模式，成功避障后，計算與標簽的偏移角后進行修正，之后繼續(xù)進入跟隨模式。

圖1 系統(tǒng)流程圖

■1.2 硬件結構

為實現(xiàn)小跟搬達到精準跟隨的目的，該方案采用了一個主控元件，四個超寬帶模塊，一個電機驅動模塊，四個超聲波模塊和一個藍牙模塊。硬件總框圖設計如圖2所示。

圖2 硬件總框圖

1.2.1 主控元件

該系統(tǒng)采用的MCU單片機為STM32F103ZET6。相較于STC單片機有著更高的實用性，STM32具有最高72MHz工作頻率，串行單線調試（SWD)和JTAG接口可用于仿真，可以進行復雜的計算和精準的控制，2個看門狗定時器防止程序跑飛，單片機自動復位。甚至可以用IIC通訊，而STC系列目前只能進行串口通訊，其他通訊方式不能精準穩(wěn)定進行。

1.2.2 UWB超寬帶

小跟搬采用的UWB超聲波模塊的主控芯片的型號為STM32F105，內置電路中包含DW1000、電源、撥碼開關等模塊。其中，DW1000是UWB的一個信號收發(fā)器。該模塊具有許多優(yōu)點，如穿透力強、安全性高、系統(tǒng)復雜度低、定位精度高等。此外，UWB通信模塊通過撥碼開關的變化可以同時作為基站和標簽。

1.2.3 電機驅動模塊

小跟搬選用的電機驅動模塊的型號為L298N，是專用的驅動集成電路。由于L298N可通過主控芯片的I/O輸入對電機的控制電平進行直接設定，因此能實現(xiàn)電機的正反轉的變化。程序化控制簡便、硬件操作易上手、穩(wěn)定性優(yōu)良，滿足直流電機的大電流驅動條件，符合小跟搬運動的條件要求。

1.2.4 超聲波模塊

小跟搬選用的超聲波模塊的型號為US-015，US-015可實現(xiàn)0.02m-4m的非接觸測距功能，探測精度為0.1cm+1%，分辨率高于1mm，可達0.5mm。與普通的超聲波模塊（HC-SR 04）相比，其具有高分辨率、重復測量一致性高、工作性能穩(wěn)定等特點。

1.2.5 藍牙模塊

系統(tǒng)應用的藍牙模塊為jdy-30是依據(jù)藍牙3.0協(xié)議標準，工作頻段為2.4GHz范圍的片上系統(tǒng)，主要適用于藍牙高速數(shù)據(jù)傳輸。數(shù)據(jù)傳輸比BLE藍牙快、可達到8K每秒以上的速率，并且功率消耗低。利用這個優(yōu)勢可將藍牙與MCU進行串口通訊來實現(xiàn)標簽，基站的時鐘同步。

2 智能跟隨

■2.1 跟隨原理

本次研究設計的小跟搬自動跟隨系統(tǒng)是通過UWB定位技術來實現(xiàn)的。其共使用到4個UWB單片機模塊，小跟搬上裝有3個UWB基站（A1、A2、A3），外加一個標簽（B0）裝備在手環(huán)中，模塊示意圖如圖3所示。

圖3 模塊示意圖

其基本的工作原理為：由小跟搬上固定位置的三個基站模塊（A1、A2、A3）發(fā)射信號，位于手環(huán)上的標簽（B0）接收到此信號后返回信息，用算術平均濾波法濾除出現(xiàn)的噪聲和不可靠信號，并利用雙向飛行時間法計算得到標簽與三個基站之間的幾何信息，完成距離數(shù)據(jù)的初步收集。可根據(jù)三個基站與標簽B0的直線距離，利用勾股定理可得到手環(huán)和基站的水平高度差H（單位：m）。計算出水平差后，可根據(jù)主基站A1與手環(huán)的直線距離x1（單位：m）實現(xiàn)小跟搬自動跟隨功能，而A2、A3輔助基站與手環(huán)的直線距離x2、x3（單位：m）則作為小跟搬的轉向判定，小跟搬主芯片通過PID算法進行計算，調節(jié)控制電機，最終實現(xiàn)小跟搬的跟隨和控制轉向功能。跟隨系統(tǒng)軟件設計流程圖如圖4所示。

圖4 跟隨系統(tǒng)軟件設計流程圖

■2.2 UWB定位算法

2.2.1 測距算法

UWB技術的測距原理大致有兩種，一種是飛行時間測距法，另一種是雙向飛行時間法。

飛行時間測距法：主要利用信號在兩個異步收發(fā)機（Transceiver）之間飛行時間來測量節(jié)點間的距離。因為在視距視線環(huán)境下，基于TOF測距方法是隨距離呈線性關系。

雙向飛行時間法：設備A首先向設備B發(fā)出一個數(shù)據(jù)包，并記錄下發(fā)包時刻Ta1，設備B收到數(shù)據(jù)包后，記下收包時刻Tb1。之后設備B等待Treply時刻，在Tb2（Tb2=Tb1+Treply）時刻，向設備A發(fā)送一個數(shù)據(jù)包，設備A收到數(shù)據(jù)包后記下時刻值Ta2。

飛行時間測距法雖然計算簡單，但是測距精度有所欠缺，并且設備約束比較嚴格，需要設備之間時鐘必須同步，所以采取雙向飛行時間法。

首先將固定在小跟搬上的三個基站通過有線連接將時鐘同步，在手環(huán)上的標簽B0利用藍牙模塊實現(xiàn)時鐘同步，減小計算式的誤差值。

當小跟搬開機，并與智能手環(huán)匹配后，首先將各基站與標簽時鐘同步，之后A1、A2、A3基站在Ta1時刻發(fā)送信號，標簽BO分別在Tt1、Tt2、Tt3時刻接收到，然后等待Treply（單位：s）時間后，標簽T0再分別發(fā)送信號給基站，收到的時間為Tb1、Tb2、Tb3。由此，根據(jù)公式（1）（2）（3）就可分別計算出各個基站與標簽之間的直線距離S1、S2、S3（單位：m）。

（C 為光速，C約為3×108m/s）

2.2.2定位算法

當小跟搬成功將基站與標簽之間的直線距離測算出來后，首先進行身高估算階段。小跟搬通過PID算法調節(jié)小跟搬方向，使得A2、A3兩個輔助基站與手環(huán)標簽的直線距離相等，如圖5定位算法所示。距離調節(jié)完畢后，因為已知小跟搬長度為I（單位：m），寬度為W（單位：m），預設的跟隨距離為L（單位：m），根據(jù)勾股定理可分別計算出各基站與標簽的水平高度差h（單位：m），最后通過算數(shù)平均法計算得到小跟搬與手環(huán)的水平高度差H（單位：m）。

圖5 定位算法

公式(4)為已知小跟搬長度I（單位：m）和預設跟隨距離L（單位：m），根據(jù)主基站A1與手環(huán)標簽的直線距離S1（單位：m），計算得出A1基站預估的基站與標簽的水平高度差h1（單位：m）；

公式(5)為已知小跟搬寬度W（單位：m）和預設跟隨距離L（單位：m），可算出輔助基站A2、A3與手環(huán)標簽垂直投射的陰影距離X（單位：m）；

公式(6)(7)為已知基站A2、A3與手環(huán)標簽垂直投射的陰影距離X（單位：m），根據(jù)基站與手環(huán)標簽的直線距離S2、S3（單位：m），計算得出兩個輔助基站預估的基站與標簽的水平高度差h2、h3（單位：m）；

公式(8)為將三個基站預估的水平高度差取其平均值，得到最終結果H（單位：m）。

已知水平高度差后，通過讀取主基站A1與手環(huán)標簽的直線距離來實現(xiàn)跟隨功能，兩個輔助基站的主要作用為計算標簽與小跟搬的偏移角度，實現(xiàn)轉向。

2.2.3 角算法

當從避障模式切換回跟隨模式后，小跟搬與標簽會出現(xiàn)較大的角度偏移，如果讀取出標簽與小跟搬的偏移角度，就能很快重新控制電機使小跟搬回歸到跟隨模式的路徑中。如圖6所示為定位的俯視圖，A為手環(huán)標簽，BC分別是兩個輔助基站的位置，AD為目前小跟搬與手環(huán)投影的直線距離，E為兩個輔助基站的中點。

圖6 定位的俯視圖

因為已經(jīng)算出了基站與標簽的水平高度差H，根據(jù)勾股定理可算出AB、AC的長度，BC為小跟搬的寬度，根據(jù)公式(9)(10)海倫公式就可以算出ABC的面積S（單位：m2），根據(jù)公式(11)則可以算出AD的距離，最后就可以計算出BD的距離，BD再與BE作差可得ED的距離，以此類推可推導出AE的距離，利用反三角公式(12)可得出∠EAD的角度。通過得到夾角∠EAD的大小即可得到標簽與小跟搬的偏移角度，從而控制小跟搬按規(guī)定進行偏轉運動。

3 實驗測試結果

■3.1 數(shù)據(jù)測試

為了檢測基站與標簽之間的水平高度差和偏移角度的精度，設計了兩個實驗來對精度進行檢驗測試，然后分析實驗中所得實際數(shù)據(jù)與真實測量數(shù)據(jù)出現(xiàn)的誤差。

第一個實驗測試是在水平地面上進行，通過將預設跟隨距離分為0.5m、1m、1.5m，實際水平高度差分為0.5m、1m、1.5m后，在每種條件下進行五次實驗，計算出的標簽與基站之間的水平高度差，并記錄下與實際水平高度差作差得到的實驗誤差量，得到表1。然后將在每種條件下五次實驗的數(shù)據(jù)算出平均值后取絕對值，得到每種條件下的誤差數(shù)據(jù)，繪制出如圖7所示的水平高度測量誤差趨勢圖。

表1 不同條件下的水平高度差測量誤差量

圖7 水平高度測量誤差趨勢圖

第二個實驗測試是將手環(huán)標簽與基站放置在同一水平面上進行，通過將手環(huán)標簽與兩個輔助基站的垂直距離分為0.5m、1.0m、1.5m，實際偏移角度分為30°、45°、60°后，在每種條件下進行五次實驗，計算出手環(huán)標簽對于輔助基站的偏移角度，并記錄下與實際偏移角度作差得到的實驗誤差量，得到表2。然后將在每種條件下五次實驗的數(shù)據(jù)算出平均值后取絕對值，得到每種條件下的誤差數(shù)據(jù)，繪制出如圖8所示的偏移角度測量誤差趨勢圖。

表2 不同條件下的偏移角度測量誤差量

圖8 偏移角度測量誤差趨勢圖

■3.2 誤差分析

本次實驗可以看出在平面上，計算基站與標簽之間的水平高度差的誤差量會隨著預設的跟隨距離的增加而增加，其實際水平高度差對于誤差量的影響較小，但在可以接受的范圍內，并且對跟隨的影響較小。而在同一水平面上的計算偏移角度在±6°以內，在可接受范圍以內，并搭配上PID算法進行調試，誤差即可得到有效控制。

4 總結

本文在研究智能跟隨搬運機器人技術的背景下，所設計出的小跟搬采用了近年熱門的超寬帶（UWB）定位技術，以此具備了抗干擾能力強，穿透力強等優(yōu)勢，同時運用了雙向飛行時間算法（TW-TOF）大大縮小了由模塊間的高度差所帶來的誤差，實現(xiàn)了更精準的跟隨，此外超聲波避障系統(tǒng)使小跟搬在復雜的地形情況能夠自動避障達到了跟隨的精度要求。通過試驗機檢驗可知我們的技術方案是可行的，但對于該方案中的信號降噪技術仍需進一步的優(yōu)化與改進。