贝塞尔曲线融合双向蚁群算法的移动机器人路径规划

李二超，齐款款

(兰州理工大学 电气工程与信息工程学院, 甘肃 兰州 730050)

随着科技迅速发展，人类生活日愈趋向自动化，智能移动机器人已应用于众多领域，如移动仓储机器人、服务移动机器人等，其中移动机器人路径规划是研究热点之一。静态环境移动机器人路径规划是指在已知环境中根据目标函数(如距离最短等)，寻找一条从起点到终点的无障碍的最优或次优路径[1]。

移动机器人路径规划算法有人工势场法[2]、A*算法[3]、蚁群算法[4]和遗传算法[5-6]等。人工势场法适用于动态的局部路径规划，在静态环境下易陷入极小值点，找不到最优路径；A*算法遍历节点过多，占用内存大，得到的路径拐点较多，得到的路径并非最优；遗传算法需要复杂的建模，路径搜索效率低，占用很大的计算量和存储空间；蚁群算法具有鲁棒性较强，易与其他算法融合的优点[7-8]，但该算法存在收敛速度慢、盲目性大、路径无法做到最短的缺陷，学者们提出各自的改进方法。王晓燕等利用人工势场法求得的初始路径信息对蚁群算法的启发函数进行改进，增强路径的引导作用，减小路径搜索的盲目性[9]。陈志等在全局信息素更新方面，借鉴最大-最小蚁群系统和精英蚁群算法的思想，对最差路径进行惩罚，减少最差路径对路径搜索带来的干扰，增强最优路径的指导作用，提高收敛速度[10]。王娇娇等采用基于中心点的平滑方法平滑尖峰的路径，但此方法在平滑尖峰的同时增加了路径的拐点，路径依然不够安全平滑[11]。王红君等采用冗余点的删除策略减少路径上转折点的数目，路径相对平滑[12]。文献[13]采用贝塞尔曲线优化路径，最大限度地平滑路径，但出现了路径碰撞。赵开新等引入遗传算法的路径交叉策略得到更短路径[14]。文献[15]对初始信息素建立数学模型，信息素的非均匀分布使得蚂蚁能够在初始路径搜索时，更倾向于选择距离起点和终点连线较近的栅格作为下一节点，提高了算法的收敛速度，减少了路径搜索的盲目性。Luo等使用伪随机概率转移公式提高算法的全局搜索能力和收敛速度[16]。

基于以上研究，针对传统蚁群算法无法找到最短路径、路径搜索盲目性大、拐点多问题，本文提出改进贝塞尔曲线与双向蚁群算法融合的机器人路径规划算法。首先，添加双向蚂蚁的路径搜索策略，能够找到最优解。其次，采用路径交叉策略产生新解，能够获得更优解。然后，改进了启发式函数，使路径搜索更具有目的性，采用只更新有效路径上信息素的全局信息素更新方式，避免无效路径的干扰，为防止信息素积累过多，自适应调节信息素挥发系数，设置信息素的取值范围。最后，应用改进的贝塞尔曲线优化路径，能够根据机器人需求调节平滑程度。

1 环境建模

本文机器人的工作环境为栅格地图，即利用栅格法来建立二维静态环境。栅格法由栅格取值为二进制0和1的矩阵构成，矩阵中0 代表自由栅格，机器人可自由移动，用白色栅格表示；1 代表障碍栅格，机器人需要绕行前进，用黑色栅格表示。

本文将栅格地图进行了划分，划分为Nx×Ny个小正方形栅格，地图按照从左到右、从下到上的顺序依次编号1、2、…，栅格序号与坐标一一对应，如图1所示，坐标与栅格编号的关系表达式如式(1)，求得的结果为栅格的中心点。

图1 栅格地图

(1)

式中:i代表栅格序号;Nx为栅格地图的行数;Ny为栅格地图的列数。

为防止机器人与障碍物发生碰撞，根据机器人的半径将障碍物进行膨化处理，处理方式如图2所示。当不规则障碍物不满一个栅格时，将其填充为一个栅格，灰色部分为障碍物的膨化，其边长为机器人半径。整体构成了障碍物，此时把机器人看作质点来处理。假设膨化后的正方形边长为1，即式(1)中a=1。

图2 障碍物膨化处理图

机器人运动方向为当前位置中心与其相邻8个栅格中心连线的方向。为避免机器人走回头路，去掉前一步走过的方向，只有7个方向可以选择，具体情况如图3所示。

图3 机器人运动方式

本文研究的目标函数为最短路径

(2)

式中：{O1,O2,…,ON+1}为组成路径经过的栅格节点集合，且O1=S(起点)，ON+1=E(终点)；L为路径长度。

2 传统蚁群算法

2.1 路径选择概率

状态转移概率

(3)

式中：j∈Gk表示蚂蚁k下一步可到达的相邻栅格集合；τij(t)表示在t时刻蚂蚁从当前节点i到下一节点j之间的信息素浓度；ηij(t)表示在t时刻蚂蚁从当前节点i到下一节点j之间的启发信息；α和β分别表示信息素因子和启发式因子。启发式函数的表达式为

(4)

式中：dij为当前节点i到下一节点j的欧式距离。

2.2 信息素更新

全局信息素更新公式如下：

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

(5)

(6)

(7)

式中：τij(t+1)表示信息素更新后路径(i,j)的信息素浓度；τij(t)表示当前路径(i,j)的信息素浓度；ρ为信息素挥发系数；Δτij(t)表示所有蚂蚁在路径(i,j)留下的信息素浓度之和；Q为信息素强度；Lk为蚂蚁k所走的路径长度。

3 改进蚁群算法

3.1 双向搜索策略

所谓双向蚁群搜索方法就是将所有蚂蚁平均分成两组，一组从起始点向目标点进行搜索(正向搜索)，一组从目标点向起始点进行搜索(反向蚂蚁)，路径搜索时轮流交替从两个方向进行搜索，即先由正向的一只蚂蚁进行路径搜索，到达目标点或者发生“死锁”，再由反向的一只蚂蚁进行路径搜索，与前一只蚂蚁相遇(包括起点和目标点，在这两点相遇，可理解为正向或反向蚂蚁搜索路径成功，是相遇情况中的特殊情况)或者发生“死锁”，再由正向的一只蚂蚁进行路径搜索，如此反复。为了区别正向蚂蚁和反向蚂蚁路径搜索，用A0表示蚂蚁从起点出发进行路径搜索，用A1表示蚂蚁从目标点出发进行路径搜索。

3.2 路径交叉策略

双向蚁群算法能够快速搜索出一条有效路径，在此基础上，当有2条有效路径存在路径重叠部分，即它们有除了起点和目标点之外的交叉点，满足上述条件可进行路径交叉。若存在多个交叉点，依次进行判断此路径段是否更短，交叉完成后，整条路径是否最短，若不是，继续保留之前的最短路径，若是，将其更新为当前最优路径。若存在一个交叉点，方法同上，路径交叉前后示意图如图4和图5所示。

交叉操作如下：图4实线线段{S,7,12,17,18,19,E}为有效路径1，虚线线段{S,2,7,12,18,24,E}为有效路径2，两条路径都经过的交叉点为{7,12,18},从第一个交叉点7进行交叉操作并判断，很显然实线部分长度较短，暂时保留这一部分；交叉点7和12长度相同，任选其一；交叉点12和18之间，虚线部分较短，暂时选择此虚线部分；交叉点18到终点之间长度相同，任选其一，整个交叉完成后，计算出交叉后路径长度，得到的路径如图5的实线线段，得到的更短路径为{S,7,12,18,24,E}。

图4 路径交叉前

3.3 改进转移概率公式

由于在传统蚁群算法中引入了双向策略，每个方向路径搜索都有自己的转移概率公式,两个方向转移概率公式最大的不同点在于启发函数不同。传统的启发函数只与当前点和下一节点之间的距离有关，启发信息引导性较弱；本文采用的启发函数能够更好地引导蚂蚁向目标点前进。

1) 正向蚂蚁A0路径搜索的启发函数为

(8)

式中djE表示正向搜索时下一节点与目标点之间的欧氏距离。

2)反向蚂蚁A1路径搜索的启发函数为

(9)

式中dbS表示反向搜索时下一节点与起点之间的欧氏距离。

根据式(8)和式(9)，两个方向的转移概率公式如下：

正向蚂蚁A0路径搜索时，

(10)

反向蚂蚁A1路径搜索时，

(11)

3.4 改进挥发系数

由于蚁群算法在不同阶段需要不同大小的挥发系数，因此有着固定值的传统挥发系数是行不通的，需要动态调整其值，由此本文引入动态调整挥发系数[13]

(12)

式中：ρmin为挥发系数的最小值；c、d为系数。

3.5 改进信息素更新方式

由于本文算法设计的特殊性，只采用全局信息素更新方式且只更新有效路径，如式(13)～(15)所示。

(13)

(14)

(15)

3.6 信息素限制策略

为防止算法陷入局部收敛，对信息素浓度进行限制。在信息素挥发阶段，需要施加最小范围的控制；在信息素更新阶段，需要施加最大范围的控制。具体公式如下：

(16)

3.7 路径平滑策略

传统蚁群算法在栅格环境下规划的路径转折较多，机器人会消耗大量的能量，路径并非最优。因此，有必要进行路径平滑，使路径更符合机器人实际行走的环境。本文采用文献[13]的贝塞尔曲线，并加入了自己的改进思想。

n阶贝塞尔曲线公式如下：

(17)

式中：P(u)为贝塞尔曲线的运动控制点；P(i)为位置点，P(0)和P(1)分别为起点和终点。

i=0,1,2,…,n。

(18)

本文改进思想如图6和图7所示。贝塞尔曲线是由无数个点组成的曲线，本文将曲线分成若干个点，根据机器人对路径平滑精度的不同要求，可以合理选择点的数目，点的数目越多，路径平滑越接近曲线。极端情况下，可以将曲线变成折线段，曲线分段思想如图6所示。另一方面的改进是插入控制点，插入控制点的位置在路径转折处的线段上。插入的控制点越多，控制点越逼近拐点处，平滑的长度就会减小，当达到一定的数目时，平滑路径与原路径重合。这一改进可以保证路径最短并且安全性较高，插入控制点方法如图7所示。图6和图7分别是同一栅格的放大，折线线段为平滑前的路径，曲线为平滑后的路径。

图6 曲线分段思想示意图

图7 插入控制点示意图

4 改进蚁群算法流程

改进后的蚁群算法流程图如图8所示。

图8 改进蚁群算法流程图

5 实验仿真与分析

为了验证本文算法的可行性、有效性和优越性，在MATLAB 2016a上进行仿真实验。本文从以下几个方面进行实验验证：1)在文献[13]的环境下进行对比分析;2)在不同的栅格地图尺寸和不同复杂程度的环境下，将本文改进蚁群算法与传统蚁群算法和文献[13]进行对比分析，验证本文算法的优点;3)应用贝塞尔曲线平滑处理本文算法最优路径。

仿真参数设置分别为：蚂蚁数目为50；最大迭代次数为50；挥发因子的系数c=0.8,d=25；α=1；β=10；Q=150。以下结果都是算法运行50次的平均值。

5.1 文献[13]的15×15运行环境

传统蚁群算法、文献[13]和本文算法的最优路径图与收敛曲线图以及平滑前后的对比图，分别如图9～18所示。仿真结果见表1。

图9 传统蚁群算法的最优路径图

图10 传统蚁群算法的收敛曲线图

图11 文献[13]的最优路径图

图12 文献[13]的收敛曲线图

图13 本文算法的最优路径图

图14 本文算法的收敛曲线图

图15 传统蚁群算法的平滑前后对比图

图16 文献[13]平滑前后对比图

图17 本文算法平滑前后对比图

表1 算法仿真结果

3种算法都能找到各自的最优路径，但传统蚁群算法找到的不是最短路径，并且拐点很多，从而增加了路径长度。本文算法得到的最短路径、拐点数目和路径搜索的目的性都优于传统算法。和文献[13]算法相比，本文算法得到的最短路径一样，拐点的稳定性(标准差)仅次于文献[13],但本文算法的迭代次数优于文献[13]。值得注意的是，文献[13]所使用的平滑方法使得路径与障碍物发生碰撞(如图16和图18所示)。为此，本文对此碰撞问题进行了改进，如前文路径平滑策略所述，最终得到的路径更适合机器人运行。

图18 图16的拐点放大图

5.2 20×20复杂环境

为进一步验证本文算法的可行性、有效性和优越性，在其他不同尺度的地图和复杂程度不同环境中，将本文算法与传统算法、文献[13]进行性能比较。因为文献[13]平滑方法容易与路径发生碰撞，为方便比较平滑后路径，对文献[13]采用本文的路径平滑方法。传统算法和文献[13]与本文算法的最优路径图和收敛曲线图以及平滑前后的对比图，分别如图19～27所示。仿真结果见表2。

表2 3种算法的仿真结果

图19 传统算法的最优路径图

图20 传统算法的收敛曲线图

图21 文献[13]算法的最优路径图

图22 文献[13]算法的收敛曲线图

图23 本文算法的最优路径图

图24 本文算法的收敛曲线图

图25 传统蚁群算法的平滑前后对比图

图26 文献[13]平滑前后对比图

图27 本文算法平滑前后对比图

显而易见，3种算法都能找到各自的最优路径，但传统蚁群算法找到的不是最短路径，并且在复杂的环境中拐点更多，从而增加了路径长度，路径搜索时盲目性太大。本文算法在数值稳定性(平均值和标准差都最小)方面优于2种对比算法，得到的最短路径、拐点数目和路径搜索的目的性都优于传统算法；和文献[13]算法相比，本文算法得到的最短路径更短。本文算法在某些方面仍有改进空间，后续会继续进行研究。

6 结论

在静态的全局路径规划中，针对传统蚁群算法无法找到最短路径、拐点多和盲目性大等缺陷，本文提出了一种基于贝塞尔曲线融合双向蚁群算法。在传统蚁群算法的基础上，引入了双向的路径搜索方式，能够找到最优解；双向策略与路径交叉策略的结合能够优化最优解；把目标点的信息加入启发式函数中，增加路径搜索的目的性，间接改进了概率转移公式，使得蚂蚁更倾向于距离目标点近的下一节点转移；采用有效路径的信息素更新方式，添加动态调整挥发系数公式。对贝塞尔曲线进行了相应改进，增加控制点使曲线优化的路径更加安全，也对曲线进行了分割，以满足对路径平滑度不同的需求。同时，通过实验验证了本文算法的可行性、有效性和优越性。