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

150 阅读 0 评论 99 点赞

我是靠谱客的博主喜悦面包，这篇文章主要介绍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 nn Path3 Length=%d nn', 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'); %蓝线表示改进后

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
%% 初始化
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 nn Path3 Length=%d nn', 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'); %蓝线表示改进后