三维A星路径规划

admin

[mw_shl_code=applescript,true]% 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;
[numberOfSphere, ~] = size(obstacleMatrix);
% [numberOfCylinder, ~] = size(cylinderMatrix);
Alldirec = [[1,0,0];[0,1,0];[0,0,1];[-1,0,0];[0,-1,0];[0,0,-1]];%设置六个搜索方向
threshold = 1.0;
stop = threshold*1.5;
g = [start, 0; goal, inf]; % 每一行前三个数为点坐标，第四个数为路径耗散
Path = [];%路径
Parent = [];%父节点数组
Open = [start, g(findIndex(g,start),4) + getDist(start,goal)];%开集合数组
%% 绘制障碍环境
figure(1)
for i=1:300        %三百个随机障碍物
    obstacleMatrix(i,1)=randi([1,9]);
    obstacleMatrix(i,2)=randi([1,9]);
    obstacleMatrix(i,3)=randi([1,9]);
end

%% 主循环：开始计算路径
while ~isempty(Open)
    [xi, index] = 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;
    [n,~] = size(children);
    for i = 1:n   %开始判断搜索到相邻路径点的合理性
        child = children(i,;
        if findIndex(g, child) == 0   % child不在g
            g = [g; child, inf];
        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;
[n,~] = size(Parent);
while any(x ~= start)
    for i = 1:n
        if Parent(i,1:3) == x
            Path = [Parent(i,4:6); Path];
            break;
        end
    end
    x = Parent(i,4:6);
end
plot3([Path(:,1);goal(1)],[Path(:,2);goal(2)],[Path(:,3);goal(3)],'LineWidth',3,'color','r');%画出路径点
hold on;
plot3(obstacleMatrix(:,1),obstacleMatrix(:,2),obstacleMatrix(:,3),'o');%画出障碍物点
%% 计算轨迹距离
pathLength = 0;
[n,~] = 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 = [];
[n,~] = size(Alldirec);
for i = 1:n
    direc = Alldirec(i,;
    child = pos + direc * step;
    if ~checkCol(child, circleCenter)%判断搜索到的路径点是否是障碍物点
         continue;
    end
    allchild = [allchild; child];%收集非障碍物点的路径点
end
children = allchild;
end

function flag = checkCol(pos, circleCenter)%判断搜索到的路径点是否是障碍物点
[numberOfSphere, ~] = 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)
[n,~] = size(Parent);
if n == 0
    Par = [xj, xi];
else
    for i = 1:n
        if Parent(i,1:3) == xj
            Parent(i,4:6) = xi;
            Par = Parent;
            break;
        end
        if i == n
            Par = [Parent; xj, xi];
        end
    end
end
end

function Ope = setOpen(Open, child, a, goal)
[n,~] = size(Open);
if n == 0
    Ope = [child, a + getDist(child, goal)];
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 = [Open; child, a + getDist(child, goal)];
        end
    end
end
end

function h = heuristic(pos, goal)
h = max([abs(goal(1) - pos(1)),abs(goal(2) - pos(2)),abs(goal(3) - pos(3))]);
end

function index = findIndex(g, pos)
[n,~] = 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 [pos, index] = findMin(Open)
[~,index] = min(Open(:,4));
pos = Open(index,1:3);
end
[/mw_shl_code]