融合改进A*蚁群和滚动窗口法的平滑路径规划

殷绍伟，彭 力+，戴菲菲

1.物联网技术应用教育部工程研究中心（江南大学 物联网工程学院），江苏 无锡 214122

2.台州市产品质量安全检测研究院，浙江 台州 318000

路径规划是移动机器人领域的一个重要研究方向，其主要目标就是通过一定的智能算法求得避开所有障碍物的最优安全路径。目前对于已知静态障碍物环境下的全局路径规划研究已经有很多，比如人工势场法[1]、遗传算法[2-3]、神经网络[4]等。人工势场法相对简单，且便于底层实时控制，但是对于稍微复杂的地图，就会出现死锁、停滞的现象，而且极易陷入局部最优；遗传算法虽然全局搜索能力强，但是计算量大，收敛慢；神经网络学习能力强，但是地图变化或者存在动态障碍物时，网络结构复杂且需要不断地改变阈值。

蚁群算法（ant colony optimization，ACO）[5]是Dorigo受生物进化启发提出，其因为正反馈原理、天然的并行性以及较强的鲁棒性等优点，在路径规划[6-8]、网络路由[9]等方面得到广泛应用。但是蚁群算法也有一定的缺点，比如收敛速度慢、易陷入局部最优。针对蚁群算法的不足，国内外不少学者提出过改进，文献[10]提出将人工势场法与ACO 结合，并且对路径进行几何优化；文献[11]引入动态搜索诱导算子控制算法的搜索方向，提高算法多样性与收敛速度，再结合k 交换算子避免早熟。虽然文献[10-11]的实验结果表明各自改进后算法收敛速度加快，规划的路径长度变短，但是未解决改进后算法参数多而且难调的问题，并且文献[10-11]都只是在静态环境下路径规划，最后规划出的路径也并不平滑。

针对蚁群算法的不足以及动态路径规划难题，本文提出一种融合改进A*蚁群算法与滚动窗口法的平滑路径规划方法。首先以改进A*算法的评价函数为标准，规划出一条长度相对较短的路径，增强该路径上的初始信息素浓度，优化信息素初值，再用改进的蚁群算法规划出全局路径。然后结合滚动窗口法，制定一系列的动态避障策略，在全局路径的基础上进行实时的局部路径规划。最后，引入贝塞尔曲线对路径进行平滑处理，使移动机器人更加准确地向目标点移动。为确保改进蚁群算法展现出最好的性能，提出使用遗传算法（genetic algorithms，GA）对本文算法中重要参数进行自主优化选择。

1 环境建模

在二维移动区域内，假设移动机器人占地是半径为r的圆形，对于各种形状的障碍物，对其进行膨胀处理，膨胀宽度为r。然后用栅格法对二维环境进行建模，其中不满一个栅格的障碍按一个栅格计算。因为建模中有对障碍物进行膨胀处理，所以本文移动机器人可以视为质点，并且假设机器人有匀速运动与暂停两种状态，且这两种状态可以任意切换。

利用栅格法进行环境建模，能够很好地描述任何障碍物，而且可用二值信息存储障碍物的特征和位置，方便计算机存储与计算。计算机存储中，每个栅格的信息包括状态和索引。其中状态为障碍物栅格和可行栅格，而索引包括坐标和序号两种。如图1所示，对栅格地图建立直角坐标系，以地图的左下角作为坐标原点，水平向右为X轴正方向，垂直向上为Y轴正方向。假设栅格边长为λ，以λ为基本单位来划分X轴和Y轴，则地图的行数为Ny=ymax/λ，列数为Nx=xmax/λ，其中xmax和ymax分别为X轴和Y轴上的最大值。同时标记原点栅格序号为1，栅格序号从左到右、从下到上依次递增。坐标法与序号法是同一个栅格的两种不同表示方法，之间可以互相转换，如i号栅格对应的栅格中心坐标(xi,yi)为：

Fig.1 Grid index图1 栅格索引

其中，mod 表示取余操作，即取(i-1)/Nx的余数，ceil表示取整操作。

2 改进A*蚁群算法

传统的蚁群算法通过每代蚂蚁不断尝试寻找目标点，并且在路线上留下信息素。下一代蚂蚁总是会优先沿着信息素浓度高的道路追寻目标点。蚁群算法在用于路径规划时，整个寻找最优路径的过程有以下几点不足：第一，初始几代蚂蚁由于道路上信息素匮乏且浓度差别不大，搜索路线盲目，导致效率低；第二，它的正反馈机制虽有利于加快收敛，但是容易陷入局部最优，对于稍微复杂地图尤其是含有凹型障碍物的地图，还有可能出现死锁现象；第三，信息素更新方式过于笼统，优秀蚂蚁与劣质蚂蚁之间的差距体现不明显，导致算法收敛速度较慢。

2.1 改进A*算法初始化信息素

蚁群在前几次搜索尤其是首次搜索时，路径上的信息素都是均匀分布的，故蚁群只能进行盲目搜索，效率极低。因此提出用改进A*算法对信息素进行初始化，使蚁群算法在初期的收敛速度加快。

A*算法是一种全局路径规划算法，它的主要优点是速度快。其使用的评价函数为：

式中，g(n) 表示起点s到当前栅格n的移动消耗；h(n)表示当前栅格n到终点g的预估移动消耗（启发函数）[12]。

A*算法是一种启发式搜索算法，启发函数h(n)对算法的路径规划性能有较大影响。假设H(n)为当前栅格n到终点g的实际移动消耗，当h(n)H(n)时搜索节点少，运行效率高，但一般难以找到最优路径。

传统的A*算法的启发函数通常采用Euclidean 距离或Manhattan 距离定义，即：

式中，nx、ny、gx、gy分别为当前栅格点和终点。

A*算法每次搜索当前栅格的8 个相邻栅格，如果采用曼哈顿距离来定义启发函数，则只允许朝着上下左右四个方向移动，而本文允许对角线方向移动，因此曼哈顿距离定义启发函数并不合适；而欧几里德距离则允许朝着任意方向移动，不满足本文采用栅格法对环境建模的基本条件，因此也不适合作为启发函数。

考虑到这两种启发函数各自的优点与不足，本文设计了一种更接近实际移动消耗H(n)的启发函数。首先考虑沿着对角线运动，朝着对角线移动一个栅格代价为（假设直线移动代价为1），则总的对角线移动代价为×min(|nx-gx|,|ny-gy|)；然后考虑沿着直线运动，得出总的直线移动代价为hM(n)-2×(min(|nx-gx|,|ny-gy|))。本文设计的启发函数为总的对角线移动代价与总的直线移动代价的算术和，即：

式中，dx(n)=|nx-gx|，dy(n)=|ny-gy|。

本文将A*算法所求路径上的初始信息素设置为：

其余路径上的初始信息素保持不变，仍然为常数c。

2.2 改进状态转移概率函数

经典蚁群算法的状态转移概率函数主要考虑了信息素分布以及当前点与相邻可行栅格点之间的距离。其应用于复杂环境时，尤其是有凹型障碍物的环境中，机器人很可能陷入如图2 所示的死锁状态。

Fig.2 Robot deadlock state图2 机器人死锁状态

针对死锁问题，常见的解决办法是允许陷入死锁的蚂蚁后退，更新禁忌表后再依据状态转移函数重新选择下一个栅格[13]。如果是实时在线路径规划，这种方法在复杂环境下得到的路线很可能不是最优的；即使是离线规划，蚂蚁也需要判断是否处于死锁状态，如果是则需后退重新选择，搜索效率降低，而且死锁点的信息素浓度需要经过多次更新后才能与其他栅格点拉开差距，从而避免下一代的蚂蚁再次进入死锁点，显然采用这种策略来规避死锁点可行性不高。文献[14]通过设置进入死锁点的蚂蚁死亡，并把死亡蚂蚁留下的信息素清除，可以有效避免死锁。但是如果地图较为复杂，每一代都会有大量蚂蚁被设置为死亡，这将不利于算法得到收敛，并且会影响算法对最优解的探索能力。

本文改进状态转移函数，在其中加入“活跃度”因子，具体如下：

其中τij(t)为在时刻t栅格i到j之间路径的信息素浓度[5]；Aj(t)为栅格j的“活跃度”；ηij(t)为启发函数，式（10）表示传统蚁群算法的启发函数，很明显该公式只考虑栅格i到栅格j之间的距离，因此，正方向的相邻可行栅格比斜方向的相邻栅格更容易被启发函数选中。这样选择过于片面，故此处考虑终点栅格位置，本文将启发函数改为式（11）。

式（8）中liveness表示与栅格j相邻的栅格个数，地图中间与之相邻的栅格个数为8，而四周（不包括顶点）与之相邻的栅格个数为5，顶点则为3；obj表示栅格j周边最大连续障碍物的个数，如图2 中的栅格41 为4，栅格75 为3。如此对于栅格41 与55，如果它们非终点，其“活跃度”经过式（8）、式（9）计算为0，代入式（7）得死锁点被选择的概率为0。因此加入“活跃度”因子后，根据式（7）得出的状态转移概率是否为0，就可以判断该点是否为死锁点。一旦判断为死锁点，蚂蚁就不会经过该栅格，也就不用使陷入死锁点的蚂蚁后退或直接把进入死锁点的蚂蚁设置为死亡等操作，使得该算法的效率和收敛速度得到较大提高。同时为了保证选择的多样性，根据计算出的状态转移概率值，用轮盘赌算法对下一个栅格进行选择。

2.3 基于不平等原则的信息素更新机制

传统的蚁群算法信息素更新策略是将前几代蚂蚁留下的信息素之和按照一定的比例挥发后加上当代蚂蚁留下的信息素，具体公式如下：

式中，ρ∈[0,1]为信息素挥发系数，Q是信息素强度，Lk表示第k只蚂蚁本次迭代所走的路径长度。由式（14）可知，当代蚂蚁留下的信息素只是与走过的路径长度成简单反比关系。当地图变得复杂，起点与终点相距较远时，Lk取值必将变大。同时如果信息素强度Q取值不变大，当代优秀蚂蚁与劣质蚂蚁留下的信息素将几乎没什么差距，放缓了算法的收敛的速度。故此处将式（14）改为式（15），对蚂蚁之间的差距进行放大，并且直接清除掉每代最差蚂蚁信息素。

算法中对于未找到终点的蚂蚁，其路径长度定义为inf，式中Lmax是指除inf以外的最大值。当蚂蚁路径长度为Lmax时，也就是在找到终点的蚂蚁中最差的，其经过式（15）计算产生的信息素为0。当Lk∈[Lmin,Lmax)时，计算式（14）与式（15）之间的差值，记为E，如式（16）。将式（16）转化为式（17），当时，将E对Lk求导记为E′，且易得E′<0，如同式（18）；同理当时，得E′>0。由此可得，相对于传统的信息素更新策略，式（15）拉大了不同蚂蚁之间的差距，使优质蚂蚁与劣质蚂蚁差距变大。路程更短的蚂蚁，遗留的信息素相比传统的更新方式更多；路程更长的蚂蚁，遗留的信息素比传统的更新方式更少。当路程短的蚂蚁可以留下更多的信息素时，经过算法的信息素更新机制数次迭代之后，路程短的与路程长的蚂蚁之间的差距迅速增大。较短路径由于经过的蚂蚁数量较多，栅格上的信息素越来越多；而相对长的路径由于只有较少的蚂蚁会经过，栅格上的信息素得不到显著增加，而且由于信息素会慢慢挥发，以至于最后较长路径上的信息素可以忽略不记。因此在数次迭代之后，最短路径上的信息素浓度会比其他路径上的更多，而且差距显著，代入式（7）计算，得到的状态转移概率会逐渐接近于1，路径规划算法也得到收敛。因此，基于不平等原则的信息素更新机制使得改进后的算法更容易收敛。

3 基于遗传算法的参数优化

本文改进的A*蚁群算法需要对较多参数进行初始化，并且目前还没有严格的理论依据对蚁群算法参数进行正定，一般都是根据具体情况进行分析，依据实验仿真效果与自身经验不断调试所得。然而这样做不仅费时、主观性强，而且选择的参数容易是局部最优参数。本文需要初始化的参数共有7 个，其中较为重要的参数有：改进A*算法初始化信息素放大倍数k，信息素与启发函数的权重α和β，信息素挥发系数ρ，信息素强度Q。这些参数在很大程度上会影响算法的性能，虽然根据经验各自都有取值范围，但是每个参数排列组合难以调取，因此本文提出用带精英策略的简单遗传算法对这5 个较为重要参数进行自主优化选择。

3.1 带精英策略的遗传算法

选择遗传算法主要是因为遗传算法寻优能力强，参数少，且参数取值范围小，易调节。文献[15]用粒子群算法（particle swarm optimization，PSO）优化蚁群算法中的参数α和β，但是PSO 本身要对5 个参数进行初始化，其中惯性权值w，学习因子c1和c2对算法性能有较大影响，并且PSO 易早熟于局部最优。

传统遗传算法主要参数只有交叉概率pc和变异概率pm，且这两个参数的取值范围较小，一般pc∈[0.7,1.0)，pm∈(0,0.3]。当交叉概率过小时，种群不能进行有效的更新，收敛结果不具有说服力；而交叉概率过大时，随机性增大，容易破坏已有的最优解，错失最优个体。变异概率过小时，种群的多样性下降快，会导致收敛不稳定；而变异概率过大时，虽然会增加种群的多样性，但是可能会使优秀个体尚未得到保存就被破坏，最后得到的结果并不是最优解。但是总的来说遗传算法参数相对于蚁群算法个数少，取值范围小，因此更易选择。本文遗传算法的参数是根据自身经验以及经过不断调试所得，同时为加快遗传算法的收敛速度，本文在经典遗传算法的基础上添加精英策略，即从父代种群中抽取一定比例的精英不经过遗传算子操作直接复制到子代。

3.2 参数优化步骤

本文遗传算法的适应度函数设置为路径的长度，依据一般经验与算法本身的约束条件，将改进的A*蚁群算法中5个重要参数限定为以下取值范围：k∈(1,5),α∈(0,2),β∈(5,25),ρ∈(0,1),Q∈(5,35)。具体步骤如下：

步骤1初始化带精英策略的遗传算法最大迭代次数为200，交叉概率pc与变异概率pm分别为0.8 和0.1，种群大小m为200。

步骤2对改进蚁群算法中5 个重要的参数进行有约束的编码，编码方式为二进制编码，约束条件是各参数的取值范围。

步骤3计算适应值，依据适应值进行排序，将前5%的精英不经过交叉变异直接复制给下一代。

步骤4父代进行交叉与变异操作，将产生的子代与父代合并，用二元锦标赛法从中选出种群大小m的95%传入下一代。

步骤5判断遗传算法是否达到最大迭代次数或者收敛。若是，则选取出最优解作为本文改进A*蚁群算法的参数，否则转到步骤3。

4 基于滚动窗口法的动态防碰策略

设滚动窗口刷新时间为T，窗口的大小（即机器人的探测范围）为以机器人为中心半径为R的圆形区域，机器人在时间T内的最大移动距离为Sr，动态障碍物在相同时间内的移动距离为So[16]。根据滚动窗口的定义，想要滚动窗口有效，需满足So≤Sr的前提，其中Sr∈(0,R/2]。

4.1 对直线运动的障碍物防碰策略

对于以速度V0沿直线运动的障碍物，可以预测出在滚动窗口周期内障碍物的运动轨迹。根据此轨迹与滚动窗口内机器人的全局规划路径，可以判断出二者是否有碰撞的风险。

（1）若无碰撞，则机器人按照全局规划路径移动一个周期，进入下次窗口刷新。

（2）若侧面碰撞，则在预估碰撞点前一个栅格点等待Δt。Δt为：

式中，d0为动态障碍物的长度。

（3）若正面碰撞，将全局路径与滚动窗口的交点视为局部规划的子目标点，预估碰撞点视为静态障碍物，以当前点为起始点，用本文算法进行局部路径规划。此时，分为两种情形：局部路径规划有解与无解。针对无解的情形，如图3 所示，在滚动窗口内，机器人r与动态障碍物O相向运动，预估二者将在A点相遇。按照上面局部规划方法，子目标点B将到达不了。因此，针对上述局部路径规划无解的情况，将不再进行局部规划，重新开始全局规划。

Fig.3 Sub-goal point is unreachable图3 子目标点不可达

4.2 对运动方向不确定的障碍物防碰策略

对于以速度V0运动且方向不确定的动态障碍物，文献[16]的策略是将动态障碍物的活动范围全部视为静态障碍物。这种策略过于保守，且动态障碍物的整个运动范围可能包含机器人当前的位置。

本文的策略是：当滚动窗口内检测到动态障碍物时，则开始以其所在位置为圆心，以速度V0向外进行膨胀，膨胀区域则可视为静态障碍物。机器人因此可以判断滚动窗口内的全局规划路径是否会有与动态障碍物碰撞的风险。若有，则按照5.1 节的策略进行局部路径规划，若无法规划出可行的局部路径，则需要重新进行全局路径规划。

5 贝塞尔曲线平滑路径

路径规划是求得移动机器人从起始点到终点的最优路径。因此，其所产生的路径需满足平滑性的需求，尽可能地保证所规划出的路径与实际运动路径相同。由于本文采用栅格法表示环境地图，可能会在路径转折处产生尖峰。

贝塞尔曲线的最大优点是具有优良的拟合特性，即如果控制点构成的多边形是凸的，那么贝塞尔曲线也会是凸的。因此，本文采用贝塞尔曲线来平滑规划出的路径。

5.1 贝塞尔曲线方程

给定R3:B:[0,1]→R3，它代表n+1 个控制点，i=0,1,…,n。n次贝塞尔曲线可由n次Bernestein 基多项式组成的参数曲线表示，定义为[17]：

式中，t表示归一化时间变量；Bi,n(t)是Bernestein 基多项式，代表贝塞尔曲线中的基函数，其表达式定义如下：

贝塞尔曲线的导数由控制点决定，其一阶导数可以表示为：

在二维空间中，贝塞尔曲线相对于t的曲率可以表示为：

式中，R(t)为曲率半径；分别为贝塞尔曲线坐标分量对x和y的一阶导数和二阶导数。

5.2 贝塞尔曲线平滑路径

在改进A*蚁群算法在存在静态或动态障碍物的情况下得到最短路径后，由贝塞尔曲线对路径进行平滑处理，称为路径的后续处理环节，其具体流程图如图4 所示。

Fig.4 Smoothing process of Bessel curve图4 贝塞尔曲线的平滑流程

6 仿真分析

本文对传统蚁群算法在路径规划应用上的一些不足之处进行了改进，下面借助Matlab，在CPU 为i5-6500、系统为Win10 的电脑上进行仿真实验。

6.1 与原蚁群算法比较

将本文改进的A*蚁群算法与原蚁群算法在20×20 的栅格地图上进行对比实验，本文算法的初始条件为：最大迭代数为100，蚁群个数为30，遗传算法的最大迭代数为20，种群大小为25，交叉概率为0.8，变异概率为0.05，剩余5 个重要参数的变化范围见表1。由带精英策略的遗传算法得到它们的最佳组合为k=3.9，α=0.98，β=14.01，ρ=0.69，Q=18.53。设起始点为栅格1，终点为栅格400。对于原有的蚁群算法，所有参数都需按照经验自行设定，现设置具体参数为：最大迭代数为100，蚁群个数为30，α=0.60，β=11.00，ρ=0.50，Q=15.50 。图5 为未进行参数优化的原蚁群算法和本文改进的A*蚁群算法路径图的对比结果。将本文算法得到的最佳组合参数（k除外）应用于原蚁群算法，其得到的路径与本文改进的A*蚁群算法路径对比图如图6 所示。以上三种算法分别对应的收敛曲线图如图7 所示。图8 为本文改进的A*蚁群算法规划出的路径和采用贝塞尔曲线平滑后的路径对比图，对比原路径可知贝塞尔曲线对其平滑后的路径更加符合实际运动路线。

Table 1 Variation range of important parameters表1 重要参数的变化范围

Fig.5 Compared with ACO without parameter optimization图5 与未参优蚁群算法对比

Fig.6 Compared with ACO after parameter optimization图6 与参优后蚁群算法对比

Fig.7 Convergence curves of three algorithms图7 三种算法的收敛曲线

Fig.8 Smooth path diagram under simple obstacles图8 简单障碍物下的平滑路径图

Table 2 Compared with original ACO表2 与原蚁群算法对比

图5～图7 的具体对比数据见表2，对比参数优化与非参数优化情况下原蚁群的路径长度，可知蚁群算法的性能很大程度上受参数的影响，虽然原蚁群的参数是依据经验而非胡乱选得，但是也是局部最优参数，可见本文用遗传算法优化参数有一定的必要性与优越性。对比本文算法与参数优化后的原蚁群算法，不仅路径长度由30.041 6 缩短到28.627 4，而且收敛情况也由未收敛变为迭代11 次后收敛，可见本文算法寻优能力与收敛速度都大幅度提高。

6.2 与改进蚁群算法比较

为了证明本文算法在复杂静态环境中的优越性，选用其他改进的蚁群算法进行对比。文献[11]提出了一种动态搜索策略的蚁群算法（ant colony algorithm for dynamic search strategy），并在50×50 的复杂环境中进行验证。由于在文献[11]中已用其方法与其他改进算法在相同环境下进行了对比实验，证明了其优越性，故在此只需与文献[11]进行对比。本文算法的参数初始化同6.1 节（下文同），图9 为两种方法的路径对比图，其中本文算法的收敛曲线图如图10 所示，相关数据对比见表3，其中标准差按照文献[11]中的定义，为算法运行10 次的误差。而图11 为本文算法规划出的路径与采用贝塞尔曲线对其进行平滑处理后的路径对比图。

Fig.9 Compared with algorithm in ref.[11]图9 与文献[11]算法对比

Fig.10 Convergence curve of algorithm in this paper图10 本文算法收敛曲线

Table 3 Compared with improved ACO表3 与改进蚁群算法对比

Fig.11 Smooth path diagram under complex obstacles图11 复杂障碍物下的平滑路径图

从路径对比图与表3 中的数据对比可知，本文算法无论是路径长度、收敛迭代次数，还是稳定性（标准差比较）都略优于文献[11]。而且文献[11]中的参数是通过经验与不断调试所得，本文改进A*蚁群的重要参数是自主寻优所得。

6.3 动态条件下验证可行性

仿真实验1在20×20 和40×40 的栅格地图中，矩形动态障碍物沿竖直方向，以小于机器人的速度做匀速往返运动。当动态障碍物与全局规划路径未发生碰撞时，机器人的路径如图12 所示；当动态障碍物影响到既定的全局规划路径时，路径图如图13 所示。图13 中虚线是原定路线，实线是根据动态避障策略，进行局部路径规划的实际路线。

仿真实验2实验1 验证了障碍物沿直线运动的情况，下面验证障碍物运动的方向随机的情况。如图14 所示，在50×50 的复杂环境中，机器人1 从S1(0.5,0.5)到G1(49.5,49.5)，而机器人2 从S2(0.5,31.5)到G2(49.5,13.5) 。两个机器人相互独立，速度相同（满足滚动窗口前提），对于两个机器人而言，另一方均可视为运动方向不确定的动态障碍物。图中的交点为（17.730 8,20.730 8），由于两机器人速度相同，故二者并未同时到达，图中路径为安全路径。

7 结束语

本文针对静态与动态环境下移动机器人路径规划问题，提出一种融合改进A*蚁群算法与滚动窗口法的平滑路径规划方法。该方法克服了传统蚁群算法收敛慢、易陷入局部最优等一系列问题。通过对三组仿真数据的分析与对比，证明了该算法每个环节改进的必要性与优越性。仿真结果表明，本文算法不仅能实现复杂静态障碍物环境下机器人的路径规划，而且对于一定的动态障碍物也能实现高效的避让。

Fig.12 Path planning without collision图12 未发生碰撞时路径规划图

Fig.13 Path planning when avoiding dynamic obstacles图13 避开动态障碍物时路径规划图

Fig.14 Dynamic obstacles avoidance diagram with uncertain motion direction图14 对运动方向不确定的动态障碍物避障图

本文也尚存在不足之处，例如本文在对动态障碍物的仿真实验中假设移动机器人一直处于匀速运动状态，然而在实际生活中机器人并不会一直处于匀速运动状态。在以后的研究工作中，针对这个问题将会引入不同速度以及存在加速度的机器人进行各种仿真实验，进一步提高该算法的实际应用价值。