基于人工势场的机器人路径规划-MATLAB源程序

%main %初始化粒子的参数 Xo=[0 0];%起点位置 k=1;%计算引力需要的增益系数 m=1;%计算斥力的增益系数 n=1;%障碍个数 longth=0.5;%步长 J=200;%循环迭代次数 %如果不能实现预期目标，可能也与初始的增益系数，Po设置的不合适有关。 K=0;%初始化 m_Target=[100 100]; %[m_Obs,m_ObsR]=Obs_Generate([5 10],[20 80],[10 80],n); Xj=Xo;%j=1循环初始，将车的起始坐标赋给Xj %随即产生障碍 m_Obs=[50 50]; m_ObsR=[10]; Po=min(m_ObsR)/2; %***************初始化结束，开始主体循环****************** for j=1:J%循环开始 Current(j,1)=Xj(1);%Goal是保存车走过的每个点的坐标。刚开始先将起点放进该向量。 Current(j,2)=Xj(2); %调用计算角度模块 [angle_att,angle_rep]=compute_angle(Xj,m_Target,m_Obs,n); %调用计算引力模块 [Fatt,Uatt(j)]=compute_Attract(Xj,m_Target,k,angle_att); %调用计算斥力模块 [Frep,Fatt_add,Urep(j)]=compute_repulsion(Xj,m_Target,m_Obs,m_ObsR,m,angle_att,angle_rep,n,Po); %计算合力和方向 [Position_angle(j)]=compute_Ftotal(Fatt,Frep,Fatt_add,n); %计算车的下一步位置 Xnext(1)=Xj(1)+longth*cos(Position_angle(j)); Xnext(2)=Xj(2)+longth*sin(Position_angle(j)); %保存车的每一个位置在向量中 Xj=Xnext; % Draw(Xj,m_Obs,m_Target,n); %判断 if (Is_Reach(Xj,m_Target,longth)==1)%是应该完全相等的时候算作到达，还是只是接近就可以？现在按完全相等的时候编程。 K=j;%记录迭代到多少次，到达目标。 break; %记录此时的j值 end%如果不符合if的条件，重新返回循环，继续执行。 end%大循环结束 %****************************以下是图形显示部分**************************** %画势场分析图 Draw(Uatt,Urep); %画模型效果图 Draw_Model(Xo,Current,m_Obs,m_ObsR,m_Target,n); %画总体势场分布图 Draw_Potential(Xo,m_Obs,m,m_ObsR,Po,m_Target,k,n,[0 100],[0 100]); clear;