高宏偉，于 斌，武炎明

(1.沈陽理工大學(xué) 自動(dòng)化與電氣工程學(xué)院，沈陽 110159；2.沈陽自動(dòng)化研究所，沈陽 110017)

近年來無人機(jī)與無人車協(xié)同作業(yè)的應(yīng)用越來越廣泛。無人車雖然能夠?qū)Φ孛娴慕嚯x目標(biāo)精準(zhǔn)定位并執(zhí)行任務(wù)，但其視野范圍具有局限性，限制其工作的區(qū)域和效率；無人機(jī)機(jī)動(dòng)性強(qiáng)、視野范圍廣，但對(duì)地面物體定位的精確度不高，對(duì)地面物體操作的靈活性差。二者組成的協(xié)同系統(tǒng)能夠構(gòu)建出空地層次的全局地圖，實(shí)現(xiàn)對(duì)環(huán)境更精確、更完整的感知[1]。

傳統(tǒng)無人機(jī)與無人車空地協(xié)同實(shí)現(xiàn)地面導(dǎo)航的技術(shù)核心是將無人機(jī)航拍的圖像序列拼接為全景圖像，無人車經(jīng)過圖像處理將其中的障礙物識(shí)別并標(biāo)記出來，進(jìn)而開始路徑規(guī)劃并沿著規(guī)劃路線到達(dá)目標(biāo)地點(diǎn)。Jianwen Huo等[2]提出了一種基于地圖匹配的路徑規(guī)劃算法，通過在傳統(tǒng)的無人機(jī)拼接全局地圖方法中加入圖像校正來減小誤差，同時(shí)利用無人車的激光雷達(dá)建立局部地圖，通過最小二乘法確定地圖匹配中的未知值，有效地提高了協(xié)同導(dǎo)航精度；但該過程中的無人車無法從拼接圖像中確定障礙物的具體位置信息，導(dǎo)致協(xié)同導(dǎo)航系統(tǒng)的環(huán)境適應(yīng)性不足。王晨捷等[3]在點(diǎn)線特征同步定位與地圖構(gòu)建(Simultaneous Localization And Mapping，SLAM)方法的基礎(chǔ)上加入了跟蹤顯著閉合邊界的線程，通過基于線特征的閉合區(qū)域表示障礙物，同時(shí)配合無人車的激光雷達(dá)構(gòu)建局部地圖更新數(shù)據(jù)，實(shí)現(xiàn)了對(duì)無人車的導(dǎo)航；該方法在障礙物較少的環(huán)境下取得了不錯(cuò)的效果，但欠缺對(duì)于航拍視覺存在遮擋情況的處理。

隨著視覺SLAM技術(shù)迅速發(fā)展，在建圖和定位方面取得了不錯(cuò)的效果。目前，視覺SLAM系統(tǒng)按照建圖結(jié)構(gòu)可分為稀疏SLAM、半稠密SLAM和稠密SLAM[4]。由于后兩者建圖時(shí)恢復(fù)出的信息更加豐富，對(duì)系統(tǒng)硬件的要求高，而導(dǎo)航任務(wù)中只需要知道障礙物的基本信息，故選用稀疏SLAM方法以提高系統(tǒng)的實(shí)時(shí)性?；谔卣鼽c(diǎn)的單目實(shí)時(shí)ORB-SLAM2是目前使用最廣泛的稀疏SLAM方法[5]。

本文在ORB-SLAM2基礎(chǔ)上，將2D線段和3D線段分別匹配，減少線段的誤匹配，增加建圖的準(zhǔn)確性。融合視覺SLAM建圖與圖像拼接建圖構(gòu)建全局地圖，使構(gòu)建出的全局地圖既有前者的準(zhǔn)確位置信息又有后者的準(zhǔn)確形狀信息，提高地圖的還原度。地面無人車通過進(jìn)一步融合激光雷達(dá)局部地圖的方法實(shí)時(shí)修正地圖，完成路徑規(guī)劃。

1 系統(tǒng)整體框架

本系統(tǒng)可分為兩部分，即全局地圖的構(gòu)建和地面機(jī)器人的導(dǎo)航。首先通過無人機(jī)采集地面航拍圖像并進(jìn)行全局地圖構(gòu)建，地面機(jī)器人通過構(gòu)建好的全局地圖進(jìn)行路徑規(guī)劃，通過自身的激光雷達(dá)構(gòu)建局部地圖并對(duì)全局地圖進(jìn)行更新，同時(shí)對(duì)預(yù)先規(guī)劃出的路徑進(jìn)行調(diào)整，最后到達(dá)指定位置。系統(tǒng)主要流程如圖1所示。

2 無人機(jī)視覺SLAM的全局地圖構(gòu)建

2.1 線特征SLAM全局地圖構(gòu)建

在航拍圖像中地面的絕大多數(shù)障礙物都可以近似為一個(gè)閉合圖形，故采用線特征SLAM的方法構(gòu)建全局地圖。

線特征提取算法(Line Band Discriptor，LBD)[6]首先對(duì)圖像進(jìn)行下采樣，構(gòu)建出尺度金字塔，然后對(duì)金字塔每層都進(jìn)行一次線段提取并重構(gòu)線段的集合，最后通過線段構(gòu)成的線段支持區(qū)域計(jì)算描述符，提取出每一幀圖像中的線特征。

采用基于特征點(diǎn)的ORB-SLAM2方法用作無人機(jī)構(gòu)建地圖的整體框架。因地面導(dǎo)航需要二維平面地圖，而物體垂直方向上的邊界也會(huì)被識(shí)別成線特征對(duì)地圖構(gòu)建造成干擾，針對(duì)這一問題本系統(tǒng)在其原有基礎(chǔ)上添加了3D-2D的線特征匹配線程。

3D場(chǎng)景中的線段如圖2所示。

圖2 3D線段匹配

圖2a中O為相機(jī)光心；Lc為光軸；K為關(guān)鍵幀；L為空間中直線；α為過光心和直線L的平面；θ為L(zhǎng)c與平面α的夾角。

圖2b中Knear為K的相鄰關(guān)鍵幀；L′為L(zhǎng)在Knear上的投影；β為過光心垂直于L的平面；A、B為L(zhǎng)上的端點(diǎn)；a、b為L(zhǎng)′上的點(diǎn)。

對(duì)于2D場(chǎng)景中的線段，采用LBD算法對(duì)其進(jìn)行匹配。3D-2D的線特征匹配方法是：通過平面β與L′的交點(diǎn)即可確定出端點(diǎn)A在平面上的匹配點(diǎn)a，同理可確定端點(diǎn)B的匹配點(diǎn)b。

基于直線的方向計(jì)算每個(gè)關(guān)鍵幀中的候選匹配線段l，具體過程為：將當(dāng)前幀中的線段根據(jù)其方向分為36組，對(duì)特征點(diǎn)的處理方式和ORB-SLAM2中相同；選擇l中具有相同方向的線段并分組，將每組中的全部線段添加到集合SL中作為候選匹配線段；在SL中根據(jù)最鄰近算法計(jì)算出直線的最佳匹配。

由于線段是多維的，所以采用一種點(diǎn)線距離和點(diǎn)點(diǎn)距離結(jié)合的方法對(duì)線段匹配誤差進(jìn)行優(yōu)化。線段l和l′的匹配誤差優(yōu)化公式為

d1(l，l′)=α(d(l，p1)+d(l，p2))

(1)

d2(l，l′)=(1-α)(d′(p1，p3)+d′(p2，p4))

(2)

(3)

式中：p1、p2為l′的兩個(gè)端點(diǎn)；p3、p4為l的兩個(gè)端點(diǎn)；d1(l，l′)為l與l′的點(diǎn)線距離；d2(l，l′)為l與l′的點(diǎn)點(diǎn)距離；d(l，p1)表示l到p1的距離；d(l，p2)表示l到p2的距離；d′(p1，p3)表示p1到點(diǎn)p3的距離；d′(p2，p4)表示p2到點(diǎn)p4的距離；lave是當(dāng)前幀所有線長(zhǎng)的平均值；δ為權(quán)重系數(shù)；d(l，l′)為兩直線優(yōu)化后的距離。

由線段之間的匹配關(guān)系將空間線段投影到地圖中。和空間點(diǎn)的投影不同，線段會(huì)隨相機(jī)運(yùn)動(dòng)出現(xiàn)不完整觀測(cè)的情況，故采用如下解決策略。

(1)首先通過SLAM前端初始化求得的變換矩陣T計(jì)算出兩個(gè)端點(diǎn)相對(duì)于當(dāng)前相機(jī)坐標(biāo)系的坐標(biāo)；

(2)若兩端點(diǎn)均在相機(jī)前方且方向相同，則對(duì)兩點(diǎn)進(jìn)行投影，繪制兩點(diǎn)間線段即可得到線段在地圖上的投影；

(3)若只有一個(gè)端點(diǎn)在相機(jī)前方，則令點(diǎn)的Z軸坐標(biāo)分量為0，求出空間直線與圖像平面交點(diǎn)的坐標(biāo)；

(4)若兩個(gè)端點(diǎn)均在相機(jī)后方，則不進(jìn)行投影。

由此可得到空間中直線投影后的地圖。

對(duì)于線特征SLAM建圖過程中出現(xiàn)的線段誤匹配和識(shí)別錯(cuò)誤，通過多次測(cè)試分析，將線段長(zhǎng)度的閾值設(shè)為2.5lave，可將超出閾值范圍的線段剔除。同時(shí)將三維地圖擬合到二維平面，對(duì)圖像進(jìn)行多邊形近似擬合處理，得到SLAM全局地圖。

2.2 圖像拼接全局地圖構(gòu)建

在線特征SLAM構(gòu)建初始地圖的同時(shí)，啟動(dòng)圖像拼接線程。首先對(duì)無人機(jī)視頻序列每隔50幀選中一幀圖像作為關(guān)鍵幀，對(duì)每個(gè)關(guān)鍵幀采用快速提取方法(Speeded Up Robust Features，SURF)進(jìn)行特征點(diǎn)提取[7]。

SURF方法首先對(duì)圖像中像素構(gòu)建海森矩陣，再通過圖像金字塔構(gòu)建圖像的尺度空間，然后通過濾波器檢測(cè)出特征點(diǎn)并確定每個(gè)特征點(diǎn)的主方向，最后通過計(jì)算8個(gè)方向的梯度直方圖生成特征點(diǎn)描述子，由此完成特征點(diǎn)的檢測(cè)與匹配。

為減少誤匹配的現(xiàn)象，針對(duì)每幀圖像檢測(cè)出的特征點(diǎn)進(jìn)行閾值篩選。如果兩幀圖像中匹配成功的特征點(diǎn)數(shù)量多于100則視為匹配成功，使用隨機(jī)抽樣一致性(Random Sample Consensus，RANSAC)方法繼續(xù)篩選可靠的匹配點(diǎn)[8]，求得兩個(gè)關(guān)鍵幀的變換矩陣，實(shí)現(xiàn)圖像配準(zhǔn)。

配準(zhǔn)圖像后，將第一個(gè)關(guān)鍵幀拷貝到配準(zhǔn)圖上并對(duì)兩幅圖像拼接處的像素值進(jìn)行漸入漸出加權(quán)融合，具體公式為

X(m，n)=

(4)

(5)

式中：m、n為像素值；x1，x2為待拼接圖像；X為拼接后圖像；γ為加權(quán)因子；w為重疊區(qū)域橫坐標(biāo)的差值；wd為重疊區(qū)域像素距離。

根據(jù)無人機(jī)圖像序列的特點(diǎn)，采用“U形”遞增的順序拼接圖像序列[9]，對(duì)拼接后的圖像運(yùn)用二值化、多級(jí)邊緣檢測(cè)算法Canny等[10]基本圖像處理方法，得到此線程的全局初始地圖。

2.3 輪廓相似度融合的全局地圖構(gòu)建

SLAM構(gòu)建的全局地圖能夠較好地還原地面的位置信息，但對(duì)于障礙物的識(shí)別不夠清晰；圖像拼接線程構(gòu)建的全局地圖能夠較好地分辨出障礙物，但拼接過程易產(chǎn)生較大的位置誤差。本文對(duì)兩個(gè)線程得到的地圖進(jìn)行融合以構(gòu)建最終的全局地圖。地圖融合方法是通過對(duì)比兩個(gè)初始地圖中障礙物輪廓特征進(jìn)行相似度計(jì)算完成。

使用Suv表示輪廓u和輪廓v的相似度，基于邊界跟蹤法構(gòu)建表示特征輪廓的函數(shù)fu、fv，可得

(6)

當(dāng)Suv=1時(shí)，兩個(gè)輪廓完全相同；當(dāng)0.7≤Suv≤1時(shí)，認(rèn)為兩個(gè)輪廓是同一個(gè)障礙物，并對(duì)兩個(gè)輪廓進(jìn)行融合。

(7)

候選融合線段傾斜角差值小于等于閾值θs[11]，則將其合并為一條線段。此處θs取值為10°。

最后對(duì)圖像中閉合區(qū)域進(jìn)行提取，得到用于地面導(dǎo)航的全局地圖[12]。

3 激光雷達(dá)的局部地圖構(gòu)建與導(dǎo)航

由于無人機(jī)得到的全局地圖對(duì)于地面細(xì)小的障礙感知能力較差，而且當(dāng)無人機(jī)航拍視角存在遮擋時(shí)會(huì)對(duì)地面路徑規(guī)劃造成干擾，對(duì)于地面新增障礙物的情況全局地圖不能及時(shí)更新，因此對(duì)地面無人車配備激光雷達(dá)構(gòu)建局部地圖，并與全局地圖相融合構(gòu)建導(dǎo)航地圖。

樹木遮擋情況仿真圖與激光雷達(dá)建圖如圖3所示。

圖3 樹木遮擋情況仿真與建圖

當(dāng)藍(lán)色的無人車被樹木遮擋時(shí)，航拍的地圖無法協(xié)助地面感知真實(shí)的環(huán)境，此時(shí)采用激光雷達(dá)可以很好地獲取周圍障礙物的位置信息。

建圖過程如下：

(1)將無人機(jī)傳入的全局地圖轉(zhuǎn)換成占據(jù)柵格地圖，通過視覺標(biāo)靶[13]的方式確定無人車在地圖的位置，采用目前普遍使用的雙向快速拓展隨機(jī)樹(Rapidly-exploring Random Tree，RRT)路徑規(guī)劃算法解決復(fù)雜約束的路徑規(guī)劃，在全局地圖中規(guī)劃出到達(dá)目標(biāo)點(diǎn)的最優(yōu)路徑[14]。

(2)地面無人車采用2D-SLAM算法Cartographer[15]進(jìn)行局部地圖構(gòu)建，通過激光雷達(dá)實(shí)時(shí)掃描感知周圍環(huán)境。

(3)無人車在前進(jìn)的同時(shí)對(duì)局部地圖和全局地圖進(jìn)行融合，當(dāng)檢測(cè)到地圖更新后無人車對(duì)路徑進(jìn)行重新規(guī)劃，自主導(dǎo)航到達(dá)目標(biāo)點(diǎn)。

4 仿真結(jié)果與分析

系統(tǒng)的仿真實(shí)驗(yàn)選擇在Ubuntu18.04下的ROS系統(tǒng)中進(jìn)行，采用Gazebo軟件構(gòu)建仿真環(huán)境并對(duì)空地協(xié)同無人系統(tǒng)進(jìn)行仿真[16]。

表1 實(shí)驗(yàn)環(huán)境

無人機(jī)模型選用了開源仿真四旋翼無人機(jī)RotorS，用于地面導(dǎo)航的無人車選用了配備激光雷達(dá)的開源機(jī)器人模型mbot。無人機(jī)與無人車仿真模型如圖4所示。

圖4 無人機(jī)與無人車仿真模型

為減小建圖誤差，將相機(jī)角度調(diào)整為垂直于地面。在場(chǎng)景中模擬了光照、摩擦力和重力等因素以保證和真實(shí)環(huán)境高度一致。同時(shí)在場(chǎng)景中加入了汽車、路樁和多種形狀的物體作為地面導(dǎo)航的障礙物，如圖5所示。

圖5 仿真場(chǎng)景

場(chǎng)景中右上方放置了一個(gè)視覺標(biāo)靶，以輔助無人車定位。

實(shí)驗(yàn)完成了如下仿真過程：

(1)控制無人機(jī)在仿真場(chǎng)景中全局飛行并對(duì)全局環(huán)境進(jìn)行圖像采集；

(2)啟動(dòng)SLAM線程對(duì)全局地圖進(jìn)行實(shí)時(shí)構(gòu)建，并融合拼接線程建圖；

(3)本系統(tǒng)無人車選用雙向RRT算法作為路徑規(guī)劃方法。

無人機(jī)在仿真場(chǎng)景采集圖像拼接并處理后得到的效果如圖6所示。全局地圖構(gòu)建結(jié)果如圖7所示。

圖6 圖像拼接線程建圖

圖7 全局地圖構(gòu)建

圖7a為SLAM線程構(gòu)建的地圖，圖7b為融合圖6后用于地面導(dǎo)航的全局地圖。

對(duì)于建圖質(zhì)量的評(píng)價(jià)，主要通過將建圖結(jié)果進(jìn)行二值化，計(jì)算圖中占據(jù)柵格和仿真場(chǎng)景中對(duì)應(yīng)柵格的距離，將所有距離總和除以仿真場(chǎng)景中占據(jù)柵格的數(shù)量，算出的建圖誤差為

(8)

式中：ri、ti為第i個(gè)柵格在x、y方向上的坐標(biāo)；k為仿真環(huán)境中占據(jù)柵格的總數(shù)量；r′i、t′i為仿真場(chǎng)景中第i個(gè)柵格在x、y方向上的坐標(biāo)。

對(duì)建圖效果測(cè)試，結(jié)果如表2所示。

表2 全局建圖效果評(píng)價(jià)

由表2可見，融合邊緣特征的建圖方法相較于未融合的SLAM建圖方法有效減小了建圖的誤差。

在實(shí)驗(yàn)過程中將本文算法與使用未進(jìn)行融合的全局地圖路徑規(guī)劃的方法進(jìn)行對(duì)比，二者路徑規(guī)劃效果如圖8所示。

圖8 全局路徑規(guī)劃

測(cè)試全局地圖路徑規(guī)劃效果的評(píng)價(jià)如表3所示。

表3 全局地圖路徑規(guī)劃效果評(píng)價(jià)

圖9為無人車按照全局規(guī)劃的路線進(jìn)行導(dǎo)航，最終到達(dá)目標(biāo)點(diǎn)過程。

圖9 無人車導(dǎo)航過程

圖10為航拍圖像出現(xiàn)遮擋時(shí)無人車通過激光雷達(dá)局部建圖并融合地圖進(jìn)行導(dǎo)航的仿真。

由此可見，加入圖像拼接線程后構(gòu)建的全局地圖更精確可靠，通過無人車激光雷達(dá)局部建圖并融合地圖，提高了路徑規(guī)劃效率，能幫助空地協(xié)同系統(tǒng)規(guī)劃出更短的有效路徑。

圖10 模擬特殊場(chǎng)景

5 結(jié)論

提出一種2D、3D線段分別匹配的線特征SLAM方法對(duì)無人機(jī)獲得的連續(xù)航拍圖像進(jìn)行定位和建圖，并基于輪廓相似度融合圖像拼接線程構(gòu)建的全局地圖，獲得更加精確的地面導(dǎo)航地圖。地面無人車通過路徑規(guī)劃算法以及激光雷達(dá)建立局部地圖實(shí)現(xiàn)路徑重新規(guī)劃，并解決動(dòng)態(tài)障礙物和遮擋情況。通過在仿真環(huán)境中完成空地協(xié)同導(dǎo)航任務(wù)對(duì)比實(shí)驗(yàn)，表明上述方法可實(shí)現(xiàn)空地協(xié)同導(dǎo)航任務(wù)，對(duì)于障礙特征較為明顯的場(chǎng)景效果較好。