陽(yáng)霜，羅 堪，韋曉慧，周志偉，彭國(guó)超

(1.湖南機(jī)電職業(yè)技術(shù)學(xué)院，湖南長(zhǎng)沙 410151；2.長(zhǎng)沙師范學(xué)院，湖南長(zhǎng)沙 410100)

0 引言

隨著機(jī)器人技術(shù)的飛速發(fā)展，如何實(shí)現(xiàn)機(jī)器人在未知環(huán)境下的定位與導(dǎo)航，并自主地完成特定任務(wù)，是一個(gè)火熱的研究方向。在深海、隧道、室內(nèi)等應(yīng)用場(chǎng)景，GPS等全球衛(wèi)星定位系統(tǒng)的信號(hào)存在高衰減現(xiàn)象，機(jī)器人面臨“在哪里”“去哪里”“怎么去”等問題。為了實(shí)現(xiàn)這些功能，讓移動(dòng)機(jī)器人能夠同步定位與建圖(SLAM)，需要讓其能夠明確自身的位置和位姿，感知周圍環(huán)境的信息并創(chuàng)建地圖。這一技術(shù)的發(fā)展，對(duì)于實(shí)現(xiàn)機(jī)器人的智能定位與導(dǎo)航具有非常重要的意義。

目前，SLAM技術(shù)主要分為兩類：基于激光雷達(dá)的SLAM[1]和基于視覺的SLAM[2]，這兩種技術(shù)在機(jī)器人定位和地圖構(gòu)建方面都有廣泛的應(yīng)用。其中，激光SLAM 是一種較為成熟的方法，已成功應(yīng)用于多種商業(yè)產(chǎn)品。然而，激光SLAM 構(gòu)建的地圖信息過于簡(jiǎn)單，無(wú)法讓機(jī)器人獲取更豐富的環(huán)境信息[3]。視覺SLAM 是近十年來(lái)發(fā)展迅速的SLAM 解決方案，可以使用攝像頭重建3D 地圖。此外，圖像中包含豐富的物體信息，可以幫助機(jī)器人完成各種基于視覺的智能任務(wù)[4]。

Cesar Cadena 等人將視覺SLAM 的發(fā)展，劃分為三個(gè)代表性的時(shí)代[5]，第一個(gè)時(shí)代“經(jīng)典時(shí)代”，主要解決SLAM 問題，憑借提出的數(shù)學(xué)公式，SLAM問題首次得到了有效應(yīng)用。第二個(gè)時(shí)代“算法改進(jìn)時(shí)代”，引入GPU、RGB-D、立體相機(jī)等新硬件，用于改進(jìn)視覺SLAM 算法。第三個(gè)時(shí)代“提高魯棒性時(shí)代”，主要是針對(duì)特定場(chǎng)景，提高系統(tǒng)魯棒性。比較有代表的是基于稀疏直接法的視覺里程計(jì)(DSO, Direct Sparse Odometry)[5]，ORB-SLAM[7]等算法，這一時(shí)期SLAM 問題最大的挑戰(zhàn)，是在控制計(jì)算量大小的情況下，構(gòu)建一致收斂的軌跡和地圖，保證移動(dòng)機(jī)器人定位精度的同時(shí)，提高算法的運(yùn)行效率。2017年Mur-Artal R等，在ORB-SLAM 算法基礎(chǔ)上，開源了ORB-SLAM2[8]方案，新增了對(duì)雙目相機(jī)、RGB-D 相機(jī)的源生支持，是目前市場(chǎng)上主流的具有實(shí)時(shí)性好、地圖重用和動(dòng)態(tài)管理關(guān)鍵幀等優(yōu)點(diǎn)的完整視覺SLAM 系統(tǒng)。D3VO[9]是一種基于深度學(xué)習(xí)的視覺里程計(jì)框架，由Stumberg等人在2020年提出。該框架將深度學(xué)習(xí)的深度估計(jì)、位姿估計(jì)、不確定度估計(jì)應(yīng)用于視覺里程計(jì)方法中，以優(yōu)化前端追蹤效果，并提高后端非線性優(yōu)化的性能[10]，但目前的深度學(xué)習(xí)解決方案仍處于探索階段，對(duì)數(shù)據(jù)和計(jì)算機(jī)設(shè)備要求較高，且現(xiàn)有的模型還不夠成熟，運(yùn)行效果有待提高。

綜上所述，ORB-SLAM2 是目前視覺SLAM 中，最前沿的同步定位與建圖算法之一，本文選擇ORBSLAM2 算法，對(duì)機(jī)器人進(jìn)行同步定位與建圖，提出自適應(yīng)降低閾值的方法提取特征點(diǎn)，當(dāng)通過高響應(yīng)閾值搜索，未搜索到特征點(diǎn)時(shí)，自適應(yīng)逐級(jí)降低閾值搜索特征點(diǎn)，提高所提取特征點(diǎn)的響應(yīng)值，從而提高移動(dòng)機(jī)器人在未知環(huán)境下，進(jìn)行同步定位與建圖的精度。

1 ORB-SLAM2簡(jiǎn)介

ORB-SLAM2由跟蹤、局部建圖、閉環(huán)優(yōu)化檢測(cè)三大線程組成，如圖1所示。

圖1 ORB-SLAM2框架圖

在跟蹤線程中，首先從圖像中提取ORB 特征點(diǎn)，并運(yùn)用四叉樹法對(duì)這些特征點(diǎn)進(jìn)行篩選。然后，根據(jù)上一幀的位姿信息，進(jìn)行位姿估計(jì)或進(jìn)行重定位以初始化位姿。接下來(lái)，跟蹤線程會(huì)跟蹤重建局部地圖，并進(jìn)一步優(yōu)化位姿以確定新的關(guān)鍵幀。

在局部建圖線程中，機(jī)器人會(huì)將跟蹤線程生成的新的關(guān)鍵幀插入，并驗(yàn)證最新獲取的地圖點(diǎn)，進(jìn)而構(gòu)建新的地圖點(diǎn)。這些地圖點(diǎn)通過局部捆集調(diào)整(Local BA)進(jìn)一步優(yōu)化，最后機(jī)器人會(huì)對(duì)插入的關(guān)鍵幀進(jìn)行篩選，刪除冗余關(guān)鍵幀，實(shí)現(xiàn)局部地圖構(gòu)建。

為了減小之前步驟所積累產(chǎn)生的誤差，閉環(huán)優(yōu)化線程的主要任務(wù)是通過詞袋庫(kù)判別閉環(huán)幀。然后，利用Sim3算法計(jì)算相似變換，對(duì)地圖進(jìn)行矯正及優(yōu)化，從而減小之前步驟中，所積累產(chǎn)生的誤差。

系統(tǒng)的地圖由4個(gè)部分組成，即地圖點(diǎn)、關(guān)鍵幀、一致圖和生成樹。其中，一致圖是由節(jié)點(diǎn)和邊構(gòu)成的數(shù)據(jù)結(jié)構(gòu)，節(jié)點(diǎn)指的是關(guān)鍵幀，邊是關(guān)鍵幀之間的聯(lián)系。生成樹則是按照地圖點(diǎn)和關(guān)鍵幀之間的關(guān)系，以樹狀結(jié)構(gòu)進(jìn)行存儲(chǔ)，用于查找相鄰的關(guān)鍵幀。

位置識(shí)別模塊包括視覺詞袋和識(shí)別數(shù)據(jù)庫(kù)兩部分。視覺詞袋利用ORB特征信息對(duì)特征點(diǎn)進(jìn)行分類，實(shí)現(xiàn)位置的快速匹配。識(shí)別數(shù)據(jù)庫(kù)是利用關(guān)鍵幀所匹配的特征點(diǎn)，與視覺詞袋進(jìn)行關(guān)聯(lián)，將相似度得分最高的，形成關(guān)鍵幀數(shù)據(jù)庫(kù)，運(yùn)用到重定位及回環(huán)檢測(cè)。

2 ORB特征點(diǎn)提取優(yōu)化

原算法在跟蹤線程中提取ORB特征點(diǎn)時(shí)，爭(zhēng)取特征點(diǎn)在圖像的所有部分分布均勻，將圖像分為30*30大小的CELL 區(qū)域，然后對(duì)每個(gè)CELL 搜索特征點(diǎn)，優(yōu)先使用高響應(yīng)閾值搜索特征點(diǎn)，若使用高響應(yīng)閾值沒找到特征點(diǎn)，則將搜索閾值調(diào)整為低響應(yīng)閾值，重新對(duì)該CELL 進(jìn)行搜索，直至將所有CELL 搜索完畢后，特征點(diǎn)提取工作結(jié)束。

由于原算法，在ORB 特征點(diǎn)提取環(huán)節(jié)，若通過高響應(yīng)閾值沒有搜索到特征點(diǎn)，直接采用低響應(yīng)閾值進(jìn)行搜索，由于高響應(yīng)閾值與低響應(yīng)閾值之間的跨度較大，因此在搜索特征點(diǎn)時(shí)，提取了較多低響應(yīng)值特征點(diǎn)，將增大后續(xù)特征點(diǎn)篩選及匹配誤差，從而影響機(jī)器人在進(jìn)行同步定位與建圖的精度。

本文對(duì)提取ORB特征點(diǎn)算法進(jìn)行改進(jìn)，若通過高響應(yīng)閾值搜索，未搜索到特征點(diǎn)，則將搜索響應(yīng)值在原搜索響應(yīng)值的基礎(chǔ)上，降低t來(lái)搜索特征點(diǎn)。如果此時(shí)搜索到特征點(diǎn)，則記錄特征點(diǎn)后，來(lái)到下一CELL進(jìn)行特征點(diǎn)搜索；若未搜索到特征點(diǎn)，且此時(shí)搜索響應(yīng)值大于低響應(yīng)閾值，則在此搜索響應(yīng)值的基礎(chǔ)上，再次降低t來(lái)搜索特征點(diǎn)。由此，通過將響應(yīng)值逐次降低t來(lái)搜索特征點(diǎn)，直至遍歷完所有的CELL，如圖2所示。通過對(duì)響應(yīng)值進(jìn)行自適應(yīng)調(diào)整，在特征點(diǎn)提取時(shí)，將有效減少低響應(yīng)值的特征點(diǎn)，在降低后續(xù)對(duì)特征點(diǎn)進(jìn)行篩選與匹配的誤差的同時(shí)，還可以減少特征點(diǎn)匹配的工作量，提高系統(tǒng)運(yùn)行效率。

圖2 特征點(diǎn)提取優(yōu)化流程圖

3 實(shí)驗(yàn)與分析

3.1 數(shù)據(jù)準(zhǔn)備

EuRoC MAV 數(shù)據(jù)集是微型飛行器(MAV)上收集的視覺慣性數(shù)據(jù)集。數(shù)據(jù)集包含立體圖像、同步IMU測(cè)量以及準(zhǔn)確的運(yùn)動(dòng)和結(jié)構(gòu)地面實(shí)況。本文采用Eu-RoC MAV 數(shù)據(jù)集中的01、03、05 號(hào)場(chǎng)景，各數(shù)據(jù)集特點(diǎn)如表1所示。通過對(duì)三個(gè)數(shù)據(jù)集進(jìn)行比較，得知廠房01的數(shù)據(jù)拍攝條件最好，飛行器運(yùn)行速度低，拍攝場(chǎng)景紋理及光照均較好，廠房03 的數(shù)據(jù)較廠房01 而言，運(yùn)行速度有所加快，而廠房05的數(shù)據(jù)最難，不僅運(yùn)行速度快，其場(chǎng)景的紋理及光照條件均較差。因此，筆者利用這三個(gè)數(shù)據(jù)集，來(lái)運(yùn)行原始ORB-SLAM2 算法及改進(jìn)后的ORB-SLAM2 算法，有助于分析算法對(duì)高速運(yùn)行、弱紋理、光照較差的不良條件的魯棒性。

表1 數(shù)據(jù)集介紹

3.2 評(píng)價(jià)標(biāo)準(zhǔn)

針對(duì)改進(jìn)算法和原始算法，分別計(jì)算相對(duì)定位誤差(RPE，Relative Positioning Error) 和絕對(duì)定位誤差(APE，Absolute Positioning Error) 的 均 方 根 誤 差(RMSE)、標(biāo)準(zhǔn)差(σ)。

APE是估計(jì)位置(2D 或3D)與真實(shí)位置之間的歐幾里得距離，用于檢驗(yàn)算法的全局準(zhǔn)確性和穩(wěn)定性。其RMSE計(jì)算如式(1)，RMSE越小，估計(jì)軌跡越接近真實(shí)軌跡，標(biāo)準(zhǔn)差計(jì)算如式(2):

式中，n表示關(guān)鍵幀數(shù)目，i表示第i個(gè)關(guān)鍵幀，xobs.i表示第i個(gè)關(guān)鍵幀姿態(tài)真實(shí)值，xmodel.i為第i個(gè)關(guān)鍵幀姿態(tài)的估計(jì)值。

式中，n表示關(guān)鍵幀數(shù)目，i表示第i個(gè)關(guān)鍵幀，xmodel.i表示第i個(gè)關(guān)鍵幀姿態(tài)真實(shí)值，μ為xmodel.i為的平均值。

RPE是連續(xù)位置估計(jì)(2D 或3D)之間的歐幾里得距離，用于計(jì)算漂移，即每米的平移或旋轉(zhuǎn)誤差。第i幀的RPE定義如下：

式中，Pi表示第i幀的估計(jì)位姿，Qi表示第i幀的真實(shí)位姿，Δt表示間隔時(shí)間。

式中，n表示觀測(cè)次數(shù)，Δt表示時(shí)間間隔t，m=n- Δt×RPE，Ei表 示i時(shí) 刻 相 對(duì) 位 姿 估 計(jì) 誤 差，trans(Ei)代表相對(duì)位姿誤差中的平移部分。

相對(duì)定位誤差、絕對(duì)定位誤差的具體計(jì)算，可運(yùn)用開源的EVO Python計(jì)算包進(jìn)行計(jì)算。

最后運(yùn)用式(5)計(jì)算估計(jì)軌跡與真實(shí)軌跡長(zhǎng)度的相對(duì)誤差，用于評(píng)價(jià)算法對(duì)軌跡尺度估計(jì)的能力。

3.3 誤差分析

運(yùn)用原算法及改進(jìn)算法，對(duì)廠房01、03、05號(hào)數(shù)據(jù)集進(jìn)行同步定位與建圖仿真，其絕對(duì)定位誤差及相對(duì)定位誤差比較，分別如表2、表3所示。

表2 不同數(shù)據(jù)集下APE對(duì)比表

表3 不同數(shù)據(jù)集下RPE對(duì)比表

從表2、表3 可知，改進(jìn)算法的相對(duì)定位誤差的RMSE與σ值，略小于原算法，而改進(jìn)算法的絕對(duì)定位誤差的RMSE與σ值，遠(yuǎn)優(yōu)于原算法，三個(gè)場(chǎng)景中，其絕對(duì)定位誤差的RMSE的平均值下降了15.87%。由此可知，改進(jìn)算法在進(jìn)行機(jī)器人同步定位與建圖時(shí)，其漂移誤差略微減小，而其全局準(zhǔn)確性和穩(wěn)定性得到大幅提升。

對(duì)改進(jìn)算法在廠房01、03、05，三個(gè)不同場(chǎng)景下的APE、RPE進(jìn)行比較，其中廠房01 和廠房03 的APE、RPE相差較小，而廠房05 的APE、RPE較另兩種相差略大，說明改進(jìn)算法對(duì)高速運(yùn)動(dòng)的魯棒性較好，而對(duì)于弱紋理的魯棒性有待提高。

運(yùn)用ORB-SLAM2改進(jìn)算法，對(duì)廠房01、03、05數(shù)據(jù)集進(jìn)行同步定位與建圖仿真，其軌跡仿真如圖3所示，圖中X、Y、Z 為三維空間坐標(biāo)軸，圖例data 為真實(shí)軌跡，圖例KeyFrameTrajectory 為估計(jì)軌跡。通過對(duì)圖3進(jìn)行觀察，可分析估計(jì)軌跡與真實(shí)軌跡的擬合程度，其中廠房01與03的軌跡仿真圖中，估計(jì)軌跡與真實(shí)軌跡無(wú)較大軌跡錯(cuò)位，幾乎重合，而廠房05的估計(jì)軌跡與真實(shí)軌跡在某一區(qū)域內(nèi)，存在一定偏差。再次驗(yàn)證了，改進(jìn)算法對(duì)高速運(yùn)動(dòng)的魯棒性較好，而對(duì)于弱紋理的魯棒性有待提高。

圖3 軌跡仿真圖

ORB-SLAM2 改進(jìn)算法對(duì)廠房01、03、05 數(shù)據(jù)集仿真，其真實(shí)軌跡與估計(jì)軌跡長(zhǎng)度，及軌跡長(zhǎng)度誤差計(jì)算，如表4所示。從表中數(shù)據(jù)分析可知，估計(jì)軌跡長(zhǎng)度均與真實(shí)軌跡長(zhǎng)度相近，且誤差較小。結(jié)合軌跡仿真圖及軌跡長(zhǎng)度比較表，總體來(lái)說，對(duì)ORB-SLAM2算法進(jìn)行ORB特征點(diǎn)提取優(yōu)化后，能夠較好地實(shí)現(xiàn)機(jī)器人同步定位與建圖。

表4 軌跡長(zhǎng)度比較表

4 結(jié)論與展望

通過上述實(shí)驗(yàn)分析，在提取ORB 特征點(diǎn)時(shí)，通過對(duì)響應(yīng)值進(jìn)行自適應(yīng)調(diào)整，對(duì)ORB-SLAM2 算法進(jìn)行改進(jìn)，能夠較好地應(yīng)用于未知環(huán)境下，機(jī)器人同步定位與建圖。通過實(shí)驗(yàn)數(shù)據(jù)對(duì)比分析可知，改進(jìn)算法的相對(duì)定位誤差的RMSE與σ值，略小于原算法，而改進(jìn)算法的絕對(duì)定位誤差的RMSE與σ值，遠(yuǎn)優(yōu)于原算法，三個(gè)場(chǎng)景中，其APE的RMSE的平均值下降了15.87%。由此可知，通過對(duì)ORB-SLAM2算法進(jìn)行改進(jìn)，在進(jìn)行同步定位與建圖時(shí)，每米的平移或旋轉(zhuǎn)誤差略微有所下降，準(zhǔn)確性及穩(wěn)定性得到較大提高，估計(jì)軌跡更接近于真實(shí)軌跡。因此，運(yùn)用改進(jìn)算法使機(jī)器人在未知環(huán)境下，進(jìn)行同步定位與建圖的精度得到提高。

通過對(duì)廠房01 和廠房03 的絕對(duì)定位誤差、相對(duì)定位誤差、軌跡仿真圖進(jìn)行對(duì)比，得知改進(jìn)算法，對(duì)于高速運(yùn)動(dòng)的魯棒性較好，但對(duì)廠房05與廠房01、03場(chǎng)景的絕對(duì)定位誤差、相對(duì)定位誤差、軌跡仿真圖進(jìn)行對(duì)比，得知后續(xù)對(duì)于環(huán)境光照變化、低紋理環(huán)境等問題，如何提升系統(tǒng)魯棒性，提高智能移動(dòng)機(jī)器人定位與導(dǎo)航精度，仍有待加強(qiáng)研究。

另外，視覺SLAM 主要用于機(jī)器人或無(wú)人機(jī)等嵌入式平臺(tái)的場(chǎng)景識(shí)別、深度學(xué)習(xí)、3D重建等應(yīng)用。如何在提高機(jī)器人或無(wú)人機(jī)進(jìn)行實(shí)時(shí)定位與導(dǎo)航精度的同時(shí)，加快運(yùn)行速度，還有待進(jìn)一步探索。