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

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

即時(shí)定位與建圖(simultaneous localization and mapping，SLAM)技術(shù)作為移動(dòng)機(jī)器人實(shí)現(xiàn)真正自主的核心技術(shù)之一，在智能移動(dòng)機(jī)器人、無(wú)人駕駛等領(lǐng)域發(fā)揮著重大的作用[1]。常用的SLAM 系統(tǒng)根據(jù)使用傳感器的不同，可分為激光SLAM 和視覺(jué)SLAM。視覺(jué)SLAM 主要依靠攝像機(jī)傳感器數(shù)據(jù)，融合計(jì)算機(jī)視覺(jué)、深度學(xué)習(xí)等技術(shù)可更好地解決回環(huán)檢測(cè)、語(yǔ)義場(chǎng)景構(gòu)建等問(wèn)題，具有簡(jiǎn)易便攜、硬件成本低且定位精度高的優(yōu)勢(shì)，已逐步成為SLAM 研究的主流趨勢(shì)[2]。傳統(tǒng)視覺(jué)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è)。客觀世界中，存在行人、動(dòng)物、車(chē)輛等動(dòng)態(tài)物體的環(huán)境是不可避免的，動(dòng)態(tài)物體上的特征點(diǎn)會(huì)影響特征匹配結(jié)果，致使算法的魯棒性和定位精度明顯下降。

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

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

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

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

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

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

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

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

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

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

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

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

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

1.2 八叉樹(shù)建圖算法

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

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

2 實(shí)驗(yàn)與結(jié)果分析

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

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

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

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

2.2 位姿誤差估計(jì)

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

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

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

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

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

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

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

2.3 語(yǔ)義八叉樹(shù)建圖

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

圖6 語(yǔ)義八叉樹(shù)建圖實(shí)驗(yàn)結(jié)果Fig.6 Experimental results of semantic octree mapping

2.4 實(shí)時(shí)性分析

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

表3 本文算法各模塊的平均運(yùn)行時(shí)間Tab.3 Average running time of each module of this algorithm

3 結(jié) 論

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