A星路径规划算法Matlab实现A星算法可自己改变地图和障碍物自定义起点坐标和终点坐标直接手撸A星算法才是理解路径规划最快的方式。咱今天用Matlab整活先看效果在10x10网格地图里红色方块是障碍物绿色是起点蓝色终点程序能自动规划黄色路径。最骚的是改两行代码就能自定义地图适合想快速验证算法的伙计们。先看地图数据结构怎么搞。Matlab里用矩阵最方便0表示可通行1是障碍物map zeros(10,10); % 初始化地图 obstacle [3,3;4,3;5,3;6,3]; % 障碍物坐标 for i1:size(obstacle,1) map(obstacle(i,1), obstacle(i,2)) 1; end start_node [2,2]; % 起点坐标[x,y] target_node [8,8]; % 终点坐标障碍物位置用数组存着循环写入这样改障碍物形状就跟玩拼图似的。想看地图长啥样直接imagesc(map)注意坐标轴方向要转置。核心在于节点结构体每个节点都得记着g值实际代价和h值预估代价nodes struct(pos,[], g,inf, h,[], parent,[]); open_list containers.Map(KeyType,char,ValueType,any); current_pos start_node; nodes(1).pos current_pos; nodes(1).g 0; nodes(1).h heuristic(start_node, target_node); open_list(mat2str(current_pos)) nodes(1);这里用哈希表做开放列表键是坐标字符串值存节点对象。mat2str把坐标转成字符串当key比直接操作矩阵快得多。启发函数用曼哈顿距离足够应付网格地图function h heuristic(a, b) h abs(a(1)-b(1)) abs(a(2)-b(2)); end想换其他启发方式比如对角线距离改成h max(abs(a(1)-b(1)), abs(a(2)-b(2)))就行。A星路径规划算法Matlab实现A星算法可自己改变地图和障碍物自定义起点坐标和终点坐标主循环里有个骚操作——用f值排序找最优节点while ~isempty(open_list) keys open_list.keys(); [~, idx] min(cellfun((k) open_list(k).g open_list(k).h, keys)); current open_list(keys{idx}); if isequal(current.pos, target_node) path reconstruct_path(current); return; end % 生成邻居节点 neighbors get_neighbors(current.pos, map); for i1:size(neighbors,1) new_pos neighbors(i,:); tentative_g current.g 1; % 假设移动代价为1 % 检查是否在开放列表 if open_list.isKey(mat2str(new_pos)) existing_node open_list(mat2str(new_pos)); if tentative_g existing_node.g existing_node.g tentative_g; existing_node.parent current.pos; end else new_node.pos new_pos; new_node.g tentative_g; new_node.h heuristic(new_pos, target_node); new_node.parent current.pos; open_list(mat2str(new_pos)) new_node; end end end这里有个坑Matlab的哈希表删除元素效率低所以实际项目建议用优先队列。不过教学演示嘛这样写更直观。路径回溯用递归实现function path reconstruct_path(node) path node.pos; while ~isempty(node.parent) parent_pos node.parent; % 从所有节点里找parent parent_node []; for n 1:length(nodes) if isequal(nodes(n).pos, parent_pos) parent_node nodes(n); break; end end path [parent_pos; path]; node parent_node; end end这明显不是最优写法但胜在好理解。优化方向可以用字典存储节点位置到索引的映射。最后画路径时注意Matlab坐标系和矩阵索引的关系hold on; plot(start_node(2), start_node(1), go, MarkerSize,10); plot(target_node(2), target_node(1), bo, MarkerSize,10); plot(path(:,2), path(:,1), y-, LineWidth,2);这里x轴对应矩阵列索引y轴对应行索引所以坐标要反过来画。遇到死胡同怎么办程序会直接报错退出。想处理这种情况加个关闭列表检测循环或者设定最大循环次数。算法跑10x10地图基本秒出结果但到100x100就得优化数据结构了——这时候该上二叉堆实现的优先队列哈希表扛不住大数据量。想让障碍物动起来在每次循环前更新map矩阵就行。Matlab的实时绘图功能配合pause命令能做出动态避障效果。不过那就是另一个故事了——先跑通这个基础版再考虑魔改吧。