路径规划A*算法
来源:互联网 发布:6s百度云网络连接失败 编辑:程序博客网 时间:2024/04/27 23:20
A*算法是在Dijkstra算法上进行改进,毕竟,我们是知道终点和起点的位置信息的,Dijkstra算法完全是四面八方全都找,然而我们既然已经知道,比方说重点在起点的北方,那么完全可以直接往北方搜索。
所以算法综合了Best-First Search和Dijkstra算法的优点:在进行启发式搜索提高算法效率的同时,可以保证找到一条最优路径(基于评估函数)。
在此算法中,如果以g(n)表示从起点到任意顶点n的实际距离,h(n)表示任意顶点n到目标顶点的估算距离(根据所采用的评估函数的不同而变化),那么A*算法的估算函数为:
- 如果g(n)为0,即只计算任意顶点 n到目标的评估函数 h(n),而不计算起点到顶点n的距离,则算法转化为使用贪心策略的Best-First Search,速度最快,但可能得不出最优解;
- 如果h(n)不高于实际到目标顶点的距离,则一定可以求出最优解,而且h(n)越小,需要计算的节点越多,算法效率越低,常见的评估函数有——欧几里得距离、曼哈顿距离、切比雪夫距离;
如果h(n)为0,即只需求出起点到任意顶点n的最短路径 g(n),而不计算任何评估函数h(n),则转化为单源最短路径问题,即Dijkstra算法,此时需要计算最多的定点;
4方向用
L1 距离- 8方向用
L∞ 距离,也就是max - 任意方向用
L2 距离
heuristic function
这个评估函数,其实就是给搜索算法一个知识,告诉搜索算法往哪个方向比较对。
评估函数有这么几个特性:
- 将每一个节点映射到非负实数
H:node→{x|x≥0,x∈R} H(goal)=0 - 对任意两个相邻的节点
x,y H(x)≤H(y)+d(x,y) d(x,y)=weightheight of edge fromx toy
这些属性可以令:
关于H函数:
http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
伪代码
- 对于每一个node n
- n.f = Infinity, n.g = Infinity
- 创建一个空的List
- start.g = 0, start.f = H(start),将start加入到list里边
- While(List非空)
- current = list中最小f值的那个node
- 如果current == goal,那么就完成啦
- 对每一个node, n是这个node的临近的节点
- if(n.g > (current.g + cost of edge from n to current))
- n.g = current.g + cost of edge from n to current
- n.f = n.g + H(n)
- n.parent = current
- add n to list if it’s not there already
matlab代码
function [route,numExpanded] = AStarGrid (input_map, start_coords, dest_coords, drawMapEveryTime)% Run A* algorithm on a grid.% Inputs : % input_map : a logical array where the freespace cells are false or 0 and% the obstacles are true or 1% start_coords and dest_coords : Coordinates of the start and end cell% respectively, the first entry is the row and the second the column.% Output :% route : An array containing the linear indices of the cells along the% shortest route from start to dest or an empty array if there is no% route. This is a single dimensional vector% numExpanded: Remember to also return the total number of nodes% expanded during your search. Do not count the goal node as an expanded node. % set up color map for display% 1 - white - clear cell% 2 - black - obstacle% 3 - red = visited% 4 - blue - on list% 5 - green - start% 6 - yellow - destinationcmap = [1 1 1; ... 0 0 0; ... 1 0 0; ... 0 0 1; ... 0 1 0; ... 1 1 0; ... 0.5 0.5 0.5];colormap(cmap);% variable to control if the map is being visualized on every% iterationdrawMapEveryTime = true;[nrows, ncols] = size(input_map);% map - a table that keeps track of the state of each grid cellmap = zeros(nrows,ncols);map(~input_map) = 1; % Mark free cellsmap(input_map) = 2; % Mark obstacle cells% Generate linear indices of start and dest nodesstart_node = sub2ind(size(map), start_coords(1), start_coords(2));dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2));map(start_node) = 5;map(dest_node) = 6;% meshgrid will `replicate grid vectors' nrows and ncols to produce% a full grid% type `help meshgrid' in the Matlab command prompt for more informationparent = zeros(nrows,ncols);% [X, Y] = meshgrid (1:ncols, 1:nrows);xd = dest_coords(1);yd = dest_coords(2);% Evaluate Heuristic function, H, for each grid cell% Manhattan distanceH = abs(X - xd) + abs(Y - yd);H = H';% Initialize cost arraysf = Inf(nrows,ncols);g = Inf(nrows,ncols);g(start_node) = 0;f(start_node) = H(start_node);% keep track of the number of nodes that are expandednumExpanded = 0;% Main Loopwhile true % Draw current map map(start_node) = 5; map(dest_node) = 6; % make drawMapEveryTime = true if you want to see how the % nodes are expanded on the grid. if (drawMapEveryTime) image(1.5, 1.5, map); grid on; axis image; drawnow; end % Find the node with the minimum f value [min_f, current] = min(f(:)); if ((current == dest_node) || isinf(min_f)) break; end; % Update input_map map(current) = 3; f(current) = Inf; % remove this node from further consideration % Compute row, column coordinates of current node [i, j] = ind2sub(size(f), current); % ********************************************************************* % ALL YOUR CODE BETWEEN THESE LINES OF STARS % Visit all of the neighbors around the current node and update the % entries in the map, f, g and parent arrays % numExpanded = numExpanded + 1; if(i-1>=1) %upper id = sub2ind(size(map), i-1, j); if((map(id) ~= 2) ... %if not obst && (map(id) ~= 3) ... % if not visited && (map(id) ~= 5)) ... % if not start if(g(id) >= g(current) + 1) g(id) = g(current) + 1; f(id) = g(id) + H(id); parent(id) = current; map(id) = 4; end end end if(i+1 <= nrows) %lower id = sub2ind(size(map), i+1, j); if((map(id) ~= 2) ... %if not obst && (map(id) ~= 3) ... % if not visited && (map(id) ~= 5)) ... % if not start if(g(id) >= g(current) + 1) g(id) = g(current) + 1; f(id) = g(id) + H(id); parent(id) = current; map(id) = 4; end end end if(j-1 >= 1) %left id = sub2ind(size(map), i, j-1); if((map(id) ~= 2) ... %if not obst && (map(id) ~= 3) ... % if not visited && (map(id) ~= 5)) ... % if not start if(g(id) >= g(current) + 1) g(id) = g(current) + 1; f(id) = g(id) + H(id); parent(id) = current; map(id) = 4; end end end if(j+1 <= ncols) %left id = sub2ind(size(map), i, j+1); if((map(id) ~= 2) ... %if not obst && (map(id) ~= 3) ... % if not visited && (map(id) ~= 5)) ... % if not start if(g(id) >= g(current) + 1) g(id) = g(current) + 1; f(id) = g(id) + H(id); parent(id) = current; map(id) = 4; end end end %*********************************************************************end%% Construct route from start to dest by following the parent linksif (isinf(f(dest_node))) route = [];else route = [dest_node]; while (parent(route(1)) ~= 0) route = [parent(route(1)), route]; end % Snippet of code used to visualize the map and the path for k = 2:length(route) - 1 map(route(k)) = 7; pause(0.1); image(1.5, 1.5, map); grid on; axis image; endendend
测试代码
map = false(10); %Input map parametersmap (2:10, 6) = true; %Obstacle Declarationstart_coords = [6, 2]; %Starting coordinatesdest_coords = [8, 9]; %Destination CoordinatesdrawMapEveryTime = false; %Display Outputs[route, numExpanded] = AStarGrid(map, start_coords, dest_coords,drawMapEveryTime) %Implementatio
结果:
阅读全文
0 0
- A*算法-路径规划
- 路径规划A*算法
- a*算法(路径规划)
- 使用A*算法规划路径
- 路径规划之A* 算法
- 使用Java编写A*路径规划算法
- 机器人路径规划及A*算法详解
- 在A*路径规划算法中使用二叉堆
- 【无人车研究】A*算法实现路径规划
- A*路径规划初探
- 路径规划算法浅谈
- 路径规划算法初探
- 路径规划算法概论
- 路径规划算法
- 浅谈路径规划算法
- PRM路径规划算法
- 路径规划Dijkstra算法
- 路径规划A*算法及SLAM自主地图创建导航算法
- Web安全 -- 信息收集(下)
- seo的基准点在于内容营销
- Linux中tree命令的C语言实现
- 文章标题
- 智能指针(pointer like class)和仿函数(function like class)
- 路径规划A*算法
- c++中->与.的区别
- Web安全 -- 前言(续)
- 【斯坦福AI百年报告2017】重磅发布人工智能与机器学习全景式概览
- JPA
- 活动丨引爆用户增长:一场长达5小时的深度闭门分享
- Web安全 -- XSS漏洞
- Pycharm中快捷键大全
- 程序员除了编代码,还能做哪些职业规划?