机器人路径规划中基于栅格地图的A星搜索算法

在机器人路径规划中，基于栅格地图的A*搜索算法是一种非常流行且高效的方法。它结合了启发式搜索和图搜索的优点，能够在复杂的环境中快速找到从起点到终点的最优路径。以下是对该算法的详细说明，包括其原理、实现步骤以及一个简单的MATLAB实现示例。

1. A*搜索算法原理

A算法是一种启发式搜索算法，它通过评估每个节点的代价来选择最优路径。A 算法的核心思想是利用启发式函数 h(n)h(n)h(n) 来估计从当前节点 n 到目标节点的代价，同时结合实际代价 g(n)g(n)g(n) 来评估每个节点的总代价 f(n)f(n)f(n) ：
f(n)=g(n)+h(n)f(n) = g(n) + h(n)f(n)=g(n)+h(n)

g(n)g(n)g(n) ：从起点到当前节点 n 的实际代价。
h(n)h(n)h(n) ：启发式函数，估计从当前节点 n 到目标节点的代价。
f(n)f(n)f(n) ：总代价，用于选择最优路径。

2. 基于栅格地图的路径规划

在栅格地图中，环境被划分为一个个离散的单元格，每个单元格可以是可通行的（自由空间）或不可通行的（障碍物）。机器人只能在可通行的单元格之间移动。

2.1 算法步骤

初始化：
- 将起点加入开放列表（Open List），并设置其 g 、 h 和 f 值。
- 初始化关闭列表（Closed List）为空。
主循环：
- 从开放列表中选择 f 值最小的节点作为当前节点。
- 如果当前节点是目标节点，则路径搜索成功，通过回溯父节点构建路径。
- 否则，将当前节点从开放列表中移除，并加入关闭列表。
- 对当前节点的每个邻居节点进行处理：
  - 如果邻居节点是障碍物或已在关闭列表中，则跳过。
  - 如果邻居节点不在开放列表中，则将其加入开放列表，并设置其g 、 h 和 f 值。
  - 如果邻居节点已在开放列表中，但通过当前节点到达该邻居节点的代价更低，则更新其 g 和 f 值，并设置其父节点为当前节点。
终止条件：
- 如果开放列表为空且未找到目标节点，则路径搜索失败。
- 如果找到目标节点，则路径搜索成功。

3. MATLAB实现

matlab 复制代码

function [path, openList, closedList] = AStarSearch(gridMap, start, goal)
    % 输入参数：
    % gridMap - 栅格地图，0表示可通行，1表示障碍物
    % start - 起点坐标 [x, y]
    % goal - 目标点坐标 [x, y]

    % 初始化
    [rows, cols] = size(gridMap);
    openList = [];
    closedList = false(rows, cols);
    g = inf(rows, cols);
    h = manhattanDistance(gridMap, goal);
    f = g + h;
    parent = zeros(rows, cols, 2);

    % 起点
    startNode = struct('position', start, 'g', 0, 'h', h(start(2), start(1)), 'f', 0 + h(start(2), start(1)));
    openList = [openList; startNode];
    g(start(2), start(1)) = 0;
    f(start(2), start(1)) = 0 + h(start(2), start(1));

    % 主循环
    while ~isempty(openList)
        % 选择f值最小的节点
        [~, idx] = min([openList.f]);
        currentNode = openList(idx);
        openList(idx) = [];
        currentPos = currentNode.position;

        % 检查是否到达目标
        if isequal(currentPos, goal)
            disp('路径搜索成功！');
            path = reconstructPath(parent, start, goal);
            return;
        end

        % 将当前节点加入关闭列表
        closedList(currentPos(2), currentPos(1)) = true;

        % 遍历邻居节点
        neighbors = getNeighbors(currentPos, rows, cols);
        for i = 1:size(neighbors, 1)
            neighborPos = neighbors(i, :);
            if gridMap(neighborPos(2), neighborPos(1)) == 1 || closedList(neighborPos(2), neighborPos(1))
                continue; % 跳过障碍物或已关闭的节点
            end

            % 计算移动代价
            tentative_g = currentNode.g + 1; % 假设每次移动代价为1

            % 如果邻居节点不在开放列表中，或新的g值更小
            if tentative_g < g(neighborPos(2), neighborPos(1))
                parent(neighborPos(2), neighborPos(1), :) = currentPos;
                g(neighborPos(2), neighborPos(1)) = tentative_g;
                f(neighborPos(2), neighborPos(1)) = tentative_g + h(neighborPos(2), neighborPos(1));

                % 如果邻居节点不在开放列表中，加入开放列表
                if ~ismember(neighborPos, [openList.position], 'rows')
                    neighborNode = struct('position', neighborPos, 'g', tentative_g, ...
                        'h', h(neighborPos(2), neighborPos(1)), 'f', tentative_g + h(neighborPos(2), neighborPos(1)));
                    openList = [openList; neighborNode];
                end
            end
        end
    end

    % 如果未找到路径
    disp('未找到路径');
    path = [];
end

function h = manhattanDistance(gridMap, goal)
    % 曼哈顿距离启发式函数
    [rows, cols] = size(gridMap);
    h = zeros(rows, cols);
    for i = 1:rows
        for j = 1:cols
            h(i, j) = abs(j - goal(1)) + abs(i - goal(2));
        end
    end
end

function neighbors = getNeighbors(pos, rows, cols)
    % 获取当前节点的邻居节点
    directions = [0 1; 1 0; 0 -1; -1 0]; % 上、右、下、左
    neighbors = [];
    for i = 1:size(directions, 1)
        neighborPos = pos + directions(i, :);
        if neighborPos(1) >= 1 && neighborPos(1) <= cols && neighborPos(2) >= 1 && neighborPos(2) <= rows
            neighbors = [neighbors; neighborPos];
        end
    end
end

function path = reconstructPath(parent, start, goal)
    % 从父节点信息中重建路径
    path = [];
    currentPos = goal;
    while ~isequal(currentPos, start)
        path = [currentPos; path];
        currentPos = parent(currentPos(2), currentPos(1), :);
    end
    path = [start; path];
end

4. 使用示例

matlab 复制代码

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

% 定义起点和目标点
start = [1, 1];
goal = [5, 5];

% 调用A*搜索算法
[path, openList, closedList] = AStarSearch(gridMap, start, goal);

% 可视化路径
figure;
imagesc(gridMap);
hold on;
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'LineWidth', 2);
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
if ~isempty(path)
    plot(path(:, 1), path(:, 2), 'b', 'LineWidth', 2);
end
axis equal;

参考代码机器人路径规划中基于栅格地图的A*搜索算法] www.youwenfan.com/contentcsm/53191.html

5. 输出结果

运行上述代码后，MATLAB将输出路径搜索结果，并在栅格地图上可视化路径。如果找到路径，path变量将包含从起点到目标点的路径坐标；否则，path为空。

6. 总结

基于栅格地图的A*搜索算法是一种高效且实用的路径规划方法，特别适用于机器人导航和自动驾驶等领域。通过合理设计启发式函数和优化搜索过程，可以进一步提高算法的性能和效率。