【路径规划】基于人工势场算法求解动态路径规划 带GUI界面matlab代码

1 简介

针对移动机器人的实时导航和避障设计了基于人工势场的控制算法,用该算法控制移动机器人能在未知的环境中,实时检测出障碍物,并实时规划出合理路径,稳定,平滑连续地向目标行驶,给出了机器人行驶的实验结果.通过小车车体方位计算确定了避障方法-人工势场法,即目标位置对移动机器人产生一种虚拟的吸引力,而障碍物对机器人产生一种虚拟的排斥力,这两种力的合成就决定了移动机器人的运动.通过对处于静态环境下的小车的路径进行了规划并进行计算机仿真.仿真结果表明,该人工势场法能有效地实现机器人小车的避障功能.

2 部分代码

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

3 仿真结果

4 参考文献

[1]张煌辉. 基于动态人工势场的路径规划研究与应用. Diss. 长沙理工大学, 2010.