趙劍川，陳榕福，蘭小海，何 威，張 宇，吳志威

(廣東機電職業(yè)技術(shù)學院電子與通信學院,廣東 廣州 510550)

1 引言

目前，由于智能跟隨技術(shù)在工農(nóng)業(yè)生產(chǎn)、科學技術(shù)、智慧生活等各方面都有著廣泛的應用需求[1]，故它已經(jīng)成為了定位領(lǐng)域的一個研究熱點。在各種定位方案中，UWB（Ultra-Wide Band）定位技術(shù)與其它定位技術(shù)相比具有精度高、功耗低、抗干擾能力強等顯著特點[2,3]，使得它在精確定位領(lǐng)域得到了廣泛的應用[4,5]。在短距離的智能目標跟隨中使用UWB 技術(shù)，可以實現(xiàn)對目標進行精確的定位和跟蹤。因此，開發(fā)基于UWB技術(shù)的智能跟隨小車，對于研究UWB 跟隨技術(shù)具有重要意義。

2 系統(tǒng)組成及其原理

智能跟隨小車的控制系統(tǒng)如圖1 所示。由圖可見，整機控制系統(tǒng)由電源1、電源2、基站模塊1、基站模塊2、微處理器、電機驅(qū)動器組成。其中，電源1 為微處理器、基站模塊1 和基站模塊2 供電；電源2 單獨為電機供電。UWB 標簽接在被跟蹤的移動物體上，基站1 和基站2 各自與UWB 標簽進行消息通信并計算出各自與UWB 標簽的距離后，基站1 把計算所得距離通過無線方式發(fā)送給基站2，再由基站2 通過串口轉(zhuǎn)發(fā)給微處理器，基站2 把自己計算所得距離通過串口發(fā)送給微處理器。微處理器根據(jù)基站1 和基站2 所測得的距離解算出標簽方位，再根據(jù)標簽方位通過PWM 脈沖驅(qū)動電機1 和電機2，從而達到跟蹤標簽的目的。

圖1 智能跟隨小車的系統(tǒng)框圖

2.1 UWB 模塊設計

基站1、基站2 和標簽電路都是一樣，只是因為內(nèi)部程序不同導致實現(xiàn)的功能也不同，所以名稱不同，但本質(zhì)上都屬于UWB 模塊，該模塊的電路如圖2 所示。UWB 模塊主要包含Decawave 公司生產(chǎn)的DW1000射頻芯片和STM32F4 單片機[6]，兩者通過SPI 接口進行連接。其中DW1000 用于發(fā)出和接收無線信號，STM32用于對收發(fā)信號進行處理并獲知兩片相互通信的DW1000 之間的距離。

圖2 UWB 模塊原理圖

2.2 電機驅(qū)動單元設計

本系統(tǒng)的電機驅(qū)動采用東芝公司的TB6612 FNG 直流電機驅(qū)動芯片來驅(qū)動，該芯片具有大電流MOSFET-H 橋結(jié)構(gòu)，雙通道電路輸出，可以同時驅(qū)動2 個電機。TB6612 FNG 芯片有4 種電機控制模式：正轉(zhuǎn)、反轉(zhuǎn)、制動、停止，其PWM 頻率可達100KHz。本系統(tǒng)的電機驅(qū)動設計原理圖如圖3 所示：

圖3 電機驅(qū)動電路圖

3 跟蹤算法的設計

3.1 跟蹤目標的定位

實要想對目標進行準確跟蹤，首先要準確計算目標的方位。本設計通過建立參考坐標系后再確定被跟蹤目標方位的方式來進行方位解算，其過程如下:

1、建立參考坐標系

如圖4所示，基站1和基站2放置于小車的車頭，他們之間的距離為lcm，標簽掛在跟蹤目標上。接下來，以基站1和基站2所在直線為x軸，基站1和基站2的中點o為原點，建立如圖4所示的直角坐標系。

圖4 整機坐標圖

2、方位解算

假設基站1、基站2 和標簽的坐標分別為(x1,y1)、(x2,y2)和(x,y)，基站1 和基站2 測得自身與跟蹤目標的距離分別是d1和d2，則由圖4 可知，這些物理量之間具有如下關(guān)系

求解以上方程組，可得到方程組的關(guān)于被跟蹤目標的(x,y)坐標有兩個解，且兩個解關(guān)于x軸是對稱的。我們假設目標一直在車頭的前方，則可知y＞0 的那個解是目標的實際位置。將基站1 和基站2 的坐標值代入式(1)和(2)所組成的方程組，可得跟蹤目標位置坐標如下：

另有以下結(jié)論:

（1）當x＜0 時，目標在車的左側(cè)；

（2）當x＞0 時，目標在車的右側(cè)；

（3）當x=0 時，目標在車的正前方；

（4）目標與車的偏離角為arctg(x/y)；

（5）目標與車的垂直距離為y。

3.2 跟蹤目標定位精度的優(yōu)化

在實際的定位測量中，由于各種因素的影響，DW1000 測得的距離值存在著較大的偏差。為了提高測量精度，本系統(tǒng)在采用雙向測距的到達時間差進行定位[7]過程中，對測量到的距離值采用滑動平均值法進行濾波，濾波前后解算得到的位置分布圖如圖5 所示。其中圖5(a)是沒有使用滑動平均值濾波的情況，圖5(b)是使用了滑動平均值濾波的情況。由圖可以看到，濾波前y在-94.5cm～-99.5cm 之間變化約4cm 時，x的變化從-15cm 到10cm，整個變化達到25cm，而濾波后，y在-96.2cm 到-97.8 之間變化約1.6cm 時，x的變化只有6cm左右。可以發(fā)現(xiàn)，采用滑動平均值法后定位精度得到了明顯的提高。

圖5 使用滑動平均值法后標簽位置分布對比

進一步研究發(fā)現(xiàn)，兩個基站之間的距離大小對定位精度也有影響。圖6 的(a)圖和(b)圖分別給出了基站間距為50cm 和基站間距為20cm 時跟蹤目標的位置分布圖，對比這兩個圖可以發(fā)現(xiàn)，基站間的距離對定位精度有著重要的影響。所以在實際應用中，需要對該距離進行不斷的調(diào)整優(yōu)化，在本系統(tǒng)中，經(jīng)過調(diào)整基站間距設置為40cm 時定位效果相對較佳。

圖6 基站間距對標簽位置的影響

3.3 目標跟蹤控制算法

在確定跟蹤對象的位置后，接下來采用位置式PID 算法進行目標跟蹤控制。其差分方程如下式所示[8]

其中，ek為k時刻設置值與實際值的差，uk為計算得到的控制量。

對于本設計，需要考慮兩個問題：

(1) 距離的控制，通過控制小車的速度來控制小車與目標的距離。

假設保持的距離設置值為d，則標簽實際距離和設定的保持距離的偏差為(d-y)，把(d-y)代入公式(5)計算得到udk，用udk調(diào)整小車前進速度，當udk較大時，加快小車速度，當udk較小時，減小小車速度，當udk＜0時，小車停止。

(2) 偏離角的控制，通過控制小車的旋轉(zhuǎn)角度來控制偏離角。

偏離角大小可以用x/y來衡量，把x/y代入公式(5)得到uak，用uak控制旋轉(zhuǎn)角度，當uak較大時，大幅旋轉(zhuǎn)；當uak較小時，小幅旋轉(zhuǎn)；uak為正，右轉(zhuǎn)；uak為負，左轉(zhuǎn)。

以上兩個目標實際上都通過對小車車輪轉(zhuǎn)動的控制來實現(xiàn)，具體可以采用融合控制的方法。該方法是先分別計算得到距離控制量udk和偏離角控制量uak，再把兩個控制量相加得到總輸出控制量，即

以上兩式中，kLu用于作為控制左輪的PWM信號的脈寬參數(shù)，kRu用于作為控制右輪的PWM信號的脈寬參數(shù)。對小車進行融合控制后，其靈敏度和平滑度改善非常明顯。

4 結(jié)論

本文設計了一種智能跟隨小車控制系統(tǒng)，對系統(tǒng)的跟隨控制進行了詳細的研究。研究結(jié)果表明，使用滑動平均值濾波法能夠減少UWB 測量過程中的噪聲干擾，有效提高對跟蹤目標的定位精度。在得到相對精準的測量數(shù)據(jù)后，采用距離與偏離角融合的PID 控制算法可以提高跟隨過程的平穩(wěn)性和靈敏度。實驗研究還表明，定位基站的間距太小會降低方位解算的精度，實際應用中應根據(jù)具體場景不斷優(yōu)化選擇定位基站的間距。