鄭雪鶴，孫作雷

(上海海事大學(xué) 信息工程學(xué)院，上海 201306)

基于雙向激光回環(huán)檢測(cè)的SLAM算法研究

鄭雪鶴1，孫作雷2

(上海海事大學(xué) 信息工程學(xué)院，上海 201306)

針對(duì)移動(dòng)機(jī)器人長(zhǎng)時(shí)間運(yùn)動(dòng)后無(wú)法自身修正累計(jì)誤差以及傳統(tǒng)EKF (Extended Kalman Filter)算法計(jì)算復(fù)雜度大的問(wèn)題，提出了一種基于雙向激光進(jìn)行回環(huán)檢測(cè)的方法，通過(guò)有效的相似度檢測(cè)算法檢測(cè)出真實(shí)的回環(huán)，及時(shí)修正機(jī)器人的位姿。同時(shí)使用精確稀疏滯后濾波算法相輔，利用信息矩陣的自然稀疏性來(lái)降低計(jì)算復(fù)雜度。通過(guò)實(shí)驗(yàn)結(jié)果分析，上述兩種方法的結(jié)合可以有效地減少移動(dòng)機(jī)器人行駛過(guò)程中的累計(jì)誤差。

回環(huán)檢測(cè)；精確稀疏滯后濾波；同時(shí)定位與地圖構(gòu)建；移動(dòng)機(jī)器人

0 引言

同步定位和構(gòu)圖(Simultaneous Localization and Map Building,SLAM)最早由DURRANT-WHYTE H和LEONARD J J提出[1]。SLAM一般用于解決機(jī)器人在未知環(huán)境下定位導(dǎo)航以及構(gòu)圖問(wèn)題。EKF(Extended Kalman Filtering)算法是SLAM算法中經(jīng)典的算法之一，通過(guò)線(xiàn)性化及高斯分布假設(shè)對(duì)狀態(tài)空間中的機(jī)器人位姿和特征位置進(jìn)行估計(jì)。EKF SLAM自提出后一直被許多研究者采用，然而其計(jì)算復(fù)雜度與環(huán)境特征個(gè)數(shù)呈二次方關(guān)系，因此EKF SLAM只能在一般不超過(guò)上百個(gè)特征的較小范圍內(nèi)應(yīng)用[2]。如何在大地圖減少計(jì)算復(fù)雜度，這是SLAM算法中一個(gè)公共的難題。許多學(xué)者在EKF SLAM算法上進(jìn)行改進(jìn)， THRUN S通過(guò)經(jīng)驗(yàn)觀(guān)察發(fā)現(xiàn)，EKF SLAM 中的信息矩陣中許多非對(duì)角元素的值接近于零，具有一種近似的稀疏性。EUSTICE R對(duì)SEIF(Sparse Extended Information Filter)的稀疏過(guò)程及算法的一致性進(jìn)行了分析，發(fā)現(xiàn)SEIF可以基本滿(mǎn)足局部一致性，但是無(wú)法保證全局一致性[3]。而ESDF(Exactly Sparse Delayed-State Filters) 算法通過(guò)增加一個(gè)延時(shí)狀態(tài)，將信息矩陣自然稀疏，可以保證局部和全局的一致性[4]。

回環(huán)檢測(cè)(Loop Closure Detection)，又稱(chēng)閉環(huán)檢測(cè)，是指機(jī)器人是否可以識(shí)別自己曾經(jīng)到達(dá)過(guò)某個(gè)地點(diǎn)的能力。一旦檢測(cè)成功，可以顯著地減小累積誤差[5]。回環(huán)檢測(cè)實(shí)質(zhì)上是一種檢測(cè)觀(guān)測(cè)數(shù)據(jù)相似性的算法。錯(cuò)誤的回環(huán)會(huì)使地圖變得很糟糕，優(yōu)秀的回環(huán)檢測(cè)算法應(yīng)該盡量檢測(cè)出真實(shí)回環(huán)[5]。

本文利用ESDF算法以及雙向激光進(jìn)行回環(huán)檢測(cè)，顯著地減小了機(jī)器人行駛過(guò)程中的累計(jì)誤差。

1 ESDF算法推導(dǎo)

1.1 狀態(tài)增量

當(dāng)需要存儲(chǔ)一個(gè)新視圖時(shí)，通過(guò)增加一個(gè)新的狀態(tài)延時(shí)來(lái)形成狀態(tài)增量，以便在之后的時(shí)間里回顧之前的狀態(tài)[6]。

1.1.1增加一個(gè)延時(shí)狀態(tài)

假定在時(shí)間t分布下給出協(xié)方差和信息表達(dá)式：

(1)

這個(gè)分布代表著一個(gè)地圖M和當(dāng)前機(jī)器人的狀態(tài)xt、所有的觀(guān)測(cè)量zt以及輸入的控制量μt。在xt+1時(shí)刻獲得分布p(xt+1,xt,M|zt,μt+1)可以分解為：

p(xt+1,xt,M|zt,ut+1)=

p(xt+1|xt,M,zt,ut+1)p(xt,M|zt,ut+1)=

p(xt+1|xt,ut+1)p(xt,M|zt,ut)

(2)

下式描述的是一般非線(xiàn)性馬爾可夫機(jī)器人運(yùn)動(dòng)模型以及它的一階線(xiàn)性形式，F(xiàn)是在μt下的估計(jì)雅克比矩陣，w～N(0,Q)是高斯白噪聲。

xt+1=f(xt,ut+1)+wt≈f(μt,ut+1)+F(xt-μt)+wt

(3)

1.1.2增量的協(xié)方差形式

在公式(3)的線(xiàn)性條件下，增加的狀態(tài)分布式(2)仍然是高斯的，它的協(xié)方差形式由下式給出：

(4)

其中，

1.1.3增量的信息形式

(5)

1.2 馬爾可夫精確稀疏

由上述推導(dǎo)的公式(5)可知，擴(kuò)充機(jī)器人的運(yùn)動(dòng)狀態(tài)到xt+1時(shí)，僅僅對(duì)先前的xt狀態(tài)提供信息[7]。增廣狀態(tài)的一個(gè)重要的屬性是用額外的滯后狀態(tài)繼續(xù)增加狀態(tài)矢量，信息矩陣將展現(xiàn)出一塊三對(duì)角結(jié)構(gòu)連接每個(gè)滯后位姿以及先前的狀態(tài)。從公式(6)中可以看出，滯后狀態(tài)SLAM信息矩陣是自然的沒(méi)有任何近似的。

(6)

1.3 狀態(tài)更新

假設(shè)下面的一般非線(xiàn)性方法和它的一階線(xiàn)性形式為：

(7)

(8)

這個(gè)計(jì)算中的所有非一般模型都會(huì)導(dǎo)致每次更新時(shí)出現(xiàn)二次計(jì)算復(fù)雜度。相反地，擴(kuò)展信息濾波更新表達(dá)為：

(9)

1.4 預(yù)測(cè)模型

預(yù)測(cè)模型對(duì)應(yīng)于機(jī)器人從t傳播到t+1時(shí)刻的狀態(tài)。為了得到時(shí)間分布p(xt+1,xt,M|zt,ut+1)，在公式(5)的聯(lián)合分布中需要簡(jiǎn)單地邊緣化先前的狀態(tài)xt。最后得到信息預(yù)測(cè)模型：

(10)

這里

Ω=Λxtxt+FTQ-1

ψ=Q-1-Q-1FΩ-1F-1Q-1=

1.5 狀態(tài)恢復(fù)

信息形式的高斯分布的兩個(gè)參數(shù)分別為ηt和Λt。然而，公式(10)的預(yù)測(cè)模型和公式(9)的觀(guān)測(cè)模型都額外地增加了狀態(tài)均值矢量μt中的子塊，因此，為了讓延時(shí)狀態(tài)中的信息形式計(jì)算高效，需要容易地恢復(fù)狀態(tài)矢量均值部分。

1.5.1全局狀態(tài)恢復(fù)

通過(guò)矩陣直接求逆的方法恢復(fù)初始狀態(tài)會(huì)導(dǎo)致三次復(fù)雜性，并且會(huì)破壞在EKF上獲得的任何效率。狀態(tài)均值μt可以更有效地解決稀疏、對(duì)稱(chēng)、正定、線(xiàn)性方程：

Λtμt=ηt

(11)

這樣的系統(tǒng)可以通過(guò)經(jīng)典的共軛梯度迭代法來(lái)解決。通常，共軛梯度每次迭代會(huì)產(chǎn)生O(n)的復(fù)雜度，這里n是狀態(tài)矢量的尺寸。如果是已經(jīng)初始化好的，則迭代次數(shù)會(huì)更少。

1.5.2局部狀態(tài)恢復(fù)

計(jì)算運(yùn)動(dòng)預(yù)測(cè)公式(10)和測(cè)量更新公式(8)時(shí)，只需要知道狀態(tài)均值的子集，而不是總為完整的狀態(tài)均值向量求解，可以將公式(11)分為兩組耦合方程:

(12)

這種劃分所說(shuō)的就是地圖的“局部”，從而能夠不時(shí)地對(duì)地圖的局部部分進(jìn)行解析。通過(guò)目前的固定估計(jì)，可以解決公式(12)的估計(jì)：

(13)

公式(13)提供了一種恢復(fù)局部地圖估計(jì)的方法，對(duì)局部部分的估計(jì)是對(duì)實(shí)際平均值的近似值。在其他情況下(例如，由于閉環(huán)的結(jié)果)，真正的全局平均值應(yīng)該通過(guò)公式(11)來(lái)恢復(fù)。

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

本文在機(jī)器人行駛的前方和后方分別使用激光傳感器，為了便于區(qū)分，此處分別稱(chēng)之為前向激光傳感器和后向激光傳感器。所得到的數(shù)據(jù)則為前向激光和后向激光。回環(huán)的原理是機(jī)器人在未知環(huán)境中行走后掉頭(反方向行走)，如果到達(dá)曾經(jīng)到過(guò)的地方，當(dāng)前時(shí)刻其后向激光傳感器得到的數(shù)據(jù)和歷史某時(shí)刻前向激光傳感器得到的數(shù)據(jù)相似度極高。通過(guò)性狀的相似度檢測(cè)，檢測(cè)出真實(shí)的閉環(huán)，形成閉環(huán)后可以顯著地減少算法上的累積誤差[8]。為了確保前后激光是在相同時(shí)刻上進(jìn)行采樣，需要進(jìn)行前后激光傳感器的時(shí)間對(duì)準(zhǔn)[9]。

由圖1可知，前后激光是在相同的時(shí)刻進(jìn)行采樣,可以在之后的程序中進(jìn)行相同時(shí)刻上的回環(huán)檢測(cè)。

圖1 前后激光時(shí)間對(duì)準(zhǔn)

3 實(shí)驗(yàn)結(jié)果

本文利用機(jī)器人在未知環(huán)境中行駛了兩圈，機(jī)器人行走的第二圈中進(jìn)行掉頭的操作。其中圖2為機(jī)器人行駛第一圈時(shí)的位姿，使用精確稀疏滯后算法進(jìn)行位姿的估計(jì)。機(jī)器人行駛的過(guò)程中通過(guò)前后激光傳感器得到激光數(shù)據(jù)。圖3為機(jī)器人當(dāng)前時(shí)刻的前向激光數(shù)據(jù)與機(jī)器人歷史時(shí)刻的前向激光數(shù)據(jù)做回環(huán)檢測(cè)后的結(jié)果，主要進(jìn)行了歐氏距離檢測(cè)、馬氏距離檢測(cè)、AdaBoost檢測(cè)以及最近鄰檢測(cè)[10]?？梢钥闯觯瑱C(jī)器人在掉頭操作后產(chǎn)生了較為明顯的累計(jì)誤差，偏離之前的行駛路線(xiàn)。圖4為機(jī)器人在進(jìn)行前向激光回環(huán)檢測(cè)后加入后向激光數(shù)據(jù)進(jìn)行回環(huán)檢測(cè)的結(jié)果。

圖2 機(jī)器人行駛第一圈的位姿

圖3 僅使用前向激光效果圖

圖4 使用雙向激光效果圖

4 結(jié)論

通過(guò)實(shí)驗(yàn)觀(guān)測(cè)可知，使用雙向激光進(jìn)行回環(huán)檢測(cè)，檢測(cè)出真實(shí)的閉環(huán)，可以有效地減少累計(jì)誤差。同時(shí)，一個(gè)好的回環(huán)檢測(cè)算法應(yīng)當(dāng)在保證檢測(cè)出真實(shí)回環(huán)的同時(shí)降低運(yùn)算復(fù)雜性，提高時(shí)間運(yùn)行效率。

[1] SMITH &SELF M, CHEESEMAN E. Estimating uncertain spatial relationships in robotics [J]. Uncertainty in Artificial Intelligence, 1988(2): 435-461.

[2] THMN S, LIU Y, KOLLER D. Simultaneous localization and mapping with sparse extended information filters[J]. International Journal of Robotics Research, 2004, 23(7/8):693-716.

[3] EUSTICE SINGH H, LEONARD J. Exactly sparse delayed state filters[C].IEEE International Conference on Robotics and Automation, 2005:2417-2424.

[4] EUSTICE R, WALTER M, LEONARD J. Sparse extended information filters: insights into sparsification[C].IEEE RSJ International Conference on Intelligent Robots and Systems，2005: 3281-3288.

[5] 董海巍．大范圍環(huán)境下移動(dòng)機(jī)器人同步定位和地圖創(chuàng)建研究[D]．上海：上海交通大學(xué)，2008．

[6] 郭劍鋒，趙春霞，石杏喜.稀疏擴(kuò)展信息濾波SLAM算法的稀疏規(guī)則研究[J].系統(tǒng)仿真學(xué)報(bào)，2008, 20(12): 6673-6679.

[7] SMITH R, SELF M, CHEESEMAN P. Estimating uncertain spatial relationships in robotics[J]. Machine Intelligence and Pattern Recognition, 2013, 5(5): 435-461.

[8] ANGELI A, FILLIAT D, DONCIEUX S, et al. Fast and incremental method for loop-closure detection using bags of visual words[J]. IEEE Transaction on Robot,2008,24(5):1027-1037.

[9] NISHANT K, SWAGAT K, TOMOHIRO S. High performance loop closure detection using bag of word pairs[J]. Robotics and Autonomous Systems, 2015,77: 55-65.

[10] 崔大成，曾連蓀.基于視覺(jué)字典的移動(dòng)機(jī)器人閉環(huán)檢測(cè)方法研究[J].微型機(jī)與應(yīng)用,2015,34(9):85-88.

Research of SLAM algorithm based on bidirectional laser loop closure detection

Zheng Xuehe1, Sun Zuolei2

(School of Information Engineering, Shanghai Maritime Univeristy, Shanghai 201306, China)

Aiming at the problem that the mobile robot can not correct the cumulative error itself and the complexity of the traditional extended Kalman filter (EKF) algorithm, this paper proposes a method based on bidirectional laser for loop closure detection. Through the effective similarity detection algorithm, it can detect the real loop and timely correct the robot position. At the same time, we use the exactly sparse delayed state filters algorithm to supplement the natural sparsity of the information matrix to reduce the computational complexity. The experimental results show that the combination of the two methods can effectively reduce the cumulative error in the process of moving the robot.

loop closure detection; ESDF; SLAM; mobile robot

TP242

A

10.19358/j.issn.1674- 7720.2017.20.006

鄭雪鶴，孫作雷.基于雙向激光回環(huán)檢測(cè)的SLAM算法研究[J].微型機(jī)與應(yīng)用，2017,36(20)：19-22.

2017-03-31)

鄭雪鶴(1992-)，女，碩士，主要研究方向：移動(dòng)機(jī)器人導(dǎo)航。

孫作雷(1982-)，男，博士，副教授，主要研究方向：移動(dòng)機(jī)器人導(dǎo)航。