[Path planning] A* algorithm robot path planning [Matlab 469]

1. Introduction

A algorithm
A algorithm is a typical heuristic search algorithm, based on the Dijkstra algorithm, and is widely used in game maps and the real world to find the shortest path between two points. The main thing of the A algorithm is to maintain a heuristic evaluation function, as shown in equation (1).
f(n)=g(n)+h(n)(1)
Among them, f(n) is the corresponding heuristic function when the algorithm searches for each node. It consists of two parts. The first part g(n) is the actual travel cost from the starting node to the current node, and the second part h(n) is the estimated value of the travel cost from the current node to the end point. Each time the algorithm expands, the node with the smallest value of f(n) is selected as the next node on the optimal path.
In practical applications, if the shortest distance is the optimization goal, h(n) is often taken as the Euclidean Distance or Manhattan Distance from the current point to the end point. If h(n)=0, it means that no information about the current node and end point is used, and the A algorithm degenerates to the non-heuristic Dijkstra algorithm. The algorithm search space becomes larger and the search time becomes longer.
The steps of the A* algorithm are as follows. The algorithm maintains two sets: P list and Q list. The P table stores those nodes that have been searched but have not been added to the optimal path tree; the Q table maintains those nodes that have been added to the optimal path tree.
(1) Set the P and Q tables to be empty, add the starting point S to the P table, set its g value to 0, the parent node is empty, and set the g value of other nodes in the road network to infinity.
(2) If the P list is empty, the algorithm fails. Otherwise, select the node with the smallest value of f in the P list, record it as BT, and add it to the Q list. Judge whether BT is the end point T, if yes, go to step (3); otherwise, find each adjacent node NT of BT according to the topological attributes of the road network and traffic rules, and perform the following steps:

①Calculate the heuristic value of
NT f(NT)=g(NT)+h(NT)(2)
g(NT)=g(BT)+cost(BT, NT)(3)
where cost(BT, NT) It is the cost of passing BT to NT.
② If NT is in the P table, and the g value calculated by formula (3) is smaller than the original g value of NT, then the g value of NT is updated to the result of formula (3), and the parent node of NT is set to BT.
③If NT is in the Q table, and the g value calculated by formula (3) is smaller than the original g value of NT, then the g value of NT is updated to the result of formula (3), and the parent node of NT is set to BT, and Move NT to the P list.
④ If NT is neither in the P list nor in the Q list, set the parent node of NT to BT and move NT to the P list.
⑤Go to step (2) to continue execution.
(3) Backtrack from the end point T, find the parent node in turn, and add it to the optimized path until the starting point S, then the optimized path can be obtained.

Second, the source code

close all; clear all;
%initial the map size
size_x = 10;
size_y = 10;
size_t = 100;
%1 - white - clear cell
%2 - black - obstacle
%3 - Grayish blue   - robot 1
%4 - Reddish blue - robot 2
%5 - purple - robot 3
%6 - vivid green - robot 4
%7 - Plum red - robot 5
cmap = [1 1 1;            
              0 0 0;
              0.69 0.87 0.90;
              0.25 0.41 0.88;
              0.63 0.13 0.94;
              0 1 0.5;
              0.87 0.63 0.87;
              ];
colormap(cmap);    %将颜色进行映射
%initial the space_time_map and reservation table
space_map = ones(size_x, size_y);
space_time_map = ones(size_x, size_y, size_t);
reservation_table = zeros(size_x, size_y, size_t);
%add the static obstacle
space_time_map(1:5, 5, :) = 2*ones(5,1, size_t);
reservation_table(1:5, 5, :) = 2*ones(5,1, size_t);
%the start point array and end point array
start_points= [2 4; 5 1];%[2 4; 5 1; 1 1; 9 6;7 3 ];
end_points = [8 7; 8 10];%[8 7; 10 10; 7 3; 3 2; 1 1];
len_route_max = 0;
route_all = [];
for index = 1:length(start_points(:,1))
%initial the start point and the end point
function [route] = Time_Astar_For_Cooperative_Path_Finding(start_coords, end_coords, space_time_map, reservation_table)
%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);
[size_x, size_y, size_t] = size(space_time_map);
%add the start point and the end point 
space_time_map(start_coords(1), start_coords(2), :) = 5;
space_time_map(end_coords(1), end_coords(2), :) = 6;
%initial parent array and Heuristic array
parent = zeros(size_x, size_y, size_t);
[X, Y] = meshgrid(1:size_y, 1:size_x);
xd = end_coords(1);
yd = end_coords(2);
H =abs(X-xd) + abs(Y-yd);
H = H';
%initial cost arrays
g = inf(size_x, size_y, size_t);
f = inf(size_x, size_y, size_t);
g(start_coords(1), start_coords(2), 1) = 0;
f(start_coords(1), start_coords(2), 1) = H(start_coords(1), start_coords(2));
end_time = 1;
while true
    %find the node with the minimum f values
    [min_f, current] = min(f(:));
    [current_x, current_y, current_t] = ind2sub(size(space_time_map), current);
    if((current_x == end_coords(1) && current_y == end_coords(2))  || isinf(min_f))
        end_time = current_t;
        disp('hheheh');
        break;
   end
    %add the current node to close list
    space_time_map(current) = 3;
    f(current) = Inf;
    [i, j, k] = ind2sub(size(space_time_map), current);
    neighbours = [i-1, j, k+1;
                           i+1, j, k+1;
                           i, j-1, k+1;
                           i, j+1, k+1;
                           i, j, k+1];
    for index = 1:size(neighbours, 1)
            px = neighbours(index, 1);
            py = neighbours(index, 2);
            pt = neighbours(index, 3);
             % judge whether out of bound or not
            if (px >0 && px<=size_x && py >0 && py<= size_y )
                sub_neighbours = sub2ind(size(space_time_map), px, py, pt);
                %judge whether the node is obstacle, start point, end
                %point, or not
                if(space_time_map(sub_neighbours) ~= 2 && space_time_map(sub_neighbours) ~= 5 && space_time_map(sub_neighbours) ~= 3)
                    %judge whether the node has less f 
                    if(g(current) +1+ H(px, py) < f(sub_neighbours))
                        %judge whether the node is in reservation table or not
                        if (~reservation_table(sub_neighbours ))
                            %judge whether the special action
                             [special_action] = Special_action(current, sub_neighbours,reservation_table);
                            if (~special_action)
                                g(sub_neighbours) = g(current) + 1;
                                f(sub_neighbours) = g(sub_neighbours) +  H(px, py);
                                parent(sub_neighbours) = current;
                                space_time_map(sub_neighbours) = 4;
                            end
                       end
                    end
                end
            end
    end

Three, running results

Insert picture description here

Four, remarks