目录

一、 算法核心步骤

阶段一：学习阶段（构建路线图）

阶段二：查询阶段（路径搜索）

二、 关键参数与特性

三、优缺点分析

四、 应用场景

五、MATLAB实现

5.1 整体架构

5.2 详细模块说明

1. 主程序流程

2. 核心功能函数

3. 辅助工具函数

4. 数据结构设计

5. 关键参数说明

6. 算法流程图

5.3 结果

---

PRM（Probabilistic Roadmap Method，概率路线图方法）是一种经典的基于采样的运动规划算法，主要用于解决高维空间（如机械臂、多关节机器人）的路径规划问题。它的核心思想是“先建图，后查询”，通过随机采样在构型空间（C-Space）中构建一个无碰撞的图结构（Roadmap），然后利用图搜索算法（如A*、Dijkstra）在图上寻找起点到终点的路径。

一、 算法核心步骤

PRM算法通常分为两个阶段：学习阶段（Learning Phase）​ 和 查询阶段（Query Phase）。

阶段一：学习阶段（构建路线图）

这一阶段不关心具体的起点和终点，只负责构建一张覆盖自由空间（Free Space）的“地图”。

1. 采样（Sampling）：在机器人的构型空间中随机生成大量的样本点（Configurations）。例如，对于一个6自由度机械臂，每个样本点就是6个关节角度的组合。

2. 碰撞检测（Collision Checking）：对每个采样点进行碰撞检测，剔除那些与障碍物发生碰撞的点，只保留“自由点”。

3. 邻域连接（Local Planning）：对于每个自由点，在其一定半径范围内（或最近的K个点）寻找邻居。尝试用一条简单的局部路径（通常是直线）连接该点与邻居点。

4. 边验证（Edge Validation）：对尝试连接的局部路径进行密集的碰撞检测（或插值检测）。如果整条路径都无碰撞，则在图中添加一条边（Edge）。

最终，我们得到一个由节点（自由点）和边（无碰撞连接）组成的图 G=(V,E)。

阶段二：查询阶段（路径搜索）

当给定具体的起点 qstart​和终点 qgoal​时：

1. 连接起终点：尝试将 qstart​和 qgoal​连接到已有的路线图 G上。即找到图中离它们最近的无碰撞节点，并验证连接路径是否可行。

2. 图搜索：如果连接成功，起点和终点都成为了图的一部分。使用图搜索算法在 G中寻找从起点节点到终点节点的最短路径。

二、 关键参数与特性

• 采样策略：纯随机采样（Uniform Sampling）是基础，但可能导致在狭窄通道处采样困难。改进版使用高斯采样、桥测试采样或障碍物边界采样来提高在狭窄区域的采样密度。

• 邻域选择：通常使用 K-最近邻（K-NN）​ 或 固定半径（R-disc）。K太小图可能不连通，K太大会增加计算量。

• 概率完备性（Probabilistic Completeness）：如果解存在，当采样点数 N→∞时，算法找到解的概率趋近于 1。但它不是完备的（Complete），因为有限的采样点可能永远错过某个关键通道。

• 多查询效率：PRM最大的优势在于，对于同一张地图（环境不变），只需要构建一次路线图，之后对于任意新的起终点组合，查询速度都非常快（仅需毫秒级）。​​​​​​​

三、优缺点分析

优点	缺点
适用于高维空间，计算复杂度与维度呈多项式关系（优于栅格法）	对于狭窄通道（Narrow Passage）问题，采样效率极低，难以生成连接
多查询场景下效率极高（一次建图，多次查询）	路径质量可能不高（由于随机采样，路径可能不是最优的）
实现相对简单	建图阶段耗时较长，且对于动态环境不适用

四、 应用场景

PRM及其变种（如PRM*，Lazy PRM）广泛应用于：

• 机械臂轨迹规划：在6-DoF或更高维的关节空间中进行规划。

• 虚拟原型验证：在CAD软件中验证装配序列或运动可行性。

• 动画角色运动生成：规划复杂环境中角色的运动。​​​​​​​

五、MATLAB实现

%% PRM算法主程序
clear; clc; close all;

%% 1. 参数设置
params.N = 500;          % 采样点数量
params.K = 10;           % 每个点连接的最近邻数量
params.radius = 1.0;     % 用于局部规划的半径（或使用KNN）
params.stepSize = 0.05;  % 边验证时的插值步长
params.mapSize = [0, 10, 0, 10]; % 地图边界 [xmin, xmax, ymin, ymax]

%% 2. 创建环境（障碍物）
% 障碍物用凸多边形表示：[x1,y1; x2,y2; ...]
obstacles = cell(3, 1);
obstacles{1} = [2, 2; 2, 4; 4, 4; 4, 2];  % 矩形障碍物1
obstacles{2} = [6, 6; 6, 8; 8, 8; 8, 6];  % 矩形障碍物2
obstacles{3} = [1, 7; 3, 8; 5, 7; 3, 6];  % 不规则四边形障碍物

%% 3. 定义起点和终点
q_start = [0.5, 0.5];
q_goal = [9.5, 9.5];

%% 4. 构建PRM路线图
fprintf('开始构建PRM路线图...\n');
[V, E, G] = buildPRM(obstacles, params);
fprintf('路线图构建完成！\n');
fprintf('有效节点数: %d, 有效边数: %d\n', size(V, 1), size(E, 1));

%% 5. 查询路径
fprintf('\n开始查询路径...\n');
[path, success] = queryPRM(q_start, q_goal, V, E, obstacles, params);

%% 6. 可视化结果
if success
    fprintf('找到路径！路径长度: %.2f\n', pathLength(path));
    visualizePRM(q_start, q_goal, obstacles, V, E, path, params);
else
    fprintf('未找到路径！\n');
    visualizePRM(q_start, q_goal, obstacles, V, E, [], params);
end

%% 7. 辅助函数定义

function [V, E, G] = buildPRM(obstacles, params)
    % 构建PRM路线图
    V = [];  % 节点集合
    E = [];  % 边集合
    attempts = 0;
    maxAttempts = params.N * 5;  % 最大尝试次数，避免无限循环
    
    while size(V, 1) < params.N && attempts < maxAttempts
        attempts = attempts + 1;
        
        % 1. 随机采样
        q_rand = randomSample(params);
        
        % 2. 碰撞检测
        if isCollisionFree(q_rand, obstacles)
            V = [V; q_rand];
            
            % 3. 寻找邻居（K最近邻）
            if size(V, 1) > 1
                [neighbors, dists] = findNeighbors(q_rand, V, params.K);
                
                % 4. 尝试连接邻居
                for i = 1:length(neighbors)
                    q_near = V(neighbors(i), :);
                    
                    % 检查是否已存在边（修复：处理E为空的情况）
                    edgeExists = false;
                    if ~isempty(E)
                        % 检查两个方向的边
                        if any(ismember(E, [size(V,1), neighbors(i)], 'rows')) || ...
                           any(ismember(E, [neighbors(i), size(V,1)], 'rows'))
                            edgeExists = true;
                        end
                    end
                    
                    if ~edgeExists && dists(i) < params.radius * 3
                        % 局部规划（直线连接）
                        if localPlanner(q_rand, q_near, obstacles, params.stepSize)
                            E = [E; size(V,1), neighbors(i)];
                        end
                    end
                end
            end
        end
    end
    
    % 构建邻接矩阵表示的图
    n = size(V, 1);
    G = zeros(n, n);
    for i = 1:size(E, 1)
        p1 = E(i, 1);
        p2 = E(i, 2);
        dist = norm(V(p1,:) - V(p2,:));
        G(p1, p2) = dist;
        G(p2, p1) = dist;
    end
    
    fprintf('总采样尝试次数: %d\n', attempts);
end

function q_rand = randomSample(params)
    % 在构型空间中随机采样
    x = params.mapSize(1) + rand() * (params.mapSize(2) - params.mapSize(1));
    y = params.mapSize(3) + rand() * (params.mapSize(4) - params.mapSize(3));
    q_rand = [x, y];
end

function collision = isCollisionFree(q, obstacles)
    % 检查点是否与障碍物碰撞
    collision = true;
    
    for i = 1:length(obstacles)
        obs = obstacles{i};
        if inpolygon(q(1), q(2), obs(:,1), obs(:,2))
            collision = false;
            return;
        end
    end
end

function [neighbors, dists] = findNeighbors(q, V, K)
    % 寻找K个最近邻
    n = size(V, 1);
    distances = zeros(n, 1);
    
    for i = 1:n
        distances(i) = norm(q - V(i,:));
    end
    
    [sortedDist, sortedIdx] = sort(distances);
    
    % 排除自身（新点本身在V的最后）
    K = min(K, n-1);  % 确保K不超过节点数-1
    if K <= 0
        neighbors = [];
        dists = [];
        return;
    end
    
    % 如果新点是最后添加的，从排序中排除它
    if sortedIdx(1) == n
        startIdx = 2;
    else
        startIdx = 1;
    end
    
    endIdx = min(K + startIdx - 1, length(sortedIdx));
    neighbors = sortedIdx(startIdx:endIdx);
    dists = sortedDist(startIdx:endIdx);
end

function feasible = localPlanner(q1, q2, obstacles, stepSize)
    % 检查两点之间的直线路径是否可行
    feasible = true;
    dist = norm(q2 - q1);
    
    if dist == 0
        return;
    end
    
    steps = max(1, ceil(dist / stepSize));
    
    for s = 0:steps
        t = s / steps;
        q = q1 + t * (q2 - q1);
        if ~isCollisionFree(q, obstacles)
            feasible = false;
            return;
        end
    end
end

function [path, success] = queryPRM(q_start, q_goal, V, E, obstacles, params)
    % 查询路径
    success = false;
    path = [];
    
    % 检查V和E是否为空
    if isempty(V) || isempty(E)
        fprintf('路线图为空，无法查询路径！\n');
        return;
    end
    
    % 1. 连接起点和终点到路线图
    [V, E, startIdx, goalIdx] = connectToRoadmap(q_start, q_goal, V, E, obstacles, params);
    
    if isempty(startIdx) || isempty(goalIdx)
        fprintf('起点或终点无法连接到路线图！\n');
        return;
    end
    
    % 2. 构建图数据结构用于搜索
    n = size(V, 1);
    adjMatrix = zeros(n, n);
    for i = 1:size(E, 1)
        p1 = E(i, 1);
        p2 = E(i, 2);
        dist = norm(V(p1,:) - V(p2,:));
        adjMatrix(p1, p2) = dist;
        adjMatrix(p2, p1) = dist;
    end
    
    % 3. 使用A*算法搜索路径
    [pathNodes, found] = astar(startIdx, goalIdx, V, adjMatrix);
    
    if found
        % 4. 转换为连续路径
        path = V(pathNodes, :);
        success = true;
    end
end

function [V_new, E_new, startIdx, goalIdx] = connectToRoadmap(q_start, q_goal, V, E, obstacles, params)
    % 连接起点和终点到现有路线图
    V_new = V;
    E_new = E;
    
    % 检查起点是否在自由空间
    if ~isCollisionFree(q_start, obstacles)
        fprintf('起点在障碍物内！\n');
        startIdx = [];
        goalIdx = [];
        return;
    end
    
    % 检查终点是否在自由空间
    if ~isCollisionFree(q_goal, obstacles)
        fprintf('终点在障碍物内！\n');
        startIdx = [];
        goalIdx = [];
        return;
    end
    
    % 添加起点
    V_new = [V_new; q_start];
    startIdx = size(V_new, 1);
    
    % 连接起点到最近的K个邻居
    if ~isempty(V)
        [neighbors, dists] = findNeighbors(q_start, V, min(params.K, size(V,1)));
        for i = 1:length(neighbors)
            if dists(i) < params.radius * 2
                if localPlanner(q_start, V_new(neighbors(i),:), obstacles, params.stepSize)
                    E_new = [E_new; startIdx, neighbors(i)];
                end
            end
        end
    end
    
    % 添加终点
    V_new = [V_new; q_goal];
    goalIdx = size(V_new, 1);
    
    % 连接终点到最近的K个邻居
    [neighbors, dists] = findNeighbors(q_goal, V_new(1:end-1,:), min(params.K, size(V_new,1)-1));
    for i = 1:length(neighbors)
        if neighbors(i) ~= startIdx && dists(i) < params.radius * 2
            if localPlanner(q_goal, V_new(neighbors(i),:), obstacles, params.stepSize)
                E_new = [E_new; goalIdx, neighbors(i)];
            end
        end
    end
end

function [path, found] = astar(startIdx, goalIdx, V, adjMatrix)
    % A*算法实现
    n = size(V, 1);
    gScore = inf(n, 1);
    fScore = inf(n, 1);
    cameFrom = zeros(n, 1);
    
    gScore(startIdx) = 0;
    fScore(startIdx) = heuristic(V(startIdx,:), V(goalIdx,:));
    
    openSet = startIdx;
    found = false;
    
    while ~isempty(openSet)
        [~, currentIdx] = min(fScore(openSet));
        current = openSet(currentIdx);
        
        if current == goalIdx
            % 重构路径
            path = reconstructPath(cameFrom, current);
            found = true;
            return;
        end
        
        openSet(currentIdx) = [];
        
        % 获取所有邻居
        neighbors = find(adjMatrix(current, :) > 0);
        
        for i = 1:length(neighbors)
            neighbor = neighbors(i);
            tentative_gScore = gScore(current) + adjMatrix(current, neighbor);
            
            if tentative_gScore < gScore(neighbor)
                cameFrom(neighbor) = current;
                gScore(neighbor) = tentative_gScore;
                fScore(neighbor) = gScore(neighbor) + heuristic(V(neighbor,:), V(goalIdx,:));
                
                if ~ismember(neighbor, openSet)
                    openSet = [openSet; neighbor];
                end
            end
        end
    end
    
    path = [];
    found = false;
end

function h = heuristic(q1, q2)
    % 启发函数（欧氏距离）
    h = norm(q1 - q2);
end

function path = reconstructPath(cameFrom, current)
    % 重构路径
    path = current;
    while cameFrom(current) ~= 0
        current = cameFrom(current);
        path = [current; path];
    end
end

function length = pathLength(path)
    % 计算路径长度
    length = 0;
    for i = 1:size(path,1)-1
        length = length + norm(path(i+1,:) - path(i,:));
    end
end

function visualizePRM(q_start, q_goal, obstacles, V, E, path, params)
    % 可视化PRM结果
    figure('Position', [100, 100, 1200, 500]);
    
    % 子图1: 路线图
    subplot(1,2,1);
    hold on; grid on; axis equal;
    xlim([params.mapSize(1), params.mapSize(2)]);
    ylim([params.mapSize(3), params.mapSize(4)]);
    title('PRM路线图');
    xlabel('X'); ylabel('Y');
    
    % 绘制障碍物
    for i = 1:length(obstacles)
        obs = obstacles{i};
        fill(obs(:,1), obs(:,2), [0.8, 0.2, 0.2], 'FaceAlpha', 0.7, 'EdgeColor', 'k', 'LineWidth', 1.5);
    end
    
    % 绘制边
    if ~isempty(E)
        for i = 1:size(E, 1)
            p1 = V(E(i,1), :);
            p2 = V(E(i,2), :);
            plot([p1(1), p2(1)], [p1(2), p2(2)], 'b-', 'LineWidth', 0.5, 'Color', [0.7, 0.7, 1]);
        end
    end
    
    % 绘制节点
    if ~isempty(V)
        scatter(V(:,1), V(:,2), 20, 'b', 'filled');
    end
    
    % 绘制起点和终点
    scatter(q_start(1), q_start(2), 100, 'g', 'filled', 'MarkerEdgeColor', 'k', 'LineWidth', 2);
    scatter(q_goal(1), q_goal(2), 100, 'r', 'filled', 'MarkerEdgeColor', 'k', 'LineWidth', 2);
    text(q_start(1), q_start(2), '起点', 'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'center', 'FontSize', 10, 'FontWeight', 'bold');
    text(q_goal(1), q_goal(2), '终点', 'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'center', 'FontSize', 10, 'FontWeight', 'bold');
    
    % 子图2: 最终路径
    subplot(1,2,2);
    hold on; grid on; axis equal;
    xlim([params.mapSize(1), params.mapSize(2)]);
    ylim([params.mapSize(3), params.mapSize(4)]);
    title('最终路径');
    xlabel('X'); ylabel('Y');
    
    % 绘制障碍物
    for i = 1:length(obstacles)
        obs = obstacles{i};
        fill(obs(:,1), obs(:,2), [0.8, 0.2, 0.2], 'FaceAlpha', 0.7, 'EdgeColor', 'k', 'LineWidth', 1.5);
    end
    
    % 绘制路线图（可选）
    if ~isempty(V)
        scatter(V(:,1), V(:,2), 20, 'b', 'filled', 'MarkerFaceAlpha', 0.3);
    end
    
    % 绘制起点和终点
    scatter(q_start(1), q_start(2), 100, 'g', 'filled', 'MarkerEdgeColor', 'k', 'LineWidth', 2);
    scatter(q_goal(1), q_goal(2), 100, 'r', 'filled', 'MarkerEdgeColor', 'k', 'LineWidth', 2);
    text(q_start(1), q_start(2), '起点', 'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'center', 'FontSize', 10, 'FontWeight', 'bold');
    text(q_goal(1), q_goal(2), '终点', 'VerticalAlignment', 'bottom', 'HorizontalAlignment', 'center', 'FontSize', 10, 'FontWeight', 'bold');
    
    % 绘制路径
    if ~isempty(path)
        plot(path(:,1), path(:,2), 'r-', 'LineWidth', 3);
        scatter(path(:,1), path(:,2), 40, 'r', 'filled');
        legend('障碍物', '路线图节点', '起点', '终点', '路径', 'Location', 'best');
    else
        legend('障碍物', '路线图节点', '起点', '终点', 'Location', 'best');
    end
    
    hold off;
end

5.1 整体架构

主程序 (Main Script)
├── 参数设置
├── 环境创建
├── 起点终点定义
├── 路线图构建
├── 路径查询
├── 可视化
└── 辅助函数集合

5.2 详细模块说明

1. 主程序流程

%% 主程序 (Main Script)
1. 参数设置 (Parameters)           % 定义算法参数
2. 创建环境 (Environment)         % 定义障碍物多边形
3. 定义起点/终点                  % 设置规划任务
4. 构建PRM路线图 (buildPRM)       % 核心建图过程
5. 查询路径 (queryPRM)            % 路径搜索过程
6. 可视化结果 (visualizePRM)      % 结果展示

2. 核心功能函数

路线图构建模块

function [V, E, G] = buildPRM(obstacles, params)
├── 输入: obstacles(障碍物), params(参数)
├── 输出: V(节点坐标), E(边列表), G(邻接矩阵)
└── 流程:
    1. 初始化节点集V和边集E
    2. While 节点数 < N
        ├── 随机采样 randomSample()
        ├── 碰撞检测 isCollisionFree()
        └── If 无碰撞
            ├── 添加节点到V
            ├── 寻找最近邻 findNeighbors()
            └── For 每个邻居
                ├── 检查边是否已存在
                └── 局部规划 localPlanner()

路径查询模块

function [path, success] = queryPRM(q_start, q_goal, V, E, obstacles, params)
├── 输入: 起终点, 路线图, 障碍物, 参数
├── 输出: path(路径), success(是否成功)
└── 流程:
    1. 连接起终点 connectToRoadmap()
    2. 构建邻接矩阵
    3. A*搜索 astar()
    4. 返回路径

3. 辅助工具函数

几何与碰撞检测

randomSample()       % 在构型空间内均匀随机采样
isCollisionFree()   % 点与障碍物的碰撞检测(基于inpolygon)
localPlanner()      % 直线路径碰撞检测(插值法)

图结构操作

findNeighbors()     % K最近邻搜索(欧氏距离)
connectToRoadmap()  % 将起终点连接到现有路线图
astar()            % A*图搜索算法
heuristic()        % 启发函数(欧氏距离)
reconstructPath()  % 从cameFrom重建路径

工具函数

pathLength()        % 计算路径长度(欧氏距离和)
visualizePRM()      % 双子图可视化

4. 数据结构设计

节点集 V

V = [x1, y1;        % 节点1坐标
     x2, y2;        % 节点2坐标
     ...]           % N×2矩阵

边集 E

E = [node1, node2;  % 边1连接节点1和2
     node3, node4;  % 边2连接节点3和4
     ...]           % M×2矩阵

邻接矩阵 G

G(i,j) = distance   % 节点i到j的距离
G(j,i) = distance   % 对称矩阵
G(i,j)=0            % 无边连接

障碍物 obstacles

obstacles = {poly1, poly2, ...}  % 单元格数组
poly1 = [x1,y1; x2,y2; ...]      % 多边形顶点

5. 关键参数说明

参数	变量名	作用	推荐值
采样点数	N	控制路线图密度	300-1000
最近邻数	K	每个点连接的邻居数	8-15
连接半径	radius	限制连接距离	0.5-2.0
插值步长	stepSize	碰撞检测精度	0.05-0.2

6. 算法流程图

开始
  ↓
初始化参数和环境
  ↓
构建PRM路线图
├── 随机采样N个点
├── 保留无碰撞点
├── 连接K个最近邻
└── 验证边无碰撞
  ↓
连接起点和终点
  ↓
A*图搜索
  ↓
输出路径
  ↓
可视化结果

5.3 结果

命令行窗口：

开始构建PRM路线图...
总采样尝试次数: 562
路线图构建完成！
有效节点数: 500, 有效边数: 4617

开始查询路径...
找到路径！路径长度: 13.52

可视化：

会显示两个子图：

1. 左图: 完整的PRM路线图，包含所有采样点和连接边

2. 右图: 最终找到的路径（如果存在）​​​​​​​