RRT算法matlab实现（未改进）

基本思想

1.在已知的静态地图中随机生成点，可以预设这个点是目标点的概率使其随机性减小

2.寻找随机生成点的最近点，连接并在最近点延时固定步长，判断是否与障碍物碰撞

3.重复上述，直到离终点一个范围，到达终点

代码

clear()
ob=[10,10;50,80;70,20;40,40;80,50];
ob_r=[3;5;10;6;5];%% map setting
rows=100;
cols=100;
start=[5,5];
goal=[100,100];
figure(1)
plot(start(1),start(2),'*r');
hold on
plot(goal(1),goal(2),'*g');
%obstacle draw
aplha=0:pi/40:2*pi;
for ob_r_count=1:length(ob_r)x=ob(ob_r_count,1)+ob_r(ob_r_count)*cos(aplha);y=ob(ob_r_count,2)+ob_r(ob_r_count)*sin(aplha);plot(x,y,'-');axis equal
end
%% parameter setting
current_point=start;
current_parent=[];
current_cost=[];
tree_point=start;
tree_parent=start;
tree_cost=0;
step=5;%% loop
while 1if current_point==goal%node recallline([current_point(1),parent_point(1)],[current_point(2),parent_point(2)]);breakend%generate random dot trace,the goal point chance is 10%rand_count=rand(1);if rand_count<=0.1new_point=goal;elsenew_point=rand(1,2)*diag([rows,cols],0);end%find parent pointparent_point=parent(new_point,tree_point);point=parent_point+step*(new_point-parent_point)/norm(parent_point-new_point);%judge the distance from obstaclesif distance(ob,ob_r,point,parent_point,step)plot(point(1),point(2),'.b',parent_point(1),parent_point(2),'.b');line([point(1),parent_point(1)],[point(2),parent_point(2)]);%pause(0.2);%animationcurrent_point=point;tree_point=[tree_point;point];tree_parent=[tree_parent;parent_point];tree_cost=[tree_cost,];endif norm(current_point-goal)<=steptree_parent=[tree_parent;current_point];current_point=goal;tree_point=[tree_point;current_point];end
end%% function parent
function parent_point=parent(new_point,tree_point)
point=dsearchn(tree_point,new_point);
parent_point=tree_point(point,:);
end
%% function distance
function feasibility=distance(ob,ob_r,point,parent_point,step)
feasibility=1;
for count2=1:length(ob_r)if norm(point-ob(count2,:))<=ob_r(count2)feasibility=0;breakendline=ob(count2,:)-parent_point;line1=point-parent_point;if norm(line)<=step+ob_r(count2) && abs(sum(line.*line1)/(norm(line)*norm(line1)))>=(norm(line)^2-ob_r(count2)^2)^0.5/norm(line)feasibility=0;end
end
end

RRT算法matlab实现（未改进）相关推荐

风电功率预测优化算法MATLAB程序基于改进神经网络
风电功率预测优化算法MATLAB程序基于改进神经网络 (1) 该程序为基于改进神经网络的风电功率预测优化算法程序,风电预测程序,期刊论文源程序,配有该论文. (2) 该程序所用的ICA-BP 神经网络 ...
【RRT三维路径规划】基于matlab RRT算法无人机三维路径规划【含Matlab源码 155期】
一.获取代码方式获取代码方式1: 通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码. 获取代码方式2: 完整代码已上传我的资源:[三维路径规划]基于matlab RRT算法无人机三维 ...
【RRT三维路径规划】基于matlab RRT算法无人机三维路径规划【含Matlab源码 1363期】
一.获取代码方式获取代码方式1: 通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码. 获取代码方式2: 完整代码已上传我的资源:[三维路径规划]基于matlab RRT算法无人机三维 ...
最优隐神经元数目算法 matlab,BP算法的改进在Matlab的实现研究
BP 算法的改进在M a tlab 的实现研究姚文俊 (中南民族大学电子信息工程学院湖北武汉 430074) 摘要:利用M atlab 中的神经网络工具箱提供的丰富网络学习和训练函数,对BP 网 ...
改进后的A星三维路径规划完整算法(matlab语言)，包括障碍物模型优化
改进后的A星三维路径规划完整算法(matlab语言),包括障碍物模型优化,平滑处理,启发函数的改进,环境地图可以根据自己的实际情况进行改进,算法包含了非常详细的代码注释 ID:695006710638 ...
改进后的A星三维路径规划完整算法(matlab语言)
改进后的A星三维路径规划完整算法(matlab语言),包括障碍物模型优化,平滑处理,启发函数的改进,环境地图可以根据自己的实际情况进行改进,算法包含了非常详细的代码注释 YID:69500671063 ...
DG储能选址定容模型matlab 程序采用改进粒子群算法，考虑时序性得到分布式和储能的选址定容模型
DG储能选址定容模型matlab 程序采用改进粒子群算法,考虑时序性得到分布式和储能的选址定容模型,程序运行可靠 YID:87140641990659957爱熬夜的程序猿
DG储能选址定容模型matlab 程序采用改进粒子群算法
DG储能选址定容模型matlab 程序采用改进粒子群算法,考虑时序性得到分布式和储能的选址定容模型,程序运行可靠编号:75140641990659957爱熬夜的程序猿
DG储能选址定容模型matlab 程序采用改进粒子群算法，考虑时序性得到分布式和储能的选址定容模型，程序运行可靠
DG储能选址定容模型matlab 程序采用改进粒子群算法,考虑时序性得到分布式和储能的选址定容模型,程序运行可靠 YID:87140641990659957爱熬夜的程序猿

RRT算法matlab实现（未改进）

基本思想

代码

RRT算法matlab实现（未改进）相关推荐

最新文章

热门文章