CN111338364A - 快速响应的高超声速飞行器轨迹优化高精度控制器 - Google Patents

Abstract

本发明公开了快速响应的高超声速飞行器轨迹优化高精度控制器，用于对飞行器轨迹进行控制。所述的快速响应的高超声速飞行器轨迹优化高精度控制器由飞行器海拔高度传感器、飞行器速度传感器、飞行器飞行航道倾角传感器、飞行器水平飞行距离传感器、飞行器微控制单元(MCU)、飞行器攻角控制器构成。高超声速飞行器到达再入段空域后，飞行器MCU自动执行内部优化算法，得到使高超声速飞行器航程最远的轨迹优化控制策略，飞行器MCU将获得的控制策略转换为控制指令发送给飞行器攻角控制器执行。本发明能够根据高超声速飞行器不同的海拔高度、速度、飞行航道倾角和飞行水平距离状态快速地得到轨迹优化控制策略，使高超声速飞行器获得更远航程。

Description

快速响应的高超声速飞行器轨迹优化高精度控制器

技术领域

本发明涉及高超声速飞行器再入段轨迹优化领域，尤其涉及快速响应的高超声速飞行器轨迹优化高精度控制器。在高超声速飞行器到达再入段后能够给出高超声速飞行器轨迹优化控制策略并转换为飞行器攻角控制指令，在满足安全要求的条件下，使高超声速飞行器获得更长的水平飞行距离。

背景技术

高超声速飞行器是实现远程快速精确打击和全球快速到达的新型飞行器，在未来的军事、政治和经济中具有十分重要的战略地位，已经成为世界航空航天领域一个极其重要的发展方向，是世界主要航天大国的竞争领域之一。研究和发展高超声速飞行器在开发太空和国家安全方面具有非常重要的意义。

在高超声速飞行器的研究中，轨迹优化是现代飞行器设计和控制的重要内容，不仅有利于提高飞行器飞行品质以满足既定任务要求，同时也是完成飞行任务的重要保证和实现机动飞行的必要条件，近些年来一直受到国内外各军事强国的重视，是当前国内外研究的热点和难点。

由于从飞行器从外缘进入大气层，高度和速度的变化范围很大，高超声速飞行器面临各种严峻的再入环境，再入段轨迹优化技术则是保证高超声速飞行器顺利完成飞行任务的关键，对提高其打击范围和落点精度具有重要的实用价值。因此，研究高效的高超声速飞行器再入段轨迹优化方法显得尤为重要。

发明内容

为了使高超声速飞行器获得更远的航程，提高高超声速飞行器的打击范围，本发明的目的在于提供一种实现快速响应的高超声速飞行器轨迹优化高精度控制器。该控制器借助MCU作为最优控制方法的实现载体。高超声速飞行器再入段轨迹优化问题飞行过程可以用数学模型描述为：

其中t表示时间，t₀表示高超声速飞行器再入段轨迹优化问题飞行过程开始时间，t_f表示高超声速飞行器再入段轨迹优化问题飞行过程结束时间，且t_f不固定；

被称为状态变量，依次表示飞行器海拔高度、飞行器速度、飞行器飞行航道倾角、飞行器水平飞行距离等物理参数，n_x为状态变量的维度，这里n_x＝4。x₀是状态向量的初始值，

是其一阶导数；u(t)表示高超声速飞行器的攻角控制量，为本问题的控制变量，u_l、u_u分别为其下限值和上限值；

是根据能量守恒以及力学原理建立的微分方程组；G[u(t),x(t),t]是高超声速飞行器再入段过程中必须满足的不等式路径约束条件。

对于高超声速飞行器再入段过程，使航程最大的数学模型可表示为：

其中J[u(t)]表示目标函数J由攻角控制量u(t)决定。该问题本质上是一个最优控制问题。

本发明解决其技术问题所采用的技术方案是：一种快速响应的高超声速飞行器轨迹优化高精度控制器，它由飞行器海拔高度传感器、飞行器速度传感器、飞行器飞行航道倾角传感器、飞行器水平飞行距离传感器、高超声速飞行器MCU、飞行器攻角控制器构成。飞行器海拔高度传感器、飞行器速度传感器、飞行器飞行航道倾角传感器、飞行器水平飞行距离传感器和飞行器攻角控制器均通过数据总线与高超声速飞行器MCU相连。所述飞行器微控制单元MCU包括依次相连的信息采集模块、初始化模块、高精度模块、非线性规划(Non-linear Programming，简称NLP)问题求解模块、快速响应模块、控制策略输出模块。

所述快速响应的高超声速飞行器轨迹优化高精度控制器所述装置的运行过程如下：

步骤1)：在高超声速飞行器MCU中输入对应于该飞行器的气动系数模型、飞行器性能约束条件、指定优化目标；

步骤2)：高超声速飞行器到达再入段后，开启飞行器海拔高度传感器、飞行器速度传感器、飞行器飞行航道倾角传感器和飞行器水平飞行距离传感器，得到高超声速飞行器当前的海拔高度、速度、飞行航道倾角和飞行水平距离状态信息；

步骤3)：飞行器MCU根据设定的海拔高度、速度、飞行航道倾角要求执行内部优化算法，得到使高超声速飞行器水平飞行距离最长的轨迹优化控制策略；

步骤4)：高超声速飞行器MCU将获得的轨迹优化控制策略发送给控制策略输出模块，并转换为控制指令发送给飞行器攻角控制器执行。

所述步骤3包括以下子步骤：

步骤3.1)：信息采集模块获取高超声速飞行器当前的海拔高度、速度、飞行航道倾角和飞行水平距离状态信息；

步骤3.2)：初始化模块开始运行，设置轨迹优化过程时间的离散段数、攻角控制量的初始猜测值u⁽⁰⁾(t)，设定优化精度要求tol，将迭代次数k置零；

步骤3.3)：通过高精度模块将常微分方程组在时间轴[t₀,t_f]上全部离散；

步骤3.4)：通过NLP问题求解模块获得所需的攻角控制策略和对应状态轨迹，这个过程包括多次内部迭代，每次迭代都要求解寻优方向和寻优步长，并进行寻优修正。对于某一次迭代得到的攻角控制量u^(k)(t)，如果其对应目标函数值J[u^(k)(t)]与前一次迭代的目标函数值J[u^(k-1)(t)]之差小于精度要求tol，则判断收敛性是否满足，若满足则将指令输出到控制策略输出模块26；否则进行下一次迭代；

步骤3.5)：对得到的控制向量u^(k)(t)和状态向量x^(k)(t)进行分析，收敛性性条件满足，将攻角控制量u^(k)(t)作为指令输出；否则执行下一步快速响应模块处理。

所述步骤3.3包括以下子步骤：

步骤3.3.1)：将攻角控制量u(t)、状态轨迹x(t)采用如下插值公式进行离散化，即：

其中N是对时间区间[t₀,t_f]进行离散的段数，M_i为第i段上的配置点数量，α(t)和β(t)满足

和

离散化系数u_i,j和s_i,j分别是u(t)和x(t)在配置点t_i,j上的值，s'_i,j为

在配置点t_i,j上的值，由具体的状态方程通过u(t)和x(t)确定，满足s'_i,j＝f(u_i,j,s_i,j,t_i,j)，其中f为状态方程。

步骤3.3.2)：增加非配置点处的非均匀分布检测点

为第i段上的检测点数量。由步骤3.3.1确定检测点处的控制变量值和状态变量值，通过状态方程得到检测点处导数值的第一种表示

另一方面通过(2)的导数表达式得到检测点处导数值的第二种表示

将

作为新的约束以使得状态变量离散化在检测点处符合状态方程，以得到更高的求解精度。

步骤3.3.3)：将目标函数、其他约束等用u_i,j和s_i,j进行配置点处和检测点处离散表达，与步骤3.3.2得到的新约束共同作为待求解的NLP问题。

NLP求解模块包括寻优方向求解、寻优步长求解、寻优修正、NLP收敛性判断四个子模块，所述步骤3.4包括如下子步骤：

步骤3.4.1)：将攻角控制量u^(k-1)(t)作为向量空间中的某个点，记作P₁，P₁对应的目标函数值就是J[u^(k-1)(t)]；

步骤3.4.2)：从点P₁出发，根据选用的NLP算法，构造向量空间中的一个寻优方向向d^(k-1)和步长λ^(k-1)；

步骤3.4.3)：通过式u^(k)(t)＝u^(k-1)(t)+λ^(k-1)d^(k-1)构造向量空间中对应u^(k)的另外一个点P₂，使得P₂对应的目标函数值J[u^(k)(t)]比J[u^(k-1)(t)]更优。

步骤4)：采用寻优校正u^(k)(t)，得到校正后的点

记为点P₃，同时令

使得P₃对应的目标函数值J[u^(k)(t)]比J[u^(k-1)(t)]更优；

步骤3.4.5)：如果本次迭代的目标函数值J[u^(k)(t)]与上一次迭代的目标函数值J[u^(k-1)(t)]的绝对值之差小于精度tol，则判断收敛性满足，将本次迭代得到的控制策略u^(k)(t)输出至控制策略输出模块26；如果收敛性不满足，迭代次数k增加1，将u^(k)(t)设置为初始值，继续执行步骤3.4.2)。

所述步骤3.5中，所述快速响应模块的处理过程如下：

步骤3.5.1)：设置曲率半径阈值r，误差阈值ε，配置点增加常数N，最大迭代次数l_max。设置l:＝1，f₁＝0,f₂＝0。

步骤3.5.2)：利用NLP求解模块得到第l次迭代的控制向量u_l(t)，得到相应的曲率

当t取配置点的值t_i,j和检测点的值

时，得到ρ_l(t_i,j)和

的值，由此得到所有配置点处和检测点处的曲率半径

和

若存在

或

大于设定的阈值r，则在对应点处进行分段处理，设置f₁＝1；

步骤3.5.3)：对第l次迭代NLP求解模块得到的最优解x_l(t),u_l(t)，定义误差矩阵

其中t_i为第i段的起点，n_x是状态变量的个数。定义误差综合指标

j＝1,…,n_x，其中e_kj为误差矩阵E的第k行第j列的元素。如果存在v_j(j＝1,…,n_x)大于预先给定的阈值ε，则增加N个配置点数，重新计算配置点的分布，设置f₂＝1；

步骤3.5.4)：若f₁＝1或f₂＝1，且l＜l_max，l:＝l+1，返回步骤2，并设置f₁＝0和f₂＝0，否则将u_l(t)作为最终最优控制输出。

本发明的有益效果主要表现在：所述快速响应的高超声速飞行器轨迹优化高精度控制器对高超声速飞行器实现控制，克服高超声速飞行器在面临各种严峻的飞行环境时已有轨迹优化控制器在求解精度较低、响应速度较慢等的不足，获得使高超声速飞行器水平飞行距离更远的轨迹优化攻角控制指令，提高高超声速飞行器打击范围，提高飞行器自主制导的能力，提高飞行器制导的精度，增强制导系统的鲁棒性。

附图说明

图1是快速响应的高超声速飞行器轨迹优化高精度控制器的结构示意图；

图2是快速响应的高超声速飞行器轨迹优化高精度控制器MCU内部模块结构图。

具体实施方式

实施例1

快速响应的高超声速飞行器轨迹优化高精度控制器的结构如图1所示。高超声速飞行器到达再入段空域，高超声速飞行器海拔高度传感器、速度传感器、飞行航道倾角传感器、水平飞行距离传感器和MCU均已开启。信息采集模块立即采集飞行器进入再入段时的初始海拔高度、速度、飞行航道倾角和水平飞行距离，设当前初始时刻t₀＝0s，海拔高度传感器传入MCU的海拔高度为h₀＝80 000m，速度传感器传入MCU的速度为v₀＝6400m/s，飞行航道倾角传感器传入MCU的飞行航道倾角为γ₀＝-0.052rad，水平飞行距离传感器感器传入MCU的水平飞行距离为r₀＝0m；终值时刻t_f高超声速飞行器需要满足的条件为海拔高度设定为h_f＝24 000m，速度设定为v_f＝760m/s，飞行航道倾角设定为γ_f＝-0.08rad；结合飞行器的三维空间运动方程、气动系数模型、飞行器性能约束条件和指定优化目标，得到该问题的数学模型如下：

max J[u(t)]＝x₄(t_f)

s.t.

C_L＝-0.15+3.44u(t)

C_D＝0.29-1.51u(t)+5.87u(t)²

x₁(0)＝80×10³,x₁(t_f)＝24×10³

x₂(0)＝6.4×10³,x₂(t_f)＝760

x₃(0)＝-0.052,x₃(t_f)＝-0.08

x₄(0)＝0

其中L表示升力，D表示阻力，C_L表示升力系数，C_D表示阻力系数。为了便于表述，采用F(x(t),u(t),t)表示高超声速飞行器再入段三维空间运动方程建立的微分方程组数学模型，即：

采用G[u(t),x(t),t]表示高超声速飞行器再入段过程的约束条件，为：

此外，J[u(t)]表示高超声速飞行器轨迹优化的目标函数即优化结束时刻飞行器的水平飞行距离。

信息采集模块21，用于采集当前飞行器海拔高度和速度、当前飞行器飞行航道倾角和飞行水平距离、飞行器海拔高度和速度设定、飞行器飞行航道倾角信息、飞行器的气动系数模型和性能约束条件以及指定优化目标参数。

高超声速飞行器MCU自动产生攻角控制指令的快速响应高精度优化算法如图2所示，其运行步骤如下：

步骤1)：高超声速飞行器到达再入段后，飞行器海拔高度传感器、飞行器速度传感器、飞行器飞行航道倾角传感器和飞行器水平飞行距离传感器开启，信息采集模块21获取初始时刻t₀＝0s时高超声速飞行器海拔高度h₀＝80 000m，速度为v₀＝6400m/s，飞行航道倾角为γ₀＝-0.052rad，水平飞行距离传感器感器水平飞行距离设置为r₀＝0m；终值时刻t_f高超声速飞行器海拔高度要求设定为h_f＝24 000m，速度要求设定为v_f＝760m/s，飞行航道倾角要求设定为γ_f＝-0.08rad；

步骤2)：初始化模块22开始运行，设置轨迹优化过程时间的离散段数、攻角控制量的初始猜测值u⁽⁰⁾(t)，设定优化精度要求tol，将迭代次数k置零；

步骤3)：通过高精度模块23将常微分方程组在时间轴[t₀,t_f]上全部离散；

步骤4)：通过NLP问题求解模块24获得所需的攻角控制策略和对应状态轨迹，这个过程包括多次内部迭代，每次迭代都要求解寻优方向和寻优步长，并进行寻优修正。对于某一次迭代得到的攻角控制量u^(k)(t)，如果其对应目标函数值J[u^(k)(t)]与前一次迭代的目标函数值J[u^(k-1)(t)]之差小于精度要求tol，则判断收敛性是否满足，若满足则将指令输出到控制策略输出模块26；否则进行下一次迭代；

步骤5)：对得到的控制向量u^(k)(t)和状态向量x^(k)(t)进行分析，收敛性性条件满足，将攻角控制量u^(k)(t)作为指令输出；否则执行下一步快速响应处理25。

高精度模块23的运行过程如下：

步骤1)：将攻角控制量u(t)、状态轨迹x(t)采用如下插值公式进行离散化，即：

其中N是对时间区间[t₀,t_f]进行离散的段数，M_i为第i段上的配置点数量，α(t)和β(t)满足

和

离散化系数u_i,j和s_i,j分别是u(t)和x(t)在配置点t_i,j上的值，s'_i,j为

在配置点t_i,j上的值，由具体的状态方程通过u(t)和x(t)确定，满足s'_i,j＝f(u_i,j,s_i,j,t_i,j)，其中f为状态方程。

步骤2)：增加非配置点处的非均匀分布检测点

为第i段上的检测点数量。由步骤1确定检测点处的控制变量值和状态变量值，通过状态方程得到检测点处导数值的第一种表示

另一方面通过(2)的导数表达式得到检测点处导数值的第二种表示

将

作为新的约束以使得状态变量离散化在检测点处符合状态方程，以得到更高的求解精度。

步骤3)：将目标函数、其他约束等用u_i,j和s_i,j进行配置点处和检测点处离散表达，与步骤2得到的新约束共同作为待求解的NLP问题。

NLP求解模块24包括寻优方向求解、寻优步长求解、寻优修正、NLP收敛性判断四个子模块，运行过程如下：

步骤1)：将攻角控制量u^(k-1)(t)作为向量空间中的某个点，记作P₁，P₁对应的目标函数值就是J[u^(k-1)(t)]；

步骤2)：从点P₁出发，根据选用的NLP算法，构造向量空间中的一个寻优方向向d^{(k -1)}和步长λ^(k-1)；

步骤3)：通过式u^(k)(t)＝u^(k-1)(t)+λ^(k-1)d^(k-1)构造向量空间中对应u^(k)的另外一个点P₂，使得P₂对应的目标函数值J[u^(k)(t)]比J[u^(k-1)(t)]更优。

步骤4)：采用寻优校正u^(k)(t)，得到校正后的点

记为点P₃，同时令

使得P₃对应的目标函数值J[u^(k)(t)]比J[u^(k-1)(t)]更优；

步骤5)：如果本次迭代的目标函数值J[u^(k)(t)]与上一次迭代的目标函数值J[u^{(k -1)}(t)]的绝对值之差小于精度tol，则判断收敛性满足，将本次迭代得到的控制策略u^(k)(t)输出至控制策略输出模块26；如果收敛性不满足，迭代次数k增加1，将u^(k)(t)设置为初始值，继续执行步骤2)。

快速响应模块25的运行过程如下：

步骤1)：设置曲率半径阈值r，误差阈值ε，配置点增加常数N，最大迭代次数l_max。设置l:＝1，f₁＝0,f₂＝0。

步骤2)：利用NLP求解模块得到第l次迭代的控制向量u_l(t)，得到相应的曲率

当t取配置点的值t_i,j和检测点的值

时，得到ρ_l(t_i,j)和

的值，由此得到所有配置点处和检测点处的曲率半径

和

若存在

或

大于设定的阈值r，则在对应点处进行分段处理，设置f₁＝1；

步骤3)：对第l次迭代NLP求解模块得到的最优解x_l(t),u_l(t)，定义误差矩阵

其中t_i为第i段的起点，n_x是状态变量的个数。定义误差综合指标

j＝1,…,n_x，其中e_kj为误差矩阵E的第k行第j列的元素。如果存在v_j(j＝1,…,n_x)大于预先给定的阈值ε，则增加N个配置点数，重新计算配置点的分布，设置f₂＝1；

步骤4)：若f₁＝1或f₂＝1，且l＜l_max，l:＝l+1，返回步骤2，并设置f₁＝0和f₂＝0，否则将u_l(t)作为最终最优控制输出。

最后，飞行器MCU将获得的优化轨迹作为指令输出到控制策略输出模块，转换为控制指令发送给攻角控制器，完成轨迹优化的执行。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (3)

1.一种快速响应的高超声速飞行器轨迹优化高精度控制器，其特征在于，它由飞行器海拔高度传感器、飞行器速度传感器、飞行器飞行航道倾角传感器、飞行器水平飞行距离传感器、高超声速飞行器MCU、飞行器攻角控制器构成。飞行器海拔高度传感器、飞行器速度传感器、飞行器飞行航道倾角传感器、飞行器水平飞行距离传感器和飞行器攻角控制器均通过数据总线与高超声速飞行器MCU相连。所述飞行器微控制单元MCU包括依次相连的信息采集模块、初始化模块、高精度模块、非线性规划(Non-linear Programming，简称NLP)问题求解模块、快速响应模块、控制策略输出模块。

所述快速响应的高超声速飞行器轨迹优化高精度控制器所述装置的运行过程如下：

步骤1)：在高超声速飞行器MCU中输入对应于该飞行器的气动系数模型、飞行器性能约束条件、指定优化目标；

步骤2)：高超声速飞行器到达再入段后，开启飞行器海拔高度传感器、飞行器速度传感器、飞行器飞行航道倾角传感器和飞行器水平飞行距离传感器，得到高超声速飞行器当前的海拔高度、速度、飞行航道倾角和飞行水平距离状态信息；

步骤3)：飞行器MCU根据设定的海拔高度、速度、飞行航道倾角要求执行内部优化算法，得到使高超声速飞行器水平飞行距离最长的轨迹优化控制策略；

步骤4)：高超声速飞行器MCU将获得的轨迹优化控制策略发送给控制策略输出模块，并转换为控制指令发送给飞行器攻角控制器执行。

所述步骤3包括以下子步骤：

步骤3.1)：信息采集模块获取高超声速飞行器当前的海拔高度、速度、飞行航道倾角和飞行水平距离状态信息；

步骤3.2)：初始化模块开始运行，设置轨迹优化过程时间的离散段数、攻角控制量的初始猜测值u⁽⁰⁾(t)，设定优化精度要求tol，将迭代次数k置零；

步骤3.3)：通过高精度模块将常微分方程组在时间轴[t₀,t_f]上全部离散；

步骤3.4)：通过NLP问题求解模块获得所需的攻角控制策略和对应状态轨迹，这个过程包括多次内部迭代，每次迭代都要求解寻优方向和寻优步长，并进行寻优修正。对于某一次迭代得到的攻角控制量u^(k)(t)，如果其对应目标函数值J[u^(k)(t)]与前一次迭代的目标函数值J[u^(k-1)(t)]之差小于精度要求tol，则判断收敛性是否满足，若满足则将指令输出到控制策略输出模块26；否则进行下一次迭代；

步骤3.5)：对得到的控制向量u^(k)(t)和状态向量x^(k)(t)进行分析，收敛性性条件满足，将攻角控制量u^(k)(t)作为指令输出；否则执行下一步快速响应模块处理。

所述步骤3.3包括以下子步骤：

步骤3.3.1)：将攻角控制量u(t)、状态轨迹x(t)采用如下插值公式进行离散化，即：

其中N是对时间区间[t₀,t_f]进行离散的段数，M_i为第i段上的配置点数量，α(t)和β(t)满足

和

离散化系数u_i,j和s_i,j分别是u(t)和x(t)在配置点t_i,j上的值，s'_i,j为

在配置点t_i,j上的值，由具体的状态方程通过u(t)和x(t)确定，满足s'_i,j＝f(u_i,j,s_i,j,t_i,j)，其中f为状态方程。

步骤3.3.2)：增加非配置点处的非均匀分布检测点

为第i段上的检测点数量。由步骤3.3.1确定检测点处的控制变量值和状态变量值，通过状态方程得到检测点处导数值的第一种表示

另一方面通过(2)的导数表达式得到检测点处导数值的第二种表示

将

作为新的约束以使得状态变量离散化在检测点处符合状态方程，以得到更高的求解精度。

步骤3.3.3)：将目标函数、其他约束等用u_i,j和s_i,j进行配置点处和检测点处离散表达，与步骤3.3.2得到的新约束共同作为待求解的NLP问题。

2.根据权利要求1所述快速响应的高超声速飞行器轨迹优化高精度控制器，其特征在于：NLP求解模块包括寻优方向求解、寻优步长求解、寻优修正、NLP收敛性判断四个子模块，所述步骤3.4包括如下子步骤：

步骤3.4.1)：将攻角控制量u^(k-1)(t)作为向量空间中的某个点，记作P₁，P₁对应的目标函数值就是J[u^(k-1)(t)]；

步骤3.4.2)：从点P₁出发，根据选用的NLP算法，构造向量空间中的一个寻优方向向d^{(k -1)}和步长λ^(k-1)；

步骤3.4.3)：通过式u^(k)(t)＝u^(k-1)(t)+λ^(k-1)d^(k-1)构造向量空间中对应u^(k)的另外一个点P₂，使得P₂对应的目标函数值J[u^(k)(t)]比J[u^(k-1)(t)]更优。

步骤4)：采用寻优校正u^(k)(t)，得到校正后的点

记为点P₃，同时令

使得P₃对应的目标函数值J[u^(k)(t)]比J[u^(k-1)(t)]更优；

步骤3.4.5)：如果本次迭代的目标函数值J[u^(k)(t)]与上一次迭代的目标函数值J[u^{(k -1)}(t)]的绝对值之差小于精度tol，则判断收敛性满足，将本次迭代得到的控制策略u^(k)(t)输出至控制策略输出模块26；如果收敛性不满足，迭代次数k增加1，将u^(k)(t)设置为初始值，继续执行步骤3.4.2)。

3.根据权利要求1所述快速响应的高超声速飞行器轨迹优化高精度控制器，其特征在于：所述步骤3.5中，所述快速响应模块的处理过程如下：

步骤3.5.1)：设置曲率半径阈值r，误差阈值ε，配置点增加常数N，最大迭代次数l_max。设置l:＝1，f₁＝0,f₂＝0。

步骤3.5.2)：利用NLP求解模块得到第l次迭代的控制向量u_l(t)，得到相应的曲率

当t取配置点的值t_i,j和检测点的值

时，得到ρ_l(t_i,j)和

的值，由此得到所有配置点处和检测点处的曲率半径

和

若存在

或

大于设定的阈值r，则在对应点处进行分段处理，设置f₁＝1；

步骤3.5.3)：对第l次迭代NLP求解模块得到的最优解x_l(t),u_l(t)，定义误差矩阵

其中t_i为第i段的起点，n_x是状态变量的个数。定义误差综合指标

其中e_kj为误差矩阵E的第k 行第j列的元素。如果存在v_j(j＝1,…,n_x)大于预先给定的阈值ε，则增加N个配置点数，重新计算配置点的分布，设置f₂＝1；

步骤3.5.4)：若f₁＝1或f₂＝1，且l＜l_max，l:＝l+1，返回步骤2，并设置f₁＝0和f₂＝0，否则将u_l(t)作为最终最优控制输出。

Images