引入控制量變化率約束的雙層協同軌跡規劃器

溫 岳，馮肖雪，謝 天，龔柏春，吳云華

（1.北京理工大學自動化學院，北京 100081；2.南京航空航天大學航天學院，南京 210016）

0 引言

隨著戰場模式不斷發展，單亞音速飛行器目標攻擊已逐漸不能滿足戰場復雜態勢需要［1］，多亞音速飛行器協同攻擊逐漸成為戰場主要作戰方式［2］。而多飛行器協同軌跡規劃是實現多飛行器協同攻擊的關鍵技術之一。多飛行器協同軌跡規劃需要充分考慮環境障礙、飛行器性能約束、任務目標等條件，為了更好地完成任務通常需要飛行器實現編隊飛行。因此對于多飛行器編隊飛行軌跡規劃的研究具有重要意義。

多飛行器協同軌跡規劃問題通常建模為非線性動力學模型約束、狀態量和控制量約束等的最優控制問題。求解其最優控制問題的方法一般分為間接法和直接法［3］。間接法根據Pontryagin 極小值原理得到最優控制問題的一階必要條件，將其轉化為Hamilton邊值問題求解。但是由于亞音速飛行器動力學方程的強非線性和強耦合性，使用間接法求解存在轉化困難、推導復雜、難以求解等問題。而直接法通過對變量進行離散化，將最優控制問題轉化為非線性規劃（NLP）問題，再采用許多成熟的非線性規劃算法進行快速求解。直接法中的偽譜法由于其快速的收斂性和較高的計算精度，近年來逐漸成為軌跡規劃問題的一種常用方法。偽譜法將狀態量和控制量同時進行離散，通常再結合序列二次規劃（SQP）求解得到高精度軌跡。根據插值基函數以及配點類型的不同，目前發展比較完備的偽譜法主要包括Gauss 偽譜法［4］（GPM）、Legendre 偽譜法［5］（LPM）、Radau 偽譜法［6］（RPM）和Chebyshev 偽譜法［7］（CPM）。文獻［8］改進了Gauss 偽譜法，定義兩種新的斷點，將密集的高斯點定義到指定區域附近，使所有位置均可滿足復雜約束的要求。文獻［9］綜合考慮了無人作戰飛機氣動力特性、大氣環境特性等構建了無人作戰飛機四維攻擊軌跡規劃理論框架模型，采用RPM 進行轉化，利用SNOPT 軟件包進行求解。文獻［10］考慮無人機集群飛行過程中的約束條件，建立了集群飛行路徑規劃的數學模型，采用Rauda偽譜法進行求解，分別進行了無人機群集結任務仿真和編隊飛行任務仿真。

盡管基于偽譜法的軌跡規劃有諸多研究成果，但目前偽譜法仍然存在一些問題：1）大多文獻均未考慮控制量的變化率約束［11-13］，而控制量的變化率過大可能使制導系統無法進行跟蹤，造成實際規劃的路徑結果不可行。為了避免過度劇烈的控制量變化，控制量變化率的限制是非常重要的，這可以確?？刂葡到y在操作過程中更加平穩和可靠，同時也保證了系統的安全性和高效性。目前常用的處理形式是引入控制變量的導數作為虛擬控制量，將實際控制量轉化為狀態變量［14-16］。這種處理方式形式簡單，只需增加與控制變量相同數量的狀態方程即可達到控制量變化率約束的要求。但是這種處理方法增加了待優化變量的個數，進而增加了求解的困難度。同時增加的控制變量微分方程為線性微分方程，可能會產生奇異弧引起虛擬控制變量振蕩甚至發散。為此文獻［17］引入有限差分法將控制量變化率約束轉化為相鄰離散點處控制量的線性約束，避免了虛擬控制量振蕩問題。但是有限差分法的精度受到區間分辨率的限制，對于Legendre 偽譜法中使用的Legendre-Gauss-Lobatto配點（LGL配點）兩端緊密，中間稀疏的分布，可能會造成較大誤差。2）多導飛行器協同軌跡規劃問題約束更加復雜，使用偽譜法求解時較差的初值可能造成計算時間長、迭代次數增加甚至結果無法收斂。文獻［18-20］采用群智能算法進行初始值猜測，作為偽譜法求解的初始參考解，以提升解的最優性和收斂速度。但是文獻［18］采用群智能算法時并未考慮動力學方程，只給出了在位置狀態量初值，無法求解出全部的狀態量及控制量初值。文獻［19-20］的方法在使用群智能算法求解初值時需要進一步求解動力學方程，因此會額外增加大量計算量。

針對上述問題，本文提出了一種雙層亞音速飛行器協同軌跡規劃器。為了保證軌跡的可行性，基于Lagrange 插值函數計算控制量的導數作為約束項，避免引入新的狀態方程造成計算復雜度增加。使用頂層單飛行器規劃器在不考慮多飛行器協同約束的條件下使用較少的配點快速得到每個飛行器的粗略軌跡結果，將求解結果進行拉格朗日插值作為底層多飛行器規劃器的初值，以解決偽譜法初值敏感問題并提高優化效率。在底層多飛行器規劃器使用更多的配點以確保偽譜法求解的精度，在考慮多飛行器協同約束條件下進行偽譜法求解，實現多飛行器編隊飛行協同規劃，提高算法效率。最后通過仿真實驗驗證了算法的有效性。

1 編隊軌跡規劃問題建模

1.1 亞音速飛行器動力學模型

亞音速飛行器模型涉及自身姿態、空氣阻力、發動機推力等多個因素，并且是一個強耦合的復雜非線性系統。而亞音速飛行器動力學特性并非本文研究重點，因此為了簡化研究，將亞音速飛行器看作一個無延遲的理想模型，并將其看作一個質點。亞音速飛行器三維動力學模型如下［21］

式中：第i個亞音速飛行器狀態記為Xi=[xi，yi，zi，Vi，γi，ψi]T；N表示飛行器數量；[xi，yi，zi]T表示飛行器在三維空間下的坐標；Vi表示飛行器的速度大??；γi和ψi分別代表飛行器的航跡角和航向角；m代表飛行器的質量；g代表重力加速度取常值9.8 m/s2；ayi和azi分別代表水平橫向加速度垂直縱向加速度，即為飛行器的控制量記為Ui=［azi，ay］iT；Di代表飛行阻力，計算公式如下

式中：ρ代表空氣密度，其計算方式為ρ=ρ0exp (-zi／β)，ρ0=1.225 kg/m3，β=7 100 m，zi為飛行器高度；S代表飛行器參考面積；Cd代表阻力系數。

1.2 約束條件

1）狀態量和控制量約束

對于狀態量的約束主要包含飛行中的過程約束和終端約束，過程約束為飛行器狀態量的閾值，可表示為

式 中：xmin，xmax，ymin，ymax，zmin，zmax，Vmin，Vmax，γmin，γmax，ψmin，ψmax分別代表各狀態量的邊界值。

第i個飛行器終端約束為在初始時刻t0，i和末端時刻tf，i的狀態量，即

式中：X（t0，）i表示飛行器在初始時刻的狀態向量，X0，i表示每個飛行器的起始狀態值約束向量。X（tf，）i表示飛行器在終端時刻的狀態向量，Xf，i表示第i個飛行器的終端時刻的狀態值約束向量。

控制量約束為受限于飛行器的機動性，表示為

式中：aymax和azmax為大于0 的常量，決定了飛行器的機動性。

同時考慮軌跡可行性增加控制量變化率約束

式中：uymax和uzmax為大于0的常量。

2）威脅約束

在亞音速飛行器攻擊目標過程中可能遇到敵方火力威脅或由于天氣原因帶來的不可行區域，在飛行器飛行過程中需要盡可能避開這些威脅區域以確保攻擊任務的成功。為了簡化處理，將其空間區域分為可行區域與禁飛區，并將禁飛區建模為無限高的圓柱體。假設M個圓柱體中心坐標為（xjF，yjF），半徑為Rj，j=1，2，…，M，則威脅約束表示為

式中：（xi，y）i為第i個飛行器軌跡的水平面坐標，RF為飛行器到威脅區域邊界的最小安全距離。

3）編隊約束

針對多飛行器協同規劃問題，若要求到達目標點時刻一致，則飛行器協同時間約束為

為了描述飛行器編隊隊形，設定相鄰飛行器之間的橫向距離和縱向距離之間保持在一定的范圍內，隊形約束表示為［9］

式中：xij，min，yij，min，zij，min，xij，max，yij，max，zij，max為相鄰飛行器之間的距離閾值。

1.3 目標函數

根據亞音速飛行器執行任務的不同，多飛行器軌跡規劃可以選擇不同的性能指標。當確定飛行器起始位置和目標位置后，可以選擇飛行時間最短、飛行路徑最短、能量消耗最少等。不失一般性，本文選擇飛行器路徑長度和最短作為性能指標

1.4 最優控制問題

因此多飛行器軌跡規劃問題可以建模為如下Bolza形式目標函數的經典最優控制問題

式中：Φ表示終端性能值；l表示系統動態性能值。

最優控制問題的目標是確定控制量x（t）和狀態量u（t），最小化式（11）目標函數，同時滿足以下動力學方程約束

式中：f（x（t），u（t），t）為形如式（1）的動力學方程。

飛行器的終端約束、時間協同約束等等式約束統一表示為邊界約束

飛行器飛行中的過程約束、威脅約束、隊形約束等不等式約束統一表示為路徑約束，即

2 雙層亞音速飛行器協同軌跡規劃器

2.1 引入控制量變化率約束的Legendre偽譜法

Legendre 偽譜法的基本原理是將最優控制問題在一系列LGL 配點處離散化，然后通過拉格朗日插值多項式逼近連續控制和狀態，將其轉化為非線性規劃問題（NLP）。

因為軌跡優化問題是在時間區間［t0，t］f上定義的，但是LGL 點落在［-1，1］范圍內，所以自變量應該映射到一般區間τ∈［-1，1］，進行下式轉換

配點τk（k=1，2，…，n）是K階LGL 點，是多項式(1 -τ2)(τ)的根，即n-1階Legendre多項式的導數和點-1，1。

使用Lagrange插值多項式近似狀態變量x（τ）和控制變量u（τ）

對式（16）中狀態變量的近似表達式求導得到

每個拉格朗日多項式在LGL 點處的導數可以定義為如下微分矩陣

因此將動力學微分方程轉換為

控制量變化率約束對于軌跡可行性具有重要作用。通常為了滿足控制量變化率約束，需要引入新的狀態方程和虛擬控制量，即

式中：βzi和βyi表示控制量的變化率即引入的虛擬控制量。

將式（20）與式（1）共同作為亞音速飛行器動力學方程約束以進行求解，這種方法會增加問題的求解難度和計算復雜度。

為了保證飛行路徑的可行性并保證優化效率，本文改進控制量變化率約束。如式（16）偽譜法使用Lagrange 插值多項式近似控制變量，因此對控制量的近似表達求導可以快速得到控制量的導數。由控制量近似式計算得到控制量變化率為

因此控制量變化率約束表示為

式中：umax為控制量變化率的閾值。

通過引入控制量變化率不等式約束式（22），避免了傳統方法中引入新的狀態方程和虛擬控制量所帶來的計算復雜度增加問題，并在滿足控制量變化率約束的條件下實現了優化過程。由于在確定配點數后微分矩陣Dki為常量矩陣，因此式（22）將控制量變化率的約束轉化為控制量離散形式的線性約束，對控制量的一階偏導數為0，在求解過程中無需反復計算偏導數，減小計算量。

邊界約束轉換為

路徑約束轉換為

因此目標函數轉換為

2.2 雙層協同軌跡規劃器

雙層協同軌跡規劃器分為頂層單飛行器快速規劃器與底層多飛行器協同規劃器。頂層單飛行器快速規劃器在不考慮多飛行器協同約束的條件下使用較少的配點快速得到每個飛行器的粗略軌跡結果，將求解結果進行拉格朗日插值作為底層多飛行器規劃器的初值，底層多飛行器協同規劃器在考慮多飛行器協同約束條件下使用更多的配點進行偽譜法求解。具體流程如圖1所示。

2.2.1 頂層單飛行器快速規劃器

頂層單飛行器快速規劃器為每個飛行器分別單獨求解規劃軌跡，頂層規劃器不考慮編隊約束，僅考慮飛行器動力學方程（1）、狀態量和控制量約束式（3）～（6）以及威脅約束式（7）。

單飛行器快速規劃器采用少量的LGL 配點，設置迭代次數較少，可以有效地減少計算量，提高計算效率。由于單飛行器約束項較少，因此采用線性插值方法進行偽譜法求解初始化，其主要特點是快速求解，可以快速得到一條基本滿足約束條件的路徑，但是由于配點數較少，規劃的航路可能可行性較差，造成航路點精度差、除配點外的其它航路點不滿足威脅要求、控制量不滿足閾值要求等問題［8］。但是頂層單飛行器快速規劃器是以較少的計算量快速得到每一個飛行器的粗略軌跡，為底層多飛行器協同規劃器求解提供初值參考，優化底層多飛行器協同規劃器的計算效率。飛行器之間的編隊協同通過底層多飛行器協同規劃器進一步優化求解。

2.2.2 底層多飛行器協同規劃器

底層多飛行器協同規劃器在單飛行器規劃的基礎上進行精細化協同規劃。底層多飛行器協同規劃器除了考慮飛行器的動力學方程式（1）、狀態量和控制量約束式（3）～（6）、威脅約束式（7）外，重點考慮多飛行器間編隊約束式（8）～（9）。由于底層多飛行器協同規劃器將所有飛行器進行集中式求解，求解變量較多，求解規模較大，因此底層多飛行器協同規劃器對于初值選取要求更高。通過頂層單飛行器快速規劃器得到的求解結果可以為底層多飛行器協同規劃器提供一個較好的初值參考，提升NLP的求解效率。

設由單飛行器快速規劃器求解分別得到的各飛行器狀態量和控制量分別為Xsi，Usi（i=1，2，…，N）。在多飛行器協同規劃器增加配點數量以增加軌跡規劃的精度，解決單飛行器快速規劃器出現的精度差等問題。若多飛行器規劃器LGL 配點數為n2，根據單飛行器規劃器的求解結果分別對各飛行器狀態量和控制量進行Lagrange插值：

將插值后的結果xmi，umi（i=1，2，…，N）作為多飛行器協同規劃器的初值進行偽譜法求解，可以更好地解決偽譜法對初值敏感的問題，提高多飛行器協同軌跡規劃的效率。在此基礎上，結合飛行器之間的編隊約束，確保實現共同攻擊任務。

2.2.3 算法流程

雙層亞音速飛行器協同軌跡規劃器算法步驟如下：

①根據單飛行器軌跡規劃問題約束式（1）～（7）、目標函數式（10）將問題建模為式（11）～（14）的公式化表述。

②設定頂層單飛行器快速規劃器LGL 配點數n1，設定求解迭代次數I1。

③根據飛行器初始時刻狀態值X（t0，）i和終端時刻狀態值X（tf，）i采用線性插值作為頂層單飛行器快速規劃器求解的初值。

④根據式（16）離散每一個飛行器的狀態變量和控制變量，通過LPM 將問題轉化為式（19）、式（23）～（25）的NLP問題，使用SQP方法求解。

⑤根據多飛行器軌跡規劃問題約束式（1）～（9）、目標函數式（10）將問題建模為式（11）～（14）的公式化表述。

⑥設定底層多飛行器協同規劃器LGL 配點數n2，設定求解迭代次數I2。

⑦將頂層單飛行器快速規劃器求解結果Xsi，Usi根據式（26）進行n2個LGL點的Lagrange插值得到插值結果xmi，umi。

⑧將插值結果xmi，umi作為底層多飛行器協同規劃器的初值，通過LPM 將問題轉化為式（19）、式（23）～（25）的NLP 問題，使用SQP 方法求解得到最終軌跡結果。

3 仿真結果與分析

針對提出的雙層亞音速飛行器協同軌跡規劃器，進行單飛行器軌跡規劃實驗和多飛行器協同軌跡規劃實驗。

表1為仿真實驗亞音速飛行器所需參數［21］。仿真場景設定參數如表2所示。

表1 亞音速飛行器參數Table 1 Parameters of subsonic vehicles

表2 實驗場景參數Table 2 Parameters of experimental scene

3.1 單飛行器軌跡規劃實驗

為了驗證控制量變化率約束改進的有效性和可行性，使用表2 中飛行器1 參數進行單亞音速飛行器軌跡規劃仿真實驗。對于控制量變化率約束的處理方式對比文獻［17］采用的有限差分法和文獻［16］采用的增加控制量變化率方程方法。所有方法配點數設置為21，迭代次數設置為200。

各方法得到飛行器的三維軌跡曲線如圖2 所示，控制量變化曲線如圖3 所示。文獻［16］增加控制量約束方程可以實現對于控制量變化率約束的要求，控制量的變化率得到約束。但是增加新的約束方程帶來了復雜度的增加，因此增加了計算的困難度。以亞音速飛行器約束方程為例，共6 個變量和2個控制變量，增加控制量變化率方程后變為8個狀態變量與2 個控制變量，理論上會增加25%的復雜度與計算量。對于偽譜法實際計算，所有的變量都需要SQP 算法迭代計算求解，變量的震蕩會導致NLP收斂變慢，由此會造成計算更加耗時，降低算法的可行性。文獻［17］引入的有限差分法與本文方法不需要引入虛擬控制變量，避免了計算復雜度的增加，減小計算耗時。三種方法各自重復10次得到算法耗時平均值，文獻［16］方法為5.890 2 s，文獻［17］方法為4.386 1 s，本文方法為3.716 1 s。

圖2 三維軌跡結果Fig.2 3D trajectory results

圖3 控制量變化曲線對比Fig.3 Comparison of control variable change curves

根據實驗結果可知，增加控制量變化率方程的方法明顯增加了計算耗時。而本文方法相較于文獻［17］的方法進一步提升了算法效率。對于有限差分法受限于區間分辨率，由于LGL 配點分布并非等間距分布，因此有限差分法與偽譜法結合存在一定的局限性，造成了一定的優化困難。本文算法通過Lagrange多項式的計算得到控制量變化率作為約束條件，契合偽譜法理論推導，對控制量變化率約束的同時保證了算法效率。由實驗結果可知，本文方法較文獻［17］的有限差分法計算得到的控制量變化曲線更加平滑，同時有更優的計算速度。

3.2 多飛行器編隊協同軌跡規劃仿真實驗

在單飛行器情況下約束較少，優化變量較少，改進的控制量變化率約束方法和無控制量變化率約束的傳統偽譜法優化效率大體相當。通過多飛行器協同軌跡規劃仿真實驗進一步體現本文雙層飛行器協同軌跡規劃器在求解性能上的提升。多飛行器仿真實驗飛行器參數如表1 所示，實驗場景環境設定與各飛行器初始狀態如表2所示。文獻［16］增加控制量變化率方程方法配點數設置為19，迭代次數設置為200；本文所提雙層規劃器頂層規劃器配點數設置為9，迭代次數20次，底層規劃器配點數為19，迭代次數200次。

求解結果如圖4 和圖5 所示。實驗結果表明，兩種方法均可獲得規劃結果。在控制量限制閾值為50 的條件下，增加控制量變化率方程后，控制量的變化曲線表明橫向加速度在10 s左右時控制量明顯超過閾值。這是由于拉格朗日插值法在兩端點較稠密，而在中間段較稀疏，插值誤差會導致控制量超過閾值。另外，增加控制量變化率方程會增加計算的復雜度，并且缺乏良好的初始值的情況下，導致優化困難。本文方法中頂層規劃器可以提供底層規劃器所需的優化初始值，從而更好地控制優化過程。其次，控制量變化率的約束可以進一步限制控制量的震蕩，避免控制量的過度波動。

圖4 三維軌跡結果Fig.4 3D trajectory results

圖5 控制量變化曲線對比Fig.5 Comparison of control variable change curves

算法運行時間統計如表3 所示?？梢钥闯?，增加控制量變化率方程方法的消耗時間是本文方法的8.61 倍。其主要原因是多飛行器情況下約束條件更復雜，待優化變量顯著增加，雙層規劃器結構結合對控制量變化率約束的改進，避免增加新的約束方程和新的待優化變量，使算法更加高效。頂層規劃器得到的結果盡管精度較低，可行性較差，但是其求解迅速，再通過底層規劃器的進一步優化求解，保證了求解的精度，同時提高了規劃的效率。因此，本文方法可以有效地解決軌跡規劃問題中的初值敏感和高維度優化問題，并且具有更好的求解效率和精度。

表3 算法耗時Table 3 Algorithm time consumption

4 結論

本文提出了一種雙層亞音速飛行器協同軌跡規劃求解器，解決了多飛行器編隊軌跡規劃問題中的約束復雜和求解困難的問題。該方法首先在傳統偽譜法的基礎上，通過基于Lagrange 插值多項式的方法計算控制量變化率，從而避免了引入虛擬控制變量和增加狀態方程所帶來的計算復雜度增加問題。然后，采用雙層規劃器，有效解決了偽譜法初值敏感問題，提高了算法的求解效率。通過仿真實驗的結果表明，相比于增加控制量變化率方程的方法，本文提出的雙層亞音速飛行器協同軌跡規劃求解器在多飛行器編隊飛行的軌跡規劃問題中具有更高的優化效率。