无惩罚SQP方法在动力下降制导问题中的应用

Application of Penalty-Free SQP Method in Powered Descent Guidance Problem

摘要: 序列二次规划(SQP)方法是一个较为实用的求解非线性约束优化问题的方法。本文提出了一种基于无惩罚框架的改进的SQP算法。该算法不涉及罚函数，不需要考虑罚因子的选取，也不需要滤子和可行性恢复阶段。提出的算法能够克服二次规划子问题的不相容性，并且迭代点列关于目标函数和约束函数是非单调的。在有界性假设条件下，算法具有全局收敛性。当原问题无解时，算法可以收敛到原问题的不可行稳定点。此外，本文建立了动力下降制导问题的数学模型，以燃油消耗质量最低为目标，给出了算法的仿真结果和分析。

Abstract: The Sequential Quadratic Programming (SQP) method is a practical approach for solving nonlinear constrained optimization problems. This paper proposes an improved SQP algorithm based on a penalty-free framework. The algorithm eliminates the need for penalty functions, avoids considerations of penalty factors, and dispenses with filters and feasibility restoration phases. The proposed algorithm addresses the compatibility of quadratic programming subproblems and ensures that the iterates are non-monotonic with respect to both the objective function and constraint functions. Under boundedness assumptions, the algorithm exhibits global convergence. In cases where the original problem is infeasible, the algorithm can converge to infeasible stationary points of the original problem. Additionally, this paper establishes a mathematical model for the descent guidance problem, aiming to minimize fuel consumption, and provides simulation results and analysis of the algorithm.

1. 引言

非线性约束优化是数学规划的一个分支，旨在找到一组决策变量，使得在一定约束条件下最小化目标函数。通常，约束中涉及到非线性函数或者非凸函数。在本研究中，我们考虑以下形式的非线性约束优化问题：

$\begin{array}{ll}\underset{x\in {ℝ}^n}{\min }\hfill & f\left(x\right)\hfill \\ \text{s}.\text{t}.\hfill & h\left(x\right)=0,\hfill \\ \hfill & g\left(x\right)\le 0,\hfill \end{array}$ (1)

其中， $f:{ℝ}^n\to ℝ$ ， $h:{ℝ}^n\to {ℝ}^l$ 和 $g:{ℝ}^n\to {ℝ}^m$ 是连续可微的实值函数， ${ℝ}^n$ 是n维向量空间。

作为有效求解非线性优化问题(1)的方法之一，序列二次规划(Sequential Quadratic Programming, SQP)的本质是在当前迭代点求解一个二次规划子问题来得到搜索方向，再通过接受准则得到下一迭代点，具有广泛的实际应用前景。在航空航天领域，着陆段的精确制导控制是火箭回收技术的重点和难点之一，它在实现安全性保障、提高准点率和扩大适航范围等方面具有重大意义。本文基于杨沐明 [1] 等人探讨的火箭回收中的动力下降制导问题，设计了无惩罚SQP方法并进行了数值仿真。

众所周知，二次规划子问题的不可行性是SQP方法的一个缺点。早期，Powell [2] 、Fletcher [3] 等研究人员提出了一些改进的SQP方法，以解决二次规划子问题的可行性。近年来，Fletcher和Leyffer [4] 引入了一种滤子SQP算法，该算法通过可行性恢复阶段处理不可行的二次规划子问题。它通过构建滤子集来避免使用罚函数。Liu和Yuan [5] 提出了一种无惩罚SQP算法，不需要滤子并避免了可行性恢复。值得注意的是，这种方法不假设Mangasarian-Fromovitz约束规格(MFCQ)作为前提条件。Byrd等人 [6] 引入了一种舵性规则(Steering Rule)方法，通过解决两阶段兼容子问题和动态调整罚参数来处理不可行性。受舵性规则的启发，Zhao和Chen [7] 引入了一种两阶段算法，而Fu和Chen [8] 提出了一种非线性半定规划的双目标方法。基于这些方法，不对二次规划子问题的可行性做出假设，出现了许多全局收敛的SQP算法，能够识别由特定约束违反度量所刻画的不可行稳定点。例如文献 [9] [10] [11] 等。

对于制导问题，杨沐明 [1] 提出了一种序列凸优化方法。该方法可视为二次规划子问题中二阶项恒取对角矩阵的信赖域SQP方法。由于没有利用实际问题的二阶信息，该方法迭代次数较多，算法得到的解精度不高。此外，还有一些基于深度学习的方法，例如文献 [12] 等。

文章的主要贡献可以总结如下：首先，提出了一种无惩罚SQP算法，其能够全局地收敛到问题的KKT点，而这一收敛性的结论并不需要约束规格作为前提假设，并且具有探测不可行稳定点的特性。其次，该算法不使用罚函数，因此可以避免极端罚因子导致的算法失败。第三，迭代点列关于目标函数和约束违反度都是非单调的，这使得算法具有更宽松的接受准则。第四，算法使用了两阶段方法求得搜索方向，保证了需要求解的二次规划子问题一定是有解的。最后，将该算法应用于火箭回收中的动力下降制导问题，验证其能够有效地求得回收策略。

文章框架如下。第二章将给出具体的算法框架；第三章给出算法的适定性结论和全局收敛性结论；第四章将给出火箭回收中动力下降制导问题的优化建模，并运用本文提出的无惩罚SQP算法求解，对数值结果进行对比分析；最后给出总结。

2. 无惩罚SQP算法

2.1. 算法描述

为了简化算法，在当前迭代点 $x_k$ ，定义

$f_k:=f\left(x_k\right),h_k:=h\left(x_k\right),g_k:=g\left(x_k\right),v_k:=v(\; x\; k\; )$

其中 $v\left(x\right)={\Vert h\left(x\right)\Vert }_1+{\Vert {\left[g\left(x\right)\right]}_{+}\Vert }_1$ 为约束违反度函数。对于给定的方向 $d\in R^n$ ，定义 $v\left(x\right)$ 在 $x_k$ 处沿d的线性约束违反度为

$l_k^v\left(d\right):={\Vert h_k+Dh\left(x_k\right)d\Vert }_1+{\Vert {\left[g_k+Dg\left(x_k\right)d\right]}_{+}\Vert }_1$

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

$l_k^f\left(d\right):=f_k+\nabla f{\left(x_k\right)}^{T}d$

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

$\begin{array}{cc}\nabla l_k^f\left(d\right):=l_k^f\left(0\right)-l_k^f\left(d\right)=-\nabla f{\left(x_k\right)}^{T}d,& \nabla l_k^v\left(d\right):=l_k^v\left(0\right)-l_k^v(\; d\; )\end{array}$

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

$\begin{array}{ll}\underset{d,r,s,t}{\min }\hfill & e^T\left(r+s\right)+e^{T}t\hfill \\ \text{s}.\text{t}.\hfill & h_k+Dh\left(x_k\right)d=r-s\hfill \\ \hfill & \begin{array}{l}{g}_k+Dg\left(x_k\right)d\le t\\ r,s,t\ge 0,{\Vert d\Vert }_{\infty }\le \Delta \end{array}\hfill \end{array}$ (2)

其中 $\Delta >0$ 为较大的常数，用来保证子问题(2)的有界性。记子问题(2)的解为 $\left(d_k^{fea},r_k,s_k,t_k\right)$ 。

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

$\begin{array}{ll}\underset{d\in R^n}{\min }\hfill & \nabla f{\left(x_k\right)}^{T}d+\frac{1}{2}{d}^T{B}_{k}d\hfill \\ \text{s}.\text{t}.\hfill & h_k+Dh\left(x_k\right)d=r_k-s_k\hfill \\ \hfill & g_k+Dg\left(x_k\right)d\le t_k\hfill \end{array}$ (3)

记(3)的解为 $d_k$ 。设 $\left({\mu }_{k+1},Y_{k+1}\right)$ 为等式约束和不等式约束对应的拉格朗日乘子。

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

$\Delta l_k^f\left(\alpha d_k\right)\ge \delta \Delta l_k^v{\left(\alpha d_k\right)}^{\tau }$ (4)

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

$\begin{array}{cc}f\left(x_k\right)-f\left(x_k+\alpha d_k\right)\ge {\eta }_f\Delta l_k^f\left(\alpha d_k\right),& {\eta }_f\in \left(0,1\right)\end{array}$ (5)

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

$v\left(x_k+\alpha d_k\right)\le v_k^{\max }$ (6)

其中 $v_k^{\max }$ 是约束违反度 $v\left(x\right)$ 的一个上界。若条件 ， 和 均成立，记 $x_{k+1}=x_k+{\alpha }_k{d}_k$ 。此时称第 $\left(k+1\right)$ 次迭代为f型迭代。在f型迭代中， $v_{k+1}^{\max }$ 保持不变，即 $v_{k+1}^{\max }=v_k^{\max }$ 。

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

$\begin{array}{cc}v\left(x_k\right)-v\left(x_k+\alpha d_k\right)\ge {\eta }_v\Delta l_k^v\left(\alpha d_k\right),& {\eta }_v\in \left(0,1\right)\end{array}$ (7)

若条件(7)成立，记 $x_{k+1}=x_k+{\alpha }_k{d}_k$ 。此时称第 $\left(k+1\right)$ 次迭代为c型迭代。在c型迭代中，我们按照以下方式更新 $v_{k+1}^{\max }$ ：

$v_{k+1}^{\max }=\max \left\{{\beta }_1{v}_k^{\max },{\beta }_{2}v\left(x_k\right)+\left(1-{\beta }_2\right)v\left(x_{k+1}\right)\right\}$ (8)

其中 ${\beta }_1,{\beta }_2\in \left(0,1\right)$ 为常数。

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

Figure 1. A penalty-free SQP algorithm

图1. 无惩罚SQP算法

2.2. 全局收敛性

假设A

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

(A2) 存在一个有界闭凸集 $\Omega \subseteq {ℝ}^n$ 使得对于所有的k都有 $x_k\in \Omega$ 成立。

(A3) 矩阵序列 $\left\{B_k\right\}$ 一致正定且有上界。

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

定理4.1 [9] 假设图1无惩罚SQP算法在 $x_k$ 处没有终止，则它将产生一个搜索方向 $d_k$ 和一个步长 ${\alpha }_k$ 。

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

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

1) $x^{\ast }$ 是问题(1)的KKT点。

2) $x^{\ast }$ 是问题(1)不满足MFCQ约束规格的可行点。

3) $x^{\ast }$ 是问题(1)的不可行稳定点。

3. 应用与仿真

3.1. 动力下降制导问题

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

Table 1. Problem data and parameter values

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

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

Figure 2. Vertical descent of a rocket

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

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

$\underset{X,T,t_f}{\min }-m\left(t_f\right)$ (9)

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

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

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

$m\left(t\right)g+D\left(t\right)+T\left(t\right)=m\left(t\right)\stackrel{\dot{}}{v}\left(t\right)$ (10)

其中， $D\left(T\right)$ 为火箭所受阻力，

$D\left(t\right)=-\frac{1}{2}\rho S_D{C}_D\Vert v\left(t\right)\Vert v\left(t\right)$ (11)

$\rho$ 为大气密度， $S_D$ 为火箭有效横截面积， $C_D$ 表示气动力轴向系数。

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

$\stackrel{\dot{}}{m}\left(t\right)=-\alpha T\left(t\right)$ (12)

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

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

$m\left(t\right)\ge m_{dry}$ (13)

其中， $m_{dry}$ 为火箭燃料耗尽时质量。

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

$r\left(t\right)\cos {\theta }_1\le e_U^{T}r\left(t\right)$ (14)

其中， $r\left(t\right)$ 表示t时刻火箭所处位置， $e_U={\left[0,1,0\right]}^T$ 。

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

$T_{\min }\le \Vert T\left(t\right)\Vert \le T_{\max }$ (15)

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

$\frac{-v{\left(t\right)}^{T}T\left(t\right)}{\Vert v\left(t\right)\Vert \Vert T\left(t\right)\Vert }\ge \cos {\theta }_2$ (16)

其中， ${\theta }_2$ 为推力与火箭的最大夹角。在下降过程中，火箭主轴方向与箭体方向近似平行，因此将火箭方向记为 $-v$ 。

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

${\stackrel{\dot{}}{T}}_{\min }\le \frac{\text{d}\Vert T\left(t\right)\Vert }{\text{d}t}\le {\stackrel{\dot{}}{T}}_{\max }$ (17)

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

$m\left(0\right)=m_{init},r\left(0\right)=r_{init},v\left(0\right)=v_{init},T\left(0\right)=\Vert T\left(0\right)\Vert \frac{-v_{init}}{\Vert v_{init}\Vert }$ (18)

$r\left(t_f\right)=0,v\left(t_f\right)=0,T\left(t_f\right)=\Vert T\left(t_f\right)\Vert e_U$ (19)

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

$\Vert T\left(t\right)\Vert \le {\rm T}\left(t\right)$ (20)

将0到 $t_f$ 时刻N等分，每份的时长记为 $\Delta t$ ，即

$t_f=N\Delta t,X_k=X\left(\frac{k{t}_f}{N}\right),K=0,1,\cdots ,N$ (21)

其中， $t_k$ 为第k个时间点，而 $X\left(t\right)$ 经离散化后，可以表示成 $X_k=\left[m_k,r_k,v_k\right]$ 。

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

$X_{k+1}=X_k+F_k\left(Y_k\right),k=0,1,2,\cdots N-1$ (22)

$$ (23)

其中，

$F_k\left(Y_k\right)=\frac{\Delta t}{2}\times \left[\begin{array}{c}-\alpha \left({\Gamma }_k+{\Gamma }_{k+1}\right)\\ v_k+v_{k+1}\\ \frac{T_k+D_k}{m_k}+\frac{T_{k+1}+D_{k+1}}{m_{k+1}}+2g\end{array}\right]$ (24)

$Y_k=\left[\Delta t,X_k,U_k,X_{k+1},U_{k+1}\right]$ (25)

$U_k=\left[T_k,{\Gamma }_k\right]$ (26)

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

$\begin{array}{ll}\underset{Y_k,U_k}{\min }\hfill & -m_N\hfill \\ \text{s}.\text{t}.\hfill & \{\begin{array}{l}{X}_{k+1}=X_k+F_k\left(Y_k\right),k=0,1,2,\cdots ,N-1\hfill \\ \Vert r_k\Vert \cos {\theta }_1\le e_U^T{r}_k,k=0,1,2,\cdots ,N\hfill \\ T_{\min }\le {\Gamma }_k\le T_{\max },k=0,1,2,\cdots ,N\hfill \\ \frac{-v_k^T{T}_k}{\Vert v_k\Vert {\Gamma }_k}\ge \cos {\theta }_2,k=0,1,2,\cdots ,N\hfill \\ \Vert T_k\Vert \le {\Gamma }_k,k=0,1,2,\cdots ,N\hfill \\ {\stackrel{\dot{}}{T}}_{\min }\Delta t\le {\Gamma }_{k+1}-{\Gamma }_k\le {\stackrel{\dot{}}{T}}_{\max }\Delta t,k=0,1,2,\cdots ,N-1\hfill \\ m_0=m_{init},r_0=r_{init},v_0=v_{init},T_0={\Gamma }_0\frac{-v_{init}}{\Vert -v_{init}\Vert }\hfill \\ m_N\ge m_{dry},r_N=0,v_N=0,T_N={\Gamma }_N{e}_U\hfill \end{array}\hfill \end{array}$ (27)

3.2. 数值优化

本节我们将基于图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中分别描述了质量、推力大小和推力与速度反方向夹角曲线。

Figure 3. Three-dimensional position trajectory

图3. 三维位置轨迹

Figure 4. Position and velocity in three directions

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

Figure 5. Mass, thrust magnitude, and the curve of the thrust angle with respect to velocity

图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度。火箭着陆较为稳定。

4. 总结

本文主要介绍了一种无惩罚SQP算法，用于解决在燃料最优目标下的动力下降制导最优控制问题。首先，文章提出了该算法的框架，并讨论了其全局收敛性。针对动力下降制导最优控制模型，采用了无损凸化和时间离散化的方法，将其转化为非线性规划问题，并应用无惩罚SQP算法进行求解。最后，通过MATLAB仿真模拟验证了算法的收敛性结论。在动力下降制导最优控制问题中，本文的方法与已有的序列凸化方法进行了对比分析。通过无惩罚SQP算法的仿真结果观察到，飞行器轨迹的稳定性得到了改善，下降方向与速度方向的夹角变化幅度不大，并且燃料损耗速度较为稳定。总的来说，本文提出的无惩罚SQP算法在解决动力下降制导最优控制问题上表现出了较好的性能，具有较好的收敛性和稳定性，对飞行器的控制具有实际意义。

基金项目

苏州科技大学2023年江苏省大学生创新创业训练计划项目《最小约束违背下的SQP算法及其应用》(编号：202310332088Y)。

NOTES

^*通讯作者。