張樂添,趙冬青,賈曉雪,楊朝永,賴路廣,郭文卓
面向動(dòng)態(tài)環(huán)境的雙目/慣性SLAM系統(tǒng)
張樂添,趙冬青,賈曉雪,楊朝永,賴路廣,郭文卓
(信息工程大學(xué)地理空間信息學(xué)院,鄭州 450001)
基于靜態(tài)場景假設(shè)的即時(shí)定位與建圖(SLAM)系統(tǒng)仍存在諸多問題:傳統(tǒng)視覺SLAM中的關(guān)鍵幀并不包含語義信息,在實(shí)際場景中總包含大量動(dòng)態(tài)點(diǎn),不僅會(huì)影響系統(tǒng)精度,甚至可能出現(xiàn)跟蹤失敗。針對(duì)以上問題,提出一種語義SLAM思路。首先,提出利用掩膜區(qū)域卷積神經(jīng)網(wǎng)絡(luò)(Mask R-CNN)給出的先驗(yàn)語義信息進(jìn)行篩選,摒棄動(dòng)態(tài)點(diǎn)的干擾,將這項(xiàng)功能作為新的線程加入尺度不變特征變換即時(shí)定位與建圖(ORB-SLAM3)。其次,結(jié)合ORB-SLAM3的雙目/慣性里程計(jì)部分,基于慣導(dǎo)預(yù)積分協(xié)方差設(shè)置自適應(yīng)閾值,判斷先驗(yàn)動(dòng)態(tài)點(diǎn)的真實(shí)動(dòng)態(tài)性。最后,利用實(shí)測(cè)數(shù)據(jù)對(duì)算法進(jìn)行驗(yàn)證。實(shí)驗(yàn)結(jié)果表明,3個(gè)數(shù)據(jù)集序列絕對(duì)軌跡誤差分別降低56.3%、71.5%、19.4%。因此,該系統(tǒng)能夠有效判別特征點(diǎn)的真實(shí)動(dòng)態(tài)性,屏蔽動(dòng)態(tài)點(diǎn)的干擾,從而提高定位精度。
動(dòng)態(tài)環(huán)境;即時(shí)定位與建圖;預(yù)積分;語義分割;神經(jīng)網(wǎng)絡(luò)
近年來,視覺即時(shí)定位與建圖(simultaneous localization and mapping, SLAM)的整體框架已趨近成熟。當(dāng)前成熟的開源方案[1-4]一般都基于靜態(tài)環(huán)境的假設(shè),然而真實(shí)環(huán)境往往難以滿足條件[5]。由于系統(tǒng)估計(jì)的是載體與物體之間的相對(duì)運(yùn)動(dòng),此類環(huán)境下運(yùn)行會(huì)分不清載體與物體的運(yùn)動(dòng),而最終導(dǎo)致絕對(duì)定位信息不準(zhǔn)確、環(huán)境地圖構(gòu)建發(fā)生嚴(yán)重的偏差等問題[6]。
隨著深度學(xué)習(xí)算法的迅速發(fā)展,基于卷積神經(jīng)網(wǎng)絡(luò)的目標(biāo)檢測(cè)算法與視覺SLAM相結(jié)合,取得了良好的性能。文獻(xiàn)[7]基于尺度不變特征變換即時(shí)定位與建圖(oriented fast and rotated brief simultaneous localization and mapping,ORB-SLAM2),結(jié)合目標(biāo)檢測(cè)(you only look once,YOLO)[8]算法對(duì)檢測(cè)到的關(guān)鍵幀進(jìn)行檢測(cè),剔除位于矩形框內(nèi)的特征點(diǎn)。然而,僅給出矩形框難免會(huì)影響篩選的精度,但是語義分割方法可達(dá)到像素級(jí)別從而識(shí)別環(huán)境中動(dòng)態(tài)特征點(diǎn)的先驗(yàn)信息。文獻(xiàn)[9]增加了一個(gè)基于語義分割網(wǎng)絡(luò)(segmentation network,SegNet)[10]的語義分割線程,移除先驗(yàn)?zāi)繕?biāo),利用隨機(jī)抽樣一致算法(random sample consensus, RANSAC)估計(jì)位姿,隨后利用極線約束做運(yùn)動(dòng)一致性判斷。文獻(xiàn)[11]同樣基于ORB-SLAM2,考慮了3種相機(jī)模型,利用Mask R-CNN[12]分割出潛在的運(yùn)動(dòng)目標(biāo)后,利用剩余的圖像區(qū)域進(jìn)行后續(xù)的定位與建圖,針對(duì)RGB-D相機(jī)能采集深度信息,添加了一個(gè)基于多視圖幾何的運(yùn)動(dòng)一致性檢測(cè),用于判斷分割區(qū)域的真實(shí)動(dòng)態(tài)性。文獻(xiàn)[13]能夠?qū)崿F(xiàn)運(yùn)動(dòng)分割、動(dòng)態(tài)目標(biāo)跟蹤、攝像機(jī)位姿估計(jì)以及靜態(tài)和動(dòng)態(tài)結(jié)構(gòu)估計(jì),輸出動(dòng)態(tài)物體的軌跡以及它們隨時(shí)間化的位姿估計(jì)值,并提取速度信息。文獻(xiàn)[14]在ORB-SLAM3上融合了SegNet和Mask R-CNN,提出適用3種相機(jī)的系統(tǒng),利用先驗(yàn)的語義信息更新地圖點(diǎn)的移動(dòng)概率信息用來判斷動(dòng)態(tài)性,同時(shí)為了解決分割的實(shí)時(shí)性問題提出了一種非阻塞關(guān)鍵幀模型。
上述研究中,大多僅從理論說明單目、雙目的可用性,并沒有進(jìn)行實(shí)驗(yàn)驗(yàn)證,對(duì)于物體真實(shí)動(dòng)態(tài)性的判斷依賴位姿的估計(jì),其本身缺乏準(zhǔn)確性。并且純視覺SLAM在動(dòng)態(tài)物體占幅大時(shí),僅用靜態(tài)點(diǎn)跟蹤容易造成跟蹤丟失。本文在ORB-SLAM3的基礎(chǔ)上,通過建立單獨(dú)的語義線程,利用Mask R-CNN對(duì)雙目圖像進(jìn)行分割,計(jì)算IMU預(yù)積分的點(diǎn)位中誤差,對(duì)于分割出的目標(biāo)進(jìn)行真實(shí)動(dòng)態(tài)性檢測(cè),并利用實(shí)測(cè)數(shù)據(jù)對(duì)系統(tǒng)性能進(jìn)行分析和驗(yàn)證。
圖1 系統(tǒng)結(jié)構(gòu)
RANSAC、IMU和多地圖機(jī)制,使得ORB-SLAM3在一定程度上可以抵抗動(dòng)態(tài)點(diǎn)的干擾。然而,在實(shí)際場景中,例如道路街景,動(dòng)態(tài)目標(biāo)在圖像中占比較大,且持續(xù)出現(xiàn),難以保證系統(tǒng)的精度。
RANSAC對(duì)暴力匹配產(chǎn)生的誤匹配進(jìn)行篩選,一定程度上能將動(dòng)態(tài)物體上的特征點(diǎn)識(shí)別為離群值,將其剔除;但動(dòng)態(tài)物體占比較大時(shí),特征點(diǎn)可能大量分布在動(dòng)態(tài)物體上。
關(guān)鍵幀機(jī)制可以有效避免跟蹤和優(yōu)化過程中不必要的冗余。每個(gè)關(guān)鍵幀存儲(chǔ)相機(jī)姿態(tài)、特征點(diǎn)和共視關(guān)系。大多語義分割方法的分割速度難以滿足每個(gè)圖像幀的即時(shí)需求,因此通常的做法是僅僅獲取關(guān)鍵幀的語義標(biāo)簽。然而,仍有一部分,例如Mask R-CNN,無法為系統(tǒng)提供最新的語義標(biāo)簽。因此,本文采用了文獻(xiàn)[14]中的非阻塞模型,在一個(gè)滑窗內(nèi)同時(shí)分割最舊的幀和最新的幀。從而,盡量為語義線程提供最新的語義標(biāo)簽。
Mask R-CNN可實(shí)現(xiàn)像素級(jí)別的圖像實(shí)例分割,并行處理物體檢測(cè)和目標(biāo)分割。Mask R-CNN是在快速區(qū)域卷積神經(jīng)網(wǎng)絡(luò)(faster region convolutional neural network, Faster R-CNN)[15]基礎(chǔ)上發(fā)展而來,在其基礎(chǔ)上增加興趣區(qū)域?qū)R,將分類預(yù)測(cè)和掩膜預(yù)測(cè)拆分為網(wǎng)絡(luò)的2個(gè)分支,分類預(yù)測(cè)分支與Faster R-CNN相同,對(duì)興趣區(qū)域給出預(yù)測(cè),產(chǎn)生類別標(biāo)簽以及矩形框坐標(biāo)輸出,而掩碼預(yù)測(cè)分支產(chǎn)生的每個(gè)二值掩碼依賴分類預(yù)測(cè)結(jié)果,基于此刻分割出物體,對(duì)每個(gè)類別均獨(dú)立地預(yù)測(cè)一個(gè)二值掩碼,避開類間的競爭。圖2為Mask R-CNN分割結(jié)果。
圖2 分割結(jié)果
在真實(shí)的高動(dòng)態(tài)場景中,并不是所有的先驗(yàn)?zāi)繕?biāo)均為動(dòng)態(tài)。如果僅使用靜態(tài)背景上的點(diǎn)進(jìn)行跟蹤,難以保證跟蹤點(diǎn)的質(zhì)量,進(jìn)而影響系統(tǒng)的精度,尤其是動(dòng)態(tài)目標(biāo)占比較高時(shí),甚至?xí)斐筛檨G失。為了避免使用系統(tǒng)估計(jì)出的剛體姿態(tài)變換,而是通過IMU預(yù)積分量對(duì)投影點(diǎn)位置進(jìn)行預(yù)測(cè)。通過預(yù)積分的協(xié)方差傳遞,得到點(diǎn)位誤差,判斷匹配點(diǎn)位是否在限差內(nèi)。
為了獲得預(yù)測(cè)點(diǎn)位與匹配點(diǎn)位的距離,需要利用IMU預(yù)積分量,對(duì)上一關(guān)鍵幀中的先驗(yàn)語義點(diǎn)進(jìn)行投影,得到下一關(guān)鍵幀中的預(yù)測(cè)像素坐標(biāo)。
式中:為零矩陣;為單位矩陣。
已知上一關(guān)鍵幀載體系在世界坐標(biāo)系下的位姿,可得當(dāng)前關(guān)鍵幀在世界坐標(biāo)系下的位姿為
本文的標(biāo)定結(jié)果,相機(jī)內(nèi)參以及相機(jī)系和載體系的轉(zhuǎn)換,均是利用校準(zhǔn)(Kalibr)功能包[17]標(biāo)定得到。
在ORB-SLAM3中,預(yù)積分協(xié)方差不斷地傳遞。相鄰二幀之間的IMU預(yù)積分量協(xié)方差為
由此可以得到像素坐標(biāo)的中誤差分別為
則該點(diǎn)的點(diǎn)位誤差為
依據(jù)正態(tài)分布,按照95.45%的置信概率,將閾值設(shè)置為
圖3為本文采集數(shù)據(jù)所用的車載實(shí)驗(yàn)平臺(tái)。平臺(tái)上傳感器指標(biāo)如表1所示。表1中,PPI(pixels per inch)為每英寸的像素?cái)?shù)量,1英寸等于 2.54 cm。
圖3 車載多源試驗(yàn)平臺(tái)
表1 傳感器指標(biāo)
相機(jī)和IMU的標(biāo)定使用Kalibr工具箱,傳感器的空間位置采用事先標(biāo)定的方式,所有傳感器統(tǒng)一至GPS時(shí)(GPS time, GPST)。實(shí)驗(yàn)采用PPP/INS緊組合算法結(jié)果作為對(duì)比方法,NovAtel IE(NovAtel inertial exploer)軟件解算的實(shí)時(shí)動(dòng)態(tài)差分定位(real-time kinematic positioning, RTK)/戰(zhàn)術(shù)級(jí)慣導(dǎo)事后雙向緊組合平滑解作為參考值。
本文截取了3段數(shù)據(jù)集,用來證明不同環(huán)境下的系統(tǒng)性能。Car_01數(shù)據(jù)集處于城市停車場周邊,目標(biāo)動(dòng)態(tài)、靜態(tài)參半;Car_02數(shù)據(jù)集處于城市街道,先驗(yàn)語義目標(biāo)大多為動(dòng)態(tài);Car_03數(shù)據(jù)集處于鄉(xiāng)村街道,車輛在路邊???,先驗(yàn)語義目標(biāo)大多為靜態(tài)。本文使用絕對(duì)軌跡誤差(absolute trajectory error, ATE)的均方根誤差(root mean square error, RMSE)作為評(píng)價(jià)算法精度的指標(biāo)。
3.2.1 Car_01數(shù)據(jù)驗(yàn)證
圖4所示為利用IMU輔助動(dòng)態(tài)性判斷前后圖像幀中特征點(diǎn)的情況。路邊植被和路燈上的深色點(diǎn)為靜態(tài)點(diǎn),車輛上較淺色的點(diǎn)為動(dòng)態(tài)點(diǎn)。
圖4 利用IMU修正前后特征點(diǎn)對(duì)比
從圖4不難看出,僅利用語義分割(左)時(shí),系統(tǒng)默認(rèn)右側(cè)停車場內(nèi)靜態(tài)車輛上所提取特征點(diǎn)為動(dòng)態(tài)點(diǎn),然而這些點(diǎn)可以提供高質(zhì)量的觀測(cè)信息。如果貿(mào)然剔除,不僅會(huì)影響精度,并且在面對(duì)圖片質(zhì)量不高、特征信息缺少的場景時(shí),很有可能造成跟蹤丟失。右圖為進(jìn)行利用IMU進(jìn)行判斷動(dòng)態(tài)性后,修補(bǔ)誤剔除特征點(diǎn)后的情況。
圖5、圖6為Car_01序列分別在3種方法下運(yùn)行的軌跡和ATE曲線。Car_01序列所處場景為動(dòng)、靜態(tài)目標(biāo)參半,理論上來說此場景下本文方法更為適用。不難看出,本文方法所得到的軌跡,與真實(shí)IE解算的軌跡更為近似。表2為計(jì)算的不同方法ATE的RMSE(10次取平均)??梢钥闯?,本文方法的軌跡誤差明顯降低,性能提升達(dá)56.3%,因此可以證明本文方法在該場景下是更為有效的。
圖5 Car_01序列軌跡對(duì)比
圖6 Car_01序列ATE
表2 Car_01序列ATE對(duì)比
3.2.2 Car_02數(shù)據(jù)驗(yàn)證
Car_02數(shù)據(jù)集所處場景中先驗(yàn)?zāi)繕?biāo)均為動(dòng)態(tài)。為了證明本文方法的普適性,需要證明在此場景下,系統(tǒng)精度并不會(huì)比僅加入語義分割的方法差。
圖7、圖8為Car_02序列軌跡對(duì)比以及ATE曲線,表3為Car_02序列ATE的RMSE(10次取平均)。結(jié)合圖7、圖8和表3,可以看出加入IMU修正前后性能提升分別為70.2%、71.5%,其軌跡也十分相似。2種方法在此場景下的精度相當(dāng)??梢宰C明,本文的方法在動(dòng)態(tài)目標(biāo)占比大的場景也可以保證穩(wěn)定性,具有普適性。
圖7 Car_02序列軌跡對(duì)比
圖8 Car_02序列ATE
表3 Car_02序列ATE對(duì)比
3.2.3 Car_03數(shù)據(jù)驗(yàn)證
Car_03數(shù)據(jù)集所處場景中先驗(yàn)?zāi)繕?biāo)大多數(shù)為靜態(tài),更符合ORB-SLAM3的原本場景假設(shè)。同樣,為了證明本文方法的普適性,需要證明在此場景下,系統(tǒng)精度并不會(huì)比原本ORB-SLAM3的方法變差。圖9、圖10為CAR_03序列的不同方法軌跡對(duì)比圖和ATE曲線圖,表4為對(duì)應(yīng)的ATE的RMSE(10次取平均)。
從圖9、圖10中可以看出,因?yàn)閳鼍爸械南闰?yàn)?zāi)繕?biāo)大多數(shù)為靜態(tài),僅加入語義分割的方法精度最低,主要原因是對(duì)所有靜態(tài)的先驗(yàn)?zāi)繕?biāo)上的特征點(diǎn)進(jìn)行剔除,只利用樹葉、光滑的馬路等之類的特征點(diǎn)進(jìn)行跟蹤,特征信息質(zhì)量不夠高。同時(shí),由于只有一輛路中行駛的車輛,這是導(dǎo)致本文方法精度略高于原ORB-SLAM3的原因,但是其精度基本是近似的。因此,可以認(rèn)為本文方法即使在原ORB-SLAM3的靜態(tài)場景的假設(shè)下,同樣具備適用性。
圖9 Car_03序列軌跡對(duì)比
圖10 Car_03序列ATE
表4 Car_03序列ATE對(duì)比
本文基于ORB-SLAM3提出了一種結(jié)合語義分割和IMU判斷真實(shí)動(dòng)態(tài)性的動(dòng)態(tài)VI-SLAM。首先,通過在ORB-SLAM3中加入一個(gè)獨(dú)立的線程,利用Mask R-CNN進(jìn)行語義分割,從而得到先驗(yàn)?zāi)繕?biāo)的物體掩膜,并將其中的特征點(diǎn)設(shè)置為待定點(diǎn)。其次,基于IMU預(yù)積分協(xié)方差遞推得到點(diǎn)位中誤差,并據(jù)此設(shè)定閾值,對(duì)待定點(diǎn)判斷其真實(shí)動(dòng)態(tài)性。最后,基于車載多源平臺(tái)所采的數(shù)據(jù)集對(duì)本文方法進(jìn)行驗(yàn)證:Car_01序列絕對(duì)軌跡誤差均方根降低了56.3%;Car_02序列絕對(duì)軌跡誤差均方根降低了71.5%;Car_03序列絕對(duì)軌跡誤差均方根降低了19.4%。通過利用3個(gè)不同場景的數(shù)據(jù)集對(duì)3種方法進(jìn)行對(duì)比,驗(yàn)證了本文方法的有效性和普適性。
[1] MUR-ARTAL R, MONTIEL J M M, TARDOS J D. ORB-SLAM: a versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163.
[2] MUR-ARTAL R, TARDOS J D. ORB-SLAM2: an open-source slam system for monocular, stereo and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255-1262.
[3] CAMPOS C, ELVIRA R, JJG RODRIGUEZ, et al. ORB-SLAM3: an accurate open-source library for visual, visual-inertial and multi-map SLAM[J]. IEEE Transactions on Robotics, 2020, 37(6): 1874-1890.
[4] QIN T, LI P L, SHEN S J, et al. VINS-Mono:a robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.
[5] 王柯賽, 姚錫凡, 黃宇, 等. 動(dòng)態(tài)環(huán)境下的視覺SLAM研究評(píng)述[J]. 機(jī)器人, 2021, 43(6): 18.
[6] 高興波, 史旭華, 葛群峰, 等. 面向動(dòng)態(tài)物體場景的視覺SLAM綜述[J]. 機(jī)器人, 2021, 43(6): 18.
[7] ZHANG L, WEI L, SHEN P, et al. Semantic slam based on object detection and improved octomap[J]. IEEE Access, 2018, 6: 1-1.
[8] REDMON J, FARHADI A. YOLO9000: better, faster, stronger[EB/OL].[2022-03-06]. https://openaccess.thecvf. com/ content_cvpr_2017/papers/Redmon_YOLO9000_Better_Faster_CVPR_2017_paper.pdf.
[9] YU C, LIU Z, LIU X, et al. DS-SLAM:a semantic visual SLAM towards dynamic environments[EB/OL].[2022-03-06].https://arxiv.org/ftp/arxiv/papers/1809/1809.08379.pdf.
[10] BADRINARAYANAN V, HANDA A, CIPOLLA R. SegNet: a deep convolutional encoder-decoder architecture for robust semantic pixel-wise labelling[J]. Computer Science, 2017, 39(12): 2481-2495.
[11] BESCOS B, FáCIL J M, CIVERA J, et al. DynaSLAM: tracking, mapping and inpainting in dynamic scenes[EB/OL].[2022-03-06]. https://arxiv.org/pdf/1806.05620.pdf.
[12] HE K, GKIOXARI G, P DOLLáR, et al.Mask R-CNN[EB/OL].[2022-03-06]. https://openaccess.thecvf.com/ content_ICCV_2017/papers/He_Mask_R-CNN_ICCV_2017_paper.pdf
[13] ZHANG J, HENEIN M, MAHONY R, et al. VDO-SLAM: a visual dynamic object-aware SLAM system [EB/OL]. [2022-03-06]. https://arxiv. org/abs/2005.11052.
[14] LIU Y, MIURA J. RDS-SLAM: real-time dynamic SLAM using semantic segmentation methods[J]. IEEE Access, 2021, 9: 1-1.
[15] REN S, HE K, GIRSHICK R, et al. Faster R-CNN: towards real-time object detection with region proposal networks[EB/OL].(2016-01-06)[2022-03-06].http://papers.nips.cc/paper/5638-faster-r-cnn-towards-realtime-object- detection-with-region-proposal-networks.pdf.
[16] 高翔, 張濤, 劉毅, 等. 視覺SLAM十四講: 從理論到實(shí)踐[M]. 2版.北京:電子工業(yè)出版社, 2019: 46-47.
[17] FURGALE P, REHDER J, SIEGWART R. Unified temporal and spatial calibration for multi-sensor systems[EB/OL]. [2022-03-06]. https://furgalep.github.io/bib/furgale_iros13.pdf.
Stereo/Inertial SLAM system for dynamic environment
ZHANG Letian, ZHAO Dongqing, JIA xiaoxue, YANG Chaoyong, LAI Luguang, GUO Wenzhuo
(School of Geodesy and Geomatics, Information Engineering University, Zhengzhou 450001, China)
There are still many problems with the static scenario based on simultaneous localization and mapping (SLAM): the keyframes in traditional visual SLAM do not contain semantic information, but a large number of dynamic points in the actual scenario that not only affect system accuracy, but also easily track failure. This paper proposes a semantic slam system. First, the interference of the dynamic point is removed with prior semantic information given by mask region convolutional neural network (Mask R-CNN), the function join oriented fast and rotated brief simultaneous localization and mapping (ORB-SLAM3) as a new thread .Secondly, combining the stereo/inertial odometry, the adaptive threshold is set based on the inertial measurement unit (IMU) preintegration covariance, and the real dynamics of the transcendental dynamic point is determined. Finally, the experimental results show that the absolute trajectory errors of the three datasets are reduced by 56.3%, 71.5% and 19.4%, respectively. Therefore, the system effectively distinguish the real dynamics of feature points, shield the interference of dynamic points, and improve the positioning accuracy.
dynamic environment; simultaneous localization and mapping; preintegration; semantic segmentation; neural network
P228
A
2095-4999(2022)06-0150-07
張樂添,趙冬青,賈曉雪,等. 面向動(dòng)態(tài)環(huán)境的雙目/慣性SLAM系統(tǒng)[J]. 導(dǎo)航定位學(xué)報(bào), 2022, 10(6): 144-150.(ZHANG Letian, ZHAO Dongqing, JIA xiaoxue, et al. Stereo/Inertial SLAM system for dynamic environment[J]. Journal of Navisgation and Positioning, 2022, 10(6): 144-150.)
10.16547/j.cnki.10-1096.20220619.
2022-07-12
國際自然科學(xué)基金項(xiàng)目(41774037,42104033)。
張樂添(1998—),男,河南焦作人,碩士研究生,研究方向?yàn)橐曈X/慣性SLAM。
趙冬青(1976—),男,湖北十堰人,博士,教授,研究方向?yàn)閷?dǎo)航定位與位置服務(wù)。