一、算法简介

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/24clcclearclose 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;               %将车的肇危坐标赋给Xii = 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%     endendPath(i，:)=P(n，:);            %把旅途向量的终末一个点赋值为主意%% 绘画figurelen_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 onfill([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 equalset(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西野翔快播

本站仅提供存储作事，统共试验均由用户发布，如发现存害或侵权试验，请点击举报。