[RRT 3D path planning] rapid expansion of random tree UAV 3D path planning based on MATLAB [including Matlab source code phase 1914]

One 、 A brief introduction to the three-dimensional path planning of UAV Based on the rapidly expanding random tree

1 The mathematical model of UAV path planning
When establishing the mathematical model of the three-dimensional track planning problem , Not only consider the basic constraints of UAV , Also consider the complex flight environment , Including mountain terrain and thunderstorm threat areas .

1.1 UAV basic constraints
Planned UAV 3D track , There are some basic constraints that need to be satisfied , Including the maximum turning angle 、 Maximum climb angle or glide angle 、 Minimum track segment length 、 Minimum and maximum flight altitude , And the maximum track length . among , Maximum turning angle constraint , It means that the UAV can only turn within the specified maximum turning angle in the horizontal plane ; Maximum climb angle or glide angle constraint , It means that the UAV can only climb or glide within the specified maximum climb angle or glide angle in the vertical plane ; Minimum track segment length constraint , Before requiring the UAV to change its flight attitude , The shortest flight range in the current track direction ; Minimum and maximum flight altitude constraints , The UAV is required to fly in the specified flight altitude range ; Maximum track length constraint , It means that the track length of UAV is less than or equal to the specified threshold .

remember q (x, y, z, θ, ψ) For the flight position and attitude of UAV , among , (x, y, z) For the location of the UAV , θ Is the horizontal turning angle of the UAV , ψ Is the vertical climb angle or glide angle of the UAV , Then the mathematical expression of the above basic constraints is established .

1.2 Modeling of obstacles and threat areas in flight environment
In the flight environment , The towering mountains are approximately represented by cone equivalent , In order to use e Generate the natural Index Graph for the bottom , that , Mountain terrain can be formed by superposition of multiple cones with different positions . If the reference altitude reference height is set to xOy Plane , remember (x, y, z) Points in mountain terrain , that

In style :N Is the number of mountains ;xk0 and yk0 For the first time k Abscissa and ordinate of the central symmetry axis of the mountain ;hk For the first time k The maximum height of the mountain ;xki and yki For the first time k The transverse and longitudinal slope of the mountain .

In the flight environment , Thunderstorms and other extreme weather usually exist near mountains , This article is regarded as a flight threat zone , And the approximate equivalent representation of the sphere , And remember the first k The spherical coordinates of the flight threat area near the mountain are (xks0, yks0, zks0) , The radius is rk.

1.3 Objective function and track representation
In this paper , A certain type of UAV that performs missions , The objective function of track planning is to generate a collision free feasible track from the starting point to the target point . use q (x, y, z, θ, ψ) It indicates the specific attitude of the UAV at a specific position in the flight airspace , that (x, y, z) Indicates the track point where the UAV is located , θ Indicates the horizontal turning angle of the UAV , ψ Indicates the vertical climb angle or glide angle of the UAV . use r (q) Indicates the starting point qinitial To the target point qgoal Collision free feasible track , Then the process of track planning can be written in the following form :

1.4 Mathematical model representation
be based on 1.1~1.3 section , The mathematical model of UAV path planning problem is obtained .

Objective function :

classic RRT-Connect The algorithm only relies on the implicit connection heuristic greedy search function , Feasible track has been obtained , Although not the optimal track , And the search time cost of feasible track points is high , However, the feasible path of UAV can be obtained without introducing external mathematical model , This is a remarkable feature of the algorithm ; When the mathematical model is introduced to solve the path planning problem of UAV , The search of track points by random tree will be changed from the greedy search in the whole space to the greedy search in the region satisfying the constraint function of the mathematical model , It greatly reduces the search space , At the same time, the searching time of feasible track points is saved , And to a certain extent, it shortens the total length of the generated feasible track .

Two 、 Partial source code

%%  Draw obstacles ( Take the ball as an example , Mainly for convenient calculation )
x0=100; y0=100; z0=100;% Spheric Center 
circleCenter = [100,100,100;50,50,50;100,40,60;150,100,100;60,130,50];
r=[50;20;20;15;15];% radius 
% Let's start painting 
figure(1);
[x,y,z]=sphere;
for i = 1:length(circleCenter(:,1))
    mesh(r(i)*x+circleCenter(i,1),r(i)*y+circleCenter(i,2),r(i)*z+circleCenter(i,3));hold on;
end
axis equal

%%  Parameters 
source=[10 10 10];
goal=[150 150 150];
stepsize = 10;
threshold = 10;
maxFailedAttempts = 10000;
display = true;
searchSize = [250 250 250];      % Explore the space hexahedron 

%%  Draw start and end points 
hold on;
scatter3(source(1),source(2),source(3),'filled','g');
scatter3(goal(1),goal(2),goal(3),'filled','b');

tic;  % tic-toc: Functions for Elapsed Time
RRTree = double([source -1]);
failedAttempts = 0;
pathFound = false;

%%  loop 
while failedAttempts <= maxFailedAttempts  % loop to grow RRTs
    %% chooses a random configuration
    if rand < 0.5
        sample = rand(1,3) .* searchSize;   % random sample
    else
        sample = goal; % sample taken as goal to bias tree generation to goal
    end

3、 ... and 、 Running results

Four 、matlab Edition and references

1 matlab edition
2014a

2 reference
[1] Baoziyang , Yu Jizhou , Poplar . Intelligent optimization algorithm and its application MATLAB example （ The first 2 edition ）[M]. Electronic industry press ,2016.
[2] Zhang Yan , Wu Shuigen .MATLAB Optimization algorithm source code [M]. tsinghua university press ,2017.
[3] Tianjiang , Li Erchao . Improved connected fast extended random tree algorithm for UAV 3D path planning [J]. Progress in aeronautical engineering . 2018,9(04)

3 remarks
This part of the introduction is taken from the Internet , For reference only , If infringement , Contact deletion