基于视野域的快速动态路径规划

钟志峰， 周 霖， 柯 伟

（湖北大学 计算机与信息工程学院， 湖北 武汉 430062）

0 引 言

伴随科技的发展和现实的需要，路径规划应用范围越来越广，如日常生活中的手机导航、仓储的自动物流分拣、智能仓储系统等。在路径规划的系统要求中，如何在最短时间内自主规划一条最短、行走最快的路径是评判路径规划算法有效性的重要标准之一。而蚁群算法作为一种群智能算法[1]，具有很强的鲁棒性和搜索较好解的能力，因此广泛地被国内外学者应用于自主路径规划中。但是随着时代的发展，基础算法面对越来越多的不同领域的应用需求，出现了如容易陷入局部最优、计算时间较长、收敛慢以及转弯角度过多等问题。因此，针对该算法的改进一直是热门方向[2]。

对于陷入局部最优的改进，提出了诸如在选择决策中通过对蚂蚁转向角度的限制减少转角次数，从而减少路径长度[3]；文献[4]通过设置不均等的初始化信息素浓度，使蚂蚁在最有可能出现最优解的区域进行重点搜索，提高算法效率；文献[5-6]结合人工势场，能有效地绕开障碍物，并加大了启发因子的影响。以上文献虽然对于自主寻迹的下一步移动策略有较大的优化，但都忽略了环境因素的影响，在复杂的环境条件下，仍然会存在收敛速度慢、难以找到最优解等问题。文献[7]考虑到环境的影响，应用负反馈机制增大搜索范围，但也增加了陷入死胡同的概率。文献[8]采用回退策略来减少蚂蚁进入凹形障碍物的概率，但这种方法计算量大且不能完全杜绝进入凹形障碍物的情况。

对于减少转折角的改进，首先是提出加入转角数判断因子[9]，结合转角和最短路径来选择最优路径，但是算法中单步步长[10-11]的约束使其转折角的数量无法进一步降低。因此继续提出步长优化算法[12-13]，通过对指定步长的优化来减少转角数量，但是指定步长不够灵活，固定步长难以适应不同的场合。在此基础上，马飞宇等人进一步提出变步长算法[14]，将判断区域和行走步长分开来，扩大判断区域来实现动态步长。但是判断区域的范围已被指定，导致步长只能在判断区域范围内进行行走，对于步长选择优化不明显。

本文提出一种基于视野域的动态快速路径规划算法，优先对于地图进行处理，采用填充法解决了蚂蚁陷入凹陷问题，也提高了蚂蚁行走的效率。再采取改进的负反馈思想，结合负信息素减少没有找到终点路径和最差路径上的后续蚂蚁行走概率。最后借鉴自然界中兵蚁和工蚁的现象，给工蚁加上视觉，引进视野域的思想优化当前找出的最优路径。在实际行走中若存在障碍物动态变化和多个新增障碍物的现象，重新规划部分路径时结合DWA 算法找出新的最优路径。实验结果表明，本文提出的改进算法能够很好地解决基础算法出现的不足，具有迭代次数少、转角少、行走时间短、能快速重新规划新的最优路径等优点。

1 相关算法介绍

1.1 建立栅格地图环境

常用的环境建模方法有栅格图法、自由空间法等。本文选择简单直观的栅格图法对目标二维运动空间进行环境建模。栅格由0、1 矩阵组成，0 代表自由栅格，表示无障碍物，颜色为白色；1 代表障碍栅格，表示存在障碍物，无法通过，颜色为黑色。设置自由栅格和禁忌栅格都为边长为1 的正方形，如图1 所示。同时设置蚂蚁的行走方向为8 个方向角，但是存在2 个障碍物夹角是不可以通过的，如图2 所示。图中右上角位置的箭头方向代表不可通过，其他箭头代表可以通过。

图1 栅格地图

图2 方向图

对于图1 建立的栅格图，为便于在后续计算中标识蚂蚁的位置，需将n×n栅格矩阵中的每一个栅格按照从左上角开始从上往下，再从左往右进行编号，编号i和其坐标(x,y)的对应关系如下：

1.2 负反馈蚁群算法

负反馈蚁群算法[15]是针对基础蚁群算法中没有找到目标点蚂蚁，提出的一种在这些死去的蚂蚁的路径上释放一种负信息的思想，其蚂蚁移动概率公式如下：

式中增加了负反馈信息因子δ-φij(t)。其中：δ为常数，表示负信息素浓度的上限；φij(t)为负信息素矩阵。设信息素挥发系数为ρ（0＜ρ＜1），则公式（3）分别表示正、负信息素的更新规则。

1.3 自适应动态窗口算法

动态窗口算法（DWA）是一种局部避障算法[16-17]，将机器人的位置控制优化为速度控制，然后将角速度旋转和速度控制范围结合起来得出所有可能路线，如图3 所示。再根据目标函数选出其中合适的路线。针对经典DWA 算法不能兼顾速度与安全，以及在高速通过障碍密集区时通过性较差的问题，提出一种自适应DWA 算法，其在不同障碍物密度环境下的目标函数中的速度权值不同，如下：

图3 动态路线选择

式中：heading 代表与障碍物的距离；dist 代表与当前目标点的距离；v、w代表当前速度和角速度；α、β、γd代表不同的权值。γd的取值如下：

2 基于视野域的快速动态路径规划的蚁群算法模型

2.1 地图环境填充优化

图1 中这种存在大量凹陷的障碍物，会使算法中的蚂蚁更容易陷入凹形陷阱当中，需要对其进行优化，这也是为后文中的视野域算法降低图形复杂度和减少计算量。为解决这一类问题，本文采用的环境优化算法通过分解-填充-合并的方式将地图环境中的凹形障碍物进行处理。

分解过程：将地图中连在一起的点作为一个整体提取出来。分解过程表达式如下：

式中：f(x,y)函数的含义为当点(x,y)处存在障碍物时，将与点(x,y)相邻且存在障碍物的点的坐标放在一个集合S(x,y)中；f(i1,j1) ⋃f(i2,j2) 表示若集合S(i1,j1)与集合S(i2,j2)存在重叠坐标点时，则将集合S(i2,j2)中的元素放入集合S(i1,j1)中；unique 函数作用是去除集合中的重复元素。

通过上面的公式最终可以得到多个集合，每个集合存放的便是一块独立障碍物的位置坐标信息（即每个集合就是一块独立障碍物的子地图），对其进行标注序号，然后对每个子地图进行填充处理。其公式如下：

式中：Xmin、Xmax分别表示集合S中x的最小值和最大值；Ymin、Ymax分别表示集合S中y的最小值和最大值；F(x,y)函数给点(x,y)处赋值，1 代表存在障碍物，0 代表无障碍物。F函数的含义是当x= index（或者y= index）时，相应地在x方向（或者y方向上）进行填充。

对每个子地图进行填充后，将子地图合并得到最终处理后的地图。S计算公式为：

整个环境优化过程如图4 所示。

图4 环境优化过程

2.2 全局规划算法改进

在负反馈的基础上，为进一步提高搜索效率，本文采用了无效路径（没有到达目标点的路径）上的负反馈方法对基本蚁群算法进行改进，减少算法在无效区域的运算，同时也保留了最差有效路径负反馈的方法，减少陷入局部最优的情况。此外，为了减少算法的迭代次数，更快地找到最短路径，对于信息素和挥发系数等都进行优化。

2.2.1 改进的负反馈蚁群算法

公式（3）中Στkij(t)表示本次迭代中第k只蚂蚁在本次循环中留在节点i和j之间的信息素，这里采用Dorigom 提出的Ant-Cycle 模型，如式（9）所示：

式中：Q为信息素强度；Lk为蚂蚁k在本次循环中所走过路径的总长度；pk(begin,end)为蚂蚁k在本次循环中从起点到终点所走过的路径。

在文献[7]中只有最差路径上的负信息素，在这里增加未到达目标点路径上的负信息素。公式（3）中的Δτaddij表示每一代蚂蚁释放的负反馈激素，具体更新方式如下：

式中：Cw表示最差路径的长度；dk表示未到达目标点的路径长度；N为常量。当一次迭代完成后，会更新φij(t)矩阵，更新区域为：在整个最差路径和无效路径的后部分上释放负信息素。

2.2.2 改进的信息素分布方法

本文中对初始化地图上的信息素分布采取不均匀[18-19]的分布方式，如图5 所示。连接目标点和起始点，为了蚁群算法能够更快收敛得出结果的同时，也希望其能够在障碍物途中找出最短的路径。所以在距离起始和终点连线距离的dij2 的外部信息素浓度只有中间区域信息素浓度的2 3，有利于蚁群提高搜索速度，快速找到最优路线。

图5 信息素起始分布

同时为了防止负反馈导致某条路径信息素过高而另外的某条路径上信息素又过低，导致蚂蚁过于集中某条道路或者完全不走某条道路，将每个区域的信息素设置一个上下限度[1，8]，扩大蚂蚁的搜索能力，避免陷入局部最优解。

2.2.3 改进的启发函数

传统蚁群算法的启发函数都是在利用当前和下一步之间的距离倒数，或者当前位置和终点之间的距离倒数。这种函数前面的收敛性要么不强，使得蚁群寻找的路径冗长，要么后面一种收敛性过强，过于引导蚂蚁选择终点方向，从而出现错误选择而导致路线冗长。改进后的启发函数如下：

式中：dij为当前和下一步选择之间的距离；dis为当前位置和最终点之间的距离。将它们两个相减是为了权衡下一步的选择，使其能尽量避免陷入局部最优，增加蚁群算法的全局性。

2.2.4 增加的结束判断函数

在一般算法中是让蚁群固定地循环固定次数，但是蚁群已经找到最优路径而循环还没结束，因此在本文中设置了结束判断，即当蚂蚁已经找到了最优路径的情况下结束算法的运行。在算法中每一轮循环都会寻找出一个此轮最优的路径，根据这个机制结合实验，提出了当某个最优路径连续出现预设循环轮数K的1 10 次，默认为已经找到最优路径，退出循环。

2.2.5 动态视野域

视野域是指蚂蚁在网格地图中被网格地图能够看到的范围大小[20]。自然界中蚂蚁部落中有工蚁和兵蚁两种蚂蚁，工蚁根据信息素行走的同时也会跟随兵蚁，在前期工蚁已经找到食物和当前最短的路径时，会出动强壮的兵蚁前往。设定兵蚁能够使用嗅觉的同时也会使用它们的视觉，此时工蚁依旧在它们认为的最短路径中行走于起始点和终点之间（视作可以看见的一条线路），而兵蚁在嗅着信息素往前走的同时也观察视野里行走的蚂蚁，它们会朝向当前视野中最远的蚂蚁位置直线走去，而工蚁会跟随。兵蚁只有当视觉观察到转角（即当和最近障碍物的距离为移动机器人的半径时）或者走到上次视野观察最远的位置蚂蚁相遇，利用触角交换信息后，才会暂时停下来继续观察下一步。视野域如图6 所示。

图6 视野域

图6 中的粗线为初步规划路线，黑色框为障碍物，工蚁的视野域规划为图中虚线线段，最后新生成的路线如图中的细线段。

视野域的原理是：将先前规划好的路线分为若干个节点，蚂蚁从一个节点出发时视野域是全局的，但为加快程序运行速度，采取工蚁会优先向目标点方向望去（图6 中为右下角）。看到视野里最远的蚂蚁时会直接向该蚂蚁的位置走去，直至遇到转角（即当和最近障碍物的距离为移动机器人的半径时）或者走到观察最远的位置蚂蚁相遇时，生成一个新的节点和新的路线并记录，删除走过的路径。重新循环该方法直至走到终点。

式中：nextpot是下一个选择节点集合；A为路线节点集合；B是视野范围内的节点。从交集当中选出距离最远的。

采用动态视野域的思想主要目的是优化路线且减少运算时间，使得移动机器人更快地到达终点。其原理是移动机器人在行走过程中设定有最大和最小速度，以及加减加速度，而过多的转弯会使机器人频繁地加减速度，浪费不必要的时间。

2.3 局部规划算法

2.3.1 改进动态窗口算法

本文中，在Matlab 上仿真，设置地图每隔0.2 s 更新一次，检测地图规划路线上是否出现新的障碍物。当检测可以正常通过的位置遇到动态障碍物或者出现多个静态障碍物时，本文采取的是以当前节点为起点，重新规划路线和DWA 局部路径规划两种方法结合来处理。当路线上出现多个静态障碍物时，从当前起点重新规划最优路线。

式中：heading(v,w)、dist(v,w)、v(v,w)和high(v,w)分别为偏向目标方向、距离最近障碍物距离、当前速度和距离规划路线距离；Vα、Vβ、Vγ、Vδ代表权重因子。

自适应窗口法能够解决动态障碍物和单个静态障碍物的问题，改进部分使其能够快速寻找到最短的路径。机器人在相邻的节点运动过程中，通过自适应动态窗口算法[21]动态调节探索窗口大小得到所有可行行走方向区域后，在可行区域内，机器人通过式（14）中不同评价因素共同来确定最优前进方向。

自适应动态窗口算法关注的是在具有复杂障碍物的地图环境中行走时，过快速度和角速度容易发生危险，但是过慢会浪费时间，因此提出了自适应的速度和角速度，使其在障碍物较少的时候以较高的速度和较低的角速度行走，在障碍物较多的地方以较低的速度和较高的角速度行走，使其在障碍物多时行走的路线更加平滑。但是速度不是瞬间变化的，而是有加速度和角加速度的，所以随着周围环境的变化，下一时刻规定了最大的线速度和角速度。

式（15）是机器人的运动学约束，Vk表示机器人底盘实际上能够跑到的最大最小线速度和角速度。式（16）是机器人的动力学约束，Vb表示机器人底盘从当前时刻vc在经过时间段dt后能够达到的最大最小线速度和角速度范围，其中Vv代表线速度加减速度，Vw代表角速度加减速度。式（17）是机器人在环境障碍物的约束，Pnull、Pall是障碍物占栅格的比率，V(t+ 1)和W(t+ 1)代表下一时刻所走区域的最大最小线速度和角速度，Kv表示线速度调整参数，Kw表示角速度调整参数，主要是为了使其能够以较低的速度和较高的角速度通过障碍物较多的区域。

2.3.2 DWA 算法与重新规划结合

在实验仿真行走时，会出现多个静态障碍物挡住行走路线规划，如图7 所示。而此时采用DWA 算法进行局部规划，在障碍物中行走时，由于自适应速度，行走较慢；而以当前起点重新采取全局规划，可以使得行走时间更短，转弯节点更少。

图7 静态障碍物

判断应该采取DWA 算法还是重新规划的标准为：

式中：η、μ是权重参数；obs 是障碍物数量；Δx是第一个在线上到最后一个在线上的静态障碍物之间的距离；Pfull、Pall是障碍物占栅格的比率。通过实验测试表明，将η、μ设置为6 和4 的时候，当temp 为3.1，最佳；当temp小于3.1 时，采取DWA 算法；当temp 大于3.1 时，采取重新规划。

3 消融实验仿真与结果分析

为了验证环境优化算法改进的有效性，本文采用控制单一变量法进行多组对比实验，分别在简单环境下和复杂环境下进行对比试验，简单环境为20×20 栅格图，复杂环境为40×40 栅格图。同时在本文中，对于算法的静态实验和动态实验进行分开对比，对于静态实验，主要是对比算法步数、路径距离和收敛速度，对于动态实验，主要是研究路径距离。

3.1 实验环境及参数设置

实验环境为：WIN10 系统，CPU 为E5-2697，128 GB运行内存，Matlab 2016a。算法中使用的部分参数见表1，行走时间包括直线行走时间和固定转弯时间。

表1 主要参数表

为了验证环境优化和算法优化对提高目标物自主寻迹效率的有效性，本文采用传统常见的AS-N 算法（负反馈蚁群算法）、多步蚁长算法和本文改进的蚁群算法在2 组不同环境下进行对比测试实验，简单环境为20×20 栅格图，复杂环境为40×40 栅格图。

3.2 改进蚁群算法实验对比

简单环境下， AS-N、AS-N+填充、多步步长+填充、改进算法+填充对比结果如图8～图11 所示，不同算法的最小路径长度随迭代次数的变化曲线如图12 所示。

图8 AS-N

图9 AS-N+填充（一）

图10 多步步长+填充（一）

图11 改进算法+填充（一）

图12 迭代图（一）

比较图8 和图9 可知，在未填充的地图环境下，算法找到的最优路径都会陷入陷阱中，并最后出现局部收敛的情况，这种情况既增大了路径长度，也增大了计算量和行走时间。在填充后的环境图10 中，AS-N 算法找到的最优路径都相对短，几乎杜绝了陷入凹形障碍物的情况。从图12 可以看出，填充后的最小路径都相对较小，收敛速度都相对较快。而从表2 简单环境实验结果中可以详细地看到，改进的算法即使在填充后相同的环境，转角数量和行走时间都少于前者。

表2 简单环境实验结果

由图9～图11 比较可知，在简单环境下，本文改进的算法与AS-N 算法找到的最小路径长度相近，这是由于在相对简单的地图环境下，每代“蚂蚁”几乎都能顺利找到目标点；AS-N 算法和多步步长算法在这种情况下够用，这是由于本文改进算法在此类环境下对于减小最小路径长度的效果并不明显，但改进后的算法在行走时间上相比前两种算法较短。

但从图13～图15 复杂环境下不同算法实验结果中可以明显看出，在复杂的环境下，AS-N 算法和多步步长算法已经开始不适用了，最终收敛的路径并不是全局最优解。同时通过图16 和表3 可以看出，改进后的算法收敛路径变短、收敛速度提高且行走时间减少明显，算法运行时间也变短。

表3 复杂环境实验结果

图13 AS-N+填充（二）

图14 多步步长+填充（二）

图15 改进算法+填充（二）

图16 迭代图（二）

综上所述，在地图环境较为复杂时，AS-N 算法虽然能够找到最优解，但会有很大可能陷入局部最优，即陷入凹形障碍物中；经过环境优化后，AS-N 算法和多步步长算法便很容易找到最优解，且不会陷入凹形障碍物中，同时也减少了收敛所需的迭代次数，有效地提高了算法运行的效率和精度；在地图环境极其复杂的情况下，单靠AS-N 算法和多步步长算法已经很难找到最优解，且收敛速度慢。但本文改进算法能够降低蚂蚁前往无效区域探索的概率，使得算法集中在有效区域运算，从而找到的路径相对较短。最后，再通过环境的优化和改进算法的双重作用，最终取得了较好效果，解决了前文提出的各种问题。

3.3 改进结合动态算法实验分析

对于动态地图实验的仿真，为了便于观察和对比，对40×40 地图模型进行放大10 倍，在此基础上添加了如图7 中间的静态和移动障碍物，障碍物模型为边长为1的正方形方向指向正上，速度为0.1 m/s，在放大过后的模型上显示为1 m/s 和在地图中显示为10 的正方形，其余参数也对应放大10 倍。其运行结果如图17、图18所示。

图17 改进DWA 算法

图18 DWA 算法

从图17、图18 可以看出：对于中间的一个稍空旷地带的动态障碍物，改进DWA 算法的移动轨迹规划能更加靠近移动障碍物和全局路径来行走，生成的路径更优，而在距离目的地较近的两个障碍物，仍采取DWA算法会出现绕远路的情况，重新规划反而更加快捷。最后从表4 可以看出，改进算法的路径长度更短、行走时间更少。

表4 改进DWA 算法与DWA 算法实验结果对比

4 结 语

本文通过消融仿真实验，在改进的蚁群算法中，首先利用AS-N 算法进行了环境优化前后的对比实验，主要通过收敛所需的迭代次数（收敛速度）和收敛路径的长短这两个指标来进行性能分析，验证了填充的有效性；然后将改进AS-N 算法和动态视野域结合起来，进一步优化步长选择并更好地加快收敛，在简单和复杂地图下分别与AS-N 和多步步长算法比较。实验还证明改进算法在复杂环境中效果明显，能有效解决诸如收敛速度慢、盲目性搜索、局部最优、步长选择等复杂环境下的快速路径规划问题。最后采用改进的DWA 算法与重新规划结合，使其能够快速规避行走过程中新出现的动态障碍物或者多个静态障碍物。整体实验表明：采用该规划算法可使路径长度减少了11%，转角和行走时间减少了45.4%和32.3%。所以，该算法能为智能机器人在动态环境的自主规划与导航提供一种可行的解决办法。

注：本文通讯作者为周霖。