【路径规划】基于matlab A_star算法机器人走迷宫路径规划【含Matlab源码 1332期】

一、A_star算法简介

1 A Star算法及其应用现状
进行搜索任务时提取的有助于简化搜索过程的信息被称为启发信息.启发信息经过文字提炼和公式化后转变为启发函数.启发函数可以表示自起始顶点至目标顶点间的估算距离, 也可以表示自起始顶点至目标顶点间的估算时间等.描述不同的情境、解决不同的问题所采用的启发函数各不相同.我们默认将启发函数命名为H (n) .以启发函数为策略支持的搜索方式我们称之为启发型搜索算法.在救援机器人的路径规划中, A Star算法能结合搜索任务中的环境情况, 缩小搜索范围, 提高搜索效率, 使搜索过程更具方向性、智能性, 所以A Star算法能较好地应用于机器人路径规划相关领域.

2 A Star算法流程
承接2.1节, A Star算法的启发函数是用来估算起始点到目标点的距离, 从而缩小搜索范围, 提高搜索效率.A Star算法的数学公式为:F (n) =G (n) +H (n) , 其中F (n) 是从起始点经由节点n到目标点的估计函数, G (n) 表示从起点移动到方格n的实际移动代价, H (n) 表示从方格n移动到目标点的估算移动代价.

如图2所示, 将要搜寻的区域划分成了正方形的格子, 每个格子的状态分为可通过(walkable) 和不可通过 (unwalkable) .取每个可通过方块的代价值为1, 且可以沿对角移动 (估值不考虑对角移动) .其搜索路径流程如下:

图2 A Star算法路径规划
Step1:定义名为open和closed的两个列表;open列表用于存放所有被考虑来寻找路径的方块, closed列表用于存放不会再考虑的方块;
Step2:A为起点, B为目标点, 从起点A开始, 并将起点A放入open列表中, closed列表初始化为空;
Step3:查看与A相邻的方格n (n称为A的子点, A称为n的父点) , 可通过的方格加入到open列表中, 计算它们的F, G和H值.将A从open移除加入到closed列表中;
Step4:判断open列表是否为空, 如果是, 表示搜索失败, 如果不是, 执行下一步骤;
Step5:将n从open列表移除加入到closed列表中, 判断n是否为目标顶点B, 如果是, 表示搜索成功, 算法运行结束;
Step6:如果不是, 则扩展搜索n的子顶点:
a.如果子顶点是不可通过或在close列表中, 忽略它.
b.子顶点如果不在open列表中, 则加入open列表, 并且把当前方格设置为它的父亲, 记录该方格的F, G和H值.
Step7:跳转到步骤Step4;
Step8:循环结束, 保存路径.从终点开始, 每个方格沿着父节点移动直至起点, 即是最优路径.A Star算法流程图如图3所示.

图3 A Star算法流程

二、部分源代码

function varargout = astar_jw(varargin)if nargin == 0 % Test case, use testCase=2 (Maze) by default  selectedGrid = 2;[grid, init, goal] = defineGrid(selectedGrid);% Whether or not to display debug infoprintDebugInfo = true;elseif nargin == 1 % Test case, set testCase to the inputselectedGrid = varargin{1};[grid, init, goal] = defineGrid(selectedGrid);% Whether or not to display debug infoprintDebugInfo = true;elseif nargin == 3 % Function call with inputsgrid = varargin{1};init = varargin{2};goal = varargin{3};printDebugInfo = false;end% Define all possible movesdelta = [[-1  0][ 0 -1][ 1  0][ 0  1]];% Add g & f terms to init if necessaryif length(init)==2init(3) = 0;    % ginit(4) = inf;  % fend% Perform search[path, directions] = search(grid, init, goal, delta, printDebugInfo);if nargout==1varargout{1} = path;elseif nargout==2varargout{1} = path;varargout{2} = directions;endendfunction [path, directions] = search(grid, init, goal, delta, printDebugInfo)% This function implements the A* algorithm% Initialize the open, closed and path listsopen = []; closed = []; path = [];open = addRow(open, init);% Initialize directions listdirections = [];% Initialize expansion timing gridexpanded = -1 * ones(size(grid));expanded(open(1), open(2)) = 0;% Compute the heuristic measurement, hh = computeHeuristic(grid, goal, 'euclidean');  % Open window for graphical debug display if desiredif printDebugInfo; fig = figure; end% Keep searching through the grid until the goal is foundgoalFound = false;while size(open,1)>0 && ~goalFound[open, closed, expanded] = expand(grid, open, closed, delta, expanded, h);% Display debug info if desiredif printDebugInfodisplayDebugInfo(grid, init, goal, open, closed, fig);    endgoalFound = checkForGoal(closed, goal);end% If the goal was found lets get the optimal pathif goalFound% We step from the goal to the start location. At each step we % select the neighbor with the lowest 'expanded' value and add that% neighbor to the path list.path = goal;% Check to see if the start location is on the path list yet[~, indx] = ismember(init(1:2), path(:,1:2), 'rows');while ~indx% Use our findNeighbors function to find the neighbors of the% last location on the path listneighbors = findNeighbors(grid, path, size(path,1), delta);% Look at the expanded value of all the neighbors, add the one% with the lowest expanded value to the pathexpandedVal = expanded(goal(1),goal(2));for R = 1:size(neighbors,1)neighborExpandedVal = expanded(neighbors(R,1), neighbors(R,2));if neighborExpandedVal<expandedVal && neighborExpandedVal>=0chosenNeighbor = R;expandedVal = expanded(neighbors(R,1), neighbors(R,2));endendpath(end+1,:) = neighbors(chosenNeighbor,:);% Check to see if the start location has been added to the path% list yet[~, indx] = ismember(init(1:2), path(:,1:2), 'rows');end% Reorder the list to go from the starting location to the end locpath = flipud(path);% Compute the directions from the pathdirections = zeros(size(path)-[1 0]);for R = 1:size(directions,1)directions(R,:) =  path(R+1,:) - path(R,:);endend% Display resultsif printDebugInfohomeif goalFounddisp(['Goal Found! Distance to goal: ' num2str(closed(goalFound,3))])disp(' ')disp('Path: ')disp(path)fig = figure;displayPath(grid, path, fig)elsedisp('Goal not found!')enddisp(' ')disp('Expanded: ')disp(expanded)disp(' ')disp([' Search time to target: ' num2str(expanded(goal(1),goal(2)))])endendfunction A = deleteRows(A, rows)
% The following way to delete rows was taken from the mathworks website
% that compared multiple ways to do it. The following appeared to be the
% fastest.index = true(1, size(A,1));index(rows) = false;A = A(index, :);
endfunction A = addRow(A, row)A(end+1,:) = row;
endfunction [open, closed, expanded] = expand(grid, open, closed, delta, expanded, h)
% This function expands the open list by taking the coordinate (row) with
% the smallest f value (path cost) and adds its neighbors to the open list.% Expand the row with the lowest frow = find(open(:,4)==min(open(:,4)),1);% Edit the 'expanded' matrix to note the time in which the current grid% point was expandedexpanded(open(row,1),open(row,2)) = max(expanded(:))+1;% Find all the neighbors (potential moves) from the chosen spotneighbors = findNeighbors(grid, open, row, delta);% Remove any neighbors that are already on the open or closed listsneighbors = removeListedNeighbors(neighbors, open, closed);% Add the neighbors still left to the open listfor R = 1:size(neighbors,1)g = open(row,3)+1;f = g + h(neighbors(R,1),neighbors(R,2));open = addRow(open, [neighbors(R,:) g f] ); end% Remove the row we just expanded from the open list and add it to the% closed listclosed(end+1,:) = open(row,:);open = deleteRows(open, row);
endfunction h = computeHeuristic(varargin)
% This function is used to compute the distance heuristic, h. By default
% this function computes the Euclidean distance from each grid space to the
% goal. The calling sequence for this function is as follows:
%   h = computeHeuristic(grid, goal[, distanceType])
%       where distanceType may be one of the following:
%           'euclidean' (default value)
%           'city-block'
%           'empty' (returns all zeros for heuristic function)grid = varargin{1};
goal = varargin{2};if nargin==3distanceType = varargin{3};
elsedistanceType = 'euclidean';
end[m n] = size(grid);
[x y] = meshgrid(1:n,1:m);if strcmp(distanceType, 'euclidean')h = sqrt((x-goal(2)).^2 + (y-goal(1)).^2);
elseif strcmp(distanceType, 'city-block')h = abs(x-goal(2)) + abs(y-goal(1));
elseif strcmp(distanceType, 'empty')h = zeros(m,n);
elsewarning('Unknown distanceType for determining heuristic, h!')h = [];
endendfunction neighbors = findNeighbors(grid, open, row, delta)
% This function takes the desired row in the open list to expand and finds
% all potential neighbors (neighbors reachable through legal moves, as
% defined in the delta list).% Find the borders to the gridborders = size(grid);borders = [1 borders(2) 1 borders(1)]; % [xMin xMax yMin yMax] % Initialize the current location and neighbors listcenSq = open(row,1:2);neighbors = [];% Go through all the possible moves (given in the 'delta' matrix) and% add moves not on the closed listfor R = 1:size(delta,1)potMove = cenSq + delta(R,:);if potMove(1)>=borders(3) && potMove(1)<=borders(4) ...&& potMove(2)>=borders(1) && potMove(2)<=borders(2)if grid(potMove(1), potMove(2))==0neighbors(end+1,:) = potMove; endendendendfunction neighbors = removeListedNeighbors(neighbors, open, closed)
% This function removes any neighbors that are on the open or closed lists% Check to make sure there's anything even on the closed listif size(closed,1)==0returnend% Find any neighbors that are on the open or closed listsrowsToRemove = [];for R = 1:size(neighbors)% Check to see if the neighbor is on the open list[~, indx] = ismember(neighbors(R,:),open(:,1:2),'rows');if indx>0rowsToRemove(end+1) = R;else% If the neighbor isn't on the open list, check to make sure it% also isn't on the closed list[~, indx] = ismember(neighbors(R,:),closed(:,1:2),'rows');if indx>0rowsToRemove(end+1) = R;endendend% Remove neighbors that were on either the open or closed listsif numel(rowsToRemove>0)neighbors = deleteRows(neighbors, rowsToRemove);end
endfunction goalRow = checkForGoal(closed, goal)
% This function looks for the final goal destination on the closed list.
% Note, you could check the open list instead (and find the goal faster);
% however, we want to have a chance to expand the goal location itself, so
% we wait until it is on the closed list.[~, goalRow] = ismember(goal, closed(:,1:2), 'rows');
endfunction displayDebugInfo(grid, init, goal, open, closed, h)
% Display the open and closed lists in the command window, and display an
% image of the current search of the grid.homedisp('Open: ')disp(open)disp(' ')disp('Closed: ')disp(closed)displaySearchStatus(grid, init, goal, open, closed, h)pause(0.05)
endfunction displaySearchStatus(grid, init, goal, open, closed, h)
% This function displays a graphical grid and search status to make
% visualization easier.grid = double(~grid);grid(init(1),init(2)) = 0.66;grid(goal(1),goal(2)) = 0.33;figure(h)imagesc(grid); colormap(gray); axis square; axis off; hold onplot(open(:,2),   open(:,1),   'go', 'LineWidth', 2)plot(closed(:,2), closed(:,1), 'ro', 'LineWidth', 2)hold off
endfunction displayPath(grid, path, h)grid = double(~grid);figure(h)imagesc(grid); colormap(gray); axis off; hold ontitle('Optimal Path', 'FontWeight', 'bold');plot(path(:,2),  path(:,1),   'co', 'LineWidth', 2)plot(path(1,2),  path(1,1),   'gs', 'LineWidth', 4)plot(path(end,2),path(end,1), 'rs', 'LineWidth', 4)end

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]钱程,许映秋,谈英姿.A Star算法在RoboCup救援仿真中路径规划的应用[J].指挥与控制学报. 2017,3(03)