Table of contents
1. Theoretical basis
1. Introduction
The UAV path planning algorithm based on the potential field method is a commonly used UAV path planning algorithm. machine path. The algorithm has the advantages of simple calculation and smooth path, and is widely used in UAV path planning. This paper will introduce the principle, mathematical formula and implementation process of the UAV path planning algorithm based on the potential field method in detail.
2. Principle
The basic principle of the UAV path planning algorithm based on the potential field method is to use the gradient of the potential field to plan the path of the UAV. The potential field can be understood as the influence of the environment around the drone, such as obstacles, target points, etc. In the potential field method, the target point of the UAV is regarded as an attractor, and the obstacle is regarded as a repulsor. The path planning process of the UAV is a process of finding a balance point between the attractor and the repulsion.
In the potential field method, the position of the drone is expressed as (x, y), and the potential field can be expressed as U(x, y), then the gradient of the potential field can be expressed as:
?U(x,y) = (?U/?x, ?U/?y)
In UAV path planning, the target point is usually regarded as an attractor, and its potential field can be expressed as:
U_goal(x,y) = k * ((x-x_goal)^2 + (y-y_goal)^2)
Among them, k is a constant, and (x_goal, y_goal) is the coordinate of the target point.
Obstacles are usually regarded as repulsors, and their potential field can be expressed as:
U_obs(x,y) = ∑(k_obs/d_obs - 1/d_obs^2) * f(d_obs)
Among them, k_obs is a constant, d_obs is the distance from the drone to the obstacle, f(d_obs) is the correction function of the obstacle, usually:
f(d_obs) = {1, d_obs > d0
{(d_obs-d0)^2/((d_obs-d0)^2-1), d_obs <= d0
Among them, d0 is the scope of the obstacle.
The displacement of the UAV in the potential field can be expressed as:
Δx = -α * ?U(x,y)
Among them, α is the step size parameter.
In UAV path planning, it is necessary to continuously calculate the potential field around the UAV, and update the position of the UAV according to the gradient of the potential field until the UAV reaches the target point.
3. Mathematical formula
Gradient of the potential field
?U(x,y) = (?U/?x, ?U/?y)
The potential field of the target point
U_goal(x,y) = k * ((x-x_goal)^2 + (y-y_goal)^2)
The potential field of obstacles
U_obs(x,y) = ∑(k_obs/d_obs - 1/d_obs^2) * f(d_obs)
Correction function of obstacles
f(d_obs) = {1, d_obs > d0
{(d_obs-d0)^2/((d_obs-d0)^2-1), d_obs <= d0
The displacement of the drone
Δx = -α * ?U(x,y)
4. Implementation process
The realization process of UAV path planning algorithm based on potential field method includes the following steps:
Initialize the position of the drone and the position of the target point.
Obtain the surrounding environment information, including the position and range of obstacles, etc.
Calculate the potential field according to the position of the target point and the obstacle, and find the gradient of the potential field.
Calculate the displacement of the UAV according to the gradient of the potential field, and update the position of the UAV.
Determine whether the UAV reaches the target point, and if so, end the path planning.
If the UAV encounters an obstacle, the potential field is adjusted according to the position and range of the obstacle to make it generate repulsion, so as to avoid the UAV from colliding with the obstacle.
Repeat the above steps until the drone reaches the target point.
The following is the pseudocode of the UAV path planning algorithm based on the potential field method:
1. Initialize the position of the UAV and the position of the target point
2. while (the UAV has not reached the target point) do
3. Obtain the surrounding environment information , including the position and scope of the obstacle, etc.
4. Calculate the potential field according to the position of the target point and the obstacle, and find the gradient of the potential field
5. Calculate the displacement of the UAV according to the gradient of the potential field, and update the UAV's Position
6. Determine whether the drone has reached the target point. If it arrives, end the path planning.
7. If the drone encounters an obstacle, adjust the potential field according to the position and range of the obstacle to make it generate repulsion to avoid When the UAV collides with obstacles
end while
, it should be noted that in practical applications, in order to improve the efficiency and accuracy of path planning, the algorithm needs to be optimized and improved, such as using multi-layer potential fields, dynamically adjusting the step size, etc. method.
In conclusion, the UAV path planning algorithm based on the potential field method is a simple and effective UAV path planning algorithm, and its basic idea is to use the gradient of the potential field to plan the path of the UAV. In practical applications, it can be optimized and improved according to specific situations to achieve better path planning results.
2. Core program
............................................................................
%,
%
Xo=[0 0];%
k=20;%
K=0;%
m=5;%
Po=2;%?0?
n=8;%
a=0.5;
l=0.05;%
J=500;%
%?Po?
%end
%
Xsum=[10 10;1 1.5;3 2.2;4 4.5;7 6;6 2;5.5 6;8 7.8;9.5 7];%(n+1)*2?[10 10]
Xj=Xo;%j=1??Xj
[x,y]=meshgrid(-1:0.5:12,-1:0.5:12);
z=0.5*k./(sqrt((x-10).^2+(y-10).^2+0.09))-0.5*m*(1./(sqrt((x-1).^2+(y-1.5).^2+0.09))-1/3.5).^2-0.5*m*(1./(sqrt((x-3).^2+(y-2.2).^2+0.09))-1/3.5).^2 ...
-0.5*m*(1./(sqrt((x-4).^2+(y-4.5).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-7).^2+(y-6).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-6).^2+(y-2).^2+0.09))-1/3.5).^2 ...
-0.5*m*(1./(sqrt((x-5.5).^2+(y-6).^2+0.09))-1/4).^2-0.5*m*(1./(sqrt((x-8).^2+(y-7.8).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-9.5).^2+(y-7).^2+0.09))-1/3).^2;
% contour(x,y,z,[-50:20:1000]);
[C,h]=contour(x,y,z,[-80:10:300]);
set(h,'ShowText','on','TextStep',get(h,'LevelStep')*2)
colormap cool
[px,py]=gradient(z);%?x,y?
quiver(x,y,px,py,'k') %
p=sqrt(px.^2+py.^2);
%%
t=1;
M(t)=getframe;
t=t+1;
%*********************************
for j=1:J%
Goal(j,1)=Xj(1);%Goal?
Goal(j,2)=Xj(2);
%
Theta=compute_angle(Xj,Xsum,n);%Theta??X?
%
Angle=Theta(1);%Theta?1?
angle_at=Theta(1);%?angle_at
[Fatx,Faty]=compute_Attract(Xj,Xsum,k,Angle,0,Po,n);%?x,y?
for i=1:n
angle_re(i)=Theta(i+1);%?n??n?
end
%
[Frerxx,Freryy,Fataxx,Fatayy]=compute_repulsion(Xj,Xsum,m,angle_at,angle_re,n,Po,a);%?x,y?
%?j?
Fsumyj=Faty+Freryy+Fatayy;%y?
Fsumxj=Fatx+Frerxx+Fataxx;%x?
Position_angle(j)=atan(Fsumyj/Fsumxj);%?x?
%
Xnext(1)=Xj(1)+l*cos(Position_angle(j));
Xnext(2)=Xj(2)+l*sin(Position_angle(j));
%
Xj=Xnext;
X=Goal(:,1);
Y=Goal(:,2);
plot(X,Y,'.r');
M(t)=getframe;t=t+1;
%
if ((Xj(1)-Xsum(1,1))>0)&((Xj(2)-Xsum(1,2))>0)%
K=j;%
break;
%?j?
end%?if?
end%
up21303. Simulation conclusion
