RRT算法的仿真
% % An example of rapidly-exploring random trees and path planning in 2-D
% % Ref: "Rapidly-Exploring Random Trees: A New Tool for Path Planning",
% % Steven M. LaValle, 1998
%~~~~
% Code can also be converted to function with input format
% [tree, path] = RRT(K, xMin, xMax, yMin, yMax, xInit, yInit, xGoal, yGoal, thresh)
% K is the number of iterations desired.
% xMin and xMax are the minimum and maximum values of x
% yMin and yMax are the minimum and maximum values of y
% xInit and yInit is the starting point of the algorithm
% xGoal and yGoal are the desired endpoints
% thresh is the allowed threshold distance between a random point the the
% goal point.
% Output is the tree structure containing X and Y vertices and the path
% found obtained from Init to Goal
%~~~~
% Written by: Omkar Halbe, Technical University of Munich, 31.10.2015
%~~~~
clear all; close all;
K=2000;
xMin=0; xMax=100;
yMin=0; yMax=100;
xInit=0; yInit=0; %initial point for planner
xGoal=100; yGoal=100; %goal for planner
thresh=5; %acceptable threshold for solution
tree.vertex(1).x = xInit;
tree.vertex(1).y = yInit;
tree.vertex(1).xPrev = xInit;
tree.vertex(1).yPrev = yInit;
tree.vertex(1).dist=0;
tree.vertex(1).ind = 1; tree.vertex(1).indPrev = 0;
xArray=xInit; yArray = yInit;
figure(1); hold on; grid on;
plot(xInit, yInit, 'ko', 'MarkerSize',10, 'MarkerFaceColor','k');
plot(xGoal, yGoal, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');
for iter = 2:K
xRand = (xMax-xMin)*rand;
yRand = (yMax-yMin)*rand;
dist = Inf*ones(1,length(tree.vertex));
for j = 1:length(tree.vertex)
dist(j) = sqrt( (xRand-tree.vertex(j).x)^2 + (yRand-tree.vertex(j).y)^2 );
end
[val, ind] = min(dist);
tree.vertex(iter).x = xRand; tree.vertex(iter).y = yRand;
tree.vertex(iter).dist = val;
tree.vertex(iter).xPrev = tree.vertex(ind).x;
tree.vertex(iter).yPrev = tree.vertex(ind).y;
tree.vertex(iter).ind = iter; tree.vertex(iter).indPrev = ind;
if sqrt( (xRand-xGoal)^2 + (yRand-yGoal)^2 ) <= thresh
plot([tree.vertex(iter).x; tree.vertex(ind).x],[tree.vertex(iter).y; tree.vertex(ind).y], 'r');
break
end
plot([tree.vertex(iter).x; tree.vertex(ind).x],[tree.vertex(iter).y; tree.vertex(ind).y], 'r');
pause(0);
end
if iter < K
path.pos(1).x = xGoal; path.pos(1).y = yGoal;
path.pos(2).x = tree.vertex(end).x; path.pos(2).y = tree.vertex(end).y;
pathIndex = tree.vertex(end).indPrev;
j=0;
while 1
path.pos(j+3).x = tree.vertex(pathIndex).x;
path.pos(j+3).y = tree.vertex(pathIndex).y;
pathIndex = tree.vertex(pathIndex).indPrev;
if pathIndex == 1
break
end
j=j+1;
end
path.pos(end+1).x = xInit; path.pos(end).y = yInit;
for j = 2:length(path.pos)
plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
% plot([tree.vertex(i).x; tree.vertex(ind).x],[tree.vertex(i).y; tree.vertex(ind).y], 'r');
% pause(0);
end
else
disp('No path found. Increase number of iterations and retry.');
end
相关文章
- C#数据结构与算法揭秘十
- Java实现 蓝桥杯 算法提高 歌唱比赛
- Java实现 蓝桥杯VIP 算法训练 大小写判断
- Java实现 蓝桥杯VIP 算法训练 集合运算
- DL之模型调参:深度学习算法模型优化参数之对深度学习模型的超参数采用网格搜索进行模型调优(建议收藏)
- DL之FCN:FCN算法的简介(论文介绍)、架构详解、案例应用等配图集合之详细攻略
- 车间调度|基于帝王蝶优化算法的车间调度(Matlab代码实现)
- 基于SOA海鸥优化算法的二阶时滞系统PID控制器最优参数计算matlab仿真
- 基于人工鱼群优化的电网规划算法matlab仿真
- 基于蚁群优化算法的三维路径规划算法matlab仿真
- 基于L2-RLS算法的目标跟踪算法matlab仿真,可处理小范围遮挡问题
- 基于LGC局部全局一致性和SVM支持向量机的半监督分类算法matlab仿真
- 智能优化算法应用:基于麻雀搜索算法的水文地质参数优化 -附代码
- 智能优化算法:原子搜索优化算法 -附代码
- 【算法】打靶问题求解
- 排序算法
- 算法工程师面试题十二之LSTM简析
- 暑假加餐|有钱人和你想的不一样(第20天)+改进的多目标差分进化算法在电力系统环境经济调度中的应用(Python代码实现)
- 基于平均不同分辨率的共振峰跟踪算法matlab仿真
- m基于matlab的polar码误码率仿真,译码算法采用SC算法
- m基于大衍数无高阶环稀疏校验矩阵H构造算法和RMP消息传递的QC-LDPC性能matlab仿真
- 基于目标运动模型和观测模型的交互多模算法IMM卡尔曼滤波目标跟踪matlab仿真
- Opencv之---棋盘格角点检测算法源码解析
- 每日算法之旅(第三天):分而治之算法思想
- Python 3 标准库 base64 加密与解密算法