【路径规划】A*算法机器人路径规划【Matlab 469期】

一、简介

A算法
A算法是一种典型的启发式搜索算法，建立在Dijkstra算法的基础之上，广泛应用于游戏地图、现实世界中，用来寻找两点之间的最短路径。A算法最主要的是维护了一个启发式估价函数，如式(1)所示。
f(n)=g(n)+h(n)(1)
其中，f(n)是算法在搜索到每个节点时，其对应的启发函数。它由两部分组成，第一部分g(n)是起始节点到当前节点实际的通行代价，第二部分h(n)是当前节点到终点的通行代价的估计值。算法每次在扩展时，都选取f(n)值最小的那个节点作为最优路径上的下一个节点。
在实际应用中，若以最短路程为优化目标，h(n)常取作当前点到终点的欧几里得距离(Euclidean Distance)或曼哈顿距离(Manhattan Distance)等。若令h(n)=0，表示没有利用任何当前节点与终点的信息，A算法就退化为非启发的Dijkstra算法，算法搜索空间随之变大，搜索时间变长。
A*算法步骤如下，算法维护两个集合：P表与Q表。P表存放那些已经搜索到、但还没加入最优路径树上的节点；Q表维护那些已加入最优路径树上的节点。
（1）P表、Q表置空，将起点S加入P表，其g值置0，父节点为空，路网中其他节点g值置为无穷大。
（2）若P表为空，则算法失败。否则选取P表中f值最小的那个节点，记为BT，将其加入Q表中。判断BT是否为终点T，若是，转到步骤（3）；否则根据路网拓扑属性和交通规则找到BT的每个邻接节点NT，进行下列步骤:

①计算NT的启发值
f(NT)=g(NT)+h(NT)(2)
g(NT)=g(BT)+cost(BT, NT)(3)
其中,cost(BT, NT)是BT到NT的通行代价。
②如果NT在P表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，并将NT的父节点设为BT。
③如果NT在Q表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，将NT的父节点设为BT，并将NT移出到P表中。
④若NT既不在P表，也不在Q表中，则将NT的父节点设为BT，并将NT移到P表中。
⑤转到步骤（2）继续执行。
（3）从终点T回溯，依次找到父节点，并加入优化路径中，直到起点S，即可得出优化路径。

二、源代码

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

三、运行结果

在这里插入图片描述

四、备注