RRT算法的一种关于最终路径的创新matlab仿真

代码的前半段基础是用的别人的，后面一半的改进是自己写的 

%% 初始化
map=im2bw(imread('map2.bmp')); % bmp无损压缩图像500x500,im2bw把灰度图转换成二值图像01
source=[10 10]; % 起始点位置
goal=[490 490]; % 目标点位置
stepsize=20; % RRT每步步长
disTh=20; % 直到qnearest和目标点qgaol距离小于一个阈值
maxFailedAttempts = 10000;  % 最大尝试次数
display=true; % RRT是否展示

%%  %%%% 参数 %%%%%

tic;  % 保存当前时间
if ~feasiblePoint(source,map), error('source lies on an obstacle or outside map'); end
if ~feasiblePoint(goal,map), error('goal lies on an obstacle or outside map'); end
if display, imshow(map);rectangle('position',[1 1 size(map)-1],'edgecolor','k'); end  %展示图像，并创建带有尖角的矩形边框
RRTree=double([source -1]); % RRT 从起点开始（索引为-1），经过的结点和索引
failedAttempts=0;  % 已经尝试失败的次数
counter=0;  % 循环计数
pathFound=false;  % 是否找到路径的flag
while failedAttempts<=maxFailedAttempts  % RRT循环
    if rand < 0.5 
        sample=rand(1,2) .* size(map);   % 50%几率随机采点
    else
        sample=goal; % 50%几率向目标前进
    end
    
    % 每一个分支都会继续分支
    [A, I]=min( distanceCost(RRTree(:,1:2),sample) ,[],1); % 发现结点和随机采样点最小距离的一行，并返回对应索引,[],1可以去掉
    closestNode = RRTree(I,1:2); %树结点最近点坐标，最近点可能多个(1)不可取
    theta=atan2(sample(1)-closestNode(1),sample(2)-closestNode(2));  % 产生新结点的方向
    newPoint = double(int32(closestNode(1:2) + stepsize * [sin(theta)  cos(theta)]));  % 产生新结点，先计算纵坐标，再计算横坐标
    
    if ~checkPath(closestNode(1:2), newPoint, map) % 检测最近结点到新结点的路径是否可行
        failedAttempts=failedAttempts+1;
        continue;
    end
    if distanceCost(newPoint,goal)0
    path0=[RRTree(prev0,1:2);path0];
    prev0=RRTree(prev0,3);
end                                                %最初轨迹path0

path=[goal];

prev=I;
path=[RRTree(prev,1:2);path];
prev1=RRTree(prev,3);
prev=RRTree(prev1,3);
j=2;
while prev>2
 path1=path;
 path=[RRTree(prev,1:2);path];
         if ~checkPath(path(j,1:2),path(j+1,1:2), map) % 检测新路径是否可行
               path=path1;
               path=[RRTree(prev1,1:2);path];
               path=[RRTree(prev,1:2);path];
         else
             path=path;
         end
               prev=RRTree(prev,3);
               prev=RRTree(prev,3);
               prev1=RRTree(prev1,3);
               prev1=RRTree(prev1,3);
         j=j+1;
end

 path=[RRTree(3,1:2);path];
 path=[RRTree(2,1:2);path];
 path=[source;path];
                                                                                    %尝试每两个节点合一之后轨迹path

path2=[j+3  2];
path2(1,1:2)=path(1,1:2);
m=1;
n=1;
while m