为了简化算法，在当前迭代点 x k ，定义

f k : = f ( x k ) , h k : = h ( x k ) , g k : = g ( x k ) , v k : = v ( x k )

其中 v ( x ) = ‖ h ( x ) ‖ 1 + ‖ [ g ( x ) ] + ‖ 1 为约束违反度函数。对于给定的方向 d ∈ R n ，定义 v ( x ) 在 x k 处沿d的线性约束违反度为

l k v ( d ) : = ‖ h k + D h ( x k ) d ‖ 1 + ‖ [ g k + D g ( x k ) d ] + ‖ 1

其中D为雅可比算子。那么，目标函数 f ( x ) 在 x k 沿着方向d的线性近似可以表示为

l k f ( d ) : = f k + ∇ f ( x k ) T d

分别设目标函数和约束违反度的沿着方向d的线性化改善量为

∇ l k f ( d ) : = l k f ( 0 ) − l k f ( d ) = − ∇ f ( x k ) T d , ∇ l k v ( d ) : = l k v ( 0 ) − l k v ( d )

在以上记号下，算法首先考虑一个线性规划问题：

min d , r , s , t e T ( r + s ) + e T t s . t . h k + D h ( x k ) d = r − s g k + D g ( x k ) d ≤ t r , s , t ≥ 0 ,       ‖ d ‖ ∞ ≤ Δ (2)

其中 Δ > 0 为较大的常数，用来保证子问题(2)的有界性。记子问题(2)的解为 ( d k f e a , r k , s k , t k ) 。

取n阶正定矩阵 B k ，算法接着求解下面的二次规划问题：

min d ∈ R n ∇ f ( x k ) T d + 1 2 d T B k d s . t . h k + D h ( x k ) d = r k − s k g k + D g ( x k ) d ≤ t k (3)

记(3)的解为 d k 。设 ( μ k + 1 , Y k + 1 ) 为等式约束和不等式约束对应的拉格朗日乘子。

如果算法在 x k 处未终止，我们将沿着 d k 方向在 x k 处做线搜索。即选择搜索步长 α k ∈ ( 0 , 1 ] 并记下一迭代点为 x k + 1 = x k + α k d k 。搜索步长 α k 的接受准则中不涉及罚因子，具体描述如下。首先考虑条件

Δ l k f ( α d k ) ≥ δ Δ l k v ( α d k ) τ (4)

其中 δ 是大于零的常数， τ > 1 。如果条件(4)成立，则将优先考虑提高最优性，即要求目标函数有充分的下降。此时进一步要求搜索步长满足

f ( x k ) − f ( x k + α d k ) ≥ η f Δ l k f ( α d k ) , η f ∈ ( 0 , 1 ) (5)

同时还要求新的迭代点的约束违反度在可控范围内，即

v ( x k + α d k ) ≤ v k max (6)

其中 v k max 是约束违反度 v ( x ) 的一个上界。若条件 ， 和 均成立，记 x k + 1 = x k + α k d k 。此时称第 ( k + 1 ) 次迭代为f型迭代。在f型迭代中， v k + 1 max 保持不变，即 v k + 1 max = v k max 。

若条件(4)不成立，则当前迭代点的线性化可行性有较大改善空间。因此，我们进一步要求线性化可行性具有一定的充分改善量，即

v ( x k ) − v ( x k + α d k ) ≥ η v Δ l k v ( α d k ) , η v ∈ ( 0 , 1 ) (7)

若条件(7)成立，记 x k + 1 = x k + α k d k 。此时称第 ( k + 1 ) 次迭代为c型迭代。在c型迭代中，我们按照以下方式更新 v k + 1 max ：

v k + 1 max = max { β 1 v k max , β 2 v ( x k ) + ( 1 − β 2 ) v ( x k + 1 ) } (8)

其中 β 1 , β 2 ∈ ( 0 , 1 ) 为常数。

我们将无惩罚SQP算法的流程图总结在图1中。

图1. 无惩罚SQP算法

假设A

(A1) f , g , h 是二阶连续可微函数。

(A2) 存在一个有界闭凸集 Ω ⊆ ℝ n 使得对于所有的k都有 x k ∈ Ω 成立。

(A3) 矩阵序列 { B k } 一致正定且有上界。

首先我们说明图1无惩罚SQP算法是适定的。

定理4.1 [ 9 ] 假设图1无惩罚SQP算法在 x k 处没有终止，则它将产生一个搜索方向 d k 和一个步长 α k 。

接下来给出算法的全局收敛性。

定理4.2 [ 9 ] 假设图1无惩罚SQP算法产生一个无限序列 { x k } 有一个聚点 x ∗ 。以下三个情况之一成立：

1) x ∗ 是问题(1)的KKT点。

2) x ∗ 是问题(1)不满足MFCQ约束规格的可行点。

3) x ∗ 是问题(1)的不可行稳定点。

本节将给出垂直起降方案下连续时间的燃料最优动力下降制导模型，并进一步将该模型转化为图1无惩罚SQP算法能够求解的非线性规划问题。首先，我们将动力下降制导模型中拟提及的参数汇总在表1中。

表1. 问题数据及其参数取值

图2为火箭垂直下降的简单示意图。

图2. 火箭垂直下降示意图

考虑将火箭主体视为一个质点且在除推力方向约束外的平行移动，实现火箭着陆时的剩余燃料质量极大化，即极大化火箭终端质量。我们考虑目标函数为燃料最终剩余量，即：

min X , T , t f − m ( t f ) (9)

其中，X为所有状态变量的矢量，即 X ( t ) = [ m ( t ) , r ( t ) , v ( t ) ] 。

接下来给出该问题的约束条件。具体分为如下几个方面：

1) 对火箭受力分析，可由牛顿第二定律得：

m ( t ) g + D ( t ) + T ( t ) = m ( t ) v ˙ ( t ) (10)

其中， D ( T ) 为火箭所受阻力，

D ( t ) = − 1 2 ρ S D C D ‖ v ( t ) ‖ v ( t ) (11)

ρ 为大气密度， S D 为火箭有效横截面积， C D 表示气动力轴向系数。

2) 火箭燃料的消耗速率与所受推力呈线性关系：

m ˙ ( t ) = − α T ( t ) (12)

其中，常量 α 表示发动机比冲的倒数。

3) 火箭在最终时刻的质量应非负：

m ( t ) ≥ m d r y (13)

其中， m d r y 为火箭燃料耗尽时质量。

4) 火箭在制动下降的过程中需要在一定区域内飞行，以避免与干涉飞行物相撞，因此将火箭飞行区域表示在一个角度为 θ 1 的锥形区域内如下：

r ( t ) cos θ 1 ≤ e U T r ( t ) (14)

其中， r ( t ) 表示t时刻火箭所处位置， e U = [ 0 , 1 , 0 ] T 。

5) 为使火箭在飞行过程中保持稳定状态，发动机的推力有上限和下限，即

T min ≤ ‖ T ( t ) ‖ ≤ T max (15)

6) 推力的方向与火箭的方向应当成锐角，即

− v ( t ) T T ( t ) ‖ v ( t ) ‖ ‖ T ( t ) ‖ ≥ cos θ 2 (16)

其中， θ 2 为推力与火箭的最大夹角。在下降过程中，火箭主轴方向与箭体方向近似平行，因此将火箭方向记为 − v 。

7) 推力大小在飞行过程中单位时间变化量的变化范围表示如下：

T ˙ min ≤ d ‖ T ( t ) ‖ d t ≤ T ˙ max (17)

对于初始时刻和终端时刻，我们有如下边界条件：

m ( 0 ) = m i n i t , r ( 0 ) = r i n i t , v ( 0 ) = v i n i t , T ( 0 ) = ‖ T ( 0 ) ‖ − v i n i t ‖ v i n i t ‖ (18)

r ( t f ) = 0 , v ( t f ) = 0 , T ( t f ) = ‖ T ( t f ) ‖ e U (19)

利用无损凸化方法，引入新的控制变量 Γ 表示推力大小上界，即

‖ T ( t ) ‖ ≤ Τ ( t ) (20)

将0到 t f 时刻N等分，每份的时长记为 Δ t ，即

t f = N Δ t , X k = X ( k t f N ) , K = 0 , 1 , ⋯ , N (21)

其中， t k 为第k个时间点，而 X ( t ) 经离散化后，可以表示成 X k = [ m k , r k , v k ] 。

利用差分方法，将(10)、(12)和(17)转化为下述有限个等式/不等式约束：

X k + 1 = X k + F k ( Y k ) , k = 0 , 1 , 2 , ⋯ N − 1 (22)

(23)

其中，

F k ( Y k ) = Δ t 2 × [ − α ( Γ k + Γ k + 1 ) v k + v k + 1 T k + D k m k + T k + 1 + D k + 1 m k + 1 + 2 g ] (24)

Y k = [ Δ t , X k , U k , X k + 1 , U k + 1 ] (25)

U k = [ T k , Γ k ] (26)

综上，给出动力下降制导问题经无损凸化和离散化后的非线性规划问题：

min Y k , U k − m N s . t . { X k + 1 = X k + F k ( Y k ) , k = 0 , 1 , 2 , ⋯ , N − 1 ‖ r k ‖ cos θ 1 ≤ e U T r k , k = 0 , 1 , 2 , ⋯ , N T min ≤ Γ k ≤ T max , k = 0 , 1 , 2 , ⋯ , N − v k T T k ‖ v k ‖ Γ k ≥ cos θ 2 , k = 0 , 1 , 2 , ⋯ , N ‖ T k ‖ ≤ Γ k , k = 0 , 1 , 2 , ⋯ , N T ˙ min Δ t ≤ Γ k + 1 − Γ k ≤ T ˙ max Δ t , k = 0 , 1 , 2 , ⋯ , N − 1 m 0 = m i n i t , r 0 = r i n i t , v 0 = v i n i t , T 0 = Γ 0 − v i n i t ‖ − v i n i t ‖ m N ≥ m d r y , r N = 0 , v N = 0 , T N = Γ N e U (27)

本节我们将基于图1无惩罚SQP算法对于4.1节中建立的动力下降制导问题进行数值模拟。图1无惩罚SQP算法是使用MATLAB编写的，其中子问题通过自带的“quadprog.m”函数进行处理。测试环境为一台装有Intel(R) Core(TM) i7-8550U CPU，主频为1.80GHz的笔记本电脑。

杨沐明等人 [ 1 ] 将动力学约束和推力方向约束通过线性化的方式实现近似凸化，并通过序列凸化和两阶段法的方式求解问题(27)，并得到其如下结果：终端质量和终端时刻分别为13557.8和22.25； 和 在最终解处的距离不超过10⁻⁸。从数值上看，该方法几乎是无损的。图1无惩罚SQP算法求得的终端质量和终端时刻分别为13247.5和31。图3~5展示了火箭在图1无惩罚SQP算法求得的策略下的轨迹信息。具体来说，图3给出了火箭垂直降落的轨迹图；图4展示了三个坐标方向的位置与速度；图5中分别描述了质量、推力大小和推力与速度反方向夹角曲线。

图3. 三维位置轨迹

图4. 三个坐标方向的位置与速度

图5. 质量、推力大小和推力与速度反方向夹角曲线

本文所得的结果与杨沐明等人的序列凸优化方法相比，在火箭质量、推力大小和速度推力反方向夹角曲线方面呈现出差异。基于序列凸化和两阶段法所得的质量曲线为折线，在约8秒时，火箭质量以更陡峭的倾斜程度线性下降直至终端质量 [ 1 ] 。相比之下，基于本文的无惩罚SQP算法所得的质量图像呈现近似为一条线段，火箭质量以近似线性的消耗速度下降至终端质量。各时间节点之间的目标函数的变化率不大，说明本文算法得到的结果较为稳健，同时也与实际情况吻合。

基于序列凸化和两阶段法所得的推力大小曲线更具有“突变性”，在时刻0至7秒和10秒后几乎保持恒定在100,000和250,000之间，而在7至10秒内呈现近乎竖直的变化趋势 [ 1 ] 。相比之下，基于无惩罚SQP算法所得的推力大小曲线更具有“末尾突变性”，其在初始时刻至终端之前的图像呈现较大的斜率，靠近终端时呈现近乎竖直的变化趋势。

基于序列凸化和两阶段法所得的夹角曲线变化幅度较大，夹角曲线最大值超过15度，并迅速回落至8秒时刻所处位置，并实现反弹，呈现先增加后减少的曲线态势 [ 1 ] 。而基于无惩罚SQP算法所得的夹角图像最大值小于5度。在前2秒内，角度由0度急剧增大至4.5度，随后下降，至5秒左右降至低于3度后再次上升，至近似20秒时上升至接近4度后再次下降，至终端时，夹角近似为0.45度。火箭着陆较为稳定。