a*算法路径规划matlab

以下是一个简单的A*算法路径规划的MATLAB示例代码：

function [path, cost] = astar(start, goal, map)
% A*算法路径规划
% start: 起点坐标 [x,y]
% goal: 终点坐标 [x,y]
% map: 地图，1为可通过，0为障碍物
% path: 路径坐标 [x1,y1;x2,y2;...]
% cost: 路径总代价

% 地图尺寸
[nrows, ncols] = size(map);

% 节点代价
g = Inf(nrows, ncols);

% 节点启发式代价
h = heuristic(start, goal);

% 节点总代价
f = g + h;

% 节点父节点坐标
parent = zeros(nrows, ncols, 2);

% 起点代价
g(start(1), start(2)) = 0;

% 待扩展节点列表
open_list = [start, f(start(1), start(2))];

% 已扩展节点列表
closed_list = [];

% 扩展节点
while ~isempty(open_list)
    % 选择最小代价节点
    [~, idx] = min(open_list(:, 3));
    curr = open_list(idx, 1:2);
    
    % 到达终点
    if isequal(curr, goal)
        path = reconstruct_path(parent, start, goal);
        cost = g(goal(1), goal(2));
        return;
    end
    
    % 从待扩展列表中移除
    open_list(idx, :) = [];
    
    % 添加到已扩展列表
    closed_list = [closed_list; curr];
    
    % 扩展邻居
    [x, y] = meshgrid(curr(1)-1:curr(1)+1, curr(2)-1:curr(2)+1);
    neighbors = [x(:), y(:)];
    neighbors = neighbors(neighbors(:,1) > 0 & neighbors(:,1) <= nrows & ...
                          neighbors(:,2) > 0 & neighbors(:,2) <= ncols & ...
                          ~ismember(neighbors, closed_list, 'rows') & ...
                          map(sub2ind([nrows, ncols], neighbors(:,1), neighbors(:,2))) == 1, :);
    for i = 1:size(neighbors, 1)
        neighbor = neighbors(i, :);
        
        % 计算邻居代价
        tentative_g = g(curr(1), curr(2)) + distance(curr, neighbor);
        
        % 更新邻居代价
        if tentative_g < g(neighbor(1), neighbor(2))
            parent(neighbor(1), neighbor(2), :) = curr;
            g(neighbor(1), neighbor(2)) = tentative_g;
            h(neighbor(1), neighbor(2)) = heuristic(neighbor, goal);
            f(neighbor(1), neighbor(2)) = g(neighbor(1), neighbor(2)) + h(neighbor(1), neighbor(2));
            
            % 添加到待扩展列表
            if ~any(ismember(open_list(:, 1:2), neighbor, 'rows'))
                open_list = [open_list; neighbor, f(neighbor(1), neighbor(2))];
            end
        end
    end
end

% 找不到路径
path = [];
cost = Inf;
end

function d = distance(a, b)
% 计算两点距离
d = norm(a - b, 2);
end

function h = heuristic(a, b)
% 启发式函数，使用曼哈顿距离
h = abs(a(1) - b(1)) + abs(a(2) - b(2));
end

function path = reconstruct_path(parent, start, goal)
% 重构路径
path = [goal];
while ~isequal(path(1,:), start)
    path = [parent(path(1,1), path(1,2), :); path];
end
path = reshape(path, [], 2);
end

使用示例：

% 定义地图
map = [1 1 1 1 0 1 1 1 1 1;
       1 0 1 1 1 1 0 1 1 1;
       1 0 1 1 1 1 0 1 1 1;
       1 0 1 1 1 1 0 1 1 1;
       1 0 1 1 1 1 0 1 1 1;
       1 0 1 1 1 1 0 1 1 1;
       1 1 1 1 1 1 0 1 1 1;
       1 1 1 1 1 1 0 1 1 1;
       1 1 1 1 1 1 0 1 1 1;
       1 1 1 1 1 1 0 1 1 1];

% 路径规划
start = [1, 1];
goal = [10, 10];
[path, cost] = astar(start, goal, map);

% 显示结果
figure;
imagesc(map);
hold on;
plot(start(2), start(1), 'go', 'MarkerSize', 10, 'LineWidth', 2);
plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
plot(path(:,2), path(:,1), 'b', 'LineWidth', 2);
axis equal;
axis tight;
grid on;
title(sprintf('Path cost: %.2f', cost));

该示例代码中使用了曼哈顿距离作为启发式函数，可以根据实际情况进行修改。