【路径规划-二维路径规划】基于人工势场求解二维障碍路径规划问题附matlab代码

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法  神经网络预测 雷达通信  无线传感器

信号处理 图像处理 路径规划 元胞自动机 无人机

⛄ 内容介绍

人工势场法是移动机器人路径规划的一种常用方法,是势场法思想的起源,是最基础的方法.笔者利用人工势场法原理对移动机器人路径规划进行了研究,并通过MATLAB进行仿真.

⛄ 部分代码

clear all;

x=[1 3 4 7 6 5.5 8 9.5];%???x??

y=[1.5 2.2 4.5 6 2 6 7.8 7];

plot(x,y,'O',10,10,'v',0,0,'ms');

hold on

%??????????????????,??????????????????????????

%???????

Xo=[0 0];%????

k=20;%???????????

K=0;%???

m=5;%??????????????????

Po=2;%??????????????????????????0??????????????????

n=8;%????

a=0.5;

l=0.05;%??

J=500;%??????

%???????????????????????Po?????????

%end

%?????????

Xsum=[10 10;1 1.5;3 2.2;4 4.5;7 6;6 2;5.5 6;8 7.8;9.5 7];%?????(n+1)*2????[10 10]?????????????????

Xj=Xo;%j=1??????????????Xj

[x,y]=meshgrid(-1:0.5:12,-1:0.5:12);

z=0.5*k./(sqrt((x-10).^2+(y-10).^2+0.09))-0.5*m*(1./(sqrt((x-1).^2+(y-1.5).^2+0.09))-1/3.5).^2-0.5*m*(1./(sqrt((x-3).^2+(y-2.2).^2+0.09))-1/3.5).^2 ...

    -0.5*m*(1./(sqrt((x-4).^2+(y-4.5).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-7).^2+(y-6).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-6).^2+(y-2).^2+0.09))-1/3.5).^2 ...

    -0.5*m*(1./(sqrt((x-5.5).^2+(y-6).^2+0.09))-1/4).^2-0.5*m*(1./(sqrt((x-8).^2+(y-7.8).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-9.5).^2+(y-7).^2+0.09))-1/3).^2;

% contour(x,y,z,[-50:20:1000]);

[C,h]=contour(x,y,z,[-80:10:300]);

set(h,'ShowText','on','TextStep',get(h,'LevelStep')*2)

colormap cool

[px,py]=gradient(z);%????x,y??????????

quiver(x,y,px,py,'k') %???????????

p=sqrt(px.^2+py.^2);

%%??

t=1;

M(t)=getframe;

t=t+1;

%***************????????????******************

for j=1:J%????

    Goal(j,1)=Xj(1);%Goal???????????????????????????

    Goal(j,2)=Xj(2);

%????????

   Theta=compute_angle(Xj,Xsum,n);%Theta??????????????????X????????????????????????????????

%????????

   Angle=Theta(1);%Theta?1????????????????????

   angle_at=Theta(1);%???????????????????angle_at

   [Fatx,Faty]=compute_Attract(Xj,Xsum,k,Angle,0,Po,n);%???????????x,y?????????

   

    for i=1:n

       angle_re(i)=Theta(i+1);%?????????????????n??????n????

     end

%????????

    [Frerxx,Freryy,Fataxx,Fatayy]=compute_repulsion(Xj,Xsum,m,angle_at,angle_re,n,Po,a);%??????x,y????????

%????????????????????j???????????????????????????????????????????????

    Fsumyj=Faty+Freryy+Fatayy;%y?????

    Fsumxj=Fatx+Frerxx+Fataxx;%x?????

    Position_angle(j)=atan(Fsumyj/Fsumxj);%???x????????

%?????????

    Xnext(1)=Xj(1)+l*cos(Position_angle(j));

    Xnext(2)=Xj(2)+l*sin(Position_angle(j));

    %?????????????

    Xj=Xnext;

    X=Goal(:,1);

    Y=Goal(:,2);

    plot(X,Y,'.r');

    M(t)=getframe;t=t+1;

    %??

    if ((Xj(1)-Xsum(1,1))>0)&((Xj(2)-Xsum(1,2))>0)%??????????????????????????????????????

       K=j;%??????????????

       break;

       %?????j?

    end%?????if????????????????

end%?????

⛄ 运行结果

⛄ 参考文献

[1]刘翔, LIU, Xiang,等. 利用人工势场法实现无人艇二维路径规划[J]. 舰船科学技术, 2017(5X):3.

[2]王帅, 连翠玲, 赵旭. 基于人工势场法的移动机器人路径规划研究[J]. 河北省科学院学报, 2011, 28(4):3.

❤️ 关注我领取海量matlab电子书和数学建模资料

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