B样条曲线融合蚁群算法的机器人路径规划

李二超，齐款款

（兰州理工大学电气工程与信息工程学院，兰州 730050）

（∗通信作者电子邮箱lecstarr@163.com）

0 引言

静态环境下的移动机器人路径规划是指在环境已知条件下，移动机器人根据目标函数（如距离最短等），按照已知算法寻找一条从起点到终点的安全且最优路径。移动机器人路径规划算法有很多，如人工势场法［1］、A*算法［2］、蚁群算法［3］、遗传算法［4-5］等。其中，蚁群算法是一种启发式的随机搜索算法，鲁棒性强，具有优良的并行分布式计算能力和易于与其他算法融合的优点［6-7］，但无法找到最短路径，存在收敛速度慢、路径搜索盲目性大、路径拐点较多、路径不平滑等不足。针对这些不足：孟冠军等［8］利用A*算法搜索速度快的特点计算得到蚁群算法的初始路径，实现了初始信息素的非均匀分布，减少了路径搜索的盲目性；曹新亮等［9］对初始信息素建立数学模型，实现了信息素的非均匀分布，使蚂蚁能够在初始路径搜索时更倾向于选择距离起点和终点连线较近的栅格作为下一节点，以提高算法的收敛速度，减少路径搜索的盲目性；王红君等［10］使用冗余点的删除策略减少了路径上的拐点数目；Luo等［11］使用伪随机概率转移公式提高了算法的全局搜索能力和收敛速度；李志锟等［12］采用多步长路径搜索策略，实现了拐点数目少且路径最短；胡浍冕等［13］采用双向蚁群算法路径搜索策略加快了算法运行速度，提高了全局搜索能力；张军明等［14］采用自适应调整挥发系数来加强较优路径信息素并减弱较差路径信息素的方法，加快了算法的收敛；封声飞等［15］使用分段三阶贝塞尔曲线优化最优路径，能够得到更短路径且拐点处的路径平滑性较好。

基于前人的研究，本文主要解决传统蚁群算法无法找到最短路径、路径搜索盲目性大、收敛速度慢和路径不平滑问题，提出B 样条曲线融合蚁群算法。与折线优化路径相比，B样条曲线优化的路径较为光滑，而与其他曲线如贝塞尔曲线优化路径相比，B 样条曲线除具有贝塞尔曲线的优点外，还能够克服其缺乏局部性质的缺点。本文算法首先对路径初始信息素进行非均匀分布，在起点和终点连线附近设置最大浓度的信息素，且距离该线段越远，信息素浓度越低；其次，在启发式函数中加入当前节点、下一节点和目标点的信息并加入动态调节因子，实现前期路径搜索主要依靠启发信息，后期削弱，使路径搜索更具有目的性，能够朝着路径最短方向上移动，同时结合伪随机（确定性概率、随机性概率和任一性概率）状态转移策略，降低传统算法路径搜索的盲目性，加快收敛，减少转折点数目；然后，为防止信息素积累过多，在自适应调节信息素挥发系数的同时，设置信息素浓度的取值范围；最后，使用B样条曲线对得到的最优路径进行平滑处理，以进一步使拐点处路径更短、更光滑。

1 环境建模

本文机器人工作环境为栅格地图，具体如图1所示。

图1 栅格地图Fig.1 Grid map

栅格法由栅格取值为二进制的0 和1 矩阵构成：0 代表自由栅格（白色栅格），机器人可自由移动；1 代表障碍栅格（黑色栅格），机器人需要绕行前进。地图按照从左到右、从下到上的顺序依次编号1、2、…，栅格序号与坐标一一对应，坐标与栅格编号的关系表达式如式（1），求得的结果为栅格的中心点。

其中：i代表栅格序号；Nx和Ny分别代表栅格地图的行数与列数；a表示一个单位的栅格边长；mod（）是求余运算，ceil（）是向上取整运算。

为防止机器人与障碍物发生碰撞，当不规则障碍物不满一个栅格时，将其填充为一个栅格，再将障碍物向外膨化，宽度为机器人的半径，整体构成障碍物，此时把机器人看作质点来处理，假设膨化后的正方形边长为1，即式（1）中的a=1。

机器人运动方向为8 个，去掉前一步走过的，只有7 个方向可以选择。

2 传统蚁群算法

2.1 路径选择概率

状态转移概率公式如式（2）：

其中：j∈allowedm表示蚂蚁m下一步可到达的相邻栅格集合；τij(t)表示信息素浓度；ηij(t)表示启发信息；α和β分别表示信息素因子和启发式因子，启发式函数如式（3）所示。

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

2.2 信息素更新

信息素更新公式如式（4）～（6）所示：

其中：τij(t+1)表示信息素更新后的信息素浓度；ρ为挥发系数；Δτij(t)表示信息素浓度增量；Q为信息素强度；Lm为蚂蚁m所走的路径长度。

3 改进蚁群算法

3.1 初始信息素不平等分布

传统算法的初始信息素浓度相等，选择路径的概率差异不大，路径选择的盲目性很大，本文将初始信息素进行差异化处理，在起点和终点连线附近信息素浓度最大，距离该线段越远，信息素浓度越低，在路径搜索时，使得蚂蚁更倾向于选择该线段附近节点作为待选节点，得到的路径更接近最优解。初始信息素分布公式如式（7）所示：

其中：dSi表示起点与当前点的欧氏距离，djE表示下一节点与目标点的欧氏距离；C为信息素浓度常数；τ0为初始信息素。

3.2 改进启发函数

本文的启发函数是在文献［16］中启发函数的基础上加入了动态调节因子（由最大迭代次数itermax和当前迭代次数iter构成）。在迭代前期，该调节因子促使启发函数起主导作用；随着迭代次数的增加，路径上积累了一定量的信息素，此时调节因子会减弱启发信息的引导作用，加强信息素的引导作用。

3.3 伪随机状态转移策略

转移概率公式如式（9）所示：

本文算法引入伪随机状态转移策略，确定性概率用来减少路径搜索的随机性，同时也不能过于偏向于确定性概率而导致陷入局部最优值，因此引入任一性概率，如式（10）所示：

式中：rand为［0，1］区间的随机数；q0、q1、q2由前人经验以及反复实验来确定的常数，范围为（0，1）；randj为任意选择下一可行节点。

3.4 改进挥发系数

由于蚁群算法的特殊性，在不同阶段需要不同大小的挥发系数：如果ρ设置过大，蚂蚁无法依靠信息素信息进行路径搜索，导致收敛慢；如果ρ设置过小，信息素过度积累，则会使路径搜索陷入局部最优。固定值的挥发系数无法动态调整，因此引入动态调整挥发系数如式（11）所示；同时，为防止算法陷入局部收敛，对信息素浓度进行限制，如式（12）所示。

式中：ρmin为挥发系数的最小值。

3.5 B样条曲线平滑策略

改进蚁群算法生成的最优路径仍不够平滑，部分路径中还存在尖锐拐点。因此，在改进蚁群算法基础上，引入三次均匀B样条曲线平滑优化拐点处的路径。

由公式

可知，k=3时的B样条曲线数学表达式为：

当三次B 样条曲线各节点矢量间插值为常数时，为三次均匀B样条曲线，第i段三次均匀B样条曲线数学表达式为：

由式（13）、（15）、（16）可得三次均匀B 样条曲线的基函数数学表达式：

将式（17）代入式（13）可得：

将式（18）用矩阵形式表达为：

式（18）、（19）为三次均匀B样条曲线数学表达式。

图2 为B 样条曲线平滑最优路径仿真示意图，折线为平滑前最优路径，曲线为B 样条曲线，该平滑策略在拐点附近，以曲线代替折线，得到的路径更短且平滑。

图2 B样条曲线平滑最优路径仿真示意图Fig.2 Simulation diagram of B-spline curve smoothing optimal path

3.6 改进蚁群算法流程

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

图3 改进蚁群算法流程Fig.3 Flowchart of improved ant colony algorithm

4 实验仿真与分析

为验证本文算法的可行性、有效性和优越性，在Matlab 2016a上进行仿真实验。主要从以下几个方面进行实验验证：1）对主要参数进行敏感性分析；2）在稍微复杂的栅格地图环境中，在相同的参数条件下，为更方便验证各个改进环节的可行性和有效性以及本文算法的优越性，在传统算法上单独添加改进的伪随机转移策略（方案1）、在方案1的基础上加初始信息素不平等分布（方案2），在方案2的基础上加改进启发函数（方案3）和在方案3的基础上加改进挥发系数（本文算法平滑前）以及在传统算法上单独添加平滑策略（传统算法+平滑）进行对比分析，将整体改进方法（本文算法平滑前后）分别与传统算法平滑前后和文献［16］改进蚁群算法平滑前后进行仿真对比分析；3）在大型复杂栅格地图环境下将本文算法与传统算法和文献［16］改进蚁群算法（使用文献参数）进行对比分析，验证本文算法的优点。

仿真参数设置：蚂蚁数目为50，最大迭代次数为100，α=2，β=7，ρmin=0.1，ρ(iter=0)=0.9，Q=150，C=20，q0=0.8，q1=0.9，q2=1。除参数分析实验外，其他实验结果都是算法运行50次得到的平均值。

4.1 主要参数分析

本文采用控制变量法，设置一系列的组合，每种组合运行20 次，对均值进行比较，原始参数组合为：α=1，β=6，Q=50，ρmin=0.3，ρmax=0.8，q0=0.6，q1=0.8，q2=1，本文算法参数分析结果如表1 所示。从表1 中可知，适当增加Q值，能够改善路径长度；由表2可知，q0的值较大时，路径长度和迭代次数较优；表3 中最小值取值不宜过小，最大值不宜过大，路径长度和迭代次数较优；表4 中参数α的值较小，β值适当较大时效果较好。

表1 参数Q对路径长度和迭代次数的影响Tab.1 Influence of parameter Q on path length and iteration times

表2 参数［q0，q1］对路径长度和迭代次数的影响Tab.2 Influence of parameters［q0，q1］on path length and iteration times

表3 参数［ρmin，ρmax］对路径长度和迭代次数的影响Tab.3 Influence of parameters［ρmin，ρmax］on path length and iteration times

表4 参数［α，β］对路径长度和迭代次数的影响Tab.4 Influence of parameters［α，β］on path length and iteration times

4.2 20×20运行环境的最优路径平滑效果对比

各方案的最优路径图如图4 所示，本文算法与传统算法和文献［16］改进蚁群算法的最优路径平滑前后图如图5 所示；上述各方案和三种算法仿真数据如表5所示。

表5 20×20运行环境下的仿真结果Tab.5 Simulation results in 20×20 running environment

图4 20×20运行环境下不同方案的最优路径对比Fig.6 Comparison of optimal paths of different schemes in 20×20 running environment

图5 20×20运行环境下各算法平滑前后最优路径对比Fig.5 Comparison of optimal paths of each algorithm before and after smoothing in 20×20 running environment

从每一改进部分得到的数据可知，传统算法最优路径长度为41.414 2，经过B 样条曲线对拐点附近平滑优化后最优路径长度为39.241 0，路径长度缩短了5.2%，说明了本文平滑策略的可行性和有效性；在传统算法基础上单独添加伪随机转移策略（方案1），最优路径长度为36.242 6，优于传统算法，算法运行时间有效缩短，拐点数目较少，迭代次数欠佳，说明了伪随机转移策略可行性和有效性；在方案1的基础上加信息素不平等分布（方案2），最优路径长度为35.071 1，优于方案1，算法运行时间进一步缩短，路径搜索的目的性有所增强（在起点和终点连线附近搜索），启发信息较弱，说明了本文初始信息素不平等分布的可行性和有效性；在方案2的基础上加改进启发函数（方案3），最短路径、算法运行时间和拐点数等指标全面改善，说明本文改进启发函数可行性和有效性；在方案3的基础上加改进挥发系数（本文平滑前），拐点数和运行时间优于方案3，说明本文改进挥发系数可行性和有效性。

从整体上看，本文改进蚁群算法和文献［16］改进蚁群算法都能够找到比传统算法得到的路径更短、拐点相对较少的路径，而且运行时间较短，能够快速收敛，路径搜索的目的性加强。本文改进算法和文献［16］改进蚁群算法平滑前最优路径相同，但本文采用了B样条曲线对路径进行二次优化，平滑后的路径优于文献［16］改进算法。为说明本文平滑策略的有效性，将本文平滑策略加入到无平滑策略的文献［16］算法中，结果显示能使文献［16］算法的路径进一步缩短，进一步验证了本文算法平滑策略的有效性。本文引入初始信息素不平等分布策略，减少了路径盲目性；引入伪随机转移策略，提高了收敛速度，得到的路径长度均值与标准差和拐点的均值与标准差都较小。均值与标准差可以衡量在某一数值附近的波动程度和稳定性，其值越小代表越好，上述实验结果验证了本文算法改进的可行性、有效性和优越性。

4.3 50×50复杂环境的最优路径平滑效果对比

为进一步验证本文算法也能适用于复杂环境，在50×50复杂环境进行仿真。传统算法和文献［16］算法与本文算法的平滑前后最优路径图和收敛曲线图分别如图6和图7所示；上述三种算法仿真数据如表6所示。

表6 50×50复杂环境下三种算法的仿真结果Tab.6 Simulation results of three algorithms in 50×50 complex environment

图6 50×50复杂环境下各算法平滑前后最优路径对比Fig.6 Comparison of optimal paths of each algorithm before and after smoothing in 50×50 complex environment

图7 50×50复杂环境下各算法收敛曲线Fig.7 Convergence curve of each algorithm in 50×50 complex environment

由实验结果可知，三种算法都能找到各自的最短路径，但本文算法所得出的路径最短，且稳定在71.639 6，仿真50次最短路径出现50 次；而传统算法和文献［16］算法搜索不到最短路径且最优解各出现1 次，由于传统算法盲目性大，启发信息较弱，在路径搜索时，拐点较多，有时出现回环交叉，远离目标行走，导致路径长度增加。文献［16］算法的启发函数加入当前节点、下一节点和目标点信息，得到的最短路径比传统算法的更短，但由于地图过于复杂，非常有必要进行初始信息素不平等分布，由于本文算法加入该策略，本文改进算法的拐点数目最少，路径搜索往往选择距离起点和终点连线最近的栅格。在大型复杂地图中，传统算法缺陷充分暴露，而本文算法优势更加突出。总之，本文算法的平滑前后最短路径、收敛速度、拐点数目和路径搜索的目的性都优于传统算法和文献［16］算法。

5 结语

在静态的全局路径规划中，本文针对传统蚁群算法的不足，提出了一种改进的蚁群算法。该算法对初始信息素进行不平等分布以降低盲目性；改进启发式函数，使其包含当前节点、下一节点和目标点的信息，以增加路径搜索的目的性；采用伪随机状态转移策略，减少了路径选择的盲目性，提高算法收敛速度以及减少拐点数目；动态调整信息素挥发系数和设置信息素浓度范围，避免算法陷入早熟；应用B 样条平滑策略，在得到最优解的基础上，进一步优化最优解。基于以上改进，本文算法能够很好地适用于不同尺度和不同复杂程度的栅格地图。