clc;
clear;
close all;
l = 25;%汽车长
w = 20; %汽车宽
v = 5; %汽车速度
Ts =0.4; %采样时间间隔 - second
N=2000; % 最大循环次数
fuzzyfis=readfis('boche.fis');
%汽车车身起始位置
x(1) = 10; %汽车起点的x坐标
y(1) = 30; %汽车起点的y坐标
phi(1) =0; %汽车车身起始倾角
figure;
pause(1);
for i=1:1:N-1
pause(0.1);
inputcanshu=[x(i),y(i),phi(i)];
outputcanshu=evalfis(inputcanshu,fuzzyfis);
theta(i)=outputcanshu;
x(i+1)=x(i)+v*Ts*cos(phi(i)*pi/180)*cos(theta(i)*pi/180);
y(i+1)=y(i)+v*Ts*sin(phi(i)*pi/180)*cos(theta(i)*pi/180);
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
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');
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', 'r');
set(h2, 'linewidth', 5, 'color', 'r');
set(h3, 'linewidth', 5, 'color', 'r');
set(h4, 'linewidth', 5, 'color', 'r');
set(h5, 'linewidth', 5, 'color', 'r');
%画车身轮廓
c1 = line([x0 x1], [y0 y1]);
c2 = line([x1 x3], [y1 y3]);
c3 = line([x2 x3], [y2 y3]);
c4 = line([x0 x2], [y0 y2]);
set(c1, 'linewidth', 4, 'color', 'm');
set(c2, 'linewidth', 2, 'color', 'b');
set(c3, 'linewidth', 4, 'color', 'g');
set(c4, 'linewidth', 2, 'color', 'b');
end
评论3