【机器人栅格地图】基于A星算法求解栅格路径规划及避障matlab源码

1 简介

移动机器人路径规划一直是一个比较热门的话题,A星算法以及其扩展性算法被广范地应用于求解移动机器人的最优路径.该文在研究机器人路径规划算法中,详细阐述了传统A星算法的基本原理,并通过栅格法分割了机器人路径规划区域,利用MATLAB仿真平台生成了机器人二维路径仿真地图对其进行仿真实验,并对结果进行分析和研究,为今后进一步的研究提供经验.

2 部分代码

登录后复制

          
          

           
           function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords)
           
           
% Run Dijkstra's 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 - destination
           
           
cmap = [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
           
           
% iteration
           
           
drawMapEveryTime = true;
           
           
[nrows, ncols] = size(input_map);
           
           
% map - a table that keeps track of the state of each grid cell
           
           
map = zeros(nrows,ncols);
           
           
map(~input_map) = 1;   % Mark free cells
           
           
map(input_map)  = 2;   % Mark obstacle cells
           
           
% Generate linear indices of start and dest nodes
           
           
start_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;
           
           
% Initialize distance array
           
           
distanceFromStart = Inf(nrows,ncols);
           
           
% For each grid cell this array holds the index of its parent
           
           
parent = zeros(nrows,ncols);
           
           
distanceFromStart(start_node) = 0;
           
           
% keep track of number of nodes expanded 
           
           
numExpanded = 0;
           
           
% Main Loop
           
           
while 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 distance
           
           
    [min_dist, current] = min(distanceFromStart(:));
           
           
    if ((current == dest_node) || isinf(min_dist))
           
           
        break;
           
           
    end;
           
           
    % Update map
           
           
    map(current) = 3;         % mark current node as visited
           
           
    numExpanded=numExpanded+1;
           
           
    % Compute row, column coordinates of current node
           
           
    [i, j] = ind2sub(size(distanceFromStart), current);
           
           
   % ********************************************************************* 
           
           
    % YOUR CODE BETWEEN THESE LINES OF STARS
           
           
    % Visit each neighbor of the current node and update the map, distances
           
           
    % and parent tables appropriately.
           
           
    action=[-1 0; 1 0; 0 -1; 0 1];%上，下，左，右
           
           
    for a=1:4
           
           
        expand=[i,j]+action(a,:);
           
           
        expand1=expand(1,1);
           
           
        expand2=expand(1,2);
           
           
        %不超出边界，不穿越障碍，不在CLOSED列表里，则进行扩展
           
           
        if ( expand1>=1 && expand1<=20 && expand2>=1 && expand2<=20 && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5 )
           
           
            if ( distanceFromStart(expand1,expand2)> distanceFromStart(i,j)+1 )
           
           
                distanceFromStart(expand1,expand2)= distanceFromStart(i,j)+1;
           
           
                parent(expand1,expand2)=current;
           
           
                map(expand1,expand2)=4;
           
           
            end
           
           
        end
           
           
    end
           
           
    distanceFromStart(current) = Inf; % remove this node from further consideration
           
           
    %*********************************************************************
           
           
end
           
           
%% Construct route from start to dest by following the parent links
           
           
if (isinf(distanceFromStart(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;
           
           
    end
           
           
end
           
           
end
          
          

• 1.
• 2.
• 3.
• 4.
• 5.
• 6.
• 7.
• 8.
• 9.
• 10.
• 11.
• 12.
• 13.
• 14.
• 15.
• 16.
• 17.
• 18.
• 19.
• 20.
• 21.
• 22.
• 23.
• 24.
• 25.
• 26.
• 27.
• 28.
• 29.
• 30.
• 31.
• 32.
• 33.
• 34.
• 35.
• 36.
• 37.
• 38.
• 39.
• 40.
• 41.
• 42.
• 43.
• 44.
• 45.
• 46.
• 47.
• 48.
• 49.
• 50.
• 51.
• 52.
• 53.
• 54.
• 55.
• 56.
• 57.
• 58.
• 59.
• 60.
• 61.
• 62.
• 63.
• 64.
• 65.
• 66.
• 67.
• 68.
• 69.
• 70.
• 71.
• 72.
• 73.
• 74.
• 75.
• 76.
• 77.
• 78.
• 79.
• 80.
• 81.
• 82.
• 83.
• 84.
• 85.
• 86.
• 87.
• 88.
• 89.
• 90.
• 91.
• 92.
• 93.
• 94.
• 95.
• 96.
• 97.
• 98.
• 99.
• 100.
• 101.
• 102.
• 103.
• 104.
• 105.
• 106.
• 107.
• 108.
• 109.
• 110.

3 仿真结果

4 参考文献

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

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。