【运动学】不等半径泊车方法matlab仿真

1 简介

自动泊车路径的规划属于从低约束到高约束的问题,也属于非完整约束问题.一般的路径规划总是不尽如人意,对泊车条件要求高.根据一般问题的解决从高约束到低约束的过程,在最短路径的基础上,先确定高约束阶段泊车车辆进入泊车位的路径,计算出停车位长度与直线斜率的拟合公式,得到直线的斜率,再改变泊车半径,确定低约束段的泊车路径.利用车辆运动学模型,对车辆位置和姿态的分析,碰撞点到车辆的距离计算,证明了路径规划的可行性,生成的泊车轨迹比较符合泊车实际情况.​

2 完整代码

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((width/2)/(long-qianxuan-houxuan));%以下描写车的四个轮子

    jiao6=jiao5;

    jiao5=theta-jiao5;

    jiao6=theta+jiao6;

    jiao7=theta+(pi/2);

    jiao8=theta-(pi/2);

    

    jiao9=theta+fai1;

    jiaoa=theta;

    

    r3=sqrt((width/2)^2+(long-qianxuan-houxuan)^2);

    r4=width/2;

    

    yqianlunzx=x+r3*cos(jiao5);

    yqianlunzy=y+r3*sin(jiao5);

    yqianlunqx=yqianlunzx+lunjin*cos(jiao9);

    yqianlunqy=yqianlunzy+lunjin*sin(jiao9);

    yqianlunhx=yqianlunzx-lunjin*cos(jiao9);

    yqianlunhy=yqianlunzy-lunjin*sin(jiao9);

    h21=plot([yqianlunqx,yqianlunhx],[yqianlunqy,yqianlunhy],'-k');

    

    zqianlunzx=x+r3*cos(jiao6);

    zqianlunzy=y+r3*sin(jiao6);

    zqianlunqx=zqianlunzx+lunjin*cos(jiao9);

    zqianlunqy=zqianlunzy+lunjin*sin(jiao9);

    zqianlunhx=zqianlunzx-lunjin*cos(jiao9);

    zqianlunhy=zqianlunzy-lunjin*sin(jiao9);

    h22=plot([zqianlunqx,zqianlunhx],[zqianlunqy,zqianlunhy],'-k');

    

    zhoulunzx=x+r4*cos(jiao7);

    zhoulunzy=y+r4*sin(jiao7);

    zhoulunqx=zhoulunzx+lunjin*cos(jiaoa);

    zhoulunqy=zhoulunzy+lunjin*sin(jiaoa);

    zhoulunhx=zhoulunzx-lunjin*cos(jiaoa);

    zhoulunhy=zhoulunzy-lunjin*sin(jiaoa);

    h23=plot([zhoulunqx,zhoulunhx],[zhoulunqy,zhoulunhy],'-k');

    

    yhoulunzx=x+r4*cos(jiao8);

    yhoulunzy=y+r4*sin(jiao8);

    yhoulunqx=yhoulunzx+lunjin*cos(jiaoa);

    yhoulunqy=yhoulunzy+lunjin*sin(jiaoa);

    yhoulunhx=yhoulunzx-lunjin*cos(jiaoa);

    yhoulunhy=yhoulunzy-lunjin*sin(jiaoa);

    h24=plot([yhoulunqx,yhoulunhx],[yhoulunqy,yhoulunhy],'-k');

    

    

    %在此添加判决函数，判断是否第一阶段会碰到障碍物

     if(yhoulunzx-x6>=0 & yhoulunzy-y6>=0)

         fprintf('第一阶段碰到障碍物，不等半径泊车方法无法泊车n');

         delete(h17);

    delete(h18);

    delete(h19);

    delete(h20);

    delete(h21);

    delete(h22);

    delete(h23);

    delete(h24);

         return;

     end

    

    

    pause(0.01);

    

    delete(h17);

    delete(h18);

    delete(h19);

    delete(h20);

    delete(h21);

    delete(h22);

    delete(h23);

    delete(h24);

  

    end

    %第二阶段轨迹

    x2=xd-r;

    y2=yd;

    fai2=-fai;

    for i=t3:(-ds):0

    x=x2+r*cos(i);

    y=y2+r*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);

    

    %在此添加判决函数，判断是否第二阶段会碰到障碍物

     if(youqianx-x6>=0 & youqiany-y6>=0)

         fprintf('第二阶段碰到障碍物，不等半径泊车方法无法泊车n');

        return;

     end

    

    zuohoux=x+r2*cos(jiao3);

    zuohouy=y+r2*sin(jiao3);

    

    youhoux=x+r2*cos(jiao4);

    youhouy=y+r2*sin(jiao4);

    

    h25=plot([youqianx,zuoqianx],[youqiany,zuoqiany],'-b');

    h26=plot([zuoqianx,zuohoux],[zuoqiany,zuohouy],'-b');

    h27=plot([zuohoux,youhoux],[zuohouy,youhouy],'-b');

    h28=plot([youhoux,youqianx],[youhouy,youqiany],'-b');

    

    jiao5=atan((width/2)/(long-qianxuan-houxuan));%以下描写车的四个轮子

    jiao6=jiao5;

    jiao5=theta-jiao5;

    jiao6=theta+jiao6;

    jiao7=theta+(pi/2);

    jiao8=theta-(pi/2);

    

    jiao9=theta+fai2;

    jiaoa=theta;

    

    r3=sqrt((width/2)^2+(long-qianxuan-houxuan)^2);

    r4=width/2;

    

    yqianlunzx=x+r3*cos(jiao5);

    yqianlunzy=y+r3*sin(jiao5);

    yqianlunqx=yqianlunzx+lunjin*cos(jiao9);

    yqianlunqy=yqianlunzy+lunjin*sin(jiao9);

    yqianlunhx=yqianlunzx-lunjin*cos(jiao9);

    yqianlunhy=yqianlunzy-lunjin*sin(jiao9);

    h29=plot([yqianlunqx,yqianlunhx],[yqianlunqy,yqianlunhy],'-k');

    

    zqianlunzx=x+r3*cos(jiao6);

    zqianlunzy=y+r3*sin(jiao6);

    zqianlunqx=zqianlunzx+lunjin*cos(jiao9);

    zqianlunqy=zqianlunzy+lunjin*sin(jiao9);

    zqianlunhx=zqianlunzx-lunjin*cos(jiao9);

    zqianlunhy=zqianlunzy-lunjin*sin(jiao9);

    h30=plot([zqianlunqx,zqianlunhx],[zqianlunqy,zqianlunhy],'-k');

    

    zhoulunzx=x+r4*cos(jiao7);

    zhoulunzy=y+r4*sin(jiao7);

    zhoulunqx=zhoulunzx+lunjin*cos(jiaoa);

    zhoulunqy=zhoulunzy+lunjin*sin(jiaoa);

    zhoulunhx=zhoulunzx-lunjin*cos(jiaoa);

    zhoulunhy=zhoulunzy-lunjin*sin(jiaoa);

    h31=plot([zhoulunqx,zhoulunhx],[zhoulunqy,zhoulunhy],'-k');

    

    yhoulunzx=x+r4*cos(jiao8);

    yhoulunzy=y+r4*sin(jiao8);

    yhoulunqx=yhoulunzx+lunjin*cos(jiaoa);

    yhoulunqy=yhoulunzy+lunjin*sin(jiaoa);

    yhoulunhx=yhoulunzx-lunjin*cos(jiaoa);

    yhoulunhy=yhoulunzy-lunjin*sin(jiaoa);

    h32=plot([yhoulunqx,yhoulunhx],[yhoulunqy,yhoulunhy],'-k');

    

    pause(0.01);

    

     delete(h25); 

     delete(h26);

     delete(h27);

     delete(h28);

     delete(h29);

     delete(h30);

     delete(h31);

     delete(h32);

            

    end

    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);

    

    %在此添加判决函数，判断是否第二阶段会碰到障碍物

     if(youqianx-x6>=0 & youqiany-y6>=0)

         fprintf('第二阶段碰到障碍物，不等半径泊车方法无法泊车n');

        return;

     end

    

    zuohoux=x+r2*cos(jiao3);

    zuohouy=y+r2*sin(jiao3);

    

    youhoux=x+r2*cos(jiao4);

    youhouy=y+r2*sin(jiao4);

    

    h25=plot([youqianx,zuoqianx],[youqiany,zuoqiany],'-b');

    h26=plot([zuoqianx,zuohoux],[zuoqiany,zuohouy],'-b');

    h27=plot([zuohoux,youhoux],[zuohouy,youhouy],'-b');

    h28=plot([youhoux,youqianx],[youhouy,youqiany],'-b');

    

    jiao5=atan((width/2)/(long-qianxuan-houxuan));%以下描写车的四个轮子

    jiao6=jiao5;

    jiao5=theta-jiao5;

    jiao6=theta+jiao6;

    jiao7=theta+(pi/2);

    jiao8=theta-(pi/2);

    

    jiao9=theta+fai2;

    jiaoa=theta;

    

    r3=sqrt((width/2)^2+(long-qianxuan-houxuan)^2);

    r4=width/2;

    

    yqianlunzx=x+r3*cos(jiao5);

    yqianlunzy=y+r3*sin(jiao5);

    yqianlunqx=yqianlunzx+lunjin*cos(jiao9);

    yqianlunqy=yqianlunzy+lunjin*sin(jiao9);

    yqianlunhx=yqianlunzx-lunjin*cos(jiao9);

    yqianlunhy=yqianlunzy-lunjin*sin(jiao9);

    h29=plot([yqianlunqx,yqianlunhx],[yqianlunqy,yqianlunhy],'-k');

    

    zqianlunzx=x+r3*cos(jiao6);

    zqianlunzy=y+r3*sin(jiao6);

    zqianlunqx=zqianlunzx+lunjin*cos(jiao9);

    zqianlunqy=zqianlunzy+lunjin*sin(jiao9);

    zqianlunhx=zqianlunzx-lunjin*cos(jiao9);

    zqianlunhy=zqianlunzy-lunjin*sin(jiao9);

    h30=plot([zqianlunqx,zqianlunhx],[zqianlunqy,zqianlunhy],'-k');

    

    zhoulunzx=x+r4*cos(jiao7);

    zhoulunzy=y+r4*sin(jiao7);

    zhoulunqx=zhoulunzx+lunjin*cos(jiaoa);

    zhoulunqy=zhoulunzy+lunjin*sin(jiaoa);

    zhoulunhx=zhoulunzx-lunjin*cos(jiaoa);

    zhoulunhy=zhoulunzy-lunjin*sin(jiaoa);

    h31=plot([zhoulunqx,zhoulunhx],[zhoulunqy,zhoulunhy],'-k');

    

    yhoulunzx=x+r4*cos(jiao8);

    yhoulunzy=y+r4*sin(jiao8);

    yhoulunqx=yhoulunzx+lunjin*cos(jiaoa);

    yhoulunqy=yhoulunzy+lunjin*sin(jiaoa);

    yhoulunhx=yhoulunzx-lunjin*cos(jiaoa);

    yhoulunhy=yhoulunzy-lunjin*sin(jiaoa);

    h32=plot([yhoulunqx,yhoulunhx],[yhoulunqy,yhoulunhy],'-k');

    

    pause(0.01);

    

    s1=t3*rr1;

    s2=t3*r;    

    fprintf('s1=%f,s2=%fn',s1,s2);

% --- Executes on button press in pushbutton3.

function pushbutton3_Callback(hObject, eventdata, handles)

cla reset;

clear all;

% --- Executes on button press in pushbutton4.

function pushbutton4_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<0)

        fprintf('轨迹无解，不等半径泊车方法无法泊车n');

        return;

    end

    x1=x0+rr1;

    y1=y0;

    fai1=fai;

    %第一阶段轨迹

    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((width/2)/(long-qianxuan-houxuan));%以下描写车的四个轮子

    jiao6=jiao5;

    jiao5=theta-jiao5;

    jiao6=theta+jiao6;

    jiao7=theta+(pi/2);

    jiao8=theta-(pi/2);

    

    jiao9=theta+fai1;

    jiaoa=theta;

    

    r3=sqrt((width/2)^2+(long-qianxuan-houxuan)^2);

    r4=width/2;

    

    yqianlunzx=x+r3*cos(jiao5);

    yqianlunzy=y+r3*sin(jiao5);

    yqianlunqx=yqianlunzx+lunjin*cos(jiao9);

    yqianlunqy=yqianlunzy+lunjin*sin(jiao9);

    yqianlunhx=yqianlunzx-lunjin*cos(jiao9);

    yqianlunhy=yqianlunzy-lunjin*sin(jiao9);

    h21=plot([yqianlunqx,yqianlunhx],[yqianlunqy,yqianlunhy],'-k');

    

    zqianlunzx=x+r3*cos(jiao6);

    zqianlunzy=y+r3*sin(jiao6);

    zqianlunqx=zqianlunzx+lunjin*cos(jiao9);

    zqianlunqy=zqianlunzy+lunjin*sin(jiao9);

    zqianlunhx=zqianlunzx-lunjin*cos(jiao9);

    zqianlunhy=zqianlunzy-lunjin*sin(jiao9);

    h22=plot([zqianlunqx,zqianlunhx],[zqianlunqy,zqianlunhy],'-k');

    

    zhoulunzx=x+r4*cos(jiao7);

    zhoulunzy=y+r4*sin(jiao7);

    zhoulunqx=zhoulunzx+lunjin*cos(jiaoa);

    zhoulunqy=zhoulunzy+lunjin*sin(jiaoa);

    zhoulunhx=zhoulunzx-lunjin*cos(jiaoa);

    zhoulunhy=zhoulunzy-lunjin*sin(jiaoa);

    h23=plot([zhoulunqx,zhoulunhx],[zhoulunqy,zhoulunhy],'-k');

    

    yhoulunzx=x+r4*cos(jiao8);

    yhoulunzy=y+r4*sin(jiao8);

    yhoulunqx=yhoulunzx+lunjin*cos(jiaoa);

    yhoulunqy=yhoulunzy+lunjin*sin(jiaoa);

    yhoulunhx=yhoulunzx-lunjin*cos(jiaoa);

    yhoulunhy=yhoulunzy-lunjin*sin(jiaoa);

    h24=plot([yhoulunqx,yhoulunhx],[yhoulunqy,yhoulunhy],'-k');

    

    

    %在此添加判决函数，判断是否第一阶段会碰到障碍物

     if(yhoulunzx-x6>=0 & yhoulunzy-y6>=0)

         fprintf('第一阶段碰到障碍物，不等半径泊车方法无法泊车n');

         delete(h17);

    delete(h18);

    delete(h19);

    delete(h20);

    delete(h21);

    delete(h22);

    delete(h23);

    delete(h24);

         return;

     end

    

    

    pause(0.01);

    

    end

    %第二阶段轨迹

    x2=xd-r;

    y2=yd;

    fai2=-fai;

    for i=t3:(-ds):0

    x=x2+r*cos(i);

    y=y2+r*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);

    

    %在此添加判决函数，判断是否第二阶段会碰到障碍物

     if(youqianx-x6>=0 & youqiany-y6>=0)

         fprintf('第二阶段碰到障碍物，不等半径泊车方法无法泊车n');

        return;

     end

    

    zuohoux=x+r2*cos(jiao3);

    zuohouy=y+r2*sin(jiao3);

    

    youhoux=x+r2*cos(jiao4);

    youhouy=y+r2*sin(jiao4);

    

    h25=plot([youqianx,zuoqianx],[youqiany,zuoqiany],'-b');

    h26=plot([zuoqianx,zuohoux],[zuoqiany,zuohouy],'-b');

    h27=plot([zuohoux,youhoux],[zuohouy,youhouy],'-b');

    h28=plot([youhoux,youqianx],[youhouy,youqiany],'-b');

    

    jiao5=atan((width/2)/(long-qianxuan-houxuan));%以下描写车的四个轮子

    jiao6=jiao5;

    jiao5=theta-jiao5;

    jiao6=theta+jiao6;

    jiao7=theta+(pi/2);

    jiao8=theta-(pi/2);

    

    jiao9=theta+fai2;

    jiaoa=theta;

    

    r3=sqrt((width/2)^2+(long-qianxuan-houxuan)^2);

    r4=width/2;

    

    yqianlunzx=x+r3*cos(jiao5);

    yqianlunzy=y+r3*sin(jiao5);

    yqianlunqx=yqianlunzx+lunjin*cos(jiao9);

    yqianlunqy=yqianlunzy+lunjin*sin(jiao9);

    yqianlunhx=yqianlunzx-lunjin*cos(jiao9);

    yqianlunhy=yqianlunzy-lunjin*sin(jiao9);

    h29=plot([yqianlunqx,yqianlunhx],[yqianlunqy,yqianlunhy],'-k');

    

    zqianlunzx=x+r3*cos(jiao6);

    zqianlunzy=y+r3*sin(jiao6);

    zqianlunqx=zqianlunzx+lunjin*cos(jiao9);

    zqianlunqy=zqianlunzy+lunjin*sin(jiao9);

    zqianlunhx=zqianlunzx-lunjin*cos(jiao9);

    zqianlunhy=zqianlunzy-lunjin*sin(jiao9);

    h30=plot([zqianlunqx,zqianlunhx],[zqianlunqy,zqianlunhy],'-k');

    

    zhoulunzx=x+r4*cos(jiao7);

    zhoulunzy=y+r4*sin(jiao7);

    zhoulunqx=zhoulunzx+lunjin*cos(jiaoa);

    zhoulunqy=zhoulunzy+lunjin*sin(jiaoa);

    zhoulunhx=zhoulunzx-lunjin*cos(jiaoa);

    zhoulunhy=zhoulunzy-lunjin*sin(jiaoa);

    h31=plot([zhoulunqx,zhoulunhx],[zhoulunqy,zhoulunhy],'-k');

    

    yhoulunzx=x+r4*cos(jiao8);

    yhoulunzy=y+r4*sin(jiao8);

    yhoulunqx=yhoulunzx+lunjin*cos(jiaoa);

    yhoulunqy=yhoulunzy+lunjin*sin(jiaoa);

    yhoulunhx=yhoulunzx-lunjin*cos(jiaoa);

    yhoulunhy=yhoulunzy-lunjin*sin(jiaoa);

    h32=plot([yhoulunqx,yhoulunhx],[yhoulunqy,yhoulunhy],'-k');

    

    pause(0.01);

    

            

    end

    

    s1=t3*rr1;

    s2=t3*r;    

    fprintf('s1=%f,s2=%fn',s1,s2);

function edit1_Callback(hObject, eventdata, handles)

% hObject    handle to edit1 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit1 as text

%        str2double(get(hObject,'String')) returns contents of edit1 as a double

% --- Executes during object creation, after setting all properties.

function edit1_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit1 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit2_Callback(hObject, eventdata, handles)

% hObject    handle to edit2 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit2 as text

%        str2double(get(hObject,'String')) returns contents of edit2 as a double

% --- Executes during object creation, after setting all properties.

function edit2_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit2 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit3_Callback(hObject, eventdata, handles)

% hObject    handle to edit3 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit3 as text

%        str2double(get(hObject,'String')) returns contents of edit3 as a double

% --- Executes during object creation, after setting all properties.

function edit3_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit3 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit4_Callback(hObject, eventdata, handles)

% hObject    handle to edit4 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit4 as text

%        str2double(get(hObject,'String')) returns contents of edit4 as a double

% --- Executes during object creation, after setting all properties.

function edit4_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit4 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit5_Callback(hObject, eventdata, handles)

% hObject    handle to edit4 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit4 as text

%        str2double(get(hObject,'String')) returns contents of edit4 as a double

% --- Executes during object creation, after setting all properties.

function edit5_CreateFcn(hObject, eventdata, handles)

set(hObject,'string','0.31')

% hObject    handle to edit4 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit6_Callback(hObject, eventdata, handles)

% hObject    handle to edit6 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit6 as text

%        str2double(get(hObject,'String')) returns contents of edit6 as a double

% --- Executes during object creation, after setting all properties.

function edit6_CreateFcn(hObject, eventdata, handles)

set(hObject,'string','0.4870');

% hObject    handle to edit6 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

    

end

function edit7_Callback(hObject, eventdata, handles)

% hObject    handle to edit7 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit7 as text

%        str2double(get(hObject,'String')) returns contents of edit7 as a double

% --- Executes during object creation, after setting all properties.

function edit7_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit7 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit14_Callback(hObject, eventdata, handles)

% hObject    handle to edit14 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit14 as text

%        str2double(get(hObject,'String')) returns contents of edit14 as a double

% --- Executes during object creation, after setting all properties.

function edit14_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit14 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit15_Callback(hObject, eventdata, handles)

% hObject    handle to edit15 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit15 as text

%        str2double(get(hObject,'String')) returns contents of edit15 as a double

% --- Executes during object creation, after setting all properties.

function edit15_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit15 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit12_Callback(hObject, eventdata, handles)

% hObject    handle to edit12 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit12 as text

%        str2double(get(hObject,'String')) returns contents of edit12 as a double

% --- Executes during object creation, after setting all properties.

function edit12_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit12 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit13_Callback(hObject, eventdata, handles)

% hObject    handle to edit13 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit13 as text

%        str2double(get(hObject,'String')) returns contents of edit13 as a double

% --- Executes during object creation, after setting all properties.

function edit13_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit13 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit10_Callback(hObject, eventdata, handles)

% hObject    handle to edit10 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit10 as text

%        str2double(get(hObject,'String')) returns contents of edit10 as a double

% --- Executes during object creation, after setting all properties.

function edit10_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit10 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit11_Callback(hObject, eventdata, handles)

% hObject    handle to edit11 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit11 as text

%        str2double(get(hObject,'String')) returns contents of edit11 as a double

% --- Executes during object creation, after setting all properties.

function edit11_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit11 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit8_Callback(hObject, eventdata, handles)

% hObject    handle to edit8 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit8 as text

%        str2double(get(hObject,'String')) returns contents of edit8 as a double

% --- Executes during object creation, after setting all properties.

function edit8_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit8 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

function edit9_Callback(hObject, eventdata, handles)

% hObject    handle to edit9 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit9 as text

%        str2double(get(hObject,'String')) returns contents of edit9 as a double

% --- Executes during object creation, after setting all properties.

function edit9_CreateFcn(hObject, eventdata, handles)

% hObject    handle to edit9 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.

%       See ISPC and COMPUTER.

if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

    set(hObject,'BackgroundColor','white');

end

% --- Executes on mouse press over axes background.

function axes1_ButtonDownFcn(hObject, eventdata, handles)

% hObject    handle to axes1 (see GCBO)

% eventdata  reserved - to be defined in a future version of MATLAB

% handles    structure with handles and user data (see GUIDATA)

3 仿真结果

4 参考文献

[1]王龙, 朱凯. 基于最短路径的变半径泊车路径规划研究[J]. 汽车科技, 2015(6):7.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。

最后

以上就是醉熏朋友为你收集整理的【运动学】不等半径泊车方法matlab仿真的全部内容，希望文章能够帮你解决【运动学】不等半径泊车方法matlab仿真所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。