一、算法简介

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) > 1i = 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)];%% 计算斥力 % 在原斥力势场函数增加目标调节因子（即车辆至目标距离），以使车辆到达目标点后斥力也为0for j = 1:n-1if dist(j,1) >= d0F_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;                                   endend% 增加边界斥力势场，根据车辆当前位置，选择对应的斥力函数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

视频链接：路径规划与轨迹跟踪系列算法学习_第6讲_人工势场法_哔哩哔哩_bilibili

学习笔记：人工势场法相关推荐

路劲规划与轨迹跟踪学习4——人工势场法
本文参考(85条消息) [路径规划]局部路径规划算法--人工势场法(含python实现 | c++实现)_CHH3213的博客-CSDN博客_人工势场法路径规划路径规划与轨迹跟踪系列算法学习_第6讲 ...
matlab人工势场法三维演示图,人工势场法（Artificial Potential Field Method）的学习
最近的工作重心回到到算法上之后,陆陆续续开始学习一些自动驾驶的控制算法.目前工作的方向主要是online trajectory generation和底层控制. 对于在线路径规划,一个重要的概念是其应 ...
matlab人工势场法三维演示图,运动规划入门 | 5. 白话人工势场法，从原理到Matlab实现...
如何利用人工势场进行运动规划? 1.1 引力势场(Attractive Potential Field) 人工势场这个特殊的势场并不是一个单一的场,其实它是由两个场叠加组合而成的,一个是引力场,一个是 ...
人工势场法路径规划算法（APF）
本文主要对人工势场法路径规划算法进行介绍,主要涉及人工势场法的简介.引力和斥力模型及其推导过程.人工势场法的缺陷及改进思路.人工势场法的Python与MATLAB开源源码等方面一.人工势 ...
基于人工势场法的二维平面内无人机的路径规划的matlab仿真,并通过对势场法改进避免了无人机陷入极值的问题
目录 1.算法描述 2.matlab算法仿真效果 3.MATLAB核心程序 4.完整MATLAB 1.算法描述人工势场法原理是:首先构建一个人工虚拟势场,该势场由两部分组成,一部分是目标点对移动机器 ...
多机器人编队人工势场法协同避障算法原理及实现
多机器人编队(二)多机器人编队人工势场法协同避障算法原理及实现避障算法原理避障算法仿真多机器人协同编队需要将理论和实践紧密地结合起来,其应用包括编队队形生成.保持.变换和路径规划与避障等等都是基 ...
【全局规划】人工势场法（APF）
人工势场法APF clc clear close all%% 初始化车的参数 d = 3.5; % 道路标准宽度 W = 1.8; % 汽车宽度 L = 4.7; % 车长P0 = [0,-d/2,1 ...
基于人工势场法的路径规划
基于人工势场法的路径规划 \qquad 路径规划是移动机器人领域的一个重要组成部分,人工势场法是机器人路径规划算法中一种简单有效的方法. \qquad 势场法的基本思想是在移动机器人的工作环境中构造一 ...
机器人路径规划_人工势场法
机器人路径规划_人工势场法原理人工势场法是由Khatib提出的一种虚拟力法.原理是:将机器人在环境中的运动视为一种机器人在虚拟的人工受力场的运动.障碍物对机器人产生斥力,目标点对机器人产生引力,引 ...
路径规划算法3 改进的人工势场法（Matlab）
目录传统人工势场引力势场斥力势场合力势场传统人工势场法存在的问题改进的人工势场函数 Matlab代码实现参考链接: [1]朱伟达. 基于改进型人工势场法的车辆避障路径规划研究[D]. 江 ...

学习笔记：人工势场法

一、算法简介

二、算法思想

三、算法内容

四、算法缺陷与改进

学习笔记：人工势场法相关推荐

最新文章

热门文章