趙 凱 ，李 丹 ，程 星 ，管 玲 ，葛仕全

(安徽工業(yè)大學(xué) 電氣與信息工程學(xué)院, 安徽 馬鞍山 243032)

即時定位與建圖(simultaneous localization and mapping，SLAM)技術(shù)作為移動機(jī)器人實現(xiàn)真正自主的核心技術(shù)之一，在智能移動機(jī)器人、無人駕駛等領(lǐng)域發(fā)揮著重大的作用[1]。常用的SLAM 系統(tǒng)根據(jù)使用傳感器的不同，可分為激光SLAM 和視覺SLAM。視覺SLAM 主要依靠攝像機(jī)傳感器數(shù)據(jù)，融合計算機(jī)視覺、深度學(xué)習(xí)等技術(shù)可更好地解決回環(huán)檢測、語義場景構(gòu)建等問題，具有簡易便攜、硬件成本低且定位精度高的優(yōu)勢，已逐步成為SLAM 研究的主流趨勢[2]。傳統(tǒng)視覺SLAM 有ORB(oriented fast and rotated brief)-SLAM[3]、DVO(dense visual odometry)-SLAM[4]和VINS(visual-inertial system)-SLAM[5]等，但傳統(tǒng)SLAM 框架皆采用靜態(tài)環(huán)境的剛性假設(shè)?？陀^世界中，存在行人、動物、車輛等動態(tài)物體的環(huán)境是不可避免的，動態(tài)物體上的特征點會影響特征匹配結(jié)果，致使算法的魯棒性和定位精度明顯下降。

學(xué)者們常采用傳統(tǒng)機(jī)器視覺方法解決動態(tài)環(huán)境下視覺SLAM 的定位建圖問題。Kim 等[6]利用IMU 數(shù)據(jù)對RGB-D 相機(jī)自身運動進(jìn)行旋轉(zhuǎn)分量的補(bǔ)償，根據(jù)位姿變換后生成空間的運動向量區(qū)分圖像中的動態(tài)特征點；Alcantarilla 等[7]利用連續(xù)圖像序列估計相機(jī)的粗略位姿，使用粗略位姿計算圖像中的稠密3D 光流，并根據(jù)測量的不確定度計算圖像間匹配點的馬氏距離并剔除外點；Wang 等[8]對連續(xù)圖像之間的光流軌跡進(jìn)行聚類分析，合并擁有相同運動趨勢的區(qū)域，并假設(shè)圖像中靜態(tài)區(qū)域占多數(shù)，利用面積最大的區(qū)域計算出相應(yīng)的基礎(chǔ)矩陣，從而區(qū)分動態(tài)區(qū)域。傳統(tǒng)機(jī)器視覺方法本身在正確率上存在限制，近年學(xué)者們利用卷積神經(jīng)網(wǎng)絡(luò)等深度學(xué)習(xí)方法解決動態(tài)環(huán)境下的SLAM 定位建圖問題。Yang 等[9]利用YOLOv3 進(jìn)行高動態(tài)性的目標(biāo)分割及移除，在此基礎(chǔ)上計算相應(yīng)基礎(chǔ)矩陣判斷特征點的真實動態(tài)性。以上方法均只能得到先驗動態(tài)物體的信息，對非先驗動態(tài)物體的魯棒性較差。鑒于此，提出一種基于語義分割網(wǎng)絡(luò)的動態(tài)場景下的視覺SLAM 算法，同時構(gòu)建一個純靜態(tài)語義八叉樹地圖，以期實現(xiàn)復(fù)雜動態(tài)場景下的精度定位。

1 基于語義分割網(wǎng)絡(luò)的SLAM 算法

基于ORB-SLAM3[10]在靜態(tài)環(huán)境下良好位姿估計和定位性能，選擇ORB-SLAM3 作為主體框架，在原有跟蹤線程、局部建圖線程、回環(huán)檢測線程的基礎(chǔ)上增加語義建圖線程，提出一種動態(tài)場景下基于語義分割網(wǎng)絡(luò)的視覺SLAM 算法，其整體框架如圖1。

圖1 系統(tǒng)框架Fig.1 System framework

圖1 中灰色部分為對ORB-SLAM3 的改進(jìn)部分。利用RGB-D 相機(jī)獲取RGB 圖像和深度圖像，RGB 圖像經(jīng)LR-ASPP[11](lite reduced atrous spatial pyramid pooling)語義分割網(wǎng)絡(luò)處理，獲得像素級的語義信息，利用語義信息剔除圖像中先驗動態(tài)物體上的特征點；對于圖像上剩余的特征點，通過輕量化追蹤模塊獲得當(dāng)前幀位姿的粗略估計，該模塊只用于估計當(dāng)前幀位姿，不參與后續(xù)建圖過程；通過多視角幾何算法進(jìn)一步檢測非先驗動態(tài)物體對應(yīng)的特征點，對多視角幾何的檢測結(jié)果和語義分割網(wǎng)絡(luò)的檢測結(jié)果進(jìn)行交叉驗證，得到完整的動態(tài)區(qū)域，過濾掉動態(tài)區(qū)域上的特征點后進(jìn)入跟蹤線程，得到更準(zhǔn)確的位姿。如果產(chǎn)生關(guān)鍵幀，語義建圖線程會根據(jù)關(guān)鍵幀的相關(guān)數(shù)據(jù)構(gòu)建動態(tài)場景下的八叉樹地圖[12]。

1.1 動態(tài)物體剔除算法

語義分割網(wǎng)絡(luò)可較好地剔除先驗動態(tài)物體，但對場景中的非先驗動態(tài)物體區(qū)分效果有限，采用多視角幾何算法[13]做進(jìn)一步處理。提出的動態(tài)物體剔除算法流程如圖2。

圖2 動態(tài)物體剔除算法流程圖Fig.2 Flowchart of dynamic object rejection algorithm

根據(jù)關(guān)鍵幀(key frame，KF)之間的運動關(guān)系檢測動態(tài)點，動態(tài)點被檢測出來后，通過判斷是否擁有語義標(biāo)簽將動態(tài)點分為擁有語義信息的動態(tài)點和沒有語義信息的動態(tài)點；對擁有語義信息的特征點在語義圖上進(jìn)行語義輪廓搜索，對沒有語義信息的動態(tài)點在深度圖上進(jìn)行區(qū)域生長，充分利用語義信息減少區(qū)域生長種子點的數(shù)量，提高系統(tǒng)的運行效率；融合沒有語義的動態(tài)物體掩膜和擁有語義信息的動態(tài)物體掩膜，獲得完整的動態(tài)物體掩膜。

在KF 間運動檢測時，需根據(jù)當(dāng)前幀(current frame，CF)從KF 數(shù)據(jù)庫中選取與CF 重疊度最高的若干KF[13]。KF 數(shù)據(jù)庫上限一般設(shè)置為20 個，數(shù)據(jù)庫越大，系統(tǒng)初始化越困難，并影響幀查找的速度；重疊KF 的選取數(shù)同樣會影響系統(tǒng)的運行速度和動態(tài)物體檢測的準(zhǔn)確度。本文實驗中，設(shè)定KF 數(shù)據(jù)庫上限為20 個、重疊KF 為5 個，作為計算成本和動態(tài)物體檢測準(zhǔn)確性之間的折衷。重疊度的判斷標(biāo)準(zhǔn)是KF 與CF 的距離和旋轉(zhuǎn)。多視角幾何算法的核心原理如圖3[13]。其中x為被挑選出的重疊KF 上的關(guān)鍵點，x在CF 坐標(biāo)系下的投影為x′，對應(yīng)的三維點為X，計算x,x′,X之間投影深度lproj和 角度 α。

圖3 多視角幾何檢測動態(tài)點的原理示意圖Fig.3 Schematic diagram of principle of multi-view geometry detection dynamic points

理想情況下，式(1)可根據(jù)對極幾何關(guān)系推理出，x′和x為 特征點的歸一化坐標(biāo)，l′和l為 三維點X在對應(yīng)幀坐標(biāo)系下的深度，r為KF 到CF 的旋轉(zhuǎn)矩陣，t為KF 到CF 的平移向量。將式(1)左乘x的反對稱矩陣得到式(2)，其中 ∧代表反對稱矩陣。根據(jù)式(2)的左半部分等式求得l，根據(jù)右半部分等式求得l′。角度可由式(3)求得。

根據(jù)式(2)可求得X的深度信息，從而求得其三維坐標(biāo)。設(shè)p為從KF 相機(jī)光心指向X的向量，則可根據(jù)式(3)求出 β，同理求出 γ，從而求得 α。實驗測試發(fā)現(xiàn)，當(dāng) α大于某一閾值時，該關(guān)鍵點可能被遮擋，不做處理。α小于角度閾值時，獲得CF 中x′關(guān) 鍵點的深度lproj，并將其與深度圖投影得到的l′比 較，如果 Δl=lproj-l′超過閾值 τz，即認(rèn)定其為動態(tài)點。經(jīng)過在TUM 數(shù)據(jù)集上的實驗測試，α= 30°和 τz=0.2 m。

1.2 八叉樹建圖算法

八叉樹地圖是通過概率更新地圖。存儲f來表示節(jié)點是否被占據(jù)，體素的占據(jù)概率可通過反對數(shù)變換獲得。設(shè)f∈R，為概率對數(shù)；a為節(jié)點被占據(jù)的概率為0～1 之間，他們之間的變換可由logit 變換描述：

八叉樹地圖觀測到某節(jié)點被“占據(jù)”時，f增加，否則f減小。f從-∞到+∞時，a從0 變到1。當(dāng)某節(jié)點被反復(fù)觀察到時，其f值會不斷增加，從而a會不斷增加，a超過設(shè)定閾值時，該節(jié)點就會被判定為占用，并將在八叉樹圖中可視化。

2 實驗與結(jié)果分析

使用TUM RGB-D 數(shù)據(jù)集[14]中關(guān)于動態(tài)物體的子數(shù)據(jù)集對本文算法的動態(tài)物體剔除、定位和建圖效果進(jìn)行實驗評估。TUM RGB-D 數(shù)據(jù)集的動態(tài)物體類子數(shù)據(jù)集中，數(shù)據(jù)集sitting 和walking 的8 個序列可代表多數(shù)動態(tài)環(huán)境，對傳統(tǒng)的SLAM 系統(tǒng)具有很大挑戰(zhàn)性。其中：sitting 序列(下文簡稱s 序列)屬于低動態(tài)環(huán)境；walking 序列(下文簡稱w 序列)屬于高動態(tài)環(huán)境。對于2 種類型的序列，相機(jī)有4 種運動模式：按照半徑1 m的半球軌跡移動(halfSphere)；相機(jī)沿xyz軸移動(xyz)；相機(jī)在滾動、俯仰和偏航軸上旋轉(zhuǎn)(rpy)；相機(jī)手動保持靜止(static)。實驗均在1 臺PC 上進(jìn)行，CPU 為AMD Ryzen 3700，內(nèi)存為16 GB，GPU 為RTX 2080，顯存為8 GB，依賴于OpenCV 4.3.0，PCL 1.12.0 等第三方庫。

2.1 動態(tài)物體剔除

采用語義分割網(wǎng)絡(luò)和本文動態(tài)物體剔除算法進(jìn)行動態(tài)物體剔除實驗的結(jié)果如圖4。比較圖4(a)(b)可看出：對于w_static 序列中某幀場景，兩人正從椅子上起身，并將椅子推入桌下，語義分割網(wǎng)絡(luò)只分割出人的部分，并未分割出被人移動的椅子；語義分割網(wǎng)絡(luò)增加多視角幾何算法后，椅子也得到了有效分割，因在深度圖上進(jìn)行區(qū)域生長的，故椅子的下半部分并未得到有效劃分。比較圖4(c)(d)可看出：對于w_rpy序列中某幀場景，兩人剛進(jìn)入相機(jī)視野中，正準(zhǔn)備移動椅子，因人在相機(jī)視野中占比過少，語義分割網(wǎng)絡(luò)未能成功分割出人和被人拖動的椅子；本文算法同樣能夠分割出被移動的椅子和人的手的部分，進(jìn)一步證明了本文算法的有效性和魯棒性。

圖4 動態(tài)場景下不同算法的動態(tài)物體剔除法效果Fig.4 Removal effect of dynamic object with different algorithms in dynamic scenes

2.2 位姿誤差估計

利用evo 工具包對ORB-SLAM3 與本文算法在w_half 序列上的位姿估計實驗結(jié)果進(jìn)行繪制，如圖5，包含整體軌跡、軌跡在xyz坐標(biāo)軸上和在rpy旋轉(zhuǎn)軸上的分量。其中g(shù)roundtruth 代表真實軌跡，ORB camera trajectory代表ORB-SLAM3 的軌跡，camera trajectory 代表本文算法的軌跡。分析圖5 可看出：ORB-SLAM3 的軌跡與真實軌跡偏差較大，兩者不能重合，而本文算法的軌跡與真實軌跡基本保持重合；ORB-SLAM3 的軌跡在x和y軸上的分量在53 s 處出現(xiàn)了波動，這是因為ORB-SLAM3 在移動的人身上提取了大量特征點，導(dǎo)致軌跡與真實軌跡產(chǎn)生位置上的偏差，而本文算法的軌跡在x和y軸上的分量與真實軌跡基本一致；ORB-SLAM3的軌跡在旋轉(zhuǎn)軸上的分量在65 s 產(chǎn)生了劇烈波動，這是因為相機(jī)處于旋轉(zhuǎn)狀態(tài)，人在椅子上并未移動，ORB-SLAM3 在人的格子衫上提取了大量特征點，產(chǎn)生的誤匹配點較多，導(dǎo)致旋轉(zhuǎn)分量出現(xiàn)偏差，而本文算法因剔除了動態(tài)物體，保持了較好的跟蹤效果。

圖5 動態(tài)場景下ORB-SLAM3 與本文算法的軌跡跟蹤結(jié)果Fig.5 Track tracking results of ORB-SLAM3 and this algorithm in dynamic scenes

絕對軌跡誤差(absolute trajectory error，ATE)為相機(jī)位姿的真實值和估計值的差，可直觀反映系統(tǒng)精度和軌跡的全局一致性。文中選用ATE 的均方根誤差(root mean square error，RMSE)評估算法的定位精度。表1 為ORB-SLAM3 與本文算法的ATE，其中N 代表使用語義分割網(wǎng)絡(luò)，N+G 代表同時使用語義分割網(wǎng)絡(luò)和多視角幾何算法，加粗部分表示在數(shù)據(jù)集中表現(xiàn)最好的結(jié)果。用相對提升率 ρ評估本文算法(N+G)的提升效果，計算公式為

其中：m為SLAM 算法的測試結(jié)果；n為本文算法的測試結(jié)果。每次測試均進(jìn)行5 次，取其結(jié)果的平均值為最終測試結(jié)果。由表1 可知：本文算法(N+G)無論在低動態(tài)還是在高動態(tài)的環(huán)境中都有卓越表現(xiàn)，在高動態(tài)環(huán)境下的相對提升率達(dá)80%以上，最高為97.16%，體現(xiàn)了本文算法的魯棒性和穩(wěn)定性；本文算法在s_rpy序列和s_xyz序列中的表現(xiàn)比ORB-SLAM3 略有下降，因部分圖像中的人在圖像中所占比重過大，動態(tài)區(qū)域剔除了較多的特征點，提取到的靜態(tài)特征點過少，影響位姿估計，但位姿精度仍保持在厘米級。由此表明，使用多視角幾何和語義分割網(wǎng)絡(luò)的效果較好，故后文實驗均采取N+G 的模式。

表1 動態(tài)場景下本文算法的定位精度提升效果Tab.1 Positioning accuracy improvement effect of this algorithm in dynamic scenes

本文算法與其他動態(tài)場景下的DS-SLAM[15]，Detect-SLAM[16]，DVO-SLAM[4]和MR-SLAM[17]的ATE測試結(jié)果如表2。由表2 可看出：本文算法在8 個序列上的位姿估計精度均明顯優(yōu)于其他4 種算法。

表2 動態(tài)場景下不同SLAM 算法的ATE 測試結(jié)果Tab.2 ATE test results of different SLAM algorithms in dynamic scenes

2.3 語義八叉樹建圖

八叉樹建圖線程通過體素濾波降低點云冗積后構(gòu)建純靜態(tài)環(huán)境的語義八叉樹地圖。圖6 為本文算法在s_static 序列的測試結(jié)果；圖6(a)為數(shù)據(jù)集拍攝的真實場景；圖6(b)為根據(jù)s_static 序列生成的語義八叉樹地圖。對比圖6(a)(b)可看出：坐在椅子上人的部分基本被移除，沒有因為人一直在場景中移動而出現(xiàn)“鬼影”現(xiàn)象。s_static 序列共707 幀圖像，根據(jù)圖片序列生成的點云地圖磁盤文件為55.6 MB；八叉樹地圖的磁盤文件僅3.1 MB，為點云文件的5.64%，采用八叉樹地圖可有效對大規(guī)模場景進(jìn)行建圖。

圖6 語義八叉樹建圖實驗結(jié)果Fig.6 Experimental results of semantic octree mapping

2.4 實時性分析

通過各模塊運行時間評估算法的實時性，本文算法各模塊的平均運行時間如表3。輕量化追蹤模塊、多視角幾何、語義分割網(wǎng)絡(luò)和特征點提取基本構(gòu)成SLAM 算法的跟蹤線程。語義建圖線程與跟蹤線程并行，因此不參與計算。由表3 可看出：本文算法各模塊的平均跟蹤耗時累計130 ms 左右，故本文算法的位姿估計基本滿足實時性要求。

表3 本文算法各模塊的平均運行時間Tab.3 Average running time of each module of this algorithm

3 結(jié) 論

針對室內(nèi)復(fù)雜動態(tài)環(huán)境，提出一種基于語義分割網(wǎng)絡(luò)的動態(tài)場景視覺SLAM 算法。通過RGB-D 相機(jī)采集彩色圖像與深度圖像，結(jié)合輕量化的LR-ASPP 語義分割網(wǎng)絡(luò)先剔除先驗動態(tài)物體，再使用多視角幾何算法剔除非先驗動態(tài)物體，得到更準(zhǔn)確的位姿估計，最后結(jié)合深度圖與語義分割網(wǎng)絡(luò)的語義圖，同時構(gòu)建一個純靜態(tài)的語義八叉樹地圖直接用于導(dǎo)航系統(tǒng)。在TUM 數(shù)據(jù)集上的驗證結(jié)果表明：本文算法具有良好的實時性和位姿估計精度；特別是在高動態(tài)環(huán)境中，依然保持良好的SLAM 定位精度。但本文算法也存在可改進(jìn)之處，當(dāng)動態(tài)物體在視野中的比重過大時，位姿估計會受到較大的影響，下一步可通過對分割模型進(jìn)一步優(yōu)化，使網(wǎng)絡(luò)能分割更精細(xì)的目標(biāo)區(qū)域來解決該問題；多視角幾何檢測動態(tài)物體有滯后性，只有當(dāng)動態(tài)目標(biāo)移動足夠的距離后才會被檢測到，可考慮采用概率傳播的方式將該幀的動態(tài)點向前傳播，降低滯后性；同時，在定位過程中沒有利用語義信息，下一步可考慮將語義信息用于輔助定位。