最佳路径优先搜索算法

本来想直接写A* 的,不过看完最佳路径优先搜索算法后觉得还是要先理解一下这个算法后才能更好的理解A* 算法,所以把这篇文章放到A* 前面。

基本概念

最佳优先搜索算法(Best-first-searching)是一种启发式搜索算法(Heuristic Algorithm),其基于广度优先搜索算法,不同点是其依赖于估价函数对将要遍历的节点进行估价,选择代价小的节点进行遍历,直到找到目标点为止。BFS算法不能保证找到的路径是一条最短路径,但是其计算过程相对于Dijkstra算法会快很多。

BFS算法的启发估价函数公式为:

f ( n ) = h ( n ) f(n) = h(n) f(n)=h(n)

n表示当前的点,g(n)为从起始点到点n的实际代价,h(n)为从点n到目标点的估价。

从上述公式我们可以看出,BFS的搜索方式非常简单粗暴,直接从起点开始连接终点,对线上的点进行搜索,遇到障碍物时向四周展开。在没有障碍物的场景下,BFS是一种快速有效的路径搜索方式:
在这里插入图片描述
但是在大部分情况下,我们的场景中肯定会存在不同的障碍物,此时它的搜索路径就不再是最优的了:
在这里插入图片描述

代码实现

import os
import sys
from collections import deque
import matplotlib.pyplot as plt

import math
import heapq

class BFS:
    """BFS add the new visited node in the end of the openset
    """
    def __init__(self, s_start, s_goal, heuristic_type,xI, xG):
        self.s_start = s_start
        self.s_goal = s_goal
        self.heuristic_type = heuristic_type

        self.u_set = [(-1, 0), (-1, 1), (0, 1), (1, 1),
                        (1, 0), (1, -1), (0, -1), (-1, -1)]  # feasible input set
        self.obs = self.obs_map()  # position of obstacles

        self.OPEN = []  # priority queue / OPEN set
        self.CLOSED = []  # CLOSED set / VISITED order
        self.PARENT = dict()  # recorded parent
        self.g = dict()  # cost to come
        self.x_range = 51  # size of background
        self.y_range = 31
        
        self.xI, self.xG = xI, xG
        self.obs = self.obs_map()

    def update_obs(self, obs):
        self.obs = obs

    def obs_map(self):
        """
        Initialize obstacles' positions
        :return: map of obstacles
        """

        x = 51
        y = 31
        obs = set()

        for i in range(x):
            obs.add((i, 0))
        for i in range(x):
            obs.add((i, y - 1))

        for i in range(y):
            obs.add((0, i))
        for i in range(y):
            obs.add((x - 1, i))

        for i in range(10, 21):
            obs.add((i, 15))
        for i in range(15):
            obs.add((20, i))

        for i in range(15, 30):
            obs.add((30, i))
        for i in range(16):
            obs.add((40, i))

        return obs
        
    def update_obs(self, obs):
        self.obs = obs

    def animation(self, path, visited, name):
        self.plot_grid(name)
        self.plot_visited(visited)
        self.plot_path(path)
        plt.show()


    def plot_grid(self, name):
        obs_x = [x[0] for x in self.obs]
        obs_y = [x[1] for x in self.obs]

        plt.plot(self.xI[0], self.xI[1], "bs")
        plt.plot(self.xG[0], self.xG[1], "gs")
        plt.plot(obs_x, obs_y, "sk")
        plt.title(name)
        plt.axis("equal")

    def plot_visited(self, visited, cl='gray'):
        if self.xI in visited:
            visited.remove(self.xI)

        if self.xG in visited:
            visited.remove(self.xG)

        count = 0

        for x in visited:
            count += 1
            plt.plot(x[0], x[1], color=cl, marker='o')
            plt.gcf().canvas.mpl_connect('key_release_event',
                                         lambda event: [exit(0) if event.key == 'escape' else None])

            if count < len(visited) / 3:
                length = 20
            elif count < len(visited) * 2 / 3:
                length = 30
            else:
                length = 40
            #
            # length = 15

            if count % length == 0:
                plt.pause(0.001)
        plt.pause(0.01)

    def plot_path(self, path, cl='r', flag=False):
        path_x = [path[i][0] for i in range(len(path))]
        path_y = [path[i][1] for i in range(len(path))]

        if not flag:
            plt.plot(path_x, path_y, linewidth='3', color='r')
        else:
            plt.plot(path_x, path_y, linewidth='3', color=cl)

        plt.plot(self.xI[0], self.xI[1], "bs")
        plt.plot(self.xG[0], self.xG[1], "gs")

        plt.pause(0.01)

        
    def searching(self):
        """
        Breadth-first Searching.
        :return: path, visited order
        """

        self.PARENT[self.s_start] = self.s_start
        self.g[self.s_start] = 0
        self.g[self.s_goal] = math.inf
        heapq.heappush(self.OPEN,
                       (self.heuristic(self.s_start), self.s_start))

        while self.OPEN:
            _, s = heapq.heappop(self.OPEN)
            self.CLOSED.append(s)

            if s == self.s_goal:
                break

            for s_n in self.get_neighbor(s):
                new_cost = self.g[s] + self.cost(s, s_n)

                if s_n not in self.g:
                    self.g[s_n] = math.inf

                if new_cost < self.g[s_n]:  # conditions for updating Cost
                    self.g[s_n] = new_cost
                    self.PARENT[s_n] = s

                    # best first set the heuristics as the priority 
                    heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n))

        return self.extract_path(self.PARENT), self.CLOSED
    def get_neighbor(self, s):
        """
        find neighbors of state s that not in obstacles.
        :param s: state
        :return: neighbors
        """

        return [(s[0] + u[0], s[1] + u[1]) for u in self.u_set]

    def cost(self, s_start, s_goal):
        """
        Calculate Cost for this motion
        :param s_start: starting node
        :param s_goal: end node
        :return:  Cost for this motion
        :note: Cost function could be more complicate!
        """

        if self.is_collision(s_start, s_goal):
            return math.inf

        return math.hypot(s_goal[0] - s_start[0], s_goal[1] - s_start[1])

    def is_collision(self, s_start, s_end):
        """
        check if the line segment (s_start, s_end) is collision.
        :param s_start: start node
        :param s_end: end node
        :return: True: is collision / False: not collision
        """

        if s_start in self.obs or s_end in self.obs:
            return True

        if s_start[0] != s_end[0] and s_start[1] != s_end[1]:
            if s_end[0] - s_start[0] == s_start[1] - s_end[1]:
                s1 = (min(s_start[0], s_end[0]), min(s_start[1], s_end[1]))
                s2 = (max(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
            else:
                s1 = (min(s_start[0], s_end[0]), max(s_start[1], s_end[1]))
                s2 = (max(s_start[0], s_end[0]), min(s_start[1], s_end[1]))

            if s1 in self.obs or s2 in self.obs:
                return True

        return False

    def extract_path(self, PARENT):
        """
        Extract the path based on the PARENT set.
        :return: The planning path
        """

        path = [self.s_goal]
        s = self.s_goal

        while True:
            s = PARENT[s]
            path.append(s)

            if s == self.s_start:
                break

        return list(path)

    def heuristic(self, s):
        """
        Calculate heuristic.
        :param s: current node (state)
        :return: heuristic function value
        """

        heuristic_type = self.heuristic_type  # heuristic type
        goal = self.s_goal  # goal node

        if heuristic_type == "manhattan":
            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
        else:#sqrt(x^2+y^2)
            return math.hypot(goal[0] - s[0], goal[1] - s[1])



def main():
    s_start = (5, 5)
    s_goal = (45, 25)
    bfs = BFS(s_start, s_goal, 'None',s_start,s_goal)
    path, visited = bfs.searching()
    bfs.animation(path, visited, "Breadth-first Searching (BFS)")


if __name__ == '__main__':
    main()

简单讲解一下原理(跟A*那篇类似):
简单解析一下:

在获取起点后,算法维护了两个字典:

        self.PARENT[self.s_start] = self.s_start
        self.g[self.s_start] = 0
        self.g[self.s_goal] = math.inf

PARENT字典中存取的是这个点由哪个点延伸出来的,即它的上一个节点是谁,用于最后的路径的返回;g这个字典中存储的是每个坐标的代价值,即g[s]。

然后,算法通过堆栈的概念维护了一个堆栈:

        heapq.heappush(self.OPEN,(self.heuristic(self.s_start), self.s_start))

将初始值的f[s]存储在这里,然后进行while循环:

首先从堆栈中取出栈顶元素:

_, s = heapq.heappop(self.OPEN)

注意到这里使用的heapq堆栈功能中的两个函数heapq.heappop与heapq.heappush。heappop会取出栈顶元素并将原始数据从堆栈中删除,而heappush则是对插入的数据按大小排序并存储在堆栈中。所以每一个遍历的点都会按照它的代价值放入堆栈中,同时每次取出的都是代价值最小的那个。

然后判断出栈顶元素是否为目标点,如果为目标点,则退出:

            if s == self.s_goal:  # stop condition
                break

如果不是,则更新该点附近点的代价值:

            for s_n in self.get_neighbor(s):
                new_cost = self.g[s] + self.cost(s, s_n)

                if s_n not in self.g:
                    self.g[s_n] = math.inf

                if new_cost < self.g[s_n]:  # conditions for updating Cost
                    self.g[s_n] = new_cost
                    self.PARENT[s_n] = s

                    # best first set the heuristics as the priority 
                    heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n))

get_neighbor为获取该点周围的点的坐标。heappush入栈时需要存储的该点的代价值的计算方式为:

     def heuristic(self, s):
        """
        Calculate heuristic.
        :param s: current node (state)
        :return: heuristic function value
        """

        heuristic_type = self.heuristic_type  # heuristic type
        goal = self.s_goal  # goal node

        if heuristic_type == "manhattan":
            return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
        else:#sqrt(x^2+y^2)
            return math.hypot(goal[0] - s[0], goal[1] - s[1])

这里计算了一个启发函数的代价值h(n),h的计算可以根据实际情况进行选择,例如在栅格地图中,计算h的方式一般可以分为两种:曼哈顿距离与欧几里德距离。

另外这里注意一个非常巧妙的问题,对于同一个点,如果它在计算其左边的点的时候会将其添加到堆栈中,计算其本身时会出栈,再计算其右边时是否还需要重新入栈呢?

这个是不一定的,是否重新入栈的关键取决于该点在右边的点的计算代价值是否小于左边:

if new_cost < self.g[s_n]:

如果代价值比原来的小,则会重新入栈,否则,这个点就不需要重新计算,这样子就避免了大量的重复计算以及死循环的问题。

最后得到结果如下:
在这里插入图片描述
从结果可以看出,虽然最佳路径优先搜索算法得到的结果不是最优的,但是在搜索过程中确实效率还是非常高的。

参考:

1、最佳优先搜索和A*搜索算法

2、路径规划算法

  • 0
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
最佳路径优先搜索算法,也称为Dijkstra算法,是一种用于在加权图中找到最短路径算法。它通过计算从起始节点到所有其他节点的最短路径来工作。 以下是最佳路径优先搜索算法的Java实现: ```java import java.util.*; public class DijkstraAlgorithm { private static final int INF = Integer.MAX_VALUE; public static void dijkstra(int[][] graph, int start) { int n = graph.length; int[] dist = new int[n]; boolean[] visited = new boolean[n]; Arrays.fill(dist, INF); dist[start] = 0; for (int i = 0; i < n - 1; i++) { int minDist = INF; int minIndex = -1; for (int j = 0; j < n; j++) { if (!visited[j] && dist[j] < minDist) { minDist = dist[j]; minIndex = j; } } visited[minIndex] = true; for (int j = 0; j < n; j++) { if (!visited[j] && graph[minIndex][j] != 0 && dist[minIndex] != INF && dist[minIndex] + graph[minIndex][j] < dist[j]) { dist[j] = dist[minIndex] + graph[minIndex][j]; } } } // 打印最短路径 System.out.println("节点\t最短距离"); for (int i = 0; i < n; i++) { System.out.println(i + "\t\t" + dist[i]); } } public static void main(String[] args) { int[][] graph = { {0, 4, 0, 0, 0, 0, 0, 8, 0}, {4, 0, 8, 0, 0, 0, 0, 11, 0}, {0, 8, 0, 7, 0, 4, 0, 0, 2}, {0, 0, 7, 0, 9, 14, 0, 0, 0}, {0, 0, 0, 9, 0, 10, 0, 0, 0}, {0, 0, 4, 14, 10, 0, 2, 0, 0}, {0, 0, 0, 0, 0, 2, 0, 1, 6}, {8, 11, 0, 0, 0, 0, 1, 0, 7}, {0, 0, 2, 0, 0, 0, 6, 7, 0} }; dijkstra(graph, 0); } } ``` 这段代码实现了Dijkstra算法,通过传入一个加权图和起始节点,计算出从起始节点到其他节点的最短路径,并打印出最短距离。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

一叶执念

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值