蔣志華，陶德桂

（中國人民解放軍92419部隊，遼寧 興城 125106）

在20世紀(jì)90年代由美國主導(dǎo)的海灣戰(zhàn)爭和科索沃戰(zhàn)爭中，無人機卓有成效地執(zhí)行了多種軍事任務(wù)，其突出的戰(zhàn)績令世界各國刮目相看[1]。隨著高新技術(shù)的發(fā)展，各國相繼投入巨資發(fā)展無人機產(chǎn)業(yè)以滿足軍事需求和其他領(lǐng)域的應(yīng)用，我國近年來更是迎來無人機發(fā)展的井噴期。軍事領(lǐng)域是無人機應(yīng)用的主要領(lǐng)域，軍用無人機的主要用途包括：戰(zhàn)術(shù)偵察和地域監(jiān)視、目標(biāo)定位和火炮校射、電子偵察和干擾、空中通信中繼、戰(zhàn)況攝錄和態(tài)勢評估、靶機和實施攻擊等[2]。針對執(zhí)行不同的使命任務(wù)，無人機可選擇不同的飛行模式，包括遙控飛行模式、程控飛行模式、自主導(dǎo)航飛行模式以及它們的組合飛行模式等，其中作為靶機的無人機要求其具有航路規(guī)劃和自主導(dǎo)航飛行功能，使其能按照預(yù)先裝定的航線精確飛行。某小型無人機采用GPS接收機設(shè)計了自主導(dǎo)航系統(tǒng)實現(xiàn)了上述功能。

1 航路規(guī)劃設(shè)計

該小型無人機采用的是程控+遙控+自主導(dǎo)航的飛行模式，可通過遙控指令在后兩種飛行模式間進行切換。無人機起飛并經(jīng)過一段時間的程控飛行后自動轉(zhuǎn)入自主導(dǎo)航飛行模式。當(dāng)自主導(dǎo)航飛行模式下的無人機高度不滿足任務(wù)要求時，可以通過發(fā)送遙控指令對無人機的飛行高度進行調(diào)整，調(diào)整完后重新選定需要進入自主導(dǎo)航飛行模式下的航點號，并切回自主導(dǎo)航飛行模式繼續(xù)飛行。自主導(dǎo)航的基本設(shè)計思想是利用無人機機載GPS接收機接收到的位置信息作為無人機的當(dāng)前位置，通過計算準(zhǔn)實時求得無人機偏離預(yù)期航線的側(cè)偏距及側(cè)偏距的變化速率，根據(jù)導(dǎo)航控制律解算出舵翼偏角，經(jīng)由飛控系統(tǒng)產(chǎn)生指令控制副翼動作，從而修正飛行航跡，最終消除側(cè)偏距。為此，需要在飛行前規(guī)劃好航路并裝定存儲在無人機內(nèi)。由于GPS系統(tǒng)采用的是WGS?84坐標(biāo)系[3]，為了描述無人機自主飛行的運動軌跡和飛行狀態(tài)，必須選用適當(dāng)?shù)膶?dǎo)航坐標(biāo)系。考慮到小型無人機的飛行距離和可用GPS接收機的精度，定義如下二維導(dǎo)航坐標(biāo)系：起飛點為坐標(biāo)原點，x軸為正東方向，y軸為正北方向，單位為m，則WGS?84坐標(biāo)系轉(zhuǎn)換為導(dǎo)航坐標(biāo)系的公式為[4]：

式中：（L0，B0）、（L，B）分別為導(dǎo)航坐標(biāo)系原點及無人機當(dāng)前位置在WGS?84坐標(biāo)系下的經(jīng)度和緯度值，由GPS接收機獲取，單位為rad，Ax=6 383 487.606 m，Bx=5 357.31m，Ay=6 367 449.134m，By=32 077.0m。

預(yù)期飛行航線通常是由直線段和圓弧段組成，裝定的飛行航線數(shù)據(jù)則為飛行航線中的一些特征點，簡稱航點，其結(jié)構(gòu)為：

typedef struct_flypoints{

int nPt； 航點號

double dLon； 經(jīng)度

double dLat； 緯度

double dR； 半徑（直線段為0，圓弧段不為0）

double dArc；

圓弧角（左轉(zhuǎn)為正，右轉(zhuǎn)為負）或航向角（直線段）

}flyPoint；

這樣，就可以定義一條由N個點組成的航線fly?Point flyPt[N]，如果當(dāng)前航點n（n＜N）經(jīng)由直線到達下一航點n+1，則當(dāng)前航點的半徑和圓弧角均為0，如果當(dāng)前航點n經(jīng)由圓弧到達下一航點n+1，則該段圓弧與由航點n-1和航點n組成的直線段和由航點n+1與航點n+2組成的直線段相切，如圖1所示。

在設(shè)計圓弧段航線時應(yīng)確保其半徑大于無人機的最小轉(zhuǎn)彎半徑，無人機的轉(zhuǎn)彎半徑可采用以下方法計算：假設(shè)飛行空速為v，轉(zhuǎn)彎傾斜角為γ，則協(xié)調(diào)轉(zhuǎn)彎半徑為[5?6]一條完整的航線還應(yīng)該包括起點和終點，通常起點即為發(fā)射點，而終點則應(yīng)該是回收點，在自主導(dǎo)航飛行模式下，無人機飛到終點附近后將自動執(zhí)行停車開傘指令。以上航點數(shù)據(jù)是錄入時的格式，為了減輕飛行解算時的CPU負荷，提高自主導(dǎo)航的實時性，這些數(shù)據(jù)在錄入時通過程序及時轉(zhuǎn)換為導(dǎo)航坐標(biāo)系下的數(shù)據(jù)并進行存儲，其存儲格式與上述格式類似，只是經(jīng)、緯度值變?yōu)閷?dǎo)航坐標(biāo)系下的橫、縱坐標(biāo)值，直線段的航向角經(jīng)由以下公式計算得到（真北為0°）：

圖2為一條典型的飛行航線示意圖。

圖1 圓弧段與直線相切

圖2 典型飛行航線示意圖

2 導(dǎo)航系統(tǒng)設(shè)計

對于該小型無人機，選用數(shù)據(jù)刷新率為5 Hz的機載GPS接收機，無人機飛行速度約為200 m/s，導(dǎo)航系統(tǒng)的控制律解算周期取20 ms，則在一個解算周期內(nèi)，無人機飛行距離約為4m，考慮GPS單點定位精度及飛控系統(tǒng)和氣動力反應(yīng)時間，無人機的飛行誤差可以控制在100m之內(nèi)，能夠滿足精度要求。由于起飛前機載設(shè)備已經(jīng)加電并處于正常工作狀態(tài)，GPS接收機正常輸出數(shù)據(jù)，導(dǎo)航系統(tǒng)中存放的數(shù)據(jù)會不斷刷新，但GPS接收機的數(shù)據(jù)更新周期大于導(dǎo)航系統(tǒng)控制律解算周期，且當(dāng)無人機進行機動飛行時，可能會出現(xiàn)GPS接收機的信號中斷或者不連續(xù)的情況，則GPS信號不能準(zhǔn)確反應(yīng)無人機的實時位置，需對無人機的當(dāng)前位置進行推算，可采用兩點式航路推算公式近似計算，其計算方法如下：

設(shè)飛機在tM時刻和tN時刻的GPS位置數(shù)據(jù)分別為（LM，BM）和（LN，BN）（注意：tM＜tN，tM時刻和tN時刻可以直接從GPS接收機的輸出信息中獲?。?，轉(zhuǎn)換為導(dǎo)航坐標(biāo)系下的位置分別為（xM，yM）和（xN，yN），自tN時刻開始計時，則t時刻無人機的位置（xt，yt）可計算為：

由此可見，導(dǎo)航系統(tǒng)至少需要保存兩組最新得到的GPS數(shù)據(jù)信息，為了減小誤差，可以保存更多組最新得到的GPS數(shù)據(jù)信息，比如10組，分別為tN-9，tN-8，…，tN-1，tN時刻的GPS位置數(shù)據(jù)，則可取tM=tN-9。

GPS接收機正常工作與否直接關(guān)系到無人機自主導(dǎo)航的飛行精度，在GPS接收機短暫失效的情況下雖然可以按上述公式進行位置推算，但隨著中斷或不連續(xù)時間的延長，積累誤差會越來越大。解決這種問題的辦法可以通過采用高品質(zhì)的GPS接收機或者增加GPS接收機備份的措施，也可以設(shè)置當(dāng)GPS信號中斷時間超過某一數(shù)值后自動在原地盤旋，或者轉(zhuǎn)入遙控飛行模式通過外測手段獲取無人機地理位置信息完成任務(wù)。

3 控制律設(shè)計

該小型無人機導(dǎo)航控制律采用常用的比例微分控制，導(dǎo)航的目的就是使飛機能按預(yù)定航線飛行，不會出現(xiàn)偏離航線現(xiàn)象。當(dāng)飛機偏離航線時，導(dǎo)航計算機就會根據(jù)設(shè)計的控制律計算出相應(yīng)的偏轉(zhuǎn)舵角，交由飛控計算機控制副翼舵執(zhí)行偏轉(zhuǎn)動作，使飛機重新回到預(yù)定航線飛行。除此之外，設(shè)計的控制律還應(yīng)保證飛機能在各種可能情況下準(zhǔn)確地進入航線。副翼舵的偏轉(zhuǎn)大小和方向又與飛機當(dāng)時的速度v、偏航距d以及航跡角誤差Δψ有關(guān)，在直線飛行段時其控制律可采用偏航距d和飛機速度垂直航線的速度分量d v來設(shè)計：

式中速度垂直于航線的分量dv事實上就是飛機到航線距離的一階微分。因此，式（4）正是飛控系統(tǒng)中常用的比例加一階微分式控制律[7?8]，其中Δδa為副翼舵偏角，規(guī)定其使飛機向左轉(zhuǎn)為正，向右轉(zhuǎn)為負；當(dāng)飛機在航線右側(cè)偏航時，偏航距d為正，否則為負；Δψ=ψ-ψi（ψ為GPS實時測得的當(dāng)前無人機飛行航向角），右偏為正，左偏為負；k1和k2為比例系數(shù)且均大于0，調(diào)整其大小可以改變副翼舵變化的靈敏度。

當(dāng)無人機進入圓弧段飛行時，則可以利用航點的轉(zhuǎn)彎半徑和公式tgγ=v2(Rg)，通過查表法得到轉(zhuǎn)彎傾斜角，此即為副翼舵偏角，其正負參考該航點的圓弧角正負值確定。查表法較之解算超越函數(shù)arctg（）運算速度更快，且減少CPU運算量。

4 算法實現(xiàn)

要實現(xiàn)控制律算法，首先需要計算飛行偏航距。航線一般包括直線段和圓弧段，當(dāng)要求無人機沿著直線段飛行時，應(yīng)該首先判斷無人機是否在該直線段范圍內(nèi)，如果無人機處于該線段的兩側(cè)延伸范圍，則按照點到直線的距離公式所計算的偏航距肯定不符合實際情況。如圖3所示，當(dāng)無人機當(dāng)前位置Z處于直線段Zn-1Zn范圍外時，則∠ZZnZn-1＞90°，否則∠ZZnZn-1＜90°，為了避免解算超越函數(shù)arccos（），通過計算判斷即的關(guān)系就可知道無人機是否飛出直線段Zn-1Zn范圍外了。

圖3 當(dāng)前位置Z處于直線段Zn-1 Zn范圍外的情況

當(dāng)確定無人機當(dāng)前位置處于直線段范圍內(nèi)時，可以通過如下方法計算與直線段的偏航距：

對于航線中的直線段，采用以下直線方程表示：

Ax+By+C=0

假設(shè)Zn-1（xn-1，yn-1）、Zn（xn，yn）為航線直線段的兩個端點坐標(biāo)，則 A=yn-1-yn，B=xn-xn-1，C=xn-1yn-xnyn-1，無人機從Zn-1點飛向Zn點，無人機當(dāng)前的位置為Zt（xt，yt），則偏航距為[9]：

式中：sign（B）為取符號函數(shù)，下同。

當(dāng)無人機處于圓弧段飛行時，其偏航距與在直線段時的計算方法是不一樣的。如圖3所示，已知Zn點的航點數(shù)據(jù)，可以求出圓心O的坐標(biāo)，其計算公式為：

當(dāng) |xn-xn-1|＞0.1時，

則圓心O的坐標(biāo)為O（xo，yo）：

當(dāng)前點Zt（xt，yt）的偏航距為：

當(dāng) | xn-xn-1|≤0.1時，則可認為直線方程為x=xn-1=xn，此時k1=0，上述公式仍然有效。

為了避免超越三角函數(shù)的計算，在判斷無人機是否飛出圓弧段時可采用計算無人機是否進入到下一直線段范圍內(nèi)的方法完成，其計算方法同上。根據(jù)控制律公式，還需要計算偏航距的微分[10]。對于直線段，可根據(jù)公式 dv=v·sin(Δψ)（其中 Δψ 為航跡偏差角），對于圓弧段可以采用其離散近似公式代替[11]：dv=Δd Δt=(dt-dt-1) Δt，其中dt和dt-1分別表示當(dāng)前時刻和前一時刻的偏航距，Δt為采樣間隔時間。

5 結(jié)語

某小型無人機采用比例微分式導(dǎo)航控制律設(shè)計，控制機理成熟可靠。其對于航線的確定、偏航距及其微分的計算等都采用了較為理想的算法。同時，它應(yīng)用GPS全球定位系統(tǒng)的定位信息，定位精度和可靠性大幅提高；這些設(shè)計方法和措施提高了該無人機的自主導(dǎo)航控制精度和穩(wěn)定性，使其具有良好的飛行品質(zhì)。

[1]鄒湘伏，何清華，賀繼林.無人機發(fā)展現(xiàn)狀及相關(guān)技術(shù)[J].飛航導(dǎo)彈，2006（10）：9?14.

[2][美]法斯多姆.無人機系統(tǒng)導(dǎo)論[M].吳漢平，譯.北京：電子工業(yè)出版社，2003.

[3]袁建平，方群，鄭諤.GPS在飛行器定位導(dǎo)航中的應(yīng)用[M].西安：西北工業(yè)大學(xué)出版社，2000.

[4]屈薔，梅勁松，袁家斌.小型無人機GPS自主導(dǎo)航算法的研究與實現(xiàn)[J].大眾科技，2006（3）：47?48

[5]祝小平.無人機設(shè)計手冊[M].北京：國防工業(yè)出版社，2007.

[6]賈玉紅.航空航天概論[M].北京：北京航空航天大學(xué)出版社，2013.

[7]劉亮亮，胡延霖，易牧，等.無人機半實物仿真系統(tǒng)研究[J].兵工自動化，2008，27（3）：44?45.

[8]梁振東，李華英，管軍.小型無人機SINS/GPS組合導(dǎo)航系統(tǒng)研究[J].科學(xué)技術(shù)與工程，2013，13（3）：290?295.

[9]郭鎖鳳.先進飛行控制系統(tǒng)[M].北京：國防工業(yè)出版社，2003.

[10]呂曉林，羅純哲.無人機低空小捷徑飛行研究[J].宇航計測技術(shù)，2013，33（6）：84?87.

[11]王昱輝，雷金奎，田力.基于SmartFusion的無人機飛行控制系統(tǒng)設(shè)計[J].現(xiàn)代電子技術(shù)，2012，35（10）：137?139.