基于模型預測靜態(tài)二階錐規(guī)劃的協同末制導

權申明，左 坤，晁 濤，楊 明

（哈爾濱工業(yè)大學 控制與仿真中心，哈爾濱 150080）

單個飛行器在對目標探測與機動能力上均存在一定的局限性，傳統(tǒng)單飛行器對目標制導效果有限。相比之下，多飛行器協同探測目標存在更大的優(yōu)勢，多飛行器可同時進行探測識別與信息交流，顯著提高對真實目標的識別能力。多飛行器協同還能降低飛行器的彈載計算和探測設備的要求，從而降低成本。

在協同末制導階段，相關學者針對不同約束條件進行制導律設計。趙久奮等人[1]針對推力可控的導彈，在視線和視線法線方向上，結合超螺旋算法和有限時間滑模設計分布式協同制導律，并針對機動目標進行仿真驗證。肖惟等人[2]研究了考慮過載約束的多枚弱機動能力導彈強目標協同問題，結合飛行器可達域、可行域以及目標逃逸域，設計了協同攔截策略。司玉潔等人[3]研究了考慮通訊拓撲條件下的協同末制導律，并基于有限時間理論設計快收斂的滑模制導律。此外，強化學習等方法也用于協同末制導律設計中，并取得較好的制導效果。

模型預測靜態(tài)規(guī)劃（Model Predictive Static Programming,MPSP）自Padhi 等人[4]提出以來，在飛行器制導控制[5-10]方面有較多的應用，該方法在計算效率和制導精度上具有一定優(yōu)勢。MPSP 根據靈敏度矩陣不斷更新控制量，能夠滿足終端約束[11]，后續(xù)研究中，輸入與狀態(tài)約束逐漸引起關注[12,13]。文獻[14]考慮中制導過程中，零控攔截和交班視窗角約束，推導了時間固定的廣義模型預測靜態(tài)規(guī)劃算法。文獻[15]基于優(yōu)化模型預測靜態(tài)規(guī)劃算法，針對吸氣式高超聲速飛行器設計了滿足終端碰撞角約束的制導律。文獻[16]提出了一種初、中制導聯合規(guī)劃制導方法，用于解決多階段目標攔截問題。在MPSP 的基礎上，廣義MPSP 算法被提出[17,18]，該方法無需對動力學模型離散化，提高算法求解速度與精度。

MPSP 算法同模型預測控制有相似之處，離散步長通常為定值，因此難以處理終端時間自由的問題。然而，在協同末制導問題中，終端時間難以事先獲得；同時，初始軌跡的選取影響了優(yōu)化結果與優(yōu)化效率。

針對現有序列二階錐規(guī)劃（Second-Order Cone Programming,SOCP）求解協同制導問題速度較慢難以滿足實時性要求的問題，本文結合MPSP 與SOCP，提出一種基于模型預測靜態(tài)二階錐規(guī)劃的協同末制導算法。首先，建立考慮過載約束的飛行器協同末制導模型；然后，在MPSP 算法基礎上，提出終端時間自由的MPSP 算法；進一步將有狀態(tài)約束的MPSP 問題轉化為模型預測靜態(tài)二階錐規(guī)劃問題，可以有效處理過程約束。仿真結果表明，所提出的模型預測靜態(tài)二階錐規(guī)劃能兼?zhèn)銶PSP 算法的快速性與SOCP 的處理約束與最優(yōu)性特點，通過減少求解變量個數，提升求解速度，易于實現在線快速計算。通過不同仿真場景、不同優(yōu)化方法的仿真與分析，驗證了本文所提算法的計算效率與精度。

1 問題描述

建立發(fā)射系下飛行器運動模型為：

其中，i為每個飛行器的編號，xi、yi、zi、Vi分別表示第i個飛行器的在空間坐標系下的位置分量、速度大小，θi、σi、γi分別是第i個飛行器的彈道傾角、彈道偏角、傾側角。Di、Li分別是第i個飛行器在飛行過程中受到的阻力與升力，速度系下氣動力分量為：

其中ρ=ρ0e-y/hs是大氣密度，ρ0為標準大氣密度，hs為標準高度。Sref為飛行器的最大橫截面積。CL和CD分別是升力與阻力系數，由飛行攻角α與飛行馬赫數M計算得到，馬赫數的數值大小由飛行器的飛行速度進行計算。

在飛行器協同飛行過程中，需滿足初始狀態(tài)約束、終端狀態(tài)約束以及過載約束。其中，過載約束為：

2 算法設計

本節(jié)首先進行終端時間固定、無過程約束MPSP算法介紹。在此基礎上，提出考慮過載約束的終端時間自由協同打擊末制導算法。

2.1 終端時間固定的MPSP 算法

記離散時間的非線性動力學系統(tǒng)為：

其中，k=1,2,......N-1為一系列的離散點，離散步長為tN為設定的終端時間，x∈Rn是系統(tǒng)的狀態(tài)變量，u∈Rm是系統(tǒng)的控制變量，F關于x與u連續(xù)可導，與飛行器的動力學方程有關，MPSP的目的是找到合適的du，對于先前的輸入up進行校正（上角標p表示上一次迭代結果），得到新的控制量u=up+du，狀態(tài)變量隨之得到更新。即：x=xp+dx。其中du、dx分別是控制變量與狀態(tài)變量的增量。

假設狀態(tài)變量的變化dx與控制變量的變化du較小，那么在更新k+1時刻的狀態(tài)時，可以對離散的狀態(tài)方程進行泰勒展開，考慮一階展開形式：

同理可以得到：

將式(7)代入式(6)，得：

不斷迭代可得：

其中，

其中j=1,2…k-1。

當j=k時，有：

初始狀態(tài)通常為給定值，那么dx1=0，則式(9)可寫為：

式(13)表明，第k+1個時間點的狀態(tài)是由前k個控制量進行校正的，為簡化B陣的計算，引入中間變量：

至此B計算完成，稱B為靈敏度矩陣。

2.2 基于二階錐規(guī)劃終端時間自由的MPSP

考慮終端時間自由的離散時間的動力學系統(tǒng)方程：

其中，k表示第k個離散點，系統(tǒng)的非線性動力學方程為f(xk,uk)，本文離散點總數N保持不變，終端時刻變化后，每一個離散點表示的時間將不斷改變，離散時間間隔tk為：

將模型(17)在前一次結果(xkp,ukp,tkp)的基礎上進行線性化，可以得到：

其中，終端時間的變化量為dtN=tN-tNp，每個離散時間間隔的變化量為表示前一次迭代得到的終端時間。

滿足上述式子的du很多，難以獲得解析解，因此使用優(yōu)化的思想來進行求解。

構造目標函數為：

其中，L為過程中的優(yōu)化目標，φ為終端時刻的優(yōu)化目標。本文不僅考慮終端約束的影響，還要考慮過程約束。過程約束與控制約束線性化后可以寫成如下形式：

其中，Lk、Ck與具體的約束有關，已知代入式(22)可以得到：

由上述不等式可知，上述約束為一個凸約束，可以使用二階錐規(guī)劃方法解決。

綜上，初始狀態(tài)為x1*，期望終端狀態(tài)為xf*，可以將求控制變量du與終端時間tN的問題轉化為一個模型預測靜態(tài)二階錐規(guī)劃（MPSP&SOCP）問題：

由于在MPSP&SOCP 中，優(yōu)化變量只有終端時間和輸入量的偏差，因此無法直接計算迭代過程中的過載大小，采用一階泰勒展開近似線性化后，通過逐級求導可得到攻角對速度、高度的影響，進而通過序列迭代求出過載值，具體如式(25)：

設計MPSP&SOCP 算法求解流程如圖1所示。

圖1 MPSP&SOCP 算法流程圖Fig.1 MPSP&SOCP algorithm flow chart

MPSP&SOCP 算法需要設置初始軌跡，傳統(tǒng)MPSP 求解末制導問題時，往往通過比例導引進行初始軌跡的計算。本文中，為了降低算法對初值的依賴性，提高算法的適應性，采用常值控制量，開環(huán)積分后得到初值。在求解帶有過程約束最優(yōu)問題時，第一次迭代時，僅加入終端位置約束，在求得結果基礎上，增加終端角度約束。通過將約束條件逐步加入，可以有效提升求解速度與算法穩(wěn)定性。計算靈敏度矩陣后，將過程約束轉化為SOCP 問題，僅需求解控制量的更新值即可，無需進行狀態(tài)量的優(yōu)化，大大減少了求解時間。

3 數值仿真及分析

為了驗證本文所提算法的有效性與適應性，本節(jié)分別進行無過程約束MPSP 末制導仿真、考慮側窗約束MPSP 末制導仿真、對比算法仿真以及適應性仿真實驗。其中，選取文獻[19]中SOCP 算法作為對比算法。

飛行器的升力系數與阻力系數是關于攻角和馬赫數的多項式，如式(26)所示：

協同飛行器數量為3（M1、M2、M3），飛行器質量為907.8 kg，面積為0.355 m2，最大過載約束為1.5g，仿真中初始條件如表1所示。

表1 仿真初始條件Tab.1 Initial conditions of simulation

3.1 二階錐規(guī)劃方法仿真結果

在相同仿真條件下，采用二階錐規(guī)劃方法進行對比仿真，仿真結果如圖2-7所示（圖中Initial 表示第一次求解得到的軌跡，Opt 表示最終優(yōu)化軌跡）。

圖4 速度傾角變化曲線Fig.4 Flight path angles

圖5 攻角變化曲線Fig.5 Angles of attack

圖 傾 化曲Fig.6 Curves of bank angles

由仿真結果可以看出，基于二階錐規(guī)劃方法的協同末制導可以實現對目標的精確打擊。該算法首先加入終端位置約束，因此圖7中第一次求解結果中峰值過載接近2.4g，然后考慮終端速度傾角約束，在上述結果基礎上，考慮過載約束能夠實現收斂，同時過程約束滿足要求。

圖7 過載變化曲線Fig.7 Curves of overload

3.2 MPSP&SOCP 算法仿真結果

基于MPSP&SOCP 算法仿真結果如圖8-14所示（圖中Guess 表示初始猜測軌跡，Opt 表示計算得到的最優(yōu)軌跡）。

圖8 三維軌跡Fig.8 Curves of 3D trajectories

圖9 速度變化曲線Fig.9 Curves of speed

圖10 速度傾角變化曲線Fig.10 Flight path angles

圖11 攻角變化曲線Fig.11 Angles of attack

圖12 傾側角變化曲線Fig.12 Curves of bank angles

圖13 過載變化曲線Fig.13 Curves of overload

圖14 相對距離變化曲線Fig.14 Curves of relative distances

由圖8可以看出，3 個飛行器由猜測控制量開環(huán)積分得到的軌跡不滿足終端位置約束，飛行器1 和飛行器3 相距目標點較遠，3 個飛行器終端高度為負，圖11和圖12中開環(huán)控制量相同，因此各飛行器初始軌跡相似。在第一次迭代中，僅加入終端位置約束，從第二次迭代開始，加入終端彈道傾角約束如圖10。由于末制導階段飛行器高度下降較快，阻力減速效果沒有重力勢能轉化為動能的效果明顯，因此圖9中速度略有增加。經過MPSP&SOCP 迭代計算后，能夠實現對目標的精確打擊。

本仿真算例中，過載約束為1.5g，初始軌跡中由于控制量變化率為常值，因此過載較小，最大過載為1.1g，在優(yōu)化中考慮過載約束，求得圖13中最優(yōu)軌跡滿足過載約束。

3.3 對比分析

將兩種方法得到的最優(yōu)軌跡進行對比，如圖15所示?？梢钥闯龌赟OCP 與MPSP&SOCP 的求解三維軌跡相吻合。

圖15 不同方法求解結果Fig.15 Results of different methods

對終端位置約束進行小范圍改變，變化范圍為原點附近水平面內1000 m 的半徑圓區(qū)域，進行50 次蒙特卡洛仿真。統(tǒng)計終端時刻脫靶量，如圖16所示?？梢钥闯鰞煞N方法求解精度均在2 m 以內。

圖16 求解精度對比Fig.16 Solving accuracy comparison

將上述MPSP&SOCP 算法與二階錐規(guī)劃的求解效率進行對比仿真分析，結果如圖17所示，可以看出在同等迭代次數、求解精度情況下，MPSP&SOCP算法能夠節(jié)省40%的時間，有望實現在線實時規(guī)劃求解。

圖17 求解效率對比Fig.17 Solving efficiency comparison

綜上，基于MPSP&SOCP 算法的協同末制導可以在保障求解精度的前提下，耗費更短的時間，易于實現在線計算，工程前景廣闊。

4 結 論

本文針對現有序列二階錐規(guī)劃求解協同打擊問題速度較慢難以滿足實時性要求的問題，結合MPSP 與SOCP，提出一種基于模型預測靜態(tài)二階錐規(guī)劃的協同末制導算法。主要結論如下：

(1) 在MPSP 算法基礎上，提出終端時間自由的MPSP 算法，能夠解決傳統(tǒng)MPSP 問題終端時間固定的缺陷，求解時間最優(yōu)問題。

(2) 為了解決傳統(tǒng)MPSP 算法過于依賴初始軌跡的局限性，提出逐步增加約束條件的迭代求解流程，從而使得小偏差線性化更具合理性，算法穩(wěn)定性更強。

(3) 所提出的模型預測靜態(tài)二階錐規(guī)劃能兼?zhèn)銶PSP算法的快速性與SOCP 的處理約束與最優(yōu)性特點，通過減少求解變量個數，提升求解速度，易于實現在線快速計算。

本文目前工作從軌跡優(yōu)化的角度進行，后續(xù)將開展在線實時優(yōu)化-跟蹤的閉環(huán)制導算法研究，同時考慮更多的約束條件，以增加本文算法的適用性。