传统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))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);