基于双层蚁群算法和轨迹优化的移动机器人导航算法

郝 延 春

(吉林大学电子科学与工程学院 吉林 长春 130012) (吉林铁道职业技术学院质量监控处 吉林 吉林132200)

0 引 言

全局路径规划是机器人技术中的一个重要问题，其主要满足在如何避免与障碍物碰撞并满足约束条件下，找到从起始位置到目标位置的最优路径，目前全局路径规划已经被广泛应用于自动驾驶、民用搜索、紧急救援和资源开发等领域[1]。

传统的全局路径规划方法包括人工势场法、细胞分解法以及计算智能算法，其中计算智能算法又包括神经网络、模糊逻辑技术、遗传算法(GA)、粒子群优化(PSO)和蚁群优化(ACO)算法等[2]。对于路径规划算法而言不仅需要其拥有良好的稳定性，而且要具有较强的全局搜索能力[3]。然而，在一些计算智能算法中如PSO和GA，其初始值均随机获得，而这种随机性在很大程度上影响最终的路径优化结果。

ACO算法是一种高效、稳健的优化技术，在不同的领域得到了广泛的应用[4]。近年来，国内外众多专家学者致力于利用蚁群算法解决全局路径规划问题。Duan等[5]提出了一种基于蚁群算法和差分进化的无人机三维路径规划方法，并引入模糊成本函数实现动态环境下无人机路径规划。韩颜等[6]提出一种粒子群算法和蚁群算法改进求解路径规划问题的融合算法，有效提升了移动机器人在全局静态环境下搜寻到达指定目标点的最优路径的能力。李理等[7]针对传统蚁群算法易陷入局部最优提出基于路径长度、转弯次数和坡度平滑性三种因素共同影响的改进启发函数，并根据三因素综合指标分配各路径上的信息素量，有效实现全局路径规划。Chen等[8]介绍了一种两阶段蚁群算法，并将启发式搜索引入到预处理阶段和路径规划阶段，以避免算法陷入局部最小值。Abdulkader等[9]提出了一种混合的ACO算法，该算法将局部搜索与现有的ACO算法相结合，从而在出现较大问题时减少了计算时间，提高了性能。

虽然蚁群算法具有良好的搜索特性，但存在收敛速度慢、收敛过早等缺点。此外，蚂蚁在单次运动中所能行走的最大距离是有限的，这会导致路径的扭曲和转弯，并影响路径的长度和平滑度[10]。鉴于此，本文将路径规划问题分为路径生成和轨迹优化两部分，提出了一种双层蚁群优化算法(DL-ACO)。首先，提出了并行蚁群优化算法用于产生初始无碰撞路径。其次，根据初始路径曲折程度，提出了转折点优化算法(TPOA)，以对初始路径进行进一步的改进。最后针对拐角处路径设计了分段B样条平滑器，使路径中不连续部分更为平滑，从而有效提高路径优化质量。

1 环境模型构建

本文构建了基于网格的二维(2-D)场作为移动机器人运行区域。如图1所示，其中分布了多个静态障碍物，因此网格被划分为自由空间网格Tf和障碍网格To，而障碍物四周的网格被定义为高风险网格Th。移动机器人被视为质点R，并以固定速度移动，出于安全考虑机器人应与障碍物保持安全距离，但若通过狭窄通道时可进入高风险网格Th区域。

图1 网格二维场示意图

基于网格的路径规划问题定义如下：

给定一个起始网格S和一个目标网格E，找到一组网格π(t)：[0，t]→Tf，使得π(0)=S且π(t)=E，路径用π(t)中的网格表示，这些网格从S到E依次连接，并且连接的网格不能与任何To碰撞。为了获取最佳路径，需要从路径长度、平滑度和安全性等方面对路径进行优化，因此本文的全局路径规划问题实质上为多目标优化问题。

2 算法设计

2.1 基本蚁群算法

蚁群算法模拟了蚂蚁“探索”的行为特征，即蚂蚁将使用前蚂蚁在觅食过程中留下的信息素，根据一定的概率选择路径或探索新的路径并同时分泌信息素。假设蚂蚁k在t时刻位于节点i，则路径转换的概率可由式(1)表示，然后利用轮盘赌模型选择下一个节点。

(1)

式中：C为蚂蚁允许在下一步选择的节点集；τij为路径(i，j)的信息素值；α为信息素激励因子，它反映了信息素浓度对路径选择的重要性，α越大则信息素对于路径选择越重要，蚂蚁将更倾向于选择大多数蚂蚁采取的路径；β是预期的启发因子，它反映了启发式信息在路径上的重要性。ηij(t)为启发式值，表示蚂蚁从节点j转移到目标节点g的期望程度，其计算式为：

(2)

式中：djg是节点j和目标节点g之间的欧几里得距离。每个蚂蚁在使用路径上的信息素的同时也会不断分泌信息素，因此路径上的信息素将不断积累。为了防止过多的信息素导致启发信息泛滥，在每个蚂蚁采取一步或完成一个周期之后更新路径上的信息素，并且从t+1时刻获得路径上的信息素，其计算式为：

τij(t+1)=(1-ρ)τij(t)+ρΔτij(t)

(3)

式中：ρ为信息素挥发系数，主要模拟蚂蚁信息素随时间的自然挥发；1-ρ为信息素残留因子；Δτij(t)为循环路径(i，j)上信息素的增量。当信息素更新策略时，有不同的计算方法。蚂蚁循环模型计算式表示为：

(4)

式中：Q为信息素的强度；Lk为蚂蚁在此循环中所采用路径的总长度。

(5)

2.2 并行蚁群优化算法

为了在复杂环境中生成初始的无碰撞路径，本文在基本蚁群基础上提出了一种改进的并行蚁群优化算法。与传统的ACO不同，并行蚁群算法借助3-opt邻域搜索算法将所需求解的任务划分为若干个并行的子任务，并分配给多个独立的蚁群，每个蚁群之间独立迭代同时进行信息互换，从而对全局信息进行更新，直到算法获得最优解。

从算法原理上来看，3-opt邻域搜索算法是一种局部搜索算法，该算法执行效率高，其基本过程为：假定q为初始回路，算法随机产生3个位置q1、q3和q5，则下一节点可表示为q2、q4和q6，此时对应的三条边可表示为{(q1,q2)，(q3,q4)，(q5,q6)}，算法在运行过程中通过将这三条边断开，然后尝试寻找另外三条边的集合，从而使形成的新的回路路径更短，如图2所示。

图2 3-opt局部搜索

将3-opt应用到ACO算法中，不仅能够有效提高ACO算法的运行效率，同时能够有效地避免算法进入局部最优。改进后的ACO算法步骤如下所示：

步骤1集群环境初始化。主节点计算路径i、j之间的欧几里得距离，由式(3)计算i、j之间的信息素初始值，并将其广播到从计算节点上。

步骤2将m只蚂蚁随机地分配到集群的g个计算节点上，从而形成g个并行蚁群。

步骤3对算法进行迭代。g个并行的蚁群各自进行迭代寻找最优解，并进行局部信息素更新。

步骤4利用3-opt邻域搜索算法对g个并行蚁群所获得的最优解进行二次寻优。

步骤5每个并行蚁群在完成各自迭代之后，按照式(3)和式(4)完成各自的全局信息素更新。

步骤6判断迭代条件是否满足，若满足进入步骤7，否则返回到步骤3继续迭代。

步骤7各并行蚁群进行蚁群间信息的交互，并判断当前算法是否收敛，若收敛则输出最优结果，否则转入步骤2。

3 轨迹优化

3.1 转折点优化算法(TPOA)

图3所示为原始无碰撞路径和消除了不必要转折点后的路径，其中S→I1→A→B→I2→E是从起点到目标的初始路径，显然该路径并非最佳路径。例如S上的蚂蚁可以不经过I1就直接到达A，而B也可以不经过I2直接到达E。如果删除I1和I2，则路径S→A→B→E的长度将大幅缩短，并且步数较少。其中点I1和I2被定义为不必要的转折点。

图3 TPOA路径

对于具有许多转弯的复杂路线，消除这些不必要的转弯点可以有效地改善路径长度和平滑度。因此，本文提出的TPOA如下：

对于初始路径，节点集包括起点、目标和所有转折点，表示为：

T={S,T1,T2，…,Tn,E}

(6)

考虑到路线的方向，只允许蚂蚁在单个方向上行驶。因此当一只蚂蚁到达节点Ti时，节点的集合表示为：

T(i)={Ti+1,Ti+2,…,Tn,E}

(7)

如果节点Ta和Tb之间的直线不穿过任何障碍网格To，则认为Ta上的蚂蚁会直接到达Tb，即：

(8)

式中：Ta和Tb为路径上的任意两个节点。此时节点Ti上蚂蚁的可到达节点Tr(i)的集合可定义为：

Tr(i)={T|T∈T(i)∩con(Ti,T)=1}

(9)

此时，蚂蚁从节点Ta走到节点Tb的概率为：

(10)

式中：τab为信息素浓度，ηab为启发式信息，且:

ηab=dab

(11)

式中：dab为Ta和Tb之间的距离。此时信息素更新计算式表示为：

(12)

式中：n为转折点的数量；Lk为路径的长度；sfk为蚂蚁经过的高风险区域网格的数量；smk为路径平滑度。

(13)

式中：

θi+1=atan[(yi+1-yi)/(xi+1-xi)]

θi=atan[(yi-yi-1)/(xi-xi-1)]

(14)

图4所示为TPOA实现的伪代码，首先输入节点T的坐标和路径上的节点数n，进行参数进行初始化处理，并计算函数con(Ta,Tb)。然后依次将所有的蚂蚁放入集合S中，当蚂蚁k在节点Ti上是选择可达到的节点Tr(i)的集合。最后当所有蚂蚁均到达E时，此时对信息素进行更新，当算法达到最大迭代次数或满足终止条件时，输出最佳的路径。

图4 TPOA伪代码

3.2 分段B样条路径平滑器

B样条曲线是折线平滑度中最常用的曲线，但是在某些情况下(例如较大的转向角)，由B样条曲线平滑的路径与原始路径不太吻合，从而增加碰撞风险。因此本文设计了一种分段B样条路径平滑器，以实现更高的拟合度。传统的B样条曲线由n+1个控制点Bi和一个节点向量u组成，其参数方程表示为：

(15)

式中：Bi(i=0,1,…，n)为控制顶点，Nik(u)为k次规范B样条基函数，基函数由节点u的序列决定，u：u0≤u1≤…≤un+k+1。

B样条曲线的基函数为：

(16)

B样条拟合曲线为:

(17)

与传统B样条不同，分段B样条分别平滑了每个角附近的路径。如图5所示，Ti-1→Ti→Ti+1为原始路径，Ti为转折点，在Ti-1、Ti之间增加P1、P2两点，在Ti、Ti+1之间增加P3、P4两点，其位置可由下式计算得到：

(18)

式中：Xsafe为安全距离。在运行过程中机器人通过配置的红外测距传感器测量小车与障碍物的安全距离后，在每个拐角处计算P1、P2、P3和P4的位置，这样可以使小车在平滑拐角的过程中避免与障碍物碰撞。

图5 拐角点分段B样条曲线处理结果

图6显示了通过常规B样条和分段B样条平滑的相同路径。显然，通过分段B样条曲线平滑的路径比通过常规B样条曲线平滑的路径优化效果更好。分段B样条的总体优势主要包括：

(1) 平滑后的路径与原始路径相切，即机器人在跟踪路径期间不必转弯。

(2) 仅拐角处的两条线会对曲线产生影响，可以更改任何其他路径而无须变换平滑路径。

(3) 当转向角较大时，原始路径和平滑路径之间的拟合度仍然很高。

图6 拐角点曲线平滑

4 实验与结果分析

为了验证本文提出算法的可行性和有效性，在不同的地图下与文献[11]的APACA和文献[12]的MO-FA进行对比仿真实验，并将本文算法应用到真实环境的机器人导航中实现路径规划。

4.1 仿真实验与分析

为了验证本文的算法在不同环境中的适应性，选择了8幅50 cm×50 cm地图进行仿真，其中每幅地图的障碍物数量、障碍物形状以及道路宽度均不同。随机设置起始点和终点坐标，如表1所示。并行蚁群优化算法的参数设置如下：n=100、m=20、α=1、β=3、ρ=0.03，TPOA的参数设置为：m=10、α=0.3、Nmax=100、β=0.8、ρ=0.1、Nmax=100、Xsafe=1。

表1 仿真环境描述

图7所示为三种算法生成的最佳路径，可以看出本文所提出的算法产生的路径具有较少的步数和较短的路径长度。从图7(c)和图7(e)可以看出APACA容易陷入“U”型陷阱，且在复杂环境中APACA所规划的路径质量较差，这是因为启发式信息使蚂蚁直奔终点，并且蚂蚁直到遇到障碍物才改变方向。相较而言，MO-FA可以提供更好的路径规划结果，但是由于粒子是随机放置的，拐角处的路径通常会有些曲折，这会影响路径的质量。相比之下DL-ACO针对拐角点规划的路径更为平滑，由此也可以看出分段B样条平滑器的巨大作用。从算法稳定性上来看，MO-FA仅可以在简单的环境中保持稳定(参见图7(b)、(e)和(g))，而APACA的稳定性很差，由此也可以看出在复杂地图中本文提出的方法优势更加明显。

(a) (b) (c) (d)

(e) (f) (g) (h)图7 三种算法生成的最佳路径

表2和图8所示为本文算法与其他两种算法特征(包括长度，平滑度和Rateg)之间的定性比较。其中，长度由式(5)计算，路径的平滑度为路径上所有内部点的线段之间的角度之和，由式(13)计算。Rateg为算法稳定性指标，即算法全局收敛次数与算法总运行次数的比值，该指标可反映算法的全局收敛性，实验中针对各地图每种算法分别运行了20次，以检验各算法的收敛性。

表2 三种算法仿真结果

续表2

(a) 路径长度对比

(b) 平滑度对比

(c) 算法稳定性对比图8 三种算法性能比较

总之，尽管APACA可以生成可行的路径，但其避免陷阱的能力很差，这影响了其解决方案的质量和稳定性。虽然MO-FA可以在一段时间内提供类似的解决方案，但其在复杂地图中的稳定性也会迅速下降。而本文提出的算法可以在长度、平滑度和稳定性方面始终如一地产生更好和更稳健的路线。

4.2 真实环境下的路径规划

为了验证算法的实际可行性，利用机器人模型在真实环境下进行实验。如图9所示，本文采用的机器人由树莓派驱动，并使用激光雷达检测障碍物，分别选取一个室内和六个室外环境作为实验区域。图10描述了路径生成的过程，首先从真实地图中提取自由空间区域，然后将真实地图建模为基于2D网格的地图，并输入到树莓派中。生成路线后，运动指令将被传输到机器人，并使其沿路线移动直至到达目标。图10中，(a)为原始地图；(b)为提取可用空间；(c)为基于网格的地图；(d)为生成的路径。

图9 树莓派机器人

(a) (b)

(c) (d)图10 地图建模和路径生成的插图

图11所示为室内实验结果，实验选取10 m×10 m的室内场地作为实验场所，并将其转换为二维网格模型，显示了机器人运行画面和相应的轨迹。实验中机器人成功地避开了障碍物并保持一定距离，此外机器人在整个导航过程中都要经过五个高风险网格，这表明本文的算法可以有效地权衡路径长度和安全性。

图11 室内实验结果

室外环境中的轨迹如图12所示，机器人在每个地图中运行五次，并且轨迹几乎相同，拐角处的路径也很顺畅，由此也可以证明本文算法在实际应用中的有效性。此外，在实验中还发现算法的运行时间与网格图的分辨率直接相关，例如在10 m×10 m室内地图下，运行时间小于0.5 s，当将室外地图建模为20 m×25 m时，运行时间增加到3 s。显然分辨率越高运行时间越长，但是高分辨率又可以提高路径规划的准确性，因此在实际应用中，需要在运行时间和路径质量之间保持平衡。

图12 室外环境中的轨迹

5 结 语

路径规划是当前机器人在复杂环境下行进的关键技术，蚁群算法作为一种仿生算法能够有效实现机器人的路径规划。针对基本蚁群算法在路径规划中存在的收敛速度慢、易陷入局部最优的问题提出了一种双层蚁群优化算法，并采用转折点优化算法和分段B样条实现轨迹优化。实验中将改进的蚁群算法应用于机器人路径规划，通过模拟多个移动环境，并与其他算法进行比较，实验结果表明改进的蚁群算法可以生成长度较短、更平滑和稳定性较好的路径。