多约束条件下的无人机轨迹快速规划

吴成振，孙大运

（1.200093 上海市 上海理工大学 机械工程学院；2.200093 上海市 上海理工大学 医疗器械与食品学院）

0 引言

航空科学和现代信息技术的发展为无人机在现代战争中扮演重要角色提供了重要基础，而航路规划是实现无人机自主作战高效能的关键步骤，备受国内外学者的重视[1]。目前，无人机在民用领域的应用日渐广泛，比如无人机能够在抢险救灾、环境监测、精准农业生产、航空摄影等领域发挥巨大作用[2]。由于实际应用中复杂的飞行环境和约束条件，所以建立合理的规划模型和高效规划算法是无人机航迹快速规划的一个重要内容，其核心内容为在满足各项约束前提下，快速规划出无人机在目标任务下的最优航迹[3]。飞行航路规划的常见的约束条件有规划中的导航精度约束、威胁约束、飞行器物理性能约束等，其中的部分约束条件还是相互耦合的，因而多约束问题是航路规划的一个至关重要的问题[4]。由于无人机物理性能的限制，其定位系统会因飞行距离产生定位误差，如果定位误差累计超过系统允许的最大范围就会导致任务失败，所以，校正无人机的飞行过程产生的定位误差是无人机航迹规划中的一项重要任务。此外，飞行器在转弯过程中受到自身结构限制，其转弯过程必须存在合理的转弯半径，所以，在三维航迹规划过程中，空间圆弧轨迹的处理也是一大重点。

贪心算法是求解过程相对简单的算法，其思想是从待求解问题的初始状态出发，由当前每一步的局部最优决策得到该算法下的全局最优解[5]。虽然贪心算法不一定得到所有问题的整体最优解，但它能得到大部分问题的整体最优解或者近似最优解[6]。因此，本文通过构造无人机在定位误差约束下的数学模型，结合航迹点和转弯半径推导出空间圆弧轨迹方程，利用贪心算法对无人机的航迹进行快速规划，结合软件仿真得到飞行过程中的最优航迹方案。

1 多约束条件

无人机在飞行过程中，其由系统结构限制原因产生的定位误差会随飞行轨迹越来越大，因此在其飞行区域必须设置校正点，而校正点需在一定误差范围内才能成功校正误差。校正点分为垂直和水平两种，垂直校正点和水平校正点可校正的误差允许范围也不相同，且达到终点时也需要满足误差范围，具体的约束条件如下：

（1）无人机从起始点到达终点的航迹长度尽可能短，经过校正点处的要校正次数尽可能少；

（2）无人机在空间飞行过程中的定位误差有垂直误差和水平误差，误差增长量均为δ个单位每米，假设当两种误差均小于θ个单位时，无人机仍按照预定路径正常飞行；

（3）无人机在出发点处不存在定位误差，其飞行过程途经垂直误差校正点可将垂直误差清零，水平误差不变，途经水平误差校正点可将水平误差清零，垂直误差不变；

（4）在垂直误差校正点能够进行校正的条件为无人机的垂直误差不超过α1个单位且水平误差不超过α2个单位。在水平误差校正点能够进行校正的条件为无人机的垂直误差不超过β1个单位且水平误差不超过β2个单位；

（5）无人机由于自身结构限制，其最小转弯半径为rmin。

2 航迹规划模型

因为该模型要求无人机的航迹尽可能短，且通过校正点进行校正次数尽可能少，因此该问题属于多约束条件下的最短路径求解模型[7]。

2.1 模型建立

假设无人机经过的两个连续校正点Pi和Pj之间的轨迹长度Dij，集合E 的元素为无人机经过的所有校正点P（ii=0，1，…，n），决策变量为xij。当xij=1 时，表示航迹连续经过校正点Pi和Pj，所有决策变量构成的集合为C，将进行校正的点Vi中的垂直校正点构成集合T，水平校正点构成集合H,并设到达校正点Pi时的累计垂直误差为Terr（Pi），累计水平误差为Herr（Pi），ci=1 表示该校正点进行校正，ci=0 表示经过校正点但不需要进行校正。

2.1.1 建立目标函数

根据无人机航迹轨迹尽可能短和经过校正区域进行校正次数尽可能少的要求，建立目标函数如下：

2.1.2 确定约束条件

约束条件1：飞行器到达终点的定位误差满足Terr（Pn）＜θ，Herr（Pn）＜θ；

约束条件2：飞行器在水平校正点能进行校正需要满足Terr（Hi）＜β1，Herr（Hi）＜β2。

约束条件3：飞行器在垂直校正点能进行校正需要满足Terr（Ti）＜α1，Herr（Ti）＜α2。

综上所述，可建立飞行器在本问题约束条件下飞行器航迹快速规划的数学模型为

2.1.3 空间圆弧轨迹求解

由于无人机存在不小于rmin的转弯半径，如果无人机连续经过空间的3 个校正点不共线，则该段轨迹必定存在空间圆弧，则计算该3 点形成轨迹产生的飞行误差以及绘制飞行器轨迹时，就必须求得该段轨迹的空间圆弧的方程。空间曲线方程复杂，为避免空间几何求解的复杂性，本文利用三点共面的性质，先将空间中每一段圆弧轨迹的求解转换为平面几何问题。如图1 所示，假设无人机连续经过的3 个已知点为Pk，Pk+1（作为圆弧轨迹的切入点坐标），Pk+2，无人机选择的转弯半径为rk，以Pk为坐标原点，PkPk+2为X轴建立相应的局部平面坐标系，很显然，的长度仍为其空间上的几何直线长度。

图1 航迹平面图Fig.1 Track plan of UAV

其中，D 可由任意一点已知坐标所决定。

根据空间距离公式可得

因为式（5）是根据实际几何图形建立的，很显然此方程必有解，由一元二次方程求根公式可得式（5）的解为

运用同样的方法容易解得圆心Ok+1的空间坐标。因此，可求得无人机飞行轨迹存在转弯半径时的航迹长度以及每段圆弧轨迹的圆心坐标和圆弧轨迹的切出点坐标。

2.2 基于贪心算法的求解

贪心算法的思想是，在求解问题的过程中，对每一步都做出当前最好的判断。本文航迹求解思想如图2 所示。

*式（5）中：

图2 基于贪心算法的航迹求解思想Fig.2 Model of track solving based on greedy algorithm

第1 步：在起始点 P0的可选范围内随机选择一个点P1。

第2 步：以P1为圆心，结合上述的空间圆弧轨迹模型，以满足模型约束条件的最大距离作为搜索半径，选择一个距离终点 B 最近的点P2作为下一步要走的点。

第3 步：判断是否到达终点，如果没有到达终点，计算P1和P2之间的转弯半径，使P1和P2两点间的曲线长度最小，计算该段曲线中的圆弧轨迹的切出点T，令P0=T，P1=P2，转第2 步；否则，转第4 步。

第4 步：停止。

3 仿真实例及结果分析

因为本问题的航线轨迹是空间曲线图而不是折线图，为了便于轨迹的绘制，需要得到每段空间圆弧的参数方程。每段圆弧的圆心坐标Ok+1（Ox，Oy，Oz），圆弧半径r，以及圆弧所在平面的法向量nk+1=（A，B，C）可由上文得知，则圆弧的参数方程可以表示为

本文以“华为杯”第十六届中国研究生数学建模竞赛F 题中的附件1 校正点数据作为仿真数据，无人机最小转弯半径为300 m，附件1 数据的约束参数为：α1=25，α2=15，β1=20，β2=25，θ=30，δ=0.001。

基于上述模型与算法，在MATLAB 中编写程序[8]得到的无人机飞行的校正点的数据仿真结果如表1 所示，且求得每段圆弧轨迹的转弯半径均为300 m，最优航迹为A-200-509-28-183-194-450-594-397-B。

表1 航迹规划结果Tab.1 Results of UAV trajectory fast planning

图3 为求得的航迹图。因为各圆弧段半径相对于两航迹点间的距离非常小，在整体航迹图上基本看不出圆弧轨迹，本文采用局部放大示意图。

图3 航迹图及局部放大图Fig.3 Map of UAV trajectory fast planning

碍于图中视角以及距离比过大原因，本文求出的所有圆弧轨迹的圆心坐标以及求得的切出点坐标如表2 所示。

表2 圆弧轨迹圆心和切出点坐标Tab.2 Center and cut-out point coordinates of arc trajectory

4 结语

本文建立了无人机关于定位误差的多约束条件下的飞行模型，通过推导空间曲线航迹的数学方程，解决了空间圆弧的绘制问题，利用贪心算法快速规划出最优航迹。此外，基于此模型和算法，还可应用于仓储物流路径规划领域以及车辆无人驾驶目标路径规划中。