王洪波，張若男，梁 卓，呂 瑞

（中國運載火箭技術(shù)研究院，北京 100076）

耗盡關(guān)機(jī)固體運載火箭的推力大小和工作時間無法主動控制，如何利用有限速度調(diào)節(jié)能力實現(xiàn)高精度入軌，是當(dāng)前國內(nèi)外固體火箭制導(dǎo)領(lǐng)域研究熱點。凸優(yōu)化算法有確定收斂性、求解快速的優(yōu)點，能夠滿足多飛行階段固體運載火箭的在線規(guī)劃和制導(dǎo)要求。

文獻(xiàn)[1][2]利用凸優(yōu)化方法解決火箭上升動力段入軌制導(dǎo)問題，其發(fā)動機(jī)推力可調(diào)，末端關(guān)機(jī)時間可控；文獻(xiàn)[3]使用凸優(yōu)化方法尋找火箭上升段推力下降故障時最優(yōu)圓軌，但其只作為后續(xù)迭代初值，未嚴(yán)格考慮終端約束；文獻(xiàn)[4][5]針對運載器回收制導(dǎo)問題，構(gòu)建多階段二階錐規(guī)劃模型，利用序列凸化算法離散求解；文獻(xiàn)[6]建立了有限推力遠(yuǎn)程軌道轉(zhuǎn)移問題的凸優(yōu)化模型，利用迭代逼近驗證凸優(yōu)化算法在遠(yuǎn)程軌跡規(guī)劃問題中的可行性。

基于此，本文研究耗盡關(guān)機(jī)固體火箭入軌制導(dǎo)問題，建立含推力方向和終端入軌參數(shù)約束的“滑行段+動力段”運動模型。使用拉道（Legendre–Gauss–Radau，LGR）偽譜法離散滑行段運動模型，將點火時間增廣為新的控制變量；將推力方向非凸球面可行域松弛為凸球體可行域。并從理論上證明松弛的無損性，同時對運動方程和終端等式約束進(jìn)行線性化，構(gòu)建凸優(yōu)化子問題；利用含初值生成器的序列凸優(yōu)化算法迭代逼近原問題，得到最優(yōu)滑行時間和最優(yōu)推力矢量，實現(xiàn)耗盡關(guān)機(jī)固體火箭動力段能量管理，最終高精度入軌。

1 固體火箭多階段入軌制導(dǎo)模型

1.1 最優(yōu)控制問題描述

本文所研究的固體子火箭入軌制導(dǎo)問題，飛行高度大于100 km，處于大氣層外，不考慮氣動影響[7]。動力段發(fā)動機(jī)推力為常值，火箭推力方向與箭體縱軸重合，姿態(tài)控制視作理想環(huán)節(jié)；飛行采用“滑行+動力”方案，在地心慣性系下建立的三自由度運動模型：

式中，狀態(tài)量r、v、m分別為坐標(biāo)系下位置、速度和質(zhì)量；g為火箭重力加速度：

Isp為發(fā)動機(jī)比沖；T為發(fā)動機(jī)推力，滑行段發(fā)動機(jī)推力為零，動力段發(fā)動機(jī)推力為不可調(diào)常值[8]，u為推力方向單位矢量：

設(shè)滑行段開始時間為t0=0，滑行段結(jié)束（動力段開始）時間為ts，終端時間為tf。

目標(biāo)軌道為圓軌道，發(fā)動機(jī)燃料耗盡關(guān)機(jī)時，火箭需滿足目標(biāo)軌道約束：

其中，Rf、Vf、If分別為目標(biāo)軌道地心距、軌道速度和軌道傾角。

實際飛行中，長時間滑行中姿態(tài)調(diào)整將給姿控系統(tǒng)帶來不利影響，因此選擇飛行時間作為需要優(yōu)化的性能指標(biāo)：

綜上，固體火箭入軌段最優(yōu)控制問題為式(1)-(5)。

該入軌制導(dǎo)問題分為滑行與動力兩階段，各段內(nèi)狀態(tài)與控制量變化連續(xù)。由于滑行段時間較長，為了獲得較高的離散精度，采用收斂速度較快的LGR 偽譜法離散[9]；動力段時間相對較短，采用一階保持器的等距離散。

1.2 滑行段離散建模

LGR 偽譜法配點定義在[-1，+1)之間，將原問題時域映射至[-1，+1)區(qū)間內(nèi)，如式(6):

式中，τi(i=1...N1)是[-1，+1)上的N1階Legendre多項式零點。τs=+1 和τi(i=1...N1)構(gòu)成N1+1個插值點。

為表述方便，以τ為變量的狀態(tài)量與控制量仍舊使用x、u表示，狀態(tài)量及其導(dǎo)數(shù)的插值近似為：

式中Li(i=1...N1+1)為Lagrange 插值基函數(shù)。

滑行段動力學(xué)方程離散模型如下：

式中，D為N1×(N1+1)階微分矩陣，f(x(τk),u(τk))為動力學(xué)微分方程(1)的右端函數(shù)，其中發(fā)動機(jī)推力T=0。

滑行段初始狀態(tài)已知：

1.3 動力段離散建模

動力段設(shè)置N2個離散點，離散時間間隔為：

采用一階保持器的動力段均勻離散模型如下[10]：

動力階段有式(3)所示推力矢量約束。飛行過程中狀態(tài)量連續(xù)，階段間連接約束為：

因為火箭發(fā)動機(jī)耗盡關(guān)機(jī)，終端約束除式(4)的入軌條件外還有終端確定質(zhì)量約束：

通過上述分段離散，控制量加入發(fā)動機(jī)開機(jī)時間ts，解決了終端時間不確定問題，將經(jīng)過離散后的模型稱為問題1。

2 推力方向約束無損凸化

問題1 中動力段推力大小恒定，推力矢量等式約束為球面非凸可行域，對該約束進(jìn)行無損凸化，即將問題的非凸可行域進(jìn)行適當(dāng)松弛放大構(gòu)成新的凸可行域，針對本文形成的球面可行域進(jìn)行如下松弛變換：

僅當(dāng)式(14)取等號時該約束為積極約束，與原問題等價。通過龐特里亞金提出的極大值原理可證明，松弛后問題最優(yōu)解仍滿足原問題可行域約束，即松弛后子問題最優(yōu)控制分量時刻滿足。

本文所研究的固體運載火箭，其推力大小不可調(diào)節(jié)，動力段質(zhì)量變化與狀態(tài)量和控制量無關(guān)，在每個制導(dǎo)周期內(nèi)質(zhì)量為常量，在整個動力段線性時變[11～13]，因此證明中將質(zhì)量看作常量，同時需要增加動力段時長等式約束,ΔTburn為動力段發(fā)動機(jī)工作時長。

模型描述：

證明：

松弛后最優(yōu)控制問題的哈密頓函數(shù)：

最優(yōu)控制問題存在終端狀態(tài)和時間約束，終端函數(shù)為：

協(xié)態(tài)方程：

極大值條件：

控制約束松弛條件：

由于終端時間受約束，哈密頓函數(shù)：

根據(jù)終端函數(shù)可知哈密頓函數(shù)跳變條件：

將式(22)中兩式相加，并結(jié)合式(25)可得：

又因該最優(yōu)控制問題末端時間自由，終端時刻哈密頓函數(shù)為零，即：

由式(26)可知：

3 序列凸優(yōu)化算法

問題1 非凸性除了推力分量約束形成的球面可行域外，還有非線性動力學(xué)方程和終端入軌非凸等式約束。利用線性化方法，問題1 將轉(zhuǎn)化成二階錐規(guī)劃問題，繼而可利用凸優(yōu)化算法求解。

3.1 動力學(xué)方程線性化

針對非線性動力學(xué)方程中的非凸約束，通過在上一次迭代結(jié)果處進(jìn)行一階泰勒級數(shù)展開，將其轉(zhuǎn)化成凸約束，通過迭代，結(jié)果將逐漸逼近原問題最優(yōu)解[14]。

問題1 中滑行段動力學(xué)方程左端矩陣D是常數(shù)矩陣，將右端增廣非線性函數(shù)進(jìn)行一階泰勒級數(shù)展開：

動力段運動方程進(jìn)行一階泰勒級數(shù)展開，形式與滑行段類似。

3.2 終端約束線性化

運載火箭入軌時終端約束為關(guān)于狀態(tài)的非線性非凸等式約束，在終端時刻對式(4)進(jìn)行一階泰勒級數(shù)展開：

3.3 序列凸優(yōu)化問題及其求解

問題1 經(jīng)過線性化和無損松弛，得到序列二階錐規(guī)劃問題：

1）松弛因子

經(jīng)過上述凸化后的子問題，稱為問題2，在迭代求解初期，由于線性化產(chǎn)生的偏差，可能會造成“偽不可行”，即原問題存在可行解，但線性化后問題不可行。因此對問題2 中動力學(xué)方程（以滑行段為例）和終端約束添加松弛因子[15]：

2）信賴域

利用序列凸優(yōu)化算法對問題2 迭代求解時，每一次都選擇上一次迭代結(jié)果作為本次泰勒展開參考點，因此前后兩次求解結(jié)果必須滿足小偏差約束，否則會導(dǎo)致子問題線性化帶來誤差過大從而迭代失敗。引入第i次和第i+1 次變量迭代結(jié)果的信賴域約束：

3）初值生成

迭代開始需提供初始狀態(tài)序列，良好的初始軌跡有利于迭代的快速收斂[16,17]。本文所研究的入軌問題末端狀態(tài)受等式約束，設(shè)計如下初值生成策略：首先選擇點火時間初值，在動力段，令俯仰角為時間的線性函數(shù)，偏航角恒定，得到姿態(tài)角曲線為：

4 仿真校驗

選擇某固體運載火箭的第三級子火箭作為仿真對象，使用改進(jìn)序列凸優(yōu)化算法和LGR 偽譜法，對比說明本文所提出的改進(jìn)序列凸優(yōu)化算法的有效性和高效率。火箭發(fā)動機(jī)參數(shù)如表1所示，地心慣性系下初始狀態(tài)和終端入軌約束如表2所示，目標(biāo)軌道為圓軌。

表1 火箭參數(shù)Tab.1 The rocket parameters

表2 初始狀態(tài)和終端約束Tab.2 Initial state and terminal constraints

本文數(shù)值計算在i7 3.6 GHz 筆記本電腦下進(jìn)行，程序均在MATLAB 2014a 環(huán)境下編譯運行，采用CVX工具箱和SDPT3 求解器進(jìn)行解算。

4.1 仿真結(jié)果

滑行段設(shè)置40 個LGR 離散點，動力段設(shè)置20個離散點，火箭滑行段時長作為性能指標(biāo)，優(yōu)化得到滑行時間為160.2 s，動力飛行50.0 s。

圖1 滑行+動力段三維軌跡Fig.1 Three dimensional trajectory of sliding and power segments

圖2 地心距隨時間變化曲線Fig.2 Geocentric distance curve over time

圖3 速度隨時間變化曲線Fig.3 Velocity curve over time

圖1是改進(jìn)序列凸優(yōu)化算法迭代得到的三維軌跡，圖2、3分別為各次迭代得到的飛行過程中地心距和速度變化曲線，最終得到的滿足收斂誤差閾值的軌跡在圖中標(biāo)出。火箭耗盡關(guān)機(jī)時，滿足終端軌道約束。

圖4 松弛量變化曲線Fig.4 Relaxation curve

圖5 推力方向模值變化曲線Fig.5Thrust vector modulus curve

圖4中松弛因子隨迭代次數(shù)增加趨近于0，最終小于10-9，說明序列凸化求解結(jié)果與原問題最優(yōu)解等價性。圖5為動力段推力方向模值，可以發(fā)現(xiàn)始終滿足，驗證結(jié)論1。

4.2 求解精度與效率分析

使用本文提出的初值生成器，耗時0.12s構(gòu)建初始軌跡，得到的初始軌跡滑行時長為155.72s，終端地心距誤差為0.6km，終端入軌速度誤差為21.76m/s，終端當(dāng)?shù)厮俣葍A角誤差為0.056 rad，終端軌道傾角誤差為0.0071rad。改進(jìn)序列凸優(yōu)化算法優(yōu)化后的終端地心距誤差為0.22m，速度誤差為-0.063m/s，當(dāng)?shù)厮俣葍A角誤差為0.00047rad，軌道傾角誤差為-0.00023rad，火箭成功入軌，驗證本文提出的改進(jìn)序列凸化方法的可行性。

圖6-7為采用序列凸優(yōu)化算法和偽譜法求解得到地心距、速度變化曲線對比，兩種方法計算結(jié)果吻合，說明本文所提的改進(jìn)序列凸優(yōu)化算法在實現(xiàn)高精度入軌上的有效性。

圖6 凸優(yōu)化方法和偽譜法地心距變化Fig.6 Geocentricvariation by convex optimization and pseudospectral method

圖7 凸優(yōu)化方法和偽譜法速度變化Fig.7 Velocity variation by convex optimization and pseudo spectralmethod

圖8 凸優(yōu)化方法和偽譜法控制分量變化Fig.8 Thrust vector by convex optimization and pseudo spectralmethod

圖8為分別采用初值生成器、凸優(yōu)化方法和偽譜法得到的發(fā)射慣性坐標(biāo)系下動力段發(fā)動機(jī)推力方向曲線，可以發(fā)現(xiàn)凸優(yōu)化方法和偽譜方法仿真得到結(jié)果基本吻合。

采用改進(jìn)序列凸優(yōu)化算法經(jīng)過5次迭代結(jié)果收斂，每次迭代中求解凸優(yōu)化子問題單步CPU 時間平均為0.16 s，模型初始化到得到收斂結(jié)果共耗時7.96 s，而偽譜法耗時118.42 s，改進(jìn)序列凸優(yōu)化算法計算效率相較于LGR 偽譜法,求解時間縮短了93.2%，具有顯著優(yōu)勢。

5 結(jié)論

本文針對耗盡關(guān)機(jī)固體運載火箭入軌段制導(dǎo)問題，提出一種改進(jìn)序列凸優(yōu)化算法，以滑行段時長為優(yōu)化指標(biāo)，規(guī)劃點火時間和動力段推力方向，實現(xiàn)火箭耗盡關(guān)機(jī)時精確入軌，通過仿真驗證得到主要結(jié)論如下：

利用LGR 偽譜法進(jìn)行時域映射，將發(fā)動機(jī)開機(jī)時間作為新的控制變量，解決了點火時間不確定問題，保證了滑行時長的最優(yōu)性。

針對推力方向非凸球面約束，通過松弛轉(zhuǎn)化為凸的球體約束，并在理論上證明松弛手段的無損性；將非凸的動力學(xué)方程和終端狀態(tài)等式約束在參考軌跡上進(jìn)行線性化，從而將其轉(zhuǎn)化為凸約束，利用序列凸優(yōu)化算法，結(jié)合松弛因子和信賴域等迭代策略，使構(gòu)建的凸優(yōu)化子問題最優(yōu)解逼近原問題最優(yōu)解。

含初始軌跡生成器的序列凸優(yōu)化算法求解本文研究的入軌制導(dǎo)問題時間僅為偽譜法的6.72%，解算結(jié)果終端地心距誤差為0.22 m，速度誤差-0.063 m/s，當(dāng)?shù)厮俣葍A角誤差為 0.00047 rad，軌道傾角誤差為-0.00023 rad，驗證本文所提出的改進(jìn)序列凸優(yōu)化方法解決耗盡關(guān)機(jī)固體火箭入軌制導(dǎo)問題的有效性。