【路径规划】基于A星算法机器人走迷宫路径规划matlab代码

1 简介

基本的迷宫搜索算法被称为无信息规划算法是一种盲从状态下的搜索算法。所谓的无信息规划，指的是除了起点和终点之间的点以外的中间节点都是可扩展节点，且它们成为系统后续搜索节点的概率是相同的。无信息规划算法中最常见的是广度优先和深度优先两种。而传统的向心法则是由右手法则、左手法则、中左法则和中右法则按照一定规则有机组合起来的一种深度优先的搜索法则。这种算法首先将迷宫分为 4 个区域，当电脑鼠处在不同区域时再根据电脑鼠的行进方向选择不同的搜索法则，其具体组合方式如图 1所示。大多数情况下其搜索效率优于右手法则和左手法则。但在一些特定的迷宫中，其搜索效率可能更低，搜索处的路径可能更长，或者只比右手法则的搜索效率略高。

2 部分代码

function robotSimulation
% robotSimulation - Robotic exploration simulation.
%   This function serves to show how one can use the A* path finding
%   function, astar_jw.
%
%   A random world is made with starting and goal locations. A simple robot
%   with the ability to sense nearby obstacles is used to move towards the
%   goal location according to the output of the astar_jw function. At each
%   step the robot senses the world (updating its own map of the world) and
%   recomputes the optimal path to the goal according to its own map. Each
%   simulation trial ends when either the robot reaches the goal or no path
%   to the goal can be found.

   home

   while true % Simulation never stops
       
       % Generate a random world with starting and goal locations
       n = 30;   % World size is n by n
       wallpercent = 0.45;  % Percent of world that is a wall
      [world, init, goal] = initializeWorld(n,wallpercent);

       % Initialize the world as the robot knows it (aka the 'grid')
       grid = zeros(size(world));

       % Initialize the current robot location
       loc = init;

       % Set up figures
       h = figure(1); drawWorld(world, init, goal, h);
       g = figure(2); % figure for grid
       
       % Explore the world until we reach the goal or deem reaching the
       % goal impossible
       while true
           % Start the clock for this loop iteration
           tic

           % Sense world (update grid)
           grid = senseWorld(world, grid, loc);

           % Compute path/directions to goal using known grid
          [path, directions] = astar_jw(grid, loc, goal);
           
           % If we couldn't find the goal then there is no path to goal
           if numel(path)==0
               drawGrid(grid, init, goal, loc, g);
               home; disp('No path to goal!'); pause(1)
               break
           end

           % Draw the grid
           drawGrid(grid, init, goal, loc, g);
           
           % Draw path overlayed on the grid
           drawPath(path, g); 

           % Move robot according to directions
           loc = loc + directions(1,:);
           
           % Detect if we made it to the goal
           if loc(1)==goal(1) && loc(2)==goal(2)
               home; disp('Reached goal!'); pause(1)
               break
           end

           % Slow down simulation
           pause(0.15-toc)

       end

   end
end

function drawWorld(world, init, goal, h)
   
   world = 1-world;
   world(init(1),init(2)) = 0.66;
   world(goal(1),goal(2)) = 0.33;
   
   figure(h)
   imagesc(world); colormap(gray); axis off; hold on
   
end

function drawGrid(grid, init, goal, loc, h)

   grid = 1-grid;
   grid(init(1),init(2)) = 0.66;
   grid(goal(1),goal(2)) = 0.33;
   
   figure(h)
   imagesc(grid); colormap(gray); axis off; hold on
   plot(loc(2), loc(1), 'go')
end

function drawPath(path, g)
   
   figure(g); hold on
   
   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)
   
   drawnow
   hold off
end

function grid = senseWorld(varargin)
% This function senses the world and updates the robot's grid. The calling
% form for this function is:
%
%   grid = senseWorld(world, grid, loc[, senseScheme, option])
%
% where the optional 'senseScheme' can be one of:
%   {'line of sight'}
%   {'circle', vision radius}
% by default senseScheme is set to 'line of sight'
% (note: in the case of two arguments for 'senseScheme' you need them both)

world = varargin{1};
grid = varargin{2};
loc = varargin{3};

radius = 1;

if nargin==3
   senseScheme = 'line of sight';
elseif nargin>3
   senseScheme = varargin{4};
   
   if strcmp(senseScheme, 'circle')
       if nargin==5
           radius = varargin{5};
       else 
           warning('You need to input a radius measurement if you set senseScheme to ''circle''!')
           return
       end
   end
end

worldSize = size(world);

sightDir = [[-1  0]
          [ 0 -1]
          [ 1  0]
          [ 0  1]];
 
for R = 1:size(sightDir,1)
   sightLoc = loc(1:2);
   canSee = true;
   
   while canSee
       xMin = max(1, sightLoc(2)-radius);
       xMax = min(worldSize(2), sightLoc(2)+radius);
       yMin = max(1, sightLoc(1)-radius);
       yMax = min(worldSize(1), sightLoc(1)+radius);
       
       grid(yMin:yMax, xMin:xMax) = world(yMin:yMax, xMin:xMax);
       
       canSee = false;
       nextPos = sightLoc + sightDir(R,:);
       % If we're not going off the grid
       if nextPos(1)>=1 && nextPos(1)<=worldSize(1) ...
           && nextPos(2)>=1 && nextPos(2)<=worldSize(2)
           
           % And while we aren't seeing through obstacles
           if world(nextPos(1), nextPos(2))==0
               % Continue viewing down the corridor
               canSee = true;
               sightLoc = nextPos;
           end
       end
   end
end



end

function [world, init, goal] = initializeWorld(n,wallpercent)
% This function will create an example n by n world where wallpercent of it
% is blocked off. 1s denote walls and 0s denote traversable terrain.

   % create the world and place walls with infinite cost
   world = ones(n,n) + 10*rand(n,n);
   world(ind2sub([n n],ceil(n^2.*rand(floor(n*n*wallpercent),1)))) = Inf; 
   
   world(world==inf) = 1;
   world(world~=1) = 0;
   
   % set random starting and ending locations
   init = randi(n, [1 2]); world(init(1),init(2)) = 0;
   goal = randi(n, [1 2]); world(goal(1),goal(2)) = 0;
end

3 仿真结果

4 参考文献

[1]周宇杭等. "基于A星算法的移动机器人路径规划应用研究." 电脑知识与技术：学术版 16.13(2020):4.