车辆状态估计：从EKF到UKF的探索-程序员充电站

电源滤波器车辆状态估计，扩展卡尔曼滤波EKF，无迹卡尔曼滤波UKF 角阶跃输入+整车7自由度模型+UKF状态估计模型+附送EKF状态估计模型，针对于轮毂电机分布式驱动车辆，进行车速，质心侧偏角，横摆角速度估计。模型输入：方向盘转角delta，车辆纵向加速度ax 模型输出：横摆角速度wz，纵向车速vx，质心侧偏角β

在智能驾驶和车辆动力学研究中，状态估计是一个绕不开的话题。无论是自动驾驶还是车辆稳定性控制，准确估计车辆的状态参数（如车速、侧偏角、横摆角速度等）都是实现精准控制的基础。今天，我想和大家分享一个有趣的研究方向：基于扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）的车辆状态估计方法，特别是针对轮毂电机分布式驱动车辆的应用。

一、为什么需要状态估计？

车辆是一个复杂的非线性系统，其状态参数（如车速、侧偏角、横摆角速度等）往往无法直接测量，或者测量成本过高。因此，通过传感器数据和数学模型间接估计这些状态参数，就显得尤为重要。

在这个研究中，我们关注的是以下状态量：

纵向车速 \( v_x \)
质心侧偏角 \( \beta \)
横摆角速度 \( \omega_z \)

模型的输入包括方向盘转角 \( \delta \) 和车辆纵向加速度 \( a_x \)，这些输入可以通过车载传感器（如方向盘转角传感器和加速度计）方便地获取。

二、扩展卡尔曼滤波（EKF）：经典方法的挑战

扩展卡尔曼滤波（EKF）是状态估计领域中的一种经典方法，广泛应用于非线性系统。它的基本思想是通过对非线性模型进行泰勒展开，将问题线性化，从而应用卡尔曼滤波的框架进行状态更新。

对于我们的车辆状态估计问题，EKF的处理流程大致如下：

状态方程：基于整车7自由度模型，描述车辆状态的动态变化。
观测方程：将状态量与实际可测量的输出（如横摆角速度 \( \omegaz \) 和纵向车速 \( vx \)）关联起来。
滤波步骤：包括预测和更新两个阶段，通过不断迭代，逐步逼近真实状态。

EKF的代码实现大致如下：

def EKF_predict(state, input, P, F, Q): # 预测阶段 state_pred = F(state, input) # 状态方程 P_pred = F_jacobian(state, input) @ P @ F_jacobian(state, input).T + Q return state_pred, P_pred def EKF_update(state_pred, P_pred, measurement, H, R): # 更新阶段 innovation = measurement - H(state_pred) S = H_jacobian(state_pred) @ P_pred @ H_jacobian(state_pred).T + R K = P_pred @ H_jacobian(state_pred).T @ np.linalg.inv(S) state_est = state_pred + K @ innovation P_est = (np.eye(len(state_pred)) - K @ H_jacobian(state_pred)) @ P_pred return state_est, P_est

然而，EKF的一个显著问题是，它依赖于对非线性模型的线性化，这在模型高度非线性时可能导致较大的误差。特别是在车辆状态估计中，质心侧偏角 \( \beta \) 和横摆角速度 \( \omega_z \) 之间的关系往往非常复杂，EKF的线性化假设可能无法准确捕捉这些非线性特性。

三、无迹卡尔曼滤波（UKF）：更优的非线性处理

无迹卡尔曼滤波（UKF）通过使用无迹变换（UT）来避免对非线性模型进行线性化，从而在一定程度上解决了EKF的缺点。UKF的核心思想是通过选择一组特定的采样点（Sigma点），将这些点通过非线性模型传播，从而更准确地估计状态的均值和协方差。

UKF的处理流程与EKF类似，但预测和更新阶段的实现方式有所不同：

Sigma点生成：根据当前状态和协方差矩阵，生成一组Sigma点。
Sigma点传播：将这些Sigma点通过状态方程和观测方程传播。
状态更新：通过Sigma点的统计量计算状态的均值和协方差。

UKF的代码实现大致如下：

def UKF_predict(state, P, input, F, Q, alpha, beta, kappa): # 生成Sigma点 n = len(state) X = np.zeros((n, 2*n + 1)) X[:, 0] = state for i in range(n): X[:, i+1] = state + np.sqrt((n + kappa) * P)[:, i] X[:, i+n+1] = state - np.sqrt((n + kappa) * P)[:, i] # 传播Sigma点 X_pred = np.zeros_like(X) for i in range(2*n + 1): X_pred[:, i] = F(X[:, i], input) # 计算预测状态和协方差 state_pred = np.mean(X_pred, axis=1) P_pred = np.zeros((n, n)) for i in range(2*n + 1): dx = X_pred[:, i] - state_pred P_pred += (alpha**2 + beta) * dx[:, np.newaxis] @ dx[np.newaxis, :] / (2*n + kappa) P_pred += Q return state_pred, P_pred def UKF_update(state_pred, P_pred, measurement, H, R, alpha, beta, kappa): # 传播Sigma点 n = len(state_pred) X_pred = np.zeros((n, 2*n + 1)) for i in range(2*n + 1): X_pred[:, i] = state_pred + np.sqrt((n + kappa) * P_pred)[:, i] # 计算观测值 Z = np.zeros((len(measurement), 2*n + 1)) for i in range(2*n + 1): Z[:, i] = H(X_pred[:, i]) # 计算观测均值和协方差 z_pred = np.mean(Z, axis=1) S = np.zeros((len(measurement), len(measurement))) for i in range(2*n + 1): dz = Z[:, i] - z_pred S += (alpha**2 + beta) * dz[:, np.newaxis] @ dz[np.newaxis, :] / (2*n + kappa) S += R # 计算交叉协方差 T = np.zeros((n, len(measurement))) for i in range(2*n + 1): dx = X_pred[:, i] - state_pred dz = Z[:, i] - z_pred T += (alpha**2 + beta) * dx[:, np.newaxis] @ dz[np.newaxis, :] / (2*n + kappa) # 更新状态和协方差 K = T @ np.linalg.inv(S) state_est = state_pred + K @ (measurement - z_pred) P_est = P_pred - K @ S @ K.T return state_est, P_est

与EKF相比，UKF在处理高度非线性问题时表现更好，尤其是在车辆状态估计中，UKF能够更准确地捕捉质心侧偏角 \( \beta \) 和横摆角速度 \( \omega_z \) 之间的非线性关系。