基于柱狀特征與隨機有限集的激光SLAM 方法研究

摘要：傳統的基于數據關聯的同時定位與建圖（simultaneous localization and mapping，SLAM）方法易引起觀測與目標之間的誤匹配，進而導致位姿估計精度下降. 結合柱狀特征提取方法和隨機有限集理論，提出一種基于序貫蒙特卡羅實現的車輛3D激光SLAM方法. 利用M估計抽樣一致性算法從分割后的點云中提取穩定的柱狀特征，捕獲單幀點云中的靜態存活特征和新生特征；在Rao-Blackwellized-概率假設密度同時定位與建圖（Rao-Blackwellizedprobabilityhypothesis density-simultaneous localization and mapping，RB-PHD-SLAM）框架中引入兩種特征，并運用序貫蒙特卡羅方法完成車輛軌跡概率密度和地圖后驗強度在幀間的傳遞，實現對環境特征和車輛位姿的同時估計. 模擬數據集和KITTI數據集試驗結果顯示，與經典的FastSLAM算法相比，本文算法使車輛定位精度提升44.99%，并使環境特征位置估計和環境特征數量估計的平均誤差分別降低49.24%和56.22%，顯著提升了SLAM的運行精度和魯棒性，有助于保障智能汽車的運行安全.

關鍵詞：智能車輛；位置測量；特征提?。浑S機有限集；序貫蒙特卡羅

中圖分類號：TP242.6 文獻標志碼：A

在智能駕駛領域中，同時定位與建圖（simultane?ous localization and mapping，SLAM）是指未知環境中的車輛利用特定傳感器獲取環境信息，同時創建地圖模型并估計自身運動的過程［1］. 與視覺傳感器相比，激光雷達不受環境光照影響，能更準確測量環境深度信息，因此激光SLAM 在自動駕駛領域得到廣泛應用［2-3］.

傳統的基于濾波器的SLAM方法通常需要建立觀測數據和特征狀態之間的數據關聯. 這些方法通常使用不同的濾波器估計車輛位姿，然后為每個特征分配一個擴展卡爾曼濾波器（extended Kalman fil?ter， EKF）來估計地圖特征的狀態. 一些經典算法通常基于數據關聯的方法實現，如EKF-SLAM算法［4］、FastSLAM算法［5］. 常用的數據關聯方法包括最近鄰數據關聯（nearest neighbor data association， NNDA）［6］，聯合相容分支定界（joint compatibility branch andbound， JCBB）［ 7］等. 采用傳感器對智能汽車的行駛環境進行檢測時，采集的數據常常會受到雜波的影響，基于數據關聯的SLAM 算法很難在這些雜波的影響下表現出較好的性能［ 8-9］. 因此，在雜波影響下如何保證車輛良好的定位和建圖效果成為SLAM領域的關鍵問題之一. 這一問題可以通過隨機有限集（random finite sets， RFS）［9］的建模方法解決. 隨機有限集可表示為由多個隨機變量組成的集合，該集合的特點是變量個數和狀態都是隨機且無序的. 在基于RFS的SLAM 算法中，地圖特征和傳感器量測將不再被建模為向量，而是采用有限集的形式進行描述. 因此消除了地圖特征與傳感器觀測之間的數據關聯過程，從而避免了由數據關聯錯誤導致的定位和建圖效果不佳的問題. 然而，地圖特征和車輛軌跡的聯合后驗密度是耦合的，這制約了基于隨機有限集的SLAM 方法的實際應用. 因此， Grisetti 等［10］、Adams 等［11］采用Rao-Blackwellized 粒子濾波器（Rao-Blackwellized particle filter， RBPF）對其進行解耦，解決了實際應用問題. 根據濾波器的不同類型，RFS-SLAM可以分為基于概率假設密度（probabilityhypothesis density， PHD）濾波器［12］及其變種方法的SLAM，基于多伯努利（multi-Bernoulli， MB）濾波器［13］及其變種方法的SLAM. 在以上算法中，車輛軌跡估計均依賴于粒子濾波器來實現，而地圖特征的后驗密度傳遞均依賴于基于隨機有限集的濾波器來實現. 對于這些濾波算法的實現，常用的方法是利用高斯混合模型（Gaussian mixture model， GMM）［14］， 該模型可視為由多個單一高斯模型組合而成. 使用高斯模型的原因在于其具備良好的數學性質和計算性能，但同時在處理非線性和非高斯問題上存在困難.針對這一問題，Vo 等［15］提出了基于序貫蒙特卡羅（sequential Monte Carlo， SMC）實現的概率假設密度濾波器，該方法不采用線性高斯近似模型，從原理上避免了GMM方法的局限性.

雖然基于濾波的SLAM技術已經取得了許多進展，但在智能汽車領域的應用中仍存在一些問題. 首先，智能汽車的行駛環境復雜，SLAM算法的核心之一是位置識別，該算法主要為規劃層提供定位信息［16-17］，這要求算法在雜波、漏檢等影響下仍能準確且穩定地估計環境特征和車輛位姿. 其次，實際應用于智能車輛的是3D 激光雷達［18］，而傳統的PHDSLAM算法主要適用于2D 激光雷達，難以適配. 最后，當前基于濾波的SLAM 算法處理點云時會將所有點視作特征，這不僅引入了許多不穩定的特征，同時大量的3D點云數據也會在一定程度上影響算法的實時性.

為了解決上述問題，本文提出一種結合柱狀特征與隨機有限集的車輛激光SLAM 方法. 該方法無須進行觀測與目標的數據匹配，能夠同時準確估計環境特征和車輛位姿，具有高精度和魯棒性. 本文的主要貢獻包括：1）將傳統針對2D激光雷達的SLAM擴展至3D，并采用基于SMC 的RB-PHD-SLAM 算法，實現智能汽車環境特征和車輛位姿的同時估計.2）利用智能駕駛環境中穩定且豐富的柱狀特征，規避了直接處理原始點云數據所帶來的不穩定特征增多和實時性差的問題. 3）通過模擬數據集和KITTI數據集的驗證，證實了該方法的有效性.

1 系統架構

本文提出的基于柱狀特征與隨機有限集的車輛激光SLAM 系統架構如圖1 所示，主要包含4 個模塊. 在柱狀特征提取模塊中，鑒于3D激光雷達點云數據量龐大且包含噪聲，直接將所有點視作特征會引入許多不穩定特征，因此首先對原始數據進行預處理，特別是設置了地面點去除和點云分割步驟，以最大限度保障柱狀特征提取過程的效率和提取結果的準確性. 點云分割的結果是若干簇表示不同物體的點云集合，針對這些點云簇，接下來基于M估計抽樣一致性（M-estimator sample consensus， MSAC）算法［19］提取柱狀特征，并利用半徑閾值篩選出真實的柱狀物. 在位姿預測模塊中，通過建立速度運動模型，將上一時刻的位姿遞推得到當前車輛位姿的預測值. 在RB-PHD濾波模塊中，考慮了存活目標和新生目標，通過預測、更新、多目標狀態提取以及重采樣步驟，利用加權粒子傳遞多目標后驗概率密度的強度函數. 在位姿與地圖更新模塊中，首先對全局地圖進行更新，然后對每個粒子的權重進行更新，通過更新的權重計算車輛位姿，使系統能夠進入下一次迭代.

2 位姿預測與柱狀特征提取

貝葉斯濾波器中的運動模型描述了車輛狀態隨控制輸入的變化關系. 考慮到智能汽車的實際運行場景，采用三自由度運動學模型［20］并引入高斯白噪聲對運動進行描述. 智能汽車的運行環境一般為市區道路和高速路，這些場景通常包含大量穩定的柱狀物體，如信號燈桿，行道樹，交通指示牌立柱等.Ren等［21］采用了區域生長算法和聚類算法得到柱狀點云類，表明柱狀特征的穩定性. 穩定的特征有利于更精準定位，因此本文在特征提取階段充分發掘柱狀物體，并將其作平面投影后在濾波器中進行處理.

2.1 位姿預測模型

位姿預測模型示意圖如圖2所示，x、y、θ 分別代表車輛橫軸坐標、縱軸坐標和航向角，因此車輛位姿可表示為x = [ x，y，θ ]T. t 時刻車輛位姿的概率密度可表示為：

p（xt|ut - 1，xt - 1 ） （1）

式中：ut - 1 = [ vt - 1，wt - 1 ]T 為控制輸入，vt - 1 為t - 1時刻的車輛速度，wt - 1 為車輛的橫擺角速度；xt - 1 為車輛位姿.

本研究中用來預測汽車狀態的三自由度運動學模型定義如下：

式中：Δt 為t - 1 時刻到t 時刻的時間間隔；v?t - 1 =vt - 1 + ξvt - 1 和w?t - 1 = wt - 1 + ξwt - 1 表明傳感器量測數據存在一定的誤差，ξt - 1 = [ ξ vt - 1，ξwt - 1 ]T 表示均值為0的高斯白噪聲.

2.2 點云處理與柱狀特征提取

柱狀特征主要代表市區和高速場景中大量穩定的柱狀物體，如信號燈桿、行道樹、交通指示牌立柱等. 由于激光雷達點云具有數據量大、噪聲數據多的特點，柱狀物體和周圍環境融合且結構尺寸相對固定. 因此，需要對點云進行一系列處理，以提取高質量的柱狀特征. 基于激光雷達原始點云和環境中柱狀物體的特性，本文設計了以下點云處理程序：點云預處理、點云分割和柱狀特征提取. 選擇點云處理程序中的每一個步驟所使用的方法時，主要考慮不同方法的適應性、魯棒性和復雜度差異.

2.2.1 點云預處理

1）點云降采樣. 圖3（a）顯示了HDL-64E激光雷達采集的KITTI 數據集單幀點云數據，該幀包含125 347個點. 為滿足實時性要求和保持柱狀特征形狀，采用隨機下采樣方法（random down-samplingmethod， RDM）［ 22］剔除一定比例的點云. 圖3（b）表明了該方法在減小數據量的同時盡量保留原始數據的形狀特征.

2）車輛點、遠距離點去除. 通過直通濾波器，清除來自采集車輛本身的數據（這些數據對算法性能無貢獻且會產生額外干擾）. 同時，清除遠距離的稀疏點云. 圖3（c）展示了去除X 軸±3.5 m、Y 軸±1.9 m以內，X 軸±40 m、Y 軸±30 m以外的點云.

3）點云去噪. 柱狀特征提取的過程對噪聲較為敏感，因此采用半徑濾波器去除離群點，得到的點云如圖3（d）所示.

4）地面點去除. 地面點通常占據原始點云數據的一半以上［23］. 然而，對于定位與建圖任務，提取的特征只針對地面上的物體. 大量的地面點不僅影響算法實時性，而且會對接下來的點云分割造成不利影響. 因此，需要對地面點云進行擬合和清除，本文采用隨機抽樣一致（random sample consensus，RANSAC）算法［22］對地面進行處理，如圖3（e）所示.

2.2.2 點云分割

點云分割的目標是將具有相同或相似屬性的點云數據分組，以減少后續特征提取階段的計算量并提高準確性. 由于自動駕駛場景的復雜性和多變性，無法提前確定聚類數量，因此采用基于密度的含噪應用空間聚類（density-based spatial clustering ofapplications with noise，DBSCAN）［24］方法對點云進行分割. 然而，這種方法會分割出包含較少激光點的點云簇，這類點云簇通常穩定性較差，因此SLAM算法更關注環境中較大的物體. 由此，需要設置點云數量閾值來清除這類點云簇. 經過試驗，本文選用30作為數量閾值，以保留環境的主要特征，同時去除包含較少激光點的點云簇，得到的點云如圖3（f）所示.

2.2.3 柱狀特征提取

柱狀特征提取的效果將直接影響SLAM算法的效果. 基于模型擬合的柱狀特征提取方法效果較好，但對離群點較為敏感. 不過，在點云預處理和分割階段已對離群點進行了抑制，因此本文選用MSAC算法提取柱狀特征. 由于該算法能在幾乎所有的點云簇中找到符合要求的圓柱，因此所得的圓柱體半徑差別很大. 例如，一個表示墻面的點云簇可擬合出一個半徑很大的圓柱. 為了剔除不符合實際的柱狀物，本文設置了半徑閾值為0.2 m，在KITTI數據集2011_09_30_drive_0027_sync下第878幀的試驗結果中，得到了如圖4所示的柱狀特征. 對于每個柱狀特征，篩選出圓柱體中軸線和水平面夾角大于85°的柱狀物，并計算中軸線中點，用以近似表示該柱狀物在二維平面上的位置.

3 基于柱狀特征與隨機有限集的SLAM 方法

車輛三自由度運動學模型表征了智能汽車的運動特性，而柱狀特征則代表了在智能汽車運行環境中穩定的靜態物體. 因此，本文將這兩者融入RBPHD濾波器框架，并利用SMC方法進行實現. 這一融合使得傳統針對2D激光雷達的SLAM技術得以拓展至3D空間，在雜波和漏檢的影響下仍然能夠精確地估計智能汽車的環境特征和車輛位姿.

3.1 RB-PHD-SLAM 算法流程

基于隨機有限集的SLAM在貝葉斯框架下可描述為：已知車輛信息和傳感器量測信息的前提下，估計環境中靜態特征Mt 和車輛軌跡X0：t 的聯合后驗密度pt （Mt，X0：t|Z1：t，U1：t，X0 ）. 定義如下縮寫：

pt|t - 1 （Mt，X0：t ） = pt|t - 1 （Mt，X0：t|Z1：t，U1：t - 1，X0 ）（3）

pt （Mt，X0：t ） = pt （Mt，X0：t|Z1：t，U1：t - 1，X0 ） （4）

式中：U1：t - 1 為車輛控制量；Z1：t 為量測信息；pt （Mt，X0：t ） 為車輛軌跡與地圖特征的后驗密度；pt|t - 1 （Mt，X0：t ）為對應的預測后驗密度.

基于RFS的地圖和位姿估計的遞歸式為［8］：

式中：δ 定義了集合積分計算方法；gt （Zt|Xt，Mt ） 表示獲取量測信息的似然函數，該函數是在車輛位姿和地圖已知的條件下進行的. 式（5）和式（6）已經包含了由運動過程噪聲、傳感器測量噪聲和傳感器檢測準確度和虛警等引起的誤差，即建立遞歸方程的過程已經考慮了特征狀態和數量的可變性.

FastSLAM算法［5］采用解耦的思想解決了集合積分無法直接計算的問題. 基于此，本文采用RB-PHD濾波器將式（4）解耦為：

pt （Mt，X0：t ） =pt （X0：t|Z1：t，U1：t - 1，X0 ）pt （Mt|Z1：t，X0：t ） （7）

根據式（5）和式（6），聯合后驗密度的遞歸過程可以表示為先傳遞軌跡的后驗密度，然后再傳遞地圖特征的后驗密度的過程，因此式（7）右側兩項可表示為：

式（8）表示地圖特征狀態的后驗強度，式（9）表示車輛軌跡的后驗概率密度. 以上過程已采用解耦的思想解決了集合積分無法直接計算的問題. 此處將地圖特征和量測信息都表示為RFS，因此任意多個目標狀態后驗概率密度的傳遞過程可以同時進行.

3.2 基于SMC 實現的RB-PHD-SLAM 算法

本研究中，對于式（8），采用基于SMC 實現的PHD濾波器來進行傳遞；對于式（9），采用粒子濾波來進行傳遞. t - 1時刻的聯合后驗密度為：

{w（k） t - 1，X （k 0：t） - 1，M1（：kt） - 1 （?|X0（：kt） - 1 ）}Nk = 1（10）

式中：w（k） t - 1 表 示 第 k 個 粒 子 的 權 重 ；X0（：kt） - 1 =[ X0（k），X1（k），X2（k），…，X（k） t - 1 ]表示第k 個粒子在不同時刻估計得到的車輛位姿；M1（：kt） - 1 （?|X0（：kt） - 1 ）為從開始到t-1時粒子 k 的特征集合 . M1（：kt） - 1 由若干粒子進行加權，即

式中：Nt - 1，m 為粒子數量；w（i） t - 1，m 和m（i） t - 1 分別為地圖特征的后驗強度中某個粒子的權重和狀態.式（12）為按照同樣的方式得到的t 時刻的聯合后驗密度.

在算法中引入位姿預測模型和柱狀特征，實現從式（10）至式（12）的遞歸過程. 根據式（1）的運動模型預測t 時刻第k 個粒子的車輛狀態：

X （k） t|t - 1 ～ p（?|X （k） t - 1，Ut - 1 ） （13）

式中：X （k） t - 1 和Ut - 1 分別表示t - 1時刻車輛位姿和控制量. 對于每一個預測的車輛位姿，將車輛坐標系下的量測信息轉換到世界坐標系下：

Zt，veh →X （k） t|t - 1 Zt，world = {z1，world，z2，world，…，znt - 1，world} （14）

SLAM問題中往往只關心環境中的靜態物體，因此對特征狀態的預測只考慮存活和新生特征，則重采樣后有：

pt - 1 = {τ（i） t - 1，l（i） t - 1}Lt - 1i = 1（15）

式中：Lt - 1 表示t - 1時刻重采樣后的粒子數量；τ（i） t - 1表示第i 個粒子的權重；l（i） t - 1 表示第i 個粒子的狀態；pt - 1 為重采樣后的粒子集合.

對于地圖存活特征預測，因為傳感器噪聲和算法本身存在誤差，因此在對存活特征狀態進行預測時，需要在重采樣的粒子狀態基礎上人為疊加噪聲.則式（15）中的粒子集合在t 時刻的預測狀態為：

式中：pt|t - 1 表示對存活特征進行預測得到的粒子集；εt 為高斯白噪聲，且有τ（i） t|t - 1 = τ（i） t - 1.

式中：k 表示蒙特卡羅試驗序號；N 表示試驗總次數；T 為單次試驗的總步數；x?t 和xt 分別為車輛位置的估計量和真實值.

為了量化評估不同SLAM 算法的特征估計效果，采用最優子模式分配（optimal sub-pattern assign?ment， OSPA）［26］方法作為地圖特征估計精度評價指標. 對于狀態空間中的兩個有限子集X 和Y，其勢分別為m 和n. 若m ≤ n，則兩個子集的OSPA距離為：

式中：p 為均值階數；Ωn 為排列的集合；yσ （i） 表示兩個子集中對應元素形成最優點分配；d（c） （?，?）表示兩個點之間的距離；c 表示最小截止距離.

4.1 模擬數據集試驗

4.1.1 試驗環境搭建及參數設置

圖5展示了模擬數據集試驗環境，車輛的真實運行軌跡用綠色曲線表示，真實的固定特征用綠色圓點表示，傳感器的量測結果用黑色小點表示.

圖6（a）表示圖5中軌跡的真實角速度. 試驗過程中，將實際的車輛角速度和速度分別和高斯函數疊加后當作傳感器獲得的信息. 圖6（b）為圖5中紅色圓圈所代表的傳感器檢測范圍，黑色小點表示真實特征產生的量測. 若某特征未被檢測到，則形成漏檢；若被檢測到，則在真實位姿上疊加高斯噪聲輸出. 紅色小叉表示服從泊松分布的雜波，在檢測范圍內均勻生成. 更多數據集試驗參數見表1.

4.1.2 模擬數據集試驗結果及分析

圖7為一次蒙特卡羅試驗的車輛軌跡和地圖特征對比. 相比于FastSLAM算法，本文算法的車輛軌跡與真實軌跡更加貼近；本文算法的地圖特征絕大部分與真實特征更加吻合. 以上結果說明，本文算法在雜波和漏檢環境下能夠更準確地估計車輛位姿和地圖特征，顯示出更強的魯棒性和準確性. 本文算法同時估計特征的數量和位置，避免了單獨管理特征帶來的內存和效率問題.

一次蒙特卡羅試驗結果往往是隨機的，統計結果能更加綜合地評價算法的性能. 為了更準確地對比不同算法的性能，本文在同樣試驗條件下，分別對算法開展了50 次蒙特卡羅試驗，并采用RMSE 和OSPA 分別對車輛定位精度和特征估計精度進行評估.

圖 8展示了進行50次蒙特卡羅試驗后，車輛定位精度對比. 由圖8可以看出，相較于FastSLAM 算法，在大部分試驗中，本文算法的軌跡保持更低的RMSE誤差. 此外，在50次試驗中，FastSLAM算法的均值誤差為3.89 m，而本文算法為2.14 m，降低了44.99%. 因此，本文算法產生的軌跡誤差更小、更穩定，可有效地提高車輛定位精度.

圖9展示了進行50次蒙特卡羅試驗后，地圖特征估計精度對比. 圖9中，OSPADist為估計的特征與真實的特征集合間總的OSPA誤差，OSPALoc為相應的位置誤差，OSPACard為相應的數量誤差. 對于這3個指標，本文算法僅在少數試驗中高于FastSLAM算法，而在絕大多數試驗中明顯低于FastSLAM 算法.具體地，對于OSPADist、OSPALoc 和OSPACard 的平均值，本文算法分別為5.600 m、1.643 m和3.334 m，FastSLAM則為11.200 m、3.237 m和7.615 m. 本文算法在3個指標上分別降低了50%、49.24%和56.22%.因此，可以明顯看出，本文算法更為精確地估計了地圖特征的位置和數量，基于本文算法可以有效提升地圖特征的估計精度.

4.2 KITTI 數據集試驗

4.2.1 試驗環境介紹

本研究基于真實場景采集的KITTI 數據集2011_09_30_drive_0027_sync（以下簡稱drive_0027）和2011_09_26_drive_0036_sync（以下簡稱drive_0036）進行算法對比驗證. 其中算法估計的地圖特征是環境中真實柱狀物體在地面上的投影，因為KITTI數據集缺乏對環境柱狀物位置的詳細描述，因此本節僅針對車輛軌跡進行對比分析.

4.2.2 KITTI數據集試驗結果及分析

圖10展示了針對KITTI數據集中drive_0027和drive_0036場景下兩種算法的車輛軌跡和地圖特征的對比結果. 由圖10可以看出，相較于FastSLAM算法，本文算法在這兩個真實場景中所估計的車輛軌跡更接近真實軌跡. 為了更加直觀地對軌跡結果進行對比，將軌跡投影到X 軸和Y 軸，如圖11所示. 由圖11可以看出，本文算法估計的軌跡在X 和Y 軸上都更接近真實位置，這進一步驗證了該算法在車輛定位精度方面的優勢.

圖12 展示了兩種算法在drive_0027 和drive_0036場景中進行50次蒙特卡羅試驗后的車輛定位精度對比結果. 由圖12可以看出，在大多數試驗中，相較于FastSLAM算法，本文算法保持更低的RMSE誤差. 具體而言，對于50次試驗，FastSLAM在drive_0027 場景下的均值誤差為4.53 m，在drive_0036 場景下為6.05 m，而本文算法分別為2.08 m和2.56 m，分別降低了54.08%和57.69%. 這表明本文算法得到的軌跡誤差更小，且更為穩定，使用本文算法能夠有效提升車輛定位精度.

4.3 運行效率分析

為了驗證本文算法的運行效率，在同樣的硬件平臺和場景設置條件下，對本文算法和FastSLAM算法的運行時間進行對比. 硬件平臺采用一臺配備12th Gen Intel Core i5-12600KF處理器的臺式電腦.首先，在MATLAB平臺上實現本文算法和FastSLAM算法；然后，在4.1節描述的仿真環境下進行50次蒙特卡羅試驗，并記錄每一幀的運行時間；最后，計算所有單幀運行時間的平均值，如表2所示. 由表2可知，相較于FastSLAM算法，本文算法的運行時間略有增加，增幅為13.60%. 這與算法本身的編寫方式和較為嚴格的參數設置有關. 盡管如此，本文算法在保持運行時間增加不多的前提下，顯著提升了算法的性能.

5 結 論

為了提升智能車輛的定位精度和特征估計精度，本文結合柱狀特征提取方法和隨機有限集理論，提出了一種基于SMC的車輛3D激光SLAM方法. 該方法首先進行點云預處理、點云分割和柱狀特征提取步驟，以獲取單幀點云中的靜態存活特征和新生特征；隨后，在 RB-PHD-SLAM框架中引入這兩種特征，通過SMC方法完成車輛軌跡概率密度和地圖后驗強度在幀間的傳遞，實現同時估計環境特征和車輛位姿. 這種方法將傳統基于PHD-SLAM框架的2D激光SLAM擴展到3D，并避免了處理原始點云所帶來的計算量大和穩定性差的問題. 模擬數據集和KITTI數據集的試驗結果表明，與經典的FastSLAM算法相比，本文算法對車輛定位精度的提升超過了40%，并使環境特征位置和數量的估計平均誤差分別降低了40% 和50% 以上. 本文方法顯著提高了SLAM的精確度和魯棒性，有助于保障智能汽車的安全運行.

未來將嘗試引入場景中的平面特征，以擴充用于定位和建圖的特征類型和數量，從而進一步提升算法的性能.

參考文獻

1］ 韓彥峰，李君君，肖科．基于優化Gmapping算法的巷道噴漿機器人建圖研究［J］． 湖南大學學報（自然科學版）， 2023，50（6）： 118-126．

HAN Y F，LI J J，XIAO K． Research on mapping of shotcreterobot in roadway based on optimized Gmapping algorithm［J］．Journal of Hunan University （Natural Sciences），2023，50（6）：118-126．（in Chinese）

［2］ ZHENG X，ZHU J K．Efficient LiDAR odometry for autonomousdriving［J］．IEEE Robotics and Automation Letters，2021，6（4）：8458-8465．

［3］ 秦曉輝， 周洪， 廖毅霏， 等．動態環境下基于時序滑動窗口的魯棒激光SLAM 系統［J］． 湖南大學學報（自然科學版），2023， 50（12）： 49-58．

QIN X H，ZHOU H，LIAO Y F，et al．Robust laser SLAM systembased on temporal sliding window in dynamic scenes［J］．Journalof Hunan University （Natural Sciences），2023，50（12）：49-58．（in Chinese）

［4］ SMITH R C， CHEESEMAN P. On the representation andestimation of spatial uncertainty［J］. The International Journal ofRobotics Research，1986， 5（4）： 56-68.

［5］ MONTEMERLO M， THRUN S， KOLLER D， et al. FastSLAM： afactored solution to the simultaneous localization and mappingproblem［C］// Proceedings of the AAAI National Conference onArtificial Intelligence， July 28-August 1， 2002. Edmonton，Canada： AAAI， 2002： 592-598.

［6］ SINGER R A， SEA R G. New results in optimizing surveillancesystem tracking and data correlation performance in densemultitarget environments［J］. IEEE Transactions on EdmontonControl， 1973， 18（6）： 571-582.

［7］ NEIRA J，TARDOS J D． Data association in stochastic mappingusing the joint compatibility test［J］． IEEE Transactions onRobotics and Automation， 2001， 17（6）： 890-897．

［8］ MULLANE J，VO B N，ADAMS M D． Rao-Blackwellised PHDSLAM［C］//2010 IEEE International Conference on Robotics andAutomation， May 3-7，2010. Anchorage，AK， USA： IEEE，2010：5410-5416．

［9］ MULLANE J，VO B N，ADAMS M D，et al．A random-finite-setapproach to Bayesian SLAM［J］．IEEE Transactions on Robotics，2011， 27（2）： 268-282．

［10］ GRISETTI G，STACHNISS C，BURGARD W． Improving gridbasedSLAM with Rao-Blackwellized particle filters by adaptiveproposals and selective resampling［C］//Proceedings of the 2005IEEE International Conference on Robotics and Automation，April 18-22，2005. Barcelona， Spain： IEEE，2005：2432-2437．

［11］ ADAMS M，VO B N，MAHLER R，et al．SLAM gets a PHD：newconcepts in map estimation［J］． IEEE Robotics amp; AutomationMagazine， 2014， 21（2）： 26-37．

［12］ GAO L，BATTISTELLI G，CHISCI L． PHD-SLAM 2.0：efficientSLAM in the presence of missdetections and clutter［J］． IEEETransactions on Robotics， 2021， 37（5）： 1834-1843．

［13］ VO B N， VO B T， HOANG H G．An efficient implementation ofthe generalized labeled multi-Bernoulli filter ［J］ ． IEEETransactions on Signal Processing，2017，65（8）：1975-1987．

［14］ VO B N，MA W K．The Gaussian mixture probability hypothesisdensity filter［J］．IEEE Transactions on Signal Processing，2006，54（11）：4091-4104．

［15］ VO B N，SINGH S，DOUCET A．Sequential Monte Carlo methodsfor multitarget filtering with random finite sets［J］． IEEETransactions on Aerospace and Electronic Systems，2005，41（4）：1224-1245．

［16］ 韓彥峰，唐超超，肖科．基于改進ORB-SLAM2算法的RGB-D稠密地圖構建［J］．湖南大學學報（自然科學版），2023，50（2）：52-62．

HAN Y F，TANG C C，XIAO K．RGB-D dense map constructionbased on improved ORB-SLAM2 algorithm［J］．Journal of HunanUniversity（ Natural Sciences），2023，50（2）：52-62．（in Chinese）

［17］ DAI A， LUND G， GAO G. PlaneSLAM：plane-based LiDARSLAM for motion planning in structured 3D environments［EB/OL］. （2022-09-08）［2024-01-05］. https：//arxiv. org/abs/2209.08248v2.

［18］ CHGHAF M，RODRIGUEZ S，EL OUARDI A． Camera，LiDARand multi-modal SLAM systems for autonomous ground vehicles：a survey［J］． Journal of Intelligent amp; Robotic Systems， 2022，105（2）： 1-35．

［19］ SONG F，SHAO Y T，GAO W，et al. Layer-wise geometry aggregationframework for lossless LiDAR point cloud compression［J］.IEEE Transactions on Circuits and Systems for Video Technology，2021， 31（12）： 4603-4616．

［20］ KONG J， PFEIFFER M， SCHILDBACH G， et al. Kinematicand dynamic vehicle models for autonomous driving controldesign［C］//2015 IEEE Intelligent Vehicles Symposium （IV），June 28 -July 1，2015. Seoul，Korea： IEEE，2015：1094-1099．

［21］ REN Y， LIU B B， CHENG R， et al． Lightweight semanticaidedlocalization with spinning LiDAR sensor［J］ ． IEEETransactions on Intelligent Vehicles，2023，8（1）：605-615．

［22］ EBADI K，PALIERI M，WOOD S，et al． DARE-SLAM：degeneracy-aware and resilient loop closing in perceptuallydegradedenvironments［J］． Journal of Intelligent amp; RoboticSystems，2021，102（2）：1-25．

［23］ ANAND B，SENAPATI M，BARSAIYAN V，et al． LiDAR-INS/GNSS-based real-time ground removal， segmentation， andgeoreferencing framework for smart transportation［J］． IEEETransactions on Instrumentation and Measurement，2021，70：8504611．

［24］ KRISHNA G S，SUPRIYA K，BAIDYA S． 3DS-SLAM：a 3Dobject detection based semantic SLAM towards dynamic indoorenvironments［EB/OL］. （2023-10-10）［2024-01-05］. https：//arxiv.org/abs/2310.06385v1.

［25］ MULLANE J，VO B N，ADAMS M D，et al． A random setformulation for Bayesian SLAM［C］//2008 IEEE/RSJ InternationalConference on Intelligent Robots and Systems， September 22-26，2008. Nice， France： IEEE，2008：1043-1049．

［26］ ZHU Y，LIANG S，LI P，et al． Robust human pose qualityassessment using optimal sub-pattern assignment［C］//2022 11thInternational Conference on Control，Automation and InformationSciences （ICCAIS），November 21-24，2022. Hanoi，Vietnam：IEEE， 2022： 419-423.

基金項目：重慶市技術創新與應用發展專項重點項目（cstc2021jscx-dxwtBX0023）， Chongqing Technology Innovation and ApplicationDevelopment Project（ cstc2021jscx-dxwtBX0023）