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

% Main entry:

ObstList = [- 25:25;15*ones(1.51)]'; % Obstacle point list ObstList = [ObstList; [-10:10; 0*ones(1,21)]'];
ObstList = [ObstList; [- 25:- 10; 5*ones(1.16)]']. ObstList = [ObstList; [10:25; 5*ones(1,16)]'];
ObstList = [ObstList; [ 10*ones(1.6);0:  5; ]']. ObstList = [ObstList; [-10*ones(1,6); 0:5;]'];
% Park lot line for collision check
ObstLine = [- 25.15 , 25.15;
            - 25.5.- 10.5;
            - 10.5.- 10.0;
            - 10.0.10.0;
             10.0.10.5;
             10.5.25.5;
            - 25.5.- 25.15;
             25.5.25.15];
% ObstList and ObstLine
ObstInfo.ObstList = ObstList;
ObstInfo.ObstLine = ObstLine;
% ObstInfo.ObstMap = GridAStar(ObstList,End,XY_GRID_RESOLUTION);

Vehicle.WB = 3.7;         % [m] wheel base: rear to front steer
Vehicle.W  = 2.6;         % [m] width of vehicle
Vehicle.LF = 4.5;         % [m] distance from rear to vehicle front end of vehicle
Vehicle.LB = 1.0;         % [m] distance from rear to vehicle back end of vehicle
Vehicle.MAX_STEER = 0.6;  % [rad] maximum steering angle 
Vehicle.MIN_CIRCLE = Vehicle.WB/tan(Vehicle.MAX_STEER); % [m] mininum steering circle radius

% Motion resolution define
Configure.MOTION_RESOLUTION = 0.1;             % [m] path interporate resolution
Configure.N_STEER = 20.0;                      % number of steer command
Configure.EXTEND_AREA = 0;                     % [m] map extend length
Configure.XY_GRID_RESOLUTION = 2.0;            % [m]
Configure.YAW_GRID_RESOLUTION = deg2rad(15.0); % [rad]
% Grid bound
Configure.MINX = min(ObstList(:,1))-Configure.EXTEND_AREA;
Configure.MAXX = max(ObstList(:,1))+Configure.EXTEND_AREA;
Configure.MINY = min(ObstList(:,2))-Configure.EXTEND_AREA;
Configure.MAXY = max(ObstList(:,2))+Configure.EXTEND_AREA;
Configure.MINYAW = -pi0.01;
Configure.MAXYAW = pi;
% Cost related define
Configure.SB_COST = 0;             % switch back penalty cost
Configure.BACK_COST = 1.5;         % backward penalty cost
Configure.STEER_CHANGE_COST = 1.5; % steer angle change penalty cost
Configure.STEER_COST = 1.5;        % steer angle change penalty cost
Configure.H_COST = 10;             % Heuristic cost

StartState = [22.13, pi  ];
EndState =   [7.2, pi/2];
[x,y,th,~,~] = HybridAStar(StartState,EndState,Vehicle,Configure,ObstInfo);
if isempty(x)
    disp('Failed to find path! ')
else
    hold on;
    VehicleAnimation(x,y,th,Configure,Vehicle,ObstInfo)
end
% path = FindRSPath(5.1,pi);


function path = FindRSPath(x,y,phi,veh)
    rmin = veh.MIN_CIRCLE; %minimum turning radius
    x = x/rmin;
    y = y/rmin;
    [isok1,path1] = CSC(x,y,phi);
    [isok2,path2] = CCC(x,y,phi);
    [isok3,path3] = CCCC(x,y,phi);
    [isok4,path4] = CCSC(x,y,phi);
    [isok5,path5] = CCSCC(x,y,phi);
    isoks = [isok1, isok2, isok3, isok4, isok5];
    paths = {path1, path2, path3, path4, path5};
    Lmin = inf;
    for i = 1:5
        if isoks(i) == true
            elem = paths{i};
            if Lmin > elem.totalLength
                Lmin = elem.totalLength;
                path = elem;
            end
        end
    end
%     PlotPath(path,veh);
end

function v = mod2pi(x)
    v = rem(x,2*pi);
    if v < -pi
        v = v+2*pi;
    elseif v > pi
        v = v2 -*pi;
    end
end

function [tau,omega] = tauOmega(u,v,xi,eta,phi)
    delta = mod2pi(u-v);
    A = sin(u)-sin(delta);
    B = cos(u)-cos(delta)- 1;
    t1 = atan2(eta*A-xi*B,xi*A+eta*B);
    t2 = 2* (cos(delta)2 -*cos(v)2 -*cos(u))+3;
    if t2 < 0
        tau = mod2pi(t1+pi);
    else
        tau = mod2pi(t1);
    end
    omega = mod2pi(tau-u+v-phi);
end

% formula 8.1
function [isok,t,u,v] = LpSpLp(x,y,phi)
    [t,u] = cart2pol(x-sin(phi),y- 1+cos(phi));
    if t >= 0
        v = mod2pi(phi-t);
        if v >= 0
            isok = true;
            return
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end

% formula 8.2
function [isok,t,u,v] = LpSpRp(x,y,phi)
    [t1,u1] = cart2pol(x+sin(phi),y- 1-cos(phi));
    if u1^2> =4
        u = sqrt(u1^24 -);
        theta = atan2(2,u);
        t = mod2pi(t1+theta);
        v = mod2pi(t-phi);
        if t >= 0 && v >= 0
            isok = true;
            return
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end

function [isok,path] = CSC(x,y,phi)
    Lmin = inf;
    type = repmat('N'[1.5]);
    path = RSPath(type,0.0.0.0.0);
    [isok,t,u,v] = LpSpLp(x,y,phi);
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPath.Types(15,:),t,u,v,0.0);
        end
    end
    [isok,t,u,v] = LpSpLp(-x,y,-phi); % timeflip
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPath.Types(15,:),-t,-u,-v,0.0);
        end
    end
    [isok,t,u,v] = LpSpLp(x,-y,-phi); % reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPath.Types(16,:),t,u,v,0.0);
        end
    end
    [isok,t,u,v] = LpSpLp(-x,-y,phi); % timeflp + reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPath.Types(16,:),-t,-u,-v,0.0);
        end
    end
    [isok,t,u,v] = LpSpRp(x,y,phi);
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPath.Types(13,:),t,u,v,0.0);
        end
    end
    [isok,t,u,v] = LpSpRp(-x,y,-phi); % timeflip
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPath.Types(13,:),-t,-u,-v,0.0);
        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)