路径规划之A*算法与matlab实现
2018-03-21 14:00
459 查看
1.算法原理
我觉得原理这篇博客讲的非常清楚,大家可以看一下:
http://blog.csdn.net/b2b160/article/details/4057781
与DijkstraDijkstra算法比较而言,A∗A∗不但考虑了,从初始结点到任意结点nn的代价g(n)g(n),还考虑了从结点nn到目标点的启
ea83
发式评估代价h(n)h(n),当从初始点向目标点移动时,A∗A∗权衡这两者。每次进行主循环时,它检查f(n)f(n)最小的结点nn,其中f(n)=g(n)+h(n)f(n)=g(n)+h(n),从而得能够减少搜索空间,提高运行速度。当h(n)=0h(n)=0时,A∗A∗就是DijkstraDijkstra。
2.算法流程
①.首先创建地图,将地图分为10*10的网格,绿色为起始点,黄色为目标点,黑色为障碍物。
②.以绿色方框为中心搜索上下右左四个框里离目标点最近的框,已搜索过的地方表为红色,下次搜索的中心表为蓝色。
③.如此一直搜索将达到黄色区域。
④.最短的那条路径就是我们需要找的解。
3.matlab完整代码
这是我学习的代码:
运行结果:
我觉得原理这篇博客讲的非常清楚,大家可以看一下:
http://blog.csdn.net/b2b160/article/details/4057781
与DijkstraDijkstra算法比较而言,A∗A∗不但考虑了,从初始结点到任意结点nn的代价g(n)g(n),还考虑了从结点nn到目标点的启
ea83
发式评估代价h(n)h(n),当从初始点向目标点移动时,A∗A∗权衡这两者。每次进行主循环时,它检查f(n)f(n)最小的结点nn,其中f(n)=g(n)+h(n)f(n)=g(n)+h(n),从而得能够减少搜索空间,提高运行速度。当h(n)=0h(n)=0时,A∗A∗就是DijkstraDijkstra。
2.算法流程
①.首先创建地图,将地图分为10*10的网格,绿色为起始点,黄色为目标点,黑色为障碍物。
②.以绿色方框为中心搜索上下右左四个框里离目标点最近的框,已搜索过的地方表为红色,下次搜索的中心表为蓝色。
③.如此一直搜索将达到黄色区域。
④.最短的那条路径就是我们需要找的解。
3.matlab完整代码
这是我学习的代码:
clc; clear; close all; %% % set up color map for display cmap = [1 1 1; ...% 1 - white - 空地 0 0 0; ...% 2 - black - 障碍 1 0 0; ...% 3 - red - 已搜索过的地方 0 0 1; ...% 4 - blue - 下次搜索备选中心 0 1 0; ...% 5 - green - 起始点 1 1 0];% 6 - yellow - 到目标点的路径 colormap(cmap); map = zeros(10); % 设置障障碍 map (1:5, 7) = 2; map(6, 2) = 5; % 起始点 map(4, 8) = 6; % 目标点 image(1.5,1.5,map); grid on; axis image; %% 建立地图 nrows = 10; ncols = 10; start_node = sub2ind(size(map), 6, 2); dest_node = sub2ind(size(map), 4, 8); % 距离数组初始化 distanceFromStart = Inf(nrows,ncols); distanceFromStart(start_node) = 0; %定义启发函数 [X, Y] = meshgrid (1:ncols, 1:nrows); H = abs(Y - 4) + abs(X - 8); f = Inf(nrows,ncols); f(start_node) = H(start_node); % 对于每个网格单元,这个数组保存其父节点的索引。 parent = zeros(nrows,ncols); % 主循环 while true % 画出现状图 map(start_node) = 5; map(dest_node) = 6; image(1.5, 1.5, map); grid on; axis image; drawnow; [~, current] = min(f(:)); %返回当前fn最小值的索引。 [min_dist, ~] = min(distanceFromStart(:)); %返回当前距离数组的最小值。 if ((current == dest_node) || isinf(min_dist)) %搜索到目标点或者全部搜索完,结束循环。 break; end; map(current) = 3; %将当前颜色标为红色。 f(current) = Inf; %当前区域在距离数组中设置为无穷,表示已搜索。 [i, j] = ind2sub(size(distanceFromStart), current); %返回当前位置的坐标 neighbor = [i-1,j;... i+1,j;... i,j+1;... i,j-1] %确定当前位置的上下左右区域。 outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +... (neighbor(:,2)<1) + (neighbor(:,2)>ncols ) %判断下一次搜索的区域是否超出限制。 locate = find(outRangetest>0); %返回超限点的行数。 neighbor(locate,:)=[] %在下一次搜索区域里去掉超限点。 neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) %返回下次搜索区域的索引号。 for i=1:length(neighborIndex) if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5) map(neighborIndex(i)) = 4; %如果下次搜索的点不是障碍,不是起点,没有搜索过就标为蓝色。 if distanceFromStart(neighborIndex(i))> min_dist + 1 distanceFromStart(neighborIndex(i)) = min_dist+1; parent(neighborIndex(i)) = current; %如果在距离数组里,。 f(neighborIndex(i))=H(neighborIndex(i)); end end end end %% if (isinf(distanceFromStart(dest_node))) route = []; else %提取路线坐标 route = [dest_node]; while (parent(route(1)) ~= 0) route = [parent(route(1)), route]; end % 动态显示出路线 for k = 2:length(route) - 1 map(route(k)) = 7; pause(0.1); image(1.5, 1.5, map); grid on; axis image; end end
运行结果:
相关文章推荐
- 路径规划的MATLAB实现
- 在matlab中实践采用A*算法仿真AGV路径规划-初步
- 路径规划之Dijkstra算法与matlab实现
- 【无人车研究】A*算法实现路径规划
- 线性规划(matlab实现)
- 【龙印】步进电机使用七段式抛物线型S曲线加减速和路径规划的理论分析和实现
- 平台转型-规划路径、实现落地
- Autolabor Simulation(AS)实现机器人路径规划与障碍物避障
- 机器人自主移动的秘密:实际应用中,SLAM究竟是如何实现的?(二)SLAM与路径规划有什么关系?(三)
- MATLAB 实现轨迹分类(路径分类)
- matlab获取目录中图像名称及路径的递归实现
- 调起地图App实现路径规划导航等
- 路径搜索漫谈和A*算法简明实现
- 高德地图简单实现及路径规划
- a*算法(路径规划)
- 路径规划A*算法及SLAM自主地图创建导航算法
- Dijkstra(迪杰斯特拉)最短路径算法之matlab实现(修正+验证)
- 路径规划(最短路径)算法C#实现
- A*算法实现寻找较优路径
- A*算法 (MATLAB) -路径搜索