（武漢工程大學(xué) 電氣信息學(xué)院，武漢430205）

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

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

1 系統(tǒng)總體設(shè)計方案

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

考慮到移動機器人的自適應(yīng)跟蹤技術(shù)的需要，基于STM32F407ZGT6 芯片構(gòu)建出整體系統(tǒng)設(shè)計框圖，如圖1所示。

圖1 機器人自適應(yīng)跟蹤系統(tǒng)總體框圖Fig.1 General block diagram of the robot adaptive tracking system

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

2 主要技術(shù)分析

2.1 定位系統(tǒng)

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

2.2 主芯片及電機驅(qū)動模塊

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

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

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

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

3.1 IMM 跟蹤算法

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

式中：xk為k時刻目標(biāo)的狀態(tài)向量；uk為控制輸入向量；wk為過程噪聲；vk為量測噪聲；zk為觀測向量；fk和hk分別為目標(biāo)狀態(tài)函數(shù)、控制輸入函數(shù)。

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

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

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

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

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

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

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

3.2 VFH 避障技術(shù)

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

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

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

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

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

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

在機器人的避障過程中，通過設(shè)定合適的信任度值當(dāng)作避障路徑規(guī)劃的標(biāo)準(zhǔn)，γ為柵格分辨率，機器人所屬的運動區(qū)域劃可劃分成n個扇形區(qū)間，劃分方式如圖2所示。任意區(qū)間存在：

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

式中：hk為觀測矩陣。

機器人在避障過程中根據(jù)障礙物的密度分布，優(yōu)選低于信任度設(shè)定值的方向作為避障的運動方向。

3.3 機器人避障流程分析

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

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

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

4 仿真驗證

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)語

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