張 玲
(榆林職業技術學院 基礎部,陜西 榆林 719000)
近年來,隨著無人機及其相關技術的不斷發展,越來越多的無人機正在被使用。而四旋翼憑借著其小巧靈活,對起飛場地要求極低等優點被廣泛地應用到生活中的各個方面,如生活中的航拍四旋翼無人機以及電力巡檢四旋翼無人機等。利用無線電遙控設備與自備的程序控制裝置操縱的不載人飛機即為無人機,能夠執行各種復雜、困難的任務。因此被廣泛應用到管線巡檢、地質勘察、森林防火、地震救災、交通管理等多項工作中,并起到良好的輔助作用[1]。由于其新穎的結構、良好的飛行性能以及其特有的姿態控制方法,使得它具有廣泛的應用前景,因此受到了國內外眾多專家和學者的廣泛重視。
然而在實際的無人機使用過程中,要求無人機按照既定軌跡飛行,最大程度地減少其與固定建筑物以及其他障礙物之間的碰撞次數,并精準地到達指定地點。可以說,無人機的飛行精度直接影響其在各個領域中的應用價值。文獻[3]方法提出了一種目標檢測算法,并經實驗驗證,該算法具有較好的魯棒性。無人直升機通過機載相機獲取目標圖像,在對獲取的圖像進行一系列預處理操作后,結合所設計的目標特征標志的大小、形狀以及目標輪廓不變距等特征,實現對“H”型著陸標志的自主檢測,并設計了定高、懸停以及定速3種飛行模式,通過在3種飛行模式之間的切換,實現了無人直升機的自主著陸,之后研究者又將GPS信號與視覺信號相結合,使得無人直升機可以執行遠程自主飛行的任務。文獻[4]方法提出了一種能夠識別和估計無人直升機的位置與姿態信息的解算方法,利用機載的雙目相機對特征點進行匹配并進行位態的解算,從而估計無人直升機相對于著陸平面的位置和姿態信息。針對著陸平面運動的情況,將卡爾曼濾波技術與雙目視覺系統相結合,從而實現對目標物的跟蹤。
針對上述問題,本文提出一種基于粒子濾波的無人機自主軌跡視覺導航控制方法研究。采用柵格法對無人機的飛行環境進行量化描述通過對無人機飛行環境的柵格化處理,得出環境地圖創建結果,采用電動機的驅動方式,對4個轉子進行推力調節,實現了對飛行器的俯仰和總推力的控制。通過當前無人機位置與環境地圖中標注的建筑物位置之間的關系直接判斷得出,而動態障礙物以及其他障礙物的識別,通過逐行遍歷整個圖像,確定了移動物體的左右兩個邊界點,采用粒子濾波算法對無人機位姿進行自動定位,通過粒子濾波算法的運行,得出任意時刻無人機位姿的量化結果采用代價函數對路徑中的柵格節點進行評估,根據控制器內部結構決定該參數的具體取值,完成對無人機飛行參數的實時導航控制,進而提升導航控制方法在無人機領域的應用價值。
在無人機自主軌跡視覺導航控制方法的基礎上,結合粒子濾波算法,分別從視覺圖像、飛行控制、執行機構等方面實現導航控制方法的優化,其基本框架如圖1所示。

圖1 無人機自主軌跡視覺導航控制方法框圖
無人機飛行控制模塊根據視覺圖像的采集結果以及無人機姿態的判定結果,得到各個參數的控制量計算結果,最后經過控制分配形成直流無刷電機的控制量,控制電機槳葉旋轉使得無人機機身產生相應運動。
在無人機自主軌跡規劃以及視覺導航之前應考慮其周圍的環境問題,并根據無人機的不同特性,選擇不同的規劃算法。無人機的實時飛行位置需保證每個軌跡點均滿足設定的環境約束條件,因此采用柵格法對無人機的飛行環境進行量化描述[2]。該方法將網格分為一組具有二值信息的網格,該網格具有兩種屬性,并利用最優算法對網格進行規劃。在網格模型中,一般都是將網格模型的大小和步長作為網格的大小,但也要根據實際情況作出改變。通過對無人機飛行環境的柵格化處理,得出環境地圖創建結果,如圖2所示。

圖2 無人機飛行環境柵格地圖
由此可以得出飛行環境的約束條件為:

(1)
上式中imin和imax分別為x、y、z三個方向上的飛行坐標最大值和最小值,并在實際的飛行導航控制過程中,以公式1作為空間約束條件[3]。另外需要標記固定靜態障礙物的位置,即建筑物的位置。在已知建筑物高度的前提下,障礙物信息可以描述為:
(2)
其中:變量Wi為第i個建筑物所占的區域范圍,K表示無人機飛行環境中建筑物的數量。同理也可以得出其他飛行障礙物的占用面積和位置坐標,并將其標記在創建的初始環境地圖中。
從結構上來說,無人機的旋翼是固定在一個平面上的,4個旋翼均勻地分布在飛行器的四角,兩個相對的旋翼轉動的方向是一樣的,而另外兩個則是相反的。一般認為,一個逆時針轉動的旋翼被定義為飛機的前進方向,并將該轉子的旋轉中心和飛機的質量也連線相等的角度平分線作為飛行器的前進方向。由于飛行器的空間自由度為6個,4個為可控制的基本動作,因此該飛行器為一個非主動控制的系統[4]。這6種空間自由度包含了3種姿態角,即沿3個方向的運動,航向、俯仰角、橫滾角。采用電動機的驅動方式,對4個轉子進行推力調節,實現了對飛行器的俯仰和總推力的控制。當4個推進器同時增大推進器的推力,使得轉子的總推力超過了機身自身的自重,從而產生一個向上的加速度。結合上述無人機的運行原理,從牛頓第二定律可以看出,在平面坐標系統的3個方向上,無人機運動方程可以量化表示為:

(3)
其中:變量m為無人機的機體質量,Fx、Fy和Fz分別表示的是無人機的所受旋翼升力在3個方向上的分量,κi為空氣阻力系數[5]。在此基礎上,根據剛體轉動的動力學方程:
(4)
公式(4)中J和ν分別為無人機的慣性矩陣和角速度矢量,M為總力矩,由橫滾力矩、俯仰力矩、偏航力矩三部分組成,其量化計算公式如下:

(5)
上式中F1-F4表示的是無人機上4個旋翼所受的升力值,參數L和C分別代表單個旋翼桿長和比例系數,由于4個旋翼的安裝高度相同,因此L的取值為定值[6]。結合無人機結構參數,將其運動方程與動力方程融合在一起,得出無人機運動狀態模型的構建結果。
1.3.1 自主生成無人機視覺圖像
將攝像機設備安裝到無人機上,并利用攝像機、圖像以及世界坐標系之間的關系,對視覺相機進行標定。在采集無人機視覺圖像時,保證攝像機與無人機機體平面處于平行狀態,利用攝像機設備的成像原理得出實時視覺圖像。目標在相機坐標系中距離像素中心的坐標及其在世界坐標系中實際坐標的關系可以表示為:

(6)
式中:f和H為相機的焦距及其距離地面的高度,(x,y)和(x′,y′)分別為目標在相機坐標系和世界坐標系中的位置坐標。由于在獲取目標影像時,飛行器的側面模組會隨著機身的傾斜而產生偏差,因此測量結果并非與飛行器的垂直高度一致,因此必須進行相應的補償[7]。最終對實時采集影像圖像進行逐幀輸出,便能夠得出初始圖像的采集結果。
1.3.2 轉換圖像灰度
在無人機的視覺導航中,由于對視覺圖像的檢測速度具有較高要求,在對圖像進行預處理時,需要將采集到的圖像轉換成灰度圖像,且注意保留圖像的結構特征。由于無人機獲取的原始圖像具有隨機性,且與目標對象的邊界沒有相應的對應關系,采用加權方法可以很好地識別出目標與背景之間的差異,且存在著一定的模糊性[8]。采用高斯函數進行處理,在獲取圖像時,給定的像素點與場景區的周邊點之間的亮度貢獻是正常的,所以選取了高斯光滑函數g(x,y),并對距離不同的給定像素點的周邊點進行不同的處理。高斯光滑函數表達式如下:
(7)
其中:σ為高斯光滑偏置。由此可以得出初始目標圖像的灰度轉換結果為:
gray=g(x,y)(R·ωR+G·ωG+B·ωB)
(8)
公式(8)中R、G和B為圖像像素點的紅、綠、藍色彩分量,而ωi表示對應的權重值。聯立公式(7)和公式(8),并將初始采集的圖像代入其中,即可得出灰度圖像轉換輸出結果。
1.3.3 圖像幾何校正
無人機采集到的遙感影像畸變,主要是由于攝像機鏡頭的非線性失真,以及拍攝過程中由于無人機姿態的改變而產生的影像轉動和投影變形[9]。一般情況下,無人機的感應器焦距都是固定的,所以對每個圖像的偏差都是一樣的,這樣就可以將鏡頭的畸變歸類為系統錯誤,并通過模型和公式來修正。對于復雜的幾何變形可以使用二階多項式進行糾正,具體的幾何校正過程可以表示為:

(9)
公式(9)的輸出結果(xcor,ycor)為校正后任意像素的坐標位置,ai和bi均為多項式系數,取值為常數。在實際的圖像幾何校正過程中,為了提高圖像處理效率,需要先對其幾何畸變情況進行判定,若當前圖像未出現畸變情況,則不需要執行校正程序。
1.3.4 圖像濾波
圖像濾波是降低圖像噪聲的重要手段。圖像噪聲是由多種因素引起的,既有系統的外部因素,也有系統內部的因素。實際獲取的圖像均包含有噪聲,其特征和與圖像信號之間的關系是由噪聲引起的[10]。選取3*3的滑動窗口,在影像資料矩陣內按一行或一行的方向,逐個滑移。在每一次滑動之后,根據公式10對窗口中的像素灰度進行排序。
yi=Med{xi} = [Med[xi-k,…,xi,…,xi + k]]
(10)
其中:Med{·}為中值計算函數。公式(10)所得的中間值代替窗口中心位置像素的灰度值[11]。滑動窗口遍歷完圖像中的所有像素點后,輸出的結果即為圖像的濾波處理結果。整合所有圖像處理結果,最終將其標記為D(x)。
建筑障礙物的識別可以通過當前無人機位置與環境地圖中標注的建筑物位置之間的關系直接判斷得出,而動態障礙物以及其他障礙物的識別,則需要根據實時采集的視覺圖像,判斷當前無人機航行路徑上是否存在障礙物[12]。首先以采集并處理完成的圖像為基礎,對其進行逐行遍歷,確定了運動物體的最高點和最低點,并用yhighest和yminimum對其進行了縱向標注。在此基礎上,通過逐行遍歷整個圖像,確定了移動物體的左右兩個邊界點,并用xleft、xright標出相應的橫坐標。最終,獲得了整個圖像的4個頂點(xleft,yhighest)、(xright,yhighest)、(xleft,yminimum)、(xright,yminimum),再選擇一個最小的長方形框架,將上述4個坐標點作為頂點,對圖像進行裁剪,由此保證最小的方框可以容納所有的障礙物特征信息。在分割后的圖像中,含有前景障礙的物體的邊界,而在計算機視覺中,大多數的特征提取和識別都是以物體的邊界為對象。在對障礙物特性進行研究時,可以充分地利用障礙物的邊緣輪廓來獲得特征信息[13]。因此,在此基礎上,對分割后的圖像進行邊緣提取,并將其用于下一步的處理。分別計算各個圖像塊中相應的輪廓點數量,進而得出障礙物的特征提取結果如下:
Φ=[φ1,φ2,φ3,φ4,φ5]
(11)
式中,φ1表示導航控制的特征量,φ2表示導航控制的數據節點,φ3表示特征信息融合參數,φ4表示飛行障礙的序列節點,φ5為目標的移動速度。最終將提取的目標特征與設置的障礙物邊緣輪廓特征進行匹配,便可以確定當前無人機飛行區域是否存在障礙物及其類型[14]。將障礙物的識別結果添加到創建的環境地圖中,并將障礙物所在位置的柵格轉換為“0”,即不予通過標記處理。
無人機位姿估計可以視為一個概率推測的過程,采用粒子濾波可以將無人機位姿估計問題轉換為解算后驗概率密度問題,從而得到最優的目標狀態估計[15]。粒子濾波算法的基本流程如圖3所示。

圖3 粒子濾波算法流程圖

(12)
其中:Dt和xt分別為實時視覺圖像及求得狀態,P(xt∣Dt)為利用所獲得實時信息求得狀態的后驗概率密度函數[17]。在此基礎上根據概率密度分布生成N個新的粒子,并利用公式(13)計算生成新粒子的權重值。
(13)


(14)
由公式(14)所得到的結果為粒子濾波的最優解。通過加權、概率分布對微粒進行再抽樣,形成N個新的、獨立的、隨機粒子。最后,用粒子濾波方法得到的無人飛行器姿態數據進行了分析,當發現與目標的初始模板存在較大的偏差時,說明該方法的跟蹤失效。因此,將會對該算法進行初始化,并對無人機實時位姿進行重新判斷[18]。通過粒子濾波算法的運行,得出任意時刻無人機位姿的量化結果如下:
(15)
以公式(15)計算得出的無人機實時航行位置作為視覺導航的起點位置,并考慮無人機航行模型,對其姿態進行調整。
以當前無人機的位置確定結果為飛行軌跡的規劃起點,采用啟發式方法,使搜索節點沿目標方向運動,從而減少了運算速度,提高了運算速度。與此同時,該方法還以初始結點至當前結點的代數為總路徑代價,以防止在該路徑上遇到障礙物。在飛行軌跡規劃過程中,首先采用代價函數對路徑中的柵格節點進行評估,評估函數可以表示為:
f(n)=ψ(n)+r(n)
(16)
式中,參數n代表柵格節點,ψ(n)和r(n)分別為由起始節點到柵格節點n以及當前柵格節點到導航目標終點的評估代價值,f(n)則為全局評估代價值。由此可以確定創建的環境地圖中,任意一個柵格的允許通過狀態。在無人機航行的軌跡規劃過程中,首先,將無人機的初始航行位置納入open表中。通過對現有的結點進行擴充,計算每個待擴充結點的代值,選取由公式(16)所輸出的最小值節點,將其添加到 closed表格中。判斷擴展節點是否為軌跡導航終點,若判斷結果為否,則繼續向前進行擴展,直至達到目標點為止。如果open表中的柵格節點為空,則表示無可以被擴充的節點,若此時未到達導航軌跡終點,則表示無人機自主飛行軌跡規劃失敗,需要重新規劃[19]。由于在障礙物識別過程中,將識別結果添加到環境地圖中,并對障礙物所處位置進行柵格標記,因此在軌跡規劃過程中無需進行進一步障礙物篩選與判斷,在一定程度上提高了無人機航行軌跡的規劃效率和質量。無人機軌跡規劃流程圖如圖4所示。

圖4 無人機軌跡規劃流程圖
無人機視覺導航控制的最終目的就是保證實時無人機的運行姿態與參數與規劃的路徑一致,根據無人機航行軌跡的規劃結果,可以確定各個軌跡點位置上無人機的預期位姿,將其表示為:
(17)
其中:xt、yt和zt為軌跡點預期位置坐標,vxt、vyt和vzt對應的是無人機航行速度在x、y、z三個方向上的分量,而φ、θ和φ表示的是滾轉角、俯仰角和偏航角。利用粒子濾波算法可以直接得出無人機相應時刻的所處位置坐標和飛行運動參數,并將其記為Xactual-t,那么無人機視覺導航的控制量可以表示為:
Δ=|Xexpect-t-Xactual-t|s(v)
(18)
式中,s(v)表示控制方向函數,由此可以得出無人機軌跡位置、姿態角度以及航行速度3個方面的控制量計算結果。公式(18)中|·|為絕對值計算符號,能夠保證計算得出的控制量始終為正數。但在實際的控制過程中,需要判斷無人機預期運行參數與實際參數之間的大小,從而確定具體的導航控制方向。
無人機自主軌跡視覺導航控制器是導航控制任務的硬件支持設備,能夠實現驅動、開關狀態調整等功能,同時能夠將計算的無人機導航控制量數據轉換成控制信號,直接作用在無人機設備上。由于無人機自主軌跡導航需要從3個方面執行,因此需要連接3個不同功能的控制器,并利用姿態控制回路,速度控制回路和位置控制回路,完成控制器的連接。安裝的無人機自主UI及視覺導航控制器結構如圖5所示。

圖5 無人機自主軌跡視覺導航控制器結構圖
控制器為深圳市捷紅昌科技有限公司,型號為JHC-JQW-B,最高工作頻率為128 GHz,外部設備齊全,內置雙精度FPU,并支持多種 DSP指令。特別是FPU和 DSP指令集的設計,通過位置控制器、速度控制器和姿態角控制器,得到帶限幅的PID,得到后將其輸入到驅動模塊,從而實現卡爾曼濾波器的實時控制。在實際操作中,將三軸的轉速和所得到的轉速輸入到轉速控制器中,再由轉速控制器計算出所需的轉子總升力和目標姿態角控制值,再由姿態控制器控制4個轉子的張力,從而達到對飛機的姿態控制。另外,為了降低無人機飛行環境中無線電或其他信號對控制效果產生的干擾,在控制器內部加設一個自抗擾芯片,在保證控制精度的同時也最大程度地保護控制器設備的運行安全。
將計算得出的無人機自主軌跡視覺導航控制量輸入到安裝的控制器中,分別從位置、角度和速度3個方面完成導航控制任務。以姿態角中的偏航角為例,利用導航控制器對操縱桿進行操舵,使其產生相應的轉向,從而實現對無人機的轉向和保持。假設當前無人機處于直線飛行狀態,則偏航角控制率可表示為:
φexpect=φactual-tan-1(kpΔφ)
(19)
其中:φactual表示控制速度函數,Δφ為偏航角控制量,其具體取值可以通過公式(18)計算得出,kp為偏航角比例系數,根據控制器內部結構決定該參數的具體取值[20]。同理可以得出其他姿態角以及位置、速度控制律,在控制器的支持下,完成對無人機飛行參數的實時導航控制,保證無人機的實際飛行軌跡與規劃軌跡一致。
以測試設計的基于粒子濾波的無人機自主軌跡視覺導航控制方法的應用效果為目的,針對導航控制效果進行測試,并通過與預設值的對比,體現出設計控制方法的性能優勢。
此次實驗以MS-680型四旋翼無人機為實驗樣本,該機的結構以軸對稱為主,結構較為簡潔,4個轉子均勻地排列在4個轉子上,中間裝有電池、控制器等機載設備。4個螺旋槳為飛機的飛行提供動力,而起落架則是對機身的支持。采用外轉式無刷電動機C28-30-15,并配備與電動機相匹配的電動機調速器,以保證其在空中的穩定運行。選擇的外轉式無刷電動機,其額定電源電壓是22 V,最大電流可達23 A。兩個對稱的馬達軸距為620毫米,推進器采用14寸塑料三片槳葉。根據對電動機運行電流的粗略測定,四臺電動機在懸掛狀態下的最高功率不大于25 A,選用22.0 V的鋰電池,其容量為600米 Ah,可持續30分鐘。為了最大限度地降低機身的重量,除了發動機支架和起落架,其他部件均由碳纖維制造,電機底座則是由尼龍制成。電池已安裝完畢。這架無人機的總重量是1 560 g,最大載荷是600 g。
從基于粒子濾波的無人機自主軌跡視覺導航控制方法的開發環境,以及其應用環境兩個方面進行實驗環境的配置,其中利用導航控制方法對開發環境進行構建,將控制器以及傳感器設備安裝到無人機上,并在地面上設置一個控制中心,要求控制中心發出的控制指令能夠通過無線通信網絡被無人機接收。而設置的導航控制方法應用環境分別為室外環境和室內環境。室外環境選擇的是某城市的主干道和居民區,在滿足無人機飛行條件的同時,提供了天然的障礙物條件。而室內環境主要選擇的是某大型建筑的走廊以及房屋空間,在室內環境中需要通過加設紙箱、桌椅等設施,實現環境障礙物的設置。
試飛準備調試的內容包括:控制信號能否正常被無人機接收、導航控制方法能否在無人機設備上運行、視覺模塊運行是否正常、無人機飛行動態參數是否能夠及時采集與傳輸等。選擇人口稀少的郊區,且要求試飛區域具有專用的起飛跑道,為起飛、降落等模擬飛行任務提供條件。在無高大建筑和樹木的干擾下,最大限度地降低了無人機在空中的干擾。選擇的無人機研究對象能夠在配置的實驗環境中正常飛行,可以執行無人機自主軌跡視覺導航控制測試任務。
在配置的實驗環境中選擇多個導航終點,在考慮環境障礙物的前提下,生成無人機自主航行軌跡的測試樣本,以此作為無人機的理想航行軌跡。每個實驗環境中設置4條測試軌跡,實驗中設置的測試軌跡數量為8,由此將實驗劃分成8個組別,記錄生成軌跡上各個點的軌跡坐標以及無人機到達的時刻。將理想航行軌跡數據輸入到無人機的飛行程序中,并同時啟動無人機、控制器以及基于粒子濾波的無人機自主軌跡視覺導航控制程序。在無人機飛行過程中,實時采集環境視覺圖像數據,并根據采集結果調整實際飛行軌跡。圖6為無人機視覺圖像的輸出結果。

圖6 無人機視覺圖像輸出界面
在程序以及控制器的協同作用下,完成無人機的導航控制任務,最終的控制結果如圖7所示。

圖7 無人機自主軌跡視覺導航控制界面
同理可以得出所有控制任務的執行結果,最終調取控制方法的后臺運行數據,獲得無人機的實時運行參數信息。
由于設計的導航控制方法分別從位置、速度和姿態角3個方面進行控制,因此設置的控制效果測試指標分別為位置控制誤差、速度控制誤差和姿態角控制誤差,其中位置控制誤差的數值結果為:
(20)
其中:(xac,yac,zac)和(xex,yex,zex)分別為無人機的理想軌跡坐標和實際軌跡坐標。另外其他兩個測試指標的數值結果如下:

(21)
式中,νac和νex為目標控制速度和實際速度,φac、θac、φac、φex、θex和φex對應的是滾轉角、俯仰角、偏航角的目標值和實際值。計算得出的誤差值越大,證明導航控制方法的控制效果越差,因此設定εxyz、εv和εφθφ的預設值分別為3.0 m、0.2 m/s和0.1°,且要求應用設計導航控制方法下,無人機的實際飛行數據不高于預設值。
通過相關數據的提取及其與目標數據的比對,得出最終的控制效果測試結果。位置控制誤差的量化測試結果,如圖8所示。

圖8 無人機飛行軌跡位置誤差測試結果
將圖8中的數據代入到公式(20)中,計算得出無人機的位置控制誤差的平均值為2.1 m,低于預設值。同理可以得出反映無人機飛行速度和姿態角控制效果的測試結果,如表1所示。

表1 無人機飛行速度和姿態角控制效果測試數據表
將表1中的數據代入到公式(21)中,計算得出應用控制方法下,無人機的平均速度控制誤差為0.063 m/s,姿態角控制誤差的平均值為0.05°,均低于預設值,由此證明設計的基于粒子濾波的無人機自主軌跡視覺導航控制方法具有良好的控制效果。根據上述數據,得到速度誤差如圖9所示。

圖9 速度控制誤差對比圖
根據圖9可知,本文方法的速度控制誤差最低為0.07%,文獻[3]方法的速度控制誤差最低為0.15%,文獻[4]方法的速度控制誤差最低為0.17%。由此可知,本文方法的速度控制誤差最低,可有效提高控制效果。姿態控制誤差如10所示。
根據圖10可知,本文方法的速度控制誤差最低為14%,文獻[3]方法的姿態控制誤差最低為25%,文獻[4]方法的姿態控制誤差最低為40%。由此可知,本文方法的姿態控制誤差最低。

圖10 姿態控制誤差對比
無人機作為重要的輔助工具,其飛行質量直接影響到無人機的應用價值。通過粒子濾波算法的應用,對處理非線性、非高斯動態系統相關問題的有效性已形成共識,進而有效地解決了現有控制方法存在的控制精度等問題。然而受到時間和空間的限制,實驗設置的測試次數較少,因此得出的實驗結果存在一定的局限性,需要在今后的研究工作中進一步補充與優化。