递推最小二乘法RLS的轮胎侧偏刚度估计(原书缺失代码已补全)
采用具有遗忘因子的递推最小二乘法在线估计轮胎侧偏刚度，搭建 carsim/Simulink 联合平台仿真验证。补全了原书缺失的代码。
·
目录
Carsim 数据传输、预测侧偏角 α 、求解侧偏刚度 Cα
1 参数辨识
背景知识补充。
1.1 最小二乘法
问题描述：
先给出未知参数 θ 求解公式：
推导过程：
缺点：1、需要大量的历史数据，导致占用存储。
2、无法实时更新。
1.2 递推最小二乘法 RLS
问题描述：如果得到一组新的输入输出数据，怎样利用旧估计，完成新估计，即避免重复使用历史输入输出数据，而一步得到最新估计。
递推公式：
给定初值
公式证明
1.3 具有遗忘因子 λ 的递推最小二乘法
问题描述
遗忘因子 λ 的加入
递推公式：
2 轮胎线性侧偏刚度估计
基于车辆动力学模型进行模型预测控制需要用到车辆的状态信息、惯性参数、参考道路信息以及轮胎线性侧偏刚度等。其中，车辆状态信息包括纵向速度、侧向速度、前轮偏角等，车辆惯性参数包括车身质量、车体转动惯量、质心到前后轴的距离位置等，参考道路信息包括路面倾角、道路曲率等。
轮胎线性侧偏刚度是对车辆模型进行线性化的重要参数，反映了车辆在轮胎线性区域内的操作稳定性，而且随行驶工况、路面条件的不同，此参数发生变化。因此对轮胎线性侧偏刚度进行实时估计可以提高无人驾驶车辆对不同附着系数路面的适应性。下面根据车辆的状态信息和惯性参数的历史数据，采用递推最小二乘法估计轮胎的线性侧偏刚度。
2.1 RLS 算法分析
递推最小二乘法公式（这部分可以不看）
公式（5.24）说明（可以不看）：
2.2 联合仿真平台的设计
carsim 设置车辆参数
carsim 设置工况
carsim 设置 IO 模块
求解器设置：
输出变量设置（共13个），添加顺序与 matlab 中对变量 u 的访问顺序保持一致。
修正 7、8 顺序：
Simulink 搭建
注意事项：轮胎侧偏角为左右轮胎的平均侧偏角，需要乘0.5，轮胎纵向力为左右轮胎的合成，不需要乘0.5。
2.3 MATLAB 代码分析
主函数框架
function [sys,x0,str,ts] =chapter5_5_2(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初始化函数
function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 6;
sizes.NumOutputs = 10; % 10 个输出
sizes.NumInputs = 13; % 13 个输入（来自 carsim）
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0 = zeros(sizes.NumDiscStates,1);
str = [];
ts = [0.05 0];
global InitialGapflag;
InitialGapflag = 0;
global VehicleParams;
VehicleParams.Lf = 1.12; % 前轴距
VehicleParams.Lr = 1.48; % 后轴距
[y, c] = func_RLSEstimation_Cf('initial', 0.95, 1, 10); % λ=0.95，θ数目为1，Pk 初值为 10
[y, c] = func_RLSEstimation_Cr('initial', 0.95, 1, 10);
endmdlOutputs()：变量定义
function sys = mdlOutputs(t,x,u)
global InitialGapflag;
global VehicleParams;
Cf_Hat = 0; % Cf 估计值，输出可视
Cr_Hat = 0;
Pk_f = 0; % Pk 协方差，输出可视
Pk_r = 0;
Fyf_err = 0; % Fy 误差值，输出可视
Fyr_err = 0;
alpha_f_Hat = 0;
alpha_r_Hat = 0;
alpha_f_Direct = 0;
alpha_r_Direct = 0;
if InitialGapflag < 3 % 防止carsim 传输的前几个值为空
InitialGapflag = InitialGapflag + 1;Carsim 数据传输、预测侧偏角 α 、求解侧偏刚度 Cα
else
InitialGapflag = InitialGapflag + 1;
[Carsim_export] = func_CarsimData_Parse(u);
Vx = Carsim_export.x_dot;
Vy = Carsim_export.y_dot;
yawrate = Carsim_export.phi_dot; % rad/s
delta_f = Carsim_export.delta_f; % 前轮转角 rad
Fyf_Direct = Carsim_export.Fyf;
Fyr_Direct = Carsim_export.Fyr;
alpha_f_Direct = Carsim_export.alphaf; % 来自 carsim 的前轮侧偏角
alpha_r_Direct = Carsim_export.alphar; % 来自 carsim 的后轮侧偏角
% 前轮
alpha_f_Hat = (Vy + yawrate * VehicleParams.Lf)/Vx - delta_f;
[Fyf_Hat, Cf_Hat, Pk_f] = func_RLSEstimation_Cf(alpha_f_Hat, Fyf_Direct);
Fyf_err = Fyf_Hat - Fyf_Direct; % 横向摩擦力的误差比较
% 后轮
alpha_r_Hat = (Vy - yawrate * VehicleParams.Lr)/Vx;
[Fyr_Hat, Cr_Hat, Pk_r] = func_RLSEstimation_Cr(alpha_r_Hat, Fyr_Direct);
Fyr_err = Fyr_Hat - Fyr_Direct;
end
sys = [Cf_Hat; Cr_Hat; Pk_f; Fyf_err; Pk_r; Fyr_err; alpha_f_Hat; alpha_r_Hat; alpha_f_Direct; alpha_r_Direct];
end其中 αf 和 αr 的求解是通过如下公式：
Carsim 数据传输子函数
function [Sparsed_Carsim_Data] = func_CarsimData_Parse(CarsimData)
Sparsed_Carsim_Data.x_dot = CarsimData(1)/3.6; %Unit:km/h-->m/s，保留1位小数
Sparsed_Carsim_Data.y_dot = CarsimData(2)/3.6; %Unit:km/h-->m/s，保留1位小数
Sparsed_Carsim_Data.phi_dot = (round(10*CarsimData(3))/10)*pi/180; %Unit：deg/s-->rad/s，保留1位小数
Sparsed_Carsim_Data.delta_f = (round(10*0.5*(CarsimData(4)+ CarsimData(5)))/10)*pi/180; % 左、右前轮转角 % deg-->rad，对应carsim Steer_L1 R1
Sparsed_Carsim_Data.alphaf = (round(10*0.5 * (CarsimData(6)+ CarsimData(8)))/10)*pi/180; % 左、右前轮侧偏角 对应 carsim Alpha_L1 R1 deg-->rad，保留1位小数
Sparsed_Carsim_Data.alphar = (round(10*0.5 * (CarsimData(7)+ CarsimData(9)))/10)*pi/180; % 左、右后轮侧偏角 对应 carsim Alpha_L2 R2 deg-->rad，保留1位小数
Fy_l1 = round(10*CarsimData(10))/10; %Unit:N，保留1位小数
Fy_l2 = round(10*CarsimData(11))/10; %Unit:N，保留1位小数
Fy_r1 = round(10*CarsimData(12))/10; %Unit:N，保留1位小数
Fy_r2 = round(10*CarsimData(13))/10; %Unit:N，保留1位小数
Sparsed_Carsim_Data.Fyf = Fy_l1 + Fy_r1;
Sparsed_Carsim_Data.Fyr = Fy_l2 + Fy_r2;
end前轮侧偏刚度估计（RLS主体，原书代码缺失部分）
回顾具有遗忘因子的递推最小二乘公式
更新 K(k+1)、P(k+1)，求解 θ(k+1)
function[Fyf_hat, Cf_hat,Pkf] = func_RLSEstimation_Cf(alpha_f_hat,Fyf_direct, Param_Num, Pkf_init)
% Fyf_hat 是 Fy 估算值
% Cf_hat 是前轮侧偏刚度 Cf 的估算值
% Pkf 是 Pk
% alpha_f_hat ：已知量（来自模型预测）
% Fyf_direct ：已知量（来自 Carsim ）
% Param_Num ：待估计的参数数量，这里只有 Cf，所以 Param_Num =1
% Pkf_init ：Pk 初始值
persistent alpha_f Cf lambda_f Pk_f % 定义静态变量，在Simulink整个运行期间都有值
%% 初始化：静态变量赋值
if (ischar(alpha_f_hat) && strcmp(alpha_f_hat,'initial'))
alpha_f = zeros(Param_Num,1); % 定义 alpha_f 的维度为1
Cf = zeros(Param_Num,1); % 定义 Cf 的维度为 1
lambda_f = Fyf_direct; % λ= 0.95，Fy_direct 作为形参传递 λ
Pk_f = Pkf_init * eye(Param_Num); % Pk = 10,初始值
Fyf_hat = 0; % initial 阶段返回值为0
Cf_hat = 0;
else
%% 带λ的 RLS算法主体:
alpha_f= alpha_f_hat; % 初值选取，Pk 已经在initial 阶段赋了初值
% RLS 的3个公式
Kf = Pk_f * alpha_f / (lambda_f + alpha_f' * Pk_f * alpha_f);
Pkf_next = (Pk_f - Kf * alpha_f' * Pk_f) / lambda_f;
Cf = Cf + Kf * (Fyf_direct - alpha_f' * Cf);
Pk_f = Pkf_next; % Pk 更新，作为下一时刻初值
% 输出变量
Fyf_hat = alpha_f'* Cf;
Cf_hat = Cf; % 最终值
Pkf = Pk_f;
end
end后轮侧偏刚度估计（RLS主体）
function[Fyr_hat, Cr_hat,Pkr] = func_RLSEstimation_Cr(alpha_r_hat,Fyr_direct, Param_Num, Pkr_init)
persistent alpha_r Cr lambda_r Pk_r
if (ischar(alpha_r_hat) && strcmp(alpha_r_hat,'initial'))
alpha_r = zeros(Param_Num,1);
Cr = zeros(Param_Num,1);
lambda_r = Fyr_direct;
Pk_r = Pkr_init * eye(Param_Num);
Fyr_hat = 0;
Cr_hat = 0;
else
alpha_r= alpha_r_hat;
Kr = Pk_r * alpha_r / (lambda_r + alpha_r' * Pk_r * alpha_r);
Pkr_next = (Pk_r - Kr * alpha_r' * Pk_r) / lambda_r;
Cr = Cr + Kr * (Fyr_direct - alpha_r' * Cr);
Pk_r = Pkr_next;
Fyr_hat = alpha_r'* Cr;
Cr_hat = Cr;
Pkr = Pk_r;
end
end2.4 仿真结果分析
侧偏刚度 Cf、Cr
侧偏角对比（实际 VS 预测）
车轮横向摩擦力
车轮侧向力误差
误差方差：Pk
车辆运动状态
Carsim 仿真动画
结论
从车辆前轮和后轮的侧偏刚度估计结果可以看出，估计值快速地达到收敛状态。
侧向力峰值 Ff = 5000 N
侧偏角峰值 αf = 0.05 rad
侧偏刚度估算值恰好为 5000 / 0.05 ≈ 10000，即为如下值：
所以本算法能够很好地提供轮胎侧偏刚度估计。另外发现轮胎的侧偏刚度保持在一个恒定值附近波动，因此在车辆动力学中可以被当作固定值处理。
开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！
更多推荐
38
0- 0
已为社区贡献2条内容
所有评论(0)