兔子3131. 项目概述
在自动驾驶和车辆动力学控制领域，准确估计车辆状态参数是确保系统安全性和稳定性的基础。其中，质心侧偏角（β）和横摆角速度（γ）是两个关键参数，它们直接反映了车辆的横向稳定性。传统方法如互补滤波器虽然简单，但在动态工况下精度有限。本文将详细介绍如何基于扩展卡尔曼滤波（EKF）和非联合仿真方法，利用三自由度车辆模型实现这两个参数的高精度估计。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 核心模型构建
2.1 三自由度车辆模型
我们采用经典的自行车模型作为基础框架，考虑车辆的纵向、横向和横摆三个自由度。状态向量选择为：
code复制x = [v_x, v_y, γ, β]^T
其中：
- v_x：纵向速度（m/s）
- v_y：横向速度（m/s）
- γ：横摆角速度（rad/s）
- β：质心侧偏角（rad）
质心侧偏角与速度分量的关系为：
code复制β = arctan(v_y/v_x)
2.2 轮胎力建模
采用线性轮胎模型计算前后轮侧向力：
code复制F_yf = C_f * α_f
F_yr = C_r * α_r
前后轮侧偏角计算公式：
code复制α_f = δ - (v_y + l_f*γ)/v_x
α_r = -(v_y - l_r*γ)/v_x
其中：
- C_f, C_r：前后轮侧偏刚度（N/rad）
- l_f, l_r：质心到前后轴距离（m）
- δ：前轮转向角（rad）
3. EKF算法实现
3.1 状态方程
车辆动力学方程：
code复制dv_x/dt = a_x + v_y*γ
dv_y/dt = (F_yf + F_yr)/m - v_x*γ
dγ/dt = (l_f*F_yf - l_r*F_yr)/I_z
dβ/dt = (dv_y/dt*v_x - dv_x/dt*v_y)/(v_x^2 + v_y^2)
3.2 观测方程
假设可测量量为纵向加速度a_x、横向加速度a_y和横摆角速度γ：
code复制z = [a_x, a_y, γ]^T
观测方程：
code复制a_x = (F_xf + F_xr)/m
a_y = (F_yf + F_yr)/m + v_x*γ
γ = γ
3.3 雅可比矩阵计算
EKF需要计算状态转移矩阵F和观测