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
| %astart.m
map=im2bw(imread('map1.bmp')); % input map read from a bmp file. for new maps write the file name here，灰度图转为二值图
source=[10 10]; % source position in Y, X format，根节点坐标
goal=[490 490]; % goal position in Y, X format，目标节点坐标
stepsize=20; % size of each step of the RRT，步长
disTh=20; % nodes closer than this threshold are taken as almost the same
maxFailedAttempts = 10000;
display=true; % display of RRT
%%%%% parameters end here %%%%%
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 rooted at the source, representation node and parent index
failedAttempts=0;
counter=0;
pathFound=false;
while failedAttempts<=maxFailedAttempts % loop to grow RRTs
if rand < 0.5
sample=rand(1,2) .* size(map); % random sample，随机生成元素，元素属于0到1.令矩阵对应元素相乘，生成随机点。
else
sample=goal; % sample taken as goal to bias tree generation to goal
end
[A, I]=min( distanceCost(RRTree(:,1:2),sample) ,[],1); % find closest as per the function in the metric node to the sample,找距离最小的元素，A表示距离，I表示索引
closestNode = RRTree(I(1),1:2);%I表示索引，返回最近节点坐标
theta=atan2(sample(1)-closestNode(1),sample(2)-closestNode(2)); % direction to extend sample to produce new node
newPoint = double(int32(closestNode(1:2) + stepsize * [sin(theta) cos(theta)]));%%根据步长，线性拓展新节点
if ~checkPath(closestNode(1:2), newPoint, map) % if extension of closest node in tree to the new point is feasible可行的
failedAttempts=failedAttempts+1;
continue;
end
if distanceCost(newPoint,goal)<disTh, pathFound=true;break; end % goal reached，如果新节点和目标节点距离在一定范围内，认为找到了目标
[A, I2]=min( distanceCost(RRTree(:,1:2),newPoint) ,[],1); % check if new node is not already pre-existing in the tree
if distanceCost(newPoint,RRTree(I2(1),1:2))<disTh, failedAttempts=failedAttempts+1;continue; end
RRTree=[RRTree;newPoint I(1)]; % add node，拓展RRTree，I[1]其实表达是他的父节点
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
path=[goal];
prev=I(1);
while prev>0
path=[RRTree(prev,1:2);path];%%获取path，即从目标开始追溯每个父节点
prev=RRTree(prev,3);%%代表父节点
end
pathLength=0;
for i=1:length(path)-1, pathLength=pathLength+distanceCost(path(i,1:2),path(i+1,1:2)); end
fprintf('processing time=%d \nPath Length=%d \n\n', toc,pathLength); %%输出距离总长，停止计时
imshow(map);rectangle('position',[1 1 size(map)-1],'edgecolor','k');
line(path(:,2),path(:,1));%%绘制最终的多段线。
|