基于輪式機(jī)器人的實(shí)時(shí)3D柵格地圖構(gòu)建

劉安睿劼，王耀力

太原理工大學(xué) 信息與計(jì)算機(jī)學(xué)院，太原 030024

1 引言

SLAM（Simultaneous Localization and Mapping）同時(shí)定位與地圖構(gòu)建是機(jī)器人自主導(dǎo)航中的一個(gè)非常重要的領(lǐng)域，它是機(jī)器人在未知環(huán)境中通過傳感器信息確定自身的空間位置，并建立所處空間的環(huán)境模型。隨著機(jī)器人技術(shù)的不斷發(fā)展，作為機(jī)器人的一個(gè)重要分支，SLAM技術(shù)也越來越重要。無論是室內(nèi)機(jī)器人導(dǎo)航還是旋翼無人機(jī)自主導(dǎo)航又或是現(xiàn)在火熱的自動(dòng)駕駛，SLAM 技術(shù)都作為高效的建圖和定位工具。傳統(tǒng)的SLAM 技術(shù)大多采用激光雷達(dá)或者聲吶系統(tǒng)進(jìn)行二維地圖的創(chuàng)建[1-3]。雖然現(xiàn)在激光雷達(dá)在室內(nèi)和室外應(yīng)用不懼任何挑戰(zhàn)，但是由于激光雷達(dá)成本過高，所以它不能夠在低成本系統(tǒng)中應(yīng)用。因此SLAM 領(lǐng)域出現(xiàn)了以視覺傳感器為基礎(chǔ)的VSLAM（Visual Simultaneous Localization and Mapping）。VSLAM 能夠?qū)Νh(huán)境中的復(fù)雜結(jié)構(gòu)進(jìn)行完整的表示?，F(xiàn)在比較流行的系統(tǒng)有MSCKF[4]、OKVIS[5]、LSD-SLAM[6]、ORB_SLAM2[7-8]、VINS-Mono[9]等。但是這些系統(tǒng)只是簡單地輸出了位姿軌跡和環(huán)境三維點(diǎn)云，并沒有具體構(gòu)建能用于機(jī)器人導(dǎo)航的地圖。

Santana 等人[10]提出了一種基于單目相機(jī)使用單應(yīng)矩陣構(gòu)建2D 地圖的方法，該方法在分割的時(shí)候判斷圖像的特征點(diǎn)是否在同一平面來快速判斷是否有障礙物，但是由于單目視覺沒有尺度信息，所以建立的2D 柵格地圖并不適用于現(xiàn)實(shí)中的機(jī)器人定位和導(dǎo)航。Dia 等人[11]提出了一種新型的用于占有網(wǎng)格地圖的逆?zhèn)鞲衅髂Ｐ停↖SM），但是并沒有SLAM 方面的實(shí)際應(yīng)用。Hull[12]提出了一種基于LSD-SLAM 的實(shí)時(shí)占據(jù)網(wǎng)格建圖，與文獻(xiàn)[10]的方法類似，使用平面信息構(gòu)建單目SLAM 地圖，利用點(diǎn)云地圖投影的方式生成柵格地圖。Gregorio 等人[13]提出了一個(gè)高效的機(jī)器人導(dǎo)航建圖框架，一種3D 柵格地圖構(gòu)建算法，但是其計(jì)算量太大，對(duì)于大型機(jī)器人系統(tǒng)來說，并不希望部分功能占用太多系統(tǒng)資源。

ORB-SLAM2[7-8]是一個(gè)比較完善的VSLAM 系統(tǒng)，包含跟蹤、局部建圖、回環(huán)檢測和全局BA，并且提供RGB-D相機(jī)的使用接口，但是它只能輸出關(guān)鍵幀、稀疏點(diǎn)云和位姿信息。稀疏點(diǎn)云并不能用于輪式機(jī)器人的導(dǎo)航，因此針對(duì)ORB_SLAM2 的這些缺點(diǎn)，利用它提供的關(guān)鍵幀、位姿信息以及RGB-D 相機(jī)接口，通過3D 柵格地圖構(gòu)建算法生成可用于輪式機(jī)器人導(dǎo)航的3D柵格地圖。

2 逆?zhèn)鞲衅髂Ｐ?/h2>

機(jī)器人在沒有先驗(yàn)的未知環(huán)境中，通過自身的傳感器建立考慮噪聲和不確定因素的概率分布模型來估計(jì)外部環(huán)境，這個(gè)模型稱為傳感器模型（Sensor Model，SM）。在傳感器觀測到外部環(huán)境數(shù)據(jù)之后，利用這些數(shù)據(jù)建立環(huán)境地圖的模型稱為逆?zhèn)鞲衅髂Ｐ停↖nverse Sensor Model，ISM）

逆?zhèn)鞲衅髂Ｐ妥畛跤蒃lfes[14]在1989 年提出，后來由Thrun[15]進(jìn)行了詳細(xì)推導(dǎo)。ISM模型主要是利用傳感器的測量數(shù)據(jù)來表示占用網(wǎng)格地圖的單元格是否被占用的概率。從傳感器角度來說，它與測量的障礙物之間的所有單元格都需要一個(gè)概率值，該概率值表示空閑或被占據(jù)的二元狀態(tài)的可能性。超出障礙物的任何單元格或者并沒有被傳感器觀測到的單元格都可以被視為未知。傳感器測量通常會(huì)受到某種形式的噪聲和不確定性的影響，所以單元格的概率狀態(tài)分配有助于將噪聲和不確定性結(jié)合到地圖中[16]。

2.1 理想的逆?zhèn)鞲衅髂Ｐ?/h3>

進(jìn)行ISM 模型建立之前，先需要做出一些假設(shè)，使得傳感器模型易于構(gòu)建。

假設(shè)1 獨(dú)立性假設(shè)。每個(gè)單元格的占用概率獨(dú)立于地圖中的每個(gè)其他單元。

假設(shè)2 使用高斯分布來表示觀測數(shù)據(jù)的噪聲。

噪聲測量的高斯概率分布函數(shù)，用于模擬傳感器測量的噪聲。方差σz表示測量z的不確定性，主要由正態(tài)分布曲線的寬度表示。方差越大表明測量的不確定性越大，導(dǎo)致曲線越寬。相反，較小的方差表明測量的不確定性較小，曲線較薄。

理想的逆?zhèn)鞲衅髂Ｐ褪窃跓o噪聲、完美的傳感器的前提下建模的。如圖1 所示。在網(wǎng)格地圖中，mi代表相應(yīng)的網(wǎng)格單元是否被占用的隨機(jī)變量。P(mi)代表i單元格的占用概率。P(mi)=0 代表絕對(duì)空閑，P(mi)=1代表絕對(duì)占據(jù)，P(mi)=0.5 代表未知。在這種理想情況下，在障礙物或距離測量之前的每個(gè)單元被設(shè)置為空閑，并且在障礙物之后的每個(gè)單元被設(shè)置為未知。在障礙物的實(shí)際范圍測量中，設(shè)置緩沖范圍L以考慮測量不以網(wǎng)格內(nèi)的單元為中心的可能性。使用進(jìn)行測量的單元的中心計(jì)算到傳感器的距離。理想情況下，在此單元格內(nèi)的任何位置進(jìn)行的測量都會(huì)為此單元格指定1。

理想的逆?zhèn)鞲衅髂Ｐ涂杀硎緸椋?/p>

其中，r表示傳感器的觀測范圍，L為緩沖范圍；區(qū)間內(nèi)g(r)=0 表示傳感器沒有觀測到障礙物。z-區(qū)間內(nèi)g(r)=1 表示障礙物已經(jīng)被傳感器觀測到。otherwiser>0 區(qū)間內(nèi)g(r)=0.5 代表未知，傳感器無法對(duì)該區(qū)域進(jìn)行觀測。r必須為正的，因?yàn)閭鞲衅饔肋h(yuǎn)不應(yīng)該返回它后面的測量值。緩沖范圍L用來考慮測量不以網(wǎng)格內(nèi)的單元為中心的情況。通常L的大小選擇為單元格對(duì)角之間的距離。

范圍測量z包含高斯噪聲，因此可以寫成：

標(biāo)準(zhǔn)的噪聲測量的高斯概率分布函數(shù)如圖2所示，其中r為沿測量線到測量z的范圍，所以：

圖2 具有正態(tài)分布不確定性的高斯概率分布函數(shù)（方差σ=0.5）

2.2 實(shí)際逆?zhèn)鞲衅髂Ｐ?/h3>

實(shí)際中的ISM 模型是將高斯噪聲融入理想傳感器模型得到的，所以對(duì)它們進(jìn)行卷積即可得到實(shí)際ISM模型：

由公式（1）能很明顯地看出來g(r)是分段函數(shù)，因此必須以分段的方式進(jìn)行模型的卷積，這意味著g(r)必須分割積分，因此完整的卷積寫成：

其中r的范圍為(a,b)，并定義分段代表的單元格占用概率，所以可以得到：

取誤差函數(shù)：

所以公式（7）可以寫成：

這時(shí)候考慮g(r)的分段特性，由公式（1）給出，所以可以得到完整的分段卷積：

公式（11）顯示了最終推導(dǎo)出來的逆?zhèn)鞲衅髂Ｐ凸健?/p>

高斯逆?zhèn)鞲衅髂Ｐ腿鐖D3所示。

2.3 逆?zhèn)鞲衅髂Ｐ蛥?shù)選擇

2.3.1L值的選取

在評(píng)估L值對(duì)ISM模型的影響時(shí)，采用固定變量的方法。首先固定z=2 m 值和高斯噪聲模型σ=0.15，通過改變L值的大小，評(píng)估ISM 的變化。不同L值對(duì)ISM模型的影響，如圖4所示。

圖4 顯示了不同的L值如何縮小或者加寬曲線的峰值。隨著L值的增大，波峰高度在不斷上升，說明ISM 模型的網(wǎng)格單元被占用的概率在不斷增大。因此如果L值過大，那么將會(huì)有更多的單元格被判斷為有障礙物，現(xiàn)實(shí)中沒有障礙物的地方可能會(huì)被判斷成有障礙物，造成比較大的偏差。如果L值過小，就會(huì)釋放比較多的單元格變成未占用，可能會(huì)把有障礙物的單元格釋放，造成誤差。通過實(shí)驗(yàn)，當(dāng)L的取值為單元格的對(duì)角線長度的時(shí)候，概率偏差最小[17]。

2.3.2σ值對(duì)ISM的影響

在研究σ值對(duì)ISM 的影響時(shí)，先固定變量z=2 m和緩沖范圍L=0.5 m，通過改變?chǔ)业牟煌?，評(píng)估ISM模型。對(duì)于ISM 模型來說標(biāo)準(zhǔn)偏差σ表示占用預(yù)測的不確定性。如圖5所示，較小的σ值對(duì)應(yīng)較高較窄的峰值，較大的σ值對(duì)應(yīng)較低較寬的峰值。隨著σ值的不斷增大，不確定因素和噪聲的影響越來越大，實(shí)際測量周圍的更多單元可能被認(rèn)為是占用的。經(jīng)過實(shí)驗(yàn)測試，σ=0.15 時(shí)，構(gòu)建出來的地圖效果最好[17]。

2.3.3觀測范圍的不確定性

對(duì)于不同的觀測范圍z，也會(huì)影響ISM模型，如圖6所示。從圖中可以看出，隨著觀測范圍的不斷擴(kuò)大，相同模型的網(wǎng)格單元被占用的概率越來越小。這可能導(dǎo)致實(shí)際包含障礙物的單元被分配的概率低于總測量范圍的峰值概率，ISM模型的判斷會(huì)越來越不準(zhǔn)確。經(jīng)過實(shí)驗(yàn)測試，觀測范圍z=0.3～5 m 時(shí)，ISM 模型判斷準(zhǔn)確率最高[17]。

圖3 卷積得到高斯逆?zhèn)鞲衅髂Ｐ?z=2 m,L=1 m,σ=0.5)

圖4 參數(shù)L 的不同取值對(duì)ISM模型的影響（z=2 m,σ=0.15）

圖5 σ 值對(duì)逆?zhèn)鞲衅髂Ｐ偷挠绊懀▃=2 m,L=0.5 m）

圖6 測量距離的變化對(duì)ISM模型的影響（L=0.4 m）

3 柵格地圖模型

柵格圖旨在將環(huán)境劃分為均勻大小的單元網(wǎng)格。網(wǎng)格單元表示環(huán)境的一部分，并且可以存儲(chǔ)是被占用、空閑還是未知相對(duì)應(yīng)的信息。在占用的情況下，它表示在單元格內(nèi)存在障礙物，并且對(duì)于路徑規(guī)劃算法或機(jī)器人，該空間不可穿越；在空閑的情況下，環(huán)境的這一部分是可穿越的，沒有障礙；在未知的情況下，沒有關(guān)于該區(qū)域的環(huán)境狀況的信息，表示需要探索該區(qū)域。根據(jù)環(huán)境所需的細(xì)節(jié)不同，可以將網(wǎng)格劃分為各種分辨率，例如1 cm 至5 m 的范圍內(nèi)。網(wǎng)格單元可以各自包含被占用概率的值，其中概率值的范圍從0（空閑）到1（占用），而值0.5 意味著單元的狀態(tài)不明，這允許將具有噪聲或不確定性的傳感器測量建模到地圖中。

理想情況下，從開始到時(shí)間t的給定傳感器測量值z和位姿估計(jì)x已知的情況下，系統(tǒng)將計(jì)算地圖m上的后驗(yàn)分布：

為了計(jì)算方便，把所有相對(duì)于機(jī)器人位姿x的傳感器測量值z轉(zhuǎn)換為世界坐標(biāo)系中的坐標(biāo)，因此可以將位姿并入測量值z。消去x得到如下公式：

柵格地圖m可以被劃分成i個(gè)大小相等的網(wǎng)格單元，其中單個(gè)單元格可以表示為。每個(gè)單元格mi被占用的概率（簡寫成（簡寫成）表示單元格空閑的概率。對(duì)于擁有1 000 個(gè)單元格的網(wǎng)格地圖來說，它擁有21000種可能，這個(gè)計(jì)算量過大，因此將單元格獨(dú)立統(tǒng)計(jì)是一個(gè)比較好的選擇。公式（12）可以寫成：

把公式（16）代入公式（15）可得：

根據(jù)相同原理互補(bǔ)概率為：

為了方便計(jì)算，可以用公式（17）除以公式（18），可得：

為了避免概率在接近0 或1 時(shí)數(shù)值不穩(wěn)定，對(duì)公式（19）取對(duì)數(shù)：

4 ORB_SLAM2柵格地圖

ORB_SLAM2 是 2017 年 Raul Mur-Artal 等人提出的一個(gè)完整的SLAM 系統(tǒng)，它在ORB_SLAM 的基礎(chǔ)上增加了雙目和RGB-D 相機(jī)的使用算法，并且增加了重定位模塊。該系統(tǒng)包含地圖復(fù)用、閉環(huán)檢測以及重定位功能，并且可以在CPU中實(shí)時(shí)工作。

ORB_SLAM2 系統(tǒng)有三個(gè)主要的并行線程：（1）跟蹤線程。跟蹤線程通過尋找與本地地圖匹配的特征，應(yīng)用BA（Bundle Adjustment）最小化重投影誤差，實(shí)現(xiàn)了相機(jī)每幀的定位。（2）局部地圖線程。局部地圖線程映射管理局部地圖并對(duì)其進(jìn)行優(yōu)化，執(zhí)行局部BA 優(yōu)化。（3）循環(huán)關(guān)閉線程。循環(huán)關(guān)閉線程檢測大循環(huán)并通過執(zhí)行姿勢圖優(yōu)化來校正累積漂移。該線程啟動(dòng)第四個(gè)線程以在姿勢圖優(yōu)化之后執(zhí)行完整BA，以計(jì)算最佳結(jié)構(gòu)和運(yùn)動(dòng)解決方案。RGB-D相機(jī)預(yù)處理如圖7所示；ORB_ SLAM2系統(tǒng)框架如圖8所示。

圖7 ORB_SLAM2 RGB-D相機(jī)預(yù)處理

圖8 ORB_SLAM2算法框架

本文算法以O(shè)RB_SLAM2 為前端，ROS 為中間層，ISM和柵格地圖構(gòu)建算法為后端，實(shí)時(shí)構(gòu)建能用于機(jī)器人導(dǎo)航的3D柵格地圖。前端ORB_SLAM2利用RGBD相機(jī)完成關(guān)鍵幀選取、相機(jī)位姿估計(jì)以及稀疏點(diǎn)云的計(jì)算，并在ROS 上發(fā)布這些話題。ISM 算法從ROS 訂閱這些數(shù)據(jù)，作為傳感器觀測數(shù)據(jù)zt和機(jī)器人姿態(tài)xt，進(jìn)而通過柵格地圖構(gòu)建算法生成所需的地圖，地圖采用八叉樹進(jìn)行儲(chǔ)存以減少存儲(chǔ)空間和內(nèi)存使用[18]。具體流程圖如圖9所示。

圖9 ORB_SLAM2實(shí)時(shí)柵格地圖構(gòu)建算法流程圖

5 實(shí)驗(yàn)

5.1 實(shí)驗(yàn)說明

本文采用ROS 系統(tǒng)作為中間平臺(tái)，采用Rviz（Ros Visualization）作為實(shí)時(shí)顯示平臺(tái)。本文實(shí)驗(yàn)Rviz 中地圖網(wǎng)格大小選取為1 m2。3D 柵格地圖精度（單元格大?。Q定了地圖的精細(xì)程度，體現(xiàn)更多的細(xì)節(jié)單元格需要小一點(diǎn)（不小于0.01 m），如果為了體現(xiàn)地圖的大輪廓，那么單元格就要選擇的大一點(diǎn)。本文所有實(shí)驗(yàn)中地圖單元格都選取為0.05 m，因此根據(jù)本文2.3.1小節(jié)可得，σ取值為經(jīng)驗(yàn)值0.15。

5.2 TUM數(shù)據(jù)集實(shí)驗(yàn)

慕尼黑工業(yè)大學(xué)（TUM）的計(jì)算機(jī)視覺實(shí)驗(yàn)室提供了RGB-D 大型數(shù)據(jù)集。數(shù)據(jù)集采用Microsoft Kinect傳感器以全幀速率（30 Hz）和傳感器分辨率（640×480）記錄彩色圖像和深度圖像數(shù)據(jù)，并且提供了地面實(shí)況軌跡。本文采用輪式機(jī)器人所采集的數(shù)據(jù)集rgbd_dataset_freiburg2_pioneer_slam作為本次實(shí)驗(yàn)圖像輸入。RGB-D相機(jī)運(yùn)動(dòng)軌跡及點(diǎn)云圖如圖10所示。圖10顯示了輪式機(jī)器人的運(yùn)動(dòng)軌跡以及稀疏點(diǎn)云，但是由于點(diǎn)云太過稀疏無法進(jìn)行導(dǎo)航以及路徑規(guī)劃。實(shí)時(shí)生成的3D 柵格地圖如圖11所示。該圖能夠清楚地表示出障礙物所在的位置，以及房間的墻面信息，但是障礙物和墻面略有不規(guī)則是因?yàn)樵谟?jì)算特征點(diǎn)的時(shí)候，有的地方缺少紋理，有的地方超出深度傳感器的探測范圍，所以無法計(jì)算出足夠的特征點(diǎn)，因此輸入到ISM 模型中的數(shù)據(jù)有部分偏差，使得生成的墻面和障礙物有些不規(guī)則。圖11 下方?jīng)]有墻面是因?yàn)閿?shù)據(jù)集中并沒有拍攝到該處的圖像。

圖10 rgbd_dataset_freiburg2_pioneer_slam運(yùn)動(dòng)軌跡及點(diǎn)云圖

圖11 rgbd_dataset_freiburg2_pioneer_slam 3D柵格地圖

圖12 機(jī)器人仿真環(huán)境

5.3 仿真輪式機(jī)器人實(shí)驗(yàn)

本文采用中科院軟件所-重德智能機(jī)器人聯(lián)合研究中心提供的機(jī)器人仿真環(huán)境，該環(huán)境包含一個(gè)完整的機(jī)器人系統(tǒng)。仿真環(huán)境由Gazebo構(gòu)建，提供對(duì)外接口，能夠通過ROS獲取RGB圖像數(shù)據(jù)和深度圖像數(shù)據(jù)。本次實(shí)驗(yàn)利用該仿真環(huán)境，并與ORB_SLAM2 進(jìn)行融合，通過鍵盤控制輪式機(jī)器人的運(yùn)動(dòng)。仿真環(huán)境如圖12 所示，運(yùn)動(dòng)軌跡及點(diǎn)云圖如圖13 所示，實(shí)時(shí)3D 柵格地圖如圖14 所示。圖14 能夠非常完美地顯示該仿真環(huán)境，因?yàn)樵摲抡姝h(huán)境有大量紋理豐富的墻面以及物品，并且沒有特別多的雜物。在圖中左下角缺少一塊是因?yàn)樵撎帀γ媸枪饣?。沒有任何紋理存在，因此ISM模型沒有接收到任何數(shù)據(jù)，也就無法生成地圖。

5.4 室內(nèi)輪式機(jī)器人實(shí)驗(yàn)

圖13 仿真機(jī)器人運(yùn)動(dòng)軌跡及點(diǎn)云圖

圖14 仿真機(jī)器人實(shí)時(shí)3D柵格地圖

對(duì)于室內(nèi)的輪式機(jī)器人實(shí)驗(yàn)，實(shí)驗(yàn)設(shè)備如圖15 所示，采用三輪全向輪底盤，最大移動(dòng)速度0.5 m/s，最大角速度180（°）/s。采用DELLG3（i5-8300H 3.3 GHz 8 GB內(nèi)存）筆記本電腦作為輪式機(jī)器人的控制器。實(shí)驗(yàn)攝像頭為Microsoft KinectV2 RGB-D 相機(jī)，彩色圖像分辨率為960×540，深度圖像為320×240，攝像頭采集頻率為30 Hz。本實(shí)驗(yàn)在Ubuntu16 中利用ROS 系統(tǒng)作為平臺(tái)進(jìn)行 RGB-D 相機(jī)、ORB_SLAM2 以及 ISM 模型之間的通信。實(shí)時(shí)3D柵格地圖由ROS系統(tǒng)內(nèi)的Rviz顯示。

圖15 室內(nèi)實(shí)驗(yàn)設(shè)備

室內(nèi)實(shí)驗(yàn)場景如圖16 所示，室內(nèi)運(yùn)動(dòng)軌跡及點(diǎn)云地圖如圖17 所示，室內(nèi)實(shí)時(shí)3D 柵格地圖如圖18 所示。室內(nèi)實(shí)驗(yàn)生成的柵格地圖能夠清晰地看到障礙物和兩側(cè)的板子，能夠很好地反映室內(nèi)環(huán)境。通過室內(nèi)實(shí)驗(yàn)可以看出，對(duì)于有豐富紋理的室內(nèi)環(huán)境能夠建立準(zhǔn)確的3D 柵格地圖，能夠反映障礙物所在的空間位置。但是對(duì)于紋理不豐富的障礙物側(cè)面以及光滑反光的鋼板，就沒辦法獲得充足的信息，因此本文的算法對(duì)于這種情況并沒有辦法建立柵格地圖。

圖16 室內(nèi)實(shí)驗(yàn)場景

圖17 室內(nèi)機(jī)器人運(yùn)動(dòng)軌跡及點(diǎn)云圖

5.5 算法對(duì)比分析

本文采用2017 年Gregorio[13]等人提出的Skimap 算法作為對(duì)比方法。Skimap是一種高效的機(jī)器人導(dǎo)航算法地圖結(jié)構(gòu)，能夠快速獲取3D地圖、2.5D高度地圖以及2D 占據(jù)地圖。Skimap 為導(dǎo)航映射框架，依賴于前端提供的位姿、RGB 圖像以及深度圖像。因此對(duì)比實(shí)驗(yàn)采用ORB_SLAM2 作為前端，Skimap 作為后端地圖生成算法。對(duì)比圖如圖19所示。

圖18 室內(nèi)機(jī)器人實(shí)時(shí)3D柵格地圖

圖19 室內(nèi)機(jī)器人實(shí)時(shí)Skimap 3D柵格地圖

為了評(píng)估地圖的精度，本文采用長度誤差lerror與角度誤差θerror作為評(píng)價(jià)指標(biāo)。分別從本文算法建圖（圖18）以及Skimap 算法建圖（圖19）中取出五段線段以及三個(gè)夾角（如圖18），對(duì)比實(shí)際場景得到誤差值，如表1所示。

表1 地圖誤差值

從建圖的精度上看，本文算法精度和Skimap 算法精度都處于2%之內(nèi)，本文算法精度略高于Skimap算法精度。ORB_SLAM2 算法有閉環(huán)檢測及全局優(yōu)化模塊[8]，并且本文在進(jìn)行實(shí)驗(yàn)的時(shí)候有較好的閉環(huán)條件，能夠很好地減小累計(jì)誤差，進(jìn)一步提高了建圖精度。

實(shí)時(shí)性方面，本文實(shí)時(shí)性略差于Skimap，由文獻(xiàn)[13]可知Skimap算法樹搜索時(shí)要快于八叉樹和KD樹，但在實(shí)驗(yàn)過程中Skimap算法速度為10 Hz，本文算法處理速度為10 Hz，實(shí)時(shí)性基本滿足要求。

6 結(jié)束語

針對(duì)以往的視覺SLAM 系統(tǒng)只能生成位姿以及稀疏點(diǎn)云圖的缺陷，本文在ORB_SLAM2 的基礎(chǔ)上，提出了一種3D 柵格地圖的實(shí)時(shí)構(gòu)建算法。該算法建立了適用于視覺SLAM的逆?zhèn)鞲衅髂Ｐ停↖SM），詳細(xì)推導(dǎo)了柵格地圖的生成機(jī)制，并利用算法流程圖清晰地表示ORB_SLAM2 與ISM 結(jié)合生成3D 柵格地圖的具體流程。通過數(shù)據(jù)集實(shí)驗(yàn)、仿真實(shí)驗(yàn)以及室內(nèi)輪式機(jī)器人實(shí)驗(yàn)，表明ISM 模型與ORB_SLAM2 的結(jié)合具有較強(qiáng)的實(shí)用性和較高的建圖精度，并能夠?qū)崟r(shí)構(gòu)建3D 柵格地圖。