CN110426044A - 一种基于凸集计算和优化蚁群算法的避障路径规划方法 - Google Patents

Abstract

本发明公开了一种基于凸集计算和优化蚁群算法的避障路径规划方法，该法包括以下步骤：S1、生成密集障碍物环境地图，并生成障碍物集合；S2、根据凸集计算生成障碍物凸包；S3、根据路径设置的起始点和目标点，将障碍物集合转化为点集和边集；S4、根据蚁群算法进行路径规划，输出当前最短路径。在发明能在一定程度上加快传统蚁群算法在前期的收敛速度，充分利用障碍物的轴对齐特征，加快了凸包的求解过程；凸包信息，快速规划路径，减少了复杂度。

Description

一种基于凸集计算和优化蚁群算法的避障路径规划方法

技术领域

本发明属于避障路径规划领域，具体涉及一种基于凸集计算和优化蚁群算法的避障路径规划方法。

背景技术

近年来,移动机器人技术在工业、农业、医药、社会服务等领域的应用前景越来越广阔,成为国际机器人学界研究的热点问题。在一个特殊的环境中,如密集的障碍物环境,大多数优化路径与障碍物发生碰撞。人们迫切需要一种更安全、更可靠、更有效的方法,使机器人避开障碍物,达到目标,完成既定任务,并在灾后搜救。

目前国内外在基于神经网络的机器人路径规划方面已经做了大量的研究工作，提出了许多用于路径规划的神经网络模型，Chen Huahua组合神经网络和遗传算法在移动机器人路径规划中的研究。很难保证实时需求。GIasius R和Komoda A提出了一种基于神经网络的实时、动态避障网络模型。该模型可以避免局部极小点,但很难适应高速动态环境。蚁群算法由意大利学者Dorigo等人提出。蚁群算法具有良好的鲁棒性，并行分布式计算等启发式方法易于与其他优点相结合，在短期内得到了较大发展，其应用领域也得到了拓展，表明其在解决方案中的复杂性。组合优化问题。同时，由于蚁群算法收敛速度慢，容易出现停滞现象，容易陷入局部最优解。国内外专家学者对此算法的性能进行了不断的改进。

发明内容

为了解决上述现有技术所存在的问题，本发明提供一种基于凸集计算和优化蚁群算法的避障路径规划方法，通过凸集计算和优化蚁群算法，进行避障路径的最优规划。

本发明通过如下技术方案实现。

一种基于凸集计算和优化蚁群算法的避障路径规划方法，包括以下步骤：

S1、生成密集障碍物环境地图，并生成障碍物集合；

S2、根据凸集计算生成障碍物凸包；

S3、根据路径设置的起始点和目标点，将障碍物集合转化为点集和边集；

S4、根据蚁群算法进行路径规划，输出当前最短路径。

进一步的，步骤S1所述的生成密集障碍物环境地图是将环境划分成网格；

所述障碍物是边与x、y轴平行的矩形，用矩形的对角线顶点的坐标来表示改障碍物的定点信息；

根据设定好的生成概率在密集障碍物环境地图中由轮盘随机转动选出障碍物的网格，选出的网格中所有障碍物即为障碍物集合。

进一步的，步骤S2的生成障碍物凸包，其过程如下：

先创建一个临时数组来暂时存放障碍物集合中所有障碍物定点信息，这些点生成其对应的凸包，即点集Q＝{p₀，p₁，...，p_n}，其中点集包含n+1个点；然后选出点集Q中y坐标最小的点记为p₀，如果y坐标相同，则选出y坐标相同点中x坐标最小的点记为p₀，以p₀为极点按照极角(polar angle)将点p₁，p₂，...，p_n从小到大排序，极角相同的点按照到p₀的距离从小到大排序，把p₀，p₁，p₂压入栈，接着比较p₃，p₄，...，p_n，以此类推，循环遍历剩下的点，把不是凸包顶点的点从点集Q移除出去，得到障碍物凸包，即点集Q′。

进一步的，步骤S3的转化具体包括以下步骤：

S31、建立规划栈(OPEN Stack)和完成表(CLOSE Table)，其中，规划栈用来表示下一跳可达点，完成表用来表示不可访问点，每个顶点有一个已访问标志变量，用来表示该顶点是否已被访问；

S32、将路径的起始点设置为当前点，从当前点开始，检测与路径终点即目标点是否直达目标点，即两点之间的线段与其他障碍物无相交，若直达，将该边添加到边集，将当前点和目标点添加到点集，边不重复添加；若不直达，对其他的障碍物进行宽度扫描；

S33、以当前扫描点为原点作坐标系，当障碍物在x轴正方向，y轴负方向时，只需检测障碍物对角线的两个顶点；当障碍物在x轴正方向，y轴负方向时，只需检测障碍物对角线的两个顶点；当障碍物在当前点右方，假设当前点为坐标原点，则障碍物在x轴上，需要检测障碍物的一侧的两个顶点，确定障碍物需要检测的点之后，便进行扫描.

进一步的，对于障碍物生成的凸包，采用格雷厄姆(Graham)扫描，对点集Q各个点进行扫描，计算当前扫描点与上一个扫描点与原点之间的角的幅度，选出与当前点连线产生的偏角最大的一对顶点作为下一跳可达点；若检测结果为当前点与检测点直达，则将检测点与当前点连线，将该线添加到边集，将该检测点与当前点添加到点集，该线不重复添加，并将该检测点与当前点作为下一跳可达点依次压进规划栈中，然后检测当前障碍物的对角线顶点标志位是否已被标记，即被访问过，若访问过则将当前障碍物所有顶点压进完成表中，除了已经加入完成表中的顶点，再从规划栈中将栈顶顶点设置为当前点，以同样方式，直到所有障碍物顶点都被压入完成表。

进一步的，蚁群算法基于真实蚁群行为的蚁群算法是一种从巢穴到食物来源的自然方法。它是一种并行架构，可以强制蚂蚁同时独立移动，无需主管。在蚁群算法中，每个蚂蚁合作选择具有由先前蚂蚁铺设的重信息素的路径。旅行商问题(Travelling SalesmanProblem，TSP)可以说是使用蚂蚁访问每个城镇的蚂蚁算法找到最小长度闭合路径的问题。

可见，蚁群算法的求解过程主要由三个规则控制，即状态转移规则，信息素全局更新规则，信息素局部更新规则。

1)、状态转移规则，位于城市r的蚂蚁通过如下的状态转移规则选择要访问的城市s由下式得到：

上式中J_k(r)表示蚂蚁于在当前位置r能访问的城市的集合。h(r，u)是城市r和城市u之间的路径的信息素。是城市间距离的倒数，表达了启发式信息。β(β＞0)是描述状态转移时信息素和启发式信息(距离)相对重要性的参数。m是服从均匀分布的[0，1]之间的随机数，m₀是一个随机决定参数(0≤m₀≤1)，参数m₀决定了“勘探”和“开采”之间的相对重要性：当位于城市r的蚂蚁选择下一个将要访问的城市时，它选择一个随机数0≤m≤1，如果m≤m₀，则根据启发信息和信息素强度取边，否则按照下式随机选一条边S。S是由下式给出的概率分布选出的一个随机变量：

2)、信息素全局更新规则，蚁群算法中只有属于最短路径上的边的信息素才被得到增强，最短路径的寻找始终是在当前找到的最短路径的附近进行。在所有的蚂蚁遍历完每个城市之后，按照式(3)进行全局更新

h(r，s)←(1-α)·h(r，s)+α·Δh(r，s)#(3)

α是信息素的蒸发系数h(r，s)是城市r和城市s之间的路径的信息素，其中Δh(r，s)的定义是：

其中L_gb是全局最短路径长度。

3)、信息素局部更新规则，单个蚂蚁在遍历的过程中应用信息索局部更新规则对它所经过的边按照式(4)进行信息素局部更新

h(r，s)←(1-ρ)·h(r，s)+ρ·Δh(r，s)#(4)

式(4)中，ρ为参数，0＜ρ＜1。

具体的，在步骤S4所述的蚁群算法如下：

设蚁群中蚂蚁的个数n，d_i，j为平面环境中障碍物需要检测的点i与点j之间的距离，i＝1，2，...，n₁；j＝1，2，...，n₂；n₁和n₂分别是对平面工作环境的二维划分维数；a_i(t)为t时刻位于i位置点处的蚂蚁数目；显然有等式h_i，j(t)为t时刻边(i，j)的信息素轨迹强度，因为在初始时刻每条路径的信息素轨迹的量都是相等的所以有预设条件h_i，j(0)＝C，C为某个常数；

蚂蚁在运动过程中会根据各条路径上的信息素轨迹强度h_i，j决定其下一步的转移方向，在该时刻时蚂蚁要从位置点向转移其位置转移，对应的转移概率定义为：

式(1)中：q_i，j(t)是能见度的局部启发式函数，通常设为为参数α和β分别表示h_i，j(t)和q_i，j(t)对整个转移概率的影响权值；表示蚂蚁在k位置点i处的可行邻域即与点i相邻且尚未被蚂蚁访问过的位置点的集合，借助于种群的记忆功能集合在进化过程中将会不断地动态调整。

进一步的，蚂蚁算法的求解步骤包括如下：

第一步、根据移动过程中途经各点周围的距离启发式信息概率产生若干条从起点到终点的产生初始时刻的可行移动路径，每一条路径代表一只蚂蚁的爬行轨迹；

第二步、对所产生的每一条可行移动路径分别采用信息素轨迹更新函数 对路径上各点所对应的信息素进行更新，其中是不同时间的信息素的差值，h表示经过的时刻，ρ为参数，0＜ρ＜1；

第三步、对第二步中产生的每一可行路径进行修正处理，所述修正处理是将蚂蚁所走的弯曲路径逐段拉直，形成由直线段连接的可行路径即成为一条折线；

第四步、将第三步的折线与当前最短路径进行比较，如果路径长度更小则用该折线替换当前最短路径并作为当前最短路径，对折线上的所有点的信息素根据第二步进行更新；否则不替换；如果当前时刻已达到预先设定的终止时刻，则转第六步；

第五步、下一时刻蚂蚁路径的产生综合使用当前点周围的距离启发式信息概率和基于信息素轨迹的转移概率，产生由起点到终点的可行路径，并转第二步；

第六步、将当前路径作为最短路径输出。

相对于现有技术，本发明具有如下的优点及有益效果：

1.充分利用障碍物的轴对齐特征，加快了凸包的求解过程。

2.利用凸包信息，快速规划路径，减少了复杂度。

3.利用蚁群算法规划路径，加快了最优路径的求解过程。

附图说明

图1是本实施例的障碍物环境生成示意图；

图2是本实施例网格的局部观测网格图；

图3是本实施例的障碍物在障碍物环境中的简化示意图；

图4是本实施例生成的四个障碍物图；

图5生成障碍物过程生成全部障碍物；

图6是本实施例凸包生成的效果图；

图7是本实施例路线的规划过程图；

图8是本实施例路线的规划过程绕过凸包的边界产生的两种路径图；

图9是本实施例路线的规划过程起点与所有障碍物的切点效果图；

图10是本实施例产生的所有可能的路径的路径图；

图11是本实施例以凸包作为整体产生的路径图；

图12是本实施例最终生成的两条路径图；

图13为本实施例一种基于凸集计算和优化蚁群算法的避障路径规划方法的流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合附图说明对本发明的技术方案进行详细说明。

如图13所示的一种基于凸集计算和优化蚁群算法的避障路径规划方法，该方法包括以下步骤：

S1、将障碍物所在的环境划分成网格从而生成如图1和图2所示的密集障碍物环境地图；

所述障碍物是边与x、y轴平行的矩形，图3所示，用障碍物左上顶点与右下顶点的坐标来表示改障碍物的定点信息；

根据随机均匀分布的生成概率，在密集障碍物环境地图中由轮盘随机转动选出生成障碍物，选出的网格中所有障碍物即为障碍物集合，如图4生成四个障碍物，图5生成全部障碍物。

S2、凸集计算生成障碍物凸包，具体是：先创建一个临时数组来暂时存放障碍物集合中所有障碍物定点信息，这些点生成其对应的凸包，从而形成点集Q＝{p₀，p₁，...，p_n}(点集包含n+1个点，p₀为第一个，以此类推)。然后选出点集Q中y坐标最小的点记为p₀，如果y坐标相同，则选出y坐标相同点中x坐标最小的点记为p₀，以p₀为极点按照极角(polar angle)将点p₁，p₂，...，p_n从小到大排序，极角相同的点按照到p₀的距离从小到大排序，把p₀，p₁，p₂压入栈，接着比较p₃，p₄，...，p_n，以此类推，循环遍历剩下的点，把不是凸包顶点的点从点集Q移除出去，得到点集Q′，最后凸包的生成效果如图6所示，图中一些障碍物视作一个整体，生成凸包。

S3、根据设置的起始点S和目标点T，将障碍物集合转化为点集和边集，转化过程具体包括以下步骤：

S31、建立规划栈(OPEN Stack)和完成表(CLOSE Table)，其中，规划栈用来表示下一跳可达点，完成表用来表示不可访问点，每个顶点有一个已访问标志变量，用来表示该顶点是否已被访问；

S32、将路径的起始点S设置为当前点，从当前点开始，检测与路径终点即目标点是否直达目标点，即两点之间的线段与其他障碍物无相交。若直达，将该边添加到边集，将当前点和目标点添加到点集，边不重复添加；若不直达，对其他的障碍物进行宽度扫描；

S33、以当前扫描点为原点作坐标系，当障碍物在当前点的右上方即x轴正方向，y轴负方向时，只需检测障碍物的左上顶点与右下顶点；当障碍物在当前点左下方即x轴正方向，y轴负方向时，只需检测障碍物的左下顶点与右上顶点；当障碍物在在当前点的右方，假设当前点为坐标原点，则障碍物在x轴上，需要检测障碍物的左上顶点与左下顶点。得到障碍物需要检测的点之后，便可以进行扫描。

路线的规划过程中，图8显示了起点S′绕过凸包的边界到达终点T′两种方式：路径分别与A或者B相切。图9显示了路径与所有障碍物的切点。

对于密集障碍物生成的凸包，采用格雷厄姆(Graham)扫描，对点集Q各个点进行扫描，计算当前扫描点与上一个扫描点与原点之间的角的幅度，选出与当前点连线产生的偏角最大的一对顶点作为下一跳可达点；若检测结果为当前点与检测点直达，则将检测点与当前点连线，将该线添加到边集，将该检测点与当前点添加到点集，该线不重复添加，并将该检测点与当前点作为下一跳可达点依次压进规划栈中，之后检测当前障碍物的对角线顶点标志位是否已被标记，即被访问过，若访问过则将当前障碍物所有顶点压进完成表中，除了已经加入完成表中的顶点，再从规划栈中将栈顶顶点设置为当前点，以同样方式，直到所有障碍物顶点都被压入完成表。图11为以凸包作为整体考虑得到的路径。

S4、根据蚁群算法进行路径规划，输出当前最短路径；

设蚁群中蚂蚁的个数n，d_i，j为平面环境中障碍物需要检测的点i与点j之间的距离，i＝1，2，...，n₁；j＝1，2，...，n₂；n₁和n₂分别是对平面工作环境的二维划分维数；a_i(t)为t时刻位于i位置点处的蚂蚁数目；显然有等式h_i，j(t)为t时刻边(i，j)的信息素轨迹强度，因为在初始时刻每条路径的信息素轨迹的量都是相等的所以有预设条件h_i，j(0)＝C，C为某个常数；

蚂蚁在运动过程中会根据各条路径上的信息素轨迹强度h_i，j决定其下一步的转移方向，在该时刻时蚂蚁要从位置点向转移其位置转移，对应的转移概率定义为：

式(1)中：q_i，j(t)是能见度的局部启发式函数，设为参数α和β分别表示h_i，j(t)和q_i，j(t)对整个转移概率的影响权值；表示蚂蚁在k位置点i处的可行邻域即与点i相邻且尚未被蚂蚁访问过的位置点的集合，借助于种群的记忆功能集台在进化过程中将会不断地动态调整。

进一步的，蚂蚁算法的求解步骤包括如下：

第一步、根据移动过程中途经各点周围的距离启发式信息概率产生多条从起点到终点的产生初始时刻的可行移动路径，每一条路径代表一只蚂蚁的爬行轨迹；如图7和图10所示，在路线的规划过程中列出了所有可能的路径。

第二步、对所产生的每一条可行移动路径分别采用信息素轨迹更新函数 对路径上各点所对应的信息素进行更新，其中是不同时间的信息素的差值，h表示经过的时刻，ρ为参数，0＜ρ＜1；

第三步、对第二步中产生的每一可行路径进行修正处理，所述修正处理是将蚂蚁所走的弯曲路径逐段拉直为由直线段连接的可行路径即成为一条折线；

第四步、将第三步的折线与当前最短路径进行比较，如果路径长度更小则用该折线替换当前最短路径并作为当前最短路径，对折线上的所有点的信息素根据第二步进行更新；如果当前时刻已达到预先设定的终止时刻，则转第六步；

第五步、下一时刻蚂蚁路径的产生综合使用当前点周围的距离启发式信息概率和基于信息素轨迹的转移概率，产生由起点到终点的可行路径，并转第二步；

第六步、将当前路径作为最短路径输出。

采用蚁群算法与凸集计算结合的蚁群算法进行路径规划实验结果，最终结果如图12所示，

找到了两条路径，一条从上绕过障碍物，一条从下绕过障碍物。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (7)

1.一种基于凸集计算和优化蚁群算法的避障路径规划方法，其特征在于，该方法包括以下步骤：

S1、生成密集障碍物环境地图，并生成障碍物集合；

S2、根据凸集计算生成障碍物凸包；

S3、根据路径设置的起始点和目标点，将障碍物集合转化为点集和边集；

S4、根据蚁群算法进行路径规划，输出当前最短路径。

2.根据权利要求1所述的一种基于凸集计算和优化蚁群算法的避障路径规划方法，其特征在于，步骤S1所述的生成密集障碍物环境地图是将环境划分成网格；

所述障碍物是边与x、y轴平行的矩形，用矩形的对角线顶点的坐标来表示改障碍物的定点信息；

根据设定好的生成概率在密集障碍物环境地图中由轮盘随机转动选出障碍物的网格，选出的网格中所有障碍物即为障碍物集合。

3.根据权利要求1所述的一种基于凸集计算和优化蚁群算法的避障路径规划方法，其特征在于，步骤S2的生成障碍物凸包，其过程如下：

先创建一个临时数组来暂时存放障碍物集合中所有障碍物定点信息，这些点生成其对应的凸包，即点集Q＝{p₀,p₁,…,p_n}，其中点集包含n+1个点；然后选出点集Q中y坐标最小的点记为p₀,如果y坐标相同，则选出y坐标相同点中x坐标最小的点记为p₀，以p₀为极点按照极角(polar angle)将点p₁,p₂,…,p_n从小到大排序，极角相同的点按照到p₀的距离从小到大排序，把p₀,p₁,p₂压入栈，接着比较p₃,p₄,…,p_n,以此类推，循环遍历剩下的点，把不是凸包顶点的点从点集Q移除出去，得到障碍物凸包，即点集Q′。

4.根据权利要求1所述的一种基于凸集计算和优化蚁群算法的避障路径规划方法，其特征在于，步骤S3的转化过程具体包括以下步骤：

S31、建立规划栈(OPEN Stack)和完成表(CLOSE Table)，其中，规划栈用来表示下一跳可达点，完成表用来表示不可访问点，每个顶点有一个已访问标志变量，用来表示该顶点是否已被访问；

S32、将路径的起始点设置为当前点，从当前点开始，检测与路径终点即目标点是否直达目标点，即两点之间的线段与其他障碍物无相交，若直达，将该边添加到边集，将当前点和目标点添加到点集，边不重复添加；若不直达，对其他的障碍物进行宽度扫描；

S33、以当前扫描点为原点作坐标系，当障碍物在x轴正方向，y轴负方向时，只需检测障碍物对角线的两个顶点；当障碍物在x轴正方向，y轴负方向时，只需检测障碍物对角线的两个顶点；当障碍物在当前点右方，假设当前点为坐标原点，则障碍物在x轴上，需要检测障碍物的一侧的两个顶点，确定障碍物需要检测的点之后，便进行扫描。

5.根据权利要求1所述的一种基于凸集计算和优化蚁群算法的避障路径规划方法，其特征在于，对于障碍物生成的凸包，采用格雷厄姆(Graham)扫描，对点集Q各个点进行扫描，计算当前扫描点与上一个扫描点与原点之间的角的幅度，选出与当前点连线产生的偏角最大的一对顶点作为下一跳可达点；若检测结果为当前点与检测点直达，则将检测点与当前点连线，将该线添加到边集，将该检测点与当前点添加到点集，该线不重复添加，并将该检测点与当前点作为下一跳可达点依次压进规划栈中，然后检测当前障碍物的对角线顶点标志位是否已被标记，即被访问过，若访问过则将当前障碍物所有顶点压进完成表中，除了已经加入完成表中的顶点，再从规划栈中将栈顶顶点设置为当前点，以同样方式，直到所有障碍物顶点都被压入完成表。

6.根据权利要求1所述的一种基于凸集计算和优化蚁群算法的避障路径规划方法，其特征在于，步骤S4所述的蚁群算法如下：

设蚁群中蚂蚁的个数n，d_i,j为平面环境中障碍物需要检测的点i与点j之间的距离，i＝1,2,…,n₁；j＝1,2,…,n₂；n₁和n₂分别是对平面工作环境的二维划分的维数；a_i(t)为t时刻位于i位置点处的蚂蚁数目，则有等式h_i,j(t)为t时刻边(i，j)的信息素轨迹强度，因为在初始时刻每条路径的信息素轨迹的量都是相等的，所以有预设条件h_i,j(0)＝C，C为常数；

蚂蚁在运动过程中会根据各条路径上的信息素轨迹强度h_i,j决定其下一步的转移方向，在该时刻时蚂蚁要从位置点向转移其位置转移，对应的转移概率定义为：

式(1)中:m_i,j(t)是能见度的局部启发式函数，设为参数α和β分别表示h_i,j(t)和m_i,j(t)对整个转移概率的影响权值；表示蚂蚁在k位置点i处的可行邻域即与点i相邻且尚未被蚂蚁访问过的位置点的集合，借助于种群的记忆功能集合在进化过程中将会不断地动态调整。

7.根据权利要求1所述的一种基于凸集计算和优化蚁群算法的避障路径规划方法，其特征在于，蚂蚁算法的求解步骤包括如下：

第一步、根据移动过程中途经各点周围的距离启发式信息概率产生若干条从起点到终点的产生初始时刻的可行移动路径，每一条路径代表一只蚂蚁的爬行轨迹；

第二步、对所产生的每一条可行移动路径分别采用信息素轨迹更新函数 对路径上各点所对应的信息素进行更新，其中是不同时间的信息素的差值，h表示经过的时刻，ρ为参数，0<ρ<1；

第三步、对第二步中产生的每一可行路径进行修正处理，所述修正处理是将蚂蚁所走的弯曲路径逐段拉直，形成由直线段连接的可行路径即成为一条折线；

第四步、将第三步的折线与当前最短路径进行比较，如果路径长度更小则用该折线替换当前最短路径并作为当前最短路径，对折线上的所有点的信息素根据第二步进行更新；否则不替换；如果当前时刻已达到预先设定的终止时刻，则转第六步；

第五步、下一时刻蚂蚁路径的产生综合使用当前点周围的距离启发式信息概率和基于信息素轨迹的转移概率，产生由起点到终点的可行路径，并转第二步；

第六步、将当前路径作为最短路径输出。