ZIP传统rrt算法仿真代码 71.52KB

winner0111

资源文件列表:

传统RRT.zip 大约有4个文件
  1. 传统RRT/demo.png 70.89KB
  2. 传统RRT/demo1.m 2.63KB
  3. 传统RRT/distance.m 61B
  4. 传统RRT/ob.m 887B

资源介绍:

传统rrt算法仿真代码
clc clear all [f,n1]=ob(10);%随机生成障碍物 Xinit=[1,1];%定义初始点 Xgoal=[95,90];%定义目标点 plot(Xinit(1),Xinit(2),'ro'); plot(Xgoal(1),Xgoal(2),'ko'); T=[Xinit(1),Xinit(2)];%已生成节点集合用顺序表的数据结构存储 Xnew=Xinit; D(1)=0;%初始节点的父节点指向0 while distance(Xnew(1),Xnew(2),Xgoal(1),Xgoal(2))>3 %进入目标区域 Xrand=round(rand(1,2)*100)+1;%状态采样空间：横纵坐标均为整数，范围1~100 k=1;%进入循环 while k==1 k=0;%初始化采样成功 for i=1:n1 if distance(Xrand(1),Xrand(2),f(i*3-2),f(i*3-1))<(f(i*3)+1)%判断随机采样点是否在障碍物内 k=1;%采样不成功 break; end end Xrand=round(rand(1,2)*100);%重新采样 end min=100;%该参数对路径方向有影响 for i=1:size(T,2)/2 %遍历已生成节点集合 if distance(T(2*i-1),T(2*i),Xrand(1),Xrand(2))<min %得到最近的节点 min=distance(T(2*i-1),T(2*i),Xrand(1),Xrand(2)); Xnear=[T(2*i-1),T(2*i)]; end end stepsize=3/distance(Xnear(1),Xnear(2),Xrand(1),Xrand(2));%以一定步长前进，这里选择3 Xnew=[Xrand(1)*stepsize+Xnear(1)*(1-stepsize),Xrand(2)*stepsize+Xnear(2)*(1-stepsize)]; t=0;%初始状态未碰撞 if Xnear(1)>Xnew(1) caiyang=-0.001; else caiyang=0.001; end for i=Xnear(1):caiyang:Xnew(1)%均匀采样进行碰撞检测 for j=1:n1 if distance(f(3*j-2),f(3*j-1),i,Xnear(2)+(i-Xnear(1))/(Xnew(1)-Xnear(1))*(Xnew(2)-Xnear(2)))<=(f(3*j)+1) t=1;%代表碰撞 break; end end if t==1 break; end end if t==0 T=[T,Xnew(1),Xnew(2)]; for i=1:size(T,2)/2 %遍历已生成节点集合 if (T(i*2-1)==Xnear(1))&&(T(i*2)==Xnear(2)) %得到最近的节点的索引 D(size(T,2)/2)=i;%记录父节点 break; end end plot([Xnew(1),Xnear(1)],[Xnew(2),Xnear(2)],'b-');hold on;pause(0.005); plot(Xnew(1),Xnew(2),'r.');xlim([0,100]);ylim([0,100]); end end i=size(T,2)/2; jg=[i]; while D(i) i=D(i); %通过链表回溯 if D(i)~=0 jg=[D(i),jg];%存储最短路径通过的节点 end end Fx=T(jg(1)*2-1);Fy=T(jg(1)*2); i=2; while jg(i)~=size(T,2)/2 x=T(jg(i)*2-1); y=T(jg(i)*2); plot([x,Fx],[y,Fy],'g-');hold on;pause(0.05); Fx=x;Fy=y; i=i+1; end plot([T(jg(i)*2-1),Fx],[T(jg(i)*2),Fy],'g-');hold on;pause(0.05);
100+评论
captcha
    相关资源
    类型标题大小时间
    ZIP此文件为作者自己制作C语言c++入门到进阶课无偿领取，输入密码qqdzy即可获取89.27MB8月前
    ZIPautohidedesk自动隐藏桌面图标89.13KB8月前
    ZIP滑板 滑板 滑板 滑板 滑板 滑板21.91MB8月前
    ZIP基于PythonFlaskWeb人工智能恶意文件检测系统框架 html + css + jquery + python 37.14MB8月前
    ZIP必修三简约版(2).zip7.87MB8月前
    ZIP目标检测目标检测目标检测目标检测目标检测目标检测34.51MB8月前
    ZIP网易云评论数据获取（代码+js）121.81KB8月前
    ZIP冰蝎2.0源码编译可运行48.15MB8月前