基于路徑像素分布統(tǒng)計導引的無人機自主循跡

馮宇，崔峰,2，高東,2

（1.國家空間科學中心，北京100000；2.中國科學院大學，北京100000）

0 引言

隨著計算機視覺技術的快速發(fā)展，基于低成本視覺傳感器的視覺導航方法在無人機導航和路徑規(guī)劃領域顯示出極大的優(yōu)勢，被廣泛研究和應用，尤其應用于全球定位系統(tǒng)（global positioning system，GPS）信號弱，甚至信號被阻斷的場景中。文章研究在無路徑先驗信息的情況下，無人機基于視覺傳感器沿指定路徑進行循跡飛行的路徑規(guī)劃算法設計。循跡飛行的前提是識別圖像路徑。文章［1］提出根據高斯分布模型的自適應閾值分割方法，使用了基于形態(tài)學變換的二值圖優(yōu)化算法得到車道線的邊緣圖，改進概率霍夫變換，有效地檢測出車道路。文章［2］主要針對自動駕駛過程中路徑類型預判較慢且準確度不高的問題，提出了一種圖像邊沿特征提取算法。文章［3］基于改進的自適應遺傳分割算法對圖像進行閾值分割，使用霍夫變換的圖像處理方法，并通過逆透視變換矯正路徑的畸變，有效地提高了路徑識別的精確度。

傳統(tǒng)的路徑規(guī)劃算法，如A*算法［4］、人工勢場法［5］等，需要提前載入環(huán)境信息。智能算法如遺傳算法［6］、粒子群算法［7］等實時采集環(huán)境信息，根據載體在環(huán)境中的狀態(tài)進行路徑規(guī)劃。這些算法用于在復雜環(huán)境中尋找最優(yōu)路徑來避開障礙，與循跡飛行的應用場景和目的略有不同。

循跡飛行的難點之一是拐角處的路徑規(guī)劃，檢測出路徑上的拐角是基于視覺信息進行導引的難點之一。關鍵拐角檢測法之一是基于模板的角點檢測，如Harris角點檢測算法［8］、KLT角點檢測算法［9］等。以上算法可以檢測出所有符合條件的拐角，但具體拐角的結構分布還需要進一步分析。文章［10］建立了基于機載DSP對采集的圖像進行連通域分割提取目標，進行結構矩匹配路徑模板，進而解算位置偏差和運動方向指令的系統(tǒng)。

目前，基于視覺的無人機自主循跡路徑規(guī)劃算法的不足是，對于不同形狀的拐角沒有統(tǒng)一的直接選取目標位置的方法。受A*算法中環(huán)境建模的思想啟發(fā)，對提取出路徑的圖像進行柵格化處理，并統(tǒng)計各柵格中路徑像素和，從統(tǒng)計結果中分析出路徑在圖像中的分布信息，并確定局部路徑規(guī)劃中的目標位置坐標。根據無人機當前位置、姿態(tài)和目標點坐標信息，確定無人機的機動位置和轉向指令。設置包含不同形狀的路徑環(huán)境，使用無人機進行算法測試。飛行結果表明，設計的路徑規(guī)劃算法對不同形狀路徑有適用性，能夠滿足無人機自主循跡飛行的需求。

1 基于視覺導引的無人機飛行控制

在基于視覺信息導引飛行的問題中，慣導器件等傳感器獲取無人機的位姿信息，視覺信息用于解算無人機與指定路徑的相對位置信息，并根據該信息設計無人機的飛行路徑。

圖像中路徑識別過程去除噪聲和背景干擾，明確路徑區(qū)域對應的坐標；提取路徑分布信息是將圖像中路徑幾何信息轉換為數字信息，為路徑規(guī)劃提供輸入；路徑規(guī)劃是確定飛行目標航跡點。

路徑規(guī)劃的結果是得到路徑的點序列，將路徑點序列坐標信息轉換到導航坐標系下。定義導航坐標系為北東地（NED），原點定義為起飛前無人機重心位置。無人機本體系定義如圖1所示，以重心為原點，oxb軸正方向沿機體縱軸指向機頭，ozb軸正方向指向機體正下方。ozb、oxb和oyb軸構成右手法則［11-12］。

圖1 無人機本體坐標系

多旋翼底層飛行控制分為四個層次，分別為位置控制、姿態(tài)控制、控制分配和電機控制。X型四旋翼的動力學方程為：

其中，P為無人機位置向量；V為無人機速度矢量；e為導航坐標系D軸正方向的單位向量；R是由無人機本體系到導航系的旋轉矩陣；u是無人機旋翼產生的升力；Θ表示無人機的姿態(tài)角；W是機體姿態(tài)角變化率與機體角速度之間的轉換矩陣；ω表示無人機三軸角速度；J表示無人機的轉動慣量；Gτ表示陀螺力矩；τ表示三軸力矩。各向量分量如下式定義：

在滾轉角和俯仰角較小的飛行姿態(tài)下，忽略高階項，對動力學方程進行線性化，得到解耦的動力學方程。

水平位置動力學方程：

其中，φ，θ和ψ分別表示無人機的滾轉角、俯仰角和偏航角。

高度動力學方程：

姿態(tài)動力學方程:

多旋翼飛行器是欠驅動系統(tǒng)［13］，有4個輸入（u∈?,τ∈?3），6個輸出（P∈?3,Θ∈?3），因此多旋翼只能跟4個指令（Pd∈?3,ψd∈?），剩余的指令（φd∈?,θd∈?）由期望指令Pd和ψd確定。因此，無人機機動的關鍵在于給出期望位置和期望偏航角，即從路徑點坐標信息轉換為控制指令。

圖2 無人機級聯控制

2 自主循跡算法設計

2.1 路徑信息提取

無人機循跡飛行的前提是能夠辨認出目標路徑，視覺傳感器采集的信息包含目標路徑及背景干擾信息，在路徑規(guī)劃前需要從采集的原始圖像中提取不含干擾的路徑信息。

視覺傳感器輸出RGB格式的圖像，基于像素統(tǒng)計的算法，關鍵在于將路徑作為圖像前景，使其和背景顯式區(qū)分開，最優(yōu)選擇是將背景像素置為0，即轉化為二值圖像。

一般視覺傳感器的主要噪聲是椒鹽噪聲，采用效率較高的中值濾波來去除椒鹽噪聲［14］。對于一副灰度圖像g(x,y)，令Sxy為中心在點(x,y)處，大小為m×n的矩形子圖像窗口的中值濾波器，使用一個像素鄰域中灰度的中值代替該像素的值，即：

采集的圖像經過灰度化和中值濾波處理后，只含有較少的噪聲。隨后使用灰度閾值進行二值處理［15］，在圖像中只顯示主路徑區(qū)域。

測試環(huán)境中主路徑為紅色，背景為綠色，如圖3所示。對應的灰度圖，背景的灰度大于路徑的灰度，二值化結果為路徑為黑色區(qū)域，背景為白色區(qū)域。為達到統(tǒng)計目標，對二值圖進行補圖處理。

圖3 路徑整體圖和初步處理

在路徑兩側加入干擾條紋保證視覺傳感器的光流計算起作用，在二值后統(tǒng)計像素時是一大干擾，嚴重時會丟失有用路徑信息，如圖4所示。

圖4 包含干擾的路徑圖及普通二值處理

去除干擾項的一種方法為直接提取主路徑，根據實時光照條件下采集的圖像像素點三通道矩陣的元素數值確定圖像中主路徑區(qū)域的RGB三通道閾值，設置同時滿足3個閾值條件的像素點為前景，其余像素點為背景。對一幅RGB圖像I(x,y)=(R,G,B)，(x,y)為像素點在數字圖像坐標系中的坐標，R,G,B為對應三通道的值。主路徑的像素點的三通道值滿足如下條件：

創(chuàng)建同大小的單通道全零矩陣g(x,y)，在該矩陣中，原圖像處于上述閾值的像素點對應的坐標(xp,yp)處的值為1，即g(xp,yp)=1，則新的g(x,y)矩陣即為待統(tǒng)計的圖像矩陣。

第二種方法是基于圖像灰度分布直方圖［16］，但噪聲像素點數量和主路經區(qū)域像素點數量有時難以區(qū)分，因此選用上文的去干擾的方法，如圖5所示。

圖5 去除干擾后的灰度圖

2.2 路徑分布統(tǒng)計和路徑規(guī)劃

經過2.1節(jié)的預處理，獲取了實時局部路徑信息，本節(jié)論述目標路徑點選取方法。

在無法獲取全局路徑信息的前提下，提出一種在飛行過程中利用當前時刻采集的圖像，進行局部路徑規(guī)劃，最終拼接為全局路徑的方法。

在提取出路徑區(qū)域后，將圖像柵格化，統(tǒng)計路徑像素在不同柵格中的分布情況，確定圖像中的目標柵格，轉換到實際運動場景中，作為目標路徑點。

圖像gh×w(x,y)有h行w列像素，對應像素最大橫坐標值為w，最大縱坐標值為h。將圖像均勻分為m×n個柵格區(qū)域，m,n均為大于1的整奇數，則每一柵格的高h0、寬w0為：

其中，h0,w0均為大于0的整數。由于gh×w的元素為0或1，用矩陣SZm×n的元素表示各個柵格中的像素的和。

按上述處理，若柵格(p,q)被背景覆蓋，則對應SZm×n[(p,q)]=0，若柵格(r,s)被路徑覆蓋，則SZm×n(r,s)≈h0×w0，其余情形的值介于二者之間。

為了保持無人機跟蹤的準確度和效率，使單個本體軸持續(xù)指向運動方向，因此在初始對準路徑中間線后，大部分時間采集的圖像兩側區(qū)域是背景。計算線狀結構分布時，可以只關注圖像中間區(qū)域，當圖像兩側出現路徑時，不影響對當前彎道的處理。具體處理方法為只對120×(51:110)區(qū)域的像素進行柵格化，兩側圖像直接設置為背景。

將整幅圖g120×160均勻劃分為3×3個柵格，分塊統(tǒng)計各塊像素和，如圖6所示。

圖6 不同拐角處理和像素統(tǒng)計結果

使無人機oxb軸指向運動方向，則已經飛過的路徑在柵格網中的區(qū)域是圖像中心與下邊緣中點連線經過的柵格，在后續(xù)選取時排除該柵格。小姿態(tài)角時，無人機在圖像中的投影位于SZm×n((m+1)/2,(n+1)/2)處。選取SZm×n的其他元素中最大元素下標(mi,ni)對應的柵格作為目標柵格。當路徑完整覆蓋多個柵格，(mi,ni)對應多個元素下標，則取幾個坐標的中心坐標作為(mi,ni)。將其轉換到圖片gh×w(x,y)中，則目標點在圖像矩陣的下標(Y,X)計算為：

轉換到導航坐標系的OXwYw平面內的坐標(Pwx,Pwy)為：

其中，Rwb為從oxbyb平面到OXwYw平面的旋轉變換矩陣；Twb為本體系原點到導航系原點位移在OXwYw平面內投影向量；Rbp為從數字圖像坐標系到本體坐標系oxbyb平面的旋轉變換矩陣；k為像素與實際場景之間的尺度因子。實時處理得到一系列路徑點序列Pxyz={(Pwxi,Pwyi,Z),i>1}，Z為飛行高度在導航系的對應坐標。

直線段飛行只需給出前進指令，控制的關鍵點在拐角處。無人機由進入拐角時的姿態(tài)機動為指向數字圖像坐標系的點P(X,Y)，偏航角的變化Δψ為：

在沒有大角度俯仰和滾轉運動的情況下，本體系ozb軸與導航系OZw軸近似平行，因此本體系的偏航角的變化可視為在導航系中的偏航角變化。

由于上述目標路徑點的計算是基于實時采集圖像結果，因此局部路徑飛行過程中，不能以式（11）的計算結果作為飛控輸入。飛控指令分為兩個階段，第一階段為改變偏航角：

式中，Pd,X,Pd,Y,Pd,Z,ψd為飛控的期望水平位置、期望高度和期望偏航角；Pt,X,Pt,Y,Pt,Z,ψt為無人機實時位置和偏航角。第二階段進行位置調整，調整偏航角后，規(guī)劃的目標路徑點坐標與無人機實時位置差值在無人機本體系中表示為：

在導航系中的期望狀態(tài)為：

式中，Rwb為第一階段改變偏航角后，本體系到導航系的旋轉矩陣。

由于硬件系統(tǒng)執(zhí)行指令的同時在采集圖像，新圖像的處理結果生成新的指令，如果一次指令對應的機動，如偏航角，變化比較大，則可能造成指令疊加，因此采用步進指令調整無人機位姿，例如每次改變0.03弧度。

3 飛行試驗驗證

使用的無人機平臺為parrot mambo，底部搭載120×160分辨率、60FPS的相機、超聲波傳感器和慣性測量單元。

無人機上并未搭載GPS接收裝置。如圖7所示，為無人機的外部結構。

圖7 Parrot mambo外部結構

主路徑設置為如圖8所示。

圖8 主路徑形狀

導航坐標系為NED?；谔岢龅乃惴?，驅動無人機沿路徑飛行。無人機飛行軌跡如圖9所示。

圖9 無人機飛行軌跡

圖10 導航系X軸運動狀態(tài)和指令

圖11 導航系Y軸運動和指令

圖12 無人機的偏航角狀態(tài)和指令

指令只在本體系oxb和偏航角上有偏移量，但最終控制量轉換到參考坐標系中，作為期望位置和期望偏航角。oxb軸正方向始終指向前進方向。在仿真和飛行測試中，提出的算法滿足了循跡飛行的需要，但還面臨在實際飛行過程中，硬件步進式地執(zhí)行位置偏置和偏航角指令，帶來的效率較低的問題。

結果分析

（1）在起飛階段設置起飛至距離地面1.1 m高，循跡飛行過程保持定高，算法在不同形狀的路段都能選取對應的目標路徑序列點。

（2）在直道飛行過程中，按算法中的控制指令設置，偏航角期望值和估計值保持一致。

（3）在本體軸oxb上的指令經過旋轉矩陣轉換到導航系中，在二維平面上運動。無人機狀態(tài)滯后于指令的狀態(tài)。

（4）在弧度拐角處，由于給出線性的分布，無人機飛行軌跡呈現較為尖銳的拐角，在銳角拐角處，能夠較為平滑地偏轉和運動。

（5）在實際機動過程中，設置偏航角期望值和位置偏置命令頻率較高，幅度較小，增大了機動需要消耗的時間。

4 結語

為了達到快速精準循跡飛行的目的，本文研究了搭載視覺傳感器的無人機實時采集的環(huán)境圖像，對圖像灰度化處理，去除背景中的干擾，提取出路徑區(qū)域，將預處理圖像柵格化，分別統(tǒng)計不同柵格的像素和。為了保證跟蹤效率，避免視野外圍的路徑對當前位置規(guī)劃的影響，只對圖像中間部分的像素進行統(tǒng)計，根據統(tǒng)計結果確定路徑序列點。路徑點選取在直線、弧線段和不同拐角處都適用。為了保證跟蹤效率，在飛行規(guī)程中，結合路徑點的相對位置關系和無人機實時姿態(tài)，設置對應的位移和偏航角指令，無人機本體