APF人工势场轨迹规划
发布时间
阅读量：
阅读量
该文本介绍了实现改进的APF算法，包括二维Matlab版本和三维Python版本的实现。二维版本中，引力和斥力均进行了改进，参数设置如引力尺度因子ε、斥力尺度因子η、引力分界值dgoal等均被详细说明。代码中使用了引力函数、斥力函数和路径追踪函数，并通过散点图展示了障碍物、目标点和路径。三维版本的代码未公开，但欢迎讨论。需要注意的是，引力和斥力因子是关键参数，APF算法容易陷入局部最优，可引入正态微小扰动解决，但可能导致轨迹不够平滑。
实现原博客网址
基于APF的改进版本已在上文末尾部分详细说明，其中，引力和斥力的改进工作均已完成。
实现效果
二维(Matlab写的，比较原始)
在学习这个项目初期，我曾查阅过网上的其他人的代码实现，但整体质量有待提升，命名不够规范，导致代码整体可读性较差。基于开源分享的态度，我把matlab版本的二维改进APF代码附上：
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写的）
代码不公开了，不过欢迎讨论呀~
注意事项
引力因子与斥力因子被视为关键参数，此外，其易陷入局部最优状态。通过引入正态微小扰动，可以有效缓解这一问题，然而，这可能导致轨迹不够平滑的现象出现。
全部评论 (0)
还没有任何评论哟~