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

最新推荐文章于 2025-04-19 11:15:36 发布

不懂音乐的欣赏者

最新推荐文章于 2025-04-19 11:15:36 发布

阅读量1.6w

点赞数 7

分类专栏：无人机集群航迹规划及避障文章标签：算法人工势场 Matlab 人工智能航迹规划

本文链接：https://blog.csdn.net/qq_16775293/article/details/79637314

版权

无人机集群航迹规划及避障专栏收录该内容

12 篇文章

订阅专栏

人工势场算法简介

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

人工势场算法缺陷

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

缺陷解决方法

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

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

重点代码解析

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

输入	意义
X	起点坐标
Xsum	目标和障碍的坐标向量,是(n+1)*2矩阵

输出	意义
Y	是引力，斥力与x轴的角度向量

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

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

输入	意义
X	当前坐标
Xsum	障碍的坐标
k	增益常数
angle	角度分量
b	null
Po	障碍影响距离，大于Po时影响力为0
n	障碍物数量

输出	意义
Yatx	引力在智能体当前位置的 X 轴分量
Yaty	引力在智能体当前位置的 Y 轴分量

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