CN106842926B - 一种基于正实b样条的飞行器轨迹优化方法 - Google Patents

Abstract

本发明涉及一种基于正实B样条的飞行器轨迹优化方法；首先，针对飞行器飞行轨迹规划问题，搭建飞行器飞行的运动学模型和动力学模型，并将其转化为微分平坦系统模型；其次，针对飞行器飞行速度约束问题，将其由非凸约束转化为分段凸约束问题，利用正实B样条性质，将其转化为B样条函数控制点的优化问题；再次，针对飞行器地理环境约束问题，将其转化为B样条函数控制点的优化问题；最后，将系统轨迹规划问题转化为LMI优化问题，得到符合约束的控制点的最优解，并利用B样条函数得到飞行器的最优轨迹。此方法可用于飞行器飞行路径规划过程。

Description

一种基于正实B样条的飞行器轨迹优化方法

技术领域

本发明涉及一种基于正实B样条的飞行器轨迹优化方法，针对飞行器飞行过程中的轨迹规划问题，采用B样条参数化方法，转化为LMI问题，得到最优轨迹，与传统优化方法相比，提高了避障的精确性和安全性，涵盖了更多种类约束的处理，并一次完成在各维度的轨迹规划，提高了效率，增强了实际工程应用价值。

背景技术

飞行器是指在大气层内或(与)大气层外空间(太空)飞行的器械，在大气层内的飞行器主要有：航天运载器、(民用、军用)飞机，各种导弹(地地、地空、地天、天地、空空、空地、各种巡航)、无人机、中高空(临近空间)飞行器、微型飞行器等；在大气层外的飞行器主要有：卫星、天基飞行器系统、月球探测飞行器、深空探测飞行器等。无论在工程生产中还是军事活动中，飞行器都有着重要的地位。尤其在现代防空对战任务中，飞行器在低、中、高空的侦察、作战能力强的特点使得飞行器成为首选。在进行低空操作时，飞行器受到的环境影响很多，加上飞行器本身的性能因素，使得飞行器可行的飞行空间被严格限制。如何安全、快速的飞行至目标区域成为飞行器飞行的重要研究内容。而安全的飞行接近过程在于飞行器的飞行轨迹，因而飞行器的轨迹优化设计在整个飞行器设计环节中占有重要的地位，对总体、气动、控制、动力及结构等多个分系统的设计有着极为重要的影响。同时，飞行器的轨迹规划决定着能否匹配(静态与动态的优化组合)飞行器的各个子系统，使其达到最佳的性能指标(起飞重量最小、燃料消耗量最少、有效载荷最大、最大可靠性、最低成本)。因此，在就飞行器设计而言，轨迹优化设计是其成功与否的重要因素之一，对其有着非常重要的工程应用价值。飞行器安全路径规划是亟需完善的关键技术，关系到低空飞行任务的成功与否。专利申请号为201410457163.2中提出了基于微分平坦与B样条的时间最优桥式吊车轨迹规划方法，但存在两个问题：(1)由于吊车本身对象运动属性，使得此轨迹规划方法是平面轨迹规划，未考虑三维空间的轨迹优化问题；(2)发明利用多项式优化算法直接对B样条控制点进行表示，计算量大，在取控制点较多的情况下效率偏低；专利申请号为201310303352.X中提出了一种基于在线构图的飞行器返航路线规划方法，但其滚动优化中每一时刻的优化结果需要利用上一时刻信息进行迭代，计算量大，效率较低，对处理器计算能力和速度要求高。为解决此问题，亟需设计一种飞行器轨迹优化方法

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供低空状态下飞行器轨迹优化方法，得到光滑性良好、可解决实际工程问题的飞行器优化轨迹。

本发明的技术解决方案为：一种基于正实B样条的飞行器轨迹优化方法，其特征在于：针对飞行器飞行过程中的轨迹规划问题，采用B样条参数化方法，转化为LMI问题，得到最优轨迹，与传统优化方法相比，提高了避障的精确性和安全性，涵盖了更多种类约束的处理，并一次完成在各维度的轨迹规划，提高了效率，增强了实际工程应用价值。

具体包括以下步骤：首先，针对飞行器飞行轨迹规划问题，搭建飞行器飞行的运动学模型和动力学模型，并将其转化为微分平坦系统模型；其次，针对飞行器飞行速度约束问题，将其由非凸约束转化为分段凸约束问题，利用正实B样条性质，将其转化为B样条函数控制点的优化问题；再次，针对飞行器地理环境约束问题，将其转化为B样条函数控制点的优化问题；最后，将系统轨迹规划问题转化为LMI优化问题，得到符合约束的控制点的最优解，并利用B样条函数得到飞行器的最优轨迹。此方法可用于飞行器飞行路径规划过程；具体步骤如下：

第一步，搭建飞行器运动学模型和动力学模型，并将运动学和动力学模型进行微分平坦处理和B样条参数化，得到基于B样条参数化的微分平坦系统模型

假设飞行器在空间机动飞行过程中无侧滑，推力与速度矢量间的夹角为小量，搭建飞行器动力学模型如下：(原表示飞行器速度的v均已改为V)

其中，m为飞行器质量，V(t)为飞行速度，T为发动机推力，D为阻力，L为升力，θ(t)为航迹倾角，ψ(t)为航迹偏角，γ(t)为滚转角；V(t)，θ(t)，ψ(t)为系统状态变量。

在飞行过程中，过载表征可操纵力的大小和方向，因此可以利用过载的概念表征飞行器机动性，定义过载为：

其中，n_x(t)称为切向过载，沿飞行速度方向；n_y(t)称为法向过载，垂直于飞行速度矢量。

利用过载概念，可以将Σ₁变形得到飞行器动力学模型：

其中，n_x(t)，n_y(t)和γ(t)为系统控制量，g为重力加速度。

搭建飞行器运动学模型如下：

其中，x(t)，y(t)和h(t)分别为飞行器质心位置在地面坐标系三轴上的分量。

根据Σ₃可推导得到下式：

由上式可知，系统状态量V(t)，θ(t)，ψ(t)可由x(t)，y(t)和h(t)及其导数表示。

根据Σ₂，可以得到：

由上式可知，系统控制输入n_x(t)，n_y(t)，γ(t)可由系统状态量V(t)，θ(t)，ψ(t)表示，继而可知系统控制输入n_x(t)，n_y(t)，γ(t)可由x(t)，y(t)和h(t)及其导数表示。

综合Σ₄和Σ₅得知，系统状态变量V(t)，θ(t)，ψ(t)和控制输入n_x(t)，n_y(t)，γ(t)均可由x(t)，y(t)和h(t)及其导数表示。因此飞行器系统是微分平坦系统，即可用一组微分平坦输出χ(t)及χ(t)的导数来表示原系统的状态变量和输入变量。取微分平坦输出χ(t)＝[x(t) y(t) h(t)]^T，可利用χ及其导数表示系统状态变量X(t)＝[V(t) θ(t) ψ(t)]^T和控制变量u(t)＝[n_x(t) n_y(t) γ(t)]^T。

在微分平坦系统中，可采用B样条方法对系统状态进行参数化，即将系统平坦输出χ(t)表示为：

其中，χ_i(t)为微分平坦输出χ的第i个元素；χ(t)＝[x(t) y(t) h(t)]^T是系统微分平坦输出；为定义在一个非单调递减节点序列O_B的第j个k_B次B样条基函数，n_B为B样条基函数的维数，C_i,j为第i个微分平坦输出向量的第j个B样条函数控制点。

第二步，对飞行器飞行速度约束进行非凸处理和正实B样条参数化处理，得到第一步中系统模型的B样条函数控制点的优化条件；

在实际的控制飞行中，飞行器的飞行速度需要介于其最大平飞速度与最小平飞速度之间，即V_min≤V(t)≤V_max，其中V(t)为t时刻飞行器的飞行速度，V_min为允许的飞行器最小平飞速度，V_max为允许的飞行器最大平飞速度。

根据Σ₄，可以得到如下关于V的方程：

其中χ_i(t),i＝1,2,3为微分平坦输出χ(t)的第i个元素，χ(t)＝[x(t) y(t) h(t)]^T，是χ(t)的一阶导数。结合约束表达式得到：

即

可见飞行速度约束是非凸的，故对其进行变形得到：

飞行速度约束表达为两个半正定多项式形式。其中 l是飞行器速度矢量在地面坐标系XOY平面上的投影矢量l的绝对值。

由于β和γ是随着航迹倾角θ变化的，所以将β和γ看作为分段常值函数，因此飞行器飞行速度约束是分段约束。

如果θ∈(0+i_H,1+i_H),i_H＝0,1,…,k,k为一常数

其中，β₁,γ₁是β和γ的初始时刻值，β_f,γ_f是β和γ的最终时刻值。

将飞行器飞行速度约束统一表达为：

其中

经过一系列转化，可以得到等效于半正定多项式的凸优化约束条件

其中，q＝1表明与V(t)≤V_max等效的凸优化约束条件，q＝2表明与V_min≤V(t)等效的凸优化约束条件。定义为：

为辅助变量，表示α_q的第i_v行第l×j列元素，的值由下式求得：

其中，H_q,j为H(t)第q行第j列的元素，为关于t的一阶微分，为关于t的二阶微分；是定义在一非递减节点序列O_v上的第i_v个k_v次B样条基函数，维数为n_v，i_v＝1,...,n_v；x^p是x的p次方，f(x)是关于x的连续函数，t_f为终端状态时间。选取合适的p，求出唯一解。

定义为b₁＝[V_max ... V_max]^T，b₂＝[V_min ... V_min]^T。

Λ^*(Y_q)定义如下：

算子代表α和β为实数，可得到Λ*(Y_q)表达式：

其中表示Y_q的所有特征根均为非负的，为Y_q的第i_w行j_w列的元素，q＝1,2，i_w,j_w＝1,...,n_w，是的第i_v个元素，i_v＝1,...,n_v，由下式求解：

其中，是定义在一非递减节点序列O_w上的第i_w个k_w次B样条基函数，维数为n_w，i_w,j_w＝1,...,n_w。

第三步，对飞行器地理环境约束进行正实B样条参数化处理，得到第一步中系统模型的B样条函数控制点的优化条件；

为保证飞行安全，飞行器在飞行过程中要高于地形高度。且由于降低飞行高度会增加撞地概率，因此需要留出一定安全距离。可将地理环境约束表示为：

h(t)≥r(x(t),y(t))+Δh

其中，r(x(t),y(t))表示(x,y)点的地形高度，Δh为安全高度阈值。

将时变函数r(x(t),y(t))进行简化，使其为分段函数：

其中m为给定常数，表示时刻点的地形高度，将地理位置约束表示为：

其中，

对地理位置约束的参数化处理同第二步。其约束条件作为第三个约束半平面，q＝3将地理位置约束表达为一个凸优化条件形式：

其中，q＝3表明与地理环境约束h(t)≥r(x(t),y(t))+Δh等效的凸优化约束条件，为控制点集合，定义为：

q＝3；为辅助变量，表示α_q的第i_v行第l×j列元素，可以由下式求解得到：

其中，H_q,l为H(t)第q行第l列的元素，定义为 是一组待求解的矩阵，表示要求Y_q的所有特征根均为非负的，为Y_q的第i_w行j_w列的元素。

第四步，结合第一步、第二步、第三步，得到系统模型的B样条函数控制点的LMI优化模型，利用最优控制点和B样条函数可得到飞行器的最优轨迹；

经过上述步骤对于约束条件的分析和转化，服务航天器轨迹优化问题可表示为对于飞行器系统模型的B样条函数控制点的LMI优化求解：

满足

其中，为LMI优化问题的优化指标，意为寻找C使得关于的目标函数最小；t_i为初始时间，t_f为最终时间，X₀为系统的初始状态，X_f为系统最终状态，u₀为系统初始输入，u_f为系统最终输入，为控制点集合，其中i＝1,2,3为χ_i(t)i＝1,2,3的控制点；q＝1表明与V(t)≤V_max等效的凸优化约束条件，q＝2表明与V_min≤V(t)等效的凸优化约束条件，q＝3表明与地理环境约束h(t)≥r(x(t),y(t))+Δh等效的凸优化约束条件。

通过LMI求解，得到最优解C，求得飞行器t时刻最优位置信息：

并得到飞行器最优飞行轨迹。其中χ₁(t)＝x(t),χ₂(t)＝y(t),χ₃(t)＝h(t)。

本发明与现有技术相比的优点在于：

本发明的基于正实B样条的飞行器轨迹优化方法利用正实B样条性质将飞行器飞行过程中的约束进行转化，将轨迹优化转化为B样条控制点的LMI优化问题；相对于现有的轨迹优化方法，本发明得到的轨迹考虑了多种约束，可将多维度上的约束进行统一处理，集中得到最终轨迹，显著提高优化效率。

附图说明

图1为本发明一种基于正实B样条的飞行器轨迹优化方法流程图。

具体实施方式

1.搭建飞行器运动学模型和动力学模型，并将运动学和动力学模型进行微分平坦处理和B样条参数化，得到基于B样条参数化的微分平坦系统模型

假设飞行器在空间机动飞行过程中无侧滑，推力与速度矢量间的夹角为小量，搭建飞行器动力学模型如下：

其中，m为飞行器质量，V(t)为飞行速度，T为发动机推力，D为阻力，L为升力，θ(t)为航迹倾角，ψ(t)为航迹偏角，γ(t)为滚转角；V(t)，θ(t)，ψ(t)为系统状态变量。

在飞行过程中，过载表征可操纵力的大小和方向，因此可以利用过载的概念表征飞行器机动性，定义过载为：

其中，n_x(t)称为切向过载，沿飞行速度方向；n_y(t)称为法向过载，垂直于飞行速度矢量。

利用过载概念，可以将Σ₁变形得到飞行器动力学模型：

其中，n_x(t)，n_y(t)和γ(t)为系统控制量，g为重力加速度。

搭建飞行器运动学模型如下：

其中，x(t)，y(t)和h(t)分别为飞行器质心位置在地面坐标系三轴上的分量。

根据Σ₃可推导得到下式：

由上式可知，系统状态量V(t)，θ(t)，ψ(t)可由x(t)，y(t)和h(t)及其导数表示。

根据Σ₂，可以得到：

由上式可知，系统控制输入n_x(t)，n_y(t)，γ(t)可由系统状态量V(t)，θ(t)，ψ(t)表示，继而可知系统控制输入n_x(t)，n_y(t)，γ(t)可由x(t)，y(t)和h(t)及其导数表示。

综合Σ₄和Σ₅得知，系统状态变量V(t)，θ(t)，ψ(t)和控制输入n_x(t)，n_y(t)，γ(t)均可由x(t)，y(t)和h(t)及其导数表示。因此飞行器系统是微分平坦系统，即可用一组微分平坦输出χ(t)及χ(t)的导数来表示原系统的状态变量和输入变量。取微分平坦输出χ(t)＝[x(t) y(t) h(t)]^T，可利用χ及其导数表示系统状态变量X(t)＝[V(t) θ(t) ψ(t)]^T和控制变量u(t)＝[n_x(t) n_y(t) γ(t)]^T。

在微分平坦系统中，可采用B样条方法对系统状态进行参数化，即将系统平坦输出χ(t)表示为：

其中，χ_i(t)为微分平坦输出χ的第i个元素；χ(t)＝[x(t) y(t) h(t)]^T是系统微分平坦输出；为定义在一个非单调递减节点序列O_B的第j个k_B次B样条基函数，n_B为B样条基函数的维数，C_i,j为第i个微分平坦输出向量的第j个B样条函数控制点。取k_B＝4，n_B＝19。

2.对飞行器飞行速度约束进行非凸处理和正实B样条参数化处理，得到第一步中系统模型的B样条函数控制点的优化条件；

在实际的控制飞行中，飞行器的飞行速度需要介于其最大平飞速度与最小平飞速度之间，即V_min≤V(t)≤V_max，其中V(t)为t时刻飞行器的飞行速度，V_min为允许的飞行器最小平飞速度，V_max为允许的飞行器最大平飞速度。取V_min＝750m/s，V_max＝1200m/s。

根据Σ₄，可以得到如下关于V的方程：

其中χ_i(t),i＝1,2,3为微分平坦输出χ(t)的第i个元素，χ(t)＝[x(t) y(t) h(t)]^T，是χ(t)的一阶导数。结合约束表达式得到：

即

可见飞行速度约束是非凸的，故对其进行变形得到：

飞行速度约束表达为两个半正定多项式形式。其中 l是飞行器速度矢量在地面坐标系XOY平面上的投影矢量l的绝对值。

由于β和γ是随着航迹倾角θ变化的，所以将β和γ看作为分段常值函数，因此飞行器飞行速度约束是分段约束：

如果θ∈(0+i_H,1+i_H),i_H＝0,1,…,k,k为一常数

其中，β₁,γ₁是β和γ的初始值，β_f,γ_f是β和γ的最终值。

将飞行器飞行速度约束统一表达为：

其中

经过一系列转化，可以得到等效于半正定多项式的凸优化约束条件

其中，q＝1表明与V(t)≤V_max等效的凸优化约束条件，q＝2表明与V_min≤V(t)等效的凸优化约束条件。定义为：

为辅助变量，表示α_q的第i行第l×j列元素，的值由下式求得：

其中，H_q,j为H(t)第q行第j列的元素，为关于t的一阶微分，为关于t的二阶微分；是一组定义在节点序列O_v上的k_v次B样条基函数，维数为n_v，i_v＝1,...,n_v；t_f为终端状态时间。选取合适的p，求出唯一解。取：

k_v＝4，n_v＝45，t_f＝15，p＝45。

定义为b₁＝[V_max ... V_max]^T，b₂＝[V_min ... V_min]^T。

Λ^*(Y)定义如下：

算子代表可得到Λ^*(Y)表达式：

其中为Y_q的第i_w行j_w列的元素，q＝1,2，i_w,j_w＝1,...,n_w，是的第i_v个元素，i_v＝1,...,n_v，由下式求解：

其中，是定义在一非递减节点序列O_w上的第i_w个k_w次B样条基函数，维数为n_w，i_w,j_w＝1,...,n_w。取O_w＝[0 0 12 ... 14 15 15]，k_w＝2，n_w＝13。

3.对飞行器地理环境约束进行正实B样条参数化处理，得到第一步中系统模型的B样条函数控制点的优化条件；

为保证飞行安全，飞行器在飞行过程中要高于地形高度。且由于降低飞行高度会增加撞地概率，因此需要留出一定安全距离。可将地理环境约束表示为：

h(t)≥r(x(t),y(t))+Δh

其中r(x(t),y(t))表示(x,y)点的地形高度，Δh为安全高度阈值。取：

Δh＝3.5

将时变函数r(x(t),y(t))进行简化，使其为分段函数：

其中m为给定常数，取m＝15。

可以将地理位置约束表示为：

其中H₃(t)＝[0 0 1 0 0 0]，V₃＝r_ir+Δh。

对地理位置约束的参数化处理同第二步。其约束条件作为第三个约束半平面，q＝3，将地理位置约束表达为一个凸优化条件形式：

其中，q＝3表明与地理环境约束h(t)≥r(x(t),y(t))+Δh等效的凸优化约束条件，为控制点集合，定义为：

q＝3；为辅助变量，表示α_q的第i_v行第l×j列元素，可以由下式求解得到：

其中，H_q,l为H(t)第q行第l列的元素，定义为 是一组待求解的矩阵，表示要求Y_q的所有特征根均为非负的，为Y_q的第i_w行j_w列的元素。

4.结合第一步、第二步、第三步，得到系统模型的B样条函数控制点的LMI优化模型，利用最优控制点和B样条函数可得到飞行器的最优轨迹；

经过上述步骤对于约束条件的分析和转化，服务航天器轨迹优化问题可表示为对于飞行器系统模型的B样条函数控制点的LMI优化求解：

满足

其中，为LMI优化问题的优化指标，意为寻找C使得目标函数最小，取t_i为初始时间，t_f为最终时间，X₀为系统的初始时刻状态，X_f为系统最终时刻状态，u₀为系统初始时刻输入，u_f为系统最终时刻输入，取t_i＝0，t_f＝15。

通过LMI求解，得到最优解C，求得飞行器t时刻最优位置信息：

并得到飞行器最优飞行轨迹。其中χ₁(t)＝x(t),χ₂(t)＝y(t),χ₃(t)＝h(t)，C_i,j为第i个微分平坦输出向量的第j个B样条函数控制点，为定义在一个非单调递减节点序列O_B的第j个k_B次B样条基函数，k_B＝4。

Claims (1)

1.一种基于正实B样条的飞行器轨迹优化方法，其特征在于：包括以下步骤：

第一步，搭建飞行器运动学模型和动力学模型，并将运动学和动力学模型进行微分平坦处理和B样条参数化，得到基于B样条参数化的微分平坦系统模型，系统模型中的B样条函数控制点的优化条件在第二步和第三步中得到；

第二步，对飞行器飞行速度约束进行非凸处理和正实B样条参数化处理，得到第一步中系统模型的B样条函数控制点的优化条件；

第三步，对飞行器地理环境约束进行正实B样条参数化处理，得到第一步中系统模型的B样条函数控制点的优化条件；

第四步，结合第二步根据飞行器飞行速度约束得到的B样条函数控制点优化条件、第三步根据飞行器地理环境约束得到的B样条函数控制点优化条件，得到系统模型的B样条函数控制点的LMI优化模型，利用最优控制点和第一步的基于B样条参数化的微分平坦系统模型得到飞行器的最优轨迹；

所述第一步，搭建飞行器动力学模型Γ₁和运动学模型Γ₂如下：

其中，m为飞行器质量，V(t)为飞行速度，T为发动机推力，D为阻力，L为升力，θ(t)为航迹倾角，ψ(t)为航迹偏角，γ(t)为滚转角，x(t)，y(t)和h(t)分别为飞行器质心位置在地面坐标系三轴上的分量，V(t)，θ(t)，ψ(t)、x(t)，y(t)、h(t)为系统状态变量，g为重力加速度；

将运动学和动力学模型进行微分平坦处理和B样条参数化，得到基于B样条参数化的微分平坦系统模型如下：

其中，χ_i(t)为χ(t)的第i个元素，χ(t)＝[x(t) y(t) h(t)]^T是系统微分平坦输出；为定义在一个非单调递减节点序列O_B的第j个k_B次B样条基函数，n_B为B样条基函数的维数，C_i,j为第i个微分平坦输出向量的第j个B样条函数控制点；

所述第二步，对飞行器飞行速度约束进行非凸处理和正实B样条参数化处理，得到第一步中系统模型的B样条函数控制点的优化条件过程如下：

飞行器飞行速度约束描述为：

V_min≤V(t)≤V_max

其中，V_min为飞行器允许的最小平飞速度，V_max为飞行器允许的最大平飞速度；得到如下关于V的方程：

其中是χ(t)的一阶导数，结合约束表达式得到：

速度约束为非凸约束，对飞行器飞行速度约束进行非凸处理，得到

其中

β和γ是随着航迹倾角θ变化的特征角度，有

如果θ∈(0+i_H,1+i_H),i_H＝0,1,…,k,k为一常数

其中，β₁,γ₁是设定的β和γ的初始值，β_f,γ_f是设定的β和γ的最终值；

将约束表达式进行正实B样条参数化处理得到相应优化条件：

其中，q＝1表明与V(t)≤V_max等效的凸优化约束条件，q＝2表明与V_min≤V(t)等效的凸优化约束条件，为控制点集合，其中均为控制点，n_v为维数，定义为：

为辅助变量，表示α_q的第i_v行第l×j列元素，的值由下式求得：

其中，H_q,l为H(t)第q行第l列的元素，为关于t的一阶微分，为关于t的二阶微分； 是一组定义在节点序列O_v上的k_v次B样条基函数，维数为n_v，i_v＝1,...,n_v；x^p是x的p次方，f(x)是关于x的连续函数，t_f为终端状态时间；选取合适的p，求出唯一解，定义为b₁＝[V_max … V_max]^T，b₂＝[V_min … V_min]^T，其中i_w,j_w＝1,…,n_w，是的第i_v个元素，i_v＝1,…,n_v，是一组待求解的矩阵，表示要求Y_q的所有特征根均为非负的，为Y_q的第i_w行j_w列的元素，由下式求解：

其中 是定义在一非递减节点序列O_w上的第i_w个k_w次B样条基函数，维数为n_w；

所述第三步，对飞行器地理环境约束进行正实B样条参数化处理，得到第一步中系统模型的B样条函数控制点的优化条件过程如下：

地理环境约束表示为：

h(t)≥r(x(t),y(t))+Δh

其中，r(x(t),y(t))表示t时刻(x,y)点的地形高度，Δh为给定的安全高度阈值；将时变函数r(x(t),y(t))进行简化，使其为分段函数：

其中ρ为给定常数，表示时刻点的地形高度，地理位置约束表示为：

其中，H₃(t)＝[0 0 1 0 0 0]，

对地理位置约束进行正实B样条参数化处理，得到凸优化条件：

其中，q＝3表明与地理环境约束h(t)≥r(x(t),y(t))+Δh等效的凸优化约束条件，为控制点集合，定义为：

q＝3；为辅助变量，表示α_q的第i_v行第l×j列元素，可以由下式求解得到：

其中，H_q,l为H(t)第q行第l列的元素，定义为 是一组待求解的矩阵，表示要求Y_q的所有特征根均为非负的，为Y_q的第i_w行j_w列的元素；

所述第四步，得到系统模型的B样条函数控制点的LMI优化模型如下：

系统模型的B样条函数控制点的LMI优化模型为：

满足

其中，为LMI优化问题的优化指标，意为寻找C使得关于的目标函数最小；t_i为初始时间，X₀为系统的初始状态，X_f为系统最终状态，u₀为系统初始输入，u_f为系统最终输入，为χ_i(t)的控制点；q＝1表明与V(t)≤V_max等效的凸优化约束条件，q＝2表明与V_min≤V(t)等效的凸优化约束条件，q＝3表明与地理环境约束h(t)≥r(x(t),y(t))+Δh等效的凸优化约束条件；

通过LMI求解，得到最优解C，求得飞行器最优飞行轨迹：

并得到飞行器最优飞行轨迹，其中χ₁(t)＝x(t),χ₂(t)＝y(t),χ₃(t)＝h(t)。