matlab模糊控制侧方位泊车

Posted 2022-08-07 studyer_domi

1、内容简介

略
446-可以交流、咨询、答疑

2、内容说明

略

3、仿真分析

略

clc;
clear;
close all;
l = 25;%车宽
w = 20; %车长
v = 5; %车速
Ts =0.4; %采样时间间隔 - s
N=2000; % 最大循环次数
fuzzyfis=readfis('boche.fis'); %读取模糊文件

x(1) = 20; %汽车起点的x坐标
y(1) = 30; %汽车起点的y坐标
phi(1) =0; %汽车车身起始倾角

figure;
pause(1);
for i=1:1:N-1
    pause(0.1); %暂停0.1s
    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 ');ylabel('y ');
    hold on
    grid on;
    %画车位示意图
    h1 = line([-20 50], [60 60]);
    h2 = line([50 50], [60 90]);
    h3 = line([50 115], [90 90]);
    h4 = line([115 115], [60 90]);
    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', 2, 'color', 'g');
    set(l1, 'linewidth', 2, 'color', 'g');
    set(l2, 'linewidth', 2, 'color', 'g');
    set(l3, 'linewidth', 2, 'color', 'g');
end

 

4、参考论文

略