基于RGB-D圖像的室內機器人同時定位與地圖構建

趙 宏，劉向東，楊永娟

（蘭州理工大學計算機與通信學院，蘭州 730050）

（?通信作者電子郵箱Liuxd1994@foxmail.com）

0 引言

2020 年的春天，一場席卷全球的COVID-19（Corona Virus Disease 2019）病毒使得人人自危。醫護人員每天和病患頻繁的接觸大大增加了交叉感染的幾率。一款具有自主導航功能的送藥、送餐醫療機器人能減少醫患人員之間接觸的頻率，降低醫護人員被感染的幾率。同時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）技術能夠讓機器人定位自己在未知環境中的具體位置，了解周圍環境的結構，這些信息是機器人實現自主導航功能的基礎。自1986 年被提出之后，SLAM 技術一直是機器人領域的熱點研究問題［1-3］。RGB-D（RGB-Depth）傳感器能有效避免單目和雙目相機恢復的三維點云尺度不確定與精度低的問題。基于RGB-D傳感器的SLAM 方案，對室內機器人自主導航具有重要的意義［4-5］。

基于特征點的視覺SLAM（Visual SLAM）［6］根據相機的種類可以分為三類：1）單目SLAM［7］。單目相機結構簡單、成本低廉，單目SLAM 通過相機的運動恢復自身的位置和周圍場景的結構，但存在尺度不確定性的問題，且恢復的是稀疏結構。2）雙目SLAM［2］通過視差恢復場景的稠密結構，但雙目相機恢復的空間三維點的坐標誤差較大。3）RGB-D SLAM［8］則是借助RGB-D 相機［9］實現的，RGB-D 相機由彩色攝像頭和深度攝像頭組成，可以同時采集場景的彩色圖像（如圖1（a））和深度圖像（如圖1（b））。深度圖像是一種特殊的“灰度圖”，圖中每個像素的值代表相機和場景之間的距離。當環境中不存在大量的紅外光源時，RGB-D 相機可以準確地恢復出場景的三維點云（如圖1（c））。

圖1 RGB-D相機采集的圖像及生成的三維點云Fig.1 Images captured by RGB-D camera and generated 3D point cloud

Hentry 等［10］搭建了一套基于特征點的RGB-D SLAM，可以完成稠密點云的重建，但實時性不高。Endres［11］在迭代最近點（Iterative Closest Point，ICP）的基礎上提出了RGB-D SLAMv2 系統，建立了一套完整的RGB-D SLAM 系統，該系統提供了ORB（Oriented FAST and Rotated BRIEF）［12］、SIFT（Scale Invariant Feature Transform）［13］、SURF（Speeded Up Robust Features）［14］、SiftGPU［15］四種特征提取算法的接口，使用之前只需要在配置文件中進行配置，并提供了ROS（Robot Operating System）［16］系統的接口，方便使用者進行實地建模；但系統在運行時有卡頓現象，且生成的點云文件占用空間存儲量過大，機器人在導航時難以應用。文獻［17］中采用非線性優化的方法求解相機姿態，準確度較ICP 方法有所提升。文獻［18］中提出的ORB-SLAM2是ORB-SLAM［17］的升級版本，提供了單目、雙目、RGB-D 相機的接口，整個系統圍繞ORB 特征檢測算法建立，可以在CPU 上實時完成稀疏三維點云的重建，并使用三個線程完成SLAM。在目前基于特征點的SLAM方案中，ORB-SLAM2 擁有較高的精度和實時性，但ORBSLAM2 得到的是稀疏的點云地圖，無法滿足機器人導航和避障的需求。文獻［19］中在ORB-SLAM2的基礎上，增加了構建稠密地圖的功能，并針對點云地圖難以用于機器人導航的問題，提出了一種基于八叉樹的導航地圖構建方法，完成機器人在未知空間的導航。

為彌補現有RGB-D SLAM 系統實時性差和精度低的問題，本文搭建一套RGB-D SLAM系統，主要內容如下：

1）對特征檢測算法進行改進，在ORB 特征的基礎上引入一種基于四叉樹的特征點均勻化策略，保證特征點的均勻分布，并在特征匹配階段結合詞袋（Bag of Words，BoW）模型［20］提升特征匹配的速度和精度；

2）為準確求解相機姿態初始值，采用PnP（PerspectivenPoint）［21］+RANSAC（RANdom SAmple Consensus）［22］+非線性優化的方法估計每幀圖像的相機姿態；

3）為避免誤差的累積，結合回環檢測和光束法平差（Bundle Adjustment，BA）［2］對相機姿態初始值進行優化，得到相機姿態的最優值，并在此基礎上，根據點云數據和相機坐標系的對應性，將所有點云數據轉換到同一個坐標系中，得到場景的稠密點云地圖；

4）對稠密點云地圖進行壓縮和轉換，得到用于機器人導航的八叉樹地圖［19］。

1 系統設計

本文系統實現主要分為：特征點檢測與匹配，相機姿態估計及優化，回環檢測，地圖構建等部分。本文系統整體框架如圖2所示。

圖2 本文系統框架Fig.2 Framework of proposed system

1.1 特征檢測與匹配

文獻［12］中提出的ORB 特征檢測算法由FAST（Features from Accelerated Segment Test）關鍵點和BRIEF（Binary Robust Independent Elementary Feature）描述子構成。相較SIFT［13］和SURF［14］算法，ORB 具有更快的速度（三者的比較如表1 所示，檢測的特征點數量為1 000，數據來源于文獻［12］）。因此本文采用ORB算法檢測特征點。

表1 不同特征檢測算法時間對比Tab.1 Time comparison of different feature detection algorithm

ORB 特征檢測算法由四個部分構成：1）圖像金字塔的構建；2）FAST特征點的檢測；3）特征點旋轉角度的計算；4）特征點描述子的生成。本文在此基礎上對ORB 算法進行改進，引入一種基于四叉樹的特征點均勻分布策略，從而使ORB 提取的特征點分布更加均勻。特征點的均勻分布步驟如下：

步驟1 對輸入的圖像構建圖像金字塔。

步驟2 在金字塔的每一層圖像中，按照大小為30×30的像素進行分格處理。

步驟3 在每個格子中，提取FAST角點。

步驟4 對所有提取到的角點，采用四叉樹進行劃分，初始節點為1。

步驟5 計算當前節點的特征點數量，如果大于1，在當前節點繼續按照四叉樹進行劃分；否則，當前節點停止劃分。

步驟6 計算節點的數量，如果小于設定的需求特征點數N，則繼續劃分；否則，停止所有劃分。

步驟7 遍歷滿足條件的節點，對每個節點中按照特征響應值c排序，c的計算如式（1）所示，選取特征響應值最大的特征點作為代表。

其中：IP是特征點對應的灰度值；Ii是以特征點為圓心、半徑為3的圓上標號為1～16的像素的灰度值。

如圖3 所示，OpenCV（Open source Computer Vision library）［23］實現的ORB 關鍵點有扎堆現象，本文方法實現的ORB關鍵點分布均勻。

圖3 不同方法實現的ORB特征分布結果Fig.3 Distribution results of ORB features achieved by different methods

特征點檢測與匹配的速度顯著影響系統的實時性。本文在使用改進的ORB 算法檢測特征點的基礎上，在特征匹配階段，引入一種基于詞袋模型［20］的特征匹配方法。首先，對輸入的圖像使用ORB 算法提取特征。然后，運用K-means++將ORB 算法提取的圖像描述子進行聚類，構建k叉樹，生成圖像的詞典。最后，當進行特征匹配時，從k叉樹的根節點出發，查找特征對應的單詞，將圖像特征轉換成單詞向量的描述。當對應的單詞向量一致時，對特征進行匹配。在該過程中，有效減少了特征點匹配的范圍，因此特征匹配的準確率得到提升。同時，k叉樹便于查找的特性，保證了在匹配時有較快的速度。詞袋模型的生成原理如圖4 所示，最終得到一棵深度為d（d=logkN，N為特征點的個數）的k叉樹。

圖4 詞袋模型的生成原理示意圖Fig.4 Generation principle schematic diagram of BoW

1.2 相機姿態的求解與優化

視覺SLAM 將整個系統分為前端視覺里程計（Visual Odometry，VO）［2］和后端優化兩個部分。后端對前端輸入的相機姿態進行優化，從而得到相機姿態的最優值。因此，前端估計的相機姿態初始值對于整個系統的精度具有顯著的影響。本文采用PnP+RANSAC+非線性優化的方法估計相機姿態的初始值。

PnP是一種相機位姿求解方法。P3P算法［24］（圖5）是PnP中的一種，只需四對3D-2D 匹配點，其中：一對匹配點作為驗證點；其余三對匹配點中，2D 點a、b、c是3D 點A、B、C在相機成像平面上的投影。O表示相機的光心。

圖5 P3P問題示意圖Fig.5 Schematic diagram of P3P problem

三角形之間的對應關系如式（2）所示。

根據余弦定理，三角形之間的關系如式（3）所示。

經過改進的特征匹配關系中仍然可能存在誤匹配，誤匹配會造成P3P算法的失效，因此，本文采用RANSAC 模型來增強P3P算法的魯棒性。

其中：(XW，YW，ZW)表示空間點P在世界坐標系下的三維坐標；(Xc，Yc，Zc)表示P點在相機坐標系下的三維坐標；T表示變換矩陣，由旋轉矩陣R和平移向量t構成；β表示距離閾值。運用RANSAC算法的具體步驟如下：

步驟1 隨機抽取四對匹配點，作為初始的內點集合，通過該內點集合計算出變換矩陣T。

步驟2 依次判斷剩余的點，計算三維坐標點之間的距離，如果距離小于設定的閾值（本文設定的閾值為4.0［2］），則將當前點加入到內點集合中。

步驟3 重復迭代步驟M（M的值為100［2］）次，選取內點樣本數量最多的一組作為最終的匹配點集。

為對P3P 算法求解的相機姿態進行優化，將相機的姿態根據BA定義成一個最小二乘問題，采用小孔相機模型描述成像原理。小孔相機模型的定義如下：

其中：(XW，YW，ZW)是空間點P在世界坐標系中的坐標；K是攝像機的內參；R是旋轉矩陣；t是平移向量；(u，v)是P點投影在成像平面上的像素坐標。

待優化的誤差項e的計算如式（6）所示：

其中：P′是投影點P的真實像素坐標；s是P點的深度；K是攝像機的內參；ξ是相機姿態的李代數［2］；ξ∧表示李代數的反對稱矩陣。最小化誤差項ξ*的計算如式（7）所示。

使用列文伯格馬夸爾特（Levenberg Marquardt，LM）方法不斷調整ξ的值，最終得到與當前匹配關系最優的相機姿態。在LM 方法中，使用JTJ代替海塞矩陣（Hessian Matrix），避免計算量大的問題。ξ∧左乘擾動量δξ后，相機姿態的雅可比矩陣JC的計算方法如式（8）所示。

其中：fx和fy分別是攝像機在水平和豎直方向的焦距；(Xc，Yc，Zc)表示P點在相機坐標系下的三維坐標。

上述內容介紹了系統前端相機姿態的求解方法。由于噪聲的存在，上一時刻的誤差會傳遞到下一時刻，隨著系統的運行，誤差會不斷地積累，最終造成系統的崩潰。為減小誤差累積，系統在后端將前端輸入的所有相機姿態和三維點進行更大范圍的優化。后端局部BA 優化的計算如式（5）所示，空間三維點的雅可比矩陣JP的計算方法如式（9）所示。

1.3 回環檢測

回環檢測的目的是讓機器人在運動的過程中識別出曾經到過的地方，從而在采集的圖像數據中添加一個有間隔時間的約束，進而減小誤差的積累，提高重建系統的精度。本文采用基于詞袋模型［20］的回環檢測方法，判斷圖像之間的相似性。該方法首先將圖像用詞袋模型轉換成單詞向量，然后通過對單詞向量的比較判斷兩幀圖像之間的相似性。兩幅圖像fA和fB之間的相似性計算如式（10）所示：

回環檢測的具體步驟如下：

步驟1 輸入圖像。

步驟2 計算特征點和描述子。

步驟3 從k叉樹的根節點出發，查找描述子對應的單詞，生成單詞向量。

步驟4 和關鍵幀序列中的圖像進行對比，計算圖像之間的相似性。

步驟5 如果滿足設定的閾值（本文設定的閾值為0.32），則將當前幀設置為回環幀候選幀；否則，返回步驟1。

步驟6 計算當前幀和回環候選幀的特征匹配數量（本文設定的閾值為45）。如果滿足閾值，則判定當前幀和當前回環候選幀產生回環；否則，返回步驟1。

步驟7 在當前幀和回環幀中計算相機姿態，更新當前幀的狀態。

1.4 導航地圖

受深度相機量程和視角的限制，使用傳感器每次僅僅能獲取一部分待重建的場景，因此需要通過不同角度來掃描，最終根據深度圖和相機坐標系的對應關系，使用優化后的相機姿態Ri和ti將各個角度下的深度圖恢復的點云數據轉換到同一個坐標系中，得到場景的稠密點云地圖。

但稠密點云地圖的文件存儲量過大，包含了大量對于機器人導航無用的信息，且無法處理動態的物體，因此難以應用于機器人導航。八叉樹地圖是一種靈活的、壓縮的、能實時更新的地圖形式，相較點云地圖更有利于機器人的導航。八叉樹的生成原理如圖6所示。

圖6 八叉樹生成原理示意圖Fig.6 Generation principle schematic diagram of octree

八叉樹的每個葉子節點中存儲是否被點云占據的概率值，即表示機器人在某個地方能不能通過。對于第n個葉子節點，設系統在Ti狀態下觀測到的數據為zi，則第n個葉子節點存儲的概率值Poct為：

引入logit變換，有：

則式（11）變換為：

即第n個葉子節點的值為當前觀測結果與上一次觀測結果的和。在系統運行的過程中，根據觀測到某個節點被占據的情況，在初始值（Poct的初始值為0.5［19］）的基礎上不斷增加或減小Poct的值。父親節點的概率值為孩子節點的平均值，父親節點l(n)的計算如式（14）所示：

2 實驗與結果分析

2.1 實驗環境及數據集

為驗證本文構建的系統的有效性，使用TUM RGB-D數據集［25］，從系統運行時間和絕對位姿誤差（Absolute Pose Error，APE）［25］兩個方面與現有的系統進行對比實驗。實驗環境為：CPU 為Intel Core i5-U @42582.40 GHz；內存為8 GB；系統為Windows10/Ubuntu16.04；語言為C/C++。選用的三個數據集序列分別是：fr1_desk2、fr1_xyz、fr1_room。

2.2 運行時間對比

本文系統、ORB-SLAM2［18］、RGB-D SLAMv2［11］在運行時間方面的對比結果如表2所示。為保證公平性，RGB-D SLAMv2中同樣使用ORB算法檢測特征點。

表2 不同系統運行時間對比Tab.2 Running time comparison of different systems

由表2 可知，本文構建的系統平均運行時間約為ORBSLAM2［18］的1.25 倍，這是由于本文進行稠密點云地圖重建造成的。ORB-SLAM2雖然快，但得到稀疏點云地圖不能應用于機器人導航。另外，本文系統在RGB-D SLAMv2 的基礎上平均運行時間降低了37.697%，這得益于基于詞袋模型的特征匹配方法具有較快的速度。最后，在運行期間，場景的點云是不斷更新的，沒有卡頓現象，表明本文構建的系統可以實時運行。

2.3 系統精度對比

絕對位姿誤差（APE）描述的是每一幀相機的真實姿態和估計姿態之間的差值，可以直觀地反映系統的精度。對于第i幀圖像，APE的計算如式（15）所示：

其中：Qi是真實的相機姿態；Ti是系統估計的相機姿態；S是兩者之間的轉換矩陣。使用均方根誤差（Root Mean Square Error，RMSE）統計總體值，計算方法如式（16）所示：

本文系統、ORB-SLAM2［18］、RGB-D SLAMv2［11］在精度方面的對比結果如表3所示。

表3 不同系統APE對比 單位：mTab.3 APE comparison of different systems unit：m

由表3 可知，在fr1_desk2 和fr1_xyz 序列上，本文系統與ORB-SLAM2 精度幾乎相同。在fr1_room 序列上，本文系統的精度約為ORB-SLAM2 的57.89%。與RGB-D SLAMv2 的比較中，在三個序列上，本文系統實現的結果精度均優于RGB-D SLAMv2，在RGB-D SLAMv2的基礎上平均提升了約56.104%。

三個圖像序列上，APE 的實時變化情況分別如圖7所示。

圖7 絕對位姿誤差Fig.7 Absolute pose error

三個系統和真實軌跡的對比結果分別如圖8 所示（其中groundtruth是真實軌跡，proposed為本文系統）。

圖8 相機軌跡對比Fig.8 Comparison of camera trajectory

由圖8 可以看出，和真實軌跡的對比也印證了系統的精度，本文系統和ORB-SLAM2生成的軌跡和真實軌跡的貼合程度較高，而RGB-D SLAMv2生成的軌跡偏離程度較大。

2.4 八叉樹地圖

本文系統在fr1_room 序列上得到的稠密點云地圖如圖9所示，稠密點云地圖經過八叉樹壓縮得到的3D 導航地圖（分辨率為0.05 m）如圖10 所示。雖然，本文選擇的系統精度衡量標準不能直接衡量生成地圖的質量，但是相機坐標系和單幀點云數據具有對應關系，相機姿態的偏差程度直接決定了點云地圖的偏差程度。

圖9 稠密點云地圖Fig.9 Dense point cloud map

圖10 3D導航地圖Fig.10 3D navigation map

三個圖像序列上稠密點云地圖和八叉樹地圖文件的存儲量對比如表4所示。

表4 地圖文件存儲量對比Tab.4 Comparison of map file size

由表4 可知，經過八叉樹壓縮后的稠密點云地圖的文件大小顯著減小，平均大小約為原來稠密點云地圖大小的3.217%。在機器人自主導航時，較小的地圖文件，便于機器人攜帶和加載。

3 結語

本文基于RGB-D圖像，根據視覺SLAM 的相關技術原理，建立了一套便于機器人導航的八叉樹地圖。在TUM RGB-D數據集上的實驗結果表明，本文構建的RGB-D SLAM 系統在運行速度和精度上要優于RGB-D SLAMv2［11］系統，在RGB-D SLAMv2 的基礎上，平均運行速度降低約37.697%，平均精度提升約56.104%。與目前具有代表性的ORB-SLAM2［18］系統相比，ORB-SLAM2的平均運行速度約為本文系統速度的1.25倍，這是本文系統進行稠密點云地圖重建造成的。就系統的精度而言，在fr1_desk2 和fr1_xyz 序列上兩個系統幾乎相同。在fr1_room 序列上，本文構建的RGB-D SLAM 系統精度約為ORB-SLAM2的57.89%。綜合而言，本文構建的RGB-D SLAM系統具有較高的實時性和精度。另外，建立的基于八叉樹的導航地圖，平均約為稠密點云文件存儲量的3.217%，滿足了機器人對導航地圖的需要。在接下來的工作中將探索機器人導航時，如何在八叉樹地圖上進行路徑規劃與避障。