目录

1 运动学模型

2 matlab 代码解析

主函数模板 Main_KinematicModel_Validation()

初始化子函数 mdlInitializeSizes()

mdlUpdates()

mdlOutputs() 算法主体

采集 carsim 状态信息

运动学模型离散化预测

离散化模型子函数 func_UpdateState_EulerM_2_7()

Simulink 搭建

3 carsim 搭建

车身设置

 路况设置

添加 I/O 变量 

 4 验证结果

工况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

Simulink 搭建

3 carsim 搭建

车身设置

 路况设置

添加 I/O 变量 

 添加 10 个输出变量

 4 验证结果

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

carsim 设置工况

simulink 查看方向盘转角、前轮转角

simulink 对比 yaw 、X Y 位置

carsim动画效果:

结论:通过模型得出的小车X Y 位置、横摆角,和 实际 carsim中的结果重合度较高,因此运动学模型可以用在小车控制算法设计中。

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

carsim设置工况

添加车轮转角变化规律

 MATLAB 生成 sin 轨迹点

Carsim 中添加运动规律

simulink 查看方向盘转角、前轮转角:

 simulink 仿真结果 yaw 对比

simulink 仿真结果 X Y对比

X Y 精确对比:

carsim X Y 轨迹动画

结论:车轮运动学模型在中低速情况下实际拟合效果良好。

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

simulink 仿真结果 X Y 对比

精确对比 X Y:

simulink 仿真结果 yaw 对比

结论:运动学模型没有考虑轮胎侧滑等实际工况,所以只使用于中低速情况,在高速情况下模型不准确,不能用于模型预测控制。 

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

待补充..

Logo

开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!

更多推荐