基于UWB的自主跟隨機(jī)器人系統(tǒng)設(shè)計

楊再標(biāo)，易先軍，彭 萌，耿翰夫，付 龍

（武漢工程大學(xué) 電氣信息學(xué)院，武漢 430205）

隨著人工智能技術(shù)和網(wǎng)絡(luò)化通信技術(shù)的發(fā)展，人們開始解放雙手，更加注重設(shè)備的智能化和信息化，自主跟隨機(jī)器人也因此受到了社會各界的廣泛關(guān)注。 目前，通過攝像頭圖像識別的方法運(yùn)用在跟隨目標(biāo)對象移動范圍較大的場合時，很容易丟失跟隨目標(biāo)對象，特別是在多人的場合中，很難實現(xiàn)單一目標(biāo)的跟蹤，從而導(dǎo)致跟隨失敗。

UWB 技術(shù)是近年來興起的一種高速傳輸數(shù)據(jù)的無線通信技術(shù)，具有傳輸數(shù)據(jù)效率高的優(yōu)點(diǎn)，可高達(dá)1000 Mb/s 以上。 UWB 具有抗干擾性強(qiáng)、傳輸效率高、能耗小等優(yōu)勢。 在邊長檢測中，能得到較為精確的結(jié)果，很好地運(yùn)用于跟隨機(jī)器人中[1-2]。 本文基于2011年提出的IEEE802.15.4-2011 標(biāo)準(zhǔn)中的IR-UWB 技術(shù)開發(fā)高精度射頻測距系統(tǒng)， 采用Decawave 公司的DWM1000 芯片設(shè)計射頻測距系統(tǒng)通信節(jié)點(diǎn)，應(yīng)用SDS-TWR 算法實現(xiàn)測距，使自主跟隨機(jī)器人系統(tǒng)可以更加安全有效地跟隨目標(biāo)對象[3]。本文旨在研究并設(shè)計一種基于UWB 的自主跟隨機(jī)器人系統(tǒng)，在利用角度大小跟隨中，通過減少跟隨邊長的誤差，進(jìn)而減小跟隨角度誤差，以提高自主跟隨機(jī)器人的跟隨精度。

1 跟隨機(jī)器人原理

1.1 跟隨邊長檢測原理

自主跟隨機(jī)器人系統(tǒng)的跟隨邊長檢測算法采用雙邊雙向測距（SDS-TWR）[4-7]。 具體以基站0 和標(biāo)簽之間的通信為例。 基站0 向標(biāo)簽發(fā)送Poll 數(shù)據(jù)包，同時記錄下發(fā)送Poll 數(shù)據(jù)包的時刻T1，并在一段時間后打開基站0 的串口RX 通道； 標(biāo)簽提前開啟標(biāo)簽的串口RX 通道， 收到Poll 數(shù)據(jù)包后并記錄下接收到Poll 數(shù)據(jù)包的時刻T2；標(biāo)簽等待先前設(shè)定好的延時處理時間Treply1后， 通過已經(jīng)開啟的串口TX 通道向基站0 發(fā)送Response 數(shù)據(jù)包， 同時記錄下發(fā)送Response 數(shù)據(jù)包的時刻T3，并在發(fā)送完后打開設(shè)備B 的串口RX 通道； 基站0 接收到Response數(shù)據(jù)包后并記錄下接收到Response 數(shù)據(jù)包的時刻T4； 基站0 等待先前設(shè)定好的延遲處理時間Treply2后，通過已經(jīng)開啟的串口TX 通道向標(biāo)簽發(fā)送Final數(shù)據(jù)包，同時記錄下發(fā)送Final 數(shù)據(jù)包的時刻T5；標(biāo)簽接收到Final 數(shù)據(jù)包后并記錄下接收到Final 數(shù)據(jù)包的時刻T6。 測距過程如圖1 所示。

圖1 SDS-TWR 算法示意圖Fig.1 Schematic diagram of SDS-TWR algorithm

則電磁波飛行時間為

式中：Tround1=T4-T1；Tround2=T6-T3；Treply1=T3-T2；Treply2=T5-T4。

通過式（1）求得電磁波飛行時間Tprop，再乘以光速，可得到基站0 到標(biāo)簽之間的距離。

1.2 跟隨機(jī)器人跟隨原理

首先將基站0 和基站1 分別固定于機(jī)器人同一水平線上，并且基站0 和基站1 相距30 cm，人手持標(biāo)簽于兩個基站的前方，具體如圖2 所示。 基站0、基站1 和標(biāo)簽三者構(gòu)成一個三角形。

圖2 跟隨原理Fig.2 Following principle

設(shè)三角形三邊邊長分別為a、b、c，b 邊所對應(yīng)的角為θ。 a=30 cm，由SDS-TWR 算法可測得b、c 的距離。 于是，由余弦定理有：

利用反三角函數(shù)可求得θ 的值。 計算得到的θ角度與控制器設(shè)定的角度相比較，進(jìn)而控制機(jī)器人的運(yùn)動。

2 跟隨機(jī)器人系統(tǒng)總體設(shè)計

2.1 跟隨機(jī)器人系統(tǒng)總體結(jié)構(gòu)

跟隨機(jī)器人系統(tǒng)總體結(jié)構(gòu)主要由控制器（STM32F103）、聲光報警器、直流減速電機(jī)驅(qū)動器、直流減速電機(jī)、 光電編碼器、12 V 鋰電池、OLED 顯示屏、基站（基站0、基站1）、標(biāo)簽以及紅外線超聲波傳感器共10 個模塊電路組成。 跟隨機(jī)器人系統(tǒng)總體結(jié)構(gòu)如圖3 所示。 跟隨機(jī)器人的跟隨目標(biāo)對象為具有接收信號和發(fā)送信號功能的標(biāo)簽。 當(dāng)人手持標(biāo)簽運(yùn)行時，跟隨機(jī)器人就會跟隨標(biāo)簽運(yùn)動。

2.2 跟隨機(jī)器人硬件設(shè)計

圖3 跟隨機(jī)器人系統(tǒng)總體結(jié)構(gòu)Fig.3 Overall structure of the follow robot system

主控制器選用STM32F103ZET6 芯片作為最小系統(tǒng)，主控制器主要是控制跟隨機(jī)器人的運(yùn)動和從控制器進(jìn)行信息交互。12 V 鋰電池作為整個跟隨機(jī)器人的供電系統(tǒng)，提供了系統(tǒng)的動力來源。OLED 顯示屏模塊將電源電量和跟隨機(jī)器人的運(yùn)動狀態(tài)呈現(xiàn)在屏幕上，當(dāng)電源電量低于5 V 時，電量字體變?yōu)榧t色并且不停閃爍， 以提示用戶及時給電源充電。聲光報警器模塊采用蜂鳴器和LED 燈制作而成，當(dāng)電源電量不足或者跟隨機(jī)器人系統(tǒng)出現(xiàn)故障時，蜂鳴器將發(fā)出警報聲音，同時LED 燈不停閃爍，警告用戶電源電量不足或需要重啟系統(tǒng)。 直流減速電機(jī)驅(qū)動器、直流減速電機(jī)和光電編碼器三者構(gòu)成一個閉環(huán)回路， 通過增量PID 算法對電機(jī)進(jìn)行速度控制。 紅外線傳感器和超聲波傳感器兩者對外部環(huán)境信息進(jìn)行采集，使跟隨機(jī)器人能夠檢測并避開障礙物。

標(biāo)簽、基站0 和基站1 的硬件結(jié)構(gòu)相同，都由控制器（STM32F103）、DWM1000 模塊、OLED 顯示屏和5 V 電源四部分組成。 DWM1000 模塊是DecaWave公司生產(chǎn)的低功耗芯片， 該模塊傳輸數(shù)據(jù)效率高，可高達(dá)6.8 Mb/s，可以實現(xiàn)雙邊雙向測距，在跟隨機(jī)器人中有很好的運(yùn)用。

3 跟隨機(jī)器人軟件程序

跟隨機(jī)器人軟件設(shè)計包括運(yùn)動控制程序和基站跟隨邊長檢測程序。

3.1 跟隨機(jī)器人運(yùn)動控制

跟隨機(jī)器人運(yùn)動程序流程如圖4 所示。 跟隨機(jī)器人供電后， 首先進(jìn)行的是全部硬件模塊的初始化，包括通信串口初始化、定時器初始化以及OELD顯示屏的初始化等。 接著進(jìn)入判斷程序，當(dāng)接收到融合信息“R”時，機(jī)器人右轉(zhuǎn)；當(dāng)接收到融合信息“L”時，機(jī)器人左轉(zhuǎn)；當(dāng)機(jī)器人接收到融合信息“F”時，機(jī)器人前進(jìn)；當(dāng)機(jī)器人接收到融合信息“S”時，機(jī)器人停止前進(jìn)。

圖4 跟隨機(jī)器人運(yùn)動控制流程Fig.4 Motion control flow chart of the follow robot

3.2 基站跟隨邊長檢測程序

基站跟隨邊長檢測程序流程如圖5 所示。 基站開機(jī)初始化后，發(fā)送Poll 數(shù)據(jù)包給標(biāo)簽，標(biāo)簽有響應(yīng)后，進(jìn)行跟隨邊長檢測，執(zhí)行雙邊雙向檢測步驟。當(dāng)基站檢測完成并沒有響應(yīng)時， 基站進(jìn)入休眠狀態(tài)，當(dāng)收到指令時再次進(jìn)行檢測跟隨長度。

圖5 基站跟隨邊長檢測程序流程Fig.5 Base station following side length detection procedure flow chart

4 系統(tǒng)功能測試

4.1 跟隨邊長檢測

為了測試本設(shè)計中跟隨機(jī)器人的測距精度，設(shè)計了如下實驗，實驗俯視示意圖如圖6 所示。 將基站0 和基站1 固定于一水平、相距30 cm 的直線上，并且基站0 用USB 轉(zhuǎn)串口線與電腦相連；標(biāo)簽置于基站0 和基站1 的平行且相距100 cm 的直線上，基站1、基站0 和標(biāo)簽形成一個角度，通過改變該角度的大小來獲得標(biāo)簽的測試點(diǎn)，在測試點(diǎn)上測取基站0 到標(biāo)簽的距離信息并將距離信息通過串口打印在電腦串口助手中。 本次實驗測量的角度分別取了30°、45°、60°、90°、120°、135°、150°共7 個角度，所對應(yīng)的的測試點(diǎn)分別為點(diǎn)A、B、C、D、E、F、G。 在測試點(diǎn)檢測的邊長距離結(jié)果如表1 所示。 實驗數(shù)據(jù)表明，邊長檢測誤差在±10 cm 之內(nèi)，進(jìn)而減少了跟隨角度的誤差，提高了跟隨精度。

圖6 實驗設(shè)計Fig.6 Experimental design

表1 邊長檢測結(jié)果Tab.1 Side length test result

4.2 跟隨測試

將跟隨機(jī)器人硬件平臺供電后，將標(biāo)簽0 通過USB 轉(zhuǎn)串口連接線連接到電腦上，通過電腦串口調(diào)試助手和跟隨機(jī)器人的實際運(yùn)動來檢測標(biāo)簽與基站0、 基站1 的角度和距離的融合信息是否已經(jīng)傳輸?shù)礁S機(jī)器人的主控制器上。 串口打印信息和跟隨機(jī)器人運(yùn)動信狀態(tài)息如表2 所示。

表2 機(jī)器人跟隨狀態(tài)Tab.2 Robot following state

5 結(jié)語

上述所設(shè)計的硬件電路和相應(yīng)的軟件控制流程，實現(xiàn)了機(jī)器人的自主跟隨。 在測取實驗數(shù)據(jù)中，驗證了基于UWB 的自主跟隨機(jī)器人系統(tǒng)能夠?qū)崿F(xiàn)在人員移動密集的環(huán)境下對目標(biāo)對象實時、準(zhǔn)確的跟隨。 通過減少跟隨邊長誤差，進(jìn)而減小跟隨角度誤差，以提高自主跟隨機(jī)器人的跟隨精度，完成跟隨目標(biāo)對象的任務(wù)。 但在邊長檢測精度上和跟隨算法上還不夠精確，還可以進(jìn)一步提高。