一、简介
人工势场法是局部路径规划的一种比较常用的方法。这种方法假设机器人在一种虚拟力场下运动。
如图所示，机器人在一个二维环境下运动，图中指出了机器人，障碍和目标之间的相对位置。
这个图比较清晰的说明了人工势场法的作用，物体的初始点在一个较高的“山头”上，要到达的目标点在“山脚”下，这就形成了一种势场，物体在这种势的引导下，避开障碍物，到达目标点。
人工势场包括引力场合斥力场，其中目标点对物体产生引力，引导物体朝向其运动（这一点有点类似于A*算法中的启发函数h）。障碍物对物体产生斥力，避免物体与之发生碰撞。物体在路径上每一点所受的合力等于这一点所有斥力和引力的和。这里的关键是如何构建引力场和斥力场。下面我们分别讨论一下：
引力场：
二、存在的问题
（a） 当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物
（b）当目标点附近有障碍物时，斥力将非常大，引力相对较小，物体很难到达目标点
（c）在某个点，引力和斥力刚好大小相等，方向想反，则物体容易陷入局部最优解或震荡
三、各种改进版本的人工势场法
（a）对于可能会碰到障碍物的问题，可以通过修正引力函数来解决，避免由于离目标点太远导致引力过大
二、源代码
close all
clear all
tic
%%
%==========================================================================
%初始化
%==========================================================================
P0 = [0;0;0];%起点位置
n = 7;%障碍个数
Tar = [10;10;10];%%目标点坐标
Obs = [10 10 10;1 1.2 1;3 2.5 3;4 4.5 4;3 6 3;6 2 6;6 6 6;8 8.5 8]';%目标点与障碍位置 第一列是目标点
%%
figure(1);
plot3(Obs(1,2:8),Obs(2,2:8),Obs(3,2:8),'o');hold on;
plot3(Obs(1,1),Obs(2,1),Obs(3,1),'O');
for i=1:5
po = i;%障碍影响距离
[P,K]=shi(po,Tar,n,P0,Obs);
%==========================================================================
%画图
%==========================================================================
switch i
case 1
plot3(P(1,1:K),P(2,1:K),P(3,1:K),'.b');
case 2
plot3(P(1,1:K),P(2,1:K),P(3,1:K),'.r');
case 3
plot3(P(1,1:K),P(2,1:K),P(3,1:K),'.g');
case 4
plot3(P(1,1:K),P(2,1:K),P(3,1:K),'.m');
otherwise
plot3(P(1,1:K),P(2,1:K),P(3,1:K),'.k');
end
end
toc
三、运行结果
四、备注
版本：2014a
