基于改進ORB_SLAM2的機器人視覺導航方法

董蕊芳 王宇鵬 闞江明

(1.北京林業大學工學院， 北京 100083； 2.林業裝備與自動化國家林業和草原局重點實驗室， 北京 100083)

0 引言

即時定位與地圖構建(Simultaneous localization and mapping，SLAM)是機器人進行自主導航的先決條件，能實現在無全球導航衛星系統(Global navigation satellite system，GNSS)信號的情況下定位[1]。視覺SLAM系統主要包括傳感器信息獲取模塊、前端視覺里程計模塊、后端優化模塊和地圖創建模塊[2-5]。視覺SLAM算法根據對圖像處理方法的不同分為基于特征的方法和直接法[6-8]。

基于特征的視覺SLAM指對輸入的圖像進行特征檢測和提取，利用算法或者傳感器獲得特征點的深度，通過特征匹配構建幀與幀之間的共視關系，計算相機的位姿同時對3D特征進行優化建立地圖。ORB_SLAM2[9]是第一個同時支持單目、雙目、RGB-D相機的視覺SLAM系統，該系統包含跟蹤線程、局部地圖構建線程以及回環檢測線程，但其生成的稀疏點云信息不充分、復用性差而無法直接用于后續導航。文獻[10]提出一種基于ORB_SLAM2實時構建網格地圖的方法，根據稀疏點云直接生成2D占據網格地圖。文獻[11]提出一種用于機器人RGB-D視覺SLAM的算法，提取ORB特征，改進RANSAC(Random sample consensus)算法，與GICP(Generalized-iterative closest point)結合并加入隨機閉環檢測，構建場景環境的三維點云地圖。文獻[12]提出一種基于ORB_SLAM2的跳表地圖構建算法，可實時構建3D網格地圖。文獻[13]對ORB_SLAM2進行拓展，開發地圖保存功能。文獻[14]提出一種針對機器人的導航地圖制備方法，根據機器人在地面的運動約束通過點云地圖可以推導出2D占據網格地圖。

目前來看，已有眾多研究針對機器人的自主導航，在ORB_SLAM2的基礎上提升了建圖性能或定位精度，但仍然沒有一套完整的策略實現機器人自主導航。因此，本文提出一種基于改進ORB_SLAM2的視覺導航方法，以ORB_SLAM2的RGB-D模式為基礎，融合環境的3D、2D占據特征以及路標點空間位置、視覺特征等多模態信息，構建包含定位、規劃、交互圖層的多圖層地圖以支撐機器人的精準定位和最優路徑規劃，并在多圖層地圖的基礎上實現定位和導航。

本文提出的方法將實現多圖層地圖構建、基于先驗地圖的視覺定位、規劃導航3個功能。以ORB_SLAM2關鍵幀為基礎，構建多圖層地圖；以多圖層地圖作為先驗信息，進行機器人實時定位；設定目標點后進行路徑規劃，最終實現機器人自主導航。

1 多圖層地圖構建

對ORB_SLAM2的RGB-D模式進行改進，根據其關鍵幀機制構建多圖層地圖。首先通過關鍵幀深度圖計算局部稠密點云，通過分裂局部點云計算環境3D占據信息并更新交互圖層；之后，對環境3D占據信息進行二維映射得到2D占據信息并以此來更新規劃圖層；與此同時，對關鍵幀中環境的路標點以及視覺特征建立對應關系，更新定位圖層。將交互圖層、規劃圖層以及定位圖層進行坐標對齊，構建成融合了環境的3D、2D占據信息以及路標點和視覺特征的多圖層地圖，整體框架如圖1所示。

圖1 多圖層地圖框架Fig.1 Framework of multi-layer map

1.1 交互圖層構建

交互圖層負責實現導航過程中的人機交互，應具備豐富的外觀信息，在此構建八叉樹地圖[15]作為交互圖層。構建過程包括局部稠密點云計算、點云配準以及環境3D占據信息更新3部分。

1.1.1局部稠密點云計算

以ORB_SLAM2關鍵幀的深度圖作為輸入，通過RGB-D相機模型計算局部稠密點云，構建點云條件濾波器以剔除離群點。離群點剔除的方法如下：

取局部稠密點云其中一點P，取其最相近的N個點構成一個小樣本，設P(x，y，z)，在樣本中另取一點Pi(xi，yi，zi)，其中i=0，1，…，N。計算P到Pi的距離ti為

(1)

平均距離μ計算公式為

(2)

(3)

圖2 離群點剔除效果Fig.2 Outliers removed effect

圖3 局部稠密點云的小樣本距離均值Fig.3 Small sample distance mean of local dense point cloud

1.1.2點云配準

采用FAST ICP[16](基于改進迭代最近點快速點云配準算法)進行配準，具體流程如下：假設圖2中紅色點云A為待配準點云，綠色點云B為目標點云，首先對A和B使用體素濾波器以降低分辨率，再對A和B分別用ISS[17](Intrinsic shape signatures)算法提取幾何特征并計算內部形狀描述子，用RANSAC算法迭代剔除外點。設G左上標為世界坐標系，C左上標為相機坐標系，在觀測點云A時刻的相機在世界坐標系下的位置為GPA、姿態為GRA，在觀測點云B時刻的相機在世界坐標系下的位置為GPB、姿態為GRB，對A中的任意一點CPai進行坐標變換，得到GPai，即

GPaiT=GRACPaiT+GPAT

(4)

之后，在B中尋找與GPai距離最近的點GPbi，此時認為GPai和GPbi是一組匹配點。

根據上述方法，假設在A和B中有n對匹配點，定義A到B的變換矩陣為

(5)

構造目標函數求解最優解

(6)

式中e——匹配點位置誤差

Pai——A中點云的坐標

Pbi——B中與A對應點云的坐標

根據迭代次數以及迭代誤差是否收斂(以前后兩次迭代的轉換矩陣的最大容差判斷)等情況判斷迭代是否結束，本文設定的迭代次數為20次，收斂范圍為0.000 1，配準結果如圖4(原始點云為紅色，目標點云為綠色，配準后點云為藍色)所示。

圖4 點云配準結果Fig.4 Result of point cloud registration

以圖4為例，配準前的ΔT為

(7)

配準后ΔT*為

(8)

1.1.3環境3D占據信息更新

完成點云配準后，對點云構建包圍盒并進行空間分裂。首先根據點云尺寸計算根節點包圍盒尺寸，再根據設定分裂步長將根節點均勻劃分為8等份，再依次判斷是否到達遞歸深度、新的子節點中是否包含點云、子節點包含點云數量是否與父節點包含數量相同。點云空間分裂過程如圖5所示。

圖5 點云空間分裂流程Fig.5 Point cloud space segmentation process

根據點云空間分裂結果進行環境3D占據信息更新過程如下：假設S為空間分裂后的一個正方體，其占據概率為ps∈[0,1]，依據其概率對數值ls更新S的占據狀態ps，S的概率對數值ls為

(9)

則有

(10)

當觀測到S的次數增加時，ls增加，隨著ls增大，ps將趨近于1，表示S被占據的概率增大；反之，當觀測到S的次數減少時，ls減小，隨著ls減小ps將趨近于0，表示S被占據的概率減小。

根據上述步驟，可通過配準后的點云信息計算得到環境的3D占據信息，之后將其融合至多圖層地圖中，完成交互圖層構建。

1.2 規劃圖層構建

規劃圖層用于機器人的路徑規劃，而移動機器人的運動限制在2D空間，因此本文以2D占據網格地圖作為規劃圖層。在此對交互圖層進行2D投影，計算2D占據信息，整體流程如下：

首先提取在交互圖層中移動機器人運動空間部分的3D占據信息，設機器人高為h，提取高度位于[0，h]內的交互圖層3D占據信息，如圖6所示。

圖6 移動機器人運動空間占據信息提取Fig.6 Motion space occupancy information extraction of mobile robot

之后進行2D占據信息更新，過程如圖7所示。以機器人運動空間的占據信息作為集合C。定義以x-o-y為平面，在豎直方向的每一列正方體方格為集合C的子集Ci，記作

C={Ci，i=1，2，…，N}

(11)

令左上標G2表示2D空間，左上標G3表示3D空間，則Ci表示為

Ci={G3P=(x，y，z)，z=0，1，…，h}

(12)

定義Ci中具有最大占據概率的點為G3Pp=max，Ci的2D映射點的坐標為G2P=(x，y)，此時G2P的占據概率p(G2P)計算更新為

p(G2P)=p(G3Pp=max)

(13)

通過上述方法可對交互圖層中的3D占據信息進行提取以及二維映射，得到環境的2D占據信息并將其融合到多圖層地圖中，最終完成規劃圖層的構建。

圖7 環境2D占據信息更新Fig.7 Environment 2D occupancy information update

1.3 定位圖層構建

為了實現在已知地圖中快速定位，本文融合環境的路標點信息以及圖像的特征信息構建定位圖層，使其能夠為移動機器人快速定位提供基礎。定位圖層構建過程如圖8所示。

圖8 定位圖層構建Fig.8 Localization layer construction

首先，提取關鍵幀圖像2D特征以及3D路標特征，并將兩者按照對應關系建立哈希表

Hhastable(FK)={[3D(Pi)--2D(Pi)]}

(14)

式中，“--”表示一一對應。之后，將每一個Hhastable(FK)存入一個向量列表中，假設共有n個關鍵幀，最終可得到定位圖層

L={Hhastable(FK)K，K=0，1，…，n}

(15)

通過上述步驟，利用關鍵幀的機制，將環境的路標點信息和圖像的特征信息按照對應關系進行融合，最終實現定位圖層的構建。

2 基于先驗地圖的視覺定位

本文提出的基于先驗地圖的機器人視覺定位方法以定位圖層為基礎，整體框架如圖9所示。定位方法包括4個主要部分：視覺字典構建、參考關鍵幀搜索、基于地圖約束的機器人位姿估計、基于運動約束的機器人位姿優化。

圖9 基于先驗地圖的視覺定位算法框架Fig.9 Framework of visual localization algorithm based on prior map

2.1 視覺字典構建

視覺字典構建類似于一個聚類問題，能夠使機器人尋找數據中的規律。本文采用的方法是構建k叉樹結構視覺字典，以保證對數級別的查找效率，下文中以k表示視覺字典的分支數，d表示視覺字典的深度。

(1)假設樣本有n個特征點，k個類簇中心。

(2)根據樣本中的特征計算得到n個描述子向量,計算樣本中的每一個特征的描述向量到每個聚類中心的歐氏距離

(16)

式中vi——特征i的描述向量

cj——第j個聚類中心向量

將Di進行排序，取距離最短的作為它的歸類。

(3)計算所有類簇描述符的平均值作為新類簇中心。

(4)計算更新的類簇中心和之前的類簇中心變化值，如果每個類簇的變化值都很小，則迭代結束，否則循環步驟(2)、(3)。

視覺字典分支數k和視覺字典深度d這兩個參數對視覺字典的構建耗時、大小以及查找效率起決定性作用，因此在應用過程中需根據實際情況去選擇最優參數。

2.2 參考關鍵幀搜索

基于定位圖層的參考關鍵幀搜索主要作用是快速索引出與當前圖像具有最好共視關系的關鍵幀。首先，搜索參考關鍵幀需要計算當前圖像的BOW[18-19](Bag-of-words)，檢索BOW通常采用TF-IDF[20-21](Term frequency-inverse document frequency)算法。此時假設在定位圖層中有N個關鍵幀，每一個關鍵幀都可以用向量VN表示，計算當前圖像中的特征并以向量表示為VI，計算當前圖像與定位圖層中每一個關鍵幀的相似度s，找到最相似的圖像作為參考關鍵幀，公式為

s(VI-VN)=2∑(|VI|+|VN|-|VI-VN|)

(17)

2.3 基于地圖約束的機器人位姿估計

基于所檢索到的參考關鍵幀，將當前幀和參考關鍵幀建立共視關系得到地圖約束，進行機器人的位姿初始估計。首先，通過計算圖像和參考關鍵幀描述向量的漢明距離進行匹配，匹配結果如圖10所示。

圖10 圖像描述子匹配Fig.10 Matches of image descriptors

根據圖10可知，當環境特征相似度較高時，誤匹配現象嚴重，因此進行誤匹配點對的剔除。采用RANSAC剔除誤匹配，實現方法如下：假設在圖像I1中共有m個特征點，在圖像I2中有n個特征點，根據描述子匹配計算得出匹配點有k對，在k對匹配點中隨機抽取4個不共線的樣本計算出單應性矩陣H，公式為

(18)

式中λ——尺度因子

(ui,vi)——I1中第i個匹配的特征點

(u′i,v′i)——I2中第i個匹配的特征點

定義代價函數為

(19)

重新隨機抽取4個不共線的樣本計算矩陣H，迭代計算代價函數cM直至到達全局最小，此時得到單應矩陣H的最優解，根據H進行投影剔除誤匹配。剔除誤匹配后結果如圖11所示。

圖11 通過RANSAC算法剔除誤匹配結果Fig.11 Wrong matches eliminated by RANSAC algorithm

建立當前圖像特征和參考關鍵幀的2D特征對應關系后，根據定位圖層中Hhastable(FKn)索引對應3D點，可得當前幀的2D特征與定位圖層中的3D特征匹配關系，如圖12(圖中R表示相機的平移量，t表示時間)所示。用PnP[22](Perspective-n-point)求解當前幀位姿，即可根據地圖約束估計機器人位姿。

圖12 PnP算法求解相機在先驗地圖中位姿Fig.12 Camera poses estimated by PnP algorithm

2.4 基于運動約束的機器人位姿優化

基于地圖約束的機器人位姿估計結果存在一定誤差，說明相機的運動約束不可忽略，因此本文在上述位姿估計結果的基礎上，基于運動約束進行機器人的位姿優化，以提升定位精度。

假設在k幀，已利用PnP求解出當前相機的初始位姿xk，在此聯系第k-1幀建立相機的運動約束。首先尋找第k幀和第k-1幀的共視3D地圖特征點，設為集合Pk,k-1，然后計算Pk,k-1在第k幀的重投影誤差建立運動約束，其誤差目標函數

e(xk,xk-1,Pk,k-1)=∑‖h(xk,Pk,k-1)-zk‖2

(20)

式中e——Pk,k-1的重投影誤差

h——3D特征點Pk,k-1到xk的投影函數

zk——Pk,k-1在第k幀的觀測量，即像素坐標

通過高斯牛頓法建立誤差目標函數的正規方程

JTJΔx=-JTe(xk,xk-1,Pk,k-1)

(21)

式中 Δx——迭代步長

J——誤差雅可比矩陣

以PnP估計的相機位姿作為初始值進行迭代，對于第N次迭代，計算系統當前雅可比矩陣為JN和誤差e(xk,xk-1,Pk,k-1)N，求解正規方程式(21)，若在第N次時Δx足夠小，則停止，否則繼續迭代，直到解出最優解。

3 基于多圖層地圖的機器人自主導航

本文提出的基于多圖層地圖的導航策略如圖13所示，具體包括：

(1)機器人進行環境探索，構建多圖層地圖。

(2)基于先驗地圖進行初始定位，計算機器人在先驗地圖中的初始位姿。

(3)設定目標點，基于規劃圖層利用A*算法規劃最優路徑。

(4)根據實時定位結果，采用DWA(Dynamic window approach)算法計算軌跡和運動策略[23]，最終實現機器人自主導航。

圖13 機器人自主導航框架Fig.13 Framework of autonomous navigation

3.1 基于規劃圖層的A*算法路徑規劃

機器人獲取目標信息后，需要根據建立的規劃圖層判斷可行域，再通過路徑規劃算法生成一條可通行全局安全的軌跡。A*算法是最常用的基于圖搜索的路徑規劃算法，在二維柵格地圖實現A*算法需首先構建兩個二維向量組：openlist和closelist，假設機器人當前定位點為A，目標點為B，根據圖搜索的思想，把柵格地圖地中每一個方格(分辨率0.05 m)當作成一個節點，A*算法搜索步驟如下：

(1)定義機器人水平移動代價值為10，斜向移動的代價值為14，代價函數f為

f(n)=g(n)+h(n)

(22)

式中g——起始點至n的代價

h——n至目標點的代價

以圖14中點C為例，g(C)=24，h(C)= 44，f(C)=68。

圖14 移動機器人在規劃圖層中移動的代價值Fig.14 Cost value of mobile robot moving in grid map

(2)將節點A放入closelist中，在節點A周圍的方格中搜索可通行的區域，并計算每個方格的代價值f(n)，A的所有子節點放入openlist中。

(3)在openlist中取出f(n)最小的節點并將其子節點加入openlist，忽略障礙物和已在closelist中的節點。

(4)遍歷上述步驟，如果節點B被加入openlist中則表示搜索到最優路徑，當所有柵格都被訪問后節點B仍然沒有被加入openlist中，那么則表示沒有路徑可由節點A到達節點B。

(5)從目標點B回溯每個節點的父節點，最終得到最優路徑。

上述A*算法進行路徑搜索過程如圖15所示。

圖15 A*算法示意圖Fig.15 A* algorithm schematic

式(22)中的h(n)也被稱為A*算法的啟發式函數，有多種表示方式，如：曼哈頓、對角線、歐幾里得等。本文選用啟發式對角線函數，其定義如下：

假設機器人對角線移動的代價為hd(n)，定義為

(23)

式中n(x)、n(y)——可通行柵格n的x、y坐標

g(x)、g(y)——移動代價在x和y方向的分量

直線移動代價定義為

hs(n)=min(|n(x)-g(x)|,|n(y)-g(y)|)

(24)

此時的啟發式函數為

(25)

在規劃圖層中設置兩組相同的起點和終點，使用A*算法進行路徑規劃，結果見圖16。

圖16 A*算法在規劃圖層路徑規劃Fig.16 A* algorithm in planning layer path planning

3.2 基于DWA算法的軌跡規劃

機器人獲得目標點后，根據A*算法規劃出大致可行的路徑，然后需要在設定好的路徑計算運動策略。DWA算法可根據規劃的路徑實時計算出速度。機器人應用DWA算法進行軌跡規劃步驟如下：

(1)在速度空間(v,ω)中進行采樣，假設當前機器人位于(x,y)處航向角為θ，根據不同的采樣速度推算運動軌跡，則可推算出多條軌跡

(26)

式中 (x′,y′)——推演的位置

θ′——推演的航向角

ω——角速度v——線速度

Δt——采樣時間間隔

(2)根據式(26)對每一條推算的軌跡進行評分(G表示得分)

G(v,ω)=λ(αheading(v,ω)+βdist(v,ω)+γvel(v,ω))

(27)

其中，heading(v,ω)表示機器人根據DWA算法推演出的最終航向角與目標點航向角的偏差，dist(v,ω)表示機器人推演的最終位置和目標位置偏差，vel(v,ω)表示推演的停止速度與最終設定速度偏差。α、β、γ分別為航向角偏差權重、位置偏差權重以及速度偏差權重，λ為整體權重，保證機器人運動軌跡更佳平滑。為了使機器人能安全低速行駛，DWA設定參數如表1所示。

表1 DWA算法參數設置Tab.1 DWA algorithm parameters setting

根據工作環境的不同，DWA算法的朝向、位置以及速度權重選擇的占比也不相同，機器人在環境中工作需要能盡快安全到達指定的目標點，因此最終位置的權重應該最高。設λ=0.8，α=0.7，β=0.2，γ=0.1，在規劃圖層中測試DWA算法，給定目標點，根據A*算法生成一條全局軌跡(綠色軌跡)，再通過DWA算法進行航跡推演(黑色框為推演范圍)得到最優的運動策略(紅色軌跡)，如圖17所示。

圖17 DWA算法的生成軌跡Fig.17 Trajectory generated by DWA algorithm

4 實驗

本文包括兩組實驗：驗證實驗和實地實驗。第1組選用TUM數據集[24]，驗證本文的多圖層地圖構建方法所估計軌跡的精度和實時性，以及基于先驗多圖層地圖的視覺定位方法估計結果與未知環境下SLAM定位結果的一致性。

第2組實驗為北京鷲峰國家森林公園實地實驗，將本文的視覺導航方法應用于所搭建的輪式機器人。

4.1 TUM數據集實驗

TUM數據集為Kinect視覺傳感器采集的標準數據集，該數據集中包含彩色圖和深度圖以及相機運動過程中的Ground truth(軌跡真值)。本組實驗將通過TUM的fr1/360、fr1/floor、fr1/rpy序列驗證。

4.1.1多圖層地圖構建實驗

本文對ORB-SLAM2算法進行改進，構建多圖層地圖以支撐機器人的精準定位和最優路徑規劃。為驗證其所構建多圖層地圖的優越性，將其輸出的位姿估計結果與Ground truth和經典的RGB-D SLAM[25]結果進行比較，結果如表2所示，主要對比算法耗時和全局軌跡均方根誤差兩個指標。通過表2可知，在fr1/360序列，本文方法估計的軌跡精度比RGB-D SLAM提升8.91%，建圖效率提高49.38%；在fr1/floor序列，本文方法估計的軌跡精度比RGB-D SLAM提升12.57%、建圖效率提高43.95%。總體而言，本文方法的平均運行速度較RGB-D SLAM提升了50%，估計軌跡精度提升了10%。因此本文提出的方法比RGB-D SLAM建立地圖的效率更高，軌跡更加準確。

表2 TUM數據集位姿估計結果Tab.2 Results of pose estimating with TUM

4.1.2基于先驗地圖的視覺定位實驗

由于TUM數據集僅提供了單次機器人運行數據，因此，本文進行基于先驗地圖的視覺定位實驗，思路為：首先，使用TUM數據集進行SLAM，構建多圖層地圖并輸出機器人運行序列位姿；然后，在所構建地圖的基礎上，再次輸入相同的數據，模擬機器人第2次進入相同的環境，通過對比基于先驗地圖定位方法得到的序列位姿與SLAM過程估計的位姿之間偏差來驗證算法的性能，實驗結果見表3，在fr1/floor、fr1/rpy這兩個序列的定位結果與SLAM所估計軌跡的均方根誤差僅有0.01 m。這說明本文提出的基于先驗多圖層地圖的視覺定位方法能夠進行機器人的精確定位，助力機器人實現準確的自主導航。

表3 基于先驗地圖的視覺定位結果Tab.3 Results of visual localization with prior map m

4.2 北京鷲峰國家森林公園實驗

利用所搭建的輪式機器人在北京鷲峰國家森林公園進行實驗。

機器人為四輪差分機器人，搭載的視覺傳感器為D435i型深度相機。主控單元為工控機，配備CPU是第8代酷睿i5，256 GB固態硬盤，操作系統為Ubuntu 18.04，如圖18a所示。機器人高1.163 m，相機距地面高度為1.026 m，機器人尺寸參數如圖18b所示。

圖18 實驗平臺示意圖Fig.18 Schematic of experimental platform

4.2.1多圖層地圖構建實驗

首先，需要根據機器人高度確定多圖層地圖中各個圖層的坐標系位置。將交互圖層按照機器人的高度分割成3部分：地面部分，地面到機器人高度范圍部分，高于機器人部分。確定好提取范圍后，對每一個關鍵幀生成的交互圖層提取范圍(-1.026 m，0.110 m)內的包圍盒，以0.05 m分辨率創建規劃圖層。在多圖層地圖中，定位圖層和交互圖層坐標軸一致，兩者都以地圖初始化時建立的初始坐標系為全局坐標系，而規劃圖層的坐標系在全局坐標系正下方1.026 m處。如圖19所示，O1-x-y-z為世界坐標系，O2-x-y-z為規劃圖層坐標系。

圖19 多圖層地圖坐標系示意圖Fig.19 Schematic of coordinate system of multi-layer map

由于實驗環境中難以獲取相機軌跡真值，采用以下方式來檢驗建立地圖的精度：實驗場地為非規則圖形，因此實驗過程中設置了4個路標點，分別為A、B、C、D，機器人在實驗場地行走的路線為：由點A出發，按照A→B→C→D→A的路線行走，實時構建多圖層地圖，圖20為行走路線示意圖。

圖20 機器人行走路線Fig.20 Experimental design of robot walking route

構建的多層地圖如圖21所示，其中定位圖層為特征點地圖；交互圖層為八叉樹地圖；規劃圖層為2D占據網格地圖。

圖21 實驗場景多圖層地圖Fig.21 Muti-layer map of experimental site

為方便統計，對交互圖層中的樹木進行編號，如圖22所示，并分別用米尺測得實驗場地中AC、BD的長度lAC和lBD，再通過多圖層地圖測量AC、BD之間的長度進行對比。

圖22 交互圖層中樹木編號Fig.22 Number trees in interactive layer

實驗結果見表4，結果表明，lAC的測量值與真實值之間的誤差為6.7%，lBD的測量值與真實值之間的誤差為5.6%，機器經過的路徑周圍共30棵樹木，在地圖中編號樹木也為30棵。

表4 實驗結果Tab.4 Result of experimental site

在真實環境中采用RGB-D SLAM[25]算法實時構建地圖時發現，該算法隨著地圖的增加會出現信息滯后的情況，而本文方法能夠實時重建交互圖層、規劃圖層以及定位圖層，且地圖精度和完整度都能得到保障，能為機器人自主導航提供準確的先驗地圖。

4.2.2視覺字典構建實驗

自主導航要求機器人快速準確地實現自身定位，在線構建視覺字典的時間以及字典大小直接影響機器人定位的效率以及精度，因此，本文對比選擇k(視覺字典分支數)、d(視覺字典深度)的最優參數來提升計算效率及精度。

實驗中不同k、d構建字典的時間和生成字典的大小見表5、6。

表5 不同k、d構建視覺字典的時間Tab.5 Cost time of k and d for generating visual dictionary s

表6 不同k、d構建視覺字典的大小Tab.6 Memory size of k and d for generating visual dictionary kb

對比不同k、d構建的12組視覺字典進行圖像檢索，驗證識別效率。實驗中對每一個關鍵幀進行順序檢索，按照關鍵幀出現的順序進行驗證，圖23所示為檢索結果，在使用d=2、k=6和d=2、k=7兩種參數組合進行視覺字典構建時識別準確率分別為51.62%和99.16%，在d=2、k=8時識別準確率達100%。其余參數的視覺字典識別準確率均為100%。

在驗證參數k、d對計算效率的影響方面，分別記錄利用以上12組視覺字典進行圖像檢索的時間，每組求和再計算平均耗時，結果見表7。對比k、d對識別效率的影響，當d=2、k=9時，視覺字典的識別準確率為100%，且在具有同等識別準確率當中具有最高的識別效率。因此，本文選擇d=2、k=9作為最優參數進行計算。

圖23 不同k、d的字典識別結果Fig.23 Recognition results using dictionaries with different k and d

表7 k、d對識別耗時的影響

4.2.3基于先驗地圖的視覺定位實驗

進行基于先驗地圖的視覺定位實驗方式為：選擇建圖時的4個標記點A、B、C、D(在地圖中用紅色點表示)，使機器人分別行走4段路線：A→B、B→C、C→D、D→A(A默認為坐標原點)，即分別以A、B、C、D為起點進行實驗，并分別記錄下定位結果。

實驗結果如圖24所示。以圖24b為例，機器人在實際地圖中以B為起點，行走B→C段路線，但當機器人開始進入地圖時，系統默認其初始位置為地圖坐標原點(點A)，通過初始定位后，立刻確定自身位置在點B，并在后續運動過程中進行實時定位，最終結果表明機器人能夠實現準確的定位。

4.2.4基于多圖層地圖導航實驗

圖24 基于先驗地圖的視覺定位結果Fig.24 Result of visual localization based on prior maps

圖25 機器人自主導航過程Fig.25 Process of autonomous robot navigation

本節實驗按照第3節內容中所述步驟進行，機器人首先進行環境探索，構建實驗環境的多圖層地圖，然后，根據定位圖層和坐標變換確定機器人在規劃圖層的位置，即進行初始定位，如圖25a所示；再次，根據任務目標點通過A*算法生成最優路徑，如圖25b所示；基于規劃的路徑，采用DWA算法計算軌跡和運動策略，控制機器人移動，如圖25c所示，同時，進行機器人的實時視覺定位；最終機器人到達任務點，完成導航內容，如圖25d所示。

為了更加直觀地展示機器人在交互圖層進行導航的過程，如圖26所示，用紅色點標記機器人導航的起點和終點，紅色線段表示機器人的運動軌跡。交互圖層能夠顯示較為完整的環境信息，設定目標點后，機器人根據規劃圖層進行路徑規劃，基于先驗地圖的視覺定位方法實時將機器人的位置進行反饋，實時檢測機器人的位置，使整個導航的過程更加完整。

圖26 機器人在交互圖層自主導航Fig.26 Autonomous robot navigation in interactive layer

實驗結果表明，本文提出的基于改進ORB_SLAM2的視覺導航方法能夠構建完整的多圖層地圖，各圖層分工明確，交互圖層負責人機交互和生成規劃圖層，定位圖層用來實現初始定位和導航過程中機器人的實時定位，規劃圖層用來生成最優路徑；通過基于先驗地圖的視覺定位算法完成機器人的初始定位以及移動過程中的實時定位；最終完成機器人實時和精準地自主導航。

5 結論

(1)融合環境的3D、2D占據特征以及路標點空間位置、視覺特征等多模態信息構建包含定位、規劃、交互等圖層的地圖，并通過實驗證明本文方法在fr1/360序列比RGB-D SLAM軌跡精度提升8.91%、建圖效率提高49.38%；在fr1/floor序列中表現比RGB-D SLAM精度提升12.57%、效率提高43.95%；最后在北京鷲峰國家森林公園構建多圖層地圖,對比分析得出lAC的測量值與真實值誤差為6.7%，lBD的測量值與真實值誤差為5.6%。

(2)基于先驗多圖層地圖建立約束進行機器人的位姿估計，融合運動約束進行機器人的位姿優化，實現了基于先驗地圖的機器人精確定位，利用TUM數據集驗證得出，在fr1/360序列，定位軌跡與建圖軌跡絕對軌跡誤差相差0.05 m，fr1/floor序列相差0.01 m，fr1/rpy序列相差0.01 m；在北京鷲峰國家森林公園，控制機器人走過4段路徑，實驗結果證明機器人在先驗地圖中能夠利用視覺進行準確地定位。

(3)基于所構建的多圖層地圖進行機器人自主導航，實驗結果證明機器人能夠實時、準確地完成導航任務。