基于多模態數據和粒子濾波器的移動機器人目標跟蹤方法

李 強

(江蘇安全技術職業學院，江蘇 徐州 221000)

0 引言

移動機器人是一種能自動完成任務的機械設備，可以按照事先設定好的程序執行任務。它具備感知環境、決策規劃和執行行動的能力，能夠通過傳感器獲取周圍環境的信息，并利用算法進行數據處理和決策。目標檢測與跟蹤是移動機器人的主要工作內容之一，目標跟蹤是利用視頻或圖像序列的上下文信息，對目標的外觀和運動信息進行建模，從而對目標運動狀態進行預測并標定目標的位置。從建模方式上看，目標跟蹤算法可劃分為生成性和判別性兩種，從被跟蹤對象的個數上看，可劃分為單個和多個目標。目標跟蹤是圖像處理、機器學習、優化等多學科交叉融合的一種新方法，是實現更高層次圖像理解任務的先決條件。在復雜場景下，由于目標受多種因素影響，使移動機器人對目標跟蹤的精度和效率降低。為此，對移動機器人的目標跟蹤進行研究。

從現階段機器人目標跟蹤方法的研究情況來看，文獻[1]提出了基于自適應隨動機構的機器人目標跟蹤方法。利用YOLOv3對多個目標進行定位，并將其定位結果與三維坐標解相結合，以達到對多個目標的實時追蹤。文獻[2]提出了基于遺傳迭代和模糊控制的機器人目標跟蹤方法。以采摘機器人作為處理對象，在機器人視覺系統和控制系統的設計上引入遺傳迭代算法，通過對圖像處理過程和機械執行末端動作過程進行優化，確定目標的跟蹤結果。文獻[3]方法針對小型移動機器人因跟蹤目標速度快而導致跟蹤精度低的問題，提出一種基于ROS的移動機器人目標跟蹤優化方法。通過建立足部動力學模型預測移動位置，結合濾波器跟蹤算法實現目標跟蹤優化。文獻[4]方法利用ORB算法和Kalman 預測器改進Camshift 算法，通過ORB算法匹配預測區域與目標區域，降低背景干擾，采用Kalman 濾波作為預測機制，初始化Camshift 算法，結合ORB算法和Kalman更新預測器參數，實現對目標的穩定跟蹤，文獻[5]方法根據目前移動機器人在目標跟蹤中存在的難點問題，設計基于視覺的目標跟蹤框架，采用深度學習中的卷積神經網絡實現目標跟蹤。然而上述目標跟蹤方法在實際運行過程中存在明顯的跟蹤誤差問題。

多模態數據指的是由兩種或兩種以上模態組成的數據，而粒子濾波器是一種基于蒙特卡羅算法的遞推濾波器，通過對隨機事件的加權后驗概率進行描述，實現對含噪音和殘缺數據的動力系統狀態估計，適用于任意的狀態空間?；谏鲜龇治?，提出了基于多模態數據和粒子濾波器的移動機器人目標跟蹤方法，以期能夠提升移動機器人的目標跟蹤精度和效率。

1 移動機器人目標跟蹤方法設計

粒子濾波器在視覺處理領域得到廣泛應用，但由于移動機器人目標跟蹤存在的問題較為復雜，粒子濾波器在實際目標跟蹤過程中會產生粒子衰退現象，在實際應用中，由于噪聲的存在，不僅會導致對粒子權值的描述不精確，而且還會導致粒子的分散度不斷降低，從而無法確保目標跟蹤效果[3]。在移動機器人目標跟蹤方法的設計中，綜合應用多模態數據和粒子濾波器，利用多模態數據解決粒子濾波器單獨使用中存在的粒子退化問題，提高移動機器人目標的跟蹤效果。

1.1 多模態數據提取方法設計

1.1.1 采集移動機器人目標多模態數據

根據多模態數據的定義，利用移動機器人內置的多個視覺傳感器設備獲取多個視角的目標圖像數據，得出多模態數據的采集結果。移動機器人目標多模態數據采集原理如圖1所示。

圖1 移動機器人目標多模態數據采集原理圖

按照圖1所示的原理，得出移動機器人對目標的數據采集結果，其中，左側視角得出目標數據采集結果為：

(1)

式中，f為成像焦距，θleft為移動機器人左側成像設備與成像目標之間的角度，即左側視角下的成像角，(x，y)為移動機器人跟蹤目標的實際位置數據，h為移動機器人跟蹤目標成像的深度坐標，其計算公式為：

(2)

式中，L和d分別對應的是左右兩端成像位置之間的距離以及視差，將式(2)的計算結果代入到式(1)中，即可得出跟蹤目標左側模態數據的采集結果。同理可以得出右側視角下的模態數據采集結果：

(3)

其中：θright為右側視角下的成像角。按照上述方式可以得到跟蹤目標所在區域所有位置的成像結果，即多模態數據的初始采集結果。

1.1.2 移動機器人目標多模態數據預處理

移動機器人目標初始多模態數據預處理的目的是保證數據的采集質量，為目標的檢測與跟蹤提供滿足要求的數據支持[4]。移動機器人目標多模態數據的預處理包括畸變校正、圖像匹配/融合、去噪、增強等步驟，多模態數據畸變的產生是由于光線通過成像設備產生的不規則折射，其目的是保證多模態數據像點的實際位置與理想位置一致，初始多模態數據的畸變校正處理結果可以表示為：

(4)

其中：δxr、δxt和δyr、δyt分別為徑向畸變和切向畸變在水平和豎直方向上的分量，其計算公式如下：

(5)

式中，κradial1、κradial2和κradial3均為徑向畸變系數，κtangential1和κtangential2表示的是切向畸變系數，r為鏡頭半徑。通過式(4)和式(5)的聯立，即可完成對初始度模態數據的畸變校正處理，將數據的畸變校正處理結果幅值給初始多模態數據[5]。采用局部立體匹配的方式對兩視角的模態采集數據進行匹配融合，以匹配點為中心建立窗口。利用圖像中相鄰像素點具有相同的特性，通過圖像中的鄰域像素來刻畫圖像中的當前匹配點，從而降低了圖像中出現的離群點的錯配問題。該方法使用相似度度量函數，對兩幅圖像中的像素點進行相似度度量，并以最近的像素點作為最好的匹配點[6]。局部立體匹配算法是以局部窗口中的鄰域像素為基礎，假設參考圖中一待匹配點的坐標為(xmate，ymate)，以該點為中心在其周圍選擇一個尺寸為J×K的視窗做為支撐范圍。為了降低由于離群點所引起的誤配，利用極線限制原理，對左右兩視角的模態數據采集結果進行分別校正，尋找數據之間的視差，在目標圖像中建立與參考圖像等大小的窗口，并在視差范圍內選擇最佳匹配點[7]。由此得出多模態數據的融合結果為：

(6)

按照上述流程，得出多模態數據的融合處理結果，保證能夠在同一數據中反映出待跟蹤目標的全部信息。多模態數據去噪處理的目的是降低無關因素對目標跟蹤結果造成的影響。為了能夠在完全去除噪聲信號的同時，最大程度地保留多模態數據中的有效信息，采用均值濾波與高斯濾波相結合的方式進行多模態數據的去噪處理，均值濾波方法采用域平均方法，將各像素的灰度值取若干像素的平均值，從而達到去噪的目的[8]。高斯濾波是將輸入陣列中的每個像素點與高斯核心作卷積，并將其結果作為輸出像素的一種簡單、快速的線性光滑降噪方法。通過均值濾波與高斯濾波處理，得出的多模態數據去噪處理結果為：

(7)

其中：Mx，y表示均值濾波窗口中心與圖像像素點重合時，窗口內的所有像素點集合，Npixel為多模態數據量，λ(s，v)為卷積模板系數，s和v分別為長度系數和寬度系數，按照上述方式將多模態數據的去噪處理結果賦值給初始數據[9]。在此基礎上，采用形態學處理的方式實現對多模態數據的增強處理，通過形態學操作來減輕噪聲對圖像的干擾，然后采用圖像分割算法將前景目標從場景中提取出來。在數學中，構建幾何模型是分析形態學處理的基礎，形態學處理中膨脹和腐蝕操作是最基本的形態學處理，它們被廣泛地應用在運動目標檢測中[10]。采用膨脹和腐蝕算法，可以使檢測到的前景目標更加完整，并且能夠消除部分背景中的干擾信息。多模態數據的膨脹與腐蝕處理過程可以描述為：

(8)

1.1.3 提取移動機器人目標多模態數據特征

以移動機器人的多模態數據預處理結果為研究對象，從邊緣、顏色、紋理等多個方面提取數據特征，為目標的檢測與識別提供參考[12]。移動機器人目標的邊緣特征用來描述目標的基本結構，在邊緣特征的提取過程中，首先將目標多模態數據均勻分割成多個模塊，采用[-1，0，1]的離散微分模板計算每個模塊內像素在水平和垂直方向上的梯度矢量，計算公式如下：

(9)

利用式(9)的求解結果，得出多模態數據中任意像素位置處梯度大小和方向特征的提取結果為：

(10)

按照上述方式可以得出多模態數據中所有像素位置的梯度大小與方向的特征提取結果，最終將所有的特征分量進行融合，即可得出邊緣特征的提取結果：

(11)

顏色特征是一種更直接，更容易觀察的視覺特性[13]。由于多模態數據所包含的環境、背景、物體以及其他與顏色相關的事物，都與顏色相關，具有良好的抗干擾能力。在RGB顏色空間內，構建目標的顏色直返圖，在任意像素點位置上，顏色分布特征的提取結果為：

(12)

式中，fi()和W分別為核函數和核帶寬，fKronecker delta()為Kronecker delta函數，γ為歸一化常數，Q表示的是多模態數據對應直方圖的顏色索引。另外移動機器人目標紋理特征的提取結果為：

(13)

其中：E(x，y)和g分別為多模態數據的能量分量和灰度均值。綜合邊緣、顏色和紋理3個方面，得出移動機器人目標特征的最終提取結果為：

τ=?1τgradient+?2τcolour+?3τtexture

(14)

式中，?1、?2和?3對應的是邊緣特征、顏色特征以及紋理特征的融合權重值。按照上述流程，完成對移動機器人目標的特征提取任務。

1.2 利用多模態數據特征和粒子濾波器檢測機器人目標

以提取的多模態數據特征為分析對象，構建粒子濾波器，通過粒子初始化、粒子更新、粒子擴散、粒子退化等環節，實現對移動機器人跟蹤目標的檢測。移動機器人目標多模態數據范圍內，粒子濾波器的連接結構[15]如圖2所示。

圖2 多模態數據范圍內粒子濾波器連接結構圖

在移動機器人目標檢測過程中，單個粒子濾波器的運行流程如圖3所示。

圖3 粒子濾波器運行流程圖

粒子濾波器通過一組帶權粒子樣本集對后驗概率密度進行估計，并不斷更新粒子樣本集以及粒子權值，從而實現了對目標狀態的準確估計。粒子濾波器中目標跟蹤模型狀態方程為：

Υt=Ψ(Υt-1，Φt-1，Κt-1)

(15)

式中，Υt-1為t-1時刻的目標狀態，Φt-1為t-1時刻的控制輸入，Κt-1為t-1時刻的過程噪聲。

粒子濾波通過對粒子運動模型進行建模，提取粒子特征，定義粒子與目標之間的相似性測度，并對下一幀粒子集進行預測，根據粒子集的候選區域，利用相似性測度，實現對目標的定位。在經過一段時間的追蹤之后，再將更多的粒子拋到最后一幀，從而提高了跟蹤的魯棒性[16]。在粒子濾波器運行過程中，首先對用于檢測目標的粒子進行初始化，在已知先驗概率分布的情況下，得到初始采樣粒子集。移動機器人目標跟蹤工作的目標多模態數據逐幀計算過程中，需要求解各個粒子的重要性權值，該參數決定了粒子的擴散和更新情況，粒子重要性權值的求解結果為：

(16)

(17)

其中：NL為粒子數量，若式(17)的計算結果低于閾值C0，則進行粒子重采樣操作，將權值大的粒子分解為多個粒子，替換權值小的粒子，否則無需進行粒子重采樣操作，重采樣后，所有粒子的權重是相同的。目標檢測工作中，粒子濾波器的執行過程也是粒子的尋優過程，將多模態數據范圍內粒子距離中心粒子的平均距離作為多樣性測度，研究粒子在高似然區域內的粒子數目，并結合粒子數目，對其數目進行控制，最終將粒子數目降低至一定的臨界值，進而利用減少的粒子進行粒子擴散，提高粒子的多樣性[18]。粒子濾波器執行過程中的粒子擴散原理如圖4所示。

圖4 粒子濾波器的粒子擴散原理圖

多模態數據范圍內粒子的擴散行為可以量化描述為：

(18)

(19)

將式(16)和式(18)的計算結果代入到式(19)中，即可得出目標狀態的預測估計結果[19]。按照上述方式可以得出多模態數據區域內所有粒子狀態的預測估計結果，將Pt(i)最大值對應的候選粒子作為當前時刻移動機器人目標的檢測結果。

1.3 實現移動機器人目標跟蹤

由于機器人以及目標處于實時移動狀態，因此需要利用采集的多模態數據和粒子濾波器對目標的實時位置進行更新，從而實現對目標的跟蹤。移動機器人在t+1時刻對目標的跟蹤結果為：

(20)

式中，max[]為最大值求解函數，pt+1為目標跟蹤節點。同理可以得出機器人移動狀態下對目標實時位置的跟蹤結果，通過多跟蹤點的融合，即可輸出移動機器人跟蹤得出的目標移動路線為：

rtarget={pt，pt+1，pt+2，…}

(21)

在實際的移動機器人目標跟蹤過程中，由于多模態數據的支持，因此在粒子濾波器執行過程中基本不會出現大批量粒子退化的情況，因此在此次研究中未考慮粒子退化的情況[20]。另外，在移動機器人視野范圍內存在的目標存在不唯一的情況，此時需要對多目標進行跟蹤，根據多個目標模態數據的采集結果，擴大粒子濾波器的擴散與搜索范圍，對跟蹤目標對應粒子進行分類，重復上述操作得出各個粒子對應目標的跟蹤結果，由此便可得出移動機器人多目標的跟蹤結果，并最終以可視化的形式輸出。

2 跟蹤性能仿真

為驗證設計的基于多模態數據和粒子濾波器的移動機器人目標跟蹤方法的跟蹤性能，采用白盒測試與對比測試相結合的方式，設計仿真測試，白盒測試的基本原理是提前設置移動機器人跟蹤目標的實時位置，以此作為仿真跟蹤結果，判斷設計方法輸出的跟蹤結果是否與設置數據一致，從而反應設計方法跟蹤性能的測試結果。而對比測試的基本原理則是：選擇多個現有的目標跟蹤方法作為仿真的對比方法，在相同的仿真場景下，對同一目標進行跟蹤，將對比方法得出的跟蹤數據作為仿真的對比項，以此驗證設計方法在跟蹤性能方面的優勢。

2.1 選擇移動機器人樣機

此次仿真選擇MIR型自主移動機器人作為執行對象，該移動機器人能夠實現自主移動，主要用于對道路交通與安全的監測。選擇的移動機器人樣機以IR2110作為驅動電機，移動機器人樣機內部裝設了超聲傳感器，能夠檢測出前方是否存在障礙物并進行躲避，避免移動機器人在移動過程中出現碰撞情況。為了保證移動機器人能夠成功接收移動指令與目標跟蹤指令，在移動機器人中內置無線收發MODEM芯片，可連接到計算機的RS232接口。從幾何形態方面來看，移動機器人的整機高度為1.7 m，為滿足多模態數據的采集工作，設置兩個視覺傳感器設備，傳感器的設置高度為1.5 m，兩傳感器之間的間隔距離為0.2 m，移動機器人的最大移動速度為35 km/h。在仿真開始前，首先需要規劃并控制移動機器人的移動路線，保證移動機器人的跟蹤目標在移動機器人的視野范圍內。

2.2 搭建目標跟蹤仿真環境

分別從移動機器人、跟蹤目標的實際移動環境和設計目標跟蹤方法的運行環境兩個方面，通過Matlab軟件構建移動機器人模型，并將攝像頭、激光雷達、聲音傳感器等添置至機器人模型中，根據接收到的多模態數據信息，通過粒子濾波器對目標進行定位和跟蹤。此次仿真選擇某市郊區的空曠馬路作為移動機器人和跟蹤目標的移動環境，設置的跟蹤目標為車輛，根據馬路中行駛車輛的數量，生成單目標場景和多目標場景，其中，單目標場景中只存在1輛車，而多目標場景中的車輛數量為2。為滿足移動機器人的目標跟蹤要求，要求車輛的行駛速度不高于30 km/h?；诙嗄B數據和粒子濾波器的移動機器人目標跟蹤方法采用DM6446評估板作為粒子濾波跟蹤算法實現的嵌入式平臺。DM6446是一種以DaVinci技術為基礎的下一代嵌入式處理器，是為了適應高效音視頻處理及編解碼需要而設計的，采用了ARM9+DSP的二核架構，擁有較高的外設控制能力及數據運算能力。

2.3 輸入粒子濾波器運行參數

為保證移動機器人目標跟蹤方法中粒子濾波器的正常運行，設置粒子濾波器中的初始粒子數量為200個，粒子濾波器通過增加粒子數來提高算法的跟蹤性能。在粒子濾波器運行過程中，設置粒子的初始權值和正則化參數分別為0.1和0.01。將上述運行參數的設置結果輸入到目標跟蹤方法的運行程序中，完成參數的設置與輸入工作。

2.4 生成移動機器人目標跟蹤任務

針對單目標和多目標兩個仿真場景，通過控制目標車輛的行駛參數，確定跟蹤目標的實際移動位置，以此作為移動機器人的目標跟蹤任務。部分目標跟蹤任務的生成情況如表1所示。

表1 移動機器人目標跟蹤任務表

仿真中，單目標場景與多目標場景的仿真環境相同，在該環境中構建坐標系，獲取跟蹤目標的實時位置坐標，以此作為判定移動機器人目標跟蹤精度的比對標準。

2.5 描述跟蹤性能測試仿真過程

在仿真開始前，利用編程工具對設計的基于多模態數據和粒子濾波器的移動機器人目標跟蹤方法進行編碼，保證主測計算機能夠直接運行對應的目標跟蹤程序。同時啟動移動機器人及其內部的傳感器設備，將目標跟蹤程序切換至運行狀態，啟動跟蹤目標車輛引擎。通過多模態數據采集與分析、粒子濾波等步驟，得出單目標和多目標場景下輸出的目標跟蹤結果。移動機器人目標跟蹤方法在單目標仿真場景下的目標跟蹤結果如圖5所示。

圖5 移動機器人目標跟蹤結果輸出

按照上述方式可以得出目標運行過程中任意時刻的目標跟蹤結果。仿真中設置的對比方法包括：基于自適應隨動機構的機器人目標跟蹤方法(文獻[1]方法)和基于遺傳迭代和模糊控制的機器人目標跟蹤方法(文獻[2]方法)，按照上述方式，在相同的仿真環境下完成對比方法的開發，得出相應的目標跟蹤結果。

2.6 設置跟蹤性能量化測試指標

此次仿真分別從跟蹤精度和跟蹤效率兩個方面測試設計方法的跟蹤性能，跟蹤精度的測試指標為跟蹤誤差，其數值結果為：

(22)

其中：(xtrack，ytrack)和(xreality，yreality)分別為移動機器人輸出的目標跟蹤結果以及跟蹤車輛目標的實際位置，計算得出跟蹤誤差越小，證明對應方法的跟蹤精度越高。另外，跟蹤效率的量化測試指標設置為跟蹤平均更新時延，該指標的測試結果為：

(23)

其中：ti和tt-1分別為第i和第i-1次的更新時間，nrenew為目標跟蹤結果的更新時間。計算得出跟蹤平均更新時延取值越小，證明對應方法的跟蹤效率越高。

2.7 跟蹤性能仿真結果與分析

2.7.1 單目標跟蹤性能

通過相關數據的統計，得出單目標場景下，移動機器人目標跟蹤方法的跟蹤精度性能測試結果，如表2所示。

表2 單目標場景下移動機器人目標跟蹤精度測試數據表

將表1和表2中的數據代入到式(22)中，得出基于自適應隨動機構的機器人目標跟蹤方法和基于遺傳迭代和模糊控制的機器人目標跟蹤方法的平均跟蹤誤差為10.75 m和6.50 m，而設計基于多模態數據和粒子濾波器的移動機器人目標跟蹤方法跟蹤誤差的平均值為1.13 m。另外，通過式(23)的計算，得出單目標場景下跟蹤效率的測試結果，如圖6所示。

圖6 單目標場景下移動機器人跟蹤效率測試結果

從圖6中可以直觀地看出，與基于自適應隨動機構的機器人目標跟蹤方法和基于遺傳迭代和模糊控制的機器人目標跟蹤方法相比，設計基于多模態數據和粒子濾波器的移動機器人目標跟蹤方法的跟蹤平均更新時延更短，即跟蹤效率更高。

2.7.2 多目標跟蹤性能

在多目標跟蹤仿真場景下，統計移動機器人輸出的目標跟蹤結果，得出反映目標跟蹤精度性能的測試結果，如圖7所示。

圖7 多目標場景下移動機器人目標跟蹤精度測試結果

從圖7中可以看出，設計方法在x和y方向上的跟蹤誤差均低于其他兩種方法，通過式(22)的計算，得出基于自適應隨動機構的機器人目標跟蹤方法、基于遺傳迭代和模糊控制的機器人目標跟蹤方法和設計方法的平均跟蹤誤差分別為19.8、11.8、2.2 m。利用式(23)計算得出多目標場景下的目標跟蹤效率結果，如圖8所示。

圖8 多目標場景下移動機器人目標跟蹤效率結果

通過對圖8中數據的分析，得出基于自適應隨動機構的機器人目標跟蹤方法、基于遺傳迭代和模糊控制的機器人目標跟蹤方法和設計方法的跟蹤平均更新時延分別為7.6、6.8、1.8 s。

3 結束語

目標跟蹤技術在軍用和民用領域均具有重大的應用價值。在實際應用中，目標跟蹤方法的優劣也會影響到跟蹤結果的穩定性與精確性。在此次研究中，在多模態數據和粒子濾波器的支持下，實現對移動機器人目標跟蹤方法的設計，從仿真結果中可以看出，設計方法的跟蹤精度達到預期效果，可以應用到實際項目工作中。