基于地面約束的改進(jìn)A-LOAM算法

王樂兵,王 挺,姜 祎,李亞偉

(1. 中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室,遼寧 沈陽(yáng) 110016;2. 中國(guó)科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院,遼寧 沈陽(yáng) 110169;3. 中國(guó)科學(xué)院大學(xué),北京100049)

1 引言

隨著機(jī)器人導(dǎo)航技術(shù)的發(fā)展,SLAM已成為近些年的研究熱點(diǎn),其精度和魯棒性的提高也面臨諸多挑戰(zhàn)[1]。激光雷達(dá)是SLAM算法常用的傳感器之一。激光傳感器的測(cè)量信息不受光照等環(huán)境因素的影響且可以直接獲得環(huán)境的深度信息。以激光雷達(dá)為傳感器的激光SLAM算法應(yīng)用前景廣闊。

LOAM[2][3]是激光SLAM的代表算法,該算法首次提出了粗匹配到精匹配的優(yōu)化框架,Odometry線程負(fù)責(zé)利用相鄰兩幀間的特征點(diǎn)實(shí)現(xiàn)粗匹配,實(shí)時(shí)跟蹤位姿變化,Mapping線程則將特征點(diǎn)與局部地圖匹配進(jìn)一步優(yōu)化位姿。A-LOAM是LOAM算法的簡(jiǎn)化版本。LeGO-LOAM[4]在LOAM的基礎(chǔ)上對(duì)激光點(diǎn)云進(jìn)行了更精細(xì)的處理并增加了回環(huán)檢測(cè)。HDL-graph-slam[5]是基于點(diǎn)云配準(zhǔn)的3D激光SLAM算法,除此之外該算法還假設(shè)了全局一致地面的存在用于約束機(jī)器人位姿。由于激光雷達(dá)獲取的真實(shí)點(diǎn)云存在運(yùn)動(dòng)畸變、缺少特征等情況,導(dǎo)致基于匹配的SLAM算法在應(yīng)用于地面機(jī)器人時(shí)隨著時(shí)間增加會(huì)產(chǎn)生較大的高程誤差,如何提取環(huán)境中的有效信息進(jìn)行配準(zhǔn)是激光SLAM算法的挑戰(zhàn)。

本文針對(duì)地面機(jī)器人提出一種基于地面約束的改進(jìn)A-LOAM算法。在數(shù)據(jù)預(yù)處理時(shí)通過(guò)分割地面點(diǎn)與非地面點(diǎn)提取更為可靠的特征用于匹配。Odometry階段則負(fù)責(zé)構(gòu)建增量式里程計(jì),實(shí)施跟蹤位姿變化。Mapping階段將Odometry的結(jié)果作為初值,通過(guò)閉環(huán)檢測(cè)、地面約束和局部地圖結(jié)束進(jìn)一步優(yōu)化位姿。與HDL-graph-slam中的全局一致地面不同,本文通過(guò)在局部地圖中提取地面約束機(jī)器人的位姿,當(dāng)機(jī)器人所在環(huán)境地面發(fā)生變化時(shí),該約束仍有效。

2 改進(jìn)的A-LOAM算法框架

為了改進(jìn)純激光SLAM算法A-LOAM在應(yīng)用于地面機(jī)器人時(shí)出現(xiàn)的特征匹配不可靠、高程誤差漂移等問題,本文在該算法基礎(chǔ)上構(gòu)建了基于地面約束的激光SLAM算法,算法結(jié)構(gòu)示意圖如圖1所示。與A-LOAM框架不同的是本文算法增加了回環(huán)檢測(cè)階段。

圖1 改進(jìn)的A-LOAM算法結(jié)構(gòu)示意圖

2.1 數(shù)據(jù)預(yù)處理

為了獲取更精確的匹配結(jié)果,本文首先對(duì)原始激光點(diǎn)云做去畸變的處理,然后進(jìn)行特征點(diǎn)的提取。

2.1.1 畸變?nèi)コ?/p>

在機(jī)器人運(yùn)動(dòng)過(guò)程中,激光點(diǎn)云中的每個(gè)點(diǎn)都是在不同的基準(zhǔn)位姿獲取的,從而導(dǎo)致點(diǎn)云的運(yùn)動(dòng)畸變,尤其是當(dāng)激光雷達(dá)掃描頻率較低或機(jī)器人運(yùn)動(dòng)較快時(shí),這種運(yùn)動(dòng)畸變不可忽略。去除點(diǎn)云運(yùn)動(dòng)畸變的基本思想是將一幀點(diǎn)云內(nèi)的點(diǎn)轉(zhuǎn)換到同一時(shí)刻的(一般是該幀的起始或結(jié)束時(shí)刻)基準(zhǔn)位姿上。假設(shè)一幀時(shí)間內(nèi)獲取的點(diǎn)云為P,機(jī)器人在該段時(shí)間的位姿變化估計(jì)為ΔR和Δt,利用插值得到每個(gè)點(diǎn)相對(duì)于起始時(shí)刻的運(yùn)動(dòng),進(jìn)而每個(gè)點(diǎn)將換到該幀起始時(shí)刻去除運(yùn)動(dòng)畸變。

2.1.2 特征提取

特征提取即從點(diǎn)云中提取能夠反映當(dāng)前環(huán)境信息的特殊點(diǎn),例如角點(diǎn)一般位于拐角,平面點(diǎn)則位于平面上。考慮到地平面也屬于平面且地平面中的點(diǎn)對(duì)匹配精度影響不大,因此在提取特征點(diǎn)之前,首先標(biāo)記點(diǎn)云中的地面點(diǎn)和非地面點(diǎn)。只在非地面點(diǎn)中選取角點(diǎn)和平面點(diǎn)以提高效率,地面點(diǎn)則用于后續(xù)的地面約束。

2.2 Odometry階段

在odometry階段,接受來(lái)自傳感器的信息進(jìn)行幀間匹配,獲得增量式里程計(jì)。

對(duì)非地面點(diǎn)云計(jì)算每個(gè)點(diǎn)的局部平整度c,c值較大的作為角點(diǎn),較小的作為平面點(diǎn)。

(1)

通過(guò)分割地面點(diǎn)和非地面點(diǎn)可以提取到更可靠的特征點(diǎn)信息從而降低匹配誤差。

2.2.1 點(diǎn)云匹配

點(diǎn)云特征匹配的方法[6][7]有最近點(diǎn)迭代法(Iterative Closest Point,ICP)[8][9]、正態(tài)分布變換(Normal Distribution Transform, NDT)[10][11]和基于特征的匹配方法[2]。

基于特征的匹配與ICP匹配方法的區(qū)別在于源點(diǎn)云在目標(biāo)點(diǎn)云中的匹配不再是一個(gè)點(diǎn),而是一條直線或一個(gè)平面。該直線(平面)由源點(diǎn)云在目標(biāo)點(diǎn)云的鄰近點(diǎn)表示(如圖2所示,i是源點(diǎn)云中的某一特征點(diǎn),j、l、m為該點(diǎn)在目標(biāo)點(diǎn)云中的匹配點(diǎn))。將源點(diǎn)云某一點(diǎn)與其匹配點(diǎn)變換到同一坐標(biāo)系后利用點(diǎn)到線和點(diǎn)到面的距離構(gòu)建殘差,通過(guò)迭代優(yōu)化匹配殘差即可得到源點(diǎn)云和目標(biāo)點(diǎn)云間的相對(duì)位姿,將連續(xù)兩幀間的相對(duì)位姿累積即可獲得激光雷達(dá)相對(duì)于初始時(shí)刻的位姿。

圖2 角點(diǎn)(左)與平面點(diǎn)(右)的匹配

(2)

(3)

2.3 Mapping階段

Odometry部分實(shí)現(xiàn)了幀間匹配,實(shí)時(shí)跟蹤兩幀點(diǎn)云間的位姿變化。由于匹配時(shí)僅將當(dāng)前時(shí)刻的位姿與上一關(guān)聯(lián),累積誤差將隨著時(shí)間增加而增大。實(shí)際中機(jī)器人某一時(shí)刻位姿與之前多個(gè)時(shí)刻均存在“共視關(guān)系”,將這些時(shí)刻作為關(guān)鍵幀并利用關(guān)鍵幀點(diǎn)云構(gòu)建局部地圖進(jìn)一步優(yōu)化位姿,提高軌跡估計(jì)精度。

2.3.1 關(guān)鍵幀提取

Mapping階段位姿優(yōu)化時(shí)局部地圖來(lái)自當(dāng)前時(shí)刻機(jī)器人所處位置的附近關(guān)鍵幀點(diǎn)云,關(guān)鍵幀的數(shù)量和質(zhì)量決定當(dāng)前幀與局部地圖的關(guān)聯(lián)強(qiáng)度。除此之外回環(huán)檢測(cè)也是基于關(guān)鍵幀進(jìn)行的。若關(guān)鍵幀選取不合理可能會(huì)遺漏真閉環(huán)或者出現(xiàn)過(guò)多相似的閉環(huán)候選幀。因此關(guān)鍵幀的選取對(duì)提高算法的效率和精度至關(guān)重要。因此本文以兩幀間的點(diǎn)云關(guān)聯(lián)性和機(jī)器人的相對(duì)位姿為條件選取關(guān)鍵幀,盡可能使其均勻分布在機(jī)器人行駛路線上。

2.3.2 局部地圖匹配

僅依靠相鄰兩幀的點(diǎn)云信息的位姿估計(jì)結(jié)果可靠性不高且會(huì)導(dǎo)致誤差傳遞和累積。Mapping階段利用幀間匹配的結(jié)果作為初值,將當(dāng)前點(diǎn)云與局部地圖對(duì)齊進(jìn)一步優(yōu)化位姿。除此之外,還利用局部地圖中的地面信息進(jìn)一步約束位姿估計(jì)在Z軸方向的誤差(包括Z軸位移、俯仰角和橫滾角)。在將當(dāng)前幀點(diǎn)云注冊(cè)到局部地圖的同時(shí),檢測(cè)當(dāng)前幀是否與歷史幀形成閉環(huán)進(jìn)一步約束關(guān)鍵幀位姿估計(jì)結(jié)果。全局位姿優(yōu)化框架如圖3所示。

圖3 位姿約束示意圖

2.3.3 地面約束

本文提出的算法主要針對(duì)地面機(jī)器人,由于機(jī)器人的Z軸位移不會(huì)突變,因此考慮加入地面約束。由于實(shí)際環(huán)境中不存在絕對(duì)平坦的地面且可能出現(xiàn)斜坡、樓梯等情況,因此本文用于約束機(jī)器人位姿的地面不是全局一致地面,而是跟隨局部地圖變化。在1.1.2小節(jié)提取特征點(diǎn)時(shí)已經(jīng)對(duì)地面點(diǎn)和非地面地進(jìn)行標(biāo)記。將局部地圖和當(dāng)前點(diǎn)云中的地面點(diǎn)分別擬合為一個(gè)地面(如圖4(a)所示),記為π0和π1。

圖4 地面約束原理

假設(shè)局部地圖地面某點(diǎn)pm=[xm,ym,zm]T,法向量nm=[xnm,ynm,znm]T,則π0在全局坐標(biāo)系中的參數(shù)方程為

(x-pm)·nm=0

(4)

(5)

也可化簡(jiǎn)為如下形式

A0x+B0y+C0z+D0=0

(6)

同時(shí)π1在激光雷達(dá)坐標(biāo)系中的參數(shù)方程表示為

A1x+B1y+C1z+D1=0

(7)

(8)

Mapping階段接收到新的一幀點(diǎn)云后,利用該幀點(diǎn)云和局部地圖中的地面點(diǎn)擬合出兩個(gè)平面并根據(jù)平面參數(shù)構(gòu)建殘差參與位姿優(yōu)化,對(duì)位姿進(jìn)行修正。

2.2 回環(huán)檢測(cè)

Odometry階段和Mapping階段構(gòu)成一個(gè)開環(huán)的地面機(jī)器人SLAM系統(tǒng),隨著運(yùn)行時(shí)間變長(zhǎng),累積誤差也不斷增加。回環(huán)檢測(cè)是減小累積漂移誤差的重要步驟[9]。本文將歷史位姿以關(guān)鍵幀的形式存儲(chǔ),通過(guò)搜索當(dāng)前幀附近的歷史關(guān)鍵幀進(jìn)行閉環(huán)檢測(cè),將距離最近的歷史幀標(biāo)記為閉環(huán)候選幀并利用ICP算法將當(dāng)前幀與其匹配。如果匹配結(jié)果收斂,說(shuō)明當(dāng)前幀與候選幀相似度很高,符合閉環(huán)條件。最后將ICP算法得到的兩幀間的相對(duì)位姿作為約束對(duì)與當(dāng)前幀關(guān)聯(lián)的關(guān)鍵幀進(jìn)行位姿調(diào)整,減小累積誤差。

3 實(shí)驗(yàn)

以下實(shí)驗(yàn)均在Ubuntu16.04環(huán)境下完成,計(jì)算機(jī)型號(hào)為NUC8I7HVK。本文所用實(shí)驗(yàn)數(shù)據(jù)來(lái)自兩個(gè)公開數(shù)據(jù)集:KITTI[13]和GroundRobotDatabases[14](以下分別以Part＿s00和R4＿01表示)。其中GroundRobotDatabases是由中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所發(fā)布的面向地面移動(dòng)機(jī)器人的數(shù)據(jù)集,內(nèi)含16線激光雷達(dá)、雙目相機(jī)、IMU、編碼器等傳感器數(shù)據(jù),覆蓋室內(nèi)長(zhǎng)走廊、室外開闊草坪、斜坡、樓梯以及地下車庫(kù)等各種復(fù)雜環(huán)境。

參與實(shí)驗(yàn)的算法有A-LOAM、OUR及OUR＿NGC。其中OUR為本文提出的包括所有約束的完整算法,OUR＿NGC表示去除地面約束后的本文算法。

3.1 算法精度對(duì)比

為了驗(yàn)證提出的改進(jìn)版A-LOAM算法的性能,本文估計(jì)了上述兩個(gè)數(shù)據(jù)集的軌跡,如圖5和圖6所示((a)為本文提出的算法軌跡估計(jì)結(jié)果,(b)為A-LOAM軌跡估計(jì)結(jié)果)。圖中虛線為groundtruth。

圖5 軌跡估計(jì)結(jié)果(Part＿s00)

圖6 軌跡估計(jì)結(jié)果(R4＿01)

為了定量評(píng)估算法的精度,本文還對(duì)比了A-LOAM和本文提出的算法(OUR)在上述兩個(gè)數(shù)據(jù)集的估計(jì)軌跡與真實(shí)軌跡間的誤差(均方根誤差RMSE、標(biāo)準(zhǔn)偏差STD、誤差均值MEAN和最大誤差MAX),結(jié)果如表1、表2所示。

表1 兩種算法的軌跡估計(jì)誤差(Part＿s00)

表2 兩種算法的軌跡估計(jì)誤差(R4＿01)

本文在R4＿01上驗(yàn)證了回環(huán)檢測(cè)的效果。根據(jù)圖7中軌跡估計(jì)誤差隨時(shí)間的變化曲線((a)為本文算法,(b)為A-LOAM)。可以看出與(b)相比,(a)的誤差增長(zhǎng)趨勢(shì)得到有效抑制。

圖7 軌跡誤差變化曲線(R4＿01)

3.2 地面約束可靠驗(yàn)證

除此之外,為了驗(yàn)證本文提出的地面約束對(duì)提高算法精度起到了積極作用,測(cè)試有地面約束(OUR)和無(wú)地面約束時(shí)(OUR＿NGC)的兩種算法在數(shù)據(jù)集Part＿s00的軌跡估計(jì)結(jié)果,如圖8所示,其中虛線為groundtruth,藍(lán)色為帶有地面約束的算法結(jié)果,綠色為不帶地面約束的算法結(jié)果。由該圖可以看出,加入地面約束后的軌跡估計(jì)更加靠近groundtruth。

圖8 兩種算法軌跡估計(jì)結(jié)果(Part＿s00)

由表3中的各種誤差值也可看出,加入地面約束后除最大誤差的其它誤差均有下降,這說(shuō)明加入的地面約束可以作為有效信息約束軌跡估計(jì)的誤差。

表3 兩種算法的軌跡估計(jì)誤差(Part＿s00)

4 結(jié)束語(yǔ)

針對(duì)現(xiàn)有純激光SLAM算法應(yīng)用于地面機(jī)器人存在高程誤差漂移的問題,本文提出了一種基于地面約束的改進(jìn)A-LOAM算法。算法接收到激光點(diǎn)云后首先進(jìn)行畸變?nèi)コ?yōu)化特征點(diǎn)提取過(guò)程獲得更為可靠的特征,然后在Odometry階段匹配特征實(shí)時(shí)跟蹤位姿變化,在Mapping階段本文算法采用了關(guān)鍵幀策略節(jié)省存儲(chǔ)空間,根據(jù)關(guān)鍵幀構(gòu)建局部地圖并加入地面約束和回環(huán)檢測(cè)進(jìn)一步約束機(jī)器人位姿,減小累積誤差,最終實(shí)現(xiàn)精確估計(jì)機(jī)器人的位姿。

本文在兩個(gè)公開數(shù)據(jù)集上設(shè)計(jì)了實(shí)驗(yàn)用于驗(yàn)證算法性能。通過(guò)本文算法和A-LOAM的估計(jì)軌跡與groundtruth對(duì)比結(jié)果可得,本文提出的算法軌跡估計(jì)結(jié)果精度更高,且加入的地面約束可以有效降低誤差。