索文凱，胡文剛，班利明，林源和，錢 樂

（1.陸軍工程大學(xué)石家莊校區(qū) 電子與光電工程系，石家莊050003；2.中國人民解放軍32272部隊，蘭州730000；3.中國人民解放軍西部戰(zhàn)區(qū)陸軍保障部，蘭州730000）

引 言

近年來，在應(yīng)用市場多元激勵的作用下，無人機(jī)（unmarned aerial vehide，UAV）的研發(fā)經(jīng)歷了前所未有的變革。伴隨著各種單一功能和多用途軍用無人機(jī)實踐應(yīng)用的日益成熟化和產(chǎn)品化，無人機(jī)在民事領(lǐng)域的應(yīng)用前景也越來越廣闊。與此同時，用無人機(jī)執(zhí)行任務(wù)與反無人機(jī)介入的斗爭也愈演愈烈。在應(yīng)用市場、任務(wù)背景和應(yīng)對多重挑戰(zhàn)等因素的作用下，無人機(jī)的獨(dú)立性、自主性、實時性要求越來越高。因此，如何快速準(zhǔn)確地獲取無人機(jī)和目標(biāo)的飛姿信息，并將之高效精準(zhǔn)地轉(zhuǎn)化為無人機(jī)的飛控參量成為無人機(jī)邁向智能化的關(guān)鍵環(huán)節(jié)［1］。

在無人機(jī)領(lǐng)域的研究中，基于視覺圖像的位姿信息解算與參量轉(zhuǎn)換研究正逐漸成為無人機(jī)自主化研究的熱點(diǎn)，其核心是從連續(xù)的2維視覺圖像及高度信息中獲取無人機(jī)六自由度信息，利用牛頓力學(xué)和動量力矩等定理完成運(yùn)動方程的構(gòu)建和位姿信息的轉(zhuǎn)換。參考文獻(xiàn)［2］中通過對比無人機(jī)輸入航跡與自身航跡的偏差，計算方向向量與加速度向量，利用比例-積分-微分（proportion-integration-differentiation，PID）控制算法完成無人機(jī)航跡和姿態(tài)進(jìn)行控制。參考文獻(xiàn)［3］中提出了一種結(jié)合學(xué)習(xí)特征的圖像視覺伺服方法，利用統(tǒng)一投影模型，分析了虛擬單位球體投影的解耦視覺伺服方法，研究了結(jié)合非線性模型預(yù)測控制的球體投影約束視覺伺服控制方法，并設(shè)計出結(jié)合離散化參考軌跡的無標(biāo)定視覺預(yù)測控制算法。參考文獻(xiàn)［4］中介紹了神經(jīng)網(wǎng)絡(luò)補(bǔ)償結(jié)構(gòu)的作用和設(shè)計方法，基于無人機(jī)非線性姿態(tài)運(yùn)動學(xué)和動力學(xué)模型設(shè)計了基于神經(jīng)網(wǎng)絡(luò)補(bǔ)償?shù)膭討B(tài)逆控制器，并進(jìn)行了數(shù)學(xué)仿真實驗。本文中主要進(jìn)行無人機(jī)飛行六自由度信息的解算、運(yùn)動模型轉(zhuǎn)換以及動態(tài)跟蹤方法的研究，并通過仿真實驗對所提出的方法進(jìn)行驗證。

1 無人機(jī)的結(jié)構(gòu)、控制原理及過程

經(jīng)過應(yīng)用環(huán)境的選擇和淘汰，無人機(jī)主要分固定翼、旋翼和直升機(jī)三大類，其中基于視覺導(dǎo)航跟蹤及自主控制的研究又以旋翼無人機(jī)為主。旋翼無人機(jī)包括四旋翼、六旋翼和其它多旋翼無人機(jī)，四旋翼無人機(jī)又分為“十”型和“X”型兩類［5］。作者以“十”型無人機(jī)為例，進(jìn)行了相關(guān)理論研究和仿真實驗。

1.1 布局結(jié)構(gòu)

“十”型四旋翼無人機(jī)的結(jié)構(gòu)如圖1所示。從圖中可以看出兩條機(jī)臂成90°相交于無人機(jī)的質(zhì)心，1號～4號旋翼和電機(jī)分別安裝于無人機(jī)飛行空間的前后左右4個方位，其結(jié)構(gòu)、大小完全一樣，且關(guān)于質(zhì)心對稱，每個旋翼和電機(jī)距離質(zhì)心的長度相同，電機(jī)控制旋翼轉(zhuǎn)速產(chǎn)生升力。在調(diào)整無人機(jī)飛行姿態(tài)時，通過電機(jī)調(diào)整旋翼的轉(zhuǎn)速，改變旋翼的總力矩。轉(zhuǎn)速調(diào)整越大，總力矩變化越大，無人機(jī)飛姿調(diào)整就越快。由牛頓定理可知，旋翼在旋轉(zhuǎn)的同時，會向電機(jī)施加一個反作用力（又稱反扭矩），為了消除這種反作用力，就需要在同一方向上施加另一種反方向的力［6］。因此，四旋翼無人機(jī)的4個旋翼中相鄰兩個旋翼的方向是相反的。在無人機(jī)飛行時，1號、3號旋翼和2號、4號旋翼產(chǎn)生的順時針和逆時針反作用力相互抵消，無人機(jī)就可以保持平穩(wěn)飛行。

Fig.1 Structure of the“十”four-rotor UAV

1.2 控制原理

展開無人機(jī)控制原理的分析之前，先介紹無人機(jī)飛行的六自由度信息，分為質(zhì)心的3個沿軸移動自由度和3個繞軸轉(zhuǎn)動自由度，分別對應(yīng)無人機(jī)的空間位置（前后量x，側(cè)向量y，升降量z）和無人機(jī)姿態(tài)（俯仰角 θ，滾轉(zhuǎn)角 φ，偏航角 ψ），即六自由度信息｛x，y，z，θ，φ，ψ｝，或六自由度速度信息，或六自由度加速度信息｝［7-8］。

無人機(jī)的飛行模式對應(yīng)其六自由度信息，即前后、側(cè)向、升降、俯仰、滾轉(zhuǎn)和偏航運(yùn)動。四旋翼無人機(jī)的不同飛行模式主要通過調(diào)整4個控制參量，使4個旋翼電機(jī)產(chǎn)生不同的旋轉(zhuǎn)速度，提供4個不同大小的升力（累計為總升力Fsum）與重力m g（m為質(zhì)量，g為重力加速度）矢量合成，產(chǎn)生指定方向和角度的外力Fext，控制無人機(jī)按照外力的方向飛行，如圖2所示。當(dāng)外力方向與重力方向同軸時，無人機(jī)進(jìn)行懸停、升降飛行，當(dāng)外力方向與重力方向垂直時，無人機(jī)進(jìn)行橫移、偏航飛行，當(dāng)外力方向與重力方向成其它角度時，無人機(jī)進(jìn)行俯仰、滾轉(zhuǎn)飛行。

Fig.2 Schematic diagram of the action of the four-rotor UAV

1.3 控制過程

四旋翼無人機(jī)可進(jìn)行6個自由度的飛行運(yùn)動，但是只有4個獨(dú)立的控制輸入量，因此其是1種欠驅(qū)動系統(tǒng)。由牛頓定理可知，升力隨著轉(zhuǎn)速的變化而變化，所以通過調(diào)節(jié)旋翼電機(jī)的轉(zhuǎn)速就可以控制無人機(jī)的飛行姿態(tài)。無人機(jī)處于懸停狀態(tài)時，4個旋翼提供的合力剛好等于無人機(jī)自身的重力，即外力Fext＝0，設(shè)定此時旋翼的轉(zhuǎn)速為N0，當(dāng)改變部分或全部旋翼的轉(zhuǎn)速時，外力Fext≠0，無人機(jī)就會按照外力的方向進(jìn)行飛行，如圖3所示。圖中 Δa，Δb，Δa′為旋翼的轉(zhuǎn)速調(diào)整變量，其中Δa，Δa′為一組對應(yīng)的變量，當(dāng)同一軸上的旋翼分別為N0+Δa，N0-Δa時，無人機(jī)只存在此軸上的外力，此時無人機(jī)沿該軸做直線運(yùn)動，如圖3a～圖3c所示；當(dāng)同一軸上的旋翼分別為N0+Δa，N0-Δb時，無人機(jī)存在該軸與z軸組成平面的外力，此時無人機(jī)繞另一軸做繞軸運(yùn)動，如圖3d～圖3e所示；當(dāng)不同軸上的旋翼分別為 N0+Δa，N0-Δa時，無人機(jī)存在Ouxuyu平面的外力，此時無人機(jī)繞z運(yùn)動，如圖3f所示。

Fig.3 Control process of four-rotor UAV

2 模型構(gòu)建與參量轉(zhuǎn)換

2.1 空間模型

無人機(jī)飛行過程中有兩個通用的坐標(biāo)系，即無人機(jī)質(zhì)心坐標(biāo) u系 Ouxuyuzu和地面坐標(biāo) g系Ogxgygzg

［9-10］。與不斷變化的質(zhì)心坐標(biāo)系不同，地面坐標(biāo)系是固定的，如圖4所示。為便于理解，必須先將無人機(jī)的姿態(tài)角進(jìn)行明確的定義，從圖中可以看出，俯仰角θ是質(zhì)心坐標(biāo)系xu軸與地面坐標(biāo)系Ogxgyg平面的夾角，以O(shè)gxgyg平面上方為正；滾轉(zhuǎn)角φ是質(zhì)心坐標(biāo)系yu軸與地面坐標(biāo)系Ogxgyg平面的夾角，以O(shè)gxgyg平面上方為正；偏航角ψ是質(zhì)心坐標(biāo)系zu軸在地面坐標(biāo)系Ogxgyg平面的投影與地面坐標(biāo)系xg軸夾角，角度值以xg軸為起點(diǎn)，以東偏北逆時針方向為正。

Fig.4 Spatial model construction

2.2 運(yùn)動方程

四旋翼無人機(jī)運(yùn)動方程主要包括物像關(guān)系式、質(zhì)心運(yùn)動方程和地面坐標(biāo)運(yùn)動方程［11-12］，如下所示：

（1）式為物像關(guān)系式，式中，C0為相機(jī)已知的內(nèi)參系數(shù)，其中 d x，d y為像元素大小，（iO，jO）為質(zhì)心 Ou在像平面的坐標(biāo)，f為相機(jī)的焦距；（2）式為無人機(jī)質(zhì)心運(yùn)動方程，式中，（xu，0，yu，o，zu，o）為無人機(jī)在 g系的初始坐標(biāo)，（xu，yu，zu）為無人機(jī)質(zhì)心在無人機(jī)坐標(biāo)系下實時坐標(biāo)，（xg，yg，zg）為無人機(jī)質(zhì)心在地面坐標(biāo)系的實時坐標(biāo)，Lu，g為無人機(jī)坐標(biāo)系相對于地面坐標(biāo)系的轉(zhuǎn)換系數(shù)，且滿足：

式中，θ，φ，ψ為無人機(jī)的三軸姿態(tài)角；（3）式為地面坐標(biāo)運(yùn)動方程，其中，b為旋翼電機(jī)的升力系數(shù)，Ωi為旋翼電機(jī)的轉(zhuǎn)速，i為1號 ～4號電機(jī)；K＝｛k1k2k3｝，分別為無人機(jī)在地面坐標(biāo)系x軸、y軸、z軸3軸的空氣阻力系數(shù)，綜上可得，在地面坐標(biāo)系下，無人機(jī)運(yùn)動方程為：

2.3 參量轉(zhuǎn)換

四旋翼無人機(jī)的控制參量主要是用來控制無人機(jī)的四旋翼電機(jī)，給無人機(jī)提供特定方向的動力，因此無人機(jī)的欠輸入?yún)⒘颗c無人機(jī)的飛行動力緊密相關(guān)。由于四旋翼無人機(jī)的質(zhì)心即為其中心，所以其姿態(tài)轉(zhuǎn)動可以視為剛體繞質(zhì)心的定點(diǎn)轉(zhuǎn)動。根據(jù)轉(zhuǎn)動力學(xué)定理可知，地面坐標(biāo)系下四旋翼無人機(jī)的力學(xué)方程［13-15］如下式所示：

式中，L為角動量，I為轉(zhuǎn)動慣量，ω為角速度，M為無人機(jī)的外力矩，（d L／d t）u為無人機(jī)質(zhì)心坐標(biāo)系下的角動量的變化率。

無人機(jī)轉(zhuǎn)動過程中，作用力主要有旋翼產(chǎn)生的動力和空氣阻力。由牛頓定理和歐拉轉(zhuǎn)動定理可知，“十”型四旋翼的轉(zhuǎn)動力矩τ如下式所示：式中，L為旋翼中心距質(zhì)心的距離，τa（τa，x，τa，y，τa，z）為空氣阻力力矩，τF為動力阻力力矩。

繼而可得十字型無人機(jī)的角運(yùn)動方程如（9）式所示，考慮到四旋翼無人機(jī)飛行速度較小，受到空氣阻力作用也較小，可將空氣阻力忽略，得到（10）式：

設(shè)無人機(jī)的四輸入控制量的力矩分別為俯仰力矩μ1、滾轉(zhuǎn)力矩 μ2、偏航力矩 μ3、爬升力矩 μ4，可得下式：

忽略空氣阻力的作用，由（5）式、（10）式可得無人機(jī)的六自由度系統(tǒng)方程［16］如下式所示：

無人機(jī)實際飛行中，通常只涉及俯仰飛行和偏航飛行，特殊條件下才伴有滾轉(zhuǎn)飛行，因此對滾轉(zhuǎn)角暫不進(jìn)行研究。本文中基于視覺圖像的無人機(jī)飛姿控制研究核心內(nèi)容就是：通過初始狀態(tài)下無人機(jī)的定速定向飛行，識別已知運(yùn)動狀態(tài)的特定地面合作目標(biāo)，通過圖像中目標(biāo)像點(diǎn)位移情況，推算無人機(jī)的飛姿信息和高度，并利用無人機(jī)航跡與合作目標(biāo)軌跡的夾角等信息，得出無人機(jī)完成跟蹤或降落過程所需要的飛控參量。從前面可知，“十”型四旋翼無人機(jī)初始高度H0定向飛行時，質(zhì)心坐標(biāo)系與地面坐標(biāo)系只存在一個夾角也即俯仰角θ0，由于此時無人機(jī)沒有進(jìn)行滾轉(zhuǎn)飛行即φ0＝0，因此此時無人機(jī)航跡與合作目標(biāo)的航跡的像平面夾角也即偏航角ψ0，無人機(jī)要完成跟蹤或降落功能，就需要對航跡進(jìn)行修正，修正量即為 H0，θ0，ψ0。由參考文獻(xiàn)［17］和參考文獻(xiàn) ［18］可知，θn＝arctan［Δan*Hn／（fa）］，θ˙n＝（θn-θn-1）／Δt，θ¨n＝（θ˙nθ˙n-1）／Δt，ψn＝arctan［（ani*d y）／（anj*d x）］，ψ˙n＝（ψn-ψn-1）／Δt，ψ¨n＝（ψ˙n-ψ˙n-1）／Δt，其中，a為合作目標(biāo)外邊長度，an*為n時刻外邊a的像素值，Δan*為兩條相對外邊像的長度差，ani*，anj*分別為 an*在像素坐標(biāo)的行寬和列寬。所以基于視覺圖像的四旋翼無人機(jī)四輸入控制量為：

3 目標(biāo)識別與跟蹤算法

3.1 目標(biāo)識別與信息提取

Fig.5 Cooperation target

Fig.6 Target recognition and extraction processa—RGB original imag—HSV transformation imag—color threshold processin—inner contour extractio—color region segmentatio—linear defection effec—confour detection effec—geometric feature effect

設(shè)計了一種具有方向引導(dǎo)功能的多形狀組合彩色合作目標(biāo)，如圖5所示。紅色、綠色引導(dǎo)帶指示目標(biāo)移動方向，黑色、白色圖形提升目標(biāo)與周圍環(huán)境的識別率，圓形、正方形能夠提供較為精確地方向和角度信息。根據(jù)合作目標(biāo)獨(dú)有的灰度特征、幾何特征、顏色特征、以及點(diǎn)特征，先后經(jīng)灰度變換、色相、飽和度明度（hue saturation value，HSV）色彩變換、以及 Hu不變距匹配等方法，從機(jī)載相機(jī)拍攝的連續(xù)幀實時圖像中準(zhǔn)確識別出合作目標(biāo)，并獲取有關(guān)的位姿信息。如位移像素 xu，yu，zu，外邊像素 an*，外邊像素差Δan*等。同時，在無人機(jī)低空和超低空飛行時，顏色導(dǎo)引帶可以快速提供準(zhǔn)確的目標(biāo)方向信息，便于無人機(jī)快速的識別和跟蹤合作目標(biāo)，如圖6所示。

3.2 位姿計算與控制算法

根據(jù)同物不同時像平面像素變化情況，利用無人機(jī)物像關(guān)系式、質(zhì)心運(yùn)動方程和地面坐標(biāo)方程等，計算出地面坐標(biāo)系下合作目標(biāo)相對于無人機(jī)的3軸位移變量，再結(jié)合第2.3節(jié)中θn和ψn的公式以及φ0＝0，得出無人機(jī)的3軸角度，相對時間進(jìn)行求導(dǎo)，得出無人機(jī)三軸線速度信息｛˙x，˙y，˙z｝和角速度信息｛˙θ，˙φ，˙ψ｝，進(jìn)而獲得無人機(jī)的實時高度、坐標(biāo)轉(zhuǎn)換矩陣Lu，g等。

使用基于視覺圖像的無人機(jī)飛行控制算法如圖7所示。主要通過提取圖像中目標(biāo)特征點(diǎn)的位移信息，獲取無人機(jī)的實時位姿信息，利用前后兩個固定時間段位姿信息對時間的導(dǎo)數(shù)，得到無人機(jī)的速度信息。以當(dāng)前時刻無人機(jī)與合作目標(biāo)的位姿差信息，作為下一時刻加速度的控制矢量，直至在地面坐標(biāo)系下無人機(jī)與合作目標(biāo)的位姿信息差為0，此時無人機(jī)完成穩(wěn)定跟蹤功能［19-20］，如下式所示：

式中，ΔOc*為合作目標(biāo)當(dāng)前時刻與前一時刻的位移像差。同理，當(dāng)合作目標(biāo)中心與無人機(jī)質(zhì)心在像平面保持重合或近似重合時，無人機(jī)即可完成自主降落功能。

從（14）式可知，當(dāng)Δxg，Δyg，Δzg三者出現(xiàn)數(shù)值時，表示無人機(jī)完成搜索識別階段，當(dāng)Δxg，Δyg兩者為零時，無人機(jī)完成穩(wěn)定跟蹤階段，當(dāng) Δxg，Δyg，Δzg都為零時，即完成自主降落功能。

Fig.7 UAV flight control algorithm based on visual image

4 實驗與分析

為了進(jìn)一步驗證基于視覺圖像的無人機(jī)控制算法的科學(xué)性和可行性，利用MATLAB軟件進(jìn)行了仿真實驗，主要研究不同高度不同速度下無人機(jī)完成識別跟蹤及降落過程的時效性和精準(zhǔn)性。

4.1 參量定義

表1中為仿真實驗的部分參量。

4.2 實驗結(jié)果及分析

本文中無人機(jī)控制系統(tǒng)分為位置控制器和姿態(tài)控制器，利用同物不同時像素位移情況，解算出無人機(jī)與合作目標(biāo)的位姿關(guān)系作為當(dāng)前時刻實測值，通過基于視覺圖像的無人機(jī)飛行控制算法，解算無人機(jī)與合作目標(biāo)位姿差作為下一時刻的期望值，根據(jù)期望值不斷調(diào)整位置與姿態(tài)兩個控制器，進(jìn)而控制不同時刻的實測值，當(dāng)實測值逐漸降為0時，即完成自主降落功能。通過研究同高異初速條件下無人機(jī)與合作目標(biāo)的高度和中心位距離隨時間的變換關(guān)系，驗證算法的可行性和精確性，實驗結(jié)果如圖8和圖9所示。

Table 1 Parameters of simulation experiment

Fig.8 Landing height curve

Fig.9 Center and centroid distance

通過仿真實驗發(fā)現(xiàn)，無人機(jī)初速對跟蹤、靠近、著陸3個階段的影響是不盡相同的。高度方面：跟蹤階段，初速對合作目標(biāo)的識別效率有一定影響，初速為0時，識別時間越短，主要因為懸停時姿態(tài)角為0，便于無人機(jī)搜索識別?？拷A段：初速小時，無人機(jī)z軸方向分量也就小，高度變化就慢，隨著期望位姿信息的逐漸變大，z軸方向分量也隨之變大，高度變換也隨之加大；初速大時，無人機(jī)z軸方向分量也就大，高度變化就快，隨著期望位姿信息的逐漸變小，z軸方向分量也隨之變小，高度變化也隨之減小，達(dá)到最大值時保持不變，降落曲線則表現(xiàn)為類直線。著陸階段，由于經(jīng)過前兩個階段的調(diào)整控制，無人機(jī)的狀態(tài)基本一致，所以初速對著陸階段沒有影響，高度隨z軸速度一致逐漸減為0。距離差方面：跟蹤階段，中心距離都是逐漸增大，初速越大，距離越大，合作目標(biāo)運(yùn)動方向分量大小相同后達(dá)最大值?？拷A段，距離越小，位姿信息變化的期望值越小，距離變化就越緩；距離越大，位姿信息變化的期望值越大，距離變化就越劇烈，同理達(dá)到最大值時保持不變直至進(jìn)入著陸階段。著陸階段，初速對距離差同樣沒有影響，距離隨著無人機(jī)的降落逐漸減為0。

5 結(jié) 論

針對無人機(jī)跟蹤或降落過程中信源易被干擾、環(huán)境感知較差問題，利用視覺圖像的無源性、實時性特性，在同物不同時物像像素變化關(guān)系的基礎(chǔ)上，根據(jù)牛頓力學(xué)定理與剛體轉(zhuǎn)動定理，通過無人機(jī)位姿信息的提取與控制參量的轉(zhuǎn)換，確立了無人機(jī)六自由度信息與四輸入?yún)⒘恐g的關(guān)系，提出了基于視覺圖像的無人機(jī)飛行控制算法，并進(jìn)行了仿真實驗。實驗結(jié)果表明，所提出的控制系統(tǒng)算法在識別跟蹤目標(biāo)后，可以獨(dú)立完成無人機(jī)自主降落功能，克服了無線電信源易干擾有延時的不足，且動態(tài)性能良好。該算法的成熟與應(yīng)用將有助于真正實現(xiàn)無人機(jī)自主化和智能化。