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

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

0 引言

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

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

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

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

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

圖1 典型“V”字型編隊(duì)示意圖

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

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

2 編隊(duì)算法設(shè)計(jì)

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

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

圖3 航路飛行階段編隊(duì)示意圖

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

(1)

(2)

λ=|ψ-χ|

(3)

(4)

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

[WP1,P1]=d1cosλ+L1

(5)

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

P1N=WP1N+[WP1,P1]sinψ

(6)

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

(7)

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

PV2N=PV1N-dcos(ψ-α)

(8)

PV2E=PV1E-dsin(ψ-α)。

(9)

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

P2N=PV2N+L1cosα

(10)

P2E=PV2E+L1sinα。

(11)

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

(12)

經(jīng)導(dǎo)引律公式計(jì)算橫向位置保持的期望滾轉(zhuǎn)角需要各飛機(jī)的地速Vn、地速夾角Ω以及與PUi的夾角λ。UAV1在飛行時(shí)的位置為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對(duì)PUi跟蹤參考長度。根據(jù)UAV2在飛行時(shí)的位置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 航路切換段橫向位置保持控制方法

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

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

圖5 航路切換階段編隊(duì)示意圖

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

PV1N=ON+Rsin(δ+ξ)

(23)

PV1E=OE-Rcos(δ+ξ)

(24)

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

(25)

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

(26)

ξ=ωt。

(27)

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

ψ=δ+ξ。

(28)

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

2.3 隊(duì)形縱向位置保持控制方法

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

(29)

偏差eD為

eD=Di-D

(30)

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

(31)

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

eV=Vi-V。

(32)

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

(33)

2.4 隊(duì)形高度保持控制方法

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

eh=hi-PViD

(34)

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

(35)

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

eθ=θi-θ

(36)

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

(37)

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

3 數(shù)值仿真驗(yàn)證

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

3.1 航路飛行段隊(duì)形保持仿真實(shí)驗(yàn)

數(shù)值仿真實(shí)驗(yàn)中目標(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的速度沿航路飛行，編隊(duì)飛機(jī)從初始位置出發(fā)，進(jìn)行編隊(duì)飛行。結(jié)果如圖6所示。

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

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

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

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

3.3 航路切換段隊(duì)形仿真實(shí)驗(yàn)(5機(jī))

為驗(yàn)證本文所設(shè)計(jì)算法的適用性，對(duì)前述編隊(duì)的隊(duì)形進(jìn)行擴(kuò)展，采用5架飛機(jī)進(jìn)行編隊(duì)飛行，增加UAV4和UAV5構(gòu)成新的“V”字型編隊(duì)，UAV4，UAV5與UAV1保持d=200 m,α=45°的隊(duì)形飛行。目標(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ī)編隊(duì)初始位置信息

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

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

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

4 總結(jié)

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