0002人工势能避障算法凸形障碍物.zip_matlab_matlab 避障_matlab避障_人工势能法_避障 matlab

clear clc %初始化参数 Xo = [-10 -10]; % 起点 m_Target = [55,40]; % 目标点 attk = 1; % 计算引力需要的增益系数 repk = 2; % 计算斥力的增益系数 step = 1; %步长 J = 200; %迭代次数 % 障碍物的圆心坐标 Xgr = [3,2 ; 15,20 ; 20,3 ; 25,40 ; 30,12 ; 40,26 ; 40,42 ; 50,35 ; ]; Radius = [5, 5 , 4, 4 , 5 , 4 , 5 , 2]; Po = [8, 8 , 6, 6 , 8 , 6 , 8 , 4]; % 障碍影响距离，当障碍和车的距离大于这个距离时，斥力为0，即不受该障碍的影响。也是自己设定。 % 障碍物的圆心坐标 % Xgr = round(500*rand(20,2)); % Radius = round(50*rand(20,1)); repn = size(Xgr,1); %障碍个数 % 起点位置 Xj = Xo; %***************初始化结束，开始迭代计算************** for j = 1:J %迭代开始 Goal(j,:) = Xj ; %Goal是保存车走过的每个点的坐标。 % 调用计算角度模块 [angle_att,angle_rep] = compute_angle(Xj,m_Target,Xgr,repn); % 与目标、障碍的夹角 if j ==1 disp('引力角度'); angle_att*180/pi disp('斥力角度'); angle_rep*180/pi end %调用计算引力模块 [Fattx,Fatty] = compute_attract(Xj,m_Target,attk,angle_att); %调用计算斥力模块 [Frepx,Frepy,Frattx,Fratty] = compute_repulsion(Xj,m_Target,Xgr,repk,angle_att,angle_rep,repn,Po,Radius); %计算合力和方向 Fsumx = Fattx + Frattx + Frepx; Fsumy = Fatty + Fratty + Frepy; r = sqrt(Fsumx^2 + Fsumy^2); if Fsumx >= 0 && Fsumy >= 0 angle_sum = acos(Fsumx/r); elseif Fsumx >= 0 && Fsumy < 0 angle_sum = 2*pi-acos(Fsumx/r); elseif Fsumx < 0 && Fsumy >= 0 angle_sum = pi - acos(-Fsumx/r); elseif Fsumx < 0 && Fsumy < 0 angle_sum = pi + acos(-Fsumx/r); end % 计算车的下一步位置 Xnext(1) = Xj(1) + step*cos(angle_sum); Xnext(2) = Xj(2) + step*sin(angle_sum); % 记录车的下一节点 Xj = Xnext; %------------------画图-------------------- pause(0.1); plot(m_Target(1,1),m_Target(1,2),'v-b',Xo(1),Xo(2),'ms',Goal(:,1),Goal(:,2),'.r'); hold on; for i = 1:repn alpha = 0:pi/20:2*pi; r = Radius(i); plot(Xgr(i,1)+r*cos(alpha),Xgr(i,2)+r*sin(alpha),'k--','LineWidth',1); fill(Xgr(i,1)+r*cos(alpha),Xgr(i,2)+r*sin(alpha),'k'); hold on plot(Xgr(i,1)+Po(i)*cos(alpha),Xgr(i,2)+Po(i)*sin(alpha),'g--','LineWidth',1.5); fill(Xgr(i,1)+r*cos(alpha),Xgr(i,2)+r*sin(alpha),'k'); end axis([-10 60 -10 60]); axis('square'); %判断 if abs(Xj(1)-m_Target(1,1))<0.5 && abs(Xj(2) - m_Target(1,2)) < 0.5; K = j % 记录迭代次数到达目标 break; end end