基于粒子濾波的無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制方法研究

張 玲

(榆林職業(yè)技術(shù)學(xué)院 基礎(chǔ)部，陜西 榆林 719000)

0 引言

近年來(lái)，隨著無(wú)人機(jī)及其相關(guān)技術(shù)的不斷發(fā)展，越來(lái)越多的無(wú)人機(jī)正在被使用。而四旋翼憑借著其小巧靈活，對(duì)起飛場(chǎng)地要求極低等優(yōu)點(diǎn)被廣泛地應(yīng)用到生活中的各個(gè)方面，如生活中的航拍四旋翼無(wú)人機(jī)以及電力巡檢四旋翼無(wú)人機(jī)等。利用無(wú)線電遙控設(shè)備與自備的程序控制裝置操縱的不載人飛機(jī)即為無(wú)人機(jī)，能夠執(zhí)行各種復(fù)雜、困難的任務(wù)。因此被廣泛應(yīng)用到管線巡檢、地質(zhì)勘察、森林防火、地震救災(zāi)、交通管理等多項(xiàng)工作中，并起到良好的輔助作用[1]。由于其新穎的結(jié)構(gòu)、良好的飛行性能以及其特有的姿態(tài)控制方法，使得它具有廣泛的應(yīng)用前景，因此受到了國(guó)內(nèi)外眾多專家和學(xué)者的廣泛重視。

然而在實(shí)際的無(wú)人機(jī)使用過(guò)程中，要求無(wú)人機(jī)按照既定軌跡飛行，最大程度地減少其與固定建筑物以及其他障礙物之間的碰撞次數(shù)，并精準(zhǔn)地到達(dá)指定地點(diǎn)?？梢哉f(shuō)，無(wú)人機(jī)的飛行精度直接影響其在各個(gè)領(lǐng)域中的應(yīng)用價(jià)值。文獻(xiàn)[3]方法提出了一種目標(biāo)檢測(cè)算法，并經(jīng)實(shí)驗(yàn)驗(yàn)證，該算法具有較好的魯棒性。無(wú)人直升機(jī)通過(guò)機(jī)載相機(jī)獲取目標(biāo)圖像，在對(duì)獲取的圖像進(jìn)行一系列預(yù)處理操作后，結(jié)合所設(shè)計(jì)的目標(biāo)特征標(biāo)志的大小、形狀以及目標(biāo)輪廓不變距等特征，實(shí)現(xiàn)對(duì)“H”型著陸標(biāo)志的自主檢測(cè)，并設(shè)計(jì)了定高、懸停以及定速3種飛行模式，通過(guò)在3種飛行模式之間的切換，實(shí)現(xiàn)了無(wú)人直升機(jī)的自主著陸，之后研究者又將GPS信號(hào)與視覺(jué)信號(hào)相結(jié)合，使得無(wú)人直升機(jī)可以執(zhí)行遠(yuǎn)程自主飛行的任務(wù)。文獻(xiàn)[4]方法提出了一種能夠識(shí)別和估計(jì)無(wú)人直升機(jī)的位置與姿態(tài)信息的解算方法，利用機(jī)載的雙目相機(jī)對(duì)特征點(diǎn)進(jìn)行匹配并進(jìn)行位態(tài)的解算，從而估計(jì)無(wú)人直升機(jī)相對(duì)于著陸平面的位置和姿態(tài)信息。針對(duì)著陸平面運(yùn)動(dòng)的情況，將卡爾曼濾波技術(shù)與雙目視覺(jué)系統(tǒng)相結(jié)合，從而實(shí)現(xiàn)對(duì)目標(biāo)物的跟蹤。

針對(duì)上述問(wèn)題，本文提出一種基于粒子濾波的無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制方法研究。采用柵格法對(duì)無(wú)人機(jī)的飛行環(huán)境進(jìn)行量化描述通過(guò)對(duì)無(wú)人機(jī)飛行環(huán)境的柵格化處理，得出環(huán)境地圖創(chuàng)建結(jié)果，采用電動(dòng)機(jī)的驅(qū)動(dòng)方式，對(duì)4個(gè)轉(zhuǎn)子進(jìn)行推力調(diào)節(jié)，實(shí)現(xiàn)了對(duì)飛行器的俯仰和總推力的控制。通過(guò)當(dāng)前無(wú)人機(jī)位置與環(huán)境地圖中標(biāo)注的建筑物位置之間的關(guān)系直接判斷得出，而動(dòng)態(tài)障礙物以及其他障礙物的識(shí)別，通過(guò)逐行遍歷整個(gè)圖像，確定了移動(dòng)物體的左右兩個(gè)邊界點(diǎn)，采用粒子濾波算法對(duì)無(wú)人機(jī)位姿進(jìn)行自動(dòng)定位，通過(guò)粒子濾波算法的運(yùn)行，得出任意時(shí)刻無(wú)人機(jī)位姿的量化結(jié)果采用代價(jià)函數(shù)對(duì)路徑中的柵格節(jié)點(diǎn)進(jìn)行評(píng)估，根據(jù)控制器內(nèi)部結(jié)構(gòu)決定該參數(shù)的具體取值，完成對(duì)無(wú)人機(jī)飛行參數(shù)的實(shí)時(shí)導(dǎo)航控制，進(jìn)而提升導(dǎo)航控制方法在無(wú)人機(jī)領(lǐng)域的應(yīng)用價(jià)值。

1 無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制方法設(shè)計(jì)

在無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制方法的基礎(chǔ)上，結(jié)合粒子濾波算法，分別從視覺(jué)圖像、飛行控制、執(zhí)行機(jī)構(gòu)等方面實(shí)現(xiàn)導(dǎo)航控制方法的優(yōu)化，其基本框架如圖1所示。

圖1 無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制方法框圖

無(wú)人機(jī)飛行控制模塊根據(jù)視覺(jué)圖像的采集結(jié)果以及無(wú)人機(jī)姿態(tài)的判定結(jié)果，得到各個(gè)參數(shù)的控制量計(jì)算結(jié)果，最后經(jīng)過(guò)控制分配形成直流無(wú)刷電機(jī)的控制量，控制電機(jī)槳葉旋轉(zhuǎn)使得無(wú)人機(jī)機(jī)身產(chǎn)生相應(yīng)運(yùn)動(dòng)。

1.1 創(chuàng)建無(wú)人機(jī)飛行環(huán)境地圖

在無(wú)人機(jī)自主軌跡規(guī)劃以及視覺(jué)導(dǎo)航之前應(yīng)考慮其周圍的環(huán)境問(wèn)題，并根據(jù)無(wú)人機(jī)的不同特性，選擇不同的規(guī)劃算法。無(wú)人機(jī)的實(shí)時(shí)飛行位置需保證每個(gè)軌跡點(diǎn)均滿足設(shè)定的環(huán)境約束條件，因此采用柵格法對(duì)無(wú)人機(jī)的飛行環(huán)境進(jìn)行量化描述[2]。該方法將網(wǎng)格分為一組具有二值信息的網(wǎng)格，該網(wǎng)格具有兩種屬性，并利用最優(yōu)算法對(duì)網(wǎng)格進(jìn)行規(guī)劃。在網(wǎng)格模型中，一般都是將網(wǎng)格模型的大小和步長(zhǎng)作為網(wǎng)格的大小，但也要根據(jù)實(shí)際情況作出改變。通過(guò)對(duì)無(wú)人機(jī)飛行環(huán)境的柵格化處理，得出環(huán)境地圖創(chuàng)建結(jié)果，如圖2所示。

圖2 無(wú)人機(jī)飛行環(huán)境柵格地圖

由此可以得出飛行環(huán)境的約束條件為：

(1)

上式中imin和imax分別為x、y、z三個(gè)方向上的飛行坐標(biāo)最大值和最小值，并在實(shí)際的飛行導(dǎo)航控制過(guò)程中，以公式1作為空間約束條件[3]。另外需要標(biāo)記固定靜態(tài)障礙物的位置，即建筑物的位置。在已知建筑物高度的前提下，障礙物信息可以描述為：

(2)

其中:變量Wi為第i個(gè)建筑物所占的區(qū)域范圍，K表示無(wú)人機(jī)飛行環(huán)境中建筑物的數(shù)量。同理也可以得出其他飛行障礙物的占用面積和位置坐標(biāo)，并將其標(biāo)記在創(chuàng)建的初始環(huán)境地圖中。

1.2 構(gòu)建無(wú)人機(jī)運(yùn)動(dòng)狀態(tài)模型

從結(jié)構(gòu)上來(lái)說(shuō)，無(wú)人機(jī)的旋翼是固定在一個(gè)平面上的，4個(gè)旋翼均勻地分布在飛行器的四角，兩個(gè)相對(duì)的旋翼轉(zhuǎn)動(dòng)的方向是一樣的，而另外兩個(gè)則是相反的。一般認(rèn)為，一個(gè)逆時(shí)針轉(zhuǎn)動(dòng)的旋翼被定義為飛機(jī)的前進(jìn)方向，并將該轉(zhuǎn)子的旋轉(zhuǎn)中心和飛機(jī)的質(zhì)量也連線相等的角度平分線作為飛行器的前進(jìn)方向。由于飛行器的空間自由度為6個(gè)，4個(gè)為可控制的基本動(dòng)作，因此該飛行器為一個(gè)非主動(dòng)控制的系統(tǒng)[4]。這6種空間自由度包含了3種姿態(tài)角，即沿3個(gè)方向的運(yùn)動(dòng)，航向、俯仰角、橫滾角。采用電動(dòng)機(jī)的驅(qū)動(dòng)方式，對(duì)4個(gè)轉(zhuǎn)子進(jìn)行推力調(diào)節(jié)，實(shí)現(xiàn)了對(duì)飛行器的俯仰和總推力的控制。當(dāng)4個(gè)推進(jìn)器同時(shí)增大推進(jìn)器的推力，使得轉(zhuǎn)子的總推力超過(guò)了機(jī)身自身的自重，從而產(chǎn)生一個(gè)向上的加速度。結(jié)合上述無(wú)人機(jī)的運(yùn)行原理，從牛頓第二定律可以看出，在平面坐標(biāo)系統(tǒng)的3個(gè)方向上，無(wú)人機(jī)運(yùn)動(dòng)方程可以量化表示為：

(3)

其中:變量m為無(wú)人機(jī)的機(jī)體質(zhì)量，F(xiàn)x、Fy和Fz分別表示的是無(wú)人機(jī)的所受旋翼升力在3個(gè)方向上的分量，κi為空氣阻力系數(shù)[5]。在此基礎(chǔ)上，根據(jù)剛體轉(zhuǎn)動(dòng)的動(dòng)力學(xué)方程：

(4)

公式(4)中J和ν分別為無(wú)人機(jī)的慣性矩陣和角速度矢量，M為總力矩，由橫滾力矩、俯仰力矩、偏航力矩三部分組成，其量化計(jì)算公式如下：

(5)

上式中F1-F4表示的是無(wú)人機(jī)上4個(gè)旋翼所受的升力值，參數(shù)L和C分別代表單個(gè)旋翼?xiàng)U長(zhǎng)和比例系數(shù)，由于4個(gè)旋翼的安裝高度相同，因此L的取值為定值[6]。結(jié)合無(wú)人機(jī)結(jié)構(gòu)參數(shù)，將其運(yùn)動(dòng)方程與動(dòng)力方程融合在一起，得出無(wú)人機(jī)運(yùn)動(dòng)狀態(tài)模型的構(gòu)建結(jié)果。

1.3 采集并處理無(wú)人機(jī)視覺(jué)圖像

1.3.1 自主生成無(wú)人機(jī)視覺(jué)圖像

將攝像機(jī)設(shè)備安裝到無(wú)人機(jī)上，并利用攝像機(jī)、圖像以及世界坐標(biāo)系之間的關(guān)系，對(duì)視覺(jué)相機(jī)進(jìn)行標(biāo)定。在采集無(wú)人機(jī)視覺(jué)圖像時(shí)，保證攝像機(jī)與無(wú)人機(jī)機(jī)體平面處于平行狀態(tài)，利用攝像機(jī)設(shè)備的成像原理得出實(shí)時(shí)視覺(jué)圖像。目標(biāo)在相機(jī)坐標(biāo)系中距離像素中心的坐標(biāo)及其在世界坐標(biāo)系中實(shí)際坐標(biāo)的關(guān)系可以表示為：

(6)

式中:f和H為相機(jī)的焦距及其距離地面的高度，(x,y)和(x′,y′)分別為目標(biāo)在相機(jī)坐標(biāo)系和世界坐標(biāo)系中的位置坐標(biāo)。由于在獲取目標(biāo)影像時(shí)，飛行器的側(cè)面模組會(huì)隨著機(jī)身的傾斜而產(chǎn)生偏差，因此測(cè)量結(jié)果并非與飛行器的垂直高度一致，因此必須進(jìn)行相應(yīng)的補(bǔ)償[7]。最終對(duì)實(shí)時(shí)采集影像圖像進(jìn)行逐幀輸出，便能夠得出初始圖像的采集結(jié)果。

1.3.2 轉(zhuǎn)換圖像灰度

在無(wú)人機(jī)的視覺(jué)導(dǎo)航中，由于對(duì)視覺(jué)圖像的檢測(cè)速度具有較高要求，在對(duì)圖像進(jìn)行預(yù)處理時(shí)，需要將采集到的圖像轉(zhuǎn)換成灰度圖像，且注意保留圖像的結(jié)構(gòu)特征。由于無(wú)人機(jī)獲取的原始圖像具有隨機(jī)性，且與目標(biāo)對(duì)象的邊界沒(méi)有相應(yīng)的對(duì)應(yīng)關(guān)系，采用加權(quán)方法可以很好地識(shí)別出目標(biāo)與背景之間的差異，且存在著一定的模糊性[8]。采用高斯函數(shù)進(jìn)行處理，在獲取圖像時(shí)，給定的像素點(diǎn)與場(chǎng)景區(qū)的周邊點(diǎn)之間的亮度貢獻(xiàn)是正常的，所以選取了高斯光滑函數(shù)g(x,y)，并對(duì)距離不同的給定像素點(diǎn)的周邊點(diǎn)進(jìn)行不同的處理。高斯光滑函數(shù)表達(dá)式如下：

(7)

其中:σ為高斯光滑偏置。由此可以得出初始目標(biāo)圖像的灰度轉(zhuǎn)換結(jié)果為：

gray=g(x,y)(R·ωR+G·ωG+B·ωB)

(8)

公式(8)中R、G和B為圖像像素點(diǎn)的紅、綠、藍(lán)色彩分量，而ωi表示對(duì)應(yīng)的權(quán)重值。聯(lián)立公式(7)和公式(8)，并將初始采集的圖像代入其中，即可得出灰度圖像轉(zhuǎn)換輸出結(jié)果。

1.3.3 圖像幾何校正

無(wú)人機(jī)采集到的遙感影像畸變，主要是由于攝像機(jī)鏡頭的非線性失真，以及拍攝過(guò)程中由于無(wú)人機(jī)姿態(tài)的改變而產(chǎn)生的影像轉(zhuǎn)動(dòng)和投影變形[9]。一般情況下，無(wú)人機(jī)的感應(yīng)器焦距都是固定的，所以對(duì)每個(gè)圖像的偏差都是一樣的，這樣就可以將鏡頭的畸變歸類為系統(tǒng)錯(cuò)誤，并通過(guò)模型和公式來(lái)修正。對(duì)于復(fù)雜的幾何變形可以使用二階多項(xiàng)式進(jìn)行糾正，具體的幾何校正過(guò)程可以表示為：

(9)

公式(9)的輸出結(jié)果(xcor,ycor)為校正后任意像素的坐標(biāo)位置，ai和bi均為多項(xiàng)式系數(shù)，取值為常數(shù)。在實(shí)際的圖像幾何校正過(guò)程中，為了提高圖像處理效率，需要先對(duì)其幾何畸變情況進(jìn)行判定，若當(dāng)前圖像未出現(xiàn)畸變情況，則不需要執(zhí)行校正程序。

1.3.4 圖像濾波

圖像濾波是降低圖像噪聲的重要手段。圖像噪聲是由多種因素引起的，既有系統(tǒng)的外部因素，也有系統(tǒng)內(nèi)部的因素。實(shí)際獲取的圖像均包含有噪聲，其特征和與圖像信號(hào)之間的關(guān)系是由噪聲引起的[10]。選取3*3的滑動(dòng)窗口，在影像資料矩陣內(nèi)按一行或一行的方向，逐個(gè)滑移。在每一次滑動(dòng)之后，根據(jù)公式10對(duì)窗口中的像素灰度進(jìn)行排序。

yi=Med{xi} = [Med[xi-k,…,xi,…,xi + k]]

(10)

其中:Med{·}為中值計(jì)算函數(shù)。公式(10)所得的中間值代替窗口中心位置像素的灰度值[11]?；瑒?dòng)窗口遍歷完圖像中的所有像素點(diǎn)后，輸出的結(jié)果即為圖像的濾波處理結(jié)果。整合所有圖像處理結(jié)果，最終將其標(biāo)記為D(x)。

1.4 識(shí)別環(huán)境中的飛行障礙

建筑障礙物的識(shí)別可以通過(guò)當(dāng)前無(wú)人機(jī)位置與環(huán)境地圖中標(biāo)注的建筑物位置之間的關(guān)系直接判斷得出，而動(dòng)態(tài)障礙物以及其他障礙物的識(shí)別，則需要根據(jù)實(shí)時(shí)采集的視覺(jué)圖像，判斷當(dāng)前無(wú)人機(jī)航行路徑上是否存在障礙物[12]。首先以采集并處理完成的圖像為基礎(chǔ)，對(duì)其進(jìn)行逐行遍歷，確定了運(yùn)動(dòng)物體的最高點(diǎn)和最低點(diǎn)，并用yhighest和yminimum對(duì)其進(jìn)行了縱向標(biāo)注。在此基礎(chǔ)上，通過(guò)逐行遍歷整個(gè)圖像，確定了移動(dòng)物體的左右兩個(gè)邊界點(diǎn)，并用xleft、xright標(biāo)出相應(yīng)的橫坐標(biāo)。最終，獲得了整個(gè)圖像的4個(gè)頂點(diǎn)(xleft,yhighest)、(xright,yhighest)、(xleft,yminimum)、(xright,yminimum)，再選擇一個(gè)最小的長(zhǎng)方形框架，將上述4個(gè)坐標(biāo)點(diǎn)作為頂點(diǎn)，對(duì)圖像進(jìn)行裁剪，由此保證最小的方框可以容納所有的障礙物特征信息。在分割后的圖像中，含有前景障礙的物體的邊界，而在計(jì)算機(jī)視覺(jué)中，大多數(shù)的特征提取和識(shí)別都是以物體的邊界為對(duì)象。在對(duì)障礙物特性進(jìn)行研究時(shí)，可以充分地利用障礙物的邊緣輪廓來(lái)獲得特征信息[13]。因此，在此基礎(chǔ)上，對(duì)分割后的圖像進(jìn)行邊緣提取，并將其用于下一步的處理。分別計(jì)算各個(gè)圖像塊中相應(yīng)的輪廓點(diǎn)數(shù)量，進(jìn)而得出障礙物的特征提取結(jié)果如下：

Φ=[φ1,φ2,φ3,φ4,φ5]

(11)

式中，φ1表示導(dǎo)航控制的特征量，φ2表示導(dǎo)航控制的數(shù)據(jù)節(jié)點(diǎn)，φ3表示特征信息融合參數(shù)，φ4表示飛行障礙的序列節(jié)點(diǎn)，φ5為目標(biāo)的移動(dòng)速度。最終將提取的目標(biāo)特征與設(shè)置的障礙物邊緣輪廓特征進(jìn)行匹配，便可以確定當(dāng)前無(wú)人機(jī)飛行區(qū)域是否存在障礙物及其類型[14]。將障礙物的識(shí)別結(jié)果添加到創(chuàng)建的環(huán)境地圖中，并將障礙物所在位置的柵格轉(zhuǎn)換為“0”，即不予通過(guò)標(biāo)記處理。

1.5 利用粒子濾波算法確定無(wú)人機(jī)位姿

無(wú)人機(jī)位姿估計(jì)可以視為一個(gè)概率推測(cè)的過(guò)程，采用粒子濾波可以將無(wú)人機(jī)位姿估計(jì)問(wèn)題轉(zhuǎn)換為解算后驗(yàn)概率密度問(wèn)題，從而得到最優(yōu)的目標(biāo)狀態(tài)估計(jì)[15]。粒子濾波算法的基本流程如圖3所示。

圖3 粒子濾波算法流程圖

(12)

其中:Dt和xt分別為實(shí)時(shí)視覺(jué)圖像及求得狀態(tài)，P(xt∣Dt)為利用所獲得實(shí)時(shí)信息求得狀態(tài)的后驗(yàn)概率密度函數(shù)[17]。在此基礎(chǔ)上根據(jù)概率密度分布生成N個(gè)新的粒子，并利用公式(13)計(jì)算生成新粒子的權(quán)重值。

(13)

(14)

由公式(14)所得到的結(jié)果為粒子濾波的最優(yōu)解。通過(guò)加權(quán)、概率分布對(duì)微粒進(jìn)行再抽樣，形成N個(gè)新的、獨(dú)立的、隨機(jī)粒子。最后，用粒子濾波方法得到的無(wú)人飛行器姿態(tài)數(shù)據(jù)進(jìn)行了分析，當(dāng)發(fā)現(xiàn)與目標(biāo)的初始模板存在較大的偏差時(shí)，說(shuō)明該方法的跟蹤失效。因此，將會(huì)對(duì)該算法進(jìn)行初始化，并對(duì)無(wú)人機(jī)實(shí)時(shí)位姿進(jìn)行重新判斷[18]。通過(guò)粒子濾波算法的運(yùn)行，得出任意時(shí)刻無(wú)人機(jī)位姿的量化結(jié)果如下：

(15)

以公式(15)計(jì)算得出的無(wú)人機(jī)實(shí)時(shí)航行位置作為視覺(jué)導(dǎo)航的起點(diǎn)位置，并考慮無(wú)人機(jī)航行模型，對(duì)其姿態(tài)進(jìn)行調(diào)整。

1.6 規(guī)劃無(wú)人機(jī)自主飛行軌跡

以當(dāng)前無(wú)人機(jī)的位置確定結(jié)果為飛行軌跡的規(guī)劃起點(diǎn)，采用啟發(fā)式方法，使搜索節(jié)點(diǎn)沿目標(biāo)方向運(yùn)動(dòng)，從而減少了運(yùn)算速度，提高了運(yùn)算速度。與此同時(shí)，該方法還以初始結(jié)點(diǎn)至當(dāng)前結(jié)點(diǎn)的代數(shù)為總路徑代價(jià)，以防止在該路徑上遇到障礙物。在飛行軌跡規(guī)劃過(guò)程中，首先采用代價(jià)函數(shù)對(duì)路徑中的柵格節(jié)點(diǎn)進(jìn)行評(píng)估，評(píng)估函數(shù)可以表示為：

f(n)=ψ(n)+r(n)

(16)

式中，參數(shù)n代表柵格節(jié)點(diǎn)，ψ(n)和r(n)分別為由起始節(jié)點(diǎn)到柵格節(jié)點(diǎn)n以及當(dāng)前柵格節(jié)點(diǎn)到導(dǎo)航目標(biāo)終點(diǎn)的評(píng)估代價(jià)值，f(n)則為全局評(píng)估代價(jià)值。由此可以確定創(chuàng)建的環(huán)境地圖中，任意一個(gè)柵格的允許通過(guò)狀態(tài)。在無(wú)人機(jī)航行的軌跡規(guī)劃過(guò)程中，首先，將無(wú)人機(jī)的初始航行位置納入open表中。通過(guò)對(duì)現(xiàn)有的結(jié)點(diǎn)進(jìn)行擴(kuò)充，計(jì)算每個(gè)待擴(kuò)充結(jié)點(diǎn)的代值，選取由公式(16)所輸出的最小值節(jié)點(diǎn)，將其添加到 closed表格中。判斷擴(kuò)展節(jié)點(diǎn)是否為軌跡導(dǎo)航終點(diǎn)，若判斷結(jié)果為否，則繼續(xù)向前進(jìn)行擴(kuò)展，直至達(dá)到目標(biāo)點(diǎn)為止。如果open表中的柵格節(jié)點(diǎn)為空，則表示無(wú)可以被擴(kuò)充的節(jié)點(diǎn)，若此時(shí)未到達(dá)導(dǎo)航軌跡終點(diǎn)，則表示無(wú)人機(jī)自主飛行軌跡規(guī)劃失敗，需要重新規(guī)劃[19]。由于在障礙物識(shí)別過(guò)程中，將識(shí)別結(jié)果添加到環(huán)境地圖中，并對(duì)障礙物所處位置進(jìn)行柵格標(biāo)記，因此在軌跡規(guī)劃過(guò)程中無(wú)需進(jìn)行進(jìn)一步障礙物篩選與判斷，在一定程度上提高了無(wú)人機(jī)航行軌跡的規(guī)劃效率和質(zhì)量。無(wú)人機(jī)軌跡規(guī)劃流程圖如圖4所示。

圖4 無(wú)人機(jī)軌跡規(guī)劃流程圖

1.7 計(jì)算無(wú)人機(jī)導(dǎo)航控制量

無(wú)人機(jī)視覺(jué)導(dǎo)航控制的最終目的就是保證實(shí)時(shí)無(wú)人機(jī)的運(yùn)行姿態(tài)與參數(shù)與規(guī)劃的路徑一致，根據(jù)無(wú)人機(jī)航行軌跡的規(guī)劃結(jié)果，可以確定各個(gè)軌跡點(diǎn)位置上無(wú)人機(jī)的預(yù)期位姿，將其表示為：

(17)

其中:xt、yt和zt為軌跡點(diǎn)預(yù)期位置坐標(biāo)，vxt、vyt和vzt對(duì)應(yīng)的是無(wú)人機(jī)航行速度在x、y、z三個(gè)方向上的分量，而φ、θ和φ表示的是滾轉(zhuǎn)角、俯仰角和偏航角。利用粒子濾波算法可以直接得出無(wú)人機(jī)相應(yīng)時(shí)刻的所處位置坐標(biāo)和飛行運(yùn)動(dòng)參數(shù)，并將其記為Xactual-t，那么無(wú)人機(jī)視覺(jué)導(dǎo)航的控制量可以表示為：

Δ=|Xexpect-t-Xactual-t|s(v)

(18)

式中，s(v)表示控制方向函數(shù)，由此可以得出無(wú)人機(jī)軌跡位置、姿態(tài)角度以及航行速度3個(gè)方面的控制量計(jì)算結(jié)果。公式(18)中|·|為絕對(duì)值計(jì)算符號(hào)，能夠保證計(jì)算得出的控制量始終為正數(shù)。但在實(shí)際的控制過(guò)程中，需要判斷無(wú)人機(jī)預(yù)期運(yùn)行參數(shù)與實(shí)際參數(shù)之間的大小，從而確定具體的導(dǎo)航控制方向。

1.8 安裝無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制器

無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制器是導(dǎo)航控制任務(wù)的硬件支持設(shè)備，能夠?qū)崿F(xiàn)驅(qū)動(dòng)、開(kāi)關(guān)狀態(tài)調(diào)整等功能，同時(shí)能夠?qū)⒂?jì)算的無(wú)人機(jī)導(dǎo)航控制量數(shù)據(jù)轉(zhuǎn)換成控制信號(hào)，直接作用在無(wú)人機(jī)設(shè)備上。由于無(wú)人機(jī)自主軌跡導(dǎo)航需要從3個(gè)方面執(zhí)行，因此需要連接3個(gè)不同功能的控制器，并利用姿態(tài)控制回路，速度控制回路和位置控制回路，完成控制器的連接。安裝的無(wú)人機(jī)自主UI及視覺(jué)導(dǎo)航控制器結(jié)構(gòu)如圖5所示。

圖5 無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制器結(jié)構(gòu)圖

控制器為深圳市捷紅昌科技有限公司，型號(hào)為JHC-JQW-B，最高工作頻率為128 GHz，外部設(shè)備齊全，內(nèi)置雙精度FPU，并支持多種 DSP指令。特別是FPU和 DSP指令集的設(shè)計(jì)，通過(guò)位置控制器、速度控制器和姿態(tài)角控制器，得到帶限幅的PID，得到后將其輸入到驅(qū)動(dòng)模塊，從而實(shí)現(xiàn)卡爾曼濾波器的實(shí)時(shí)控制。在實(shí)際操作中，將三軸的轉(zhuǎn)速和所得到的轉(zhuǎn)速輸入到轉(zhuǎn)速控制器中，再由轉(zhuǎn)速控制器計(jì)算出所需的轉(zhuǎn)子總升力和目標(biāo)姿態(tài)角控制值，再由姿態(tài)控制器控制4個(gè)轉(zhuǎn)子的張力，從而達(dá)到對(duì)飛機(jī)的姿態(tài)控制。另外，為了降低無(wú)人機(jī)飛行環(huán)境中無(wú)線電或其他信號(hào)對(duì)控制效果產(chǎn)生的干擾，在控制器內(nèi)部加設(shè)一個(gè)自抗擾芯片，在保證控制精度的同時(shí)也最大程度地保護(hù)控制器設(shè)備的運(yùn)行安全。

1.9 實(shí)現(xiàn)無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制

將計(jì)算得出的無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制量輸入到安裝的控制器中，分別從位置、角度和速度3個(gè)方面完成導(dǎo)航控制任務(wù)。以姿態(tài)角中的偏航角為例，利用導(dǎo)航控制器對(duì)操縱桿進(jìn)行操舵，使其產(chǎn)生相應(yīng)的轉(zhuǎn)向，從而實(shí)現(xiàn)對(duì)無(wú)人機(jī)的轉(zhuǎn)向和保持。假設(shè)當(dāng)前無(wú)人機(jī)處于直線飛行狀態(tài)，則偏航角控制率可表示為：

φexpect=φactual-tan-1(kpΔφ)

(19)

其中:φactual表示控制速度函數(shù)，Δφ為偏航角控制量，其具體取值可以通過(guò)公式(18)計(jì)算得出，kp為偏航角比例系數(shù)，根據(jù)控制器內(nèi)部結(jié)構(gòu)決定該參數(shù)的具體取值[20]。同理可以得出其他姿態(tài)角以及位置、速度控制律，在控制器的支持下，完成對(duì)無(wú)人機(jī)飛行參數(shù)的實(shí)時(shí)導(dǎo)航控制，保證無(wú)人機(jī)的實(shí)際飛行軌跡與規(guī)劃軌跡一致。

2 實(shí)測(cè)分析

以測(cè)試設(shè)計(jì)的基于粒子濾波的無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制方法的應(yīng)用效果為目的，針對(duì)導(dǎo)航控制效果進(jìn)行測(cè)試，并通過(guò)與預(yù)設(shè)值的對(duì)比，體現(xiàn)出設(shè)計(jì)控制方法的性能優(yōu)勢(shì)。

2.1 選擇無(wú)人機(jī)研究對(duì)象

此次實(shí)驗(yàn)以MS-680型四旋翼無(wú)人機(jī)為實(shí)驗(yàn)樣本，該機(jī)的結(jié)構(gòu)以軸對(duì)稱為主，結(jié)構(gòu)較為簡(jiǎn)潔，4個(gè)轉(zhuǎn)子均勻地排列在4個(gè)轉(zhuǎn)子上，中間裝有電池、控制器等機(jī)載設(shè)備。4個(gè)螺旋槳為飛機(jī)的飛行提供動(dòng)力，而起落架則是對(duì)機(jī)身的支持。采用外轉(zhuǎn)式無(wú)刷電動(dòng)機(jī)C28-30-15，并配備與電動(dòng)機(jī)相匹配的電動(dòng)機(jī)調(diào)速器，以保證其在空中的穩(wěn)定運(yùn)行。選擇的外轉(zhuǎn)式無(wú)刷電動(dòng)機(jī)，其額定電源電壓是22 V，最大電流可達(dá)23 A。兩個(gè)對(duì)稱的馬達(dá)軸距為620毫米，推進(jìn)器采用14寸塑料三片槳葉。根據(jù)對(duì)電動(dòng)機(jī)運(yùn)行電流的粗略測(cè)定，四臺(tái)電動(dòng)機(jī)在懸掛狀態(tài)下的最高功率不大于25 A，選用22.0 V的鋰電池，其容量為600米 Ah，可持續(xù)30分鐘。為了最大限度地降低機(jī)身的重量，除了發(fā)動(dòng)機(jī)支架和起落架，其他部件均由碳纖維制造，電機(jī)底座則是由尼龍制成。電池已安裝完畢。這架無(wú)人機(jī)的總重量是1 560 g，最大載荷是600 g。

2.2 配置實(shí)驗(yàn)環(huán)境

從基于粒子濾波的無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制方法的開(kāi)發(fā)環(huán)境，以及其應(yīng)用環(huán)境兩個(gè)方面進(jìn)行實(shí)驗(yàn)環(huán)境的配置，其中利用導(dǎo)航控制方法對(duì)開(kāi)發(fā)環(huán)境進(jìn)行構(gòu)建，將控制器以及傳感器設(shè)備安裝到無(wú)人機(jī)上，并在地面上設(shè)置一個(gè)控制中心，要求控制中心發(fā)出的控制指令能夠通過(guò)無(wú)線通信網(wǎng)絡(luò)被無(wú)人機(jī)接收。而設(shè)置的導(dǎo)航控制方法應(yīng)用環(huán)境分別為室外環(huán)境和室內(nèi)環(huán)境。室外環(huán)境選擇的是某城市的主干道和居民區(qū)，在滿足無(wú)人機(jī)飛行條件的同時(shí)，提供了天然的障礙物條件。而室內(nèi)環(huán)境主要選擇的是某大型建筑的走廊以及房屋空間，在室內(nèi)環(huán)境中需要通過(guò)加設(shè)紙箱、桌椅等設(shè)施，實(shí)現(xiàn)環(huán)境障礙物的設(shè)置。

2.3 試飛準(zhǔn)備與調(diào)試

試飛準(zhǔn)備調(diào)試的內(nèi)容包括：控制信號(hào)能否正常被無(wú)人機(jī)接收、導(dǎo)航控制方法能否在無(wú)人機(jī)設(shè)備上運(yùn)行、視覺(jué)模塊運(yùn)行是否正常、無(wú)人機(jī)飛行動(dòng)態(tài)參數(shù)是否能夠及時(shí)采集與傳輸?shù)?。選擇人口稀少的郊區(qū)，且要求試飛區(qū)域具有專用的起飛跑道，為起飛、降落等模擬飛行任務(wù)提供條件。在無(wú)高大建筑和樹(shù)木的干擾下，最大限度地降低了無(wú)人機(jī)在空中的干擾。選擇的無(wú)人機(jī)研究對(duì)象能夠在配置的實(shí)驗(yàn)環(huán)境中正常飛行，可以執(zhí)行無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制測(cè)試任務(wù)。

2.4 描述實(shí)測(cè)過(guò)程

在配置的實(shí)驗(yàn)環(huán)境中選擇多個(gè)導(dǎo)航終點(diǎn)，在考慮環(huán)境障礙物的前提下，生成無(wú)人機(jī)自主航行軌跡的測(cè)試樣本，以此作為無(wú)人機(jī)的理想航行軌跡。每個(gè)實(shí)驗(yàn)環(huán)境中設(shè)置4條測(cè)試軌跡，實(shí)驗(yàn)中設(shè)置的測(cè)試軌跡數(shù)量為8，由此將實(shí)驗(yàn)劃分成8個(gè)組別，記錄生成軌跡上各個(gè)點(diǎn)的軌跡坐標(biāo)以及無(wú)人機(jī)到達(dá)的時(shí)刻。將理想航行軌跡數(shù)據(jù)輸入到無(wú)人機(jī)的飛行程序中，并同時(shí)啟動(dòng)無(wú)人機(jī)、控制器以及基于粒子濾波的無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制程序。在無(wú)人機(jī)飛行過(guò)程中，實(shí)時(shí)采集環(huán)境視覺(jué)圖像數(shù)據(jù)，并根據(jù)采集結(jié)果調(diào)整實(shí)際飛行軌跡。圖6為無(wú)人機(jī)視覺(jué)圖像的輸出結(jié)果。

圖6 無(wú)人機(jī)視覺(jué)圖像輸出界面

在程序以及控制器的協(xié)同作用下，完成無(wú)人機(jī)的導(dǎo)航控制任務(wù)，最終的控制結(jié)果如圖7所示。

圖7 無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制界面

同理可以得出所有控制任務(wù)的執(zhí)行結(jié)果，最終調(diào)取控制方法的后臺(tái)運(yùn)行數(shù)據(jù)，獲得無(wú)人機(jī)的實(shí)時(shí)運(yùn)行參數(shù)信息。

2.5 設(shè)置軌跡視覺(jué)導(dǎo)航控制效果測(cè)試指標(biāo)

由于設(shè)計(jì)的導(dǎo)航控制方法分別從位置、速度和姿態(tài)角3個(gè)方面進(jìn)行控制，因此設(shè)置的控制效果測(cè)試指標(biāo)分別為位置控制誤差、速度控制誤差和姿態(tài)角控制誤差，其中位置控制誤差的數(shù)值結(jié)果為：

(20)

其中:(xac,yac,zac)和(xex,yex,zex)分別為無(wú)人機(jī)的理想軌跡坐標(biāo)和實(shí)際軌跡坐標(biāo)。另外其他兩個(gè)測(cè)試指標(biāo)的數(shù)值結(jié)果如下：

(21)

式中,νac和νex為目標(biāo)控制速度和實(shí)際速度，φac、θac、φac、φex、θex和φex對(duì)應(yīng)的是滾轉(zhuǎn)角、俯仰角、偏航角的目標(biāo)值和實(shí)際值。計(jì)算得出的誤差值越大，證明導(dǎo)航控制方法的控制效果越差，因此設(shè)定εxyz、εv和εφθφ的預(yù)設(shè)值分別為3.0 m、0.2 m/s和0.1°，且要求應(yīng)用設(shè)計(jì)導(dǎo)航控制方法下，無(wú)人機(jī)的實(shí)際飛行數(shù)據(jù)不高于預(yù)設(shè)值。

2.6 實(shí)測(cè)結(jié)果分析

通過(guò)相關(guān)數(shù)據(jù)的提取及其與目標(biāo)數(shù)據(jù)的比對(duì)，得出最終的控制效果測(cè)試結(jié)果。位置控制誤差的量化測(cè)試結(jié)果，如圖8所示。

圖8 無(wú)人機(jī)飛行軌跡位置誤差測(cè)試結(jié)果

將圖8中的數(shù)據(jù)代入到公式(20)中，計(jì)算得出無(wú)人機(jī)的位置控制誤差的平均值為2.1 m，低于預(yù)設(shè)值。同理可以得出反映無(wú)人機(jī)飛行速度和姿態(tài)角控制效果的測(cè)試結(jié)果，如表1所示。

表1 無(wú)人機(jī)飛行速度和姿態(tài)角控制效果測(cè)試數(shù)據(jù)表

將表1中的數(shù)據(jù)代入到公式(21)中，計(jì)算得出應(yīng)用控制方法下，無(wú)人機(jī)的平均速度控制誤差為0.063 m/s，姿態(tài)角控制誤差的平均值為0.05°，均低于預(yù)設(shè)值，由此證明設(shè)計(jì)的基于粒子濾波的無(wú)人機(jī)自主軌跡視覺(jué)導(dǎo)航控制方法具有良好的控制效果。根據(jù)上述數(shù)據(jù)，得到速度誤差如圖9所示。

圖9 速度控制誤差對(duì)比圖

根據(jù)圖9可知，本文方法的速度控制誤差最低為0.07%，文獻(xiàn)[3]方法的速度控制誤差最低為0.15%，文獻(xiàn)[4]方法的速度控制誤差最低為0.17%。由此可知，本文方法的速度控制誤差最低，可有效提高控制效果。姿態(tài)控制誤差如10所示。

根據(jù)圖10可知，本文方法的速度控制誤差最低為14%，文獻(xiàn)[3]方法的姿態(tài)控制誤差最低為25%，文獻(xiàn)[4]方法的姿態(tài)控制誤差最低為40%。由此可知，本文方法的姿態(tài)控制誤差最低。

圖10 姿態(tài)控制誤差對(duì)比

3 結(jié)束語(yǔ)

無(wú)人機(jī)作為重要的輔助工具，其飛行質(zhì)量直接影響到無(wú)人機(jī)的應(yīng)用價(jià)值。通過(guò)粒子濾波算法的應(yīng)用，對(duì)處理非線性、非高斯動(dòng)態(tài)系統(tǒng)相關(guān)問(wèn)題的有效性已形成共識(shí)，進(jìn)而有效地解決了現(xiàn)有控制方法存在的控制精度等問(wèn)題。然而受到時(shí)間和空間的限制，實(shí)驗(yàn)設(shè)置的測(cè)試次數(shù)較少，因此得出的實(shí)驗(yàn)結(jié)果存在一定的局限性，需要在今后的研究工作中進(jìn)一步補(bǔ)充與優(yōu)化。