基于ISRCDKF的移動機(jī)器人同時定位與建圖研究

齊詠生 孫作慧 李永亭 劉利強(qiáng)

(內(nèi)蒙古工業(yè)大學(xué)電力學(xué)院， 呼和浩特 010080)

0 引言

隨著人工智能技術(shù)的不斷發(fā)展，移動機(jī)器人的自主定位已成為機(jī)器人研究領(lǐng)域的熱點(diǎn)問題。同時定位和建圖(Simultaneous localization and mapping，SLAM)[1]是指當(dāng)移動機(jī)器人處于未知環(huán)境中時，如何憑借自身攜帶的外部傳感器獲取周圍環(huán)境信息，增量式地構(gòu)建地圖，并且在已經(jīng)構(gòu)建好的地圖上完成自主定位。這也是移動機(jī)器人實(shí)現(xiàn)智能導(dǎo)航的關(guān)鍵技術(shù)之一。在研究SLAM問題的過程中，移動機(jī)器人的自主定位與地圖的構(gòu)建密不可分，地圖的構(gòu)建依賴于機(jī)器人精確的位姿信息，而準(zhǔn)確的地圖又能促進(jìn)機(jī)器人實(shí)現(xiàn)精確的自主定位。所以有學(xué)者將SLAM中定位與建圖的關(guān)系比喻成“雞和蛋”的關(guān)系[2]，兩者相輔相成，任何一方都無法單獨(dú)實(shí)現(xiàn)。

SLAM問題主要分為基于視覺的SLAM算法和基于激光雷達(dá)的SLAM算法兩大類[3-4]。在視覺算法方面，有學(xué)者提出了RGB-D視覺SLAM算法[5-7]，該算法通過構(gòu)建深度圖像地圖解決自主定位問題。與視覺傳感器相比，激光雷達(dá)具有精度高、抗干擾能力強(qiáng)、夜間也能正常工作等優(yōu)點(diǎn)，成為目前研究SLAM問題主要使用的外部傳感器[8]。另外，SLAM問題中存在諸多不確定性因素，包括環(huán)境的不確定性、噪聲的不確定性等，所以基于概率估計(jì)的方法成為研究SLAM問題的主流思想。該方法以貝葉斯濾波估計(jì)理論為基礎(chǔ)，最早是通過擴(kuò)展卡爾曼濾波(EKF)與SLAM算法結(jié)合[9-10]，形成了EKF-SLAM框架，該算法的計(jì)算復(fù)雜度為O(L2)，L為系統(tǒng)狀態(tài)向量的維數(shù)。但是擴(kuò)展卡爾曼濾波需要進(jìn)行復(fù)雜的矩陣計(jì)算，難以滿足實(shí)時性的需要，而且EKF在線性化時舍棄了系統(tǒng)的高階項(xiàng)，導(dǎo)致誤差變大。文獻(xiàn)[11]提出了基于無跡卡爾曼濾波(UKF)的SLAM算法，計(jì)算復(fù)雜度為O(L3)。UKF是基于無跡變換(Unscented transform，UT)的一種濾波方法，不需要計(jì)算雅可比矩陣，復(fù)雜度相應(yīng)降低。但是，隨著系統(tǒng)狀態(tài)向量的增加，無跡變換中采樣點(diǎn)與中心點(diǎn)的距離越來越遠(yuǎn)，導(dǎo)致濾波發(fā)散。近年來，容積卡爾曼濾波(CKF)也被應(yīng)用于SLAM中[12]，CKF根據(jù)容積變換實(shí)現(xiàn)系統(tǒng)狀態(tài)的線性化，與UKF計(jì)算復(fù)雜度相同，而且具有更少的采樣點(diǎn)，所以理論上精度更高。然而，隨著系統(tǒng)狀態(tài)向量的增加，CKF同樣存在計(jì)算復(fù)雜的問題，無法從根本上減少算法的計(jì)算量。除此之外，還有研究提出了基于粒子濾波(Particle filter，PF)的SLAM算法[13-14]和經(jīng)過改進(jìn)的FastSLAM算法[15-19]。由于PF存在計(jì)算量大、粒子集容易退化、需要進(jìn)行重采樣等缺陷，故在SLAM的應(yīng)用中還存在一些問題。

在中心差分卡爾曼濾波(Central difference Kalman filter，CDKF)的基礎(chǔ)上，文獻(xiàn)[20]提出了迭代中心差分卡爾曼濾波算法，該算法通過Gauss-Newton迭代方法進(jìn)行觀測更新，在一定程度上提高了算法的精度。但是該算法在狀態(tài)更新中傳遞的是整個協(xié)方差矩陣，無法解決計(jì)算復(fù)雜的問題，而且Gauss-Newton迭代方法在處理該問題時性能不夠穩(wěn)定。文獻(xiàn)[21]提出了自適應(yīng)平方根中心差分卡爾曼濾波算法，該算法可以根據(jù)輸入的狀態(tài)量自行調(diào)整參數(shù)性能，能夠在一定程度上克服噪聲的影響。但是隨著計(jì)算步驟的增加，由于沒有觀測狀態(tài)的矯正，該算法的估計(jì)誤差逐漸增大，從而使系統(tǒng)失去穩(wěn)定性。

針對目前的SLAM算法普遍存在定位精度不高或計(jì)算復(fù)雜的問題，本文提出一種基于迭代平方根中心差分卡爾曼濾波的SLAM算法，簡稱為ISRCDKF-SLAM算法，計(jì)算復(fù)雜度為O(L3)。該算法使用中心差分變換原理，通過Stirling插值公式替代泰勒公式進(jìn)行因式分解[22]，降低復(fù)雜度；將平方根濾波理論引入算法中，通過傳遞噪聲協(xié)方差矩陣的平方根矩陣減少算法計(jì)算量；在迭代觀測更新的過程中，使用L-M方法引入調(diào)節(jié)參數(shù)，通過修正協(xié)方差矩陣增強(qiáng)算法的穩(wěn)定性。通過仿真測試實(shí)驗(yàn)和基于實(shí)物平臺的現(xiàn)場實(shí)驗(yàn)對算法的有效性和時間性能進(jìn)行驗(yàn)證。

1 算法原理

1.1 SLAM問題的概率學(xué)描述

圖1 SLAM問題的概率模型Fig.1 Probabilistic model of SLAM problem

因?yàn)镾LAM問題遵循馬爾可夫模型，所以可以根據(jù)車輛當(dāng)前時刻的位姿信息、控制信息和環(huán)境信息將其轉(zhuǎn)換為求下一時刻機(jī)器人位姿和環(huán)境特征地圖的聯(lián)合后驗(yàn)概率問題[23]，概率模型如圖1所示。圖中，xt為t時刻機(jī)器人的狀態(tài)向量；yt為t時刻的觀測向量；ut為t時刻的控制向量；m為路標(biāo)特征點(diǎn)。

假設(shè)環(huán)境中存在n個可觀測的路標(biāo)特征點(diǎn)，那么M={m1,…，mi,…，mn}表示環(huán)境地圖信息。根據(jù)以上參數(shù)，可以推算出t時刻機(jī)器人位姿信息和環(huán)境特征地圖的后驗(yàn)概率分布為

(1)

式中y1:t——初始時刻到t時刻的觀測信息

u1:t——初始時刻到t時刻的控制信息

p(yt|xt,M)——傳感器觀測概率模型

p(xt|xt-1,ut)——機(jī)器人運(yùn)動概率模型

p(xt,M|y1:t,u1:t)——t時刻的后驗(yàn)分布

p(xt-1,M|y1:t-1,u1:t-1)——t-1時刻的后驗(yàn)分布

η——?dú)w一化系數(shù)

1.2 中心差分變換

中心差分變換主要是利用Stirling插值公式來解決系統(tǒng)中的非線性問題。因?yàn)镾LAM本身是一個比較復(fù)雜的非線性系統(tǒng)，所以中心差分變換使用多項(xiàng)式逼近非線性方程導(dǎo)數(shù)的方法替代了泰勒公式等復(fù)雜的運(yùn)算，便于求解均值和協(xié)方差等統(tǒng)計(jì)量，可以從理論上減少算法的復(fù)雜度。中心差分的一階和二階導(dǎo)數(shù)公式為

(2)

(3)

h——中心差分半步長

δx——零均值的隨機(jī)變量

2 迭代平方根中心差分卡爾曼濾波SLAM算法

首先，使用一組加權(quán)的采樣點(diǎn)來近似系統(tǒng)的狀態(tài)分布，以解決非線性系統(tǒng)的概率估計(jì)問題。其次，將平方根濾波理論引入到算法中，同時在觀測更新中使用L-M方法優(yōu)化迭代過程，最終，形成了迭代平方根中心差分卡爾曼濾波來處理上述的SLAM問題。

假設(shè)移動機(jī)器人的狀態(tài)方程和觀測方程分別為

xt=f(xt-1,ut-1)+w

(4)

yt=h(xt)+v

(5)

式中xt-1——機(jī)器人t-1時刻的狀態(tài)向量

ut-1——機(jī)器人t-1時刻的控制向量

yt——機(jī)器人t時刻的觀測向量

w——服從于N(0,Q)的高斯白噪聲

Q——系統(tǒng)噪聲協(xié)方差矩陣

v——服從于N(0,R)的高斯白噪聲

R——觀測噪聲協(xié)方差矩陣

本文提出的ISRCDKF-SLAM算法由狀態(tài)預(yù)測過程和迭代觀測更新過程組成，具體算法步驟描述如下。

2.1 系統(tǒng)初始化

(6)

E()——求均值函數(shù)

(7)

式中 chol()——喬里斯基(Cholesky)分解函數(shù)，又稱平方根法，可以求得矩陣的平方根矩陣

2.2 狀態(tài)預(yù)測

(8)

式中ξt-1——t-1時刻的Sigma點(diǎn)，共2L+1個

每個Sigma點(diǎn)對應(yīng)的權(quán)值公式為

(9)

ξt|t-1=f(ξt-1)

(10)

(11)

式中ξt|t-1——Sigma點(diǎn)ξt-1沿非線性函數(shù)f(·)傳播得到的變量

ξi,t|t-1——變量ξt|t-1的第i列數(shù)據(jù)

(12)

(13)

(14)

式中χ1i——一階序列權(quán)值下的狀態(tài)量

χ2i——二階序列權(quán)值下的狀態(tài)量

qr()——矩陣的QR分解函數(shù)

2.3 迭代觀測更新

在觀測更新開始前首先定義初始值ψ1作為線性化參考點(diǎn)，其值等價于當(dāng)前時刻由狀態(tài)預(yù)測得到的狀態(tài)均值估計(jì)量，公式為

(15)

參數(shù)初始化后，開始進(jìn)行迭代更新，具體計(jì)算步驟為

Fork=1:j

(16)

(17)

(18)

(19)

(20)

(21)

(22)

(23)

(24)

(25)

End for

式中μk——L-M方法的參數(shù)，取0.1

β1i——一階序列權(quán)值下的觀測變量

β2i——二階序列權(quán)值下的觀測變量

2.4 狀態(tài)增廣

當(dāng)k=j時，迭代過程終止，可以得到t時刻經(jīng)過迭代更新后狀態(tài)矩陣的均值為

(26)

式中j——最大迭代數(shù)，均衡考慮實(shí)時性與精確性的要求，本算法中j=3

那么，t時刻狀態(tài)的協(xié)方差的平方根矩陣公式為

(27)

式中 cholupdate()——喬里斯基因子更新函數(shù)

通過重復(fù)進(jìn)行2.1～2.4節(jié)的步驟，最終可以實(shí)現(xiàn)基于ISRCDKF的SLAM算法在移動機(jī)器人中的應(yīng)用。

3 仿真實(shí)驗(yàn)與分析

3.1 建模

移動機(jī)器人運(yùn)動過程和傳感器觀測過程都需要適當(dāng)建模才能完成算法的實(shí)現(xiàn)。本文參照文獻(xiàn)[9]的SLAM模型，分別建立了簡化后前輪可以轉(zhuǎn)向的車輛運(yùn)動學(xué)模型和傳感器觀測模型。

機(jī)器人的運(yùn)動學(xué)模型為

(28)

式中xx(t)、xy(t)、β(t)——t時刻的狀態(tài)變量，車輛在x、y軸坐標(biāo)值和朝向角

ΔT——時間間隔

Vt——t時刻車輛速度

αt——t時刻車輛前輪轉(zhuǎn)向角

Ld——前軸心與后軸心之間的距離

觀測模型為距離-角度模型，用極坐標(biāo)表示，公式為

(29)

式中x0、y0、β0——車輛當(dāng)前的狀態(tài)參數(shù)

xi、yi——第i個特征點(diǎn)的全局坐標(biāo)值

ρi——環(huán)境特征點(diǎn)與機(jī)器人的距離

θi——環(huán)境特征點(diǎn)與機(jī)器人的夾角

3.2 仿真實(shí)驗(yàn)

實(shí)驗(yàn)使用的是內(nèi)存為8 GB、主頻為2.20 GHz的Intel(R)Core(TM)i5-5200四核處理器便攜式計(jì)算機(jī)，程序運(yùn)行環(huán)境為Matlab 2014a。

仿真實(shí)驗(yàn)是在澳大利亞悉尼大學(xué)野外機(jī)器人中心(ACFR)提供的開源SLAM算法仿真器上進(jìn)行的。仿真環(huán)境設(shè)定在一個長、寬為250 m×200 m的室外環(huán)境中，并且在該環(huán)境中放置了17個路徑點(diǎn)，用于確定機(jī)器人的運(yùn)行路徑，同時放置了35個環(huán)境特征點(diǎn)作為移動機(jī)器人SLAM過程中的外部環(huán)境觀測信息，具體的仿真環(huán)境如圖2所示。

在圖2中，“*”表示環(huán)境特征點(diǎn)，虛線表示機(jī)器人理想的行駛路徑。設(shè)定的實(shí)驗(yàn)參數(shù)有：機(jī)器人行駛速度4 m/s，速度的控制噪聲0.2 m/s，激光雷達(dá)的觀測距離25 m，觀測距離噪聲0.1 m。實(shí)驗(yàn)的具體過程為：機(jī)器人首先從原點(diǎn)(0,0)出發(fā)，按照參考路徑和環(huán)境特征點(diǎn)信息進(jìn)行SLAM運(yùn)算，逆時針行走兩圈后停止。實(shí)驗(yàn)中，將提出的ISRCDKF-SLAM算法分別同EKF-SLAM、UKF-SLAM、CKF-SLAM算法進(jìn)行對比，并對實(shí)驗(yàn)數(shù)據(jù)進(jìn)行比較分析。不同算法仿真結(jié)果如圖3所示。

圖2 仿真環(huán)境圖Fig.2 Diagram of simulation environment

圖3中，實(shí)線表示機(jī)器人實(shí)際行走的SLAM路徑，圓圈表示對環(huán)境特征點(diǎn)的匹配精確度。通過對4種算法的結(jié)果比較分析可以看出，在算法初始階段機(jī)器人行駛路徑的誤差都不大。但隨著環(huán)境信息增多，EKF-SLAM出現(xiàn)較大的誤差，這是因?yàn)镋KF在數(shù)據(jù)關(guān)聯(lián)中出現(xiàn)了錯誤，導(dǎo)致第2圈的行駛路徑徹底發(fā)散。ISRCDKF-SLAM算法與UKF-SLAM、CKF-SLAM相比，實(shí)際路徑與原始路徑偏移量最小，這是因?yàn)橥ㄟ^迭代觀測更新，會減少樣本處理時的線性化誤差，使機(jī)器人的位姿估計(jì)更加準(zhǔn)確。

圖3 不同算法仿真結(jié)果對比Fig.3 Comparison of simulation results of different algorithms

算法的性能估計(jì)標(biāo)準(zhǔn)采用均方根誤差(Root mean square error，RMSE)衡量，計(jì)算公式為

(30)

式中T——運(yùn)行時間

xk、yk——k時刻的理想位置坐標(biāo)值

4種算法的均方根誤差和仿真時間如圖4所示。分析數(shù)據(jù)可知，本文提出的ISRCDKF-SLAM算法與EKF-SLAM、UKF-SLAM、CKF-SLAM算法相比，均方根誤差分別降低了47.3%、32.7%和25.0%，驗(yàn)證了SLAM過程中定位更加準(zhǔn)確。通過仿真時間對比，可以得出ISRCDKF-SLAM算法耗時僅次于EKF-SLAM，在算法實(shí)時性上也具有一定的優(yōu)勢。

圖4 不同算法時間和均方根誤差對比Fig.4 Comparison of time and RMSE of different algorithm

3.3 “Car Park-dataset”停車場實(shí)驗(yàn)

“Car Park-dataset”停車場數(shù)據(jù)集是SLAM的標(biāo)準(zhǔn)數(shù)據(jù)集之一，該數(shù)據(jù)是在悉尼大學(xué)的一處面積為45 m×30 m的停車場環(huán)境中采集而來的。首先人為的在停車場中放置15個人工路標(biāo)當(dāng)做環(huán)境特征點(diǎn)，然后駕駛四輪皮卡型汽車(移動機(jī)器人)圍繞路標(biāo)低速行駛約2 min，采集到的數(shù)據(jù)包括：后輪線速度和前輪轉(zhuǎn)向角、路標(biāo)位置信息、經(jīng)緯度信息。由GPS傳感器測得的車輛真實(shí)軌跡如圖5所示。

圖5 車輛真實(shí)軌跡圖Fig.5 Diagram of vehicle real trajectory

通過停車場數(shù)據(jù)集實(shí)驗(yàn)，進(jìn)一步驗(yàn)證ISRCDKF-SLAM算法的有效性。實(shí)驗(yàn)中，設(shè)置機(jī)器人速度噪聲為0.5 m/s，車輪轉(zhuǎn)向噪聲為1°；傳感器觀測噪聲為0.2 m，觀測角度噪聲為1.5°。分別采用ISRCDKF-SLAM算法與UKF-SLAM、CKF-SLAM算法(此處只比較相同復(fù)雜度的SLAM算法)進(jìn)行仿真比較，使用式(30)所示的算法性能估計(jì)標(biāo)準(zhǔn)進(jìn)行數(shù)據(jù)分析。3種算法的車輛位置誤差如圖6所示，特征點(diǎn)位置誤差如圖7所示。

圖6 車輛位置誤差Fig.6 Vehicle position error

圖7 特征點(diǎn)位置誤差Fig.7 Characteristic point position error

從圖6中可以看出，UKF-SLAM算法的車輛位置誤差大多數(shù)情況下處于最大位置，因?yàn)闊o跡變換在線性化時產(chǎn)生了較大誤差。ISRCDKF-SLAM算法因?yàn)樵谟^測更新過程中使用了迭代更新，而且在整個濾波過程中都傳遞協(xié)方差矩陣的平方根因子，所以車輛的位置誤差始終最低。圖7中，UKF-SLAM算法的特征點(diǎn)位置誤差比其他兩種算法的誤差值稍大，ISRCDKF-SLAM算法的特征點(diǎn)位置誤差最小。

表1為分別獨(dú)立運(yùn)行30次仿真實(shí)驗(yàn)后，得到的3種算法的平均運(yùn)行時間和平均誤差最大值。分析表中數(shù)據(jù)可以看出本文提出的ISRCDKF-SLAM算法的誤差明顯低于其他兩種算法，而且與UKF-SLAM和CKF-SLAM算法相比，新算法的運(yùn)行時間分別減少了15.1%和10.8%。但是由于需要迭代觀測更新，在算法運(yùn)行時間上優(yōu)勢不是太明顯，這需要以后對迭代算法做進(jìn)一步的優(yōu)化。

表1 不同算法的誤差與時間對比Tab.1 Error and time comparison of different algorithms

4 實(shí)物平臺實(shí)驗(yàn)與分析

為了更近一步驗(yàn)證算法的有效性，本文搭建了四輪移動機(jī)器人平臺，如圖8所示。該機(jī)器人的主控板為樹莓派3B，搭載了頻率為1.2 GHz的64位四核ARM處理器；底盤由STM32單片機(jī)控制電機(jī)的運(yùn)動；使用的激光雷達(dá)為思嵐公司開發(fā)的2D雷達(dá)RPLIDAR A2。同時，機(jī)器人還安裝有IMU和編碼器等內(nèi)部傳感器。

圖8 移動機(jī)器人平臺Fig.8 Photos of mobile robot platform1.激光雷達(dá) 2. 12 V鋰電池 3.樹莓派 4.單片機(jī) 5.電源轉(zhuǎn)接板

使用機(jī)器人操作系統(tǒng)(Robot operating system，ROS)作為軟件開發(fā)平臺，安裝在基于樹莓派的Ubuntu 16.04 Linux系統(tǒng)下。在ROS自帶的Hector_mapping程序包的基礎(chǔ)上，分別將UKF、CKF和本文提出的ISRCDKF濾波算法移植到該程序中，形成了UKF-SLAM算法、CKF-SLAM算法和ISRCDKF-SLAM算法。在相同的實(shí)驗(yàn)場景與參數(shù)配置下，分別對這3種算法進(jìn)行對比實(shí)驗(yàn)。

4.1 走廊實(shí)驗(yàn)

首先在實(shí)驗(yàn)室外面的走廊進(jìn)行建圖實(shí)驗(yàn)，環(huán)境包括走廊和與走廊邊緣呈直角的拐角區(qū)域，實(shí)景拍攝如圖9所示。

圖9 走廊實(shí)驗(yàn)環(huán)境Fig.9 Photos of corridor experimental environment

配置好移動機(jī)器人后，將機(jī)器人放置在走廊的一端作為初始位置，然后將計(jì)算機(jī)與機(jī)器人連在同一個局域網(wǎng)下。計(jì)算機(jī)遠(yuǎn)程打開終端啟動程序包，運(yùn)行Rviz軟件接收各個節(jié)點(diǎn)發(fā)布的話題進(jìn)行實(shí)時的圖形化顯示，現(xiàn)場實(shí)驗(yàn)如圖10所示。

圖10 現(xiàn)場實(shí)驗(yàn)初始界面Fig.10 Field experiment initial interface

啟動SLAM程序，讓移動機(jī)器人繞走廊環(huán)境行駛一圈后回到起始點(diǎn)，可以構(gòu)建出走廊的環(huán)境地圖。圖11為不同的算法下構(gòu)建的地圖，圖中左上角為局部放大圖。

圖11 不同算法構(gòu)建的地圖對比Fig.11 Comparison of maps constructed by different algorithms1.拐角 2.樓梯口 3.洗刷間 4.立柱 5.走廊

通過對3種算法的建圖情況對比可以得出前2種SLAM算法構(gòu)建的地圖在走廊拐角處的數(shù)據(jù)關(guān)聯(lián)發(fā)生錯誤，均出現(xiàn)了誤匹配的現(xiàn)象，導(dǎo)致后面的建圖局部出現(xiàn)失真，走廊整體呈彎曲的狀態(tài)，基于ISRCDKF-SLAM算法構(gòu)建的地圖優(yōu)于其他兩種算法。

在建圖過程完成后，通過獲取激光雷達(dá)和機(jī)器人內(nèi)部傳感器的參數(shù)可以估算出建圖過程中的平均直線誤差和角度誤差，并且能夠得到算法運(yùn)行時間。分別對3種算法在走廊環(huán)境中做了3次建圖實(shí)驗(yàn)，通過式(30)所示的均方根誤差法對上述兩組參數(shù)作對比，得到的數(shù)據(jù)如表2所示。通過分析表中數(shù)據(jù)可以直觀看出ISRCDKF-SLAM算法在地圖的構(gòu)建上誤差更低，且算法平均耗時最短。

4.2 室內(nèi)實(shí)驗(yàn)

由于走廊是一個簡單的環(huán)境，特征點(diǎn)相對較少，為了完成不同環(huán)境下的SLAM建圖，接下來在相對

表2 走廊實(shí)驗(yàn)數(shù)據(jù)Tab.2 Corridor experimental data

復(fù)雜并且有回環(huán)的室內(nèi)進(jìn)行SLAM研究。實(shí)驗(yàn)環(huán)境如圖12所示，左圖中依次排放著3個桌子，桌子基本在同一水平線上，右圖放置了一個緊貼著墻的壁櫥。

本次實(shí)驗(yàn)使用了ISRCDKF-SLAM與CKF-SLAM(UKF-SLAM與CKF-SLAM效果相當(dāng)，此處未給出對比結(jié)果)2種算法進(jìn)行對比實(shí)驗(yàn)?？刂茩C(jī)器人在室內(nèi)走完一圈后，算法構(gòu)建的地圖如圖13所示。

圖12 室內(nèi)實(shí)驗(yàn)環(huán)境Fig.12 Indoor experimental environment

圖13 構(gòu)建的室內(nèi)地圖Fig.13 Built laboratory map1.桌子 2.桌底 3.壁櫥 4.拐角

通過對2種算法構(gòu)建的地圖進(jìn)行比對測量，得出改進(jìn)后算法構(gòu)建的地圖長寬方向的誤差更低一些，整體精度更高。同時在地圖上可以看出，算法改進(jìn)后地圖中的直角特征更為準(zhǔn)確，桌子的排列更加整齊。同樣使用均方根誤差法，將這兩種算法在室內(nèi)環(huán)境下分別獨(dú)立執(zhí)行了3次建圖實(shí)驗(yàn)，通過傳感器數(shù)據(jù)估算的平均直線誤差和角度誤差如表3所示。通過數(shù)據(jù)得出在特征點(diǎn)較多的環(huán)境下，新提出的算法依然能夠進(jìn)行更加精準(zhǔn)的自定位與地圖構(gòu)建。

表3 室內(nèi)實(shí)驗(yàn)數(shù)據(jù)誤差Tab.3 Indoor environmental data error

5 結(jié)束語

針對目前移動機(jī)器人在同時定位和建圖中存在的狀態(tài)精度不高、穩(wěn)定性差等問題，在濾波理論的基礎(chǔ)上，提出了一種基于ISRCDKF的SLAM自主定位算法。該算法使用中心差分變換處理SLAM的非線性問題，在濾波更新過程中通過直接傳遞協(xié)方差矩陣的平方根因子減少算法的復(fù)雜度，同時在迭代觀測更新過程中使用L-M優(yōu)化方法引入調(diào)節(jié)參數(shù)實(shí)時修正協(xié)方差矩陣。仿真實(shí)驗(yàn)表明，與傳統(tǒng)的EKF-SLAM、UKF-SLAM、CKF-SLAM算法相比，ISRCDKF-SLAM算法在自主定位與地圖構(gòu)建方面的均方根誤差分別降低了47.3%、32.7%和25.0%。與相同計(jì)算復(fù)雜度的UKF-SLAM算法和CKF-SLAM算法相比，新算法的運(yùn)行時間分別減少了15.1%和10.8%。搭建了移動機(jī)器人實(shí)物平臺進(jìn)一步驗(yàn)證了該算法的有效性，從而為移動機(jī)器人SLAM問題的研究提供了一個新的思路。