基于改进A*算法的无人机避障路径规划

无人机作为我国科技创新的重要产业, 正处于井喷式发展时期. 它的技术日渐成熟, 能够执行复杂的危险任务. 多用于军事侦查、电力巡检、货物运输、物流配送等, 在航空领域占有一席之地. 随着自动控制技术、传感技术、导航技术、实时监控技术、计算机技术的飞速发展, 无人机的功能也越来越完善, 应用领域不断扩大. 因此, 研究无人机路径规划问题有着重要意义[1,2]. 我国物流运输行业迅猛发展, 已经成为我国国民生产的重要组成部分, 与人们的生活密切相关. 而随着市场需求的增大, 配送成本也在不断升高. 合理有效的运输方案, 可以帮助企业降低运输成本, 提高运输质量, 同时还能实现节能减排的环保目标[3]. 无人机因其高效、灵活、低成本的特点, 渐渐成为各大物流公司的宠儿. 无人机本身是科技的产物, 应该合理正确的使用, 不应该再出现无人机碰撞飞机、使用无人机偷窥等负面新闻, 这些都是十分危险的行为. 再加上我国严格的航空管制, 设置禁飞区显得十分有必要. 因此, 无人机路径规划问题的研究常常与禁飞区避障问题相结合.

路径规划问题一直是国内外学者研究的热点, 无人机的路径规划问题最初是由旅行商问题(Traveling Salesman Problem, TSP)[4]和车辆路径问题(Vehicle Routing Problem, VRP)[3]衍变而来. 无人机避障路径规划问题分为二维和三维两种情况. 本文主要研究二维平面的路径规划问题. 目前大多数的二维平面路径规划问题中, 禁飞区的类型都只是单一的, 比如只存在多边形障碍物[2], 或者只存在圆形障碍物[5,6]. 文献[2]提出了一种改进的A*算法来规划无人机路线, 但只针对单一的矩形禁飞区. 文献[5]提出顺序凸规划问题逼近非凸零件的算法来规划无人机航线, 是只针对圆形禁飞区设计的. 文献[6]将融合简化稀疏A*算法与模拟退火算法相结合, 规划出了一条代价较低的航迹, 但禁飞区的类型也是单一的圆形. 本文研究的是多类型禁飞区共存的情况, 即同时包含有圆形、多边形障碍物, 航迹也可以贴着禁飞区的某条边飞行, 这大大提高了算法设计的难度. 这类问题最常用到的算法包括模拟退火算法[6]、改进的粒子群优化算法(Particle Swarm Optimization)[7]、蚁群算法(Ant Colony Algorithm)、几何法[8]等. 文献[6]提出了将A*算法与模拟退火算法相结合来解决无人机避障的路径规划问题, 能够利用较少内存, 快速的得到一条综合代价较低且较为平滑的航迹. 文献[7]提出了一种改进的粒子群算法来规划无人机航向路线, 相比较传统的粒子群算法, 改进后的算法适应性更强, 环境规划策略更加灵活, 但是集中式计算负担过大, 有时不能满足需要. 文献[8]提出了一种基于几何法的路径规划方法, 在航迹部分加入了螺旋线航迹. 本文采用的是改进后的A*算法来解决多类型禁飞区下无人机路径规划的问题.

本文研究的问题是通过算法找出无人机从任意起点到任意终点运输代价最小的安全避障路线. 假设某指定区域内有多个不同类型的禁飞区, 假设物流无人机均为充电旋翼式无人机, 对货物载重、航行时间、电池能耗等有约束限制. 为了确保货物能够安全准时运输至指定客户点, 需要对无人机进行路径规划. 本文假设仅对物流无人机航行阶段路径进行规划, 则无人机的运动可简化为二维平面运动.

无人机路径规划需要满足以下原则:

① 无人机飞行起始点记为S, 终止点记为T.

② 任务规划应该得到一条从任务起点到任务终点的路径.

③ 要在无人机避过禁飞区的同时, 保证规划的路径应尽量最优化, 满足无人机航程最短的要求.

无人机路径规划的实现主要包括两个步骤: (1) 环境建模. 根据无人机所在的实际环境, 在仿真系统中建立合适的模型, 将实际的物理空间抽象成能用数学模型求解的虚拟空间. (2)路径搜索[9]. 根据任务条件规划出一条从任意起点S到任意终点T的任务路线, 在满足任务条件约束、满足避障要求的同时使路径代价达到最短.

本文想要实现的目标是: 无人机能在准确避过多类型障碍物的同时, 最小化航行里程.

A*算法是一种静态路网中求解最短路径最有效的直接搜索算法, 它通过启发函数来引导算法的搜索方向. 针对本文研究的问题, 对A*算法做了一定的改进. 首先输入无人机飞行起始点S, 终止点T. 建立两个数组C1、C2, 用来存放无人机所经过节点的信息.

上式中, fi表示起点S到节点i的有效已走距离, 这里的有效已走距离是指已经顺利避过障碍物的路线距离; gi表示节点i到终止点T的直线距离, 这里的直线距离是指不考虑障碍物时节点i到终止点T的直线距离, 即剩余最小距离, 这是一个最小估算值. Di是fi与gi的和, 表示节点i目前的最小总代价值.

算法重要步骤:

Step 1. 输入起始点和终止点的坐标, 将起点存入C1中.

Step 2. 遍历C1, 找到最小值Dmin对应的节点对它进行展开, 即找到距离该节点最近的障碍物, 获取该障碍物的节点信息, 将该障碍物的可用节点添加到C1中, 并把C1中已展开的点移至C2中.

Step 3. 重复Step 2, 直到当前展开点与终点连线不再经过任何障碍物, 可直接到达终点时停止, 或者直到C1为空时停止循环.

Step 4. 对得到的最优路径再次检查修复.

Step 4是对路径的修复, 如图1所示, 假设我们已经找到S到T点的最优路径, 图中可以看出S点到V4点的连线不穿过禁飞区, 可以直达, 这种情况下就可以删除S到V4之间的多余节点; 而S到V6点之间的连线穿过了禁飞区, 则不可修复. 为了最终结果更加完美, 代码中对每个节点都进行了检测, 测试它们到其他节点是否可直达, 若可达则删除多余节点来修复路径[10].

重点算法部分的伪代码见算法1和算法2.

算法1. 重点算法

输入: 起点S坐标, 终点T的坐标, 障碍物信息

输出: 最优解best_solution

//C1, C2保存点的信息, 包括每个点的f值、g值、坐标、上层节点

读取障碍物点信息, 起始点信息;

设best_solution的初始f值为inf;

判断S→T是否闯过障碍物, 不穿过障碍物可直接返回;

若穿过障碍物, 则将S加入到C1中, 进入while循环;

While 停止机制: 当前迭代的节点与终点T连线不再穿过任何障碍物, 或者C1为空

　　找C1中找到min(f+g)对应的节点minPoint, 对它展开;

　　找到距离minPoint最近的障碍物的顶点信息(若是圆形障碍物, 找左右两个切点);

　　If 顶点信息输出为空(即已经不穿过任何障碍物)

　　　　更新best_solution, 跳出循环;

　　else 继续寻找

　　　　更新best_solution;

　　　　更新C1 (删去C1中已展开的点, 加入最近障碍物顶点);

end

修复best_solution中的路线(具体代码见算法2);

画出路线;

结束.

算法2. 修复部分算法

输入: best_solution

输出: new_best_solution

//num表示最优解best_solution中所经过节点的个数

for i=1,2,…, num–2

　for j=i+2,i+3,…, num–1

　　　判断best_solution中第i个点到第j个点的连线是否闯过障碍物;

　　　if 没有穿过障碍物

　　　　　将点i和点j间所有的多余节点删去;

　　　end

　end

end

更新best_solution中的路线即最终距离;

new_best_solution=best_solution;

结束.

举例说明:

假设现有多边形障碍物3个, 圆形障碍物3个. 如图2所示, 起始点为S, 终止点为T, 创建C1、C2, 它们保存的节点信息包括: f值、g值、节点坐标、上层节点坐标. 从起始点S出发, 将S保存到C1中, 遍历C1, 找到Dmin对应的节点S, 对S展开, 找到距离S最近的障碍物节点P4、P5、P6, 将P4、P5、P6中可用的节点添加到元胞数组C1中, 并将C1中已展开的点S移动到C2中; 再次遍历C1, 找到Dmin所对应的节点P6, 再继续寻找距离P6点最近的障碍物, 并将该障碍物的可用节点信息存入C1中, 这里注意, 当找到的距离P6点最近的障碍物是圆形时, 求P6与终点T之间的左右两条切线, 将可用切点当作备选点存入C1中; 再次遍历重复上述步骤, 直到当前展开点与终点T的连线不再穿过任何障碍物时, 就可得出S到T的最优路径.

寻优的结果是要经过多次循环产生的, 图3完整的复现了这一过程, 图中数值表示的是由公式1得出的D值. 算法中的每次循环都要遍历C1, 找出最小值Dmin对应的节点, 继续往下分支. 图3中P12下的分支3B表示节点P12和终点T关于圆形障碍物3所做切线得出的切点, P11下的分支表示节点P11和终点T关于圆形障碍物3所做切线得出的切点1和切点2. 这里注意, 可用切点不一定都能找到两个, 有的切点严重偏离当前圆形障碍物时, 就属于不可用切点, 不予以考虑. 图3中可以看出障碍物点P4、P5、P6多次出现, 但它们对应的上层节点有所不同, 所以它们的f值、对应的D值也都不相同, 代表着各自的意义. 图中有向下箭头分支的节点表示在某次循环中已经被展开过的点, 这类点要及时移动到C2中.

我们对改进的A*算法进行仿真实验, 对实验结果进行了分析. 实验中用到的PC系统为Windows10, 处理器型号为Intel(R) Core(TM) i7-8700, 主频率为3.19 GHZ, 开发环境为Matlab 2016b, 仿真实验过程通过Matlab编程语言实现, 使用Matlab对规划环境及规划出的航迹进行作图. 实验中的环境模型大小为800 km×900 km, 同时包含了多种类型的禁飞区, 共14个禁飞区, 其中6个圆形禁飞区, 8个多边形禁飞区.

通过仿真实验, 模拟了4条路径, 如图4所示, 每条路线的途径点信息如表1所示. 可以清楚的看到我们设计的最优路线允许贴着禁飞区的边飞行, 我们的算法也能解决多种形状并存的壁障问题.

本文改进了A*算法, 解决了圆形、多边形禁飞区共存情况下的避障路径规划问题, 对比只有单一类型禁飞区的问题有一定的实用性, 得到的路线更接近最优. 但也有一些不足之处, 比如没有应对突发威胁时的实时航迹规划手段. 本文仅讨论无人机避障路径规划的问题, 后续会对综合的多仓库、多约束条件下的物流无人机路径规划问题进行研究, 在算法自主实时性方面寻求突破, 完善无人机航迹规划策略.