（9-1）其他路径规划算法：PRM算法

码农三叔

于 2024-07-29 11:06:12 发布

阅读量813

点赞数 23

分类专栏：《路径规划和决策算法--Python卷》文章标签：算法人工智能机器学习自动驾驶路径规划 python

本文链接：https://blog.csdn.net/asd343442/article/details/140766178

版权

《路径规划和决策算法--Python卷》专栏收录该内容

85 篇文章 9 订阅

订阅专栏

9.1 PRM算法

Probabilistic Roadmap (PRM) 算法是一种基于概率的路径规划算法，用于解决机器人运动规划问题。PRM 算法通过在配置空间中随机采样一些点，并连接它们以构建一个图（Roadmap），从而在复杂环境中搜索有效路径。

9.1.1 PRM算法介绍

PRM 算法的关键思想是通过在配置空间中概率性地采样点，并在可连接的点之间建立边，以构建机器人可能的运动路径。这使得算法更具适应性，能够处理复杂的环境和高维配置空间。实现PRM 算法的基本步骤如下所示。

（1）节点采样：在配置空间中随机采样一组节点，这些节点表示机器人可能的位置。

（2）碰撞检测：对于每个采样得到的节点，进行碰撞检测，排除那些位于障碍物内的节点，以确保路径不会穿过障碍物。

（3）连接节点：对于没有碰撞的节点对，通过局部路径规划算法（如直线运动）尝试直接连接它们。如果连接是可行的且不与障碍物相交，则将这两个节点之间添加一条边。

（4）构建图：上述步骤构建了一个图，其中节点表示机器人可能的位置，边表示机器人在两个位置之间可以安全移动的路径。这个图即为 Probabilistic Roadmap。

（5）路径搜索：使用传统的路径搜索算法（如 Dijkstra 或 A*）在 Probabilistic Roadmap 上找到从起始位置到目标位置的路径。

PRM 算法的优点在于其概率性质和适用性，通过在配置空间中进行概率采样，PRM 能够适应复杂的环境，并找到在避免障碍物的同时，效率较高的路径。PRM 算法适用于高维、复杂的配置空间，例如多关节机器人的运动规划。

PRM 算法的缺点之一是构建图（Roadmap）可能需要大量的采样点，尤其是在环境具有较多复杂结构的情况下。此外，PRM 的性能可能受到配置空间维度的影响，因为在高维空间中进行有效的碰撞检测和连接操作可能变得更加困难。

9.1.2 PRM算法实战

请看下面的例子，演示了使用PRM算法规划路径的过程。在这个例子中，假设你是一个机器人路径规划的专家，现在你需要使用 Python 编程实现一个 PRM（Probabilistic Roadmap）算法来规划机器人在环境中的路径。给定一个环境边界、起点、终点和一些障碍物的坐标，需要编写一个 PRM 类来生成机器人的路径，并可视化路径规划的结果。

实例9-1：使用PRM算法为机器人规划路径（源码路径：codes\9\PRM.py）

实例文件PRM.py的具体实现代码如下所示。

import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import KDTree

class PRM:
    def __init__(self, num_samples, num_neighbors, start, goal, environment_bounds, obstacles):
        self.num_samples = num_samples
        self.num_neighbors = num_neighbors
        self.start = start
        self.goal = goal
        self.environment_bounds = environment_bounds
        self.samples = []
        self.graph = {}
        self.obstacles = obstacles

    def generate_samples(self):
        self.samples = np.random.rand(self.num_samples, 2) * np.array(self.environment_bounds)

    def collision_free(self, point):
        # Check if the point is within bounds and not colliding with any obstacles
        x, y = point
        if (x < 0 or x > self.environment_bounds[0] or
            y < 0 or y > self.environment_bounds[1]):
            return False
        for obstacle in self.obstacles:
            if np.linalg.norm(point - obstacle) < 1:
                return False
        return True

    def find_neighbors(self, point):
        distances, indices = self.kd_tree.query([point], k=self.num_neighbors)
        neighbors = [self.samples[i] for i in indices[0]]
        return neighbors

    def construct_graph(self):
        self.generate_samples()
        self.kd_tree = KDTree(self.samples)
        for sample in self.samples:
            if not self.collision_free(sample):
                continue
            neighbors = self.find_neighbors(sample)
            self.graph[tuple(sample)] = [tuple(neighbor) for neighbor in neighbors if self.collision_free(neighbor)]

    def plot(self, path=None):
        plt.figure()
        for node, neighbors in self.graph.items():
            plt.scatter(node[0], node[1], color='blue')
            for neighbor in neighbors:
                if self.collision_free(neighbor):
                    plt.plot([node[0], neighbor[0]], [node[1], neighbor[1]], color='black')
        for obstacle in self.obstacles:
            circle = plt.Circle(obstacle, 1, color='red')
            plt.gca().add_patch(circle)
        plt.scatter(self.start[0], self.start[1], color='blue', label='Start')
        plt.scatter(self.goal[0], self.goal[1], color='green', label='Goal')
        if path:
            plt.plot(*zip(*path), color='red', linewidth=2, label='Shortest Path')
        plt.xlim(0, self.environment_bounds[0])
        plt.ylim(0, self.environment_bounds[1])
        plt.title('PRM Graph with Obstacles')
        plt.xlabel('X')
        plt.ylabel('Y')
        plt.legend()
        plt.grid(True)
        plt.show()

    def find_shortest_path(self):
        start_index = np.argmin(np.linalg.norm(self.samples - self.start, axis=1))
        goal_index = np.argmin(np.linalg.norm(self.samples - self.goal, axis=1))
        path = self.shortest_path(self.samples, start_index, goal_index)
        return path

    def shortest_path(self, points, start_index, goal_index):
        graph = {tuple(point): [] for point in points}
        for point, neighbors in self.graph.items():
            for neighbor in neighbors:
                graph[tuple(point)].append(tuple(neighbor))
                graph[tuple(neighbor)].append(tuple(point))

        visited = set()
        queue = [[tuple(points[start_index])]]
        while queue:
            path = queue.pop(0)
            node = path[-1]
            if node == tuple(points[goal_index]):
                return path
            if node not in visited:
                visited.add(node)
                for neighbor in graph[node]:
                    new_path = list(path)
                    new_path.append(neighbor)
                    queue.append(new_path)

# Example usage
obstacles = np.array([[3, 3], [7, 7], [5, 3], [3, 5], [7, 5], [5, 7]])
start = [1, 1]
goal = [9, 9]
prm = PRM(num_samples=200, num_neighbors=10, start=start, goal=goal, environment_bounds=[10, 10], obstacles=obstacles)
prm.construct_graph()
shortest_path = prm.find_shortest_path()
prm.plot(shortest_path)

在这个例子中，使用了 PRM 算法来构建图，并通过图来寻找最短路径。上述代码的实现流程如下所示：

（1）首先，定义了一个 Probabilistic Roadmap（PRM）路径规划器类PRM，这个PRM 路径规划器的主要功能是在给定的环境中生成路径，并找到起点到终点的最短路径。

（2）然后，实现类PRM的构造函数，此函数接收如下所示的参数。