三维A星路径规划
% A* algorithm实现clc; clear; close all;
obstacleMatrix=zeros(300,3);
RobstacleMatrix=zeros(5,1);
for i=1:1:5
RobstacleMatrix(i,1)=0.001;
end
% for i=1:1:5
% obstacleMatrix(i,1)=3+i;
% obstacleMatrix(i,2)=3+i;
% obstacleMatrix(i,3)=3+i;
% end
start = zeros(1,3); %设置起点
start(1,1)=0;
start(1,2)=0;
start(1,3)=0;
goal = zeros(1,3);%设置目标点
goal(1,1)=10;
goal(1,2)=10;
goal(1,3)=10;
= size(obstacleMatrix);
% = size(cylinderMatrix);
Alldirec = [;;;[-1,0,0];;];%设置六个搜索方向
threshold = 1.0;
stop = threshold*1.5;
g = ; % 每一行前三个数为点坐标，第四个数为路径耗散
Path = [];%路径
Parent = [];%父节点数组
Open = ;%开集合数组
%% 绘制障碍环境
figure(1)
for i=1:300 %三百个随机障碍物
obstacleMatrix(i,1)=randi();
obstacleMatrix(i,2)=randi();
obstacleMatrix(i,3)=randi();
end
%% 主循环：开始计算路径
while ~isempty(Open)
= findMin(Open);
Open(index,:) = [];
if getDist(xi, goal) < stop%判断是否找到目标
break;
end
children = getChildren(xi, Alldirec, threshold, obstacleMatrix);%计算当前点所有的相邻路径点
% scatter3(children(:,1),children(:,2),children(:,3),10,'filled','o');
drawnow;
= size(children);
for i = 1:n %开始判断搜索到相邻路径点的合理性
child = children(i,:);
if findIndex(g, child) == 0 % child不在g
g = ;
end
a = g(findIndex(g, xi),4) + getDist(xi,child);
if a < g(findIndex(g, child),4)
g(findIndex(g, child),4) = a;
Parent = setParent(Parent, child,xi);
Open = setOpen(Open, child, a, goal);
end
end
end
lastPoint = xi;
%% 回溯轨迹
x = lastPoint;
Path = x;
= size(Parent);
while any(x ~= start)
for i = 1:n
if Parent(i,1:3) == x
Path = ;
break;
end
end
x = Parent(i,4:6);
end
plot3(,,,'LineWidth',3,'color','r');%画出路径点
hold on;
plot3(obstacleMatrix(:,1),obstacleMatrix(:,2),obstacleMatrix(:,3),'o');%画出障碍物点
%% 计算轨迹距离
pathLength = 0;
= size(Path);
for i = 1:n-1
pathLength = pathLength + getDist(Path(i,:),Path(i+1,:));
end
pathLength = pathLength + getDist(Path(end,:),goal);
fprintf('路径的长度为:%f',pathLength);
%% 函数
function children = getChildren(pos, Alldirec, step,circleCenter)
allchild = [];
= size(Alldirec);
for i = 1:n
direc = Alldirec(i,:);
child = pos + direc * step;
if ~checkCol(child, circleCenter)%判断搜索到的路径点是否是障碍物点
continue;
end
allchild = ;%收集非障碍物点的路径点
end
children = allchild;
end
function flag = checkCol(pos, circleCenter)%判断搜索到的路径点是否是障碍物点
= size(circleCenter);
flag = true;
for i = 1:numberOfSphere
if (pos(1,1)==circleCenter(i,1))&& (pos(1,2)==circleCenter(i,2))&& (pos(1,3)==circleCenter(i,3))
flag = false;
break;
end
end
end
function Par = setParent(Parent, xj, xi)
= size(Parent);
if n == 0
Par = ;
else
for i = 1:n
if Parent(i,1:3) == xj
Parent(i,4:6) = xi;
Par = Parent;
break;
end
if i == n
Par = ;
end
end
end
end
function Ope = setOpen(Open, child, a, goal)
= size(Open);
if n == 0
Ope = ;
else
for i = 1:n
if Open(i,1:3) == child
Open(i,4) = a + getDist(child, goal);
Ope = Open;
end
if i == n
Ope = ;
end
end
end
end
function h = heuristic(pos, goal)
h = max();
end
function index = findIndex(g, pos)
= size(g);
index = 0; % 表示没有找到索引
for i = 1:n
if g(i,1:3) == pos
index = i; % 索引为i
break;
end
end
end
function d = getDist(x,y)
d = sqrt(sum((x - y).^2));
end
function = findMin(Open)
[~,index] = min(Open(:,4));
pos = Open(index,1:3);
end
页:
[1]