CN112001029A - 一种基于凸优化的火箭在线轨迹优化定制化求解器 - Google Patents

Abstract

本发明提供一种基于凸优化的火箭在线轨迹优化定制化求解器，涉及火箭轨迹优化技术领域，本发明针对火箭垂直回收在线制导问题，从凸优化求解算法出发，通过凸优化求解了火箭回收着陆制导问题，并利用内点法建立了定制化凸优化求解器，使该制导算法具备实时性。本发明的有益效果是：该发明提高了火箭轨迹优化问题的求解效率，同时保证了算法的收敛性。

Description

一种基于凸优化的火箭在线轨迹优化定制化求解器

技术领域

本发明涉及火箭轨迹优化技术领域，尤其涉及一种基于凸优化的火箭在线轨迹优化定 制化求解器。

背景技术

近年来，航天任务不断扩展，空间侦察、深空探测、气象预报、空间通信等任务对运载火箭能力提出了更高的要求。空间探测的发展为轨迹优化提供了具有挑战性的应用场景，数值理论、计算能力的提高为解决新的轨迹优化问题提供了工具。近年来，“计算制 导与控制(Computational Guidance and Control,CG&C)”这一新的思路已成为主流发 展方向之一。相对传统的基于解析或简化模型的数值计算方法，CG&C方法利用先进的计算 平台和高效率算法，基于模型或数据进行计算，以实现预期的制导与控制结果，在线轨迹 优化为CG&C的重要组成部分。以往被用于飞行任务总体设计与分析的轨迹优化方法，在 CG$&$C的框架下，如果其计算效率能够满足控制周期的要求，就能利用轨迹优化程序输 出的最优状态和控制轨迹结果进行在线标称轨迹更新，或者直接作为制导指令。实现CG&$C 主要有三个要点：保证算法的计算效率；保证算法的求解精度；设计合理的制导策略。在 线轨迹优化主要关注前两点。在现有数值理论和设备的发展基础上，快速的计算能力是在 线应用的前提，需要在预期时间内保证算法达到设定的收敛精度，同时能有效应对系统偏 差和干扰；算法输出的最优控制变量需满足系统对制导指令的要求，这对算法的求解精度 提出了高要求。

近年来，凸优化在火箭轨迹优化方面取得了很多研究成果。刘新福提出建立控制-仿 射模型以提高逐次线性化收敛性能等诸多设计要点，解决了二维平面内火箭着陆问题；Szmuk将将算法扩展至了自由终端时间的六自由度火箭子级着陆轨迹优化问题；哈尔滨工业大学的王劲博对可重复使用运载火箭的轨迹优化和制导算法进行了研究，构建了伪谱-改进凸优化算法；浙江大学的马林研究了可回收火箭的轨迹优化问题，设计了运载火箭轨迹优化联立计算框架；北京宇航控制系统研究所的张志国将凸优化应用到火箭垂直软着陆在线制导；清华大学龚胜平、宋雨等将凸优化运用到火箭动力软着陆在线制导，并完成了GNC动力学仿真。

定制化求解器是相对于通用化求解器来说的，是指针对特定问题的设置条件和具体结 构，显示实现代码运码和静态内存分配的求解实现方式。定制化是在底层算法求解器的基 础上，针对特定问题在求解器外围进行封装。

本发明基于不依赖外部库的开源内点法求解器，完成了火箭在线轨迹优化问题的在线 求解。定制化求解火箭轨迹优化问题的基本流程如附图1所示。

利用凸优化方法定制化求解火箭轨迹优化问题之前，需要先了解航天器轨迹优化问题 的基本形式，并分析非凸因素的来源。航天器轨迹优化问题可表达为如下形式：

Problem 0

s.t.

s₁(x(t),u_t,t)≤0 [3]

s₂(x(t),u_t,t)≤0 [4]

ψ₁(x(t_f),t_f)≤0 [5]

ψ₂(x(t_f),t_f)＝0 [6]

不同的航天器轨迹优化问题具有特定的目标函数、动力学方程约束和其它约束，最优 控制问题的非凸性来源需要具体分析。

1、如果目标函数式中的函数

和τ在离散后是非线性的，则问题是非凸的。

2、航天器动力学方程形式如式[2]，大多数都是非线性的，在火箭轨迹优化领域也是 如此。非线性动力学方程在离散后将变成非线性等式约束，是最优控制问题非凸性的主要 来源。

3、不同的轨迹优化问题对应特定的约束如式[3]-[6]，约束的非凸性也会导致问题的 非凸。

凸优化方法在效率和收敛性方面优势，为了应用凸优化方法求解航天器轨迹优化问 题，需要将问题中的非凸因素进行凸化处理，得到离散后的凸的最优控制问题。

时间t是凸优化问题中的重要参数，Problem 0中的t为自变量，在航天器轨迹优化问 题中，当时间固定时，选择时间作为自变量；当时间不固定时，可以选择在最优控制问题外部添加循环搜索或者把时间作为优化变量的方法进行求解。

本发明针对火箭垂直回收在线制导问题，从凸优化求解算法出发，通过凸优化求解了 火箭回收着陆制导问题，并利用内点法建立了定制化凸优化求解器，使该制导算法具备实 时性。

发明内容

本发明的目的在于提供一种基于凸优化的火箭在线轨迹优化定制化求解器，主要解决 的问题是：

1、定制化求解火箭在线轨迹优化问题。

2、测试了算法的鲁棒性。

3、测试了算法的在线计算能力。

本发明为解决上述技术问题，采用以下技术方案来实现：

一种基于凸优化的火箭在线轨迹优化定制化求解器，包括如下步骤：

步骤一，建立火箭着陆段的动力学模型及火箭回收制导问题的最优控制问题模型；

步骤二，凸化处理最优控制问题模型；

步骤三，定制化求解凸优化问题。

进一步的，所述步骤一具体为：

首先，建立火箭动力软着陆段的动力学模型：

其中，r和v分别表示火箭运动的位置和速度矢量，g表示重力加速度矢量，T为火箭发动 机推力矢量，m表示火箭的质量，a_D为气动阻力矢量，I_sp表示火箭发动机比冲，g₀表示地 球海平面重力加速度常数。

针对式(1)的火箭动力学模型，建立燃料最优的火箭回收制导问题最优控制问题模型 如下：

目标函数：

min J＝-m(t_f)\*MERGEFORMAT(2)

状态约束：

控制量约束：

T_min≤||T(t)||≤T_max\*MERGEFORMAT(6)

其中，t₀为着陆起始时刻，t_f为火箭飞行时间，r₀、v₀、m₀分别表示着陆起始位置、速度矢量和火箭起始质量，m_dry表示火箭干重，β表示火箭飞行航迹的最大侧滑角；以垂直地面向上为坐标y轴，x轴指向正北方向，z轴与x、y轴呈右手坐标系，r_x、r_y和r_z分别表示任 意时刻t时，位置矢量的三个分量；T_min和T_max分别表示推力幅值的上下限约束，γ表示推力 方向与垂直方向的最大摆角，T_x、T_y和T_z分表表示推力矢量的三个分量。

进一步的，所述步骤二具体为：

步骤一所建立的火箭回收制导问题的最优控制问题模型，具有如式(1)所示的非线性 的动力学方程约束和如式(6)所示的推力幅值的非凸约束。因此，步骤二在步骤一所建立 的非凸问题基础上，对原问题进行凸化处理。

对于式(1)的非线性动力学方程约束，主要采用逐次凸化的方法。首先将动力学方程 离散，使连续时间问题转化为离散问题，且通过离散的差分方程，使得原隐含的时间自由 变量显含。取时间域上的N个离散点，每个离散点间的时间间隔表示为：

将式(1)写成Δt的展开形式，并取一阶项作为近似，可以表示为：

其中，x用以表示式(1)中状态量，即

用f表示式(1)中方程的右端项，k表 示第k个离散点，k的范围为1～N-1。

在式(9)中，等式右端第二项仍为非线性约束，因此可以通过线性化，取状态量x、控 制量u和时间项Δt的泰勒展开一阶近似，得出如下表达式：

A_kx[k]+A_k+1x[k+1]+B_ku[k]+B_k+1u[k+1]+C·Δt+D＝0\*MERGEFORMAT(10)

其中：

D＝x'[k]-x'[k+1]-A_kx'[k]-A_k+1x'[k+1]-B_ku'[k]-B_k+1u'[k+1]-CΔt'\*MERGEFORMAT(16

其中，上标“'”用来表示前一次迭代求解得到的已知量。

从式(11)～(16)可以看出，式(10)中所有未知变量前的系数矩阵均为常数矩阵。方程 为等式线性约束，满足凸优化问题的等式约束要求。由于逐次凸化过程中，问题被做了大 量的近似处理，且状态量和控制量均受到严格的约束，在逐次逼近迭代求解过程中，极易 出现前几次迭代无可行解的问题。为避免此类问题，Acikmese等人提出了一种松弛技术， 即在控制力之外，添加一项虚拟的控制力，并在目标函数中对该项施加较大的惩罚项系数， 很好的解决了收敛性的问题。本专利中动力学方程的凸化处理中，同样采取了文献中的松 弛方法，不再赘述。

对于式(6)形式的非凸推力幅值约束，则主要采用无损凸化方法。引入松弛变量Г， 将约束做松弛处理，如式(17)所示：

同时，式(1)中质量的变化率中的推力项也由新的松弛变量代替：

通过庞特里亚金极大值原理可以证明，上述松弛变换前后，最终的收敛解为原问题的 解，即松弛变换后的问题，一定会收敛到使得不等式(17)活跃的最优解上，因此被称为无 损松弛。证明过程此处不再赘述。

通过上述步骤二的凸化处理，原连续时间最优控制问题，被转化为一系列离散点上的 二阶锥优化问题(Second Order Cone Problem，SOCP)，形式如下：

subject to：

其中η_u和η_Δt分别为控制量和时间的信赖域，a_v为虚拟加速度，ω_u、ω_Δt和ω_av为惩罚 项系数；对于该SOCP问题，可通过内点法进行求解，从而获得基于给定初始状态到着陆目 标的最优控制量曲线u^*。

进一步的，所述步骤三具体为：

对于步骤二所得到的SOCP问题，基于内点法将问题进行定制求解，建立一种面向嵌入 式计算定制化的求解器，使算法具备实时性和可在线计算性。

通常情况下，采用通用凸优化求解器求解SOCP问题的一般过程如附图2a所示。通用 凸优化求解器一般具有较为友好的解释性语言接口，用户只需要将凸优化问题描述通过解 释性语言描述，求解器会通过解释性语言翻译、离散幅值等前处理，将该问题转化为一系 列子问题实例，并通过内点法求解。而求解SOCP问题完成对原问题的求解，通常需要经 过数次迭代，则需要将上次求解的结果作为最新初值，重复上一过程，直到满足优化问题 的收敛条件。不难看出，该方法存在大量的重复性计算，因此计算效率很低，不具备算法实时求解性，且问题的求解通常依赖于特定的通用求解器，不具备可在线计算性。

本发明提出的定制化凸优化求解器求解特定SOCP的过程如附图2b所示。区别于上述 通用凸优化求解器，定制化求解器针对特定SOCP问题，在进行步骤二的离散和凸化处理之后，将问题的直接描述为一系列子问题实例。对于每个离散点上的子问题实例，如式(10)中出现的系数矩阵，采用列压缩算法，可得到分别记录非零元素值、非零元素列索引、列 首个非零元素数组索引的三个数组，利用该三个数组即能反映稀疏矩阵的全部信息，极大 的减少了存储空间。该数组信息可直接作为内点法求解时的矩阵描述信息使用。此外，该 数组信息对特定的SOCP问题，只有记录非零元素值的数值会随着迭代而变化，其余部分 均为固定的常数数组。因此，在反复的迭代过程中，仅需要修改这一数组的信息即可，极 大地提高了计算的效率。相比于通用化求解器每次求解均重复一次解释性语言翻译、调用 额外库函数支持的情况，定制化求解仅需要每次更新固定数组中的某些元素即可，且不依 赖于额外的库函数，具备高效快速、轻量型、可嵌入式计算等特点。

定制化目的是提供内点法求解器求解时所需接口，主要分为两个步骤，一是提取出 SOCP问题的相关描述参数以及描述不等式约束的矩阵G和向量h、描述等式约束的矩阵A和向量b、描述目标函数的向量C；二是通过列压缩函数得到描述G矩阵的Gpr、Gjc、Gir 数组和描述A矩阵的Apr、Ajc、Air数组。

本发明的有益效果是：

该发明提高了火箭轨迹优化问题的求解效率，同时保证了算法的收敛性。

附图说明

图1为定制化求解火箭轨迹优化问题的基本流程图；

图2a为通用凸优化求解器求解一般凸优化问题的过程示意图；

图2b为定制化凸优化求解器求解特定凸优化问题的过程示意图；

图3为定制版求解器的求解流程图；

图4为火箭动力软着陆轨迹示意图；

图5为火箭动力软着陆控制量-时间图；

图6为定制求解器和通用求解器求解结果的状态量变化情况对比图；

图7为定制化求解器蒙特卡洛测试收敛性结果统计示意图；

图8为定制化求解器蒙特卡洛测试轨迹示意图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合 具体实施例和附图，进一步阐述本发明，但下述实施例仅仅为本发明的优选实施例，并非 全部。基于实施方式中的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得 其它实施例，都属于本发明的保护范围。

下面结合附图描述本发明的具体实施例。

定制化求解器是相对于通用化求解器来说的，是指针对特定问题的设置条件和具体结 构，实现代码运码和静态内存分配的求解实现方式。定制化是在底层算法求解器的基础上， 针对特定问题在求解器外围进行封装。

本发明基于不依赖外部库的开源内点法求解器，完成了火箭在线轨迹优化问题的在线 求解。定制化求解火箭轨迹优化问题的基本流程如附图1所示。一般凸优化问题的通用求 解器和定制求解器的求解过程对比如图2a和图2b所示。

利用凸优化方法定制化求解火箭轨迹优化问题之前，需要先了解航天器轨迹优化问题 的基本形式，并分析非凸因素的来源。航天器轨迹优化问题可表达为如下形式：

Problem 0

s.t.

s₁(x(t),u_t,t)≤0 [3]

s₂(x(t),u_t,t)≤0 [4]

ψ₁(x(t_f),t_f)≤0 [5]

ψ₂(x(t_f),t_f)＝0 [6]

不同的航天器轨迹优化问题具有特定的目标函数、动力学方程约束和其它约束，最优 控制问题的非凸性来源需要具体分析。

1、如果目标函数式中的函数

和τ在离散后是非线性的，则问题是非凸的。

2、航天器动力学方程形式如式[2]，大多数都是非线性的，在火箭轨迹优化领域也是 如此。非线性动力学方程在离散后将变成非线性等式约束，是最优控制问题非凸性的主要 来源。

3、不同的轨迹优化问题对应特定的约束如式[3]-[6]，约束的非凸性也会导致问题的 非凸。

凸优化方法在效率和收敛性方面优势，为了应用凸优化方法求解航天器轨迹优化问 题，需要将问题中的非凸因素进行凸化处理，得到离散后的凸的最优控制问题。

时间t是凸优化问题中的重要参数，Problem 0中的t为自变量，在航天器轨迹优化问 题中，当时间固定时，选择时间作为自变量；当时间不固定时，可以选择在最优控制问题外部添加循环搜索或者把时间作为优化变量的方法进行求解。

本发明针对火箭垂直回收在线制导问题，从凸优化求解算法出发，通过凸优化求解了 火箭回收着陆制导问题，并利用内点法建立了定制化凸优化求解器，使该制导算法具备实 时性。

所采取的技术方案如下：

步骤一：建立火箭着陆段的动力学模型及火箭回收制导问题的最优控制问题模型；

步骤二：凸化处理最优控制问题模型；

步骤三：定制化求解凸优化问题。

步骤一：

首先，所述建立火箭动力软着陆段的动力学模型：

其中，r和v分别表示火箭运动的位置和速度矢量，g表示重力加速度矢量，T为火箭发动 机推力矢量，m表示火箭的质量，a_D为气动阻力矢量，I_sp表示火箭发动机比冲，g₀表示地 球海平面重力加速度常数。

针对式(1)的火箭动力学模型，建立燃料最优的火箭回收制导问题最优控制问题模型 如下：

目标函数：

min J＝-m(t_f)\*MERGEFORMAT(2)

状态约束：

控制量约束：

T_min≤||T(t)||≤T_max\*MERGEFORMAT(6)

其中，t₀为着陆起始时刻，t_f为火箭飞行时间，r₀、v₀、m₀分别表示着陆起始位置、速度矢 量和火箭起始质量，m_dry表示火箭干重，β表示火箭飞行航迹的最大侧滑角；以垂直地面向 上为坐标y轴，x轴指向正北方向，z轴与x、y轴呈右手坐标系，r_x、r_y和r_z分表表示任 意时刻t时，位置矢量的三个分量；T_min和T_max分别表示推力幅值的上下限约束，γ表示推 力方向与垂直方向的最大摆角，T_x、T_y和T_z分表表示推力矢量的三个分量。

步骤二：

步骤一所建立的火箭回收制导问题的最优控制问题模型，具有如式(1)所示的非线性 的动力学方程约束和如式(6)所示的推力幅值的非凸约束。因此，步骤二在步骤一所建立 的非凸问题基础上，对原问题进行凸化处理。

对于式(1)的非线性动力学方程约束，主要采用逐次凸化的方法。首先将动力学方程 离散，使连续时间问题转化为离散问题，且通过离散的差分方程，使得原隐含的时间自由 变量显含。取时间域上的N个离散点，每个离散点间的时间间隔表示为：

将式(1)写成Δt的展开形式，并取一阶项作为近似，可以表示为：

其中，x用以表示式(1)中状态量，即

用f表示式(1)中方程的右端项，k表 示第k个离散点，k的范围为1～N-1。

在式(9)中，等式右端第二项仍为非线性约束，因此可以通过线性化，取状态量x、控 制量u和时间项Δt的泰勒展开一阶近似，得出如下表达式：

A_kx[k]+A_k+1x[k+1]+B_ku[k]+B_k+1u[k+1]+C·Δt+D＝0\*MERGEFORMAT(10)

其中：

D＝x'[k]-x'[k+1]-A_kx'[k]-A_k+1x'[k+1]-B_ku'[k]-B_k+1u'[k+1]-CΔt'\*MERGEFORMAT(16

其中，上标“'”用来表示前一次迭代求解得到的已知量。

从式(11)～(16)可以看出，式(10)中所有未知变量前的系数矩阵均为常数矩阵。方程 为等式线性约束，满足凸优化问题的等式约束要求。由于逐次凸化过程中，问题被做了大 量的近似处理，且状态量和控制量均受到严格的约束，在逐次逼近迭代求解过程中，极易 出现前几次迭代无可行解的问题。为避免此类问题，Acikmese等人提出了一种松弛技术， 即在控制力之外，添加一项虚拟的控制力，并在目标函数中对该项施加较大的惩罚项系数， 很好的解决了收敛性的问题。本专利中动力学方程的凸化处理中，同样采取了文献中的松

弛方法，不再赘述。

对于式(6)形式的非凸推力幅值约束，则主要采用无损凸化方法。引入松弛变量Г， 将约束做松弛处理，如式(17)所示：

同时，式(1)中质量的变化率中的推力项也由新的松弛变量代替：

通过庞特里亚金极大值原理可以证明，上述松弛变换前后，最终的收敛解为原问题的 解，即松弛变换后的问题，一定会收敛到使得不等式(17)活跃的最优解上，因此被称为无 损松弛。证明过程此处不再赘述。

通过上述步骤二的凸化处理，原连续时间最优控制问题，被转化为一系列离散点上的 二阶锥优化问题(Second Order Cone Problem，SOCP)，形式如下：

subject to：

其中η_u和η_Δt分别为控制量和时间的信赖域，a_v为虚拟加速度，ω_u、ω_Δt和ω_av为惩罚项 系数。对于该SOCP问题，可通过内点法进行求解，从而获得基于给定初始状态到着陆目 标的最优控制量曲线u^*。

步骤三：

步骤三则针对于步骤二所得到的SOCP问题，基于内点法将问题进行定制求解，建立 一种面向嵌入式计算定制化的求解器，使算法具备实时性和可在线计算性。

通常情况下，采用通用凸优化求解器求解SOCP问题的一般过程如附图2a所示。通用 凸优化求解器一般具有较为友好的解释性语言接口，用户只需要将凸优化问题描述通过解 释性语言描述，求解器会通过解释性语言翻译、离散幅值等前处理，将该问题转化为一系 列子问题实例，并通过内点法求解。而求解SOCP问题完成对原问题的求解，通常需要经 过数次迭代，则需要将上次求解的结果作为最新初值，重复上一过程，直到满足优化问题 的收敛条件。不难看出，该方法存在大量的重复性计算，因此计算效率很低，不具备算法实时求解性，且问题的求解通常依赖于特定的通用求解器，不具备在线计算能力。

本发明提出的定制化凸优化求解器求解特定SOCP的过程如附图2b所示。区别于上述 通用凸优化求解器，定制化求解器针对特定SOCP问题，在进行步骤二的离散和凸化处理之后，将问题的直接描述为一系列子问题实例。对于每个离散点上的子问题实例，如式(10)中出现的系数矩阵，采用列压缩算法，可得到分别记录非零元素值、非零元素列索引、列 首个非零元素数组索引的三个数组，利用该三个数组即能反映稀疏矩阵的全部信息，极大 的减少了存储空间。该数组信息可直接作为内点法求解时的矩阵描述信息使用。此外，该 数组信息对特定的SOCP问题，只有记录非零元素值的数值会随着迭代而变化，其余部分 均为固定的常数数组。因此，在反复的迭代过程中，仅需要修改这一数组的信息即可，极 大地提高了计算的效率。相比于通用化求解器每次求解均重复一次解释性语言翻译、调用 额外库函数支持的情况，定制化求解仅需要每次更新固定数组中的某些元素即可，且不依 赖于额外的库函数，具备高效快速、轻量型、可嵌入式计算等特点。

定制化目的是提供内点法求解器求解时所需接口，主要分为两个步骤，一是提取出 SOCP问题的相关描述参数以及描述不等式约束的矩阵G和向量h、描述等式约束的矩阵A和向量b、描述目标函数的向量C；二是通过列压缩函数得到描述G矩阵的Gpr、Gjc、Gir 数组和描述A矩阵的Apr、Ajc、Air数组。

定制版求解器的求解流程如附图3，主要函数实现以下几个功能。

函数名称：DataMatrix()

函数功能：输入接口。

函数名称：IterErr()

函数功能：计算控制量迭代误差，迭代误差的大小为结束迭代的判据。

函数名称：ECOS_setup()

函数功能：定制化内点法求解器初始化。

函数名称：ECOS_solver()

函数功能：定制化内点法求解器求解。

函数名称：ECOS_cleanup()

函数功能：定制化内点法求解器释放内存。

函数名称：DataInit()

函数功能：求解器的输入函数，输入归一化参数、火箭初始参数、初末点参数。

火箭参数以及相关计算参数的输入接口在DataInit()函数中，主要有以下参数，如表 1：

表1定制版求解器火箭主要参数定义

不同的离散点数，输出量个数不一样，当离散点数n时，输出参数如表2：

表2定制版求解器输出参数定义

本求解器具有轻量型、不依赖外部库，具备嵌入式计算的特点，可根据不同需求调整 输出。

单次迭代完成后，求解函数输出定义为：

表3求解函数输出参数定义

前期通过凸化处理，得到了火箭着陆段连续时间轨迹优化问题，写成如下有限维形式 的SOCP问题：

s.t.

其中[1]、[2]、[3]为等式约束，当离散点数为NumD时，经计算得 A∈R^{(7NumD +6)×(16NumD+2)}；[4]、[5]、[6]、[7]、[8]为一维不等式约束及锥约 束，经计算得G∈R^{(20NumD +3)×(16NumD+2)}，其它描述SOCP问题规模的参数如表4：

表4定制版问题规模参数

然后利用列压缩函数压缩存储G矩阵和A矩阵，分别得到Gpr、Gjc、Gir数组和Apr、Ajc、Air数组，对应不同离散点数，矩阵中非零元素个数也不同，非零数组维数能由压缩 函数计算得到，对应离散点20个的情况，如表5所示：

表5问题定制压缩数组信息

求解器输入接口所需的描述不等式约束的向量h、描述等式约束的向量b、描述目标 函数的向量C能直接得到，向量维数如表6：

表6问题定制向量信息

至此，已经完全的得到内点法求解器所需数据，完成火箭回收制导问题的定制化。

上述步骤三建立的定制化求解器，能够实现对步骤二所述的SOCP问题的实时、在线 求解。具体测试效果见测试算例。

测试算例1：有效性测试

以坐标轴原点作为火箭着陆段的目标落点，考虑火箭从起始位置r₀＝[500m,1600m, 0m]^T，以初速度v₀＝[-5m/s,26.7m/s,0m/s]^T为着陆段起始位置，仿真参数同表5.1所示。 本仿真算例中，飞行时间收敛的精度设置为1E-6，控制量迭代精度为1E-6，离散点个数 取20个。其中，用于求解效率对比的通用求解器采用了CVX中的SDPT3求解器，仿真结 果对比如表5和图4—图6所示，其中图6中x、y、x分别表示火箭的三个位置分量，Vx、 Vy、Vz分别表示火箭的三个速度分量。

表5定制求解器和通用求解器结果对比

图5中控制量满足bangbang控制，符合燃料最优问题的一般规律。从图6中的对比结果来看，两种求解器得到了吻合一致的优化结果，火箭最终的落点位置均为0，且三个 方向上的速度也为0，符合精确软着陆的要求。

测试算例2：鲁棒性测试

为测试算法在不同初始状态下的收敛特性，以验证算法的适应性，对算法进行蒙特卡 洛测试。测试方法为，采用本项目开发的定制版凸优化求解器，封装后作为动态链接库， 通过在指定的高度和速度范围内，随机地生成火箭初始状态，调用定制版凸优化求解器进 行求解器，记录收敛结果和迭代过程。

初始状态在测试算例一初始状态的基础上，随机产生的空间范围和速度范围定义如 下：

其中，高度在1600m高度上下200m范围内随机产生，横向的位置在500m范围内随机产生； 竖直速度在26.7m/s的基础上、±10m/s范围内随机生成，水平速度在±10m/s范围内随 机生成。求解器中，迭代次数上限设置为20次，计算结果迭代次数小于上限的，即为有效收敛解。

随机生成1000组初始状态，测试结果如表7和图7-图8所示。

表7算法蒙特卡洛测试结果

分析算法的蒙特卡洛测试结果，随机产生的1000组初始状态，均得到了收敛结果。迭 代次数和迭代时间的分布直方图如图5所示，最大迭代次数为10次(4个)，最少迭代次数为3次(2个)，平均迭代次数4.679次，平均单次迭代时间为19.381ms。通过算法的 蒙特卡洛测试，验证了算法的鲁棒性。

测试算例3：实时性测试

对于上述算例，反复运行10次，记录运行时间。

表7运行时间

10次求解均能在6次迭代后收敛，平均耗时135.914ms，满足在线求解要求。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本发明不受上述实 施例的限制，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些 变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及 其等效物界定。

Claims (6)

1.一种基于凸优化的火箭在线轨迹优化定制化求解器，其特征在于：包括如下步骤：

步骤一，建立火箭着陆段的动力学模型及火箭回收制导问题的最优控制问题模型；

步骤二，凸化处理最优控制问题模型；

步骤三，定制化求解凸优化问题。

2.根据权利要求1所述的基于凸优化的火箭在线轨迹优化定制化求解器，其特征在于，所述步骤一具体为：

首先，建立火箭动力软着陆段的动力学模型：

其中，r和v分别表示火箭运动的位置和速度矢量，g表示重力加速度矢量，T为火箭发动机推力矢量，m表示火箭的质量，a_D为气动阻力矢量，I_sp表示火箭发动机比冲，g₀表示地球海平面重力加速度常数；

针对式(1)的火箭动力学模型，建立燃料最优的火箭回收制导问题最优控制问题模型如下：

目标函数：

min J＝-m(t_f) \*MERGEFORMAT(2)

状态约束：

控制量约束：

T_min≤||T(t)||≤T_max \*MERGEFORMAT(6)

其中，t₀为着陆起始时刻，t_f为火箭飞行时间，r₀、v₀、m₀分别表示着陆起始位置、速度矢量和火箭起始质量，m_dry表示火箭干重，β表示火箭飞行航迹的最大侧滑角；以垂直地面向上为坐标y轴，x轴指向正北方向，z轴与x、y轴呈右手坐标系，r_x、r_y和r_z分表表示任意时刻t时，位置矢量的三个分量；T_min和T_max分别表示推力幅值的上下限约束，γ表示推力方向与垂直方向的最大摆角，T_x、T_y和T_z分表表示推力矢量的三个分量。

3.根据权利要求2所述的基于凸优化的火箭在线轨迹优化定制化求解器，其特征在于，所述步骤二具体为：

步骤一所建立的火箭回收制导问题的最优控制问题模型，具有如式(1)所示的非线性的动力学方程约束和如式(6)所示的推力幅值的非凸约束；因此，步骤二在步骤一所建立的非凸问题基础上，对原问题进行凸化处理；

对于式(1)的非线性动力学方程约束，主要采用逐次凸化的方法；首先将动力学方程离散，使连续时间问题转化为离散问题，且通过离散的差分方程，使得原隐含的时间自由变量显含；取时间域上的N个离散点，每个离散点间的时间间隔表示为：

将式(1)写成Δt的展开形式，并取一阶项作为近似，可以表示为：

其中，x用以表示式(1)中状态量，即

用f表示式(1)中方程的右端项，k表示第k个离散点，k的范围为1～N-1；

在式(9)中，等式右端第二项仍为非线性约束，因此可以通过线性化，取状态量x、控制量u和时间项Δt的泰勒展开一阶近似，得出如下表达式：

A_kx[k]+A_k+1x[k+1]+B_ku[k]+B_k+1u[k+1]+C·Δt+D＝0 \*MERGEFORMAT(10)

其中：

D＝x'[k]-x'[k+1]-A_kx'[k]-A_k+1x'[k+1]-B_ku'[k]-B_k+1u'[k+1]-CΔt'\*MERGEFORMAT

其中，上标“'”用来表示前一次迭代求解得到的已知量；

从式(11)～(16)可以看出，式(10)中所有未知变量前的系数矩阵均为常数矩阵；方程为等式线性约束，满足凸优化问题的等式约束要求；由于逐次凸化过程中，问题被做了大量的近似处理，且状态量和控制量均受到严格的约束，在逐次逼近迭代求解过程中，极易出现前几次迭代无可行解的问题；

为避免此类问题，Acikmese等人提出了一种松弛技术，即在控制力之外，添加一项虚拟的控制力，并在目标函数中对该项施加较大的惩罚项系数，很好的解决了收敛性的问题；

对于式(6)形式的非凸推力幅值约束，则主要采用无损凸化方法；引入松弛变量Г，将约束做松弛处理，如式(17)所示：

同时，式(1)中质量的变化率中的推力项也由新的松弛变量代替：

通过庞特里亚金极大值原理可以证明，上述松弛变换前后，最终的收敛解为原问题的解，即松弛变换后的问题，一定会收敛到使得不等式(17)活跃的最优解上，因此被称为无损松弛；

通过上述步骤二的凸化处理，原连续时间最优控制问题，被转化为一系列离散点上的二阶锥优化问题(Second Order Cone Problem，SOCP)，形式如下：

subject to：

其中η_u和η_Δt分别为控制量和时间的信赖域，a_v为虚拟加速度，ω_u、ω_Δt和ω_av为惩罚项系数；对于该SOCP问题，可通过内点法进行求解，从而获得基于给定初始状态到着陆目标的最优控制量曲线u^*。

4.根据权利要求3所述的基于凸优化的火箭在线轨迹优化定制化求解器，其特征在于，所述步骤三具体为：

针对于步骤二所得到的SOCP问题，基于内点法将问题进行定制求解，建立一种面向嵌入式计算定制化的凸优化求解器，使算法具备实时性和可在线计算性；

定制化凸优化求解器针对特定SOCP问题，在进行步骤二的离散和凸化处理之后，将问题的直接描述为一系列子问题实例；对于每个离散点上的子问题实例，采用列压缩算法，可得到分别记录非零元素值、非零元素列索引、列首个非零元素数组索引的三个数组，利用该三个数组即能反映稀疏矩阵的全部信息，极大的减少了存储空间；该数组信息可直接作为内点法求解时的矩阵描述信息使用；该数组信息对特定的SOCP问题，只有记录非零元素值的数值会随着迭代而变化，其余部分均为固定的常数数组；在反复的迭代过程中，仅需要修改这一数组的信息即可，极大地提高了计算的效率；

5.根据权利要求4所述的基于凸优化的火箭在线轨迹优化定制化求解器，其特征在于，定制化目的是提供内点法求解器求解时所需接口，主要分为两个步骤，一是提取出SOCP问题的相关描述参数以及描述不等式约束的矩阵G和向量h、描述等式约束的矩阵A和向量b、描述目标函数的向量C；二是通过列压缩函数得到描述G矩阵的Gpr、Gjc、Gir数组和描述A矩阵的Apr、Ajc、Air数组。

6.根据权利要求5所述的基于凸优化的火箭在线轨迹优化定制化求解器，其特征在于，定制版求解器的主要函数实现以下几个功能：

函数名称：DataMatrix()

函数功能：输入接口；

函数名称：IterErr()

函数功能：计算控制量迭代误差，迭代误差的大小为结束迭代的判据；

函数名称：ECOS_setup()

函数功能：定制化内点法求解器初始化；

函数名称：ECOS_solver()

函数功能：定制化内点法求解器求解；

函数名称：ECOS_cleanup()

函数功能：定制化内点法求解器释放内存；

函数名称：DataInit()

函数功能：求解器的输入函数，输入归一化参数、火箭初始参数、初末点参数；

火箭参数以及相关计算参数的输入接口在DataInit()函数中，主要有以下参数，如表1：

表1定制版求解器火箭主要参数定义

不同的离散点数，输出量个数不一样，当离散点数n时，输出参数如表2：

表2定制版求解器输出参数定义

本求解器具有轻量型、不依赖外部库，具备嵌入式计算的特点，可根据不同需求调整输出。

单次迭代完成后，求解函数输出定义为：

表3求解函数输出参数定义

前期通过凸化处理，得到了火箭着陆段连续时间轨迹优化问题，写成如下有限维形式的SOCP问题：

s.t.

其中[1]、[2]、[3]为等式约束，当离散点数为NumD时，经计算得A∈R^{(7NumD+6)×(16NumD+2)}；[4]、[5]、[6]、[7]、[8]为一维不等式约束及锥约束，经计算得G∈R^{(20NumD+3)×(16NumD+2)}，其它描述SOCP问题规模的参数如表4：

表4定制版问题规模参数

然后利用列压缩函数压缩存储G矩阵和A矩阵，分别得到Gpr、Gjc、Gir数组和Apr、Ajc、Air数组，对应不同离散点数，矩阵中非零元素个数也不同，非零数组维数能由压缩函数计算得到，对应离散点20个的情况，如表5所示：

表5问题定制压缩数组信息

求解器输入接口所需的描述不等式约束的向量h、描述等式约束的向量b、描述目标函数的向量C能直接得到，向量维数如表6：

表6问题定制向量信息

至此，已经完全的得到内点法求解器所需数据，完成火箭回收制导问题的定制化。

Images