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

（1.國(guó)家空間科學(xué)中心，北京100000；2.中國(guó)科學(xué)院大學(xué)，北京100000）

0 引言

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

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

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

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

1 基于視覺導(dǎo)引的無人機(jī)飛行控制

在基于視覺信息導(dǎo)引飛行的問題中，慣導(dǎo)器件等傳感器獲取無人機(jī)的位姿信息，視覺信息用于解算無人機(jī)與指定路徑的相對(duì)位置信息，并根據(jù)該信息設(shè)計(jì)無人機(jī)的飛行路徑。

圖像中路徑識(shí)別過程去除噪聲和背景干擾，明確路徑區(qū)域?qū)?yīng)的坐標(biāo)；提取路徑分布信息是將圖像中路徑幾何信息轉(zhuǎn)換為數(shù)字信息，為路徑規(guī)劃提供輸入；路徑規(guī)劃是確定飛行目標(biāo)航跡點(diǎn)。

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

圖1 無人機(jī)本體坐標(biāo)系

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

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

在滾轉(zhuǎn)角和俯仰角較小的飛行姿態(tài)下，忽略高階項(xiàng)，對(duì)動(dòng)力學(xué)方程進(jìn)行線性化，得到解耦的動(dòng)力學(xué)方程。

水平位置動(dòng)力學(xué)方程：

其中，φ，θ和ψ分別表示無人機(jī)的滾轉(zhuǎn)角、俯仰角和偏航角。

高度動(dòng)力學(xué)方程：

姿態(tài)動(dòng)力學(xué)方程:

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

圖2 無人機(jī)級(jí)聯(lián)控制

2 自主循跡算法設(shè)計(jì)

2.1 路徑信息提取

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

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

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

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

測(cè)試環(huán)境中主路徑為紅色，背景為綠色，如圖3所示。對(duì)應(yīng)的灰度圖，背景的灰度大于路徑的灰度，二值化結(jié)果為路徑為黑色區(qū)域，背景為白色區(qū)域。為達(dá)到統(tǒng)計(jì)目標(biāo)，對(duì)二值圖進(jìn)行補(bǔ)圖處理。

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

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

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

去除干擾項(xiàng)的一種方法為直接提取主路徑，根據(jù)實(shí)時(shí)光照條件下采集的圖像像素點(diǎn)三通道矩陣的元素?cái)?shù)值確定圖像中主路徑區(qū)域的RGB三通道閾值，設(shè)置同時(shí)滿足3個(gè)閾值條件的像素點(diǎn)為前景，其余像素點(diǎn)為背景。對(duì)一幅RGB圖像I(x,y)=(R,G,B)，(x,y)為像素點(diǎn)在數(shù)字圖像坐標(biāo)系中的坐標(biāo)，R,G,B為對(duì)應(yīng)三通道的值。主路徑的像素點(diǎn)的三通道值滿足如下條件：

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

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

圖5 去除干擾后的灰度圖

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

經(jīng)過2.1節(jié)的預(yù)處理，獲取了實(shí)時(shí)局部路徑信息，本節(jié)論述目標(biāo)路徑點(diǎn)選取方法。

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

在提取出路徑區(qū)域后，將圖像柵格化，統(tǒng)計(jì)路徑像素在不同柵格中的分布情況，確定圖像中的目標(biāo)柵格，轉(zhuǎn)換到實(shí)際運(yùn)動(dòng)場(chǎng)景中，作為目標(biāo)路徑點(diǎn)。

圖像gh×w(x,y)有h行w列像素，對(duì)應(yīng)像素最大橫坐標(biāo)值為w，最大縱坐標(biāo)值為h。將圖像均勻分為m×n個(gè)柵格區(qū)域，m,n均為大于1的整奇數(shù)，則每一柵格的高h(yuǎn)0、寬w0為：

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

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

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

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

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

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

轉(zhuǎn)換到導(dǎo)航坐標(biāo)系的OXwYw平面內(nèi)的坐標(biāo)(Pwx,Pwy)為：

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

直線段飛行只需給出前進(jìn)指令，控制的關(guān)鍵點(diǎn)在拐角處。無人機(jī)由進(jìn)入拐角時(shí)的姿態(tài)機(jī)動(dòng)為指向數(shù)字圖像坐標(biāo)系的點(diǎn)P(X,Y)，偏航角的變化Δψ為：

在沒有大角度俯仰和滾轉(zhuǎn)運(yùn)動(dòng)的情況下，本體系ozb軸與導(dǎo)航系OZw軸近似平行，因此本體系的偏航角的變化可視為在導(dǎo)航系中的偏航角變化。

由于上述目標(biāo)路徑點(diǎn)的計(jì)算是基于實(shí)時(shí)采集圖像結(jié)果，因此局部路徑飛行過程中，不能以式（11）的計(jì)算結(jié)果作為飛控輸入。飛控指令分為兩個(gè)階段，第一階段為改變偏航角：

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

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

式中，Rwb為第一階段改變偏航角后，本體系到導(dǎo)航系的旋轉(zhuǎn)矩陣。

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

3 飛行試驗(yàn)驗(yàn)證

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

無人機(jī)上并未搭載GPS接收裝置。如圖7所示，為無人機(jī)的外部結(jié)構(gòu)。

圖7 Parrot mambo外部結(jié)構(gòu)

主路徑設(shè)置為如圖8所示。

圖8 主路徑形狀

導(dǎo)航坐標(biāo)系為NED?；谔岢龅乃惴?，驅(qū)動(dòng)無人機(jī)沿路徑飛行。無人機(jī)飛行軌跡如圖9所示。

圖9 無人機(jī)飛行軌跡

圖10 導(dǎo)航系X軸運(yùn)動(dòng)狀態(tài)和指令

圖11 導(dǎo)航系Y軸運(yùn)動(dòng)和指令

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

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

結(jié)果分析

（1）在起飛階段設(shè)置起飛至距離地面1.1 m高，循跡飛行過程保持定高，算法在不同形狀的路段都能選取對(duì)應(yīng)的目標(biāo)路徑序列點(diǎn)。

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

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

（4）在弧度拐角處，由于給出線性的分布，無人機(jī)飛行軌跡呈現(xiàn)較為尖銳的拐角，在銳角拐角處，能夠較為平滑地偏轉(zhuǎn)和運(yùn)動(dòng)。

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

4 結(jié)語

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