一種融合多幀ICP和圖優化的算法研究

呂 瑞,李 明,汪明闊,劉歡歡,薛靜遠

(1.武漢大學計算機學院,武漢430072;2.國家多媒體軟件工程技術研究中心,武漢430072)

一種融合多幀ICP和圖優化的算法研究

呂 瑞1,李 明2,汪明闊1,劉歡歡1,薛靜遠1

(1.武漢大學計算機學院,武漢430072;2.國家多媒體軟件工程技術研究中心,武漢430072)

當前基于迭代最近點拼接的同時定位與建圖算法,存在誤差積累、無法滿足大范圍定位精度的缺陷。為此,提出一種融合多幀迭代最近點和圖優化的算法。在時域上處理點云拼接問題,將單幀迭代最近點算法推廣到多幀進行最近點迭代,提取同一地點在不同時刻的數據特征,形成多個封閉循環,再運用基于最小二乘的圖優化方法對點云拼接后的全網數據進行全局優化,消除累計誤差,提升整體的定位精度。采用魯巷和密歇根的數據進行測試,結果表明,該方法在一定程度上減少了匹配誤差,平均誤差為1.0 m,最小誤差為0.2 m,可以滿足大范圍同步定位與建圖的精度需求。

多幀迭代最近點;圖優化;機器人;雷達;點云;同時定位與建圖

1 概述

同時定位與建圖[1](Simultaneous Localization and Mapping,SLAM)構建是目前移動和自主機器人領域最熱門的問題之一。從20世紀90年代起,美國的Berkeley大學、卡內基梅隆大學、哥倫比亞大學等都開展了相應的研究工作。國內的北京大學、長安大學、武漢大學[2]、同濟大學、中國測繪科學研究院、吉林大學、山東科技大學、中科院深圳先進技術研究院等也開展了相應的多傳感器集成車輛的研究,并研制了相應系統。

到目前為止,已經存在多種基于不同傳感器的定位方法被提出,如基于編碼器、激光雷達、磁傳感器、視覺、GPS等傳感器的方法。在這些傳感器中,激光雷達具有距離測量精度高的特點。因此,基于激光雷達的方法是一個比較有發展前景的。近年來,該方法得到了研究人員的重視,一些具有代表性的基于激光雷達的定位算法得到了驗證和應用。

1999年,Frank等人提出基于蒙特卡羅(Monte Carlo)的定位方法[3]。該方法是一種基于概率的定位方法,需要建立起激光雷達傳感器的概率模型。由于傳感器概率模型的準確性直接影響到定位的結果,因此這種方法具有一定的局限性。2004年,楊明等人提出基于角度直方圖的定位方法[4]。該方法是匹配相鄰兩幀的激光雷達數據,比較兩幀之間的位置偏移量和角度偏移量,從而得到智能車輛的位姿估計。這種做法會引入累積誤差,并且存在類孔徑問題,因此也不適合推廣應用。之后,楊明等人又提出一種基于迭代最近點算法的地圖匹配算法[5],該方法在小范圍的場景下可以實現較高精度的定位,但也存在累計誤差,不適合大范圍SLAM的精度需求。

針對以上問題,本文提出一種融合多幀ICP和圖優化[6]的SLAM算法,通過將點云進行多幀ICP拼接后的結果帶入圖模型進行優化,一定程度上減少了系統累計誤差,可滿足大范圍SLAM的精度需求。

2 VeloSLAM系統

VeloSLAM系統可以分為前端預處理和后端優化2個部分,如圖1所示。本文主要針對后端優化中多幀ICP姿態估計算法和圖優化算法的融合進行研究。本文只采用了Velodyne HDL-64E型激光雷達作為車載行駛環境感知傳感器,它配備了64個激光測距器,每秒探測數據量可達1.33萬個點,將掃描一周獲得的數據包構成完整一幀,每秒鐘有10幀,可以保證實時探測周圍道路環境,其結構如圖2所示。

圖1 VeloSLAM系統框架

圖2 Velodyne雷達示意圖

3 ICP算法

ICP算法于1992年由Besl等人提出[5],用于在同一坐標系中匹配2個給定的點集。已知2個數據集P,Q,且pi∈P,qi∈Q,通過最小化2個點集中最近點距離的平方和,求得2個數據集之間的剛體變換。

三維空間中2個3D點可用pi=(xi,yi,zi),qi= (xi,yi,zi)表示,它們的歐式距離表示為:

其中,qi=Rpi+T+Ni,i=1,2,…,N,R為角度變換矩陣和T為位移變換矩陣。則2個點集中所有最近點歐氏距離的平方和表示為:

再通過迭代直至E的值收斂到達指定閾值。

4 圖優化

經典的SLAM圖模型[7-8]公式在每個連續的姿態xi和xj之間提供了一系列的odometry約束。它們的關系如下:

此外,前端傳感器可以檢測2個不連續姿態xi和xj之間的loop closures。Loop closures約束可以表示為:

其中,f通常是一個用來表示機器人運動模型的非線性函數;xi和xj是未知的機器人姿態。wi和λij是協方差為∑i∑i和Λij的高斯誤差。

本文用因子圖[9]對SLAM圖模型問題進行建模,如圖3所示。

圖3 因子圖

圖3中大節點代表未知的機器人姿態,小節點代表概率約束[10]。所有變量(機器人姿態)X={xi}和約束U={ui∪uij}的條件概率關系表示如下:

SLAM圖模型的關鍵就是計算概率分布P(X|U)。P(X|U)與機器人姿態的最大后驗配置結果X*相關,即概率分布有最大值時,X*為所求點。在假設所有條件概率滿足高斯分布的情況下,最優的X*在聯合概率最大的情況下取得,公式如下:

這是一個非線性最小二乘問題。在本文中,馬氏距離的平方被定義為:

5 多幀ICP和圖優化的融合

利用Velodyne雷達數據密集的特點,本文同時對多幀點云數據使用ICP,進行姿態估計,增強了數據關聯,提升了初始估計的準確性,式(2)被擴展為:

其中,s,j,k代表點集的編號。將得到的初始估計作為圖模型的輸入。最后利用Marquardt的方法[11]來解算圖模型,得到最終軌跡。具體的解算方法如下:

定義一個矢量函數f:IRn-＞IRm,m≥n,公式可以轉換為:

其中:

第1步 計算式(7),獲得初始估計X0:

其中,x0是一個n維矩陣。

第2步 當f存在二次偏導時,得到它的泰勒公式:

第3步 依據式(10)計算迭代步hM:

其中,μ是一個巧妙的約束參數,用于控制迭代步的大小。在每次迭代中,監控誤差的更新。當新的誤差小于之前的誤差,再下次迭代中將減小μ;否則,增大μ。更詳細的介紹可以參考Marquardt的方法。

第4步 更新Xk(k≥0),公式如下:

第5步 重復執行第3步、第4步,直至系統收斂。

6 實驗結果與分析

6.1 魯巷環島

本文首先在武漢市洪山區的魯巷環島這種特色閉環場景進行了測試。魯巷不僅包括普通的車輛,而且有行人和電動車等較小的運動目標,而且車輛的運動規律比較復雜,環島有5個出口,很多車輛進入環島、很多車輛離開環島,期間還有行人橫穿馬路,這些情況對前端動目標的跟蹤分類提出了很高的要求。本文用3種算法進行了對比實驗,結果如圖4所示。

算法1對場景的點云不進行任何處理,直接利用KD-tree和最鄰近算法尋找兩幀的相關點對進行6D ICP點云匹配,然后估計出兩幀的相對位置。結果運行一周后軌跡不能閉合,累計誤差比較大,實驗效果用曲線1標注。算法2在算法1的基礎上增加了對目標的分割,分割后通過目標的幾何形體,將其中的車輛目標提取出,將剩下的點云進行點云的匹配,結果整個自身位置估計的效果有了一定的提高,相應的軌跡用曲線2標注。本文提出的融合算法在多幀ICP的基礎上,通過全局圖優化,一定程度上消除了累計誤差,提升了全局精度,相應軌跡用曲線3標注。

圖4 魯巷環島測試示意圖

3種算法具體精度對比如表1所示。

表1 3種算法精度對比 m

6.2 密歇根數據

為了進一步有效地驗證融合算法在大范圍場景中自主定位的性能,本文選擇有GPS軌跡真值的密歇根數據[12]集來進行測試。密歇根數據用到的傳感器如下:

(1)Velodyne HDL-64E雷達;

(2)Point Grey Ladybug3全景相機;

(3)Applanix POS-LV 420 INS和Trimble GPS,這是一個集成高精度差分GPS和IMU(角度每小時漂移1°)的專業級、緊耦合定位系統。

密歇根數據利用高精度差分GPS+INS記錄了本車真實的運行軌跡,地圖精度為2 cm左右,整個數據有6 000幀以上,覆蓋了大約3 km以上的城市行駛道路,如圖5所示。圖中真實軌跡用曲線1表示,利用本文提出的融合算法計算出來的軌跡用曲線2表示,其中A點為軌跡的起點,B點為本文的優化結果和真實數據誤差最大的點,最大誤差為5 m,C點和其周圍為重合度99%以上的位置點。實驗數據表明,本文的方法在緊使用Velodyne雷達的基礎上實現了高精度定位,基本滿足大范圍SLAM的精度需求。

圖5 密歇根數據

7 結束語

本文提出的融合多幀ICP和圖優化的SLAM算法,在同時進行多幀點云拼接之后,離線進行圖優化處理。實驗結果表明,本文算法在一定程度上減少了基于ICP算法的累計誤差,平均誤差為1.0 m,最小誤差為0.2 m,基本滿足大范圍SLAM的精度需求。下一步將研究動態目標的跟蹤和識別,實現動態的跟蹤序列生成,提升整體定位精度和密度,實現厘米級的SLAM定位。

[1] Olson E,Leonard J,Teller S.Fast Iterative Optimization

of Pose Graphs with Poor Initial Estimates[C]//Proc.of IEEE ICRA'06.[S.1.]:IEEE Press,2006:2262-2269.

[2] 李德仁,郭 晟,胡慶武.基于3S集成技術的LD2000系列移動道路測量系統及其應用[J].測繪學報, 2008,37(3):273-276.

[3] Dellaert F,Fox D,Burgard W,et al.Monte Carlo Localization for Mobile Robots[C]//Proc.of IEEE International Conference on Robotics&Automation. Detroit,USA:IEEE Press,1999:1322-1328.

[4] 楊 明,董 斌,王 宏,等.基于激光雷達的移動機器人實時位姿估計方法研究[J].自動化學報,2004, 30(5):679-687.

[5] Wu Shunxi,Yang Ming,Qian Jun.ICP Based Localization for Intelligent Vehicles Using Laser Radar [C]//Proc.of International Conference on Intelligent Computing.Qingdao,China:[s.n.],2007:125-131.

[6] Lu F,Milios E.Globally ConsistentRange Scan Alignment for Environment Mapping[J].Autonomous Robots,1997,4(4):333-349.

[7] Kaess M,Ranganathan A,Dellaert F.iSAM:Incremental Smoothing and Mapping[J].IEEE Transactions on Robotics,2008,24(6):13965-1378.

[8] Kümmerle R,Grisetti G,Strasdat H,et al.G2O:A General Framework for Graph Optimization[C]//Proc. of IEEE InternationalConferenceon Roboticsand Automation.[S.1.]:IEEE Press,2011:222-231.

[9] Kschischang F R,Frey B J,Loeliger H A.Factor Graphs and the Sum-product Algorithm[J].IEEE Transactions on Information Theory,2001,47(2):498-519.

[10] Grisetti G,Stachniss C,Burgard W. Non-linear Constraint Network Optimization forEfficientMap Learning[J].IEEE Transactions on Intelligent Transportation Systems,2009,10(3):458-164.

[11] Hertzberg C.A Framework for Sparse,Non-linear Least Squares Problems on Manifolds[D].Bremen,Germany: Bremen University,2008.

[12] Pandey G,McBride J R,Eustice R M.Ford Campus Vision and Lidar Data Set[J].International Journal of Robotics Research,2011,30(1):1543-1552.

編輯 索書志

Research on a Algorithm Fused with Multiple Frames ICP and Graph Optimization

LV Rui1,LI Ming2,WANG Ming-kuo1,LIU Huan-huan1,XUE Jing-yuan1
(1.School of Computer Science,Wuhan University,Wuhan 430072,China;
2.National Engineering Research Center for Multimedia Software,Wuhan 430072,China)

In view of the drawback of current Simultaneous Localization and Mapping(SLAM)based on Interative Closest Point(ICP),which exits error accumulation and can not meet the demand of wide range of SLAM positioning accuracy,a fused ICP and graph optimization algorithm is proposed.Through the ICP and graph optimization,data characteristic of the same site in different time is extracted,loop closure is formed,and global optimization based on leastsquare is done.The method is tested with real datasets.Result shows that the method can decrease mapping error by some certain and increase global accuracy demand of SLAM,mean error is 1.0 m,and least error is 0.2 m.

multiple frames Interative Closest point(ICP);graph optimization;robot;radar;point cloud;Simultaneous Localization and Mapping(SLAM)

1000-3428(2014)09-0229-04

A

TP18

10.3969/j.issn.1000-3428.2014.09.046

國家自然科學基金青年基金資助項目“基于高階關聯馬爾科夫網模型的點云分類”(41001306)。

呂 瑞(1992-),男,本科生,主研方向:無人車,網絡通信;李 明,副教授;汪明闊,本科生;劉歡歡,碩士研究生;薛靜遠,本科生。

2013-07-25

2013-08-20E-mail:liming751218@gmail.com