【运动学】 GUI不等半径泊车方法仿真【含Matlab源码 691期】.zip

function varargout = budengbanjinbochefangzhen(varargin) % BUDENGBANJINBOCHEFANGZHEN M-file for budengbanjinbochefangzhen.fig % BUDENGBANJINBOCHEFANGZHEN, by itself, creates a new BUDENGBANJINBOCHEFANGZHEN or raises the existing % singleton*. % % H = BUDENGBANJINBOCHEFANGZHEN returns the handle to a new BUDENGBANJINBOCHEFANGZHEN or the handle to % the existing singleton*. % % BUDENGBANJINBOCHEFANGZHEN('CALLBACK',hObject,eventData,handles,...) calls the local % function named CALLBACK in BUDENGBANJINBOCHEFANGZHEN.M with the given input arguments. % % BUDENGBANJINBOCHEFANGZHEN('Property','Value',...) creates a new BUDENGBANJINBOCHEFANGZHEN or raises the % existing singleton*. Starting from the left, property value pairs are % applied to the GUI before budengbanjinbochefangzhen_OpeningFcn gets called. An % unrecognized property name or invalid value makes property application % stop. All inputs are passed to budengbanjinbochefangzhen_OpeningFcn via varargin. % % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one % instance to run (singleton)". % % See also: GUIDE, GUIDATA, GUIHANDLES % Edit the above text to modify the response to help budengbanjinbochefangzhen % Last Modified by GUIDE v2.5 14-Sep-2010 11:41:20 % Begin initialization code - DO NOT EDIT gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @budengbanjinbochefangzhen_OpeningFcn, ... 'gui_OutputFcn', @budengbanjinbochefangzhen_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 budengbanjinbochefangzhen is made visible. function budengbanjinbochefangzhen_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 budengbanjinbochefangzhen (see VARARGIN) % Choose default command line output for budengbanjinbochefangzhen handles.output = hObject; % Update handles structure guidata(hObject, handles); % UIWAIT makes budengbanjinbochefangzhen wait for user response (see UIRESUME) % uiwait(handles.figure1); % --- Outputs from this function are returned to the command line. function varargout = budengbanjinbochefangzhen_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 on button press in pushbutton1. function pushbutton1_Callback(hObject, eventdata, handles) global long;% 使这个变量变成全局变量 global width;% 使这个变量变成全局变量 global qianxuan;% 使这个变量变成全局变量 global houxuan;% 使这个变量变成全局变量 global lunjin;% 使这个变量变成全局变量 global fai;% 使这个变量变成全局变量 global x6;% 使这个变量变成全局变量 global y6;% 使这个变量变成全局变量 global x7;% 使这个变量变成全局变量 global y7;% 使这个变量变成全局变量 global x8;% 使这个变量变成全局变量 global y8;% 使这个变量变成全局变量 global x9;% 使这个变量变成全局变量 global y9;% 使这个变量变成全局变量 global ds;% 使这个变量变成全局变量 global r;% 使这个变量变成全局变量 global xd;% 使这个变量变成全局变量 global yd;% 使这个变量变成全局变量 axis([-10,20,-10,20]); axis manual; hold on; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %参数集，这里存放的为车辆与车位已经仿真所需的主要参数 %手动输入参数集 long=str2double(get(handles.edit1,'string')); width=str2double(get(handles.edit2,'string')); qianxuan=str2double(get(handles.edit3,'string')); houxuan=str2double(get(handles.edit4,'string')); lunjin=str2double(get(handles.edit5,'string')); fai=-str2double(get(handles.edit6,'string')); x6=str2double(get(handles.edit8,'string')); y6=str2double(get(handles.edit9,'string')); x7=str2double(get(handles.edit10,'string')); y7=str2double(get(handles.edit11,'string')); x9=str2double(get(handles.edit12,'string')); y9=str2double(get(handles.edit13,'string')); x8=str2double(get(handles.edit14,'string')); y8=str2double(get(handles.edit15,'string')); ds=str2double(get(handles.edit7,'string')); % long=6;%long 为车车长 % width=3;%width 为车宽 % qianxuan=1;%qianxuan 为车前悬长度 % houxuan=1;% houxuan 为车后悬长度 % lunjin=0.3;%lunjin为车轮半径 % fai=-(pi)/5;%fai 为前一段轨迹的车前轮偏角 % % % x6=11; % y6=15; % x7=18; % y7=15; % x8=18; % y8=0; % x9=11; % y9=0; % % ds=0.3; %仿真步长 %自动生成参数集 r=(long-qianxuan-houxuan)/abs(tan(fai));% 车辆的后轴中心点转弯半径 %theta0=(pi)/2;%theta0为起始点车身偏角 xd=(x8+x9)/2;%终点坐标 yd=(y8+y9)/2+1.5; h0=plot(xd,yd,'bo'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %画车位 %(x6,y6)、（x7,y7）、(x8,y8)、(x9,y9)分别为车位的四个端点坐标 %根据车位的四个端点画出车位的轮廓 l0=plot([x6,x7],[y6,y7],'-k'); l1=plot([x7,x8],[y7,y8],'-k'); l2=plot([x8,x9],[y8,y9],'-k'); % --- Executes on button press in pushbutton2. function pushbutton2_Callback(hObject, eventdata, handles) global long;% 使这个变量变成全局变量 global width;% 使这个变量变成全局变量 global qianxuan;% 使这个变量变成全局变量 global houxuan;% 使这个变量变成全局变量 global lunjin;% 使这个变量变成全局变量 global fai;% 使这个变量变成全局变量 global x6;% 使这个变量变成全局变量 global y6;% 使这个变量变成全局变量 global x7;% 使这个变量变成全局变量 global y7;% 使这个变量变成全局变量 global x8;% 使这个变量变成全局变量 global y8;% 使这个变量变成全局变量 global x9;% 使这个变量变成全局变量 global y9;% 使这个变量变成全局变量 global ds;% 使这个变量变成全局变量 global r;% 使这个变量变成全局变量 global xd;% 使这个变量变成全局变量 global yd;% 使这个变量变成全局变量 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %画轨迹,求t1,t2 [x0,y0] = ginput(1);%获取起点坐标 plot(x0,y0,'bo'); %原方法的两个参数 rr1=(((xd-x0)^2+(yd-y0)^2)/(2*(xd-x0)))-r; if(x0+rr1-xd+r>=0) t3=asin((y0-yd)/(rr1+r)); else t3=asin((yd-y0)/(rr1+r))+(pi); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %原方法的轨迹 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% if(rr1<r) fprintf('轨迹无解，不等半径泊车方法无法泊车\n'); return; end x1=x0+rr1; y1=y0; fai1=-atan((long-qianxuan-houxuan)/rr1); %第一阶段轨迹 for i=(pi):(ds*r/rr1):(pi+t3); x=x1+rr1*cos(i); y=y1+rr1*sin(i); theta=i-(pi/2); jiao1=atan((width/2)/(long-houxuan)); jiao2=jiao1; jiao3=atan((width/2)/houxuan); jiao4=jiao3; jiao1=theta-jiao1; jiao2=theta+jiao2; jiao3=theta+pi-jiao3; jiao4=theta+pi+jiao4; r1=sqrt((width/2)^2+(long-houxuan)^2);%以下描述车身的四个端点 r2=sqrt((width/2)^2+houxuan^2); youqianx=x+r1*cos(jiao1); youqiany=y+r1*sin(jiao1); zuoqianx=x+r1*cos(jiao2); zuoqiany=y+r1*sin(jiao2); zuohoux=x+r2*cos(jiao3); zuohouy=y+r2*sin(jiao3); youhoux=x+r2*cos(jiao4); youhouy=y+r2*sin(jiao4); h17=plot([youqianx,zuoqianx],[youqiany,zuoqiany],'-b'); h18=plot([zuoqianx,zuohoux],[zuoqiany,zuohouy],'-b'); h19=plot([zuohoux,youhoux],[zuohouy,youhouy],'-b'); h20=plot([youhoux,youqianx],[youhouy,youqiany],'-b'); jiao5=atan(