PRM算法三维地图matlab仿真

三维地图的PRM算法程序，可以用于无人机的仿真。

路线图算法（probabilistic roadmap method，PRM）主要分为两个阶段，离线学习阶段中随机采样大量的机器人位姿点，为每个节点搜索邻居节点并建立连接，构建出路标地图；在线查询阶段中根据起始点，目标点，路标地图信息，采用启发式搜索算法从路标图搜索出一条可行路径。路线图算法可以有效避免对位姿空间中障碍物进行精确建模，能够有效解决复杂的运动动力学约束下的路径规划问题。

链接：https://pan.baidu.com/s/1AiuQ7KMfKuTvDHxoCz6yAA?pwd=nmld
提取码：nmld

部分代码：

clc
clear
close allmap
peaksData = ceil(peaksData);
mr = 100;%地图大小
st = [1,1,1];%起点
ed = [99,99,99];%终点
k = 50;%随机点数量
sjd = [st;ed];%随机点集合%% 生产随机点
while length(sjd)<k+2s = randi([1,mr],1,3);if feasible(s,peaksData) %是否在范围内sjd=[sjd;s];end
endsurf(X,Y,Z)
shading flat
hold on
scatter3(sjd(:,1),sjd(:,2),sjd(:,3),300,'.b');
%% 建立无向路图
edges=cell(k+2,1);
for i=1:k+2for j=i+1:k+2if checkPath(sjd(i,:),sjd(j,:),peaksData)edges{i}=[edges{i};j];edges{j}=[edges{j};i];line([sjd(i,1);sjd(j,1)],[sjd(i,2);sjd(j,2)],[sjd(i,3);sjd(j,3)],'Color','m');endend
end
%% 求最短路径Q=[1 0 heuristic(sjd(1,:),ed) 0+heuristic(sjd(1,:),ed) -1]; % the processing queue of A* algorihtm, open list
closed=[]; % the closed list taken as a list
pathFound=false;
while size(Q,1)>0[A, I]=min(Q,[],1);n=Q(I(4),:); % smallest cost element to processQ=[Q(1:I(4)-1,:);Q(I(4)+1:end,:)]; % delete element under processingif n(1)==2 % goal testpathFound=true;break;endfor mv=1:length(edges{n(1),1}) %iterate through all edges from the nodenewVertex=edges{n(1),1}(mv);if isempty(closed) || isempty(find(closed(:,1)==newVertex, 1)) % not already in closedhistoricCost=n(2)+historic(sjd(n(1),:),sjd(newVertex,:));heuristicCost=heuristic(sjd(newVertex,:),ed);totalCost=historicCost+heuristicCost;add=true; % not already in queue with better costif length(find(Q(:,1)==newVertex))>=1I=find(Q(:,1)==newVertex);if Q(I,4)<totalCost, add=false;else Q=[Q(1:I-1,:);Q(I+1:end,:);];add=true;endendif addQ=[Q;newVertex historicCost heuristicCost totalCost size(closed,1)+1]; % add new nodes in queueendend           endclosed=[closed;n]; % update closed lists
end
figurepath=[sjd(n(1),:)]; %retrieve path from parent information
prev=n(5);
while prev>0path=[sjd(closed(prev,1),:);path];prev=closed(prev,5);
end
surf(X,Y,Z)
shading flat
hold on
line(path(:,1),path(:,2),path(:,3),'color','r');
scatter3(st(:,1),st(:,2),st(:,3),100,'pk');
scatter3(ed(:,1),ed(:,2),ed(:,3),100,'pk');

PRM算法三维地图matlab仿真相关推荐

matlab仿真ofdm的ccdf,OFDM系统峰均比降低算法的研究+Matlab仿真(6)
OFDM系统峰均比降低算法的研究+Matlab仿真(6) 时间:2016-11-28 22:19来源:毕业论文 2.2.5 自适应技术自适应调制技术在OFDM系统中也占有非常重要的地位,其调制思想是 ...
三维点云数据的读取和三维曲面重建matlab仿真
目录 1.算法概述 2.仿真效果 3.MATLAB仿真源码 1.算法概述虽然Delaunay三角剖分算法可以实现网格曲面重建,但是其应用主要在二维剖分,在三维空间网格生成中遇到了问题.因为在三维点云 ...
移动自组网络的时间校准与三维空间定位(MATLAB仿真)
移动自组网络的时间校准与空间定位(MATLAB仿真) 文章目录移动自组网络的时间校准与空间定位(MATLAB仿真) 0 问题 0.1 问题描述 0.2 假设 1 时间校准 1.1 数据组织格式(邻接 ...
matlab教程蚁群算法,蚁群算法怎样用MATLAB仿真
蚁群算法采用matlab开发的仿真平台:算法实现,路径显示,人机交互控制等希望对你有帮助! 是可以运行的 % the procedure of ant colony algorithm for ...
m瑞利信道下对比ZF-SIC,MMSE-SIC,MRC三种均衡算法的误码率matlab仿真
目录 1.算法描述 2.仿真效果预览 3.MATLAB核心程序 4.完整MATLAB 1.算法描述移动通信系统的性能在很大程度上决定于无线信道的特性.单发单收系统无线信道的特性已经研究得很透彻,针对 ...
m分集2跳OFDM系统中基于功率分配和子载波配对算法的信道容量matlab仿真
目录 1.算法描述 2.仿真效果预览 3.MATLAB部分代码预览 4.完整MATLAB程序 1.算法描述随着当代无线通信事业的迅猛发展,无线频谱资源已显得越来越匮乏,传统固定静态的无线频谱分配模式 ...
笛卡尔空间直线轨迹规划——S型曲线加减速插补算法（含MATLAB仿真验证代码）
写这个算法,是因为博主正在做一个机械臂和全向小车的项目,里面涉及需要笛卡尔空间做直线轨迹的规划.通常的算法有梯型加减速,这个算法只是速度连续,加速度并不连续.所以实际冲击较大,无法实现柔性控制.况且网 ...
【PTS】OFDM中PAPR抑制算法PTS的matlab仿真
1.软件版本 matlab2017b 2.本算法理论知识在PTS全局搜索的时候,预先设定好一个门限值,如果搜索到那么就直接退出搜索. 所以使用这个方法,可以进行快速的搜索,而不用像全局搜索那样,全 ...
【ACF和AMDF】基于ACF和AMDF合作算法的语音编码matlab仿真
1.软件版本 matlab2013b 2.系统设计概述首先,整体框架的基本构架如下所示: 编码: 解码: 传统的AMDF算法或者ACF算法都无法满足实际的需求,这里,通过计算AMDF/ACF值或者A ...

PRM算法三维地图matlab仿真

PRM算法三维地图matlab仿真相关推荐

最新文章

热门文章