CN104165627B - 一种基于线性规划的实时动态航迹规划方法 - Google Patents

Abstract

本发明公开了一种基于线性规划的实时动态航迹规划方法，包括如下内容：在导航坐标系中，根据障碍物的形态特征坐标，获得障碍物边界坐标点所包含的坐标点集合；根据建立的离散时间模型，获得飞行器的动态参数约束集；基于障碍物边界坐标点所包含的坐标点集合以及飞行动态参数约束集，根据最优航迹目标函数，获得在导航坐标系中的航迹坐标点集，解决了现有技术中无法自动实现航迹规划的技术问题，进而实现了能够自动实现航迹规划的技术效果。

Description

一种基于线性规划的实时动态航迹规划方法

技术领域

本发明涉及航迹规划技术领域，尤其涉及一种基于线性规划的实时动态航迹规划方法。

背景技术

近年来，无人直升机(Unmanned Aerial Vehicle,UAV)因为具有垂直起降、自主飞行等特点，适合代替人在危险、恶劣的环境中工作。使其运用不管是在军事方面还是民用方面都得到了空前的发展，这也是飞行器未来的一个发展趋势。无人直升机的一大特点就在于其无人驾驶，这就要求任务规划系统能够对无人直升机进行自主飞行控制。

航迹规划是无人机自主飞行的一项关键技术，它是在一定约束条件下(如飞行路径上的障碍物)，根据某个或者是某几个性能指标(如：时间最短，路程最短，燃油最省)，计算出从出发位置到终止位置的最优飞行轨迹。

由于无人机领域的发展，各类航迹规算法层出不穷，比如A*搜索算法、遗传算法、模拟退火算法、蚁群算法、粒子群优化算法、基于势能函数的算法等等。

中国专利CN201210274571.5，基于稀疏A*算法和遗传算法的航迹规划方法，将SAS算法和遗传算法相结合进行航迹规划。一定程度减少了航迹规划计算量。中国专利CN201310478422.5基于故障网格设计航迹规划算法，采用分布式技术实现计算加速。中国专利CN201310228497.8基于蜂群协同觅食算法设计多无人机航迹规划方法，提升航迹规划的稳定性，提高了搜索效率。

然后前面的这些专利虽然大都满足了航迹规划的实时性，最优性等特点，但是缺乏使用性，具体在实际运用中操作复杂，繁琐。

中国专利CN201110458232.8通过鼠标从高清地图上点航点，然后再判断航迹的飞行性，根据航迹判断结果手动拖动航点改变航迹，不需要手动输入航点，操作方便，实用性强。但是其航迹是通过手动选取的，然后在判断可行性，不断的修改得到一条可飞航迹，这严格意义上来说不能算是自动航迹规划，航迹规划只给定起始点和目标点，自动计算出从起点到终点的一条最优可飞航迹。

因此，现有技术中飞行器存在无法自动实现航迹规划的技术问题。

发明内容

本申请实施例通过提供一种基于线性规划的实时航迹规划方法，解决了现有技术中无法自动实现航迹规划的技术问题，进而实现了能够自动实现航迹规划的技术效果。

本申请实施例提供了一种基于线性规划的实时动态航迹规划方法，包括如下内容：

S101.在导航坐标系中，根据障碍物的形态特征坐标，获得障碍物边界坐标点所包含的坐标点集合；

S102.根据建立的离散时间模型，获得飞行器的动态参数约束集；

S104.基于障碍物边界坐标点所包含的坐标点集合以及飞行动态参数约束集，根据最优航迹目标函数，获得在导航坐标系中的航迹坐标点集。

进一步地，在S101之前还有：

对障碍物边界坐标点所包含的坐标点集，离散时间模型以及动态参数约束集进行初始化。

进一步地，S101具体包括：

在导航坐标系中，根据障碍物的形态特征坐标，判断障碍物是否为线性障碍物；

若判断获得为线性障碍物时，根据线性障碍物的避障约束面所构成的区域，获取避障面所构成的区域坐标点集合；

若判断获得为柱形障碍物时，获取柱形障碍物边界坐标点所包含的坐标点集合。

进一步地，S102具体为：根据建立的离散时间模型，飞行器的速度门限值以及加速度门限值，获得飞行的速度约束集和加速度约束集。

进一步地，S103具体为：采用AMPL语言编写成可调用程序对最优航迹目标函数，障碍物边界坐标点所包含的坐标点集合以及飞行动态参数约束集求解，获得飞行器在导航坐标系中的航迹坐标点集。

进一步地，在S103之后还有：

对应导航坐标系建立三维航迹规划坐标系；

将导航坐标系中飞行器的起点坐标作为三维航迹规划坐标系中的原点；

对应导航坐标系中的航迹坐标点集，依次在三维航迹规划坐标系中标出飞行器的航迹最优点集。

本申请实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：

由于采用了在导航坐标系中，根据障碍物的形态特征坐标，获得障碍物边界坐标点包含的坐标点集合；然后，根据建立的离散时间模型，获得飞行器的动态参数约束集；基于障碍物边界坐标点所包含的坐标点集合以及飞行动态参数约束集，根据最优航迹目标，获得在导航坐标系中由起点到达终点的航迹坐标点集，所以，有效解决了现有技术中无法自动实现航迹规划的技术问题，进而实现了能够自动实现航迹规划的技术效果。

附图说明

图1为本发明实施例中基于线性规划的实时动态航迹规划方法的流程图；

图2a，图2b为本发明实施例中柱形障碍物的模型规划示意图和线性障碍物的模型规划示意图；

图3为本发明实施例中飞行器在线性障碍物的实际场景中的示意图；

图4为本发明实施例中航迹规划在导航地图上的效果示意图；

图5为本发明实施例中对应导航坐标系建立航迹规划坐标系的示意图。

具体实施方式

本申请实施例通过提供一种基于线性规划的实时动态航迹规划方法，解决了现有技术中无法自动实现航迹规划的技术问题，实现了能够自动实现航迹规划的技术效果。

为了解决上述无法自动实现航迹规划的技术问题，总体思路如下：

首先，在导航坐标系中，根据确定的障碍物的形态特征坐标，获得障碍物边界坐标所包含的坐标点集合；接着，根据建立的离散时间模型，获得飞行器的动态参数约束集；最后，根据上述获得的障碍物边界坐标所包含的坐标点集合以及飞行器的动态参数约束集，以及最优航迹目标函数，从而获得在导航坐标系中的航迹坐标点集，从而有效实现了自动航迹规划的效果。

为了更好的理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

如图1所示，本发明提出的一种基于线性规划的实时动态航迹规划方法，具体包括如下步骤：

S101，在导航坐标系中根据障碍物的形态特征坐标，获得障碍物边界坐标点所包含的坐标点集合；

S102，根据建立的离散时间模型，获得飞行器的动态参数约束集；

S103，基于障碍物边界坐标点所包含的坐标点集合以及飞行动态参数约束集，根据最优航迹目标函数，获得在导航坐标系中由起点到达终点的航迹坐标点集。

在具体的实施方式中，首先，在进行由起点到达终点的航迹规划之前，先对障碍物的信息以及飞行动态信息进行初始化，从而避免没有初始化所规划出现航迹并不是最优路径。

接着，对S101进行详细描述。

在S101中，通过检测障碍物的形态特征坐标，可以将障碍物分为两类，一类是线性障碍物，一类是柱形障碍物。

具体地，像建筑物、电力塔、树木等相对规则的障碍物抽象成多边形柱形障碍物；像电力线，墙等细长的不能从其下面穿过或旁边绕过的障碍物抽象成线性障碍物。

具体如图2a，图2b所示，根据在导航坐标系中的N边型的N个顶点的经度，纬度，高度信息，例如以顶面四边形为例，获取A、B、C、D四个点的经纬度以及高度h₁，建立柱形障碍物数学模型，求出底面每条边的数学表达式：a_ix+b_iy+c_i≤0，ι＝1...N；因此，柱形障碍物的边界点所包含的坐标点的集合如下：

满足a₁x+b₁y+c₁≤0

∪a₂x+b₂y+c₂≤0

……

∪a_Nx+b_Ny+c_N≤0

∪z≤Z_min

∪z≥Z_max

对于线性障碍物，如图3所示为线性障碍物的实际场景，通过设定两个可调参数，水平安全距离D_p和高度安全距离D_h，可以得到EF平行且距离为D_p两条水平安全线L₁，L₂以及垂直方向距离为D_h的垂直安全线L。过L、L₁和L、L₂确定两个约束面：

a_mx+b_my+c_mz+d_m＝0,a_nx+b_ny+c_nz+d_n＝0

线性障碍物的避障约束面所构成的区域坐标点集如下：

满足a_mx+b_my+c_mz+d_m≤0

∪a_nx+b_ny+c_nz+d_n≤0

为了满足混合线性规划的计算需求，将避障约束不等式间“或”的关系变成“与”的关系，采用Big-M方法并引入一组整数变量b_k，避障约束可以表示为：

柱形障碍物：

线性障碍物：

满足a_mx+b_my+c_mz+d_m≤Mp₁

∩a_nx+b_ny+c_nz+d_n≤Mp₂

p₁+p₂≤1

其中b_k为决策变量取值为0或1，M为一个足够大的整数，取M＝10000。综上所有的障碍物约束集，用集合O来表示。

上述在获得了障碍物边界坐标点所包含的坐标集，从而构成了障碍物的约束条件，下面介绍飞行动态参数集，根据S102可以获得。

在具体的实施方式中，根据建立的离散时间模型，飞行器的速度门限值以及加速度门限值，来获得飞行的速度约束集和加速度约束集。

对于飞行器的动态参数特性不需要用比较复杂的飞行线性方程，可以将飞行器近似为一个质点，其离散时间模型s_k+1＝As_k+Bu_k可表示为：

其中，s_k，u_k分别为飞行器k时刻的状态量和控制输入量，

s_k＝[p_k′ v_k′]′,p_k,v_k分别为飞行器的位置向量和速度向量，控制输入量u_k为飞行器的加速度向量a_k。

接下来继续对飞行器的速度向量v_k和加速度向量a_k分别进行描述。

速度约束：近似飞行器的动态特性在高度和水平方向是相互解耦的，水平方向的速度为v_xy＝[v_xk v_yk]′，所以水平方向速度的大小约束为：

||v_xy||₂≤v_max

但这样的约束无法用线性规划的方式表示，因此，采用圆的内接正K多边形近似描述圆域，所以水平方向速度的大小约束可以表示为：

飞行器高度方向上的速度约束可以表示为：

v_zmin≤v_z≤v_zmax

加速度约束和速度约束一样，加速度约束解耦为水平方向和高度方向，水平方向的加速度约束为：

||a_xy||₂≤a_max

同样的方法把加速度预设近似约束为：

飞行器高度上的加速度约束为：

a_zmin≤a_z≤a_zmax

综上，飞行器的可飞行状态集用集合S表示，飞行器的控制量约束集用U来表示。

在设定飞行器的动态参数集时，采用如下动态参数的设定：

vmax	vzmin	vzmax	amax	azmin	azmax
						4m/s	-2m/s	3m/s	5m/s2	-3m/s2	4m/s2

上述是获得了飞行器的动态参数集。

具体如下：最优航迹目标函数，在每个规划时刻k，通过求解下列混合帧数线性规划问题，得到未来N个时域内的规划最优航迹。

在S103中，采用AMPL语言编写成可调用程序对最优航迹目标函数，障碍物边界坐标点所包含的坐标点集合以及飞行动态参数约束集求解，从而获得飞行器在导航坐标系中的航迹坐标点集。

如图4所示，为通过获得的导航坐标系中的航迹坐标点集在导航地图中显示的效果图。

接着，在S103之后，对应导航坐标系建立三维航迹规划坐标系，然后，将导航坐标系中飞行器起点坐标作为三维航迹规划坐标系中的原点，在对应导航坐标系中的航迹坐标点集，依次在三维航迹规划坐标系中标出飞行器的最优点集。

具体地，如图5所示，三维航迹坐标系中是取导航坐标系中起点作为坐标原点，东(E)、北(N)、天(U)为三个轴的地面坐标系，即规划空间是在地面坐标系中描述的，建立两者之间的转换桥梁史整个方案实施的必要准备。具体转化如下：

假设航迹规划起点坐标在导航坐标系中的经纬度信息和高度信息表示为：P₀＝(lon₀,lat₀,hei₀)，在导航坐标系中的任何点P＝(lon,lat,hei)在对应航迹规划坐标系中可以表示为：

x＝R(lon-lon₀)cos((lat₀-lat)/2)

y＝R(lat-lat₀)

z＝hei-hei₀

其中，地球近似为标准的球形，半径R≈6371km。

从而获得在三维航迹坐标系中的最优航迹点集。

在飞行器飞行的过程中，判断飞行器是否到达规划的终点，如果没有到达则继续规划下一个时刻的位置，若检测到到达规划的终点，则发送悬停指令，控制飞行器处于悬停状态。

这样可以自动实时控制飞行器的航迹规划，有效避开障碍物，达到最省时，省油的最优航迹。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (4)

1.一种基于线性规划的实时动态航迹规划方法，其特征在于，包括如下内容：

S101.在导航坐标系中，根据障碍物的形态特征坐标，获得障碍物边界坐标点所包含的坐标点集合；

S102.根据建立的离散时间模型，获得飞行器的动态参数约束集；

S103.采用AMPL语言编写成可调用程序对最优航迹目标函数，障碍物边界坐标点所包含的坐标点集合以及飞行动态参数约束集求解，获得飞行器在导航坐标系中的航迹坐标点集；

S104.对应导航坐标系建立三维航迹规划坐标系；将导航坐标系中飞行器的起点坐标作为三维航迹规划坐标系中的原点；对应导航坐标系中的航迹坐标点集，依次在三维航迹规划坐标系中标出飞行器的航迹最优点集，即航迹规划起点坐标在导航坐标系中的经纬度信息和高度信息表示为：P₀＝(lon₀，lat₀，hei₀)，在导航坐标系中的任何点P＝(lon，lat，hei)在对应航迹规划坐标系中：

x＝R(lon-lon₀)cos((lat₀-lat)/2)

y＝R(lat-lat₀)

z＝hei-hei₀

其中，地球近似为标准的球形，半径R≈6371km。

2.根据权利要求1所述的基于线性规划的实时动态航迹规划方法，其特征在于，在S101之前还有：

对障碍物边界坐标点所包含的坐标点集，离散时间模型以及动态参数约束集进行初始化。

3.根据权利要求1所述的基于线性规划的实时动态航迹规划方法，其特征在于，S101具体包括：

在导航坐标系中，根据障碍物的形态特征坐标，判断障碍物是否为线性障碍物；

若判断获得为线性障碍物时，根据线性障碍物的避障约束面所构成的区 域，获取避障面所构成的区域坐标点集合；

若判断获得为柱形障碍物时，获取柱形障碍物边界坐标点所包含的坐标点集合。

4.根据权利要求1所述的基于线性规划的实时动态航迹规划方法，其特征在于，S102具体为：根据建立的离散时间模型，飞行器的速度门限值以及加速度门限值，获得飞行的速度约束集和加速度约束集。