CN115755908B - 一种基于JPS引导Hybrid A*的移动机器人路径规划方法 - Google Patents

Abstract

本发明公开了一种基于JPS引导HybridA*的移动机器人路径规划方法，在经典HybridA*算法的基础上利用JPS算法的扩展特点进行优化，在二维网格下考虑h_{collision_avoidance1}启发值时，用JPS算法代替A*算法进行长度计算；在计算g_extend函数值中加入p_bias惩罚项，用JPS算法遍历获得引导路径点集合。本发明方法相比于经典Hybrid A*算法，在遍历速度，迭代次数及路径长度上提升效果显著。

Description

一种基于JPS引导Hybrid A*的移动机器人路径规划方法

技术领域

本发明涉及移动机器人路径规划领域，具体是一种基于JPS引导Hybrid A*的移动机器人路径规划方法。

背景技术

路径规划的研究是移动机器人研究领域中一个重要的组成部分，它的目的是使移动机器人能够在一个已知或者未知的环境中，找到一条从起始状态到目标状态的无碰撞路径。传统的路径规划算法大部分只考虑机器人的位姿空间，然而，实际上机器人不仅受到位姿空间的约束，还会受到各种外力的约束。路径规划技术作为移动机器人的导航技术甚至是整个移动机器人技术的重要核心技术，也更加受到广大研究学者的关注。

传统的移动机器人路径规划算法大多数是将地图栅格化之后，使用图搜索算法例如A*算法或Dijkstra算法进行路径规划，这些算法在生成路径时并未考虑机器人运动学约束，因此规划出的路径大多是离散的，不满足机器人非完整性约束；而Hybrid A*算法作为A*算法的拓展，在算法中加入了机器人运动学约束条件并将离散的拓展点修改为连续的拓展点，这样便可以使机器人得到一条连续可执行的路径。

但是当Hybrid A*算法在宽敞的自由空间区域拓展且终点位于狭窄区域中时，会在列表中加入大量的备选节点，只有少部分节点可以引导路径进入狭窄区域，从而导致迭代次数增加，甚至无法找到可行路径。

发明内容

发明目的：针对上述现有技术，提出一种基于JPS引导Hybrid A*的移动机器人路径规划方法，在大规模地图下，提高节点遍历速度，减少迭代次数以及提高输出路径长度。

技术方案：一种基于JPS引导Hybrid A*的移动机器人路径规划方法，包括如下步骤：

步骤1：全局地图初始化；其方法为：进行环境建模并建立二维坐标系，对环境中的所有障碍物进行离散化并用均匀分布的点集合来表示，设障碍物数量为k，组成障碍物集合为{OBS_i}，即OBS₁，i＝1,…,k；

步骤2：参数初始化；其方法为：在地图中设置初始位姿Start(x_init,y_init,θ_init)，终点位姿Goal(x_end,y_end,θ_end)，构建节点采样函数Expansion_pattern、待探索列表Openlist、已探索列表Closedlist，以及移动机器人运动学模型；其中，(x_*,y_*)为移动机器人运动学模型的质心在*条件下绝对坐标系下的坐标，θ_*表示在*条件下移动机器人航向角；

步骤3：从待探索列表Openlist中选择代价值最小的节点作为当前节点，通过前向模拟寻找下一个扩展结点，判断扩展节点与终点的距离是否小于阈值，若是，则继续步骤6，否则继续步骤4；

步骤4：计算起始点到扩展结点的代价值g_extend以及扩展结点位姿到终点位姿并同时考虑移动机器人运动学约束及避障条件的启发值h_extend，并计算扩展节点的总代价值f_extend；

步骤5：将扩展结点与其总代价值f_extend放入待探索列表Openlist中，返回步骤3继续；

步骤6：使用Reeds-Sheep曲线将扩展结点与终点连接，同时判断曲线上是否有障碍物，若是，则返回步骤3继续；否则，返回路径点集合；

步骤7：对路径点集合进行重采样，输出优化路径。

进一步的，所述步骤2中，构建节点采样函数Expansion_pattern的具体过程为：

算法先在φ∈{-φ_max,0,φ_max}，v∈{-v_max,v_max}的范围内进行均匀采样获得采样值，然后在单位时间内按照采样值[v,φ]前向模拟移动机器人前进过程来判断该采样值是否符合碰撞条件；其中，v为前行速度，φ为移动机器人前轮偏转角，φ_max为移动机器人前轮的最大偏转角，v_max为最大前行速度。

进一步的，所述步骤3中，前向模拟寻找扩展结点的具体过程为：

通过前向模拟计算公式扩展当前节点，并用Reeds-Sheep曲线将节点相连；当前节点位姿为N_current(x_current,y_current,θ_current)，扩展节点位姿为N_expend(x_expend,y_expend,θ_expend)，前向模拟计算公式如下所示：

x_expend＝x_current+cosθ_currentvdt

y_expend＝y_current+cosθ_currentvdt

式中，L_w为移动机器人运动学模型前后轴的距离；N为常数。

进一步的，所述步骤4中，计算扩展结点的总代价值f_extend的具体过程为：

f_extend＝g_extend+h_extend

其中：

g_extend＝g_current+C_simu+p_bias+p_{v_change}+p_{v_reverse}+p_{phy_change}

式中，g_current为起始点扩展到当前节点的代价值，C_simu为每次扩展子节点时叠加的前向模拟代价值，p_bias为扩展结点偏离引导路径的惩罚，p_{v_change}为移动机器人前行速度v改变时引入的惩罚，p_{v_reverse}为移动机器人前行速度v方向改变时引入的惩罚，p_{phy_change}为改变移动机器人前轮偏转角φ时引入的惩罚；

h_extend＝β*max(h_{nonholonomics},max(h_{collision_avoidance1},h_{collision_avoidance2}))

式中，β为启发值的权重系数，h_{nonholonomics}为使用Reeds-Sheep曲线在不考虑避障条件下估算从扩展结点位姿到终点位姿的启发值，h_{collision_avoidance1}为JPS算法在二维网格下从扩展结点到终点的启发值，h_{collision_avoidance2}为扩展结点到终点的曼哈顿距离。

进一步的，所述步骤3中前向模拟以及步骤6中判断曲线上是否有障碍物中均采用了碰撞检测方法，所述碰撞检测方法中将每个路径点作为移动机器人后轴轴心，通过计算移动机器人四个顶点坐标构建简单运动学模型来进行边界碰撞检测。

有益效果：(1)本发明的一种基于JPS引导Hybrid A*的移动机器人路径规划方法作为一种Hybrid A*的改进算法，结合了JPS算法本身在节点遍历速度和数量上的一定优势，使得JPS算法的生成路径会比A*算法的生成路径在转角时会更加贴合障碍物且输出路径更精简但不发生碰撞，因此改进算法在大规模地图下，节点遍历速度，迭代次数以及输出路径长度等指标提升效果显著。

(2)Hybrid A*算法在宽敞的自由空间区域拓展且终点位于狭窄区域中时，由于其遍历特点会导致迭代次数增加，甚至无法找到可行路径。JPS算法因为强迫领居的存在，对障碍物较多的狭窄区域更为敏感，因此到达终点所需的迭代次数相比于A*算法大大减少而本发明采用JPS算法代替A*算法计算考虑避障时的启发值，以及采用JPS算法获得引导路径点集合，有效避免了无关节点的扩展，快速生成有效路径。

附图说明

图1是本方法的流程图；

图2是JPS算法原理示意图；

图3是移动机器人运动学模型示意图；

图4是初始化及参数设置地图；

图5、图6是本发明的算法仿真结果示意图。

具体实施方式

下面结合附图对本发明做更进一步的解释。

参照附图1所示，本实施例的一种基于JPS引导Hybrid A*的移动机器人路径规划方法，主要包括如下步骤：

步骤1：全局地图初始化。其方法为：进行环境建模并建立二维坐标系，对环境中的所有障碍物进行离散化并用均匀分布的点集合来表示，假设障碍物数量为k，组成障碍物集合为{OBS_i}，即OBS₁，i＝1,…,k。

步骤2：参数初始化。其方法为：在地图中设置初始位姿Start(x_init,y_init,θ_init)，终点位姿Goal(x_end,y_end,θ_end)，构建节点采样函数Expansion_pattern，待探索列表Openlist，已探索列表Closedlist，以及移动机器人运动学模型；其中，(x_*,y_*)为移动机器人运动学模型的质心在*条件下绝对坐标系下的坐标，θ_*表示在*条件下移动机器人航向角，此处*条件为初始位姿init以及终点位姿end。

本步骤中，构建节点采样函数Expansion_pattern的具体过程为：算法先在φ∈{-φ_max,0,φ_max}，v∈{-v_max,v_max}的范围内进行均匀采样获得采样值，然后在单位时间内按照采样值[v,φ]前向模拟移动机器人前进过程来判断该采样值是否符合碰撞条件；其中，v为前行速度，φ为移动机器人前轮偏转角，φ_max为移动机器人前轮的最大偏转角，v_max为最大前行速度。

步骤3：从待探索列表Openlist中选择代价值f_extend最小的节点作为当前节点，通过前向模拟寻找下一个扩展结点，判断扩展节点与终点的距离是否小于阈值，若是，则继续步骤6，否则继续步骤4。

本步骤中，通过前向模拟寻找扩展结点的具体过程为：通过前向模拟计算公式扩展当前节点，并用Reeds-Sheep曲线将节点相连。当前节点位姿为N_current(x_current,y_current,θ_current)，扩展节点位姿为N_expend(x_expend,y_expend,θ_expend)，前向模拟计算公式如下所示：

x_expend＝x_current+cosθ_currentvdt

y_expend＝y_current+cosθ_currentvdt

式中，L_w为移动机器人运动学模型前后轴的距离，N为常数，将单位时间均分。

步骤4：计算起始点到扩展结点的代价值g_extend以及扩展结点位姿到终点位姿并同时考虑移动机器人运动学约束及避障条件的启发值h_extend，并计算扩展节点的总代价值f_extend。

本步骤中，计算扩展结点的总代价值f_extend的具体过程为：

f_extend＝g_extend+h_extend

其中：

g_extend＝g_current+C_simu+p_bias+p_{v_change}+p_{v_reverse}+p_{phy_change}

式中，g_current为起始点扩展到当前节点的代价值，C_simu为每次扩展子节点时叠加的前向模拟代价值，p_bias为扩展结点偏离引导路径的惩罚，p_{v_change}为移动机器人前行速度v改变时引入的惩罚，p_{v_reverse}为移动机器人前行速度v方向改变时引入的惩罚，p_{phy_change}为改变移动机器人前轮偏转角φ时引入的惩罚。

h_extend＝β*max(h_{nonholonomics},max(h_{collision_avoidance1},h_{collision_avoidance2}))

式中，β为启发值的权重系数，h_{nonholonomics}为使用Reeds-Sheep曲线在不考虑避障条件下估算从扩展结点位姿到终点位姿的启发值，h_{collision_avoidance1}为JPS算法在二维网格下从扩展结点到终点的启发值，h_{collision_avoidance2}为扩展结点到终点的曼哈顿距离。取最大值意味着两类启发值中更大的一项将被用于体现从扩展结点行驶至终点的困难程度。

步骤5：将扩展结点与其代价值f_extend放入待探索列表Openlist列表中，返回步骤3继续。

步骤6：使用Reeds-Sheep曲线将扩展结点与终点连接，同时判断曲线上是否有障碍物，若是，则返回步骤3继续；否则，返回路径点集合。

步骤7：对路径点集合进行重采样,输出优化路径。

本方法中，步骤3中前向模拟以及步骤6中判断曲线上是否有障碍物中均采用了碰撞检测方法，考虑到算法本身基于移动机器人运动学模型进行路径规划，因此将每个路径点作为移动机器人后轴轴心，通过计算移动机器人四个顶点坐标构建简单运动学模型来进行边界碰撞检测。

图2是JPS算法的一个简易原理图，图3是在绝对坐标系下建立的移动机器人运动学模型，在算法中用简易的长方形代替，运行时仅用移动机器人质心代替模型，图4是算法规划前的初始环境以及起点和终点位置，图5和图6分别是在两个不同的地图中本发明算法的仿真结果。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (2)

1.一种基于JPS引导HybridA*的移动机器人路径规划方法，其特征在于，包括如下步骤：

步骤1：全局地图初始化；其方法为：进行环境建模并建立二维坐标系，对环境中的所有障碍物进行离散化并用均匀分布的点集合来表示，设障碍物数量为k，组成障碍物集合为{OBS_i}，即i＝1,…,k；

步骤2：参数初始化；其方法为：在地图中设置初始位姿Start(x_init,y_init,θ_init)，终点位姿Goal(x_end,y_end,θ_end)，构建节点采样函数Expansion_pattern、待探索列表Openlist、已探索列表Closedlist，以及移动机器人运动学模型；其中，(x_*,y_*)为移动机器人运动学模型的质心在*条件下绝对坐标系下的坐标，θ_*表示在*条件下移动机器人航向角；

步骤3：从待探索列表Openlist中选择代价值最小的节点作为当前节点，通过前向模拟寻找下一个扩展结点，判断扩展节点与终点的距离是否小于阈值，若是，则继续步骤6，否则继续步骤4；

步骤4：计算起始点到扩展结点的代价值g_extend以及扩展结点位姿到终点位姿并同时考虑移动机器人运动学约束及避障条件的启发值h_extend，并计算扩展节点的总代价值f_extend；

步骤5：将扩展结点与其总代价值f_extend放入待探索列表Openlist中，返回步骤3继续；

步骤6：使用Reeds-Sheep曲线将扩展结点与终点连接，同时判断曲线上是否有障碍物，若是，则返回步骤3继续；否则，返回路径点集合；

步骤7：对路径点集合进行重采样，输出优化路径；

所述步骤2中，构建节点采样函数Expansion_pattern的具体过程为：

算法先在φ∈{-φ_max,0,φ_max}，v∈{-v_max,v_max}的范围内进行均匀采样获得采样值，然后在单位时间内按照采样值[v,φ]前向模拟移动机器人前进过程来判断该采样值是否符合碰撞条件；其中，v为前行速度，φ为移动机器人前轮偏转角，φ_max为移动机器人前轮的最大偏转角，v_max为最大前行速度；

所述步骤3中，前向模拟寻找扩展结点的具体过程为：

通过前向模拟计算公式扩展当前节点，并用Reeds-Sheep曲线将节点相连；当前节点位姿为N_current(x_current,y_current,θ_current)，扩展节点位姿为N_expend(x_expend,y_expend,θ_expend)，前向模拟计算公式如下所示：

x_expend＝x_current+cosθ_currentvdt

y_expend＝y_current+cosθ_currentvdt

式中，L_w为移动机器人运动学模型前后轴的距离；N为常数；

所述步骤4中，计算扩展结点的总代价值f_extend的具体过程为：

f_extend＝g_extend+h_extend

其中：

g_extend＝g_current+C_simu+p_bias+p_{v_change}+p_{v_reverse}+p_{phy_change}

式中，g_current为起始点扩展到当前节点的代价值，C_simu为每次扩展子节点时叠加的前向模拟代价值，p_bias为扩展结点偏离引导路径的惩罚，p_{v_change}为移动机器人前行速度v改变时引入的惩罚，p_{v_reverse}为移动机器人前行速度v方向改变时引入的惩罚，p_{phy_change}为改变移动机器人前轮偏转角φ时引入的惩罚；

h_extend＝β*max(h_{nonholonomics},max(h_{collision_avoidance1},h_{collision_avoidance2}))

式中，β为启发值的权重系数，h_{nonholonomics}为使用Reeds-Sheep曲线在不考虑避障条件下估算从扩展结点位姿到终点位姿的启发值，h_{collision_avoidance1}为JPS算法在二维网格下从扩展结点到终点的启发值，h_{collision_avoidance2}为扩展结点到终点的曼哈顿距离。

2.根据权利要求1所述的基于JPS引导HybridA*的移动机器人路径规划方法，其特征在于：所述步骤3中前向模拟以及步骤6中判断曲线上是否有障碍物中均采用了碰撞检测方法，所述碰撞检测方法中将每个路径点作为移动机器人后轴轴心，通过计算移动机器人四个顶点坐标构建简单运动学模型来进行边界碰撞检测。