目录

人工势场法-维基百科

路径规划-人工势场法（Artifical Potential Field）

引力场 (attractive/gravitation field)

斥力场 (repulsive field)

总场

【机器人路径规划】人工势场法

Paper

Matlab 代码

自己编写的 Matlab

1. 仅考虑引力的情况

----

人工势场法-维基百科

人工势场法是由Khatib提出的一种机器人路径规划算法。该算法将目标和障碍物分别看做对机器人有引力和斥力的物体，机器人沿引力与斥力的合力来进行运动。

该法结构简单，便于低层的实时控制，在实时避障和平滑的轨迹控制方面，得到了广泛应用，其不足在于存在局部最优解，容易产生死锁现象，因而可能使移动机器人在到达目标点之前就停留在局部最优点。

From: 人工势场法-维基百科

----

路径规划-人工势场法（Artifical Potential Field）

引力场 (attractive/gravitation field)

常用的引力函数：
U a t t ( q ) = 1 2 ξ ρ 2 ( q , q g o a l ) U_{att}(q) = \frac{1}{2}\xi\rho^2(q,q_{goal}) Uatt​(q)=21​ξρ2(q,qgoal​)

这里的 ξ \xi ξ 是尺度因子， ρ ( q , q g o a l ) \rho(q,q_{goal}) ρ(q,qgoal​) 表示物体当前状态与目标的距离。引力场有了，那么引力就是引力场对距离的导数（类比物理里面 W = F X W=FX W=FX）：
F a t t ( q ) = − ∇ U a t t ( q ) = ξ ( q g o a l − q ) F_{att}(q) = -\nabla U_{att}(q) = \xi(q_{goal}-q) Fatt​(q)=−∇Uatt​(q)=ξ(qgoal​−q)

----

斥力场 (repulsive field)

传统的斥力场公式
U r e p ( q ) = { 1 2 η ( 1 ρ ( q , q o b s ) − 1 ρ 0 ) 2 , if ρ ( q , q o b s ) ≤ ρ 0 0 , if ρ ( q , q o b s ) > ρ 0 U_{rep}(q) = \left\{\begin{aligned} \frac{1}{2} \eta (\frac{1}{\rho(q, q_{obs})} – \frac{1}{\rho_0})^2, \quad \text{if}\ \rho(q, q_{obs}) \le \rho_0 \\ 0,\quad \text{if}\ \rho(q, q_{obs}) > \rho_0 \end{aligned}\right. Urep​(q)=⎩⎪⎨⎪⎧​21​η(ρ(q,qobs​)1​−ρ0​1​)2,if ρ(q,qobs​)≤ρ0​0,if ρ(q,qobs​)>ρ0​​

其中，
η \eta η 是斥力尺度因子，
ρ ( q , q o b s ) \rho(q, q_{obs}) ρ(q,qobs​) 代表物体和障碍物之间的距离。
ρ 0 \rho_0 ρ0​ 代表每个障碍物的影响半径。
换言之，离开一定的距离，障碍物就对物体没有斥力影响。

斥力就是斥力场的梯度
F r e p ( q ) = − ∇ U r e p ( q ) = { η ( 1 ρ ( q , q o b s ) − 1 ρ 0 ) ⋅ 1 ρ 2 ( q , q o b s ) ∇ ρ ( q , q o b s ) , if ρ ( q , q o b s ) ≤ ρ 0 0 , if ρ ( q , q o b s ) > ρ 0 \begin{aligned}F_{rep}(q) &= -\nabla U_{rep}(q)\\ &= \left\{\begin{aligned} &\eta (\frac{1}{\rho(q, q_{obs})} – \frac{1}{\rho_0}) \\ & \cdot \frac{1}{\rho^2(q,q_{obs})}\nabla\rho(q,q_{obs}),\quad & \text{if}\ \rho(q, q_{obs}) \le \rho_0 \\ & 0,\quad & \text{if}\ \rho(q, q_{obs}) > \rho_0 \end{aligned}\right.\end{aligned} Frep​(q)​=−∇Urep​(q)=⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧​​η(ρ(q,qobs​)1​−ρ0​1​)⋅ρ2(q,qobs​)1​∇ρ(q,qobs​),0,​if ρ(q,qobs​)≤ρ0​if ρ(q,qobs​)>ρ0​​​

----

总场

总的场就是斥力场合引力场的叠加，也就是 U = U a t t + U r e p U=U_{att}+U_{rep} U=Uatt​+Urep​，总的力也是对对应的分力的叠加，如下图所示：

From: 路径规划-人工势场法（Artifical Potential Field）

----

【机器人路径规划】人工势场法

1.概述

我们打两个比方来说明人工势场法的作用机理。首先，我们把构型空间比作一个电势场平面，机器人（的当前构型）比作空间中一点。如果让机器人的起点和障碍物带正电荷，终点带负电荷，机器人带正电荷。由于同性电荷相斥，异性电荷相吸的原理，机器人将会在电场力的作用下沿着某条路径向终点移动 ，并避开带正电荷的障碍物，如图1所示。

类似的，我们也可以把构型空间比作一个有起伏地形的区域。其中，起点和障碍物位于较高的区域，终点位于较低的区域，机器人视作一个球体。那么在重力的作用下，机器人将沿着某条轨迹从较高的起点滑落到较低的终点，并避开较高的障碍物。如图2[2]所示。

以上的两个例子其实就是电势场与重力势场的作用机制，电势场和重力势场都是自然势场。而人工势场法就是在已知起点、终点和障碍物位置的情况下，构建一个人工势场来模仿这种作用机制。人工势场法的优点在于，它其实是一种反馈控制策略，对控制和传感误差有一定的鲁棒性；缺点在于存在局部极小值问题，因此不能保证一定能找到问题的解。

From: 【机器人路径规划】人工势场法

----

Paper

M. Zhang, Y. Shen, Q. Wang and Y. Wang, “Dynamic artificial potential field based multi-robot formation control,” 2010 IEEE Instrumentation & Measurement Technology Conference Proceedings, 2010, pp. 1530-1534, doi: 10.1109/IMTC.2010.5488238.

----

close all;clear



density = 0.2;

Grid_X = 0:density:10;

Grid_Y = 0:density:10;

Basic_Z = ones(length(Grid_X), length(Grid_Y));



P0 = 20;

a = 20; %斥力影响因素

b = 10; %引力影响因素





Goal = [10,10]; %目标

Obs = [ 3, 2;

        3, 3;

        5, 7;

      5.3, 6;

        6, 6;

        2, 4;

        3, 8;

        4, 7;

        8, 9]; %障碍物坐标

for k1 = 1: length(Grid_X)

    for k2 =1:length(Grid_Y)

        X_c = Grid_X(k1);

        Y_c = Grid_Y(k2);

        rre =[];

        

        rat = sqrt((Goal(1)-X_c)^2 +(Goal(2)-Y_c)^2);

        

        Y_rre = [];

        Y_ata = [];

        

        for k3 = 1:length(Obs)

            rre(k3) =    sqrt((Obs(k3,1)-X_c)^2 +(Obs(k3,2)-Y_c)^2);

            Y_rre(k3) = a*(1/rre(k3)  - 1/P0 ) *1/(rre(k3)^2); %基本斥力场公式

            if  isinf(Y_rre(k3))==1|| Y_rre(k3)>150 %为显示效果做的限制处理

                Y_rre = 150;

            end

        end

        Y_ata = b*rat; %基本引力场公式，注意这里引力只有一个值

        Field_rre(k1,k2) = sum(Y_rre);

        Field_ata(k1,k2) =  Y_ata;

    end

end



SUM = Field_rre  + Field_ata;

surf(Grid_X,Grid_Y,SUM) %总力场

----

Matlab 代码

人工势场算法 Matlab版源码

----

自己编写的 Matlab

1. 仅考虑引力的情况

% 自写人工势场法测试函数

% 说明：

% X 轴：时间 time

% Y 轴：小车横坐标

% Z 轴：小车纵坐标

% 初始位置：[0 0]'

% 目标位置：[10 10]'



clear



P0 = [0 0]';

Pt(:,1) = P0;

PGoal = [10 10]';

Ut(:,1) = [0 0]';



% 时间参数

tBegin = 0;

tEnd = 10;

dT = 0.01;

T(1,1) = tBegin;

times = (tEnd - tBegin)/dT;



% 其他参数

xi = 0.8;



for time=1:times

    T(1,time+1) = T(1,time) + dT;

    % 引力

    FGravitation = xi * (PGoal - Pt(:,time));

    % 斥力

    FRepulsive = 0;

    % 合力

    Ut(:,time+1) = FGravitation + FRepulsive;

    % 更新位置

    Pt(:,time+1) = Pt(:,time) + dT * Ut(:,time);

end



figure(1)

plot3(T,Pt(1,:),Pt(2,:))

xlabel('Time');

ylabel('XPosition');

zlabel('YPosition');

grid on



figure(2)

plot3(T,Ut(1,:),Ut(2,:))

xlabel('Time');

ylabel('XSpeed');

zlabel('YSpeed');

grid on