0 引言

路径规划不仅在机器人运动方面^[1-2]有重要的作用，在无人机自主避障飞行^[3]、游戏中自动引路系统^[4]等方面也有广泛应用．路径规划的目的是在充满复杂障碍物的环境中找到一条从起点到终点的最佳路径^[5]．关于机器人路径优化的问题，国内外许多学者进行了大量的研究．本文主要针对机器人路径规划效率展开研究．

现有的路径规划算法有很多种，包括A*算法^[6]、蚁群算法^[7]、D*算法^[8]、RRT算法^[9]、人工势场法^[10]、模拟退火法^[11]等．蚁群算法是一种仿生算法，它模拟大自然蚂蚁寻找食物的过程，具有较强的鲁棒性特征，但存在计算量大、求解时间长等问题．单纯的D*算法并不考虑目标方向问题，通过暴力搜索的方式寻得最优解，但在环境地图增大时，其计算量也会急剧增加．A*算法是一种基于启发函数的路径规划算法，也是目前常用的机器人路径规划算法．A*算法在求解过程中，会不断将当前节点及周围8个邻近点添加到封闭列表和开放列表中进行代价评估，因此会导致大量非必要的节点加入到计算中，存在计算效率低下等问题．Goldberg等^[12]研究了评估函数的启发强度，提升了算法的计算速度，但算法严重占用内存; Harabor等^[13]提出一种跳点搜索法，引入强迫邻居的概念，大幅度提高了运算速度; 辛煜等^[14]提出了无限邻域的思想，使得搜索路径不再局限于8个方向，该方法虽然缩短了路径的长度，但增加了计算量; Chaari等^[15]提出一种松弛A*算法，与原始A*算法相比，计算时间得到有效减少; 张庆等^[16]提出一种融合JPS（跳点搜索）策略的改进算法．

根据上述算法的改进思想及A*算法寻路的底层逻辑，本文提出一种改进的A*算法．在路径搜索方向上根据当前点与终点的夹角，设定寻路方向的优先级关系; 接着修改到达终点的判断条件，并根据当前点、障碍物及终点的位置关系给出自适应步长的求解方法; 当自适应步长的长度大于1时，其搜索方向超过8个方向，为了减少冗余评估节点的介入，提出固定八方向搜索规则．最后通过仿真及实验验证了改进算法的有效性．

1 问题描述

1.1 环境表示

栅格法是路径规划中最常用的建模方法，由Elfes等^[17]提出．它以九宫格的方式将环境地图等分，如图1所示，白色代表可通行的地方，记录为“0”，黑色代表不可通行的地方，记录为“1”，这样规定之后，地图存储的信息量少，方便创建和维护．栅格法将环境地图用二维矩阵表示后，路径规划问题就转变为处理二维矩阵问题．

1.2 原始A*算法

A*算法是启发式路径规划算法，适用于已知全局障碍物信息的路径轨迹求解，其路径搜索方向有8个方向，如图2所示．

A*算法通过一个代价评估函数f（N）来求解，其中，N为当前节点，坐标为（x_N，y_N），E为终点，坐标为（x_E，y_E）．从起点出发，朝着终点拓展，再利用f（N）计算出每个节点的代价值:

式中:g（N）为代价函数，描述的是起点到当前点的实际距离; h（N）为启发函数，描述的是当前点到终点的估计距离．根据不同情况，h（N）可使用曼哈顿距离、切比雪夫距离或欧几里得距离，本文采用曼哈顿距离，其公式为

A*算法在进行路径求解时会预先定义两个集合，一个为封闭列表，另一个为开放列表，分别用来存放已经遍历过和将要遍历的节点．首先把起点加入到封闭列表中，然后将开放列表中f（N）最小的点，加入到封闭列表中，把N的周边节点加入到开放列表中，重复以上步骤直到终点也在开放列表中，算法结束．

2 算法改进

2.1 设定寻路方向的优先级关系

改进的A*算法根据终点与当前点的夹角θ，设定算法寻路的搜索方向．如图3所示，设定地图的左上角为原点O，向右为x轴，向下为y轴，当前点坐标为（x_N，y_N），终点坐标为（x_E，y_E），则夹角为

根据θ的值，对照图2方向给出方向表1．

例如图3左图，终点坐标为（7，7），当前点坐标为（4，4），夹角θ=arctan$\left(\frac{7-4}{7-4}\right)$=0.785 398，θ∈$\left(\frac{\pi }{8}，\frac{3\pi }{8}\right]$，且sin θ＞0，参照表1得出第一搜索方向为“1”方向．例如图3右图，终点向左平移两格后夹角θ=arctan$\left(\frac{7-4}{5-4}\right)$=1.249 05，其中θ∈$\left(\frac{3\pi }{8}，\frac{5\pi }{8}\right]$，且sin θ＞0，参照表1得出第一搜索方向为“2”方向．

注1  设定寻路方向的优先级关系的目的是使得算法在寻路运算中，有针对性地沿着最佳路径搜索，减少冗余计算．

2.2 修改到达终点的判断条件

原始A*算法到达终点的条件为终点在最后一个节点的开放列表中．本文提出新的结束条件，当终点在当前节点的第一搜索方向上且无障碍物，即可结束路径搜索工作，如图4所示．

注2  对比原始算法到达终点的规则，本文规则可以实现最后一步的跳跃，减少算法运算量．若终点在起点的第一搜索方向上且无障碍物，则可直接得出路径轨迹，无需进入算法迭代．

2.3 加入自适应步长策略

针对A*算法在路径轨迹计算方面耗时长的问题，加入自适应步长策略，如图5所示，绿色栅格为起点，红色栅格为终点，灰色栅格为参与评估节点．

当步长为1格时，如图5a所示需要6步可以到达终点，此时算法迭代次数为6次; 当步长为2格时，如图5b所示，此时算法迭代次数为3次; 当步长为3格时，如图5c所示，此时算法迭代次数为2次; 当步长为6格时，如图5d所示，此时算法迭代次数为1次．当步长为其他数值时会陷入震荡，导致收敛速度慢，或者无法到达终点．例如，当步长为4格时，会陷入震荡无法到达终点．

本文提出的自适应步长策略，其步长的长度是根据当前节点、障碍物及终点的关系决定的，分以下两种情况讨论．

情况1:节点第一搜索方向上不存在障碍物

如图6a所示，当前点N与终点E的夹角θ=arctan$\left(\frac{5}{3}\right)$=1.030 38，θ∈$\left(\frac{\pi }{8}，\frac{3\pi }{8}\right]$，且sin θ＞0，选择“1”方向为第一搜索方向．当沿第一搜索方向拓展到N+1节点时，由于其横坐标与终点横坐标相同（即“1”方向已不再是第一搜索方向），故停止拓展．N的自适应步长λ=min{|x_E-x_N|，|y_E-y_N|}，其为当前点与终点横坐标差值的绝对值和纵坐标差值的绝对值中较小的那个．

情况2:节点搜索方向上存在障碍物

如图6b所示，当前点N与终点E的夹角θ=arctan$\left(\frac{5}{5}\right)$=0.785 398，θ∈$\left(\frac{\pi }{8}，\frac{3\pi }{8}\right]$，且sin θ＞0，选择“1”方向为第一搜索方向．从当前点N拓展至终点E时，记第一搜索方向上的第一个障碍物为O，坐标为（x_O，y_O），则当前点N的自适应步长λ=max{|x_S-x_O|，|y_S-y_O|}，其描述的是当前点与障碍物横坐标差值的绝对值和纵坐标差值的绝对值中较大的那个．将节点N+1更新为当前点，坐标为（x_N+λ，y_N+λ），由于该节点的第一搜索方向为不可通行状态，故在此处按照原始A*算法寻路．

注3  合适的步长会减少迭代次数，极大地缩减算法的运算耗时，特别是在大地图环境中或障碍物稀疏的地图中尤为明显．当环境地图足够大时，原始算法会出现内存溢出问题，导致机器人宕机．

2.4 八方向搜索法

步长只影响迭代次数，不会大幅度减少开放列表中的评估节点个数，所以占用内存大的问题依然存在．本文提出固定八方向搜索法，即步长增加时搜索方向数保持不变，这与文献^[14]的无限邻域思想是相反的．固定八方向原理如图7所示．

对比图5与图7中评估节点数可以得出表2的结果．

注4  评估节点的数量不仅会占用大量内存，同时也会影响算法寻路耗时，评估节点数的减少会进一步提升算法的轨迹规划效率．

2.5 算法流程

算法流程（图8）如下:

Step 1:根据当前点和终点的位置关系，计算第一搜索方向．

Step 2:根据自适应步长策略，选取步长数．

Step 3:根据到达终点条件判断是否到达终点，如果是，进入Step 5，如果不是，则进入Step 4．

Step 4:按照A*算法规则进行迭代，并返回Step 1．

Step 5:输出路径轨迹．

Step 6:算法结束．

以下给出算法的简易例程，如图9所示．开始时N=0，起点为当前点，并将起点放入封闭列表中，终点与当前点的夹角θ=arctan$\left(\frac{9}{9}\right)$=0.785 398，θ∈$\left(\frac{\pi }{8}，\frac{3\pi }{8}\right]$，且sin θ＞0．根据表1，得出“1”方向为第一搜索方向，代价评估函数f（0）=18．因该方向上第3格为障碍物，根据2.3节中的情况2，计算自适应步长λ=2，更新N=1为当前节点，并将其放入封闭列表中，代价评估函数f（1）=2$\sqrt{2}$+12．在节点N=1处，由于其第一搜索方向不可通行，按照原始A*算法寻路，将邻域节点放入开放列表中，根据式（1）可得开放列表集合为:{f（2）=2$\sqrt{2}$+12，f（4）=3$\sqrt{2}$+12，f（5）=2$\sqrt{2}$+14，f（6）=3$\sqrt{2}$+14，f（7）=2$\sqrt{2}$+14，f（8）=3$\sqrt{2}$+12，f（9）=2$\sqrt{2}$+12}，选取其中最小的作为下一评估节点，即f（2）与f（9）．以节点“9”作为当前点拓展时，根据A*算法不将已经评估过的节点重新纳入评估系统的规则，即开放列表中只有f（10）=3$\sqrt{2}$+12．以节点“2”作为当前点拓展时，夹角θ=arctan$\left(\frac{6}{5}\right)$=0.876 058，θ∈$\left(\frac{\pi }{8}，\frac{3\pi }{8}\right]$，且sin θ＞0，则“1”方向作为第一搜索方向．根据2.3节中情况1，计算出自适应步长λ=5，此时f（3）=7$\sqrt{2}$+2，由于f（3）＜f（10），故对节点“9”进行剪枝处理，舍去该节点及其分支，并将节点“2”放入封闭列表中．更新节点“3”为当前点，由于满足到达终点的判断条件，故将节点“3”与终点放入封闭列表中，此时封闭列表中有:{起点，N=1，N=2，N=3，终点}，其连线为路径轨迹，算法迭代结束．

3 仿真及实验验证

3.1 仿真分析

为了验证算法的效果，本文针对原始A*算法和改进后的算法进行了测试实验，可验证本文算法的正确性．系统CPU为i5-8257U，操作系统为Windows 10，主频为1.4 GHz，运行内存为8 GB，在主机上运行MATLAB 2021a进行仿真．

如图10所示，在15×15的栅格地图上进行仿真实验，绿色为起点，红色为终点，黑色为障碍物，黄色为路径节点，灰色为路径计算时参与的评估节点，可以看出相同环境下两种算法生成的路径轨迹区别不大，但改进的A*算法在规划路径轨迹时参与的路径节点和评估节点数要远少于原始的A*算法．

由于机器人的编程语言多为C++语言，为进一步验证算法的效果，本文在C++语言环境下做了大地图的仿真测试，实验环境均为障碍物占比率为1%的地图．在250×200的地图中，其测试结果如图11a所示; 在400×250的地图中，其测试结果如图11b所示．其中，黑色为障碍物，空白区域为可通行空间，红色为改进算法的路径轨迹，蓝色为原始A*算法的路径轨迹．

表3给出了原始算法与改进算法在不同尺寸的栅格地图中路径规划用时及节点个数的数据对比．路径规划算法的评价指标有时间和路径长度两种，本文主要以时间作为评价指标．

同时本文以障碍物占比率为变量进行多次仿真实验，结果如图12所示．可以看出障碍物的数量变化对原始A*算法的影响很小，对改进的算法影响极大．由于改进算法的自适应步长选择是根据当前节点与障碍物的位置关系决定的，所以障碍物占比率低的地图，路径规划效率提升明显．

3.2 实验分析

为了验证算法的实际效果，本文将改进的算法作为路径规划器插件应用在实物机器人上，实物机器人采用的启智底盘，如图13a所示．实验场地为18 m×12 m的密闭空间，室内放置了桌子、纸箱等障碍物，如图13b所示． 场地中以同一起点和终点做了两次路径规划任务，其路径轨迹结果在Rviz显示如图14所示，可以看出两种算法规划的路径轨迹近似，但本文改进的算法在用时上远少于原始算法．同时表4给出4种算法在实验环境下的路径规划耗时对比，可以看出文献^[5]算法的路径虽然更优，但耗时较长，在大地图时并不适用，文献^[16]与本文算法路径长度近似，但本文算法规划用时更短．

4 结束语

本文针对A*算法路径规划耗时长的问题，加入了自适应步长策略; 针对内存溢出问题，提出八方向搜索法，并通过设定搜索方向的优先级顺序，进一步减少评估节点，有效地降低了内存使用率．改进后的算法能更好地满足机器人路径规划任务，并通过仿真和实际实验验证了该方法的有效性．

参考文献