1 简介
2 部分代码
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=[20,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];
h=[20,35,25,38,20,25];
x0=[20,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 = [10,80,5,0,0,0];
end_node =[60,0,5,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
3 仿真结果
4 参考文献
[1]师颖慧, 张冰, 赵强. 基于改进型RRT*算法的水下机器人三维全局路径规划方法:.
