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

摘 要： 為了實(shí)現(xiàn)無人機(jī)自主導(dǎo)航功能，定義了以發(fā)射點(diǎn)為坐標(biāo)原點(diǎn)的地理導(dǎo)航坐標(biāo)系，給出由WGS?84坐標(biāo)系到導(dǎo)航坐標(biāo)系的具體轉(zhuǎn)換公式；設(shè)計(jì)了航路中的航點(diǎn)結(jié)構(gòu)，利用GPS接收機(jī)輸出數(shù)據(jù)對無人機(jī)的位置進(jìn)行兩點(diǎn)式實(shí)時推算，對無人機(jī)在直線航路段和轉(zhuǎn)彎段的判斷和側(cè)偏距進(jìn)行了詳細(xì)討論，避免了超越函數(shù)的計(jì)算；利用比例?微分導(dǎo)航控制律求得無人機(jī)的滾轉(zhuǎn)角，通過飛控系統(tǒng)控制無人機(jī)副翼舵機(jī)，修正飛行航跡，達(dá)到消除側(cè)偏距的目的。這些方法和措施提高了小型無人機(jī)的自主導(dǎo)航控制精度和穩(wěn)定性，使小型無人機(jī)具有較好的飛行品質(zhì)。

關(guān)鍵詞： 航路規(guī)劃； 導(dǎo)航； 控制律； 全球定位系統(tǒng)； 無人機(jī)

中圖分類號： TN919?34； V249.32 文獻(xiàn)標(biāo)識碼： A 文章編號： 1004?373X（2015）04?0005?03

在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ī)和實(shí)施攻擊等[2]。針對執(zhí)行不同的使命任務(wù)，無人機(jī)可選擇不同的飛行模式，包括遙控飛行模式、程控飛行模式、自主導(dǎo)航飛行模式以及它們的組合飛行模式等，其中作為靶機(jī)的無人機(jī)要求其具有航路規(guī)劃和自主導(dǎo)航飛行功能，使其能按照預(yù)先裝定的航線精確飛行。某小型無人機(jī)采用GPS接收機(jī)設(shè)計(jì)了自主導(dǎo)航系統(tǒng)實(shí)現(xiàn)了上述功能。

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

該小型無人機(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)航飛行模式下的航點(diǎn)號，并切回自主導(dǎo)航飛行模式繼續(xù)飛行。自主導(dǎo)航的基本設(shè)計(jì)思想是利用無人機(jī)機(jī)載GPS接收機(jī)接收到的位置信息作為無人機(jī)的當(dāng)前位置，通過計(jì)算準(zhǔn)實(shí)時求得無人機(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ī)自主飛行的運(yùn)動軌跡和飛行狀態(tài)，必須選用適當(dāng)?shù)膶?dǎo)航坐標(biāo)系?？紤]到小型無人機(jī)的飛行距離和可用GPS接收機(jī)的精度，定義如下二維導(dǎo)航坐標(biāo)系：起飛點(diǎn)為坐標(biāo)原點(diǎn)，x軸為正東方向，y軸為正北方向，單位為m，則WGS?84坐標(biāo)系轉(zhuǎn)換為導(dǎo)航坐標(biāo)系的公式為[4]：

[x=（Axcos β-Bxcos（3β））（L-L0）y=（Ay-Bycos（2β））（B-B0）β=（B+B0）2] （1）

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

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

這樣，就可以定義一條由N個點(diǎn)組成的航線flyPoint flyPt[N]，如果當(dāng)前航點(diǎn)n（n

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

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

對于該小型無人機(jī)，選用數(shù)據(jù)刷新率為5 Hz的機(jī)載GPS接收機(jī)，無人機(jī)飛行速度約為200 m/s，導(dǎo)航系統(tǒng)的控制律解算周期取20 ms，則在一個解算周期內(nèi)，無人機(jī)飛行距離約為4 m，考慮GPS單點(diǎn)定位精度及飛控系統(tǒng)和氣動力反應(yīng)時間，無人機(jī)的飛行誤差可以控制在100 m之內(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ī)的實(shí)時位置，需對無人機(jī)的當(dāng)前位置進(jìn)行推算，可采用兩點(diǎn)式航路推算公式近似計(jì)算，其計(jì)算方法如下：

設(shè)飛機(jī)在tM時刻和tN時刻的GPS位置數(shù)據(jù)分別為（LM，BM）和（LN，BN）（注意：tM

[xt=xN+xN-xMtN-tM·tyt=yN+yN-yMtN-tM·t] （3）

由此可見，導(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ì)

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

[dv=v×sinΔψΔδa=k1d+k2dv] （4）

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

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

4 算法實(shí)現(xiàn)

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

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

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

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

Ax+By+C=0

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

[d=sign（B）·Axt+Byt+CA2+B2] （5）

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

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

當(dāng)[xn-xn-1>0.1]時，[k=yn-yn-1xn-xn-1]，[k1=-1k][=xn-1-xnyn-yn-1]，則圓心O的坐標(biāo)為O（xo，yo）：

[xo=xn+sign（dArc）?R1+k21yo=yn+sign（dArc）?k1?R1+k21] （6）

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

[d=sign（xn-xn-1）?][（（xt-xo）2+（yt-yo）2-R）]

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

為了避免超越三角函數(shù)的計(jì)算，在判斷無人機(jī)是否飛出圓弧段時可采用計(jì)算無人機(jī)是否進(jìn)入到下一直線段范圍內(nèi)的方法完成，其計(jì)算方法同上。根據(jù)控制律公式，還需要計(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ì)，控制機(jī)理成熟可靠。其對于航線的確定、偏航距及其微分的計(jì)算等都采用了較為理想的算法。同時，它應(yīng)用GPS全球定位系統(tǒng)的定位信息，定位精度和可靠性大幅提高；這些設(shè)計(jì)方法和措施提高了該無人機(jī)的自主導(dǎo)航控制精度和穩(wěn)定性，使其具有良好的飛行品質(zhì)。

參考文獻(xiàn)

[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)航算法的研究與實(shí)現(xiàn)[J].大眾科技，2006（3）：47?48

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

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

[7] 劉亮亮，胡延霖，易牧，等.無人機(jī)半實(shí)物仿真系統(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].宇航計(jì)測技術(shù)，2013，33（6）：84?87.

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