A list,

Artificial potential field method is a common method in local path planning. This method assumes that the robot moves under a virtual force field.As shown in the figure, the robot is moving in a two-dimensional environment, and the figure indicates the relative position between the robot, the obstacle and the target.This figure clearly illustrates the effect of the artificial potential field method. The initial point of the object is on a high “hill”, and the target point to be reached is under the “hill foot”, which forms a potential field. Under the guidance of this potential, the object avoids obstacles and reaches the target point.

The artificial potential field includes the repulsive field in the gravitational field, where the target point exerts gravity on the object and guides the object to move towards it (this point is somewhat similar to the heuristic function H in A* algorithm). An obstacle exerts a repulsive force on an object and prevents it from colliding with it. The net force on the object at each point along its path is equal to the sum of all the repulsive and gravitational forces at that point. The key here is how to construct the gravitational and repulsive fields. Let’s discuss each one below:

The gravitational field:

Commonly used gravitational functions:ρ(q,q_goal) represents the distance between the object’s current state and the target. Gravitational field has, then gravity is the derivative of gravitational field with respect to distance (analogy in physics W=FX) :,On the gradient algorithm can refer to relevant information, briefly mention, binary function gradient is [δx,δy], this symbol is partial derivative, not quite right, sorry.Fig. Gravitational field model

Repulsive force field:Formula (3) is a traditional repulsive field formula, and it is not clear how to derive it. In the formula, η is the repulsive force scale factor and ρ (q,q_obs) represents the distance between the object and the obstacle. ρ_0 represents the radius of influence of each obstacle. In other words, at a certain distance, the obstacle has no repulsive effect on the object.

The repulsive force is just the gradient of the repulsive fieldFig repulsive field model

The total field is the superposition of the gravitational field in the repulsiveness field, that is, U=U_att+U_rep. The total force is also the superposition of the corresponding component force, as shown in the figure below:(a) When the object is far away from the target point, the gravity will become particularly large, relatively small repulsive force in even negligible circumstances, the object path may encounter obstacles

(b) When there is an obstacle near the target point, the repulsive force will be very large and the gravity is relatively small, making it difficult for the object to reach the target point

(c) At a certain point, the gravitational and repulsive forces are just equal in magnitude, and the direction is opposite, then the object is easy to fall into the local optimal solution or oscillation

3. Various improved versions of artificial potential field method (a) The problem that obstacles may be encountered can be solved by modifying the gravity function to avoid too much gravity due to being too far away from the target pointCompared with Equation (1), Equation (5) increases the range limit. D *_goal specifies a threshold that defines the distance between a goal and an object. The gradient and therefore the force of gravity becomes:(b) The problem that the target is unreachable due to obstacles near the target point. A new repulsive function is introducedHere, on the basis of the original repulsive field, the influence of the distance between the target and the object is added (n is a positive number, I saw a literature on n=2). Intuitively, when an object is close to the target, although the repulsive field increases, the distance is decreasing, so to some extent, it can play the role of dragging the repulsive field

The corresponding repulsive force becomes:So you can see that gravity has two parts here, and you have to be careful when you program it

(c) The local optimal problem is a big problem of the artificial potential field method, where the object can jump out of the local optimal value by adding a random disturbance. The solution is similar to the local optimal value of gradient descent method.

Ii. Source code

clear all; Obstacle and goal, starting position are known path planning, intention to achieve from the starting point can be planned to avoid obstacles to reach the goal of the path. % Initialization car parameters Xo=[0 0]; % starting position k=1000; % The gain coefficient required to calculate gravity %K=0; % initializes m=10; % the gain coefficients for calculating the repulsive forces are set by themselves. Po=1; % The obstacle affects the distance. When the distance between the obstacle and the car is greater than this distance, the repulsion force is0Is not affected by the disorder. I set it myself. n=9; % Number of obstacles a=0.5;
l=0.1; J = % step length300; % Number of loop iterations r =0.5; % If the desired goal is not achieved, it may also be due to the initial gain coefficient, Po set improperly. Xsum=[Xsum=[10 10;1 1.5;3 2.2;4 4.5;3 6;6 2.5;5.5 6; 6 4.5;9 9;8.5 5]; % this vector is n plus1) *2Dimension, where [10 10] is the target position, and the rest are the obstacle positions. Xj=Xo; %j=1At the beginning of the cycle to the car starting coordinate assigned to Xj % * * * * * * * * * * * * * * * end of the initialization, body circulation to * * * * * * * * * * * * * * * * * *for j=1:J% Start loop Goal(J,1)=Xj(1); %Goal saves the coordinates of each point the car passes. I'm going to start by putting my starting point in this vector. Goal(j,2)=Xj(2); Theta=compute_angle(Xj,Xsum,n); %Theta is the calculated Angle between the car and the obstacle, and the target and the X-axis. The Angle is uniformly specified as counterclockwise, which can be calculated with this module. Angle=Theta()1); % Theta (1) is the Angle between the vehicle and the target. The target is pulling on the vehicle. angle_at=Theta(1); For the following calculation of the components of the repulsion force in the direction of gravity, assign angle_at [Fatx,Faty]=compute_Attract(Xj,Xsum,k,Angle,0,Po,n); % Calculate the values of the two components of the target's gravitational force on the car in the x and Y directions.for i=1:n
        angle_re(i)=Theta(i+1); %, the Angle used to calculate the repulsion force, is a vector, because there are n obstacles, there are n angles. End % repulsion call calculation module [Frerxx Freryy, Fataxx, Fatayy] = compute_repulsion (Xj, Xsum, m, angle_at, angle_re, n, Po, a, r); % Calculate the array of components of the repulsion force in the x and y directions. % to calculate the force and the direction, there's a problem, it should be a number, the force for each j loop should be a unique number, not an array. You have to add up all the components of the repulsion, all the components of the gravity. Fsumyj=Faty+Freryy+Fatayy; %y direction net force Fsumxj=Fatx+Frerxx+Fataxx; Position_angle(j)=atan(Fsumyj/Fsumxj); % the Angle vector between the resultant force and the X-axis direction % calculate the next position of the car % Save each position of the car in the vector Xj=Xnext; % judgmentif ((Xj(1)-Xsum(1.1)) >0)&((Xj(2)-Xsum(1.2)) >0) Should % count as arriving when it is exactly equal, or just close? Now program at exactly the same time. K=j % records how many times the iteration reaches the target.break; % record the j value at this time end% if not matchifTo return to the loop and continue execution. End % large loop end K=j; Goal(K,1)=Xsum(1.1); % assign the last point in the path vector as Goal(K,2)=Xsum(1.2); % * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * draw obstacles, starting point and goal, Path point * * * * * * * * * * * * * * * * * * * * * * * * * % % path to draw the vector Goal is a two dimensional array, X, Y, respectively is an array of X, Y element set, are the two one dimension array. x=[1 3 4 3 6 5.5 6  9 8.5]; % obstacle x coordinate y=[1.5 2.2 4.5 6 2.5 6 4.5 9 5];
plot(10.10.'v'.0.0.'ms',X,Y ,'-k'.'linewidth'.3); Computing Pathkm = K * l % repulsive force function Yrerxx Yreryy, Yataxx, Yatayy] = compute_repulsion (X, Xsum, m, angle_at angle_re, n, Po, a, r) % The input parameter is the current coordinate, Xsum is the coordinate vector of the target and the obstacle, the gain constant, the obstacle, the Angle of the target direction Rat=(X(1)-Xsum(1.1)) ^2+(X(2)-Xsum(1.2)) ^2; % The square of distance between the waypoint and the target rat=sqrt(Rat); % Distance between waypoint and targetfor i=1:n
    Rrei(i)=(X(1)-Xsum(i+1.1)) ^2+(X(2)-Xsum(i+1.2)) ^2; % Distance squared between waypoint and obstacle Rre (I)=sqrt(Rrei(i))-r; % path point and obstacle distance stored in array rreI R0=(Xsum(1.1)-Xsum(i+1.1)) ^2+(Xsum(1.2)-Xsum(i+1.2)) ^2;
    r0=sqrt(R0)-r;
    if rre(i)If the distance between each obstacle and the path is greater than the obstacle effect distance, the repulsive force is set to 0Yrerx(i)=0;
        Yrery(i)=0;
        Yatax(i)=0;
        Yatay(i)=0;
    else
%if r0<Po
       if rre(i)<Po/2
           if X(2)>Xsum(i+1.2)
               Yrer(i)=m*(1/rre(i)- 1/Po)*((1/rre(i))^2)*(rat); % Fre1 vector Yata(I)=a*m*(1/rre(i)- 1/Po)^2)*(rat^a); % decomposition of Fre2 vector Yrerx(I)= -yrer (I)*cos(angle_re(i)); %angle_re(i)=Y(i+1)
               Yrery(i)=1*Yrer(i)*sin(angle_re(i));
               Yatax(i)=Yata(i)*cos(angle_at); %angle_at=Y(1)
               Yatay(i)=Yata(i)*sin(angle_at);
           else
               Yrer(i)=m*(1/rre(i)- 1/Po)*((1/rre(i))^2)*(rat); % Fre1 vector Yata(I)=a*m*(1/rre(i)- 1/Po)^2)*(rat^a); % decomposition of Fre2 vector Yrerx(I)=Yrer(I)*cos(angle_re(i)); %angle_re(i)=Y(i+1)
               Yrery(i)=- 1*Yrer(i)*sin(angle_re(i));
               Yatax(i)=Yata(i)*cos(angle_at); %angle_at=Y(1)
               Yatay(i)=Yata(i)*sin(angle_at);
           end           
       else
            if X(2)>Xsum(i+1.2)
               Yrer(i)=m*(1/rre(i)- 1/Po)*((1/rre(i))^2)*(rat); % Fre1 vector Yata(I)=a*m*(1/rre(i)- 1/Po)^2)*(rat^a); % decomposition of Fre2 vector Yrerx(I)= -yrer (I)*cos(angle_re(i)); %angle_re(i)=Y(i+1)
               Yrery(i)=1*Yrer(i)*sin(angle_re(i));
               Yatax(i)=Yata(i)*cos(angle_at); %angle_at=Y(1)
               Yatay(i)=Yata(i)*sin(angle_at);
            else
                Yrer(i)=m*(1/rre(i)- 1/Po)*((1/rre(i))^2)*Rat; % Fre1 vector Yata(I)=a*m*(1/rre(i)- 1/Po)^2)*rat; % decomposition of Fre2 vector Yrerx(I)=Yrer(I)*cos(angle_re(i)); %angle_re(i)=Y(i+1)
                Yrery(i)=Yrer(i)*sin(angle_re(i));
                Yatax(i)=Yata(i)*cos(angle_at); %angle_at=Y(1)
                Yatay(i)=Yata(i)*sin(angle_at); End end end% Determine whether the distance is within the influence range of the obstacle end Yrerxx=sum(Yrerx); % superimposed repulsion component Yreryy=sum(Yrery); Yataxx=sum(Yatax); Yatayy=sum(Yatay);Copy the code

Third, the operation result

Fourth, note

Version: 2014 a