基于七自由度车辆模型的 UKF 与 EKF 参数估计之旅

基于七自由度车辆模型的UKFEFK对质心侧偏角横摆角速度纵向车速的估计。 七自由度车辆动力学模型 UKF无迹卡尔曼滤波 EKF扩展卡尔曼滤波 质心侧偏角 横摆角速度 纵向车速 参数估计 提供参考文献pdf 车辆模型建模word文档 UKF学习文档等在车辆动力学研究领域准确估计质心侧偏角、横摆角速度和纵向车速等关键参数至关重要。而基于七自由度车辆模型运用 UKF无迹卡尔曼滤波和 EKF扩展卡尔曼滤波方法能实现较为精准的估计。今天咱就来唠唠这其中的门道。七自由度车辆动力学模型七自由度车辆模型考虑了车辆的纵向、侧向、垂向运动以及横摆、侧倾、俯仰运动更为全面地描述车辆动态特性。它是后续滤波算法施展拳脚的基础舞台。例如简单来说车辆的运动方程可以表示为\[\begin{cases}m(\dot{v}x - vy\omegar) F{x1} F{x2} F{x3} F_{x4} \\m(\dot{v}y vx\omegar) F{y1} F{y2} F{y3} F_{y4} \\Iz\dot{\omega}r lf(F{y1} F{y2}) - lr(F{y3} F{y4})基于七自由度车辆模型的UKFEFK对质心侧偏角横摆角速度纵向车速的估计。 七自由度车辆动力学模型 UKF无迹卡尔曼滤波 EKF扩展卡尔曼滤波 质心侧偏角 横摆角速度 纵向车速 参数估计 提供参考文献pdf 车辆模型建模word文档 UKF学习文档等\end{cases}\]这里 \(m\) 是车辆质量\(vx\) 是纵向车速\(vy\) 是侧向车速\(\omegar\) 是横摆角速度\(F{xi}\) 和 \(F{yi}\) 分别是轮胎的纵向力和侧向力\(lf\) 和 \(l_r\) 是车辆质心到前后轴的距离。这个模型细致地刻画了车辆各方向的受力与运动关系为参数估计提供了可靠依据。UKF 无迹卡尔曼滤波UKF 是一种非线性滤波算法它不像传统线性卡尔曼滤波要求系统必须是线性的。UKF 通过一组 Sigma 点来近似非线性分布。下面是一个简单的 Python 代码示例来展示 UKF 对质心侧偏角估计的大致过程仅为示意实际需更多细节处理import numpy as np # 定义一些参数 n 2 # 状态维度 m 1 # 观测维度 alpha 0.001 beta 2 kappa 0 # 初始化状态 x_hat np.zeros((n, 1)) P np.eye(n) # 过程噪声协方差 Q np.eye(n) * 0.01 # 观测噪声协方差 R np.eye(m) * 0.1 # Sigma 点生成 lamda alpha**2 * (n kappa) - n Wm np.zeros((2 * n 1, 1)) Wc np.zeros((2 * n 1, 1)) Wm[0] lamda / (n lamda) Wc[0] lamda / (n lamda) (1 - alpha**2 beta) for i in range(1, 2 * n 1): Wm[i] 1 / (2 * (n lamda)) Wc[i] 1 / (2 * (n lamda)) # 状态转移函数这里简单示意实际应符合车辆模型 def f(x): return x # 观测函数这里简单示意实际应符合车辆模型 def h(x): return np.array([[x[0]]]) # UKF 预测步骤 def ukf_predict(x_hat, P): Xsig np.zeros((n, 2 * n 1)) Xsig[:, 0] x_hat.flatten() for i in range(n): Xsig[:, i 1] x_hat.flatten() np.sqrt((n lamda) * P)[:, i] Xsig[:, i n 1] x_hat.flatten() - np.sqrt((n lamda) * P)[:, i] x_hat_minus np.zeros((n, 1)) P_minus np.zeros((n, n)) for i in range(2 * n 1): x_hat_minus x_hat_minus Wm[i] * f(Xsig[:, i]).reshape((n, 1)) for i in range(2 * n 1): P_minus P_minus Wc[i] * (f(Xsig[:, i]).reshape((n, 1)) - x_hat_minus) (f(Xsig[:, i]).reshape((n, 1)) - x_hat_minus).T P_minus P_minus Q return x_hat_minus, P_minus # UKF 更新步骤 def ukf_update(x_hat_minus, P_minus, z): Xsig np.zeros((n, 2 * n 1)) Xsig[:, 0] x_hat_minus.flatten() for i in range(n): Xsig[:, i 1] x_hat_minus.flatten() np.sqrt((n lamda) * P_minus)[:, i] Xsig[:, i n 1] x_hat_minus.flatten() - np.sqrt((n lamda) * P_minus)[:, i] z_hat np.zeros((m, 1)) Pzz np.zeros((m, m)) Pxz np.zeros((n, m)) for i in range(2 * n 1): z_hat z_hat Wm[i] * h(Xsig[:, i]).reshape((m, 1)) for i in range(2 * n 1): Pzz Pzz Wc[i] * (h(Xsig[:, i]).reshape((m, 1)) - z_hat) (h(Xsig[:, i]).reshape((m, 1)) - z_hat).T Pxz Pxz Wc[i] * (Xsig[:, i].reshape((n, 1)) - x_hat_minus) (h(Xsig[:, i]).reshape((m, 1)) - z_hat).T Pzz Pzz R K Pxz np.linalg.inv(Pzz) x_hat x_hat_minus K (z - z_hat) P P_minus - K Pzz K.T return x_hat, P # 模拟观测数据 z np.array([[0.1]]) x_hat_minus, P_minus ukf_predict(x_hat, P) x_hat, P ukf_update(x_hat_minus, P_minus, z) print(估计的质心侧偏角:, x_hat[0][0])在这段代码中首先定义了 UKF 所需的参数包括状态维度、观测维度、Sigma 点参数等。然后通过f和h函数定义了状态转移和观测函数虽然这里只是简单示意实际应紧密结合车辆模型。ukfpredict和ukfupdate函数分别实现了 UKF 的预测和更新步骤。通过 Sigma 点的生成、状态预测、协方差预测以及观测更新等一系列操作最终实现对质心侧偏角的估计。EKF 扩展卡尔曼滤波EKF 也是用于非线性系统的滤波算法它通过对非线性函数进行一阶泰勒展开将非线性问题近似为线性问题来应用卡尔曼滤波框架。同样以 Python 代码简单展示 EKF 对横摆角速度估计过程仅为示意import numpy as np # 定义状态转移函数的雅克比矩阵这里简单示意实际应符合车辆模型 def F(x): return np.array([[1, 0], [0, 1]]) # 定义观测函数的雅克比矩阵这里简单示意实际应符合车辆模型 def H(x): return np.array([[1, 0]]) # 初始化状态 x_hat np.zeros((2, 1)) P np.eye(2) # 过程噪声协方差 Q np.eye(2) * 0.01 # 观测噪声协方差 R np.eye(1) * 0.1 # EKF 预测步骤 def ekf_predict(x_hat, P): x_hat_minus F(x_hat) x_hat P_minus F(x_hat) P F(x_hat).T Q return x_hat_minus, P_minus # EKF 更新步骤 def ekf_update(x_hat_minus, P_minus, z): K P_minus H(x_hat_minus).T np.linalg.inv(H(x_hat_minus) P_minus H(x_hat_minus).T R) x_hat x_hat_minus K (z - H(x_hat_minus) x_hat_minus) P (np.eye(2) - K H(x_hat_minus)) P_minus return x_hat, P # 模拟观测数据 z np.array([[0.2]]) x_hat_minus, P_minus ekf_predict(x_hat, P) x_hat, P ekf_update(x_hat_minus, P_minus, z) print(估计的横摆角速度:, x_hat[1][0])在这个代码示例中先定义了状态转移函数和观测函数的雅克比矩阵F和H实际要根据车辆模型精确推导。ekfpredict和ekfupdate函数分别实现预测和更新过程。通过对非线性函数的线性近似雅克比矩阵将卡尔曼滤波应用到非线性的车辆模型中实现对横摆角速度的估计。对比与总结UKF 和 EKF 都在基于七自由度车辆模型的参数估计中发挥重要作用。UKF 直接通过 Sigma 点近似非线性分布避免了复杂的雅克比矩阵计算在一些高度非线性情况下表现更好而 EKF 通过线性近似实现相对简单但在非线性较强时可能出现较大误差。文中提供的参考文献 pdf、车辆模型建模 word 文档以及 UKF 学习文档等资料能帮助大家更深入地研究这一领域。希望通过今天的分享大家对基于七自由度车辆模型的 UKF 和 EKF 参数估计有更清晰的认识在相关研究和实践中能灵活运用这两种强大的滤波算法。