基于改進(jìn)序列凸優(yōu)化的固體火箭入軌制導(dǎo)方法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1.2 滑行段離散建模

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

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

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

式中Li(i=1...N1+1)為L(zhǎng)agrange 插值基函數(shù)。

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

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

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

1.3 動(dòng)力段離散建模

動(dòng)力段設(shè)置N2個(gè)離散點(diǎn)，離散時(shí)間間隔為：

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

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

因?yàn)榛鸺l(fā)動(dòng)機(jī)耗盡關(guān)機(jī)，終端約束除式(4)的入軌條件外還有終端確定質(zhì)量約束：

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

2 推力方向約束無(wú)損凸化

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

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

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

模型描述：

證明：

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

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

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

極大值條件：

控制約束松弛條件：

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

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

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

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

由式(26)可知：

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

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

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

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

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

動(dòng)力段運(yùn)動(dòng)方程進(jìn)行一階泰勒級(jí)數(shù)展開(kāi)，形式與滑行段類似。

3.2 終端約束線性化

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

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

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

1）松弛因子

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

2）信賴域

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

3）初值生成

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

4 仿真校驗(yàn)

選擇某固體運(yùn)載火箭的第三級(jí)子火箭作為仿真對(duì)象，使用改進(jìn)序列凸優(yōu)化算法和LGR 偽譜法，對(duì)比說(shuō)明本文所提出的改進(jìn)序列凸優(yōu)化算法的有效性和高效率。火箭發(fā)動(dòng)機(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ù)值計(jì)算在i7 3.6 GHz 筆記本電腦下進(jìn)行，程序均在MATLAB 2014a 環(huán)境下編譯運(yùn)行，采用CVX工具箱和SDPT3 求解器進(jìn)行解算。

4.1 仿真結(jié)果

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

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

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

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

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

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

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

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

4.2 求解精度與效率分析

使用本文提出的初值生成器，耗時(shí)0.12s構(gòu)建初始軌跡，得到的初始軌跡滑行時(shí)長(zhǎng)為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，火箭成功入軌，驗(yàn)證本文提出的改進(jìn)序列凸化方法的可行性。

圖6-7為采用序列凸優(yōu)化算法和偽譜法求解得到地心距、速度變化曲線對(duì)比，兩種方法計(jì)算結(jié)果吻合，說(shuō)明本文所提的改進(jìn)序列凸優(yōu)化算法在實(shí)現(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)系下動(dòng)力段發(fā)動(dòng)機(jī)推力方向曲線，可以發(fā)現(xiàn)凸優(yōu)化方法和偽譜方法仿真得到結(jié)果基本吻合。

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

5 結(jié)論

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

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

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

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