楊 森 張壽明 劉亦興 刁子健 趙宇豪

（昆明理工大學(xué)信息工程與自動(dòng)化學(xué)院）

一般情況下，智能小車可在未知環(huán)境下自主運(yùn)動(dòng)，感知探究并適應(yīng)環(huán)境，進(jìn)而對(duì)目標(biāo)進(jìn)行跟蹤與追尋。

使用計(jì)算機(jī)視覺技術(shù)解決智能小車環(huán)境感知問題有多種方法，其中，同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）方法［1］憑借著精度高、實(shí)時(shí)性好及理論依據(jù)充足等優(yōu)點(diǎn)被廣泛研究和發(fā)展。 視覺SLAM有很多種，如 單 目 視 覺SLAM［2］、雙 目 視 覺SLAM 及RGB-D（red-green-blue-depth）SLAM［3～5］等。RGB-D獲取像素距離很簡(jiǎn)單，因此，在計(jì)算量和算法復(fù)雜度上有非常大的優(yōu)勢(shì)。

目標(biāo)檢測(cè)是指從不同復(fù)雜程度的背景中辨識(shí)出運(yùn)動(dòng)目標(biāo)并分離背景，從而完成跟蹤、識(shí)別等后續(xù)任務(wù)［6］，從最開始的靜態(tài)目標(biāo)檢測(cè)到目前被廣泛使用的動(dòng)態(tài)目標(biāo)檢測(cè)，從使用建模方法對(duì)圖像檢測(cè)問題描述并解決到通過人工智能和機(jī)器學(xué)習(xí)來解決辨別問題［7］。 路徑規(guī)劃問題的關(guān)鍵在于優(yōu)化路徑，以求得最小的代價(jià)［8］，A*搜索算法（A-star Search Algorithm）是一種常見且應(yīng)用廣泛的圖搜索和尋徑算法，通過使用啟發(fā)式函數(shù)來指導(dǎo)尋徑，從而保證高效地找到一條最優(yōu)路徑［9］。

筆者選用RGB-D SLAM方法實(shí)現(xiàn)室內(nèi)環(huán)境全局地圖的建立與小車的實(shí)時(shí)定位， 用改進(jìn)后的ORB（Oriented FAST and Rotated BRIEF）特征提取算法檢測(cè)特定目標(biāo)，再用A*算法實(shí)現(xiàn)避障與路徑規(guī)劃，以實(shí)現(xiàn)智能小車在室內(nèi)環(huán)境中對(duì)特點(diǎn)目標(biāo)進(jìn)行跟蹤，在跟蹤丟失時(shí)能夠自主制定導(dǎo)航策略并重新搜尋目標(biāo)。

1 視覺SLAM方法研究

1.1 相機(jī)模型與圖像特征

1.1.1 相機(jī)模型

相機(jī)模型用于描述圖像傳感器的感知數(shù)學(xué)模型，并且最終因?yàn)橥哥R的存在，在投影時(shí)，點(diǎn)與點(diǎn)間的投影產(chǎn)生畸變。 為了便于研究與分析，筆者采用針孔相機(jī)模型與畸變模型來描述這種映射關(guān)系。

1.1.2 針孔相機(jī)模型

現(xiàn)實(shí)世界的空間點(diǎn)Ρ， 經(jīng)過相機(jī)光心Ο投影后，落在物理成像平面上，成像為Ρ′。 針孔相機(jī)模型如圖1所示。

圖1 針孔相機(jī)模型示意圖

針孔相機(jī)模型中涉及4個(gè)坐標(biāo)系： 世界坐標(biāo)系、相機(jī)坐標(biāo)系、圖像坐標(biāo)系和像素坐標(biāo)系。 假設(shè)點(diǎn)Ρ在世界坐標(biāo)系下的齊次坐標(biāo)ΡW為（xW，yW，zW，1）， 經(jīng)過旋轉(zhuǎn)變換和平移變換后可得點(diǎn)Ρ在相機(jī)坐標(biāo)系下的坐標(biāo)，再根據(jù)光學(xué)投影關(guān)系可以獲取成像點(diǎn)在圖像坐標(biāo)系中的坐標(biāo)，最后經(jīng)過采樣和量化得到對(duì)應(yīng)的像素坐標(biāo)系下的齊次坐標(biāo)為（u，v，1）。 完整的針孔相機(jī)模型［10］可表示為：

其中，s為縮放因子；K為相機(jī)的內(nèi)參矩陣，包含相機(jī)的固有屬性參數(shù)［11］；T為相機(jī)的外參矩陣，包含相機(jī)位姿變換的旋轉(zhuǎn)矩陣R″和平移向量t；fx，fy，cx，cy表示相機(jī)的內(nèi)參，在制造相機(jī)時(shí)已經(jīng)確定。

1.1.3 畸變模型

濾鏡并不嚴(yán)格與成像平面平行，導(dǎo)致圖像產(chǎn)生畸變。 徑向畸變可用多項(xiàng)式矯正：

其中，xrad，yrad是徑向畸變前的位置坐標(biāo)；x，y是橫縱坐標(biāo)；k1、k2、k3是五維向量；r2=x2+y2。

切向畸變的矯正：

其中，xtan，ytan是切向畸變前的位置坐標(biāo)；p1，p2是五維向量。

綜合以上兩種畸變， 得到相機(jī)的畸變模型，即去畸變后的圖像像素坐標(biāo)系的坐標(biāo)：

1.2 圖像特征提取與匹配

特征點(diǎn)的信息包括關(guān)鍵點(diǎn)（Key-Point）和描述子（Descriptor）。 最經(jīng)典的尺度不變特征（Scale Invariant Feature Transform，SIFT） 巨大的計(jì)算量普通計(jì)算機(jī)和實(shí)時(shí)系統(tǒng)難以接受。 改進(jìn)后的ORB特征提取算法［12］使用FAST關(guān)鍵點(diǎn)，比起SIFT特征適當(dāng)降低了精度和健壯性，但是很大程度地提升了性能。 ORB特征提取算法的特點(diǎn)是速度快，在添加了尺度和旋轉(zhuǎn)的描述后在一定程度上不受圖像噪聲像素和圖像變換的影響。

FAST關(guān)鍵點(diǎn)屬于角點(diǎn)，依靠檢測(cè)明暗變化明顯的小區(qū)域而得，F(xiàn)AST檢測(cè)的基本思想是： 如果一個(gè)像素與臨近區(qū)域的像素灰度數(shù)值差別較大，那它可能是一個(gè)角點(diǎn)。 其算法過程如下：

a. 在圖像中選定像素Q，計(jì)算其亮度IQ；

b. 設(shè)置一定的閾值T′；

c. 將Q作為中心，選取其距離中心點(diǎn)距離為3的周圍16個(gè)像素點(diǎn)；

d. 如果其16個(gè)像素點(diǎn)中的9個(gè)點(diǎn)的亮度全部大于IQ+T′或者全部小于IQ-T′，那么判定Q為特征點(diǎn)，此點(diǎn)則為FAST-9特征點(diǎn)；

e. 對(duì)圖像中每一個(gè)點(diǎn)執(zhí)行上述操作，并忽略距離邊緣像素3個(gè)像素單位以下的像素點(diǎn)。

FAST關(guān)鍵點(diǎn)中存在尺度問題，相機(jī)距離更近或者更遠(yuǎn)時(shí)， 角點(diǎn)的特征將發(fā)生尺度縮放變換，從而產(chǎn)生無法重新識(shí)別的問題。 所以針對(duì)尺度和方向問題，ORB特征添加了對(duì)尺度變換和旋轉(zhuǎn)變換的描述， 從而實(shí)現(xiàn)特征具有旋轉(zhuǎn)和尺度不變性。 對(duì)于尺度不變性，ORB特征使用圖像金字塔；對(duì)于旋轉(zhuǎn)不變性， 加入灰度質(zhì)心法（Intensity Centroid）：

a. 取一關(guān)鍵點(diǎn)圖像區(qū)域B， 定義此圖像塊的矩med=xeydI（x，y），e，d={0，1}，x，y表示像素坐標(biāo)，I（x，y）表示此像素坐標(biāo)的灰度值；

d. 針對(duì)每一關(guān)鍵點(diǎn)進(jìn)行上述步驟。

BRIEF是一種特征描述子， 對(duì)已經(jīng)檢測(cè)到的特征進(jìn)行描述，是一種二進(jìn)制編碼描述子。 經(jīng)過改進(jìn)的FAST關(guān)鍵點(diǎn)也彌補(bǔ)了描述子不具旋轉(zhuǎn)不變的缺點(diǎn)。 ORB特征提取如圖2所示。

圖2 ORB特征提取

匹配點(diǎn)數(shù)量過多時(shí)， 快速近似最鄰近（Fast Library for Approximate Nearest Neighbors，F(xiàn)LANN）算法［13，14］能大幅提升匹配速度。

設(shè)當(dāng)前幀中的特征點(diǎn)描述子X=（x1，x2，…，xn），上一幀Y=（y1，y2，…，yn），則兩個(gè)特征點(diǎn)間的歐氏距離定義如下：

相鄰幀間的特征匹配如圖3所示。

圖3 相鄰幀間的特征匹配

由于ORB特征采用BRIEF描述子描述特征點(diǎn)信息，每個(gè)特征點(diǎn)的信息是128維的二值向量，層次聚類樹匹配方式將會(huì)更加有效， 算法流程如下：

a. 在當(dāng)前圖像的特征點(diǎn)中建立樹，針對(duì)每一分支隨機(jī)選一個(gè)中心；

b. 從Root開始遍歷，計(jì)算當(dāng)前特征點(diǎn)與上一張圖像特征點(diǎn)的歐氏距離；

c. 計(jì)算Root下所有葉子節(jié)點(diǎn)與上一張圖像特征間的歐氏距離，記錄其中距離最小的節(jié)點(diǎn)L；

d. 若L沒有葉子節(jié)點(diǎn)，使L指向Root，聚類結(jié)束；

e. 若L有葉子節(jié)點(diǎn)，迭代Root為L(zhǎng)并跳轉(zhuǎn)到步驟c；

f. 若聚類結(jié)束，Root即為匹配點(diǎn)；

g. 對(duì)上一張圖像所有特征點(diǎn)進(jìn)行步驟a～f。

1.3 相機(jī)運(yùn)動(dòng)估計(jì)

利用兩幀像素點(diǎn)間的對(duì)應(yīng)，恢復(fù)出兩幀間相機(jī)的運(yùn)動(dòng)。 通過深度相機(jī)能取到相機(jī)與每個(gè)像素的距離，所以運(yùn)動(dòng)估計(jì)問題具體如下。

假設(shè)通過圖像的RGB信息和Depth信息得到了一組匹配成功的3D點(diǎn)：

其中，A和A′是相鄰兩幀圖像中匹配成功的3D點(diǎn)的集合，要想求得相機(jī)的運(yùn)動(dòng)，即找到一個(gè)歐氏變換R′，t′使得：

此問題為ICP（Iterative Closest Point）問題。使用 奇異值分 解 （Singular Value Decomposition，SVD）進(jìn)行代數(shù)求解后進(jìn)行ICP過程：

c. 根據(jù)R′計(jì)算t′，t*=a-R′a′。

相鄰兩幀色彩圖像和相鄰兩幀深度圖像分別如圖4、5所示。

圖4 相鄰兩幀色彩圖像

圖5 相鄰兩幀深度圖像

2 目標(biāo)識(shí)別理論與算法研究

2.1 顏色閾值分割

RGB顏色空間是一種人類理解較為直觀的顏色模型，采用RGB888格式存儲(chǔ)數(shù)據(jù)。 HSV顏色空間在人們的視覺中表現(xiàn)比較直觀，因此選用此模型對(duì)圖像進(jìn)行處理。 因相機(jī)數(shù)據(jù)為RGB888格式，而根據(jù)人視覺圖像處理的直觀性，需使用HSV色彩空間描述像素，因此需要轉(zhuǎn)換顏色空間。

假設(shè)RGB顏色空間描述的某個(gè)像素的值A(chǔ)″u，v=（R，G，B）， 此像素對(duì)應(yīng)的HSV顏色空間描述的像素點(diǎn)P ″u，v=（H，S，V）。 二者的轉(zhuǎn)換關(guān)系如下：

RGB顏色（A）空間轉(zhuǎn)換到HSV顏色（B）空間如圖6所示。

圖6 色彩空間轉(zhuǎn)換

設(shè)HSV模型下圖像的像素值為f（u，v），分割特征的低閾值和高閾值為D和D′，RGB模型下分割圖像后的像素點(diǎn)值為g（u，v），于是有：

閾值分割如圖7所示。

圖7 閾值分割

2.2 邊緣檢測(cè)

針對(duì)經(jīng)過閾值分割后的二值圖像，圖像的腐蝕用來濾除較小的圖像區(qū)域的干擾，圖像的膨脹用來恢復(fù)腐蝕后的圖像。

圖像的膨脹運(yùn)算為⊕，設(shè)a為一幅二值圖像，使用卷積模板b來對(duì)a圖像進(jìn)行膨脹，執(zhí)行該操作時(shí)，有：

圖像的腐蝕運(yùn)算為-，同上，執(zhí)行圖像腐蝕操作時(shí)，有：

腐蝕與膨脹對(duì)像素體積小于模板的黑色區(qū)域進(jìn)行剔除，對(duì)目標(biāo)圖像進(jìn)行腐蝕與膨脹，從而使得目標(biāo)特征更加明顯。 圖像腐蝕與膨脹如圖8所示。

圖8 圖像腐蝕與膨脹

圖像中的邊緣信息主要集中在高頻段。 在對(duì)目標(biāo)色彩進(jìn)行閾值分割、腐蝕和膨脹后，高頻段的噪聲將顯著減小，圖像的信息將變得更加利于使用，因此使用Canny算法檢測(cè)邊緣［15］。 該算法檢測(cè)的步驟是：

a. 對(duì)圖像進(jìn)行高斯模糊；

b. 計(jì)算梯度；

c. 非極大值抑制；

d. 連接邊緣。

根據(jù)邊界跟蹤算法來對(duì)邊緣點(diǎn)進(jìn)行最終篩選。 如果弱邊緣點(diǎn)和強(qiáng)邊緣點(diǎn)相連，則均保存，否則刪除弱邊緣點(diǎn)。 算法從第1個(gè)點(diǎn)開始搜索：

a. 如果此點(diǎn)為弱邊界且沒有被標(biāo)記，將其標(biāo)記放入棧與隊(duì)列中并進(jìn)入下一步，若此點(diǎn)已經(jīng)被標(biāo)記或者不是弱邊界，則重復(fù)此過程以遍歷整幅圖像。

b. 從棧中取出元素并遍歷周圍的8個(gè)點(diǎn)，如果其中一像素點(diǎn)是弱邊緣且沒有被標(biāo)記過，則標(biāo)記這個(gè)像素點(diǎn)，并加入棧與隊(duì)列中。 如果其中一個(gè)像素點(diǎn)是強(qiáng)邊緣，則表示此弱邊緣點(diǎn)和強(qiáng)邊緣點(diǎn)連通，保留標(biāo)記。 重復(fù)此過程，以遍歷棧中的所有元素， 若是不存在強(qiáng)邊緣點(diǎn)則清空棧與隊(duì)列，然后使用下一個(gè)像素執(zhí)行步驟a。

高斯模糊與邊緣檢測(cè)如圖9所示。

圖9 高斯模糊與邊緣檢測(cè)

2.3 位姿估計(jì)濾波器

其中，狀態(tài)轉(zhuǎn)換矩陣M將系統(tǒng)因果體現(xiàn)出來，在這里M陣為一個(gè)兩行兩列的常數(shù)陣。

圖像中目標(biāo)的運(yùn)動(dòng)是任意的， 所以在x軸方向和y軸方向上不存在耦合關(guān)系，故：

進(jìn)行以下迭代步驟以預(yù)測(cè)：

濾波器輸入與濾波器效果分別如圖10、11所示。

圖10 濾波器輸入

圖11 濾波器效果

3 簡(jiǎn)單色彩目標(biāo)追蹤的實(shí)現(xiàn)研究

3.1 基于ROS的小車導(dǎo)航

要在ROS（Robot Operating System）上實(shí)現(xiàn)小車的導(dǎo)航，就必須研究ROS提供的接口［16］。ROS導(dǎo)航的基本框架，即navigation框架，其中最主要的是Move_base節(jié)點(diǎn)，簡(jiǎn)化該節(jié)點(diǎn)的輸入輸出如圖12所示， 其中，Map是之前建立好的室內(nèi)環(huán)境的地圖；Point Cloud是RGB-D相機(jī)的信息；Odometry即里程計(jì)， 是使用車輪上的編碼器來實(shí)現(xiàn)的；Transforms表示小車主體、相機(jī)和車輪之間的中心坐標(biāo)變換關(guān)系；Localization即定位信息；Goal Pose是要設(shè)定的目標(biāo)點(diǎn)； 輸出信息為Velocity Commands，即速度控制信息，這部分輸出信息直接發(fā)送給小車底層的驅(qū)動(dòng)板用于驅(qū)動(dòng)小車運(yùn)動(dòng)。

圖12 Move_base輸入輸出特性

3.2 路徑規(guī)劃

A*算法根據(jù)代價(jià)函數(shù)優(yōu)化，將搜索區(qū)域劃分成簡(jiǎn)單的二維向量，向量中每個(gè)元素對(duì)應(yīng)一個(gè)搜索區(qū)域節(jié)點(diǎn)，使用啟發(fā)函數(shù)來計(jì)算路徑代價(jià)并優(yōu)化以降低此代價(jià)。 A*算法演示如圖13所示。

圖13 A*算法演示

A*算法的具體流程如下：

a. 將起點(diǎn)加入集合O，O用于存放待檢測(cè)的搜索區(qū)域節(jié)點(diǎn)， 而集合C用來存放已經(jīng)檢測(cè)過的搜索區(qū)域節(jié)點(diǎn)。

b. 對(duì)每一個(gè)O中的節(jié)點(diǎn)pi（i=1，2，…，n）檢查引起最小的F數(shù)值的子節(jié)點(diǎn)pi， 將子該節(jié)點(diǎn)pi移動(dòng)到C中。

c. 對(duì)pi周圍的8個(gè)節(jié)點(diǎn)pq（q=1，2，…，8）分別檢查。 若不可抵達(dá)或在C中則忽略。 在其他情況下， 若不在O中則加入O中并將pi設(shè)置為pn的父節(jié)點(diǎn)；若已在O中，檢測(cè)pi到pn的F值是否更小，如果更近則設(shè)置pn的父節(jié)點(diǎn)為pi，并重新計(jì)算I和F值。

d. 若將終點(diǎn)放到了O中，則算法結(jié)束。若查找重點(diǎn)失敗且O為空，則算法結(jié)束。

e. 從終點(diǎn)搜索區(qū)域節(jié)點(diǎn)開始，沿著指向父節(jié)點(diǎn)的路徑一直到初始搜索節(jié)點(diǎn)，即為最優(yōu)路徑。

其中F=I+J，I表示歷史代價(jià)；J表示啟發(fā)函數(shù)，可使用歐氏距離。

ROS 中 主 要 使 用 動(dòng) 態(tài) 窗 口 法［17］（Dynamic Window Approach，DWA）， 使用多種速度模擬一段時(shí)間得到軌跡，對(duì)軌跡進(jìn)行評(píng)價(jià)，選取最優(yōu)的軌跡對(duì)應(yīng)的速度來驅(qū)動(dòng)小車運(yùn)行。 步驟如下：

a. 建立小車的運(yùn)動(dòng)模型；

b. 建立速度采樣模型；

c. 評(píng)價(jià)函數(shù)。

DWA算法中使用的評(píng)價(jià)函數(shù)為：

其中，heading（v′，w′）為當(dāng)前軌跡結(jié)束時(shí)智能小車與目標(biāo)角度差；dist（v′，w′）為當(dāng)前軌跡與障礙物的距離；velocity（v′，w′）為當(dāng)前軌跡的速度；σ，α，β，γ為常數(shù)。

DWA局部路徑效果如圖14所示， 圖14a為DWA算法在阻礙區(qū)域進(jìn)行局部路徑規(guī)劃的結(jié)果，圖14b為DWA算法在開闊區(qū)域進(jìn)行局部路徑規(guī)劃的結(jié)果。 其中，*表示半徑為0.3的障礙物。

圖14 DWA局部路徑效果

3.3 點(diǎn)巡航仿真實(shí)驗(yàn)

根據(jù)ORB SLAM建立的稀疏地圖如圖15所示，其中標(biāo)點(diǎn)為環(huán)境的角點(diǎn)特征。

圖15 定位與建圖過程中的特征提取匹配

假設(shè)地圖在之前的過程中已經(jīng)建好，二維地圖如圖16所示。

圖16 完整地圖

根據(jù)小車的傳感器信息和定位信息，設(shè)定小車在地圖上6個(gè)點(diǎn)之間導(dǎo)航，小車按照6個(gè)定點(diǎn)進(jìn)行巡航，用于搜索目標(biāo)，巡航定點(diǎn)應(yīng)設(shè)置在視野開闊處，由路徑規(guī)劃控制器輸出導(dǎo)航信息。 使用Rviz數(shù)據(jù)可視化工具可查看小車導(dǎo)航和路徑規(guī)劃的情況。 在全局目標(biāo)的規(guī)劃下，導(dǎo)航算法已經(jīng)開始為小車規(guī)劃好了全局路徑，而圖17中小車前的黑線則為局部路徑規(guī)劃算法所做的模擬軌跡，最終，算法會(huì)選擇一條評(píng)價(jià)函數(shù)最優(yōu)的軌跡輸出。

圖17 局部路徑規(guī)劃軌跡模擬

3.4 目標(biāo)追蹤仿真

3.4.1 識(shí)別算法檢測(cè)

圖18a為相機(jī)圖像， 進(jìn)行邊緣檢測(cè)后得到的結(jié)果如圖18b所示，可以看出，此時(shí)目標(biāo)的特征已經(jīng)十分突出。

圖18 目標(biāo)識(shí)別

3.4.2 目標(biāo)追蹤檢測(cè)

有了理論依據(jù)和小車自主導(dǎo)航的實(shí)現(xiàn)，只需設(shè)置一個(gè)節(jié)點(diǎn)運(yùn)行目標(biāo)檢測(cè)跟蹤算法，便可實(shí)現(xiàn)簡(jiǎn)單色彩目標(biāo)跟蹤。 運(yùn)行效果如圖19所示，可以看出，簡(jiǎn)單色彩目標(biāo)在視野中已經(jīng)被智能小車識(shí)別到。

圖19 追蹤小車演示

4 結(jié)束語

針對(duì)室內(nèi)智能小車對(duì)色彩目標(biāo)追蹤的任務(wù)，在對(duì)SLAM問題研究的基礎(chǔ)上， 提出使用RGB-D相機(jī)基于ORB特征構(gòu)建SLAM，用于定位與建圖，并對(duì)檢測(cè)結(jié)果進(jìn)行估計(jì)，最后根據(jù)檢測(cè)算法定位信息，實(shí)現(xiàn)智能小車對(duì)室內(nèi)場(chǎng)景下色彩目標(biāo)的識(shí)別與追蹤。 實(shí)驗(yàn)結(jié)果表明，將圖像檢測(cè)算法與路徑規(guī)劃算法用在基于ROS的智能小車上， 可以實(shí)現(xiàn)追蹤與跟隨色彩目標(biāo)。 基于同步定位與地圖構(gòu)建的室內(nèi)色彩追蹤智能小車可以代替人類探索危險(xiǎn)未知的陌生室內(nèi)環(huán)境，并對(duì)該室內(nèi)陌生環(huán)境中有簡(jiǎn)單色彩的事物進(jìn)行追蹤與跟隨，這對(duì)于有效保障人類自身的安全和探索未知環(huán)境具有一定價(jià)值。