路径规划算法-多目标点路径规划对比算法 基于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得住再多了就得换遗传算法了。