路径规划——Jump Point Search算法

路径规划——Jump Point Search算法

算法原理

跳点搜索算法(Jump Point Search)，简称JPS，是由澳大利亚两位教授于2011年提出的基于Grid格子的寻路算法。JPS算法在保留A Star算法的框架的同时，进一步优化了A Star算法寻找后继节点的操作。
A Star算法可见：A Star

JPS与A Star算法的主要区别在后继节点的拓展策略上，不同于A Star算法中直接获取当前节点所有非已访问过的可达邻居节点来进行拓展的策略，JPS根据当前节点的方向，并基于搜索跳点的策略来拓展后继节点，遵循“两个定义、三个规则”。

定义一，强迫邻居(forced neighbor)：
如果节点n是x的邻居，并且节点n的邻居有障碍物，并且从parent(x)经过x再到n的路径长度比其他任何从parent(x)不经过x到n的路径短，其中parent(x)为路径中x的前一个点，则n为x的强迫邻居，x为n的跳点。

举例来具体说，如上图
先看直线方向(straight)，x的父节点是4，如果从4经过x到达节点node与从4不经过到达节点node相比，前者路径较长则没必要加入到x的邻居节点(如节点1、2、3、6、7、8)，其中节点3与8两者路径是相等的，这时路径具有对称性，而JPS的核心便是打破这种对称，所以节点3和8也是不需要加入的，这些节点称为inferior neighbors劣性节点；前者路径较短则需要将节点node加入到x的邻居节点(如节点5)，这种节点称为natural neighbors自然节点；
再来看对角线方向(diagonal)，x的父节点是6，同理，劣性节点有1、2、7、8，自然节点有2、3、5；

如果有障碍物，如下图

当原先的节点2处有障碍物时，从父节点4经过x到达节点3比不经过x到达节点3路径更短，这时节点3便不是inferior neighbor了，需要作为x的有效邻居节点进一步考虑，这种节点就是forced neighbor强迫邻居了。同样，对角线方向移动时，原先的节点4处有障碍物时，从父节点6经过x到达节点1比不经过节点x到达节点1路径更短，这时节点1便是forced neighbor加入到openlist了。

定义二，跳点(Jump Point)：
1.如果点y是起点或者目标点，则y是跳点；
2.如果y有强迫邻居则y是跳点；
3.如果parent(y)到y是对角移动，并且y经过水平或垂直方向移动可以到达跳点，则y是跳点。

先看Jumping Straight，从p(x)到x，向左一直推进直到y，节点y有一个forced neigbor节点z，那么节点y便作为跳点加入到openlist，此处对应于定义2。
再看Jumping Diagonally，从p(x)沿着对角方向到x，然后节点x沿着水平方向和垂直方向搜索，均没有发现感兴趣的节点，于是继续沿着对角方向推进，一直到节点y，节点y沿着水平方向搜索时发现了感兴趣节点z，节点z有forced neighbor节点，那么节点y便作为跳点成为了节点x的后继节点，此处对应于定义3，被加入到openlist中，节点z被标记为特殊点，之后返回到y，并沿着垂直方向搜索，垂直方向上无特殊点，那么节点x的这个方向上的工作便结束了，之后沿着另一个对角方向搜索，有forced neighbor节点w，那么节点w也作为跳点被加入到openlist中，此处对应于定义2。

**规则一：**JPS搜索跳点的过程中，如果直线方向(为了和对角线区分，直线方向代表水平方向和垂直方向，且不包括对角线等斜线方向)和对角线方向都可以移动，则首先在直线方向搜索跳点，再在对角线方向搜索跳点。

**规则二：**1.如果从parent(x)到x是直线移动，n是x的邻居，若有从parent(x)到n的路径不经过x且路径长度小于或等于从parent(x)经过x到n的路径，则走到x后下一个点不会走到n；
2.如果从parent(x)到x是对角线移动，n是x的邻居，若有从parent(x)到n的路径不经过x且路径长度小于从parent(x)经过x到n的路径，则走到x后下一个点不会走到n。

**规则三：**只有跳点才会加入openlist，因为跳点会改变行走方向，而非跳点不会改变行走方向，最后寻找出来的路径点也都是跳点。

算法流程:

首先检查当前节点是否有强迫邻居，如果有，那么此节点便可以作为跳点加入到openlist中，如果没有，那么遵循三个规则沿着水平方向、垂直方向和对角方向寻找跳点。

算法实现

"""
    Filename: jps.py
    Description: Plan path using Jump-Point-Search Algorithm
    Author: Benxiaogu:https://github.com/Benxiaogu
    Date: 2024-08-18
"""
import heapq
import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation


class JPS:
    def __init__(self,grid,start,goal,board_size):
        self.grid = grid
        self.start = self.Node(start,0,0)
        self.goal = self.Node(goal,0,0)
        self.board_size = board_size
        self.path = []

    class Node:
        def __init__(self, position, g, h, parent=None):
            self.position = position    # position of node
            self.g = g                  # distance from node to start
            self.h = h                  # heuristic value from node to goal
            self.parent = parent        # parent node

        def __eq__(self, other) -> bool:
            return self.position == other.position
        
        def __lt__(self, other):
            # Prioritise nodes based on heuristic value
            return self.g+self.h < other.g+other.h or (self.g+self.h==other.g+other.h and self.h<other.h)

    def plan(self):
        open = []
        closed = []
        self.searched = [] # Used to record nodes that are searched

        nexts = [(-1,0),(0,1),(0,-1),(1,0),(-1,1),(1,1),(1,-1),(-1,-1)]

        self.start.h = self.heuristic(self.start.position)
        heapq.heappush(open, self.start)

        while open:
            # Select the node closest to the start node
            current_node = heapq.heappop(open)
            if current_node.position in closed:
                continue

            # self.searched.append(current_node.position)
            closed.append(current_node.position)
            
            # Find the goal
            if current_node == self.goal:
                self.goal = current_node
                while current_node:
                    self.path.append(current_node.position)
                    current_node = current_node.parent
                self.path = self.path[::-1]

                return self.goal.g

            for next in nexts:
                jumppoint = self.Jumping(current_node.position,next)
                if jumppoint and jumppoint not in closed:
                    h = self.heuristic(jumppoint)
                    g = current_node.g+self.calculate_cost(current_node.position, jumppoint)
                    jp_node = self.Node(jumppoint,g,h,current_node)
                    heapq.heappush(open,jp_node)
                    self.searched.append(jumppoint)
                    if jp_node == self.goal:
                        break

        
        return -1
    
    def Jumping(self, node, direction):
        new_node = (node[0]+direction[0], node[1]+direction[1])
        
        if self.grid[new_node[0]][new_node[1]] == 1:
            return None

        if new_node == self.goal.position:
            return new_node

        # Find forced neighbor at horizaontal and vertical direction
        if self.findForcedNeighbor(new_node, direction):
            return new_node
        
        # If current direction of search is diagonal, then search jump point horizaontally or vertically
        if direction[0]!=0 and direction[1]!=0:
            y_dir = (direction[0],0)
            x_dir = (0,direction[1])
            if self.Jumping(new_node,x_dir) or self.Jumping(new_node,y_dir):
                return new_node
        
        return self.Jumping(new_node,direction)

    def findForcedNeighbor(self, node, direction):
        """
            If there be forced neighbor, return Ture otherwise False
        """
        y,x = node

        # vertical
        if direction[0]!=0 and direction[1]==0:
            if (self.grid[y][x+1] != 0 and self.grid[y+direction[0]][x+1] == 0) or \
                (self.grid[y][x-1] != 0 and self.grid[y+direction[0]][x-1] == 0):
                return True
        
        # horizontal
        if direction[0]==0 and direction[1]!=0:
            if (self.grid[y+1][x] != 0 and self.grid[y+1][x+direction[1]] == 0) or \
                (self.grid[y-1][x] != 0 and self.grid[y-1][x+direction[1]] == 0):
                return True
        
        # diagonal
        if direction[0]!=0 and direction[1]!=0:
            if (self.grid[y-direction[0]][x] != 0 and self.grid[y-direction[0]][x+direction[1]] == 0) or \
                (self.grid[y][x-direction[1]] != 0 and self.grid[y+direction[0]][x-direction[1]] == 0):
                return True
        
        return False

    # def heuristic(self, node):
    #     # Manhattan distance from current node to goal node
    #     return abs(node[0] - self.goal.position[0]) + abs(node[1] - self.goal.position[1])
    
    # def heuristic(self, node):
    #     # Chebyshev Distance
    #     D = 1
    #     D2 = math.sqrt(2)
    #     dx = abs(node[0] - self.goal.position[0])
    #     dy = abs(node[1] - self.goal.position[1])
    #     return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)
    
    def heuristic(self, node):
        # Euclidean Distance
        D = 1
        dy = abs(node[0] - self.goal.position[0])
        dx = abs(node[1] - self.goal.position[1])
        return D * math.sqrt(dx * dx + dy * dy)
    
    def calculate_cost(self, start, end):
        # The distance from the current node to the next jump point
        return math.sqrt((start[0] - end[0]) ** 2 + (start[1] - end[1]) ** 2)
        

结果如下：

完整Python及C++代码：JPS