potential_12.m

基于人工势场的路径规划算法及其具体的实现

clear;
clc;
clf;
last=0;
flag=0;
e=50;
r=50;
p0=1500;
pa=1000;
u=1e-4;
h=1e7;
x=0:100:7400;
y=0:100:7400;

d=[6000,2500];                         % destination


t(:,:,1)=[3000,2100];                % obstacles
t(:,:,2)=[3000,2200];                % the number of obstacles can be any positive number
t(:,:,3)=[3000,2300];
t(:,:,4)=[3000,2400];
 t(:,:,5)=[3000,2500];
 t(:,:,6)=[3000,2600];
t(:,:,7)=[3000,2700];
t(:,:,8)=[3000,2800];
t(:,:,9)=[3000,2900];
t(:,:,10)=[3000,3000];
t(:,:,11)=[3000,3100];

% t(:,:,12)=[3000,3200];
% t(:,:,13)=[3000,2000];
% t(:,:,14)=[3000,3300];
% t(:,:,15)=[3000,1900];
% 
% t(:,:,1)=[5000,2300];
%  t(:,:,2)=[4700,2500];
%  t(:,:,3)=[4700,1900];
%  t(:,:,4)=[5200,2000];
%  t(:,:,15)=[3000,1800];
% t(:,:,16)=[3000,3400];
for i=1:8
    tt(:,:,i)=[1e5,1e5];
%     ss(:,:,i)=[1,1];
end


ep=1e-2;                         % a parameter to avoid the number to divide is zero.

s=[1000,2500];                         % initial position of robot

z1=zeros(length(x),length(y));
z2=z1;

for i=1:length(x)
    for j=1:length(y)
        z1(j,i)=0.5*u*norm([x(i),y(j)]-d)^2;                                            % atraction
        z2(j,i)=0;
        for k=1:length(t)
            tt_emp=norm([x(i),y(j)]-t(:,:,k));
            if(tt_emp<p0)
%                 if(tt_emp<1000&&tt_emp>800)
%                     tt_emp=0.001*tt_emp;
%                 else
%                     if(tt_emp<800&&tt_emp>400)
%                         tt_emp=0.0001*tt_emp;
%                     else
%                         if(tt_emp<200)
%                             tt_emp=0;
%                         end                        
%                     end
%                 end
                z2(j,i)=z2(j,i)+0.5*h*(1/(tt_emp+ep)-1/p0)^2;                 % repelant
            end   
        end
    end
end
z=z1+z2;
figure(1);
surf(x,y,z);
xlabel('x');
ylabel('y');

% hold on;
 for i=1:500
    if(last==1)
        break;
    end
    diffs=u*(s-d); 
    for j=1:8         
        ss(:,1,j)=1;
        switch(j)
            case 1
                ss(:,2,j)=ss(:,1,j)*tan(atan2(diffs(2),diffs(1))+90/180*pi);
            case 2
                ss(:,2,j)=ss(:,1,j)*tan(atan2(diffs(2),diffs(1))+50/180*pi);
            case 3
                ss(:,2,j)=ss(:,1,j)*tan(atan2(diffs(2),diffs(1))+30/180*pi);
            case 4
                ss(:,2,j)=ss(:,1,j)*tan(atan2(diffs(2),diffs(1))+10/180*pi);
            case 5
                ss(:,2,j)=ss(:,1,j)*tan(atan2(diffs(2),diffs(1))-10/180*pi);
            case 6
                ss(:,2,j)=ss(:,1,j)*tan(atan2(diffs(2),diffs(1))-30/180*pi);
            case 7
                ss(:,2,j)=ss(:,1,j)*tan(atan2(diffs(2),diffs(1))-50/180*pi);
            case 8
                ss(:,2,j)=ss(:,1,j)*tan(atan2(diffs(2),diffs(1))-90/180*pi);
            otherwise
                disp('----Error----');
        end
        ss(:,:,j)=ss(:,:,j)./norm(ss(:,:,j));
        for jj=1:40
            tt(:,:,j)=s+25*jj*ss(:,:,j);
            ttemp=[round(tt(:,1,j)/100),round(tt(:,2,j)/100)];
            if(ttemp(1)==0)
                ttemp(1)=1;
            end
            if(ttemp(2)==0)
                ttemp(2)=1;
            end
            if(ttemp(1)>0&&ttemp(2)>0&&ttemp(1)<=75&&ttemp(2)<=75&&z2(ttemp(2),ttemp(1))>1e5)
                break;
            end
        end
   %                 t3_temp=t3_temp/exp(1*t3_temp/p0);
   if(jj<40)      
%             t3_temp=norm(s-tt(:,:,j))-200;
            t3_temp=norm(s-tt(:,:,j));
            if(t3_temp<p0)
                kk=exp(1.0355*t3_temp/p0);
                t3_temp=t3_temp./kk;
                diffs=diffs-0.5*h*((s-tt(:,:,j))./(t3_temp+ep)^3)*(1/(t3_temp+ep)-1/p0);            % the dierction of robot movement, it is the grads of potential function
%                 if(flag==1&&norm(s-getxy(:,:,num_i))<pa)
%                    diffs=diffs-0.5*h*((s-getxy(:,:,num_i))./(norm(s-getxy(:,:,num_i))+ep)^3)*(1/(norm(s-getxy(:,:,num_i))+ep)-1/pa);
%                 end
            end
        else
            tt(:,:,j)=[1e5,1e5];
        end           
    end
%     mozhi(i)=norm(diffs);
    getxy(:,:,i)=s;
     count(i)=0;
%      if(i>30&&norm(diffs)<0.2)
% %      if(i>30)
%          for j=i:-1:(i-30)
%              dd=norm(getxy(:,:,j)-getxy(:,:,i));
%              if(dd<300)
%                  count(i)=count(i)+1;
%              end
%          end
%          if(count(i)>30)
% %              for j=i:-1:(i-20)
%                j=i;
%                while(1)                 
%                  s=getxy(:,:,j);
% %                  getxy(:,:,i)
%                  plot(s(1),s(2),'r.');
%                  if((i-j)>=30)
%                      break;
%                  end
%                  j=j-1;
%                end
%             num_i=i;
%             flag=1;
%             i=i+30;  
%          else
%              flag=0;
%              i=i+1;
%          end         
%      else
%          flag=0;   
%           i=i+1;
%      end  

     diffs=diffs./norm(diffs);
     angle(i)=atan2(diffs(2),diffs(1));
   
     if(i>1)         
         if((angle(i)-angle(i-1))>2*pi/4)
            angle(i)=angle(i-1)+2*pi/4;
         else
             if((angle(i-1)-angle(i))>2*pi/4)
                 angle(i)=angle(i-1)-2*pi/4;
             end
         end
     end
     if(diffs(1)>0)
        diffs=[1,tan(angle(i))];
     else
         diffs=[-1,-tan(angle(i))];
     end
        
      diffs=diffs./norm(diffs);  
                 
     m1=0.5*u*norm(s-d)^2;                                                     % these lines of code used to draw a point in the surface to describe the path 
     m2=0;
     for j=1:8
         if(tt(:,:,j)~=[1e5,1e5])             
             m2=m2+0.5*h*(1/(norm(s-tt(:,:,j))+ep)-1/p0)^2;
         end
     end
%      figure(1);
%      plot3(s(1),s(2),m1+m2,'ro');
%      hold on;
     figure(2);
     for j=1:11
         for jj=-pi/2:0.05:pi/2
             plot(t(:,1,j)+r*cos(jj),t(:,2,j)+r*sin(jj),'g:')
             hold on;
         end
     end
       
     plot(s(1),s(2),'ro');
     hold on;
     i=i+1;
     s=s-50*diffs;        % update the robot position
     if(norm(s-d)<e)
             last=1;
     end
 end
%  number=i