移動機(jī)器人改進(jìn)激光SLAM算法研究

陳 丹，吳 欣

西安理工大學(xué) 自動化與信息工程學(xué)院，西安710048

智能化機(jī)器人在面臨復(fù)雜不確定地形環(huán)境時(shí)如何確定自身位置，并提供優(yōu)質(zhì)的地圖具有重要的理論研究價(jià)值和實(shí)際應(yīng)用意義[1]。同步定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）[2-3]利用機(jī)器人攜帶的傳感器在未知環(huán)境下觀測目標(biāo)，根據(jù)機(jī)器人與目標(biāo)之間的相對位置和里程信息來估計(jì)機(jī)器人與目標(biāo)點(diǎn)的全局坐標(biāo)。SLAM 問題一直是機(jī)器人研究中的一個(gè)基礎(chǔ)且重要的開放性研究熱點(diǎn)。2017年，Wongsuwan等人[4]提出糾正梯度細(xì)化方法對傳統(tǒng)RBPF-SLAM進(jìn)行改進(jìn)，該改進(jìn)方法可以應(yīng)用于多種類型的機(jī)器人傳感器上。2018年，Jo等人[5]提出了一種新的映射表示方法以及由基本映射和單獨(dú)映射組成的分解方案，在保證地圖精度的基礎(chǔ)上節(jié)約環(huán)境地圖的存儲空間。2020年，楊爽等[6]提出一種通過搭建地標(biāo)數(shù)據(jù)庫和位姿推導(dǎo)模型，解決了移動機(jī)器人在SLAM過程中定位失真問題。

求解SLAM 問題的方法大致可分為基于概率估計(jì)的方法和基于非概率估計(jì)的方法兩大類?；诟怕使烙?jì)的方法主要有擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）、FastSLAM與粒子濾波（particle filter，PF）等。粒子濾波的優(yōu)點(diǎn)在于解決如狀態(tài)遞歸估計(jì)或高維非線性、非高斯時(shí)變系統(tǒng)的概率推理等復(fù)雜問題。粒子濾波作為一種有效解決SLAM 問題的方法仍然存在粒子退化問題[7]。后來，Murphy 等人[8]提出采用RBPF（Rao-Blackwellised particle filter）求解SLAM問題。2007年，Grisetti 等人[9]應(yīng)用了基于有效樣本容量的選擇性重采樣策略，在一定程度上限制了重采樣次數(shù)，但對緩解粒子耗盡的現(xiàn)象并沒有做出有效的改進(jìn)。2016年，田夢楚等人[10]提出智能優(yōu)化算法，采用優(yōu)勝劣汰機(jī)制，提高了精度，但該算法對小權(quán)重粒子吸引較小，易造成粒子退化現(xiàn)象。2018年，劉云濤[11]采用基于蝴蝶優(yōu)化的粒子濾波算法處理重采樣之后的粒子，該算法避免陷入局部最優(yōu)且改善粒子耗散現(xiàn)象，但在粒子間吸引時(shí)任意兩粒子間交互計(jì)算，實(shí)時(shí)性較差。2019年，吳正越等人[12]提出了基于最小方差重采樣方法（minimum sampling variance，MSV）的一種優(yōu)化SLAM算法，在動態(tài)環(huán)境下取得了更好的建圖和定位效果，但是算法復(fù)雜度增加且只能達(dá)到有限精度。2020年，王彥等人[13]針對重采樣過程導(dǎo)致粒子多樣性降低問題，提出一種自適應(yīng)優(yōu)化組合重采樣方法，但改進(jìn)算法只能在一定程度上提高建圖精度，對于構(gòu)建尺寸較大的復(fù)雜環(huán)境地圖還有待提升。

迭代最近點(diǎn)（iterative closest point，ICP）算法是一種點(diǎn)云匹配算法，是基于激光雷達(dá)掃描相鄰幀，尋找激光雷達(dá)兩個(gè)掃描點(diǎn)之間的轉(zhuǎn)換關(guān)系。當(dāng)掃描點(diǎn)數(shù)增大時(shí)，ICP 會承受過高的計(jì)算成本。為了提高ICP 算法的效率和準(zhǔn)確性，人們提出了許多改進(jìn)方法。2016年，Lee等人[14]提出了一種在線學(xué)習(xí)的方法來進(jìn)行三維對象配準(zhǔn)，實(shí)現(xiàn)了更好的魯棒性和穩(wěn)定收斂性，但是迭代過程中可能不能收斂到全局最優(yōu)解。2018 年，Guan 等人[15]利用體素格對點(diǎn)云數(shù)據(jù)進(jìn)行重采樣，采用尋找對應(yīng)點(diǎn)的方法來提高點(diǎn)云配準(zhǔn)效率。2019 年，Wu 等人[16]通過引入最大相關(guān)準(zhǔn)則作為相似性度量，提出一種新的魯棒尺度ICP算法。這些算法雖然提高了點(diǎn)云配準(zhǔn)效率，但也丟失了部分配準(zhǔn)精度，使得對點(diǎn)云初始位姿的選取更加嚴(yán)格。

因此本文著重提供良好的點(diǎn)云初始態(tài)，從采樣點(diǎn)的選取和匹配策略方面對激光雷達(dá)快速點(diǎn)云配準(zhǔn)算法進(jìn)行了改進(jìn)，同時(shí)針對傳統(tǒng)重采樣過程中粒子多樣性喪失問題，提出了一種改進(jìn)傳統(tǒng)粒子濾波重采樣方法，最后基于ROS系統(tǒng)進(jìn)行真實(shí)較復(fù)雜場景下的地圖構(gòu)建。

1 RBPF-SLAM的算法改進(jìn)

1.1 改進(jìn)ICP算法

ICP 算法配準(zhǔn)的目標(biāo)是通過一個(gè)三維變換矩陣將源點(diǎn)云集p={pi} 轉(zhuǎn)換為目標(biāo)點(diǎn)云集q={qj}，且盡可能使兩個(gè)點(diǎn)云坐標(biāo)在同一位置重合。兩點(diǎn)云位置配準(zhǔn)誤差為：

求解誤差函數(shù)式（1）的最小值，獲得下一次位姿變換估計(jì)值：

式中，“⊕”表示旋轉(zhuǎn)平移操作符，Sref為旋轉(zhuǎn)平移參考面，k為匹配算法迭代次數(shù)，索引i和j分別代表點(diǎn)云p和q中的點(diǎn)，R和T分別為旋轉(zhuǎn)和平移矩陣。ICP算法原理簡單、直觀，但算法的效率和全局最優(yōu)收斂取決于初始迭代值。

主成分分析（principal component analysis，PCA）是一種有效的簡化檢測數(shù)據(jù)集方法，可以減少數(shù)據(jù)集維數(shù)，反映數(shù)據(jù)集對方差貢獻(xiàn)的最大特征。因此，相似度大的兩片點(diǎn)云，通過PCA 算法只要把其參考坐標(biāo)系調(diào)整到一致，即可達(dá)到粗配準(zhǔn)目的，粗配準(zhǔn)為點(diǎn)到線ICP點(diǎn)云精配準(zhǔn)提供了良好的點(diǎn)云初始變換。

傳統(tǒng)ICP 配準(zhǔn)算法采用兩點(diǎn)間歐氏距離最近點(diǎn)作為配準(zhǔn)點(diǎn)，易受到噪聲的干擾。因此，本文在對應(yīng)點(diǎn)匹配中，將最近的點(diǎn)度量從兩點(diǎn)之間的歐氏距離改變?yōu)榈絻牲c(diǎn)連線的垂直距離，對點(diǎn)云采用PCA 粗配準(zhǔn)后再進(jìn)行點(diǎn)到線ICP算法精確配準(zhǔn)，以提高匹配精度和收斂速度，實(shí)現(xiàn)移動機(jī)器人的精確定位。點(diǎn)到線ICP解決曲面匹配問題，可以表示如下：給出一個(gè)參考Sref和一組點(diǎn)云{pi}，最小化點(diǎn)pi旋轉(zhuǎn)平移到其在表面Sref上投影的距離：

式中ni為曲面在投影點(diǎn)處的法線，Qk是對點(diǎn)云粗配準(zhǔn)后得到的初始變換值。

基于點(diǎn)到線的改進(jìn)ICP配準(zhǔn)算法步驟：

（1）設(shè)置距離誤差閾值δ，改進(jìn)算法采用點(diǎn)到線的距離作為誤差；

（2）將當(dāng)前幀的點(diǎn)云數(shù)據(jù)根據(jù)初始位姿投影到參考坐標(biāo)系中；

（3）對于點(diǎn)云中的每一個(gè)點(diǎn)pi，在參考幀中找到與它最近的兩個(gè)點(diǎn)和；

（4）計(jì)算配準(zhǔn)誤差，誤差計(jì)算采用下式：

（5）當(dāng)誤差小于誤差閾值δ，停止配準(zhǔn)迭代，否則返回步驟（2）。

1.2 改進(jìn)重采樣策略

優(yōu)化的重采樣算法對于粒子濾波來說既可保持樣本的有效性和多樣性，還可提高濾波器的性能。傳統(tǒng)重采樣方法完全丟棄了權(quán)值較小的粒子，多次迭代后削弱了粒子的多樣性，導(dǎo)致收斂精度不足。針對該問題，本文提出一種改進(jìn)重采樣方法，將所有粒子按權(quán)值進(jìn)行分類，根據(jù)預(yù)先設(shè)定的粒子權(quán)值閾值wt，在大權(quán)重和小權(quán)重粒子集合內(nèi)分別進(jìn)行采樣，最后對新的粒子集進(jìn)行權(quán)重歸一化，進(jìn)入下一步濾波過程。與傳統(tǒng)重采樣方法相比，改進(jìn)重采樣著重于在大權(quán)重集合內(nèi)進(jìn)行采樣，大量復(fù)制粒子，同時(shí)在小權(quán)重集合內(nèi)采用隨機(jī)重采樣方法獲取粒子，在保證高權(quán)重粒子數(shù)的同時(shí)低權(quán)重粒子的選取機(jī)會增大，增加粒子多樣性，進(jìn)而提高機(jī)器人狀態(tài)的估計(jì)精度。

改進(jìn)重采樣算法步驟如下：

（1）設(shè)定重采樣門限閾值Nth，當(dāng)有效粒子數(shù)Neff=（其中表示歸一化權(quán)值），小于閾值Nth時(shí)，進(jìn)行重采樣步驟（2），否則進(jìn)行下一時(shí)刻的濾波過程。

（2）對權(quán)重大于閾值wt的粒子進(jìn)行重采樣，將高權(quán)值粒子進(jìn)行復(fù)制生成M個(gè)粒子，復(fù)制次數(shù)為粒子總數(shù)N與粒子權(quán)重的乘積取整floor(N×w)，高權(quán)值粒子集記為，其中r=1,2,…,M表示粒子個(gè)數(shù)。

（3）對權(quán)重小于閾值wt的粒子，進(jìn)行隨機(jī)重采樣生成低權(quán)值粒子集記為，其中j=1,2,…,N-M表示粒子個(gè)數(shù)。

（4）分別對步驟（2）和步驟（3）產(chǎn)生的兩部分粒子進(jìn)行排序，得到新的粒子集。

（5）對新的粒子集進(jìn)行權(quán)重歸一化，完成后面的濾波過程。

1.3 改進(jìn)RBPF-SLAM算法

RBPF-SLAM算法[17]將SLAM問題分解為機(jī)器人位姿估計(jì)和基于估計(jì)得到的位姿進(jìn)行地圖構(gòu)建兩部分進(jìn)行描述。核心思想是利用外部傳感器信息z1:t=z1,z2,…,zt-1和里程計(jì)信息u1:t=u1,u2,…,ut-1估計(jì)環(huán)境地圖m和機(jī)器人軌跡x1:t=x1,x2,…,xt的聯(lián)合后驗(yàn)概率密度p(x1:t,m|z1:t,u1:t-1)，該后驗(yàn)概率為：

式中，p=p(x1:t|z1:t,u1:t-1)表示利用激光雷達(dá)觀測信息和里程計(jì)信息估計(jì)機(jī)器人位姿；p(m|x1:t,z1:t)表示通過當(dāng)前機(jī)器人姿態(tài)更新地圖m。

本文對傳統(tǒng)RBPF-SLAM 算法從掃描匹配和粒子重采樣兩方面進(jìn)行改進(jìn)，改進(jìn)后的RBPF-SLAM算法步驟如下：

（1）點(diǎn)云匹配。當(dāng)機(jī)器人在移動過程中獲得來自激光雷達(dá)的掃描數(shù)據(jù)時(shí)，采用本文改進(jìn)點(diǎn)云配準(zhǔn)算法，對機(jī)器人位姿進(jìn)行精確估計(jì)。

（2）抽樣。對上一代第i個(gè)粒子估計(jì)得到當(dāng)前時(shí)刻粒子。選取激光雷達(dá)匹配結(jié)果作為優(yōu)化提議分布，如式（6）所示：

（3）更新權(quán)重。根據(jù)權(quán)重計(jì)算公式（7）計(jì)算出每個(gè)粒子的權(quán)值。

（4）重采樣。當(dāng)Neff

（5）更新地圖。根據(jù)第i個(gè)粒子表示的移動機(jī)器人位姿和當(dāng)前觀測情況，對第i個(gè)粒子構(gòu)建的地圖進(jìn)行更新。

2 地圖構(gòu)建算法性能仿真與分析

本文基于激光雷達(dá)點(diǎn)云配準(zhǔn)算法和粒子濾波重采樣算法兩方面對RBPF-SLAM 算法進(jìn)行了改進(jìn)。針對這兩種改進(jìn)策略，分別從算法配準(zhǔn)精度、估計(jì)性能以及算法運(yùn)行時(shí)間進(jìn)行了性能仿真分析，同時(shí)將本文所改進(jìn)重采樣粒子濾波算法與傳統(tǒng)重采樣粒子濾波以及文獻(xiàn)[12]所提MSV算法進(jìn)行了性能對比。

2.1 改進(jìn)點(diǎn)云配準(zhǔn)算法性能

用激光雷達(dá)數(shù)據(jù)集對原始ICP算法和本文改進(jìn)ICP算法進(jìn)行點(diǎn)云匹配仿真實(shí)驗(yàn)，原點(diǎn)云數(shù)據(jù)與目標(biāo)點(diǎn)云數(shù)據(jù)的配準(zhǔn)結(jié)果如圖1 所示，其中藍(lán)色點(diǎn)云代表源點(diǎn)云，紅色點(diǎn)云代表目標(biāo)點(diǎn)云。圖1（a）為原始點(diǎn)云分布，圖1（b）為傳統(tǒng)ICP配準(zhǔn)后點(diǎn)云分布，圖1（c）為本文改進(jìn)ICP算法配準(zhǔn)后點(diǎn)云分布。由圖1 可知，圖（c）中紅色覆蓋面積大于圖（b），因此本文改進(jìn)配準(zhǔn)算法的點(diǎn)云配準(zhǔn)準(zhǔn)確度更高。在機(jī)器人定位過程中，優(yōu)良的配準(zhǔn)算法可以進(jìn)一步提高激光雷達(dá)點(diǎn)云的配準(zhǔn)精度，從而實(shí)現(xiàn)移動機(jī)器人的精確定位。

圖1 傳統(tǒng)ICP和改進(jìn)ICP算法配準(zhǔn)后點(diǎn)云分布Fig.1 Point cloud distribution after registration of traditional ICP and improved ICP algorithms

此外，本文對傳統(tǒng)ICP 算法和改進(jìn)ICP 算法的運(yùn)行時(shí)間進(jìn)行了比較，對于相同點(diǎn)云數(shù)據(jù)集，傳統(tǒng)ICP 算法進(jìn)行點(diǎn)云配準(zhǔn)運(yùn)行時(shí)間需要12.537 s。而本文不但改進(jìn)了精配準(zhǔn)算法，同時(shí)增加了粗配準(zhǔn)，改進(jìn)精配準(zhǔn)算法與原始ICP配準(zhǔn)算法相比，算法運(yùn)行時(shí)間大大減小。因此加上粗配準(zhǔn)時(shí)間依舊比原始ICP 精配準(zhǔn)算法運(yùn)行時(shí)間短，總共時(shí)間為8.063 s，比傳統(tǒng)ICP 算法直接配準(zhǔn)時(shí)間節(jié)省了4.474 s，由此說明改進(jìn)算法實(shí)時(shí)性更好。

2.2 改進(jìn)重采樣算法性能

為了驗(yàn)證本文基于改進(jìn)重采樣方法的粒子濾波狀態(tài)估計(jì)性能，選取典型的單變量非平穩(wěn)模型進(jìn)行仿真分析。該模型具有典型的非線性特征，且后驗(yàn)分布具有雙峰特性，其狀態(tài)方程和觀測方程分別為：

其中，k為狀態(tài)時(shí)間，x(k)為狀態(tài)向量，z(k)為觀測向量，v(k-1)和n(k)是零均值高斯隨機(jī)變量，其方差分別為Q(k-1)和R(k)。

對目標(biāo)位置估計(jì)誤差性能采用均方根誤差（RMSE）進(jìn)行評價(jià)：

其中，x(k)和(k)為k時(shí)刻的濾波狀態(tài)真實(shí)值和估計(jì)值，N是蒙特卡羅模擬次數(shù)。

本文對改進(jìn)重采樣粒子濾波、MSV算法[12]和傳統(tǒng)重采樣粒子濾波三種算法進(jìn)行了性能對比。仿真中Q(k-1)和R(k)分別取10 和1，狀態(tài)初始值x(0)=0.1,N取100次[18]，粒子數(shù)設(shè)定為100。圖2 是三種粒子濾波算法對狀態(tài)估計(jì)值的仿真對比，圖3是三種粒子濾波算法狀態(tài)估計(jì)均方根誤差。由圖2、圖3可知，本文提出的改進(jìn)重采樣粒子濾波狀態(tài)估計(jì)值與真實(shí)值接近，均方根誤差均小于其他兩種算法，說明本文所提算法提高了粒子的多樣性和估計(jì)精度。

圖2 三種重采樣算法濾波狀態(tài)估計(jì)Fig.2 Three resampling algorithms for filtering state estimate

圖3 三種重采樣算法濾波均方根誤差Fig.3 Three resampling algorithms for RMSE

圖4為原始重采樣算法、MSV算法和本文改進(jìn)重采樣粒子濾波三種算法濾波后粒子分布情況。由圖4 可知改進(jìn)重采樣粒子濾波算法擬合精度高，大部分粒子分布緊密靠近真值，只存在個(gè)別粒子樣本發(fā)散，可以有效提高地圖估計(jì)精度。

圖4 三種重采樣算法濾波后粒子分布Fig.4 Particle distribution after filtering by three resampling algorithms

為了進(jìn)一步分析改進(jìn)算法的實(shí)時(shí)性，對傳統(tǒng)重采樣、MSV 算法和本文改進(jìn)重采樣粒子濾波三種算法在相同初始粒子數(shù)情況下進(jìn)行多次仿真實(shí)驗(yàn)，統(tǒng)計(jì)算法運(yùn)行時(shí)間均值，如表1 所示。可以看出，在相同粒子個(gè)數(shù)下，改進(jìn)算法的運(yùn)行時(shí)間低于傳統(tǒng)算法以及MSV算法，驗(yàn)證了本文所改進(jìn)重采樣粒子濾波算法在地圖構(gòu)建具有一定的實(shí)時(shí)性。

表1 三種算法運(yùn)行時(shí)間對比Table 1 Comparison of running time of three algorithms

3 Turtlebot機(jī)器人SLAM實(shí)驗(yàn)

在實(shí)際情況下，機(jī)器人的工作環(huán)境比仿真環(huán)境更加復(fù)雜，環(huán)境特征更加豐富，存在行人等不確定因素。為了驗(yàn)證改進(jìn)算法在這些情況下的有效性，分別采用傳統(tǒng)重采樣算法、MSV 算法以及本文改進(jìn)重采樣與配準(zhǔn)算法，基于Turtlebot機(jī)器人對西安理工大學(xué)教五樓室內(nèi)和室外走廊環(huán)境分別進(jìn)行了SLAM 實(shí)驗(yàn)。構(gòu)建地圖時(shí)所需的粒子個(gè)數(shù)如表2所示，每個(gè)環(huán)境重復(fù)10次實(shí)驗(yàn)。實(shí)驗(yàn)場景1是室外走廊，實(shí)驗(yàn)場景2為室內(nèi)，由于室內(nèi)障礙物較多，環(huán)境復(fù)雜，構(gòu)建地圖所需粒子數(shù)多于室外所需粒子數(shù)。

表2 構(gòu)建地圖粒子數(shù)Table 2 Number of particles to construct a map

圖5（a）和（b）分別為進(jìn)行地圖構(gòu)建的室外走廊和室內(nèi)真實(shí)環(huán)境。

圖5 地圖構(gòu)建真實(shí)場景Fig.5 Map building real scene

采用三種RBPF-SLAM算法構(gòu)建的網(wǎng)格圖如圖6和圖7 所示。由圖6 和圖7 紅色圈標(biāo)記地方可以看出，本文所提改進(jìn)算法構(gòu)建的地圖符合真實(shí)場景，原始算法構(gòu)建的地圖在拐角處存在偏移和重影問題，而改進(jìn)算法構(gòu)建的地圖更清晰、更整齊。對比圖6（a）、（b）和（c），圖（a）中原始算法沒有構(gòu)建出障礙物完整的形狀，圖（b）中MSV 算法對障礙物的構(gòu)建不夠完整，地圖中滅火器和電梯口位置構(gòu)建不清晰。而圖（c）中本文改進(jìn)重采樣與點(diǎn)云配準(zhǔn)算法構(gòu)建的地圖信息更加精確地反映環(huán)境信息，線條較為清晰，表示障礙物對應(yīng)所在位置的單元柵格狀態(tài)更為確定，同時(shí)對于電梯口等拐角位置信息的構(gòu)建更加完整。

圖6 實(shí)驗(yàn)室外走廊環(huán)境下構(gòu)建地圖效果圖Fig.6 Effect of map constructed in experimental outdoor corridor environment

圖7 中間部分箭頭所標(biāo)注的紅色圈為室內(nèi)擺放的障礙物，可以看出室內(nèi)靠墻兩邊的書桌位置以及中間矩形障礙物的形狀被清晰地構(gòu)建出來。圖7（c）所構(gòu)建的障礙物效果也明顯優(yōu)于采用原始構(gòu)圖算法獲得的圖7（a）和基于MSV的構(gòu)圖算法獲得的圖7（b）。對于障礙物的大小、形狀和位置構(gòu)建效果提升也很明顯，可以建立正確且一致性良好的環(huán)境地圖，算法的定位精度得到了一定程度上的改善。構(gòu)建地圖的差異主要是因?yàn)樵糞LAM算法在粒子濾波重采樣過程中，注重復(fù)制大權(quán)值粒子和頻繁的重采樣，導(dǎo)致粒子多樣性喪失、粒子退化，從而導(dǎo)致機(jī)器人位姿與估計(jì)位姿不重合，地圖構(gòu)建過程中出現(xiàn)地圖傾斜與墻體重影。而MSV算法采用計(jì)算最小采樣方差的方式，使得重采樣前后粒子分布保持一致，差異達(dá)到最小化，緩解了樣本貧化問題，但是計(jì)算復(fù)雜度增加，粒子分布比較集中，缺乏多樣性，只能達(dá)到有限精度。本文所提改進(jìn)算法克服了原始算法和MSV算法的缺點(diǎn)，并且由表2 還可以看出，改進(jìn)算法用于地圖構(gòu)建，用更少的粒子數(shù)，構(gòu)建出更加精確的地圖，減小了算法計(jì)算復(fù)雜度，進(jìn)一步驗(yàn)證了改進(jìn)算法構(gòu)建環(huán)境地圖的有效性。

圖7 實(shí)驗(yàn)室環(huán)境下構(gòu)建地圖效果圖Fig.7 Effect of map rendering in laboratory environment

4 總結(jié)

本文提出了一種基于激光雷達(dá)的改進(jìn)RBPF 算法。采用改進(jìn)ICP算法完成激光掃描點(diǎn)集配準(zhǔn)后，通過引入改進(jìn)重采樣的粒子濾波算法，減少了粒子退化，在保留好粒子并創(chuàng)造新的有用粒子基礎(chǔ)上增加粒子多樣性，提高了移動機(jī)器人的定位性能和構(gòu)建地圖的準(zhǔn)確性。將其與傳統(tǒng)SLAM算法進(jìn)行了性能仿真對比，改進(jìn)算法的估計(jì)精度優(yōu)于傳統(tǒng)SLAM算法，能更準(zhǔn)確地表達(dá)真實(shí)粒子的狀態(tài)。最后利用Turtlebot 機(jī)器人在真實(shí)場景下對改進(jìn)算法地圖構(gòu)建進(jìn)行實(shí)驗(yàn)。結(jié)果表明改進(jìn)SLAM算法比原始SLAM算法構(gòu)建地圖準(zhǔn)確性更高，能清晰地表達(dá)障礙物的形狀及其位置信息，更能清晰準(zhǔn)確體現(xiàn)所構(gòu)建環(huán)境地圖的細(xì)節(jié)及邊緣信息。