基于同步定位與地圖構建的色彩追蹤智能小車系統

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

（昆明理工大學信息工程與自動化學院）

一般情況下，智能小車可在未知環境下自主運動，感知探究并適應環境，進而對目標進行跟蹤與追尋。

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

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

筆者選用RGB-D SLAM方法實現室內環境全局地圖的建立與小車的實時定位， 用改進后的ORB（Oriented FAST and Rotated BRIEF）特征提取算法檢測特定目標，再用A*算法實現避障與路徑規劃，以實現智能小車在室內環境中對特點目標進行跟蹤，在跟蹤丟失時能夠自主制定導航策略并重新搜尋目標。

1 視覺SLAM方法研究

1.1 相機模型與圖像特征

1.1.1 相機模型

相機模型用于描述圖像傳感器的感知數學模型，并且最終因為透鏡的存在，在投影時，點與點間的投影產生畸變。 為了便于研究與分析，筆者采用針孔相機模型與畸變模型來描述這種映射關系。

1.1.2 針孔相機模型

現實世界的空間點Ρ， 經過相機光心Ο投影后，落在物理成像平面上，成像為Ρ′。 針孔相機模型如圖1所示。

圖1 針孔相機模型示意圖

針孔相機模型中涉及4個坐標系： 世界坐標系、相機坐標系、圖像坐標系和像素坐標系。 假設點Ρ在世界坐標系下的齊次坐標ΡW為（xW，yW，zW，1）， 經過旋轉變換和平移變換后可得點Ρ在相機坐標系下的坐標，再根據光學投影關系可以獲取成像點在圖像坐標系中的坐標，最后經過采樣和量化得到對應的像素坐標系下的齊次坐標為（u，v，1）。 完整的針孔相機模型［10］可表示為：

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

1.1.3 畸變模型

濾鏡并不嚴格與成像平面平行，導致圖像產生畸變。 徑向畸變可用多項式矯正：

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

切向畸變的矯正：

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

綜合以上兩種畸變， 得到相機的畸變模型，即去畸變后的圖像像素坐標系的坐標：

1.2 圖像特征提取與匹配

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

FAST關鍵點屬于角點，依靠檢測明暗變化明顯的小區域而得，FAST檢測的基本思想是： 如果一個像素與臨近區域的像素灰度數值差別較大，那它可能是一個角點。 其算法過程如下：

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

b. 設置一定的閾值T′；

c. 將Q作為中心，選取其距離中心點距離為3的周圍16個像素點；

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

e. 對圖像中每一個點執行上述操作，并忽略距離邊緣像素3個像素單位以下的像素點。

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

a. 取一關鍵點圖像區域B， 定義此圖像塊的矩med=xeydI（x，y），e，d={0，1}，x，y表示像素坐標，I（x，y）表示此像素坐標的灰度值；

d. 針對每一關鍵點進行上述步驟。

BRIEF是一種特征描述子， 對已經檢測到的特征進行描述，是一種二進制編碼描述子。 經過改進的FAST關鍵點也彌補了描述子不具旋轉不變的缺點。 ORB特征提取如圖2所示。

圖2 ORB特征提取

匹配點數量過多時， 快速近似最鄰近（Fast Library for Approximate Nearest Neighbors，FLANN）算法［13，14］能大幅提升匹配速度。

設當前幀中的特征點描述子X=（x1，x2，…，xn），上一幀Y=（y1，y2，…，yn），則兩個特征點間的歐氏距離定義如下：

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

圖3 相鄰幀間的特征匹配

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

a. 在當前圖像的特征點中建立樹，針對每一分支隨機選一個中心；

b. 從Root開始遍歷，計算當前特征點與上一張圖像特征點的歐氏距離；

c. 計算Root下所有葉子節點與上一張圖像特征間的歐氏距離，記錄其中距離最小的節點L；

d. 若L沒有葉子節點，使L指向Root，聚類結束；

e. 若L有葉子節點，迭代Root為L并跳轉到步驟c；

f. 若聚類結束，Root即為匹配點；

g. 對上一張圖像所有特征點進行步驟a～f。

1.3 相機運動估計

利用兩幀像素點間的對應，恢復出兩幀間相機的運動。 通過深度相機能取到相機與每個像素的距離，所以運動估計問題具體如下。

假設通過圖像的RGB信息和Depth信息得到了一組匹配成功的3D點：

其中，A和A′是相鄰兩幀圖像中匹配成功的3D點的集合，要想求得相機的運動，即找到一個歐氏變換R′，t′使得：

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

c. 根據R′計算t′，t*=a-R′a′。

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

圖4 相鄰兩幀色彩圖像

圖5 相鄰兩幀深度圖像

2 目標識別理論與算法研究

2.1 顏色閾值分割

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

假設RGB顏色空間描述的某個像素的值A″u，v=（R，G，B）， 此像素對應的HSV顏色空間描述的像素點P ″u，v=（H，S，V）。 二者的轉換關系如下：

RGB顏色（A）空間轉換到HSV顏色（B）空間如圖6所示。

圖6 色彩空間轉換

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

閾值分割如圖7所示。

圖7 閾值分割

2.2 邊緣檢測

針對經過閾值分割后的二值圖像，圖像的腐蝕用來濾除較小的圖像區域的干擾，圖像的膨脹用來恢復腐蝕后的圖像。

圖像的膨脹運算為⊕，設a為一幅二值圖像，使用卷積模板b來對a圖像進行膨脹，執行該操作時，有：

圖像的腐蝕運算為-，同上，執行圖像腐蝕操作時，有：

腐蝕與膨脹對像素體積小于模板的黑色區域進行剔除，對目標圖像進行腐蝕與膨脹，從而使得目標特征更加明顯。 圖像腐蝕與膨脹如圖8所示。

圖8 圖像腐蝕與膨脹

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

a. 對圖像進行高斯模糊；

b. 計算梯度；

c. 非極大值抑制；

d. 連接邊緣。

根據邊界跟蹤算法來對邊緣點進行最終篩選。 如果弱邊緣點和強邊緣點相連，則均保存，否則刪除弱邊緣點。 算法從第1個點開始搜索：

a. 如果此點為弱邊界且沒有被標記，將其標記放入棧與隊列中并進入下一步，若此點已經被標記或者不是弱邊界，則重復此過程以遍歷整幅圖像。

b. 從棧中取出元素并遍歷周圍的8個點，如果其中一像素點是弱邊緣且沒有被標記過，則標記這個像素點，并加入棧與隊列中。 如果其中一個像素點是強邊緣，則表示此弱邊緣點和強邊緣點連通，保留標記。 重復此過程，以遍歷棧中的所有元素， 若是不存在強邊緣點則清空棧與隊列，然后使用下一個像素執行步驟a。

高斯模糊與邊緣檢測如圖9所示。

圖9 高斯模糊與邊緣檢測

2.3 位姿估計濾波器

其中，狀態轉換矩陣M將系統因果體現出來，在這里M陣為一個兩行兩列的常數陣。

圖像中目標的運動是任意的， 所以在x軸方向和y軸方向上不存在耦合關系，故：

進行以下迭代步驟以預測：

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

圖10 濾波器輸入

圖11 濾波器效果

3 簡單色彩目標追蹤的實現研究

3.1 基于ROS的小車導航

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

圖12 Move_base輸入輸出特性

3.2 路徑規劃

A*算法根據代價函數優化，將搜索區域劃分成簡單的二維向量，向量中每個元素對應一個搜索區域節點，使用啟發函數來計算路徑代價并優化以降低此代價。 A*算法演示如圖13所示。

圖13 A*算法演示

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

a. 將起點加入集合O，O用于存放待檢測的搜索區域節點， 而集合C用來存放已經檢測過的搜索區域節點。

b. 對每一個O中的節點pi（i=1，2，…，n）檢查引起最小的F數值的子節點pi， 將子該節點pi移動到C中。

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

d. 若將終點放到了O中，則算法結束。若查找重點失敗且O為空，則算法結束。

e. 從終點搜索區域節點開始，沿著指向父節點的路徑一直到初始搜索節點，即為最優路徑。

其中F=I+J，I表示歷史代價；J表示啟發函數，可使用歐氏距離。

ROS 中 主 要 使 用 動 態 窗 口 法［17］（Dynamic Window Approach，DWA）， 使用多種速度模擬一段時間得到軌跡，對軌跡進行評價，選取最優的軌跡對應的速度來驅動小車運行。 步驟如下：

a. 建立小車的運動模型；

b. 建立速度采樣模型；

c. 評價函數。

DWA算法中使用的評價函數為：

其中，heading（v′，w′）為當前軌跡結束時智能小車與目標角度差；dist（v′，w′）為當前軌跡與障礙物的距離；velocity（v′，w′）為當前軌跡的速度；σ，α，β，γ為常數。

DWA局部路徑效果如圖14所示， 圖14a為DWA算法在阻礙區域進行局部路徑規劃的結果，圖14b為DWA算法在開闊區域進行局部路徑規劃的結果。 其中，*表示半徑為0.3的障礙物。

圖14 DWA局部路徑效果

3.3 點巡航仿真實驗

根據ORB SLAM建立的稀疏地圖如圖15所示，其中標點為環境的角點特征。

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

假設地圖在之前的過程中已經建好，二維地圖如圖16所示。

圖16 完整地圖

根據小車的傳感器信息和定位信息，設定小車在地圖上6個點之間導航，小車按照6個定點進行巡航，用于搜索目標，巡航定點應設置在視野開闊處，由路徑規劃控制器輸出導航信息。 使用Rviz數據可視化工具可查看小車導航和路徑規劃的情況。 在全局目標的規劃下，導航算法已經開始為小車規劃好了全局路徑，而圖17中小車前的黑線則為局部路徑規劃算法所做的模擬軌跡，最終，算法會選擇一條評價函數最優的軌跡輸出。

圖17 局部路徑規劃軌跡模擬

3.4 目標追蹤仿真

3.4.1 識別算法檢測

圖18a為相機圖像， 進行邊緣檢測后得到的結果如圖18b所示，可以看出，此時目標的特征已經十分突出。

圖18 目標識別

3.4.2 目標追蹤檢測

有了理論依據和小車自主導航的實現，只需設置一個節點運行目標檢測跟蹤算法，便可實現簡單色彩目標跟蹤。 運行效果如圖19所示，可以看出，簡單色彩目標在視野中已經被智能小車識別到。

圖19 追蹤小車演示

4 結束語

針對室內智能小車對色彩目標追蹤的任務，在對SLAM問題研究的基礎上， 提出使用RGB-D相機基于ORB特征構建SLAM，用于定位與建圖，并對檢測結果進行估計，最后根據檢測算法定位信息，實現智能小車對室內場景下色彩目標的識別與追蹤。 實驗結果表明，將圖像檢測算法與路徑規劃算法用在基于ROS的智能小車上， 可以實現追蹤與跟隨色彩目標。 基于同步定位與地圖構建的室內色彩追蹤智能小車可以代替人類探索危險未知的陌生室內環境，并對該室內陌生環境中有簡單色彩的事物進行追蹤與跟隨，這對于有效保障人類自身的安全和探索未知環境具有一定價值。