移動目標(biāo)追蹤系統(tǒng)的設(shè)計(jì)及實(shí)現(xiàn)

張建峰，秦飛舟

(寧夏大學(xué) 物理與電子電氣工程學(xué)院,銀川 750021)

0 引言

用一些智能化設(shè)備的操作代替?zhèn)鹘y(tǒng)的手工操作可以使得生產(chǎn)效率更快、產(chǎn)品質(zhì)量更高，而效率快、質(zhì)量好的前提是擁有一臺比較穩(wěn)定、反應(yīng)迅速的設(shè)備。在智能分揀快遞、機(jī)器焊接元器件設(shè)備等一些自動化設(shè)備中，能夠快速識別并穩(wěn)定迅速的執(zhí)行任務(wù)離不開一個(gè)好的算法，而作為傳統(tǒng)經(jīng)典的PID算法是自動控制、人工智能領(lǐng)域中一個(gè)非常普遍用到的算法[1-4]。

在工業(yè)過程控制中，根據(jù)被控對象的實(shí)時(shí)數(shù)據(jù)采集的信息與預(yù)設(shè)值比較產(chǎn)生的誤差的比例、積分和微分進(jìn)行控制的控制系統(tǒng)，簡稱PID(proportional integral derivative)控制系統(tǒng)。PID控制具有原理簡單，魯棒性強(qiáng)和實(shí)用廣等優(yōu)點(diǎn)，是一種技術(shù)成熟、應(yīng)用廣泛的控制系統(tǒng)[5]。

本設(shè)計(jì)采用PID算法控制小車的前進(jìn)速度以及行走的方向[6]，實(shí)現(xiàn)對物體的循跡、追蹤[7]。并在傳統(tǒng)的PID算法的基礎(chǔ)上加以改進(jìn)，使得小車更快、更穩(wěn)定的進(jìn)行物體循跡與追蹤[8-11]。

1 系統(tǒng)的總體設(shè)計(jì)

該系統(tǒng)采用分布式控制系統(tǒng)，系統(tǒng)框圖如圖1所示，系統(tǒng)由攝像頭模塊，主控制器，電機(jī)驅(qū)動，電機(jī)，編碼器以及電源組成。

圖1 系統(tǒng)框圖

其中電源模塊為整個(gè)系統(tǒng)供電。攝像頭模塊采集一幅圖像處理后，將得到的物體的坐標(biāo)以及物體到小車的距離傳給主控制器，主控制器接收攝像頭和編碼器傳入的數(shù)據(jù)，通過優(yōu)化PID算法計(jì)算出小車的應(yīng)當(dāng)行走速度以及方向，使電機(jī)驅(qū)動模塊驅(qū)動電機(jī)，實(shí)現(xiàn)對物體的循跡追蹤。

2 系統(tǒng)硬件設(shè)計(jì)

移動目標(biāo)追蹤系統(tǒng)的硬件包括主控芯片、攝像頭模塊、電源模塊、電機(jī)驅(qū)動模塊以及編碼器模塊。

電源選擇11.1 V航模電池作為輸入，通過LM2596S穩(wěn)壓芯片輸出穩(wěn)定的5 V電壓，通過LM1117穩(wěn)壓芯片輸出穩(wěn)定的3.3 V電壓，實(shí)現(xiàn)對外部模塊提供3.3 V和5 V的電源。

為了保證本次設(shè)計(jì)的所有硬件資源需求以及小車運(yùn)行過程中無延遲的現(xiàn)象，選擇一款內(nèi)部硬件資源豐富、運(yùn)行速度快的芯片尤為重要，最終選擇了STM32F407ZGT6作為主控芯片，該芯片運(yùn)行速度最大可到達(dá)168 MHz、定時(shí)器有14個(gè)，是一款穩(wěn)定可靠的芯片[12]。

2.1 openmv攝像頭模塊

追蹤移動目標(biāo)的攝像頭模塊選擇的是OPENMV模塊，該模塊集成的STM32H7芯片對所采集到的物體圖像進(jìn)行了分析，得到了所追蹤目標(biāo)的定位坐標(biāo)，通過串口的形式把定位坐標(biāo)以及小車到目標(biāo)的距離傳給主控制器[13]。

OPENMV模塊集成了STM32H7芯片和OV7725攝像頭，提供了供電和串口引腳。在STM32H7內(nèi)部集成了一些處理算法，以函數(shù)形式提供，通過python語言去編寫處理程序，將物體坐標(biāo)和物體到小車的距離發(fā)送給主控制器。

2.2 編碼器測速模塊

編碼器測速模塊主要由霍爾傳感器組成，選用的是STM15-P25雙霍爾磁性編碼器[14]。

雙霍爾磁性編碼器由磁柵和磁敏兩個(gè)元件構(gòu)成。該模塊連接到電機(jī)的轉(zhuǎn)軸上，通過電機(jī)的轉(zhuǎn)速的變化，霍爾傳感器在A、B相位引腳產(chǎn)生兩個(gè)50%占空比的方波，這兩個(gè)方波相位相差1/4個(gè)周期。STM32芯片通過定時(shí)器編碼器采集的功能或者定時(shí)器輸入捕獲的方式即可采集到小車的轉(zhuǎn)速，為PID閉環(huán)控制做準(zhǔn)備。

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

小車輪子選用的是麥克納姆輪，由4個(gè)電機(jī)分別驅(qū)動。由于單片機(jī)輸出的電流最大不超過200 mA，不足以驅(qū)動電機(jī)，所以需要加入電機(jī)驅(qū)動電路。

驅(qū)動芯片選擇的是L298N，該芯片最大輸出功率為25 W，通過輸入PWM波可控制電機(jī)的正反轉(zhuǎn)，其電路圖如圖2所示[15]。

圖2 L298N模塊電路圖

由于一個(gè)L298N模塊可以驅(qū)動兩路電機(jī)，小車采用4輪驅(qū)動[5]，所以需要兩個(gè)L298N模塊，給該模塊輸入12 V電壓以保證芯片正常工作。通過EN_A引腳輸入50 Hz的可調(diào)占空比去控制小車的轉(zhuǎn)速，占空比越大小車轉(zhuǎn)的越快。通過IN1引腳和IN2引腳，可以實(shí)現(xiàn)對電機(jī)方向的控制[16]，具體引腳與運(yùn)行情況如表1所列。其中PWM_EN代表PWM使能，PWM_DIS代表PWM失能，H代表高電平，L代表低電平，X代表引腳失能。

表1 電機(jī)方向控制表

3 系統(tǒng)軟件設(shè)計(jì)

移動目標(biāo)追蹤系統(tǒng)軟件包括攝像頭和主控制器的軟件。其中攝像頭程序使用的是PYTHON語言開發(fā)，根據(jù)“星瞳”科技公司提供的PYTHON庫，可以直接在廠商提供的IDE編譯器進(jìn)行編程。主控制器程序根據(jù)ST公司提供的庫函數(shù)，使用KeilC5平臺進(jìn)行開發(fā)。

系統(tǒng)主控流程如圖3所示。首先是各模塊的初始化，包括串口、PWM、編碼器的定時(shí)器初始化、IO初始化等。然后主控芯片發(fā)出啟動小車自轉(zhuǎn)指令，當(dāng)攝像頭模塊檢測到目標(biāo)物體之后，攝像頭模塊會給主控制器發(fā)送信號，當(dāng)主控制器接收到攝像頭發(fā)送的數(shù)據(jù)可以知道物體的坐標(biāo)和物體到小車的距離信息，就可以執(zhí)行優(yōu)化后的PID算法去計(jì)算PWM值從而控制電機(jī)，實(shí)現(xiàn)對物體的定位、追蹤、循跡，當(dāng)追蹤到小球后小車停止運(yùn)行。

圖3 主控制流程圖

3.1 攝像頭驅(qū)動程序設(shè)計(jì)

攝像頭程序采集圖片的格式為RGB565、圖片的尺寸為240×320、圖片刷新時(shí)間為1 ms等。小車可以識別不同顏色的物體，通過設(shè)置圖像識別的閾值實(shí)現(xiàn)對顏色的選擇。本次設(shè)置選擇紅色物體，主程序不斷采集圖像，當(dāng)檢測到紅色物體時(shí)，程序會在紅色物體的上、下、左、右4個(gè)方向的邊緣點(diǎn)處描繪一個(gè)白色正方形虛線邊框如圖4所示，把紅色物體包圍起來。通過描繪的正方形白色虛線邊框的長與寬計(jì)算出正方形的面積除以比例系數(shù)減去常數(shù)就是小車和紅色物體的相對距離。其距離的計(jì)算公式為：

distance=i_length*i_width/prop-k

其中：distance代表小車與物體的距離；i_length代表物體白色虛線邊框的長；i_width代表白色虛線邊框的寬；prop代表距離與面積的比例系數(shù)，為負(fù)數(shù)；k代表初始常數(shù)。

當(dāng)小車離物體越來越近時(shí)，算出的面積也就越來越大，距離也就越來越小。通過算出的距離減去給定的距離(想讓小車與物體相隔多遠(yuǎn)的距離)就是距離偏差，為PID閉環(huán)控制做準(zhǔn)備。其距離偏差的表達(dá)式為：

h_error=distance-d_need

其中：distance代表小車與物體的實(shí)時(shí)的距離；d_need代表給定的距離；h_error代表距離的測量值與實(shí)際需要值的偏差。

圖4 圖像參數(shù)說明

用描繪物體白色邊框的中心點(diǎn)橫坐標(biāo)減去一幅圖像的長除以2即可得到物體與小車的位置偏差，其表達(dá)式為：

x_error=i_centre-p_length/2

其中：i_centre代表白色邊框中心點(diǎn)的橫坐標(biāo)；p_length代表一幅圖像的長度；x_error代表測量值與實(shí)際需要值的方向偏差。

最后通過串口將距離偏差和方向偏差傳給主控制器。具體流程圖如圖5所示。

圖5 攝像頭程序流程圖

3.2 電機(jī)優(yōu)化PID算法程序設(shè)計(jì)

3.2.1 PID算法

PID算法作為自動控制領(lǐng)域中經(jīng)典算法，常用于溫度控制、水位控制、電機(jī)調(diào)速等多種閉環(huán)控制。PID算法中的P、I、D分別代表比例、積分、微分參數(shù)，PID分為位置式PID和增量式PID[17]。

位置式PID的公式為:

Kd*[e(k)-e(k-1)]

增量式PID的公式為：

u(k)=Kp*[e(k)-e(k-1)]+Ki*e(k)+

Kd*[E(K)-2e(k-1)-e(k-2)]

其中:u(k)代表當(dāng)前PID計(jì)算輸出值。Kp、Ki、Kd是PID算法的系數(shù)。e(k)是當(dāng)前偏差，e(k-1)代表上一次偏差，e(k-2)代表上兩次偏差，e(k-i)代表上i次偏差。

由位置式PID算法和增量式PID算法可以看出，兩個(gè)公式都包含當(dāng)前偏差乘以系數(shù)和上一次偏差乘以系數(shù)，差別在于位置式PID把每次偏差累加乘以系數(shù)進(jìn)行計(jì)算，而增量式PID是計(jì)算上兩次偏差乘以系數(shù)。

兩者PID的功能都有一階慣性濾波，區(qū)別在于位置式PID把每次偏差進(jìn)行累加起到一個(gè)飽和抑制的作用，而增量式PID則是把上一次與上兩次偏差進(jìn)行相減起到一個(gè)對系統(tǒng)動態(tài)過程加速的功能。

因此，位置式PID更加適用于控制某個(gè)不變量、穩(wěn)定值，如保持水位高度、無人機(jī)懸停、保持溫濕度等，因?yàn)橛欣奂拥倪^程計(jì)算量非常大，若系統(tǒng)出現(xiàn)故障等大幅度變化的情況，系統(tǒng)也會相應(yīng)做出大幅度的改變。增量式PID比較適合用在輸出不斷變化的系統(tǒng)中，比如電機(jī)控制速度，舵機(jī)控制方向等，因?yàn)樽隽硕纬{(diào)的計(jì)算即得到上一次偏差與上兩次偏差的差值，從而快速接近目標(biāo)值[18-19]。

3.2.2 傳統(tǒng)PID算法控制電機(jī)

電機(jī)驅(qū)動程序流程圖如圖6所示，電機(jī)使用STM32輸出50 Hz可變占空比的PWM，可以控制驅(qū)動電機(jī)的快慢，同時(shí)為了保持速度穩(wěn)定、反應(yīng)迅速需要加入PID閉環(huán)控制算法。

圖6 電機(jī)驅(qū)動程序流程圖

本設(shè)計(jì)中PID算法控制電機(jī)包含3個(gè)部分，分別是速度控制PID，方向控制PID，距離控制PID。

其中速度PID根據(jù)編碼器測量的速度與給定小車的速度的偏差進(jìn)行PID運(yùn)算，再根據(jù)所計(jì)算的PWM值控制小車從一個(gè)速度到另一個(gè)速度的加速、減速。方向PID根據(jù)攝像頭采集到的物體相對小車的中心點(diǎn)的偏差進(jìn)行PID運(yùn)算，計(jì)算出PWM去控制小車的左右移動。距離PID根據(jù)攝像頭采集到的物體與小車的距離偏差進(jìn)行PID運(yùn)算，計(jì)算出PWM去控制小車前后移動。

3種PID原理一樣，都是通過給定值與測量值的偏差作為PID的參數(shù)，PID函數(shù)運(yùn)算返回PWM的值，只是偏差不同和P、I、D系數(shù)不同，算出最后的PWM值的結(jié)果也就不同。

PID算法可以不需要I系數(shù)、D系數(shù)或者I、D系數(shù)，只要系統(tǒng)能夠保持穩(wěn)定單純的用P系數(shù)進(jìn)行調(diào)節(jié)，也可以達(dá)到很好的效果。經(jīng)過反復(fù)的調(diào)試，本次設(shè)計(jì)只需要比例項(xiàng)和微分項(xiàng)就可以達(dá)到很好的效果，所以選擇位置式PID和增量式PID效果一樣。所以本設(shè)計(jì)PID公式為：

IncPid=KP*EE0+KI*(EE0-EE1)

其中：KP、KI分別為PID算法中的P、I參數(shù)；EE0代表實(shí)際值與測量值的偏差也就是當(dāng)前偏差；EE1代表上次偏差也就是程序執(zhí)行第二次的上次偏差；當(dāng)前偏差是給定值與測量值的差值，通過不斷調(diào)節(jié)P、I參數(shù)確保小車能夠更快更準(zhǔn)的達(dá)到預(yù)期的值。

3.2.3 優(yōu)化PID算法控制電機(jī)

使用傳統(tǒng)PID調(diào)試小車，是通過調(diào)節(jié)P、I、D參數(shù)使用小車的輸出值呈現(xiàn)“發(fā)散”、“等幅振蕩”、“合理”、“不達(dá)標(biāo)”4種情況。

理想的情況下，傳統(tǒng)PID會開始出現(xiàn)超調(diào)，隨后緩慢恢復(fù)穩(wěn)定值。而本次的優(yōu)化在原有的基礎(chǔ)上加入超調(diào)校正和邊緣處理算法，讓小車更穩(wěn)定的運(yùn)行，如圖7所示為輸出值在理想情況下合理的曲線。

圖7 傳統(tǒng)PID輸出合理的曲線

超調(diào)校正是給定一個(gè)較大的超調(diào)角度，即P系數(shù)比較大，這樣可以讓小車剛開始用比較快的速度啟動也就是加速度更大，讓小車更快的到達(dá)實(shí)際值，在這種情況下，將會出現(xiàn)“等幅振蕩”或者“發(fā)散”現(xiàn)象。為了防止該現(xiàn)象，PID函數(shù)執(zhí)一次，程序計(jì)算偏差三次，也就是把反饋采集的數(shù)據(jù)速度調(diào)節(jié)到PID函數(shù)運(yùn)行的三倍，這樣會使小車開始會以較大的加速度運(yùn)行，當(dāng)還沒有到達(dá)目標(biāo)值時(shí)，此時(shí)反饋原件再次計(jì)算偏差，此時(shí)的偏差已經(jīng)較小就不會出現(xiàn)“等幅振蕩”現(xiàn)象，但是會出現(xiàn)輸出值穩(wěn)定的時(shí)間加長現(xiàn)象。

邊緣處理算法就是對超調(diào)校正出現(xiàn)穩(wěn)定時(shí)間較長的缺點(diǎn)進(jìn)行修正。邊緣處理算法就是對輸出值的上限進(jìn)行設(shè)置，保證上限值不會太大，以及配合調(diào)節(jié)參數(shù)I，這樣就不會現(xiàn)象大幅度的抖動的現(xiàn)象，也可以減少輸出值穩(wěn)定時(shí)間長的問題，其優(yōu)化后的理論輸出值曲線為圖8所示。

圖8 優(yōu)化PID輸出合理的曲線

通過與傳統(tǒng)PID算法的對比，我們可以清楚的看出，優(yōu)化后的PID輸出的曲線到達(dá)目標(biāo)值的速度更加迅速，由于對上限進(jìn)行了設(shè)置將不會出現(xiàn)“等幅振蕩”和“發(fā)散”的現(xiàn)象。

4 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證所設(shè)計(jì)的移動目標(biāo)追蹤系統(tǒng)的優(yōu)越性，設(shè)計(jì)了對比實(shí)驗(yàn)。分別在移動目標(biāo)追蹤系統(tǒng)的小車上采用傳統(tǒng)PID控制算法與優(yōu)化后的PID控制算法去控制小車，實(shí)現(xiàn)對物體的追蹤、循跡[20-25]。移動目標(biāo)追蹤系統(tǒng)的優(yōu)越性可以根據(jù)小車追蹤物體的時(shí)間和穩(wěn)定性綜合考慮，當(dāng)小車追蹤物體的時(shí)間越短、到達(dá)目標(biāo)位置穩(wěn)定性越好代表該系統(tǒng)越優(yōu)越。實(shí)驗(yàn)分為兩組分別為：物體動態(tài)運(yùn)動時(shí)，傳統(tǒng)PID算法控制；物體動態(tài)運(yùn)動時(shí)，優(yōu)化PID算法控制[26-31]。

在物體以一定速度運(yùn)動過程中，利用傳統(tǒng)PID控制小車對物體進(jìn)行自動追逐。小車的速度可以通過串口將編碼器測量出的速度值發(fā)送給PC端，PC端可以觀察出小車4個(gè)輪子行駛的速度。通過小車在運(yùn)行的過程發(fā)送給PC端的速度數(shù)據(jù)，可以判斷出小車到達(dá)穩(wěn)定時(shí)所用時(shí)間的長短。通過小車穩(wěn)定時(shí)小車輪子速度的變化范圍即可看出小車穩(wěn)定性的強(qiáng)度[32-40]。

4.1 物體動態(tài)運(yùn)動時(shí)，傳統(tǒng)PID控制

當(dāng)小車用傳統(tǒng)PID控制算法時(shí)，得出的數(shù)據(jù)如圖9所示。最初小車會以880的速度前進(jìn)，當(dāng)小車速度到達(dá)520的速度趨于平穩(wěn)，速度的抖動幅度為40。

圖9 傳統(tǒng)PID算法小車的速度

4.2 物體動態(tài)運(yùn)動時(shí)，優(yōu)化PID控制

當(dāng)小車用優(yōu)化PID控制算法時(shí)，得出的數(shù)據(jù)如圖10所示。最初小車會以880的速度前進(jìn)，當(dāng)小車速度達(dá)到240的速度趨于平穩(wěn)，速度抖動幅度為30。

圖10 優(yōu)化PID算法小車的速度

通過1、2對比我們可以看出傳統(tǒng)PID算法控制相對優(yōu)化后的PID算法第一次到速度穩(wěn)定值的時(shí)間相對較長，速度穩(wěn)定時(shí)上下幅度范圍較大。

綜合上述實(shí)驗(yàn)結(jié)果可以得出結(jié)論，無論是傳統(tǒng)PID控制算法還是優(yōu)化后的PID算法都可以完成對移動物體的追蹤、循跡?；谝苿幽繕?biāo)追蹤系統(tǒng)的基礎(chǔ)上，用優(yōu)化PID控制算法代替?zhèn)鹘y(tǒng)PID控制算法可以更快。更穩(wěn)定的對物體進(jìn)行追蹤、循跡[41-50]。

5 結(jié)束語

該系統(tǒng)以STM32為主控制器，通過與攝像頭交互以及硬件PCB的設(shè)計(jì)完成了移動目標(biāo)追蹤系統(tǒng)的設(shè)計(jì)[51-54]。該移動目標(biāo)追蹤系統(tǒng)能夠應(yīng)用在自動跟隨行李箱、自動追蹤人臉識別等設(shè)備中，具有非常好的實(shí)用和應(yīng)用價(jià)值[55-57]。

該系統(tǒng)在傳統(tǒng)PID算法控制小車的基礎(chǔ)上，加入超調(diào)校正與邊緣處理算法讓小車更快準(zhǔn)的達(dá)到給定值，讓小車反應(yīng)更加靈敏、反應(yīng)更加迅速[58-59]。

該小車有著較低的成本外觀小巧且能夠追蹤到各種各樣劣勢的環(huán)境下的物體的優(yōu)點(diǎn)，在未來引入物聯(lián)網(wǎng)還可以使小車能夠更加智能化[60]。