基于平方根容积卡尔曼/无迹卡尔曼 实现纵向、侧向速度、横摆角速度以及质心侧偏角的估计 轮胎模型:dugoff 算法:SRCKF/UKF Carsim/Simulink联仿 有m语言仿真模型也实现了C程序仿真模型 以下为C程序仿真模型对质心侧偏角估计效果图 分别为:低附 高附 对开 对接(双移线工况)在车辆动力学研究领域准确估计纵向、侧向速度、横摆角速度以及质心侧偏角等关键状态参数至关重要。本文将探讨如何基于平方根容积卡尔曼SRCKF和无迹卡尔曼UKF算法利用 Dugoff 轮胎模型通过 Carsim 与 Simulink 联合仿真并实现 M 语言和 C 程序仿真模型。轮胎模型 - DugoffDugoff 轮胎模型是车辆动力学仿真中常用的模型它综合考虑了轮胎的纵向力和侧向力与滑移率、侧偏角之间的关系。以下是简化的 Dugoff 轮胎模型在代码中的体现// 假设已经定义了相关的变量如侧偏角 alpha纵向滑移率 s垂直载荷 Fz 等 // 计算轮胎侧偏刚度 double C_alpha getCAlpha(); // 根据 Dugoff 模型计算侧向力 Fy double Fy C_alpha * alpha * (1 - (fabs(C_alpha * alpha) / (mu * Fz)));在这段代码中首先获取轮胎侧偏刚度C_alpha然后根据 Dugoff 模型计算侧向力Fy。这里用到了侧偏角alpha、垂直载荷Fz以及摩擦系数mu通过一定的公式关系得到侧向力为后续车辆动力学计算提供基础。算法 - SRCKF/UKF平方根容积卡尔曼滤波SRCKFSRCKF 是一种基于容积准则的卡尔曼滤波改进算法它在处理非线性系统时具有较高的精度和稳定性。以下是 SRCKF 算法核心部分的伪代码% 初始化 P chol(P); % P 是协方差矩阵chol 函数进行 Cholesky 分解 x_hat zeros(n, 1); % n 是状态向量维度初始化状态估计值 % 预测步骤 [X_sigma] SigmaPoints(x_hat, P); % 计算 Sigma 点 X_sigma f(X_sigma); % 通过系统模型传递 Sigma 点 x_hat_minus sum(W_m.* X_sigma, 2); % 计算预测状态 P_minus zeros(n); for k 1:2*n1 P_minus P_minus W_c(k) * (X_sigma(:,k) - x_hat_minus) * (X_sigma(:,k) - x_hat_minus); end P_minus cholupdate(P_minus, R, L); % 加入过程噪声协方差 R % 更新步骤 [Y_sigma] SigmaPoints(x_hat_minus, P_minus); Y_sigma h(Y_sigma); % 通过观测模型传递 Sigma 点 y_hat sum(W_m.* Y_sigma, 2); % 计算预测观测值 S zeros(m); for k 1:2*n1 S S W_c(k) * (Y_sigma(:,k) - y_hat) * (Y_sigma(:,k) - y_hat); end S cholupdate(S, Q, L); % 加入观测噪声协方差 Q K (P_minus * (Y_sigma - repmat(y_hat, 1, 2*n1)) * W_c) / S; % 计算卡尔曼增益 x_hat x_hat_minus K * (y - y_hat); % 更新状态估计值 P cholupdate(P_minus, -K * S, L); % 更新协方差矩阵在 SRCKF 预测步骤中首先计算 Sigma 点通过系统模型传递后得到预测状态和预测协方差矩阵并考虑过程噪声。更新步骤中通过观测模型传递 Sigma 点得到预测观测值计算卡尔曼增益后更新状态估计值和协方差矩阵以此不断修正估计结果。无迹卡尔曼滤波UKFUKF 同样是处理非线性系统的有效算法它通过选择一组 sigma 点来近似状态分布。以下是 UKF 的核心伪代码% 初始化 x_hat zeros(n, 1); P eye(n); % 计算 sigma 点权重 lambda alpha^2 * (n kappa) - n; W_m [lambda / (n lambda), 0.5 / (n lambda) * ones(1, 2*n)]; W_c W_m; W_c(1) W_c(1) (1 - alpha^2 beta); % 预测步骤 X_sigma SigmaPoints(x_hat, P, lambda); X_sigma f(X_sigma); x_hat_minus sum(W_m.* X_sigma, 2); P_minus zeros(n); for k 1:2*n1 P_minus P_minus W_c(k) * (X_sigma(:,k) - x_hat_minus) * (X_sigma(:,k) - x_hat_minus); end P_minus P_minus Q; % 更新步骤 Y_sigma SigmaPoints(x_hat_minus, P_minus, lambda); Y_sigma h(Y_sigma); y_hat sum(W_m.* Y_sigma, 2); S zeros(m); for k 1:2*n1 S S W_c(k) * (Y_sigma(:,k) - y_hat) * (Y_sigma(:,k) - y_hat); end S S R; T zeros(n, m); for k 1:2*n1 T T W_c(k) * (X_sigma(:,k) - x_hat_minus) * (Y_sigma(:,k) - y_hat); end K T / S; x_hat x_hat_minus K * (y - y_hat); P P_minus - K * S * K;UKF 与 SRCKF 类似在初始化后进行预测和更新步骤。不同之处在于权重计算以及协方差更新方式等细节UKF 通过特定的权重分配和协方差计算方式来处理非线性问题。Carsim/Simulink 联仿通过 Carsim 与 Simulink 联合仿真可以充分利用 Carsim 精确的车辆动力学模型和 Simulink 强大的控制算法设计与仿真能力。在联仿中将车辆的物理模型搭建在 Carsim 中而状态估计的算法如 SRCKF 或 UKF 则在 Simulink 中实现。通过接口传递数据实现车辆状态的实时估计和反馈。仿真模型实现M 语言仿真模型在 M 语言Matlab中可以方便地实现上述算法和模型。通过编写函数和脚本构建完整的车辆状态估计仿真流程。例如定义系统模型函数f和观测模型函数h结合 SRCKF 或 UKF 算法代码实现对纵向、侧向速度、横摆角速度以及质心侧偏角的估计。C 程序仿真模型在 C 程序中实现仿真模型可以提高运行效率便于在实际车辆控制系统中应用。以下是 C 程序仿真模型对质心侧偏角估计效果图分别展示了低附、高附、对开、对接双移线工况下的情况。!低附工况质心侧偏角估计效果图!高附工况质心侧偏角估计效果图基于平方根容积卡尔曼/无迹卡尔曼 实现纵向、侧向速度、横摆角速度以及质心侧偏角的估计 轮胎模型:dugoff 算法:SRCKF/UKF Carsim/Simulink联仿 有m语言仿真模型也实现了C程序仿真模型 以下为C程序仿真模型对质心侧偏角估计效果图 分别为:低附 高附 对开 对接(双移线工况)!对开工况质心侧偏角估计效果图!对接工况质心侧偏角估计效果图从这些效果图可以看出基于 SRCKF/UKF 算法的 C 程序仿真模型在不同路面附着条件和工况下都能够较好地估计质心侧偏角为车辆动力学控制提供了可靠的状态参数估计。通过上述从轮胎模型到算法实现再到联合仿真和不同语言仿真模型的构建我们全面地实现了基于平方根容积卡尔曼/无迹卡尔曼的车辆关键状态参数估计为车辆主动安全控制系统的开发提供了有力的技术支持。