【路径规划】基于matlab人工势场法自动避障【含Matlab源码 620期】

一、简介

人工势场法是局部路径规划的一种比较常用的方法。这种方法假设机器人在一种虚拟力场下运动。
在这里插入图片描述
如图所示，机器人在一个二维环境下运动，图中指出了机器人，障碍和目标之间的相对位置。

这个图比较清晰的说明了人工势场法的作用，物体的初始点在一个较高的“山头”上，要到达的目标点在“山脚”下，这就形成了一种势场，物体在这种势的引导下，避开障碍物，到达目标点。

人工势场包括引力场合斥力场，其中目标点对物体产生引力，引导物体朝向其运动（这一点有点类似于A*算法中的启发函数h）。障碍物对物体产生斥力，避免物体与之发生碰撞。物体在路径上每一点所受的合力等于这一点所有斥力和引力的和。这里的关键是如何构建引力场和斥力场。下面我们分别讨论一下：

引力场：

常用的引力函数：
在这里插入图片描述
这里的ε是尺度因子.ρ(q,q_goal)表示物体当前状态与目标的距离。引力场有了，那么引力就是引力场对距离的导数（类比物理里面W=FX）：、

关于梯度的算法可以参考相关资料，简单提一下，二元函数梯度是酱紫的[δx,δy],这个符号是偏导数，不太对，见谅。
在这里插入图片描述
Fig .引力场模型

斥力场：
在这里插入图片描述
公式（3）是传统的斥力场公式，现在还没有搞清楚是怎么推导出来的。公式中η是斥力尺度因子，ρ（q,q_obs）代表物体和障碍物之间的距离。ρ_0代表每个障碍物的影响半径。换言之，离开一定的距离，障碍物就对物体没有斥力影响。

斥力就是斥力场的梯度
在这里插入图片描述

Fig 斥力场模型

总的场就是斥力场合引力场的叠加，也就是U=U_att+U_rep,总的力也是对对应的分力的叠加，如下图所示：
在这里插入图片描述
二、存在的问题
（a）当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物

（b）当目标点附近有障碍物时，斥力将非常大，引力相对较小，物体很难到达目标点

（c）在某个点，引力和斥力刚好大小相等，方向想反，则物体容易陷入局部最优解或震荡

三、各种改进版本的人工势场法
（a）对于可能会碰到障碍物的问题，可以通过修正引力函数来解决，避免由于离目标点太远导致引力过大
在这里插入图片描述
和（1）式相比，（5）式增加了范围限定。d*_goal 给定了一个阈值限定了目标和物体之间的距离。对应的梯度也就是引力相应变成：

（b）目标点附近有障碍物导致目标不可达的问题，引入一种新的斥力函数
在这里插入图片描述
这里在原有斥力场的基础上，加上了目标和物体距离的影响,（n是正数，我看到有篇文献上n=2）。直观上来说，物体靠近目标时，虽然斥力场要增大，但是距离在减少，所以在一定程度上可以起到对斥力场的拖拽作用

相应斥力变成：
在这里插入图片描述
所以可以看到这里引力分为两个部分，编程时要格外注意

（c）局部最优问题是一个人工势场法的一个大问题，这里可以通过加一个随机扰动，让物体跳出局部最优值。类似于梯度下降法局部最优值的解决方案。

二、源代码

function [d_obs_line] = distance_fuzzyControl( X,X_target,X_obs,n)
%d_obs指机器人到最近障碍物的距离,d_obs_line指障碍物到机器人与目标连线的距离
%   Detailed explanation goes here
% d_obs=0.8;%模糊远近界点
% for i=1:n
%      D(i)=(X(1)-X_obs(i,1))^2+(X(1)-X_obs(i,2))^2;%路径点和障碍物的距离平方
%      d(i)=sqrt(D(i));
%      if(d_obs>d(i))
%          d_obs=d(i);%取最小值
%      end
% end
%     D_target=(X(1)-X_target(1))^2+(X(2)-X_target(2))^2;
%     d_target=sqrt(D_target);
  %  d_obs_line=0.8;%靠近目标点时，大于0.8认为机器人应该直线运动
Xo=[0,0];%起点位置
k=10;%计算引力需要的增益系数
K=0;%初始化
m=1;%计算斥力的增益系数，自己设定
d=2;%障碍影响距离，当障碍和车的距离大于这个距离时，斥力为0，即不受该障碍的影响,自己设定
n=10;%障碍个数
l=0.5;%步长
J=200;%循环迭代次数
X_target=[10,10];
X_obs=[1 1.5;3 3;4 4.5;3 6;6 2.5;5.5 7;8 8.5;9,9.5;10 5;7 6];%这个向量是n*2维，障碍的位置
X_robot=Xo;%X_robot是机器人的定位坐标,将车的起始坐标Xo赋给X_robt
%***************初始化结束，开始主体循环******************/
for j=1:J
   RobotPoint(j,1)=X_robot(1);%赋初值,RobotPoint存放机器人走过的每个点的坐标，刚开始先将起点放进该向量
   RobotPoint(j,2)=X_robot(2);
   %调用计算角度模块
   [angle_at,angle_re]=compute_angle(X_robot,X_target,X_obs,n);%angle_at,angle_re是计算出来的机器人和目标/障碍之间的与X轴之间的夹角，统一规定角度为逆时针方向
   %调用计算引力模块,x_at/y_at是引力在x/y轴的分量之和
   [x_at,y_at]=compute_attraction(X_robot,X_target,k,angle_at);%机器人坐标，目标点，增益常数，角度
   %调用斥力模块
   [x_re,y_re]=compute_repultion(X_robot,X_target,X_obs,m,angle_re,n,d);%输入参数为当前坐标，Xsum是障碍物的坐标向量，增益常数,障碍物方向的角度,障碍物个数,影响阈值
    %计算合力在x,y轴的分量
    F_xj=x_at+x_re;
    F_yj=y_at+y_re;
     %计算合力与x轴夹角
     if F_xj>0
       angle_F(j)=atan(F_yj/F_xj);
     else
       angle_F(j)=pi+atan(F_yj/F_xj);
     end
    %下一步行进方向
     Xnext(1)= X_robot(1)+l*cos(angle_F(j));
     Xnext(2)= X_robot(2)+l*sin(angle_F(j)); 
   %保存机器人的每一个位置
     X_robot=Xnext;
   %判断是否到达目标点,距离在一个步长之内认为到达目标点
    r_target=sqrt((X_robot(1)-X_target(1))^2+(X_robot(2)-X_target(2))^2);
   %考虑靠近目标点的情况，尽量直线运动，防止反复来回震荡
    if(r_target>l&&r_target<2*l)
       [d_obs_line] = distance_fuzzyControl( X_robot,X_target,X_obs,n);
       if( d_obs_line>=0.5)%far,不会影响机器人的直线运动
         Xnext(1)= X_robot(1)+l*cos(angle_at);
         Xnext(2)= X_robot(2)+l*sin(angle_at);  
         X_robot=Xnext;
       end
    end
    %判断是否到达目标点,距离在一个步长之内认为到达目标点
     if(r_target<=l)
       K=j;%记录迭代到多少次，到达目标。
       break;
     end%如果不符合if的条件，重新返回循环，继续执行。
end%大循环结束

三、运行结果

在这里插入图片描述

四、备注