CN112731916A - 融合跳点搜索法和动态窗口法的全局动态路径规划方法 - Google Patents

Abstract

本发明涉及一种融合跳点搜索法和动态窗口法的全局路径规划方法，包括以下步骤：步骤S1：初始化起点，并建立机器人的运动模型；步骤S2:根据外部环境，在速度空间中采样多组速度并预设约束;步骤S3:基于运动模型以及采样到的速度模拟出一系列轨迹;步骤S4:搜索最优路径点；步骤S5:构建全局最优的评价函数，并基于该评价函数对模拟的轨迹进行评价，选择出一条全局最优的轨迹；步骤S5:将得到的最优轨迹的相关参数信息发送给机器人，使之沿着最优路径移动；步骤S6:判断是否到达目标点，如果是，则路径规划成功，得到最优路径，如果否，则返回至步骤S2,重复步骤S2‑S6直至到达目标点。本发明能够保证搜索到的路径具有平滑性和实时避障能力。

Description

融合跳点搜索法和动态窗口法的全局动态路径规划方法

技术领域

本发明涉及路径规划领域，具体涉及一种融合跳点搜索法和动态窗口法的全局路径规划方法。

背景技术

路径规划是自主移动机器人领域的关键技术之一，针对这一技术目前已经开展了大量的研究工作。自主移动机器人的路径规划可以分为静态路径规划和动态路径规划。其中A*算法、Dijkstra算法等目前主要应用于已知的静态场景中，而人工势场法、D*算法、动态窗口法等则大多应用于环境未知动态场景中。同时，随着近几年人工智能的迅速发展，蚁群算法、遗传算法、神经网络算法等智能算法也开始广泛的应用于移动机器人的路径规划中。

发明内容

有鉴于此，本发明的目的在于提供一种融合跳点搜索法和动态窗口法的全局路径规划方法，

为实现上述目的，本发明采用如下技术方案：

一种融合跳点搜索法和动态窗口法的全局路径规划方法，包括以下步骤：

步骤S1：初始化起点，根据机器人的线速度、角速度的变化，计算并建立机器人的运动模型；

步骤S2:根据机器人自身及所处的外部环境，在速度空间中采样多组速度(v_t,ω_t)并预设约束；

步骤S3:基于运动模型以及采样到的速度模拟出一系列轨迹；

步骤S4:基于跳点搜索法搜索全局最优路径点；

步骤S5:基于全局最优路径点与动态窗口法评价函数构建全局最优的评价函数，并基于该评价函数对模拟的轨迹进行评价，选择出一条全局最优的轨迹；

步骤S6:将得到的最优轨迹的相关参数信息发送给机器人，使之沿着最优路径移动；

步骤S7:判断是否到达目标点，如果是，则路径规划成功，得到最优路径，如果否，则返回至步骤S2,重复步骤S2-S6直至到达目标点。

进一步的，所述机器人的运动模型具体为：

x_t+1＝x_t+v_xtΔtcosθ_t-v_ytΔtsinθ_t

y_t+1＝y_t+v_xtΔtsinθ_t+v_ytΔtcosθ_t

θ_t+1＝θ_t+ωΔt

其中，x_t、y_t和θ_t为机器人在t时刻所处的坐标位置与方位角，机器人在t+1时刻所处的坐标位置与方位角则用x_t+1、y_t+1和θ_t+1来表示，v_xt和v_yt分别为t时刻机器人横向与纵向的速度分量，ω为机器人在t时刻的旋转角速度。

进一步的，所述预设约束包括：

机器人受到自身物理结构的约束所能达到的最大最小速度：

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

其中，V_max和V_min为机器人所能达到的最大、最小的线速度，ω_max和ω_min为机器人所能达到的最大、最小角速度。

由于电机自身存在的物理约束，机器人在模拟运动过程中存在最大加速度下的实际所能达到的最大、最小速度：

V_d＝{(v,ω)|v∈[v_c-v'·t,v_c+v'·t]∧ω∈[ω_c-ω'·t,ω_c+ω'·t]}

其中，t为在加速度v'和角加速度ω'作用下所用的时间，v_c、ω_c表示机器人当前的速度和角速度；

在最大减速度条件下，让机器人在碰到障碍物之前速度减为0，则：

其中，dist(v,ω)表示速度矢量(v,ω)对应轨迹离最近障碍物的距离。

进一步的，所述步骤S4具体为:

步骤1:定义从起始点到达当前点的代价估计函数G(m)和从当前点到达终点的代价估计函数H(m)；

步骤2:根据得到的G(m)和H(m)，构建启发式函数 F(m)＝G(m)+H(m)；

步骤3:分别创建一个开启列表和关闭列表并进行初始化，并将起始点作为跳点添加到开启列表中，开启列表用来存放搜索到的跳点，关闭列表用来存放开启列表中F值最小的节点；

步骤4:检查开启列表是否为空列表，如果是，则算法结束，如果否，则继续执行步骤5；

步骤5:计算开启列表中的各节点的F值，选择F值最小的节点，将其从开启列表中删除，并放入关闭列表中，并将该节点更新为父节点；

步骤6:检查当前放入关闭列表中的节点是否为终点，如果是，则得到由一系列最优跳点形成的全局最优路径点，如果否，则继续执行步骤7；

步骤7:根据父节点的方向沿着水平和竖直方向搜索跳点；

步骤8:判断当前搜索的节点是否到达地图边界或遇到障碍物，如果是，则继续执行步骤9，如果否，则继续跳转至步骤10；

步骤9:根据父节点方向沿着对角线方向搜索跳点；

步骤10：判断当前搜索的节点是否为跳点，如果是，则执行步骤 11，如果否，则忽略该节点，返回到步骤7，重新执步骤7至步骤11；

步骤11：将搜索到的跳点添加到开启列表中，返回至步骤5，循环5至11，直至步骤6中，当前放入关闭列表的节点为终点，则得到全局最优路径点。

进一步的，所述步骤1具体为：

步骤111：采用欧氏距离定义从起始点到达当前点的代价估计函数G(m)：

G(m)＝((M_x-S_x)²+(M_y-S_y)²)^1/2

其中，M_x,M_y为当前点的横、纵坐标，S_x,S_y为起始点的横、纵坐标；

步骤112：综合曼哈顿距离和欧氏距离，定义从当前点到达终点的代价估计函数H(m)：

其中，D_x(m)＝|E_x-M_x|,D_y(m)＝|E_y-M_y|,M_x，M_y为当前点的横、纵坐标，E_x，E_y为终点的横、纵坐标。

进一步的，所述评价函数具体为:

G(v,ω)＝σ(α·PHeading(v,ω)+β·dist(v,ω)+γ·vel(v,ω))

其中，PHeading(v,ω)为当前模拟轨迹方向与当前目标点之间的方位角偏差，当前目标点为机器人在当前运动轨迹上与当前点距离最近的跳点，dist(v,ω)为当前模拟轨迹上与最近障碍物之间的距离，vel(v,ω)为机器人在相应轨迹上的速度大小，α、β、γ为这三者的权重系数，σ是平滑函数。

本发明与现有技术相比具有以下有益效果：

本发明能够保证搜索到的路径具有平滑性和实时避障能力，还在确保全局最优的基础上提高了寻路效率。

附图说明

图1是本发明跳点搜索法流程图。

图2是本发明方法流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

请参照图1，本发明提供一种融合跳点搜索法和动态窗口法的全局路径规划方法，包括以下步骤：

步骤S1：初始化起点，根据机器人的线速度、角速度的变化，计算并建立机器人的运动模型：

x_t+1＝x_t+v_xtΔtcosθ_t-v_ytΔtsinθ_t

y_t+1＝y_t+v_xtΔtsinθ_t+v_ytΔtcosθ_t

θ_t+1＝θ_t+ωΔt

其中，x_t、y_t和θ_t为机器人在t时刻所处的坐标位置与方位角，机器人在t+1时刻所处的坐标位置与方位角则用x_t+1、y_t+1和θ_t+1来表示，v_xt和v_yt分别为t时刻机器人横向与纵向的速度分量，ω为机器人在t时刻的旋转角速度。

步骤S2:根据机器人自身及所处的外部环境，在速度空间中采样多组速度(v_t,ω_t)并预设约束；

步骤S3:基于运动模型以及采样到的速度模拟出一系列轨迹；

步骤S4:基于跳点搜索法搜索全局最优路径点；

步骤S5:基于全局最优路径点与动态窗口法评价函数构建全局最优的评价函数，并基于该评价函数对模拟的轨迹进行评价，选择出一条全局最优的轨迹；

步骤S6:将得到的最优轨迹的相关参数信息发送给机器人，使之沿着最优路径移动；

步骤S7:判断是否到达目标点，如果是，则路径规划成功，得到最优路径，如果否，则返回至步骤S2,重复步骤S2-6直至到达目标点。

在本实施例中，优选的，所述预设约束包括：

机器人受到自身物理结构的约束所能达到的最大最小速度：

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

其中，V_max和V_min为机器人所能达到的最大、最小的线速度，ω_max和ω_min为机器人所能达到的最大、最小角速度。

由于电机自身存在的物理约束，机器人在模拟运动过程中存在最大加速度下的实际所能达到的最大、最小速度：

V_d＝{(v,ω)|v∈[v_c-v'·t,v_c+v'·t]∧ω∈[ω_c-ω'·t,ω_c+ω'·t]}

其中，t为在加速度v'和角加速度ω'作用下所用的时间，v_c、ω_c表示机器人当前的速度和角速度；

在最大减速度条件下，让机器人在碰到障碍物之前速度减为0，则：

其中，dist(v,ω)表示速度矢量(v,ω)对应轨迹离最近障碍物的距离。

在本实施例中，所述步骤S4具体为:

步骤1:采用欧氏距离定义从起始点到达当前点的代价估计函数 G(m)：

G(m)＝((M_x-S_x)²+(M_y-S_y)²)^1/2

其中，M_x,M_y为当前点的横、纵坐标，S_x,S_y为起始点的横、纵坐标；

综合曼哈顿距离和欧氏距离，定义从当前点到达终点的代价估计函数H(m)：

其中，D_x(m)＝|E_x-M_x|,D_y(m)＝|E_y-M_y|,M_x，M_y为当前点的横、纵坐标，E_x，E_y为终点的横、纵坐标。

步骤2:根据得到的G(m)和H(m)，构建启发式函数 F(m)＝G(m)+H(m)；

步骤3:分别创建一个开启列表和关闭列表并进行初始化，并将起始点作为跳点添加到开启列表中，开启列表用来存放搜索到的跳点，关闭列表用来存放开启列表中F值最小的节点；

步骤4:检查开启列表是否为空列表，如果是，则算法结束，如果否，则继续执行步骤5；

步骤5:检查开启列表是否为空列表，如果是，则算法结束，如果否，则继续执行步骤4，计算开启列表中的各节点的F值，选择F值最小的节点，将其从开启列表中删除，并放入关闭列表中，并将该节点更新为父节点；

步骤6:检查当前放入关闭列表中的节点是否为终点，如果是，则得到由一系列最优跳点形成的全局最优路径点，如果否，则继续执行步骤7；

步骤7:根据父节点的方向沿着水平和竖直方向搜索跳点；

步骤8:判断当前搜索的节点是否到达地图边界或遇到障碍物，如果是，则继续执行步骤9，如果否，则继续跳转至步骤10；

步骤9:根据父节点方向沿着对角线方向搜索跳点；

步骤10：判断当前搜索的节点是否为跳点，如果是，则执行步骤 11，如果否，则忽略该节点，返回到步骤7，重新执步骤7至步骤11；

步骤11：将搜索到的跳点添加到开启列表中，返回至步骤5，循环5至11，直至步骤6中，当前放入关闭列表的节点为终点，则得到全局最优路径点。

优选的，在本实施例中，所述评价函数具体为:

G(v,ω)＝σ(α·PHeading(v,ω)+β·dist(v,ω)+γ·vel(v,ω))

其中，PHeading(v,ω)为当前模拟轨迹方向与当前目标点之间的方位角偏差，当前目标点为机器人在当前运动轨迹上与当前点距离最近的跳点，dist(v,ω)为当前模拟轨迹上与最近障碍物之间的距离， vel(v,ω)为机器人在相应轨迹上的速度大小，α、β、γ为这三者的权重系数，σ是平滑函数。

以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (6)

1.一种融合跳点搜索法和动态窗口法的全局路径规划方法，其特征在于，包括以下步骤：

步骤S1：初始化起点，根据机器人的线速度、角速度的变化，计算并建立机器人的运动模型；

步骤S2:根据机器人自身及所处的外部环境，在速度空间中采样多组速度(v_t,ω_t)并预设约束；

步骤S3:基于运动模型以及采样到的速度模拟出一系列轨迹；

步骤S4:基于跳点搜索法搜索全局最优路径点；

步骤S5:基于全局最优路径点与动态窗口法评价函数构建全局最优的评价函数，并基于该评价函数对模拟的轨迹进行评价，选择出一条全局最优的轨迹；

步骤S6:将得到的最优轨迹的相关参数信息发送给机器人，使之沿着最优路径移动；

步骤S7:判断是否到达目标点，如果是，则路径规划成功，得到最优路径，如果否，则返回至步骤S2,重复步骤S2-S6直至到达目标点。

2.根据权利要求1所述的一种融合跳点搜索法和动态窗口法的全局路径规划方法，其特征在于，所述机器人的运动模型具体为：

x_t+1＝x_t+v_xtΔt cosθ_t-v_ytΔt sinθ_t

y_t+1＝y_t+v_xtΔt sinθ_t+v_ytΔt cosθ_t

θ_t+1＝θ_t+ωΔt

其中，x_t、y_t和θ_t为机器人在t时刻所处的坐标位置与方位角，机器人在t+1时刻所处的坐标位置与方位角则用x_t+1、y_t+1和θ_t+1来表示，v_xt和v_yt分别为t时刻机器人横向与纵向的速度分量，ω为机器人在t时刻的旋转角速度。

3.根据权利要求1所述的一种融合跳点搜索法和动态窗口法的全局路径规划方法，其特征在于，所述预设约束包括：

机器人受到自身物理结构的约束所能达到的最大最小速度：

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

其中，V_max和V_min为机器人所能达到的最大、最小的线速度，ω_max和ω_min为机器人所能达到的最大、最小角速度。

由于电机自身存在的物理约束，机器人在模拟运动过程中存在最大加速度下的实际所能达到的最大、最小速度：

V_d＝{(v,ω)|v∈[v_c-v'·t,v_c+v'·t]∧ω∈[ω_c-ω'·t,ω_c+ω'·t]}

其中，t为在加速度v'和角加速度ω'作用下所用的时间，v_c、ω_c表示机器人当前的速度和角速度；

在最大减速度条件下，让机器人在碰到障碍物之前速度减为0，则：

其中，dist(v,ω)表示速度矢量(v,ω)对应轨迹离最近障碍物的距离。

4.根据权利要求1所述的一种融合跳点搜索法的改进A*算法和动态窗口法的全局路径规划方法，其特征在于，所述步骤S4具体为:

步骤1:定义从起始点到达当前点的代价估计函数G(m)和从当前点到达终点的代价估计函数H(m)；

步骤2:根据得到的G(m)和H(m)，构建启发式函数F(m)＝G(m)+H(m)；

步骤3:分别创建一个开启列表和关闭列表并进行初始化，并将起始点作为跳点添加到开启列表中，开启列表用来存放搜索到的跳点，关闭列表用来存放开启列表中F值最小的节点；

步骤4:检查开启列表是否为空列表，如果是，则算法结束，如果否，则继续执行步骤5；

步骤5:计算开启列表中的各节点的F值，选择F值最小的节点，将其从开启列表中删除，并放入关闭列表中，并将该节点更新为父节点；

步骤6:检查当前放入关闭列表中的节点是否为终点，如果是，则得到由一系列最优跳点形成的全局最优路径点，如果否，则继续执行步骤7；

步骤7:根据父节点的方向沿着水平和竖直方向搜索跳点；

步骤8:判断当前搜索的节点是否到达地图边界或遇到障碍物，如果是，则继续执行步骤9，如果否，则继续跳转至步骤10；

步骤9:根据父节点方向沿着对角线方向搜索跳点；

步骤10：判断当前搜索的节点是否为跳点，如果是，则执行步骤11，如果否，则忽略该节点，返回到步骤7，重新执步骤7至步骤11；

步骤11：将搜索到的跳点添加到开启列表中，返回至步骤5，循环5至11，直至步骤6中，当前放入关闭列表的节点为终点，则得到全局最优路径点。

5.根据权利要求4所述的一种融合跳点搜索法和动态窗口法的全局路径规划方法，其特征在于，所述步骤1具体为：

步骤101：采用欧氏距离定义从起始点到达当前点的代价估计函数G(m)：

G(m)＝((M_x-S_x)²+(M_y-S_y)²)^1/2

其中，M_x,M_y为当前点的横、纵坐标，S_x,S_y为起始点的横、纵坐标；

步骤102：综合曼哈顿距离和欧氏距离，定义从当前点到达终点的代价估计函数H(m)：

其中，D_x(m)＝|E_x-M_x|,D_y(m)＝|E_y-M_y|,M_x，M_y为当前点的横、纵坐标，E_x，E_y为终点的横、纵坐标。

6.根据权利要求1所述的一种融合跳点搜索法和动态窗口法的全局路径规划方法，其特征在于，所述评价函数具体为:

G(v,ω)＝σ(α·PHeading(v,ω)+β·dist(v,ω)+γ·vel(v,ω))

其中，PHeading(v,ω)为当前模拟轨迹方向与当前目标点之间的方位角偏差，当前目标点为机器人在当前运动轨迹上与当前点距离最近的跳点，dist(v,ω)为当前模拟轨迹上与最近障碍物之间的距离，vel(v,ω)为机器人在相应轨迹上的速度大小，α、β、γ为这三者的权重系数，σ是平滑函数。

Images