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

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

1 引言

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

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

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

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

2.1 UWB 模塊設(shè)計

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

圖2 UWB 模塊原理圖

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

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

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

3 跟蹤算法的設(shè)計

3.1 跟蹤目標(biāo)的定位

實要想對目標(biāo)進(jìn)行準(zhǔn)確跟蹤，首先要準(zhǔn)確計算目標(biāo)的方位。本設(shè)計通過建立參考坐標(biāo)系后再確定被跟蹤目標(biāo)方位的方式來進(jìn)行方位解算，其過程如下:

1、建立參考坐標(biāo)系

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

圖4 整機坐標(biāo)圖

2、方位解算

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

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

另有以下結(jié)論:

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

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

（3）當(dāng)x=0 時，目標(biāo)在車的正前方；

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

（5）目標(biāo)與車的垂直距離為y。

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

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

圖5 使用滑動平均值法后標(biāo)簽位置分布對比

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

圖6 基站間距對標(biāo)簽位置的影響

3.3 目標(biāo)跟蹤控制算法

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

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

對于本設(shè)計，需要考慮兩個問題：

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

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

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

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

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

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

4 結(jié)論

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