基于雙向激光回環檢測的SLAM算法研究

鄭雪鶴，孫作雷

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

基于雙向激光回環檢測的SLAM算法研究

鄭雪鶴1，孫作雷2

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

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

回環檢測；精確稀疏滯后濾波；同時定位與地圖構建；移動機器人

0 引言

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

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

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

1 ESDF算法推導

1.1 狀態增量

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

1.1.1增加一個延時狀態

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

(1)

這個分布代表著一個地圖M和當前機器人的狀態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是在μt下的估計雅克比矩陣，w～N(0,Q)是高斯白噪聲。

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

(3)

1.1.2增量的協方差形式

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

(4)

其中，

1.1.3增量的信息形式

(5)

1.2 馬爾可夫精確稀疏

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

(6)

1.3 狀態更新

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

(7)

(8)

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

(9)

1.4 預測模型

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

(10)

這里

Ω=Λxtxt+FTQ-1

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

1.5 狀態恢復

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

1.5.1全局狀態恢復

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

Λtμt=ηt

(11)

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

1.5.2局部狀態恢復

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

(12)

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

(13)

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

2 回環檢測

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

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

圖1 前后激光時間對準

3 實驗結果

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

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

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

圖4 使用雙向激光效果圖

4 結論

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

[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] 董海巍．大范圍環境下移動機器人同步定位和地圖創建研究[D]．上海：上海交通大學，2008．

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

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

2017-03-31)

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

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