小型無人機(jī)航路規(guī)劃及自主導(dǎo)航算法研究

蔣志華，陶德桂

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

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

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

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

}flyPoint；

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

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

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

圖1 圓弧段與直線相切

圖2 典型飛行航線示意圖

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

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

設(shè)飛機(jī)在tM時刻和tN時刻的GPS位置數(shù)據(jù)分別為（LM，BM）和（LN，BN）（注意：tM＜tN，tM時刻和tN時刻可以直接從GPS接收機(jī)的輸出信息中獲?。D(zhuǎn)換為導(dǎo)航坐標(biāo)系下的位置分別為（xM，yM）和（xN，yN），自tN時刻開始計時，則t時刻無人機(jī)的位置（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接收機(jī)正常工作與否直接關(guān)系到無人機(jī)自主導(dǎo)航的飛行精度，在GPS接收機(jī)短暫失效的情況下雖然可以按上述公式進(jìn)行位置推算，但隨著中斷或不連續(xù)時間的延長，積累誤差會越來越大。解決這種問題的辦法可以通過采用高品質(zhì)的GPS接收機(jī)或者增加GPS接收機(jī)備份的措施，也可以設(shè)置當(dāng)GPS信號中斷時間超過某一數(shù)值后自動在原地盤旋，或者轉(zhuǎn)入遙控飛行模式通過外測手段獲取無人機(jī)地理位置信息完成任務(wù)。

3 控制律設(shè)計

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

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

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

4 算法實現(xiàn)

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

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

當(dāng)確定無人機(jī)當(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，無人機(jī)從Zn-1點飛向Zn點，無人機(jī)當(dāng)前的位置為Zt（xt，yt），則偏航距為[9]：

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

當(dāng)無人機(jī)處于圓弧段飛行時，其偏航距與在直線段時的計算方法是不一樣的。如圖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時，則可認(rèn)為直線方程為x=xn-1=xn，此時k1=0，上述公式仍然有效。

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

5 結(jié)語

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

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

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

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

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

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

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

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

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

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

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

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