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)<disTh, pathFound=true;break; end % 檢測新結點是否到達目標點，即小于一定的閾值
    [A, I2]=min( distanceCost(RRTree(:,1:2),newPoint) ,[],1); % 檢測檢點是否已經存在樹結點中
    if distanceCost(newPoint,RRTree(I2(1),1:2))<disTh, failedAttempts=failedAttempts+1;continue; end   %如果新結點在樹結點中，記失敗一次
    RRTree=[RRTree;newPoint I]; % 將新結點介入到如結點中
    failedAttempts=0;
    % 每擴展一個新結點，畫一條線
    if display, 
        line([closestNode(2);newPoint(2)],[closestNode(1);newPoint(1)]);
        counter=counter+1;M(counter)=getframe;
    end
end
% 補充最后一個新結點和終點的連線
if display && pathFound 
    line([closestNode(2);goal(2)],[closestNode(1);goal(1)]);
    counter=counter+1;M(counter)=getframe;
end
if display 
    disp('click/press any key');
    waitforbuttonpress; 
end
if ~pathFound, error('no path found. maximum attempts reached'); end

%% 優化最終軌跡

path0=[goal];
prev0=I;
while prev0>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<j+2
if ~checkPath(path(m,1:2),path(m+2,1:2), map) 
    path2(n+1,1:2)=path(m+1,1:2);
    m=m+1;
else
    path2(n+1,1:2)=path(m+2,1:2);
    m=m+2;
end
    n=n+1;
end
                                                                            %在path基礎上再次兩兩合一之后path2

path3=[n+3  2];
path3(1,1:2)=path2(1,1:2);
o=1;
p=1;
while o<n-1
if ~checkPath(path2(o,1:2),path2(o+2,1:2), map) 
    path3(p+1,1:2)=path2(o+1,1:2);
    o=o+1;
else
    path3(p+1,1:2)=path2(o+2,1:2);
    o=o+2;
end
    p=p+1;
end

path3(p+1,1:2)=goal;
                                                                %在path2基礎上兩兩合一，path3

%% 顯示時間和路徑長度
pathLength=0;
pathLength1=0;
for i=1:length(path0)-1, pathLength = pathLength + distanceCost(path0(i,1:2),path0(i+1,1:2)); end % calculate path length
for k=1:length(path3)-1, pathLength1 = pathLength1 + distanceCost(path3(k,1:2),path3(k+1,1:2)); end % calculate path3 length
fprintf('processing time=%d \nPath0 Length=%d \n\n Path3 Length=%d \n\n', toc,pathLength,pathLength1);   % 列印運行時間toc和改進前后路徑長度
imshow(map);rectangle('position',[1 1 size(map)-1],'edgecolor','b');

line(path0(:,2),path0(:,1),'Color','r'); %紅線表示改進前
hold on 
line(path3(:,2),path3(:,1), 'Color','b'); %藍線表示改進后