呂 瑞,李 明,汪明闊,劉歡歡,薛靜遠(yuǎn)

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

一種融合多幀ICP和圖優(yōu)化的算法研究

呂 瑞1,李 明2,汪明闊1,劉歡歡1,薛靜遠(yuǎn)1

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

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

多幀迭代最近點(diǎn);圖優(yōu)化;機(jī)器人;雷達(dá);點(diǎn)云;同時(shí)定位與建圖

1 概述

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

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

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

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

2 VeloSLAM系統(tǒng)

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

圖1 VeloSLAM系統(tǒng)框架

圖2 Velodyne雷達(dá)示意圖

3 ICP算法

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

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

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

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

4 圖優(yōu)化

經(jīng)典的SLAM圖模型[7-8]公式在每個(gè)連續(xù)的姿態(tài)xi和xj之間提供了一系列的odometry約束。它們的關(guān)系如下:

此外,前端傳感器可以檢測2個(gè)不連續(xù)姿態(tài)xi和xj之間的loop closures。Loop closures約束可以表示為:

其中,f通常是一個(gè)用來表示機(jī)器人運(yùn)動模型的非線性函數(shù);xi和xj是未知的機(jī)器人姿態(tài)。wi和λij是協(xié)方差為∑i∑i和Λij的高斯誤差。

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

圖3 因子圖

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

SLAM圖模型的關(guān)鍵就是計(jì)算概率分布P(X|U)。P(X|U)與機(jī)器人姿態(tài)的最大后驗(yàn)配置結(jié)果X*相關(guān),即概率分布有最大值時(shí),X*為所求點(diǎn)。在假設(shè)所有條件概率滿足高斯分布的情況下,最優(yōu)的X*在聯(lián)合概率最大的情況下取得,公式如下:

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

5 多幀ICP和圖優(yōu)化的融合

利用Velodyne雷達(dá)數(shù)據(jù)密集的特點(diǎn),本文同時(shí)對多幀點(diǎn)云數(shù)據(jù)使用ICP,進(jìn)行姿態(tài)估計(jì),增強(qiáng)了數(shù)據(jù)關(guān)聯(lián),提升了初始估計(jì)的準(zhǔn)確性,式(2)被擴(kuò)展為:

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

定義一個(gè)矢量函數(shù)f:IRn-＞IRm,m≥n,公式可以轉(zhuǎn)換為:

其中:

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

其中,x0是一個(gè)n維矩陣。

第2步 當(dāng)f存在二次偏導(dǎo)時(shí),得到它的泰勒公式:

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

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

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

第5步 重復(fù)執(zhí)行第3步、第4步,直至系統(tǒng)收斂。

6 實(shí)驗(yàn)結(jié)果與分析

6.1 魯巷環(huán)島

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

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

圖4 魯巷環(huán)島測試示意圖

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

表1 3種算法精度對比 m

6.2 密歇根數(shù)據(jù)

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

(1)Velodyne HDL-64E雷達(dá);

(2)Point Grey Ladybug3全景相機(jī);

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

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

圖5 密歇根數(shù)據(jù)

7 結(jié)束語

本文提出的融合多幀ICP和圖優(yōu)化的SLAM算法,在同時(shí)進(jìn)行多幀點(diǎn)云拼接之后,離線進(jìn)行圖優(yōu)化處理。實(shí)驗(yàn)結(jié)果表明,本文算法在一定程度上減少了基于ICP算法的累計(jì)誤差,平均誤差為1.0 m,最小誤差為0.2 m,基本滿足大范圍SLAM的精度需求。下一步將研究動態(tài)目標(biāo)的跟蹤和識別,實(shí)現(xiàn)動態(tài)的跟蹤序列生成,提升整體定位精度和密度,實(shí)現(xiàn)厘米級的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集成技術(shù)的LD2000系列移動道路測量系統(tǒng)及其應(yīng)用[J].測繪學(xué)報(bào), 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] 楊 明,董 斌,王 宏,等.基于激光雷達(dá)的移動機(jī)器人實(shí)時(shí)位姿估計(jì)方法研究[J].自動化學(xué)報(bào),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

國家自然科學(xué)基金青年基金資助項(xiàng)目“基于高階關(guān)聯(lián)馬爾科夫網(wǎng)模型的點(diǎn)云分類”(41001306)。

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

2013-07-25

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