基于虛擬長機(jī)的非線性視線導(dǎo)引律多機(jī)編隊控制

孫 哲， 沈 偉， 李 浩， 郭劍東， 楊忠清

(南京航空航天大學(xué)，a.航空學(xué)院; b.中小型無人機(jī)先進(jìn)技術(shù)工信部重點實驗室，南京 210000)

0 引言

近年來，無人機(jī)行業(yè)發(fā)展迅速，在軍用和民用領(lǐng)域得到了廣泛應(yīng)用。單架無人機(jī)由于固有的局限性，無法完成復(fù)雜的任務(wù)，因此人們開始通過多無人機(jī)的合作協(xié)同來完成單架無人機(jī)所無法完成的任務(wù)。其中，編隊控制問題是多機(jī)協(xié)同的基礎(chǔ)和難點，也是目前無人機(jī)領(lǐng)域研究的熱點[1]。

目前較成熟且通用的編隊控制算法主要有長機(jī)-僚機(jī)(Leader-Follower)法、虛擬結(jié)構(gòu)(Virtual Structure)法、基于行為(Behavior-Based)法[2-4]。長機(jī)-僚機(jī)法是最經(jīng)典的一種編隊控制方法，通過對長機(jī)的速度、偏航角和高度跟蹤來調(diào)整僚機(jī)，達(dá)到保持編隊隊形的目的。文獻(xiàn)[5]基于長機(jī)-僚機(jī)的方法，首先利用積分反步設(shè)計了對長機(jī)的軌跡跟蹤控制器，其次設(shè)計了滑?？刂破饕钥刂崎L機(jī)與僚機(jī)實現(xiàn)期望的編隊隊形，其原理簡單、易于實現(xiàn)，但出現(xiàn)突發(fā)事件或失去長機(jī)通信后，所有無人機(jī)位置需重新計算，嚴(yán)重情況下會導(dǎo)致編隊崩潰；文獻(xiàn)[6]提出了通信拓?fù)浜烷L機(jī)切換的方法，來改善傳統(tǒng)長機(jī)-僚機(jī)法的編隊崩潰問題，由于長機(jī)不斷隨時間輪換,信息能夠充分交換,增強(qiáng)了系統(tǒng)的魯棒性。虛擬結(jié)構(gòu)法采用虛擬剛體來描述期望的編隊構(gòu)型，多采用虛擬長機(jī)的方法來協(xié)調(diào)其他無人機(jī)，并用虛擬結(jié)構(gòu)的運動軌跡來描述整個編隊的運動。文獻(xiàn)[7]基于以虛擬長機(jī)為參考的旋轉(zhuǎn)坐標(biāo)系建立相對運動學(xué)模型,采用滑模變結(jié)構(gòu)控制方法設(shè)計了多無人機(jī)隊形保持控制器，該方法提高了編隊抗風(fēng)險的能力，容易指定機(jī)群的行為，但傳統(tǒng)的虛擬結(jié)構(gòu)法計算量大、要求的通信質(zhì)量較高?；谛袨榉ㄓ梢幌盗谢拘袨榻M成，即碰撞避免、障礙物規(guī)避、目標(biāo)搜索和隊形保持。無人機(jī)的控制行為一般為這4種基本行為的加權(quán)平均，文獻(xiàn)[8]采用基于行為的方法,將鴿群的層次學(xué)習(xí)行為應(yīng)用于無人機(jī)編隊，實現(xiàn)了無人機(jī)編隊的保持和軌跡跟蹤，該方法適用性較強(qiáng)，適用于不確定環(huán)境。但基于行為法中子系統(tǒng)的行為定義困難，設(shè)計復(fù)雜[9]。

本文設(shè)計了基于虛擬長機(jī)與非線性視線導(dǎo)引律相結(jié)合的方法，將導(dǎo)引律中的側(cè)向制導(dǎo)控制思想運用于編隊控制。首先，通過虛擬長機(jī)沿目標(biāo)航路飛行生成導(dǎo)引律參考點；其次，編隊飛機(jī)采用側(cè)向制導(dǎo)控制跟蹤參考點飛行，且將編隊飛行控制分為航路飛行與航路切換兩個階段;最終，生成保持編隊隊形所需的姿態(tài)角與速度指令。該方法能夠進(jìn)行良好的編隊隊形控制，隊形誤差小且能夠?qū)δ繕?biāo)航路進(jìn)行快速有效跟蹤，易于工程實現(xiàn)。本文所有變量均不考慮方向，視為標(biāo)量。

1 基于虛擬長機(jī)的控制策略

本文以文獻(xiàn)[10]中所提出的典型“V”字型編隊為例進(jìn)行控制方法設(shè)計，如圖1所示。

圖1 典型“V”字型編隊示意圖

采用虛擬長機(jī)的方法，選取虛擬點PVi(i=1,2,3)中的PV1為虛擬長機(jī)。圖2為多機(jī)編隊的拓?fù)浣Y(jié)構(gòu)，虛擬長機(jī)以定速沿航路方向飛行，根據(jù)虛擬長機(jī)坐標(biāo)、隊形的幾何參考位置和航路信息，通過非線性視線導(dǎo)引律[11](以下簡稱導(dǎo)引律)計算得到參考點Pi。編隊飛機(jī)UAVi實時接收Pi點坐標(biāo)信息，根據(jù)PVi與編隊飛機(jī)間的期望距離生成編隊飛機(jī)的參考點PUi，進(jìn)入航路切換階段時，PVi將直接作為編隊飛機(jī)的參考點，生成滾轉(zhuǎn)角控制指令，實現(xiàn)編隊飛機(jī)橫向位置保持控制。同時，編隊飛機(jī)通過實時接收PVi的位置信息，得出PVi與編隊飛機(jī)之間的距離誤差，采用PID控制器得到速度控制指令，實現(xiàn)編隊飛機(jī)的縱向位置保持控制。

圖2 多機(jī)編隊拓?fù)浣Y(jié)構(gòu)

2 編隊算法設(shè)計

2.1 航路飛行段橫向位置保持控制方法

以UAV1和UAV2為例進(jìn)行算法設(shè)計(UAV3可同理推得)，選取PV1作為虛擬長機(jī)，在航路飛行階段以定速V0沿航路方向飛行，在北東地(NED)坐標(biāo)系下的位置為PV1(PV1N,PV1E)，PV2與其保持如圖1所示的距離為d和夾角為α(UAV2的α取值為正，UAV3為負(fù)，即當(dāng)處于隊形對稱線逆時針方向時為正，反之為負(fù))的隊形同步飛行，如圖3所示。

圖3 航路飛行階段編隊示意圖

由PV1坐標(biāo)計算可得虛擬長機(jī)在目標(biāo)航路WP1—WP2上的參考點P1(WP1，WP2的坐標(biāo)分別為(WP1N，WP1E),(WP2N,WP2E))，P1坐標(biāo)算式為

(1)

(2)

λ=|ψ-χ|

(3)

(4)

其中：ψ為目標(biāo)航路的航線角；χ為虛擬長機(jī)和航路起始點WP1的連線與北向的夾角；d1為虛擬長機(jī)與航路起始點WP1的距離。虛擬長機(jī)與參考點P1的距離為參考長度L1(L1的取值需根據(jù)飛機(jī)飛行參數(shù)調(diào)整)，則WP1與P1之間的距離算式為

[WP1,P1]=d1cosλ+L1

(5)

P1點坐標(biāo)(P1N,P1E)為

P1N=WP1N+[WP1,P1]sinψ

(6)

P1E=WP1E+[WP1,P1]cosψ。

(7)

由虛擬長機(jī)的飛行位置以及編隊隊形信息計算得到虛擬點PV2(PV2N,PV2E)坐標(biāo)為

PV2N=PV1N-dcos(ψ-α)

(8)

PV2E=PV1E-dsin(ψ-α)。

(9)

由于在航路飛行段虛擬長機(jī)與虛擬點PV2航向相同，PV1P1與PV2P2相互平行，可得到PV2在目標(biāo)航路上的參考點P2(P2N,P2E)為

P2N=PV2N+L1cosα

(10)

P2E=PV2E+L1sinα。

(11)

最后，在已知各虛擬點在目標(biāo)航路上的參考點Pi的情況下，可由PVi與編隊飛機(jī)之間的期望距離D求得各飛機(jī)沿航路進(jìn)行編隊飛行所依據(jù)的參考點PUi。UAV1，UAV2的參考點PU1，PU2的坐標(biāo)為

(12)

經(jīng)導(dǎo)引律公式計算橫向位置保持的期望滾轉(zhuǎn)角需要各飛機(jī)的地速Vn、地速夾角Ω以及與PUi的夾角λ。UAV1在飛行時的位置為U1(U1N,U1E)，飛行速度在NED坐標(biāo)系下分量為(V1N,V1E)，具體算式為

(13)

Ω1=arctan(V1E,V1N)

(14)

λ1=arctan((PU1E-U1E),(PU1N-U1N))

(15)

η1=λ1-Ω1

(16)

則UAV1的期望滾轉(zhuǎn)角φ1為

(17)

式中：g為重力加速度；L′1為UAVi對PUi跟蹤參考長度。根據(jù)UAV2在飛行時的位置U2(U2N,U2E)，飛行速度 (V2N,V2E)，同理可得UAV2的期望滾轉(zhuǎn)角，算式為

(18)

Ω2=arctan(V2E,V2N)

(19)

λ2=arctan((PU2E-U2E)，(PU2N-U2N))

(20)

η2=λ2-Ω2

(21)

(22)

2.2 航路切換段橫向位置保持控制方法

為防止航路切換時編隊航向突變引起編隊間距誤差，本文采用兩條航路的公切圓作為航路切換段虛擬點的飛行路線，該方法可保證該階段飛行過程平穩(wěn)，不會產(chǎn)生航向突變現(xiàn)象。當(dāng)P1點到達(dá)當(dāng)前航路終點WP2時進(jìn)入航路切換階段，此時虛擬長機(jī)位于A點，通過WP2坐標(biāo)，WP1—WP2，WP2—WP3航跡角以及參考長度L1可得A點坐標(biāo)(AN,AE)和B點坐標(biāo)(BN,BE)，如圖4所示。

圖4 航路切換判斷條件示意圖

圖5 航路切換階段編隊示意圖

由2.1節(jié)可知,進(jìn)行橫向位置保持控制時需要確定虛擬長機(jī)位置，此時虛擬長機(jī)PV1的坐標(biāo)可由圓心坐標(biāo)以及圓心角ξ求得

PV1N=ON+Rsin(δ+ξ)

(23)

PV1E=OE-Rcos(δ+ξ)

(24)

δ=arctan((AN-ON),(AE-OE))。

(25)

ξ可由虛擬長機(jī)速度V0以及進(jìn)入航路切換階段的飛行時間t求得，即

(26)

ξ=ωt。

(27)

PV2，PV3坐標(biāo)仍可由式(8)、式(9)求得，此時ψ為PV1處的圓弧切線與N軸夾角，即

ψ=δ+ξ。

(28)

在航路切換階段，編隊飛機(jī)將PVi作為參考點，將PV1，PV2和PV3坐標(biāo)代入式(13)～(22)即可求得其期望滾轉(zhuǎn)角。

2.3 隊形縱向位置保持控制方法

編隊飛機(jī)的縱向位置控制分為外回路及內(nèi)回路兩部分，外回路PID控制器[12]為縱向位置控制。位置環(huán)期望輸入為編隊飛機(jī)與虛擬點PVi之間的實際距離Di和期望距離D的偏差，實際距離Di為

(29)

偏差eD為

eD=Di-D

(30)

由位置外回路計算得到期望速度指令V為

(31)

內(nèi)回路為速度控制，作為位置阻尼環(huán)節(jié)抑制位置環(huán)震蕩，其輸入為期望速度與實際飛行速度Vi的偏差，即

eV=Vi-V。

(32)

由速度內(nèi)回路計算得到油門控制量T，從而實現(xiàn)隊形的縱向位置保持控制，即

(33)

2.4 隊形高度保持控制方法

編隊飛機(jī)的高度保持分為高度外回路以及俯仰角內(nèi)回路兩部分，高度環(huán)輸入為期望航路高度PViD與飛機(jī)實際飛行高度hi的偏差，即

eh=hi-PViD

(34)

由高度外回路計算得到期望俯仰角指令θd為

(35)

俯仰角內(nèi)回路輸入為期望俯仰角θ與實際俯仰角θi的偏差，即

eθ=θi-θ

(36)

計算得到升降舵控制量δθ實現(xiàn)隊形的高度保持，即

(37)

PID控制的參數(shù)選取方式一般有試湊法、頻率法和時域法。由于采用非線性模型，本文主要采用試湊法來調(diào)節(jié)PID控制器參數(shù)。

3 數(shù)值仿真驗證

為驗證本文編隊飛行控制方法，以某固定翼無人機(jī)的全量非線性飛行動力學(xué)模型為基礎(chǔ)搭建多架飛機(jī)數(shù)值仿真系統(tǒng)，分別針對航路飛行段以及航路切換段進(jìn)行數(shù)值仿真驗證。采用隊形為圖1所示的典型“V”字型編隊，其中，d=100 m，α=45°，虛擬長機(jī)飛行速度V0=40 m/s，高度PViD=-200 m，參考長度L1=400 m，UAV的跟蹤參考長度L′1=150 m，PVi與編隊飛機(jī)期望距離D=150 m。

3.1 航路飛行段隊形保持仿真實驗

數(shù)值仿真實驗中目標(biāo)航路為WP1—WP2，WP1坐標(biāo)為(500 m,0 m,-200 m)，WP2坐標(biāo)為(3500 m,0 m,-200 m)，3架飛機(jī)以及虛擬長機(jī)的初始位置如表1所示。

表1 航路飛行階段初始位置信息

虛擬長機(jī)從(200 m,0 m,-200 m)處出發(fā)以40 m/s的速度沿航路飛行，編隊飛機(jī)從初始位置出發(fā)，進(jìn)行編隊飛行。結(jié)果如圖6所示。

圖6 航路飛行段仿真結(jié)果

如圖6所示，由于3架飛機(jī)的初始位置隨機(jī)分布，在[0 s，42 s] 飛行過程中，3架無人機(jī)實際相對距離遠(yuǎn)大于理想編隊距離，無人機(jī)將在上文所述的控制方法下調(diào)整前飛速度與滾轉(zhuǎn)角，以達(dá)到理想編隊隊形，由于飛機(jī)進(jìn)行橫向姿態(tài)調(diào)整與縱向加速飛行，飛行高度有較小波動，但始終維持在理想飛行高度200 m附近，高度控制誤差小于2 m；在(42 s，70 s]飛行過程中，編隊飛機(jī)完成設(shè)計編隊隊形，實際相對距離始終保持為100 m，此時，飛機(jī)將保持穩(wěn)定的滾轉(zhuǎn)角、高度和速度完成對航路的跟蹤控制。數(shù)值仿真結(jié)果表明：在編隊飛機(jī)初始位置隨機(jī)分布的情況下，通過橫向與縱向的控制作用，最終控制飛機(jī)進(jìn)入理想編隊隊形。在航路跟蹤過程中，隊形誤差小于2 m，相較于傳統(tǒng)的長僚機(jī)編隊方式，隊形控制效果更好。

3.2 航路切換段隊形仿真實驗(3機(jī))

在航路切換段，目標(biāo)航路為WP1—WP2—WP3，WP1坐標(biāo)為(500 m,0 m,-200 m)，WP2坐標(biāo)為(3000 m,0 m,-200 m)，WP3坐標(biāo)為(4500 m,1500 m,-200 m)，3架飛機(jī)以及虛擬長機(jī)的初始位置如表2所示。

表2 航路切換階段初始位置信息

虛擬長機(jī)初始從(200 m,0 m,-200 m)處出發(fā)，以40 m/s的速度沿WP1—WP2方向飛行，當(dāng)進(jìn)入航路切換階段時，虛擬長機(jī)將以2.2節(jié)所示方式做圓弧路線飛行，完成切換后繼續(xù)沿WP2—WP3方向飛行。結(jié)果如圖7所示。3架飛機(jī)的初始位置隨機(jī)分布，在0～40 s飛行過程中，3架飛機(jī)由初始位置向目標(biāo)航路飛行并逐漸完成編隊隊形；在60 s時，編隊飛機(jī)進(jìn)入航路切換階段，如圖4、圖5所示，此時3架飛機(jī)的參考點將從航路段的PUi切換到虛擬點PVi，PVi在航路切換階段做圓弧飛行，3架飛機(jī)也將在參考點導(dǎo)引下沿圓弧航路飛行，完成WP1—WP2段航路到WP2—WP3段航路的切換，由于飛機(jī)進(jìn)行橫向姿態(tài)調(diào)整引起編隊間橫向距離變化，導(dǎo)致飛行速度產(chǎn)生±5 m/s的波動，從而引起編隊飛機(jī)控制誤差在10 m以內(nèi)，仍保持良好的隊形飛行；在84～112 s飛行過程中，編隊飛機(jī)已完成航路切換，編隊飛機(jī)實際相對距離穩(wěn)定到理想編隊距離，此時飛機(jī)將保持穩(wěn)定的滾轉(zhuǎn)角、高度和速度完成對WP2—WP3段航路的跟蹤任務(wù)。綜上所述，傳統(tǒng)的長機(jī)領(lǐng)航編隊方式，在長機(jī)不穩(wěn)定的情況下，編隊會面臨崩潰危險，而本文設(shè)計的編隊控制算法，通過采用虛擬長機(jī)生成編隊飛機(jī)各自所需跟飛的參考點，即使在初始飛機(jī)掉隊、偏離航線或長機(jī)不穩(wěn)定的情況下，仍可快速構(gòu)造成理想編隊隊形，且對航路跟蹤精度更高，在航路切換時，隊形控制精度較好。

圖7 航路切換段仿真結(jié)果

3.3 航路切換段隊形仿真實驗(5機(jī))

為驗證本文所設(shè)計算法的適用性，對前述編隊的隊形進(jìn)行擴(kuò)展，采用5架飛機(jī)進(jìn)行編隊飛行，增加UAV4和UAV5構(gòu)成新的“V”字型編隊，UAV4，UAV5與UAV1保持d=200 m,α=45°的隊形飛行。目標(biāo)航路為WP1—WP2—WP3，WP1坐標(biāo)為(500 m,0 m,-200 m)，WP2坐標(biāo)為(3000 m,0 m,-200 m)，WP3坐標(biāo)為(4500 m,1500 m,-200 m)，各飛機(jī)以及虛擬長機(jī)的初始位置如表3所示。

表3 5架飛機(jī)編隊初始位置信息

虛擬長機(jī)初始從(200 m,0 m,-200 m)處出發(fā)，以40 m/s的速度沿WP1—WP2,WP2—WP3航路飛行，在航路切換階段，仍采用圓弧航路，仿真結(jié)果如圖8所示。

圖8 5架飛機(jī)編隊飛行仿真結(jié)果

由圖8可以看出，5架飛機(jī)的初始位置隨機(jī)分布，在0～45 s飛行過程中，5架飛機(jī)逐漸完成編隊隊形；在60 s時，編隊飛機(jī)進(jìn)入航路切換階段，此時編隊飛機(jī)間實際相對距離小幅偏離理想編隊距離，但UAV2和UAV3的編隊控制誤差在10 m以內(nèi)，UAV4和UAV5的編隊控制誤差在20 m以內(nèi)，仍能保持良好的隊形飛行；在90～112 s飛行過程中，編隊飛機(jī)已完成航路切換，編隊飛機(jī)實際相對距離穩(wěn)定到理想編隊距離，由此可見，本文所設(shè)計算法對于多機(jī)、多隊形的編隊飛行具有通用性和適用性。

4 總結(jié)

本文針對多機(jī)編隊的航路跟蹤問題，設(shè)計了基于虛擬長機(jī)與非線性視線導(dǎo)引律相結(jié)合的編隊飛行控制算法。在航路飛行階段，虛擬長機(jī)根據(jù)航路信息、導(dǎo)引律以及隊形的幾何位置生成編隊飛機(jī)的參考點，進(jìn)而得到每架飛機(jī)的滾轉(zhuǎn)角指令；在航路切換階段，虛擬長機(jī)采用圓弧飛行，通過參考點的切換生成該階段滾轉(zhuǎn)角指令，以此實現(xiàn)橫向位置保持。仿真結(jié)果表明，航路飛行和航路切換段隊形誤差分別小于2 m與10 m，能夠?qū)崿F(xiàn)無人機(jī)編隊隊形的精準(zhǔn)保持與航路跟蹤控制,并且對于多機(jī)、多隊形的編隊飛行具有通用性和適用性。