移動目標追蹤系統的設計及實現

張建峰，秦飛舟

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

0 引言

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

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

本設計采用PID算法控制小車的前進速度以及行走的方向[6]，實現對物體的循跡、追蹤[7]。并在傳統的PID算法的基礎上加以改進，使得小車更快、更穩定的進行物體循跡與追蹤[8-11]。

1 系統的總體設計

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

圖1 系統框圖

其中電源模塊為整個系統供電。攝像頭模塊采集一幅圖像處理后，將得到的物體的坐標以及物體到小車的距離傳給主控制器，主控制器接收攝像頭和編碼器傳入的數據，通過優化PID算法計算出小車的應當行走速度以及方向，使電機驅動模塊驅動電機，實現對物體的循跡追蹤。

2 系統硬件設計

移動目標追蹤系統的硬件包括主控芯片、攝像頭模塊、電源模塊、電機驅動模塊以及編碼器模塊。

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

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

2.1 openmv攝像頭模塊

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

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

2.2 編碼器測速模塊

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

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

2.3 電機驅動模塊

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

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

圖2 L298N模塊電路圖

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

表1 電機方向控制表

3 系統軟件設計

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

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

圖3 主控制流程圖

3.1 攝像頭驅動程序設計

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

distance=i_length*i_width/prop-k

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

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

h_error=distance-d_need

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

圖4 圖像參數說明

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

x_error=i_centre-p_length/2

其中：i_centre代表白色邊框中心點的橫坐標；p_length代表一幅圖像的長度；x_error代表測量值與實際需要值的方向偏差。

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

圖5 攝像頭程序流程圖

3.2 電機優化PID算法程序設計

3.2.1 PID算法

PID算法作為自動控制領域中經典算法，常用于溫度控制、水位控制、電機調速等多種閉環控制。PID算法中的P、I、D分別代表比例、積分、微分參數，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)代表當前PID計算輸出值。Kp、Ki、Kd是PID算法的系數。e(k)是當前偏差，e(k-1)代表上一次偏差，e(k-2)代表上兩次偏差，e(k-i)代表上i次偏差。

由位置式PID算法和增量式PID算法可以看出，兩個公式都包含當前偏差乘以系數和上一次偏差乘以系數，差別在于位置式PID把每次偏差累加乘以系數進行計算，而增量式PID是計算上兩次偏差乘以系數。

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

因此，位置式PID更加適用于控制某個不變量、穩定值，如保持水位高度、無人機懸停、保持溫濕度等，因為有累加的過程計算量非常大，若系統出現故障等大幅度變化的情況，系統也會相應做出大幅度的改變。增量式PID比較適合用在輸出不斷變化的系統中，比如電機控制速度，舵機控制方向等，因為做了二次超調的計算即得到上一次偏差與上兩次偏差的差值，從而快速接近目標值[18-19]。

3.2.2 傳統PID算法控制電機

電機驅動程序流程圖如圖6所示，電機使用STM32輸出50 Hz可變占空比的PWM，可以控制驅動電機的快慢，同時為了保持速度穩定、反應迅速需要加入PID閉環控制算法。

圖6 電機驅動程序流程圖

本設計中PID算法控制電機包含3個部分，分別是速度控制PID，方向控制PID，距離控制PID。

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

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

PID算法可以不需要I系數、D系數或者I、D系數，只要系統能夠保持穩定單純的用P系數進行調節，也可以達到很好的效果。經過反復的調試，本次設計只需要比例項和微分項就可以達到很好的效果，所以選擇位置式PID和增量式PID效果一樣。所以本設計PID公式為：

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

其中：KP、KI分別為PID算法中的P、I參數；EE0代表實際值與測量值的偏差也就是當前偏差；EE1代表上次偏差也就是程序執行第二次的上次偏差；當前偏差是給定值與測量值的差值，通過不斷調節P、I參數確保小車能夠更快更準的達到預期的值。

3.2.3 優化PID算法控制電機

使用傳統PID調試小車，是通過調節P、I、D參數使用小車的輸出值呈現“發散”、“等幅振蕩”、“合理”、“不達標”4種情況。

理想的情況下，傳統PID會開始出現超調，隨后緩慢恢復穩定值。而本次的優化在原有的基礎上加入超調校正和邊緣處理算法，讓小車更穩定的運行，如圖7所示為輸出值在理想情況下合理的曲線。

圖7 傳統PID輸出合理的曲線

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

邊緣處理算法就是對超調校正出現穩定時間較長的缺點進行修正。邊緣處理算法就是對輸出值的上限進行設置，保證上限值不會太大，以及配合調節參數I，這樣就不會現象大幅度的抖動的現象，也可以減少輸出值穩定時間長的問題，其優化后的理論輸出值曲線為圖8所示。

圖8 優化PID輸出合理的曲線

通過與傳統PID算法的對比，我們可以清楚的看出，優化后的PID輸出的曲線到達目標值的速度更加迅速，由于對上限進行了設置將不會出現“等幅振蕩”和“發散”的現象。

4 實驗結果與分析

為了驗證所設計的移動目標追蹤系統的優越性，設計了對比實驗。分別在移動目標追蹤系統的小車上采用傳統PID控制算法與優化后的PID控制算法去控制小車，實現對物體的追蹤、循跡[20-25]。移動目標追蹤系統的優越性可以根據小車追蹤物體的時間和穩定性綜合考慮，當小車追蹤物體的時間越短、到達目標位置穩定性越好代表該系統越優越。實驗分為兩組分別為：物體動態運動時，傳統PID算法控制；物體動態運動時，優化PID算法控制[26-31]。

在物體以一定速度運動過程中，利用傳統PID控制小車對物體進行自動追逐。小車的速度可以通過串口將編碼器測量出的速度值發送給PC端，PC端可以觀察出小車4個輪子行駛的速度。通過小車在運行的過程發送給PC端的速度數據，可以判斷出小車到達穩定時所用時間的長短。通過小車穩定時小車輪子速度的變化范圍即可看出小車穩定性的強度[32-40]。

4.1 物體動態運動時，傳統PID控制

當小車用傳統PID控制算法時，得出的數據如圖9所示。最初小車會以880的速度前進，當小車速度到達520的速度趨于平穩，速度的抖動幅度為40。

圖9 傳統PID算法小車的速度

4.2 物體動態運動時，優化PID控制

當小車用優化PID控制算法時，得出的數據如圖10所示。最初小車會以880的速度前進，當小車速度達到240的速度趨于平穩，速度抖動幅度為30。

圖10 優化PID算法小車的速度

通過1、2對比我們可以看出傳統PID算法控制相對優化后的PID算法第一次到速度穩定值的時間相對較長，速度穩定時上下幅度范圍較大。

綜合上述實驗結果可以得出結論，無論是傳統PID控制算法還是優化后的PID算法都可以完成對移動物體的追蹤、循跡。基于移動目標追蹤系統的基礎上，用優化PID控制算法代替傳統PID控制算法可以更快。更穩定的對物體進行追蹤、循跡[41-50]。

5 結束語

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

該系統在傳統PID算法控制小車的基礎上，加入超調校正與邊緣處理算法讓小車更快準的達到給定值，讓小車反應更加靈敏、反應更加迅速[58-59]。

該小車有著較低的成本外觀小巧且能夠追蹤到各種各樣劣勢的環境下的物體的優點，在未來引入物聯網還可以使小車能夠更加智能化[60]。