I. Introduction of UAV

Introduction with the development of modern technology, there are more and more types of aircraft, and their applications are becoming increasingly specialized and perfect, such as DJI PS-X625 UAV, which is used for plant protection, BAOji X8 UAV, which is used for street scene shooting and monitoring patrol, and White Shark MIX underwater UAV, which 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.

1. Common path planning algorithms 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 particle filter algorithm

Particle filter is a bayesian suboptimal estimation algorithm, which gets rid of the constraint condition that the random quantity must meet the Gaussian distribution when solving nonlinear filtering problem, and solves the problem of particle sample shortage to a certain extent. Therefore, this algorithm has been successfully applied in many fields in recent years. However, particle degradation in particle filtering seriously limits the development of its basic methods. Particle filtering see MCMC improved particle filtering algorithm and its application in target tracking

Three, some source code

% because this program involves too many random numbers, let the random number each time constant rand('seed'.3);
randn('seed'.6);
% error('Please refer to the book for the following parameter T, then delete this line of code') 
n = 9;
T = 50;

Q= [1 0 0 0 0 0 0 0 0; % process noise covariance matrix0 1 0 0 0 0 0 0 0;
    0 0 1 0 0 0 0 0 0;
    0 0 0 0.01 0 0 0 0 0;
    0 0 0 0 0.01 0 0 0 0;
    0 0 0 0 0 0.01 0 0 0;
    0 0 0 0 0 0 0.0001 0 0;
    0 0 0 0 0 0 0 0.0001 0;
    0 0 0 0 0 0 0 0 0.0001];

R = [5000 0 0; % Observation noise covariance matrix0 0.01^2 0The observed deviation of % Angle should not be too large0 0 0.01^2]; % System initialization X = zeros(9,T); % true Z = zeros(3,T); % true state initialization %X(:,1) = [1000;5000;200;10;50;10;2;4 -;2]+sqrtm(Q)*randn(n,1);
X(:,1) = [100;500;20;10;50;10;2;4 -;2]+sqrtm(Q)*randn(n,1);
state0 = X(:,1);

x0=0;           
y0=0; 
z0=0; Station=[x0;y0;z0]; % Observation position P0 =[100 0 0 0 0 0 0 0 0; % covariance initialization0 100 0 0 0 0 0 0 0;
     0 0 100 0 0 0 0 0 0;
     0 0 0 1 0 0 0 0 0;
     0 0 0 0 1 0 0 0 0;
     0 0 0 0 0 1 0 0 0;
     0 0 0 0 0 0 0.1 0 0;
     0 0 0 0 0 0 0 0.1 0
     0 0 0 0 0 0 0 0 0.1]; %%%%%%%%%%%%% EKF filter algorithm %%%%%%%%%%%% Qekf = Q; % EKF process noise variance Rekf = R; % EKF Process noise variance Xekf= Zeros9,T); % Filter status Xekf(:,1)=X(:,1); % EKF filter initialization Pekf = P0; % Tekf=zeros(1,T); % is used to record a sampling period algorithm time consumption % % % % % % % % % % % % % UKF filter algorithm % % % % % % % % % % % % Qukf = Q; % UKF process noise variance Rukf = R; % UKF Observation noise variance Xukf= Zeros9,T); % Xukf(:,1)=X(:,1); % UKF filter initialization Pukf = P0; Covariance Tukf=zeros(1,T); % is used to record a sampling period algorithm time consumption % % % % % % % % % % % % % PF filtering algorithm % % % % % % % % % % % % N =200; % particle number Xpf=zeros(n,T); % Filter status Xpf(:,1)=X(:,1); % PF filter initialization Xpfset=ones(n, n); % Particle set initializationfor j=1:N % Particle set initialization Xpfset(:,j)=state0; % all initialized to x0, equal for each particle end Tpf=zeros(1,T); % is used to record a sampling period algorithm time consumption % % % % % % % % % % % % % PF2 filtering algorithm % % % % % % % % % % % % N2 =200; % Particle number R2 = [5000 0 0; % Observation noise covariance matrix0 0.01^2 0The observed deviation of % Angle should not be too large0 0 0.01^2]; Xpf2=zeros(n,T); % Xpf2(:,1)=X(:,1); % PF filter initialization Xpf2set=ones(n,N2); % Particle set initializationfor j=1: n2% particle set initialization Xpf2set(:,j)=state0; % all initialized to x0, equal to the value of each particle end % % % % % % % % % % % % % PF3 filtering algorithm % % % % % % % % % % % % N3 =400; % particle number R3 = [5000 0 0; % Observation noise covariance matrix0 0.01^2 0The observed deviation of % Angle should not be too large0 0 0.01^2]; Xpf3=zeros(n,T); % filter status Xpf3(:,1)=X(:,1); % PF filter initialization Xpf3set=ones(n,N3); % Particle set initializationfor j=1: n3% Particle set initialization Xpf3set(:,j)=state0; % all initialized to x0, each particle has the same value end Rekf2 = R2; Xepf2=zeros(9,T); % Filter status Xepf2(:,1)=X(:,1); % EPF filter initialization Xepf2set=ones(n,N2); The % particle set is initialized and needs to be defined as one3The dimensional array, or for simplicity, written all at once, is defined as a (9XN), representing all particles in their current statefor j=1: n2% Particle set initialization Xepf2set(:,j)=state0; % all initialized to state0, with equal values for each particle End Pepf2 = P0*ones(n,n*N2); % of the covariance of each particle, and I need to define it as one3The dimensional array, or for simplicity, write it all at once, is defined as one9x(9Two dimensional array of xN) % % % % % % % % % % % % % EPF3 filtering algorithm % % % % % % % % % % % % Rekf3 = R3; Xepf3=zeros(9,T); % Filter status Xepf3(:,1)=X(:,1); % EPF filter initialization Xepf3set=ones(n,N3); The % particle set is initialized and needs to be defined as one3The dimensional array, or for simplicity, written all at once, is defined as a (9XN), representing all particles in their current statefor j=1:N3 % Particle set initialization Xepf3set(:,j)=state0; % all initialized to state0, with equal values for each particle end Pepf3 = P0*ones(n,n*N3); % of the covariance of each particle, and I need to define it as one3The dimensional array, or for simplicity, write it all at once, is defined as one9x(9Two dimensional array of xN) % % % % % % % % % % % % % UPF filtering algorithm % % % % % % % % % % % % Xupf = zeros (n, T); % Filter status Xupf(:,1)=X(:,1); % UPF filter initialization Xupfset=ones(n, n); % Particle set initializationfor j=1:N % Particle set initialization Xupfset(:,j)=state0; % all initialized to state0, with equal values for each particle end Pupf = P0*ones(n,n* n); % Covariance of each particle Tupf= Zeros (1,T); % is used to record the algorithm time consumption of a sampling period Xmupf = Zeros (n,T); % Filter status Tmupf = zeros(1,T); % % % % % % % % % % % % % % % % % % % % % % simulation system run % % % % % % % % % % % % % % % % % % % % % % % % %for t=2: T % simulation system state run step [y1, y2, y3, y4, y5, y6, y7, y8, y9] = feval ('ffun',X(:,t- 1));
    X(:,t)= [y1,y2,y3,y4,y5,y6,y7,y8,y9]'+ sqrtm(Q) * randn(n,1); For t=1: t [dd,alpha,beta]=feval('hfun',X(:,t),Station);
    Z(:,t)= [dd,alpha,beta]'+ SQRTM (R) * randn (3, 1); end [Xepf(:,t),Xepfset,Pepf,Neffepf]=epf(Xepfset,Z(:,t),n,Pepf,N,R,Qekf,Rekf,Station); % done Tepf (t) = toc; sum_epf = sum_epf + Neffepf; % call EPF2 algorithm [Xepf2 (:, t), Xepf2set, Pepf2, Neffepf] = epf (Xepf2set, Z (:, t), n, Pepf2, N2, R2, Qekf, Rekf2, Station); % % done call EPF3 algorithm [Xepf3 (:, t), Xepf3set, Pepf3, Neffepf] = epf (Xepf3set, Z (:, t), n, Pepf3, N3, R3, Qekf, Rekf3, Station); Tic % % % % done called UPF algorithm [Xupf (:, t), Xupfset, Pupf] = UPF (Xupfset, Z (:, t), n, Pupf, n, R, Qukf, Rukf, Station); % 1 %Tupf(t)=toc; End % % % % % % % % % % % % % % % % % % % % % % data analysis % % % % % % % % % % % % % % % % % % % % % % % % % % assumes the for I = 1: T Xupf (:, I) = X (: I) + 2 * sin (T); EKFrms = zeros(1,T); UKFrms = zeros(1,T); PFrms = zeros(1,T); EPFrms = zeros(1,T); PF2rms = zeros(1,T); EPF2rms = zeros(1,T); PF3rms = zeros(1,T); EPF3rms = zeros(1,T); UPFrms = zeros(1,T); EKFXrms = zeros(1,T); UKFXrms = zeros(1,T); PFXrms = zeros(1,T); EPFXrms = zeros(1,T); % X axis RMS deviation comparison chart EKFYrms = Zeros (1,T); UKFYrms = zeros(1,T); PFYrms = zeros(1,T); EPFYrms = zeros(1,T); EKFZrms = Zeros (1,T); UKFZrms = zeros(1,T); PFZrms = zeros(1,T); EPFZrms = zeros(1,T); for t=1:T EKFXrms(1,t)=abs(X(1,t)-Xekf(1,t)); UKFXrms(1,t)=abs(X(1,t)-Xukf(1,t)); PFXrms(1,t)=abs(X(1,t)-Xpf(1,t)); EPFXrms(1,t)=abs(X(1,t)-Xepf(1,t)); EKFYrms(1,t)=abs(X(2,t)-Xekf(2,t)); UKFYrms(1,t)=abs(X(2,t)-Xukf(2,t)); PFYrms(1,t)=abs(X(2,t)-Xpf(2,t)); EPFYrms(1,t)=abs(X(2,t)-Xepf(2,t)); EKFZrms(1,t)=abs(X(3,t)-Xekf(3,t)); UKFZrms(1,t)=abs(X(3,t)-Xukf(3,t)); PFZrms(1,t)=abs(X(3,t)-Xpf(3,t)); EPFZrms(1,t)=abs(X(3,t)-Xepf(3,t)); NodePostion = [100,800,100; 200,800,900; 0,0,0]; NodePostion = [100,800,100; 200,800,900; 0,0]; figure t=1:T; hold on; box on; grid on; for i=1:3 p8=plot3(NodePostion(1,i),NodePostion(2,i),NodePostion(3,i),'ro'.'MarkerFaceColor'.'b');
    text(NodePostion(1,i)+0.5,NodePostion(2,i)+0.5,NodePostion(3,i)+1,

figure
hold on;
box on;
p1=plot(1:T,EKFrms,'-k.'.'lineWidth'.2);
p2=plot(1:T,UKFrms,'-m^'.'lineWidth'.2);
p3=plot(1:T,PFrms,'-ro'.'lineWidth'.2);
%p4=plot(1:T,EPFrms,'-g*'.'lineWidth'.2);
p5=plot(1:T,UPFrms,'-bp'.'lineWidth'.2);
legend([p1,p2,p3,p5],'EKF deviation'.'UKF deviation'.'PF deviation'.'DFEPF deviation');
xlabel('time step');
ylabel('RMS prediction bias');

figure;
hold on;
box on;
p1=plot(1:T,PFrms,'-k.'.'lineWidth'.2);
p2=plot(1:T,EPFrms,'-m^'.'lineWidth'.2);
p3=plot(1:T,PF2rms,'-r.'.'lineWidth'.2);
p4=plot(1:T,EPF2rms,'-cp'.'lineWidth'.2);
p5=plot(1:T,PF3rms,'-g.'.'lineWidth'.2);
p6=plot(1:T,EPF3rms,'-bp'.'lineWidth'.2);
legend([p1,p2,p3,p4,p5,p6],'PF deviation (Rc = 5 r, N = 200)'.'DFEPF deviation (Rc = 5 r, N = 200)'.'PF deviation (Rc = 8 r, N = 200)'.'DFEPF deviation (Rc = 8 r, N = 200)'.'PF deviation (Rc = 5 r, N = 400)'.'DFEPF deviation (Rc = 5 r, N = 400)');
xlabel('time step');
ylabel('RMS prediction bias');

figure;
hold on;
box on;
p1=plot(1:T,Tekf,'-k.'.'lineWidth'.2);
p2=plot(1:T,Tukf,'-m^'.'lineWidth'.2);
p3=plot(1:T,Tpf,'-ro'.'lineWidth'.2);
p4=plot(1:T,Tepf,'-bp'.'lineWidth'.2);
legend([p1,p2,p3,p4],'EKF time'.'the UKF time'.'PF time'.'DFEPF time');
xlabel('time step');
ylabel('Single step time /s'); % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % draw a different Q, the different results of R % draw a deviation chartCopy 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] WU Qian, LUO Jinbiao, GU Xiaoqun, ZENG Qing. [4] Deng Ye, Jiang Xiangju. Optimization Algorithm of UAV 3D Trajectory Planning Based on Improved PSO [J]. Journal of Ordnance Equipment Engineering, 201,42(08). [5] Yunhong Ma, Heng Zhang, Legrong Qi, Jianliang He. Trajectory Planning Algorithm of quadrotor UAV based on improved Artificial Potential Field Method [J]. Sensors and Microsystems, 201,40(07) [5] 3d uav path planning based on improved A* algorithm [J]. Electronics optics & control, 2019,26(10) [6] jiao Yang. Research on 3d path planning of uav based on improved ant colony algorithm [J]. Ship electronic engineering, 2019,39(03)