摘" 要：

針對長航程飛行器打擊低速艦船目標(biāo)的問題，提出一種基于滾動序列凸優(yōu)化算法的中段航跡快速規(guī)劃算法。使用滾動規(guī)劃框架將航跡更新問題轉(zhuǎn)化為初始與終端位置更新后的單次航跡規(guī)劃子問題，構(gòu)造多約束凸優(yōu)化問題并使用序列凸優(yōu)化算法求解。提出滾動規(guī)劃周期收縮策略，減少航跡更新次數(shù)。設(shè)計了一種改進參考航跡生成算法并使用了信賴域自適應(yīng)收縮策略，在保證規(guī)劃效果的同時有效提高計算速度。仿真結(jié)果表明，該算法規(guī)劃效果優(yōu)良，可滿足在線規(guī)劃的要求，且與一般的滾動序列凸優(yōu)化航跡規(guī)劃算法相比速度更快。

關(guān)鍵詞：

飛行器; 航跡規(guī)劃; 凸優(yōu)化; 滾動規(guī)劃框架; 移動目標(biāo)

中圖分類號：

V 249.1

文獻標(biāo)志碼： A""" DOI：10.12305/j.issn.1001-506X.2024.07.24

Flight vehicle midcourse trajectory fast planning for low-speed target

XIONG Zichun， LIU Yongshan*

（School of Aerospace Engineering， Beijing Institute of Technology， Beijing 100081， China）

Abstract：

A midcourse flight trajectory planning algorithm based on receding sequential convex programming algorithm is proposed for a long-range flight vehicle to strike low-speed ship target. Using a receding planning framework， the trajectory update problem is transformed into a trajectory planning subproblem after initial and terminal position updates. Then， a multi-constraint convex optimization problem is constructed and solved using sequential convex programming algorithm. A receding planning cycle contraction strategy is designed to reduce the number of trajectory updates. An improved reference trajectory generation algorithm is designed and a trust region adaptive shrinkage strategy is used to effectively improve the computational speed while ensuring the planning effect. Simulation results show that the algorithm has fine planning results and can meet the online planning requirements. The computational speed is faster compared with usual receding sequential convex programming trajectory planning algorithms.

Keywords：

flight vehicle; trajectory planning; convex optimization; receding planning framework; moving target

0" 引" 言

諸如巡航導(dǎo)彈等的長航程飛行器有較大的飛行范圍和較強的機動性，是攻擊遠(yuǎn)距離艦船目標(biāo)的有利武器。但是艦載防空反導(dǎo)系統(tǒng)不斷更新迭代，區(qū)域拒止能力與反介入能力的提升使飛行器的生存能力與打擊效果下降。隨著衛(wèi)星等偵查手段以及數(shù)據(jù)鏈技術(shù)的迅速發(fā)展，飛行器可以在更遠(yuǎn)的距離上通過數(shù)據(jù)鏈等通信手段利用外部信息定位目標(biāo)艦艇并進行防區(qū)外打擊。航跡規(guī)劃算法作為飛行器制導(dǎo)的關(guān)鍵技術(shù)之一，是飛行器跟蹤航跡的前提條件。為了實現(xiàn)打擊遠(yuǎn)距離移動艦船目標(biāo)的要求，有必要研究一種可以滿足多種約束且能在線更新航跡的航跡規(guī)劃算法。

長航程飛行器打擊移動艦船目標(biāo)這一場景可歸入目標(biāo)攔截范疇。可以實現(xiàn)移動目標(biāo)攔截的航跡規(guī)劃算法形式多樣：Drake等［1］基于經(jīng)典的Dijkstra算法，通過重新搜索擴展軌跡的方式實現(xiàn)對移動目標(biāo)的攔截;Phung等［2］提出一種運動編碼粒子群優(yōu)化方法實時生成航跡，使無人機攔截并跟蹤地面目標(biāo);Triharminto等［3］提出一種將直線與改進Dubins曲線拼合的航跡生成算法，實現(xiàn)了無人機在三維有障礙空間內(nèi)對移動目標(biāo)的攔截;Meyer等［4］針對移動目標(biāo)利用Dubins曲線設(shè)計了時間最優(yōu)的航跡規(guī)劃算法，并在此基礎(chǔ)上進行改進使其可以滿足給定時間約束;文獻［5］和文獻［6］使用龐特里亞金最大（最小）值原理求解最優(yōu)控制問題為飛行器生成目標(biāo)攔截航跡;Liu等［7］將“比例微分”控制器與強化學(xué)習(xí)中的近端策略優(yōu)化算法結(jié)合，設(shè)計了目標(biāo)運動預(yù)測攔截航跡生成算法。這些算法主要針對無人機在小范圍內(nèi)運動的場景設(shè)計，存在求解問題規(guī)模較小，并且難以直接擴展到飛行器需要進行大范圍機動的情形下等問題。另外，上述部分算法未考慮環(huán)境中的威脅或障礙物，對復(fù)雜環(huán)境適應(yīng)性不佳。

基于凸優(yōu)化的航跡規(guī)劃算法根據(jù)過載或加速度等控制量建立微分運動方程，將航跡規(guī)劃轉(zhuǎn)化為最優(yōu)控制問題求解［8］。相比于圖/樹搜索算法［9］、群智能算法［1012］、曲線生成算法［13］等基于飛行器代數(shù)運動方程的航跡規(guī)劃方法，凸優(yōu)化方法可以求得多約束下的飛行器全狀態(tài)航跡。與其他可以求解最優(yōu)控制問題的算法如偽譜法［14］、強化學(xué)習(xí)算法［1516］相比，凸優(yōu)化方法在求解大規(guī)模問題時速度更快［8，17］。鑒于凸優(yōu)化方法的眾多優(yōu)點，目前已經(jīng)在航跡規(guī)劃領(lǐng)域得到廣泛應(yīng)用。Liu等［18］將飛行器高速再入的多約束航跡規(guī)劃問題轉(zhuǎn)化為凸優(yōu)化中的二階錐優(yōu)化（second-order cone programming， SOCP）問題進行求解，之后又提出一種保證收斂的迭代求解SOCP的方法［19］用于解決無人機的航跡規(guī)劃問題;Zhang等［20］基于混合整數(shù)二階錐優(yōu)化的方法設(shè)計無人機航跡規(guī)劃算法。近幾年來，序列凸優(yōu)化（sequential convex programming， SCP）算法在航跡規(guī)劃領(lǐng)域大放異彩，是一種可以求解非線性優(yōu)化問題的算法且具有線性收斂速度［2123］。2012年，Augugliaro等［24］首次將SCP算法應(yīng)用于無人機的航跡規(guī)劃中;Jiang等［25］基于比例導(dǎo)引法，使用SCP算法調(diào)整比例導(dǎo)引系數(shù)，通過改變飛行器的航跡曲率調(diào)整飛行時間實現(xiàn)協(xié)同制導(dǎo); Morgan等［26］參考模型預(yù)測控制（model predictive control， MPC）的思路，基于SCP算法設(shè)計一種航天器集群編隊變換方案MPC-SCP，將大規(guī)模的航跡規(guī)劃問題分割為多個較短時域的子問題;之后Morgan等［27］又基于滾動SCP框架設(shè)計一種針對大規(guī)模無人機集群的分布式隊形變換算法;徐廣通等［28］優(yōu)化文獻［27］中的方法，利用滾動規(guī)劃框架求解無人機的協(xié)同航跡規(guī)劃問題，之后基于此在通信距離受限的條件下設(shè)計了無人機集群的編隊方法［29］。

需要注意的是，上述凸優(yōu)化航跡規(guī)劃算法中飛行器的終端位置是不變的，而針對移動目標(biāo)的航跡規(guī)劃算法必然涉及航跡更新問題?，F(xiàn)有研究對航跡更新策略的討論較少。對于小規(guī)模問題，可以如文獻［17］在短時間內(nèi)多次更新航跡。但是對于待規(guī)劃航跡長度更長、環(huán)境更復(fù)雜的大規(guī)模問題，航跡規(guī)劃算法尤其是凸優(yōu)化方法等最優(yōu)化算法的單次規(guī)劃耗時將顯著增加，使用現(xiàn)有的航跡更新策略不易實現(xiàn)在線航跡規(guī)劃。此外，由于目標(biāo)移動速度相對較慢，短時間內(nèi)多次規(guī)劃得到的航跡將高度雷同，浪費了寶貴的計算資源。最后，在復(fù)雜現(xiàn)實條件下難以通過外部渠道高頻率更新目標(biāo)信息。

為解決現(xiàn)有航跡規(guī)劃算法在打擊遠(yuǎn)距離移動艦船目標(biāo)這一場景下存在適應(yīng)性差、計算量大、規(guī)劃速度慢的問題，本文提出一種針對低速艦船目標(biāo)的飛行器中段航跡快速規(guī)劃方法。該方法將飛行器在二維平面的航跡規(guī)劃問題轉(zhuǎn)化為凸優(yōu)化問題，使用SCP算法根據(jù)參考航跡迭代求解得到規(guī)劃航跡，然后使用滾動規(guī)劃框架對規(guī)劃航跡進行更新。本文的主要貢獻為：提出一種適用于不同場景下的基于剩余飛行時間的滾動規(guī)劃周期收縮策略，減小了航跡更新的計算量;設(shè)計了改進參考航跡生成算法并采用信賴域自適應(yīng)收縮策略，提高了單次規(guī)劃速度。最后，設(shè)計了任務(wù)場景并進行仿真實驗驗證了算法的有效性。

1" 問題描述與分析

1.1" 航跡規(guī)劃問題描述

巡航導(dǎo)彈在飛行中根據(jù)外部信息確定目標(biāo)與威脅區(qū)域的位置，根據(jù)自身測量設(shè)備確定自身位置及其在未來時刻的估計。航跡規(guī)劃算法基于上述位置信息求得滿足初始與終端狀態(tài)、邊界、運動學(xué)、威脅規(guī)避等約束的可飛航跡。

針對移動目標(biāo)的航跡規(guī)劃算法可以分為目標(biāo)跟蹤與目標(biāo)攔截兩種，長航程飛行器打擊移動艦船目標(biāo)這一場景屬于后者。由于目標(biāo)移動導(dǎo)致終端位置改變，使用滾動規(guī)劃框架對目標(biāo)位置與待飛航跡進行更新。滾動規(guī)劃框架將航跡更新問題轉(zhuǎn)化為初始與終端位置更新后的單次航跡規(guī)劃子問題，其流程如下：飛行器在根據(jù)第h步滾動規(guī)劃所得航跡飛行時進行第h+1步滾動規(guī)劃以更新航跡，之后算法不斷向前滾動求解，直到完成最后一步滾動規(guī)劃。假設(shè)飛行器可以精確跟蹤規(guī)劃航跡，那么第h+1步滾動規(guī)劃航跡的起點即為第h步滾動規(guī)劃航跡上的某點，終點為在開始第h+1步滾動規(guī)劃時的目標(biāo)位置。

航跡跟蹤控制器將規(guī)劃航跡與飛行器實際狀態(tài)反饋量的偏差作為輸入，根據(jù)控制律得到以油門開度和舵偏角為輸出的控制量并作用于飛行器的動力學(xué)與運動學(xué)模型上，完成對規(guī)劃航跡的跟蹤任務(wù)。飛行器航跡規(guī)劃與跟蹤控制系統(tǒng)原理如圖1所示。下面將單次航跡規(guī)劃問題轉(zhuǎn)化為凸優(yōu)化問題以使用SCP算法求解待飛航跡。

1.2" 飛行器運動學(xué)模型

巡航導(dǎo)彈在打擊艦船目標(biāo)時通常掠海飛行，故只考慮其在水平面內(nèi)的航跡［30］。飛行器在地面坐標(biāo)系［17］下的二維平面質(zhì)心運動方程為

x·h

y·h

·h

V·h=Vhcos h

Vhsin h

ahy/Vh

ahx（1）

式中：ph=［xh，yh］T為飛行器的位置;h為航向角，定義為以坐標(biāo)系x軸正方向為0 rad，逆時針方向為增大的方向;Vh為速度;ahx為切向加速度;ahy為側(cè)向加速度;h為滾動規(guī)劃的編號（h=1，2，…，H，H為滾動規(guī)劃的總次數(shù)）。將飛行器狀態(tài)量表示為sh=［xh，yh，h，Vh］T，控制量表示為uh=［ahy，ahx］T，那么式（1）可以表示為s·h=f（sh，uh）。

1.3" 約束條件

飛行器初次滾動規(guī)劃的初始狀態(tài)由人為給定，之后各次滾動規(guī)劃的初始狀態(tài)均為上一次滾動規(guī)劃結(jié)果中某時刻的狀態(tài)：

sh（th0）=sh0

sh（thf）=shf（2）

式中：th0為飛行器第h次規(guī)劃所得航跡的起始時刻;thf為飛行器第h次規(guī)劃所得航跡的結(jié)束時刻，也即到達目標(biāo)的時刻。規(guī)劃航跡的狀態(tài)量與控制量不應(yīng)超出飛行器的飛行包線，對這兩者分別設(shè)置簡化的幅值約束：

smin≤sh（t）≤smax

umin≤uh（t）≤umax（3）

式中：t為飛行時刻。

由于可能存在敵方防空火力等外部威脅，考慮在規(guī)劃空間內(nèi)設(shè)置禁飛區(qū)約束。此處，將禁飛區(qū)簡化為靜態(tài)的無窮高圓柱形障礙，則禁飛區(qū)約束可表示為

ph（t）－pnfz，m2≥rm， m=1，2，…，M（4）

式中：pnfz，m為第m個禁飛區(qū)的位置;rm為其半徑;M為禁飛區(qū)的個數(shù)。

1.4" 問題的離散化與凸化

1.4.1" 始末約束與邊界約束

假設(shè)離散的時間步長為Δt，那么飛行器在一次滾動規(guī)劃中的飛行時長可以分為Kh=（thf－th0）/Δt個離散區(qū)間，飛行器狀態(tài)量的初始和終端約束可以表示為

sh［0］=sh0

sh［Kh］=shf（5）

飛行器狀態(tài)量和控制量的邊界約束可以表示為

smin≤sh［k］≤smax

umin≤uh［k］≤umax（6）

式中：k=0，1，…，Kh。

1.4.2" 飛行器運動學(xué)約束

使用梯形積分法對飛行器的運動學(xué)方程進行離散化，可以得到下面的非線性約束：

sh［k+1］=sh［k］+0.5Δt·（f（sh［k］，uh［k］）+

f（sh［k+1］，uh［k+1］））， k=0，1，…，Kh－1（7）

利用凸優(yōu)化中的松弛方法，可以將上面的式（7）約束轉(zhuǎn)化為不等式約束：

|sh［k+1］－sh［k］－0.5Δt·（f（sh［k］，uh［k］）+

f（sh［k+1］，uh［k+1］））|≤ζ， k=0，1，…，Kh－1（8）

為了保證飛行器的兩個相鄰狀態(tài)之間基本滿足離散后的運動學(xué)方程，松弛誤差ζ需要設(shè)定為一個較小的值。由于飛行器的運動學(xué)約束是非線性的，還需要根據(jù)小擾動線性化假設(shè)將其在參考航跡附近線性化以滿足凸優(yōu)化問題的要求［31］。在SCP算法中，初始參考航跡由人為給定，為第一次迭代提供初值。在之后的迭代中，參考航跡是上一次迭代求得的航跡。

可以將式（8）轉(zhuǎn)化為下面的形式：

|Ahk+1sh［k+1］+Ahksh［k］+Bhk+1uh［k+1］+

Bhkuh［k］+

（Chk+Chk+1）Δt+Dhk+Dhk+1|≤ζ，

k=0，1，…，Kh－1（9）

式中：Ahk，Ahk+1，Bhk，Bhk+1，Chk，Chk+1，Dhk，Dhk+1均為系數(shù)矩陣，其值由參考航跡的狀態(tài)量s-h［k］和控制量u-h［k］（k=0，1，…，Kh）計算得出而與待求航跡無關(guān)，因此在求解凸優(yōu)化問題時它們可以被視為常數(shù)。下面給出計算公式：

Ahk=－Δt-2f（sh，uh）shs-h［k］，u-h［k］－I

Ahk+1=－Δt-2f（sh，uh）shs-h［k+1］，u-h［k+1］+I

Bk=－Δt-2f（sh，uh）uhs-h［k］，u-h［k］

Bhk+1=－Δt-2f（sh，uh）uhs-h［k+1］，u-h［k+1］

Chk=－f（s-h［k］，u-h［k］）2

Chk+1=－f（s-h［k+1］，u-h［k+1］）2

Dhk=Δt-2f（sh，uh）shs-h［k］，u-h［k］·s-h［k］+

f（sh，uh）uhs-h［k］，u-h［k］·u-h［k］

Dk+1=Δt-2f（sh，uh）shs-h［k+1］，u-h［k+1］·s-h［k+1］+

f（sh，uh）uhs-h［k+1］，u-h［k+1］·u-h［k+1］

（10）

式中：Δt-為參考時間步長;I為單位矩陣；f（sh，uh）/sh與f（sh，uh）/uh可以由下式計算得出：

f（sh，uh）sh=00－Vhsin hcos h

00Vhcos hsin h

000－ahy/（Vh）2

0000

f（sh，uh）uh=00

00

1/Vh0

01

（11）

1.4.3" 禁飛區(qū)約束

對禁飛區(qū)約束進行離散化之后可得

ph［k］－pnfz，m2≥rm， k=0，1，…，Kh（12）

參考文獻［32］中的方法，將其凸化為

p-h［k］－pnfz，m2+

（p-h［k］－pnfz，m）Tp-h［k］－pnfz，m2（ph［k］－p-h［k］）≥rm，

k=0，1，…，Kh; m=1，2，…，M（13）

以及

p-h［k－1］－pnfz，m2+

（p-h［k－1］－pnfz，m）Tp-h［k－1］－pnfz，m2（ph［k］－p-h［k－1］）≥rm，

k=1，2，…，Kh; m=1，2，…，M（14）

式中：p-h［k］為參考航跡中的位置。上面的約束可以保證各離散點及其之間的軌跡都不會穿過禁飛區(qū)。

1.4.4" 信賴域約束

因為在凸化飛行器的運動學(xué)模型時使用了小擾動線性化方法，所以需要增加如下的信賴域約束來保證凸化的精度與迭代求解時的收斂性：

|sh［k］－s-h［k］|≤ρ， k=0，1，…，Kh（15）

式中：ρ∈R4為信賴域半徑。

1.4.5" 凸優(yōu)化問題

通過對各種約束進行離散化和凸化，可以構(gòu)建以最小化時間步長Δt為目標(biāo)函數(shù)的凸優(yōu)化問題P1。由于飛行器在一次滾動規(guī)劃中所得航跡的飛行時長為KhΔt，因此只需要最小化每次滾動規(guī)劃中的Δt即可最小化總飛行時長。

P1：" minsh［k］，uh［k］，ΔtJ=Δt， k=0，1，…，Kh

s.t." （5），（6），（9），（13）～（15）

對上述凸優(yōu)化問題使用SCP算法進行迭代求解，定義收斂誤差δp為兩次迭代結(jié)果中狀態(tài)量的變化量，其中大于1的整數(shù)p為迭代的次數(shù)：

δp=|sp［k］－sp－1［k］|， k=0，1，…，Kh（16）

當(dāng)滿足δp≤ε時認(rèn)為算法收斂，其中ε∈R4為誤差閾值。

求解問題P1得到的規(guī)劃結(jié)果的控制量易發(fā)生大幅震蕩，因此希望通過構(gòu)建新的凸優(yōu)化問題以使控制量最優(yōu)（平滑控制量并使飛行器進行機動所消耗的能量最?。τ陔x散化后的凸優(yōu)化問題，上述目標(biāo)可以等價地轉(zhuǎn)化為使控制量在各個離散點處之和最小。由于在新問題中時間步長Δt-是預(yù)先設(shè)定的固定值Δt′，因此需要將式（9）改為

|Ahk+1sh［k+1］+Ahksh［k］+Bhk+1uh［k+1］+

Bhkuh［k］+（Chk+Chk+1）Δt-+Dhk+Dhk+1|≤ζ，

k=0，1，…，Kh－1（17）

之后便可以構(gòu)建凸優(yōu)化問題P2：

P2：" minsh［k］，uh［k］J=∑Khk=0uh［k］1， k=0，1，…，Kh

s.t." （5），（6），（13）～（15），（17）

2" 算法設(shè)計

為使算法規(guī)劃所得航跡在滿足控制量最優(yōu)的同時具有較短的飛行時間，提出一種兩階段航跡規(guī)劃算法：

（1） 預(yù)規(guī)劃階段：生成初始參考航跡，求解P1得到飛行時長最短的規(guī)劃航跡。

（2） 滾動規(guī)劃階段：完成預(yù)規(guī)劃后立即進行第一次滾動規(guī)劃，將預(yù)規(guī)劃階段得到的航跡作為求解P2的初始參考航跡，得到控制量最優(yōu)的規(guī)劃航跡。之后，飛行器開始按照第一次滾動規(guī)劃的航跡飛行并進行后續(xù)的滾動規(guī)劃。由于下一步滾動規(guī)劃的航跡初始與終端位置均發(fā)生了改變，因此在每次滾動規(guī)劃開始時都需要先更新飛行時長與初始參考航跡，再求解P2更新規(guī)劃航跡。

針對滾動規(guī)劃算法計算量較大的問題，提出滾動規(guī)劃周期收縮策略以減少滾動規(guī)劃次數(shù);設(shè)計了改進參考航跡生成（improved reference trajectory generation， IRTG）算法與信賴域自適應(yīng)收縮策略提高單次航跡規(guī)劃速度。

2.1" 滾動規(guī)劃周期收縮策略

為了使最終得到的航跡的終端位置與目標(biāo)位置之間的偏差較小，滾動規(guī)劃周期（兩次滾動規(guī)劃之間的時間間隔）不應(yīng)過長以提高航跡更新頻率，但同時計算量也會增大。另外，滾動規(guī)劃周期必須大于單次規(guī)劃所消耗的時間，而此時間則隨著待規(guī)劃航跡長度的縮短而減小，尤其是在問題初始規(guī)模較大時該現(xiàn)象更為明顯。因此，將滾動規(guī)劃周期設(shè)為固定值在此處并不合適。

令飛行器到達第h次規(guī)劃的航跡起點的時刻th0為開始第h+1次滾動規(guī)劃的時刻th+1p，即有th0=th+1p（h=1，2，…，H-1）。th+1p的計算公式如下：

th+1p=0， h=0

thp+dh+1p， h=1，2，…，H-1（18）

式中：dh+1p為第h次與第h+1次滾動規(guī)劃的開始時間之間的間隔，且需在開始第h次滾動規(guī)劃之前被求出以確定th0。下面給出dh+1p的計算方法。

若飛行器剛完成第h-1次滾動規(guī)劃，那么當(dāng)h=1時，d2p為從第一次滾動規(guī)劃到飛行器開始飛行之間的時長。它的值為預(yù)設(shè)的且是Δt′的整數(shù)倍。

當(dāng)hgt;1時，希望dh+1p的值以近似等差數(shù)列的形式逐漸減小。首先，求出在第h-1次規(guī)劃所得航跡的飛行時長內(nèi)可以進行滾動規(guī)劃的次數(shù)H′：

H′=ceil（（dpl-0.5dd）2+2dd（dh-1f-dHf）/dd-

（dpl）-0.5dd）/dd）（19）

式中：dh-1f為第h-1次規(guī)劃所得航跡的飛行時長;dHf為預(yù)設(shè)的最后一次規(guī)劃所得航跡的飛行時長;dpl為預(yù)設(shè)的相鄰兩次規(guī)劃之間最小的時間間隔;dd為dh+1p與dhp之間的期望差值;ceil（·）為向上取整函數(shù)。取整函數(shù)括號內(nèi)實際上是等差數(shù)列由各項和求項數(shù)的公式。

當(dāng)（dh-1f-dHf）/H′gt;dpl時，有

dh-1f=Δt′·floor2（dh-1f-dHf）H′-dplΔt′（20）

式中：floor（·）為向下取整函數(shù)。

當(dāng)（dh-1f-dHf）/H′≤dpl時，意味著由第h次滾動規(guī)劃的時刻到第h+1次滾動規(guī)劃的時刻之間的時長已經(jīng)小于或等于dpl了，所以直接將第h次滾動規(guī)劃作為最后一次滾動規(guī)劃，即有H=h。最后一次滾動規(guī)劃的航跡起點的時刻tH0為

tH0=Δt′·floor（（th-1f-dHf）/Δt′）（21）

由上述討論可知，航跡終端位置誤差同dpl與dHf成正相關(guān)。

上述策略在航跡規(guī)劃的前期與中期可以為單次規(guī)劃提供充足時間;在后期單次規(guī)劃速度顯著加快，所以可以通過提高航跡更新頻率來降低終端位置誤差，并且有助于充分利用飛行器上的計算資源。

2.2" 基于最優(yōu)航跡相似性的IRTG

目前，由于大多數(shù)研究不需要滾動求解SCP，或滾動求解問題規(guī)模較小，因此在初始參考航跡的選擇上一般都以簡便為原則：如使用直線［18］或連接初始與終端狀態(tài)的線性插值序列［31］作為初始參考航跡。實際上，在SCP中合適的初始參考航跡可以避免不必要的迭代求解，提高航跡規(guī)劃的速度［24］。由SCP算法線性收斂的性質(zhì)可知，初值離最優(yōu)解越近，后續(xù)迭代解的變化率就越小，相鄰迭代解的變化量必然更快進入到收斂誤差閾值以內(nèi)?？紤]到本文研究的問題中，目標(biāo)低速移動且禁飛區(qū)為靜態(tài)的，相鄰兩次滾動規(guī)劃所得最優(yōu)航跡除距離終端位置較近的一小段航跡會有變化外，其余部分航跡的形狀基本是一樣的，該現(xiàn)象稱為最優(yōu)航跡的相似性?；诖饲疤?，可以將上一次滾動規(guī)劃航跡在微調(diào)后作為當(dāng)前滾動規(guī)劃的初始參考航跡。此方法在本質(zhì)上是通過設(shè)置與最優(yōu)解接近的初值，以降低初始收斂誤差的方式加快了算法收斂速度。

2.2.1" 確定初始/終端狀態(tài)

對于第h（h=1，2，…，H）次滾動規(guī)劃，將上一次滾動規(guī)劃的航跡在th0時刻的狀態(tài)設(shè)為初始狀態(tài)：

sh0=s-h0=sh－1［kh0］

uh0=u-h0=uh－1［kh0］（22）

式中：kh0為上一次滾動規(guī)劃航跡對應(yīng)th0時刻的索引。

終端狀態(tài)的航向角和速度由人為給定，終端位置則由每次滾動規(guī)劃開始時飛行器接收到的目標(biāo)位置決定：

shf=s-hf=［phf，f，Vf］T

phf=phtgt（23）

式中：phf為第h次滾動規(guī)劃的終端位置;phtgt為該次滾動規(guī)劃開始時獲得的目標(biāo)位置。

2.2.2" 調(diào)整飛行時長

理論上，在每次滾動規(guī)劃時求解P1可以得到最優(yōu)的總飛行時長，且求得的航跡可以作為求解P2所需的初始參考航跡，最終得到飛行時間與控制量均為最優(yōu)的規(guī)劃航跡。但是這種方法需要額外進行一次SCP求解，計算量較大。IRTG基于最優(yōu)航跡相似性，使用下面的公式估算飛行時長的調(diào)整值：

Δdhf=Δt′·round（phf－pha）T（phf－ph－1f）Δt′·Va·phf－pha2（24）

式中：pha為假設(shè)位置，可選取完整航跡的后1/3處附近的位置為假設(shè)位置;Vha為假設(shè)速度;round（·）為四舍五入取整函數(shù)，用于將該值調(diào)整為Δt′的整數(shù)倍。由于在第一次滾動規(guī)劃時終端位置未發(fā)生改變，故有h=2，3，…，H。式（24）中各位置點的幾何關(guān)系如圖2所示。

已知上一次滾動規(guī)劃所得航跡的飛行時長dh－1f，可以求得本次航跡規(guī)劃的飛行時長為

dhf=dh－1f+Δdhf（25）

Vha的值與飛行器在末段航跡的實際飛行速度沒有關(guān)系。出于保守考慮，Vha的取值在航跡長度變長時應(yīng)偏小，在變短時應(yīng)偏大，以防止因飛行時長過短超出飛行器的機動能力導(dǎo)致規(guī)劃失敗。Vha的計算公式如下：

Vha=Vref－sign（（phf－pha）T（phf－ph－1f））·Vbias（26）

式中：Vref為基準(zhǔn)速度;Vbias為速度偏置量;sign（·）為符號函數(shù)。

2.2.3" 生成末段航跡

每次滾動規(guī)劃的參考航跡由上一次滾動規(guī)劃得到的航跡與生成的末段航跡組成。假設(shè)末段航跡有預(yù)先設(shè)定的Ke個離散區(qū)間，當(dāng)Kh－1≥Ke時其初始狀態(tài)量與控制量為

s-h，e0=sh－1［Kh－1－Ke］

u-h，e0=uh－1［Kh－1－Ke］（27）

否則有

s-h，e0=sh－1［0］

u-h，e0=uh－1［0］

Ke=Kh－1（28）

式中：h=2，3，…，H;shf為末段航跡終端狀態(tài)。

可以在不考慮障礙時使用Dubins曲線生成滿足初始/終端狀態(tài)約束以及飛行器運動學(xué)約束的最短的路徑。其中，飛行器的運動學(xué)約束主要體現(xiàn)在Dubins曲線的圓弧段曲率半徑上。為了盡量縮短機動時間，假設(shè)飛行器在最大飛行速度下以側(cè)向加速度絕對值的最大值轉(zhuǎn)彎，可以將待生成的Dubins曲線的曲率半徑保守地設(shè)為

rdbs=V2max/max（|aymax|，|aymin|）（29）

式中：Vmax為飛行器速度上界;aymax為飛行器側(cè)向加速度的上界;aymin為飛行器側(cè)向加速度的下界。由于生成的Dubins曲線為僅包含位置與方向的幾何軌跡，無法用作參考航跡。所以，使用下面的方法根據(jù)Dubins曲線生成飛行器的全狀態(tài)參考航跡并進行離散化。

假設(shè)飛行器在根據(jù)Dubins曲線飛行時做勻加速運動，且可通過幾何方法求得Dubins曲線的長度l-h，e，那么該段飛行器的切向加速度a-h，ex與飛行時長d-h，e分別為

a-h，ex=（（V-h，ef）2－（V-h，e0）2）/（2l-h，e）

d-h，e=（2l-h，e）/（V-h，ef+V-h，e0）（30）

求出末段航跡的時間步長Δt-h，e=d-h，e/Ke后即可得到離散化的飛行器速度序列V-h，e［k］，接下來可根據(jù)飛行器在某離散時刻kΔt-h，e飛行的距離l-h，e［k］求出其于該時刻在Dubins曲線上的位置p-h，e［k］和方向-h，e［k］（k=0，1，…，Ke）。l-h，e［k］的計算公式為

l-h，e［k］=V-h，e［0］·kΔt-h，e+0.5a-h，ex［k］·（kΔt-h，e）2，

k=0，1，…，Ke（31）

飛行器的切向加速度a-h，ex［k］恒為a-x，飛行器的側(cè)向加速度a-h，ey［k］則由飛行器此時刻在Dubins曲線上的位置決定。Dubins曲線由R、S、L 3種曲線組成，其中R段為順時針轉(zhuǎn)動的圓弧，L段為逆時針轉(zhuǎn)動的圓弧，S段為直線。當(dāng)p-h，e［k］在S段上時，a-h，ey［k］=0。

當(dāng)p-h，e［k］在L段上時有：

a-h，ey［k］=V-h，e［k］2cos（-h，e［k］）rdbs（32）

當(dāng)p-h，e［k］在R段上時，a-h，ey［k］為式（32）計算結(jié)果的相反數(shù)。

將上一次滾動規(guī)劃所得航跡與末段航跡拼合，當(dāng)Kh－1≥Ke時有：

s-h′=sh-1［0］，sh-1［1］，…，sh-1［Kh-1-Ke-1］，s-h，e

u-h′=uh-1［0］，uh-1［1］，…，uh-1［Kh-1-Ke-1］，u-h，e（33）

否則可得

s-h′=s-h，e

u-h′=u-h，e（34）

上述利用Dubins曲線生成初始參考航跡的方法同樣用于在預(yù)規(guī)劃階段為求解P1提供初始參考航跡：

s-=s-e

u-=u-e（35）

航跡的初始狀態(tài)為s-e0=s0，終端狀態(tài)為s-ef=sf。若離散區(qū)間個數(shù)為K，則Δt-=Δt-e=d-e/K。其中，s0與sf分別為求解P1時飛行器的初始狀態(tài)與終端狀態(tài)。

2.2.4" 調(diào)整離散區(qū)間個數(shù)

末段更新后的初始參考航跡{s-h′，u-h′}的飛行時長并不等于本次滾動規(guī)劃的期望飛行時長，另外求解P1得到的規(guī)劃航跡{s，u，Δt}的時間步長與求解P2時設(shè)定的時間步長不一定相等，所以需要調(diào)整初始參考航跡的飛行時長。在給定時間步長Δt′下，調(diào)整航跡的飛行時長等價于調(diào)整航跡的離散區(qū)間個數(shù)。此處，h=1，2，…，H。

首先求出初始參考航跡的長度l-h，然后需要求出在指定飛行時長dhf內(nèi)，滿足初始速度V-h0、終端速度V-hf以及航跡長度l-h約束的飛行器速度序列。將速度序列設(shè)計為“勻減速、勻加速、勻速”的3段形式。在變速段（勻減速與勻加速段）中，勻減速段的加速度a-decx為預(yù)設(shè)的已知量，飛行時長d-h，dec為未知量;勻加速段的加速度a-h，accx與飛行時長d-h，acc均為未知量。變速段的長度必須小于或等于參考航跡全長，可得到滿足此條件的d-h，dec的表達式：

d-h，dec=max0，ceil1a-decxΔt′（（2lh－V-hfdhf－V-h0·

（V-hf－V-h0）/a-decx）/（dhf－（V-hf－V-h0）/a-decx）－V-h0）（36）

之后便可以根據(jù)下式得到要求的速度序列：

a-h，acc=（V-hf－V-h0－a-decxd-h，dec）2/（2（V-hf（d-h，acc+

d-h，evn）－

（l-h－V-h0d-h，dec－0.5·a-decxd-h，dec2））

d-h，acc=（V-hf－V-h0－a-decxd-h，dec）/a-h，acc

d-h，evn=dhf－d-h，dec－d-h，acc（37）

式中：d-h，evn為勻速段的飛行時長。

根據(jù)上面的計算結(jié)果，可以確定滿足要求的初始參考航跡速度序列，然后便可以根據(jù)飛行器在某離散時刻kΔt′飛行的距離l-h［k］通過插值法求出其此時在參考航跡上的位置p-h［k］和航向-h［k］，進而可以求得側(cè)向加速度序列a-hy［k］。至此，已經(jīng)得到了第h次滾動規(guī)劃的初始參考航跡{s-h，u-h}。

2.3" SCP算法信賴域自適應(yīng)收縮策略

信賴域收縮是SCP算法收斂的必要條件之一［33］。此處在文獻［34］的基礎(chǔ)上，設(shè)計了根據(jù)目標(biāo)函數(shù)變化率與收斂誤差自適應(yīng)調(diào)節(jié)信賴域收縮系數(shù)的策略，在保證凸優(yōu)化問題有解的前提下提高收斂速度。

在SCP算法中每次迭代的目標(biāo)函數(shù)變化率ηp為

ηp=（Jp－Jp－1）/Jp－1（38）

當(dāng)ηp的絕對值較大時，說明兩次迭代的解之間有較大差距，算法可能還未找到最優(yōu)解，因此降低信賴域的收縮速度使算法可以在較大的范圍內(nèi)尋找最優(yōu)解;否則說明兩次迭代的解之間變化不大，可以認(rèn)為算法已經(jīng)到達最優(yōu)解附近，因此加快信賴域的收縮速度使算法盡快收斂。設(shè)第p次迭代的信賴域收縮系數(shù)為λp，那么下一次迭代的信賴域ρp+1為

ρp+1=λpρp（39）

信賴域收縮系數(shù)的計算方法如下：

λp=λ1－ξp1（λ2+ξp2λ3）

ξp1=max（0，sign（－ηp））

ξp2=max（0，sign（τ1－|ηp|））·

sign（τ2－argmax（div（δp，（λ1－λ2－λ3）ρp）））（40）

式中：λ1，λ2，λ3為收縮系數(shù)的分量，且滿足0lt;λ3≤λ2lt;λ1lt;1，λ1gt;λ2+λ3;τ1、τ2為閾值;argmax（·）為取向量中的最大元素;div（·，·）代表括號內(nèi)第一個向量的分量除以第二個向量的對應(yīng)分量;ξp1、ξp2的值只有0、1兩種可能：當(dāng)ηplt;0時說明目標(biāo)函數(shù)的值在下降，則ξp1值為1使λp減小;當(dāng)|ηp|lt;τ1且該次迭代的收斂誤差小于下一次迭代的最小信賴域的τ2倍時，說明目標(biāo)函數(shù)已經(jīng)到達最優(yōu)解附近且收縮后的信賴域不會過小，則ξp2值為1使λp進一步減小。

2.4" IRTG-MPC-SCP算法流程

將滾動規(guī)劃周期收縮策略、改進參考航跡生成算法、信賴域自適應(yīng)收縮策略與滾動SCP算法結(jié)合，得到針對低速目標(biāo)的飛行器中段航跡快速規(guī)劃算法IRTG-MPC-SCP。下面將該算法分為SCP算法與滾動規(guī)劃框架兩部分進行介紹。

SCP算法求解流程如圖3所示。

算法在求解問題P1與P2時的輸入、優(yōu)化變量、構(gòu)建約束的具體形式有所不同，但是基本結(jié)構(gòu)是一樣的。

步驟 1" 初始化參數(shù)，設(shè)置優(yōu)化變量、參考航跡、狀態(tài)量與控制量邊界、禁飛區(qū)信息，并構(gòu)造約束。

步驟 2" 求解凸優(yōu)化問題。

步驟 3" 判斷收斂誤差是否小于誤差閾值，如果不滿足則認(rèn)為解未收斂，更新迭代次數(shù)并將該次迭代求得的解作為下一次迭代的參考航跡，然后計算信賴域收縮系數(shù)并更新信賴域，返回步驟2開始下一次迭代。

步驟 4" 如果收斂誤差小于等于誤差閾值則認(rèn)為解已經(jīng)收斂，將該次迭代的結(jié)果作為SCP算法的解輸出。

滾動規(guī)劃框架為IRTG-MPC-SCP算法的外圈循環(huán)。相應(yīng)的，SCP算法為內(nèi)圈循環(huán)。IRTG-MPC-SCP算法的流程圖如圖4所示，其中步驟2、步驟3、步驟5與IRTG算法有關(guān)。

步驟 1" 初始化參數(shù)，設(shè)置飛行器預(yù)規(guī)劃階段的初始與終端狀態(tài)、狀態(tài)量與控制量邊界、禁飛區(qū)信息。

步驟 2" 根據(jù)初始狀態(tài)與終端狀態(tài)使用Dubins曲線生成初始參考航跡，并基于該航跡使用SCP求解問題P1，得到預(yù)規(guī)劃航跡{s，u，Δt}與最優(yōu)飛行時長。

步驟 3" 開始第1次滾動規(guī)劃，調(diào)整規(guī)劃航跡{s，u，Δt}的飛行時長得到新的初始參考航跡{s-h，u-h}并使用SCP求解問題P2，得到所需機動最小的規(guī)劃航跡。

步驟 4" 更新滾動規(guī)劃總次數(shù)H、下一次滾動規(guī)劃時刻th+1p、當(dāng)前滾動規(guī)劃次數(shù)h、目標(biāo)位置phtgt、終端位置shf、剩余飛行時長dhf。

步驟 5" 將上一次滾動規(guī)劃得到的規(guī)劃航跡{sh－1，uh－1}與利用Dubins曲線生成的航跡{s-h，e，u-h，e}拼接得到{s-h′，u-h′}，調(diào)整其飛行時長，最終得到初始參考航跡{s-h，u-h}并使用SCP求解問題P2，得到該次滾動規(guī)劃航跡并輸出其結(jié)果。

步驟 6" 若該次滾動規(guī)劃已是最后一次，則退出循環(huán)并結(jié)束算法，否則返回步驟4進行下一次滾動規(guī)劃。

3" 仿真結(jié)果與分析

對IRTG-MPC-SCP在不同的仿真場景下開展仿真試驗，并與不使用IRTG的滾動SCP航跡規(guī)劃算法MPC-SCP進行對比，以驗證本文設(shè)計的算法的合理性與優(yōu)越性。使用配置Intel Core i7-11800H 2.3 GHz處理器與16 GB內(nèi)存的計算機，凸優(yōu)化求解器使用MOSEK。

3.1" 航跡規(guī)劃仿真條件與算法參數(shù)設(shè)置

設(shè)置狀態(tài)量與控制量邊界約束如下：

smin=［－∞，－∞，－∞，150 m/s］T

smax=［∞，∞，∞，300 m/s］T

umin=［－49 m/s2，-5 m/s2］T

umax=［49 m/s2，10 m/s2］T（41）

SCP算法與信賴域自適應(yīng)收縮策略中參數(shù)的設(shè)置如下：

ρ0P1=［150 000 m，150 000 m，π/3 rad，150 m/s］T

ρ0P2=［25 000 m，25 000 m，π/6 rad，150 m/s］T

ζ=［10－3 m，10－3 m，10－3 rad，10－3 m/s］T

ε=［10 m，10 m，π/180 rad，1 m/s］T

λ1=0.6

λ2=0.3

λ3=0.2

τ1=0.01

τ2=3（42）

此處設(shè)計了兩個不同大小尺度的仿真場景，以驗證算法對不同規(guī)模航跡規(guī)劃問題的適應(yīng)性和可擴展性。

3.1.1" 大規(guī)模航跡規(guī)劃問題（條件1）

飛行器的初始位置為（0，100 000）m，初始航向為－π/2 rad，初始速度為200 m/s，終端航向為π/2 rad，終端速度為300 m/s;目標(biāo)的初始位置為（400 000，290 000）m，且朝π rad航向以16 m/s（約等于30節(jié)）的速度做勻速直線運動。表1給出了該條件下的禁飛區(qū)設(shè)置。

滾動規(guī)劃周期收縮策略中需要預(yù)設(shè)的參數(shù)如下：

d2p=9 s

dpl=10 s

dHf=30 s

dd=60 s（43）

IRTG算法中需要預(yù)設(shè)的參數(shù)如下：

Vref=250 m/s

Vbias=50 m/s

K=500

Ke=50

Δt′=3 s

a-decx=－8 m/s2

（44）

3.1.2" 小規(guī)模航跡規(guī)劃問題（條件2）

飛行器的初始位置為（0，0）m，初始航向為0 rad，初始速度為200 m/s，終端航向為0 rad，終端速度為300 m/s;目標(biāo)的初始位置為（35 000，23 000）m，且朝0 rad航向以16 m/s的速度做勻速直線運動。表2給出了該條件下的禁飛區(qū)設(shè)置。

Δt′=1 s（45）

相比條件1，條件2中待規(guī)劃航跡的長度顯著縮短，問題規(guī)模較小，單次規(guī)劃速度加快。減小K與Δt′可以在縮小待求解問題規(guī)模的同時增加離散化的精度;減小dHf與dh可以縮短滾動規(guī)劃周期，提高航跡更新頻率。更改上述參數(shù)可以使算法對特定問題具有更好的適應(yīng)性。

3.2" 航跡規(guī)劃仿真結(jié)果與分析

圖5和圖6中圓形黃色斜線區(qū)域為禁飛區(qū)，航跡中每一次滾動規(guī)劃得到的航跡起點使用星形或者三角形標(biāo)示，H1代表第一次滾動規(guī)劃，依此類推。條件1下飛行器進行了9次滾動規(guī)劃，條件2下進行了6次滾動規(guī)劃，且距離目標(biāo)越近滾動規(guī)劃的頻率越高。

規(guī)劃所得航跡的終端位置與目標(biāo)位置有偏差，這是因為在完成最后一次滾動規(guī)劃后目標(biāo)仍在運動。通常，飛行器在到達終端位置之前就已經(jīng)進入末制導(dǎo)階段而無需按照規(guī)劃航跡飛行，因此該偏差僅用于描述航跡規(guī)劃算法對目標(biāo)的跟蹤能力。由于目標(biāo)相對飛行器低速運動且dHp與dHf的取值較小，所以該偏差也較?。簵l件1下規(guī)劃航跡的終端位置誤差為508 m，條件2下為352 m。滾動規(guī)劃周期收縮策略可在減少航跡更新次數(shù)的同時保持較小的終端位置誤差，有效降低了計算量。

圖7與圖8給出了兩條件下飛行器航向、速度、控制量的變化曲線。由圖可知，它們均在邊界之內(nèi)，且滿足了始末約束。飛行器加速度在絕大部分時刻均為0，說明選取的目標(biāo)函數(shù)實現(xiàn)了使控制量最優(yōu)的目標(biāo)。

圖9給出了每次滾動規(guī)劃航跡的飛行時長。條件1中目標(biāo)向接近飛行器初始位置的方向運動，因此飛行時長逐漸減小，條件2則與此相反。仿真結(jié)果表明，無論目標(biāo)是背離還是朝向飛行器運動，IRTG均可以給出飛行時長的有效估計值。各次滾動規(guī)劃得到的飛行時長加d2p后即為到達時刻。

圖10給出了每次航跡規(guī)劃的迭代次數(shù)與消耗時間。其中，第0次規(guī)劃為預(yù)規(guī)劃。由于在預(yù)規(guī)劃階段初始信賴域取值較大，SCP收斂速度相對更慢。由仿真結(jié)果可知，隨著滾動規(guī)劃向前推進，待規(guī)劃航跡長度越來越短，規(guī)劃耗時也越來越小，這是滾動規(guī)劃周期收縮策略有效的必要條件。每次航跡規(guī)劃經(jīng)過平均4.5次迭代后解即可收斂，體現(xiàn)了信賴域自適應(yīng)收縮策略的有效性。結(jié)合圖11可知，單次滾動規(guī)劃耗時均明顯小于兩次滾動規(guī)劃之間的時間間隔（第0、1次規(guī)劃為第1次滾動規(guī)劃），證明算法可以在線更新航跡。

仿真實驗同時表明，僅需通過修改第3.1.2節(jié)中給定的幾個參數(shù)即可將算法應(yīng)用于不同規(guī)模的航跡規(guī)劃問題，體現(xiàn)了其良好的適應(yīng)性和可擴展性。

3.3" 算法性能對比

為驗證IRTG對MPC-SCP算法性能的提升，這里將IRTG-MPC-SCP算法與以下兩個對照算法進行對比。

對照算法1" 該算法在每次滾動規(guī)劃時先使用第2.2.2節(jié)中的方法更新飛行時長，再將連接初始與終端狀態(tài)的線性插值序列作為初始參考航跡，其余部分與IRTG-MPC-SCP相同。

對照算法2" 該算法在每次滾動規(guī)劃時通過求解P1更新最優(yōu)飛行時長，再將其結(jié)果作為求解P2的初始參考航跡，其余部分與IRTG-MPC-SCP相同，它可以得到飛行時長與控制量均為最優(yōu)的規(guī)劃航跡。

設(shè)置仿真條件如下：飛行器的初始位置為（0，0）m，初始航向為π/4 rad，初始速度為180 m/s，終端航向為π/4 rad，終端速度為300 m/s;目標(biāo)的初始位置為（392 929，292 929）m，且朝0 rad航向以16 m/s的速度做勻速直線運動;禁飛區(qū)的數(shù)量為一個，其中心位置為（220 000，200 000）m，半徑為60 000 m。算法中需要預(yù)設(shè)的參數(shù)與第3.1節(jié)及第3.1.1節(jié)中的一致。仿真得到的航跡如圖12所示。

根據(jù)仿真結(jié)果，3個算法均進行了10次滾動規(guī)劃;終端位置誤差均為816.1 m，在可接受范圍之內(nèi)。由圖12可知，IRTG-MPC-SCP與對照算法2得到的規(guī)劃航跡幾乎一致，而對照算法1得到的規(guī)劃航跡在第4次滾動規(guī)劃時出現(xiàn)明顯彎曲，與其他算法的結(jié)果有較大偏差。這是因為SCP具有依賴初始參考航跡的特性，其解有可能收斂到某個局部最優(yōu)航跡而非全局最優(yōu)的［35］。IRTG-MPC-SCP、對照算法1、對照算法2求得目標(biāo)函數(shù)值分別為160.2 m/s2、437.6 m/s2、149.3 m/s2。與對照算法1相比，在滾動規(guī)劃的過程中IRTG使MPC-SCP算法始終在原最優(yōu)航跡附近搜索解，有利于提高其數(shù)值穩(wěn)定性，保證優(yōu)化效果。

IRTG-MPC-SCP與對照算法1使用同樣的方法估計新的飛行時長，因此由圖13可知，兩算法每次滾動規(guī)劃航跡的飛行時長是一致的。與對照算法2求得的最優(yōu)飛行時長相比，參數(shù)設(shè)定相對保守的IRTG得到的最終飛行時長雖然略長，但與最優(yōu)值非常接近，僅增加了2.08%。這說明飛行時長的估計算法設(shè)計是合理的。

由圖14可知，IRTG-MPC-SCP算法單次規(guī)劃的計算速度在滾動規(guī)劃的中前期明顯快于兩個對照算法;在后期隨著問題規(guī)模的縮小，3個算法的計算速度趨于一致。IRTG-MPC-SCP、對照算法1、對照算法2單次規(guī)劃的平均迭代次數(shù)分別為3.5次、7.6次、10.1次，平均規(guī)劃耗時分別為0.29 s、0.48 s、0.51 s。IRTG-MPC-SCP算法單次規(guī)劃的平均迭代次數(shù)與平均規(guī)劃耗時分別僅為對照算法1、算法2的46.1%、34.7%與60.4%、56.9%，速度要明顯優(yōu)于對照算法。在求解大規(guī)模問題時，IRTG算法在保證優(yōu)化效果的同時通過縮小初始收斂誤差的方式顯著提高了MPC-SCP算法的單次規(guī)劃速度，有助于算法滿足在線航跡規(guī)劃的時間性能要求。

4" 結(jié)束語

本文針對長航程飛行器打擊移動艦船目標(biāo)的場景，基于MPC-SCP算法設(shè)計了中段航跡快速規(guī)劃算法。為使算法可以在線求解大規(guī)模航跡規(guī)劃問題，本文從降低計算量和提高計算速度兩方面入手對一般的MPC-SCP算法進行了改進：① 提出基于剩余飛行時間的滾動規(guī)劃周期收縮策略，在保證終端位置誤差較小的同時減少了航跡更新次數(shù);② 設(shè)計了IRTG算法并采用了信賴域自適應(yīng)收縮策略，能在更短時間內(nèi)得到最優(yōu)航跡。仿真結(jié)果表明，本文設(shè)計的算法規(guī)劃效果優(yōu)良，與一般的MPC-SCP算法相比速度更快，可以滿足在線規(guī)劃的要求，且能有效適應(yīng)不同的任務(wù)場景。IRTG對單次航跡規(guī)劃求解效果的提升說明在使用SCP算法時，通過在初始參考解中引入先驗信息有助于提高求解速度、優(yōu)化求解結(jié)果。未來可以進一步研究類似方法對SCP算法性能的影響，在不同的應(yīng)用場景下有針對性地使用先驗信息，有助于其在線應(yīng)用。

參考文獻

［1］ DRAKE D， KOZIOL S， CHABOT E. Mobile robot path planning with a moving goal［J］. IEEE Access， 2018， 6： 1280012814.

［2］ PHUNG M D， HA Q P. Motion-encoded particle swarm optimization for moving target search using UAVs［J］. Applied Soft Computing， 2020， 97： 106705.

［3］ TRIHARMINTO H H， ADJI T B， SETIAWAN N A. Dynamic UAV path planning for moving target intercept in 3D［C］∥Proc.of the IEEE 2nd International Conference on Instrumentation Control and Automation， 2011： 157161.

［4］ MEYER Y， ISAIAH P， SHIMA T. On Dubins paths to intercept a moving target［J］. Automatica， 2015， 53： 256263.

［5］ BEUL M， BEHNKE S. Fast full state trajectory generation for multirotors［C］∥Proc.of the IEEE International Conference on Unmanned Aircraft Systems， 2017： 408416.

［6］ ZHENG Y， CHEN Z， SHAO X M， et al. Time-optimal guidance for intercepting moving targets with impact-angle constraints［J］. Chinese Journal of Aeronautics， 2022， 35（7）： 157167.

［7］ LIU Y， WANG Y D， DONG L. USV target interception control with reinforcement learning and motion prediction method［C］∥Proc.of the IEEE 37th Youth Academic Annual Conference of Chinese Association of Automation， 2022： 10501054.

［8］ 王祝， 劉莉， 龍騰， 等. 基于罰函數(shù)序列凸規(guī)劃的多無人機軌跡規(guī)劃［J］. 航空學(xué)報， 2016， 37（10）： 31493158.

WANG Z， LIU L， LONG T， et al. Trajectory planning for multi-UAVs using penalty sequential convex programming［J］. Acta Aeronautica et Astronautica Sinica， 2016， 37（10）： 31493158.

［9］ MENG B B. UAV path planning based on bidirectional sparse A* search algorithm［C］∥Proc.of the IEEE International Conference on Intelligent Computation Technology and Automation， 2010： 11061109.

［10］ ZHEN Z Y， CHEN Y， WEN L D， et al. An intelligent coopera-tive mission planning scheme of UAV swarm in uncertain dynamic environment［J］. Aerospace Science and Technology， 2020， 100： 105826.

［11］ QI Y Y， LIU J C， YU J Z. Dynamic modeling and hybrid fireworks algorithm-based path planning of an amphibious robot［J］. Gui-dance， Navigation and Control， 2022， 2（1）： 2250002.

［12］ OYANA S N O， LI J， USMAN M. Three-layer multi-UAVs path planning based on ROBL-MFO［J］. Guidance， Navigation and Control， 2022， 2（3）： 2250017.

［13］ SHANMUGAVEL M， TSOURDOS A， WHITE B， et al. Cooperative path planning of multiple UAVs using Dubins paths with clothoid arcs［J］. Control Engineering Practice， 2010， 18（9）： 10841092.

［14］ YANG S B， CUI T， HAO X Y， et al. Trajectory optimization for a ramjet-powered vehicle in ascent phase via the Gauss pseudospectral method［J］. Aerospace Science and Technology， 2017， 67： 8895.

［15］ YAN C， XIANG X J， WANG C. Towards real-time path planning through deep reinforcement learning for a UAV in dynamic environments［J］. Journal of Intelligent amp; Robotic Systems， 2020， 98（12）： 297309.

［16］ ZHANG Y T， ZHANG Y M， YU Z Q. Path following control for UAV using deep reinforcement learning approach［J］. Gui-dance， Navigation and Control， 2021， 1（1）： 2150005.

［17］ 李樾， 韓維， 陳清陽， 等. 凸優(yōu)化算法在有人/無人機協(xié)同系統(tǒng)航跡規(guī)劃中的應(yīng)用［J］. 宇航學(xué)報， 2020， 41（3）： 276286.

LI Y， HAN W， CHEN Q Y， et al. Application of convex optimization algorithm in trajectory planning of manned/unmanned cooperative system［J］. Journal of Astronautics， 2020， 41（3）： 276286.

［18］ LIU X F， SHEN Z J， LU P. Entry trajectory optimization by second-order cone programming［J］. Journal of Guidance， Control， and Dynamics， 2016， 39（2）： 227241.

［19］ LIU X F. Convergence-guaranteed trajectory planning for a class of nonlinear systems with nonconvex state constraints［J］. IEEE Trans.on Aerospace and Electronic Systems， 2021， 58（3）： 22432256.

［20］ ZHANG G X， LIU X F. UAV collision avoidance using mixed-integer second-order cone programming［J］. Journal of Gui-dance， Control， and Dynamics， 2022， 45（9）： 17321738.

［21］ DINH Q T， DIEHL M. Local convergence of sequential convex programming for nonconvex optimization［C］∥Proc.of the Recent Advances in Optimization and its Applications in Engineering： the 14th Belgian-French-German Conference on Optimization， 2010： 93102.

［22］ MESSERER F， DIEHL M. Determining the exact local convergence rate of sequential convex programming［C］∥Proc.of the IEEE European Control Conference， 2020： 12801285.

［23］ BONALLI R， LEW T， PAVONE M. Analysis of theoretical and numerical properties of sequential convex programming for continuous-time optimal control［J］. IEEE Trans.on Automatic Control， 2023， 68（8）： 45704585.

［24］ AUGUGLIARO F， SCHOELLIG A P， D’ANDREA R. Gene-ration of collision-free trajectories for a quadrocopter fleet： a sequential convex programming approach［C］∥Proc.of the IEEE/RSJ International Conference on Intelligent Robots and Systems， 2012： 19171922.

［25］ JIANG H， AN Z， YU Y N， et al. Cooperative guidance with multiple constraints using convex optimization［J］. Aerospace Science and Technology， 2018， 79： 426440.

［26］ MORGAN D， CHUNG S J， HADAEGH F Y. Model predictive control of swarms of spacecraft using sequential convex programming［J］. Journal of Guidance， Control， and Dynamics， 2014， 37（6）： 17251740.

［27］ MORGAN D， SUBRAMANIAN G P， CHUNG S J， et al. Swarm assignment and trajectory optimization using variable-swarm， distributed auction assignment and sequential convex programming［J］. The International Journal of Robotics Research， 2016， 35（10）： 12611285.

［28］ 徐廣通， 鄒汝平， 王祝， 等. 基于滾動規(guī)劃框架的多無人機協(xié)同軌跡快速生成方法［J］. 無人系統(tǒng)技術(shù)， 2021， 4（2）： 3339.

XU G T， ZOU R P， WANG Z， et al. Multiple unmanned aerial vehicle rapid cooperative trajectory generation method using receding planning framework［J］. Unmanned Systems Techno-logy， 2021， 4（2）： 3339.

［29］ 徐廣通， 孟子陽， 龍騰， 等. 通信距離受限下無人機集群軌跡分布式滾動規(guī)劃［J］. 中國科學(xué)： 信息科學(xué)， 2022， 52（8）： 15271541.

XU G T， MENG Z Y， LONG T， et al. Trajectory distributed receding planning for UAV swarms subject to limited communication distance［J］. Scientia Sinica Informations， 2022， 52（8）： 15271541.

［30］ SHI Y， ZHANG L H， DONG S Q. Path planning of anti-ship missile based on Voronoi diagram and binary tree algorithm［J］. Defence Science Journal， 2019， 69（4）： 369377.

［31］ WANG Z， LIU L， LONG T. Minimum-time trajectory planning for multi-unmanned-aerial-vehicle cooperation using sequential convex programming［J］. Journal of Guidance， Control， and Dynamics， 2017， 40（11）： 29762982.

［32］ XU G T， LONG T， WANG Z， et al. Trust-region filtered sequential convex programming for multi-UAV trajectory planning and collision avoidance［J］. ISA Transactions， 2022， 128： 664676.

［33］ 鄧云山， 夏元清， 孫中奇. 基于松弛序列凸優(yōu)化的輪式機器人協(xié)同軌跡規(guī)劃［J］. 無人系統(tǒng)技術(shù)， 2021， 4（1）： 2432.

DENG Y S， XIA Y Q， SUN Z Q. Coordination trajectory planning of wheeled robot using relaxation sequential convex programming［J］. Unmanned System Technology， 2021， 4（1）： 2432.

［34］ 劉哲， 陸浩然， 鄭偉， 等. 多滑翔飛行器時間協(xié)同軌跡快速規(guī)劃［J］. 航空學(xué)報， 2021， 42（11）： 317331.

LIU Z， LU H R， ZHENG W， et al. Rapid time-coordination trajectory planning method for multi-glide vehicles［J］. Acta Aeronautica et Astronautica Sinica， 2021， 42（11）： 317331.

［35］ 鄧雁鵬， 穆榮軍， 彭娜， 等. 月面著陸動力下降段最優(yōu)軌跡序列凸優(yōu)化方法［J］. 宇航學(xué)報， 2022， 43（8）： 10291039.

DENG Y P， MU R J， PENG N， et al. Sequential convex optimization method for lunar landing during power decent phase［J］. Journal of Astronautics， 2022， 43（8）： 10291039.

作者簡介

熊子淳（1999—），男，碩士研究生，主要研究方向為運動規(guī)劃、飛行器制導(dǎo)控制與仿真。

劉永善（1965—），男，副研究員，博士，主要研究方向為飛行動力學(xué)與控制、飛行器制導(dǎo)控制與仿真。