倒车入库.rar

clc; % clear; close all; l = 26; %轴距-mm w = 15; %轮距 -mm v = -20; %车速- m/s delta_t =0.2; %采样时间间隔- second %第一组试验值 x=140; %汽车起点的 x 坐标 y=45; %汽车起点的 y 坐标 theta=0; %汽车车身起始倾角 phi=0; %max 45 degree %第二组试验值 %x = 35; %汽车起点的 x 坐标 %y = 45; %汽车起点的 y 坐标 %theta = 0; %汽车车身起始倾角 %phi = 45; %max 45 degree % %第三组试验值 %x = 58; %汽车起点的 x 坐标 %y = 60; %汽车起点的 y 坐标 %theta = 45; %汽车车身起始倾角 %phi = 0; %max 45 degree % %第四组试验值 %x = 90; %汽车起点的 x 坐标 %y = 70; %汽车起点的 y 坐标 %theta = -45; %汽车车身起始倾角 %phi = 0; %max 45 degree delta_x=v*cos(phi)*cos(theta)*(delta_t); delta_y=v*sin(theta)*cos(phi)*(delta_t); delta_theta=[v*sin(phi)/l]*(delta_t); x_seq=zeros(1,1); y_seq=zeros(1,1); theta_seq=zeros(1,1); phi_seq=zeros(1,1); figure; %画空白图片 %画车位示意图 h1 = line([-20 40], [60 60]); h2 = line([40 40], [60 85]); h3 = line([40 105], [85 85]); h4 = line([105 105], [60 85]); h5 = line([105 160], [60 60]); set(h1, 'linewidth', 5, 'color', 'b'); set(h2, 'linewidth', 5, 'color', 'b'); set(h3, 'linewidth', 5, 'color', 'b'); set(h4, 'linewidth', 5, 'color', 'b'); set(h5, 'linewidth', 5, 'color', 'b'); axis([-50 160 0 100]); xlabel('x - cm'); ylabel('y - cm'); title('Parallel Parking '); hold on grid on; % pause(1); for i = 1:1500 phi=(0-theta)*K(2)+(70-y)*K(1); delta_x=v*cos(phi)*cos(theta)*(delta_t); delta_y=v*sin(theta)*cos(phi)*(delta_t); delta_theta=[v*sin(phi)/l]*(delta_t); pause(0.1); x=x+delta_x; %计算新的 x 坐标 y=y+delta_y; %计算新的 y 坐标 theta=theta+delta_theta; %计算新的车身倾角 x_seq(i) = x; %存储 x 坐标到 x 序列 y_seq(i) = y; %存储 y 坐标到 y 序列 theta_seq(i) = theta; %存储 theta 坐标到 theta 序列 phi_seq(i) = phi; %存储 phi 坐标到 phi 序列 %fprintf('x = %f, y = %f, theta = %f, phi = %f, dx = %f, dy = %f\n', x, y,theta, phi, delta_x, delta_y); x0 = x + w/2*sin(theta); %左前角 x 坐标 y0 = y - w/2*cos(theta); %左前角 y 坐标 x1 = x - w/2*sin(theta); %右前角 x 坐标 y1 = y + w/2*cos(theta); %右前角 y 坐标 p = x - l*cos(theta); q = y - l*sin(theta); x2 = p + w/2*sin(theta); %左后角 x 坐标 y2 = q - w/2*cos(theta); %左后角 y 坐标 x3 = p - w/2*sin(theta); %右后角 x 坐标 y3 = q + w/2*cos(theta); %右后角 y 坐标 %画汽车后轴中心 plot (x, y, 'rs'); %画车身轮廓 l0 = line([x0 x1], [y0 y1]); l1 = line([x1 x3], [y1 y3]); l2 = line([x2 x3], [y2 y3]); l3 = line([x0 x2], [y0 y2]); set(l0, 'linewidth', 4, 'color', 'm'); set(l1, 'linewidth', 2, 'color', 'b'); set(l2, 'linewidth', 4, 'color', 'g'); set(l3, 'linewidth', 2, 'color', 'b'); if (x<86) break; end end