【机器人栅格地图】基于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% 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;​% Initialize distance arraydistanceFromStart = Inf(nrows,ncols);​% For each grid cell this array holds the index of its parentparent = zeros(nrows,ncols);​distanceFromStart(start_node) = 0;​% keep track of number of nodes expanded numExpanded = 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 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 linksif (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;    endend​end​​

3 仿真结果

4 参考文献

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

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

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