朱玉华，王飞
（沈阳工业大学 化工过程自动化学院，辽宁辽阳，111003）
0 引言
随着机器人技术的日益成熟，移动机器人智能化和自动化的水平逐步提高[1]，它们被广泛用于生产和生活之中。由于机器人工作环境的复杂性，如何快速无碰撞地规划出一条从起点到终点的路线成为当前移动机器人的关键技术之一，因此机器人路径规划算法已成为国内外研究的热点[2～3]。经典的全局规划算法主要有Dijkstra 算法[4]、蚁群算法[5～6]、概率路图算法[7～8]、A＊[9]等。其中A＊算法是一种以栅格地图为基础的路径规划算法，A＊由于采用了较为单一的启发式搜索算法，使得在较大的场景地图中，会搜索一些不必要的节点且存在较多的拐角，因此提高搜索效率减少拐点、减少机器人导航到目标点路径距离一直是工程应用中改进的方向。
目前很多基于改进A＊的算法被提出。文献[10]、[11]通过扩展邻域使搜索效率提高，但是有些场景地图中会出现穿越地图中障碍物的情况。文献[12]将父节点加入到评价函数中加快了搜索效率，但是没有考虑引入父节点失去平衡问题。文献[13]通过引入父节点的影响力提高了搜索的速度，引入转弯惩罚函数和邻域扩展使拐点减少，但是最终规划处理的路线没有进行平滑处理仍然存在较尖锐的拐角。文献[14]利用动态加权并修改搜索领域的方法提高搜索速度但是未进行拐点去除后就直接进行路径平滑导致平滑曲线较为复杂。
综合上述文献为了减少传统A＊算法运行时间、拐点数量和规划路径的距离，本文通过探究启发式函数、扩展领域对算法的影响，在保留传统A＊算法优势的基础上重新对启发式函数进行设计，使改进的算法在路径规划上具有较高的运行效率，在最优路径的基础上采用拐点去除策略使其平滑度也相对较好，采用动态窗口法[15]完成实时躲避环境中的障碍物，最后进行仿真测试并将该算法部署到实际机器人中进行验证。
1 传统A＊路径规划算法
传统的A＊算法综合考虑Dijkstra 算法和最佳优先搜索算法的优缺点并将两者结合起来。开始节点到任意节点n的代价g(n)与从节点n到目标节点的启发式评估代价h(n)进行相加以评价当前节点，其评价函数公式如下：
n表示当前进行评价的节点；g(n)表示从起始节点移动到当前节点的距离；h(n)表示当前节点到目标节点的直接距离也可以称为预估代价，其中预估代价对A＊算法的搜索效率起着关键性的作用。如果h(n)的权重占比很小以至于忽略，此时就变成了Dijkstra 算法，此时算法搜索效率较低。若g(n)占比权重较小则此时就变成了最佳优先搜索算法，路径搜索速度变快但是会导致计算出现局部最优解。
本文选择的预估代价为欧氏距离。其计算公式如下：
(xn,yn)表示当前节点的坐标，(xend,yend)表示最终目标节点的坐标。
室内机器人路径规划一般采用栅格地图，将A＊引入两个表用来保存扩展的节点和最优的节点，记为OPEN 和CLOSE。主要路径规划过程如下：
（1）从起点S 开始，将其放入到OPEN 中。
（2）寻找S 上下左右、左上、右上、左下、右下的8个方格，并将其它们放入到OPEN 中，设置它们的父节点为S。
（3）将S 从OPEN 中删除并将其加入到CLOSE 中。
（4）计算周围方格的评价函数值，OPEN 选择评价函数最小的节点a将其纳入到CLOSE 后从OPEN 删除。
（5）检查a周围8 个节点；障碍物以及CLOSE 中的节点不予考虑，计算周围节点评价函数值，并设置父节点为a,若相邻的节点b已经在OPEN 中，检查比较S到b与经过a到b的g(n)，若经过a的低则更新父节点为a反之则不变。
（6）执行步骤4-5，找出周围可以到达的节点，如此循环。
（7）当OPEN 中出现目标节点E 时，则路径已经找到，当OPEN 中没有数据的时候则没有合适的路径。
2 改进A＊算法
■2.1 障碍物占比启发函数
针对传统算法搜索效率较低问题，本文对启发式评估函数进行改进，在原式的基础上加入当前节点的父节点对扩展节点的影响，如下式所示：
其中：w表示权重函数，取值范围w∈[0,1]。引入父节点之后会导致原本g(n) 与h(n)不平衡，在缩短时间的同时出现局部最优解，导致搜索路径较长。考虑到当地图中障碍物较多时也会出现局部最优解，因此首先通过实验进一步选取合适的权重取值范围，同时构建相对合理的障碍物占比函数以适应不同的障碍物占比场景，针对障碍物占比启发函数的实验分析将在4.1 节详细阐述。
■2.2 拐点优化策略
由于传统的A＊算法一般只搜索当前节点的8 邻域或者4 邻域，使得机器人只能沿着8 或者4 个方向移动，所以导致规划的路径较为曲折，文献[10]扩展A＊算法的搜索邻域为32 邻域，文献[11]将邻域扩展为16 邻域上述改进方法能够使机器人能够沿着更多的方向运行，使路径看起来相对顺滑，将A＊搜索邻域分别扩展16 邻域和32 邻域，如图1所示。
图1 扩展邻域路径规划效果
由上图知文献[10][11]规划出的全局路径会跨过障物，这是由于扩展邻域导致当前节点搜索的范围变大，有些障碍物被包括进来，当前节点搜索下一个最优节点可能在障碍物后面，故搜索的路径可能会出现穿越障碍物的情况。为了避免上述现象的发生且不降低节点搜索效率，本文依旧采用传统的8 邻域搜索算法来进行寻路，然后对路径进行拐点优化以消除多余拐点，进一步缩短规划路径的距离。
先找出路径中的拐点，选取拐点的方法：将起始记为p1点，然后找到起始点后面的两个点分别记为p2和p3，计算p1和p2的坐标差值分别记为dx21和dy21，计算p3和p2的坐标差值记为dx32和dy32，分别比较坐标差值是否相等，若不相等则将p2点记录到拐点列表inflection_list 中，然后再从p2依次顺序取相邻的三个节点按照上述算法计算，直到路径终点为止，再将起点和终点分别加入到inflection_list 的首位和末位中。
从inflection_list 中依次取出三个相邻的拐点，按照取出的先后分别记为i1、i2、i3，利用i1、i3这两点构造一条直线I 并利用点到直线的距离如公式(4)所示，计算障碍物的到直线的距离，其中 (x0,y0)表示障碍物的坐标。
障碍物选取的标准为该障碍物是否处于i1、i3所形成的矩形内。这里可以设置距离阈值以增强拐点去除的灵活性，当距离d大于我们设定的阈值或矩形内没有障碍物的时候，则i2剔除，当d小于阈值的时候则保留i2。然后再顺序取连续的三个拐点i2、i3、i4进行计算直到取到最后一个拐点为止，这里循环设置的大小为初始拐点个数减去2。
■2.3 路径平滑
室内机器人一般采用的大多为差分运动模型，该模型可以直线运动、弧线运动。在拐点优化之后，依旧有一定数量的拐角，为了为局部规划路径提供相对平滑的目标点序列，对去除拐点后的路径使用贝塞尔曲线进行平滑，使机器人大致沿着曲线运行。贝塞尔曲线具有如下特性：使用n个控制节点 {P1,P2,...,Pn}来控制曲线的形状；曲线经过起点P1和终点Pn，但是不经过中间点P2到Pn-1。考虑到要在拐角处进行平滑且要保证规划线路的连续性，三阶贝塞尔曲线更符合我们的需求，其具体表达式如下所示：
其中P1，P2，P3，P4表示四个控制节点，t为参数其范围为0 到1。
获取四个控制点的方法为从路径起点s1取第一组顺序拐点s1、s2、s3，根据s2与s1、s3与s2之间的横坐标差值进行拓展采点，并设置采样步长stride为可调步长。选取采样点的规则如下：当s2与s1的横坐标差值大于零则取构建贝塞尔曲线的控制点坐标为 (xs1+stride,P1y)其中xs1为s1的横坐标P1y为s1、s2构成直线xs1+stride处的纵坐标；类似地，当s2与s1的横坐标小于零则取坐标为 (xs1+stride,P1y)，当s2与s1的横坐标相等的时候则点为 (xs1,P1y+stride)，依此规则再计算s3与s2新的采样点P2，我们根据s1、s2、P1、P2绘制贝塞尔曲线，下一次取点的时候则以P2为起点顺序选取两个拐点并按照上述过程进行计算。
3 算法融合
■3.1 动态窗口算法
动态窗口法是在二维速度空间中实时对运动的机器人
■3.2 算法融合的路径规划
为了让室内机器人在规划处最优全局路径的同时实现安全避障，将上述两种算法进行融合。改进A＊算法提供一条相对最优的全局路径，DWA 算法根据全局路径提供的目标点评估出机器人运动的最优速度，二者融合的实现思路简述如下：
（a）利用建图算法绘制室内环境栅格地图，加载环境地图并给予机器人正确的初始化位姿。
（b）设置导航的目标点，执行拐点优化改进A＊算法，规划出一条全局最优路径,提取路径中的关键节点。
（c）将全局路径中的关键点作为DWA 的目标点，DWA 算法选择最优轨迹对应的速度和角速度，并实时避障。
（d）判断是否到达目标点且是否为导航终点？
case1：若节点为目标节点但不为终点，则跳到（c）更换下一个关键节点。case2：若节点为终点则结束循环。进行速度采样，然后利用机器人运动学模型预测机器人在采样速度组下相对一段时间内的轨迹，根据评价函数选择相对最优路径下的速度，发送给机器人执行机构。
4 实验及仿真分析
■4.1 障碍物占比函数分析实验
本文仿真运行环境为Ubuntu20.04，采Python 版本为3.8.10，选择地图的尺寸大小为51×40。针对权重对A＊算法的影响进行五次仿真实验，统计权重对搜索算法耗时以及路径规划距离的影响，如图3 所示。
图3 权重对耗时、距离的影响
通过实验发现当w的取值逐渐增大的时候路径距离不断增大然后减少，搜索耗时在骤降后逐渐减少趋于不变。综合考虑到搜索效率和路径代价选择w的取值范围为0.5～0.6，设计障碍物占比启发函数如式(6)所示：
其中障碍物占比p为起点和终点形成矩形内的障碍物面积与矩形总面积的比值。
■4.2 融合算法仿真实验
本文融合算法仿真实验采用开源机器人仿真平台Gazebo。搭建20m×20m 的室内仿真环境，在室内布置若干静态物体，仿真机器人为turtlebot3_waffle，利用开源建gmapping 算法建立仿真环境地图。机器人的起点设置为开始建图的坐标原点，在仿真环境中分别不设置障碍物和设置障碍物分别进行算法验证。仿真环境及环境地图如图4 所示。
图4 仿真环境及地图
选取终点坐标为(8,-9)方向角(0,0,0)，分别运行传统A＊与拐点优化改进A＊算法。
为了验证本文改进效果，截取传统A＊和拐点优化改进算法进行对比如图5 所示，其中图5(a)中实线表示传统A＊规划的路线，图5(b)中实线表示本文拐点优化改进A＊规划处的路线，由图5 知传统A＊算法在规划路径后存在较多的拐点，利用本文拐点滤除方法可以有效滤除拐点使规划出的路径相对平滑，利用三阶贝塞尔曲线在一定程度上可以进一步优化拐点。
图5 运行效果对比
由表1 知传统A＊运行平均耗时79.27s，平均运行的里程为14.98m，本文拐点优化改进A＊运行平均耗时为74.41s，平均运行的里程为14.35m，其中运行里程在本场景下减少4.19%，时间减少6.13%。
进一步验证融合算法的避障能力，在行驶到终点坐标为(5,5)方向角(0,0,0)的路径上加上五个仿真箱子。启动程序发送导航终点，本文改进算法规划出全局路径，当检测到障碍物遮挡全局路径中的路径节点时则重新进行全局路径规划，当全局路径没有被遮挡时则DWA 进行局部路径规划直到行驶到终点为止，规划出全局路径如图6 所示，其中附着在地图边上的点表示仿真中的激光点云，其中实线表示全局路径。
图6 避障仿真实验
5 结束语
本文提出改进的A＊算法，在评价函数中引入当前节点的父节点，为了更好地平衡评价函数且考虑到地图中障碍物对算法的影响，构造障碍物占比启发函数，考虑到可能穿越障碍物的情况采用了传统的8 邻域搜索。由于路径规划会出现较多的冗余拐点，本文提出拐点优化策略，在拐点优化完毕之后再利用三阶贝塞尔曲线进一步对路径做平滑。用Gazebo 和python 对相关的设计进行仿真，根据仿真结果知本文改进A＊算法搜索效率、路径平滑度上有一定的提升。后续将进一步深入研究并优化DWA 算法进一步提升其效率。
