基于極大化思想的無人機安全避障域識別算法

王家亮 ，董 楷 ,，顧兆軍 ，陳 輝 ,，韓 強 ,

（1.中國民航大學計算機科學與技術學院，天津 300300；2.中國民航大學信息安全測評中心，天津 300300）

隨著無人機技術發(fā)展，四軸飛行器作為典型無人機應用越來越廣泛.四軸飛行器在攝影航拍、農業(yè)植保、軍事偵察、搜索救援、物流運輸等領域有著重要應用價值.小型無人機由于具有成本低、飛行靈活等特點，在交通運輸領域的橋梁道路檢測、輔助交通指揮、交通監(jiān)控中有著較大的應用優(yōu)勢.無人機自主避障是無人機飛行使用過程中最為基礎和關鍵的技術，因此，無人機自主避障技術成為研究熱點.

四軸飛行器主要避障方法可以根據所使用的傳感器進行分類，常見用于避障的傳感器主要有紅外傳感器、激光傳感器、超聲波傳感器和視覺傳感器等，利用上述傳感器獲取四軸飛行器與環(huán)境之間的距離信息.如林立雄等[1]提出一種利用激光傳感器獲取位置信息并改進人工勢場的局部實時避障算法，解決無人機跟蹤目標時避障問題；王梓豪等[2]提出一種基于單目視覺融合多傳感器實現(xiàn)無人機集群穿越障礙的技術，在低精度GPS 條件下，利用視覺定位與激光雷達等驗證在室外穿越障礙的可行性；Wang 等[3]利用激光傳感器測量距離，并采用人工勢場的方法完成飛行器室內避障.

基于視覺傳感器的避障方法主要是結合圖像處理與深度學習等技術處理無人機采集的圖像，計算飛行器與障礙物之間的距離遠近關系，再進行路徑規(guī)劃以躲避障礙物.如運用圖像處理技術中的光流法計算不同障礙物之間的相對遠近關系.張小東等[4]提出基于徑向光流的單目視覺自主實時障礙物檢測方法，解決無人機在室外復雜環(huán)境下，傳統(tǒng)金字塔LK（Lucas-Kanade）光流法檢測障礙物準確性不高、適應性差的問題.付強等[5]提出一種運用稠密光流法，計算視頻幀的光流場，將光流信息作為輸入量，并設計模糊避障控制器的撲翼飛行器避障方法.Huang 等[6]提出一種利用雙目視覺傳感器并基于圖像特征點提取的障礙物測距方法，優(yōu)化雙目視覺測距，并建立距離估計模型.Lin 等[7]提出基于視覺的四軸飛行器自主導航算法，在飛行器飛行過程中利用光流信息檢測障礙物并進行躲避.Dai 等[8]以卷積神經網絡（CNN）作為預測機制，利用無人機單目攝像頭在室內或者室外未知環(huán)境中自主避障.張午陽等[9]基于深度學習，利用目標檢測框選出的目標上下邊距的長度估計障礙物與無人機之間的距離，再根據距離判斷是否進行避障，實現(xiàn)了對人避障.Fu 等[10]提出基于雙目視覺的自動避障方法，采用基于深度學習的單次多框檢測器（SSD）對圖像中的障礙物進行檢測，完成撲翼飛行器避障.關震宇等[11]提出一種基于Dubins 路徑無人機實時避障算法，在已知障礙物情況下進行路徑規(guī)劃，并能夠在未知障礙物信息環(huán)境中完成實時避障.李樾等[12]采用傳統(tǒng)速度障礙法，建立三維機動障礙的模型，設定系統(tǒng)遇障時的通行法則，將三維環(huán)境離散為多個障礙平面，通過障礙平面解決速度障礙法難以處理的三維避障問題.郭行等[13]將動態(tài)障礙威脅、控制約束、終端約束等分解處理，設計初始軌跡、規(guī)避軌跡、優(yōu)化軌跡以使最終飛行軌跡具備近似最優(yōu)特性，并保證動態(tài)避障和滿足各種約束.Huang 等[14]融合雷達與單目攝像頭的數據信息并改進A* 算法，提高了飛行器在農田中的避障能力.Lee等[15]使用Faster R-CNN（faster region-based convolutional neural network）估計障礙物與飛行器之間的距離，解決單目視覺距離信息問題，實現(xiàn)了單目飛行器在室外環(huán)境中避障.Ou 等[16]提出一種基于深度學習的四軸飛行器自主避障算法，采用非監(jiān)督方式從攝像機圖像中提取深度信息，并利用Q 學習提高避障能力.張香竹等[17]提出一種利用單目深度估計和目標檢測的四軸飛行器避障算法，利用單目深度估計模型與目標檢測模型獲得圖像的深度信息與障礙物位置信息進行避障.Wang等[18]利用深度相機與深度學習技術，通過目標識別算法將識別結果映射到深度圖上，提高障礙物識別準確率.

以上文獻通過采用雷達、激光測距儀、雙目攝像頭等傳感器在已知或未知的環(huán)境中識別并躲避障礙物，相比于使用其他傳感器避障，單目視覺傳感器具有結構簡單、價格較低和應用范圍廣的優(yōu)點，本文針對單目四軸飛行器，從求解圖像幀中安全避障域角度解決自主避障問題.角點包含圖像中重要的結構信息，可有效地作為圖像中的特征點或關鍵點，通過葛立恒（Graham）算法遍歷該區(qū)域內的角點，識別障礙物區(qū)域.因此，本文提出基于角點聚類與Graham 算法的障礙物區(qū)域識別方法，采用極大化思想[19]求解安全避障域.在無人機避障飛行時，較為準確地識別安全區(qū)域完成自主避障，提高四軸飛行器自主避障能力.

1 四軸飛行器動力模型

四軸飛行器是一種典型的結構簡單、可垂直起降的多旋翼無人機，從結構上可以分為“ × ”型和“+”型，如圖1 所示.四軸飛行器對角線上旋翼旋轉方向相同，另一組對角線上旋翼旋轉方向相反，相互抵消飛行時旋翼產生的反扭矩力.四軸飛行器通過電機帶動旋翼產生升力進行飛行.本文采用“ × ”型結構四軸飛行器.飛行器機體坐標系原點在飛行器重心，x、y軸分別為四軸飛行器所在水平面的機頭方向、重心指向的左側，用右手定則確定z軸方向.四軸飛行器的飛行姿態(tài)分別用俯仰角θ、滾動角?、偏航角ψ描述.其中，θ為機體軸（沿x軸機頭方向）與地平面（水平面）之間的夾角，飛行器抬頭時，θ為正，否則為負；?為飛行器繞前后軸線（x軸）轉動的角度，機體向右滾時為正，反之為負；ψ為實際航向與計劃航向之間的夾角，向右偏航為正.本文對無人機采集到的圖像進行處理，得到四軸飛行器避障指令，根據指令與無人機動力模型調整俯仰角、滾動角、偏航角，使無人機完成避障.

通過調整4 個電機的速度改變四軸飛行器4 個旋翼產生的升力差，從而改變3 個角度，并控制飛行器起飛、懸停、前進后退和轉彎偏航等動作.飛行器合外氣動力矩為

式中：l為旋翼中心與飛行器中心的距離；μ為阻力系數；Γr為第r個旋翼產生的升力，r=1，2，3，4；ωr為第r個旋翼轉速.

四軸飛行器總升力 Γ、飛行器合外氣動力矩τ和旋翼轉速ωr的關系如式（2）所示.

式中：λ 為常數.

2 基于角點聚類與Graham 掃描的障礙物識別算法

無人機避障過程第一步為環(huán)境感知，即識別障礙物區(qū)域，本文設計的基于角點聚類與Graham 掃描的障礙物識別算法流程如圖2 所示.首先，運用Shi-Tomasi 算法計算圖像幀中的角點；然后，運用LK 光流法計算該角處的光流，根據距離攝像頭近處光流大、距離攝像頭遠處光流小的特點，剔除一些距離較遠的角點，再將剔除后的角點進行聚類；最后，用Graham 掃描算法得到每組點的外包輪廓作為障礙物輪廓.

圖2 障礙物識別流程Fig.2 Flow chart of obstacle recognition

2.1 LK 光流法

LK 光流算法是一種基于兩幀差分的光流估計算法，光流包含著物體的運動信息，反映四軸飛行器與障礙物之間的相對運動.該算法基于3 個假設：1）亮度恒定，即對于相近的兩幀圖像運動目標的灰度值不變；2）時間連續(xù)性，運動目標在幀間對應的像素點位移較小；3）空間一致性，因為相鄰點運動相似，所以對于前后兩幀圖像中相鄰點保持相鄰.根據LK 光流法可以得到點 (x,y) 處光流為

式中：(Ixi,Iyi,Iti) 為圖像第i個像素點在(x,y,t)（點(x,y)在時刻t處）的梯度.

由于無人機避障實時性，應避免在圖像幀每個像素點計算光流花費大量時間，對視頻流中相近的兩幀圖像運用Shi-Tomasi 算法與LK 光流法計算圖像角點處光流，并判斷障礙物的位置和大小.根據距離近的障礙物角點光流較大、距離遠的障礙物角點光流較小的性質，設置合適的閾值f，剔除光流小于f的點，即相對于四軸飛行器較遠的點.圖3 為光流f取不同值時的實驗效果.

圖3 障礙物角點識別效果Fig.3 Effect of obstacle corner point recognition

當光流值f=1 像素時，算法同時識別出近處與遠處障礙物角點，在計算障礙物輪廓時，會導致安全避障域計算錯誤；當光流值f=4 像素時，近處障礙物的角點被識別出來，而遠處障礙物的角點被剔除，效果較好；當光流值f=8 像素時，無法識別出障礙物的角點，四軸飛行器無法避障.因此，根據實驗結果，本文選取閾值f=4 像素.

2.2 基于角點聚類與Graham 掃描的障礙物輪廓

識別算法

首先，根據經過剔除后角點間的距離對角點進行聚類，使得距離相近的角點分為一組，然后通過Graham 掃描算法計算每組角點的外包輪廓，計算出的外包輪廓即為障礙物區(qū)域輪廓.

角點聚類時，首先計算角點之間的距離并設置閾值d，使得角點之間距離小于d的聚為一類；然后，剔除角點個數小于3 的組.

分組后對每組角點使用Graham 掃描算法計算輪廓，具體步驟如下：

步驟1選取圖像幀中最下方的角點作為原點P0.

步驟2計算各個角點相對于P0的幅角，按從小到大的順序對各個點排序，排序為P0、P1、…、Pn.

步驟3P0、P1入棧，將P0作為前點、P1為當前點、P2為判斷點.

步驟4直線L為棧頂的2 個點連線.判斷點如果在直線的右側，執(zhí)行步驟5；如果在直線上或者直線的左側，執(zhí)行步驟6.

步驟5如果棧頂的元素不是障礙物輪廓上的點，則將棧頂元素出棧，執(zhí)行步驟4.

步驟6把判斷點壓入棧，執(zhí)行步驟7.

步驟7如果棧頂元素是Pn，結束掃描；否則，將下個點作判斷點，返回步驟4.

在四軸飛行器前避障區(qū)域（3 m）時，確定最小安全通過區(qū)域在四軸飛行器拍攝圖像中的對應像素位置 (x1,y1) 與 (x2,y2)，根據公式d=(x1,y1)2-(x2,y2)2得到參數d的最小值.通過在不同場景的實驗選取滿足條件的參數d的值，使得不同大小的障礙物（椅子、大小不同的紙箱、垃圾桶、路障等）都得到合理的障礙物輪廓，因此，參數d適用于多個場景.圖4為角點之間距離d取不同值時，用Graham 掃描算法識別出障礙物輪廓的實驗效果，藍色框為系統(tǒng)終端識別出的障礙物區(qū)域，余圖同.

圖4 障礙物輪廓識別效果Fig.4 Recognition effect of obstacle contour

當d=180 時，對于較大的障礙物無法計算出完整輪廓；當d=280 時，識別出近處障礙物完整輪廓；當d=400 時，幾個障礙物被認作為一個而得到錯誤的障礙物輪廓.因此，根據實驗結果，本文選取閾值d=280.

3 四軸飛行器安全避障域分析

為控制四軸飛行器躲避障礙物，需要在采集的圖像幀上計算可通行區(qū)域.將不包含障礙物且面積最大的矩形區(qū)域認為是最優(yōu)通行區(qū)域.通過分析圖像幀計算最優(yōu)通行區(qū)域，然后控制四軸飛行器經過最優(yōu)通行區(qū)域，躲避障礙物.

本文系統(tǒng)結構如圖5，算法運行流程如下：

步驟1系統(tǒng)初始化.系統(tǒng)通過WiFi 與四軸飛行器建立連接，接收四軸飛行器拍攝的視頻流信息，然后讀取圖像幀進行處理.

步驟2基于角點聚類與Graham 掃描的障礙物識別算法.首先，運用LK 光流法計算圖像幀中角點處光流，并根據光流的特征剔除距離四軸飛行器較遠的點，然后對角點聚類，運用Graham 算法找到每組點的外包輪廓.

步驟3基于極大化思想的安全避障域算法.根據障礙物輪廓在已劃分的圖像幀上標記障礙物矩形，根據極大化思想計算最優(yōu)通行區(qū)域.

步驟4基于安全避障域識別的四軸飛行器控制算法.計算最優(yōu)通行區(qū)域中心點與圖像中心點像素偏差，并通過比例-積分（PD）算法得到控制信息，并發(fā)送給四軸飛行器，控制四軸飛行器完成避障.

3.1 基于極大化思想的安全避障域算法

采用極大化思想解決如何在包含障礙物的圖像幀中計算最優(yōu)通行區(qū)域問題.將圖像幀分為3 600 個長、寬像素值分別為16、12 像素的小矩形，并對包含障礙物區(qū)域的小矩形進行標記.劃分小矩形的數量過多，會導致計算量增加，從而影響避障實時性；反之，則會導致障礙物部分區(qū)域未被標記，影響通行區(qū)域的計算.為保證實時性，算法首先遍歷每行小矩形的下邊界，每隔5 像素判斷一次該像素是否在障礙物輪廓內，若在障礙物輪廓內，則標記此小矩形，如圖6（a）中情形1 所示，小矩形只有下邊界有像素點在障礙物輪廓內；再遍歷每列小矩形的右邊界，判斷是否有障礙物穿過，若該矩形未被標記，則每隔5 像素判斷一次該矩形右邊界上像素是否在障礙物輪廓內，若在障礙物輪廓內，則標記此小矩形，如圖6（a）情形2 小矩形只有右邊界有像素在障礙物輪廓內；因為每隔5 像素點判斷一次，且只判斷小矩形右邊界與下邊界，所以可能出現(xiàn)恰好小矩形包含障礙物，卻未標記的情形，如圖6（a）情形3 所示.情形3 中未被標記的小矩形一定是障礙物輪廓邊緣，而且由于劃分的小矩形大小合適，所以并不會因為這部分矩形未被標記而影響計算通行區(qū)域，如圖6（b）中椅子的邊緣并未標記卻并不影響四軸飛行器避障.

圖6 標記障礙物矩形Fig.6 Marked obstacle rectangle

分析圖像幀中可通行區(qū)域，通行區(qū)域邊界有2 種情形：1）通行區(qū)域邊界部分與障礙物邊界重合（圖7（a）情形1）；2）通行區(qū)域邊界與障礙物邊界重合或者圖像幀邊界重合（圖7（a）情形2）；當通行區(qū)域為圖像幀底端時，四軸飛行器距離障礙物還有一段距離，地面可能會被認為是通行區(qū)域（圖7（a）情形3），此時，距離障礙物較遠，四軸飛行器繼續(xù)向前飛行.對于圖像幀中每一個通行區(qū)域都可以通過圖像中一列小矩形左右移動得到，如圖7（b）中以橙色矩形為下端點的一列黃色小矩形，可以通過左右移動得到整個綠色通行區(qū)域.左右能移動的最大范圍由這一列黃色矩形上每個小矩形距離障礙物或邊界的最小值確定.通行區(qū)域面積就是這一列黃色小矩形高度與左右能移動最大距離的乘積.所以，為找到最優(yōu)通行區(qū)域，需要枚舉出圖像幀上所有通行區(qū)域，然后找到其中的最大值.

以圖像幀中劃分的每個矩形分別作為端點，計算不同高度的通行區(qū)域面積，并找到最大值作為包含該點處的最大通行區(qū)域，遍歷整個圖像幀中所有小矩形，得到最大通行區(qū)域.

首先，對圖像幀建立如圖7（b）所示坐標系；然后，計算圖像幀中沒有被標記的任意小矩形(w,h)左、右側到標記矩形或邊界的最遠距離，并分別記為L[w,h]和R[w,h] ；最后，遞歸法算出每個小矩形處的最大通行區(qū)域面積.

左側最遠距離Dl[w,h] 為當前左側最遠距離L[w,h]與這一列小矩形的下一個小矩形左側最遠距離Dl[w,h-1] 的較小值，即

右側最遠距離Dr[w,h] 為當前右側最遠距離R[w,h]與這一列小矩形的下一個小矩形右側最遠距離Dr[w,h-1] 的較小值，即

通行區(qū)域高度H[w,h] 為此列小矩形的個數：

通行區(qū)域面積A[w,h] 為通行區(qū)域高度與通行區(qū)域左右最遠距離和的乘積：

根據得到的最大通行區(qū)域左上角和右下角坐標，畫出通行區(qū)域矩形和通行區(qū)域中心點（如圖8 所示），對應圖中的綠色框和綠色點，余圖同.

圖8 通行區(qū)域分析結果Fig.8 Passing domain analysis result

安全避障域識別算法偽代碼如下：

3.2 基于安全避障域識別的四軸飛行器控制算法

在系統(tǒng)處理完圖像幀找到通行區(qū)域后，采用PD 算法控制無人機飛行躲避障礙物.PD 控制是比例積分微分（PID）控制的一種，是線性控制方法[20].

首先，確定是否有四軸飛行器可通行區(qū)域，如果通行區(qū)域面積小于四軸飛行器可通過最小面積，則向四軸飛行器發(fā)送降落指令；如果有可通過的區(qū)域，則分別計算最優(yōu)通行區(qū)域中心點（綠色中心點）與當前圖像幀的中心點（黑色中心點）的水平像素偏差d1與垂直像素偏差d2，如圖9 所示.根據PD 算法，分別以d1、d2作為偏差值，得到水平控制量u1(t)和垂直控制量u2(t)，再將u1(t)與u2(t)作為飛控算法輸入，控制無人機完成避障飛行.

圖9 PD 控制避障示意Fig.9 Obstacle avoidance by PD control

4 實驗與結果分析

4.1 實驗平臺

采用深圳睿熾科技的Tello 四軸飛行器作為實驗平臺，該飛行器搭載大疆公司的飛控技術和英特爾Movidius 芯片，并配有500 萬像素的攝像頭.電腦終端配置為Intel（R）Core（TM）i5-7300HQ 處理器，顯卡為NVIDIA GeForce 1050Ti，網卡為Intel Dual Band Wireless-AC 3168.

4.2 實驗結果分析

為驗證算法是否有效，用紙箱、椅子和垃圾桶作為障礙物，通過在室內與室外布置的3 個實驗場景驗證算法的避障效果，如圖10 所示.實驗1 為多組障礙物障礙實驗，實驗2、3 為復雜程度不同的復雜障礙物避障實驗.

圖10 實驗場景Fig.10 Experimental scenes

設定四軸飛行器的前進速度為0.42 m/s，單目相機采集頻率為30 幀/s，算法執(zhí)行周期為1 s.實驗場景尺寸為：箱子53 cm × 32 cm × 56 cm、路障65 cm ×27 cm × 27 cm、書包45 cm × 30 cm × 12 cm、椅子85 cm × 50 cm × 43 cm.

4.2.1 多組障礙物避障實驗（實驗1）

按照圖10（a）所示場景進行避障測試，四軸飛行器起飛后向前飛行，飛行過程中在t11、t12、t13時刻依次躲避3 組障礙物，實驗結果如圖11 所示.

圖11 躲避多組障礙物實驗（實驗1）Fig.11 Experiment of avoiding multiple groups of obstacles (experiment 1)

對四軸飛行器飛行過程中記錄的俯仰角和滾動角數據信息進行分析，如圖12 所示.實驗過程中，系統(tǒng)終端通過處理四軸飛行器所拍攝的圖像得到控制指令，再將控制指令發(fā)送給四軸飛行器.四軸飛行器在俯仰角 θ ＜0 的范圍內波動（θ ＜0 表示四軸飛行器向前飛行）.在第0～16 幀，滾動角 ?=0，此時，四軸飛行器距離障礙物較遠，所以，滾動角并未改變；在第17～40 幀，? ＜0，表示四軸飛行器向左移動躲避第1 組障礙物；在第41～57 幀，? ＞0，表示四軸飛行器向右飛行躲避第2 組障礙物；第58～71 幀內，四軸飛行器 ? ＜0，表示四軸飛行器向左飛行躲避第3 組障礙物.由于起飛時四軸飛行器距離第1 組障礙物較遠，所以滾動角變化范圍較小.3 組障礙物之間距離較小，所以，滾動角變化范圍較大.

圖12 四軸飛行器數據結果（實驗1）Fig.12 Quad-rotor helicopter data results (experiment 1)

4.2.2 復雜障礙物避障實驗（實驗2、3）

按照圖11（b）、（c）所示場景進行復雜障礙物避障實驗，結果分別如圖13、14 所示.

圖13 復雜障礙物避障實驗（實驗2）Fig.13 Experiment of avoiding complex obstacles (experiment 2)

圖14 復雜障礙物避障實驗（實驗3）Fig.14 Experiment of avoiding complex obstacles (experiment 3)

為檢驗本文算法的避障準確性和實時性，將安全避障域面積與計算時間作為評價指標，與改進的基于貝葉斯估計與區(qū)域劃分遍歷[21]的避障路徑規(guī)劃算法（BR 算法）進行對比分析.本文算法與BR 算法計算時間對比如圖15 所示.在處理每幀圖像平均用時上，前者0.17 s，后者0.13 s 左右，本文算法雖然處理圖像時間增加，但仍滿足無人機避障實時性要求.將通行區(qū)域所包含像素個數作為通行區(qū)域面積評價指標，2 種算法對比結果如圖16 所示.在第15～58 幀、第64～168 幀，本文算法求解出的避障域比BR 算法更準確；在其余幀，2 種算法求解避障域面積相同，則只顯示本文算法面積.在障礙物識別時，BR 算法使用貝葉斯估計與背景差分法，本文算法采用基于角點聚類與Graham 掃描的障礙物識別算法；在計算安全避障域時，BR 算法雖然找到無人機安全通行區(qū)域，但并不一定是最優(yōu)安全避障域.本文算法基于極大化思想，在計算最優(yōu)安全避障域的同時，兼顧準確性和實時性.

圖15 處理時間對比Fig.15 Comparison of processing time

圖16 通行區(qū)域面積對比Fig.16 Comparison of passing domain area

5 總結

本文針對單目四軸飛行器自主避障中障礙物識別與計算安全通行區(qū)域問題，分別提出基于角點聚類與Graham 掃描的障礙物識別算法和基于極大化思想的安全避障域算法.在Tello 四軸飛行器上對安全避障域識別算法進行測試實驗，結果表明，算法能夠合理識別障礙物并使四軸飛行器完成避障，同時算法滿足準確性與實時性要求.下一步將使用機載計算機平臺或融合其他傳感器進行相關研究.