基于3D 點(diǎn)云地圖和描述子的蒙特卡羅定位方法?

吳官東 宋勇磊

（南京理工大學(xué) 南京 210094）

1 引言

位姿跟蹤是機(jī)器人領(lǐng)域中最為重要的技術(shù)之一。移動(dòng)機(jī)器人在自主導(dǎo)航過程中需要實(shí)時(shí)感知自身位姿。在未裝配GPS 的機(jī)器人上，或處于GSP無法有效工作的環(huán)境中，位姿跟蹤是實(shí)現(xiàn)機(jī)器人實(shí)時(shí)定位的主要方式。它通常是指在已知機(jī)器人上一時(shí)刻位姿的條件下，借助控制指令及機(jī)器人自身攜帶的傳感器感知信息推斷當(dāng)前時(shí)刻機(jī)器人所處的位置及朝向。

粒子濾波（Particle Filter，PF）［1］是卡爾曼濾波的非參數(shù)實(shí)現(xiàn)，它使用大量樣本實(shí)現(xiàn)對(duì)概率密度的離散近似。蒙特卡羅定位（Monte Carlo Localization，MCL）［2］，作為粒子濾波在機(jī)器人定位領(lǐng)域的應(yīng)用，是目前最為流行的位姿跟蹤算法。蒙特卡羅定位算法是一個(gè)循環(huán)迭代過程，每次迭代過程中以舊的粒子群、控制信息、觀測(cè)信息和環(huán)境地圖作為輸入，經(jīng)過位置預(yù)測(cè)、觀測(cè)更新以及重采樣三個(gè)步驟，最終輸出更新后的粒子群。

傳統(tǒng)的蒙特卡羅定位［3～6］需要機(jī)器人提供里程計(jì)和單線激光雷達(dá)的實(shí)時(shí)感知信息，同時(shí)需要提供機(jī)器人所處環(huán)境的二維占據(jù)柵格地圖（Occupancy Grid Map，OGM）［7］，該地圖通常使用Gmapping［8］、Cartographer［9］等2D 同時(shí)定位與地圖創(chuàng)建（Simultaneous Localization And Mapping，SLAM）算法建立得到。隨著光電行業(yè)的發(fā)展，多線激光雷達(dá)的價(jià)格降至民用標(biāo)準(zhǔn)，在自主導(dǎo)航機(jī)器人上得以普及，通常作為必備傳感器之一。在裝配有多線激光雷達(dá)的機(jī)器人上使用傳統(tǒng)蒙特卡羅定位，通常僅使用多線中水平射出的單線作為算法的輸入，這種方式浪費(fèi)了n線激光雷達(dá)的(n-1)/n的信息，因此在室外環(huán)境中經(jīng)常出現(xiàn)位姿跟蹤丟失的現(xiàn)象。

與此同時(shí)，以多線激光雷達(dá)傳感器作為數(shù)據(jù)來源的3D SLAM 技術(shù)也得到了快速的發(fā)展。Zhang等［10］提出了以點(diǎn)云的角點(diǎn)特征、平面特征的匹配作為幀間配準(zhǔn)技術(shù)的LOAM 算法，該算法能夠用于激光里程計(jì)或建立環(huán)境的3D 點(diǎn)云地圖。在此基礎(chǔ)上，又產(chǎn)生了如Lego-LOAM［11］、LIO-SAM［12］等改進(jìn)算法，進(jìn)一步提升了算法的實(shí)時(shí)性和生成地圖的一致性。3D SLAM 生成的全局點(diǎn)云地圖正逐漸應(yīng)用于機(jī)器人定位和導(dǎo)航技術(shù)中。

點(diǎn)云描述子是對(duì)空間中某一點(diǎn)周圍一定半徑鄰域內(nèi)所有點(diǎn)云結(jié)構(gòu)信息的描述和概括，其在保持差異性特征的前提下盡可能地壓縮信息的維度。全局描述子用一個(gè)低維的數(shù)據(jù)結(jié)構(gòu)描述整幀激光雷達(dá)點(diǎn)云。常見的描述子的設(shè)計(jì)方式為，首先將掃描點(diǎn)云所在的空間以一定的形式劃分為眾多子空間，然后使用特征統(tǒng)計(jì)或直方圖的方式對(duì)子空間內(nèi)點(diǎn)的結(jié)構(gòu)信息進(jìn)行描述，最后合并所有子空間的描述形成描述子。Cop 等［13］提出的DELIGHT 全局描述子首先使用三個(gè)相互正交的平面將三維空間等分為8 個(gè)子空間，使用兩個(gè)同心球面再次劃分，形成16 個(gè)子空間，然后利用直方圖對(duì)每個(gè)子空間內(nèi)點(diǎn)云的強(qiáng)度信息進(jìn)行統(tǒng)計(jì)，最后拼接所有直方圖形成描述子。由于直方圖統(tǒng)計(jì)過程較為耗時(shí)，該描述子在實(shí)時(shí)性方面表現(xiàn)較差。Kim 等［14～15］提出了2.5D 的Scan Context 描述子，對(duì)空間進(jìn)行徑向和朝向上的劃分，使用子空間點(diǎn)云的最大高度作為特征，形成矩陣形式的描述結(jié)構(gòu)。該描述子具有較好的實(shí)時(shí)性，但僅適用于掃描與掃描之間（Scan-to-Scan）的匹配。

為了充分利用多線激光雷達(dá)的感知信息，提高機(jī)器人導(dǎo)航過程中位姿跟蹤的魯棒性，本文對(duì)傳統(tǒng)的蒙特卡羅定位方法進(jìn)行了改進(jìn)，提出了新的觀測(cè)模型。本文首先提出了全新的適用于掃描與局部點(diǎn)云地圖之間（Scan-to-Submap）相似度匹配的周圍環(huán)境二值占據(jù)狀態(tài)（Binary Occupancy Status of Surroundings，BOSS）描述子，并設(shè)計(jì)了基于快速位運(yùn)算的描述子相似度計(jì)算方法。通過引入基于BOSS 描述子匹配的觀測(cè)模型，提出了更加魯棒的BOSS-MCL位姿跟蹤方法。

2 方法介紹

2.1 BOSS描述子

為了實(shí)現(xiàn)點(diǎn)云信息壓縮，本文設(shè)計(jì)了全新的BOSS 點(diǎn)云全局描述子。BOSS 描述子在不丟失障礙物位置及體積信息的前提下，盡可能地壓縮原始點(diǎn)云的數(shù)據(jù)量，同時(shí)提供了Scan-to-Submap 匹配的能力。BOSS描述子既可以用于描述單幀掃描點(diǎn)云，也可以用于描述局部點(diǎn)云地圖（在全局點(diǎn)云地圖中截取的半徑等于雷達(dá)最大感知距離的球形子點(diǎn)云）。該描述子的設(shè)計(jì)思想是對(duì)點(diǎn)云空間進(jìn)行三個(gè)維度上的劃分，統(tǒng)計(jì)每個(gè)子空間的二值占據(jù)狀態(tài)，形成三維的布爾數(shù)組。

為了使得生成的BOSS描述子不受地面及傳感器傾斜的影響，同時(shí)降低計(jì)算量，需要對(duì)輸入點(diǎn)云進(jìn)行預(yù)處理。預(yù)處理過程包含4 個(gè)步驟：首先使用RANSAC［16］算法擬合地面，得到地面的高度和法向量；然后對(duì)點(diǎn)云中所有點(diǎn)執(zhí)行高度校正和傾斜校正，使得地面點(diǎn)接近水平且高度為0；接著使用直通濾波去除地面點(diǎn)；最后對(duì)點(diǎn)云執(zhí)行體素網(wǎng)格濾波降采樣。

設(shè)P為經(jīng)過預(yù)處理后的點(diǎn)云，其所覆蓋球形范圍的半徑為Rlidar。對(duì)點(diǎn)云所在的空間進(jìn)行三個(gè)維度的劃分：在方位維度上將空間劃分為S個(gè)扇形區(qū)域，每個(gè)扇形的圓心角為Asector=2π/S；在徑向維度上將空間劃分為C個(gè)同心圓環(huán)，每個(gè)圓環(huán)的環(huán)寬為Wcircle=Rlidar/C；在高度維度上將空間劃分為F層，Hmin和Hmax分表表示最小、最大高度的參數(shù)，則每層的高度為Hfloor=(Hmax-Hmin)/F。將三個(gè)劃分維度相交的子空間稱為箱，最終點(diǎn)云空間被不等分為N=S×C×F個(gè)箱。式（1）中Pijk表示第i環(huán)、第j個(gè)扇區(qū)和第k層相交區(qū)域?qū)?yīng)的箱中的所有點(diǎn)：

箱與箱之間緊密無縫毗鄰且不相交，點(diǎn)云中的每個(gè)點(diǎn)屬于且僅屬于一個(gè)箱，因此如式（2）所示所有箱中的點(diǎn)的并集即為原點(diǎn)云。

式（3）中的函數(shù)Ψ(?)用于描述箱所覆蓋的子空間的信息。本文選用二值化的方式，設(shè)定閾值t，當(dāng)箱內(nèi)點(diǎn)數(shù)小于t時(shí)，將箱標(biāo)記為空閑狀態(tài)，當(dāng)箱內(nèi)點(diǎn)數(shù)大于等于t時(shí)，將箱標(biāo)記為占據(jù)狀態(tài)。此特征提取方式能夠使得描述子生成和匹配在O(n)時(shí)間復(fù)雜度內(nèi)完成，從而保證了定位方法的實(shí)時(shí)性。

對(duì)所有箱進(jìn)行特征提取，按照箱與箱在空間中的位置關(guān)系排列所有二值占據(jù)狀態(tài)值，形成布爾類型的三維數(shù)組形式的BOSS 描述子（如式（4）所示）。其中，數(shù)組中的每行對(duì)應(yīng)一個(gè)環(huán)，每列對(duì)應(yīng)一個(gè)扇區(qū)，每個(gè)二維切片對(duì)應(yīng)一層。

2.2 快速描述子相似度計(jì)算

本文設(shè)計(jì)的BOSS描述子相似度計(jì)算方法主要用于Scan-to-Submap，即用于估計(jì)實(shí)際掃描點(diǎn)云與局部點(diǎn)云地圖之間的匹配度。局部點(diǎn)云地圖，作為全局點(diǎn)云地圖的區(qū)域截取，包含更豐富的環(huán)境信息。即使從相同的位姿采集，實(shí)際掃描和局部點(diǎn)云地圖之間也存在很大的差異。圖1（a）、（b）展示了同一場(chǎng)景下兩者之間的區(qū)別。因此，如圖1（c）、（d）所示，實(shí)際掃描和局部點(diǎn)云地圖生成的描述子也存在很大差異。

圖1 點(diǎn)云和BOSS描述子可視化

BOSS描述子設(shè)計(jì)之初就是用來解決現(xiàn)有描述子不具有Scan-to-Submap 能力的問題。由于保留了原點(diǎn)云中障礙物的相對(duì)位置及體積信息，BOSS描述子可以做到實(shí)際掃描到局部點(diǎn)云地圖的單向匹配。具體思想是，對(duì)于實(shí)際掃描描述子中某個(gè)占據(jù)狀態(tài)的箱，判斷局部點(diǎn)云地圖中對(duì)應(yīng)位置的箱是否也為占據(jù)狀態(tài)。

由于箱只有空間和占據(jù)兩種狀態(tài)，因此可以使用計(jì)算機(jī)中的一位（bit）進(jìn)行表示，以減小描述子數(shù)據(jù)的內(nèi)存占用。同時(shí)，使用計(jì)算機(jī)中的位運(yùn)算完成相似度計(jì)算，以縮短耗時(shí)。

正式計(jì)算前，如式（5）所示，將掃描幀BOSS描述子和局部點(diǎn)云BOSS 描述子均轉(zhuǎn)換為32 位無符號(hào)整型（uint32）數(shù)組表示方式。

函數(shù)?w(x) 用來計(jì)算十進(jìn)制數(shù)x的漢明重量（Hamming Weight），即統(tǒng)計(jì)x的二進(jìn)制形式中1 出現(xiàn)的次數(shù)。對(duì)x執(zhí)行(x-1)x操作，能使x的二進(jìn)制形式最后一處的1變?yōu)?。使用循環(huán)統(tǒng)計(jì)x不斷執(zhí)行該操作直至變?yōu)? 的操作次數(shù)，即可得到的x漢明重量。

通過式（6）可以快速計(jì)算出Scan-to-Submap的相似度。該式分子統(tǒng)計(jì)了實(shí)際掃描描述子所有占據(jù)狀態(tài)的箱中，對(duì)應(yīng)的局部地圖描述子的箱也為占據(jù)狀態(tài)的個(gè)數(shù)；通過除以用于統(tǒng)計(jì)實(shí)際描述子占據(jù)狀態(tài)箱數(shù)量的分母，將相似度標(biāo)準(zhǔn)化至[0,1] 。

2.3 基于描述子匹配的觀測(cè)模型

觀測(cè)模型在蒙特卡羅定位方法中用于計(jì)算粒子的重要性權(quán)重。從輸入輸出的角度來看，觀測(cè)模型輸入位姿粒子、觀測(cè)數(shù)據(jù)以及地圖，輸出該粒子的概率值。

本文設(shè)計(jì)的基于描述子匹配的觀測(cè)模型的基本思想是將觀測(cè)數(shù)據(jù)即多線激光雷達(dá)掃描幀轉(zhuǎn)換為BOSS描述子，然后以粒子位置為中心，從全局點(diǎn)云地圖中截取與雷達(dá)感知距離等半徑的局部點(diǎn)云地圖，轉(zhuǎn)換為BOSS描述子，計(jì)算兩個(gè)描述子之間的相似度作為粒子的重要性權(quán)重。

為了降低觀測(cè)模型的計(jì)算耗時(shí)，提升單位時(shí)間內(nèi)處理粒子的數(shù)量，需要對(duì)全局點(diǎn)云地圖預(yù)采樣，生成局部點(diǎn)云地圖描述子采樣集。

在全局點(diǎn)云地圖M覆蓋的平面區(qū)域，每隔w米取一個(gè)采樣點(diǎn)，得到采樣點(diǎn)集S={si} 。

接著對(duì)局部點(diǎn)云地圖進(jìn)行預(yù)處理，生成BOSS描述子，并使用cvt(?)函數(shù)轉(zhuǎn)換為uint32數(shù)組形式：

最后，將“描述子· 位置”二元組加入采樣集中。

對(duì)所有采樣點(diǎn)執(zhí)行上述處理后，得到的描述子采樣集如式（9）所示。

在觀測(cè)模型運(yùn)行期間，設(shè)當(dāng)前時(shí)刻的觀測(cè)為zt，一個(gè)表示機(jī)器人位姿的粒子為

首先如式（10）生成激光雷達(dá)觀測(cè)點(diǎn)云對(duì)應(yīng)的BOSS描述子。為了保證實(shí)際掃描和全局地圖之間的方位一致性，使用rot(?,col)函數(shù)將描述子循環(huán)右移col列。

最后，如式（11）計(jì)算兩個(gè)描述子之間的相似度，作為觀測(cè)模型最終輸出的概率值。

2.4 BOSS-MCL定位方法

將基于BOSS描述子匹配的觀測(cè)模型應(yīng)用到傳統(tǒng)蒙特卡羅定位方法中，從而形成了基于已知全局3D 點(diǎn)云地圖和多線激光雷達(dá)傳感器的BOSS-MCL定位方法。BOSS-MCL 定位方法分為部署階段和運(yùn)行階段。

在部署階段中，首先需要操縱機(jī)器人運(yùn)行3D SLAM 算法，建立工作環(huán)境的全局點(diǎn)云地圖；然后在全局點(diǎn)云地圖覆蓋的平面范圍內(nèi)進(jìn)行等距離采樣，生成采樣點(diǎn)集；最后以所有采樣點(diǎn)為中心截取局部點(diǎn)云并生成BOSS描述子，得到描述子采樣集，并以文件的形式存放于磁盤中。

機(jī)器人開機(jī)后，首先將描述子采樣集加載至內(nèi)存，用戶手動(dòng)指定機(jī)器人的初始位姿，初始粒子群由以初始位姿為中心的高斯分布隨機(jī)生成。本文選用里程計(jì)采樣模型作為BOSS-MCL 的運(yùn)動(dòng)模型。在運(yùn)行階段，運(yùn)動(dòng)模型獲取里程計(jì)的增量信息，對(duì)粒子的位姿進(jìn)行更新；感知模型以激光雷達(dá)的實(shí)時(shí)感知數(shù)據(jù)為輸入，完成粒子重要性權(quán)重的計(jì)算；重采樣模塊從舊粒子群中根據(jù)權(quán)值隨機(jī)采樣，并在粒子群發(fā)散時(shí)根據(jù)KLD 規(guī)則增加一定數(shù)量的隨機(jī)粒子。此三個(gè)步驟不斷循環(huán)迭代，實(shí)現(xiàn)機(jī)器人自主導(dǎo)航過程中的實(shí)時(shí)位姿跟蹤。

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

3.1 實(shí)驗(yàn)平臺(tái)和場(chǎng)地

本文使用的實(shí)驗(yàn)平臺(tái)為一輛小型輪式無人車，如圖2 所示。該無人車采用四輪阿克曼結(jié)構(gòu)的底盤，能夠提供實(shí)時(shí)的前輪轉(zhuǎn)向角度和后輪速度信息，通過積分能夠轉(zhuǎn)換為里程計(jì)。在底盤上方安裝了以下設(shè)備：Intel Celeron J1900（1.99GHz× 4）CPU和4G內(nèi)存的工控機(jī)，為無人車提供計(jì)算能力；Velodyne HDL-32E 多線激光雷達(dá)，用于環(huán)境感知；Trimble BD982 衛(wèi)星-慣導(dǎo)組合定位系統(tǒng)（GNSS/INS），用于提供真實(shí)位姿信息。

圖2 無人車實(shí)驗(yàn)平臺(tái)

如圖3 所示，本文的實(shí)驗(yàn)場(chǎng)地為校園內(nèi)的一個(gè)環(huán)形道路（圖中白色虛線）。選擇環(huán)形道路有助于SLAM 建圖時(shí)完成回環(huán)檢測(cè)，從而生成一致性地圖。

3.2 實(shí)驗(yàn)方法

本文使用C++語言和點(diǎn)云庫（Point Cloud Library，PCL）實(shí)現(xiàn)了BOSS-MCL 定位方法，并集成至機(jī)器人操作系統(tǒng)（Robot Operating System，ROS）中。通過和ROS官方的AMCL節(jié)點(diǎn)對(duì)比，驗(yàn)證本文提出的BOSS-MCL方法的有效性和改進(jìn)性。

在實(shí)驗(yàn)過程中，首先，分別使用2D Cartographer 和Lego-LOAM 算法建立環(huán)境的占據(jù)柵格地圖（如圖4所示）和全局點(diǎn)云地圖（如圖5所示）；然后，使用rosbag 命令，手動(dòng)遙控車輛在實(shí)驗(yàn)環(huán)境行駛一圈，錄制包含單線激光（多線激光雷達(dá)的水平線束）掃描話題、激光點(diǎn)云話題消息和里程計(jì)話題消息的數(shù)據(jù)序列；最后，以柵格地圖和單線數(shù)據(jù)為輸入測(cè)試AMCL 定位效果，以點(diǎn)云地圖和多線數(shù)據(jù)為輸入測(cè)試BOSS-MCL定位效果。

圖4 占據(jù)柵格地圖

圖5 全局點(diǎn)云地圖

本文實(shí)驗(yàn)以差分（RTK）GNSS/INS 設(shè)備的位姿輸出作為機(jī)器人的真實(shí)位姿（Ground Truth），精度為厘米級(jí)。使用相同數(shù)據(jù)序列，分別對(duì)比AMCL 和BOSS-MCL在低粒子數(shù)量、高粒子數(shù)量下位姿跟蹤的跟蹤軌跡、位置誤差、航向誤差。

為了保證對(duì)比實(shí)驗(yàn)的公平性，粒子濾波器、里程計(jì)模型、激光觀測(cè)模型均使用ROS 默認(rèn)的參數(shù)。全局點(diǎn)云地圖描述子采樣的步長為w=0.2；BOSS描述子相關(guān)參數(shù)如表1所示。

3.3 實(shí)驗(yàn)結(jié)果分析

圖6 展示了實(shí)驗(yàn)過程中機(jī)器人的真實(shí)軌跡、純里程計(jì)積分軌跡，以及不同粒子數(shù)量范圍配置下的AMCL 位姿跟蹤軌跡、BOSS-AMCL 位姿跟蹤軌跡。對(duì)比圖6（a）和 圖6（b）可見，僅依靠里程計(jì)進(jìn)行位姿推算會(huì)產(chǎn)生極大的累積誤差；圖6（c）和圖6（d）展示了將粒子數(shù)量動(dòng)態(tài)范圍設(shè)置為200～500 時(shí)兩種方法的跟蹤軌跡，該實(shí)驗(yàn)結(jié)果表示在粒子數(shù)量較多的情況下，AMCL 和BOSS-MCL 均能夠順利完成整個(gè)行駛過程的實(shí)時(shí)定位，均未出現(xiàn)粒子發(fā)散及定位飄移的現(xiàn)象；為了驗(yàn)證粒子數(shù)量對(duì)定位效果的影響，將粒子數(shù)量縮減至20～50，在較少粒子的情況下，如圖6（e）所示，AMCL 方法在第二個(gè)轉(zhuǎn)彎處開始出現(xiàn)定位偏移，行駛至坐標(biāo)（-16，-15）時(shí)，粒子群發(fā)散導(dǎo)致位姿跟蹤失敗，而BOSS-MCL在粒子較少時(shí)依然順利完成了全程的位姿跟蹤，該實(shí)驗(yàn)結(jié)果表示，在粒子數(shù)量較少的情況下，BOSS-MCL 憑借信息更加豐富的3D點(diǎn)云地圖和多線激光雷達(dá)感知數(shù)據(jù)，依然能夠保持穩(wěn)定的位姿跟蹤表現(xiàn)。

圖8 繪制了兩種方法在不同粒子數(shù)量配置下，定位位置誤差和定向航向誤差隨著時(shí)間的變化曲線。圖8（b）中虛線的兩個(gè)波峰（時(shí)間220s、260s處），表示AMCL 方法在粒子數(shù)量設(shè)置為200～500，無人車行駛至第2 個(gè)和第3 個(gè)轉(zhuǎn)彎處時(shí)，出現(xiàn)較大的定向誤差。如圖7（a）、（b）所示，由于轉(zhuǎn)彎位置較為空曠，AMCL 單線激光數(shù)據(jù)與地圖匹配出現(xiàn)了較大誤差，對(duì)比圖7（c）、（d）展示的BOSS-MCL 在轉(zhuǎn)彎位置的匹配表現(xiàn)，可以得到的結(jié)論是，本文提出的BOSS-MCL方法在相對(duì)空曠的環(huán)境下，能夠取得比AMCL更好的匹配效果。

圖7 轉(zhuǎn)彎處匹配情況圖

圖8 誤差-時(shí)間曲線圖

對(duì)比圖8 中兩種方法位置誤差和航向誤差隨時(shí)間變化的趨勢(shì)，可以明顯看出，AMCL 方法的誤差會(huì)隨著定位時(shí)間的推移逐漸增加，這體現(xiàn)了在環(huán)境感知信息量不足的情況下，匹配程度適中的粒子取得更高的權(quán)值，最貼近真實(shí)位姿的粒子逐漸被淘汰，出現(xiàn)了“劣粒子驅(qū)逐良粒子”的現(xiàn)象。BOSSMCL的定位誤差隨著時(shí)間的推移，一直處于一定的范圍內(nèi)，體現(xiàn)了其粒子重采樣后的平均權(quán)值的穩(wěn)定性。

表2 展示了基于二維信息的AMCL 和基于三維信息的BOSS-MCL，完成單個(gè)粒子重要性權(quán)重計(jì)算所需要的時(shí)間，以及BOSS-MCL方法單幀點(diǎn)云數(shù)據(jù)預(yù)處理平均耗時(shí)。數(shù)據(jù)表明，BOSS-MCL通過快速位運(yùn)算，單個(gè)粒子權(quán)重計(jì)算耗時(shí)相比AMCL 減少了65%。BOSS-MCL 在增加粒子匹配信息量的同時(shí)，降低了粒子權(quán)重更新耗時(shí)。BOSS-MCL需要對(duì)每幀點(diǎn)云做預(yù)處理，考慮綜合可得，當(dāng)粒子數(shù)量大于300時(shí)，BOSS-MCL具有更好的實(shí)時(shí)性。

表2 計(jì)算耗時(shí)統(tǒng)計(jì)表

4 結(jié)語

本文通過設(shè)計(jì)BOSS激光點(diǎn)云全局描述子和基于描述子匹配的觀測(cè)模型，對(duì)傳統(tǒng)蒙特卡羅方法進(jìn)行了改進(jìn)。該觀測(cè)模型充分利用了多線激光雷達(dá)的感知信息完成粒子重要性權(quán)重的計(jì)算，從而減少了因誤匹配導(dǎo)致粒子群發(fā)散的可能性，提高了定位的魯棒性；信息量的增加使得維護(hù)較少數(shù)量的粒子依然能夠保持穩(wěn)定的位姿跟蹤；借助快速位運(yùn)算降低了粒子權(quán)重更新耗時(shí)，提高了實(shí)時(shí)性。綜上，本文提出的BOSS-MCL 位姿跟蹤方法具有較高的實(shí)用價(jià)值。