在给定运行空间中, 移动机器人路径规划的主要目标是通过一定的算法寻求一条从起始位置到目标位置的最优或次优无碰路径。目前针对全局信息已知的静态环境下移动机器人路径规划的研究已经很成熟。但是如何在有动态障碍物的环境背景下进行移动机器人路径规划仍是一个难题。

1 环境描述

设二维平面上的矩形区域E为机器人的运行环境空间。对机器人运行环境空间的建模采用栅格法, 且其中同时存在静态和动态障碍物。静态障碍物可以是任意形状, 且数量有限; 动态障碍物为圆形且数量有限。若静态障碍物只占据某个栅格的一部分, 则将其视为完全占据此栅格。此外对移动机器人运行环境和移动机器人本身做出以下假设:

1) 移动机器人视为直径为D_r的圆盘, 每次占据一个栅格, 在规划过程作为质点考虑, 且起始位置和目标位置已知。

2) 环境中静态障碍物位置信息已知, 并进行“膨化”处理, 以确保静态障碍物边界为安全区域。

3) 环境中动态障碍物视为圆形, 其直径为单个栅格宽度(即为1)且其速度固定, 但方向不定, 不会和环境中的静态障碍物及边界发生碰撞。

4) 移动机器人能够通过传感器感知有限范围环境中动态障碍物的运行速度和方向。

5) 移动机器人和动态障碍物的可移动方向如图 1所示。

6) 移动机器人可在匀速运动与暂停两种运行状态下任意转换。

2 经典蚁群算法的改进

蚁群算法^[1]提出至今, 国内外学者对其进行了大量的研究, 并已被广泛地应用于解决旅行商问题^[2]、路由问题^[3]、工件排序问题^[4]、车辆运输调度问题^[5]、图着色问题^[6]、机器人路径规划问题^[7-8]。

经典蚁群算法中蚂蚁通过向着信息素浓度较高的路径移动来寻找最优路径, 这种正反馈机制可以加快收敛速度, 但会导致蚁群多样性减小, 全局搜索能力减弱。此外在一些比较复杂的环境下, 蚁群算法可能进入死锁状态。为保证蚁群算法能够收敛到全局最优或近似最优并解决死锁问题, 针对其存在的不足之处, 本文做出以下的改进。

1) 调整转移概率

在经典蚁群算法的转移概率基础上引入随机选择机制以增加解的多样性。操作如下:设置随机选择参数q₀ ∈(0, 1), 及随机数q∈(0, 1)。如果q < q₀, 查看当前节点r周围所有节点的信息, 排除有障碍物节点和已走节点后, 随机选取任意一个节点作为可行节点; 如果q≥q₀, 则需要计算转移概率, 然后根据转移概率值来选择下一个可行节点。新的可行节点选取为:

$ S = \left\{ \begin{array}{l} \rm{rand}(\rm{allowed}_\mathit{k}){\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \mathit{q} < {\mathit{q}_0}\\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} S{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {其他} \end{array} \right. $

(1)

式中, rand(allowed_k)表示可行节点从allowed_k中随机选取; S为可行节点, 由转移概率公式获得。为增强改进蚁群算法全局搜索的能力, 新增了两个启发式因子, 改进后的转移概率为:

$ p_{rs}^k = \left\{ \begin{gathered} \frac{{{{[{\tau _{rs}}(t)]}^\alpha }{{[{\eta _{rs}}(t)]}^\beta }{D_s}^{ - 1}{T_s}^{ - 1}}}{{\sum\limits_{s \in \rm{allowed}_\mathit{k}} {{{[{\tau _{rs}}(t)]}^\alpha }{{[{\eta _{rs}}(t)]}^\beta }D_s^{ -1}T_s^{ -1}} }}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} s \in {\text{allowe}}{{\text{d}}_k} \hfill \\ {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} 0{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\text{ }}{其他} \hfill \\ \end{gathered} \right. $

(2)

式中, D_s表示下一待选节点s到目标节点G的欧式距离; T_s表示节点s累计被访问的次数, 当节点s有蚂蚁经过后, T_s自增1。由式(2)可以得出, 在蚁群算法固有要求基础上, p_rs^k与D_s近似成反比, 由此可使算法的收敛速度加快; 同时, p_rs^k与T_s近似成反比, 利于增加解的多样性, 降低陷入局部最优的可能。

2) 信息素阈值限定

为进一步提高算法的全局搜索能力, 借鉴最大最小蚂蚁系统(MMAS), 在算法每次迭代中, 全局更新最优路径上的信息素时, 对其进行上下界限定。限定公式为:

$ {\tau _{rs}} = \left\{ \begin{array}{l} {\tau _{rs}}\;\;\;\;\;\;\;\;{\tau _{\min }} < {\tau _{rs}} < {\tau _{\max }}{\kern 1pt} \\ {\tau _{\min }}\;\;\;\;\;\;\;{\tau _{rs}} \le {\tau _{\min }}\\ {\tau _{\max }}\;\;\;\;\;\;{\tau _{rs}} > {\tau _{\max }}{\kern 1pt} {\kern 1pt} \end{array} \right. $

(3)

通过将信息素强度限定在[τ_min, τ_max]范围内, 可减小算法早熟并使信息素在各路径节点上的差异不会过大。

3) 死锁问题处理

当环境条件较为复杂时, 移动机器人可能陷入死锁状态。如图 2所示, 机器人运行到R时, 无法向其周围任意位置移动, 即陷入死锁状态。

针对死锁问题, 文献[9]采取“早期死亡”(early death)方法, 该方法的主要思想为令处于死锁状态的蚂蚁死亡, 且不对其已走路径的信息素进行更新。但当有较多蚂蚁陷入死锁状态时, 该方法不利于全局最优解的搜索且会减小解的多样性。对此, 本文的解决方法如下:当蚂蚁陷入死锁时, 允许其回退一步, 不令其死亡, 然后更新禁忌表信息, 并对死锁边上信息素进行惩罚, 在当前路径上蚂蚁重新选择移动节点, 如此即可避免其他蚂蚁陷入死锁。信息素惩罚函数为:

$ {\tau _{rs}} = (1-\lambda ){\tau _{rs}} $

(4)

式中, (1-λ)为相应惩罚系数。本文对于死锁问题的解决方法能够提高算法全局搜索的能力, 并有效地减小蚂蚁在同一位置陷入死锁的可能性。

3 基于改进蚁群算法的路径规划

综上所述, 具体的改进蚁群算法流程如下:

1) 对环境的建模采用栅格法, 并初始化移动机器人起始位置S和目标位置G及相关参数。

2) 令蚂蚁k (k = 1, 2, ..., m)从起始点S出发, 将其添加到相应禁忌表tabu_k, 然后由式(1)和式(2)得到下个可行节点s, 添加至禁忌表。

3) 若蚂蚁k在搜索路径时陷入死锁状态, 则采取死锁问题处理方法。若目标点到达, 则令k=k +1随后转到步骤2), 直到此次迭代过程中每一只蚂蚁的搜索都已完成时结束, 此时k=m。

4) 通过禁忌表tabu_k中记录的路径信息, 计算出每只蚂蚁搜索的路径长度并进行比较, 从而得到最短路径。然后调整最短路径全局信息素, 并进行区间限制。

5) 迭代次数N自增, 若N≤N_max, 将重置蚂蚁禁忌表, 即删除所有数据, 然后跳转步骤2)再次进行迭代计算; 若N>N_max, 则不再进行计算, 此时找到了最优路径, 即满足算法结束条件, 输出最终结果。

对应的流程图如图 3所示。

4 动态环境避碰规划策略 4.1 直线运动障碍物预测避碰

若环境中存在直线运动的障碍物, 模拟滚动窗口, 预测移动机器人与动态障碍物在一个滚动窗口周期T内的运行轨迹, 通过判断它们是否相交来判断碰撞是否发生, 相应的碰撞避免策略如下:

1) 若移动机器人与动态障碍物无碰撞, 则其按原始路径移动一个步长进入下个滚动窗口;

2) 若移动机器人与动态障碍物发生侧面碰撞, 则其在到达碰撞点之前停顿ΔT时间, 随后继续按原始路径行进。ΔT的值为:

$ \Delta T = \max \{ {D_r}/{v_r}, {D_o}/{v_o}\} $

(5)

式中, D_r、D_o分别为机器人和动态障碍物的直径; v_r、v_o为其对应的速度。

3) 若两者发生正面碰撞, 则重新规划一条局部路径。

① 将滚动窗口边界与原始路径的交点所在栅格设为局部子目标。

② 视碰撞处栅格为静态障碍物, 然后由当前位置至局部子目标点, 利用改进蚁群算法获得无碰最优局部路径。

4) 再次根据相应策略进行预测避碰, 此次将移动机器人置于规划好的局部路径中。

但当环境条件较为复杂时, “局部子目标不可达”的现象可能会出现。如图 4所示, 在位置r处, 机器人在当前滚动窗口内探测到动态障碍物的存在, 且将在s点处发生正面碰撞。若根据上述步骤进行规划, 则无法规划出局部路径。

针对以上问题, 本文采用如下方法:

1) 机器人不再规划局部路径, 通过改进蚁群算法将发生碰撞处栅格视为静态障碍物获得一条新的全局路径。

2) 机器人将规划出来的新的路径作为原始路径, 并沿此路径进行局部路径规划。

4.2 运行方向不定障碍物预测避碰

针对动态障碍物运行方向不确定的情况, 文献[10]中的策略(以下称为策略一)是对动态障碍物进行膨化处理后, 视整个膨化区域为静态障碍物, 然后通过判断移动机器人的运行轨迹是否与膨化区域相交来判断碰撞是否存在, 进而进行局部路径规划, 但该做法具有很大的保守性。对此, 本文提出一种新策略(以下称为策略二)来避免移动机器人与动态障碍物发生碰撞, 两种策略的性能比较在仿真实验部分给出。

策略二的主要步骤如下:

设动态障碍物的运动方向改变周期为ΔT(ΔT为其移动单个栅格所需时间)。

1) 若预测到碰撞可能发生, 则将时间T分为μ等份, 即T=μΔT, 对应ΔT时间内移动机器人的步长为Δε。

2) 在ΔT时间内, 若预测到无碰撞发生, 则移动机器人按原路径行进Δε, 然后进入新的滚动窗口, 直到移动机器人完成步长Δε。

3) 如果运行过程中碰撞可能存在, 则对动态障碍物实施ΔT膨化处理, 并且视滚动窗口内膨化区域为静态障碍物, 生成局部子目标, 再进行局部路径规划。

4) 按照步骤3)规划出的局部路径, 移动机器人行进并进行预测避碰。

本文的策略能够减小碰撞可能发生时的动态障碍物的膨化区域, 从而可以提供更好的规划路径, 进而对移动机器人进行有效的指导。两种策略的对比分析在仿真实验部分给出。

4.3 局部滚动预测避碰算法

结合上述碰撞避免策略和滚动窗口原理, 本文提出局部滚动预测避碰算法, 其主要步骤如下:

1) 相关参数初始化, 主要有起始位置S、目标位置G、机器人步长ε以及滚动窗口半径R, 并根据环境先验知识不考虑动态障碍物, 由改进蚁群算法规划全局路径Gpath。

2) 机器人按全局路径以滚动窗口方式行进, 刷新当前所在滚动窗口内的环境信息, 判断其是否到达目标点, 若到达, 则算法终止。

3) 在滚动窗口内进行碰撞检测, 若无碰撞, 机器人按原始路径移动一个步长ε, 转步骤2)。

4) 根据碰撞避免策略, 规划局部路径Lpath, 机器人沿此路径行进, 转步骤2)。

首先由改进蚁群算法规划出一条全局路径, 然后根据实时的局部环境信息, 将预测避碰策略用于滚动窗口内, 进行局部滚动预测避碰。由此避免与所有动态障碍物碰撞, 随滚动窗口逐步推进目标点, 最终获取无碰最优或次优路径。

5 仿真实验 5.1 改进蚁群算法性能比较

本文的实验对比了改进蚁群算法和经典蚁群算法并验证了其对死锁问题的处理。蚁群算法相关参数如表 1所示, 经典蚁群算法无惩罚系数设置, 实验环境为VC++6.0。图 5为两种蚁群算法规划出的全局路径。图 6为运行过程中两种蚁群算法在最短路径长度和收敛速度两方面性能的比较。两种算法分别运行100次后, 各指标的统计结果如表 2所示。

由改进蚁群算法获取的全局无碰最优路径如图 5中实线所示, 而由经典蚁群算法获取的全局无碰最优路径为图 5中的虚线路径。

图 6中数据显示出改进蚁群算法的收敛速度比经典蚁群算法要快, 而且这两种算法所获取的最短路径长度前者优于后者。

由表 2得出, 改进蚁群算法与经典蚁群算法相比, 其最短路径长度、最优解比例以及平均耗时都有一定的提升。在改进蚁群算法条件下, 没有蚂蚁进入死锁状态且所有蚂蚁都获得了最优路径, 这一点可以从最优解比例为100%得出。同时说明改进蚁群算法可以有效避免死锁问题。由以上实验数据得出, 相比于经典蚁群算法, 改进蚁群算法可以加快收敛速度, 时间性能也有所提升, 并且能有效地避免局部最优和死锁问题。

5.2 动态环境下改进蚁群算法的可行性

本文的仿真实验验证了环境空间中存在动态障碍物时, 由本文提出的改进蚁群算法所规划出路径的可行性。

仿真实验中, 环境运行区域被划分为100x80的栅格, 移动机器人起始位置坐标为S(0, 0), 目标位置坐标为G(99, 79), 步长为ε=4, 感知半径为r=10。此外, 环境中存在4个动态障碍物DO₁、DO₂、DO₃和DO₄, 其中DO₁和DO₂做直线运动, DO₃和DO₄运动方向不确定, 且每经过一个栅格后, 移动方向随机变化。动态障碍物移动步长为2。机器人运行环境如图 7所示, 实验环境为VC++6.0。

首先由改进蚁群算法根据静态全局环境的先验信息且不考虑动态障碍物的情况下, 规划出一条全局最优路径, 然后移动机器人按此路径行进并进行预测避碰。规划的全局路径如图 7中虚线所示。

在t₁时刻, 机器人到达位置p₁。在当前滚动窗口c₁内检测到运行方向不确定的动态障碍物DO₄。首先对其进行膨化处理, 再根据“运行方向不定障碍物预测避碰”方法, 得到在下一个步长内将不会发生移动机器人与动态障碍物的碰撞, 此时移动机器人继续按照原路径行进。此过程如图 8a所示, 其中p₁之前的实线为移动机器人已走路径, e₁为DO₄的T膨化区域。

在t₂时刻, 机器人到达位置p₂。在当前滚动窗口c₂内检测到直线运动的动态障碍物DO₁。根据“直线运动障碍物预测避碰”方法, 得出移动机器人与动态障碍物将在位置d₁发生侧碰。机器人通过在p₂位置处停留ΔT时间, 此过程如图 8b所示, 如此则可避免与动态障碍物DO₁碰撞。

在t₃时刻, 机器人到达位置p₃。检测到当前滚动窗口c₃内, 存在动态障碍物DO₂做直线运动。利用“直线运动障碍物预测避碰”方法, 得出移动机器人与动态障碍物将在位置d₂发生正碰。通过规划局部路径避免碰撞, 此过程如图 8c所示。其中Psub₁为局部子目标点, 由p₃到Psub₁的实线为所规划的局部路径。

在t₄时刻, 机器人到达位置p₄。在当前滚动窗口c₄内检测到运行方向不确定的动态障碍物DO₃。首先对其进行膨化处理, 然后根据“运行方向不定障碍物预测避碰”方法, 得出碰撞可能存在。在此情况下, 两种碰撞避免策略的规划过程如图 8d所示。其中, p₄到Psub₂的实线部分为策略一所得路径。策略二所规划的局部路径与原始路径一致, 即DO₃的总体向着远离原始路径方向移动。

由实验结果得, 本文的避碰策略优于文献[10]中的策略。如图 8d所示, 当检测到碰撞有可能发生时, 策略一对动态障碍物进行T膨化处理。此做法保守性很大, 将大部分的可行区域都膨化成为了静态障碍物。另外, 这也常常引起在某些情况下, 存在移动机器人无法规划出可行路径或者规划出来的路径较长的问题。在检测到可能存在碰撞时, 本文的策略二只对动态障碍物进行ΔT膨化处理, 并减小移动机器人的步长, 通过减小相应的膨化区域, 从而进一步检测碰撞是否发生。策略二的做法, 可增加机器人的可行区域, 从而能够更高效地指导其进行路径规划。

在t₅时刻, 机器人到达位置p₅。在当前滚动窗口c₅内检测到有障碍物停留在原始路径上。此时引入Follow_wall行为修正原始路径, 即移动机器人沿障碍物边界行走直至回到预定路径, 如图 8e所示。

图 9中所示路径即为本文所提改进蚁群算法指导移动机器人所获得的无碰最优路径。

6 结论

本文研究了动态复杂环境下, 基于环境先验知识的单移动机器人路径规划问题, 利用改进蚁群算法获取全局路径, 并结合滚动窗口原理进行局部滚动预测避碰, 最终获取无碰最优路径。本文考虑了动态障碍物直线运动和运动方向不确定的情况, 给出了相应的碰撞避免策略, 并针对不能适应环境变化的缺陷, 加入Follow_wall行为。仿真实验证明了本文算法的有效性, 但是针对动态复杂情况下多移动机器人的路径规划问题, 还有待于进一步的学习和研究。