基于STM32的移動機器人自適應跟蹤系統設計

（武漢工程大學 電氣信息學院，武漢430205）

機器人自動跟蹤技術的發展使其在醫療、物流等領域得到逐步認可并開始推廣應用，自動跟蹤功能使得人們的工作和生活更加便利，降低了很多行業和工種的工作強度，在提高工作效率的基礎上，避免了人工操作的隨意性。

從當前研究狀況看，機器人的功能深化研究成為全球的研究熱點，很多學者、專家深入研究了移動機器人的跟蹤技術，機器人替代人工所遇到的外部環境多數較為復雜，在機器人跟蹤研究過程中如何實現機器人的自適應跟蹤成為研究難點。在機器人自適應跟蹤方面，不僅要實現智能自動跟蹤，還要有效躲避障礙物。文獻[1]提出跟蹤過程中避障功能可采用人工勢場算法及超聲波三邊定位算法完成對目標的定位；文獻[2]研制的機器人采用無線通信及影像處理技術實現簡單的移動、導航等功能；文獻[3]設計的室內服務機器人不僅可以實現精準定位，還可以通過視覺導航在雜亂的室內工作；文獻[4]設計的智能輪椅依靠傳感器視覺及里程計實現自主定位與導航；文獻[5]設計了自主導航車和服務機器人。然而，在機器人自適應跟蹤避障方面，仍然存在定位精度差、跟蹤精度不足和避障性能較低等問題。故在此提出了基于STM32 芯片的移動機器人自適應跟蹤系統。

1 系統總體設計方案

該系統采用激光雷達作為精準定位系統，采用PWM 波實現對電機的轉速和方向的控制，同時采用基于交互式多模型IMM（interacting multiple model）算法和矢量場矩形VFH （vector field histogram）方法的新型避障方法，實現移動機器人的自適應跟蹤和避障功能。

考慮到移動機器人的自適應跟蹤技術的需要，基于STM32F407ZGT6 芯片構建出整體系統設計框圖，如圖1所示。

圖1 機器人自適應跟蹤系統總體框圖Fig.1 General block diagram of the robot adaptive tracking system

在該系統中，機器人部分主要由控制器MCU、激光雷達、避障傳感器和姿態傳感器，以及舵機和直流電機等組成。該機器人的自適應跟蹤的控制過程如下：機器人的激光雷達時刻掃描接受目標機器人的信號，通過目標機器人信號準確判斷其所處位置，控制器在得到位置指令后，按照程序控制要求形成確定的PWM 波，從而驅動舵機和直流電機，實現對于目標的跟蹤功能。在跟蹤過程中，要滿足自動避障功能和目標丟失下的預測跟蹤功能，實現機器人自適應跟蹤功能。

2 主要技術分析

2.1 定位系統

機器人定位的準確性對于實現精準跟蹤至關重要，在此采用激光雷達的定位測距方式來實現對于目標的定位[6]。該方式的主要原理[7-8]是通過激光雷達的掃描技術實現目標的定位信號采集，通過波束形成處理、擴頻處理、測距碼加權技術實現對目標位置的精準識別。激光雷達定位方式較其他定位方式具有定位精度更高的特點，為自適應跟蹤系統的研究提供了可靠的基礎支撐。

2.2 主芯片及電機驅動模塊

在此采用的主芯片為STM32F407ZGT6，最高工作頻率高達168 MHz，性能可達1.25 DMips/MHz，能實現要求很高的運算[9]。采用的Thumb-2 指令集使得處理器具有更高的指令效率，其集成單精度浮點運算和增強型DSP 處理指令使得中斷響應迅速，功耗低，僅為230 μA/MHz。

直流電機驅動選用L298N 驅動芯片，其作用在于可控制電機的加減速[10-11]。

3 基于IMM 和VFH 的機器人跟蹤避障技術

所采用的新型避障方法融合了IMM 和VFH 的優勢，利用IMM 算法的位置預測功能，有效適應目標的機動運動變化，利用VFH 方法實現對于運動區域柵格化分區并對障礙物密度進行檢測，解決在趨向目標點附近存在的陷阱干擾和轉向抖動的問題。

3.1 IMM 跟蹤算法

在機器人跟蹤目標運動軌跡的過程中，一般不考慮目標的形狀等因素的影響，而是將目標簡化為一個點，通過牛頓運動定律加以描述。其目標運行模型為[12]

式中：xk為k時刻目標的狀態向量；uk為控制輸入向量；wk為過程噪聲；vk為量測噪聲；zk為觀測向量；fk和hk分別為目標狀態函數、控制輸入函數。

在描述目標運動狀態時，一般采用位置、速度和加速度，分別用x，和來表示。目標跟蹤模型可分為多種，在此以CA（constant acceleration）模型為例，分析目標運動模型。

通過離散化處理，可得到該模型的離散模型[12]，即

式中：Ф（k+1，k）為狀態轉移矩陣；G（k+1，k）為噪聲系數矩陣；w（t）～N（0，σ2）為一個隨機白噪聲過程。同理，其他模型也能得到類似的狀態表達式。

交互式多模型算法可以在運算過程中實現多個目標的匹配。因此在系統估計中，該算法能夠有效實現多個并行濾波器的合理方式的聯合。正是由于交互式多模型算法實現了交互和動態變化的功能，因此，交互式多模型算法仍然作為最有效的跟蹤算法被廣泛應用。

交互式多模型算法的基本原理如下：假設跟蹤目標的運動模型個數為r，則會有r個數學模型來描述目標運動狀態，因此形成模型集合M=｛m1，m2，…，mr｝。對于r值的選取，要同時考慮到目標所有可能運動模式，但是又不能取值過大，造成模型算法的競爭。按照目標跟蹤模型推導過程，可以得到交互式多模型算法的第j個狀態為[12]

式中：Фj（k-1）為過程矩陣；Gj（k-1）為噪聲傳遞矩陣；hj（k-1）為觀測矩陣；wj（k-1）為過程噪聲；vj（k-1）為觀測噪聲。過程噪聲和觀測噪聲相互獨立，其均值均為零，協方差矩陣為Qj和Rj。交互式多模型算法計算步驟有輸入交互、并行濾波器過濾、濾波數據融合，以及概率更新等4個。

由于應用過程中跟蹤目標機動方式是變化的，這就要求跟蹤算法模型中包含多種機動形式，通過它們的組合能夠提高系統的抗干擾性能。該方法在實際應用中表現出較好的魯棒性，而且跟蹤精度和收斂速度均得到較大的提升。

3.2 VFH 避障技術

將機器人跟蹤運動的平面劃分成為含二值信息的柵格單元，可通過計算不同單元的信任度值來表示機器人在跟蹤路線存在障礙物的密度，即可得出越過障礙物是否可靠，可靠性高低的分布情況能夠將最合適的的避障路線呈現出來。該方法解決了機器人在避障過程中的震顫問題，且可靠性高[13]。

機器人在對前進方向的相關空間范圍進行柵格化之后，柵格內的信息值分布情況即可呈現出來，因為需要將機器人可能遇到的障礙物及方向數據具體化，在此將機器人柵格化的平面區域進行向量化處理，劃分成若干空間的運動區域，如圖2所示。

圖2 機器人運動區域柵格向量化示意圖Fig.2 Diagrammatic sketch of raster vectorization of robot moving area

通過將柵格進行向量化劃分，可以得到障礙物處于運動區間的向量值pi，j；障礙物到機器人位置的矢量角[14]δi，j。具體為

式中：a，b為取值為正整數的權數，機器人的避障行為可通過賦值改變；di，j為機器人和障礙物之間的距離；i，j 分別為所屬障礙物柵在運動區域內的行、列；cvi，j為運動區域（i，j）的信任度值。

通過式（6）的向量值可以體現出障礙物柵格的信任度，以及障礙物與機器人之間存在的具體距離；式（7）的矢量角值能夠反映出障礙物柵格在運動區域的方位。

在機器人的避障過程中，通過設定合適的信任度值當作避障路徑規劃的標準，γ為柵格分辨率，機器人所屬的運動區域劃可劃分成n個扇形區間，劃分方式如圖2所示。任意區間存在：

式中：k為機器人在所屬區域的運動劃分空間。k 區間的障礙密度計算需滿足以下2個條件：障礙物柵格的向量值，其值可通過計算得到；明確k 區域的具體范圍，即

式中：hk為觀測矩陣。

機器人在避障過程中根據障礙物的密度分布，優選低于信任度設定值的方向作為避障的運動方向。

3.3 機器人避障流程分析

IMM 算法具有實時更新的優勢[15]，結合VFH 方法能將下一時刻機器人所處的位置、信任度值分布等信息，通過上一時刻機器人在平面內運動的障礙物柵格分布信息預測出來。

選擇閾值范圍內的優先方向，設置非靜態的路徑計劃[16]，其通過優先路徑可以完成避障。機器人避障的算法流程如圖3所示。

圖3 算法流程Fig.3 Algorithm working flow chart

4 仿真驗證

4.1 無障礙物的機器人跟蹤運動仿真

為了驗證所設計機器人的自適應跟蹤性能，利用MatLab 進行仿真研究。

假設，機器人在二維平面運動，目標的初始位置坐標為（200 m，100 m），跟蹤機器人的初始位置坐標為（150 m，100 m），在t=0～42 s時目標做勻速直線運動，在t=42～100 s時目標逐漸做慢轉彎運動。設定掃描周期為T=1 s，觀測噪聲的均值為0，方差為100，交互式多模型采用的算法分別為CA 和CT 模型。仿真目標運動軌跡，以及跟蹤機器人與目標軌跡的對比如圖4所示。在運動初期，目標運動接近線性；在目標運動后期，目標逐漸轉彎運行，所形成的運行軌跡接近圓。

圖4 運動軌跡仿真Fig.4 Moving trajectory simulation

設定跟蹤機器人的初始坐標為［150，100］，通過對目標進行跟蹤仿真，得到圖4b所示的跟蹤機器人與目標的運行軌跡的對比。由圖可見機器人根據本文方法設計具有較好的跟蹤能力。

圖5 跟蹤機器人和目標軌跡誤差Fig.5 Errors between robot trajectory and target trajectory

機器人跟蹤過程中，在x 方向和y 方向上產生的路徑誤差值如圖5所示。由圖可見，機器人跟蹤誤差最大發生在58～76 s 的連續轉彎部分，最大值誤差值在轉彎直徑占比小于6%，由于目標初轉彎中，跟蹤機器人會重新計算多模型交互狀態，這其間會產生較大誤差。但是由本文結果可見，誤差水平較低，說明在此時間段跟蹤精度可以認為較好，機器人在其他時間的跟蹤運行階段則具有非常好的跟蹤精度。

4.2 障礙物的機器人跟蹤運動仿真

為了研究機器人自適應跟蹤能力，需要對避障能力進行檢驗。在上述仿真中加入障礙物，設置目標軌跡不受障礙物影響，障礙物僅對跟蹤機器人有效，仿真試驗結果如圖6所示。

圖6 機器人跟蹤避障仿真運動軌跡Fig.6 Robot moving trajectory in obstacle avoidance simulation experiment

在該跟蹤機器人路徑上設置3個障礙。由仿真試驗結果可見，機器人均能有效避開障礙物，VHF方法在剛開始檢測到障礙物時，具有較大的轉動角度，隨著逐步前進，跟蹤機器人逐漸回來原跟蹤軌跡上來，符合柵格化運行區域檢測障礙物密度的避障原理。

5 結語

采用IMM 和VHF 方式的新型機器人跟蹤避障方法，設計了跟蹤機器人的自適應跟蹤系統。在無障礙物的目標跟蹤過程中，考察了目標在直線和轉彎運動過程中的跟蹤精度，結果發現在直線運動過程中，機器人具有非常高的跟蹤精度，x，y 方向的誤差值均小于5 m，在部分連續轉彎的過程中機器人的跟蹤精度雖有所下降，但最大誤差值在轉彎直徑中占比小于6%，所以仍然處于較高水平；在存在障礙物的仿真過程中，機器人實現了跟蹤和避障功能，證明本文方法具有可行性。