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实现 详细注释几个可以魔改的地方把地图改成20x20加点随机障碍物用randi生成在计算g值时加入地形权重比如沼泽地g20允许斜角移动八方向搜索遇到坑爹情况怎么办比如完全封闭的环境代码会陷入死循环。解决方法是在while循环里加个计数器超过最大步数直接break报错。不过咱们示例地图是通的所以不用慌。