基于ROS目標(biāo)跟蹤移動(dòng)機(jī)器人設(shè)計(jì)與實(shí)驗(yàn)

金志熙 王宏遠(yuǎn) 勾尤旭 郝佳鑫

摘要：目標(biāo)跟蹤技術(shù)在機(jī)器人領(lǐng)域中占據(jù)非常重要的一部分，可應(yīng)用于日常生活、軍事工業(yè)等多個(gè)領(lǐng)域。該文采用改進(jìn)的KCF算法，基于ROS（Robot Operating System）搭建目標(biāo)跟蹤移動(dòng)機(jī)器人。該機(jī)器人主要由驅(qū)動(dòng)執(zhí)行系統(tǒng)、傳感系統(tǒng)和控制系統(tǒng)組成。利用Kinect深度攝像頭和hector_slam算法建立環(huán)境地圖，再利用KCF算法進(jìn)行目標(biāo)識(shí)別與跟蹤，獲取目標(biāo)位置，利用move_base功能包實(shí)現(xiàn)機(jī)器人的導(dǎo)航。

關(guān)鍵詞：目標(biāo)跟蹤技術(shù);KCF算法

中圖分類號(hào)：TP3? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2021）02-0001-03

近年來，由于機(jī)器人技術(shù)的高速發(fā)展，使得機(jī)器人進(jìn)入了我們生活中的方方面面，改變了我們的生活方式。當(dāng)機(jī)器人加上視覺傳感器實(shí)現(xiàn)目標(biāo)跟蹤功能，可以使得機(jī)器人的應(yīng)用更加的廣泛，可以完成更為復(fù)雜的任務(wù)，例如應(yīng)用于服務(wù)業(yè)的各種服務(wù)型機(jī)器人、應(yīng)用于物流管理的倉儲(chǔ)機(jī)器人、應(yīng)用于軍事領(lǐng)域的巡邏機(jī)器人等，都可以為我們的生活帶來更多的便利。

2014年，Henriques等人提出了一種精度高、速度快的運(yùn)動(dòng)目標(biāo)跟蹤算法，即KCF （Kernel Correlation Filter）算法[1]。但KCF算法也有不足，如提取外觀特征單一，不能自適應(yīng)尺度變化和模型更新因子，當(dāng)運(yùn)動(dòng)物體被嚴(yán)重遮擋的時(shí)候會(huì)導(dǎo)致跟蹤失敗[2]。針對(duì)這兩個(gè)不足，本文設(shè)計(jì)的移動(dòng)機(jī)器人的視覺跟蹤使用了改進(jìn)的KCF算法，該算法借鑒快速分類尺度空間跟蹤器（FDSST）進(jìn)行尺度估計(jì)，從而達(dá)到目標(biāo)尺寸的準(zhǔn)確估計(jì);同時(shí)采用Kalman濾波器，使用預(yù)測位置進(jìn)行跟蹤[3]。本文設(shè)計(jì)的移動(dòng)機(jī)器人是基于ROS平臺(tái)，相當(dāng)于一個(gè)“機(jī)器人框架”，是一個(gè)功能實(shí)現(xiàn)平臺(tái)。其兼容性強(qiáng)，支持多種編程語言接口，使得ROS系統(tǒng)中的代碼復(fù)用率高，為機(jī)器人操作系統(tǒng)的開發(fā)和后續(xù)維護(hù)提供了方便。在傳感系統(tǒng)方面，我們采用了Kinect深度攝像頭，可以在一定的范圍內(nèi)采集到深度信息，可替代一般的激光雷達(dá)完成地圖的建立;利用采集到的信息，使用改進(jìn)過的KCF算法，通過計(jì)算機(jī)的處理，得到跟蹤目標(biāo)的位置，實(shí)現(xiàn)移動(dòng)機(jī)器人目標(biāo)跟蹤的功能。

1 移動(dòng)機(jī)器人硬件部分

移動(dòng)機(jī)器人硬件部分的設(shè)計(jì)分為了傳感器層、執(zhí)行層和控制層。其中傳感器層包含Kinect深度攝像頭，完成圖像和深度位置信息的獲取工作。執(zhí)行層包括DC24V的直流電機(jī)、電機(jī)驅(qū)動(dòng)器和獲取里程計(jì)信息的編碼器。控制層包括Arduino控制板和PC機(jī)。Arduino控制板用于獲取編碼器信號(hào)來計(jì)算里程計(jì)信息，同時(shí)接收PC機(jī)的速度信息控制電機(jī)工作。PC機(jī)利用得到的圖像信息和深度信息，通過使用改進(jìn)過的KCF算法進(jìn)行目標(biāo)的追蹤，得出目標(biāo)的位置，然后運(yùn)用move_base功能包得出移動(dòng)機(jī)器人的速度，發(fā)送到Arduino控制板從而控制電機(jī)實(shí)現(xiàn)移動(dòng)機(jī)器人目標(biāo)跟蹤的工作。

本文的移動(dòng)機(jī)器人使用的是兩個(gè)驅(qū)動(dòng)輪的差動(dòng)輪式設(shè)計(jì)，由兩個(gè)DC24V的電機(jī)控制機(jī)器人運(yùn)動(dòng)，為實(shí)現(xiàn)機(jī)器人的運(yùn)動(dòng)控制，建立機(jī)器人差動(dòng)驅(qū)動(dòng)模型，機(jī)器人的幾何模型圖如圖1所示。

以中心點(diǎn)C為參考點(diǎn)描述下的機(jī)器人運(yùn)動(dòng)學(xué)建模如下：

2 移動(dòng)機(jī)器人軟件部分

ROS作為一個(gè)開源的機(jī)器人操作系統(tǒng)，它的運(yùn)行架構(gòu)是一種使用ROS通信模塊實(shí)現(xiàn)各個(gè)節(jié)點(diǎn)之間的通信，包括基于同步通信方式的服務(wù)、基于異步通信的話題等多種通信方式。它支持多種編程語言，方便了更多的使用者[4]，官方提供給開發(fā)者各種實(shí)用的工具和功能包，而且在網(wǎng)絡(luò)上還有全球機(jī)器人軟件開發(fā)者發(fā)布的特殊功能包[5]，提高了代碼的復(fù)用率，為機(jī)器人開發(fā)節(jié)約了不少時(shí)間和精力，提高了開發(fā)效率。根據(jù)設(shè)計(jì)要求，設(shè)計(jì)出了本文的移動(dòng)機(jī)器人系統(tǒng)的節(jié)點(diǎn)框圖，如圖2所示，可更直觀地了解系統(tǒng)中各個(gè)節(jié)點(diǎn)之間的通信關(guān)系。

本文設(shè)計(jì)的移動(dòng)機(jī)器人追蹤系統(tǒng)就是基于ROS平臺(tái)，利用Kinect深度攝像頭獲取節(jié)點(diǎn)云信息，利用hector_slam算法功能包建立環(huán)境的柵格地圖;利用Kinect獲取圖像和深度位置信息，使用改進(jìn)的KCF進(jìn)行對(duì)目標(biāo)的跟蹤，得出目標(biāo)位置，然后運(yùn)用導(dǎo)航功能包得出移動(dòng)機(jī)器人的速度，發(fā)送信息到Arduino控制功能包來控制機(jī)器人的移動(dòng)。目標(biāo)跟蹤這一部分的內(nèi)容在ROS自帶的gazebo物理仿真程序中進(jìn)行仿真。

3 改進(jìn)的KCF算法

3.1 KCF跟蹤算法

KCF全稱為Kernel Correlation Filter 核相關(guān)濾波算法，提出時(shí)轟動(dòng)一時(shí)，取得了巨大的成功，其核心步驟有采樣、訓(xùn)練、檢測和更新。為了增強(qiáng)核濾波器的穩(wěn)定器，需要大量的訓(xùn)練數(shù)據(jù)集，KCF算法采用目標(biāo)樣本循環(huán)位構(gòu)建訓(xùn)練數(shù)據(jù)集[xi（i∈{0，...，m-1}×{0，...，n-1}）]和跟蹤目標(biāo)標(biāo)簽[yi]。其中m和n為圖像x方向和y方向像素點(diǎn)的數(shù)量。

但是傳統(tǒng)的KCF算法不能根據(jù)跟蹤目標(biāo)的尺度變化對(duì)初始化跟蹤矩陣進(jìn)行大小改變使得模型的特征丟失、偏移，和當(dāng)目標(biāo)被嚴(yán)重遮擋會(huì)使模型被破壞，[3]這兩種情況都會(huì)使得跟蹤失敗。所以該算法需要進(jìn)行改進(jìn)。

3.2 KCF算法改進(jìn)

將尋找的對(duì)象進(jìn)行一定比例的放縮，生成M組目標(biāo)（本文的實(shí)驗(yàn)中M=33），使用3.1的基本KCF跟蹤算法來生成[wm（m=1，...，M）]，分別使用[wm]計(jì)算最大響應(yīng)位置。取[max（wmX）]，作為計(jì)算結(jié)果，得出目標(biāo)所在位置以及目標(biāo)縮放比例。

3.3 卡爾曼濾波算法

卡爾曼濾波是由消息過程、測量過程和濾波過程組成，是一套線性無偏最小均方誤差的遞推公式，該濾波算法利用前一刻的估計(jì)值和當(dāng)前時(shí)刻的觀測值，進(jìn)行預(yù)測當(dāng)前時(shí)刻目標(biāo)出現(xiàn)的位置。

通過結(jié)合該算法，可解決當(dāng)跟蹤目標(biāo)被嚴(yán)重遮擋時(shí)跟蹤失敗的問題。

3.4 改進(jìn)KCF整體流程

針對(duì)KCF算法難以應(yīng)對(duì)尺寸變化、目標(biāo)完全遮擋等問題，使用基于改進(jìn)核相關(guān)濾波的運(yùn)動(dòng)目標(biāo)跟蹤算法。

首先融合FDSST算法的尺度濾波器，增強(qiáng)算法對(duì)目標(biāo)尺寸變化的適應(yīng)能力。然后融合卡爾曼濾波器對(duì)人物目標(biāo)下一幀的位置進(jìn)行預(yù)測，結(jié)合遮擋判斷，當(dāng)目標(biāo)被遮擋時(shí)，停止KCF算法的模型更新和尺度更新，使用卡爾曼濾波的預(yù)測位置進(jìn)行跟蹤，當(dāng)目標(biāo)重新回到視野當(dāng)中時(shí)，繼續(xù)使用KCF算法進(jìn)行跟蹤。

改進(jìn)的KCF目標(biāo)跟蹤算法的流程圖如圖3所示。

4 視覺跟隨工作流程

本文的視覺跟隨移動(dòng)機(jī)器人基于ROS平臺(tái)，各部分的工作由不同的節(jié)點(diǎn)完成，各節(jié)點(diǎn)通過通信發(fā)布信息或訂閱信息完成整體的視覺跟隨工作。具體步驟如下：

第一步：在系統(tǒng)中訂閱Kinect深度攝像頭發(fā)布的色彩圖像話題，使得在筆記本屏幕上顯示當(dāng)前的攝像頭拍攝的視頻，然后使用鼠標(biāo)對(duì)跟蹤目標(biāo)進(jìn)行框選，獲取框的坐標(biāo)信息和圖像信息，使目標(biāo)跟蹤算法進(jìn)行初始化;

第二步：利用攝像頭發(fā)布的圖像信息，目標(biāo)跟蹤算法開始工作。為了實(shí)現(xiàn)機(jī)器人移動(dòng)追蹤功能，需要使用一個(gè)話題將圖像的深度信息和框坐標(biāo)信息進(jìn)行分析后獲得的坐標(biāo)位置進(jìn)行發(fā)布。對(duì)攝像頭發(fā)布的每一幀圖像重復(fù)執(zhí)行第二步的操作;

第三步：為了實(shí)現(xiàn)機(jī)器人的跟蹤導(dǎo)航及避障移動(dòng)，需要第二步中話題發(fā)布的位置信息，當(dāng)move_base功能包獲取位置信息后，會(huì)進(jìn)行全局路徑規(guī)劃（global planner）和本地實(shí)時(shí)規(guī)劃（local planner），最后得出機(jī)器人所需的移動(dòng)速度和方向并進(jìn)行發(fā)布;

第四步：對(duì)于移動(dòng)機(jī)器人的執(zhí)行部分，會(huì)由ROS_arduino_bridge功能包進(jìn)行控制。該功能包接收move_base功能包發(fā)布的速度信息，進(jìn)行對(duì)電機(jī)控制，使得機(jī)器人進(jìn)行移動(dòng)。

5 實(shí)驗(yàn)結(jié)果

圖4為選擇背景圖片和目標(biāo)對(duì)象。

圖5為正常的跟蹤所執(zhí)行的效果，正常捕捉到了目標(biāo)對(duì)象。

圖6為我們將目標(biāo)遠(yuǎn)離攝像頭時(shí)，改進(jìn)的KCF同樣正常的捕捉了目標(biāo)對(duì)象，同時(shí)捕捉框變小，和目標(biāo)大小基本一致，實(shí)驗(yàn)效果良好。

圖7為我們將目標(biāo)靠近攝像頭時(shí)，改進(jìn)的KCF同樣正常的捕捉了目標(biāo)對(duì)象，同時(shí)捕捉框變大，和目標(biāo)大小基本一致，實(shí)驗(yàn)效果良好。

6 結(jié)論

本文設(shè)計(jì)的目標(biāo)跟蹤移動(dòng)機(jī)器人，利用Kinect深度攝像頭進(jìn)行數(shù)據(jù)采集，使用改進(jìn)的KCF跟蹤算法對(duì)目標(biāo)進(jìn)行追蹤，利用move_base進(jìn)行路徑的規(guī)劃，完成目標(biāo)跟蹤的功能。在仿真實(shí)驗(yàn)中，本文設(shè)計(jì)的目標(biāo)跟蹤機(jī)器在攝像頭跟蹤效果中可以很好地完成對(duì)目標(biāo)的跟蹤，實(shí)現(xiàn)了預(yù)期的目標(biāo)任務(wù)。目前完成了機(jī)器人的仿真實(shí)驗(yàn)，接下來就對(duì)機(jī)器人進(jìn)行實(shí)物的搭建，完成實(shí)物的組裝，繼續(xù)改進(jìn)。

參考文獻(xiàn)：

[1] Henriques Jo?o F， Caseiro Rui， Martins Pedro， et al. High-Speed Tracking with Kernelized Correlation Filters，2015， 37（3）：583-596.

[2] 周正松，陳虹君，周紅.基于多特征融合的尺度自適應(yīng)KCF目標(biāo)跟蹤算法[J].四川大學(xué)學(xué)報(bào)（自然科學(xué)版），2020，57（4）：697-703.

[3] 王林，胥中南.改進(jìn)的KCF算法在車輛跟蹤中的應(yīng)用[J].計(jì)算機(jī)測量與控制，2019，27（7）：195-199.

[4] 曹正萬，平雪良，陳盛龍，等.基于ROS的機(jī)器人模型構(gòu)建方法研究[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2015（8）：51-54.

[5] 李賀喜，李富強(qiáng)，牛童立，等.基于ROS系統(tǒng)移動(dòng)機(jī)器人SLAM算法的研究與實(shí)現(xiàn)[J].技術(shù)與市場，2020，27（7）：97，99.

【通聯(lián)編輯：代影】