【路径规划-机器人栅格地图】基于A星算法结合Floyd实现机器人栅格地图动态路径规划附Matlab代码

% A*算法结合Floyd算法实现机器人栅格地图动态路径规划

% 定义栅格地图
gridMap = [
0, 0, 0, 0, 0, 0, 0;
0, 1, 0, 1, 0, 1, 0;
0, 0, 0, 0, 0, 0, 0;
0, 1, 0, 1, 0, 1, 0;
0, 0, 0, 0, 0, 0, 0;
0, 1, 0, 1, 0, 1, 0;
0, 0, 0, 0, 0, 0, 0
];

% 定义起点和终点坐标
startPos = [1, 1];
goalPos = [7, 7];

% 定义栅格地图尺寸
mapSize = size(gridMap);

% 定义A*算法中的开启列表和关闭列表
openList = [];
closedList = [];

% 定义起点节点
startNode = struct(‘pos’, startPos, ‘g’, 0, ‘h’, 0, ‘f’, 0, ‘parent’, []);

% 将起点节点加入开启列表
openList = [openList, startNode];

% 定义Floyd算法中的最短路径矩阵
shortestPaths = zeros(mapSize(1)*mapSize(2));

% A*算法主循环
while ~isempty(openList)
% 在开启列表中选择f值最小的节点作为当前节点
[~, currentIdx] = min([openList.f]);
currentNode = openList(currentIdx);

% 如果当前节点为终点节点，则路径规划完成
if isequal(currentNode.pos, goalPos)
    break;
end

% 将当前节点从开启列表中移除，并加入关闭列表
openList(currentIdx) = [];
closedList = [closedList, currentNode];

% 获取当前节点周围的邻居节点
neighbors = getNeighbors(currentNode.pos, mapSize);

for i = 1:numel(neighbors)
    neighbor = neighbors(i);
    
    % 如果邻居节点在关闭列表中，则跳过
    if isInList(neighbor, closedList)
        continue;
    end
    
    % 计算邻居节点的g值、h值和f值
    g = currentNode.g + 1;
    h = heuristic(neighbor, goalPos);
    f = g + h;
    
    % 如果邻居节点不在开启列表中，则加入开启列表
    if ~isInList(neighbor, openList)
        neighborNode = struct('pos', neighbor, 'g', g, 'h', h, 'f', f, 'parent', currentNode);
        openList = [openList, neighborNode];
    else
        % 如果邻居节点已经在开启列表中，比较新的g值与原有g值的大小
        neighborIdx = getNodeIndex(neighbor, openList);
        if g < openList(neighborIdx).g
            openList(neighborIdx).g = g;
            openList(neighborIdx).f = g + openList(neighborIdx).h;
            openList(neighborIdx).parent = currentNode;
        end
    end
end

end

% 通过Floyd算法计算最短路径矩阵
for i = 1:mapSize(1)*mapSize(2)
for j = 1:mapSize(1)*mapSize(2)
shortestPaths(i, j) = inf;
end
end

for i = 1:numel(closedList)
node = closedList(i);
shortestPaths(node.pos(1), node.pos(2)) = node.g;
end

for k = 1:mapSize(1)*mapSize(2)
for i = 1:mapSize(1)*mapSize(2)
for j = 1:mapSize(1)*mapSize(2)
if shortestPaths(i, k) + shortestPaths(k, j) < shortestPaths(i, j)
shortestPaths(i, j) = shortestPaths(i, k) + shortestPaths(k, j);
end
end
end
end

% 根据最短路径矩阵获取路径
path = getPath(startPos, goalPos, shortestPaths);

% 绘制栅格地图和路径
figure;
hold on;
for i = 1:mapSize(1)
for j = 1:mapSize(2)
if gridMap(i, j) == 0
rectangle(‘Position’, [j-0.5, i-0.5, 1, 1], ‘FaceColor’, ‘white’);
else
rectangle(‘Position’, [j-0.5, i-0.5, 1, 1], ‘FaceColor’, ‘black’);
end
end
end

for i = 1:numel(closedList)
node = closedList(i);
rectangle(‘Position’, [node.pos(2)-0.5, node.pos(1)-0.5, 1, 1], ‘FaceColor’, ‘yellow’);
end

for i = 1:numel(path)-1
p1 = path(i);
p2 = path(i+1);
line([p1(2)-0.5, p2(2)-0.5], [p1(1)-0.5, p2(1)-0.5], ‘Color’, ‘red’, ‘LineWidth’, 2);
end

axis equal;
axis off;

% 获取邻居节点
function neighbors = getNeighbors(pos, mapSize)
neighbors = [];
row = pos(1);
col = pos(2);

if row > 1
    neighbors = [neighbors; row-1, col];
end
if row < mapSize(1)
    neighbors = [neighbors; row+1, col];
end
if col > 1
    neighbors = [neighbors; row, col-1];
end
if col < mapSize(2)
    neighbors = [neighbors; row, col+1];
end

end

% 判断节点是否在列表中
function result = isInList(node, list)
result = false;
for i = 1:numel(list)
if isequal(node, list(i).pos)
result = true;
break;
end
end
end

% 获取节点在列表中的索引
function index = getNodeIndex(node, list)
index = -1;
for i = 1:numel(list)
if isequal(node, list(i).pos)
index = i;
break;
end
end
end

% 启发式函数（估计函数）
function h = heuristic(pos, goalPos)
h = norm(pos - goalPos);
end

% 获取路径
function path = getPath(startPos, goalPos, shortestPaths)
currentPos = startPos;
path = [currentPos];

while ~isequal(currentPos, goalPos)
    nextPos = getNextPos(currentPos, goalPos, shortestPaths);
    path = [path; nextPos];
    currentPos = nextPos;
end

end

% 获取下一个位置
function nextPos = getNextPos(currentPos, goalPos, shortestPaths)
nextPos = currentPos;
minDist = inf;

neighbors = getNeighbors(currentPos, size(shortestPaths));
for i = 1:numel(neighbors)
    neighbor = neighbors(i);
    dist = shortestPaths(neighbor(1), neighbor(2)) + heuristic(neighbor, goalPos);
    if dist < minDist
        nextPos = neighbor;
        minDist = dist;
    end
end

end