面向動態(tài)環(huán)境的雙目/慣性SLAM系統(tǒng)

張樂添，趙冬青，賈曉雪，楊朝永，賴路廣，郭文卓

面向動態(tài)環(huán)境的雙目/慣性SLAM系統(tǒng)

張樂添，趙冬青，賈曉雪，楊朝永，賴路廣，郭文卓

（信息工程大學地理空間信息學院，鄭州 450001）

基于靜態(tài)場景假設(shè)的即時定位與建圖(SLAM)系統(tǒng)仍存在諸多問題：傳統(tǒng)視覺SLAM中的關(guān)鍵幀并不包含語義信息，在實際場景中總包含大量動態(tài)點，不僅會影響系統(tǒng)精度，甚至可能出現(xiàn)跟蹤失敗。針對以上問題，提出一種語義SLAM思路。首先，提出利用掩膜區(qū)域卷積神經(jīng)網(wǎng)絡（Mask R-CNN）給出的先驗語義信息進行篩選，摒棄動態(tài)點的干擾，將這項功能作為新的線程加入尺度不變特征變換即時定位與建圖（ORB-SLAM3）。其次，結(jié)合ORB-SLAM3的雙目/慣性里程計部分，基于慣導預積分協(xié)方差設(shè)置自適應閾值，判斷先驗動態(tài)點的真實動態(tài)性。最后，利用實測數(shù)據(jù)對算法進行驗證。實驗結(jié)果表明，3個數(shù)據(jù)集序列絕對軌跡誤差分別降低56.3%、71.5%、19.4%。因此，該系統(tǒng)能夠有效判別特征點的真實動態(tài)性，屏蔽動態(tài)點的干擾，從而提高定位精度。

動態(tài)環(huán)境；即時定位與建圖；預積分；語義分割；神經(jīng)網(wǎng)絡

0 引言

近年來，視覺即時定位與建圖（simultaneous localization and mapping, SLAM）的整體框架已趨近成熟。當前成熟的開源方案[1-4]一般都基于靜態(tài)環(huán)境的假設(shè)，然而真實環(huán)境往往難以滿足條件[5]。由于系統(tǒng)估計的是載體與物體之間的相對運動，此類環(huán)境下運行會分不清載體與物體的運動，而最終導致絕對定位信息不準確、環(huán)境地圖構(gòu)建發(fā)生嚴重的偏差等問題[6]。

隨著深度學習算法的迅速發(fā)展，基于卷積神經(jīng)網(wǎng)絡的目標檢測算法與視覺SLAM相結(jié)合，取得了良好的性能。文獻[7]基于尺度不變特征變換即時定位與建圖（oriented fast and rotated brief simultaneous localization and mapping，ORB-SLAM2），結(jié)合目標檢測（you only look once，YOLO）[8]算法對檢測到的關(guān)鍵幀進行檢測，剔除位于矩形框內(nèi)的特征點。然而，僅給出矩形框難免會影響篩選的精度，但是語義分割方法可達到像素級別從而識別環(huán)境中動態(tài)特征點的先驗信息。文獻[9]增加了一個基于語義分割網(wǎng)絡（segmentation network，SegNet）[10]的語義分割線程，移除先驗目標，利用隨機抽樣一致算法（random sample consensus, RANSAC）估計位姿，隨后利用極線約束做運動一致性判斷。文獻[11]同樣基于ORB-SLAM2，考慮了3種相機模型，利用Mask R-CNN[12]分割出潛在的運動目標后，利用剩余的圖像區(qū)域進行后續(xù)的定位與建圖，針對RGB-D相機能采集深度信息，添加了一個基于多視圖幾何的運動一致性檢測，用于判斷分割區(qū)域的真實動態(tài)性。文獻[13]能夠?qū)崿F(xiàn)運動分割、動態(tài)目標跟蹤、攝像機位姿估計以及靜態(tài)和動態(tài)結(jié)構(gòu)估計，輸出動態(tài)物體的軌跡以及它們隨時間化的位姿估計值，并提取速度信息。文獻[14]在ORB-SLAM3上融合了SegNet和Mask R-CNN，提出適用3種相機的系統(tǒng)，利用先驗的語義信息更新地圖點的移動概率信息用來判斷動態(tài)性，同時為了解決分割的實時性問題提出了一種非阻塞關(guān)鍵幀模型。

上述研究中，大多僅從理論說明單目、雙目的可用性，并沒有進行實驗驗證，對于物體真實動態(tài)性的判斷依賴位姿的估計，其本身缺乏準確性。并且純視覺SLAM在動態(tài)物體占幅大時，僅用靜態(tài)點跟蹤容易造成跟蹤丟失。本文在ORB-SLAM3的基礎(chǔ)上，通過建立單獨的語義線程，利用Mask R-CNN對雙目圖像進行分割，計算IMU預積分的點位中誤差，對于分割出的目標進行真實動態(tài)性檢測，并利用實測數(shù)據(jù)對系統(tǒng)性能進行分析和驗證。

圖1 系統(tǒng)結(jié)構(gòu)

1 基于語義的雙目視覺/慣性系統(tǒng)

RANSAC、IMU和多地圖機制，使得ORB-SLAM3在一定程度上可以抵抗動態(tài)點的干擾。然而，在實際場景中，例如道路街景，動態(tài)目標在圖像中占比較大，且持續(xù)出現(xiàn)，難以保證系統(tǒng)的精度。

1.1 離群點剔除

RANSAC對暴力匹配產(chǎn)生的誤匹配進行篩選，一定程度上能將動態(tài)物體上的特征點識別為離群值，將其剔除；但動態(tài)物體占比較大時，特征點可能大量分布在動態(tài)物體上。

1.2 語義關(guān)鍵幀篩選策略

關(guān)鍵幀機制可以有效避免跟蹤和優(yōu)化過程中不必要的冗余。每個關(guān)鍵幀存儲相機姿態(tài)、特征點和共視關(guān)系。大多語義分割方法的分割速度難以滿足每個圖像幀的即時需求，因此通常的做法是僅僅獲取關(guān)鍵幀的語義標簽。然而，仍有一部分，例如Mask R-CNN，無法為系統(tǒng)提供最新的語義標簽。因此，本文采用了文獻[14]中的非阻塞模型，在一個滑窗內(nèi)同時分割最舊的幀和最新的幀。從而，盡量為語義線程提供最新的語義標簽。

1.3 語義分割方法

Mask R-CNN可實現(xiàn)像素級別的圖像實例分割，并行處理物體檢測和目標分割。Mask R-CNN是在快速區(qū)域卷積神經(jīng)網(wǎng)絡（faster region convolutional neural network, Faster R-CNN）[15]基礎(chǔ)上發(fā)展而來，在其基礎(chǔ)上增加興趣區(qū)域?qū)R，將分類預測和掩膜預測拆分為網(wǎng)絡的2個分支，分類預測分支與Faster R-CNN相同，對興趣區(qū)域給出預測，產(chǎn)生類別標簽以及矩形框坐標輸出，而掩碼預測分支產(chǎn)生的每個二值掩碼依賴分類預測結(jié)果，基于此刻分割出物體，對每個類別均獨立地預測一個二值掩碼，避開類間的競爭。圖2為Mask R-CNN分割結(jié)果。

圖2 分割結(jié)果

1.4 真實動態(tài)性檢測

在真實的高動態(tài)場景中，并不是所有的先驗目標均為動態(tài)。如果僅使用靜態(tài)背景上的點進行跟蹤，難以保證跟蹤點的質(zhì)量，進而影響系統(tǒng)的精度，尤其是動態(tài)目標占比較高時，甚至會造成跟蹤丟失。為了避免使用系統(tǒng)估計出的剛體姿態(tài)變換，而是通過IMU預積分量對投影點位置進行預測。通過預積分的協(xié)方差傳遞，得到點位誤差，判斷匹配點位是否在限差內(nèi)。

2 基于預積分點位誤差閾值設(shè)置

2.1 基于IMU預積分量的像素點位預測

為了獲得預測點位與匹配點位的距離，需要利用IMU預積分量，對上一關(guān)鍵幀中的先驗語義點進行投影，得到下一關(guān)鍵幀中的預測像素坐標。

式中：為零矩陣；為單位矩陣。

已知上一關(guān)鍵幀載體系在世界坐標系下的位姿，可得當前關(guān)鍵幀在世界坐標系下的位姿為

本文的標定結(jié)果，相機內(nèi)參以及相機系和載體系的轉(zhuǎn)換，均是利用校準（Kalibr）功能包[17]標定得到。

2.2 基于協(xié)方差的點位誤差閾值設(shè)定

在ORB-SLAM3中，預積分協(xié)方差不斷地傳遞。相鄰二幀之間的IMU預積分量協(xié)方差為

由此可以得到像素坐標的中誤差分別為

則該點的點位誤差為

依據(jù)正態(tài)分布，按照95.45%的置信概率，將閾值設(shè)置為

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

3.1 實驗配置

圖3為本文采集數(shù)據(jù)所用的車載實驗平臺。平臺上傳感器指標如表1所示。表1中，PPI（pixels per inch）為每英寸的像素數(shù)量，1英寸等于 2.54 cm。

圖3 車載多源試驗平臺

表1 傳感器指標

相機和IMU的標定使用Kalibr工具箱，傳感器的空間位置采用事先標定的方式，所有傳感器統(tǒng)一至GPS時（GPS time, GPST）。實驗采用PPP/INS緊組合算法結(jié)果作為對比方法，NovAtel IE（NovAtel inertial exploer）軟件解算的實時動態(tài)差分定位（real-time kinematic positioning, RTK）/戰(zhàn)術(shù)級慣導事后雙向緊組合平滑解作為參考值。

3.2 IMU輔助動態(tài)性判斷實驗驗證

本文截取了3段數(shù)據(jù)集，用來證明不同環(huán)境下的系統(tǒng)性能。Car_01數(shù)據(jù)集處于城市停車場周邊，目標動態(tài)、靜態(tài)參半；Car_02數(shù)據(jù)集處于城市街道，先驗語義目標大多為動態(tài)；Car_03數(shù)據(jù)集處于鄉(xiāng)村街道，車輛在路邊停靠，先驗語義目標大多為靜態(tài)。本文使用絕對軌跡誤差（absolute trajectory error, ATE）的均方根誤差（root mean square error, RMSE）作為評價算法精度的指標。

3.2.1 Car_01數(shù)據(jù)驗證

圖4所示為利用IMU輔助動態(tài)性判斷前后圖像幀中特征點的情況。路邊植被和路燈上的深色點為靜態(tài)點，車輛上較淺色的點為動態(tài)點。

圖4 利用IMU修正前后特征點對比

從圖4不難看出，僅利用語義分割（左）時，系統(tǒng)默認右側(cè)停車場內(nèi)靜態(tài)車輛上所提取特征點為動態(tài)點，然而這些點可以提供高質(zhì)量的觀測信息。如果貿(mào)然剔除，不僅會影響精度，并且在面對圖片質(zhì)量不高、特征信息缺少的場景時，很有可能造成跟蹤丟失。右圖為進行利用IMU進行判斷動態(tài)性后，修補誤剔除特征點后的情況。

圖5、圖6為Car_01序列分別在3種方法下運行的軌跡和ATE曲線。Car_01序列所處場景為動、靜態(tài)目標參半，理論上來說此場景下本文方法更為適用。不難看出，本文方法所得到的軌跡，與真實IE解算的軌跡更為近似。表2為計算的不同方法ATE的RMSE（10次取平均）。可以看出，本文方法的軌跡誤差明顯降低，性能提升達56.3%，因此可以證明本文方法在該場景下是更為有效的。

圖5 Car_01序列軌跡對比

圖6 Car_01序列ATE

表2 Car_01序列ATE對比

3.2.2 Car_02數(shù)據(jù)驗證

Car_02數(shù)據(jù)集所處場景中先驗目標均為動態(tài)。為了證明本文方法的普適性，需要證明在此場景下，系統(tǒng)精度并不會比僅加入語義分割的方法差。

圖7、圖8為Car_02序列軌跡對比以及ATE曲線，表3為Car_02序列ATE的RMSE（10次取平均）。結(jié)合圖7、圖8和表3，可以看出加入IMU修正前后性能提升分別為70.2%、71.5%，其軌跡也十分相似。2種方法在此場景下的精度相當。可以證明，本文的方法在動態(tài)目標占比大的場景也可以保證穩(wěn)定性，具有普適性。

圖7 Car_02序列軌跡對比

圖8 Car_02序列ATE

表3 Car_02序列ATE對比

3.2.3 Car_03數(shù)據(jù)驗證

Car_03數(shù)據(jù)集所處場景中先驗目標大多數(shù)為靜態(tài)，更符合ORB-SLAM3的原本場景假設(shè)。同樣，為了證明本文方法的普適性，需要證明在此場景下，系統(tǒng)精度并不會比原本ORB-SLAM3的方法變差。圖9、圖10為CAR_03序列的不同方法軌跡對比圖和ATE曲線圖，表4為對應的ATE的RMSE（10次取平均）。

從圖9、圖10中可以看出，因為場景中的先驗目標大多數(shù)為靜態(tài)，僅加入語義分割的方法精度最低，主要原因是對所有靜態(tài)的先驗目標上的特征點進行剔除，只利用樹葉、光滑的馬路等之類的特征點進行跟蹤，特征信息質(zhì)量不夠高。同時，由于只有一輛路中行駛的車輛，這是導致本文方法精度略高于原ORB-SLAM3的原因，但是其精度基本是近似的。因此，可以認為本文方法即使在原ORB-SLAM3的靜態(tài)場景的假設(shè)下，同樣具備適用性。

圖9 Car_03序列軌跡對比

圖10 Car_03序列ATE

表4 Car_03序列ATE對比

4 結(jié)束語

本文基于ORB-SLAM3提出了一種結(jié)合語義分割和IMU判斷真實動態(tài)性的動態(tài)VI-SLAM。首先，通過在ORB-SLAM3中加入一個獨立的線程，利用Mask R-CNN進行語義分割，從而得到先驗目標的物體掩膜，并將其中的特征點設(shè)置為待定點。其次，基于IMU預積分協(xié)方差遞推得到點位中誤差，并據(jù)此設(shè)定閾值，對待定點判斷其真實動態(tài)性。最后，基于車載多源平臺所采的數(shù)據(jù)集對本文方法進行驗證：Car_01序列絕對軌跡誤差均方根降低了56.3%；Car_02序列絕對軌跡誤差均方根降低了71.5%；Car_03序列絕對軌跡誤差均方根降低了19.4%。通過利用3個不同場景的數(shù)據(jù)集對3種方法進行對比，驗證了本文方法的有效性和普適性。

[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] 王柯賽, 姚錫凡, 黃宇, 等. 動態(tài)環(huán)境下的視覺SLAM研究評述[J]. 機器人, 2021, 43(6): 18.

[6] 高興波, 史旭華, 葛群峰, 等. 面向動態(tài)物體場景的視覺SLAM綜述[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十四講: 從理論到實踐[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

張樂添，趙冬青，賈曉雪，等. 面向動態(tài)環(huán)境的雙目/慣性SLAM系統(tǒng)[J]. 導航定位學報, 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

國際自然科學基金項目（41774037，42104033）。

張樂添（1998—），男，河南焦作人，碩士研究生，研究方向為視覺/慣性SLAM。

趙冬青（1976—），男，湖北十堰人，博士，教授，研究方向為導航定位與位置服務。