基于UWB雙向飛行算法完成精準(zhǔn)跟隨實(shí)驗(yàn)的研究

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

（1.珠海科技學(xué)院，廣東珠海，519041；2.珠海城市職業(yè)技術(shù)學(xué)院，廣東珠海，519090）

0 引言

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

1 總體設(shè)計(jì)

■1.1 系統(tǒng)流程設(shè)計(jì)

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

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

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

■1.2 硬件結(jié)構(gòu)

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

圖2 硬件總框圖

1.2.1 主控元件

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

1.2.2 UWB超寬帶

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

1.2.3 電機(jī)驅(qū)動模塊

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

1.2.4 超聲波模塊

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

1.2.5 藍(lán)牙模塊

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

2 智能跟隨

■2.1 跟隨原理

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

圖3 模塊示意圖

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

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

■2.2 UWB定位算法

2.2.1 測距算法

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

飛行時間測距法：主要利用信號在兩個異步收發(fā)機(jī)（Transceiver）之間飛行時間來測量節(jié)點(diǎn)間的距離。因?yàn)樵谝暰嘁暰€環(huán)境下，基于TOF測距方法是隨距離呈線性關(guān)系。

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

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

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

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

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

2.2.2定位算法

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

圖5 定位算法

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

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

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

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

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

2.2.3 角算法

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

圖6 定位的俯視圖

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

3 實(shí)驗(yàn)測試結(jié)果

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

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

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

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

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

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

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

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

■3.2 誤差分析

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

4 總結(jié)

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