[Path planning] Automatic obstacle avoidance based on matlab artificial potential field method [including Matlab source code 620]

1. Introduction

Artificial potential field method is a relatively common method for local path planning. This method assumes that the robot is moving under a virtual force field.
Insert picture description here
As shown in the figure, the robot moves in a two-dimensional environment. The figure points out the relative positions of the robot, obstacles and targets.

This figure clearly illustrates the effect of the artificial potential field method. The initial point of the object is on a higher "mountain top", and the target point to be reached is under the "mountain foot". This forms a potential field. Under the guidance of this trend, avoid obstacles and reach the target point.

The artificial potential field includes the repulsive force field of gravitational occasions, in which the target point generates gravitational force on the object and guides the object to move towards it (this is a bit similar to the heuristic function h in the A* algorithm). Obstacles generate repulsive force on objects to avoid collisions with objects. The resultant force of the object at each point on the path is equal to the sum of all repulsive forces and gravitational forces at that point. The key here is how to construct the gravitational field and the repulsive field. Let's discuss them separately below:

Gravitational field:

Commonly used gravitational function:
Insert picture description here
where ε is the scale factor. ρ(q,q_goal) represents the distance between the current state of the object and the target. With the gravitational field, then the gravitation is the derivative of the gravitational field with respect to the distance (W=FX in analogy physics): For the
Insert picture description here
gradient algorithm, please refer to related materials. Simply mention that the binary function gradient is Jiangzi’s [δx, δy], This symbol is a partial derivative, which is not quite right, forgive me.

Fig. Gravitational field model

Repulsion field:
Insert picture description here
Formula (3) is a traditional repulsion field formula, and it is still unclear how it was derived. 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 influence radius of each obstacle. In other words, leaving a certain distance, the obstacle has no repulsive influence on the object.

The repulsion is the gradient of the repulsion field.
Insert picture description here

Fig. The model of the repulsion field

The total field is the superposition of the gravitational field in the case of repulsion, that is, U=U_att+U_rep, and the total force is also the superposition of the corresponding component forces, as shown in the following figure:
Insert picture description here
2. Existing problems
(a) When the object is compared to the target point When far away, the gravitational force will become very large, and the relatively small repulsion force may even be negligible, and obstacles may be encountered in the path of the object.

(B) When there is an obstacle near the target point, the repulsive force will be very large, and the gravitational force will be relatively small, so it is difficult for the object to reach the target point.

(C) At a certain point, the gravitational force and the repulsive force are exactly equal, and the direction is reversed, the object will easily fall into a local optimal solution or oscillate

3. Various improved versions of the artificial potential field method
(a) For the problem that may encounter obstacles, it can be solved by modifying the gravitation function to avoid excessive gravitation due to too far from the target point
Insert picture description here
. Compared with (1) , (5) formula increases the scope limit. d*_goal gives a threshold that limits the distance between the target and the object. The corresponding gradient, that is, the gravitational force becomes:

(b) The problem of obstacles near the target point causing the target to be unreachable, introduce a new repulsive force function
Insert picture description here
. Based on the original repulsive force field, the distance between the target and the object is added. Influence, (n is a positive number, I see a document where n=2). Intuitively speaking, when an object approaches the target, although the repulsive force field will increase, but the distance will decrease, so to a certain extent, it can play a dragging effect on the repulsive force field.

The corresponding repulsive force becomes:
Insert picture description here
so you can see that the gravitational force is divided into two parts, you must pay special attention when programming

(C) The local optimal problem is a big problem of the artificial potential field method. Here, a random disturbance can be added to make the object jump out of the local optimal value. It is similar to the solution of the local optimal value of the gradient descent method.

Second, the source code

function [d_obs_line] = distance_fuzzyControl( X,X_target,X_obs,n)
%d_obs指机器人到最近障碍物的距离,d_obs_line指障碍物到机器人与目标连线的距离
%   Detailed explanation goes here
% d_obs=0.8;%模糊远近界点
% for i=1:n
%      D(i)=(X(1)-X_obs(i,1))^2+(X(1)-X_obs(i,2))^2;%路径点和障碍物的距离平方
%      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;%靠近目标点时，大于0.8认为机器人应该直线运动
Xo=[0,0];%起点位置
k=10;%计算引力需要的增益系数
K=0;%初始化
m=1;%计算斥力的增益系数，自己设定
d=2;%障碍影响距离，当障碍和车的距离大于这个距离时，斥力为0，即不受该障碍的影响,自己设定
n=10;%障碍个数
l=0.5;%步长
J=200;%循环迭代次数
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];%这个向量是n*2维，障碍的位置
X_robot=Xo;%X_robot是机器人的定位坐标,将车的起始坐标Xo赋给X_robt
%***************初始化结束，开始主体循环******************/
for j=1:J
   RobotPoint(j,1)=X_robot(1);%赋初值,RobotPoint存放机器人走过的每个点的坐标，刚开始先将起点放进该向量
   RobotPoint(j,2)=X_robot(2);
   %调用计算角度模块
   [angle_at,angle_re]=compute_angle(X_robot,X_target,X_obs,n);%angle_at,angle_re是计算出来的机器人和目标/障碍之间的与X轴之间的夹角，统一规定角度为逆时针方向
   %调用计算引力模块,x_at/y_at是引力在x/y轴的分量之和
   [x_at,y_at]=compute_attraction(X_robot,X_target,k,angle_at);%机器人坐标，目标点，增益常数，角度
   %调用斥力模块
   [x_re,y_re]=compute_repultion(X_robot,X_target,X_obs,m,angle_re,n,d);%输入参数为当前坐标，Xsum是障碍物的坐标向量，增益常数,障碍物方向的角度,障碍物个数,影响阈值
    %计算合力在x,y轴的分量
    F_xj=x_at+x_re;
    F_yj=y_at+y_re;
     %计算合力与x轴夹角
     if 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;
   %判断是否到达目标点,距离在一个步长之内认为到达目标点
    r_target=sqrt((X_robot(1)-X_target(1))^2+(X_robot(2)-X_target(2))^2);
   %考虑靠近目标点的情况，尽量直线运动，防止反复来回震荡
    if(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,不会影响机器人的直线运动
         Xnext(1)= X_robot(1)+l*cos(angle_at);
         Xnext(2)= X_robot(2)+l*sin(angle_at);  
         X_robot=Xnext;
       end
    end
    %判断是否到达目标点,距离在一个步长之内认为到达目标点
     if(r_target<=l)
       K=j;%记录迭代到多少次，到达目标。
       break;
     end%如果不符合if的条件，重新返回循环，继续执行。
end%大循环结束

Three, running results

Insert picture description here

Four, remarks