多無(wú)人機(jī)自適應(yīng)編隊(duì)協(xié)同航跡規(guī)劃

許 洋 ，秦小林 *，劉 佳 ，張力戈

（1.中國(guó)科學(xué)院成都計(jì)算機(jī)應(yīng)用研究所，成都610041； 2.中國(guó)科學(xué)院大學(xué)計(jì)算機(jī)與控制學(xué)院，北京100080）

（?通信作者電子郵箱qinxl2001@126.com）

0 引言

無(wú) 人 機(jī)（Unmanned Aerial Vehicle，UAV）航 跡 規(guī) 劃（trajectory planning）是指在初始狀態(tài)、任務(wù)目標(biāo)、威脅區(qū)和一些已知環(huán)境信息的情況下獲得性能最優(yōu)的規(guī)劃問(wèn)題，是任務(wù)規(guī)劃系統(tǒng)的關(guān)鍵技術(shù)之一。目前單機(jī)航跡規(guī)劃技術(shù)已經(jīng)相當(dāng)成熟，然而在未來(lái)戰(zhàn)場(chǎng)，多無(wú)人機(jī)編隊(duì)協(xié)同執(zhí)行任務(wù)將成為主要的作戰(zhàn)形式，而協(xié)同航跡規(guī)劃是提高無(wú)人機(jī)協(xié)同作戰(zhàn)效能，保證無(wú)人機(jī)協(xié)同作戰(zhàn)順利實(shí)施的關(guān)鍵。多無(wú)人機(jī)協(xié)同航跡規(guī)劃在為各無(wú)人機(jī)規(guī)劃出滿足約束條件的最優(yōu)航跡時(shí)，還需考慮各無(wú)人機(jī)之間的空間協(xié)同性以及時(shí)間協(xié)同性［1］，其主要任務(wù)包括航跡規(guī)劃、碰撞避免、隊(duì)形形成/重構(gòu)以及隊(duì)形保持等。

目前，針對(duì)多無(wú)人機(jī)協(xié)同航跡規(guī)劃問(wèn)題，主要采用的算法包括人工勢(shì)場(chǎng)法（Artificial Potential Field，APF）［2-3］、粒子群優(yōu)化（Particle Swarm Optimization，PSO）算法［4］、滑?？刂疲?］、PID（Proportion Integration Differentiation）［6-7］等。文獻(xiàn)［1］采用協(xié)同非支配排序進(jìn)化算法，根據(jù)多架無(wú)人機(jī)的航行距離、安全性、時(shí)間以及空間的協(xié)同性進(jìn)行規(guī)劃；文獻(xiàn)［8］采用混合粒子群優(yōu)化算法與遺傳算法相結(jié)合的方法，實(shí)現(xiàn)了對(duì)多無(wú)人機(jī)編隊(duì)重構(gòu)過(guò)程中參數(shù)最優(yōu)化問(wèn)題的求解；文獻(xiàn)［9］通過(guò)引入分布式結(jié)構(gòu)和層次網(wǎng)絡(luò)，并基于由指揮官、虛擬領(lǐng)航者和跟隨者組成的多無(wú)人機(jī)系統(tǒng)建立無(wú)人機(jī)協(xié)同機(jī)制，實(shí)現(xiàn)了多無(wú)人機(jī)自主編隊(duì)和重組任務(wù)；文獻(xiàn)［10］采用任務(wù)分配和粒子群優(yōu)化算法，實(shí)現(xiàn)了對(duì)無(wú)人機(jī)群編隊(duì)轉(zhuǎn)換的高效策略。以上幾種方法都是針對(duì)無(wú)障礙環(huán)境下多無(wú)人機(jī)的編隊(duì)協(xié)同控制問(wèn)題。

為了確保復(fù)雜環(huán)境下多無(wú)人機(jī)編隊(duì)能夠安全、快速到達(dá)任務(wù)區(qū)域，需要設(shè)計(jì)滿足一定約束條件及性能指標(biāo)最優(yōu)的編隊(duì)航跡。文獻(xiàn)［11］利用共識(shí)的幾何優(yōu)化方法，結(jié)合分布式策略，實(shí)現(xiàn)了在包含靜、動(dòng)態(tài)障礙物的二、三維環(huán)境中，求解一組機(jī)器人協(xié)同控制的最優(yōu)參數(shù)；文獻(xiàn)［12］通過(guò)建立多無(wú)人機(jī)系統(tǒng)的特殊正交群（Special Orthogonal group）SO（3）控制模型，實(shí)現(xiàn)了多無(wú)人機(jī)在姿態(tài)一致的狀態(tài)下以期望的隊(duì)形穩(wěn)定飛行；文獻(xiàn)［13］采用神經(jīng)網(wǎng)絡(luò)方法，自適應(yīng)地逼近和在線補(bǔ)償動(dòng)態(tài)逆誤差，提高了多無(wú)人機(jī)隊(duì)形的穩(wěn)定性；文獻(xiàn)［3，14-15］采用了人工勢(shì)場(chǎng)法解決復(fù)雜環(huán)境下的編隊(duì)控制及避障問(wèn)題，通過(guò)各無(wú)人機(jī)之間的斥力與引力相互作用，實(shí)現(xiàn)整個(gè)協(xié)同任務(wù)，然而復(fù)雜環(huán)境下的避障動(dòng)作會(huì)在一定程度上擾亂無(wú)人機(jī)編隊(duì)隊(duì)形，影響編隊(duì)的穩(wěn)定性。

基于滾動(dòng)時(shí)域優(yōu)化思想的模型預(yù)測(cè)控制（Model Predictive Control，MPC）策略能夠顯式地考慮各種約束條件，并滾動(dòng)實(shí)施控制輸入，具有魯棒性較強(qiáng)等特點(diǎn)，因此在多無(wú)人機(jī)編隊(duì)控制等問(wèn)題中得到了廣泛應(yīng)用［16］；文獻(xiàn)［17］利用分布式模型預(yù)測(cè)控制（Distributed Model Predictive Control，DMPC）對(duì)計(jì)算得到的參考軌跡進(jìn)行跟蹤，并對(duì)障礙物進(jìn)行碰撞檢測(cè)，通過(guò)加入懲罰項(xiàng)實(shí)現(xiàn)動(dòng)態(tài)航跡跟蹤及避障；文獻(xiàn)［18］采用DMPC方法將編隊(duì)重構(gòu)問(wèn)題轉(zhuǎn)化為滾動(dòng)在線優(yōu)化問(wèn)題，并采用微分進(jìn)化（Differential Evolution，DE）方法進(jìn)行求解。

然而上述工作主要集中在單一編隊(duì)隊(duì)形控制、編隊(duì)飛行避障控制等方面，未考慮復(fù)雜環(huán)境下多無(wú)人機(jī)編隊(duì)隊(duì)形的適應(yīng)性以及隊(duì)形轉(zhuǎn)換的高效性。

本文針對(duì)以上問(wèn)題，提出了一種基于自適應(yīng)分布式模型預(yù)測(cè)控制的快速粒子群優(yōu)化（Adaptive Distributed Model Predictive Controller-Fast Particle Swarm Optimization，ADMPCFPSO）方法。該方法利用虛擬結(jié)構(gòu)法與領(lǐng)航跟隨法相結(jié)合的編隊(duì)策略，構(gòu)造出自適應(yīng)虛擬編隊(duì)引導(dǎo)點(diǎn)，并設(shè)計(jì)基于最小代價(jià)函數(shù)的優(yōu)化目標(biāo)；采用分布式模型預(yù)測(cè)控制方法將其轉(zhuǎn)化為滾動(dòng)在線優(yōu)化問(wèn)題，通過(guò)快速粒子群優(yōu)化（Fast Particle Swarm Optimization，F(xiàn)PSO）算法對(duì)問(wèn)題進(jìn)行求解。仿真結(jié)果表明，該方法可以高效解決復(fù)雜環(huán)境下的多無(wú)人機(jī)協(xié)同航跡規(guī)劃問(wèn)題，與基于人工勢(shì)場(chǎng)法的編隊(duì)協(xié)同控制方案［3］相比，航跡規(guī)劃的平均消耗時(shí)間降低了31.6%。

1 相關(guān)工作

1.1 模型預(yù)測(cè)控制

模型預(yù)測(cè)控制是一種基于滾動(dòng)時(shí)域的在線優(yōu)化控制策略，其基本思想是，假設(shè)當(dāng)前時(shí)間點(diǎn)為k，在有限時(shí)域［k，k+j］，通過(guò)使某一性能指標(biāo)最優(yōu)化來(lái)確定其未來(lái)的控制作用，選擇第一個(gè)最優(yōu)控制輸入作為當(dāng)前的控制輸入，在下一時(shí)刻k+1，計(jì)算［k+1，k+j+1］的控制輸出，重復(fù)以上過(guò)程，直到任務(wù)完成。其原理如圖1所示。

圖1 模型預(yù)測(cè)控制原理框圖Fig.1 Functional block diagramof model predictivecontrol

1.2 粒子群優(yōu)化

粒子群優(yōu)化（PSO）算法［4］是由美國(guó) Kennedy和Elberhart于1995年，受到鳥群覓食的行為啟發(fā)而提出的，是一種模擬鳥類飛行過(guò)程中群聚行為的智能優(yōu)化算法。

由m個(gè)粒子組成的群體在超維空間內(nèi)以一定的速度飛行，每個(gè)粒子表示問(wèn)題的一個(gè)解。粒子在優(yōu)化過(guò)程中，跟隨兩個(gè)極值實(shí)現(xiàn)自身的迭代。一個(gè)是粒子自身找到的歷史最優(yōu)解，稱為個(gè)體極值；另一個(gè)是整個(gè)群體找到的最優(yōu)解，稱為全局極值。粒子的速度和位置更新公式分別為：

式（1）是PSO的速度更新公式，其中表示第i個(gè)粒子在第k次迭代時(shí)的速度；c1和c2為學(xué)習(xí)因子；r1和r2是[0，1]區(qū)間內(nèi)均勻分布的隨機(jī)數(shù)；ω為慣性權(quán)重，表示粒子對(duì)原有速度繼承的程度；式（2）是位置更新公式，表示第i個(gè)粒子在第k次迭代時(shí)的位置；式（3）為慣性權(quán)重的更新公式，其中ωmin為最小權(quán)重；ωmax為最大權(quán)重；itermax為最大迭代次數(shù)。

2 問(wèn)題描述

假定在某次任務(wù)中，由n（n＞1）架無(wú)人機(jī)組成的編隊(duì)從某一起始區(qū)域飛往目標(biāo)區(qū)域執(zhí)行任務(wù)。以多旋翼無(wú)人機(jī)為例，討論其由初始狀態(tài)（初始區(qū)域）出發(fā)，以最小代價(jià)到達(dá)目標(biāo)區(qū)域。在此過(guò)程中，需要綜合考慮飛行環(huán)境中的威脅、無(wú)人機(jī)自身性能以及編隊(duì)控制等約束條件，為各無(wú)人機(jī)實(shí)時(shí)規(guī)劃航跡。因此，提出以下兩個(gè)假設(shè)。

假設(shè)1 在航跡規(guī)劃過(guò)程中，將單個(gè)無(wú)人機(jī)看作質(zhì)點(diǎn)，多無(wú)人機(jī)編隊(duì)需考慮編隊(duì)寬度；線性編隊(duì)屬于特例，不考慮寬度。

假設(shè)2 多無(wú)人機(jī)在寬闊區(qū)域，保持特定的編隊(duì)飛行，增大偵察范圍，從而減小可能存在的風(fēng)險(xiǎn)；在狹窄區(qū)域，無(wú)需考慮偵察范圍，以線性編隊(duì)飛行，保證多無(wú)人機(jī)安全通過(guò)該區(qū)域。

2.1 協(xié)同控制架構(gòu)

多無(wú)人機(jī)協(xié)同控制架構(gòu)包括全局航跡規(guī)劃系統(tǒng)和自適應(yīng)編隊(duì)協(xié)同系統(tǒng)，如圖2所示。全局航跡規(guī)劃系統(tǒng)主要根據(jù)部分已知環(huán)境信息，預(yù)先計(jì)算全局代價(jià)圖，在滿足相應(yīng)的約束條件（自身動(dòng)力學(xué)約束、編隊(duì)隊(duì)形約束、環(huán)境約束等）下，利用ADMPC-FPSO動(dòng)態(tài)規(guī)劃航跡，引導(dǎo)整個(gè)編隊(duì)以最低代價(jià)從初始區(qū)域前往目標(biāo)區(qū)域。

圖2 多無(wú)人機(jī)協(xié)同控制架構(gòu)Fig.2 Framework of multi-UAV cooperative control

自適應(yīng)編隊(duì)協(xié)同系統(tǒng)是以虛擬編隊(duì)引導(dǎo)點(diǎn)為主的編隊(duì)控制策略。無(wú)人機(jī)編隊(duì)通過(guò)感知環(huán)境（通道寬度），選取合適隊(duì)形，結(jié)合虛擬編隊(duì)引導(dǎo)點(diǎn)與ADMPC-FPSO，在滿足所有約束條件下，動(dòng)態(tài)規(guī)劃最優(yōu)航跡并保持特定的隊(duì)形。規(guī)劃過(guò)程中，編隊(duì)隊(duì)形之間的變換通過(guò)改變虛擬編隊(duì)引導(dǎo)點(diǎn)實(shí)現(xiàn)。

2.2 編隊(duì)飛行中全局航跡規(guī)劃問(wèn)題

為滿足假設(shè)2，在航跡規(guī)劃過(guò)程中，若只考慮某一固定編隊(duì)，可能因部分狹窄區(qū)域只允許線性編隊(duì)通過(guò)，而造成該區(qū)域被忽略，大大增加全局代價(jià)，導(dǎo)致最終無(wú)法得到全局最優(yōu)航跡。

利用“領(lǐng)航者-跟隨者”結(jié)構(gòu)（后文，將該結(jié)構(gòu)中的“領(lǐng)航者”統(tǒng)稱為“領(lǐng)航無(wú)人機(jī)”，而“跟隨者”統(tǒng)稱為“跟隨無(wú)人機(jī)”），在全局航跡規(guī)劃過(guò)程中，主要對(duì)領(lǐng)航無(wú)人機(jī)進(jìn)行動(dòng)態(tài)航跡規(guī)劃。領(lǐng)航無(wú)人機(jī)處于編隊(duì)的中間位置，在非線性編隊(duì)的情況下，規(guī)劃過(guò)程必須考慮無(wú)人機(jī)編隊(duì)寬度約束，即領(lǐng)航無(wú)人機(jī)與任一障礙物的距離至少保持編隊(duì)寬度的一半。當(dāng)無(wú)人機(jī)進(jìn)入狹窄區(qū)域，編隊(duì)變換為線性編隊(duì)，此時(shí)可不考慮編隊(duì)寬度，從而盡可能減少全局代價(jià)。

在上述約束條件下，可將其看作單獨(dú)地對(duì)領(lǐng)航無(wú)人機(jī)的航跡規(guī)劃問(wèn)題。

2.3 基于虛擬編隊(duì)引導(dǎo)點(diǎn)的編隊(duì)變換及控制

在本文中，主要考慮線性編隊(duì)和三角編隊(duì)之間的變換，如圖3所示。在自適應(yīng)編隊(duì)變換過(guò)程中，首先根據(jù)領(lǐng)航無(wú)人機(jī)的狀態(tài)信息，選取合適編隊(duì)，并計(jì)算虛擬編隊(duì)引導(dǎo)點(diǎn)。而后跟隨無(wú)人機(jī)前往其對(duì)應(yīng)的虛擬編隊(duì)引導(dǎo)點(diǎn)，實(shí)現(xiàn)編隊(duì)變換。

在形成虛擬編隊(duì)引導(dǎo)點(diǎn)后，對(duì)各跟隨無(wú)人機(jī)來(lái)說(shuō)，與2.2節(jié)中描述的問(wèn)題類似，只需將虛擬編隊(duì)引導(dǎo)點(diǎn)作為各對(duì)應(yīng)跟隨無(wú)人機(jī)的臨時(shí)目標(biāo)位置即可。

各無(wú)人機(jī)的規(guī)劃相互獨(dú)立，通過(guò)建立通信保證實(shí)時(shí)信息交互，以實(shí)現(xiàn)對(duì)各無(wú)人機(jī)的分布式控制。

圖3 多無(wú)人機(jī)編隊(duì)變換示意圖Fig.3 Schematic diagramof multi-UAVformationtransformation

3 數(shù)學(xué)模型

3.1 無(wú)人機(jī)動(dòng)力學(xué)模型

在飛行過(guò)程中，假定無(wú)人機(jī)在短時(shí)間內(nèi)保持在同一高度飛行，其二維質(zhì)心運(yùn)動(dòng)方程［17］為：

式中：（xi，yi）為無(wú)人機(jī)i的二維坐標(biāo)；V表示飛行速度；ψ?i為航向角；ui為對(duì)應(yīng)的控制量?？刂屏颗c航向角約束為：

3.2 航跡規(guī)劃目標(biāo)函數(shù)模型設(shè)計(jì)

多無(wú)人機(jī)系統(tǒng)中，對(duì)各無(wú)人機(jī)建立的數(shù)學(xué)模型一致，區(qū)別在于目標(biāo)位置的選取，其線性時(shí)不變系統(tǒng)動(dòng)力學(xué)可統(tǒng)一描述為：

式中：x(k)∈ RNx是狀態(tài)向量，u(k)∈ RNu是輸入向量；式（5）為無(wú)人機(jī)的狀態(tài)空間模型，式（6）為狀態(tài)和輸入需要滿足的約束條件；其中γ(k)為k時(shí)刻需要滿足的約束條件，輸出向量y(k)∈ RNy。

一般的目標(biāo)函數(shù)，采用如下形式：

式中：l(?)是代價(jià)函數(shù)，χF是目標(biāo)集。

在航跡規(guī)劃過(guò)程中，當(dāng)控制輸入被執(zhí)行，形成新的狀態(tài)，基于該狀態(tài)進(jìn)行下一步優(yōu)化，產(chǎn)生新的控制輸入，循環(huán)執(zhí)行直到任務(wù)結(jié)束。采用MPC算法，基于滾動(dòng)時(shí)域框架的優(yōu)化描述為

3.3 自適應(yīng)虛擬編隊(duì)引導(dǎo)點(diǎn)

本文采用虛擬編隊(duì)引導(dǎo)點(diǎn)方法完成自適應(yīng)編隊(duì)飛行控制任務(wù)，如圖4所示，以三個(gè)無(wú)人機(jī)為例，UAVleader為領(lǐng)航無(wú)人機(jī)，UAVfollower#1和UAVfollower#2為跟隨無(wú)人機(jī)所對(duì)應(yīng)的虛擬編隊(duì)引導(dǎo)點(diǎn)，引導(dǎo)點(diǎn)沿?zé)o人機(jī)運(yùn)動(dòng)方向?qū)ΨQ分布，領(lǐng)航無(wú)人機(jī)輸入向量為（ulx，uly），UAVfollower#i的坐標(biāo)為(xfi，yfi)，編隊(duì)半徑為rf。

圖4 虛擬編隊(duì)引導(dǎo)點(diǎn)坐標(biāo)系Fig.4 Virtual formation guidepoint coordinatesystem

由于本文針對(duì)多旋翼無(wú)人機(jī)飛行控制，在飛行過(guò)程中，領(lǐng)航無(wú)人機(jī)可能會(huì)出現(xiàn)轉(zhuǎn)彎角度較大的情況，為避免出現(xiàn)大幅度甩尾情況，對(duì)ld做如下處理：

式中cld為減幅權(quán)重。

結(jié)合式（11）、（12）、（13），求出虛擬編隊(duì)引導(dǎo)點(diǎn)中UAVfollower#1和UAVfollower#2的坐標(biāo)。

式中(xfi，yfi)為第i個(gè)虛擬引導(dǎo)點(diǎn)的坐標(biāo)。

通過(guò)自適應(yīng)調(diào)節(jié)rf值，可在一定程度上提高復(fù)雜環(huán)境下編隊(duì)的適應(yīng)度。假設(shè)檢測(cè)前方通道寬度為Rdet，那么：

若Rdet＜Rmin，則變換為線性編隊(duì)。Rmin為無(wú)人機(jī)之間的最小安全距離；φ為保證無(wú)人機(jī)飛行安全的一個(gè)極小值。

3.4 多無(wú)人機(jī)編隊(duì)通信拓?fù)淠Ｐ?/h3>

本文采用圖論對(duì)無(wú)人機(jī)之間的通信拓?fù)潢P(guān)系進(jìn)行描述，設(shè)G（V，E，A）表示一個(gè)加權(quán)無(wú)向圖，其中：V是圖的頂點(diǎn)集，E是圖的邊集，A=(aij)N×N稱為圖的鄰接矩陣。在多旋翼編隊(duì)飛行問(wèn)題中，將每一個(gè)無(wú)人機(jī)看作圖的一個(gè)頂點(diǎn)，無(wú)人機(jī)之間的信息交互連接就可以用圖G相應(yīng)的邊來(lái)表示。若aij＞0，則表示邊(j，i)∈E，即第i個(gè)無(wú)人機(jī)能夠從第j個(gè)無(wú)人機(jī)獲得所需信息，否則aij=0。由此可以定義頂點(diǎn)i的鄰居集為Ni={j∈V|(j，i)∈E}。

如圖5所示，在無(wú)人機(jī)的整個(gè)通信拓?fù)浣Y(jié)構(gòu)中，各無(wú)人機(jī)只與距離其半徑r以內(nèi)的無(wú)人機(jī)建立通信，完成信息交換，滿足以下公式：

式中：d ij為i與j之間的距離，r為無(wú)人機(jī)的通信半徑。

圖5 多無(wú)人機(jī)編隊(duì)通信拓?fù)涫疽鈭DFig.5 Multi-UAV formation communication topology

4 ADMPC-FPSO自適應(yīng)協(xié)同航跡規(guī)劃

多無(wú)人機(jī)協(xié)同航跡規(guī)劃主要分為全局航跡規(guī)劃和自適應(yīng)編隊(duì)協(xié)同控制兩部分。全局航跡規(guī)劃可簡(jiǎn)單看作對(duì)領(lǐng)航無(wú)人機(jī)的航跡規(guī)劃過(guò)程，與單個(gè)無(wú)人機(jī)不同，這里必須考慮編隊(duì)寬度的約束。對(duì)此，本文通過(guò)擴(kuò)充障礙物的方法實(shí)現(xiàn)可視點(diǎn)遷移，以此滿足編隊(duì)協(xié)同控制的相應(yīng)約束條件。

編隊(duì)全局航跡規(guī)劃過(guò)程主要包括兩個(gè)階段：第1階段為代價(jià)評(píng)估階段，根據(jù)當(dāng)前環(huán)境生成代價(jià)圖，利用計(jì)算得到的代價(jià)圖來(lái)表示航跡端點(diǎn)到目標(biāo)點(diǎn)的代價(jià)估計(jì)；第2階段為實(shí)時(shí)航跡規(guī)劃階段，在問(wèn)題描述中，已經(jīng)將航跡規(guī)劃表示成一個(gè)優(yōu)化問(wèn)題，在此主要是基于ADMPC-FPSO實(shí)時(shí)航跡規(guī)劃的一個(gè)優(yōu)化過(guò)程。

自適應(yīng)編隊(duì)協(xié)同控制主要包括兩個(gè)階段：第1個(gè)階段為虛擬編隊(duì)引導(dǎo)點(diǎn)的形成，根據(jù)無(wú)人機(jī)所處環(huán)境與自身狀態(tài)，自適應(yīng)選取并計(jì)算最優(yōu)虛擬編隊(duì)引導(dǎo)點(diǎn)的位置，為編隊(duì)隊(duì)形的形成、保持及變換提供前提條件；第2階段為各無(wú)人機(jī)在整個(gè)多無(wú)人機(jī)系統(tǒng)中，基于ADMPC-FPSO協(xié)同完成編隊(duì)隊(duì)形的形成、保持及變換任務(wù)。

4.1 代價(jià)圖

根據(jù)第1章可知，從航跡規(guī)劃全局看，粒子群優(yōu)化有可能陷入局部最優(yōu)。因此，需要預(yù)先進(jìn)行合理的代價(jià)轉(zhuǎn)換，如文獻(xiàn)［18］中提到的代價(jià)圖方法，利用可視圖［19］和Dijkstra［20］單源最短路徑算法，搜索從目標(biāo)點(diǎn)到各個(gè)點(diǎn)的最短路徑。

對(duì)于領(lǐng)航無(wú)人機(jī)，由第3章可知，航跡端點(diǎn)為x(k+Hp+1)，因此基于代價(jià)圖的終端懲罰項(xiàng)f(?)可以表示如下：

式中：xvis為基于代價(jià)圖的可視點(diǎn)；d(x(k+Hp+1)，xvis)為航跡端點(diǎn)到可視點(diǎn)的代價(jià)；Cvis為可視點(diǎn)到目標(biāo)點(diǎn)的代價(jià)。

對(duì)于跟隨無(wú)人機(jī)，在滿足相應(yīng)的約束條件，其規(guī)劃過(guò)程實(shí)際上是局部的，因此終端懲罰項(xiàng)如下：

式中：xvfgp為對(duì)應(yīng)的虛擬編隊(duì)引導(dǎo)點(diǎn)；d(x(k+Hp+1)，xvfgp)為航跡端點(diǎn)到對(duì)應(yīng)虛擬編隊(duì)引導(dǎo)點(diǎn)的代價(jià)。

4.2 基于ADMPC-FPSO協(xié)同航跡規(guī)劃

協(xié)同航跡規(guī)劃是復(fù)雜約束條件下的最優(yōu)化問(wèn)題，采用粒子群優(yōu)化算法進(jìn)行航跡規(guī)劃時(shí)，迭代初期，控制輸入和狀態(tài)可能存在違反約束的情況，算法需要迭代若干次才能產(chǎn)生滿足要求的輸入，并在此基礎(chǔ)上進(jìn)一步尋優(yōu)。為提高算法的效率，本文根據(jù)粒子違反約束的程度來(lái)更新粒子。

在1.1節(jié)中對(duì)MPC進(jìn)行了介紹，實(shí)際應(yīng)用過(guò)程中，MPC分為集中式和分布式兩種。集中式在多無(wú)人機(jī)編隊(duì)飛行控制中，隨著無(wú)人機(jī)數(shù)量的增加，其求解復(fù)雜度、通信量急劇增長(zhǎng)。實(shí)際上，各無(wú)人機(jī)的動(dòng)力學(xué)特性互相解耦［21］，無(wú)人機(jī)可以通過(guò)各自的約束和狀態(tài)條件進(jìn)行獨(dú)立控制，故適于采用DMPC結(jié)構(gòu)進(jìn)行協(xié)同控制。

4.2.1 評(píng)價(jià)函數(shù)設(shè)計(jì)

評(píng)價(jià)函數(shù)用于粒子群中每個(gè)個(gè)體計(jì)算適應(yīng)度。在文獻(xiàn)［18］中，對(duì)任意輸入，可采用下面的評(píng)價(jià)函數(shù)：

式中：C（u）為式（8）計(jì)算的航跡代價(jià)；BigM為一個(gè)比較大的數(shù)；C為約束違背量，采用式（19）計(jì)算如下：

式中：f(li)表示航跡段穿越禁飛區(qū)的約束違背量；g(vi)和h(ai)表示速度和加速度的約束違背量；w1、w2、w3為權(quán)系數(shù)。

4.2.2 粒子群優(yōu)化求解

根據(jù)第1章中相關(guān)工作的介紹，假設(shè)粒子群種群數(shù)為N，每個(gè)粒子的空間維數(shù)為D，第i個(gè)粒子的輸入向量和速度為u i=(ui，1，ui，2，…，ui，D)，v i=(vi，1，vi，2，…，vi，D)。第i個(gè)粒子最優(yōu)解 為P i=(pi，1，pi，2，…，pi，D)，粒 子 群 最 優(yōu) 解 為G=(g1，g2，…，gD)。根據(jù)粒子群優(yōu)化策略，按照式（1）、（2）、（3）更新粒子。在本文中，式（3）中的ωmax不再是一成不變的，而是根據(jù)粒子的適應(yīng)值來(lái)更新，如果粒子是可行解，則按照ωk更新，如果粒子是不可行解，則按照式（20）更新：

式中，F(xiàn)k，i(u)為第k次迭代第i個(gè)粒子的適應(yīng)度?；讦豰ax改進(jìn)的快速粒子群優(yōu)化（FPSO）算法與經(jīng)典粒子群優(yōu)化算法相比，優(yōu)化速度表現(xiàn)出明顯的提升。

4.2.3 粒子群優(yōu)化算法流程

根據(jù)上述描述，以下部分給出了粒子群優(yōu)化算法的基本流程。

初始化粒子群P，包括粒子的位置和速度，全局最優(yōu)值以及局部最優(yōu)值

4.2.4 ADMPC滾動(dòng)優(yōu)化

在每一個(gè)采樣時(shí)刻，MPC通過(guò)求解一個(gè)有限時(shí)域的優(yōu)化問(wèn)題，得到系統(tǒng)當(dāng)前時(shí)刻的最優(yōu)控制序列，從而實(shí)現(xiàn)在整個(gè)控制時(shí)域內(nèi)系統(tǒng)的在線閉環(huán)控制。多無(wú)人機(jī)協(xié)同航跡規(guī)劃ADMPC滾動(dòng)優(yōu)化過(guò)程如圖6所示。

圖6 多無(wú)人機(jī)協(xié)同航跡規(guī)劃ADMPC滾動(dòng)優(yōu)化過(guò)程Fig.6 Flowchart of multi-UAV cooperativetrajectory planning ADMPCrolling optimization

其中，根據(jù)環(huán)境約束及自身狀態(tài)，多無(wú)人機(jī)編隊(duì)采取必要的隊(duì)形變換或避障等動(dòng)作。通過(guò)ADMPC方法對(duì)無(wú)人機(jī)狀態(tài)進(jìn)行預(yù)測(cè)，得到滾動(dòng)在線優(yōu)化問(wèn)題，通過(guò)4.2.3節(jié)中提到的FPSO算法對(duì)其進(jìn)行優(yōu)化求解，計(jì)算其最優(yōu)適應(yīng)度，得到最優(yōu)控制序列并執(zhí)行，完成整個(gè)自適應(yīng)編隊(duì)控制任務(wù)過(guò)程。

4.3 多無(wú)人機(jī)協(xié)同航跡規(guī)劃流程

步驟1 首先初始化無(wú)人機(jī)參數(shù)，設(shè)置最大速度、最大加速度等限制，設(shè)置模型預(yù)測(cè)控制參數(shù)，參數(shù)設(shè)置如表1所示。

步驟2 根據(jù)實(shí)時(shí)仿真環(huán)境，自適應(yīng)選擇多無(wú)人機(jī)編隊(duì)寬度rf或隊(duì)形，計(jì)算并更新虛擬編隊(duì)引導(dǎo)點(diǎn)及與跟隨無(wú)人機(jī)的對(duì)應(yīng)關(guān)系，并根據(jù)式（15）建立各無(wú)人機(jī)間的通信關(guān)系。

步驟3 利用粒子群優(yōu)化算法，分別對(duì)領(lǐng)航無(wú)人機(jī)和各跟隨無(wú)人機(jī)進(jìn)行航跡規(guī)劃問(wèn)題求解。

步驟4 取最優(yōu)結(jié)果的第一個(gè)控制輸入模型中，即代入式（5），計(jì)算無(wú)人機(jī)下一時(shí)刻的狀態(tài)。

步驟5 如果到達(dá)目標(biāo)點(diǎn)，完成協(xié)同航跡規(guī)劃，任務(wù)結(jié)束；否則轉(zhuǎn)步驟2。

5 仿真實(shí)驗(yàn)分析

為了驗(yàn)證方法的有效性，本文在Matlab 2016環(huán)境下進(jìn)行了仿真實(shí)驗(yàn)。仿真實(shí)驗(yàn)將空間的障礙抽象為大小不一的矩形。領(lǐng)航無(wú)人機(jī)的預(yù)定起始位置為（25，0），目標(biāo)位置為（-28，-6）。

多旋翼無(wú)人機(jī)其動(dòng)力學(xué)可以由具有速度和加速度約束的質(zhì)點(diǎn)動(dòng)力學(xué)模型近似：

實(shí)驗(yàn)主要針對(duì)三架無(wú)人機(jī)在不同編隊(duì)形式下的協(xié)同航跡規(guī)劃，包括線性、三角、線性-三角編隊(duì)（二者可根據(jù)環(huán)境情況相互變換），同時(shí)對(duì)三種編隊(duì)形式下的飛行總航程進(jìn)行了對(duì)比。另外，對(duì)九架無(wú)人機(jī)也做了簡(jiǎn)單的仿真實(shí)驗(yàn)，進(jìn)一步展示了文中方法的靈活性和可擴(kuò)展性。

表1為仿真實(shí)驗(yàn)參數(shù)，包括模型預(yù)測(cè)控制、多旋翼無(wú)人機(jī)基本參數(shù)、粒子群以及約束等基礎(chǔ)參數(shù)。

表1 仿真實(shí)驗(yàn)參數(shù)Tab.1 Simulation experiment parameters

實(shí)驗(yàn)一 三架無(wú)人機(jī)編隊(duì)飛行。

圖7展示了三架無(wú)人機(jī)協(xié)同航跡規(guī)劃的預(yù)測(cè)過(guò)程，每次預(yù)測(cè)未來(lái)6個(gè)航跡點(diǎn)，并將第1個(gè)控制輸入作為實(shí)際的輸入值，剩下的預(yù)測(cè)航跡點(diǎn)作為參考航跡點(diǎn)。在此實(shí)驗(yàn)中，跟隨無(wú)人機(jī)的初始位置分別為P1（26，0）和P2（27，0）。

圖7 多無(wú)人機(jī)協(xié)同航跡規(guī)劃預(yù)測(cè)示意圖（三架無(wú)人機(jī)）Fig.7 Prediction of multi-UAV cooperative trajectory planning（three UAVs）

如圖8所示，實(shí)線為將每次預(yù)測(cè)值的第1個(gè)控制輸入作為實(shí)際輸入后規(guī)劃的領(lǐng)航無(wú)人機(jī)完整航跡，6個(gè)虛線框則分別展示了不同環(huán)境下三架無(wú)人機(jī)的編隊(duì)情況。在經(jīng)過(guò)狹窄區(qū)域時(shí)，多無(wú)人機(jī)以線性編隊(duì)飛行，而在經(jīng)過(guò)空曠區(qū)域時(shí)，則以三角編隊(duì)飛行?？梢钥闯?，在滿足所有約束條件的前提下，規(guī)劃出一條能保證多無(wú)人機(jī)編隊(duì)安全穿過(guò)障礙區(qū)的實(shí)際可飛航跡。

實(shí)驗(yàn)二 線性、三角、線性-三角編隊(duì)航跡規(guī)劃對(duì)比。

圖9展示了線性、三角、線性-三角三種編隊(duì)形式下的飛行航跡對(duì)比情況。線性編隊(duì)在編隊(duì)航跡規(guī)劃中，可簡(jiǎn)單地看作單個(gè)無(wú)人機(jī)并需滿足其相應(yīng)約束條件；而三角編隊(duì)必須考慮編隊(duì)寬度的約束；線性-三角編隊(duì)表示整個(gè)編隊(duì)根據(jù)不同環(huán)境進(jìn)行相應(yīng)隊(duì)形轉(zhuǎn)換，使得規(guī)劃出的航跡在保證較低代價(jià)的同時(shí)，還滿足假設(shè)2的要求。

圖8 多無(wú)人機(jī)協(xié)同飛行過(guò)程Fig.8 Processof multi-UAV cooperativeflight

圖9 基于不同編隊(duì)形式的航跡規(guī)劃對(duì)比Fig.9 Trajectory planningcomparison of different formations

表2展示了在三種不同編隊(duì)形式下，三架無(wú)人機(jī)飛行的航程對(duì)比情況，可以看出，在以航程為代價(jià)的航跡規(guī)劃算法中，線性-三角編隊(duì)在三種編隊(duì)形式里，航跡規(guī)劃最優(yōu)，這也充分說(shuō)明了本文所提出方法的優(yōu)勢(shì)所在。

表2 不同編隊(duì)形式航程比較 單位：mTab.2 Flight distancecomparison of different formations unit：m

實(shí)驗(yàn)三 基于人工勢(shì)場(chǎng)法（APF）的編隊(duì)協(xié)同控制方案［3］與本文編隊(duì)協(xié)同控制方案的對(duì)比。

圖10是利用APF進(jìn)行多無(wú)人機(jī)編隊(duì)協(xié)同控制的飛行效果，實(shí)線為虛擬領(lǐng)航無(wú)人機(jī)飛行航跡（航跡規(guī)劃算法與本文一致），4個(gè)虛線框展示了4個(gè)代表性階段的無(wú)人機(jī)編隊(duì)飛行情況，實(shí)線上的圓點(diǎn)為虛擬領(lǐng)航無(wú)人機(jī)在各階段的對(duì)應(yīng)位置?？梢钥闯觯噍^于本文的方案（如圖8），基于APF的方案，無(wú)人機(jī)在一定程度上保持了編隊(duì)飛行，但很難形成穩(wěn)定的編隊(duì)隊(duì)形，且難以預(yù)測(cè)任一無(wú)人機(jī)的飛行情況。

圖11進(jìn)一步展示了利用本文方案，在整個(gè)規(guī)劃過(guò)程中，其單步時(shí)間及平均時(shí)間都少于基于APF的編隊(duì)控制方案。APF平均時(shí)間為0.0918 s，本文方案平均時(shí)間為0.0609 s。

實(shí)驗(yàn)四 無(wú)人機(jī)集群（九架）編隊(duì)協(xié)同航跡規(guī)劃。

無(wú)人機(jī)集群與三架無(wú)人機(jī)略有不同，其編隊(duì)控制必須考慮通信距離與通信拓?fù)潢P(guān)系。在本實(shí)驗(yàn)中，增加的無(wú)人機(jī)以前三架無(wú)人機(jī)為參考，依據(jù)編隊(duì)情況，確定相互之間對(duì)應(yīng)的位置關(guān)系以及飛行次序，從而建立合理的通信連接，以保證實(shí)時(shí)信息交換，并形成穩(wěn)定的編隊(duì)隊(duì)形。

圖10 基于人工勢(shì)場(chǎng)法實(shí)現(xiàn)編隊(duì)協(xié)同控制效果Fig.10 Effect of formation cooperativecontrol based on artificial potential field method

圖11 協(xié)同航跡規(guī)劃方案時(shí)間對(duì)比Fig.11 Timecomparison of cooperativetrajectory planningschemes

圖12展示了無(wú)人機(jī)集群（九架）自適應(yīng)編隊(duì)協(xié)同飛行過(guò)程圖，實(shí)線表示領(lǐng)航無(wú)人機(jī)飛行軌跡，兩個(gè)虛線框展示了兩個(gè)代表性階段的無(wú)人機(jī)協(xié)同編隊(duì)飛行情況?？梢钥闯?，在狹窄區(qū)域多無(wú)人機(jī)以線性編隊(duì)飛行，在寬闊區(qū)域則以形呈特定編隊(duì)飛行。通過(guò)此實(shí)驗(yàn)進(jìn)一步驗(yàn)證了本文方法，有效解決了多無(wú)人機(jī)數(shù)量限制、通信拓?fù)浣Y(jié)構(gòu)約束等方面的難題。

6 結(jié)語(yǔ)

本文設(shè)計(jì)的基于自適應(yīng)分布式模型預(yù)測(cè)控制的多無(wú)人機(jī)協(xié)同航跡規(guī)劃方案，采用模型預(yù)測(cè)控制思想，將整個(gè)控制過(guò)程轉(zhuǎn)化為基于滾動(dòng)時(shí)域的在線優(yōu)化問(wèn)題；結(jié)合分布式策略，有效解決了多無(wú)人機(jī)在計(jì)算資源與通信鏈路等方面的約束限制問(wèn)題，以及各無(wú)人機(jī)獨(dú)立避障的問(wèn)題。采用虛擬結(jié)構(gòu)法與領(lǐng)航跟隨法結(jié)合的策略，提出了虛擬編隊(duì)引導(dǎo)點(diǎn)，對(duì)多無(wú)人機(jī)進(jìn)行自適應(yīng)編隊(duì)控制，同時(shí)通過(guò)改進(jìn)的粒子群優(yōu)化算法對(duì)整個(gè)控制過(guò)程進(jìn)行處理。仿真結(jié)果表明，利用本文提出的方案，有效解決了多無(wú)人機(jī)協(xié)同航跡規(guī)劃問(wèn)題，同時(shí)在編隊(duì)隊(duì)形的保持與變換控制中也表現(xiàn)出較好的效果；另外，該方案的應(yīng)用還極大地增加了多無(wú)人機(jī)控制的靈活性，并降低了整個(gè)控制過(guò)程中的代價(jià)總和。在今后的工作中，可以把該方案擴(kuò)展到三維環(huán)境下的無(wú)人機(jī)集群自適應(yīng)編隊(duì)協(xié)同航跡規(guī)劃問(wèn)題，并進(jìn)一步提高求解算法的效率。