3维RRT避障路径规划算法

function c; clc clear all close all %map1 随机地表。 % a=10; % b=0.2; % c=0.1; % d=0.6; % e=1; % f=0.1; % g=0.1; % for x=1:80 % for y=1:80 % Z1=sin(y+a)+b*sin(x)+cos(d*(x^2+y^2)^(1/2))+e*cos(y)+f*sin(f*(x^2+y^2)^(1/2))+g*cos(y); % % Z1=SquareDiamond(6,2,8); % figure(1); % surf(Z1); %画出三维曲面 % shading flat; %各小曲面之间不要网格 % %map2 山峰图 tic; h=[20,35,25,38,20,25]; x0=[10,40,45,60,20,20]; y0=[10,25,50,30,45,10]; xi=[5.5,8,5,4.5,5.5,3.5]; yi=[5,7,6,5.5,6,4.5]; Z2=CeatHill(6,h,x0,y0,xi,yi,80); figure(2); surf(Z2); %画出三维曲面 shading flat; %各小曲面之间不要网格 %map3 合成图 % Z3=max(Z1,Z2); % figure(3); % surf(Z3); %画出三维曲面 % shading flat; %各小曲面之间不要网格 segmentLength =5; start_node = [5,70,5,0,0,0]; end_node =[80,30,10,1,0,0]; hold on plot3(start_node(:,1),start_node(:,2),start_node(:,3),'r*'); plot3(end_node(:,1),end_node(:,2),end_node(:,3),'r*'); tree = start_node; if ( (norm(start_node(1:3)-end_node(1:3))<segmentLength )... &(collision(start_node,end_node)==0) ) path = [start_node; end_node]; else numPaths = 0; while numPaths<1, [tree,flag] = extendTree(tree,end_node,segmentLength,Z2); numPaths = numPaths + flag; end end path = findMinimumPath(tree); plot3(path(:,1),path(:,2),path(:,3),'r'); toc; function [data]=CeatHill(N,h,x0,y0,xi,yi,num) x=1:1:num;y=1:1:num; for m=1:num for n=1:num Sum=0; for k=1:N s=h(k)*exp(-((x(m)-x0(k))/xi(k))^2-((y(n)-y0(k))/yi(k))^2); Sum=Sum+s; end data(m,n)=Sum; end end function collision_flag = collision(node, parent,Z2,high); collision_flag = 0; h=[20,35,25,38,20,25]; x0=[10,40,45,60,20,20]; y0=[10,25,50,30,45,10]; xi=[5.5,8,5,4.5,5.5,3.5]; yi=[5,7,6,5.5,6,4.5]; Z1=Z2; % for i=1:80 % for j=1:80 % if(Z1(j,i)>high) % Z1(j,i)=10000; % end % end % end if ((node(1)>80)... | (node(1)<0)... | (node(2)>80)... | (node(2)<0)) collision_flag = 1; else for sigma = 0:0.1:1, p = sigma*node(1:3) + (1-sigma)*parent(1:3); Sum1=0; for k=1:6 s=h(k)*exp(-((p(2)-x0(k))/xi(k))^2-((p(1)-y0(k))/yi(k))^2); Sum1=Sum1+s; end if(p(3)<Sum1) collision_flag = 1; end % [line,row]=find(Z1==10000); % ct=length(line); % for tt=1:ct % B=floor(p(1)); % C=floor(p(2)); % if (B==line(tt,1)&C==row(tt,1)) % collision_flag = 1; % end % end end end function [new_tree,flag,high] = extendTree(tree,end_node,segmentLength,Z2); flag1 = 0; qet=1; while flag1==0, % select a random point if(qet==0) randomPoint = [... 80*rand,... 80*rand,... 80*rand]; else randomPoint = [... end_node(:,1),... end_node(:,2),... end_node(:,3)]; end % find leaf on node that is closest to randomPoint %0.45*sqrt(tree(:,1:2)-ones(size(tree,1),1)*randomPoint) tmp = sqrt(tree(:,1:3)-ones(size(tree,1),1)*end_node(:,1:3)); [dist,idx] = min(diag(tmp*tmp')); A=end_node(:,1:2)-tree(idx,1:2); B=randomPoint(:,1:2)-tree(idx,1:2); agl=acos((dot(A,B)/(norm(A)*norm(B))))*180/pi; if (agl>80) continue; end cost= tree(idx,4) + segmentLength; new_point = (randomPoint-tree(idx,1:3)); new_point = tree(idx,1:3)+new_point/norm(new_point)*segmentLength; if(qet==1) high=new_point(:,3); end if(tree(idx,3)==end_node(:,3)) new_point(:,3)=end_node(:,3); end if(tree(idx,3)>end_node(:,3)) new_point(:,3)=tree(idx,3)-new_point(:,3)/norm(new_point)*segmentLength; end new_node = [new_point, 0, cost, idx]; % hold on if collision(new_node, tree(idx,:),Z2,high)==0, new_tree = [tree; new_node]; % plot3(new_point(:,1),new_point(:,2),new_point(:,3),'r*'); flag1=1; else qet=0; flag1=0; end end % check to see if new node connects directly to end_node if ( (norm(new_node(1:3)-end_node(1:3))<segmentLength )... &(collision(new_node,end_node,Z2,high)==0) ) flag = 1; new_tree(end,4)=1; % mark node as connecting to end. end没找到 else flag = 0; end function path = findMinimumPath(tree); connectingNodes = []; for i=1:size(tree,1), if tree(i,4)==1, connectingNodes = [connectingNodes; tree(i,:)]; end end % find minimum cost last node [tmp,idx] = min(connectingNodes(:,5)); % construct lowest cost path path = [connectingNodes(idx,:)]; parent_node = connectingNodes(idx,6); while parent_node>1, path = [tree(parent_node,:); path]; parent_node = tree(parent_node,6); end