航迹规划——人工势场算法

深碍√TFBOYSˉ_ 2022-05-29 09:18 280阅读 0赞

# **人工势场算法简介** #

--------------------

人工势场法路径规划是由Khatib提出的一种虚拟力法（Oussama Khatib，Real-Time obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1994 IEEE.）。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。  
  该算法在网上已经有较多的详细解释，在这里不做具体解释，主要讲解下Matlab下代码的编写。想了解算法原理的可以看最后的参考资料。

--------------------

# **人工势场算法缺陷** #

1.  当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物。
2.  当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物。
3.  在某个点，引力和斥力刚好大小相等，方向想反，则物体容易陷入局部最优解或震荡。

--------------------

# **缺陷解决方法** #

缺陷一：对于可能会碰到障碍物的问题，可以通过修正引力函数来解决，避免由于离目标点太远导致引力过大。

（b）目标点附近有障碍物导致目标不可达的问题，引入一种新的斥力函数。  
![这里写图片描述][SouthEast]  
缺陷三：局部最优问题是一个人工势场法的一个大问题，这里可以通过加一个随机扰动，让物体跳出局部最优值。类似于梯度下降法局部最优值的解决方案。

--------------------

# **重点代码解析** #

--------------------

## 计算目标点和障碍物对智能体所在位置的角度 ##

--------------------

<table> 
 <thead> 
  <tr> 
   <th>输入</th> 
   <th align="left">意义</th> 
  </tr> 
 </thead> 
 <tbody> 
  <tr> 
   <td>X</td> 
   <td align="left">起点坐标</td> 
  </tr> 
  <tr> 
   <td>Xsum</td> 
   <td align="left">目标和障碍的坐标向量,是(n+1)*2矩阵</td> 
  </tr> 
 </tbody> 
</table>

<table> 
 <thead> 
  <tr> 
   <th>输出</th> 
   <th align="left">意义</th> 
  </tr> 
 </thead> 
 <tbody> 
  <tr> 
   <td>Y</td> 
   <td align="left">是引力，斥力与x轴的角度向量</td> 
  </tr> 
 </tbody> 
</table>

function Y=compute_angle(X,Xsum,n)%Y是引力，斥力与x轴的角度向量,X是起点坐标，Xsum是目标和障碍的坐标向量,是(n+1)*2矩阵
      for i=1:n+1%n是障碍数目
          deltaX(i)=Xsum(i,1)-X(1);
          deltaY(i)=Xsum(i,2)-X(2);
          r(i)=sqrt(deltaX(i)^2+deltaY(i)^2);
          if deltaX(i)>0
              theta=acos(deltaX(i)/r(i));
          else
              theta=pi-acos(deltaX(i)/r(i));
          end
          if i==1%表示是目标
              angle=theta;
          else
              angle=theta;
          end     
          Y(i)=angle;%保存每个角度在Y向量里面，第一个元素是与目标的角度，后面都是与障碍的角度
      end
    end

--------------------

## 目标点对智能体所在位置引力计算函数 ##

--------------------

<table> 
 <thead> 
  <tr> 
   <th>输入</th> 
   <th align="left">意义</th> 
  </tr> 
 </thead> 
 <tbody> 
  <tr> 
   <td>X</td> 
   <td align="left">当前坐标</td> 
  </tr> 
  <tr> 
   <td>Xsum</td> 
   <td align="left">障碍的坐标</td> 
  </tr> 
  <tr> 
   <td>k</td> 
   <td align="left">增益常数</td> 
  </tr> 
  <tr> 
   <td>angle</td> 
   <td align="left">角度分量</td> 
  </tr> 
  <tr> 
   <td>b</td> 
   <td align="left">null</td> 
  </tr> 
  <tr> 
   <td>Po</td> 
   <td align="left">障碍影响距离，大于Po时影响力为0</td> 
  </tr> 
  <tr> 
   <td>n</td> 
   <td align="left">障碍物数量</td> 
  </tr> 
 </tbody> 
</table>

<table> 
 <thead> 
  <tr> 
   <th>输出</th> 
   <th align="left">意义</th> 
  </tr> 
 </thead> 
 <tbody> 
  <tr> 
   <td>Yatx</td> 
   <td align="left">引力在智能体当前位置的 X 轴分量</td> 
  </tr> 
  <tr> 
   <td>Yaty</td> 
   <td align="left">引力在智能体当前位置的 Y 轴分量</td> 
  </tr> 
 </tbody> 
</table>

function [Yatx,Yaty]=compute_Attract(X,Xsum,k,angle,b,Po,n)
    %把路径上的临时点作为每个时刻的Xgoal
    R=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路径点和目标的距离平方
    r=sqrt(R);%路径点和目标的距离
    Yatx=k*r*cos(angle);%angle=Y(1)
    Yaty=k*r*sin(angle);
    end

--------------------

## 障碍物对智能体所在位置斥力计算函数 ##

--------------------

function [Yrerxx,Yreryy,Yataxx,Yatayy]=compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po,a)%输入参数为当前坐标，Xsum是目标和障碍的坐标向量，增益常数,障碍，目标方向的角度
    Rat=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路径点和目标的距离平方
    rat=sqrt(Rat);%路径点和目标的距离
    for i=1:n
        Rrei(i)=(X(1)-Xsum(i+1,1))^2+(X(2)-Xsum(i+1,2))^2;%路径点和障碍的距离平方
        rre(i)=sqrt(Rrei(i));%路径点和障碍的距离保存在数组rrei中
        R0=(Xsum(1,1)-Xsum(i+1,1))^2+(Xsum(1,2)-Xsum(i+1,2))^2;
        r0=sqrt(R0);
        if rre(i)>Po%如果每个障碍和路径的距离大于障碍影响距离，斥力令为0
              Yrerx(i)=0;
              Yrery(i)=0;
              Yatax(i)=0;
              Yatay(i)=0;
        else
            %if r0<Po
            if rre(i)<Po/2
           Yrer(i)=m*(1/rre(i)-1/Po)*(1/Rrei(i))*(rat^a);%分解的Fre1向量
           Yata(i)=a*m*((1/rre(i)-1/Po)^2)*(rat^(1-a))/2;%分解的Fre2向量
           Yrerx(i)=(1+0.1)*Yrer(i)*cos(angle_re(i));%angle_re(i)=Y(i+1)
           Yrery(i)=-(1-0.1)*Yrer(i)*sin(angle_re(i));
           Yatax(i)=Yata(i)*cos(angle_at);%angle_at=Y(1)
           Yatay(i)=Yata(i)*sin(angle_at);
            else
           Yrer(i)=m*(1/rre(i)-1/Po)*1/Rrei(i)*Rat;%分解的Fre1向量
           Yata(i)=m*((1/rre(i)-1/Po)^2)*rat;%分解的Fre2向量
           Yrerx(i)=Yrer(i)*cos(angle_re(i));%angle_re(i)=Y(i+1)
           Yrery(i)=Yrer(i)*sin(angle_re(i));
           Yatax(i)=Yata(i)*cos(angle_at);%angle_at=Y(1)
           Yatay(i)=Yata(i)*sin(angle_at);
            end
       end%判断距离是否在障碍影响范围内
    end
       Yrerxx=sum(Yrerx);%叠加斥力的分量
       Yreryy=sum(Yrery);
       Yataxx=sum(Yatax);
       Yatayy=sum(Yatay);
    end

# **完整Matlab代码下载链接** #

--------------------

[人工势场算法完整“.m”文件下载][.m]

# **参考资料** #

--------------------

[1.路径规划-人工势场法][1._-]  
[2.机器人路径规划\_人工势场法][2.]

[SouthEast]: /images/20220529/d45f84df7a8848f483da7df05b2bab3f.png
[.m]: https://download.csdn.net/download/qq_16775293/10299516
[1._-]: http://blog.csdn.net/junshen1314/article/details/50472410
[2.]: http://blog.csdn.net/xiao_lxl/article/details/50343195