A brief introduction of A_star algorithm

1 A Star algorithm and its application Status The information that is helpful to simplify the search process extracted during the search task is called heuristic information. Heuristic information is transformed into heuristic function after text extraction and formulation. Heuristic functions can represent the estimated distance from the initial vertex to the target vertex, or the estimated time from the initial vertex to the target vertex. Different heuristic functions are used to describe different situations and solve different problems. Heuristic function is named H (n) by default. Heuristic function is called heuristic search algorithm. In the path planning of rescue robot, A Star algorithm can combine the environment in the search task, narrow the search scope, improve the search efficiency, and make the search process more directional and intelligent, so A Star algorithm can be better applied to the robot path planning related fields.

2 the process of A Star algorithm follows from Section 2.1. The heuristic function of A Star algorithm is used to estimate the distance from the starting point to the target point, so as to narrow the search range and improve the search efficiency. The mathematical formula of A Star algorithm is :F (n) =G (n) +H (n), where F (n) is an estimation function from the starting point to the target point via node N, G (n) represents the actual movement cost from the starting point to grid N, and H (n) represents the estimated movement cost from grid N to the target point.

As shown in Figure 2, the area to be searched is divided into square grids, each of which is walkable and unwalkable. Take the value of each passable square as 1 and move diagonally (the valuation does not take into account diagonal movement). The search path flow is as follows:Figure 2 path planning of A Star algorithm Step1: Define two lists named open and Closed; The open list is used to hold all squares that are considered to find a path, and the closed list is used to hold squares that are no longer considered. Step2:A is the starting point and B is the target point. Start from starting point A and put starting point A into the open list. The closed list is initialized to empty. Step3: Check the square N adjacent to A (n is called the child point of A, and A is called the parent point of N), add the squares that can be passed to the open list, and calculate their F, G and H values. Remove A from open and add it to closed; Step4: determine whether the open list is empty. If so, the search fails. If not, perform the next step. Step5: Remove N from the open list and add it to the Closed list to determine whether n is the target vertex B. If so, the search is successful and the algorithm is finished. Step6: If not, extend the search for n’s child vertices: a. If the child vertices are unpassable or in the close list, ignore it. b. If the child vertex is not in the open list, it is added to the open list, and the current grid is set to its parent, recording the F, G, and H values of the grid. Step7: go to Step4; Step8: end the loop and save the path. From the end point, each square moves along the parent node to the starting point, which is the optimal path. The flow chart of A Star algorithm is shown in Figure 3.Figure 3 A Star algorithm flow

Two, some source code

%% load map
clear;
ImpRgb = imread('maze.png');
Imp    = rgb2gray(ImpRgb);
Imp    = im2bw(Imp)*255;
MAX_X=size(Imp,1);
MAX_Y=size(Imp,2);
distanceFcn = @(p1,p2) norm(p1-p2);
%% AStar
GlbTab      = zeros(MAX_X, MAX_Y);  % 0|new 1|open 2|close
PathTab     = zeros(MAX_X, MAX_Y, 2);
nodeStartXY  = [1.1];
nodeTargetXY = [250.250];
startGn     = 0;
startHn     = distanceFcn(nodeTargetXY,nodeStartXY);
startFn     = startGn + startHn;
% [fn | gn | hn | x | y]
nodeStart  = [startFn, startGn, startHn, nodeStartXY]; 

%% loop
openset = [nodeStart];
foundpath = 0;
while(~isempty(openset))
    [~,minIdx] = min(openset(:,1));
    node = openset(minIdx,:);
    openset(minIdx,:) = [];
    if isequal(node(4:5), nodeTargetXY) 
       foundpath = 1;
       break;
    end
    node_x  = node(4);
    node_y  = node(5);
    node_gn = node(2);
    GlbTab(node_x, node_y) = 2;
    
    for k= 1:- 1:- 1
        for j= 1:- 1:- 1
            if (k~=j || k~=0)  %The node itself is not its successor
                s_x  = node_x+k;
                s_y  = node_y+j;
                if((s_x >0 && s_x <=MAX_X) && (s_y >0 && s_y <=MAX_Y))%node within array bound
                    % exist close node
                    if GlbTab(s_x, s_y) = =2 || Imp(s_x,s_y) == 0 
                        continue;
                    end
                    s_gn = node_gn + distanceFcn([node_x,node_y], [s_x,s_y]);
                    s_hn = distanceFcn(nodeTargetXY, [s_x,s_y]);
                    s_fn = s_gn + s_hn;
                    if GlbTab(s_x, s_y) = =0
                        % new node
                        GlbTab(s_x, s_y) = 1;
                        openset = [openset; [s_fn, s_gn, s_hn, s_x, s_y]];
                        PathTab(s_x, s_y, :) = [node_x, node_y];
                    else
                        % exist open node
                        x_set = openset(:,4);
                        y_set = openset(:,5);
                        existIdx = find(x_set == s_x & y_set ==s_y, 1);
                        assert(~isempty(existIdx))
                        exist_gn = openset(existIdx, 2);
                        if exist_gn > s_gn
                           
                        end
                    end
                end
            end
        end
    end
end
Copy the code

3. Operation results

Matlab version and references

1 matlab version 2014A

[1] Yang Baoyang, YU Jizhou, Yang Shan. Intelligent Optimization Algorithm and Its MATLAB Example (2nd Edition) [M]. Publishing House of Electronics Industry, 2016. [2] ZHANG Yan, WU Shuigen. [3] QIAN Cheng, XU Yingqiu, TAN Yingzi. [M]. Tsinghua University Press, 2017. Application of A Star Algorithm to Path Planning in RoboCup Rescue Simulation [J]. Journal of command and control. 2017,3(03)