改進FastSLAM算法在移動機器人定位中的應(yīng)用

摘 要：在過去的20年里，人們提出了許多基于激光雷達的狀態(tài)估計和建圖方法。為解決傳統(tǒng)SLAM算法（同步定位與建圖算法）的缺陷，介紹了一種基于粒子濾波的SLAM算法—FastSLAM算法，并對傳統(tǒng)FastSLAM算法進行優(yōu)化，深入分析原算法的重采樣過程，改進后得出適合移動機器人定位的算法。改進重采樣算法設(shè)定了重采樣閾值，可以根據(jù)閾值的高低選擇復(fù)制粒子或保留粒子，能夠很大程度上延緩粒子衰竭現(xiàn)象出現(xiàn)，豐富粒子的多樣性，從而提高定位算法的精度。搭建硬件平臺，分析激光雷達功能及數(shù)據(jù)，對激光雷達進行前段配準。應(yīng)用MATLAB平臺對數(shù)據(jù)進行仿真分析，通過仿真結(jié)果得出，改進FastSLAM算法與未改進算法相比，誤差和精度均有較大的提升。

關(guān)鍵詞：移動機器人；FastSLAM算法；激光雷達；重采樣；機器人定位；IMU

中圖分類號：TP242 文獻標識碼：A 文章編號：2095-1302（2025）08-0-03

0 引 言

機器人產(chǎn)業(yè)的發(fā)展，能夠很好地體現(xiàn)出大國的產(chǎn)業(yè)競爭力，我國推出《中國制造2025》[1]等相關(guān)產(chǎn)業(yè)政策，從國家戰(zhàn)略層面推動機器人產(chǎn)業(yè)的繁榮發(fā)展，助力機器人技術(shù)的提升與產(chǎn)業(yè)升級。

即時定位與地圖構(gòu)建（SLAM）算法是移動機器人領(lǐng)域的一項核心技術(shù)。該算法能夠?qū)崿F(xiàn)專業(yè)的路徑規(guī)劃，減少需要消耗的時間，降低機器人由于磨損導(dǎo)致的成本支出。SLAM算法根據(jù)傳感器的不同分為視覺SLAM算法與激光SLAM算法。視覺SLAM算法依靠攝像頭進行定位，從而實現(xiàn)構(gòu)圖與路徑規(guī)劃，激光SLAM算法則利用激光雷達及里程計來實現(xiàn)定位與構(gòu)圖[2]。本文著重研究基于激光SLAM算法的移動機器人定位問題。

1 移動機器人平臺及相關(guān)算法

1.1 移動機器人硬件組成

移動機器人平臺硬件系統(tǒng)涉及多個模塊，主要涵蓋感知、驅(qū)動、控制、機械等方面。感知模塊涉及兩大傳感器（內(nèi)外兩種），前者包括IMU跟編碼器，后者指激光雷達；驅(qū)動模塊的核心是驅(qū)動系統(tǒng)，整個系統(tǒng)由驅(qū)動輪、從動輪等構(gòu)成，為平臺提供動力支持，電源一般選擇鋰電池，以提供電力支持?？刂颇K包括兩部分，第一部分是工控機（上位機軟件系統(tǒng)）；第二部分是嵌入式控制器，一般應(yīng)用在下位機中。機械模塊涉及車身跟安全裝置，其中車身主要由底盤、車架構(gòu)成，但側(cè)重的模塊、設(shè)備存在一定的差異。安全裝置主要指障礙報警器，其可以在出現(xiàn)障礙時發(fā)出報警提醒。同時還安裝有防撞條，具有一定的緩沖作用[3-4]。

1.2 感知單元及主要參數(shù)

激光雷達安裝在機器人上進行環(huán)境信號的采集。從技術(shù)角度出發(fā)，能夠?qū)⒓す饫走_細分成三角測距與TOF兩類，前者的精度不高、采樣率較低，后者不論精度還是采樣率都能夠很好地滿足需求，并且測量距離遠，所以選擇TOF激光雷達作為此次研究的感知單元。

TOF激光雷達在發(fā)射激光脈沖環(huán)節(jié)，不論是激光出射時間還是反光接收時間，都需要借助計時器進行精確記錄，并根據(jù)飛行的具體情況，對激光雷達、反射物間的距離進行估算[5]。激光雷達相關(guān)參數(shù)見表1。

2 移動機器人軟件系統(tǒng)

2.1 軟件系統(tǒng)整體架構(gòu)

移動機器人平臺軟件系統(tǒng)涉及兩大模塊，分別是上、下位機系統(tǒng)。上位機軟件系統(tǒng)功能包括：利用串口與下位機進行動態(tài)、有效的信號傳輸；獲得下位機傳輸?shù)臄?shù)據(jù)；利用話題形式，完成指令的發(fā)布；依據(jù)運動指令，對左右輪的轉(zhuǎn)速進行計算，并對下位機發(fā)出精準的控制指令等。下位機軟件系統(tǒng)主要包括運動控制等。

2.2 激光SLAM系統(tǒng)框架

激光SLAM系統(tǒng)包括激光里程計與后端算法改進兩部分。前者輸入的數(shù)據(jù)源自于系統(tǒng)傳感器，輸出需要實時位姿以及子圖；由于時間會影響激光里程計的數(shù)據(jù)誤差，因此盡可能降低誤差能夠極大地提升SLAM算法的精度，所以選擇通過后端降低執(zhí)行過程當中出現(xiàn)的累計誤差[6]。

3 改進FastSLAM算法

3.1 FastSLAM算法基本構(gòu)成

運用SLAM算法能夠通過粒子濾波實現(xiàn)路徑的有效估計，但是因為其維度提升，直接應(yīng)用會導(dǎo)致計算任務(wù)過重，所以選擇降維的方式把該問題細分成定位、建圖，該手段被稱為FastSLAM。在執(zhí)行過程中，應(yīng)該達成對全部路徑以及環(huán)境路標信息的全方位估計[7]：

3.2 改進重采樣的FastSLAM算法

在移動機器人的實際定位過程中，由于自身位置存在不確定性，通常會用一系列概率密度函數(shù)來表示機器人的位姿數(shù)據(jù)。傳統(tǒng)的FastSLAM算法會導(dǎo)致少部分粒子占據(jù)過多權(quán)重，致使粒子衰竭。改進的FastSLAM算法加入重采樣過程，重新調(diào)整粒子權(quán)重，增加了粒子多樣性，能夠緩解粒子衰竭。

從圖1能夠清楚看出，跟之前的重采樣方法相比，該方法的優(yōu)化側(cè)重于大權(quán)重集合開展相關(guān)采樣操作，并在小權(quán)重集合中選擇隨機方式確定粒子，在確保高權(quán)重粒子數(shù)量的前提下，最大程度提升方法的多樣性特征[9-10]。

改進重采樣算法步驟如下：

（1）設(shè)定重采樣門限閾值，當有效粒子數(shù)不超過閾值N時，執(zhí)行步驟（2），否則進入下一時刻的濾波流程。

（2）如果粒子權(quán)重超過閾值wt，那么在執(zhí)行重采樣的過程中，需要對高權(quán)值粒子執(zhí)行復(fù)制操作，從而進一步獲得M個粒子，次數(shù)=總粒數(shù)×權(quán)重，獲得的數(shù)值需要取整floor（N×w），高權(quán)值粒子集記為whr，其中r=1， 2， ...， M。

（3）如果粒子權(quán)重低于閾值wt，通過隨機方式獲得的低權(quán)值粒子集記為wlk，其中k表示粒子個數(shù)。

（4）通過對步驟（2）、步驟（3）獲得的粒子開展排序操作，即可獲得新粒子集wnj=whr+wlk。但還需進一步執(zhí)行權(quán)重歸一化操作，實現(xiàn)濾波。

4 仿真分析

圖2描述了改進重采樣FastSLAM算法對機器人路徑x、y軸的估計誤差。

x軸誤差變化與FastSLAM存在差異，在300到850步長區(qū)間改進重采樣。執(zhí)行該操作后，y軸方向存在的誤差與該算法的整體變化無差異，其存在的誤差數(shù)值未高出該算法。改進重采樣后，該算法對路標運算的精準度更高，對改進重采樣FastSLAM算法的性能優(yōu)化更顯著。

5 結(jié) 語

本文針對粒子濾波FastSLAM展開了探討，針對其基本原理作出了具體論述，同時針對出現(xiàn)的問題進行了全方位分析，并針對原算法做出了調(diào)整升級。此次研究的主要內(nèi)容

包括：

（1）分析移動機器人的硬件結(jié)構(gòu)，重點介紹激光雷達及IMU的工作原理及配置。

（2）介紹基于粒子濾波的FastSLAM算法原理及流程，提出改進重采樣方法。

（3）分析改進重采樣方法的可行性并進行仿真驗證。

參考文獻

[1]融亦鳴，樸松昊，冷曉琨.仿人機器人建模與控制[M].北京：清華大學(xué)出版社，2021：13-19.

[2]林韓熙，向丹，歐陽劍，等.移動機器人路徑規(guī)劃算法的研究綜述[J].計算機工程與應(yīng)用，2021，57（18）：38-48.

[3]孫思凡.基于ROS移動機器人的室內(nèi)SLAM和路徑規(guī)劃算法研究[D].重慶：重慶郵電大學(xué)，2021.

[4]陳靜夷.基于改進動態(tài)窗口法的移動機器人路徑規(guī)劃研究[J].物聯(lián)網(wǎng)技術(shù)，2024，14（3）：102-107.

[5] LAN C W， LIU C H. The research of the RFID anchor assisted SLAM on an autonomous patrolling system [J]. Journal of marine science and technology， 2020， 28（5）： 394-403.

[6]簡榮貴，陳康，徐哲壯，等.基于四足機器人的工業(yè)儀表數(shù)字識別方法研究[J].物聯(lián)網(wǎng)技術(shù)，2024，14（3）：3-7.

[7]孫九博.動態(tài)環(huán)境移動機器人安全高效的路徑規(guī)劃方法研究[D].濟南：山東大學(xué)，2020.

[8]吳培良，孔令富，孔亮.一種普適機器人系統(tǒng)同時定位、標定與建圖方法[J].自動化學(xué)報，2012，38（4）：618-631.

[9] TANG M， CHEN Z， YIN F. An improved H-infinity unscented FastSLAM with adaptive genetic resampling [J]. Robotics and autonomous systems， 2020， 134： 103661.

[10]朱良恒，梁利東，賈文友，等.基于能量懲罰的多機器人任務(wù)分配優(yōu)化方法[J].井岡山大學(xué)學(xué)報（自然科學(xué)版），2022，43（2）：88-93.