matlab改进人工势场法模拟机器人路径规划，避障

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;