路径规划 | 图解跳点搜索JPS算法(附ROS C++/Python/Matlab仿真)

0 专栏介绍

附C++/Python/Matlab全套代码课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 A*算法的弊端

A*算法是非常有效且常用的路径规划算法之一，其是结合Dijsktra算法与GBFS各自优势的启发式搜索算法，其具体原理请参考路径规划 | 图解A*、Dijkstra、GBFS算法的异同(附C++/Python/Matlab仿真)。然而，A*算法有个明显的缺陷——拓展了太多的无用节点

例： 如图所示，从起点$s$到终点$g$有三条可行路线：

• $L_1=\left\{\left(1,2\right)\to \left(2,3\right)\to \left(3,2\right)\right\}$
• $L_2=\left\{\left(1,2\right)\to \left(2,2\right)\to \left(3,2\right)\right\}$
• $L_3=\left\{\left(1,2\right)\to \left(2,1\right)\to \left(3,2\right)\right\}$

直观地，路径$L_1$、$L_3$的探索对最优路径的生成时没有意义的，但传统A*算法仍会在扩展邻域时评估这些不必要的节点，例如图中的灰色栅格，造成其应用时存在内存消耗大、运行时间长等缺陷

2 跳点搜索算法

2.1 自然与强制邻点

针对A*算法的缺陷，Harabor等提出跳点搜索算法(Jump Point Search, JPS)，筛选出有价值的节点——称为跳点而剪去那些无意义的冗余节点。

设$n$是节点$x$的邻接点，$p$是节点$x$的父节点，节点$x$的邻域采用八邻域法，则跳点剪枝策略中的基本概念如下：

• 自然邻点：当节点$x$邻域不存在障碍时(暂时移除障碍)，若从$p$出发任意不经过节点$x$到达$n$的路径$<p,\cdots ,n|x>$比经过$x$到达$n$的路径$<p,\cdots ,x,\cdots ,n>$长，即
  $$\{\begin{array}{l}L\left(<p,\cdots ,n|x>\right)⩾L\left(<p,\cdots ,x,\cdots ,n>\right),p\text{是}x\text{的对角节点}\\ L\left(<p,\cdots ,n|x>\right)>L\left(<p,\cdots ,x,\cdots ,n>\right),\mathrm{otherwise}\end{array}$$
  恒成立，则称邻接点$n$是节点$x$在父节点是$p$时的自然邻点

• 强制邻点：当节点$x$的邻域存在障碍时，若从$p$出发任意不经过节点$x$到达$n$的路径$<p,\cdots ,n|x>$比经过节点$x$到达$n$的路径$<p,\cdots ,x,\cdots ,n>$长，且节点$n$不是节点$x$的自然邻点，则称邻接点$n$是节点$x$在父节点是$p$时的强制邻点，如图所示为水平搜索与斜搜索情况下的八种强制邻点情况。

2.2 跳点剪枝策略

跳点本质上是搜索方向可能发生改变的点，其筛选规则为

• 若节点$x$是终点，则节点$x$是跳点；
• 若节点$x$存在至少一个强制邻点，则节点$x$是跳点；
• 在斜向搜索时，若节点$x$在水平或垂直分量上有跳点，则节点$x$是跳点

用一个实例说明。如图所示的栅格地图中，绿色是起点，红色是终点，黑色是障碍物

我们首先用A*算法看看规划的路径，其中绿色是开节点表中的节点，蓝色是闭节点表中的节点，黄色是规划路径

再用JPS算法执行路径规划，灰色的是在搜索过程中被跳过的点，而绿色节点就是跳点，实际应用于算法搜索中的节点(绿色和蓝色)，与A*算法相比大幅度减少。

3 算法仿真与实现

3.1 算法流程

JPS算法的核心就是jump()函数，我采用递归方法来实现，使得整体代码量下降，且逻辑更清晰明确。总的思路就是实现跳点剪枝的三条策略

• 若节点$x$是终点，则节点$x$是跳点；
• 若节点$x$存在至少一个强制邻点，则节点$x$是跳点；
• 在斜向搜索时，若节点$x$在水平或垂直分量上有跳点，则节点$x$是跳点

跳点剪枝策略就是通过筛选跳点作为探索域，而忽略对搜索路径没有实质影响的冗余节点，实现大幅度降低计算复杂度的效果。JPS算法最大的优势是在障碍密集的情况下，相较传统A*算法的计算速度一般有量级的提升；缺陷是其不能应用于广义图搜索，只适用于栅格地图。

3.2 ROS C++实现

Node JumpPointSearch::jump(const Node& point, const Node& motion)
{
  Node new_point = point + motion;
  new_point.id_ = this->grid2Index(new_point.x_, new_point.y_);
  new_point.pid_ = point.id_;
  new_point.h_ = std::sqrt(std::pow(new_point.x_ - this->goal_.x_, 2) + std::pow(new_point.y_ - this->goal_.y_, 2));

  // next node hit the boundary or obstacle
  if (new_point.id_ < 0 || new_point.id_ >= this->ns_ || this->costs_[new_point.id_] >= this->lethal_cost_ * this->factor_)
    return Node(-1, -1, -1, -1, -1, -1);

  // goal found
  if (new_point == this->goal_)
    return new_point;

  // diagonal
  if (motion.x_ && motion.y_)
  {
    // if exists jump point at horizontal or vertical
    Node x_dir = Node(motion.x_, 0, 1, 0, 0, 0);
    Node y_dir = Node(0, motion.y_, 1, 0, 0, 0);
    if (this->jump(new_point, x_dir).id_ != -1 || this->jump(new_point, y_dir).id_ != -1)
      return new_point;
  }

  // exists forced neighbor
  if (this->detectForceNeighbor(new_point, motion))
    return new_point;
  else
    return this->jump(new_point, motion);
}

3.3 Python实现

def jump(self, node: Node, motion: Node):
    # explore a new node
    new_node = node + motion
    new_node.parent = node.current
    new_node.h = self.h(new_node, self.goal)

    # hit the obstacle
    if new_node.current in self.obstacles:
        return None

    # goal found
    if new_node == self.goal:
        return new_node

    # diagonal
    if motion.current[0] and motion.current[1]:
        # if exists jump point at horizontal or vertical
        x_dir = Node((motion.current[0], 0), None, 1, None)
        y_dir = Node((0, motion.current[1]), None, 1, None)
        if self.jump(new_node, x_dir) or self.jump(new_node, y_dir):
            return new_node
        
    # if exists forced neighbor
    if self.detectForceNeighbor(new_node, motion):
        return new_node
    else:
        return self.jump(new_node, motion)

3.4 Matlab实现

function [new_node, flag] = jump(cur_node, motion, goal, map)
    flag = false;

    % explore a new node
    new_node = [cur_node(1) + motion(1), ...
                      cur_node(2) + motion(2), ...
                      cur_node(3) + motion(3), ...
                      0, cur_node(1), cur_node(2)
                      ];
    new_node(4) = h(new_node(1:2), goal);

    % obstacle
    if new_node(1) <= 0 || new_node(2) <= 0 || map(new_node(1), new_node(2)) == 2
        return
    end

    % goal found
    if new_node(1) == goal(1) && new_node(2) == goal(2)
        flag = true;
        return
    end

    % diagonal
    if motion(1) && motion(2)
        % if exists jump point at horizontal or vertical
        x_dir = [motion(1), 0, 1];
        y_dir = [0, motion(2), 1];
        [~, flag_x] = jump(new_node, x_dir, goal, map);
        [~, flag_y] = jump(new_node, y_dir, goal, map);
        if  flag_x || flag_y 
            flag = true;
            return
        end
    end
            
    % if exists forced neighbor
    if detect_force(new_node, motion, map)
        flag = true;
        return
    else
        [new_node, flag] = jump(new_node, motion, goal, map);
        return
    end
end

完整工程代码请联系下方博主名片获取

---

更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系