董蕊芳 王宇鵬 闞江明

(1.北京林業(yè)大學(xué)工學(xué)院， 北京 100083； 2.林業(yè)裝備與自動(dòng)化國(guó)家林業(yè)和草原局重點(diǎn)實(shí)驗(yàn)室， 北京 100083)

0 引言

即時(shí)定位與地圖構(gòu)建(Simultaneous localization and mapping，SLAM)是機(jī)器人進(jìn)行自主導(dǎo)航的先決條件，能實(shí)現(xiàn)在無(wú)全球?qū)Ш叫l(wèi)星系統(tǒng)(Global navigation satellite system，GNSS)信號(hào)的情況下定位[1]。視覺(jué)SLAM系統(tǒng)主要包括傳感器信息獲取模塊、前端視覺(jué)里程計(jì)模塊、后端優(yōu)化模塊和地圖創(chuàng)建模塊[2-5]。視覺(jué)SLAM算法根據(jù)對(duì)圖像處理方法的不同分為基于特征的方法和直接法[6-8]。

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

目前來(lái)看，已有眾多研究針對(duì)機(jī)器人的自主導(dǎo)航，在ORB_SLAM2的基礎(chǔ)上提升了建圖性能或定位精度，但仍然沒(méi)有一套完整的策略實(shí)現(xiàn)機(jī)器人自主導(dǎo)航。因此，本文提出一種基于改進(jìn)ORB_SLAM2的視覺(jué)導(dǎo)航方法，以O(shè)RB_SLAM2的RGB-D模式為基礎(chǔ)，融合環(huán)境的3D、2D占據(jù)特征以及路標(biāo)點(diǎn)空間位置、視覺(jué)特征等多模態(tài)信息，構(gòu)建包含定位、規(guī)劃、交互圖層的多圖層地圖以支撐機(jī)器人的精準(zhǔn)定位和最優(yōu)路徑規(guī)劃，并在多圖層地圖的基礎(chǔ)上實(shí)現(xiàn)定位和導(dǎo)航。

本文提出的方法將實(shí)現(xiàn)多圖層地圖構(gòu)建、基于先驗(yàn)地圖的視覺(jué)定位、規(guī)劃導(dǎo)航3個(gè)功能。以O(shè)RB_SLAM2關(guān)鍵幀為基礎(chǔ)，構(gòu)建多圖層地圖；以多圖層地圖作為先驗(yàn)信息，進(jìn)行機(jī)器人實(shí)時(shí)定位；設(shè)定目標(biāo)點(diǎn)后進(jìn)行路徑規(guī)劃，最終實(shí)現(xiàn)機(jī)器人自主導(dǎo)航。

1 多圖層地圖構(gòu)建

對(duì)ORB_SLAM2的RGB-D模式進(jìn)行改進(jìn)，根據(jù)其關(guān)鍵幀機(jī)制構(gòu)建多圖層地圖。首先通過(guò)關(guān)鍵幀深度圖計(jì)算局部稠密點(diǎn)云，通過(guò)分裂局部點(diǎn)云計(jì)算環(huán)境3D占據(jù)信息并更新交互圖層；之后，對(duì)環(huán)境3D占據(jù)信息進(jìn)行二維映射得到2D占據(jù)信息并以此來(lái)更新規(guī)劃圖層；與此同時(shí)，對(duì)關(guān)鍵幀中環(huán)境的路標(biāo)點(diǎn)以及視覺(jué)特征建立對(duì)應(yīng)關(guān)系，更新定位圖層。將交互圖層、規(guī)劃圖層以及定位圖層進(jìn)行坐標(biāo)對(duì)齊，構(gòu)建成融合了環(huán)境的3D、2D占據(jù)信息以及路標(biāo)點(diǎn)和視覺(jué)特征的多圖層地圖，整體框架如圖1所示。

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

1.1 交互圖層構(gòu)建

交互圖層負(fù)責(zé)實(shí)現(xiàn)導(dǎo)航過(guò)程中的人機(jī)交互，應(yīng)具備豐富的外觀信息，在此構(gòu)建八叉樹(shù)地圖[15]作為交互圖層。構(gòu)建過(guò)程包括局部稠密點(diǎn)云計(jì)算、點(diǎn)云配準(zhǔn)以及環(huán)境3D占據(jù)信息更新3部分。

1.1.1局部稠密點(diǎn)云計(jì)算

以O(shè)RB_SLAM2關(guān)鍵幀的深度圖作為輸入，通過(guò)RGB-D相機(jī)模型計(jì)算局部稠密點(diǎn)云，構(gòu)建點(diǎn)云條件濾波器以剔除離群點(diǎn)。離群點(diǎn)剔除的方法如下：

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

(1)

平均距離μ計(jì)算公式為

(2)

(3)

圖2 離群點(diǎn)剔除效果Fig.2 Outliers removed effect

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

1.1.2點(diǎn)云配準(zhǔn)

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

GPaiT=GRACPaiT+GPAT

(4)

之后，在B中尋找與GPai距離最近的點(diǎn)GPbi，此時(shí)認(rèn)為GPai和GPbi是一組匹配點(diǎn)。

根據(jù)上述方法，假設(shè)在A和B中有n對(duì)匹配點(diǎn)，定義A到B的變換矩陣為

(5)

構(gòu)造目標(biāo)函數(shù)求解最優(yōu)解

(6)

式中e——匹配點(diǎn)位置誤差

Pai——A中點(diǎn)云的坐標(biāo)

Pbi——B中與A對(duì)應(yīng)點(diǎn)云的坐標(biāo)

根據(jù)迭代次數(shù)以及迭代誤差是否收斂(以前后兩次迭代的轉(zhuǎn)換矩陣的最大容差判斷)等情況判斷迭代是否結(jié)束，本文設(shè)定的迭代次數(shù)為20次，收斂范圍為0.000 1，配準(zhǔn)結(jié)果如圖4(原始點(diǎn)云為紅色，目標(biāo)點(diǎn)云為綠色，配準(zhǔn)后點(diǎn)云為藍(lán)色)所示。

圖4 點(diǎn)云配準(zhǔn)結(jié)果Fig.4 Result of point cloud registration

以圖4為例，配準(zhǔn)前的ΔT為

(7)

配準(zhǔn)后ΔT*為

(8)

1.1.3環(huán)境3D占據(jù)信息更新

完成點(diǎn)云配準(zhǔn)后，對(duì)點(diǎn)云構(gòu)建包圍盒并進(jìn)行空間分裂。首先根據(jù)點(diǎn)云尺寸計(jì)算根節(jié)點(diǎn)包圍盒尺寸，再根據(jù)設(shè)定分裂步長(zhǎng)將根節(jié)點(diǎn)均勻劃分為8等份，再依次判斷是否到達(dá)遞歸深度、新的子節(jié)點(diǎn)中是否包含點(diǎn)云、子節(jié)點(diǎn)包含點(diǎn)云數(shù)量是否與父節(jié)點(diǎn)包含數(shù)量相同。點(diǎn)云空間分裂過(guò)程如圖5所示。

圖5 點(diǎn)云空間分裂流程Fig.5 Point cloud space segmentation process

根據(jù)點(diǎn)云空間分裂結(jié)果進(jìn)行環(huán)境3D占據(jù)信息更新過(guò)程如下：假設(shè)S為空間分裂后的一個(gè)正方體，其占據(jù)概率為ps∈[0,1]，依據(jù)其概率對(duì)數(shù)值ls更新S的占據(jù)狀態(tài)ps，S的概率對(duì)數(shù)值ls為

(9)

則有

(10)

當(dāng)觀測(cè)到S的次數(shù)增加時(shí)，ls增加，隨著ls增大，ps將趨近于1，表示S被占據(jù)的概率增大；反之，當(dāng)觀測(cè)到S的次數(shù)減少時(shí)，ls減小，隨著ls減小ps將趨近于0，表示S被占據(jù)的概率減小。

根據(jù)上述步驟，可通過(guò)配準(zhǔn)后的點(diǎn)云信息計(jì)算得到環(huán)境的3D占據(jù)信息，之后將其融合至多圖層地圖中，完成交互圖層構(gòu)建。

1.2 規(guī)劃圖層構(gòu)建

規(guī)劃圖層用于機(jī)器人的路徑規(guī)劃，而移動(dòng)機(jī)器人的運(yùn)動(dòng)限制在2D空間，因此本文以2D占據(jù)網(wǎng)格地圖作為規(guī)劃圖層。在此對(duì)交互圖層進(jìn)行2D投影，計(jì)算2D占據(jù)信息，整體流程如下：

首先提取在交互圖層中移動(dòng)機(jī)器人運(yùn)動(dòng)空間部分的3D占據(jù)信息，設(shè)機(jī)器人高為h，提取高度位于[0，h]內(nèi)的交互圖層3D占據(jù)信息，如圖6所示。

圖6 移動(dòng)機(jī)器人運(yùn)動(dòng)空間占據(jù)信息提取Fig.6 Motion space occupancy information extraction of mobile robot

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

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

(11)

令左上標(biāo)G2表示2D空間，左上標(biāo)G3表示3D空間，則Ci表示為

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

(12)

定義Ci中具有最大占據(jù)概率的點(diǎn)為G3Pp=max，Ci的2D映射點(diǎn)的坐標(biāo)為G2P=(x，y)，此時(shí)G2P的占據(jù)概率p(G2P)計(jì)算更新為

p(G2P)=p(G3Pp=max)

(13)

通過(guò)上述方法可對(duì)交互圖層中的3D占據(jù)信息進(jìn)行提取以及二維映射，得到環(huán)境的2D占據(jù)信息并將其融合到多圖層地圖中，最終完成規(guī)劃圖層的構(gòu)建。

圖7 環(huán)境2D占據(jù)信息更新Fig.7 Environment 2D occupancy information update

1.3 定位圖層構(gòu)建

為了實(shí)現(xiàn)在已知地圖中快速定位，本文融合環(huán)境的路標(biāo)點(diǎn)信息以及圖像的特征信息構(gòu)建定位圖層，使其能夠?yàn)橐苿?dòng)機(jī)器人快速定位提供基礎(chǔ)。定位圖層構(gòu)建過(guò)程如圖8所示。

圖8 定位圖層構(gòu)建Fig.8 Localization layer construction

首先，提取關(guān)鍵幀圖像2D特征以及3D路標(biāo)特征，并將兩者按照對(duì)應(yīng)關(guān)系建立哈希表

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

(14)

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

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

(15)

通過(guò)上述步驟，利用關(guān)鍵幀的機(jī)制，將環(huán)境的路標(biāo)點(diǎn)信息和圖像的特征信息按照對(duì)應(yīng)關(guān)系進(jìn)行融合，最終實(shí)現(xiàn)定位圖層的構(gòu)建。

2 基于先驗(yàn)地圖的視覺(jué)定位

本文提出的基于先驗(yàn)地圖的機(jī)器人視覺(jué)定位方法以定位圖層為基礎(chǔ)，整體框架如圖9所示。定位方法包括4個(gè)主要部分：視覺(jué)字典構(gòu)建、參考關(guān)鍵幀搜索、基于地圖約束的機(jī)器人位姿估計(jì)、基于運(yùn)動(dòng)約束的機(jī)器人位姿優(yōu)化。

圖9 基于先驗(yàn)地圖的視覺(jué)定位算法框架Fig.9 Framework of visual localization algorithm based on prior map

2.1 視覺(jué)字典構(gòu)建

視覺(jué)字典構(gòu)建類似于一個(gè)聚類問(wèn)題，能夠使機(jī)器人尋找數(shù)據(jù)中的規(guī)律。本文采用的方法是構(gòu)建k叉樹(shù)結(jié)構(gòu)視覺(jué)字典，以保證對(duì)數(shù)級(jí)別的查找效率，下文中以k表示視覺(jué)字典的分支數(shù)，d表示視覺(jué)字典的深度。

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

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

(16)

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

cj——第j個(gè)聚類中心向量

將Di進(jìn)行排序，取距離最短的作為它的歸類。

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

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

視覺(jué)字典分支數(shù)k和視覺(jué)字典深度d這兩個(gè)參數(shù)對(duì)視覺(jué)字典的構(gòu)建耗時(shí)、大小以及查找效率起決定性作用，因此在應(yīng)用過(guò)程中需根據(jù)實(shí)際情況去選擇最優(yōu)參數(shù)。

2.2 參考關(guān)鍵幀搜索

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

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

(17)

2.3 基于地圖約束的機(jī)器人位姿估計(jì)

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

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

根據(jù)圖10可知，當(dāng)環(huán)境特征相似度較高時(shí)，誤匹配現(xiàn)象嚴(yán)重，因此進(jìn)行誤匹配點(diǎn)對(duì)的剔除。采用RANSAC剔除誤匹配，實(shí)現(xiàn)方法如下：假設(shè)在圖像I1中共有m個(gè)特征點(diǎn)，在圖像I2中有n個(gè)特征點(diǎn)，根據(jù)描述子匹配計(jì)算得出匹配點(diǎn)有k對(duì)，在k對(duì)匹配點(diǎn)中隨機(jī)抽取4個(gè)不共線的樣本計(jì)算出單應(yīng)性矩陣H，公式為

(18)

式中λ——尺度因子

(ui,vi)——I1中第i個(gè)匹配的特征點(diǎn)

(u′i,v′i)——I2中第i個(gè)匹配的特征點(diǎn)

定義代價(jià)函數(shù)為

(19)

重新隨機(jī)抽取4個(gè)不共線的樣本計(jì)算矩陣H，迭代計(jì)算代價(jià)函數(shù)cM直至到達(dá)全局最小，此時(shí)得到單應(yīng)矩陣H的最優(yōu)解，根據(jù)H進(jìn)行投影剔除誤匹配。剔除誤匹配后結(jié)果如圖11所示。

圖11 通過(guò)RANSAC算法剔除誤匹配結(jié)果Fig.11 Wrong matches eliminated by RANSAC algorithm

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

圖12 PnP算法求解相機(jī)在先驗(yàn)地圖中位姿Fig.12 Camera poses estimated by PnP algorithm

2.4 基于運(yùn)動(dòng)約束的機(jī)器人位姿優(yōu)化

基于地圖約束的機(jī)器人位姿估計(jì)結(jié)果存在一定誤差，說(shuō)明相機(jī)的運(yùn)動(dòng)約束不可忽略，因此本文在上述位姿估計(jì)結(jié)果的基礎(chǔ)上，基于運(yùn)動(dòng)約束進(jìn)行機(jī)器人的位姿優(yōu)化，以提升定位精度。

假設(shè)在k幀，已利用PnP求解出當(dāng)前相機(jī)的初始位姿xk，在此聯(lián)系第k-1幀建立相機(jī)的運(yùn)動(dòng)約束。首先尋找第k幀和第k-1幀的共視3D地圖特征點(diǎn)，設(shè)為集合Pk,k-1，然后計(jì)算Pk,k-1在第k幀的重投影誤差建立運(yùn)動(dòng)約束，其誤差目標(biāo)函數(shù)

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

(20)

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

h——3D特征點(diǎn)Pk,k-1到xk的投影函數(shù)

zk——Pk,k-1在第k幀的觀測(cè)量，即像素坐標(biāo)

通過(guò)高斯牛頓法建立誤差目標(biāo)函數(shù)的正規(guī)方程

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

(21)

式中 Δx——迭代步長(zhǎng)

J——誤差雅可比矩陣

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

3 基于多圖層地圖的機(jī)器人自主導(dǎo)航

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

(1)機(jī)器人進(jìn)行環(huán)境探索，構(gòu)建多圖層地圖。

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

(3)設(shè)定目標(biāo)點(diǎn)，基于規(guī)劃圖層利用A*算法規(guī)劃最優(yōu)路徑。

(4)根據(jù)實(shí)時(shí)定位結(jié)果，采用DWA(Dynamic window approach)算法計(jì)算軌跡和運(yùn)動(dòng)策略[23]，最終實(shí)現(xiàn)機(jī)器人自主導(dǎo)航。

圖13 機(jī)器人自主導(dǎo)航框架Fig.13 Framework of autonomous navigation

3.1 基于規(guī)劃圖層的A*算法路徑規(guī)劃

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

(1)定義機(jī)器人水平移動(dòng)代價(jià)值為10，斜向移動(dòng)的代價(jià)值為14，代價(jià)函數(shù)f為

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

(22)

式中g(shù)——起始點(diǎn)至n的代價(jià)

h——n至目標(biāo)點(diǎn)的代價(jià)

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

圖14 移動(dòng)機(jī)器人在規(guī)劃圖層中移動(dòng)的代價(jià)值Fig.14 Cost value of mobile robot moving in grid map

(2)將節(jié)點(diǎn)A放入closelist中，在節(jié)點(diǎn)A周圍的方格中搜索可通行的區(qū)域，并計(jì)算每個(gè)方格的代價(jià)值f(n)，A的所有子節(jié)點(diǎn)放入openlist中。

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

(4)遍歷上述步驟，如果節(jié)點(diǎn)B被加入openlist中則表示搜索到最優(yōu)路徑，當(dāng)所有柵格都被訪問(wèn)后節(jié)點(diǎn)B仍然沒(méi)有被加入openlist中，那么則表示沒(méi)有路徑可由節(jié)點(diǎn)A到達(dá)節(jié)點(diǎn)B。

(5)從目標(biāo)點(diǎn)B回溯每個(gè)節(jié)點(diǎn)的父節(jié)點(diǎn)，最終得到最優(yōu)路徑。

上述A*算法進(jìn)行路徑搜索過(guò)程如圖15所示。

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

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

假設(shè)機(jī)器人對(duì)角線移動(dòng)的代價(jià)為hd(n)，定義為

(23)

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

g(x)、g(y)——移動(dòng)代價(jià)在x和y方向的分量

直線移動(dòng)代價(jià)定義為

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

(24)

此時(shí)的啟發(fā)式函數(shù)為

(25)

在規(guī)劃圖層中設(shè)置兩組相同的起點(diǎn)和終點(diǎn)，使用A*算法進(jìn)行路徑規(guī)劃，結(jié)果見(jiàn)圖16。

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

3.2 基于DWA算法的軌跡規(guī)劃

機(jī)器人獲得目標(biāo)點(diǎn)后，根據(jù)A*算法規(guī)劃出大致可行的路徑，然后需要在設(shè)定好的路徑計(jì)算運(yùn)動(dòng)策略。DWA算法可根據(jù)規(guī)劃的路徑實(shí)時(shí)計(jì)算出速度。機(jī)器人應(yīng)用DWA算法進(jìn)行軌跡規(guī)劃步驟如下：

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

(26)

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

θ′——推演的航向角

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

Δt——采樣時(shí)間間隔

(2)根據(jù)式(26)對(duì)每一條推算的軌跡進(jìn)行評(píng)分(G表示得分)

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

(27)

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

表1 DWA算法參數(shù)設(shè)置Tab.1 DWA algorithm parameters setting

根據(jù)工作環(huán)境的不同，DWA算法的朝向、位置以及速度權(quán)重選擇的占比也不相同，機(jī)器人在環(huán)境中工作需要能盡快安全到達(dá)指定的目標(biāo)點(diǎn)，因此最終位置的權(quán)重應(yīng)該最高。設(shè)λ=0.8，α=0.7，β=0.2，γ=0.1，在規(guī)劃圖層中測(cè)試DWA算法，給定目標(biāo)點(diǎn)，根據(jù)A*算法生成一條全局軌跡(綠色軌跡)，再通過(guò)DWA算法進(jìn)行航跡推演(黑色框?yàn)橥蒲莘秶?得到最優(yōu)的運(yùn)動(dòng)策略(紅色軌跡)，如圖17所示。

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

4 實(shí)驗(yàn)

本文包括兩組實(shí)驗(yàn)：驗(yàn)證實(shí)驗(yàn)和實(shí)地實(shí)驗(yàn)。第1組選用TUM數(shù)據(jù)集[24]，驗(yàn)證本文的多圖層地圖構(gòu)建方法所估計(jì)軌跡的精度和實(shí)時(shí)性，以及基于先驗(yàn)多圖層地圖的視覺(jué)定位方法估計(jì)結(jié)果與未知環(huán)境下SLAM定位結(jié)果的一致性。

第2組實(shí)驗(yàn)為北京鷲峰國(guó)家森林公園實(shí)地實(shí)驗(yàn)，將本文的視覺(jué)導(dǎo)航方法應(yīng)用于所搭建的輪式機(jī)器人。

4.1 TUM數(shù)據(jù)集實(shí)驗(yàn)

TUM數(shù)據(jù)集為Kinect視覺(jué)傳感器采集的標(biāo)準(zhǔn)數(shù)據(jù)集，該數(shù)據(jù)集中包含彩色圖和深度圖以及相機(jī)運(yùn)動(dòng)過(guò)程中的Ground truth(軌跡真值)。本組實(shí)驗(yàn)將通過(guò)TUM的fr1/360、fr1/floor、fr1/rpy序列驗(yàn)證。

4.1.1多圖層地圖構(gòu)建實(shí)驗(yàn)

本文對(duì)ORB-SLAM2算法進(jìn)行改進(jìn)，構(gòu)建多圖層地圖以支撐機(jī)器人的精準(zhǔn)定位和最優(yōu)路徑規(guī)劃。為驗(yàn)證其所構(gòu)建多圖層地圖的優(yōu)越性，將其輸出的位姿估計(jì)結(jié)果與Ground truth和經(jīng)典的RGB-D SLAM[25]結(jié)果進(jìn)行比較，結(jié)果如表2所示，主要對(duì)比算法耗時(shí)和全局軌跡均方根誤差兩個(gè)指標(biāo)。通過(guò)表2可知，在fr1/360序列，本文方法估計(jì)的軌跡精度比RGB-D SLAM提升8.91%，建圖效率提高49.38%；在fr1/floor序列，本文方法估計(jì)的軌跡精度比RGB-D SLAM提升12.57%、建圖效率提高43.95%?？傮w而言，本文方法的平均運(yùn)行速度較RGB-D SLAM提升了50%，估計(jì)軌跡精度提升了10%。因此本文提出的方法比RGB-D SLAM建立地圖的效率更高，軌跡更加準(zhǔn)確。

表2 TUM數(shù)據(jù)集位姿估計(jì)結(jié)果Tab.2 Results of pose estimating with TUM

4.1.2基于先驗(yàn)地圖的視覺(jué)定位實(shí)驗(yàn)

由于TUM數(shù)據(jù)集僅提供了單次機(jī)器人運(yùn)行數(shù)據(jù)，因此，本文進(jìn)行基于先驗(yàn)地圖的視覺(jué)定位實(shí)驗(yàn)，思路為：首先，使用TUM數(shù)據(jù)集進(jìn)行SLAM，構(gòu)建多圖層地圖并輸出機(jī)器人運(yùn)行序列位姿；然后，在所構(gòu)建地圖的基礎(chǔ)上，再次輸入相同的數(shù)據(jù)，模擬機(jī)器人第2次進(jìn)入相同的環(huán)境，通過(guò)對(duì)比基于先驗(yàn)地圖定位方法得到的序列位姿與SLAM過(guò)程估計(jì)的位姿之間偏差來(lái)驗(yàn)證算法的性能，實(shí)驗(yàn)結(jié)果見(jiàn)表3，在fr1/floor、fr1/rpy這兩個(gè)序列的定位結(jié)果與SLAM所估計(jì)軌跡的均方根誤差僅有0.01 m。這說(shuō)明本文提出的基于先驗(yàn)多圖層地圖的視覺(jué)定位方法能夠進(jìn)行機(jī)器人的精確定位，助力機(jī)器人實(shí)現(xiàn)準(zhǔn)確的自主導(dǎo)航。

表3 基于先驗(yàn)地圖的視覺(jué)定位結(jié)果Tab.3 Results of visual localization with prior map m

4.2 北京鷲峰國(guó)家森林公園實(shí)驗(yàn)

利用所搭建的輪式機(jī)器人在北京鷲峰國(guó)家森林公園進(jìn)行實(shí)驗(yàn)。

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

圖18 實(shí)驗(yàn)平臺(tái)示意圖Fig.18 Schematic of experimental platform

4.2.1多圖層地圖構(gòu)建實(shí)驗(yàn)

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

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

由于實(shí)驗(yàn)環(huán)境中難以獲取相機(jī)軌跡真值，采用以下方式來(lái)檢驗(yàn)建立地圖的精度：實(shí)驗(yàn)場(chǎng)地為非規(guī)則圖形，因此實(shí)驗(yàn)過(guò)程中設(shè)置了4個(gè)路標(biāo)點(diǎn)，分別為A、B、C、D，機(jī)器人在實(shí)驗(yàn)場(chǎng)地行走的路線為：由點(diǎn)A出發(fā)，按照A→B→C→D→A的路線行走，實(shí)時(shí)構(gòu)建多圖層地圖，圖20為行走路線示意圖。

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

構(gòu)建的多層地圖如圖21所示，其中定位圖層為特征點(diǎn)地圖；交互圖層為八叉樹(shù)地圖；規(guī)劃圖層為2D占據(jù)網(wǎng)格地圖。

圖21 實(shí)驗(yàn)場(chǎng)景多圖層地圖Fig.21 Muti-layer map of experimental site

為方便統(tǒng)計(jì)，對(duì)交互圖層中的樹(shù)木進(jìn)行編號(hào)，如圖22所示，并分別用米尺測(cè)得實(shí)驗(yàn)場(chǎng)地中AC、BD的長(zhǎng)度lAC和lBD，再通過(guò)多圖層地圖測(cè)量AC、BD之間的長(zhǎng)度進(jìn)行對(duì)比。

圖22 交互圖層中樹(shù)木編號(hào)Fig.22 Number trees in interactive layer

實(shí)驗(yàn)結(jié)果見(jiàn)表4，結(jié)果表明，lAC的測(cè)量值與真實(shí)值之間的誤差為6.7%，lBD的測(cè)量值與真實(shí)值之間的誤差為5.6%，機(jī)器經(jīng)過(guò)的路徑周圍共30棵樹(shù)木，在地圖中編號(hào)樹(shù)木也為30棵。

表4 實(shí)驗(yàn)結(jié)果Tab.4 Result of experimental site

在真實(shí)環(huán)境中采用RGB-D SLAM[25]算法實(shí)時(shí)構(gòu)建地圖時(shí)發(fā)現(xiàn)，該算法隨著地圖的增加會(huì)出現(xiàn)信息滯后的情況，而本文方法能夠?qū)崟r(shí)重建交互圖層、規(guī)劃圖層以及定位圖層，且地圖精度和完整度都能得到保障，能為機(jī)器人自主導(dǎo)航提供準(zhǔn)確的先驗(yàn)地圖。

4.2.2視覺(jué)字典構(gòu)建實(shí)驗(yàn)

自主導(dǎo)航要求機(jī)器人快速準(zhǔn)確地實(shí)現(xiàn)自身定位，在線構(gòu)建視覺(jué)字典的時(shí)間以及字典大小直接影響機(jī)器人定位的效率以及精度，因此，本文對(duì)比選擇k(視覺(jué)字典分支數(shù))、d(視覺(jué)字典深度)的最優(yōu)參數(shù)來(lái)提升計(jì)算效率及精度。

實(shí)驗(yàn)中不同k、d構(gòu)建字典的時(shí)間和生成字典的大小見(jiàn)表5、6。

表5 不同k、d構(gòu)建視覺(jué)字典的時(shí)間Tab.5 Cost time of k and d for generating visual dictionary s

表6 不同k、d構(gòu)建視覺(jué)字典的大小Tab.6 Memory size of k and d for generating visual dictionary kb

對(duì)比不同k、d構(gòu)建的12組視覺(jué)字典進(jìn)行圖像檢索，驗(yàn)證識(shí)別效率。實(shí)驗(yàn)中對(duì)每一個(gè)關(guān)鍵幀進(jìn)行順序檢索，按照關(guān)鍵幀出現(xiàn)的順序進(jìn)行驗(yàn)證，圖23所示為檢索結(jié)果，在使用d=2、k=6和d=2、k=7兩種參數(shù)組合進(jìn)行視覺(jué)字典構(gòu)建時(shí)識(shí)別準(zhǔn)確率分別為51.62%和99.16%，在d=2、k=8時(shí)識(shí)別準(zhǔn)確率達(dá)100%。其余參數(shù)的視覺(jué)字典識(shí)別準(zhǔn)確率均為100%。

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

圖23 不同k、d的字典識(shí)別結(jié)果Fig.23 Recognition results using dictionaries with different k and d

表7 k、d對(duì)識(shí)別耗時(shí)的影響

4.2.3基于先驗(yàn)地圖的視覺(jué)定位實(shí)驗(yàn)

進(jìn)行基于先驗(yàn)地圖的視覺(jué)定位實(shí)驗(yàn)方式為：選擇建圖時(shí)的4個(gè)標(biāo)記點(diǎn)A、B、C、D(在地圖中用紅色點(diǎn)表示)，使機(jī)器人分別行走4段路線：A→B、B→C、C→D、D→A(A默認(rèn)為坐標(biāo)原點(diǎn))，即分別以A、B、C、D為起點(diǎn)進(jìn)行實(shí)驗(yàn)，并分別記錄下定位結(jié)果。

實(shí)驗(yàn)結(jié)果如圖24所示。以圖24b為例，機(jī)器人在實(shí)際地圖中以B為起點(diǎn)，行走B→C段路線，但當(dāng)機(jī)器人開(kāi)始進(jìn)入地圖時(shí)，系統(tǒng)默認(rèn)其初始位置為地圖坐標(biāo)原點(diǎn)(點(diǎn)A)，通過(guò)初始定位后，立刻確定自身位置在點(diǎn)B，并在后續(xù)運(yùn)動(dòng)過(guò)程中進(jìn)行實(shí)時(shí)定位，最終結(jié)果表明機(jī)器人能夠?qū)崿F(xiàn)準(zhǔn)確的定位。

4.2.4基于多圖層地圖導(dǎo)航實(shí)驗(yàn)

圖24 基于先驗(yàn)地圖的視覺(jué)定位結(jié)果Fig.24 Result of visual localization based on prior maps

圖25 機(jī)器人自主導(dǎo)航過(guò)程Fig.25 Process of autonomous robot navigation

本節(jié)實(shí)驗(yàn)按照第3節(jié)內(nèi)容中所述步驟進(jìn)行，機(jī)器人首先進(jìn)行環(huán)境探索，構(gòu)建實(shí)驗(yàn)環(huán)境的多圖層地圖，然后，根據(jù)定位圖層和坐標(biāo)變換確定機(jī)器人在規(guī)劃圖層的位置，即進(jìn)行初始定位，如圖25a所示；再次，根據(jù)任務(wù)目標(biāo)點(diǎn)通過(guò)A*算法生成最優(yōu)路徑，如圖25b所示；基于規(guī)劃的路徑，采用DWA算法計(jì)算軌跡和運(yùn)動(dòng)策略，控制機(jī)器人移動(dòng)，如圖25c所示，同時(shí)，進(jìn)行機(jī)器人的實(shí)時(shí)視覺(jué)定位；最終機(jī)器人到達(dá)任務(wù)點(diǎn)，完成導(dǎo)航內(nèi)容，如圖25d所示。

為了更加直觀地展示機(jī)器人在交互圖層進(jìn)行導(dǎo)航的過(guò)程，如圖26所示，用紅色點(diǎn)標(biāo)記機(jī)器人導(dǎo)航的起點(diǎn)和終點(diǎn)，紅色線段表示機(jī)器人的運(yùn)動(dòng)軌跡。交互圖層能夠顯示較為完整的環(huán)境信息，設(shè)定目標(biāo)點(diǎn)后，機(jī)器人根據(jù)規(guī)劃圖層進(jìn)行路徑規(guī)劃，基于先驗(yàn)地圖的視覺(jué)定位方法實(shí)時(shí)將機(jī)器人的位置進(jìn)行反饋，實(shí)時(shí)檢測(cè)機(jī)器人的位置，使整個(gè)導(dǎo)航的過(guò)程更加完整。

圖26 機(jī)器人在交互圖層自主導(dǎo)航Fig.26 Autonomous robot navigation in interactive layer

實(shí)驗(yàn)結(jié)果表明，本文提出的基于改進(jìn)ORB_SLAM2的視覺(jué)導(dǎo)航方法能夠構(gòu)建完整的多圖層地圖，各圖層分工明確，交互圖層負(fù)責(zé)人機(jī)交互和生成規(guī)劃圖層，定位圖層用來(lái)實(shí)現(xiàn)初始定位和導(dǎo)航過(guò)程中機(jī)器人的實(shí)時(shí)定位，規(guī)劃圖層用來(lái)生成最優(yōu)路徑；通過(guò)基于先驗(yàn)地圖的視覺(jué)定位算法完成機(jī)器人的初始定位以及移動(dòng)過(guò)程中的實(shí)時(shí)定位；最終完成機(jī)器人實(shí)時(shí)和精準(zhǔn)地自主導(dǎo)航。

5 結(jié)論

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

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

(3)基于所構(gòu)建的多圖層地圖進(jìn)行機(jī)器人自主導(dǎo)航，實(shí)驗(yàn)結(jié)果證明機(jī)器人能夠?qū)崟r(shí)、準(zhǔn)確地完成導(dǎo)航任務(wù)。