基于多傳感信息融合的語義詞袋SLAM優(yōu)化算法

摘 要：針對室外大范圍場景移動(dòng)機(jī)器人建圖中，激光雷達(dá)里程計(jì)位姿計(jì)算不準(zhǔn)確導(dǎo)致SLAM （simultaneous localization and mapping）算法精度下降的問題，提出一種基于多傳感信息融合的SLAM語義詞袋優(yōu)化算法MSW－SLAM（multi－sensor information fusion SLAM based on semantic word bags）。采用視覺慣性系統(tǒng)引入激光雷達(dá)原始觀測數(shù)據(jù)，并通過滑動(dòng)窗口實(shí)現(xiàn)了IMU （inertia measurement unit）量測、視覺特征和激光點(diǎn)云特征的多源數(shù)據(jù)聯(lián)合非線性優(yōu)化；最后算法利用視覺與激光雷達(dá)的語義詞袋互補(bǔ)特性進(jìn)行閉環(huán)優(yōu)化，進(jìn)一步提升了多傳感器融合SLAM系統(tǒng)的全局定位和建圖精度。實(shí)驗(yàn)結(jié)果顯示，相比于傳統(tǒng)的緊耦合雙目視覺慣性里程計(jì)和激光雷達(dá)里程計(jì)定位，MSW－SLAM算法能夠有效探測軌跡中的閉環(huán)信息，并實(shí)現(xiàn)高精度的全局位姿圖優(yōu)化，閉環(huán)檢測后的點(diǎn)云地圖具有良好的分辨率和全局一致性。

關(guān)鍵詞： 同時(shí)定位與實(shí)時(shí)建圖；語義詞袋；位姿估計(jì)

中圖分類號： TP242.6文獻(xiàn)標(biāo)志碼：A 文章編號： 1001－3695（2024）04－042－1247－05

doi： 10.19734/j.issn.1001－3695.2023.08.0356

Multi－sensor information fusion SLAM based on semantic word bags

Yuan Peng， Gu Zhiru， Liu Zhongwei， Jiao Longfei， Mao Qiyun

Abstract：This paper proposed an algorithm known as MSW－SLAM （multi－sensor information fusion SLAM based on semantic word bags） to address the issue of inaccurate LiDAR odometry position and pose calculations in the mapping of outdoor large－scale environments by mobile robots， resulting in a decrease in the accuracy of the simultaneous localization and mapping （SLAM） algorithm. This algorithm incorporated raw observation data from LiDAR using a visual inertial system and conducts joint nonlinear optimization of measurements from the inertial measurement unit （IMU）， visual features， and laser point cloud features using sliding windows， the algorithm leveraged the complementary semantic word bag characteristics of vision and LiDAR for closed－loop optimization， further enhancing the global positioning and mapping accuracy of the multi－sensor fusion SLAM system. Experimental results demonstrate that， compared to traditional tightly coupled binocular vision－inertial odometry and LiDAR odometry positioning， the MSW－SLAM algorithm can effectively detect closed－loop information in trajectories and achieve high－precision global pose optimization. The point cloud map after closed－loop detection exhibits excellent resolution and global consistency. Key words：simultaneous positioning and real－time mapping; semantic word bag; pose estimation

0 引言同步定位和映射是許多移動(dòng)機(jī)器人導(dǎo)航任務(wù)所需的基礎(chǔ)能力。單一感知傳感器，如激光雷達(dá)或相機(jī)，前者可以長距離捕捉環(huán)境的細(xì)節(jié)，但這種方法在無結(jié)構(gòu)環(huán)境中運(yùn)行時(shí)通常會失敗，比如長走廊或平坦的空地。后者雖然特別適合于位置識別，并且在紋理豐富的環(huán)境中表現(xiàn)良好，但它們的性能對光照變化、快速運(yùn)動(dòng)和初始化都很敏感。因此，基于激光雷達(dá)和基于視覺的方法通常都與一個(gè)慣性測量單元IMU耦合，以提高其各自的魯棒性和精度。激光雷達(dá)慣性系統(tǒng)可以幫助糾正點(diǎn)云失真，并可在短時(shí)間內(nèi)解決缺乏特征的問題。度量尺度和姿態(tài)可以通過IMU測量來恢復(fù)，以輔助可視化慣性系統(tǒng)。為進(jìn)一步提高系統(tǒng)性能，激光雷達(dá)、相機(jī)和IMU測量的融合受到越來越多的關(guān)注。 多傳感信息融合SLAM［ 2］設(shè)計(jì)方案，可分為松散耦合方法和緊密耦合方法兩大類。松耦合是一種將IMU和其他傳感器分別獨(dú)立進(jìn)行數(shù)據(jù)處理和狀態(tài)估計(jì)的方法，并通過數(shù)據(jù)融合來集成估計(jì)結(jié)果。松耦合方式的優(yōu)勢在于靈活性和模塊化，可以根據(jù)具體需求選擇適當(dāng)?shù)膫鞲衅鹘M合，并且每個(gè)傳感器的算法和處理過程可以單獨(dú)優(yōu)化和調(diào)整。目前主要的激光－IMU松散耦合SLAM算法包括LOAM［3］和LeGO－LOAM［4］，因?yàn)镮MU測量沒有在優(yōu)化步驟中使用。常見的視覺－IMU松散耦SLAM算法主要有ORB－SLAM［5］、DS－PTAM［6］、SVO［7］等。這些算法通過提取圖像特征點(diǎn)，如角點(diǎn)和線條，進(jìn)行特征匹配和三維重建。但是松耦合算法在SLAM中存在數(shù)據(jù)關(guān)聯(lián)困難、 誤差累積、實(shí)時(shí)性和延遲、對傳感器的依賴性強(qiáng)等缺點(diǎn)，本文將不采用此類方法。緊耦合是將激光雷達(dá)或攝像頭與IMU數(shù)據(jù)結(jié)合，實(shí)現(xiàn)高精度三維環(huán)境感知和定位的方法。通過時(shí)間同步、數(shù)據(jù)對齊和運(yùn)動(dòng)補(bǔ)償，將兩種傳感器的數(shù)據(jù)相互補(bǔ)償和校正，提高感知和定位的準(zhǔn)確性。緊耦合算法又分為基于濾波器的方法和基于優(yōu)化的方法?；跒V波器的方法通常使用的是擴(kuò)展卡爾曼濾波算法，其中具有代表性的算法是MSCKF［8］通過維護(hù)多個(gè)相機(jī)狀態(tài)和視覺路標(biāo)點(diǎn)的邊緣化處理，有效抑制了里程計(jì)的誤差累積，并利用多個(gè)相機(jī)的共視關(guān)系構(gòu)成多狀態(tài)約束，提高了定位和地圖構(gòu)建的精度。但其對初始位置和初始地圖的要求都很高，并且對于快速運(yùn)動(dòng)以及動(dòng)態(tài)場景的魯棒性相對較低?；趦?yōu)化的方法是將激光雷達(dá)的觀測數(shù)據(jù)和機(jī)器人的運(yùn)動(dòng)模型表示為圖結(jié)構(gòu)，通過最小化重投影誤差或基于約束的優(yōu)化，同時(shí)優(yōu)化機(jī)器人的軌跡和地圖。VINS－Mono［9］采用了相機(jī)圖像數(shù)據(jù)和IMU聯(lián)合優(yōu)化的方式，通過同時(shí)估計(jì)相機(jī)姿態(tài)、地圖點(diǎn)和相機(jī)的相對尺度，實(shí)現(xiàn)了高精度的定位和地圖構(gòu)建。OKVIS［10］提出了基于關(guān)鍵幀的滑動(dòng)窗口圖優(yōu)化策略，較好地提高了定位和地圖構(gòu)建的精度， 但對于大范圍場景的回環(huán)效果較差。目前LIO－MAP［11］、VIL－SLAM［12］、LVIO［13］、 LVI－SAM［14］等算法都是通過圖優(yōu)化的方式得到更精準(zhǔn)的軌跡和地圖。但這些算法的計(jì)算復(fù)雜度較高，對計(jì)算資源的要求嚴(yán)苛，并對大規(guī)模場景的處理存在很大的挑戰(zhàn)。綜上，現(xiàn)有的多傳感融合SLAM算法不夠完善，存在以下弊端：

a）采用松耦合方式時(shí)數(shù)據(jù)關(guān)聯(lián)困難；b）激光雷達(dá)點(diǎn)云數(shù)據(jù)容易產(chǎn)生畸變；c）在非結(jié)構(gòu)化場景中， SLAM的性能往往會受到明顯的退化，不能構(gòu)成回環(huán)。為解決這些不足，本文提出了一種基于多傳感信息融合的SLAM語義詞袋優(yōu)化算法MSW－SLAM。a）該方法通過在原始觀測值層面融合IMU量測、視覺特征和激光點(diǎn)云特征，已解決點(diǎn)云數(shù)據(jù)畸變問題；b）利用滑動(dòng)窗口進(jìn)行聯(lián)合非線性優(yōu)化，改善數(shù)據(jù)關(guān)聯(lián)困難問題；c）利用激光雷達(dá)和視覺詞袋的互補(bǔ)性，構(gòu)建雙語義詞袋進(jìn)行回環(huán)檢測優(yōu)化， 激光數(shù)據(jù)提供場景的幾何信息，而視覺數(shù)據(jù)提供了場景的語義信息。通過結(jié)合兩者的語義特征，優(yōu)化機(jī)器人在大規(guī)模場景下不能實(shí)現(xiàn)精準(zhǔn)閉環(huán)檢測的情況。

1 MSW－SLAM框架

基于圖優(yōu)化的多傳感融合算法的整體構(gòu)成如圖1所示。SLAM系統(tǒng)的前端處理階段首先對圖像數(shù)據(jù)進(jìn)行視覺特征提取和跟蹤，同時(shí)從激光點(diǎn)云數(shù)據(jù)中提取幾何特征，并根據(jù)IMU預(yù)測的位姿去除點(diǎn)云畸變。然后根據(jù)相機(jī)的視差變化篩選關(guān)鍵幀，并對相鄰關(guān)鍵幀間的IMU數(shù)據(jù)進(jìn)行預(yù)積分，生成預(yù)積分觀測值。

在SLAM系統(tǒng)的后端階段，根據(jù)不同傳感器的觀測數(shù)據(jù)可以通過構(gòu)建殘差項(xiàng)來實(shí)現(xiàn)聯(lián)合優(yōu)化。這些殘差項(xiàng)包括視覺重投影殘差、IMU預(yù)積分殘差、激光點(diǎn)云匹配殘差以及邊緣化殘差等。通過將這些殘差項(xiàng)加入非線性最小二乘問題中，利用非線性優(yōu)化方法進(jìn)行求解。為了兼顧精度和效率，通常采用滑動(dòng)窗口的方式進(jìn)行后端優(yōu)化。滑動(dòng)窗口中的狀態(tài)變量隨著時(shí)間的推移不斷更新，而即將滑出窗口的狀態(tài)則被邊緣化操作所處理。通過邊緣化操作，將即將滑出窗口的狀態(tài)變量轉(zhuǎn)換為先驗(yàn)項(xiàng)，參與后續(xù)的優(yōu)化過程，從而減少計(jì)算復(fù)雜度，并保證系統(tǒng)的穩(wěn)定性和一致性。通過以上方法，多傳感信息融合算法可以高效地利用非線性最小二乘優(yōu)化來融合不同傳感器的觀測數(shù)據(jù)，實(shí)現(xiàn)系統(tǒng)狀態(tài)的準(zhǔn)確估計(jì)和精確推導(dǎo)。

在回環(huán)檢測階段，首先提取激光雷達(dá)數(shù)據(jù)和圖像數(shù)據(jù)中的特征點(diǎn)，并與預(yù)定義的語義類別匹配構(gòu)建語義詞袋。當(dāng)新的關(guān)鍵幀出現(xiàn)時(shí)，與地圖數(shù)據(jù)庫中的語義特征進(jìn)行匹配，如果存在匹配的關(guān)鍵幀，且匹配的特征滿足一定的條件（如特征匹配數(shù)量、幾何一致性等），則判定檢測到了閉環(huán)，當(dāng)檢測到閉環(huán)時(shí)，通過點(diǎn)云精配準(zhǔn)方法對位姿圖進(jìn)行調(diào)整，糾正閉環(huán)帶來的誤差，并進(jìn)一步提高SLAM系統(tǒng)的精度和一致性。

1.1 前端數(shù)據(jù)預(yù)處理本文首先對視覺和IMU進(jìn)行數(shù)據(jù)預(yù)處理，獲得用于后端優(yōu)化的視覺特征點(diǎn)和IMU預(yù)積分觀測值，采用的是文獻(xiàn)［12］的方式。對于關(guān)鍵幀的選擇如下：基于連續(xù)跟蹤的特征點(diǎn)判斷方法用于確定當(dāng)前幀與上一幀之間的平均視差，若平均視差大于設(shè)定閾值，則當(dāng)前幀被標(biāo)記為關(guān)鍵幀，表示相機(jī)運(yùn)動(dòng)較大或場景發(fā)生顯著變化；反之，若平均視差小于等于閾值，則當(dāng)前幀為非關(guān)鍵幀，表示相機(jī)運(yùn)動(dòng)較小或場景變化不明顯。計(jì)算公式如式（1）所示。

其中：M表示的是跟蹤特征點(diǎn)的數(shù)目；k表示的是特征標(biāo)識ID；duk=ujk－uik，dvk=v－vik；u和v是像素坐標(biāo)，i和j表示的上一幀和當(dāng)前幀的序號。

對于獲得的激光點(diǎn)云數(shù)據(jù)，首先進(jìn)行特征提取，計(jì)算曲率大小并提取角點(diǎn)和面點(diǎn)。由于載體運(yùn)動(dòng)會引起點(diǎn)云的位置偏差，利用IMU預(yù)測的高頻位姿結(jié)果來矯正點(diǎn)云的運(yùn)動(dòng)畸變。這樣可以減小畸變對點(diǎn)云匹配和SLAM算法精度的影響。令tk時(shí)刻的單幀激光數(shù)據(jù)為qk，Iqk=（Xk，Yk，Zk）是點(diǎn)集Qk中的一點(diǎn)，i ∈［tk，tk+1］，此時(shí)Iqi在tk+1時(shí)的坐標(biāo)為

其中： T uti和 T utk+1分別表示在tk、tk+1時(shí)刻的姿態(tài)矩陣。對Qk中所有的點(diǎn)云數(shù)據(jù)進(jìn)行同樣的操作，最后可以得到畸變校正之后的點(diǎn)云集Qk 。

1.2 后端優(yōu)化

在多傳感器信息融合中，由于激光點(diǎn)云和其他傳感器的量測模型通常是高度非線性的，所以需要采用非線性優(yōu)化的方法對不同觀測值進(jìn)行聯(lián)合優(yōu)化。通過非線性優(yōu)化能夠以最小化觀測值與預(yù)測值之間的殘差來優(yōu)化系統(tǒng)狀態(tài)或參數(shù)的估計(jì)，使得模型的預(yù)測更接近實(shí)際觀測值。這種非線性優(yōu)化方法能夠有效地處理傳感器之間的非線性關(guān)系，提高系統(tǒng)的精確性和可靠性。通過將不同傳感器的觀測值進(jìn)行聯(lián)合優(yōu)化，多傳感信息融合算法能夠更準(zhǔn)確地估計(jì)系統(tǒng)的狀態(tài)和參數(shù)，從而提供更可靠的感知和決策基礎(chǔ)。非線性優(yōu)化方法能夠處理復(fù)雜的非線性模型，提供更靈活和準(zhǔn)確的優(yōu)化能力，從而實(shí)現(xiàn)視覺和激光點(diǎn)云等多傳感器信息的聯(lián)合優(yōu)化。本文的系統(tǒng)參數(shù)如式（3）所示。

滑動(dòng)窗口模型的核心思想是在窗口內(nèi)進(jìn)行局部優(yōu)化，以估計(jì)機(jī)器人的軌跡和地圖，同時(shí)保持計(jì)算和存儲的可行性。在優(yōu)化過程中，通過最小化觀測值與預(yù)測值之間的殘差，調(diào)整狀態(tài)變量的估計(jì)值，使其更符合實(shí)際觀測。本文滑動(dòng)窗口模型如圖2所示。滑動(dòng)窗口模型的核心思想是在窗口內(nèi)進(jìn)行局部優(yōu)化，以估計(jì)機(jī)器人的軌跡和地圖，同時(shí)保持計(jì)算和存儲的可行性。在優(yōu)化過程中，通過最小化觀測值與預(yù)測值之間的殘差，調(diào)整狀態(tài)變量的估計(jì)值。

2 MSW－SLAM回環(huán)檢測

隨著載體運(yùn)動(dòng)距離的增加，里程計(jì)誤差會逐漸累積導(dǎo)致漂移。然而，在載體運(yùn)動(dòng)過程中，軌跡通常包含閉環(huán)，這些閉環(huán)信息可以有效幫助SLAM系統(tǒng)建立先前和當(dāng)前時(shí)刻數(shù)據(jù)的關(guān)聯(lián)，進(jìn)而修正里程計(jì)的累積誤差，提高SLAM系統(tǒng)的準(zhǔn)確性和穩(wěn)定性。本文算法利用里程計(jì)輸出的關(guān)鍵幀信息構(gòu)建全局位姿圖，設(shè)計(jì)了一種激光雷達(dá)與視覺雙語義詞袋的閉環(huán)優(yōu)化算法，充分利用視覺圖像的紋理信息以及激光點(diǎn)云的深度信息，并通過TF－IDF（term frequency－inverse document frequency）進(jìn)行權(quán)重計(jì)算，使得語義詞袋能更加準(zhǔn)確地反映特征詞的重要程度，實(shí)現(xiàn)對回環(huán)關(guān)鍵幀更高效的選取，并通過閉環(huán)檢測關(guān)鍵幀之間的相對約束關(guān)系進(jìn)行軌跡優(yōu)化，從而實(shí)現(xiàn)更精確的定位和建圖效果。

2.1 語義詞袋構(gòu)建

激光語義詞袋從激光雷達(dá)數(shù)據(jù)中提取關(guān)鍵點(diǎn)，形成詞匯表，用于理解環(huán)境。視覺語義詞袋從圖像數(shù)據(jù)中提取關(guān)鍵點(diǎn)，形成視覺詞匯表，用于場景分類和物體識別。將數(shù)據(jù)的特征映射到詞匯表，形成語義詞袋。

激光語義詞袋的構(gòu)建：首先，從三維點(diǎn)云數(shù)據(jù)中提取出關(guān)鍵點(diǎn)或采樣點(diǎn)，并計(jì)算每個(gè)點(diǎn)的特征描述子，計(jì)算這些聚合關(guān)鍵點(diǎn)的均值點(diǎn)，并將它們投影到水平面；然后對平面進(jìn)行劃分，也就是描述子的維度，選擇當(dāng)前點(diǎn)到最近點(diǎn)的方向，將主方向所在的區(qū)域劃定為第一個(gè)區(qū)域，其他區(qū)域逆時(shí)針排列，并在每個(gè)區(qū)域中選擇最近點(diǎn)進(jìn)行描述（每個(gè)向量的值，也就是當(dāng)前點(diǎn)在區(qū)域中和最近點(diǎn)的距離，如果區(qū)域中沒有最近點(diǎn)就設(shè)置為0）；最后得到的描述子同時(shí)包含距離信息和方向信息。激光詞匯表中的單詞由兩部分構(gòu)成：一個(gè)是非零維度值；另一個(gè)是所在維度的ID。每個(gè)單詞對應(yīng)一個(gè)位姿信息，由兩部分組成：一部分是幀ID；另一部分是框架中的描述符ID。視覺語義詞袋是基于DBow（distributed bag－of－words）［15］算法的。它基于視覺詞袋模型，通過將圖像中的局部特征表示為視覺單詞的集合來描述圖像。首先，從輸入的圖像中提取局部特征，例如SIFT［16］、SURF［17］等算法提取描述子。然后，對這些特征進(jìn)行聚類形成視覺單詞的集合。接下來，將圖像中的局部特征映射到最接近的視覺單詞，形成圖像的量化表示。對于每個(gè)圖像，統(tǒng)計(jì)每個(gè)視覺單詞在圖像中的出現(xiàn)頻率，形成圖像的特征向量。TF－IDF是一種常用的文本特征權(quán)重計(jì)算方法。TF（term frequency）詞頻可以反映特征詞在圖像或激光數(shù)據(jù)中的出現(xiàn)頻率，即語義的相對重要程度，計(jì)算公式為

TF－IDF的計(jì)算方法就是將詞頻和逆文檔頻率相乘，它能夠綜合考慮一個(gè)詞在當(dāng)前文本中的重要性和在整個(gè)數(shù)據(jù)集中的重要性。對于語義詞袋，TF－IDF可以給每個(gè)特征詞賦予一個(gè)權(quán)重，使得常見的詞匯得到較低的權(quán)重，而在當(dāng)前文本中出現(xiàn)頻率較低但在整個(gè)數(shù)據(jù)集中較為稀有的詞匯時(shí)，得到較高的權(quán)重。TF－IDF計(jì)算公式如式（7）所示。式（5）～（7）符號含義如表2所示。

通過使用TF－IDF權(quán)重計(jì)算，可以使得語義詞袋更加準(zhǔn)確地反映特征詞的重要程度。常見的詞匯會得到較低的權(quán)重，從而減少了對模型的干擾，而稀有的詞匯會得到較高的權(quán)重，提供更多的語義信息。

2.2 回環(huán)關(guān)鍵幀檢索

對于詞袋模型描述符中的單詞，檢索單詞并計(jì)算每個(gè)出現(xiàn)的地方（數(shù)據(jù)幀）的頻率。如果最高的一個(gè)大于頻率閾值，則將該位置視為候選位置。此外，為了實(shí)現(xiàn)快速檢索，使用了逆向文檔頻率來避免檢索到出現(xiàn)在多個(gè)地方的單詞。更具體地說，由于單詞出現(xiàn)的頻率明顯高于其他單詞，它們的區(qū)別性較差，檢索效率降低。因此， 定義了一個(gè)與IDF相似的比率因子來測量當(dāng)前集合中的位置數(shù)和所有集合中的平均值之間的差異。 使用它來確定在計(jì)算位置的數(shù)量時(shí)是否應(yīng)該保留當(dāng)前單詞的位置集。設(shè)置檢索算法主要流程如下：

a）根據(jù)當(dāng)前的描述子，使用語義詞袋模型構(gòu)建單詞。

b）檢查構(gòu)建的單詞是否在預(yù)先構(gòu)建好的詞袋中，如果在詞袋中則進(jìn)行下一步，否則構(gòu)建下一個(gè)單詞。

c）獲取與當(dāng)前單詞相關(guān)的位置集，這些位置表示在之前的幀中提取到具有相似特征的圖像區(qū)域。

d）對位置集中出現(xiàn)的位置進(jìn)行逆向文本頻率的計(jì)算。如果逆向文本頻率過高，說明該單詞在整個(gè)數(shù)據(jù)集中出現(xiàn)的頻率較高，可能不具有很好的區(qū)分能力，因此舍棄該單詞和位置集進(jìn)行下一次循環(huán)。

e）否則將位置集中的位置分配到搜索表中，并統(tǒng)計(jì)位置的頻率。將相應(yīng)的位置頻率加1。

f）從搜索表中獲取位置頻率最大的位置。

g）如果最大頻率的位置頻率大于設(shè)定的閾值，并且與詞袋產(chǎn)生有效匹配，則將該位置設(shè)為候選閉環(huán)，并返回該位置作為檢索結(jié)果。

h）如果沒有找到符合條件的位置，則返回- 未找到有效閉環(huán)關(guān)鍵幀。

對于篩選出的候選回環(huán)幀及其相關(guān)關(guān)鍵幀，進(jìn)行回環(huán)優(yōu)化，在回環(huán)優(yōu)化完成后，將更新后的回環(huán)幀和相關(guān)關(guān)鍵幀的位姿和地圖信息反饋給SLAM系統(tǒng)。

回環(huán)關(guān)鍵幀檢索算法的核心思想是通過提取關(guān)鍵幀圖像的角點(diǎn)并計(jì)算描述子，然后利用字典查詢得到當(dāng)前關(guān)鍵幀的向量描述，并將其存儲在數(shù)據(jù)庫中用于閉環(huán)檢索，比較當(dāng)前關(guān)鍵幀與數(shù)據(jù)庫中關(guān)鍵幀的相似性，如果檢測到閉環(huán)幀，即高度相似的幀，將其作為候選幀。然而，為了確保準(zhǔn)確性，需要進(jìn)一步驗(yàn)證閉環(huán)幀，尤其是時(shí)間上連續(xù)的閉環(huán)。這種連續(xù)時(shí)間檢測到的閉環(huán)被認(rèn)為更可靠，算法會進(jìn)行進(jìn)一步處理和驗(yàn)證。當(dāng)視覺候選關(guān)鍵幀與激光候選幀的位姿信息處于一定的閾值范圍內(nèi)判定找到了回環(huán)關(guān)鍵幀。隨后對當(dāng)前關(guān)鍵幀與閉環(huán)關(guān)鍵幀進(jìn)行點(diǎn)云精配準(zhǔn)，通過計(jì)算相對位姿關(guān)系，構(gòu)建位姿圖并優(yōu)化其約束關(guān)系，實(shí)現(xiàn)對參數(shù)的最優(yōu)估計(jì)。其中，位姿圖的節(jié)點(diǎn)表示關(guān)鍵幀，邊包括序列邊和閉環(huán)邊，通過最小化殘差優(yōu)化整個(gè)位姿圖，提高位姿估計(jì)的準(zhǔn)確性。

3 實(shí)驗(yàn)論證本文算法采用M2DGR （multi－sensor and multi－scenario SLAM dataset for ground robots）［18］數(shù)據(jù)集進(jìn)行了實(shí)驗(yàn)驗(yàn)證，該數(shù)據(jù)集具有完整的傳感器套件，完整機(jī)器人部件如圖3所示，包括六個(gè)魚眼和一個(gè)指向天空的RGB相機(jī)、一個(gè)紅外相機(jī)、一臺事件相機(jī)、一種視覺慣性傳感器（VI傳感器）、一個(gè)慣性測量單元（IMU）、一臺激光雷達(dá)，消費(fèi)級全球?qū)Ш叫l(wèi)星系統(tǒng)（global navigation satellite system，GNSS）接收器和具有實(shí)時(shí)動(dòng)態(tài)（real－time kinematic，RTK）信號的GNSS－IMU導(dǎo)航系統(tǒng)。所有這些傳感器都經(jīng)過了很好的校準(zhǔn)和同步，并且同時(shí)記錄了它們的數(shù)據(jù)。地面實(shí)況軌跡由運(yùn)動(dòng)捕捉該數(shù)據(jù)集包括室內(nèi)和室外環(huán)境在內(nèi)的不同場景中捕獲的36個(gè)序列。實(shí)驗(yàn)是在一個(gè)配備英特爾酷睿i7 @2.2 GHz處理器和32 GB內(nèi)存的筆記本電腦上進(jìn)行的。

利用本文的雙語義詞袋回環(huán)算法進(jìn)行建圖，得到的點(diǎn)云地圖如圖4所示，整體上看地圖的效果好，在開闊環(huán)境能生成較大場景的點(diǎn)云地圖。由圖可知，本文優(yōu)化算法在三維點(diǎn)云地圖構(gòu)建方面表現(xiàn)良好，能夠基本真實(shí)還原實(shí)際場景特征，并且在細(xì)節(jié)方面表現(xiàn)良好，對開闊場景的點(diǎn)云地圖所采用的數(shù)據(jù)融合有較好的校正。

為了驗(yàn)證本文算法在室外大場景環(huán)境中建圖的準(zhǔn)確性以及穩(wěn)定性，將其與ALOAM以及LVI－SAM算法軌跡進(jìn)行評估，分別在street_01、street_02、street_04中進(jìn)行對比。圖5（b）（d）（f）分別對應(yīng)（a）（c）（e）中的黑色方框細(xì)節(jié)放大圖，同時(shí)虛線代表軌跡真值、藍(lán)色線條代表本文算法軌跡、綠色線條代表LVI－SAM算法軌跡、橙色線條代表ALOAM算法軌跡。由圖5可知，本文方法軌跡與真值更加重合，對比ALOAM和LVI－SAM算法，具有更高的精度和更低的漂移量，并且從圖5（f）可知，本文算法在大規(guī)模場景下可以實(shí)現(xiàn)精準(zhǔn)閉環(huán)。

將用絕對軌跡誤差（APE）和相對軌跡誤差（RPE）對SLAM系統(tǒng)進(jìn)行誤差評判，三者具體精度對比如表3～5所示。RPE 為相對誤差，驗(yàn)證了局部旋轉(zhuǎn)及平移的漂移量，計(jì)算 RPE 時(shí)設(shè)置 delta=10，即 10幀統(tǒng)計(jì)一次誤差；APE 為絕對誤差，驗(yàn)證估計(jì)軌跡與真值軌跡的全局一致性。表中max 為最大誤差值，min 為最小誤差值，mean 為平均誤差，RMSE 為均方根誤差，Std 為標(biāo)準(zhǔn)差。分析表4，本文算法相比LVI－SAM有更小的誤差，其中除了APE 的mean略高，其余max、min、RMSE分別下降了21%、56%、17.8%。而RPE max、min、mean、RMSE分別下降了72%、33%、54%、74%。

4 結(jié)束語本文提出了一種新的基于多傳感信息融合的SLAM語義詞袋優(yōu)化算法，并通過M2DGR數(shù)據(jù)集實(shí)驗(yàn)驗(yàn)證了本文算法的正確性和有效性。與傳統(tǒng)SLAM算法相比，采用基于多傳感融合語義詞袋的SLAM優(yōu)化算法在室外大場景無結(jié)構(gòu)環(huán)境中可以實(shí)現(xiàn)全局高精度位姿估計(jì)，并能提供具有良好全局一致性的高分辨率點(diǎn)云地圖；該方法在三維激光雷達(dá)系統(tǒng)中進(jìn)行重新定位和局部定位的效率、精度、魯棒性具有很大提升。未來工作中，針對本文算法中因使用多傳感混合回環(huán)檢測，存在計(jì)算量和CPU占用率較大的問題，將作進(jìn)一步研究，提升算法的魯棒性。

參考文獻(xiàn)：

［1］齊繼超，何麗，袁亮，等. 基于單目相機(jī)與激光雷達(dá)融合的SLAM方法 ［J］. 電光與控制，2022， 29 （2）： 99－102，112.（ Qi Jichao，He Li，Yuan Liang，et al. SLAM method based on fusion of monocular camera and LiDAR［J］.Electro－Optics amp; Control ，2022， 29 （2）： 99－102，112.）

［2］周治國，曹江微，邸順帆. 3D激光雷達(dá)SLAM算法綜述 ［J］. 儀器儀表學(xué)報(bào)，202 42 （9）： 13－27.（ Zhou Zhiguo，Cao Jiangwei，Di Shunfan. Review of SLAM algorithm for 3D LiDAR ［J］.Chinese Journal of Scientific Instrument ，202 42 （9）： 13－27.）

［3］Zhang Ji，Singh S. Low－drift and real－time LiDAR odometry and mapping ［J］.Autonomous Robots ，2017， 41 （2）： 104－416.

［4］Shan Tixiao，Englot B. LeGO－LOAM： lightweight and ground optimized LiDAR odometry and mapping on variable terrain ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway，NJ： IEEE Press，2018： 4758－4765.

［5］Mur－Artal R，Montiel J M. M，Tardós D. ORB－SLAM： a versatile and accurate monocular SLAM system ［J］.IEEE Trans on Robotics ，2015， 31 （5）： 1147－1262.

［6］De Croce M，Pire T，Bergero F. DS－PTAM： distributed stereo parallel tracking and mapping SLAM system ［J］.Journal of Intelligent amp; Robotic Systems： Theory amp; Application ，2019， 95 （2）：365－377.

［7］Forster C，Pizzoli M，Scaramuzza D. SVO： fast semi－direct monocular visual odometry ［C］// Proc of IEEE International Conference on Robotics and Automation. Piscataway，NJ： IEEE Press 2014： 15－22.

［8］Mourikis A I，Roumeliotis I. A multi－state constraint Kalman filter for vision－aided inertial navigation ［C］// Proc of IEEE International Conference on Robotics and Automation. Piscataway，NJ： IEEE Press，2007： 3565－3572.

［9］Qin Tong，Li Peiliang，Shen Shaojie. VINS－Mono： a robust and versatile monocular visual－inertial state estimator ［J］.IEEE Trans on Robotics ，2018， 34 （4）： 1004－1020.

［10］Leutenegger S，Lynen N，Bosse M，et al. Key－frame－based visual－inertial odometry using nonlinear optimization ［J］.The Internatio－nal Journal of Robotics Research ，2015， 34 （3）： 314－334.

［11］Ye Haoyao，Chen Yuying，Liu Ming. Tightly coupled 3D LiDAR inertial odometry and mapping ［C］// Proc of International Conference on Robotics and Automation. Piscataway，NJ： IEEE Press，2019： 3144－3150.

［12］Ehambram A，Voges R，Brenner C，et al. Interval－based visual－inertial LiDAR SLAM with anchoring poses ［C］// Proc of International Conference on Robotics and Automation. Piscataway，NJ： IEEE Press，2022： 7589－7596.

［13］Zhong Xinliang，Li Yuhua，Zhu Shiqiang，et al. LVIO－SAM： a multi－sensor fusion odometry via smoothing and mapping ［C］// Proc of IEEE International Conference on Robotics and Biomimetics. Pisca－taway，NJ： IEEE Press，2021： 440－445.

［14］Shan Tixiao，Englot B，Ratti C，et al. LVI－SAM： tightly－coupled LiDAR－visual－inertial odometry via smoothing and mapping［C］//Proc of IEEE International Conference on Robotics and Automation. Piscataway，NJ： IEEE Press，2021： 5692－5698.

［15］Galvez－lopez D，Tardos J. Bags of binary words for fast place recognition in image sequences ［J］.IEEE Trans on Robotics ，2012， 28 （5）： 1188－1197.

［16］Ng P C，Henikoff S. SIFT： predicting amino acid changes that affect protein function ［J］.Nucleic Acids Research ，2003， 31 （13）： 3812－3814.

［17］Bay H，Ess A，Tuytelaars T，et al. Speeded－up robust features （SURF） ［J］.Computer Vision and Image Understanding ，2008， 110 （3）： 346－35.

［18］Yin Jie，Li Ang，Li Tao，et al. M2DGR： a multi－sensor and multi－scenario SLAM dataset for ground robots ［J］.IEEE Robotics and Automation Letters ，2022， 7 （2）： 2266－2273.

收稿日期：2023－08－01；修回日期：2023－09－25 基金項(xiàng)目：湖南省自然科學(xué)基金資助項(xiàng)目（2022JJ50005）；湖南省研究生科研創(chuàng)新項(xiàng)目（QL20230216）；國家自然科學(xué)基金區(qū)域聯(lián)合基金重點(diǎn)項(xiàng)目（U23A20385）

作者簡介：袁鵬（1998—），男，湖南常德人，碩士研究生，主要研究方向?yàn)槎鄠鞲腥诤蟂LAM；谷志茹（1979—），女（通信作者），湖南株洲人，教授，碩導(dǎo)，博士，主要研究方向?yàn)橹悄芙煌ㄐ畔⑾到y(tǒng)與裝備（guzhiru@hut.edu.cn）；劉中偉（1998—），男，湖南株洲人，碩士研究生，主要研究方向?yàn)橹悄芙煌ㄐ畔⑾到y(tǒng)與裝備；焦龍飛（1994—），男，河南洛陽人，碩士研究生，主要研究方向?yàn)樽詣?dòng)駕駛；毛麒云（1998—），男，湖南婁底人，碩士研究生，主要研究方向?yàn)殡娏€通信載波.