【路径规划】基于人工势场法机器人自动避障matlab代码

1 简介

移动机器人的路径规划是移动机器人研究领域中的一个热点问题。

2 部分代码

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%大循环结束

%***********************************画出规划路径*************************

    x=RobotPoint(:,1);

    y=RobotPoint(:,2);

    x_obs=[1,3,4,3,6,5.5,8,9,10,7];%障碍的x坐标

    y_obs=[1.5,3,4.5,6,2.5,7,8.5,9.5,5,6];%障碍的y坐标

    plot(x_obs,y_obs,'o',10,10,'v',0,0,'ms',x,y,'r')%把路径点，障碍，起点，目标分别用不同的标记画出

    %plot(RobotPoint(:,1),RobotPoint(:,2));

    clear;

3 仿真结果

​

4 参考文献

[1]杨一波, 王朝立. 基于改进的人工势场法的机器人避障控制及其MATLAB实现[J]. 上海理工大学学报, 2013, 35(5):5.

部分理论引用网络文献，若有侵权联系博主删除。​

标签：RobotPoint,10,避障,angle,target,robot,matlab,obs,势场
来源： https://blog.csdn.net/qq_59747472/article/details/122021314