1. Introduction to RRT 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.
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.
colormap=[1 1 1; 0 0 0; 1 0 0; 0 1 0; 0 0 1];
imshow(uint8(map),colormap)
Copy the codeNote: 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 codeWhere: 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) :
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 codeAmong 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:
Three, some source code
function c; CLC Clear all close all % MAP1 % a=10;
% b=0.2;
% c=0.1;
% d=0.6;
% e=1;
% f=0.1;
% g=0.1;
% for x=1:80
% for y=1:80
% Z1=sin(y+a)+b*sin(x)+cos(d*(x^2+y^2) ^ (1/2))+e*cos(y)+f*sin(f*(x^2+y^2) ^ (1/2))+g*cos(y);
% % Z1=SquareDiamond(6.2.8);
% figure(1); % surf(Z1); % shading flat; % %map2 peaks tic; h=[20.35.25.38.20.25];
x0=[10.40.45.60.20.20];
y0=[10.25.50.30.45.10];
xi=[5.5.8.5.4.5.5.5.3.5];
yi=[5.7.6.5.5.6.4.5];
Z2=CeatHill(6,h,x0,y0,xi,yi,80);
figure(2); surf(Z2); % Draw 3d curved shading flat; % Z3= Max (Z1,Z2); % figure(3); % surf(Z3); % shading flat; % Do not mesh between small surfaces segmentLength =5;
start_node = [5.70.5.0.0.0];
end_node =[80.30.10.1.0.0];
hold on
plot3(start_node(:,1),start_node(:,2),start_node(:,3),'r*');
plot3(end_node(:,1),end_node(:,2),end_node(:,3),'r*');
tree = start_node;
if ( (norm(start_node(1:3)-end_node(1:3))<segmentLength )...
&(collision(start_node,end_node)==0) )
path = [start_node; end_node];
else
numPaths = 0;
while numPaths<1,
[tree,flag] = extendTree(tree,end_node,segmentLength,Z2);
numPaths = numPaths + flag;
end
end
path = findMinimumPath(tree);
plot3(path(:,1),path(:,2),path(:,3),'r');
toc;
function [data]=CeatHill(N,h,x0,y0,xi,yi,num)
x=1:1:num; y=1:1:num;
for m=1:num
for n=1:num
Sum=0;
for k=1:N
s=h(k)*exp(-((x(m)-x0(k))/xi(k))^2-((y(n)-y0(k))/yi(k))^2);
Sum=Sum+s;
end
data(m,n)=Sum;
end
end
function collision_flag = collision(node, parent,Z2,high);
collision_flag = 0;
x0=[10.40.45.60.20.20];
y0=[10.25.50.30.45.10];
xi=[5.5.8.5.4.5.5.5.3.5];
yi=[5.7.6.5.5.6.4.5];
Z1=Z2;
% for i=1:80
% for j=1:80
% if(Z1(j,i)>high)
% Z1(j,i)=10000;
% end
% end
% end
if ((node(1) >80)...
| (node(1) <0)...
| (node(2) >80)...
| (node(2) <0))
collision_flag = 1;
else
for sigma = 0:0.1:1,
p = sigma*node(1:3) + (1-sigma)*parent(1:3);
Sum1=0;
for k=1:6
Sum1=Sum1+s;
end
if(p(3)<Sum1)
collision_flag = 1;
end
% [line,row]=find(Z1==10000);
% ct=length(line);
% for tt=1:ct
% B=floor(p(1));
% C=floor(p(2));
% if (B==line(tt,1)&C==row(tt,1))
% collision_flag = 1;
% end
% end
end
end
function [new_tree,flag,high] = extendTree(tree,end_node,segmentLength,Z2);
flag1 = 0;
qet=1;
while flag1==0,
% select a random point
if(qet==0)
randomPoint = [...
80*rand,...
80*rand,...
80*rand];
else
randomPoint = [...
end_node(:,1),...
end_node(:,2),...
end_node(:,3)];
end
% find leaf on node that is closest to randomPoint
%0.45*sqrt(tree(:,1:2)-ones(size(tree,1),1)*randomPoint)
tmp = sqrt(tree(:,1:3)-ones(size(tree,1),1)*end_node(:,1:3));
[dist,idx] = min(diag(tmp*tmp'));
agl=acos((dot(A,B)/(norm(A)*norm(B))))*180/pi;
if (agl>80)
continue;
end
cost= tree(idx,4) + segmentLength;
new_point = (randomPoint-tree(idx,1:3));
new_point = tree(idx,1:3)+new_point/norm(new_point)*segmentLength;
if(qet==1)
high=new_point(:,3);
end
if(tree(idx,3)==end_node(:,3))
new_point(:,3)=end_node(:,3);
end
if(tree(idx,3)>end_node(:,3))
new_point(:,3)=tree(idx,3)-new_point(:,3)/norm(new_point)*segmentLength;
end
new_node = [new_point, 0, cost, idx];
% hold on
if collision(new_node, tree(idx,:),Z2,high)= =0,
new_tree = [tree; new_node];
% plot3(new_point(:,1),new_point(:,2),new_point(:,3),'r*');
flag1=1;
else
qet=0;
flag1=0;
end
end
% check to see if new node connects directly to end_node
if ( (norm(new_node(1:3)-end_node(1:3))<segmentLength )...
&(collision(new_node,end_node,Z2,high)==0))
flag = 1;
new_tree(end,4) =1; % mark node as connecting to end. end function path = findMinimumPath(tree); connectingNodes = [];for i=1:size(tree,1),
if tree(i,4) = =1,
connectingNodes = [connectingNodes; tree(i,:)];
end
end
% find minimum cost last node
[tmp,idx] = min(connectingNodes(:,5));
% construct lowest cost path
path = [connectingNodes(idx,:)];
parent_node = connectingNodes(idx,6);
while parent_node>1,
path = [tree(parent_node,:); path];
parent_node = tree(parent_node,6);
end
Copy the code4. 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