APF artificial potential field trajectory planning

Realize the original blog URL

The APF
implementation version is an improved version of APF, see the end of the text above (both gravitational and repulsive forces are improved).

Achieve effect

Two-dimensional (written by Matlab, relatively primitive)

When I first learned this, I referred to the code written by other people on the Internet. The quality is not very good, the naming is not standardized, and the readability is poor. Therefore, in the spirit of open source and happiness, I attached the two-dimensional improvement apf code of the matlab version:

close all;clear;clc;

%参数给定
obstacle = [1 1.2;3 2.5;4 4.5;3 6;6 2;5.5 5.5;8 8.5]; %障碍位置
qgoal = [10,10]; %目标点
x0 = [0 0];     %起始点
stepSize = 0.1;  %步长
iter = 1000;     %迭代次数
epsilon = 0.8;   %引力尺度因子
eta = 0.2;       %斥力尺度因子
dgoal = 5;     %引力分段函数分界值
r0 = 3;       %超过这个距离就没有斥力了
path = x0;  %记录轨迹的矩阵
threshold = 0.5;
%开始循环
q = x0;         %初始化位置
for i = 1 : iter
    Attraction = attraction(q,qgoal,dgoal,epsilon);   %引力
    Repulsion = repulsion(q,obstacle,r0,eta,qgoal);   %斥力
    compositeForce = Attraction + Repulsion;  %引力与斥力的合力(如果陷入局部最优可以加normrnd(0, 0.7, 1,2))
    q = q + stepSize * compositeForce;   %更新位置
    path = [path;q];
    if distanceCost(q,qgoal) < threshold
        path = [path;qgoal];
        break;
    end
end
figure(1)
scatter(obstacle(:,1),obstacle(:,2),'filled','r'); %绘制障碍点
hold on;
scatter(qgoal(1),qgoal(2),88,'filled','g')   %绘制目标点
scatter(x0(1),x0(2),88,'filled','b')          %绘制起始点
plot(path(:,1),path(:,2),'LineWidth',1,'Color','r');    %绘制路径
grid on;

function f = attraction(q,qgoal,dgoal,epsilon)
    r = distanceCost(q,qgoal);
    if r <= dgoal
        fx = epsilon * (qgoal(1) - q(1));
        fy = epsilon * (qgoal(2) - q(2));
    else
        fx = dgoal * epsilon * (qgoal(1) - q(1)) / r;
        fy = dgoal * epsilon * (qgoal(2) - q(2)) / r;
    end
    f = [fx,fy];
end

function output = differential(q,other)
    output1 = (q(1) - other(1)) / distanceCost(q,other);
    output2 = (q(2) - other(2)) / distanceCost(q,other);
    output = [output1,output2];
end

function h=distanceCost(a,b)         %% distanceCost.m
	h = sqrt(sum((a-b).^2, 2));
end

%% 这个函数求的是所有障碍物的斥力合力
function f = repulsion(q,obstacle,r0,eta,qgoal)
    f0 = [0 0];           %合力初始化
    Rq2qgoal = distanceCost(q,qgoal);
    for i = 1 : size(obstacle,1)
        r = distanceCost(q,obstacle(i,:));
        if r <= r0
            tempfvec = eta * (1/r - 1/r0) * Rq2qgoal^2/r^2 * differential(q,obstacle(i,:))...
                + eta * (1/r - 1/r0)^2 * Rq2qgoal * differential(q,qgoal);
            f0 = f0 + tempfvec;
        else
            tempfvec = [0,0];          %无斥力
            f0 = f0 + tempfvec;
        end
    end
    f = f0;
end

3D (written in Python)

The code is not public, but welcome to discuss it~

Precautions

Gravitation factor and repulsion factor are the key parameters. In addition, APF and it are easy to fall into local optimum. The introduction of normal small disturbance can solve this problem, but the trajectory is not smooth enough.