目录
传统人工势场
引力势场
斥力势场
合力势场
传统人工势场法存在的问题
改进的人工势场函数
Matlab代码实现
参考链接:
[1]朱伟达. 基于改进型人工势场法的车辆避障路径规划研究[D]. 江苏大学, 2017.
1986年Khatib首先提出人工势场法,并将其应用在机器人避障领域。该方法的基本思想是在障碍物周围构建障碍物斥力势场,在目标点周围构建引力势场,类似于物理学中的电磁场。被控对象在这两种势场组成的复合场中受到斥力作用和引力作用,斥力和引力的合力指引着被控对象的运动,搜索无碰的避障路径。
这种方法结构简单,便于低层的实时控制,在实时避障和平滑的轨迹控制方面,得到了广泛应用,其不足在于存在局部最优解,目标不可达、与障碍物碰撞等现象。
决定障碍物斥力势场的因素是汽车与障碍物间的距离,当汽车未进入障碍物的影响范围时,其受到的势能值为零;在汽车进入障碍物的影响范围后,两者之间的距离越大,汽车受到的势能值就越小,距离越小,汽车受到的势能值就越大。
main主函数
%main主函数
%created by MW
%date: 2023/2/28
%func: Artificial Potential Field(APF) avoiding obstacle
clc;
clear;
%% 创建并绘制障碍物
obstacle = [2 5;
3 8;
7 9;
4 2;
6 6;
9 3];
figure(1);
scatter(obstacle(:,1),obstacle(:,2),'r')
axis equal;
%% 创建并绘制初始位置和目标位置
start = [0 0];
goal = [10 10];
hold on;
scatter(start(1),start(2),'filled','g');
scatter(goal(1),goal(2),'filled','g');
%% 图片标注
text(start(1),start(2),'起点');
text(goal(1),goal(2),'终点');
grid on;
axis equal;
axis([0 10 0 10]);
xlabel('x');
ylabel('y');
title('人工势场法生成避障路径')
%% 生成并绘制避障路径
path = APF2D(start, goal, obstacle);
hold on;
plot(path(:,1), path(:,2),'LineWidth',1,'Color','b');
人工势场法主函数:
function path = APF2D(start,goal,obstacle)
% Artificial Potential Field(APF)avoiding obstacle path
%%APF参数初始化
%如果不能实现预期目标,可能也与初始的增益系数,Po设置的不合适有关。
att = 35;%引力增益系数
req = 10;%斥力增益系数
p0 = 5;%障碍物产生影响的最大距离,当障碍与移动目标之间距离大于Po时,斥力为0。
step = 2;%步长
maxIter = 200;%最大循环迭代次数
n = length(obstacle(:,1));%障碍物个数
path = start;%路径初始化
newNode = start;
for i = 1:maxIter
%% 引力计算
V_att = goal - newNode;%路径点到目标点的向量
r_att = sqrt(V_att(1)^2 + V_att(2)^2);%路径点到目标点的欧氏距离
P_att = att * V_att;%引力
%% 斥力计算
%改进的人工势场法,将斥力分散一部分到引力方向。通过添加随机扰动r_att^n实现,r_att为路径点到目标点的欧氏距离,本文n取2。
V_req = zeros(n,2);
for j =1:n
V_req(j,:) = [obstacle(j,1) - newNode(1), obstacle(j,2) - newNode(2)];%路径点到各个障碍物的向量
r_req(j) = sqrt(V_req(j,1)* V_req(j,1) + V_req(j,2)* V_req(j,2));%路径点到各个障碍物的欧氏距离
end
P_req = 0;
for k = 1:n
if r_req(k) <= p0
P_req1 = req * (1 / r_req(k) - 1 / p0) * r_att^2 / r_req(k)^2;%斥力分量1:障碍物指向路径点的斥力
P_req2 = req * (1 / r_req(k) - 1 / p0)^2 * r_att;%斥力分量2:路径点指向目标点的分引力
P_reqk = P_req1 / r_req(k) * V_req(k,:) + P_req2 / r_att * V_att;%合力分散到x,y方向
P_req = P_req + P_reqk;%斥力
end
end
%% 合力计算
P = P_att + P_req;
newNode = newNode + step * P / norm(P);
path = [path; newNode];
end
end
结果:
需要注意的是,引力、斥力增益、步长的选择都会影响到结果,可以进行适当调整。
特别地,人工势场法容易陷入局部最小值,即使是加入了调节因子进行改进,在某些情况下仍然无法避免。