賈丙佳， 李平

(華僑大學 信息科學與工程學院， 福建 廈門 361021)

地圖創(chuàng)建是移動機器人研究領域的核心及熱點問題.如果沒有精確的地圖作為先驗條件，機器人就無法自主執(zhí)行一些任務，也無法確定自身相對于環(huán)境的位姿(位置和姿態(tài)角).因此，利用傳感器感知周圍環(huán)境信息，是移動機器人必須具備的一項基本功能[1-2].常用的傳感器有聲波傳感器[3]、視覺傳感器[4]和激光傳感器[5]等.聲波傳感器具有發(fā)散性，測量距離越遠，誤差越大.視覺傳感器對環(huán)境和處理速度的要求較高.相較而言，激光傳感器在精度、速度和穩(wěn)定性方面都具有更好的性能[6].

在利用激光雷達創(chuàng)建環(huán)境地圖方面，國內(nèi)外眾多學者已進行大量的研究[7-9].其中，應用最為廣泛的是迭代最近點(ICP)算法[10].ICP算法可以對不同時刻傳感器獲取的二維或三維環(huán)境深度信息進行配準，尋求一組旋轉(zhuǎn)和平移參數(shù)，使配準后的兩幀深度數(shù)據(jù)達到最大程度的重疊[11-12]，同時，經(jīng)計算可得不同時刻傳感器在環(huán)境中的位姿.然而，傳統(tǒng)ICP算法在實際應用中存在收斂速度慢、對給定初值敏感、易陷入局部最優(yōu)、相鄰兩次掃描點不存在單映射關系等缺陷[13-15].為解決這些問題，文獻[16]采用基于改進k-D樹的方法，提高對應點查找速度.文獻[17-19]采用基于點-直線匹配(PLICP)方法和幾何方法解決掃描點集不符合單映射的問題，篩選出對應點對，并減少點對的搜索次數(shù).文獻[20]提出一種基于判斷點云領域法向量夾角的自動配準算法.基于此，針對激光雷達相鄰掃描點集之間不存在一對一單映射關系的問題，本文提出一種稀疏掃描點的對應點對搜索算法.

1 二維點集粗配準

1.1 掃描點的采集

通過傳感器采集環(huán)境的深度數(shù)據(jù)，采用Neato XV-11型激光雷達(圖1)，其有效測量范圍為0.15～4.00 m，每旋轉(zhuǎn)360°會掃描產(chǎn)生360個距離數(shù)據(jù)，即1°對應1個距離數(shù)據(jù)，并返回1組極坐標表示的二維掃描點集O={(θi,ρi)|i=0,…,n}.其中：θi為激光雷達自身掃描第i度的角度信息；ρi為第i個角度上的距離；n為掃描點的個數(shù).

將掃描點集數(shù)據(jù)從極坐標系轉(zhuǎn)換到直角坐標系，有

xi=ρicosθi,yi=ρisinθi，

(1)

并用集合P={(xi,yi)|i=1,…,n}表示.

在數(shù)據(jù)采集過程中，通常間隔一定的時間或移動機器人位姿改變一定的閾值時，進行一次數(shù)據(jù)采集.由于運行中的機器人位姿不會發(fā)生突變,且具有連續(xù)性，故采集到的相鄰時刻掃描數(shù)據(jù)點集具有明顯的對應關系.激光掃描簡圖，如圖2所示.圖2中：紅色點表示機器人在M0處的激光掃描點；藍色點表示機器人在M1處的激光掃描點；車載紅點和藍點表示激光雷達的安裝位置；點之間的連線表示激光掃描時的射線，圖中僅畫出一部分；R表示機器人；黑色邊框表示墻壁；A～C表示相同位置不同時刻激光點的分布.由于相鄰兩個時刻機器人的位姿無較大變化，故相鄰兩個時刻的數(shù)據(jù)集將大部分重疊.

圖1 激光雷達實物圖 圖2 激光掃描簡圖 Fig.1 Physical picture of laser lidar Fig.2 Laser scanning diagram

1.2 問題的分析

自主移動機器人創(chuàng)建環(huán)境地圖時，在環(huán)境中不斷地移動，一方面，有新的環(huán)境進入激光雷達的可測范圍，也有當前環(huán)境遠離可測范圍，導致相鄰兩個時刻激光雷達產(chǎn)生的兩幀點集數(shù)據(jù)不能完全重疊，而自主移動機器人連續(xù)、不突變的的運動特點使兩幀數(shù)據(jù)只有小部分無法重疊，不重疊的數(shù)據(jù)代表新地圖的特征，由此創(chuàng)建出增量式的地圖；另一方面，即使機器人不移動，激光雷達的測量噪聲也會導致兩幀數(shù)據(jù)不完全重疊，這種測量噪聲無法準確排除，特別是在復雜的環(huán)境中，更無法排除這種噪點是測量噪聲還是環(huán)境特征.

在激光掃描簡圖中，機器人兩次掃描環(huán)境時的姿態(tài)角沒有變化，僅改變位置.然而，在實際創(chuàng)建地圖過程中，機器人的位置和姿態(tài)角均發(fā)生變化，此時，以激光雷達為坐標系的掃描點集數(shù)據(jù)就會發(fā)生錯位，進一步使兩幀掃描點集重疊部分變少.為了使ICP算法具有最佳的初值，快速收斂到正確的結果，前期的粗匹配尤為重要.

1.3 方向和位置的配準

為了使相鄰時刻激光雷達掃描數(shù)據(jù)盡可能最大程度地重疊，對掃描數(shù)據(jù)進行粗配準.采用方向柱狀圖匹配法[21]，對每一幀掃描點集計算連續(xù)兩個掃描點構成直線的角度，得到兩幀掃描點集的角度柱狀圖，再經(jīng)互相關得到兩幀掃描點集的相對角度對應關系，互相關函數(shù)為

(2)

其離散形式為

(3)

式(2)，(3)中：h1，h2分別為相鄰時刻的掃描點集；X,Y，Z，I,J均為方程變量.

由于激光雷達通過不同位姿對同一環(huán)境掃描得到的數(shù)據(jù)方向柱狀圖僅相差一定的角度，利用互相關函數(shù)搜索到的最大值即兩次掃描數(shù)據(jù)的相對角度差Δθ.方向配準后，進行位置配準.分別計算兩幀掃描點集數(shù)據(jù)的重心，計算重心間的距離，可得x軸和y軸的距離差，即位置配準在x軸和y軸方向移動的距離.

通過上述方向和位置配準處理后，兩幀數(shù)據(jù)在主方向和重心上都能大致重合，這將有利于后續(xù)準確建立對應點對搜索機制，以及確定掃描點間一對一單映射關系.需要注意的是，在進行粗配準時，先要進行方向配準，只有方向配準到一致時，位置配準才能更加準確.

2 二維點集精配準

在不采用編碼器信息的情況下，僅利用激光雷達采集的掃描點集數(shù)據(jù)進行環(huán)境地圖的創(chuàng)建.由于自主移動機器人在運行過程中的位姿不會發(fā)生突變，相鄰時刻兩幀掃描點集數(shù)據(jù)具有明顯的對應關系，故采用ICP算法計算激光雷達在相鄰兩個時刻的位姿變化量.

2.1 ICP算法概述

ICP算法[10]是基于最小二乘優(yōu)化思想的配準算法，具有精度高、編程實現(xiàn)簡易等優(yōu)點.

將激光雷達當前掃描點集變換到與參考掃描點集相同的坐標系下，有

Q1:n=R·P1:n+T.

(4)

式(4)中：Q1:n，P1:n分別為參考掃描點集與當前掃描點集，兩個點集數(shù)據(jù)數(shù)量相等；R為3×3的旋轉(zhuǎn)矩陣；T為3×1的平移向量.

ICP算法有以下6個步驟.

ICP算法的核心是最小化目標函數(shù)，即

(5)

經(jīng)迭代計算出正確的配準結果.

2.2 問題分析

ICP算法的實現(xiàn)原理，必須滿足以下2個假設.

1) 參考掃描點集和待配準的當前掃描點集的個數(shù)完全相同，對應點對需完全滿足一對一單映射對應關系.

2) 選取合適的R,T作為迭代初始值，使最終目標函數(shù)最小，迭代次數(shù)最少.

然而，ICP算法的假設過于理想化，這兩個假設在實際應用中很難得到滿足.

首先，自主移動機器人搭載激光雷達在環(huán)境中移動，探測到的新環(huán)境造成相鄰時刻激光掃描數(shù)據(jù)不完全重合(圖2)，則A～C處會出現(xiàn)以下3種情況：1) 情況1，前一次的掃描點集多于后一次；2) 情況2，前一次的掃描點集和后一次相等;3) 情況3，前一次的掃描點集少于后一次.如果把前一次的掃描點集當成參考掃描點集，則相對于前一次的掃描點集，后一次的點集會出現(xiàn)一對多、一對一和多對一的情況，情況2的兩幀點集則具備一對一的單映射對應關系.

對應點示意圖，如圖3所示.圖3中：紅色點為參考掃描點；藍色點為當前掃描點.將藍色點配準到紅色點位置，在A～C處不存在一對一單映射對應關系，而其他位置的多數(shù)點存在明顯的對應關系.

其次，相鄰時刻機器人的位姿變化并不一致，迭代初值必須隨之進行適當?shù)馗?受上述因素影響，直接使用ICP算法進行配準，結果會存在較大誤差.

圖3 對應點示意圖Fig.3 Schematic diagram of corresponding points

因此，兩幀掃描點集數(shù)據(jù)對應點對的查找及迭代初值的設置是實現(xiàn)ICP精配準的關鍵，在此基礎上才能創(chuàng)建較為精確的地圖.

2.3 對應點對搜索機制

首先，不滿足一對一單映射對應關系的數(shù)據(jù)點可以忽略，即從當前掃描點集中剔除.

然后，重建點集，使最終配準的兩幀掃描點集具有一對一單映射對應關系.由于在精配準之前，已進行了粗配準，但其只能減小掃描點集之間的錯位，并不能在A，B處實現(xiàn)完全重疊.C處是小車在M1位置的環(huán)境掃描點(圖2)，代表新的環(huán)境特征，而在M0處的掃描點集中并沒有對應的點，因此，需要對M1處掃描得到的數(shù)據(jù)進行稀疏處理，具體有以下4個步驟.

步驟1根據(jù)兩幀掃描點集，構建曼哈頓距離矩陣，第i個當前掃描點和第j個參考掃描點之間的曼哈頓距離d(i,j)為

d(i,j)=|xi-xj|+|yi-yj|.

(6)

式(6)中：xi，yi分別為第i個點的橫、縱坐標;xj,yj分別為第j個點的橫、縱坐標；i，j≤360.

矩陣中的每個元素表示Q1:n點集中的每一個點相對于P1:n點集中每一個點的距離，如圖4所示.圖4中：水平方向排列的是i個當前掃描點坐標；垂直方向排列的是j個參考掃描點坐標.

步驟3構建3×1的動態(tài)搜索窗口，按照初始最小值在矩陣中的索引，進行后續(xù)迭代搜索，需滿足

(7)

圖4 曼哈頓距離矩陣示意圖 圖5 搜索路徑示意圖Fig.4 Schematic diagram of Manhattan distance matrix Fig.5 Schematic diagram of search path

步驟4重復步驟3，直至搜索完i列數(shù)據(jù).

完成步驟1～4，可搜索出一條路徑.迭代搜索過程中需要滿足以下2個約束條件.

1) 連續(xù)性.相鄰搜索點是連續(xù)的，即下一個搜索點必須是D(j-1,i+1),D(j,i+1)和D(j+1,i+1)這3點中的一點.

2) 單調(diào)性.垂直方向按照曼哈頓距離矩陣中數(shù)值遞減的方向進行搜索，水平方向按照當前掃描點P1:n索引的排列順序依次搜索.

對于節(jié)2.2描述的3種情況，其路徑搜索示意圖，如圖6所示.圖6中：左側為搜索路徑原理分析圖；右側為實驗結果圖；左側紅色虛線框表示一次搜索過程需搜索的3個點；右側紅色實線框是搜索到的距離最小值.由于采用科學計數(shù)法，搜索路徑中的距離最小值并不都為零.

(a) 一對多

(b) 一對一

(c) 多對一圖6 路徑搜索示意圖Fig.6 Schematic diagram of path search

2.4 稀疏掃描點

搜索路徑上的動態(tài)搜索窗口并沒有過多地偏離上一次搜索到的最小距離值，即動態(tài)搜索窗口的索引值變化量在[0 1]范圍內(nèi)變化，且正常情況下的索引值變化量為Δi=1，Δj=1.在索引值變化量Δi，Δj不為1時，刪除該索引值對應的當前掃描點和參考掃描點.此外，一方面，當索引值變化量較大時，點會被刪除(圖3)；另一方面，設置1個最小曼哈頓距離閾值D，當搜索到的最小值比D大時，這兩種情況下同樣刪除該索引值對應的當前掃描點和參考掃描點，這些對應點視為無效對應點.

刪除無效對應點后，重建當前掃描點集，可得到稀疏掃描點集.

2.5 點集精配準

采用ICP算法對稀疏掃描點集與參考掃描點集進行精配準，此時，待匹配的是過濾后的數(shù)據(jù)點，已排除一對多和多對一的情況，可獲得較精確的ICP配準結果.經(jīng)配準可得旋轉(zhuǎn)矩陣和平移向量，應用式(4)對當前掃描點的原始點集數(shù)據(jù)進行變換操作，將當前掃描點匹配到參考掃描點坐標系中.然后，將配準前的當前掃描點作為參考掃描點，激光雷達下一時刻掃描得到的點集作為當前掃描點集，進行下一步配準，并將上一次配準結果作為該次配準的初值.計算獲得該次配準結果并累加之前的配準結果，對當前掃描點集進行變換.依此類推，可創(chuàng)建出整個環(huán)境地圖.地圖創(chuàng)建流程圖，如圖7所示.

圖7 地圖創(chuàng)建流程圖Fig.7 Flowchart of map creation

3 結果與分析

應用MATLAB仿真平臺，在局部環(huán)境下，通過相鄰兩幀掃描點集驗證文中方法的有效性.實驗平臺為Inert(R) Core(TM) i3-3220 CPU，3.30 GHz主頻和4.00 GB內(nèi)存的筆記本電腦，Windows 10操作系統(tǒng).實驗數(shù)據(jù)來自Neato XV-11型激光雷達，開發(fā)平臺為微軟Microsoft Visual Studio 2017，采用C#編程語言編寫激光雷達的數(shù)據(jù)解析代碼和掃描點數(shù)據(jù)配準代碼.

相鄰兩幀掃描點集配準結果，如圖8所示.圖8中：紅色點為參考掃描點；藍色點為當前掃描點.實驗目的是將藍色點配準到紅色點的位置.由圖8可知：在一定距離閾值內(nèi)，存在部分無法建立對應關系的數(shù)據(jù)點；剔除無效點后，紅色和藍色數(shù)據(jù)已最大程度地重疊.將當前掃描點作為參考掃描點，對下一幀點集數(shù)據(jù)進行配準，進而將兩兩點集數(shù)據(jù)進行同樣的處理，直至停止數(shù)據(jù)采集，即可建立環(huán)境地圖.

(a) 激光掃描原始數(shù)據(jù) (b) 文中方法匹配結果圖8 相鄰兩幀掃描點集配準結果Fig.8 Matching results of two adjacent scan point sets

表1 不同方法的配準結果Tab.1 Registration results of different methods

為驗證文中算法配準結果的準確性，將其與ICP算法、文獻[16]方法、文獻[17]方法、文獻[20]方法進行對比.不同方法的配準結果，如表1所示.表1中：e為配準誤差；C為迭代次數(shù).

由表1可知:在相同的掃描點集下，ICP算法的配準誤差較大；文獻[16]方法采用k-D樹加速對應點的查找，迭代次數(shù)減少，但配準誤差較大；文獻[17]方法采用點-直線的匹配方法，迭代次數(shù)和配準誤差均有降低；文獻[20]方法雖然濾除了測量噪聲點，但依然存在相鄰兩幀掃描點集無效對應的點；文中方法在前處理過程中將無效對應點剔除，其配準誤差和迭代次數(shù)均優(yōu)于其他方法，可提高地圖創(chuàng)建的準確性.

圖9 實際環(huán)境配準圖Fig.9 Registration map of actual environment

在實際環(huán)境中，激光雷達多幀掃描數(shù)據(jù)兩兩匹配的結果，如圖9所示.由圖9可知：文中方法可以消除ICP算法因不滿足一對一單映射對應關系而產(chǎn)生的數(shù)據(jù)錯位問題.由此可知，實際環(huán)境中的配準結果驗證了文中方法的有效性.

4 結束語

提出一種稀疏激光掃描點的自主移動機器人地圖創(chuàng)建方法.在粗配準后，運用稀疏掃描點的對應點對搜索方法，對激光雷達環(huán)境掃描點集建立對應點對搜索機制，根據(jù)曼哈頓距離剔除無效對應點，在精配準時提高ICP算法的配準精度，減少搜索次數(shù)，建立環(huán)境地圖.實驗結果驗證了文中方法的準確性和有效性.