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

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

0 引言

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

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

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

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

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

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

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

為實(shí)現(xiàn)小跟搬達(dá)到精準(zhǔn)跟隨的目的，該方案采用了一個(gè)主控元件，四個(gè)超寬帶模塊，一個(gè)電機(jī)驅(qū)動(dòng)模塊，四個(gè)超聲波模塊和一個(gè)藍(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個(gè)看門狗定時(shí)器防止程序跑飛，單片機(jī)自動(dòng)復(fù)位。甚至可以用IIC通訊，而STC系列目前只能進(jìn)行串口通訊，其他通訊方式不能精準(zhǔn)穩(wěn)定進(jìn)行。

1.2.2 UWB超寬帶

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

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

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

1.2.4 超聲波模塊

小跟搬選用的超聲波模塊的型號(hào)為US-015，US-015可實(shí)現(xiàn)0.02m-4m的非接觸測(cè)距功能，探測(cè)精度為0.1cm+1%，分辨率高于1mm，可達(dá)0.5mm。與普通的超聲波模塊（HC-SR 04）相比，其具有高分辨率、重復(fù)測(cè)量一致性高、工作性能穩(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每秒以上的速率，并且功率消耗低。利用這個(gè)優(yōu)勢(shì)可將藍(lán)牙與MCU進(jìn)行串口通訊來(lái)實(shí)現(xiàn)標(biāo)簽，基站的時(shí)鐘同步。

2 智能跟隨

■2.1 跟隨原理

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

圖3 模塊示意圖

其基本的工作原理為：由小跟搬上固定位置的三個(gè)基站模塊（A1、A2、A3）發(fā)射信號(hào)，位于手環(huán)上的標(biāo)簽（B0）接收到此信號(hào)后返回信息，用算術(shù)平均濾波法濾除出現(xiàn)的噪聲和不可靠信號(hào)，并利用雙向飛行時(shí)間法計(jì)算得到標(biāo)簽與三個(gè)基站之間的幾何信息，完成距離數(shù)據(jù)的初步收集。可根據(jù)三個(gè)基站與標(biāo)簽B0的直線距離，利用勾股定理可得到手環(huán)和基站的水平高度差H（單位：m）。計(jì)算出水平差后，可根據(jù)主基站A1與手環(huán)的直線距離x1（單位：m）實(shí)現(xiàn)小跟搬自動(dòng)跟隨功能，而A2、A3輔助基站與手環(huán)的直線距離x2、x3（單位：m）則作為小跟搬的轉(zhuǎn)向判定，小跟搬主芯片通過(guò)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 測(cè)距算法

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

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

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

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

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

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

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

2.2.2定位算法

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

圖5 定位算法

公式(4)為已知小跟搬長(zhǎng)度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ì)算得出兩個(gè)輔助基站預(yù)估的基站與標(biāo)簽的水平高度差h2、h3（單位：m）；

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

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

2.2.3 角算法

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

圖6 定位的俯視圖

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

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

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

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

第一個(gè)實(shí)驗(yàn)測(cè)試是在水平地面上進(jìn)行，通過(guò)將預(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ù)算出平均值后取絕對(duì)值，得到每種條件下的誤差數(shù)據(jù)，繪制出如圖7所示的水平高度測(cè)量誤差趨勢(shì)圖。

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

圖7 水平高度測(cè)量誤差趨勢(shì)圖

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

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

圖8 偏移角度測(cè)量誤差趨勢(shì)圖

■3.2 誤差分析

本次實(shí)驗(yàn)可以看出在平面上，計(jì)算基站與標(biāo)簽之間的水平高度差的誤差量會(huì)隨著預(yù)設(shè)的跟隨距離的增加而增加，其實(shí)際水平高度差對(duì)于誤差量的影響較小，但在可以接受的范圍內(nèi)，并且對(duì)跟隨的影響較小。而在同一水平面上的計(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)勢(shì)，同時(shí)運(yùn)用了雙向飛行時(shí)間算法（TW-TOF）大大縮小了由模塊間的高度差所帶來(lái)的誤差，實(shí)現(xiàn)了更精準(zhǔn)的跟隨，此外超聲波避障系統(tǒng)使小跟搬在復(fù)雜的地形情況能夠自動(dòng)避障達(dá)到了跟隨的精度要求。通過(guò)試驗(yàn)機(jī)檢驗(yàn)可知我們的技術(shù)方案是可行的，但對(duì)于該方案中的信號(hào)降噪技術(shù)仍需進(jìn)一步的優(yōu)化與改進(jìn)。