CN104793626A - 一种引入飞行员意图的航迹规划方法 - Google Patents

Abstract

本发明公开了一种引入飞行员意图的航迹规划方法，包括以下步骤：环境标准参数的初始化设置；数据采集；地图表征；约束条件数据检测：根据目标函数和约束数据，检测混合整数线性规划所需要的变量个数；若变量个数小于或等于500，则采用基于约束条件的自主航迹规划算法；若变量个数大于500，则采用基于约束条件和飞行员意图的航迹规划算法。本发明把飞行员的意图转化为最优控制算法中的限制条件，使得自主航迹规划算法体现了飞行员的思想，克服了传统的航迹规划方法中忽视飞行员意图的问题；本发明采用滚动时域与混合整数规划相结合的最优控制问题求解方法，从而不存在增加飞行员的意图带来的计算量增大太多的问题。

Description

一种引入飞行员意图的航迹规划方法

技术领域

本发明涉及一种航迹规划方法，尤其涉及一种引入飞行员意图的航迹规划方法，属于空管技术领域。

背景技术

与商业航空的飞行路线是事先定义的、固定的相比，通用航空往往是在低空条件下自由飞行的(free flight)，其飞行员可以自主选择航线、高度和速度。在这种情况下，研究如何减轻气象、环境、地形、地貌等外界条件的复杂性对通用航空安全问题的影响，一直是空管研究中关心的问题，为此需要利用自主导航方法，例如：自主路径规划、冲突检测、风险评估和决策，为飞行器提供帮助。自主路径规划利用飞行器传感器获取的信息，检测可能遇到的障碍物或碰撞，进行风险评估，为飞行器提供可飞的航迹，因此对于飞行器的安全飞行有重要的意义。

在飞行器避险、避撞的自主路径规划研究中，相关研究学者、工程师提出了很多的算法。在这些算法中，对于多飞行器，一种鲁棒的策略是不同飞行器之间通过交换信息和飞行意图等合作的方式规划各自的路径。这种合作的方法进一步分为分散合作和集中合作两种方式。中央控制方式中，中央控制器知道每个飞行器的位置，并负责设计每个飞行器的航迹，其通常解决一个大的最优化问题。相关方法包括：semidefinite programming，nonlinear programming，mixed integer linear programming，mixed integer nonlinear programming，variational analysis。然而，规划的航迹数量n(n-1)/2与飞行器的数目n呈现二次函数关系，因此这些方法的计算量将成指数增长。中央集成系统需要对问题进行综合求解，往往能够得到最优解，但当系统很大的时候(飞行器很多)，收集信息的难度将 大大提高。

一般来讲，分散的方法性能的鲁棒性要好。有人提出一种合作的、分散式方法，该方法基于model predictive control(MPC)，最优控制问题包括飞行器的动力学，通过mixed-integer linear programming(MILP，混合整数线性规划)。每个agent分配一个时隙用于计算动力学的可行性，从而保证一条无冲突的路径。把全局最优化问题分解成几个局部问题，这些局部问题被agent以迭代的、分散处理的方式予以解决。每次迭代中agent之间进行信息交换，充分利用解的过程。其他的分散控制方法，包括基于规则的方法、基于势场的方法。势场的方法包含了飞行器的动力学，并且计算速度很快，但有时候无法在保证飞行器安全的情况下完成避障或避撞。

通过滚动时域方法(receding horizon approach)，计算时间能被减少。然而，除非每次迭代中能把问题完全解决，安全问题才能保证。例如：当规划平面(planning horizon)太短时，飞行器的最大转率相对变小，就会导致：在规划之前飞行器之间会相距很近。结果是：飞行器不能及时转向，这时的最优化问题将没有可行解。另外，规模问题在无人机(unmanned aerial vehicle，UAV)领域也会出现。当需要对大队的UAV进行路径规划的时候，中央处理器的求解将随着UAV数量的增加而变得难以求解。并且，在实际应用中，当任务展开的时候，将会出现新情况，以至于规划问题需要多次求解。所以，分布式水平退让(decentralized receding horizon,or model predictive control)规划策略看起来更适合解决多飞行器航迹生成问题。也有文献提出这样一种方法，其中静态障碍物和动态飞行器用势函数(potential function)进行说明。尽管计算效率比较高，但potential function不能保证安全性：障碍物和移动的飞行器在代价函数中用soft constraints表示。还有文献提出一种基于iterative bargaining scheme的算法， 但由于迭代可能收敛于不可行的平衡点，所以仍存在soft safety guarantees。

滚动时域Receding Horizon Planning(RHP)处理的是：在保证安全的前提下，对多个飞行器，计算其在2D空间的最优航迹。每个飞行器计算各自的飞向位点的航迹，并考虑其他飞行器的飞行意图。由于其他飞行器的意图是在线收集的，并且随着各自的航迹而变化，所以每个飞行器采用RHP策略：在每一步，通过求解一个有限平面长度T内的最优化问题来计算所有通向目的地的路径，然后对这些路径进行分割。目前滚动时域方法已经得到了广泛的应用，在无人机、有人机的避险避撞中能够在保证飞行器安全的前提下为飞行器提供航迹。

综上，目前的方法通常只考虑外界环境的因素，而完全依靠自主航迹规划算法中考虑的各种约束条件，有时候带来的计算量很大。例如：为了躲避障碍物，需要计算障碍物的边界条件，而复杂的障碍物形状必然导致计算量的增加。另外，复杂气象条件的建模，需要利用概率、模糊等现代信号处理方法，导致需求的数据量也很大。在这种条件下，如果能够合理考虑飞行员的意图，不但有利于减少计算量，更有利于增加飞行器的安全指数，本发明正是基于这种考虑而研发的航迹规划方法。

发明内容

本发明的目的就在于为了解决上述问题而提供一种引入飞行员意图的航迹规划方法。

本发明通过以下技术方案来实现上述目的：

一种引入飞行员意图的航迹规划方法，包括以下步骤：

(1)环境标准参数的初始化设置：按环境类型对相应的环境标准参数进行初始化设置，即在飞行器飞往指定位置之前，得到飞行器的起点到终点之间静态环境数据的初步表征；

(2)数据采集：飞行器在飞行过程中，实时采集可能影响飞行安全的环境数据；

(3)地图表征：对采集到的环境数据用凸多面体进行表征，不断修正、完善地图；

(4)约束条件数据检测：根据目标函数和约束数据，检测混合整数线性规划所需要的变量个数，环境数据获得的越多，滚动时域的长度就增大，反之减少，计算量随着滚动时域长度的增加而增加；

(5)若变量个数小于或等于500，则采用基于约束条件的自主航迹规划算法；若变量个数大于500，则采用基于约束条件和飞行员意图的航迹规划算法。

作为优选，所述步骤(4)中，所述目标函数的公式为： 

$\underset{x,u}{\min }J=\underset{j=0}{\overset{N-1}{\mathrm{\Σ}}}a{\left|\right|x(k+j)-x_f\left|\right|}_1+\underset{j=0}{\overset{N-1}{\mathrm{\Σ}}}b{\left|\right|u(k+j)\left|\right|}_1---\left(1\right)$

其中，x_f是终点位置坐标，N是滚动时域长度，目标函数中的第一项的物理意义是要求生成的路径最短，第二项的物理意义是要求耗能最少；a和b是常数，其用于通过调整目标函数的等号左右两项的权重，在生成路径长度和耗能方面有所侧重；x表示飞行器的空间位置坐标矢量，u表示系统的输入；滚动时域N长度越大，计算量越大；算法的求解采用基于滚动时域的混合整数线性规划。

所述步骤(5)中，所述基于约束条件的自主航迹规划算法中，所述约束条件包括但不限于飞行器系统的动力学约束、速度和加速度约束、躲避障碍物约束、躲避碰撞约束。

所述飞行器系统的动力学约束的条件为：

$\left[\begin{array}{c}\stackrel{\·}{x}\left(t\right)\\ \stackrel{\·}{v}\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{0}_{3\×3}& I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}x\left(t\right)\\ v\left(t\right)\end{array}\right]+\left[\begin{array}{c}{0}_{3\×3}\\ I_{3\×3}\end{array}\right]u\left(t\right)---\left(2\right)$

所述飞行器系统的水平方向速度的条件为：

v_h min≤||v_h(t)||≤v_h max   (3) 

所述飞行器系统的竖直方向速度的条件为：

v_v min≤v_v(t)≤v_v max   (4) 

所述飞行器系统的水平方向加速度的条件为：

||a_h(t)||≤a_h max   (5) 

所述飞行器系统的竖直方向加速度的条件为：

a_v min≤a_v(t)≤a_v max   (6) 

上述公式(2)-(6)中，x(t)＝[x(t),y(t),z(t)]^T，表示在时刻t，飞行器的空间位置坐标矢量；表示在时刻t，飞行器的空间速度矢量；u(t)表示系统的输入；表示在时刻t，飞行器在水平方向的速度矢量；表示在时刻t，飞行器在竖直方向的速度；表示在时刻t，飞行器在水平方向的加速度矢量；表示在时刻t，飞行器在竖直方向的加速度。

所述步骤(5)中，所述基于约束条件和飞行员意图的航迹规划算法中，飞 行员根据观测到的周围环境情况，通过把转弯或升降作为所述目标函数的约束条件，从而规划出体现飞行员意识的航迹。

所述转弯为水平方向的右转时，当前时刻t1的水平坐标为p(t₁)＝[x(t₁),y(t₁)]^T，下一时刻t2的右转θ度的水平期望坐标为  $\stackrel{\‾}{p}\left(t_2\right)={[\stackrel{\‾}{x}\left(t_2\right),\stackrel{\‾}{y}\left(t_2\right)]}^T,$ 两者关系如下：

$\stackrel{\‾}{p}\left(t_2\right)=\left[\begin{array}{cc}\cos \left(\mathrm{\θ}\right)& -\sin \left(\mathrm{\θ}\right)\\ \sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)\end{array}\right]p\left(t_1\right)---\left(7\right)$

期望坐标与当前坐标的差为：

$\mathrm{\Δp}=\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\end{array}\right]=\stackrel{\‾}{p}\left(t_2\right)-p\left(t_1\right)---\left(8\right)$

在混合整数线性规划框架下，下一时刻t2的右转θ度的水平坐标p(t₂)＝[x(t₂),y(t₂)]^T的约束条件表达为：

IfΔx≥0   x(t₂)≥x(t₁)+Δx   (9)

IfΔx＜0   x(t₂)＜x(t₁)+Δx   (10)

IfΔy≥0   y(t₂)≥y(t₁)+Δy   (11)

IfΔy＜0   y(t₂)＜y(t₁)+Δy   (12)

所述转弯为水平方向的左转时，与上述右转同理。

所述升降为上升时，当前时刻t1的高度坐标为z(t₁)，下一时刻t2的期望增加高度为Δz，则增加高度的约束条件为：

z(t₂)≥z(t₁)+Δz   (13)

所述升降为下降时，当前时刻t1的高度坐标为z(t₁)，下一时刻t2的期望增加高度为Δz，则降低高度的约束条件为：

z(t₂)＜z(t₁)-Δz   (14)

公式(13)和(14)中，z(t₁)为飞行器在时刻t1的高度坐标，z(t₂)为飞行器在时刻t2的高度坐标。

本发明的有益效果在于：

1、本发明把飞行员的意图转化为最优控制算法中的限制条件，使得自主航迹规划算法体现了飞行员的思想，规划出的航迹是为飞行员服务的，并需要飞行员按照规划出的航迹进行飞行；在影响航迹规划的各个限制条件中，不同限制条件对不同飞行员的影响也不尽相同；因此，本发明克服了传统的航迹规划方法中忽视飞行员意图的问题。

2、本发明的规划算法中，采用滚动时域与混合整数规划相结合的最优控制问题求解方法，从而不存在增加飞行员的意图带来的计算量增大太多的问题。

3、本发明的规划算法中，通过增加考虑飞行员的意图这个约束条件，可以有效减少对复杂环境边界约束条件的考虑，从而减少算法的总体计算量。

附图说明

图1是本发明所述引入飞行员意图的航迹规划方法的流程方框图；

图2是本发明所述基于约束条件的自主航迹规划算法规划出的航迹示意图之一；

图3是本发明所述基于约束条件和飞行员意图的航迹规划算法规划出的航迹示意图之一；

图4是将图2和图3的航迹放在一起的比较示意图；

图5是本发明所述基于约束条件的自主航迹规划算法规划出的航迹示意图之二，与图2的起点和终点不同；

图6是本发明所述基于约束条件和飞行员意图的航迹规划算法规划出的航迹示意图之二，与图3的起点和终点不同；

图7是将图5和图6的航迹放在一起的比较示意图，与图4的起点和终点不同。

图8是将图5和图6的航迹放在一起填色后的比较示意图，图中的视角与图7有所差异。

具体实施方式

下面结合附图对本发明作进一步说明：

为了便于描述问题，特作如下假设：

假设1：环境中的障碍物都能够被飞行器所检测，并用凸多面体对障碍物建模表征；

假设2：假设飞行器是质点，它的安全区域通过延长障碍物的边界而得到；

假设3：所有可能发生的碰撞都能够被飞行器检测到，即：对于卷入可能发生碰撞中的每个飞行器，其位置、速度相互间已知；避免避撞的约束条件用相互间的最小安全距离表示。

为了便于下面公式的直接应用，先对各字符定义如下：

如图1所示，本发明所述引入飞行员意图的航迹规划方法，包括以下步骤：

(1)环境标准参数的初始化设置：按环境类型对相应的环境标准参数进行初始化设置，即在飞行器飞往指定位置之前，得到飞行器的起点到终点之间静态环境数据的初步表征；这里的环境数据分为三类：第一类，环境数据完全已知，可以从已有的数据库中直接调取；第二类，环境数据完全未知，只知道起点和终点的GPS坐标；第三类，环境数据部分已知；其中，后两类的环境数据需要在飞行过程中逐步获取并修正地图。

(2)数据采集：飞行器在飞行过程中，实时采集可能影响飞行安全的环境数据。

(3)地图表征：对采集到的环境数据用凸多面体进行表征，不断修正、完善地图。

(4)约束条件数据检测：根据目标函数和约束数据，检测混合整数线性规划所需要的变量个数，环境数据获得的越多，滚动时域的长度就增大，反之减少，计算量随着滚动时域长度的增加而增加；所述目标函数的公式为：

$\underset{x,u}{\min }J=\underset{j=0}{\overset{N-1}{\mathrm{\Σ}}}a{\left|\right|x(k+j)-x_f\left|\right|}_1+\underset{j=0}{\overset{N-1}{\mathrm{\Σ}}}b{\left|\right|u(k+j)\left|\right|}_1---\left(1\right)$

其中，x_f是终点位置坐标，N是滚动时域长度，目标函数中的第一项的物理意义是要求生成的路径最短，第二项的物理意义是要求耗能最少；a和b是常数，其用于通过调整目标函数的等号左右两项的权重，在生成路径长度和耗能方面有所侧重；x表示飞行器的空间位置坐标矢量，u表示系统的输入；滚动时域N长度越大，计算量越大；算法的求解采用基于滚动时域的混合整数线性规划。

(5)若变量个数小于或等于500，则采用基于约束条件的自主航迹规划算法，所述约束条件包括但不限于飞行器系统的动力学约束、速度和加速度约束、躲避障碍物约束、躲避碰撞约束，其中，所述飞行器系统的动力学约束的条件为：

$\left[\begin{array}{c}\stackrel{\·}{x}\left(t\right)\\ \stackrel{\·}{v}\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{0}_{3\×3}& I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}x\left(t\right)\\ v\left(t\right)\end{array}\right]+\left[\begin{array}{c}{0}_{3\×3}\\ I_{3\×3}\end{array}\right]u\left(t\right)---\left(2\right)$

所述飞行器系统的水平方向速度的条件为：

v_h min≤||v_h(t)||≤v_h max   (3) 

所述飞行器系统的竖直方向速度的条件为：

v_v min≤v_v(t)≤v_v max   (4) 

所述飞行器系统的水平方向加速度的条件为：

||a_h(t)||≤a_h max   (5) 

所述飞行器系统的竖直方向加速度的条件为：

a_v min≤a_v(t)≤a_v max   (6) 

若变量个数大于500，则采用基于约束条件和飞行员意图的航迹规划算法，飞行员根据观测到的周围环境情况，通过把转弯或升降作为所述目标函数的约束条件，从而规划出体现飞行员意识的航迹，其中，所述转弯为水平方向的右转时，当前时刻t1的水平坐标为p(t₁)＝[x(t₁),y(t₁)]^T，下一时刻t2的右转θ度的水平期望坐标为两者关系如下：

$\stackrel{\‾}{p}\left(t_2\right)=\left[\begin{array}{cc}\cos \left(\mathrm{\θ}\right)& -\sin \left(\mathrm{\θ}\right)\\ \sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)\end{array}\right]p\left(t_1\right)---\left(7\right)$

期望坐标与当前坐标的差为：

$\mathrm{\Δp}=\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\end{array}\right]=\stackrel{\‾}{p}\left(t_2\right)-p\left(t_1\right)---\left(8\right)$

在混合整数线性规划框架下，下一时刻t2的右转θ度的水平坐标p(t₂)＝[x(t₂),y(t₂)]^T的约束条件表达为：

IfΔx≥0   x(t₂)≥x(t₁)+Δx   (9)

IfΔx＜0   x(t₂)＜x(t₁)+Δx   (10)

IfΔy≥0   y(t₂)≥y(t₁)+Δy   (11)

IfΔy＜0   y(t₂)＜y(t₁)+Δy   (12)

所述转弯为水平方向的左转时，与上述右转同理。

所述升降为上升时，当前时刻t1的高度坐标为z(t₁)，下一时刻t2的期望增加高度为Δz，则增加高度的约束条件为：

z(t₂)≥z(t₁)+Δz   (13)

所述升降为下降时，当前时刻t1的高度坐标为z(t₁)，下一时刻t2的期望增加高度为Δz，则降低高度的约束条件为：

z(t₂)＜z(t₁)-Δz   (14)

公式(13)和(14)中，z(t₁)为飞行器在时刻t1的高度坐标，z(t₂)为飞行器在时刻t2的高度坐标。

为了验证本发明所述引入飞行员意图的航迹规划方法与传统航迹规划方法的差别和优势，下面以飞行器的实际飞行试验为例结合附图进行说明：

图2-图4对应的起点是[120-80 8]，终点是[120 80 8]。图2规划出的航迹虽然可行，但距离地面较近，碰撞地面的风险较大；按照驾驶员的意图，在满足躲避障碍物的条件下提升了飞行器的高度，规划出的航迹如图3所示；两者的比较如图4所示。

图5-图7对应的起点是[100 80 8]，终点是[100-80 8]。图5规划出的航迹虽然可行，但一是距离地面较近，碰撞地面的风险较大，二是在躲避图5中自下而上的第一个障碍物时的转弯角度比较大，对飞行器的性能要求比较高；按照驾驶员的意图，在满足躲避障碍物的条件下提升了飞行器的高度，并从图6中自下而上的第二个障碍物的右侧避过，生成的航迹如图6所示，消除了转弯角度过大的风险；两者的比较如图7所示。

由上可知，本发明提出的方法可以按照驾驶员的意图或左转、或右转、或上升、或下降，在保证避免与障碍物碰撞的基础上提供多种航迹选择方案，以 供驾驶员决策。

上述实施例只是本发明的较佳实施例，并不是对本发明技术方案的限制，只要是不经过创造性劳动即可在上述实施例的基础上实现的技术方案，均应视为落入本发明专利的权利保护范围内。

Claims (7)

1.一种引入飞行员意图的航迹规划方法，其特征在于：包括以下步骤：

(1)环境标准参数的初始化设置：按环境类型对相应的环境标准参数进行初始化设置，即在飞行器飞往指定位置之前，得到飞行器的起点到终点之间静态环境数据的初步表征；

(2)数据采集：飞行器在飞行过程中，实时采集可能影响飞行安全的环境数据；

(3)地图表征：对采集到的环境数据用凸多面体进行表征，不断修正、完善地图；

(4)约束条件数据检测：根据目标函数和约束数据，检测混合整数线性规划所需要的变量个数，环境数据获得的越多，滚动时域的长度就增大，反之减少，计算量随着滚动时域长度的增加而增加；

(5)若变量个数小于或等于500，则采用基于约束条件的自主航迹规划算法；若变量个数大于500，则采用基于约束条件和飞行员意图的航迹规划算法。

2.根据权利要求1所述的引入飞行员意图的航迹规划方法，其特征在于：所述步骤(4)中，所述目标函数的公式为：

$\underset{x,u}{\min }J=\underset{j=0}{\overset{N-1}{\mathrm{\Σ}}}a{\left|\right|x(k+j)-x_f\left|\right|}_1+\underset{j=0}{\overset{N-1}{\mathrm{\Σ}}}b{\left|\right|u(k+j)\left|\right|}_1---\left(1\right)$

其中，x_f是终点位置坐标，N是滚动时域长度，目标函数中的第一项的物理意义是要求生成的路径最短，第二项的物理意义是要求耗能最少；a和b是常数，其用于通过调整目标函数的等号左右两项的权重，在生成路径长度和耗能方面有所侧重；x表示飞行器的空间位置坐标矢量，u表示系统的输入；滚动时域N长度越大，计算量越大；算法的求解采用基于滚动时域的混合整数线性规划。

3.根据权利要求1所述的引入飞行员意图的航迹规划方法，其特征在于：所述步骤(5)中，所述基于约束条件的自主航迹规划算法中，所述约束条件包括但不限于飞行器系统的动力学约束、速度和加速度约束、躲避障碍物约束、躲避碰撞约束。

4.根据权利要求3所述的引入飞行员意图的航迹规划方法，其特征在于：所述飞行器系统的动力学约束的条件为：

$\left[\begin{array}{c}\stackrel{\·}{x}\left(t\right)\\ \stackrel{\·}{v}\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{0}_{3\×3}& I_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}x\left(t\right)\\ v\left(t\right)\end{array}\right]+\left[\begin{array}{c}{0}_{3\×3}\\ I_{3\×3}\end{array}\right]u\left(t\right)---\left(2\right)$

所述飞行器系统的水平方向速度的条件为：

v_h min≤||v_h(t)||≤v_h max   (3)

所述飞行器系统的竖直方向速度的条件为：

v_v min≤v_v(t)≤v_v max   (4)

所述飞行器系统的水平方向加速度的条件为：

||a_h(t)||≤a_h max   (5)

所述飞行器系统的竖直方向加速度的条件为：

a_v min≤a_v(t)≤a_v max   (6)

上述公式(2)-(6)中，x(t)＝[x(t),y(t),z(t)]^T，表示在时刻t，飞行器的空间位置坐标矢量；表示在时刻t，飞行器的空间速度矢量；u(t)表示系统的输入；表示在时刻t，飞行器在水平方向的速度矢量；表示在时刻t，飞行器在竖直方向的速度；表示在时刻t，飞行器在水平方向的加速度矢量；表示在时刻t，飞行器在竖直方向的加速度。

5.根据权利要求1所述的引入飞行员意图的航迹规划方法，其特征在于：所述步骤(5)中，所述基于约束条件和飞行员意图的航迹规划算法中，飞行员根据观测到的周围环境情况，通过把转弯或升降作为所述目标函数的约束条件，从而规划出体现飞行员意识的航迹。

6.根据权利要求5所述的引入飞行员意图的航迹规划方法，其特征在于：所述转弯为水平方向的右转时，当前时刻t1的水平坐标为p(t₁)＝[x(t₁),y(t₁)]^T，下一时刻t2的右转θ度的水平期望坐标为两者关系如下：

$\stackrel{\‾}{p}\left(t_2\right)=\left[\begin{array}{cc}\cos \left(\mathrm{\θ}\right)& -\sin \left(\mathrm{\θ}\right)\\ \sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)\end{array}\right]p\left(t_1\right)---\left(7\right)$

期望坐标与当前坐标的差为：

$\mathrm{\Δp}=\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\end{array}\right]=\stackrel{\‾}{p}\left(t_2\right)-p\left(t_1\right)---\left(8\right)$

在混合整数线性规划框架下，下一时刻t2的右转θ度的水平坐标p(t₂)＝[x(t₂),y(t₂)]^T的约束条件表达为：

If Δx≥0 x(t₂)≥x(t₁)+Δx   (9)

If Δx＜0 x(t₂)＜x(t₁)+Δx   (10)

If Δy≥0 y(t₂)≥y(t₁)+Δy   (11)

If Δy＜0 y(t₂)＜y(t₁)+Δy   (12)

所述转弯为水平方向的左转时，与上述右转同理。

7.根据权利要求5所述的引入飞行员意图的航迹规划方法，其特征在于：所述升降为上升时，当前时刻t1的高度坐标为z(t₁)，下一时刻t2的期望增加高度为Δz，则增加高度的约束条件为：

z(t₂)≥z(t₁)+Δz   (13)

所述升降为下降时，当前时刻t1的高度坐标为z(t₁)，下一时刻t2的期望增加高度为Δz，则降低高度的约束条件为：

z(t₂)＜z(t₁)-Δz   (14)

公式(13)和(14)中，z(t₁)为飞行器在时刻t1的高度坐标，z(t₂)为飞行器在时刻t2的高度坐标。