Simulink + Carsim 验证车辆运动学模型

通过 Simulink/Carsim联合仿真，验证了车轮运动学模型。

@Drakie

7045人浏览 · 2022-07-22 16:44:49

@Drakie · 2022-07-22 16:44:49 发布

1 运动学模型

2 matlab 代码解析

主函数模板 Main_KinematicModel_Validation()

初始化子函数 mdlInitializeSizes()

离散化模型子函数 func_UpdateState_EulerM_2_7()

工况1：速度恒定 18Km/h、方向盘恒转矩

carsim 设置工况

simulink 仿真结果 yaw 、X Y 位置对比

工况2：车速恒定、转角为自定义sin 函数

carsim设置

simulink 仿真结果 yaw 对比

simulink 仿真结果 X Y对比

工况3：速度恒定 72Km/h、方向盘恒转矩

simulink 仿真结果 X Y 对比

simulink 仿真结果 yaw 对比

5 基于递推最小二乘法的转向传动比估计

1 运动学模型

模型推导：

2 matlab 代码实现

在 MATLAB/Simulink 环境中搭建该运动学模型，在相同输入条件下与 Carsim 中所建立的整车模型进行对比，相同的输入条件是指前轮偏角与车速随时间的变换历程相同，输出均为车辆位置和航向。

主函数模板 Main_KinematicModel_Validation()

function [sys,x0,str,ts] =Main_KinematicModel_Validation(t,x,u,flag)
    switch flag,
        case 0
            [sys,x0,str,ts] = mdlInitializeSizes;
        case 2
            sys = mdlUpdates(t,x,u);
        case 3
            sys = mdlOutputs(t,x,u);
        case {1,4,9}
            sys = [];            
        otherwise 
            error(['unhandled flag = ',num2str(flag)]);
    end
end

初始化子函数 mdlInitializeSizes()

function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 5;
sizes.NumOutputs     = 10; % 10个输入
sizes.NumInputs      = 10; % 10个输出
sizes.DirFeedthrough = 1; 
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 = zeros(sizes.NumDiscStates,1);
str = [];
ts  = [0.05 0]; % 采样时间
global InitialGapflag; 
       InitialGapflag = 0;     % 忽略来自 carsim 的前几个输入
global Previous_States;        % 存储之前的状态变量
       Previous_States.X_pred   = 0.0; 
       Previous_States.Y_pred   = 0.0; 
       Previous_States.Yaw_pred = 0.0;
end

mdlUpdates()

function sys = mdlUpdates(t,x,u)
    sys = x;
end

mdlOutputs() 算法主体

采集 carsim 状态信息

global InitialGapflag; 
global Previous_States;
    lfr = 2.70; % 轴距
    Ts  = 0.05;
    Steer_ratio = 1; % RLS估算方向盘与前轮传动比，暂时不用
%% 提取CarSim输入到Simulink的数据
    x_L2 = u(1); % 左后轮 x 坐标
    x_R2 = u(2); % 右后轮 x 坐标
    y_L2 = u(3); % 左后轮 y 坐标
    y_R2 = u(4); % 右后轮 y 坐标   
    Yaw  = u(5) * pi / 180; % 航向角Unit：deg-->rad
    Steer_SW = u(6); % 方向盘角度 steering wheel
    Steer_L1 = u(7); % 左前轮偏角
    Steer_R1 = u(8); % 右前轮偏角
    Vx_L2 = u(9);  % 左后轮纵向速度，Unit:km/h
    Vx_R2 = u(10); % 右后轮纵向速度，Unit:km/h
%% 坐标换算
    Car_X = 0.5 * (x_L2 + x_R2); % 后轴中心X坐标，Unit:m
    Car_Y = 0.5 * (y_L2 + y_R2); % 后轴中心Y坐标，Unit:m
    Vx_km_h = 0.5 * (Vx_L2 + Vx_R2); % 后轴中心处纵向速度,Unit：km/h
    Steer_deg = 0.5 * (Steer_L1 + Steer_R1); % 等效前轮偏角，Unit：deg
    Vx_m_s  = Vx_km_h / 3.6; % 后轴中心处纵向速度, Unit：m/s    
    Steer_rad = Steer_deg * pi / 180; % 等效前轮偏角 Unit：degs-->rad;

运动学模型离散化预测

if (InitialGapflag < 3) % Ignore the first few inputs
    InitialGapflag = InitialGapflag + 1;
    %% 状态初始化 
    X_pred   = Car_X;
    Y_pred   = Car_Y; 
    Yaw_pred = Yaw;
    Previous_States.X_pred   = Car_X; 
    Previous_States.Y_pred   = Car_Y; 
    Previous_States.Yaw_pred = Yaw;
else % start control
    %% 预测下一时刻状态
    Updated_state = func_UpdateState_EulerM_2_7(Previous_States, lfr, Vx_m_s, Steer_rad, Ts);
    X_pred = Updated_state.X_pred; 
    Y_pred = Updated_state.Y_pred; 
    Yaw_pred = Updated_state.Yaw_pred;
    Previous_States.X_pred = X_pred; 
    Previous_States.Y_pred = Y_pred; 
    Previous_States.Yaw_pred = Yaw_pred;
end
    sys = [Car_X; Car_Y; Yaw; X_pred; Y_pred; Yaw_pred; Vx_m_s; Steer_rad; Steer_SW; Steer_ratio];

离散化模型子函数 func_UpdateState_EulerM_2_7()

function [Updated_state] = func_UpdateState_EulerM_2_7(Previous_States, lfr, Vx_m_s, u, Ts)
    X_init      = Previous_States.X_pred;
    Y_init      = Previous_States.Y_pred;
    Yaw_init    = Previous_States.Yaw_pred;
    Updated_state.X_pred   = X_init + Ts * Vx_m_s*cos(Yaw_init);
    Updated_state.Y_pred   = Y_init + Ts * Vx_m_s*sin(Yaw_init);
    Updated_state.Yaw_pred = Yaw_init + Ts * Vx_m_s*tan(u)/lfr; % u 是前轮转角
end