周晨昶，王道波，王 猛

(南京航空航天大學(xué)自動(dòng)化學(xué)院，江蘇 南京 211106)

0 引言

無(wú)人機(jī)大致分為消費(fèi)級(jí)與工業(yè)級(jí)2類[1]，前者主要用于個(gè)人娛樂(lè)、拍照創(chuàng)作等，后者可進(jìn)行地形勘探、搜索救援以及多種軍事用途等。無(wú)人機(jī)之所以能在眾多領(lǐng)域發(fā)揮作用，較為重要的一點(diǎn)在于它能夠較為精確地跟蹤預(yù)設(shè)航線飛行[2]。同樣，無(wú)人機(jī)在飛行前，地面站會(huì)向飛控系統(tǒng)設(shè)定預(yù)設(shè)的航線，無(wú)人機(jī)在空中能自主跟隨預(yù)設(shè)的航線路徑，完成飛行任務(wù)。

針對(duì)路徑跟隨這項(xiàng)技術(shù)，國(guó)內(nèi)外的學(xué)者做了很多研究。依據(jù)解決策略的不同，大致分為2類。一類被稱為幾何學(xué)方法，如追蹤法[3]、視線法[4-6]，以及本文將重點(diǎn)介紹的向量場(chǎng)法[7-9]。追蹤法要求無(wú)人機(jī)的速度矢量始終指向目標(biāo)方向，視線法則設(shè)置虛擬點(diǎn)，逐步將無(wú)人機(jī)引導(dǎo)至路徑上，這些技術(shù)主要用于導(dǎo)彈制導(dǎo)方向。Kothari等[10]曾將追蹤法與視線法相結(jié)合，提出了PLOS(pure pursuit and line-of-sight)算法，使得無(wú)人機(jī)可以更好地在有風(fēng)環(huán)境下實(shí)現(xiàn)路徑跟隨。Park等[11]提出一種基于虛擬目標(biāo)點(diǎn)的非線性制導(dǎo)律，使得無(wú)人機(jī)在飛行過(guò)程中逐步收斂至目標(biāo)路徑。也有學(xué)者將幾何學(xué)方法與智能算法相結(jié)合，如Tabatabaei等[12]將Carrot-chasing算法與遺傳算法和模糊控制相結(jié)合，提出了三維模糊Carrot-chasing(3D fuzzy Carrot-chasing)算法，使得無(wú)人機(jī)在風(fēng)干擾環(huán)境下?lián)碛懈玫穆窂礁S結(jié)果。另一類被稱為控制學(xué)方法，分為線性控制技術(shù)與非線性控制技術(shù)，主要以非線性控制技術(shù)為主。工程應(yīng)用中較為常見(jiàn)的便是PID控制技術(shù)。Rhee等[13]對(duì)傳統(tǒng)PID技術(shù)進(jìn)行改進(jìn)，加入前饋環(huán)節(jié)，大大提高了無(wú)人機(jī)的路徑跟隨性能。主要的一些非線性控制技術(shù)包括線性二次型調(diào)節(jié)器(LQR)[14-15]、滑模控制[16]、模型預(yù)測(cè)控制[17]、自適應(yīng)控制[18]等。Wu等[19]在風(fēng)擾動(dòng)環(huán)境下，將無(wú)人機(jī)的飛行控制系統(tǒng)簡(jiǎn)化為4個(gè)回路，并對(duì)每個(gè)回路設(shè)計(jì)不同的非線性控制器，以實(shí)現(xiàn)三維空間內(nèi)的路徑跟隨問(wèn)題?；诜蔷€性控制理論的路徑跟隨技術(shù)往往具有更好的魯棒性，使無(wú)人機(jī)具有更好的飛行穩(wěn)定性。但此類方法較為依賴數(shù)學(xué)模型，實(shí)際應(yīng)用相對(duì)復(fù)雜。

針對(duì)眾多的路徑跟隨算法，許多學(xué)者進(jìn)行了結(jié)合或比較。Xue等[20]將Carrot-chase與PLOS這2類算法進(jìn)行比較，發(fā)現(xiàn)只要選擇合適的制導(dǎo)律增益，兩者均能較好地實(shí)現(xiàn)路徑跟隨。Sujit等[21-22]將5種不同策略的路徑跟隨算法進(jìn)行比較，加入風(fēng)場(chǎng)擾動(dòng)，并通過(guò)蒙特卡羅模擬，最終發(fā)現(xiàn)向量場(chǎng)法有著更好的抗風(fēng)干擾能力，能夠更為有效地實(shí)現(xiàn)路徑跟隨。

路徑跟隨算法的選擇往往要在控制精度與計(jì)算工作量之間進(jìn)行權(quán)衡。本文為提高某中小型固定翼無(wú)人機(jī)在有風(fēng)環(huán)境下的控制精度，優(yōu)化路徑跟隨性能，設(shè)計(jì)了一種基于向量場(chǎng)的側(cè)向制導(dǎo)算法，代替了基于PID的側(cè)向制導(dǎo)算法。最終通過(guò)數(shù)字仿真實(shí)驗(yàn)，證明了向量場(chǎng)制導(dǎo)算法的優(yōu)越性，通過(guò)半物理仿真以及實(shí)際飛行，驗(yàn)證了該算法與PID縱向制導(dǎo)算法融合后，可控制該固定翼無(wú)人機(jī)在有風(fēng)干擾的環(huán)境下更好地實(shí)現(xiàn)路徑跟隨。

1 問(wèn)題描述

首先給出本文所考慮的無(wú)人機(jī)數(shù)學(xué)模型，然后給出路徑跟隨問(wèn)題的數(shù)學(xué)模型。飛行路徑主要由直線段和圓弧段構(gòu)成，本文主要考慮直線段與圓弧段的路徑跟隨問(wèn)題。

1.1 無(wú)人機(jī)數(shù)學(xué)模型

無(wú)人機(jī)的橫向與縱向控制系統(tǒng)包含3個(gè)閉環(huán)回路[23]，在設(shè)計(jì)時(shí)，增穩(wěn)回路和姿態(tài)回路的傳遞函數(shù)增益近似為1，橫向和縱向控制回路的簡(jiǎn)化框圖分別如圖1和圖2所示。

圖1 橫向運(yùn)動(dòng)控制回路簡(jiǎn)圖

圖2 縱向運(yùn)動(dòng)控制回路簡(jiǎn)圖

動(dòng)力學(xué)方程即可表示為：

(1)

(2)

滾轉(zhuǎn)角指令以及俯仰角指令可表示為：

(3)

(4)

實(shí)際飛行中，航線角指令χc往往會(huì)實(shí)時(shí)變化，而高度指令hc往往以固定值的形式由預(yù)設(shè)航線給定或者由地面站發(fā)出，縱向自動(dòng)駕駛儀將自主控制無(wú)人機(jī)跟隨指令高度飛行。因此，接下來(lái)主要討論直線段與圓弧段路徑的側(cè)向跟隨問(wèn)題。

1.2 路徑跟隨

直線段路徑側(cè)向跟隨俯視圖如圖3所示。

圖3中，O為路徑起點(diǎn)，χq為目標(biāo)路徑的航線角，ePx和ePy為無(wú)人機(jī)與目標(biāo)路徑的航跡誤差分量，通過(guò)坐標(biāo)轉(zhuǎn)換可表示為

圖3 直線段路徑跟隨俯視圖

(5)

定義1：當(dāng)ePy近似為0且數(shù)值穩(wěn)定時(shí)，無(wú)人機(jī)便完成了直線段路徑側(cè)向跟隨，且ePx數(shù)值越小，無(wú)人機(jī)的側(cè)向制導(dǎo)效果越好。

圓弧段路徑側(cè)向跟隨俯視圖如圖4所示，其中d為無(wú)人機(jī)與圓弧中心點(diǎn)的距離向量；φ為其對(duì)應(yīng)的相位角；向量m與圓弧相切，其方向角為χo。

圖4 圓弧段路徑跟隨俯視圖

經(jīng)過(guò)坐標(biāo)轉(zhuǎn)換，得到無(wú)人機(jī)在d方向與m方向上的運(yùn)動(dòng)情況為

(6)

即：

(7)

(8)

定義2：當(dāng)‖d‖近似等于ρ且數(shù)值穩(wěn)定時(shí)，無(wú)人機(jī)便完成了圓弧段路徑側(cè)向跟隨。

加入風(fēng)擾動(dòng)后，無(wú)人機(jī)的地速矢量Vg，空速矢量Va和風(fēng)矢量W的關(guān)系如圖5所示。在xy平面討論無(wú)人機(jī)側(cè)向制導(dǎo)問(wèn)題時(shí)，運(yùn)動(dòng)學(xué)方程可表示為：

圖5 無(wú)人機(jī)地速、風(fēng)速和空速矢量關(guān)系

(9)

(10)

2 無(wú)人機(jī)路徑跟隨算法設(shè)計(jì)

2.1 PID制導(dǎo)算法

該無(wú)人機(jī)的俯仰指令公式[24]和滾轉(zhuǎn)指令公式[25]為：

(11)

(12)

2.2 向量場(chǎng)側(cè)向制導(dǎo)算法設(shè)計(jì)

PID側(cè)向制導(dǎo)算法實(shí)時(shí)計(jì)算出無(wú)人機(jī)飛行過(guò)程中所需的滾轉(zhuǎn)指令，現(xiàn)基于向量場(chǎng)的思想，設(shè)計(jì)一種利用側(cè)偏距，實(shí)時(shí)計(jì)算出航線角指令輸入χc的側(cè)向制導(dǎo)算法 。

該算法只考慮側(cè)偏距作為影響航線角指令變化的唯一因素，因此即使當(dāng)風(fēng)等外部干擾存在時(shí)，只要當(dāng)無(wú)人機(jī)的側(cè)偏距存在，橫向姿態(tài)控制回路便會(huì)收到相應(yīng)的航線角指令，以控制無(wú)人機(jī)消除側(cè)偏距，完成側(cè)向制導(dǎo)。

2.2.1 直線段路徑側(cè)向制導(dǎo)

無(wú)人機(jī)進(jìn)行直線段路徑側(cè)向跟隨時(shí)，當(dāng)前位置的期望航線角即可作為側(cè)向制導(dǎo)所需的航線角指令χc。這些期望航線角可以理解為具有指向功能的向量，構(gòu)成了側(cè)向制導(dǎo)所需的向量場(chǎng)，如圖6所示。

圖6 基于向量場(chǎng)法的直線段路徑側(cè)向制導(dǎo)策略

圖6中，χ∞表示當(dāng)無(wú)人機(jī)遠(yuǎn)離路徑時(shí)的期望航線角，此時(shí)無(wú)人機(jī)幾乎徑直飛向路徑。當(dāng)無(wú)人機(jī)位于路徑右側(cè)時(shí)，χ∞=χq-π/2；當(dāng)無(wú)人機(jī)位于路徑左側(cè)時(shí)，χ∞=χq+π/2。當(dāng)ePy=0時(shí)，χc=χq。

定理1：假設(shè)無(wú)人機(jī)以恒定的速度飛行，目標(biāo)直線段路徑方向已知，為完成側(cè)向路徑跟隨，無(wú)人機(jī)在當(dāng)前位置下的期望航線角可表示為

χc(ePy)=-arctankmePy+χq

(13)

km為常值系數(shù)。km的取值影響了反正切函數(shù)值的變化速率，進(jìn)而影響了期望航線角從χ∞到χq的轉(zhuǎn)換速率。

當(dāng)|χq-χ|>π時(shí)，此時(shí)給出的航線角指令往往要求無(wú)人機(jī)調(diào)整的航線角度超過(guò)180°，如圖7所示。

圖7 靶機(jī)的航線角與指定航線角相差較大

此時(shí)，χc>0。無(wú)人機(jī)航向控制回路會(huì)嚴(yán)格跟蹤航線角指令，此時(shí)無(wú)人機(jī)完成側(cè)向制導(dǎo)所需調(diào)整的角度為χ+χq；如果χq可用略小于-π的角度進(jìn)行表示，則χc<-π，無(wú)人機(jī)便會(huì)向左調(diào)整航向，更利于飛行。那么，此時(shí)目標(biāo)路徑的航線角χq可表示為

(14)

n∈N,其取值使得|χc-χ|<π；單位向量q=(qnqe)T指向路徑方向，qn、qe分別為北向和東向的分量。完成航向調(diào)整后，再將當(dāng)前航向χ與航線角指令χc調(diào)整至區(qū)間[-π,π]內(nèi)。

2.2.2 圓弧段路徑側(cè)向制導(dǎo)

與直線段路徑側(cè)向制導(dǎo)原理相同，無(wú)人機(jī)圓弧段路徑側(cè)向制導(dǎo)策略如圖8所示。

圖8 基于向量場(chǎng)法的圓弧段側(cè)向制導(dǎo)策略

圖8中，χo為無(wú)人機(jī)在圓弧段路徑上飛行時(shí)的期望航線角，可表示為

(15)

χ∞為當(dāng)無(wú)人機(jī)遠(yuǎn)離圓弧段路徑時(shí)的期望航線角，此時(shí)無(wú)人機(jī)幾乎徑直飛向圓弧中心，表示為

(16)

其中，當(dāng)無(wú)人機(jī)準(zhǔn)備以順時(shí)針?lè)较蝰側(cè)胲壍罆r(shí)，λ=1；當(dāng)無(wú)人機(jī)準(zhǔn)備以逆時(shí)針?lè)较蝰側(cè)胲壍罆r(shí)，λ=-1。

定理2：假設(shè)無(wú)人機(jī)以恒定的速度飛行，目標(biāo)圓弧段路徑位置已知，為完成圓弧段側(cè)向路徑跟隨，無(wú)人機(jī)在當(dāng)前位置下的期望航線角可表示為

(17)

kn為常值系數(shù)。kn的取值同樣影響了輸入的航線角指令的過(guò)渡速率。側(cè)向跟隨過(guò)程中，無(wú)人機(jī)在逐漸貼近圓弧段路徑飛行時(shí)，航向變化最大。若kn取值過(guò)大，航線角指令變化較為急促，不利于飛行；若kn取值過(guò)小，航線角指令變化不及時(shí)，難以滿足路徑跟隨要求。

同樣在實(shí)際的應(yīng)用中，無(wú)人機(jī)在進(jìn)行圓弧段側(cè)向路徑跟隨時(shí)，部分位置會(huì)發(fā)生航線角指令的突變，以圖9為例。

在圖9a中，無(wú)人機(jī)的當(dāng)前航線角大小為π，由于航線角的變化區(qū)間為[-π,π]，當(dāng)無(wú)人機(jī)經(jīng)過(guò)該位置時(shí)，航線角指令χc≈-π，受控的航向會(huì)發(fā)生2π的突變；圖9b中，無(wú)人機(jī)經(jīng)過(guò)該位置時(shí)，相位角φ同樣會(huì)發(fā)生幅值為2π的突變，從而造成航線角指令的變化。因此，當(dāng)|χc-χ|>π時(shí)，需對(duì)無(wú)人機(jī)相位角進(jìn)行修改，即

圖9 航線角指令發(fā)生2π突變

φ=arctan(pe-ce,pn-cn)+2πn

(18)

n∈N，n的取值使得|χc-χ|<π，航線角指令便不會(huì)發(fā)生幅值為2π的突變。

3 仿真實(shí)驗(yàn)與實(shí)際飛行

首先通過(guò)數(shù)字仿真實(shí)驗(yàn)，將基于向量場(chǎng)的側(cè)向制導(dǎo)算法與PID側(cè)向制導(dǎo)算法進(jìn)行對(duì)比，以證明前者的優(yōu)越性；然后采用改進(jìn)制導(dǎo)算法對(duì)固定翼無(wú)人機(jī)進(jìn)行了半物理仿真試驗(yàn)，以證明算法有效性。

3.1 數(shù)字仿真實(shí)驗(yàn)

仿真實(shí)驗(yàn)包括3部分，分別為近距離直線段路徑側(cè)向跟隨、遠(yuǎn)距離直線段路徑側(cè)向跟隨和圓弧段路徑側(cè)向跟隨。假設(shè)無(wú)人機(jī)的初始航向?yàn)?0°，飛行速度為20 m/s。運(yùn)算步長(zhǎng)為0.01 s。

3.1.1 近距離直線段路徑側(cè)向跟隨

目標(biāo)路徑y(tǒng)=0，無(wú)人機(jī)的初始位置為(0,100)，風(fēng)速為6 m/s，方位角為315°。2種控制算法的仿真結(jié)果如圖10～圖13所示。圖10為近距離直線段路徑的側(cè)向跟隨結(jié)果，圖11、圖12和圖13分別為無(wú)人機(jī)側(cè)偏距、航線角和滾轉(zhuǎn)角的變化曲線。

圖10 近距離直線段路徑跟隨結(jié)果

圖11 側(cè)偏距變化

圖12 航線角變化

圖13 滾轉(zhuǎn)角變化

采用向量場(chǎng)法時(shí)，無(wú)人機(jī)在30 s左右完成側(cè)向制導(dǎo)，跟隨誤差2.1 m；采用PID制導(dǎo)算法時(shí)， 無(wú)人機(jī)54 s后跟蹤誤差收斂至2.1 m內(nèi)，但航向并不穩(wěn)定。

上述結(jié)果可以看出，向量場(chǎng)制導(dǎo)算法控制下的無(wú)人機(jī)收斂至目標(biāo)航線的時(shí)間更短，航向控制更為精準(zhǔn)，跟隨誤差更小，同時(shí)滾轉(zhuǎn)角振蕩較少，飛行更平穩(wěn)。

3.1.2 遠(yuǎn)距離直線段路徑側(cè)向跟隨

目標(biāo)路徑y(tǒng)=0，無(wú)人機(jī)的初始位置為(0,400)，風(fēng)速為6 m/s，方位角為315°。2種控制算法的仿真結(jié)果如圖14～圖17所示。

圖14 遠(yuǎn)距離直線段路徑跟隨結(jié)果

圖15 側(cè)偏距變化

圖16 航線角變化

圖17 滾轉(zhuǎn)角變化

采用向量場(chǎng)法時(shí)，無(wú)人機(jī)46 s后實(shí)現(xiàn)航向穩(wěn)定，完成側(cè)向制導(dǎo)，跟隨誤差約3.5 m；采用PID制導(dǎo)算法時(shí)，無(wú)人機(jī)77 s后完成側(cè)向制導(dǎo)，跟隨誤差約為5.5 m。

當(dāng)無(wú)人機(jī)距離目標(biāo)航線更遠(yuǎn)，向量場(chǎng)制導(dǎo)算法的優(yōu)勢(shì)更加明顯。向量場(chǎng)制導(dǎo)算法控制無(wú)人機(jī)收斂至目標(biāo)航線的調(diào)節(jié)距離明顯更短，飛行曲線更為平滑，跟隨誤差更小，制導(dǎo)效果明顯更優(yōu)。

3.1.3 圓弧段路徑側(cè)向跟隨

期望圓弧段路徑由2個(gè)半徑為200 m的半圓弧組成，無(wú)人機(jī)初始位置為(0,200)，風(fēng)速為6 m/s，方位角為315°。2種控制算法的仿真結(jié)果如圖18～圖21所示。

圖18 圓弧段路徑跟隨結(jié)果

2種算法控制下的無(wú)人機(jī)在前半段路徑中的跟隨效果相仿，但在后半段路徑中，采用PID制導(dǎo)算法時(shí)，無(wú)人機(jī)出現(xiàn)較大的震蕩與超調(diào)，隨后逐漸收斂。從圖20和圖21中可以看出，向量場(chǎng)制導(dǎo)算法控制無(wú)人機(jī)74 s完成圓弧段的飛行任務(wù)，而PID制導(dǎo)算法控制無(wú)人機(jī)81 s完成圓弧段的飛行任務(wù)。

圖20 航線角變化情況

圖21 滾轉(zhuǎn)角變化情況

總體而言，在圓弧段路徑側(cè)向跟隨任務(wù)中，向量場(chǎng)制導(dǎo)算法控制下的無(wú)人機(jī)飛行耗時(shí)更短，跟隨誤差更小，飛行過(guò)程更加平穩(wěn)，制導(dǎo)效果更好。

圖19 側(cè)偏距變化情況

3.2 半物理仿真實(shí)驗(yàn)

搭建仿真平臺(tái)，開(kāi)發(fā)環(huán)境選用Microsoft Visual C++ 6.0。依據(jù)該無(wú)人機(jī)具體參數(shù)，結(jié)合6個(gè)動(dòng)力學(xué)方程和6個(gè)運(yùn)動(dòng)學(xué)方程構(gòu)造其數(shù)學(xué)模型。無(wú)人機(jī)起飛方式為彈射起飛，仿真平臺(tái)中包含了火箭助推器模型以及發(fā)動(dòng)機(jī)模型[26]，以模擬無(wú)人機(jī)的真實(shí)飛行。同時(shí)，加入水平方向上的常值風(fēng)，模擬風(fēng)的干擾。

選擇向量場(chǎng)法為側(cè)向路徑跟隨算法。模擬起飛前，上傳預(yù)定航線至飛控系統(tǒng)，輸入無(wú)人機(jī)起飛點(diǎn)的經(jīng)緯度以及起飛時(shí)的航線角和俯仰角，輸入干擾風(fēng)的方位和風(fēng)速。模擬起飛后，仿真平臺(tái)接收飛控傳入的舵面信息并經(jīng)過(guò)解算，將取代GPS以及陀螺儀，發(fā)送經(jīng)緯高、航線角、三向加速度以及歐拉角等數(shù)據(jù)給無(wú)人機(jī)的飛控系統(tǒng)。飛控接收后進(jìn)行解算，同時(shí)控制舵面偏轉(zhuǎn)，完成路徑跟隨，并將所有數(shù)據(jù)由無(wú)線電臺(tái)發(fā)送至地面站，地面站可觀測(cè)飛行期間所有狀態(tài)數(shù)據(jù)，且全程可控。半物理仿真流程如圖22所示。

圖22 仿真流程

輸入無(wú)人機(jī)起飛時(shí)仰角大小為15°。風(fēng)的方位角設(shè)為45°，風(fēng)速大小為6 m/s。仿真結(jié)果如圖23和圖24所示。

圖23 側(cè)向路徑跟隨

圖24 縱向高度跟隨

圖23中，坐標(biāo)(0,500)為起飛點(diǎn)，無(wú)人機(jī)在第二圈飛行不久后進(jìn)行回收。飛行曲線平滑，無(wú)明顯抖動(dòng)與振蕩，無(wú)人機(jī)與直線段路徑和圓弧段路徑貼合度較高，高度跟隨過(guò)程較為平穩(wěn)，路徑跟隨情況良好。向量場(chǎng)側(cè)向制導(dǎo)算法與縱向制導(dǎo)算法融合效果較好，驗(yàn)證了向量場(chǎng)側(cè)向制導(dǎo)算法的有效性。

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

為提高某中小型固定翼無(wú)人機(jī)在有風(fēng)環(huán)境下的路徑跟隨性能，本文設(shè)計(jì)了一種基于向量場(chǎng)的側(cè)向制導(dǎo)算法。該算法通過(guò)提供實(shí)時(shí)精準(zhǔn)的航線角指令，控制無(wú)人機(jī)迅速調(diào)整姿態(tài)，修正航向。數(shù)字仿真實(shí)驗(yàn)表明，在有風(fēng)的環(huán)境下，與原PID側(cè)向制導(dǎo)算法相比，該算法控制無(wú)人機(jī)收斂至目標(biāo)航線的調(diào)節(jié)時(shí)間更短，調(diào)節(jié)距離更短，航向控制更加精準(zhǔn)，跟隨誤差更小，飛行過(guò)程更加平穩(wěn)，制導(dǎo)效果明顯更優(yōu)。最終，半實(shí)物仿真實(shí)驗(yàn)證明了向量場(chǎng)側(cè)向制導(dǎo)算法與PID縱向制導(dǎo)算法可較好融合，算法改進(jìn)后的無(wú)人機(jī)擁有更好的抗風(fēng)干擾能力，路徑跟隨性能明顯增強(qiáng)。