直接上代码看看怎么用A*找单点路径。先搞个20x20的地图，障碍物随机生成
路径规划算法-多目标点路径规划对比算法 基于A*算法+Sa算法的多目标点路径规划，MATLAB运行，一键出图 可自行设置地图，改变地图大小形状 设置起始点，目标点 运行结果路径长度，运行时间等对比结果
map = ones(20); map(randperm(400,50)) = 0; % 随机50个障碍 start = [3,5]; goals = [18,12; 7,15; 15,3]; % 三个目标点A*的核心在open列表处理，这里用优先队列实现：
function path = Astar(map,start,goal) open = PriorityQueue(); open.insert(start, 0); came_from = containers.Map(); cost_so_far = containers.Map(); came_from(mat2str(start)) = NaN; cost_so_far(mat2str(start)) = 0; while ~open.is_empty() current = open.pop(); if isequal(current,goal), break; end for next = get_neighbors(current) new_cost = cost_so_far(mat2str(current)) + 1; if ~isKey(cost_so_far,mat2str(next)) || new_cost < cost_so_far(mat2str(next)) cost_so_far(mat2str(next)) = new_cost; priority = new_cost + heuristic(goal,next); open.insert(next, priority); came_from(mat2str(next)) = current; end end end % 路径回溯部分省略... end这代码里有个骚操作——用mat2str把坐标转字符串当哈希表key，比用结构体快三倍。启发函数用曼哈顿距离，实测比欧式省30%计算量。
但多目标点时全排列组合用A*会爆炸。三个目标点有6种访问顺序，五个点就120种。这时候模拟退火出场了：
function best_order = SA_TSP(goals) current_order = randperm(size(goals,1)); current_energy = calc_total_distance(current_order); temp = 1000; for k = 1:1000 new_order = perturb(current_order); new_energy = calc_total_distance(new_order); if new_energy < current_energy || rand < exp((current_energy - new_energy)/temp) current_order = new_order; current_energy = new_energy; end temp = temp * 0.95; end end扰动函数随机交换两个点的位置，能量函数是总路径长。降温系数0.95这个数调了十几遍——0.9容易早熟，0.99又太慢。
路径规划算法-多目标点路径规划对比算法 基于A*算法+Sa算法的多目标点路径规划，MATLAB运行，一键出图 可自行设置地图，改变地图大小形状 设置起始点，目标点 运行结果路径长度，运行时间等对比结果
组合起来的工作流是这样：先用SA找到较优的访问顺序，再用A*逐个计算相邻点路径。测试时发现个反直觉的现象——总路径最短的方案运行时间不一定最短。比如下面这个案例：
案例A：顺序3-1-2 总长58 耗时2.3秒 案例B：顺序2-3-1 总长61 耗时1.8秒因为中间某个段路径虽然长但障碍少，A*搜索快。这时候需要看应用场景是要时间优先还是路径最优。
最后来个一键出图的骚操作：
figure('Position', [100 100 800 400]) subplot(121) imshow(map,'InitialMagnification','fit') hold on plot_path(astar_path,'r') title(['A*单路径 长度:',num2str(len1)]) subplot(122) imshow(map,'InitialMagnification','fit') hold on plot_path(sa_path,'g') title(['SA优化后 长度:',num2str(len2),' 耗时:',num2str(time),'s'])这种左右对比图发朋友圈装逼效果拔群。实际跑起来发现SA参数设置是门玄学，有时候给个不合适的初始温度，结果能跑出贪吃蛇式的鬼畜路径。不过总体来说，这套组合拳在十个目标点以下的场景还算hold得住，再多了就得换遗传算法了。