人工势场算法路径规划演示.zip_visitg8y_人工势场_人工势场法_人工势场算法_路径规划

function varargout = AFM(varargin) % AFM MATLAB code for AFM.fig % AFM, by itself, creates a new AFM or raises the existing % singleton*. % % H = AFM returns the handle to a new AFM or the handle to % the existing singleton*. % % AFM('CALLBACK',hObject,eventData,handles,...) calls the local % function named CALLBACK in AFM.M with the given input arguments. % % AFM('Property','Value',...) creates a new AFM or raises the % existing singleton*. Starting from the left, property value pairs are % applied to the GUI before AFM_OpeningFcn gets called. An % unrecognized property name or invalid value makes property application % stop. All inputs are passed to AFM_OpeningFcn via varargin. % % *See GUI Options on GUIDE's Tools out. Choose "GUI allows only one % instance to run (singleton)". % % See also: GUIDE, GUIDATA, GUIHANDLES % Edit the above text to modify the response to help AFM % Last Modified by GUIDE v2.5 28-Nov-2013 20:49:25 % Begin initialization code - DO NOT EDIT gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @AFM_OpeningFcn, ... 'gui_OutputFcn', @AFM_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end % End initialization code - DO NOT EDIT % --- Executes just before AFM is made visible. function AFM_OpeningFcn(hObject, eventdata, handles, varargin) % This function has no output args, see OutputFcn. % hObject handle to figure % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % varargin command line arguments to AFM (see VARARGIN) % Choose default command line output for AFM handles.output = hObject; % Update handles structure guidata(hObject, handles); % UIWAIT makes AFM wait for user response (see UIRESUME) % uiwait(handles.figure1); % --- Outputs from this function are returned to the command line. function varargout = AFM_OutputFcn(hObject, eventdata, handles) % varargout cell array for returning output args (see VARARGOUT); % hObject handle to figure % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Get default command line output from handles structure varargout{1} = handles.output; % --- Executes during object creation, after setting all properties. function axes4_CreateFcn(hObject, eventdata, handles) Xo=[0 0];%起点位置 k=1;%计算引力需要的增益系数 m=1;%计算斥力的增益系数 %n=3;%障碍个数 longth=1.2;%步长 J=2000;%循环迭代次数 %如果不能实现预期目标，可能也与初始的增益系数，Po设置的不合适有关。 K=0;%初始化 %[m_Obs,m_ObsR]=Obs_Generate([5 10],[20 80],[10 80],n); Xj=Xo;%j=1循环初始，将车的起始坐标赋给Xj axis([-20 120 -20 120]); axis equal; hold on; axis off; %set(gcf,'color','y') %fill([-10,110,110,-10],[-10 -10 110 110],'w') fill([-20,120,120,-20],[-20 -20 120 120],'y') %title ('人工势场路径规划'); text(-5,-5,' Start','FontSize',12); fill([95,120,120,95],[-20 -20 10 10],'w') text(100,5,'Notes:','FontSize',12) plot(101,-5,'sb','markerfacecolor','b'); text(101,-5,' Robot','FontSize',12); plot(101,-15,'om','markerfacecolor','m'); text(101,-15,' Ball','FontSize',12); plot(0,0,'bs') car=plot(0,0,'sb','markerfacecolor','b'); %car_name=text(0,0,' ','FontSize',12); object=plot(0,100,'om','markerfacecolor','m'); %object_name=text(0,100,' Ball','FontSize',12); but=1; x_obs=1; y_obs=1; % hObject handle to axes4 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: place code in OpeningFcn to populate axes4 % --- Executes on button press in pushbutton1. function pushbutton1_Callback(hObject, eventdata, handles) %main %%%%%%%%%%%%%%%%%%%%%初始化参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 Xo=[0 0];%起点位置 k=1;%计算引力需要的增益系数 m=1;%计算斥力的增益系数 %n=3;%障碍个数 longth=1.2;%步长 %循环迭代次数 J=2000; %如果不能实现预期目标，可能也与初始的增益系数，Po设置的不合适有关。 K=0;%初始化 %[m_Obs,m_ObsR]=Obs_Generate([5 10],[20 80],[10 80],n); Xj=Xo;%j=1循环初始，将车的起始坐标赋给Xj axis([-20 120 -20 120]); axis equal; hold on; axis off; %set(gcf,'color','y') %fill([-10,110,110,-10],[-10 -10 110 110],'w') fill([-20,120,120,-20],[-20 -20 120 120],'y') %title ('人工势场路径规划'); text(-5,-5,' Start','FontSize',12); fill([95,120,120,95],[-20 -20 10 10],'w') text(100,5,'Notes:','FontSize',12) plot(101,-5,'sb','markerfacecolor','b'); text(101,-5,' Robot','FontSize',12); plot(101,-15,'om','markerfacecolor','m'); text(101,-15,' Ball','FontSize',12); plot(0,0,'bs') car=plot(0,0,'sb','markerfacecolor','b'); %car_name=text(0,0,' ','FontSize',12); object=plot(0,100,'om','markerfacecolor','m'); %object_name=text(0,100,' Ball','FontSize',12); but=1; x_obs=1; y_obs=1; %pause(1); %选取障碍 %h=msgbox('单击鼠标左键选择障碍，单击右键完成选择','提示'); %uiwait(h,10); %if ishandle(h) == 1 % delete(h); %end num_obs=1; while but == 1 [x_obs,y_obs,but] = ginput(1); if but==1 m_Obs(num_obs,:)=[x_obs y_obs]; m_ObsR(num_obs)=3; %m_Obs=[0 40;20 70;60 50]; Po=min(m_ObsR)/0.5; % for i=1:n Theta=0:pi/20:pi; xx =m_Obs(num_obs,1)+cos(Theta)*m_ObsR(num_obs); yy= m_Obs(num_obs,2)+sin(Theta)*m_ObsR(num_obs); fill(xx,yy,'w') Theta=pi:pi/20:2*pi; xx =m_Obs(num_obs,1)+cos(Theta)*m_ObsR(num_obs); yy= m_Obs(num_obs,2)+sin(Theta)*m_ObsR(num_obs); fill(xx,yy,'k') num_obs=num_obs+1; end %plot(xx,yy,'LineWidth',2); % xval=floor(xval); % yval=floor(yval); % MAP(xval,yval)=-1;%Put on the closed list as well % plot(xval+.5,yval+.5,'ro'); end%End of While loop num_obs=num_obs-1; %object=plot(0,100,'om'); %car=plot(Xo(1),Xo(2),'sb'); %car_name=text(Xo(1),Xo(2),'Robot','FontSize',12); %object_name=text(0,100,'Ball','FontSize',12); %startFlag=pushbutton2_Callback(hObject, eventdata, handles); uiwait; %***************初始化结束，开始主体循环****************** for j=1:J%循环开始 % if j<200 x=j/1.5;y=100; % else % x=100;y=100; %end m_Target=[x,y]; Goal(j,1)=m_Target(1); Goal(j,2)=m_Target(2); Current(j,1)=Xj(1);%Goal保存走过的每个点的坐标。刚开始先将起点放进该向量 Current(j,2)=Xj(2); %调用计算角度模块 [angle_att,angle_rep]=compute_angle(Xj,m_Target,m_Obs,num_obs); %调用计算引力模块 [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,num_obs,Po); %计算合力和方向 [Position_angle(j)]=compute_Ftotal(Fatt,Frep,Fatt_add,num_obs); %计算车的下一步位置 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的条件，重新返回循环，继续执行。 if x>=100&&Is_Reach(Xj,m_Target,longth)==1 break; end X=Current(j,1); Y=Current(j,2); A=Goal(j,1); B=Goal(j,2); delete(car); car=plot(X,Y,'sb','markerfacecolor','b'); %v=plot(X,Y,'ro','linewidth',2); %set(v,'Color',[1,0,0]) %plot(Xo(1),Xo(2),'ms'); delete(object); object=plot(A,B,'om','markerfacecolor','m'); %delete(car_name); %car_name=text(X,Y,' Robot','FontSize',12); %delete(object_name); %object_name=text(A,B,' Ball','FontSize',12); pause(0.05) end%大循环结束 %***