基于PRM(probabilistic roadmaps)算法的机器人路线规划算法matlab仿真

目录

一、理论基础

1.1 理论概述

1.2 基于PRM算法的机器人路线规划算法理论

二、MATLAB仿真程序

三、仿真结果

---

一、理论基础

1.1 理论概述

       PRM（Probabilistic Roadmaps）算法是一种基于概率的机器人路径规划算法，主要用于在复杂环境中寻找可行路径。该算法通过随机生成一系列路标点，并在路标点之间建立连接，从而构建一张概率路线图。然后，通过搜索这张概率路线图，找到一条从起点到终点的可行路径。下面将详细介绍PRM算法的原理、数学公式和实现步骤。

一、PRM算法的原理

PRM算法主要包括两个步骤：路标点生成和路径搜索。

路标点生成

       路标点生成是PRM算法的第一步，其目标是在机器人工作空间中随机生成一系列路标点。通常情况下，路标点生成采用以下步骤：

（1）确定机器人工作空间的范围和目标姿态。

（2）根据一定的概率密度函数，在工作空间中随机生成路标点。概率密度函数的选择应该考虑到机器人的运动能力和环境的复杂性。

（3）对于每个生成的路标点，使用机器人运动学模型进行碰撞检测，确保该点处机器人可以到达且不会与障碍物发生碰撞。

（4）将符合条件的路标点添加到路标点集合中。

路径搜索

       路径搜索是PRM算法的第二步，其目标是在生成的路标点集合中找到一条从起点到终点的可行路径。通常情况下，路径搜索采用以下步骤：

（1）将起点和终点添加到路标点集合中。

（2）对于每个路标点，计算它与其他路标点的连接可行性。连接可行性可以通过机器人运动学模型和碰撞检测来确定。

（3）根据一定的概率选择两个路标点，并尝试在这两个路标点之间建立连接。如果连接成功，则将连接添加到路径图中。

（4）重复步骤（3），直到路径图中存在一条从起点到终点的可行路径，或者达到预设的迭代次数。

（5）使用搜索算法（如Dijkstra算法或A*算法）在路径图中找到一条从起点到终点的最短路径。

PRM算法的具体实现步骤：

1. 初始化参数：确定机器人工作空间的范围和目标姿态，设定路标点数量和迭代次数等参数。
2. 生成路标点集合：根据概率密度函数在机器人工作空间中随机生成路标点，并进行碰撞检测，将符合条件的路标点添加到路标点集合中。
3. 建立路径图：对于每个路标点，计算它与其他路标点的连接可行性，并根据一定的概率选择两个路标点，尝试在这两个路标点之间建立连接。如果连接成功，则将连接添加到路径图中。重复此步骤，直到路径图中存在一条从起点到终点的可行路径，或者达到预设的迭代次数。
4. 搜索最短路径：使用搜索算法（如Dijkstra算法或A*算法）在路径图中找到一条从起点到终点的最短路径。如果找不到可行路径，则返回失败信息。

1.2 基于PRM算法的机器人路线规划算法理论

地图和机器人的模型如下：

1.使用一个2*2的网格大小(gridsize)和5度的角分辨率(angular resolution)，创建机器人的构型空间(Configurationspace)。请简单说明，并输出构型空间的视图。
机器人的初始状态：坐标(0,45)，90度

2.使用谈心搜索算法计算从(50,50)到(750, 250)的最短路径。请在图中标明并且输出最短路径的长度。

3.使用中轴变换计算最安全的路径(最安全的路径是指离墙最远的路径)。请在图中标明并且输出最短路径的长度。

4.使用PRM（probabilistic roadmaps）算法计算从(50,50)到(750, 250)的最短路径。分别使用50、100、500个样本点。请在图中标明并输出这些路径的长度。
 

---

使用一个2*2的网格大小(grid size)和5度的角分辨率(angular resolution)，创建机器人的构型空间(Configuration space)。请简单说明，并输出构型空间的视图。

机器人的初始状态：坐标(0,45)，90度

       这里，如果按1*1的方格，算法将及其复杂，数据量极大，我们这里将网格进行分割。300*800的空间，分割为10*10一个方格。

      PRM算法是通过对空间进行大量采样来构建路线图，用于后续的特定查询。路线图是无向连接图，车辆或机器人可以路线图上由任意一点移动到其他点。点与点连接线最简单的就是直线。路线图构建好之后可以采用经典的A*算法来搜索路径。PRM算法构建路线图过程如下所示。

二、MATLAB仿真程序

clc;
clear;
close all;
warning off;
addpath 'func\'
%转弯分辨率
ang   = 5/180*pi;
W     = 800;
H     = 300;
K     = 10;
Scale = max(W,H)/K;
[MAPs,Start,Ends,cc,MAPpoint] = func_wall(Scale,K);
%显示方格场景图
func_Map2fig(MAPs,Start,Ends,H/K,W,H,K);

S1    = Start;
S2    = 0;
S3    = inf;
S4    = [];
S5    = [];
Paths = ['R','L','D','U'];

%开始贪心算法搜索，
while ~max(ismember(S1,Ends))&&~isempty(S1)
    [temp,kj]           = min(S2+S3);
    %搜索路径值用来判断往哪走
    [path1,path2,path3] = func_search(S1(kj),S2(kj),MAPs,Ends);
    S4                  = [S4;S1(kj)];
    S5                  = [S5;S2(kj)];
    %判决
    if kj>1&&kj<length(S1)
        S1=[S1(1:kj-1);S1(kj+1:end)];
        S2=[S2(1:kj-1);S2(kj+1:end)];
        S3=[S3(1:kj-1);S3(kj+1:end)];
    else
        S1=[S1(kj+1:end)];
        S2=[S2(kj+1:end)];
        S3=[S3(kj+1:end)];
    end
    
    for jj=1:length(path3)
        if ~isinf(path1(jj))
            if ~max([S1;S4]==path3(jj))
                MAPpoint{path3(jj)}=Paths(jj);
                 S1 = [S1; path3(jj)];
                 S2 = [S2; path1(jj)];
                 S3 = [S3; path2(jj)];
            elseif max(S1==path3(jj))
                i=find(S1==path3(jj));
                if S2(i)>path1(jj)
                    S2(i)=path1(jj);
                    S3(i)=path2(jj);
                    MAPpoint{S1(i)}=Paths(jj);
                end
            else i=find(S4==path3(jj));
                if S5(i)>path1(jj)
                   S5(i)=path1(jj);
                   MAPpoint{S4(i)}=Paths(jj);  
                end
            end
        end
    end
    if isempty(S1)
        break;
    end
end

Pathss=func_check(Ends,MAPpoint);
 
figure(1);
plot(Pathss(:,1)+0.5,Pathss(:,2)+0.5-(W-H)/K,'color',[0 1 0],'LineWidth',2);
title('贪婪算法');
X=Pathss(:,1)+0.5;
Y=Pathss(:,2)+0.5-(W-H)/K;
%输出长度
d=0;
for i = 1:length(X)-1
    d = d+K*sqrt((X(i)-X(i+1))^2 + (Y(i)-Y(i+1))^2);
end
d

三、仿真结果

 

A16-73