自动泊车

自动泊车的matlab代码，亲测可用，已经调好，使用了模糊控制

clc;
clear;
close all;
l = 25; %车长 - cm
w = 20; %车宽 - cm
v = 5; %车速 - cm
Ts =0.4; %采样时间间隔 - second
N=2000;
fuzzyfis=readfis（‘car_parking.fis‘）;
%第一组试验值
x（1） = 10; %汽车起点的x坐标
y（1） = 30; %汽车起点的y坐标
phi（1） =-30; %汽车车身起始倾角
%第二组试验值
%x = 20; %汽车起点的x坐标
%y = 30; %汽车起点的y坐标
%phi = 0; %汽车车身起始倾角
%第三组试验值
%x = 20; %汽车起点的x坐标
%y = 30; %汽车起点的y坐标
%phi = 30; %汽车车身起始倾角
%第四组试验值
%x = 20; %汽车起点的x坐标
%y = 30; %汽车起点的y坐标
%phi = -30; %汽车车身起始倾角
figure;
pause（1）;
for i=1:1:N-1
pause（0.1）;
inputcanshu=[x（i）y（i）phi（i）];
outputcanshu=evalfis（inputcanshufuzzyfis）;
theta（i）=outputcanshu;
x（i+1）=x（i）+v*Ts*cos（phi（i）*pi/180）*cos（theta（i）*pi/180）;%计算新的x坐标
y（i+1）=y（i）+v*Ts*sin（phi（i）*pi/180）*cos（theta（i）*pi/180）;%计算新的y坐标
phi（i+1）=phi（i）-（v*Ts*300/l）*sin（theta（i）*pi/180）;%计算新的车身倾角
   if x（i+1） >= 100 || y（i+1） >= 90 %如果超出模糊逻辑控制器的输入范围，则停止
     break;
   end
%fprintf（‘x = %f y = %f phi = %f theta = %f dx = %f dy = %f\n‘ x y
%theta phi delta_x delta_y）;
x0 = x（i） + w/2*sind（phi（i））; %左前角x坐标
y0 = y（i） - w/2*cosd（phi（i））; %左前角y坐标
x1 = x（i） - w/2*sind（phi（i））; %右前角x坐标
y1 = y（i） + w/2*cosd（phi（i））; %右前角y坐标
p  = x（i） - l*cosd（phi（i））;
q  = y（i） - l*sind（phi（i））;
x2 = p + w/2*sind（phi（i））; %左后角x坐标
y2 = q - w/2*cosd（phi（i））; %左后角y坐标
x3 = p - w/2*sind（phi（i））; %右后角x坐标
y3 = q + w/2*cosd（phi（i））; %右后角y坐标
%画汽车后轴中心
plot （x（i） y（i） ‘rs‘）;
axis（[-20 120 0 100]）;
xlabel（‘x - cm‘）;
ylabel（‘y - cm‘）;
title（‘Parallel Parking with Model Car‘）;
hold on
grid on;
%画车位示意图
h1 = line（[-20 40] [60 60]）;
h2 = line（[40 40] [60 95]）;
h3 = line（[40 115] [95 95]）;
h4 = line（[115 115] [60 95]）;
h5 = line（[115 120] [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‘）;
%画车身轮廓
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‘）;
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1987  2013-12-03 20:34  car_parking.fis
     文件        2373  2020-03-13 10:31  car_parking.m