APF人工势场轨迹规划

实现原博客网址

APF
实现版本为APF的改进版，见上文文末（引力和斥力均进行改进）。

实现效果

二维(Matlab写的，比较原始)

最开始学这个的时候参考过网上其他人写的代码，确实质量不太好，命名等不规范，可读性差，因此本着开源快乐的精神我把matlab版本二维改进apf code附上：

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

三维（Python写的）

代码不公开了，不过欢迎讨论呀~

注意事项

引力因子和斥力因子是关键参数，另外APF及其容易陷入局部最优，引入正态微小扰动可以解决这个问题，但是会出现轨迹不够平滑的现象。