CN113467498A - 一种基于Bezier-凸优化的运载火箭上升段轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于Bezier‑凸优化的运载火箭上升段轨迹规划方法，针对传统方法收敛性差，计算效率低，无法应用于在线轨迹规划等缺陷，提出了一种基于Bezier曲线的改进凸优化方法。首先，建立运载火箭上升段轨迹规划的最优控制问题；其次，利用N‑K方法处理高度非线性的动力学微分方程，将其转化为关于状态增量和控制增量的线性微分方程，且具有稳定收敛的形式，提升了方法的收敛速度；最后，利用Bezier曲线代替控制量曲线，有效地降低了凸优化问题的求解规模，极大地提升了凸优化问题的求解效率。本发明能有效解决含有复杂约束的运载火箭上升段在线轨迹规划问题。

Description

一种基于Bezier-凸优化的运载火箭上升段轨迹规划方法

技术领域

本发明属于火箭制导技术领域，具体涉及一种运载火箭上升段轨迹规划方法。

背景技术

随着航天科技的快速发展，运载火箭在不同发射任务下对自主性和智能化要求越来越高。由于运载火箭上升段具有复杂的动力学约束和过程约束，上升段轨迹优化问题很难在机载计算机上实时求解。因此，能够处理复杂约束的可靠、快速的方法成为近些年来计算制导领域研究的重点与难点。一般而言，运载火箭上升段轨迹规划问题可以表述为一个具有目标函数、状态和控制约束的最优控制问题，传统上有两种方法来解决这个问题：间接法和直接法。其中间接法是利用变分法和庞特里亚金极大(极小)值原理，推导最优控制问题的一阶必要条件，从而转化为两点边值问题或多点边值问题进行求解。间接法的优点是求解精度高，求解速度快，但同时也存在收敛半径小，迭代算法对初始猜想极其敏感，复杂模型的一阶必要条件推导繁琐等问题。直接法将最优控制问题通过离散方法转化为非线性规划问题进行求解，其优点是不需要推导一阶必要条件，易于处理复杂约束问题，具有良好的通用性。但该方法一般需要大规模的数值优化求解，计算比较缓慢，此外也存在所求得的仅仅是近似解及存在维数灾难等不足。

凸优化作为一种特殊的直接法，因其对理论解和计算效率的保证得到了广泛的应用。但由于大部分航空航天问题都存在强非线性的动力学模型和状态约束，无法直接采用凸优化进行求解。因而近年来，逐次线性化法和Newton–Kantorovich(N-K)法被提出对这类问题进行凸化，他们都是将原问题转化为一系列子凸优化问题，然后采用内点法求解子问题，将该次迭代求得的子问题的解作为下一次迭代的初值。不同的是，逐次线性化法是利用泰勒展开将非线性微分方程转化为关于状态量和控制量的线性微分方程，收敛性无法保证，且对初始猜测比较敏感；而N-K法是利用变分法和广义泰勒展开将非线性微分方程转化为关于状态增量和控制增量的线性微分方程，具有稳定收敛的形式，且对初值不敏感。因此N-K法作为非线性动力学方程的凸化方法可以更快收敛到最优解，为轨迹规划问题的在线求解提供了技术保证。

发明内容

为了克服现有技术的不足，本发明提供了一种基于Bezier-凸优化的运载火箭上升段轨迹规划方法，针对传统方法收敛性差，计算效率低，无法应用于在线轨迹规划等缺陷，提出了一种基于Bezier曲线的改进凸优化方法。首先，建立运载火箭上升段轨迹规划的最优控制问题；其次，利用N-K方法处理高度非线性的动力学微分方程，将其转化为关于状态增量和控制增量的线性微分方程，且具有稳定收敛的形式，提升了方法的收敛速度；最后，利用Bezier曲线代替控制量曲线，有效地降低了凸优化问题的求解规模，极大地提升了凸优化问题的求解效率。本发明能有效解决含有复杂约束的运载火箭上升段在线轨迹规划问题。

本发明解决其技术问题所采用的技术方案包括如下步骤：

步骤1：在发射惯性坐标系下，建立运载火箭上升段动力学模型如下：

式中，自变量为运载火箭的高度h，x和y为运载火箭在地面的投影位置，v为运载火箭的速度，γ为运载火箭航迹倾角，χ为运载火箭航向角，m为运载火箭的质量，α为攻角，σ为倾侧角，T为推力，I_sp为发动机比冲，g₀为地球引力常数，G＝mg为重力，L＝ρv²S_refC_L/2为升力，D＝ρv²S_refC_D/2为阻力，其中ρ为大气密度，S_ref为运载火箭的参考面积，C_L和C_D分别为升力系数和阻力系数；

式(1)简写为

其中x＝[x,y,v,γ,χ,m]^T为状态量，u＝[u₁,u₂,u₃,T]^T为控制量，u₁＝cosα，u₂＝sinαcosσ，u₃＝sinαsinσ；

步骤2：建立运载火箭上升段轨迹规划的最优控制问题P0，性能指标为终端速度最大，约束方程包括：运载火箭上升段动力学方程、初始和终端状态约束方程、过程约束方程、控制量约束方程；

最优控制问题P0表示为：

min J＝-v(h_f)

s.t.

x(h₀)＝x₀

α_min≤α≤α_max,σ_min≤σ≤σ_max,T_min≤T≤T_max

x(h_f)＝x_f,y(h_f)＝y_f,γ(h_f)＝γ_f,χ(h_f)＝χ_f,m(h_f)≥m_dry (2)

其中，x₀＝[x₀,y₀,v₀,γ₀,χ₀,m₀]^T为运载火箭期望的初始状态量；攻角α约束的上下限为±90°，α_min＝-90°，α_max＝90°，该约束能表示为控制量u₁的约束：u₁≥0；倾侧角σ约束的上下限为±90°，σ_min＝-90°，σ_max＝90°，该约束能表示为控制量u₂和u₃的约束：-1≤u₂≤1，-1≤u₃≤1；此外，三个控制量还满足约束u₁ ²+u₂ ²+u₃ ²＝1；T_min和T_max为推力的最小值和最大值；过程约束包括动压约束q≤q_max，轴向推力加速度约束a_axial≤a_max和弯矩约束Q_α≤Q_αmax，q_max为动压允许的最大值，a_max为轴向推力加速度允许的最大值，Q_αmax为弯矩允许的最大值；x_f，y_f，γ_f，χ_f为运载火箭期望的终端状态量，m_dry为运载火箭的结构质量；h₀和h_f表示初始高度和终端高度；

步骤3：用Bezier曲线代替最优控制问题P0的控制量u的曲线，运载火箭上升段轨迹规划问题的控制变量变为Bezier曲线的控制点：

控制量约束方程变为关于Bezier曲线控制点的约束方程，从而得到基于Bezier的最优控制问题P1；

步骤3-1：所述Bezier曲线的表达式为：

其中，P_i为Bezier曲线的控制点，

为伯恩斯坦多项式；t为归一化的自变量，n为控制点个数；

将运载火箭上升段动力学模型的自变量归一化处理，令ω＝(h-h₀)/(h_f-h₀)，ω为动力学模型归一化后的自变量，此时新的性能指标变为J＝-v(1)，新的动力学模型变为x′＝dx/dω＝(h_f-h₀)f(x，u)；

步骤3-2：用Bezier曲线的控制点

代替控制量u＝[u₁，u₂，u₃，T]^T：

其中，P_kj是Bezier曲线的控制点；

为伯恩斯坦多项式；

将式(4)表示成矩阵的形式：

u_s(ω)：＝W(ω)P_s；P_s＝[P_s0，...，P_sn]^T

步骤3-3：Bezier曲线控制点的约束方程为：

轴向推力加速度约束，

弯矩约束，

推力约束，

控制量约束，

控制量约束，

步骤3-4：基于Bezier的最优控制问题P1表示为：

min J＝-v(1)

s.t.x′＝(h_f-h₀)f(x，P)

x(0)＝x₀，x(1)＝x_f，y(1)＝y_f，γ(1)＝γ_f，χ(1)＝χ_f，m(1)≥m_dry

步骤4：利用N-K方法对运载火箭上升段动力学方程和过程约束进行凸化，得到关于状态增量δx和控制增量δP的线性微分方程和凸的过程约束，利用松弛技巧对控制量约束进行凸化，得到凸的控制量约束，从而得到基于Bezier曲线的子凸优化问题P2：

步骤4-1：利用N-K方法对运载火箭上升段动力学方程和过程约束进行凸化：

定义非线性算子F[x，P]＝x′-f(x，P)＝0，对非线性算子进行广义泰勒展开并忽略高阶项：

F[x+δx，P+δP]＝F[x，P]+F_x(x，P)[δx]+F_P(x，P)[δP]＝0 (8)

其中，δx和δP分别为状态量和控制量的增量，F_x(x，P)[δx]和F_P(x，P)[δP]为Frechet导数；

步骤4-2：将式(9)和式(10)带入广义泰勒展开式(8)，得到关于状态增量δx和控制增量δu^*的线性微分方程：

步骤4-3：基于Bezier曲线的子凸优化问题P2表示为：

min J＝-δv(1)

s.t.

δx(0)＝x₀-x(0)

δx(1)＝x_f-x(1)，δy(1)＝y_f-y(1)，δγ(1)＝γ_f-γ(1)，δχ(1)＝χ_f-χ(1)，δm(1)≥m_dry-m(1)

T_min≤P_4j+δP_4j≤T_max，0≤P_1j+δP_1j≤1，-1≤P_2j+δP_2j≤1，-1≤P_3j+δP_3j≤1(P_1j+δP_1j)²+(P_2j+δP_2j)²+(P_3j+δP_3j)²≤1 (12)

步骤5：对子凸优化问题P2进行离散化，将自变量进行等距离散得到ω₀，ω₁，...，ω_N，优化变量表示为向量

动力学方程采用欧拉法进行离散，得到基于Bezier曲线的离散子凸优化问题P3表示为：

min J＝c^Tz

s.t.

δx₁＝x₀-x₁

δx_N＝x_f-x_N，δy_N＝y_f-y_N，δγ_N＝γ_f-γ_N，δχ_N＝χ_f-χ_N，δm_N≥m_dry-m_N

T_min≤P_4j+δP_4j≤T_max，0≤P_1j+δP_1j≤1，-1≤P_2j+δP_2j≤1，-1≤P_3j+δP_3j≤1

(P_1j+δP_1j)²+(P_2j+δP_2j)²+(P_3j+δP_3j)²≤1

其中，i＝1，2，...，N，j＝0，1，...，n，且N＞＞n；N为变量离散点的个数；

步骤6：采用内点法求解基于Bezier曲线的离散子凸优化问题P3，将该次迭代求得的子问题的解作为下一次迭代的初值，直到迭代收敛至最优解，具体求解过程为：

步骤6-1：令迭代步数k＝0，给定状态量和控制量的初始猜想x⁰和P⁰，并令初始猜想x^k＝x⁰，P^k＝P⁰；

步骤6-2：在第k+1步迭代过程中，将所述初始猜想x^k和P^k结合凸优化求解器--MOSEK求解器，求解所述基于Bezier曲线的离散子凸优化问题P3，得到状态量增量δx和控制量增量δP；

步骤6-3：更新状态量和控制量：x^k+1＝x^k+δx，P^k+1＝P^k+δP；

步骤6-4：判断是否满足收敛条件：max(δx_i)≤ε，若满足该条件，则停止迭代，x^k+1和P^k+1为优化问题的解；若不满足则令x^k＝x^k+1，P^k＝P^k+1；返回步骤6-2。

本发明的有益效果如下：

本发明将传统凸优化方法与Bezier曲线和N-K方法相结合，利用N-K方法处理具有强非线性的运载火箭上升段动力学方程，具有良好的收敛性；利用少量的bezier曲线的控制点代替数量较多的离散控制量，降低了问题的求解维度，此外由于Bezier曲线的凸包性，也减少了一些过程约束和控制量约束的数量，有效地提高了问题的求解效率，实现了运载火箭在强非线性动力学和复杂过程约束情况下的在线轨迹规划。

附图说明

图1为本发明方法的结构框图。

图2为本发明方法与参考方法空间三维轨迹曲线。

图3为本发明方法与参考方法速度随高度的变化曲线。

图4为本发明方法与参考方法航迹倾角随高度的变化曲线。

图5为本发明方法与参考方法攻角和倾侧角随高度的变化曲线。

图6为本发明方法与参考方法动压，轴向推力加速度和弯矩随高度的变化曲线。

图7为本发明方法与参考方法在不同离散点数量情况下总计算时间的对比曲线。

图8为本发明方法与参考方法在不同离散点数量情况下终端速度的对比曲线。

图9为本发明方法与参考方法在不同离散点数量情况下积分与优化结果的终端位置误差曲线。

图10为本发明方法与参考方法在不同离散点数量情况下积分与优化结果的终端速度误差曲线。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

本发明解决的技术问题是：针对现有技术不足，提供一种基于改进凸优化的运载火箭上升段轨迹规划方法，克服传统凸优化方法在求解复杂航天问题时收敛性差和求解效率低的问题，实现含有复杂约束和强非线性动力学方程的运载火箭上升段的轨迹在线规划。

步骤1：在发射惯性坐标系下，建立运载火箭上升段动力学模型如下：

式中，自变量为运载火箭的高度h，x和y为运载火箭在地面的投影位置，v为运载火箭的速度，γ为运载火箭航迹倾角，χ为运载火箭航向角，m为运载火箭的质量，α为攻角，σ为倾侧角，T为推力，I_sp为发动机比冲，g₀为地球引力常数，G＝mg为重力，上＝ρv²S_refC_L/2为升力，D＝ρv²S_refC_D/2为阻力，其中ρ为大气密度，S_ref为运载火箭的参考面积，C_L和C_D分别为升力系数和阻力系数；

式(1)简写为

其中x＝[x，y，v，γ，χ，m]^T为状态量，u＝[u₁，u₂，u₃，T]^T为控制量，u₁＝cosα，u₂＝sinαcosσ，u₃＝sinαsinσ；

步骤2：建立运载火箭上升段轨迹规划的最优控制问题P0，性能指标为终端速度最大，约束方程包括：运载火箭上升段动力学方程、初始和终端状态约束方程、过程约束方程、控制量约束方程；

最优控制问题P0表示为：

min J＝-v(h_f)

s.t.

x(h₀)＝x₀

α_min≤α≤α_max，σ_min≤σ≤σ_max，T_min≤T≤T_max

x(h_f)＝x_f，y(h_f)＝y_f，γ(h_f)＝γ_f，χ(h_f)＝χ_f，m(h_f)≥m_dry (2)

其中，x₀＝[x₀，y₀，v₀，γ₀，χ₀，m₀]^T为运载火箭期望的初始状态量；攻角α约束的上下限为±90°，α_min＝-90°，α_max＝90°，该约束能表示为控制量u₁的约束：u₁≥0；倾侧角σ约束的上下限为±90°，σ_min＝-90°，σ_max＝90°，该约束能表示为控制量u₂和u₃的约束：-1≤u₂≤1，-1≤u₃≤1；此外，三个控制量还满足约束

T_min和T_max为推力的最小值和最大值；过程约束包括动压约束q≤q_max，轴向推力加速度约束a_axial≤a_max和弯矩约束Q_α≤Q_αmax，q_max为动压允许的最大值，a_max为轴向推力加速度允许的最大值，Q_αmax为弯矩允许的最大值；x_f，y_f，γ_f，χ_f为运载火箭期望的终端状态量，m_dry为运载火箭的结构质量；

步骤3：用Bezier曲线代替最优控制问题P0的控制量u的曲线，运载火箭上升段轨迹规划问题的控制变量变为Bezier曲线的控制点：

控制量约束方程变为关于Bezier曲线控制点的约束方程，从而得到基于Bezier的最优控制问题P1；

步骤3-1：所述Bezier曲线的表达式为：

其中，P_i为Bezier曲线的控制点，

为伯恩斯坦多项式

由于Bezier曲线的自变量在0到1的范围，为了用Bezier曲线代替控制量曲线，首先需要将运载火箭上升段动力学模型的自变量归一化处理，令ω＝(h-h₀)/(h_f-_h0)，ω为动力学模型归一化后的自变量，此时新的性能指标变为J＝-v(1)，新的动力学模型变为x′＝dx/dω＝(h_f-h₀)f(x，u)；

步骤3-2：用Bezier曲线的控制点

代替控制量u＝[u₁，u₂，u₃，T]^T：

其中，P_kj是Bezier曲线的控制点；

为伯恩斯坦多项式；

将式(4)表示成矩阵的形式：

u_k(ω)：＝W(ω)P_k；P_k＝[P_k0，...，P_kn]^T；k＝1，2，3，4

步骤3-3：Bezier曲线控制点的约束方程为：

轴向推力加速度约束，

弯矩约束，

推力约束，

控制量约束，

控制量约束，

由于Bezier曲线的凸包性，即Bezier曲线总是位于由其控制点组成的凸包内，因此只要Bezier控制点满足的约束，Bezier曲线上任意一点都满足该约束，从而可以极大减少一些过程约束和控制量约束的数量，从而缩小问题的求解规模；

步骤3-4：基于Bezier的最优控制问题P1表示为：

min J＝-v(1)

s.t.x′＝(h_f-h₀)f(x，P)

x(0)＝x₀，x(1)＝x_f，y(1)＝y_f，γ(1)＝γ_f，χ(1)＝χ_f，m(1)≥m_dry

步骤4：利用N-K方法对运载火箭上升段动力学方程和过程约束进行凸化，得到关于状态增量δx和控制增量δP的线性微分方程和凸的过程约束，利用松弛技巧对控制量约束进行凸化，得到凸的控制量约束，从而得到基于Bezier曲线的子凸优化问题P2；

步骤4-1：利用N-K方法对运载火箭上升段动力学方程和过程约束进行凸化：

定义非线性算子F[x，P]＝x′-f(x，P)＝0，对非线性算子进行广义泰勒展开并忽略高阶项：

F[x+δx，P+δP]＝F[x，P]+F_x(x，P)[δx]+F_P(x，P)[δP]＝0 (8)

其中，δx和δP分别为状态量和控制量的增量，F_x(x，P)[δx]和F_P(x，P)[δP]为Frechet导数；

步骤4-2：将式(9)和式(10)带入广义泰勒展开式(8)，得到关于状态增量δx和控制增量δu^*的线性微分方程：

值得注意的是，上述线性微分方程左端项为非线性算子F[x，P]的导数，右端项为负的非线性算子，因此上式可以写为F′＝-F的形式，这是一种稳定控制的形式，F[x，P]的所有参数都可以指数收敛到收敛解，因此N-K方法对初始猜测不敏感且具有更快的收敛速度。

步骤4-3：基于Bezier曲线的子凸优化问题P2表示为：

min J＝-δv(1)

s.t.

δx(0)＝x₀-x(0)

δx(1)＝x_f-x(1)，δy(1)＝y_f-y(1)，δγ(1)＝γ_f-γ(1)，δχ(1)＝χ_f-χ(1)，δm(1)≥m_dry-m(1)

T_min≤P_4j+δP_4j≤T_max，0≤P_1j+δP_1j≤1，-1≤P_2j+δP_2j≤1，-1≤P_3j+δP_3j≤1

(P_1j+δP_1j)²+(P_2j+δP_2j)²+(P_3j+δP_3j)²≤1 (12)

这里应用松弛技巧将非凸的控制量约束(P_1j+δP_1j)²+(P_2j+δP_2j)²+(P_3j+δP_3j)²＝1转化为凸的控制量约束(P_1j+δP_1j)²+(P_2j+δP_2j)²+(P_3j+δP_3j)²≤1，这样处理得到的解被证明与原问题的解是相同的；

步骤5：对子凸优化问题P2进行离散化，将自变量进行等距离散得到ω₀，ω₁，...，ω_N，优化变量表示为向量

动力学方程采用欧拉法进行离散，得到基于Bezier曲线的离散子凸优化问题P3表示为：

min J＝c^Tz

s.t.

δx₁＝x₀-x₁

δx_N＝x_f-x_N，δy_N＝y_f-y_N，δγ_N＝γ_f-γ_N，δχ_N＝χ_f-χ_N，δm_N≥m_dry-m_N

T_min≤P_4j+δP_4j≤T_max，0≤P_1j+δP_1j≤1，-1≤P_2j+δP_2j≤1，-1≤P_3j+δP_3j≤1

(P_1j+δP_1j)²+(P_2j+δP_2j)²+(P_3j+δP_3j)²≤1

其中，i＝1，2，...，N，j＝0，1，...，n，且N＞＞n；

步骤6：采用内点法求解基于Bezier曲线的离散子凸优化问题P3，将该次迭代求得的子问题的解作为下一次迭代的初值，直到迭代收敛至最优解，具体求解过程为：

步骤6-1：令迭代步数k＝0，给定状态量和控制量的初始猜想x⁰和P⁰，并令初始猜想x^k＝x⁰，P^k＝P⁰；

步骤6-2：在第k+1步迭代过程中，将所述初始猜想x^k和P^k结合凸优化求解器--MOSEK求解器，求解所述基于Bezier曲线的离散子凸优化问题P3，得到状态量增量δx和控制量增量δP；

步骤6-3：更新状态量和控制量：x^k+1＝x^k+δx，P^k+1＝P^k+δP；

步骤6-4：判断是否满足收敛条件：max(δx_i)≤ε，若满足该条件，则停止迭代，x^k+1和P^k+1为优化问题的解；若不满足则令x^k＝x^k+1，P^k＝P^k+1；返回步骤6-2。

具体实施例：

本实施例以多级运载火箭的子一级为仿真对象，推力T＝2961.6×10³N，比冲I_sp＝2556.2N·s/kg，参考面积S_ref＝4m²，结构质量m_dry＝70696kg，最大动压q_max＝3×10⁴N/m²，最大轴向推力加速度a_max＝5gm/s²，最大弯矩Q_αmax＝3×10³N·rad/m²；自变量飞行高度设置为h∈[5，63]km，仿真初始条件设置为：初始位置x₀＝0km，y₀＝0km，初始速度v₀＝225.5m/s，初始航迹倾角γ₀＝80.8deg，初始航向角χ₀＝0deg，初始质量m₀＝2.124×10⁵kg；终端条件设置为：终端位置x_f＝40.5km，y_f＝0km，终端航迹倾角γ_f＝44.5deg，终端航向角χ_f＝0deg；计算机硬件条件为Inter Core i5-4210M CPU2.60GHz，软件使用基于MATLAB的MOSEK求解器。

如图1所示，本发明提供的基于Bezier-凸优化的运载火箭上升段轨迹规划方法包括以下步骤：

1、确定无量纲化参数，令长度的无量纲化参数为地心矢径R₀，速度的无量纲化参数为

时间的无量纲化参数为

质量的无量纲化参数为初始质量m₀，且R₀＝6378.13km，g₀＝9.807m/s²；确定离散点个数N＝200和Bezier控制点个数n＝20；

2、对运载火箭上升段参考轨迹进行初始化，首先根据上述参数确定运载火箭初始的运动状态x₀＝[0，0，0.0285，1.4104，0，0.7478]^T，给定控制量的初始猜想

其中P₁ ⁰设为元素都为1的20×1的向量，P₂ ⁰和P₃ ⁰设为大小为20×1的零向量，

设为元素都为最大推力的20×1的向量，并通过龙格库塔积分计算出一条参考轨迹；然后将飞行高度h₀＝7.8393×10^-4到h_f＝0.0099的变化范围均匀离散为200个区间，得到h＝[h₁，h₂，...，h_N]，并将初始参考轨迹插值到每一个对应的离散高度上，得到一条离散的初始参考轨迹

令迭代步数k＝0，初始猜想x^k＝x⁰，P^k＝P⁰；

3、在第k+1步迭代过程中，将所述初始猜想x^k和P^k结合凸优化求解器MOSEK求解器，求解所述基于Bezier曲线的离散子凸优化问题P3，具体步骤为：

(3a)性能指标的离散形式写成矩阵相乘的形式J＝c^Tz，其中向量c＝[0_1×1202，-1，0_1×83]^T，优化变量

(3b)根据x^k和P^k计算每个离散点的

f_i，构成离散的动力学约束方程

并将其写成矩阵形式Mz＝F，其中

(3c)分别获得所述运载火箭上升段的初始状态凸化方程，终端状态凸化方程，过程凸约束以及控制量凸约束方程，构建离散的运载火箭上升段Bezier-凸优化子问题：

min J＝c^Tz

s.t.Mz＝F

δx₁＝x₀-x₁

δx_N＝x_f-x_N，δy_N＝y_f-y_N，δγ_N＝γ_f-γ_N，δχ_N＝χ_f-χ_N，δm_N≥m_dry-m_N

T_min≤P_4j+δP_4j≤T_max，0≤P_1j+δP_1j≤1，-1≤P_2j+δP_2j≤1，-1≤P_3j+δP_3j≤1

(P_1j+δP_1j)²+(P_2j+δP_2j)²+(P_3j+δP_3j)²≤1

(3d)结合MOSEK求解器的语法规则，对离散化的Bezier-凸优化子问题进行求解，得到状态量增量δx和控制量增量δP；

4、更新状态量和控制量：x^k+1＝x^k+δx，P^k+1＝P^k+δP；

5、设定本方法法的收敛条件为

并判断是否满足收敛条件：max(δx_i)≤ε，若满足该条件，则停止迭代，x^k+1和P^k+1为原优化问题的解；若不满足则令x^k＝x^k+1，P^k＝P^k+1返回第3步。

根据上述步骤，本发明所提出的基于Bezier-凸优化的运载火箭上升段轨迹规划方法规划出一条满足各项约束的最优轨迹计算经历了2次迭代，总耗时为0.1180s，以目前最新的改进凸优化方法N-K伪谱凸优化(N-K/PCP)为参考，该方法在相同的参数设置和硬件条件下的计算经历了4次迭代，总耗时为4.8103s，由此证明了本发明所提方法具有收敛速度快，求解效率高的有益效果。

图2给出了基于Bezier-凸优化的运载火箭上升段轨迹规划方法规划出的最优三维轨迹曲线，可以清楚地看到，本发明方法与参考方法的优化结果一致且满足终端位置约束；图3和图4给出了本发明方法规划的最优速度曲线和航迹倾角曲线，可以看出本发明方法与参考方法优化结果基本相同；图5给出了本发明方法规划的攻角和倾侧角变化曲线，可以看到本发明方法相比于参考方法优化的控制量更加平滑，这得益于Bezier曲线光滑的特性；图6给出了本发明方法的动压，轴向推力加速度和弯矩变化曲线，可以看到三者都严格满足了过程约束。

为了进一步说明本发明方法的快速性和精确性，在不同离散点数量下对比了本发明方法与参考方法N-K/PCP方法的求解时间，性能指标和终端误差；图7给出了两种方法的计算时间随离散点数量的变化曲线，可以看出本发明方法求解时间远小于参考方法，且随着离散点数量增多，这种优势越明显；图8给出了两种方法优化的最大终端速度对比曲线，可以看出本发明方法优化的性能指标略小于参考方法，但由于计算时间的大幅降低，这种结果也是可以接受的；图9和图10给出了两种方法的终端位置误差和终端速度误差，此处的终端误差为最优控制量的龙格库塔积分解与优化解之间的误差，可以看出本发明方法的终端误差均保持在允许的误差范围内且远小于参考方法的终端误差。

本发明提供的一种基于Bezier-凸优化的运载火箭上升段轨迹规划方法，与现有技术相比，通过将传统凸优化与N-K方法和Bezier曲线相结合，利用N-K方法处理高度非线性的动力学微分方程，将其转化为关于状态增量和控制增量的线性微分方程，且具有稳定收敛的形式，提升了方法的收敛速度；利用Bezier曲线代替控制量曲线，由于Bezier曲线可以由少数控制点决定，因此优化控制量减少为少量的Bezier控制点，减少了优化变量的数量，同时由于Bezier曲线的凸包性，部分过程约束和控制量约束也减少为关于控制点的约束，有效地降低了凸优化问题的求解规模，极大地提升了每步迭代凸优化子问题的求解效率，该方法具有在线求解复杂约束的轨迹规划问题的潜力。

Claims (1)

1.一种基于Bezier-凸优化的运载火箭上升段轨迹规划方法，其特征在于，包括以下步骤：

步骤1：在发射惯性坐标系下，建立运载火箭上升段动力学模型如下：

式中，自变量为运载火箭的高度h，x和y为运载火箭在地面的投影位置，v为运载火箭的速度，γ为运载火箭航迹倾角，χ为运载火箭航向角，m为运载火箭的质量，α为攻角，σ为倾侧角，T为推力，I_sp为发动机比冲，g₀为地球引力常数，G＝mg为重力，L＝ρv²S_refC_L/2为升力，D＝ρv²S_refC_D/2为阻力，其中ρ为大气密度，S_ref为运载火箭的参考面积，C_L和C_D分别为升力系数和阻力系数；

式(1)简写为

其中x＝[x,y,v,γ,χ,m]^T为状态量，u＝[u₁,u₂,u₃,T]^T为控制量，u₁＝cosα，u₂＝sinαcosσ，u₃＝sinαsinσ；

步骤2：建立运载火箭上升段轨迹规划的最优控制问题P0，性能指标为终端速度最大，约束方程包括：运载火箭上升段动力学方程、初始和终端状态约束方程、过程约束方程、控制量约束方程；

最优控制问题P0表示为：

min J＝-v(h_f)

x(h₀)＝x₀

α_min≤α≤α_max,σ_min≤σ≤σ_max,T_min≤T≤T_max

Q_α＝|qα|≤Q_αmax

x(h_f)＝x_f,y(h_f)＝y_f,γ(h_f)＝γ_f,χ(h_f)＝χ_f,m(h_f)≥m_dry (2)

其中，x₀＝[x₀,y₀,v₀,γ₀,χ₀,m₀]^T为运载火箭期望的初始状态量；攻角α约束的上下限为±90°，α_min＝-90°，α_max＝90°，该约束能表示为控制量u₁的约束：u₁≥0；倾侧角σ约束的上下限为±90°，σ_min＝-90°，σ_max＝90°，该约束能表示为控制量u₂和u₃的约束：-1≤u₂≤1，-1≤u₃≤1；此外，三个控制量还满足约束

T_min和T_max为推力的最小值和最大值；过程约束包括动压约束q≤q_max，轴向推力加速度约束a_axial≤a_max和弯矩约束Q_α≤Q_αmax，q_max为动压允许的最大值，a_max为轴向推力加速度允许的最大值，Q_αmax为弯矩允许的最大值；x_f，y_f，γ_f，χ_f为运载火箭期望的终端状态量，m_dry为运载火箭的结构质量；h₀和h_f表示初始高度和终端高度；

步骤3：用Bezier曲线代替最优控制问题P0的控制量u的曲线，运载火箭上升段轨迹规划问题的控制变量变为Bezier曲线的控制点：

控制量约束方程变为关于Bezier曲线控制点的约束方程，从而得到基于Bezier的最优控制问题P1；

步骤3-1：所述Bezier曲线的表达式为：

其中，P_i为Bezier曲线的控制点，

为伯恩斯坦多项式；t为归一化的自变量，n为控制点个数；

将运载火箭上升段动力学模型的自变量归一化处理，令ω＝(h-h₀)/(h_f-h₀)，ω为动力学模型归一化后的自变量，此时新的性能指标变为J＝-v(1)，新的动力学模型变为x'＝dx/dω＝(h_f-h₀)f(x,u)；

步骤3-2：用Bezier曲线的控制点

代替控制量u＝[u₁,u₂,u₃,T]^T：

其中，P_kj是Bezier曲线的控制点；

为伯恩斯坦多项式；

将式(4)表示成矩阵的形式：

u_s(ω):＝W(ω)P_s；P_s＝[P_s0,…,P_sn]^T

步骤3-3：Bezier曲线控制点的约束方程为：

轴向推力加速度约束，

弯矩约束，

推力约束，

控制量约束，

控制量约束，

步骤3-4：基于Bezier的最优控制问题P1表示为：

min J＝-v(1)

s.t.x'＝(h_f-h₀)f(x,P)

x(0)＝x₀,x(1)＝x_f,y(1)＝y_f,γ(1)＝γ_f,χ(1)＝χ_f,m(1)≥m_dry

WP₄-mgα_max≤0,

T_min≤P_4j≤T_max,0≤P_1j≤1,-1≤P_2j≤1,-1≤P_3j≤1,

步骤4：利用N-K方法对运载火箭上升段动力学方程和过程约束进行凸化，得到关于状态增量δx和控制增量δP的线性微分方程和凸的过程约束，利用松弛技巧对控制量约束进行凸化，得到凸的控制量约束，从而得到基于Bezier曲线的子凸优化问题P2；

步骤4-1：利用N-K方法对运载火箭上升段动力学方程和过程约束进行凸化：

定义非线性算子F[x,P]＝x′-f(x,P)＝0，对非线性算子进行广义泰勒展开并忽略高阶项：

F[x+δx,P+δP]＝F[x,P]+F_x(x,P)[δx]+F_P(x,P)[δP]＝0 (8)

其中，δx和δP分别为状态量和控制量的增量，F_x(x,P)[δx]和F_P(x,P)[δP]为Frechet导数；

步骤4-2：将式(9)和式(10)带入广义泰勒展开式(8)，得到关于状态增量δx和控制增量δu^*的线性微分方程：

步骤4-3：基于Bezier曲线的子凸优化问题P2表示为：

min J＝-δv(1)

δx(0)＝x₀-x(0)

δx(1)＝x_f-x(1),δy(1)＝y_f-y(1),δγ(1)＝γ_f-γ(1),δχ(1)＝χ_f-χ(1),δm(1)≥m_dry-m(1)

W(P₄+δP₄)-(m+δm)gα_max≤0,

T_min≤P_4j+δP_4j≤T_max,0≤P_1j+δP_1j≤1,-1≤P_2j+δP_2j≤1,-1≤P_3j+δP_3j≤1

(P_1j+δP_1j)²+(P_2j+δP_2j)²+(P_3j+δP_3j)²≤1 (12)

步骤5：对子凸优化问题P2进行离散化，将自变量进行等距离散得到ω₀,ω₁,…,ω_N，优化变量表示为向量

动力学方程采用欧拉法进行离散，得到基于Bezier曲线的离散子凸优化问题P3表示为：

min J＝c^Tz

δx₁＝x₀-x₁

δx_N＝x_f-x_N,δy_N＝y_f-y_N,δγ_N＝γ_f-γ_N,δχ_N＝χ_f-χ_N,δm_N≥m_dry-m_N

W(P₄+δP₄)-(m_i+δm_i)g_iα_max≤0,

T_min≤P_4j+δP_4j≤T_max,0≤P_1j+δP_1j≤1,-1≤P_2j+δP_2j≤1,-1≤P_3j+δP_3j≤1

(P_1j+δP_1j)²+(P_2j+δP_2j)²+(P_3j+δP_3j)²≤1

其中，i＝1,2,...,N,j＝0,1,…,n，且N＞＞n；N为变量离散点的个数；

步骤6：采用内点法求解基于Bezier曲线的离散子凸优化问题P3，将该次迭代求得的子问题的解作为下一次迭代的初值，直到迭代收敛至最优解，具体求解过程为：

步骤6-1：令迭代步数k＝0，给定状态量和控制量的初始猜想x⁰和P⁰，并令初始猜想x^k＝x⁰，P^k＝P⁰；

步骤6-2：在第k+1步迭代过程中，将所述初始猜想x^k和P^k结合凸优化求解器--MOSEK求解器，求解所述基于Bezier曲线的离散子凸优化问题P3，得到状态量增量δx和控制量增量δP；

步骤6-3：更新状态量和控制量：x^k+1＝x^k+δx，P^k+1＝P^k+δP；

步骤6-4：判断是否满足收敛条件：max(δx_i)≤ε，若满足该条件，则停止迭代，x^k+1和P^k+1为优化问题的解；若不满足则令x^k＝x^k+1，P^k＝P^k+1；返回步骤6-2。

Images