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

吳官東 宋勇磊

（南京理工大學 南京 210094）

1 引言

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

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

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

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

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

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

2 方法介紹

2.1 BOSS描述子

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

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

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

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

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

對所有箱進行特征提取，按照箱與箱在空間中的位置關系排列所有二值占據狀態值，形成布爾類型的三維數組形式的BOSS 描述子（如式（4）所示）。其中，數組中的每行對應一個環，每列對應一個扇區，每個二維切片對應一層。

2.2 快速描述子相似度計算

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

圖1 點云和BOSS描述子可視化

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

由于箱只有空間和占據兩種狀態，因此可以使用計算機中的一位（bit）進行表示，以減小描述子數據的內存占用。同時，使用計算機中的位運算完成相似度計算，以縮短耗時。

正式計算前，如式（5）所示，將掃描幀BOSS描述子和局部點云BOSS 描述子均轉換為32 位無符號整型（uint32）數組表示方式。

函數?w(x) 用來計算十進制數x的漢明重量（Hamming Weight），即統計x的二進制形式中1 出現的次數。對x執行(x-1)x操作，能使x的二進制形式最后一處的1變為0。使用循環統計x不斷執行該操作直至變為0 的操作次數，即可得到的x漢明重量。

通過式（6）可以快速計算出Scan-to-Submap的相似度。該式分子統計了實際掃描描述子所有占據狀態的箱中，對應的局部地圖描述子的箱也為占據狀態的個數；通過除以用于統計實際描述子占據狀態箱數量的分母，將相似度標準化至[0,1] 。

2.3 基于描述子匹配的觀測模型

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

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

為了降低觀測模型的計算耗時，提升單位時間內處理粒子的數量，需要對全局點云地圖預采樣，生成局部點云地圖描述子采樣集。

在全局點云地圖M覆蓋的平面區域，每隔w米取一個采樣點，得到采樣點集S={si} 。

接著對局部點云地圖進行預處理，生成BOSS描述子，并使用cvt(?)函數轉換為uint32數組形式：

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

對所有采樣點執行上述處理后，得到的描述子采樣集如式（9）所示。

在觀測模型運行期間，設當前時刻的觀測為zt，一個表示機器人位姿的粒子為

首先如式（10）生成激光雷達觀測點云對應的BOSS描述子。為了保證實際掃描和全局地圖之間的方位一致性，使用rot(?,col)函數將描述子循環右移col列。

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

2.4 BOSS-MCL定位方法

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

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

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

3 實驗與分析

3.1 實驗平臺和場地

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

圖2 無人車實驗平臺

如圖3 所示，本文的實驗場地為校園內的一個環形道路（圖中白色虛線）。選擇環形道路有助于SLAM 建圖時完成回環檢測，從而生成一致性地圖。

3.2 實驗方法

本文使用C++語言和點云庫（Point Cloud Library，PCL）實現了BOSS-MCL 定位方法，并集成至機器人操作系統（Robot Operating System，ROS）中。通過和ROS官方的AMCL節點對比，驗證本文提出的BOSS-MCL方法的有效性和改進性。

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

圖4 占據柵格地圖

圖5 全局點云地圖

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

為了保證對比實驗的公平性，粒子濾波器、里程計模型、激光觀測模型均使用ROS 默認的參數。全局點云地圖描述子采樣的步長為w=0.2；BOSS描述子相關參數如表1所示。

3.3 實驗結果分析

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

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

圖7 轉彎處匹配情況圖

圖8 誤差-時間曲線圖

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

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

表2 計算耗時統計表

4 結語

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