一、算法简介
1986年Khatib首先提出人工势场法,并将其应用在机器人避障领域,而现代汽车可以看作是一个高速行驶的机器人,所以该方法也可应用于汽车的避障路径规划领域。
二、算法思想
1、人工势场法的基本思想是在障碍物周围构建障碍物斥力势场,在目标点周围构建引力势场,类似于物理学中的电磁场。
2、被控对象在这两种势场组成的复合场中受到斥力作用和引力作用,斥力和引力的合力指引着被控对象的运动,搜索无碰的避障路径。
3、更直观而言,势场法是将障碍物比作是平原上具有高势能值的山峰,而目标点则是具有低势能值的低谷。
三、算法内容
引力势场主要与汽车和目标点间的距离有关,距离越大,汽车所受的势能值就越大;距离越小,汽车所受的势能值则越小,所以引力势场的函数为:
其中n为正比例增益系数,p(q,qg)为一个矢量,表示汽车的位置q和目标点位置qg之间的欧几里德距离|q-qg|,矢量方向是从汽车的位置指向目标点位置。
响应的引力Fatt(X)为引力场的负梯度:
决定障碍物斥力势场的因素是汽车与障碍物间的距离,当汽车未进入障碍物的影响范围时,其受到的势能值为零;在汽车进入障碍物的影响范围后,两者之间的距离越大,汽车受到的势能值就越小,距离越小,汽车受到的势能值就越大。斥力势场的势场函数为:
其中k为正比例系数,p(q,q。)为一矢量,方向为从障碍物指向汽车,大小为汽车与障碍物间的距离|q-q。|,p。为一常数,表示障碍物对汽车产生作用的最大距离。
响应的斥力为斥力场的负梯度
设车辆位置为(×,y),障碍物位置为(xg, yg)。则引力势场函数为:
故有:
斥力势场函数为:
目标不可达的问题。由于障碍物与目标点距离太近,当汽车到达目标点时,根据势场函数可知,目标点的引力降为零,而障碍物的斥力不为零,此时汽车虽到达目标点,但在斥力场的作用下不能停下来,从而导致目标不可达的问题。
陷入局部最优的问题。车辆在某个位置时,如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反,则合力为0,这将导致车辆不再“受力”,故无法向前搜索避障路径。
四、算法缺陷与改进
1、通过改进障碍物斥力势场函数来解决局部最优和目标不可达的问题;
2、通过建立道路边界斥力势场以限制汽车的行驶区域,并适当考虑车辆速度对斥力场的影响
% 人工势场法
% 作者:Ally
% 日期:2021/1/24
clc
clear
close all
%% 初始化车的参数
d = 3.5; % 道路标准宽度
W = 1.8; % 汽车宽度
L = 4.7; % 车长
P0 = [0,-d/2,1,1]; % 车辆起点信息,1-2列位置,3-4列速度
Pg = [99,d/2,0,0]; % 目标位置
Pobs = [15,7/4,0,0;
30,-3/2,0,0;
45,3/2,0,0;
60,-3/4,0,0;
80,7/4,0,0]; % 障碍物位置
P = [Pobs;Pg]; % 将目标位置和障碍物位置合放在一起
Eta_att = 5; % 计算引力的增益系数
Eta_rep_ob = 15; % 计算斥力的增益系数
Eta_rep_edge = 50; % 计算边界斥力的增益系数
d0 = 20; % 障碍影响距离
n = size(P,1); % 障碍与目标总计个数
len_step = 0.5; % 步长
Num_iter = 200; % 最大循环迭代次数
%% ***************初始化结束,开始主体循环******************
Pi = P0; %将车的起始坐标赋给Xi
i = 0;
while sqrt((Pi(1)-P(n,1))^2+(Pi(2)-P(n,2))^2) > 1
i = i + 1;
Path(i,:) = Pi; % 保存车走过的每个点的坐标
%计算车辆当前位置与障碍物的单位方向向量、速度向量
for j = 1:n-1
delta(j,:) = Pi(1,1:2) - P(j,1:2); % 用车辆点-障碍点表达斥力
dist(j,1) = norm(delta(j,:)); % 车辆当前位置与障碍物的距离
unitVector(j,:) = [delta(j,1)/dist(j,1), delta(j,2)/dist(j,1)]; % 斥力的单位方向向量
end
%计算车辆当前位置与目标的单位方向向量、速度向量
delta(n,:) = P(n,1:2)-Pi(1,1:2); %用目标点-车辆点表达引力
dist(n,1) = norm(delta(n,:));
unitVector(n,:)=[delta(n,1)/dist(n,1),delta(n,2)/dist(n,1)];
%% 计算斥力
% 在原斥力势场函数增加目标调节因子(即车辆至目标距离),以使车辆到达目标点后斥力也为0
for j = 1:n-1
if dist(j,1) >= d0
F_rep_ob(j,:) = [0,0];
else
% 障碍物的斥力1,方向由障碍物指向车辆
F_rep_ob1_abs = Eta_rep_ob * (1/dist(j,1)-1/d0) * dist(n,1) / dist(j,1)^2;
F_rep_ob1 = [F_rep_ob1_abs*unitVector(j,1), F_rep_ob1_abs*unitVector(j,2)];
% 障碍物的斥力2,方向由车辆指向目标点
F_rep_ob2_abs = 0.5 * Eta_rep_ob * (1/dist(j,1) - 1/d0)^2;
F_rep_ob2 = [F_rep_ob2_abs * unitVector(n,1), F_rep_ob2_abs * unitVector(n,2)];
% 改进后的障碍物合斥力计算
F_rep_ob(j,:) = F_rep_ob1+F_rep_ob2;
end
end
% 增加边界斥力势场,根据车辆当前位置,选择对应的斥力函数
if Pi(1,2) > -d+W/2 && Pi(1,2) <= -d/2 %下道路边界区域力场,方向指向y轴正向
F_rep_edge = [0,Eta_rep_edge * norm(Pi(:,3:4))*(exp(-d/2-Pi(1,2)))];
elseif Pi(1,2) > -d/2 && Pi(1,2) <= -W/2 %下道路分界线区域力场,方向指向y轴负向
F_rep_edge = [0,1/3 * Eta_rep_edge * Pi(1,2).^2];
elseif Pi(1,2) > W/2 && Pi(1,2) < d/2 %上道路分界线区域力场,方向指向y轴正向
F_rep_edge = [0, -1/3 * Eta_rep_edge * Pi(1,2).^2];
elseif Pi(1,2) > d/2 && Pi(1,2)<=d-W/2 %上道路边界区域力场,方向指向y轴负向
F_rep_edge = [0, Eta_rep_edge * norm(Pi(:,3:4)) * (exp(Pi(1,2)-d/2))];
end
%% 计算合力和方向
F_rep = [sum(F_rep_ob(:,1)) + F_rep_edge(1,1),...
sum(F_rep_ob(:,2)) + F_rep_edge(1,2)]; % 所有障碍物的合斥力矢量
F_att = [Eta_att*dist(n,1)*unitVector(n,1), Eta_att*dist(n,1)*unitVector(n,2)]; % 引力矢量
F_sum = [F_rep(1,1)+F_att(1,1),F_rep(1,2)+F_att(1,2)]; % 总合力矢量
UnitVec_Fsum(i,:) = 1/norm(F_sum) * F_sum; % 总合力的单位向量
%计算车的下一步位置
Pi(1,1:2)=Pi(1,1:2)+len_step*UnitVec_Fsum(i,:);
% %判断是否到达终点
% if sqrt((Pi(1)-P(n,1))^2+(Pi(2)-P(n,2))^2) < 0.2
% break
% end
end
Path(i,:)=P(n,:); %把路径向量的最后一个点赋值为目标
%% 画图
figure
len_line = 100;
% 画灰色路面图
GreyZone = [-5,-d-0.5; -5,d+0.5; len_line,d+0.5; len_line,-d-0.5];
fill(GreyZone(:,1),GreyZone(:,2),[0.5 0.5 0.5]);
hold on
fill([P0(1),P0(1),P0(1)-L,P0(1)-L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],'b') %2号车
% 画分界线
plot([-5, len_line],[0, 0], 'w--', 'linewidth',2); %分界线
plot([-5,len_line],[d,d],'w','linewidth',2); %左边界线
plot([-5,len_line],[-d,-d],'w','linewidth',2); %左边界线
% 设置坐标轴显示范围
axis equal
set(gca, 'XLim',[-5 len_line]);
set(gca, 'YLim',[-4 4]);
% 绘制路径
plot(P(1:n-1,1),P(1:n-1,2),'ro'); %障碍物位置
plot(P(n,1),P(n,2),'gv'); %目标位置
plot(P0(1,1),P0(1,2),'bs'); %起点位置
plot(Path(:,1),Path(:,2),'.b');%路径点
学习自B站:小黎的Ally