CN112631294B - 一种移动机器人智能路径规划方法 - Google Patents

Abstract

本发明公开了一种移动机器人智能路径规划方法，建立静态二维栅格地图，利用改进蚁群算法进行全局路径规划；移动机器人传感器模块检测未知障碍物信息，计算障碍物运动轨迹和机器人运动轨迹，采用优化动态窗口法进行局部动态避障，以机器人当前位置为起始点，全局规划路径上的最临近关键节点为临时目标点动态避障；机器人沿着规划路径行驶并且安全到达目的地。本发明综合考虑了地图环境中的静态障碍物以及动态障碍物的实际问题，改进蚁群算法的启发函数，调整信息素更新规则进行全局路径规划，机器人行驶过程中遇到动态障碍物采用优化的动态窗口法进行避障，完成局部路径规划，机器人在实际地图运行有比较高的实用性和研究价值。

Description

一种移动机器人智能路径规划方法

技术领域

本发明涉及机器人路径规划领域，特别涉及一种移动机器人智能路径规划方法。

背景技术

机器人实际运行的地图环境中存在两种类型的障碍，一种是已知静态的障碍，一种是随机出现的未知障碍物。全局路径规划是指在给定的环境障碍条件下，选择一条从起始点到目标点的路径，使机器人可以安全、无碰撞地通过所有的障碍。这种自主地躲避障碍并完成作业任务的方法，是机器人研究和应用中的一个重要内容。

静态障碍物的地图，常见的机器人全局路径规划方法有A*算法、Dijkstra算法、遗传算法、粒子群算法和蚁群算法等。其中A*算法，Dijkstra算法的复杂度较低,计算量相对其他算法都较小,但是在自动化任务要求更高，环境更复杂的情况下难以满足任务需求。仿生智能算法越来越受到关注，特别是蚁群算法，由于该算法自组织，分布式，正反馈等优点广泛应用到机器人路径规划中。

在实际环境中，可能会随机出现未知动态障碍物，针对动态环境目前常见的方法有人工势场法和动态窗口法等，由于人工势场法容易陷入局部最优出现震荡，寻优失败的问题。动态窗口法由于考虑车辆运动模型，同时结合车辆的运动学约束，能够根据障碍物预测轨迹进行建模，提高动态环境下对障碍物的避障能力。因此本文采用优化动态窗口法进行动态避障。

发明内容

为了克服现有技术中的不足，本发明提供一种移动机器人智能路径规划方法，通过改进蚁群算法，启发函数引入目标点，提高算法收敛的速度；在避障路径规划过程中，通过将拐点评价函数作为路径信息素更新影响因素，减少规划路径的拐点数目，提高搜索路径的光滑性，减少了机器人碰撞的风险以及降低机器人行驶的转弯能耗。检测到动态障碍物利用优化动态窗口算法有效的避开障碍物，从而实现移动机器人的智能路径规划。

为了达到上述发明目的，解决其技术问题所采用的技术方案如下：

一种移动机器人智能路径规划方法，包括以下步骤：

步骤1：根据已知静态二维空间建立栅格地图，设置栅格序号，选择起始点S和目标点E；

步骤2：改进蚁群算法进行全局路径规划；

步骤3：移动机器人在全局规划的路径上行驶，移动机器人传感器模块检测未知障碍物，并且获取障碍物大小和速度信息；

步骤4：如果传感器检测未知障碍物距离机器人小于安全距离S1，则机器人立刻刹车并停靠，再根据障碍物运动情况是否重新规划路径；如果动态障碍物远离机器人或者障碍行驶轨迹不与机器人规划路径有交集，则继续行驶；如果动态障碍物靠近机器人或者停靠在全局规划的路径上，则控制器发出预警，机器人回退并且重新规划路径；

步骤5：如果传感器检测未知障碍物距离机器人大于安全距离S1，利用感知到障碍物信息，计算障碍物运动轨迹和机器人运动轨迹，采用优化动态窗口法进行局部动态避障，以机器人当前位置为起始点，全局规划路径上的最临近关键节点为临时目标点动态避障；

步骤6：采用动态窗口法机器人完成避障到达临近关键节点，然后重复步骤3-5，直到机器人沿着规划路径行驶并且安全到达目的地。

进一步的，步骤2蚁群算法转移概率中的启发函数引入目标点E，提高算法收敛速度，转移概率函数加入变异因子，提高解的多样性避免算法陷入局部最优：

将当前节点L到待选节点M的真实代价d_ij以及待选节点M到目标点(的预估代价d_je作为启发函数：

其中，τ_ij表示路径(i，j)信息素浓度，d_ij是当前节点i到待选节点j的欧氏距离，d_je是待选节点M到目标节点(预估的欧氏距离，η_ij是启发函数，α为信息素启发因子，表示信息素浓度对转移概率影响；β为期望启发因子，ζ表示路径信息对转移概率的影响；

其中，P_k代表蚂蚁K从当前节点i到待选节点j的转移概率，

代表无变异情况下的转移概率公式通过公式(1-1)计算得到，rand代表变异概率，每次转移都会生成一个(0，1)范围内的随机数，P_var代表变异因子，如果rand小于变异因子则表明发生变异，即当前蚂蚁概率转移发生改变。

进一步的，步骤2中改进蚁群算法信息素更新规则：

信息素更新规则引入拐点评价函数，可以使所寻路径更加平滑，节省机器人行走时间和功耗，机器人行走路径有不同的拐角，拐角大小也代表不同的功耗；

所有的蚂蚁完成一次迭代之后，路径上信息素挥发处理，在t+1时刻路径(i，j)上信息素更新方式如下：

τ_ij(t+1)＝(-ρ)*τ_ij(t)+Δτ_ij(t) (1-5)

其中，ρ表示信息素挥发系数，(1-ρ)则表示路径信息素残留因子，

表示第k只蚂蚁在此轮迭代过程中在路径(i，j)上留下的信息素，公式如下：

其中，Q为信息素强度为固定常数，L_k表示蚂蚁k在本轮迭代中所走过的路径总长度，

为规划路径拐角的大小，机器人移动过程中左右拐角大小一致都是0°，(0，45°]，(45°，90°]，(90°，180°]，

为拐角评价函数，ξ为拐角影响系数。

进一步的，步骤5建立机器人运动轨迹，预测障碍物轨迹，假设机器人不能全向移动，即不能有横向的速度，只有前进的线速度和旋转角速度即(v，ω)：

设动态障碍物的运动模型为：

将方程组联立求解，如果方程组无解则表明两个方程没有交集，即当前路径不会存在危险，机器人不会与动态障碍物发生碰撞；若方程组有解则表明两个轨迹有交集(x_i，y_i，t_i)(i＝0，1，2…)，即在t＝t_i(i＝0，1，2…)时刻机器人将会与动态障碍物发生碰撞，按照当前路径行驶存在危险，需要进行动态避障。

进一步的，步骤5建立机器人速度约束函数：

1)机器人速度限制，机器人最大和最小速度的限制：

V_m＝{v∈[v_min，v_max]，ω∈[ω_min，ω_max]} (1-11)

其中，v_min、v_max分别是最小、最大线速度；ω_min、ω_max分别是最小、最大角速度；

2)加速度限制，机器人的由于电机驱动性能限制，所能达到的最大与最小加速度，在Δt时间内最大，最小线速度和最大，最小角速度共同组合生成一个速度动态窗口：

其中，v_c、ω_c为当前线速度和角速度，

为最小线加速度和最大线加速度，

为最小角加速度和最大角加速度；

3)安全距离限制，机器人能够在最大减速的条件下，在撞到障碍物前停下来的速度约束：

其中，dist(v，ω)为速度(v，ω)对应轨迹上离障碍物最近的距离；

满足以上约束条件的速度空间为:

进一步的，步骤5动态窗口法避障，优化评价函数G，对每条速度对应的轨迹进行评价：

其中，heading(v，ω)为方位角评价函数，表示测量机器人在当前设定的采样速度下，达到预测轨迹末端时的朝向和目标之间的角度差距，角度差距越小，heading(v，ω)越大，dist(v，ω)表示轨迹曲率与最近障碍物相交的距离，距离越远dist(v，ω)越大，如果没有障碍物则设置一个较大的常数，velocity(v，ω)为机器人在相应轨迹上速度的大小，quatity(v，ω)为障碍物数量评价函数，表示机器人周围障碍物数量的多少，数量越多函数值越小，α为方向角影响因子，β为距离安全系数，γ为速率影响因子，λ为障碍物数量影响因子。

本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：

1、本发明基于改进蚁群全局路径规划，针对未知障碍物采取优化的局部动态窗口法避障，提高机器人实时避障能力。

2、本发明综合考虑了地图环境中的静态障碍物以及动态障碍物的实际问题，改进蚁群算法的启发函数，调整信息素更新规则进行全局路径规划，机器人行驶过程中遇到动态障碍物采用优化的动态窗口法进行避障，完成局部路径规划，机器人在实际地图运行有比较高的实用性和研究价值。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。附图中：

图1是本发明一个实例中提供的移动机器人智能路径规划方法的流程图；

图2是静态20*20栅格地图中改进蚁群算法与基本蚁群算法全局路径规划结果对比；

图3是机器人感知障碍物分布示意图；

图4是机器人运动学模型；

图5是机器人运行到某个节点检测到期望路径上有未知动态障碍物时采用局部动态窗口法进行局部避障仿真示意图。

具体实施方式

以下将结合本发明的附图，对本发明实施例中的技术方案进行清楚、完整的描述和讨论，显然，这里所描述的仅仅是本发明的一部分实例，并不是全部的实例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。

如图1-5所示，本实施例公开了一种移动机器人智能路径规划方法，包括以下步骤：

步骤1：根据已知静态二维空间建立栅格地图，设置栅格序号，选择起始点S和目标点E；

步骤2：改进蚁群算法进行全局路径规划；

步骤3：移动机器人在全局规划的路径上行驶，移动机器人传感器模块检测未知障碍物，并且获取障碍物大小、速度等信息；

步骤4：如果传感器检测未知障碍物距离机器人小于安全距离S1，则机器人立刻刹车并停靠，再根据障碍物运动情况是否重新规划路径；如果动态障碍物远离机器人或者障碍行驶轨迹不与机器人规划路径有交集，则继续行驶；如果动态障碍物靠近机器人或者停靠在全局规划的路径上，则控制器发出预警，机器人回退并且重新规划路径；

步骤5：如果传感器检测未知障碍物距离机器人大于安全距离S1，利用感知到障碍物信息，计算障碍物运动轨迹和机器人运动轨迹，采用优化动态窗口法进行局部动态避障，以机器人当前位置为起始点，全局规划路径上的最临近关键节点为临时目标点动态避障；

步骤6：采用动态窗口法机器人完成避障到达临近关键节点，然后重复步骤3-5，直到机器人沿着规划路径行驶并且安全到达目的地。

进一步的，步骤2蚁群算法转移概率中的启发函数引入目标点E，提高算法收敛速度，转移概率函数加入变异因子，提高解的多样性避免算法陷入局部最优：

将当前节点i到待选节点j的真实代价d_ij以及待选节点j到目标点E的预估代价d_je作为启发函数：

其中，τ_ij表示路径(i，j)信息素浓度，d_ij是当前节点i到待选节点j的欧氏距离，d_je是待选节点j到目标节点E预估的欧氏距离，η_ij是启发函数，α为信息素启发因子，表示信息素浓度对转移概率影响；β为期望启发因子，ζ表示路径信息对转移概率的影响；

其中，P_k代表蚂蚁K从当前节点i到待选节点j的转移概率，

代表无变异情况下的转移概率公式通过公式(1-1)计算得到，rand代表变异概率，每次转移都会生成一个(0，1)范围内的随机数，P_var代表变异因子，如果rand小于变异因子则表明发生变异，即当前蚂蚁概率转移发生改变。

进一步的，步骤2中改进蚁群算法信息素更新规则：

信息素更新规则引入拐点评价函数，可以使所寻路径更加平滑，节省机器人行走时间和功耗，机器人行走路径有不同的拐角，拐角大小也代表不同的功耗；

所有的蚂蚁完成一次迭代之后，路径上信息素挥发处理，在t+1时刻路径(i，j)上信息素更新方式如下：

τ_ij(t+1)＝(1-ρ)*τ_ij(t)+Δτ_ij(t) (1-5)

其中，ρ表示信息素挥发系数，(1-ρ)则表示路径信息素残留因子，

表示第k只蚂蚁在此轮迭代过程中在路径(i，j)上留下的信息素，公式如下：

其中，Q为信息素强度为固定常数，L_k表示蚂蚁k在本轮迭代中所走过的路径总长度，

为规划路径拐角的大小，机器人移动过程中左右拐角大小一致都是0°，(0，45°]，(45°，90°]，(90°，180°]，

为拐角评价函数，ξ为拐角影响系数。

进一步的，步骤5建立机器人运动轨迹，预测障碍物轨迹，假设机器人不能全向移动，即不能有横向的速度，只有前进的线速度和旋转角速度即(vω)：

设动态障碍物的运动模型为：

将方程组联立求解，如果方程组无解则表明两个方程没有交集，即当前路径不会存在危险，机器人不会与动态障碍物发生碰撞；若方程组有解则表明两个轨迹有交集(x_iy_i，t_i)(i＝0，1，2…)，即在t＝t_i(i＝0，1，2…)时刻机器人将会与动态障碍物发生碰撞，按照当前路径行驶存在危险，需要进行动态避障。

进一步的，步骤5建立机器人速度约束函数：

1)机器人速度限制，机器人最大和最小速度的限制：

V_m＝{v∈[v_min，v_max]，ω∈[ω_min，ω_max]} (1-11)

其中，v_min、v_max分别是最小、最大线速度；ω_min、ω_max分别是最小、最大角速度；

2)加速度限制，机器人的由于电机驱动性能限制，所能达到的最大与最小加速度，在Δt时间内最大，最小线速度和最大，最小角速度共同组合生成一个速度动态窗口：

其中，v_c、ω_c为当前线速度和角速度，

为最小线加速度和最大线加速度，

为最小角加速度和最大角加速度；

3)安全距离限制，机器人能够在最大减速的条件下，在撞到障碍物前停下来的速度约束：

其中，dist(v，ω)为速度(v，ω)对应轨迹上离障碍物最近的距离；

满足以上约束条件的速度空间为:

V＝V_m∩V_d∩V_sofe (1-14)。

进一步的，步骤5动态窗口法避障，优化评价函数G，对每条速度对应的轨迹进行评价：

其中，heading(v，ω)为方位角评价函数，表示测量机器人在当前设定的采样速度下，达到预测轨迹末端时的朝向和目标之间的角度差距，角度差距越小，heading(v，ω)越大，dist(v，ω)表示轨迹曲率与最近障碍物相交的距离，距离越远dist(v，ω)越大，如果没有障碍物则设置一个较大的常数，velocity(v，ω)为机器人在相应轨迹上速度的大小，quatity(v，ω)为障碍物数量评价函数，表示机器人周围障碍物数量的多少，数量越多函数值越小，α为方向角影响因子，β为距离安全系数，γ为速率影响因子，λ为障碍物数量影响因子。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (5)

1.一种移动机器人智能路径规划方法，其特征在于，包括以下步骤：

步骤1：根据已知静态二维空间建立栅格地图，设置栅格序号，选择起始点S和目标点E；

步骤2：改进蚁群算法进行全局路径规划；

步骤3：移动机器人在全局规划的路径上行驶，移动机器人传感器模块检测未知障碍物，并且获取障碍物大小和速度信息；

步骤4：如果传感器检测未知障碍物距离机器人小于安全距离S1，则机器人立刻刹车并停靠，再根据障碍物运动情况是否重新规划路径；如果动态障碍物远离机器人或者障碍行驶轨迹不与机器人规划路径有交集，则继续行驶；如果动态障碍物靠近机器人或者停靠在全局规划的路径上，则控制器发出预警，机器人回退并且重新规划路径；

步骤5：如果传感器检测未知障碍物距离机器人大于安全距离S1，利用感知到障碍物信息，计算障碍物运动轨迹和机器人运动轨迹，采用优化动态窗口法进行局部动态避障，以机器人当前位置为起始点，全局规划路径上的最临近关键节点为临时目标点动态避障；

步骤5中动态窗口法避障，优化评价函数G，对每条速度对应的轨迹进行评价：

其中，heading(v,ω)为方位角评价函数，表示测量机器人在当前设定的采样速度(v,ω)下，达到预测轨迹末端时的朝向和目标之间的角度差距，角度差距越小，heading(v,ω)越大，dist(v,ω)表示机器人轨迹曲率与最近障碍物相交的距离，距离越远dist(v,ω)越大，如果没有障碍物则设置一个较大的常数，velocity(v,ω)为机器人在相应轨迹上速度的大小，quatity(v,ω)为障碍物数量评价函数，表示机器人周围障碍物数量的多少，数量越多函数值越小，α为方向角影响因子，β为距离安全系数，γ为速率影响因子，λ为障碍物数量影响因子；

步骤6：采用动态窗口法机器人完成避障到达临近关键节点，然后重复步骤3-5，直到机器人沿着规划路径行驶并且安全到达目的地。

2.根据权利要求1所述的一种移动机器人智能路径规划方法，其特征在于，步骤2蚁群算法转移概率

中的启发函数η_ij引入目标点E，提高算法收敛速度，转移概率函数加入变异因子P_var，提高解的多样性避免算法陷入局部最优：

将当前节点i到待选节点j的真实代价dⁱ _j以及待选节点j到目标点E的预估代价d_je作为启发函数η_ij：

其中，τ_ij表示路径(i,j)信息素浓度，d_ij是当前节点i到待选节点j的欧氏距离，d_je是待选节点j到目标节点E预估的欧氏距离，η_ij是启发函数，α为信息素启发因子，表示信息素浓度对转移概率影响；β为期望启发因子，ζ表示路径信息对转移概率的影响；

其中，P_k(i,j)代表蚂蚁K从当前节点i到待选节点j的转移概率，

代表无变异情况下的转移概率公式通过公式(1-1)计算得到，rand代表变异概率，每次转移都会生成一个(0,1)范围内的随机数，P_var代表变异因子，如果rand小于变异因子则表明发生变异，即当前蚂蚁概率转移发生改变。

3.根据权利要求2所述的一种移动机器人智能路径规划方法，其特征在于，步骤2中改进蚁群算法信息素更新规则：

信息素更新规则引入拐点评价函数

可以使所寻路径更加平滑，节省机器人行走时间和功耗，机器人行走路径有不同的拐角

拐角大小也代表不同的功耗；

所有的蚂蚁完成一次迭代之后，路径上信息素挥发处理，在t+1时刻路径(i,j)上信息素τ_ij(t+1)更新方式如下：

τ_ij(t+1)＝(1-ρ)*τ_ij(t)+Δτ_ij(t) (1-5)

其中，ρ表示信息素挥发系数，(1-ρ)则表示路径信息素残留因子，

表示第k只蚂蚁在此轮迭代过程中在路径(i,j)上留下的信息素，公式如下：

其中，Q为信息素强度为固定常数，L_k表示蚂蚁k在本轮迭代中所走过的路径总长度，

为规划路径拐角的大小，机器人移动过程中左右拐角大小一致都是0°，(0,45°]，(45°,90°]，(90°,180°]，

为拐角评价函数，ξ为拐角影响系数。

4.根据权利要求1所述的一种移动机器人智能路径规划方法，其特征在于，步骤5建立机器人运动轨迹，预测障碍物轨迹，假设机器人不能全向移动，即不能有横向的速度，只有前进的线速度和旋转角速度即(v,ω)，机器人运动方程f(x,y,θ)如下：

设动态障碍物的运动模型为f₀(x₀,y₀)：

将方程f(x,y,θ)和f₀(x₀,y₀)联立求解，如果方程组无解则表明两个方程没有交集，即当前路径不会存在危险，机器人不会与动态障碍物发生碰撞；若方程组有解则表明机器人和动态障碍物轨迹有交集(x_i,y_i,t_i)(i＝0,1,2…)，即在t＝t_i(i＝0,1,2…)时刻机器人将会与动态障碍物发生碰撞，按照当前路径行驶存在危险，需要进行动态避障。

5.根据权利要求1所述的一种移动机器人智能路径规划方法，其特征在于，步骤5建立机器人速度约束函数：

1)机器人速度限制，机器人最大和最小速度的限制：

V_m＝{v∈[v_min,v_max],ω∈[ω_min,ω_max]} (1-11)

其中，v_m ⁱ _n、v_max分别是最小、最大线速度；ω_min、ω_max分别是最小、最大角速度；

2)加速度限制，机器人的由于电机驱动性能限制，所能达到的最大与最小加速度，在Δt时间内最大，最小线速度和最大，最小角速度共同组合生成一个速度动态窗口V_d：

其中，v_c、ω_c为当前线速度和角速度，

为最小线加速度和最大线加速度，

为最小角加速度和最大角加速度；

3)安全距离限制，机器人能够在最大减速的条件下，在撞到障碍物前停下来的速度约束V_safe：

其中，dist(v,ω)为机器人速度(v,ω)对应轨迹上离障碍物最近的距离；

满足以上约束条件的机器人速度空间为V：

V＝V_m∩V_d∩V_safe (1-14)。

Images