基于改进精英势场蚁群算法的机器人三维路径规划算法研究

Robot 3D Path Planning Algorithm Based on Improved Elitist Potential Field Ant Colony Algorithm

摘要: 三维路径规划是机器人完成目标任务的重要部分，针对蚁群算法存在收敛速度慢，后期易陷入局部最优解，平滑度较低等问题，提出了一种基于改进精英势场蚁群算法的机器人三维路径规划算法。首先引入人工势场的方向性初始化信息素浓度来加快算法收敛速度，同时采用基于精英策略的信息素浓度更新方式，增强优秀个体对种群的引导作用，并采用双向搜索的策略，进一步加快算法的收敛速度，避免算法陷入局部最优解，然后引入方向扩展搜索策略对轨迹进行处理，以提高平滑度。最后通过仿真实验对精英势场蚁群算法、蚁群算法和粒子群算法进行了比较。实验结果表明，相对于其它智能优化算法，精英势场蚁群算法具有收敛速度快、搜索效率高、最终搜索的路径更优且更加平滑等特点。

Abstract: Three dimensional path planning is an important part of robot to complete the target task. Aiming at the problems of slow convergence speed, easy to fall into local optimal solution and low smoothness in the later stage of ant colony algorithm, therefore this paper proposes a robot 3D path planning algorithm based on improved elitist potential field ant colony algorithm. Firstly, the orientation of the artificial potential field is introduced to initialize the pheromone concentration to accelerate the convergence speed of the algorithm, and the pheromone concentration update method based on elite strategy is adopted to enhance the guidance of excellent individuals to the population, and the two-way search strategy is adopted to further accelerate the convergence speed of the algorithm and avoid the algorithm falling into the local optimal solution, and then the direction expansion search strategy is introduced to improve the trajectory Line processing to improve smoothness. Through simulation experiments, the elitist potential field ant colony algorithm, ant colony algorithm and particle swarm algorithm are compared. The experimental results show that compared with other intelligent optimization algorithms, the elitist potential field ant colony algorithm has fast convergence speed, high search efficiency, better and smoother final search path.

1. 引言

随着科技技术的不断进步，机器人技术已经日渐成熟并逐步应用于救援、运输、探测等多种场景 [1]。路径规划是机器人研究的重要组成部分，在许多场景下，机器人需要在复杂的三维地形中自主规划出抵达目标点的距离最短、能耗最少的安全路径。解决路径规划问题是保障机器人高效、准确、安全完成任务的重要保障，具有广阔的应用前景和价值。

目前，许多国内外的研究者已经就对机器人路径规划问题进行了大量研究，常用的路径规划算法可以分为：基于几何模型的搜索算法(Dijkstra和A*算法等)、基于概率采样的路径搜索算法(概率路径图、快速拓展随机树等)、启发式算法(人工势场法、遗传算法、蚁群算法和粒子群算法 [2] 等)。姜辰凯等 [3] 提出了一种基于时间窗的改进Dijkstra算法以实现无人车的动态路径规划。李世国等 [4] 提出了一种改进双向A*算法解决机器人在室内的路径规划问题，减少了计算量，提高了搜索效率。刘亚秋等 [5] 将扩展点选择策略和自适应步长策略引入传统RRT (快速拓展随机树)算法，以指导RRT树的生长方向，避免陷入极小值。汤迪 [6] 通过增加虚拟目标并改进斥力场的方式改进人工势场法规划移动机器人路径，改进的人工势场法可以有效避免出现陷入局部极小值的情况。付兴武等 [7] 提出了一种结合天牛须搜索算法的改进粒子群算法，利用天牛个体的优势，每次迭代即可对周围环境进行判断，提高了搜索效率。陈超等 [8] 对蚁群算法进行了改进，采用局部和全局信息结合互动的信息素更新方式，加快了收敛速度。

虽然上述算法在机器人路径规划中有着不错的效果，但目前机器人三维路径规划算法仍然存在收敛速度慢、搜索精度不高、后期易陷入局部最优解，平滑度较低等问题。针对上述问题，出了一种精英势场蚁群算法，该算法引入了人工势场法为蚁群提供更加合理搜索方向，其启发信息是机器人当前位置到目标点的距离与机器人所受人工势场合力启发信息的合并结果，同时采用了基于精英策略的信息素浓度更新方式，增强优秀个体对种群的引导作用，以加快种群的收敛速度，采用双向搜索策略，进一步提升收敛速度并避免算法陷入局部最优，搜索方向用以平滑路径。通过与遗传算法、传统的蚁群算法和粒子群算法的对比结果表明，本文算法的搜索效率和收敛速度更高，且路径平滑度较高。

2. 蚁群算法

蚁群算法(Ant Colony System, ACS)是由M. Dorigo等人于1991年通过模拟蚂蚁群体觅食行为提出的一种启发式路径规划算法 [9]。蚂蚁在觅食行为中，分泌信息素记录自身路径，并且蚂蚁会选择信息素浓度较高的路径。距离较短的路径信息素浓度会逐渐升高，从而促使大多数蚂蚁个体集中至一条相对最优的路径上。

在传统蚁群算法中，蚂蚁k根据信息素浓度τ_ij(t)和当前节点至目标节点的距离启发信息η_ij(t)从当前节点i选择下一节点j，其概率如式(1)所示

$P_{ij}\left(t\right)=\{\begin{array}{l}\frac{{\left[{\tau }_{ij}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{ij}\left(t\right)\right]}^{\beta }}{{\displaystyle \underset{j\in A}{\sum }{\left[{\tau }_{ij}\left(t\right)\right]}^{\alpha }{\left[{\eta }_{ij}\left(t\right)\right]}^{\beta }}}j\in A\\ 0其他\end{array}$ (1)

式中：P_ij即为从当前节点选择j作为下一节点的概率；A为当前节点的邻接节点，即下一步可选节点；α为信息启发式因子；β为期望启发式因子；距离启发信息η_ij(t)即为两节点之间距离的倒数，即

${\eta }_{ij}\left(t\right)=\frac{1}{L_{ij}}$ (2)

式中：L_ij即为节点i和j之间的距离，本文为欧氏距离。蚂蚁选择下一节点之后，将对路径信息素进行局部更新，主要是进行信息素的增加和挥发，信息素浓度更新公式如式(3)所示

${\tau }_{ij}\left(t+1\right)=\left(1-\lambda \right){\tau }_{ij}\left(t\right)+\Delta {\tau }_{ij}$ (3)

式中： $\lambda \left(0<\lambda <\text{1}\right)$ 即为信息素浓度挥发系数， $\left(\text{1}-\lambda \right)$ 表示经挥发后路径所残留的信息素浓度，Δτ_ij表示节点i到j的信息素浓度增量，通过式(4)得出。

$\Delta {\tau }_{ij}\left(t\right)=\{\begin{array}{l}\frac{1}{L_k}{L}_{ij}\in P_k\\ 0其他\end{array}$ (4)

式中：L_k表示蚂蚁k搜索路径的总距离，P_k为蚂蚁k走过所有路径的集合，对上述路径搜索过程不断迭代即可寻找出最优路径。

3. 人工势场算法

人工势场法是通过引入虚拟力场，规划机器人路径的一种方法。假设目标点产生引力，越靠近目标点，机器人所受引力越强，相对的，障碍物产生斥力，越靠近障碍物，机器人所受斥力越强，机器人在这两种力的共同作用下在在一定空间内完成避障运动作业 [10]。假设机器人当前节点位置为 $S=\left(x_0,y_0\right)$，目标节点位置为 $E=\left(x_e,y_e\right)$。引力势场函数U_att定义如下

$U_{att}=\frac{1}{2}k\left(S-E\right)$ (5)

式中：k为引力场增益常数，(S-E)为机器人和目标节点之间的相对位置距离。

引力势场的负梯度即为目标节点对机器人引力值，如式(6)所示。

$F_{att}=-grad\left(U_{att}\right)=-k\left(S-E\right)$ (6)

同理，斥力势场函数U_rep是障碍物对机器人的斥力值F_rep, 定义如式(7)~(8)所示。

$U_{rep}=\{\begin{array}{l}\frac{1}{2}\theta {\left(\frac{1}{\rho }-\frac{1}{{\rho }_0}\right)}^2\rho \le {\rho }_0\\ 0\rho >{\rho }_0\end{array}$ (7)

$F_{rep}=-grad\left(U_{rep}\right)=\{\begin{array}{l}\theta \left(\frac{1}{\rho }-\frac{1}{{\rho }_0}\right)\frac{1}{{\rho }^2}\rho \le {\rho }_0\\ 0\rho >{\rho }_0\end{array}$ (8)

式中：θ为斥力场增益常数，ρ为当前节点到障碍物的相对距离，ρ₀为障碍物斥力场的极限作用距离。即当机器人位于极限作用距离ρ₀外时，障碍物对机器人无作用。

在一定空间内，机器人在目标节点和引力和障碍物的斥力共同作用下运动，其合力公式(9)如下所示：

$F_{tot}=F_{att}+F_{rep}=\{\begin{array}{l}\theta \left(\frac{1}{\rho }-\frac{1}{{\rho }_0}\right)\frac{1}{{\rho }^2}-k\left(S-E\right)\rho \le {\rho }_0\\ 0\rho >{\rho }_0\end{array}$ (9)

4. 精英势场蚁群算法

4.1. 精英策略

在传统蚁群算法中，所有个体均扮演同样的角色，所有个体遗留的信息素浓度相同。为了加快算法收敛速度，本研究将种群个体中表现前5% (即路径长度较短的前5%)的个体试作精英个体，重构信息素浓度更新公式如式(10)所示

${\tau }_{ij}\left(t+1\right)=\left(1-\lambda \right){\tau }_{ij}\left(t\right)+\Delta {\tau }_{ij}+{\left(\Delta {\tau }_{ij}\right)}^{+}$ (10)

式中：(Δτ_ij)⁺即为精英个体产生的增量信息素。迭代前期，精英个体的增量信息素应设置较小，以尽可能提高蚂蚁种群的多样性，随着迭代，精英个体逐渐接近最优解，在迭代后期，应该较大程度发挥精英个体的引导作用，即增大增量信息素，以促进种群收敛。基于上述思路，本文设置的增量信息素公式如式(11)

${\left(\Delta {\tau }_{ij}\right)}^{\text{+}}=\{\begin{array}{l}{\left(\frac{it}{i{t}_{\max }}\right)}^3\times \frac{1}{L_k}k是精英个体\\ 0k非精英个体\end{array}$ (11)

式中：it为当前迭代次数；it_max为最大迭代次数； $it/i{t}_{\max }\in \left[0,1\right]$，因此用幂函数构造精英个体信息素增量可以弱化精英个体的前期引导能力，并增强后期引导能力，以保证种群前期的多样性，并促进算法后期加快收敛。

4.2. 双向搜索策略

传统蚁群算法在搜索时采用单向搜索的方式，即仅设置一队蚂蚁从出发点向目标点进发，这种搜索方式效率较低，且在复杂地形情况下易陷入局部最优。在出发点和目标点已知的情况下，采用双向搜索的策略能够有效解决上述不足，即设置两队蚂蚁，一队从出发点向目标点进发，另一队则从目标点向出发点进发。两个蚂蚁队伍相互独立，具有自己的信息素，每队中蚂蚁个体只会受到所在队伍信息素的影响。在迭代过程中，若不同队伍中的两只蚂蚁个体相遇，则将两个蚂蚁已走的路径相连，形成一条完整路径，并锁定两只蚂蚁不再继续运动，上述过程的具体步骤如下：

步骤1：构建种群大小相同的正向蚁群R1和逆向蚁群R2， $\text{R1}=\left\{r_{11},r_{12},\cdots ,r_{1m},\cdots ,r_{1M}\right\}$， $\text{R1}=\left\{r_{21},r_{22},\cdots ,r_{2m},\cdots ,r_{2M}\right\}$ ；

步骤2：记录每个个体经过的节点，并建立节点记录列表，如List(r_1m)；

步骤3：如果正向蚁群R1中的某个体的节点记录列表与逆向蚁群R2中的某个体的节点记录列表产生重叠，则在该节点处，将两个列表合并，进而产生一条从出发点向目标点的可行路径，并锁定两只蚂蚁不再继续运动。

4.3. 人工势场初始化信息素浓度

传统蚁群算法各节点初始信息素浓度相同，会降低蚂蚁个体的搜索效率和迭代速度。因此，本文引入人工势场法的引力和斥力势场初始化信息素浓度分布。

引力势场和引力值函数与传统的人工势场法相同，如式(5)和(6)。同时，不同于二维情形，三维地图往往不存在严格的障碍物，本文重构斥力势场，以初始点的高度作为基准，高度越高的位置斥力越强，高度低于基准高度的位置斥力置为0。对于任意一点 $\left(x,y,z\right)$，初始点 $\left(x_0,y_0,z_0\right)$ 的三维环境而言，其产生的斥力势场函数 ${U^{\prime }}_{rep}$ 的斥力值 ${F^{\prime }}_{rep}$ 表示如式(12)和(13)所示

${U^{\prime }}_{rep}=\{\begin{array}{l}\frac{1}{2}\theta \cdot \left(z-z_0\right)\cdot {\left(\frac{1}{\rho }-\frac{1}{{\rho }_0}\right)}^2\rho \le {\rho }_0\left|\right|z>z_0\\ 0\rho >{\rho }_0\left|\right|z\le z_0\end{array}$ (12)

${F^{\prime }}_{rep}=-grad\left({U^{\prime }}_{rep}\right)=\{\begin{array}{l}\theta \cdot \left(z-z_0\right)\cdot \left(\frac{1}{\rho }-\frac{1}{{\rho }_0}\right)\frac{1}{{\rho }^2}\rho \le {\rho }_0\left|\right|z>z_0\\ 0\rho >{\rho }_0\left|\right|z\le z_0\end{array}$ (13)

其合力 ${F^{\prime }}_{tot}$ 可作为系数，初始化任意一点 $\left(x,y,z\right)$ 信息素浓度如式(14)所示

${\tau }_0\left(x,y,z\right)={F^{\prime }}_{tot}\left(x,y,z\right)\cdot {\tau }_0$ (14)

式中：τ₀是信息素浓度初始化基准值。

4.4. 方向扩展搜索策略

传统蚁群算法，通常采用8邻域搜索的方式，如图1(a)所示，即从当前节点可以抵达的下一节点为其相邻的8个节点，这种方式会限制蚂蚁个体的搜索方向，本研究将传统8邻域搜索的方式拓展为了16邻域搜索，如图1(b)所示。

4.5. 基于人工势场的启发信息改进

传统蚁群算法仅考虑距离启发信息，没有考虑障碍物的影响，本研究引入势场启发信息函数，即将距离启发信息函数η_ij(t)重构如式(12)所示

${\eta }_{ij}\left(t\right)={\left(\frac{i{t}_{\max }-it}{i{t}_{\max }}\right)}^3\times a^{F_{tot}\cdot \cos \phi }\times \frac{1}{d_{ij}}$ (12)

式中：a > 0且为常数；φ为蚂蚁当前节点和下一步节点连线与势场合力的夹角。人工势场虽能加快蚁群的收敛速度，但有可能会导致蚁群个体集中于势场分布的梯度方向上。为此，本研究引入幂函数构造自适应权重系数，以增强算法前期人工势场合力对蚂蚁个体的引导作用，弱化算法后期人工势场合力对蚂蚁个体的影响。

(a) (b)

Figure 1. Comparison of individual search direction strategies of ant colony

图1. 蚁群个体搜索方向策略对比

综上，改进的精英势场蚁群算法流程图如图2所示。

Figure 2. Flow chart of improved elitist potential field ant colony algorithm

图2. 改进的精英势场蚁群算法流程图

具体步骤如下：

(1) 初始化算法参数，输入三维地图环境，并对环境进行栅格化建模；

(2) 创建相同规模正向和逆向两个蚂蚁种群，分别置于起点和终点，对每个蚂蚁个体建立节点记录列表；

(3) 算法开始，蚂蚁按照改进后的启发信息和转移概率选择下一节点，并实时更新信息素浓度；

(4) 将选择的节点加入对应的蚂蚁个体节点记录列表中，并记录路径长度；

(5) 如果正向蚁群R1中的某个体的节点记录列表与逆向蚁群R2中的某个体的节点记录列表产生重叠，则在该节点处，将两个列表合并，进而产生一条从出发点向目标点的可行路径，并锁定两只蚂蚁不再继续运动；否则返回步骤(3)；

(6) 找出精英个体，并更新全局信息素；

(7) 判断迭代是否结束，是则输出最优路径；否则返回(3)。

5. 仿真实验研究

为了验证算法的有效性，将传统蚁群算法、粒子群算法和本文提出的精英势场蚁群算法运用至图3所示的三维地图环境中，出发点坐标为(1, 10, 800)，目标点坐标为(21, 8, 1000)。进行仿真实验，相关算法参数设置如表1所示。

Figure 3. Schematic diagram of 3D map environment

图3. 三维地图环境示意图

Table 1. Parameter setting of elitist potential field ant colony algorithm

表1. 精英势场蚁群算法参数设置

三种算法的结果如图4(a)和图4(b)所示，图4(a)为路径的三维显示，图4(b)则为路径在等高线上的投影，表2列出了三种算法的路径长度。

(a)(b)

Figure 4. Schematic diagram of path planning results of three algorithms

图4. 三种算法的路径规划结果示意图

Table 2. Comparison of path length results of three algorithms

表2. 三种算法的路径长度结果对比

结合表2和图4(a)，图4(b)可以看出，精英势场蚁群算法所找到的最优路径最短，较传统蚁群算法减少了超过10个单位的长度距离。即引入精英、人工势场和双向搜索策略能够有效增强算法的寻优能力。

为进一步分析算法收敛速度，绘制了如图5所示的迭代测试图，该图记录了三种算法蚂蚁种群的最优解随迭代次数的变化曲线。

Figure 5. Variation curve of optimal path with iteration times

图5. 最优路径随迭代次数变化曲线

由图5可以看出，传统蚁群、粒子群算法和精英势场蚁群算法在第70次、第89次和第27次收敛，精英势场蚁群算法收敛速度最快，较传统蚁群算法效率提升超过60%，并且能够有效减少最优路径的长度。

为验证算法的鲁棒性，将三种算法运用至30 ´ 30、50 ´ 50的三维地图中运行10次，结果如表3所示，其中Gap表示精英势场蚁群较传统蚁群算法的改善程度，由式(13)计算得出。由表3可以看出在不同的情形下运行多次，精英势场蚁群算法的寻优能力和收敛速度均优于传统蚁群算法和粒子群算法，即精英势场蚁群算法具有良好的鲁棒性。具体而言，改进之后的精英势场蚁群较传统蚁群能够提高近10%的寻优精度和近50%的收敛速度。

$\text{Gap}=\frac{参{数}_{精英势场蚁群}-参{数}_{蚁群}}{参{数}_{蚁群}}\times 100\%$ (13)

结合图5和表3可以看出，精英、人工势场和双向搜索策略不仅能够有效增强算法的寻优能力，而且可以加快算法收敛，提高算法的搜索效率。

Table 3. Comparison results of three algorithms

表3. 三种算法对比结果

6. 结论

针对机器人三维路径规划问题，本研究在传统蚁群算法的基础上提出了一种精英势场蚁群算法。该算法核心思路是引入人工势场的方向性初始化信息素浓度来加快算法收敛速度，并采用基于精英策略的信息素浓度更新方式，增强优秀个体对种群的引导作用，同时采用双向搜索的策略，进一步加快算法的收敛速度，避免算法陷入局部最优解，还引入方向扩展搜索策略来对轨迹进行处理，以提高平滑度。

仿真实验结果表明，精英势场蚁群算法较传统蚁群算法和粒子群算法具有寻优能力强，收敛速度快，搜索效率高，鲁棒性强等优点。

基金项目

国防预研基金(61400060502)。

NOTES

^*通讯作者。