当前位置：首页 > news >正文

最近在搞路径规划项目，发现A这玩意儿真是又好用又有趣。今天咱们用Matlab从头撸个带自定义地图的A路径规划，保证代码比教程还清楚，直接开整

news 2026/5/11 20:09:49

A*算法路径规划 A星Matlab路径规划自定义栅格可自行更改绘制栅格地图，自定义起始点目标点位置、未知障碍物位置 matlab实现详细注释！

先来个灵魂草图：地图用栅格表示，0能走，1是墙，起点终点自己定。Matlab矩阵直接当地图简直不要太方便，先搞个10x10的示例地图：

% 初始化地图（可随便改尺寸） map = zeros(10,10); % 手动加墙（中间一堵墙） map(4:7,3) = 1; map(4,4:7) = 1;

接下来定义起点终点。这里我故意把起点终点放在墙的两侧，看算法能不能绕过去：

start_node = [2, 3]; % [行, 列] goal_node = [8, 7];

A*的核心是节点数据结构，咱们用Matlab的结构体搞定：

node.g = 0; % 从起点到当前节点的实际代价 node.h = 0; % 启发式估计值 node.f = 0; % g + h node.parent = []; % 路径回溯用 node.position = []; % 当前坐标

重点来了——启发函数。这里用曼哈顿距离（适合四方向移动），想用欧式距离的自己改：

function h = heuristic(pos, goal) % 曼哈顿距离 h = abs(pos(1)-goal(1)) + abs(pos(2)-goal(2)); % 欧式距离：norm(pos - goal) * 10 end

主循环才是重头戏。开放列表用优先队列（按f值排序），每次取最优节点展开：

open_list = struct2table(node); closed_list = []; while ~isempty(open_list) % 找f值最小的节点 [~, idx] = min(open_list.f); current_node = open_list(idx, :); % 到达终点就收工 if isequal(current_node.position, goal_node) path = reconstruct_path(current_node); break end % 把当前节点挪到关闭列表 open_list(idx, :) = []; closed_list = [closed_list; current_node]; % 遍历四个邻居方向 neighbors = [... -1, 0; % 上 1, 0; % 下 0, -1; % 左 0, 1]; % 右 for k = 1:size(neighbors,1) neighbor_pos = current_node.position + neighbors(k,:); % 跳过越界和障碍 if neighbor_pos(1)<1 || neighbor_pos(1)>size(map,1) || ... neighbor_pos(2)<1 || neighbor_pos(2)>size(map,2) || ... map(neighbor_pos(1), neighbor_pos(2)) == 1 continue end % 计算新g值（移动成本算10） tentative_g = current_node.g + 10; % 检查是否在关闭列表 in_closed = any(cellfun(@(x) isequal(x, neighbor_pos), closed_list.position)); % 新路径更优时才更新 if ~in_closed || tentative_g < existing_g new_node = struct(); new_node.position = neighbor_pos; new_node.parent = current_node.position; new_node.g = tentative_g; new_node.h = heuristic(neighbor_pos, goal_node); new_node.f = new_node.g + new_node.h; % 加入开放列表前查重 in_open = find(cellfun(@(x) isequal(x, neighbor_pos), open_list.position)); if isempty(in_open) open_list = [open_list; struct2table(new_node)]; elseif open_list.g(in_open(1)) > tentative_g open_list(in_open(1), :) = struct2table(new_node); end end end end

路径回溯函数像倒带一样往回找：

function path = reconstruct_node(node) path = []; while ~isempty(node.parent) path = [node.position; path]; % 在关闭列表里找父节点 parent_idx = find(cellfun(@(x) isequal(x, node.parent), closed_list.position)); node = closed_list(parent_idx, :); end path = [start_node; path]; % 补上起点 end

跑起来的效果：算法会先贴着墙边试探，发现走不通马上绕远路。障碍物设置成L型的话，路径会拐两个直角弯。要是把启发函数换成对角线距离，路径会更丝滑，不过要注意移动成本的计算方式得跟着改。

A*算法路径规划 A星Matlab路径规划自定义栅格可自行更改绘制栅格地图，自定义起始点目标点位置、未知障碍物位置 matlab实现详细注释！

几个可以魔改的地方：