郭植星，曾 碧，劉建圻，陳文軒，王俊豐

廣東工業(yè)大學 計算機學院，廣州 510006

自主機器人進行作業(yè)首先要具備避障導航規(guī)劃的能力。目前基于不同的傳感器及其組合，如激光雷達、里程計、IMU、聲納、單目相機、雙目相機和RGBD相機等，已經(jīng)有多種避障導航方案。但基于激光的導航避障方案存在激光雷達信息少、維度單一等問題，特別是救援場景等其他需要對語義分析的場景中，純激光SLAM很難完成任務。多傳感器組合的方案在機器人導航避障方面逐漸興起，雖然組合不同類型的傳感器[1-3]可以獲得更加詳盡的信息和準確的決策，但也增加了成本并提高了算法的復雜性。

相機的優(yōu)點是便宜、容易安裝、體積小、信息豐富。對于微型飛行器（MAVs）或者小型汽車，由于體積小，載重能力弱，電池容量有限，只能攜帶重量輕的傳感器，如單目攝像頭，這可避免對電池壽命和自身移動造成過大影響，也能大大降低移動任務難度。對于自主機器人運動，移動機器人如何判斷其相對于潛在障礙的位置是至關重要的?；趩文恳曈X的方法不需要特殊的攝像頭配置，更適合各種低成本的機器人應用。本文主要側重于解決單目視覺下的實時導航規(guī)劃問題。

1 相關工作

只利用相機作為外部感知傳感器的SLAM稱為視覺SLAM（VSLAM），常見的視覺SLAM算法有ORBSLAM[4]、DSO-SLAM[5]或LSD-SLAM[6]等。ORB-SLAM穩(wěn)定性強、魯棒性高，DSO-SLAM生成的特征點更加稠密，有助于計算，LSD-SLAM在紋理比較弱的地方工作性能更好?；谥苯臃ǖ腖SD-SLAM對光照和動態(tài)干擾比較敏感，因此精度一般會劣于ORB-SLAM。LSDSLAM對純旋轉(zhuǎn)的魯棒性很大程度上取決于后端優(yōu)化中地圖的優(yōu)化效率，能否獲得準確的地圖信息是后續(xù)幀能否穩(wěn)定跟蹤的關鍵。ORB-SLAM不需要恢復稠密深度，計算效率高，所以在近似純旋轉(zhuǎn)場景中的魯棒性要比LSD-SLAM更好[7]。由于直接法需要比較兩幀之間的圖像信息，其結果容易收到外界光照干擾。DSO提出了光度標定，通過對相機曝光時間、暗角、伽馬響應等參數(shù)進行標定而使直接法更加魯棒，只要相機運動不快，模糊不明顯，DSO能順利完成工作，但如果出現(xiàn)明顯的模糊或者失真，DSO也會丟失。DSO對場景光照要求高，要求盡可能保持曝光時間的穩(wěn)定。

純視覺避障方案最大的缺陷是缺乏深度信息，只有RGB圖像很難對障礙物位置進行有效判斷。近年來，深度學習技術逐漸興起和完善，夏夢琪等[8]提出一種全卷積編碼-解碼網(wǎng)絡模型，使圖像高層信息更有效地在網(wǎng)絡中傳遞，在保證精度的同時提升了精度。Eigen等[9-10]使用深度學習進行單目深度估計，估計障礙物位置并進行避障；Wang等[11]引入了人類視覺系統(tǒng)估計深度的要素之一——運動，依此提出了一種基于深度學習的多視點稠密深度圖和里程計估計方法，采用遞歸神經(jīng)網(wǎng)絡（RNN），利用多視點圖像重投影和前后向流一致性損失進行訓練并得到預測深度圖。Riegler等[12]利用立體成像技術來改進單目深度估計，提出了一種從單個輸入圖像推斷深度的新型深度框架——monoResMatch，其原理是將單目深度估計轉(zhuǎn)化成立體匹配問題，再用立體匹配網(wǎng)絡進行視差估計。另一種思路是直接求解空間中的空閑區(qū)域，以此作為機器人的可通行區(qū)域進行導航規(guī)劃。Liang等[13]利用倒極（reciprocal-polar，RP）圖像校正和RP空間中的正弦模型擬合進行地平面分割從而得到機器人可移動的地平面區(qū)域。Zhou等[14]從圖像序列中提取連續(xù)兩幀進行單應性計算，并由此得到地平面區(qū)域。Jin等[15]通過固定相機高度和約束水平旋轉(zhuǎn)以加強單應性約束，再使用該約束進行地平面檢測。

鄭爭光等[16]通過對圖像進行自適應閾值分割，獲取障礙物和地面的邊界輪廓后進行障礙物判斷。Lenser等[17]采用低分辨率顏色分割和目標檢測（訓練為8個目標類）進行單目避障。針對室內(nèi)環(huán)境，Ohya等[18]中提出了一個導航框架，使用單幀圖像檢測靜止目標和超聲波傳感檢測運動目標，使用當前和期望圖像之間的差異來區(qū)分動靜態(tài)障礙物。Lin等[19]結合特征匹配和自適應顏色分割算法進行平面估計，經(jīng)過IPT和地面區(qū)域分割獲得可通行的柵格地圖。Souhila等[20]從圖像序列中提取光流，光流提供了機器人環(huán)境的相關信息，如障礙物布置、機器人航向、碰撞時間和深度等，該策略通過平衡左右側的光流量來避開障礙物。Cherubini等[21]用一系列有序圖像幀表示待前進路徑，將導航任務切割為多個前往下一個關鍵幀的子任務。此類方法確實能實現(xiàn)視覺導航的任務[22]，但卻忽略了避障功能的實現(xiàn)，Lamiraux等[23]對此部分進行了補充和完善。但這類方法都需要預先構建一部分環(huán)境模型，如門、窗或者墻等。以上方法通過劃分可通行區(qū)域和障礙物區(qū)域以使機器人在不同的空間區(qū)域完成實時導航規(guī)劃任務。

針對機器人導航任務，另一種解決方案是利用場景信息分析得到運動決策。文獻[24-25]引入深度強化學習方法，Zhu等[24]采用端到端的方式解決導航問題，通過對同一場景下的兩幅輸入圖像的比較，得出當前state想要到達target需要執(zhí)行什么動作。Yang等[25]將先驗場景信息進行編碼，加入到端到端的視覺導航中，可以完成去找杯子、去廚房等的語義導航能力?；谙闰炐畔⒌膶Ш讲粌H可以在已知場景中找到目標，還可以在未知場景中進行導航規(guī)劃。李鵬等[26]利用分布式環(huán)境的不同特征，對區(qū)域?qū)Ш阶幽K進行強化學習訓練，實現(xiàn)針對不同區(qū)域的獨立控制策略。

上述導航避障方案中，每種傳感器都有其優(yōu)點和不足：超聲波傳感器抗環(huán)境干擾能力強，但會受環(huán)境溫度影響，小物體難以反射足夠的聲波；激光雷達實時性強、信息準確，但價格昂貴、信息維度單一；視覺相機價格低廉，信息豐富，但缺乏直接的距離信息?；谏疃葘W習的視覺深度估計算法或者基于強化學習的導航避障算法在指定場景能出色地完成任務，但在隨機場景中魯棒性較低。為了實現(xiàn)三維空間下低成本且魯棒性高的機器人導航系統(tǒng)，受文獻[14-15，19]的啟發(fā)，本文提出了一種基于單目相機的可通行區(qū)域檢測方法（accessible space detection method based on monocularcamera，ASDM-Mono），僅通過單目視覺特征點地圖信息即可計算空間中各點的可通行代價。首先利用單目SLAM特征點法計算得到3D特征點地圖；然后在特征點地圖的基礎上，提出了一種能精確計算前方空間與其鄰近特征點簇最小距離的障礙物距離求解器；為了得到機器人導航所需的視覺代價地圖和可通行區(qū)域，設計了一個代價求解器；固定相機高度結合可通行區(qū)域的方法可以求解得到單目視覺中的尺度因子以解決單目SLAM中尺度不確定的問題。最后經(jīng)過反投影變換獲得可通行方向。該方法僅需低成本的單目相機傳感器便可實現(xiàn)可通行區(qū)域的檢測任務。此外，該方法計算得到了視覺代價地圖和可通行方向，有利于后續(xù)小車的路徑規(guī)劃模塊，更易落地，實用價值高。

2 基于單目視覺的可通行區(qū)域檢測

2.1 單目可通行區(qū)域檢測框架及原理

本文提出的ASDM-Mono方法旨在實現(xiàn)非結構化環(huán)境下魯棒性高的單目視覺導航方案，提供一種基于單目視覺特征點地圖求解可通行區(qū)域的方法。為了追求旋轉(zhuǎn)場景的魯棒性及降低室內(nèi)場景的光照依賴性，特征點法選取ORB-SLAM。

假定相機高度固定并且約束相機只在水平面上旋轉(zhuǎn)，ASDM-Mono算法實現(xiàn)步驟如下：

步驟1通過特征提取和匹配、PnP算法計算特征點集

步驟2分割前方區(qū)域空間S為m個細小子區(qū)域利用KD-Tree查找每個子區(qū)域sj附近k個鄰居特征點N=( )P1,P2,…,Pk,N?D，使用障礙物距離求解器求解得到子區(qū)域sj的最近障礙物距離dsj。

步驟3將障礙物距離信息dsj,sj∈S代入代價求解器求得視覺代價地圖Mcost，并結合預先設定好的代價閾值σ對Mcost進行劃分得到可通行區(qū)域。

步驟4利用可通行區(qū)域和隨機采樣一致算法（random sample consensus，RANSAC）過濾得到地面點集Dplane?D并進行尺度因子ρ求解。

步驟5根據(jù)尺度因子ρ更正可通行區(qū)域尺度，通過投影變換獲得可通行方向并結合機器人運動規(guī)劃器進行導航規(guī)劃。

圖1展示了ASDM-Mono算法的整體框架，主要包括特征提取、可通行區(qū)域檢測、尺度因子求解及障礙物檢測等四大部分。下面將對其中的具體細節(jié)進行詳細的闡述。

圖1 ASDM-Mono算法整體流程框圖Fig.1 Overall flow diagram of ASDM-Mono

2.2 障礙物距離求解器

ASDM-Mono使用特征點標識障礙物，利用KD-Tree收集已劃分區(qū)域sj的鄰近特征點集N，通過統(tǒng)計所有子區(qū)域sj到臨近點的距離來獲取其對應最近障礙物距離dsj。直接使用平均距離進行距離求解并不能很好表示不同位置之間的差異及準確表達最近障礙物距離，而采用高斯核密度估計獲取特征點的分布情況能更好地辨識最近障礙物。高斯核密度估計的分布密度函數(shù)fh(x)計算如公式（1）所示：

其中，f(x)表示概率密度函數(shù)；K(x)表示核函數(shù)，這里選取高斯核函數(shù)；h表示帶寬，本文取0.5；k表示收集到的臨近點數(shù)量；待計算區(qū)域sj的最近障礙物距離dsj計算公式如式（5）所示：

其中，peaki(f)表示獲取f(x)密度分布中第i個波峰值；di表示待計算區(qū)域sj與第i個臨近點Pi的距離；dkde表示利用高斯核密度估計估算的距離；dave表示特征點集平均距離；α表示權重因子。

圖2中（a）展示了待計算區(qū)域sj附近特征點的距離信息，（b）表示使用高斯核密度估計得到的分布情況，橫坐標表示臨近點的距離x，縱坐標表示該臨近點根據(jù)高斯核密度估計計算得到的概率密度f()x。由圖2（b）可以看出，該區(qū)域附近有兩個特征點簇x1、x2，分別表示兩個障礙物O1、O2，希望得到sj最近障礙物的距離信息，故取第一個波峰f(x1)的距離作為的最近障礙物距離dsj，此做法可以實現(xiàn)簡單的聚類功能，實現(xiàn)了對不同方位障礙物的區(qū)分處理。圖2中選取x1=0.3作為當前場景下的最近障礙物距離值。

圖2 障礙物距離計算示意圖Fig.2 Obstacles distance calculation

以下通過一個簡單的模擬實驗進行障礙物求解器測試。

圖3中顏色深淺表示特征點分布密度，顏色越深表示特征點數(shù)量越密集。實驗場景是模擬最常見的直線道路區(qū)域，道路總寬度為2 m，實驗目的是求得紅色塊區(qū)域（小車）離最近障礙物的距離。

圖3道路模擬實驗示意圖Fig.3 Road simulation experiment

圖4中（a）表示使用公式（3）計算得到的障礙物距離值，（b）表示使用公式（5）計算得到的障礙物距離值，（c）表示臨近特征點平均距離，（d）為臨近特征點距離標準差。

圖4 道路模擬實驗數(shù)據(jù)分析Fig.4 Analysis of road simulation experiment

如圖4（a）和（d）所示，道路中間（即水平偏移量為0處）數(shù)據(jù)方差小，數(shù)值較接近，取首波峰數(shù)據(jù)作為距離量會有較大波動和誤差，直接使用首波峰距離dkde或平均距離dave均不能很好地滿足道路中間小車距障礙物距離大，道路兩側距障礙物距離小的原則，該方法將dkde和dave相結合，當方差較小時增大對應權重，最終求得的結果如圖4（b）所示，能很好地解決了這個問題。

2.3 代價求解器

以上步驟已經(jīng)得到了前方區(qū)域的最近障礙物距離信息，接下來需要對前方區(qū)域進行視覺代價地圖求解，為了降低機器人撞擊障礙物的概率，方便后續(xù)機器人進行路徑規(guī)劃，設計了一個代價求解器，根據(jù)最近障礙物距離信息進行代價分配，且距障礙物較近區(qū)域代價值應遠高于較遠區(qū)域代價值。

基于特征點進行代價求解存在兩個問題，一是由于障礙物背部的特征點信息缺失，如果直接進行代價計算，該方法會將障礙物背部視為空閑區(qū)域，針對該問題，代價求解器中引入了代價傳播機制；二是空閑區(qū)域中心位置附近特征點數(shù)量較少，此時計算得到的距離信息可能會有較大誤差，如果不加以修正，會出現(xiàn)中心空閑區(qū)域代價值更高的情況，這是不合邏輯的，盡管該問題可以通過增大KD-Tree搜索范圍解決，但此舉會增加時間復雜度，降低實時性。為保證實時性和避免臨近特征點數(shù)量過少帶來過大誤差，代價求解中引入一個臨近特征點數(shù)量臨界值n*，若臨近點數(shù)量n過小，即n

經(jīng)過測試，代價求解器設計如公式（6）所示：

其中，C表示待計算區(qū)域代價值；d*表示影響半徑，本文設置為1.0；n*表示臨近點數(shù)量臨界值，本文設置為100；η*表示代價尺度因子，本文設置為0.1。

為了避免因障礙物背面檢測不到特征點而導致可通行區(qū)域計算出現(xiàn)錯誤的情況，代價求解過程中引入代價傳播機制，原理如算法1所示：

算法1 Cost propagate mechanism

Input c：cost to be calculated，c*：cost threshold，

Ocam：camera position and s：subspace ahead

Output Optimized cost c'

1.for each ci(1≤i≤m)do

2. if ci>c*then

3. P←Bresenham(Ocam,si)；

4. for each p in P do

5. Propagate cito cp；

6. end for

7. end if

8.end for

從相機位置（原點）向障礙物潛在區(qū)域（代價值較大區(qū)域）發(fā)射射線，將障礙物代價值向后方區(qū)域傳播，效果如圖5所示。

圖5 代價傳播示意圖Fig.5 Cost propagation

2.4 尺度因子求解器

通過對極幾何求解相機運動時，由于本質(zhì)矩陣具有尺度等價性，導致分解得到的平移矩陣具有一個未知的尺度，常見做法是對該平移矩陣進行歸一化處理，但歸一化后的尺度導致了單目視覺的尺度不確定性。

尺度因子求解的前提是相機高度固定并且只進行水平面旋轉(zhuǎn)。首先使用前面得到的可通行區(qū)域和RANSAC對特征點進行過濾，得到地面點并算出歸一化尺度下的地面高度，即圖6中的h'，然后利用預設好的相機高度h計算尺度因子ρ=h/h'，最后利用ρ進行障礙物深度估計和特征點真實尺寸恢復。

圖6 尺度因子求解示意圖Fig.6 Calculation of scale factor

3 實驗

為了驗證ASDM-Mono的可行性與實用性，在KITTI數(shù)據(jù)集和現(xiàn)實場景中分別進行了實驗測試，KITTI數(shù)據(jù)集圖像分辨率為1 241×376，實驗采用ORB-SLAM3進行特征點數(shù)據(jù)采集。實驗使用搭載Ubuntu 18.04和ROS系統(tǒng)的電腦，處理器（CPU）為Intel?CoreTMi7-6700 CPU@3.40 GHz 3.41 GHz，內(nèi)存為8 GB。顯卡（GPU）為AMD Radeon R7 350 Series，顯存為2 GB。

3.1 障礙物距離求解器實驗

下面將對障礙物距離求解器進行實驗，主要目的是對比引入障礙物距離求解器前后的差異。圖7中（a）是實驗場景原圖，（b）是使用平均距離即公式（4）計算得到的視野范圍內(nèi)視覺代價地圖，（c）是使用該方法距離求解器即公式（5）計算得到的視覺代價地圖。圖7（b）視覺代價地圖中部出現(xiàn)空閑區(qū)域中心代價值更小的情況，這是因為中心區(qū)域臨近特征點數(shù)量少、距離方差小，該情況會對可通行方向的判斷和可通行區(qū)域的計算帶來較大干擾，為了解決該問題，引入臨近點數(shù)量臨界值，同時結合平均距離和首波峰距離進行優(yōu)化計算。優(yōu)化后的效果如圖7（c）所示，可以看到，空閑區(qū)域中心同附近障礙物之間的過渡符合預期，更能滿足機器人在實際場景中進行導航規(guī)劃的需求。

圖7 障礙物距離實驗效果圖Fig.7 Experiment of obstacles distance calculation

3.2 代價傳播機制實驗

本節(jié)實驗主要測試代價求解器中的代價傳播機制。圖8中（a）是實驗場景原圖，（b）是使用該方法障礙物距離求解器但未引入代價傳播機制的視覺代價地圖，（c）是使用該方法障礙物距離求解器且引入代價傳播機制優(yōu)化后的視覺代價地圖。圖8（b）中可以看出，未引入代價傳播機制的視覺代價地圖中，由于無法獲取障礙物背部信息，基于特征點標識障礙物的方法會將特征點背部視為空閑空間，這不利于機器人的路徑規(guī)劃。而代價傳播機制能適當增加未知區(qū)域的代價值而不是單純地將其視為空閑區(qū)域，效果如圖8（c）所示。

圖8 代價傳播實驗效果圖Fig.8 Experiment of cost propagation

3.3 尺度因子求解器實驗

利用可通行區(qū)域過濾得到的地面點結合相機的固定高度可進行單目SLAM的尺度因子求解。本節(jié)實驗將對尺度因子求解器進行障礙物距離測試。實驗選用Kinect-v1.0相機進行靜態(tài)障礙物距離估計實驗，Kinect相機采集得到的深度圖像同實際測量距離作為實驗GroundTruth，Kinect相機RGB圖像作為實驗的單目圖像輸入。實驗結果如表1所示，相機被控制在距離障礙物0.5 m至1.5 m之間，障礙物距離估計受地面點單應性和數(shù)量影響，誤差范圍在2.5%至4.9%之間。

表1 障礙物深度實驗數(shù)據(jù)Table 1 Analysis of ablation experiments

圖9、圖10中，（a）是實驗環(huán)境圖像，（b）、（c）、（d）分別表示相機坐標下特征點全局圖、俯視圖和側視圖。圖中綠色部分是Kinect相機得到的深度圖像。實驗環(huán)境中相機距離障礙物距離為0.8 m，矯正前檢測深度為1.03 m，矯正后檢測深度為0.82 m。可以看出，如果不經(jīng)過任何處理，單目SLAM得到的特征點具有尺度不確定性的問題，此時所測得的深度信息具有較大的誤差，而經(jīng)過該方法尺度因子求解優(yōu)化后誤差由28.75%降至2.5%，具有較大提升。

圖9 尺度因子求解器矯正前示意圖Fig.9 Experiment of scale-factor solver（before correction）

圖10 尺度因子求解器矯正后示意圖Fig.10 Experiment of scale-factor solver（after correction）

3.4 ASDM-Mono實時性實驗

可通行區(qū)域檢測實時性實驗中，主要測試首波峰距離和代價傳播機制對實時性的影響。該方法所需時間如表2所示，當采用平均距離和代價傳播模塊時，每幀圖像所需時間為0.050 s，視覺代價地圖生成頻率為20 Hz。當繼續(xù)引入首波峰距離進行距離優(yōu)化時，時間有所增加，為每幀0.073 s，視覺代價地圖生成頻率為14 Hz。如果希望實現(xiàn)視覺實時避障功能時，采取前者的方案，而如果希望構建一張精準的代價地圖以供以后導航使用時，則選取后者，兩套方案在不同的需求下都滿足了SLAM中實時性要求。

表2 ASDM-Mono算法所需時間Table 2 Time required for ASDM-Mono algorithm

3.5 非結構化場景可通行區(qū)域檢測實驗

相比于結構化環(huán)境，非結構化環(huán)境增加了更多的復雜性和多樣性，并且道路特征不固定，較難直接從周圍環(huán)境中獲取信息。為了檢測文章方法在非結構場景下的實驗效果，本節(jié)進行了可通行區(qū)域的覆蓋率和錯檢率實驗。實驗中，激光雷達獲取到的邊界信息加上膨脹層作為可通行區(qū)域GroundTruth，比較該方法得到的可通行區(qū)域和激光柵格地圖空閑區(qū)域占比獲得覆蓋率數(shù)據(jù)，超出激光邊界的可通行區(qū)域視為錯檢，實驗對比示意圖如圖11所示，圖11（d）中紅色框表示錯檢區(qū)域，藍色框為激光雷達誤檢區(qū)域，將進行手動糾正。

圖11 非結構化場景實驗對比圖Fig.11 Unstructured environment experimental comparison

實驗過程中，小車間隔1 m提取一次可通行區(qū)域進行比對，統(tǒng)計得到的可通行區(qū)域覆蓋率和錯檢率分別如圖12、13所示。覆蓋率實驗數(shù)據(jù)表明，覆蓋率在70%~80%、80%~90%、90%~100%內(nèi)的樣本占采樣總數(shù)的比例分別為12.4%、32.4%、40.9%，平均覆蓋率為88.39%。錯檢率實驗數(shù)據(jù)表明，錯檢率在區(qū)間0%~10%、10%~20%、20%~30%內(nèi)的比例分別為70.1%、17.7%、7%，平均錯檢率為5.03%。

圖12 可通行區(qū)域覆蓋率實驗示意圖Fig.12 Experiment of accessible space coverage

3.6 可通行區(qū)域檢測綜合實驗

綜合以上障礙物距離求解器、代價傳播機制和尺度因子求解器，該算法使用特征點標識障礙物并將代價值較低區(qū)域標記為可通行區(qū)域，最后將計算得到的代價地圖進行投影變換便可得到二維圖像中的可通行方向，最終選取平均距離+首波峰距離+代價傳播的方案進行綜合實驗測試，KITTI數(shù)據(jù)集上測試的實驗效果如圖14所示，現(xiàn)實場景中實驗效果如圖15所示。

圖13 可通行區(qū)域錯檢率實驗示意圖Fig.13 Experiment of accessible space error detection

圖14 KITTI數(shù)據(jù)集可通行區(qū)域檢測效果圖Fig.14 Results of accessible space detection with KITTI dataset

圖15 現(xiàn)實場景可通行區(qū)域檢測效果圖Fig.15 Results of accessible space detection in real scene

4 總結

通過將單目視覺圖像中提取的特征點作為障礙物標記，研發(fā)了一套基于單目特征點法的純視覺導航算法——ASDM-Mono，只需單目相機便可構建視覺代價地圖并獲得可通行區(qū)域和可通行方向，可用于實現(xiàn)三維避障功能，彌補了激光雷達維度信息缺失、價格昂貴以及深度強化學習方案訓練成本高的缺點。該方法可準確計算得到機器人前方區(qū)域各個位置的通行代價，最終得到的可通行區(qū)域可用于障礙物判別，視覺代價地圖可用于路徑規(guī)劃。在未來的實驗中，可考慮將該方法移植到移動端和無人機上，擴展為未知環(huán)境中的定點探索任務，并且添加動態(tài)障礙物的避障功能。