A list,

Artificial potential field method is a common method for local path planning. This method assumes that the robot moves under a kind of virtual force field.



As shown in the figure, the robot is moving in a two-dimensional environment. The figure indicates the relative positions of the robot, the obstacle and the target.



This figure clearly illustrates the function of artificial potential field method. The initial point of the object is on a high “mountain”, and the target point to be reached is under the “mountain 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, in which the target point exerts gravity on the object and guides the object towards it (this point is somewhat similar to the heuristic function H in the A* algorithm). Obstacles act as repulsive forces to prevent objects from colliding with them. The net force on the body 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 gravitational and repulsive fields. Let’s discuss it separately:

The gravitational field:

Commonly used gravity functions:



ρ(q,q_goal) represents the distance between the current state of the object and the target. With the gravitational field, gravity is the derivative of the gravitational field with respect to distance (W=FX in physics analogy) :



[δx,δy], this symbol is partial derivative, not quite correct, sorry.



Fig. Gravitational field model

Repulsive force field:



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

Repulsion is the gradient of the repulsion field





Fig repulsive field model

The total field is the superposition of the gravitational field in the repulsive field, that is, U=U_att+U_rep. The total force is also the superposition of the corresponding components, as shown in the figure below:



Second, existing problems

(a) When the object is further from the target point, the attraction will become extremely strong, and the relatively small repulsion force may even be negligible, so that the object may encounter obstacles in its path

(b) When there is an obstacle near the target point, the repulsive force will be very large and the attraction will be 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

Various improved versions of the artificial potential field method

(a) The problem that obstacles may be encountered can be solved by modifying the gravity function to avoid excessive gravity due to the distance from the target point



Compared with Eq. (1), eq. (5) adds scope limitation. D *_goal is given a threshold that limits the distance between the target and the object. The corresponding gradient, the gravitational force, becomes:



(b) The target is unreachable due to obstacles near the target point, and a new repulsive function is introduced



Here, 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 see that n=2 in some literature). Intuitively speaking, when the object approaches the target, although the repulsive force field increases, the distance decreases, so it can play a role of dragging the repulsive force field to a certain extent

The corresponding repulsion becomes:



So you can see that there are two parts to gravity, and you have to be careful when you program it

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

Ii. Source code

Function [d_obs_line] = distance_fuzzyControl(X,X_target,X_obs,n) %d_obs indicates the distance between the robot and the nearest obstacle. D_obs_line indicates the distance between the robot and the target Detailed explanation goes here % d_obs=0.8; % blur near and far boundary point %for i=1:n
%      D(i)=(X(1)-X_obs(i,1)) ^2+(X(1)-X_obs(i,2)) ^2; % Distance between waypoint and obstacle squared % d(I)=sqrt(D(i));
%      if(d_obs>d(i)) % d_obs=d(i); % end % end % D_target=(X)1)-X_target(1)) ^2+(X(2)-X_target(2)) ^2;
%     d_target=sqrt(D_target);
  %  d_obs_line=0.8; % is greater than as it nears the target0.8Think the robot should move in a straight line Xo=[0.0]; % starting position k=10; % Calculate the gain coefficient required by gravity K=0; % initializes m=1; % Calculate the gain coefficient of repulsive force, d=2; % obstacle affects distance. When the distance between obstacle and car is greater than this distance, the repulsive force is0, that is, they are not affected by the obstacle and set n=10; % Number of obstacles L =0.5; J = % step length200; X_target=[10.10];
X_obs=[1 1.5;3 3;4 4.5;3 6;6 2.5;5.5 7;8 8.5;9.9.5;10 5;7 6]; The % vector is n star2Dimension, obstacle position X_robot=Xo; % X_robot is robot positioning coordinate, coordinate Xo is assigned to the car starting X_robt % * * * * * * * * * * * * * * * end of the initialization, the main circulation to * * * * * * * * * * * * * * * * * * /for j=1:J
   RobotPoint(j,1)=X_robot(1); % is given the initial value,RobotPoint stores the coordinates of each point passed by the robot. At the beginning, the starting point is put into the vector RobotPoint(j,2)=X_robot(2); % Call the Angle module [ANGle_AT, ANGle_re]=compute_angle(X_robot,X_target,X_obs,n); % ANGle_AT, ANGle_RE is the calculated Angle between the robot and the target/obstacle and the X-axis, X_at/y_AT is the sum of gravity components on x/ Y axis. [X_AT, y_AT]=compute_attraction(X_robot,X_target,k, ANGLE_AT); % Robot coordinates, target point, gain constant, Angle % Call repulsive module [X_re,y_re]=compute_repultion(X_robot,X_target,X_obs, M, ANGle_RE,n, D); % Input parameter is the current coordinate, Xsum is the coordinate vector of the obstacle, gain constant, Angle of the obstacle direction, number of obstacles, influence threshold % The components of the resultant force on x and y axes F_xj= X_AT + X_re; F_yj=y_at+y_re; % Calculate the Angle between the resultant force and X-axisif F_xj>0
       angle_F(j)=atan(F_yj/F_xj);
     else
       angle_F(j)=pi+atan(F_yj/F_xj); End % Xnext(1)= X_robot(1)+l*cos(angle_F(j));
     Xnext(2)= X_robot(2)+l*sin(angle_F(j)); X_robot=Xnext; % Judge whether the target point is reached, the distance within one step is considered to reach the target point r_target=sqrt((X_robot(1)-X_target(1)) ^2+(X_robot(2)-X_target(2)) ^2); Consider close to the target point, as far as possible in a straight line, to prevent repeated concussionif(r_target>l&&r_target<2*l)
       [d_obs_line] = distance_fuzzyControl( X_robot,X_target,X_obs,n);
       if( d_obs_line>=0.5%far, will not affect the linear motion of the robot Xnext(1)= X_robot(1)+l*cos(angle_at);
         Xnext(2)= X_robot(2)+l*sin(angle_at); X_robot=Xnext; End End % Judge whether the target point is reached, the distance is within one stepif(r_target<=l) K=j; % records the number of iterations to reach the target.break; End % If notifTo return to the loop and continue execution. End % major loop endsCopy the code

3. Operation results

Fourth, note

Version: 2014 a

Complete code or ghostwrite plus 1564658423