局部路径规划算法 - 人工势场法

人工势场法

参考：
（1）人工势场法
（2）人工势场法路径规划算法（APF）

1 算法概述

1.1 算法简介

1986 年 Khatib首先提出人工势场法，并将其应用在机器人避障领域，而现代汽车可以看作是一个高速行驶的机器人，所以该方法也可应用于汽车的避障路径规划领域。

1.2 算法思想

• 人工势场法的基本思想是在障碍物周围构建障碍物斥力势场，在目标点周围构建引力势场，类似于物理学中的电磁场。
• 被控对象在这两种势场组成的复合场中受到斥力作用和引力作用，斥力和引力的合力指引着被控对象的运动，搜索无碰的避障路径。
• 更直观而言，势场法是将障碍物比作是平原上具有高势能值的山峰，而目标点则是具有低势能值的低谷。
  

2 算法原理

合力和斥力的计算方法符合高中物理矢量相加法则

2.1 引力势场

引力势场主要与汽车和目标点间的距离有关, 距离越大, 汽车所受的势能值就越大；距离越小，汽车所受的势能值则越小，所以引力势场的函数为：
$${\mathrm{U}}_{\text{att }}(\mathrm{q})=\frac{1}{2}\eta {\rho }^2\left(\mathrm{q},{\mathrm{q}}_{\mathrm{g}}\right)$$
其中，$\eta$为正比例增益系数，$\rho (\mathrm{q},{\mathrm{q}}_{\mathrm{g}})$为一个矢量，表示汽车的位置 $q$ 和目标点位置 $q_g$ 之间的欧式距离$|q-q_g|$，矢量方向是从汽车的位置指向目标点位置。
相应的引力$F_{att}(q)$为引力场的负梯度，代表引力势场函数$U_{att}(q)$的变化最快方向。
$${\mathrm{F}}_{\mathrm{att}}(\mathrm{q})=-\nabla {\mathrm{U}}_{\mathrm{att}}(\mathrm{q})=-\eta \rho \left(\mathrm{q},{\mathrm{q}}_{\mathrm{g}}\right)$$

2.2 斥力势场

决定障碍物斥力势场的因素是汽车与障碍物间的距离，当汽车未进入障碍物的影响范围时，其受到的势能值就越小，距离越小，汽车受到的势能值就越大。
斥力势场的势场函数：
$$U_{req}(X)=\{\begin{array}{ll}\frac{1}{2}k{\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)}^2& 0\le \rho \left(q,q_0\right)\le {\rho }_0\\ 0& \rho \left(q,q_0\right)\ge {\rho }_0\end{array}$$
其中，$k$为正比例系数，$\rho (\mathrm{q},{\mathrm{q}}_0)$为一矢量，方向为从障碍物指向汽车，大小为汽车与障碍物间的欧式距离$|q-q_0|$，${\rho }_0$为一常数，表示障碍物对汽车产生作用的最大影响范围。

斥力势场不同于引力势场，智能汽车不总是受到障碍对它的斥力作用。当汽车与障碍物之间的相对距离超过${\rho }_0$，就判定此障碍对汽车不再有影响作用。当汽车进入障碍物的影响范围之后，即汽车与障碍的相对距离小于${\rho }_0$，汽车开始受到障碍物的斥力影响。汽车与障碍物的相对距离越小，斥力影响越大,自身势能升高。汽车与障碍物的相对距离越大，斥力影响越小，自身势能降低。

相应的斥力为斥力势场的负梯度作用力：
$$F_{\text{req }}(q)=\{\begin{array}{ll}k\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)\frac{1}{{\rho }^2\left(q,q_0\right)},& 0\le \rho \left(q,q_0\right)\le {\rho }_0\\ 0,& \rho \left(q,q_0\right)\ge {\rho }_0\end{array}$$

2.3 合力势场

机器人的合力势场大小为机器人所受的斥力势场和引力势场之和，故合力势场总函数为：
$$\mathrm{U}(\mathrm{q})={\mathrm{U}}_{\mathrm{att}}(\mathrm{q})+{\mathrm{U}}_{\mathrm{req}}(\mathrm{q})$$
所受合力：
$$\mathrm{F}(\mathrm{q})=-\nabla \mathrm{U}(\mathrm{q})={\mathrm{F}}_{\text{att }}(\mathrm{q})+{\mathrm{F}}_{\text{req }}(\mathrm{q})$$
合力的方向决定汽车的行驶朝向，合力的大小决定汽车的行驶加速度

3 求导过程

不妨设车辆位置为$(x,y)$，障碍物位置为$(x_g,y_g)$
引力势场函数：
$${\mathrm{U}}_{\mathrm{att}}(\mathrm{q})=\frac{1}{2}\eta {\rho }^2\left(\mathrm{q},{\mathrm{q}}_{\mathrm{g}}\right)\Rightarrow {\mathrm{U}}_{\mathrm{att}}(\mathrm{x},\mathrm{y})=\frac{1}{2}\eta \left[{\left(\mathrm{x}-{\mathrm{x}}_{\mathrm{g}}\right)}^2+{\left(\mathrm{y}-{\mathrm{y}}_{\mathrm{g}}\right)}^2\right]$$
故引力势场的负梯度：
$$\begin{array}{rl}-{\mathrm{gradU}}_{att}(\mathrm{x},\mathrm{y})& =-\nabla {\mathrm{U}}_{\mathrm{att}}(\mathrm{x},\mathrm{y})\\ & =-{\mathrm{U}}_{\mathrm{att},\mathrm{x}}(\mathrm{x},\mathrm{y})\overrightarrow{\mathrm{i}}-{\mathrm{U}}_{\mathrm{att},\mathrm{y}}^{\prime }(\mathrm{x},\mathrm{y})\overrightarrow{\mathrm{j}}\\ & =-\eta \left(\mathrm{x}-{\mathrm{x}}_{\mathrm{g}}\right)\overrightarrow{\mathrm{i}}-\eta \left(\mathrm{y}-{\mathrm{y}}_{\mathrm{g}}\right)\overrightarrow{\mathrm{j}}\\ & =\eta \left[\left({\mathrm{x}}_{\mathrm{g}}-\mathrm{x}\right)\overrightarrow{\mathrm{i}}+\left({\mathrm{y}}_{\mathrm{g}}-\mathrm{y}\right)\overrightarrow{\mathrm{j}}\right]\\ \Rightarrow \text{ 引力大小 }& =\eta \sqrt{{\left(\mathrm{x}-{\mathrm{x}}_{\mathrm{g}}\right)}^2+{\left({\mathrm{y}}_{\mathrm{g}}-\mathrm{y}\right)}^2}=\eta \rho \left(\mathrm{q},{\mathrm{q}}_{\mathrm{g}}\right)\end{array}$$
斥力势场函数：
$$\begin{array}{l}{U}_{req}(q)=\frac{1}{2}k{\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)}^2\Rightarrow U_{req}(x,y)=\frac{1}{2}k{\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]}^2\\ -\nabla U_{req}(x,y)=-U_{att,x}^{\prime }(x,y)\vec{i}-U_{att,y}^{\prime }(x,y)\vec{j}\end{array}$$
斥力势场的负梯度为：
$$-\nabla {\mathrm{U}}_{\text{req }}(\mathrm{x},\mathrm{y})=-{\mathrm{U}}_{\text{req },\mathrm{x}}^{\prime }(\mathrm{x},\mathrm{y})\overrightarrow{\mathrm{i}}-{\mathrm{U}}_{\mathrm{req},\mathrm{y}}^{\prime }(\mathrm{x},\mathrm{y})\overrightarrow{\mathrm{j}}$$
$$\begin{array}{l}-U_{req,x}^{\prime }(x,y)\vec{i}=-k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]{\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]}^{\prime }\\ =-k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]\left\{-\frac{1}{2}{\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{-\frac{3}{2}}\cdot \left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]\right\}\vec{i}\\ =k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]\left\{\frac{1}{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}{\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{-\frac{1}{2}}\left(x-x_0\right)\right\}\vec{i}\\ =k\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)\cdot \frac{1}{{\rho }^2\left(q,q_0\right)}\cdot \frac{1}{\rho \left(q,q_0\right)}\cdot \left(x-x_0\right)\vec{i}\end{array}$$
$$\begin{array}{l}-U_{req,y}^{\prime }(x,y)\vec{j}=-k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]{\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]}^{\prime }\\ =-k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]\left\{-\frac{1}{2}{\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{-\frac{3}{2}}\cdot \left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]\right\}\vec{j}\\ =k\left[\frac{1}{\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}}-\frac{1}{{\rho }_0}\right]\left\{\frac{1}{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2}{\left[{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2\right]}^{-\frac{1}{2}}\left(y-y_0\right)\right\}\vec{j}\\ =k\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)\cdot \frac{1}{{\rho }^2\left(q,q_0\right)}\cdot \frac{1}{\rho \left(q,q_0\right)}\cdot \left(y-y_0\right)\vec{i}\end{array}$$
化简后得斥力大小为：
$$-\nabla U_{req}(x,y)=k\left(\frac{1}{\rho \left(q,q_0\right)}-\frac{1}{{\rho }_0}\right)\cdot \frac{1}{{\rho }^2\left(q,q_0\right)}\cdot \nabla \rho \left(q,q_0\right)$$

4 算法缺陷与改进

4.1 目标不可达的问题

由于障碍物与目标点距离太近，当汽车到达目标点时，根据势场函数可知，目标点的引力降为零，而障碍物的斥力不为零，此时汽车虽到达目标点， 但在斥力场的作用下不能停下来，从而导致目标不可达的问题。

4.2 局部最优

陷入局部最优的问题。车辆在某个位置时，如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反，则合力为0，这将导致车辆不再“受力”，故无法向前搜索避障路径。

4.3 算法缺陷与改进

为了解决局部最优和目标不可达，每个障碍物的斥力分为两个：一种是沿障碍物与车辆的连线指向车辆，另一个是沿目标与车辆连线指向目标。

• 通过改进障碍物斥力势场函数来解决局部最优和目标不可达的问题
  改进后的斥力场函数:
  $${\mathrm{U}}_{\text{req }}(\mathrm{q})=\{\begin{array}{ll}\frac{1}{2}\mathrm{k}{\left(\frac{1}{\rho \left(\mathrm{q},{\mathrm{q}}_0\right)}-\frac{1}{{\rho }_0}\right)}^2{\rho }_{\mathrm{g}}^{\mathrm{n}},& 0\le \rho \left(\mathrm{q},{\mathrm{q}}_0\right)\le {\rho }_0\\ 0,& \rho \left(\mathrm{q},{\mathrm{q}}_0\right)>{\rho }_0\end{array}$$
  ${\rho }_g^n$为汽车与目标点的距离，式中n为可选的正常数
  $$\{\begin{array}{l}{\mathrm{F}}_{\mathrm{req}}={\mathrm{F}}_{\mathrm{req}1}+{\mathrm{F}}_{\mathrm{req}2}\\ {\mathrm{F}}_{\text{req1 }}=\mathrm{k}\left(\frac{1}{\rho \left(\mathrm{q},{\mathrm{q}}_0\right)}-\frac{1}{{\rho }_0}\right)\frac{{\rho }_{\mathrm{g}}^{\mathrm{n}}}{{\rho }^2\left(\mathrm{q},{\mathrm{q}}_0\right)}\\ {\mathrm{F}}_{\text{req2 }}=\frac{\mathrm{n}}{2}\mathrm{k}{\left(\frac{1}{\rho \left(\mathrm{q},{\mathrm{q}}_0\right)}-\frac{1}{{\rho }_0}\right)}^2{\rho }_{\mathrm{g}}^{\mathrm{n}-1}\end{array}$$
  

改进的斥力场函数中加入了汽车与目标点间的距离，这样使汽车在驶向目标的过程中，受到的引力和斥力同时在一定程度上减小，且只有在汽车到达目标点时，引力和斥力才同时减小为零，即目标点成为势能值的最小点，从而使局部最优和目标不可达的问题得到解决

• 通过建立道路边界斥力势场以限制汽车的行驶区域，并适当考虑车辆速度对斥力场的影响

$$F_{\text{rep,edge }}=\{\begin{array}{ll}{\eta }_{\text{edge }}\cdot v\cdot e^{\left(\frac{-d}{2}-y\right)},& -d+w/2<y<-d/2\\ \frac{1}{3}{\eta }_{\text{edge }}\cdot y^2,& -d/2<y<-w/2\\ -\frac{1}{3}{\eta }_{\text{edge }}\cdot y^2,& w/2<y<d/2\\ {\eta }_{\text{edge }}\cdot v\cdot e^{\left(y-\frac{d}{2}\right)},& d/2<y<d-w/2\end{array}$$
式${\eta }_{\text{edge }}$是常数，v为车辆速度，y为车辆横坐标。

5 代码实现

MATLAB(Ally)

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');%路径点

运行结果：

Python：

import numpy as np
import matplotlib.pyplot as plt

# 初始化车的参数
d = 3.5  # 道路标准宽度
W = 1.8  # 汽车宽度
P0 = np.array([0, -d / 2, 1, 1])  # 车辆起点位置，分别代表x,y,vx,vy
Pg = np.array([99, d / 2, 0, 0])  # 目标点位置
Pobs = np.array([[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 = np.array([[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],
              [99, d / 2, 0, 0]])

Eta_att = 5  # 计算引力的增益系数
Eta_rep_ob = 15  # 计算斥力的增益系数
Eta_rep_edge = 50  # 计算边界斥力的增益系数

d0 = 20  # 障碍影响距离
n = len(P)  # 障碍物和目标总计个数
len_step = 0.5  # 步长
Num_iter = 200  # 最大循环迭代次数

Pi = P0
Path = []  # 存储路径
delta = np.zeros((n, 2))  # 存储每一个障碍到车辆的斥力方向向量以及引力方向向量
dist = []  # 存储每一个障碍到车辆的距离以及目标点到车辆距离
unitvector = np.zeros((n, 2))  # 存储每一个障碍到车辆的斥力单位向量以及引力单位向量
F_rep_ob = np.zeros((n - 1, 2))  # 存储每一个障碍到车辆的斥力
F_rep_edge = np.zeros((1, 2))
while ((Pi[0] - Pg[0]) ** 2 + (Pi[1] - Pg[1]) ** 2) ** 0.5 > 1:
    # a = ((Pi[0] - Pg[0]) ** 2 + (Pi[1] - Pg[1]) ** 2) ** 0.5
    Path.append(Pi.tolist())
    # 计算车辆当前位置和障碍物的单位方向向量
    for j in range(len(Pobs)):
        delta[j, :] = Pi[0:2] - P[j, 0:2]
        dist.append(np.linalg.norm(delta[j, :]))
        unitvector[j, :] = [delta[j, 0] / dist[j], delta[j, 1] / dist[j]]
    # 计算车辆当前位置和目标点的单位方向向量
    delta[n - 1, :] = P[n - 1, 0:2] - Pi[0:2]
    dist.append(np.linalg.norm(delta[n - 1, :]))
    unitvector[n - 1, :] = [delta[n - 1, 0] / dist[n - 1], delta[n - 1, 1] / dist[n - 1]]

    # 计算斥力
    for j in range(len(Pobs)):
        if dist[j] >= d0:
            F_rep_ob[j, :] = [0, 0]
        else:
            # 障碍物的斥力1,方向由障碍物指向车辆
            F_rep_ob1_abs = Eta_rep_ob * (1 / dist[j] - 1 / d0) * dist[n - 1] / dist[j] ** 2  # 回看公式设定n=1
            F_rep_ob1 = np.array([F_rep_ob1_abs * unitvector[j, 0], F_rep_ob1_abs * unitvector[j, 1]])
            # 障碍物的斥力2，方向由车辆指向目标点
            F_rep_ob2_abs = 0.5 * Eta_rep_ob * (1 / dist[j] - 1 / d0) ** 2
            F_rep_ob2 = np.array([F_rep_ob2_abs * unitvector[n - 1, 0], F_rep_ob2_abs * unitvector[n - 1, 1]])
            # 改进后的障碍物和斥力计算
            F_rep_ob[j, :] = F_rep_ob1 + F_rep_ob2
    # 增加的边界斥力
    if -d + W / 2 < Pi[1] <= -d / 2:
        # 这个边界斥力只作用在y方向，因此x方向为0
        F_rep_edge = [0, Eta_rep_edge * np.linalg.norm(Pi[2:4]) * np.exp(-d / 2 - Pi[1])]
    elif -d / 2 < Pi[1] <= -W / 2:
        F_rep_edge = [0, 1 / 3 * Eta_rep_edge * Pi[1] ** 2]
    elif W / 2 < Pi[1] <= d / 2:
        F_rep_edge = [0, -1 / 3 * Eta_rep_edge * Pi[1] ** 2]
    elif d / 2 < Pi[1] <= d - W / 2:
        F_rep_edge = [0, Eta_rep_edge * np.linalg.norm(Pi[2:4]) * np.exp(Pi[1] - d / 2)]

    # 计算合力和方向
    F_rep = [np.sum(F_rep_ob[:, 0]) + F_rep_edge[0], np.sum(F_rep_ob[:, 1]) + F_rep_edge[1]]  # 合并边界斥力和障碍舞斥力
    F_att = [Eta_att * dist[n - 1] * unitvector[n - 1, 0], Eta_att * dist[n - 1] * unitvector[n - 1, 1]]  # 引力向量
    F_sum = np.array([F_rep[0] + F_att[0], F_rep[1] + F_att[1]])
    UnitVec_Fsum = 1 / np.linalg.norm(F_sum) * F_sum
    # 计算车的下一步位置
    Pi[0:2] = Pi[0:2] + len_step * UnitVec_Fsum
# 将目标添加到路径中
Path.append(Pg.tolist())

# 画图
x = []  # 路径的x坐标
y = []  # 路径的y坐标
for val in Path:
    x.append(val[0])
    y.append(val[1])

plt.plot(x, y, linewidth=0.6)
plt.axhline(y=0, color='g', linestyle=':',linewidth=0.3)
plt.axis([-5,100,-4,4])
plt.gca().set_aspect('equal')
plt.plot(0, -d / 2, 'ro', markersize=1)
plt.plot(15, 7 / 4, 'ro', markersize=1)
plt.plot(30, -3 / 2, 'ro', markersize=1)
plt.plot(45, 3 / 2, 'ro', markersize=1)
plt.plot(60, -3 / 4, 'ro', markersize=1)
plt.plot(80, 7 / 4, 'ro', markersize=1)
plt.plot(99, d / 2, 'ro', markersize=1)
plt.xlabel('x')
plt.ylabel('y')
plt.show()

运行结果：
C++：
APF.h

#pragma once

#include <iostream>
#include <Eigen/Dense>
#include<vector>
#include<cmath>
#include<algorithm>
using namespace std;
using namespace Eigen;

class APF {
private:
    double Eta_att, Eta_rep_ob, Eta_rep_edge, d_max, n;//引力的增益系数,斥力的增益系数,道路边界斥力的增益系数,障碍影响的最大距离, n系数
    Vector2d target_pos;
    vector<Vector2d>obstacle_pos;
    double d, w; //道路标准宽度，汽车宽度
    double len_step; //步长
public:
    APF(double etaAtt, double etaRepOb, double etaRepEdge, double dmax, double n);
    void setTargetPos(const Vector2d &targetPos);
    void setObstaclePos(const vector<Vector2d> &obstaclePos);
    void setEtaAtt(double etaAtt);
    void setEtaRepOb(double etaRepOb);
    void setEtaRepEdge(double etaRepEdge);
    void setDMax(double dMax);
    void setN(double n);
    void setD(double d);
    void setW(double w);
    void setLenStep(double lenStep);
    Vector2d computeForce(VectorXd robot_state);
    VectorXd runAPF(VectorXd robot_state);
};

APF.cpp

#include "../include/APF.h"

APF::APF(double etaAtt, double etaRepOb, double etaRepEdge, double dmax, double n) : Eta_att(etaAtt),
                                                                                     Eta_rep_ob(etaRepOb),
                                                                                     Eta_rep_edge(etaRepEdge), d_max(dmax),
                                                                                     n(n) {}
void APF::setTargetPos(const Vector2d &targetPos) {
    target_pos = targetPos;
}


void APF::setObstaclePos(const vector<Vector2d> &obstaclePos) {
    obstacle_pos = obstaclePos;
}

void APF::setEtaAtt(double etaAtt) {
    Eta_att = etaAtt;
}

void APF::setEtaRepOb(double etaRepOb) {
    Eta_rep_ob = etaRepOb;
}

void APF::setEtaRepEdge(double etaRepEdge) {
    Eta_rep_edge = etaRepEdge;
}

void APF::setN(double n) {
    this->n = n;
}

void APF::setD(double d) {
    this->d = d;
}

void APF::setW(double w) {
    this->w = w;
}

void APF::setDMax(double dMax) {
    d_max = dMax;
}

void APF::setLenStep(double lenStep) {
    len_step = lenStep;
}

// 计算斥力引力
Vector2d APF::computeForce(VectorXd robot_state) {
    // 引力势场计算
    Vector2d delta_att = target_pos - robot_state.head(2);
    double dist_att = delta_att.norm();
    Vector2d unite_att_vec = delta_att / dist_att;
    Vector2d F_att = Eta_att * delta_att;

    // 合力
    Vector2d F = F_att;
    // 障碍物斥力势场
    // 在斥力势场函数增加目标调节因子（即车辆至目标距离），以使车辆到达目标点后斥力也为0
    for(Vector2d obs:obstacle_pos){
        Vector2d delta = robot_state.head(2) - obs;
        double dist = delta.norm();
        Vector2d  unite_rep_vec = delta / dist;
        Vector2d F_rep_ob;
        if (dist >= d_max){
            F_rep_ob = Vector2d (0,0);
        } else{
            // 障碍物的斥力1，方向由障碍物指向车辆
            // 斥力1
            double  F_rep1_norm = Eta_rep_ob * (1 / dist - 1 / d_max) * pow(dist_att, n) / pow(dist, 2);
            Vector2d F_rep_ob1 = F_rep1_norm * unite_rep_vec;
            // 斥力2
            double F_rep2_norm = n / 2 * Eta_rep_ob * pow(1 / dist - 1 / d_max, 2) * pow(dist_att, n-1);
            Vector2d F_rep_ob2 = F_rep2_norm * unite_att_vec;
            F_rep_ob = F_rep_ob1 + F_rep_ob2;
        }
        F += F_rep_ob;
    }
    //道路边界斥力势场
    Vector2d F_rep_edge;
    double v = robot_state(2);//车辆速度
    if (robot_state(1) > -d + w / 2 && robot_state(1) <= -d / 2) {
        F_rep_edge = Vector2d (0, Eta_rep_edge * v * exp(-d / 2 - robot_state(1)));
    }else if (robot_state(1) > -d / 2 && robot_state(1) <= -w / 2) {
        F_rep_edge = Vector2d (0, 1 / 3 * Eta_rep_edge * pow(robot_state(1), 2));
    }else if (robot_state(1) > w / 2 && robot_state(1) <= d / 2) {
        F_rep_edge = Vector2d (0, -1 / 3 * Eta_rep_edge * pow(robot_state(1), 2));
    }else if (robot_state(1) > d / 2 && robot_state(1) <= d - w / 2) {
        F_rep_edge = Vector2d (0, Eta_rep_edge * v * exp(robot_state(1) - d / 2 ));
    }
    F += F_rep_edge;

    Vector2d unit_F = F / F.norm();
    return unit_F;
}
// 车辆下一步位置
VectorXd APF::runAPF(VectorXd robot_state) {
    Vector2d unit_F = computeForce(robot_state);
    Vector2d next_pos = robot_state.head(2) + len_step * unit_F;

    robot_state << next_pos(0),next_pos(1),robot_state(2);
    return robot_state;
}

main.cpp

#include "../include/APF.h"
#include "../include/matplotlibcpp.h"
namespace plt = matplotlibcpp;

int main(){
   //初始化车的参数
    double d = 3.5;  // 道路标准宽度

    double w = 1.8;  //  汽车宽度

    double L = 4.7;  // 车长

    double Eta_att = 2;  // 引力的增益系数

    double Eta_rep_ob = 1;  // 斥力的增益系数

    double Eta_rep_edge = 10;   // 道路边界斥力的增益系数

    double d_max = 5;  // 障碍影响的最大距离


    double len_step = 0.5; // 步长

    double  n=1;

    int Num_iter = 300;  // 最大循环迭代次数

    Vector2d target(99,0);
    vector<Vector2d>obstacle_pos={Vector2d (15, 7 / 4),Vector2d (30, - 3 / 2),Vector2d (45, 3 / 2),Vector2d (60, - 3 / 4),Vector2d (80, 3/2)};

    APF apf(Eta_att,Eta_rep_ob,Eta_rep_edge,d_max,n);
    apf.setD(d);
    apf.setW(w);
    apf.setLenStep(len_step);
    apf.setTargetPos(target);
    apf.setObstaclePos(obstacle_pos);
    //robot_state: 为x,y,v
    VectorXd robot_state(3),init_pos(2);
    robot_state<<0,0,2;
    init_pos<<0,0;
    vector<double>x_,y_;

    //画图
    double len_line = 100;
    vector<double>greyZone_x{-5,-5,len_line,len_line};
    vector<double>greyZone_y{- d - 0.5,d + 0.5,d + 0.5,- d - 0.5};

    for(int i=0;i<Num_iter;i++){
        plt::clf();
        robot_state = apf.runAPF(robot_state);
        x_.push_back(robot_state(0));
        y_.push_back(robot_state(1));

        //画分界线
        map<string, string> keywords;
        keywords["color"] = "grey";
        plt::fill(greyZone_x,greyZone_y,keywords);
        plt::plot({-5,len_line},{0,0},"w--");
        plt::plot({-5,len_line},{d,d},"w--");
        plt::plot({-5,len_line},{-d,-d},"w--");
        for(Vector2d obs:obstacle_pos){
            plt::plot(vector<double>{obs(0)},vector<double>{obs(1)},"ro");//障碍物位置
        }
        plt::plot(vector<double>{target(0)},vector<double>{target(1)}, "gv");//目标位置
        plt::plot(vector<double>{init_pos(0)},vector<double>{init_pos(1)}, "bs");//起点位置

        //画轨迹
        plt::plot(x_, y_,"r");

        plt::grid(true);
        plt::ylim(-10,10);
        plt::pause(0.01);
        if ((robot_state.head(2) - target).norm() < 1) { break; }

    }
    // save figure
    const char* filename = "./apf_demo.png";
    cout << "Saving result to " << filename << std::endl;
    plt::save(filename);
    plt::show();
    return 0;
}

运行结果：

6 人工势场法总结

优点：

• 简单实用，良好的实时性
• 结构简单，便于底层的实时控制，在实时避障和平滑的轨迹控制方面得到广泛的应用

缺点：

• 存在陷阱区域
• 在相近的障碍物群中不能识别路径
• 在障碍物前震荡
• 在狭窄通道中摆动
• 障碍物附近目标不可达

引力势场的范围比较大，而斥力的作用范围只是局部的，当机器人和障碍物的距离超过障碍物影响范围的时候，机器人不受排斥势场的影响。因此，势场法只能解决局部空间的避障问题，它缺乏全局信息，这样，它就很容易陷入局部最小值。所谓局部最小值点，就是在引力势场函数和斥力势场函数的联合分布的空间内，在某些区域，受到多个函数的作用，造成了局部最小点。当机器人位于局部最小点的时候，机器人容易产生振荡或者停滞不前。障碍物越多，产生局部最小点的可能性就越大，产生局部最小点的数量也就越多。