Artificial potential field method.rar_potential field_人工势场_路径规划人

clear all clc X0=[0 0]; % 起点位置 k=15; % 计算引力需要的增益系数 m=4; % 计算斥力的增益系数 a=0.5; % 斥力在引力方向上的分量的增益系数 Po=2.5; % 障碍影响距离 l=0.1; % 步长 J=600; % 循环迭代步数 % 给出障碍和目标信息 Xgoal=[7 8]; % 这个向量是(n+1)*2维，其中[10 10]是目标位置，剩下的都是障碍位置 Xobst=[1 1;3 2.2;4 4.5;3 6;6 2.2;5.5 6;8 8;3 4]; [num,~]=size(Xobst); % num是障碍物的个数 Xj=X0; % j=1循环初始，将车的起始坐标赋给Xj % 初始化结束，开始主体循环 for j=1:J % 循环开始 Path(j,:)=Xj; % 调用计算引力模块 Fattr=compute_Attract(Xj,Xgoal,k); % 计算出目标对车的引力在x,y方向的两个分量值 % 调用计算斥力模块 % Frep=compute_repulsion(Xj,Xobst,m,num,Po); % 计算出斥力在x,y方向的分量数组（原版人工势能算法） Frep=evo_compute_repulsion(Xj,Xgoal,Xobst,m,a,num,Po); % 计算出斥力在x,y方向的分量数组（改进版人工势能算法） % 计算合力和合力的方向，其实这里主要是要得到合力的方向，因为实际情况中机器人每步的运动其步长是确定的， % 只是方向不同，所以这里通过求在x,y方向上的合力得到合力的方向 Fsum=Fattr+Frep; % 计算车的下一步位置 Xnext=Xj+l*Fsum/normest(Fsum); Xj=Xnext; % 判断 distance=((Xj(1)-Xgoal(1))^2+(Xj(2)-Xgoal(1))^2)^0.5; if distance<0.1 break; end end % 画出障碍，起点，目标，路径点 plot(Xobst(:,1),Xobst(:,2),'o',Xgoal(1),Xgoal(2),'v',0,0,'ms',Path(:,1),Path(:,2),'r'); axis([0,11,0,11]); set(gca,'xtick',0:1:11); set(gca,'ytick',0:1:11); set(gca,'fontsize',10,'fontname','times new roman'); grid on