A brief introduction of A_star algorithm

0 the introductionWith the development of modern technology, there are more types of aircraft, and their applications are becoming more specialized and perfect. For example, DJI PS-X625 UAV is used for plant protection, BAOji X8 UAV is used for street scene shooting and monitoring patrol, and White Shark MIX underwater UAV is used for underwater rescue. The performance of the aircraft is mainly determined by the internal flight control system and external path planning. In terms of path problem, only by the operator in the concrete implementation of the task in the hands of the remote control of unmanned aerial vehicles to perform the corresponding work, may be psychological and technology put forward high request of the operator, in order to avoid personal error, the risk of causing the damage of aircraft, a solution is carried out on the aircraft flight path planning. The measurement accuracy of aircraft, the reasonable planning of flight path, the stability and safety of aircraft at work and other changes require more and more high requirements for the integrated control system of aircraft. Uav route planning is a problem to design the optimal route to ensure that UAV can complete specific flight tasks and avoid various obstacles and threat areas in the process of completing the tasks. Common path planning algorithms are shown in Figure 1.Figure 1 Common path planning algorithms This paper mainly studies the flight path planning of UAV in the cruise stage. Assuming that uav maintains constant altitude and speed in flight, flight path planning becomes a two-dimensional plane planning problem. In the path planning algorithm,AThe algorithm is simple and easy to implement. To improve ABased on the algorithm, A new and easy to understand improvement A is proposedAn algorithm for uav flight path planning. A traditionalIn this algorithm, the planning area is rasterized, and node expansion is limited to the intersection of grid lines, and there are usually two directions of motion at a certain Angle between the intersection of grid lines. Enlarge and refine the two sections of paths with angles infinitely, and then use the corresponding path planning points on the two sections respectively as tangent points to find the corresponding center of the circle that forms the tangent circle, then make an arc, and find the corresponding center Angle of the arc between the corresponding two tangent points, and calculate the length of the arc according to the following formulaWhere: R — radius of the tangent circle; Alpha — the central Angle corresponding to the arcs between the tangent points.

2. Introduction to RRT algorithm

Definition of RRTRapid-exploring Random Tree (RRT) algorithm is a sampling-based path planning algorithm, which is often used for mobile robot path planning. It is suitable for solving path planning problems in high dimensional space and complex constraints. The basic idea is to generate random points through a step to search the target point forward, effectively avoid obstacles, avoid the path into local minimum, fast convergence. This paper realizes RRT algorithm through MATLAB to solve the path planning problem of two-dimensional plane.2 mapTo facilitate the implementation of the algorithm, discrete quantities are used to represent the environment map. Where, the value 0 represents an empty area without obstacles, and the value 1 represents an area with obstacles.The vertex coordinates searched in THE RRT algorithm are continuous points, and random points are generated in the map. The algorithm will build a tree by continuous points. In this process, the branches and vertices are detected to detect whether the vertex is in an empty area. Download the appendix. Dat file and draw the map.

colormap=[1 1 1; 0 0 0; 1 0 0; 0 1 0; 0 0 1];
imshow(uint8(map),colormap)

Copy the code

Note: The column in the data is X-axis, and the behavior is Y-axis

3 RRT algorithm principle Through the MATLAB program to build a tree from the starting position to the target position, and generate a path connecting the two points. Use one tree with a center point at the starting point instead of two trees (one center point at the starting point and one center point at the target). Write a MATLAB function whose input and output have the same form.

function [vertices, edges, path] = rrt(map, q_start, q_goal, k, delta_q, p)

Copy the code

Where: map:.mat map matrix q_start: x and y coordinates of the starting point q_goal: x and y coordinates of the target point K: If the target point cannot be found, the maximum number of iterations to generate the search tree is controlled to be K. Delta_q: distance between q_new and q_near P: Q_goal is used as the probability of Q_RAND. When the random number generated is smaller than P, the target point is used as the random point Q_RAND. When the random number generated is greater than P, a random point is generated as the q_RAND vertices: The x and y coordinates of the vertices, the coordinates of all the points generated during the random tree generation are stored in this matrix, where the first point is the starting point, and the last point is the target point. Deges: Generate all branches of the random tree, there are n-1 branches in total, so the matrix has n-1 rows, and two columns of each row represent the index numbers of two points. Path: the index from the starting point to the target point, is a row vector. Here is a graph representing some of the variables in the algorithm mentioned above:

4 Obstacle DetectionTo detect whether the branch (the edge between q_near and q_new) is in free space, incremental method or equal division method can be used, as shown in the diagram below (assuming that there are 10 points between two points, incremental detection method is shown in the figure on the left and equal division method on the right, and it can be seen from the diagram that the number of detection times using equal division method is less) :In this paper, using k=10000, delta_q=50,p=0.3, we get the following results:

5 Path Smoothing After completing the basic RRT algorithm, we have a path from the start to the end. Now we smooth and de-noise this path. After completing the processing, we will have a shorter path. Greedy algorithm is used: connect Q_start and Q_GOAL, and if there is an obstacle detected between the two points, replace Q_GOAL with the previous point until the two points can be connected (there is no obstacle in the middle). Once q_goal has been linked, define the smoothing function in MATLAB:

function [path_smooth] = smooth(map, path, vertices, delta)

Copy the code

Among them: the path: Vertices: any of the vertices in the tree from the starting point to the target. Delta: the incremental distance used to detect whether the direct connection between the vertices of the path is in free space. Each edge is divided by delta into several segments path_smooth: After smoothing, the path points will be reduced. Use path_smooth to record the smoothed path, which is still a row vector, and record the index number of the path

The path after smoothing is: 6 summarizesRRT algorithm is an incremental search algorithm, based on the idea of probability, it is a probability complete path optimization algorithm, has the advantage of solving speed. The basic RRT algorithm has its own defects, the path obtained by solving is usually of poor quality, with edges and corners, not smooth enough. Therefore, it is necessary to smooth the path to obtain the path curve suitable for robot path tracking.

Three, some source code

 clearvars
close all
x_max = 640;
y_max = 480;
z_max = 400;

EPS = 20;
numNodes = 2000;        

q_start.coord = [0 0 0];
q_start.cost = 0;
q_start.parent = 0;
q_goal.coord = [640 400 180];
q_goal.cost = 0;

nodes(1) = q_start;
figure(1)

for i = 1:1:numNodes
    q_rand = [rand(1)*x_max rand(1)*y_max rand(1)*z_max];
    plot3(q_rand(1), q_rand(2), q_rand(3), 'x'.'Color'[0 0.4470 0.7410])
    
    % Break if goal node is already reached
    for j = 1:1:length(nodes)
        if nodes(j).coord == q_goal.coord
            break
        end
    end
    
    % Pick the closest node from existing list to branch out from
    ndist = [];
    for j = 1:1:length(nodes)
        n = nodes(j);
        tmp = dist_3d(n.coord, q_rand);
        ndist = [ndist tmp];
    end
    [val, idx] = min(ndist);
    q_near = nodes(idx);
    
    q_new.coord = steer3d(q_rand, q_near.coord, val, EPS);
    line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], [q_near.coord(3), q_new.coord(3)].'Color'.'k'.'LineWidth'.2);
    drawnow
    hold on
    q_new.cost = dist_3d(q_new.coord, q_near.coord) + q_near.cost;
    
    % Within a radius of r, find all existing nodes
    q_nearest = [];
    r = 50;
    neighbor_count = 1;
    for j = 1:1:length(nodes)
        if (dist_3d(nodes(j).coord, q_new.coord)) <= r
            q_nearest(neighbor_count).coord = nodes(j).coord;
            q_nearest(neighbor_count).cost = nodes(j).cost;
            neighbor_count = neighbor_count+1;
        end
    end
    
    % Initialize cost to currently known value
    q_min = q_near;
    C_min = q_new.cost;
    
    % Iterate through all nearest neighbors to find alternate lower
    % cost paths
    
    for k = 1:1:length(q_nearest)
        if q_nearest(k).cost + dist_3d(q_nearest(k).coord, q_new.coord) < C_min
            q_min = q_nearest(k);
            C_min = q_nearest(k).cost + dist_3d(q_nearest(k).coord, q_new.coord);
            line([q_min.coord(1), q_new.coord(1)], [q_min.coord(2), q_new.coord(2)], [q_min.coord(3), q_new.coord(3)].'Color'.'g');            
            hold on
        end
    end
    
    % Update parent to least cost-from node
    for j = 1:1:length(nodes)
        if nodes(j).coord == q_min.coord
            q_new.parent = j;
        end
    end
    
    % Append to nodes
    nodes = [nodes q_new];
end

D = [];
for j = 1:1:length(nodes)
    tmpdist = dist_3d(nodes(j).coord, q_goal.coord);
    D = [D tmpdist];
end

% Search backwards from goal to start to find the optimal least cost path
[val, idx] = min(D);
q_final = nodes(idx);
q_goal.parent = idx;
q_end = q_goal;
nodes = [nodes q_goal];
while q_end.parent ~= 0
  
    hold on
    q_end = nodes(start);
end
function d = dist_3d(q1,q2)
    d = sqrt((q1(1)-q2(1)) ^2 + (q1(2)-q2(2)) ^2 + (q1(3)-q2(3)) ^2);
end

function A = steer(qr, qn, val, eps)
   qnew = [0 0];
   if val >= eps
       qnew(1) = qn(1) + ((qr(1)-qn(1))*eps)/dist_3d(qr,qn);
       qnew(2) = qn(2) + ((qr(2)-qn(2))*eps)/dist_3d(qr,qn);
       qnew(3) = qn(3) + ((qr(3)-qn(3))*eps)/dist_3d(qr,qn);
   else
       qnew(1) = qr(1);
       qnew(2) = qr(2);
       qnew(3) = qr(3);
   end
 
Copy the code

4. 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. MATLAB Optimization Algorithm source code [M]. Tsinghua University Press, 2017. [3]RRT Path Planning Algorithm