郭 杰，相 巖，王 肖，史鵬飛，唐勝景

(1. 北京理工大學(xué)宇航學(xué)院，北京 100081；2. 飛行器動力學(xué)與控制教育部重點實驗室，北京 100081；3. 中國運載火箭技術(shù)研究院，北京 100076；4. 北京航天自動控制研究所，北京 100854)

0 引 言

近年來，可重復(fù)使用運載器成為航空航天領(lǐng)域的研究熱點。隨著SpaceX公司獵鷹火箭回收和復(fù)用的多次成功，垂直起降重復(fù)使用運載火箭受到了廣泛關(guān)注，著陸段制導(dǎo)問題是火箭垂直回收的關(guān)鍵性問題之一。著陸段約束復(fù)雜、飛行環(huán)境高動態(tài)變化，制導(dǎo)算法需要具備在線處理多約束優(yōu)化問題的能力。隨著計算機性能的提升和優(yōu)化方法的發(fā)展，一些學(xué)者提出了計算制導(dǎo)的概念，即通過在線實時軌跡優(yōu)化得到制導(dǎo)指令，直接輸入動力學(xué)方程實現(xiàn)閉環(huán)最優(yōu)制導(dǎo)?？焖佟⒏呔鹊脑诰€軌跡優(yōu)化方法是閉環(huán)最優(yōu)制導(dǎo)的基礎(chǔ)。

軌跡優(yōu)化方法一般可分為直接法和間接法。在直接法中，凸優(yōu)化方法求解速度快、具備確定收斂準(zhǔn)則的特點使其在數(shù)值計算方面具有明顯的優(yōu)勢。然而大多數(shù)飛行器軌跡優(yōu)化問題的動力學(xué)和約束都是非凸的，因此應(yīng)用凸優(yōu)化方法的難點在于問題的凸化。目前常用的凸化方法主要有序列凸化和無損凸化兩種。

相較于序列凸化，無損凸化采用變量替換、約束松弛的方式實現(xiàn)凸化，無需參考軌跡和附加的信賴域約束，具有良好的收斂特性。然而無損凸化依賴特殊的問題形式，對于飛行高度較低、大氣稠密的火箭子級，氣動力的影響不可忽略，無法直接應(yīng)用無損凸化求解。文獻[13]采用無損凸化處理推力幅值約束，利用平均速度估計氣動阻力進而完成火箭垂直回收著陸段在線制導(dǎo)律設(shè)計，但是采用平均速度估計氣動力誤差較大，導(dǎo)致軌跡優(yōu)化算法精度和制導(dǎo)算法的魯棒性降低。文獻[14]結(jié)合無損凸化和同倫方法設(shè)計了火箭子級著陸同倫迭代凸化軌跡優(yōu)化方法，但是該算法未考慮推力方向和變化率約束，加重了發(fā)動機系統(tǒng)的工作負擔(dān)。

現(xiàn)有的無損凸化方法在確定最優(yōu)終端時間方面研究較少，多將終端時間設(shè)置為固定值，無法保證開機-終端時刻組合的最優(yōu)性。然而，若直接將作為優(yōu)化變量會破壞原問題的凸性，進而影響原問題良好的收斂特性。另一方面，對尋優(yōu)則需要對包含最優(yōu)解區(qū)間的上下界進行多次試探，計算效率低，求解時間長，難以在線應(yīng)用。因此如何在著陸段初始時刻快速確定最優(yōu)終端時間成為了在火箭回收問題中應(yīng)用無損凸化方法的主要難點。

偽譜法具有求解精度高、算法成熟等優(yōu)點。因此許多學(xué)者將偽譜離散與凸優(yōu)化相結(jié)合求解軌跡優(yōu)化問題，文獻[16]將偽譜離散與無損凸化結(jié)合研究了火星著陸軌跡優(yōu)化問題。文獻[17]將偽譜法與序列凸優(yōu)化結(jié)合，在模型預(yù)測控制框架下解決了火箭垂直回收制導(dǎo)問題。然而上述方法采用全局離散策略，問題稀疏性差，求解耗時較長，難以在線應(yīng)用。

綜上所述，現(xiàn)有的無損凸化方法研究中在確定最優(yōu)終端時間方面研究較少，并且難以實現(xiàn)軌跡優(yōu)化算法求解精度和計算速度的有效權(quán)衡。本文面向可重復(fù)使用運載火箭子級回收問題，提出了一種帶有最優(yōu)終端時間估計策略的hp偽譜同倫凸優(yōu)化在線軌跡規(guī)劃算法。主要貢獻為：1)提出了一種最優(yōu)終端時間快速估計策略，能夠快速估計最優(yōu)終端時間，同時保證優(yōu)化結(jié)果的最優(yōu)性；2)所提方法無需參考軌跡，能夠有效處理氣動力和非凸約束，在保證一定求解精度的條件下有效提升了算法的計算速度。

1 問題描述

1.1 動力學(xué)模型

不考慮地球自轉(zhuǎn)，將地球視為均勻圓球，忽略哥氏慣性力的影響，考慮阻力和升力，參考文獻[18]中的發(fā)射坐標(biāo)系定義著陸點坐標(biāo)系如圖1所示。圖中，坐標(biāo)系原點為著陸點，軸垂直地面向上，軸與初始時刻的速度矢量在地面上投影的方向一致，軸與,軸呈右手坐標(biāo)系。

圖1 著陸點坐標(biāo)系Fig.1 Reference frame of the landing point

在著陸點坐標(biāo)系下建立可重復(fù)使用運載火箭子級著陸段三自由度動力學(xué)方程如下

(1)

(2)

式中：和分別為升力系數(shù)和阻力系數(shù)；為火箭子級參考特征面積；為大氣密度，其計算公式為

=exp(-)

(3)

式中：為海平面密度；為參考高度；為火箭子級飛行高度。

對于火箭垂直回收過程，令狀態(tài)變量=[,,]，控制變量==[,,]，則系統(tǒng)狀態(tài)空間表達式為

(4)

1.2 軌跡優(yōu)化問題

火箭子級垂直回收需滿足定點軟著陸的任務(wù)需求，因此定義軌跡優(yōu)化問題的初始和終端約束為

(5)

式中：和分別為著陸段初始和終端時刻；和分別為火箭子級著陸段初始質(zhì)量和結(jié)構(gòu)干重；對于運載火箭子級一般有=,=。進一步考慮控制量約束，其中推力幅值和方向約束為

(6)

(7)

式中：和為總推力幅值的上下限；max和max為最大推力方向角。

此外，考慮到火箭子級發(fā)動機推力調(diào)節(jié)能力有限，還需要對推力變化率施加約束

(8)

另一方面，由于著陸段起始高度較低，火箭子級飛行速度相對較小，而空氣密度較大，因此僅考慮動壓作為過程約束

(9)

選擇燃料消耗最少即終端剩余質(zhì)量最大為性能指標(biāo)

=-()

(10)

綜上所述，可重復(fù)使用運載火箭子級著陸段軌跡優(yōu)化問題可描述為

問題0：

(11)

問題0是一個連續(xù)的終端時間自由的最優(yōu)控制問題，其優(yōu)化變量為=[(),(),]，對應(yīng)的最優(yōu)控制量為=[(),(),]。

2 帶有最優(yōu)終端時間估計的軌跡優(yōu)化算法

2.1 問題凸化

無損凸化

針對問題0中非凸形式的動力學(xué)方程和約束，無法直接應(yīng)用凸優(yōu)化方法求解。本節(jié)主要考慮通過無損凸化將非凸問題轉(zhuǎn)化為凸問題。

首先針對非凸推力幅值約束，引入變量，則式(6)可以轉(zhuǎn)化為

≤≤

(12)

(13)

基于凸優(yōu)化理論和問題的具體形式，對式(13)進行松弛變換，變換后的約束為

(14)

關(guān)于這一凸化變換無損性的證明可參考文獻[12]和文獻[14]，本文中直接應(yīng)用結(jié)論，則動力學(xué)方程可變換為

(15)

基于上述變換，問題0中控制變量可增廣變換為=[,]，定義變換后的問題為問題1，具體形式如下

問題1：

(16)

進一步對上述動力學(xué)方程中的非線性項進行處理，作如下變量替換

(17)

變換后的動力學(xué)方程如下

(18)

相應(yīng)地，推力約束和目標(biāo)函數(shù)變換為

·e-≤≤·e-

(19)

(20)

=min-()

(21)

根據(jù)凸優(yōu)化和解析幾何理論，易知式(20)所定義的可行域為二階錐，是一種典型的凸約束。

同倫方法

對于式(18)中的非凸氣動力項和式(19)所表示的非凸約束，采用同倫方法(Homotopy approach)對其進行凸化。在數(shù)學(xué)上，兩個拓撲空間如果可以通過一系列連續(xù)變化從一個變到另一個，那么就稱這兩個拓撲空間同倫。對于函數(shù)而言，若存在連續(xù)映射(,)，使得連續(xù)函數(shù)(),()滿足

(22)

則稱()與()同倫，(,)為同倫映射，為同倫因子，由上式可知，隨著同倫因子從0變化到1，函數(shù)()逐漸延拓為()。

值得注意的是，應(yīng)用同倫方法的前提條件是同倫過程中不改變系統(tǒng)的性質(zhì)，下面對本問題中同倫方法的適用性進行簡要分析。

不同于以氣動力作為主要控制力的滑翔再入飛行器，垂直回收運載火箭的主要控制力為火箭發(fā)動機推力。此外，在垂直回收過程中火箭子級飛行速度相對較小，氣動力相對發(fā)動機推力要小很多，本文采用的仿真參數(shù)對應(yīng)的作用力曲線如圖2所示。

圖2 飛行過程中所受作用力幅值Fig.2 Magnitudes of the acting forces during flight

從圖中可以看出，氣動力小于推力，且由于推力對飛行速度的主要控制作用，氣動力曲線也在一定程度上受到了推力曲線的影響。

通過上述分析可知，氣動力為火箭子級的非主要控制力，可以應(yīng)用同倫方法凸化問題。同倫凸優(yōu)化算法的具體設(shè)計過程如下。

首先不考慮氣動力的影響，即在式(18)中刪去氣動力加速度項，基于文獻[11]中的處理方式對式(19)進行Taylor展開，將其變換為

·e-[(1-(-)+(-)2)]≤

≤·e-[(1-(-)]

(23)

由式(23)可得，不等式左側(cè)為二階錐約束,右側(cè)為線性約束，對于凸化后的問題應(yīng)用原-對偶內(nèi)點法(Primal-dual interior point method, PDIPM)求解凸優(yōu)化問題獲得初始解。隨后，在動力學(xué)方程中引入同倫因子，將動力學(xué)方程、推力約束以及過程約束變換為

(24)

·e-≤≤·e-

(25)

(26)

(27)

式中：

需要說明的是，上述狀態(tài)空間表達式中的系數(shù)矩陣與控制矩陣在同倫過程中為線性時變剖面形式，具體取值由上一次優(yōu)化結(jié)果決定。

綜上所述，凸化后的問題可以描述為

問題2：

(28)

2.2 問題離散

本文采用flipped Radau偽譜法(flipped Radau pseudospectral method, FRPM)離散連續(xù)最優(yōu)控制問題2，將離散后的參數(shù)優(yōu)化問題記為問題3。FRPM是一種非對稱的多區(qū)間偽譜法，不同于Radau偽譜法(RPM)，F(xiàn)RPM將終端時間設(shè)置為配點，可以更好地定義終端約束。

在FRPM中，首先定義新的時間變量，將時間區(qū)間從[0,]映射到[-1,1]，定義仿射變換關(guān)系如下

(29)

然后選取個Flipped Legendre Gauss Radau(FLGR)配點以及=-1作為離散點，故在∈[-1,1]上共有個配點和+1個離散點。定義階Lagrange多項式()為如下形式

(30)

相應(yīng)地，狀態(tài)量和控制量可以近似為

(31)

對上式中的狀態(tài)量求導(dǎo)即可得到狀態(tài)量微分的近似形式

(32)

因此動力學(xué)方程的離散形式為

=1,2,…,

(33)

式中：∈×(+1)為FRPM微分矩陣。此外，目標(biāo)函數(shù)為Mayer型性能指標(biāo)，其離散形式為

=[(1)]

(34)

相應(yīng)地，初始和終端約束變換為

(35)

推力約束變換為

·e-≤≤·e-,?∈[-1,1]

(36)

(37)

(38)

(39)

過程約束變換為

(40)

進一步地，為加快求解速度、提升求解效率，采用hp偽譜法代替全局偽譜法對問題進行離散化處理。本文為簡化問題，僅考慮子區(qū)間個數(shù)h和配點個數(shù)p為常值的情況。采用hp-FRPM離散軌跡優(yōu)化問題增加了雅各比矩陣的稀疏性，加快了軌跡優(yōu)化問題的求解速度。

在本文中用上標(biāo)表示第個子區(qū)間，下標(biāo)表示子區(qū)間內(nèi)的第個離散點，則動力學(xué)方程式(33)的hp離散形式為

(41)

此外，考慮各子區(qū)間之間的連接條件

(42)

綜上所述，采用hp-FRPM離散后的參數(shù)優(yōu)化問題可描述為

問題3：

(43)

2.3 最優(yōu)終端時間快速估計

現(xiàn)有的無損凸化方法在確定最優(yōu)終端時間方面研究較少，多將設(shè)置為固定值，在一定程度上對求解結(jié)果施加了限制條件，不能完全保證求解結(jié)果的最優(yōu)性。

但是，若直接將納入軌跡優(yōu)化問題框架則會破壞原問題良好的收斂特性，難以應(yīng)用無損凸化求解。另一方面，若采用傳統(tǒng)的線性搜索算法對尋優(yōu)則最優(yōu)解區(qū)間的上下界難以確定，導(dǎo)致搜索算法收斂速度慢，難以在線應(yīng)用。

基于上述分析，本文基于最優(yōu)控制理論和垂直起降可重復(fù)使用運載火箭的運動學(xué)特性,采用解析方法估計包含最優(yōu)解區(qū)間的上下界，進一步設(shè)計二次插值型搜索策略完成最優(yōu)終端時間的快速估計。

(44)

(45)

式中：為切換系數(shù)。

進一步地，由火箭子級動力學(xué)式(1)中第二式可得，不考慮氣動力時火箭子級加速度幅值為

(46)

雖然火箭子級加速度幅值為時變值，難以分析其具體變化趨勢，但結(jié)合式(45)中的燃料最優(yōu)控制形式和火箭子級變質(zhì)量飛行特性，可以通過定性分析式(46)得出，在垂直回收過程中加速度總體呈增大趨勢，終端時刻加速度最大。同時，根據(jù)終端時刻火箭子級姿態(tài)約束和最優(yōu)控制理論可知，終端時刻火箭子級推力方向沿著陸點坐標(biāo)系軸方向且幅值為。通過上述分析可得，火箭子級終端加速度具體形式為

(47)

對于上式中的終端質(zhì)量，結(jié)合動力學(xué)方程式(1)中的第三式和燃料最優(yōu)控制問題解式(45)估計其下界

(48)

則終端加速度的上界為

(49)

至此，完成終端加速度上界的解析估計。另一方面，由于火箭子級初始位置和速度已知，根據(jù)運動學(xué)關(guān)系，構(gòu)建多項式函數(shù)來估計速度，進而求解最優(yōu)終端時間上下界，原理如圖3所示。

圖3 最優(yōu)終端時間上下界解析多項式估計Fig.3 Analytic polynomial estimation of upper and lower bounds of optimal terminal time

(50)

(51)

由上式可知，終端時刻火箭子級加速度的估計值為

(52)

進一步根據(jù)運動學(xué)公式，速度曲線對時間的積分為位置，即

(53)

將式(50)代入上式求解可得

(54)

(55)

(56)

綜上所述，帶有最優(yōu)終端時間快速估計策略的火箭垂直回收hp偽譜同倫凸優(yōu)化算法流程如圖4所示。

圖4 算法流程Fig.4 Flow chart of the algorithm

3 仿真校驗

本節(jié)通過數(shù)值仿真對提出的帶有最優(yōu)終端時間快速估計策略的hp偽譜同倫凸優(yōu)化算法進行驗證，并將其與現(xiàn)有的軌跡優(yōu)化算法進行對比，分析該軌跡優(yōu)化算法的求解精度、計算時間以及收斂性能。

本文使用的仿真參數(shù)及過程約束見表1和表2，火箭子級初始位置和速度設(shè)置為=[-2700,9800,0]m、=[180,-400,0]m/s。所有程序的仿真環(huán)境均為Intel Core i7-7700HQ、2.80 GHz和Windows 10操作系統(tǒng)的筆記本電腦，仿真軟件為Matlab，對于生成的凸問題調(diào)用Mosek 9.0工具包采用原始對偶內(nèi)點法進行求解。

3.1 最優(yōu)終端時間快速估計策略仿真驗證

表1 火箭子級仿真參數(shù)Table 1 Simulation parameters of the launch vehicle stages

表2 約束邊界值Table 2 Boundary values of constraints

由于偽譜法將終端時間納入優(yōu)化框架，通過求解軌跡優(yōu)化問題來獲得最優(yōu)終端時間，因此可以將偽譜法的求解結(jié)果作為基準(zhǔn)。從表3中可以看出，四種方法的求解結(jié)果相差不大。進一步對比求解耗時，偽譜法求解時間明顯較長。通過對比文獻[21]方法和改進文獻[21]方法的求解時間可以得出，采用本文的上下界估計方法可以有效縮小估計區(qū)間，加快計算速度，提升計算效率。本文方法的求解時間為1.58 s，遠小于其余三種方法的求解時間，說明了采用二次插值法具有超線性收斂的特性，相較于基于線性收斂準(zhǔn)則的二分法，收斂速度大幅提升。同時本文方法的求解結(jié)果相較于基準(zhǔn)結(jié)果誤差較小，實現(xiàn)了具備一定精度的快速估計，適合在線應(yīng)用。

表3 算法性能對比Table 3 Performance comparison of algorithms

3.2 算法仿真分析及最優(yōu)性驗證

本節(jié)仿真以國內(nèi)外廣泛應(yīng)用的精度高、適用性強的GPOPS Ⅱ軟件包的求解結(jié)果作為基準(zhǔn)，對提出的hp偽譜同倫凸優(yōu)化算法即hp-PHCP算法的最優(yōu)性進行驗證?；贕POPS Ⅱ軟件包的求解程序采用hp網(wǎng)格自適應(yīng)更新策略，調(diào)用經(jīng)典的稀疏非線性求解器(SNOPT)求解轉(zhuǎn)化后的非線性規(guī)劃問題。hp-PHCP算法中總離散點個數(shù)為81，其中子區(qū)間個數(shù)=4，配點個數(shù)=20，飛行終端時間采用3.1節(jié)中的優(yōu)化結(jié)果，同倫參數(shù)設(shè)置為=04。上述兩個算法的迭代終止條件參數(shù)均為=10。為表示方便，在圖例中將利用GPOPS Ⅱ軟件包求解記為原算法，將hp-PHCP算法記為本文算法，仿真結(jié)果如圖5至圖13所示。

圖5 hp-PHCP求解SOCP問題時間Fig.5 Solving time of SOCP problem using hp-PHCP

圖6 動壓-時間曲線Fig.6 Curve of dynamic pressure-time

圖7 推力擺角-時間曲線Fig.7 Curve of thrust swing angle-time

圖8 推力幅值迭代過程Fig.8 Iteration history of thrust amplitude

圖9 位置-時間曲線Fig.9 Curve of position-time

圖10 速度-時間曲線Fig.10 Curve of velocity-time

圖11 質(zhì)量-時間曲線Fig.11 Curve of mass-time

圖12 推力分量-時間曲線Fig.12 Curve of thrust component-time

圖13 推力幅值-時間曲線Fig.13 Curve of thrust amplitude-time

根據(jù)仿真結(jié)果，hp-PHCP算法共迭代6次完成收斂，求解二階錐規(guī)劃(SOCP)問題總時間為1.011 s，每次求解時間如圖5所示。從圖中可以看出，hp-PHCP算法求解SOCP問題的平均時間約為0.169 s，最大求解時間為0.347 s，出現(xiàn)在求解迭代啟動解中，這是由于啟動解求解過程無需參考軌跡，采用全局尋優(yōu)造成了時間較長。從圖6至圖8可以看出，求解結(jié)果滿足動壓約束、推力幅值和方向約束。特別地，從圖8的局部放大圖可知，在前3次迭代中推力切換時間跳變較大，且推力幅值約束并不滿足。這是由于進行了控制量和質(zhì)量的變量替換，而氣動力尚未完全加入動力學(xué)系統(tǒng)，在同倫過程中以上次優(yōu)化結(jié)果為基準(zhǔn)，但質(zhì)量剖面與氣動力剖面的近似精度不足，導(dǎo)致變換后的推力幅值約束近似精度下降。這一問題在氣動力完全引入動力學(xué)方程中得到了顯著改善，從圖8的局部放大圖可以看出，在第四次迭代后，同倫參數(shù)=1即完成氣動力引入，總推力曲線迅速收斂，推力幅值約束得到了滿足。從圖9至圖13可以看出，hp-PHCP算法與GPOPS優(yōu)化軟件包求得的狀態(tài)變量曲線基本重合，控制變量曲線相差不大，因此可以說明，提出的hp-PHCP算法具備一定的局部最優(yōu)性。特別地，從圖12可以看出，由于推力方向和變化率約束的存在，分推力曲線并不平滑，然而狀態(tài)變量曲線平滑，這表明了hp-PHCP算法在嚴(yán)格滿足控制約束的同時實現(xiàn)了平穩(wěn)飛行。此外，從圖13還可以看出，松弛變量與總推力曲線完全重合，進一步驗證了凸化過程的無損性。

3.3 算法性能對比分析

為進一步分析本文提出hp-PHCP算法的性能，采用對比分析的方法對三種不同離散方法的軌跡優(yōu)化算法進行數(shù)值仿真。第一種是采用基于梯形法離散的同倫凸優(yōu)化算法(THCP)，第二種是基于全局flipped Radau偽譜法離散的同倫凸優(yōu)化算法(PHCP)，第三種是本文提出的基于hp flipped Radau偽譜法離散的同倫凸優(yōu)化算法(hp-PHCP)。三種算法的總離散點數(shù)均為81，飛行終端時間采用3.1節(jié)中的優(yōu)化結(jié)果，同倫參數(shù)設(shè)置為=04。其中hp-PHCP算法中子區(qū)間個數(shù)=2，配點個數(shù)=40。仿真結(jié)果如表4和圖14至圖18所示。

表4 三種算法優(yōu)化結(jié)果對比Table 4 Optimization results comparison of three algorithms

圖14 位置-時間曲線Fig.14 Curve of position-time

圖15 速度-時間曲線Fig.15 Curve of velocity-time

圖16 質(zhì)量-時間曲線Fig.16 Curve of mass-time

圖17 推力分量-時間曲線Fig.17 Curve of thrust component-time

圖18 推力幅值-時間曲線Fig.18 Curve of thrust amplitude-time

從圖14至圖18可以看出，三種軌跡優(yōu)化算法求解得到的狀態(tài)變量和控制變量曲線基本重合，總推力曲線呈現(xiàn)出明顯的Bang-Bang特性，且切換過程平滑無吉布斯(Gibbs)現(xiàn)象。此外，三種軌跡優(yōu)化算法均在第6次迭代時完成收斂，收斂性能良好，收斂速度快。

表4給出了三種軌跡優(yōu)化算法優(yōu)化結(jié)果對比，作為以在線應(yīng)用為目標(biāo)的軌跡優(yōu)化算法，求解精度和計算速度是衡量算法優(yōu)劣的重要標(biāo)準(zhǔn)。三種算法中THCP求解時間最短，但高度和速度的終端誤差大；而PHCP算法雖然計算精度最高，但其求解時間長。相較于THCP和PHCP，本文提出的hp-PHCP算法終端誤差與PHCP算法為同一數(shù)量級，但是求解時間不到PHCP算法的一半，在保證一定求解精度的條件下有效提升了算法的計算速度，使其具備了在線應(yīng)用的潛力。

4 結(jié) 論

面向可重復(fù)使用運載火箭子級回收問題，提出了一種帶有最優(yōu)終端時間估計策略的hp偽譜同倫凸優(yōu)化在線軌跡規(guī)劃算法。取得的主要成果如下：

1) 最優(yōu)終端時間快速估計策略能夠快速估計最優(yōu)終端時間，保證了開機-終端時刻組合的最優(yōu)性，同時能夠最大限度實現(xiàn)燃料消耗最小，進一步提升運載火箭的經(jīng)濟效益。

2) hp偽譜同倫凸優(yōu)化軌跡優(yōu)化算法(hp-PHCP)能夠有效處理非凸約束，無需參考軌跡，分推力指令平滑連續(xù)，收斂性能良好，收斂速度快，能夠保證求解結(jié)果的最優(yōu)性。

3) 相較于THCP算法和PHCP算法，hp-PHCP算法基于hp多階段偽譜離散策略，進一步提升了軌跡優(yōu)化問題的稀疏性，在保證求解精度的同時有效縮短了計算時間，具備在線應(yīng)用潛力。