人工势场法（Artificial Potential Field Method）的学习

最近的工作重心回到到算法上之后，陆陆续续开始学习一些自动驾驶的控制算法。目前工作的方向主要是online trajectory generation和底层控制。

对于在线路径规划，一个重要的概念是其应该能提供给车辆一个速度和方向控制命令，使底层控制器跟随这个输入去执行运动控制操作。

人工势场法（Artificial Potential Field Method）是一个典型的在线路径算法。其运用了'水往低处流'的思路，很自然的能够理解车辆路径的产生规律。

话不多说，直接上Matlab代码看一下。

close all;clear
%% Copyright@https://www.jianshu.com/u/1f9b23cfe862

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) %总力场

下面两个图是基于以上代码最后的路径和总势场。 [10,10] 是目标点，而[0,0]是我们的初始位置。

Path

Field

其实该方法有很多局限性，比如如果障碍物很靠近终点（如[9,9]），则可能无法达到。如果某个地方应力与斥力相等，则可能陷入布局最优。因此有很多改进法。后续再谈。