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

鄭雪鶴，孫作雷

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

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

鄭雪鶴1，孫作雷2

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

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

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

0 引言

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

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

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

1 ESDF算法推導(dǎo)

1.1 狀態(tài)增量

當需要存儲一個新視圖時，通過增加一個新的狀態(tài)延時來形成狀態(tài)增量，以便在之后的時間里回顧之前的狀態(tài)[6]。

1.1.1增加一個延時狀態(tài)

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

(1)

這個分布代表著一個地圖M和當前機器人的狀態(tài)xt、所有的觀測量zt以及輸入的控制量μt。在xt+1時刻獲得分布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)

下式描述的是一般非線性馬爾可夫機器人運動模型以及它的一階線性形式，F(xiàn)是在μt下的估計雅克比矩陣，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)的線性條件下，增加的狀態(tài)分布式(2)仍然是高斯的，它的協(xié)方差形式由下式給出：

(4)

其中，

1.1.3增量的信息形式

(5)

1.2 馬爾可夫精確稀疏

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

(6)

1.3 狀態(tài)更新

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

(7)

(8)

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

(9)

1.4 預(yù)測模型

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

(10)

這里

Ω=Λxtxt+FTQ-1

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

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

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

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

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

Λtμt=ηt

(11)

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

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

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

(12)

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

(13)

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

2 回環(huán)檢測

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

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

圖1 前后激光時間對準

3 實驗結(jié)果

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

圖2 機器人行駛第一圈的位姿

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

圖4 使用雙向激光效果圖

4 結(jié)論

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

[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] 董海?。蠓秶h(huán)境下移動機器人同步定位和地圖創(chuàng)建研究[D]．上海：上海交通大學，2008．

[6] 郭劍鋒，趙春霞，石杏喜.稀疏擴展信息濾波SLAM算法的稀疏規(guī)則研究[J].系統(tǒng)仿真學報，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] 崔大成，曾連蓀.基于視覺字典的移動機器人閉環(huán)檢測方法研究[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)檢測的SLAM算法研究[J].微型機與應(yīng)用，2017,36(20)：19-22.

2017-03-31)

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

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