当前位置：首页 > news >正文

基于 SRCKF/UKF 的车辆状态估计：从理论到仿真实现

news 2026/6/13 9:24:41

基于平方根容积卡尔曼/无迹卡尔曼实现纵向、侧向速度、横摆角速度以及质心侧偏角的估计轮胎模型:dugoff 算法:SRCKF/UKF Carsim/Simulink联仿有m语言仿真模型，也实现了C程序仿真模型以下为C程序仿真模型对质心侧偏角估计效果图分别为:低附高附对开对接(双移线工况)

在车辆动力学研究领域，准确估计纵向、侧向速度、横摆角速度以及质心侧偏角等关键状态参数至关重要。本文将探讨如何基于平方根容积卡尔曼（SRCKF）和无迹卡尔曼（UKF）算法，利用 Dugoff 轮胎模型，通过 Carsim 与 Simulink 联合仿真，并实现 M 语言和 C 程序仿真模型。

轮胎模型 - Dugoff

Dugoff 轮胎模型是车辆动力学仿真中常用的模型，它综合考虑了轮胎的纵向力和侧向力与滑移率、侧偏角之间的关系。以下是简化的 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

平方根容积卡尔曼滤波（SRCKF）

SRCKF 是一种基于容积准则的卡尔曼滤波改进算法，它在处理非线性系统时具有较高的精度和稳定性。以下是 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*n+1 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*n+1 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*n+1)) * W_c) / S; % 计算卡尔曼增益 x_hat = x_hat_minus + K * (y - y_hat); % 更新状态估计值 P = cholupdate(P_minus, -K * S, 'L'); % 更新协方差矩阵

在 SRCKF 预测步骤中，首先计算 Sigma 点，通过系统模型传递后得到预测状态和预测协方差矩阵，并考虑过程噪声。更新步骤中，通过观测模型传递 Sigma 点得到预测观测值，计算卡尔曼增益后更新状态估计值和协方差矩阵，以此不断修正估计结果。

无迹卡尔曼滤波（UKF）

UKF 同样是处理非线性系统的有效算法，它通过选择一组 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*n+1 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*n+1 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*n+1 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 中实现。通过接口传递数据，实现车辆状态的实时估计和反馈。