1. 同时估计质量和坡度这个账到底难算在哪里做车辆控制特别是商用车动力系统标定和智能驾驶决策算法的朋友基本都绕不过两个看不见的量车辆质量和道路坡度。AMT变速箱在坡道上换挡得提前预判车辆能不能加速冲上去这需要质量来算加速度巡航系统在上坡前决定是否降挡需要坡度来判断坡道阻力电动车的续航估算和制动能量回收策略也需要质量来推算剩余里程和回收扭矩。可以说一堆上层功能都在等这两个数但这两个数偏偏是整车状态里最难直接测量的。我最早做质量估计时方案很单纯只估质量用普通卡尔曼滤波把坡度当干扰项。结果一上3%的坡估计出来的质量直接胖了十几吨逻辑其实很简单坡道阻力被滤波器当成了一部分惯性力算进质量里去了。反过来如果只估坡度而忽略质量偏差空载或满载状态下的坡度估计值同样会偏因为这个坡度里掺了真实的加速度误差。这就是为什么要用基于扩展卡尔曼滤波的方案把质量和坡度放在同一个状态向量里联合估计让滤波器在解算过程中自己去权衡哪个量对应质量、哪个量对应坡度。文章里我会把这套方案的纵向动力学建模、状态方程与雅可比矩阵推导、三个典型仿真工况的结果拆解以及实车落地时我自己踩过的几个坑一次性讲清楚。适合正在做车辆状态估计、商用车整车控制器开发、智能驾驶决策或相关课题研究的朋友参考在校生做课题选型时也可以拿这篇当作入门路线图。1.1 换挡策略、坡道辅助和续航估算都在等这两个数先说结论质量主要影响驱动力里有多少分给了加速度坡度主要影响驱动力里有多少分给了坡道阻力。这两者的数学表达在纵向动力学方程里天然耦合。具体到应用场景AMT换挡策略需要实时预判车辆在当前挡位下的最大可达到加速度。如果质量估计值偏大系统会认为车辆动力不足该升挡的时候不升挡如果偏小又会过早升挡导致拖挡甚至熄火。坡度估计值则直接决定上坡辅助功能在坡道上释放刹车的时机以及巡航系统在坡顶坡谷处的油门和挡位预调节。智能驾驶的纵向规划也会依赖这两个参数做加速度边界计算尤其商用车满载四十吨和空载十二吨时制动距离和加速能力的差异是数量级的模型里必须有一个能跟随装载变化的质量参数。这里有一个容易被忽视的细节质量和坡度不是稳定不变的。质量在每次装卸货后发生跳变在一次行驶周期内基本恒定坡度则随着道路起伏持续变化但变化频率很低。这两个量在时间尺度上的差异正好给了联合估计一个突破口。把变化极慢的质量和缓变的坡度都作为待估计状态把速度和驱动力作为可测输入就可以在一个统一的滤波框架里同时完成辨识和跟踪。1.2 为什么不能先估坡度再估质量或者反过来如果把问题拆开先单独估计坡度再用坡度修正质量听起来似乎可行实际上会遇到鸡生蛋蛋生鸡的问题。坡度估计通常可以基于GPS高程差分但在高程精度只有十几米的民用GPS信号下几十米的距离内差分出来的坡度误差可能达到百分之二到百分之三这个误差对坡道阻力计算的影响已经足够让质量估计跑偏。反过来如果先估质量把坡度当噪声处理滤波器的噪声阵就得调得很大结果就是质量估计对真实质量变化的响应变迟钝。举个具体的例子一辆重载卡车在6%坡道上匀速爬坡。此时加速度为零纵向动力学方程里只剩下驱动力、空气阻力、滚动阻力和坡道阻力。如果有滤波方案误把坡度忽略掉驱动器输出的驱动力会被全部归因于质量算出来的质量将是真实质量的数倍。这不是算法调参能解决的问题而是模型结构决定的。只要把坡度和质量一起放进状态向量滤波器就可以通过动力学方程的内在约束在加速度和坡度阻力之间做概率意义上的合理分配。2. 纵向动力学模型把受力平衡写成卡尔曼滤波能吃的形式在写任何滤波算法之前先把车辆纵向受力图画清楚。整个估计方案的精度上限在模型这一步就已经定了后端的滤波器只能尽力逼近这个上限不能突破它。2.1 车辆从发动机到路面的力传递链车辆纵向动力学方程可以写成如下形式[ m \dot{v} F_t - F_{aero} - F_{roll} - F_{grade} ]其中 (F_t) 是驱动力(F_{aero}) 是空气阻力(F_{roll}) 是滚动阻力(F_{grade}) 是坡道阻力。驱动力 (F_t) 的来源需要根据车辆类型区分。传统燃油车或混合动力车驱动力等于发动机扭矩经过变速器传动比、主减速比和传动效率折算到轮端再除以轮胎滚动半径[ F_t \frac{T_{engine} \cdot i_g \cdot i_0 \cdot \eta}{r_w} ]在这个公式里(T_{engine}) 来自发动机或电机控制器上报的实时扭矩(i_g) 是当前挡位传动比(i_0) 是主减速比(\eta) 是传动效率(r_w) 是车轮滚动半径。对于纯电动车式子里的 (T_{engine}) 换成电机扭矩(i_g) 视为固定减速比即可。这里的每一项都是已知量或可以近似为已知量是整个模型里精度较高的环节。空气阻力 (F_{aero}) 为[ F_{aero} \frac{1}{2} \rho C_d A v^2 ](\rho) 是空气密度(C_d) 是风阻系数(A) 是迎风面积(v) 是车速。空气阻力只随车速平方变化在高速行驶时占比很大低速时可以忽略。滚动阻力 (F_{roll}) 为[ F_{roll} m g f_r ](f_r) 是滚动阻力系数一般取0.008到0.012与轮胎类型和路面附着有关。这一项里出现了质量 (m)但系数相对稳定不会对估计造成病态影响。坡道阻力 (F_{grade}) 为[ F_{grade} m g \sin\theta ](\theta) 是道路坡度角(\sin\theta) 在实际坡度小于10%时可以近似等于坡度百分比。这一项同样包含质量 (m)这就是耦合的根源。2.2 可测项与不可测项的划分在卡尔曼滤波的框架里我们需要把式子里的量分为三类。第一类是已知输入包括驱动力 (F_t) 和车速 (v)。驱动力由控制器上报扭矩计算车速由轮速传感器或GPS测得。第二类是待估计状态即车辆质量 (m) 和道路坡度 (\theta)。第三类是模型噪声包括路面颠簸、风阻扰动、扭矩波动等无法建模的随机误差。值得注意的是车速 (v) 本身也作为状态量放进去。原因在于速度的微分加速度是动力学方程的核心输出把速度直接作为状态量可以利用滤波的预测-校正机制对原始轮速信号做平滑同时为质量和坡度提供间接观测。最终的观测方程只需要一个传感器量车速 (v)。这是一种最小观测量方案只依赖轮速信号就能工作。不过我在实际项目中很快发现这种最小方案在部分工况下可观测性很弱收敛速度也偏慢。所以工程上更稳妥的做法是引入IMU的纵向加速度信号作为第二个观测量。IMU测量的加速度包含重力沿坡道的分量可以提供一个和坡度直接相关的信息。要注意的是IMU测量的是加速度计敏感轴方向的加速度包含重力分量和车体真实加速度两个分量耦合在一起需要利用车辆纵向动力学方程来约束。这一点在第五节里我会专门展开。2.3 为什么是扩展卡尔曼滤波而不是普通卡尔曼滤波普通卡尔曼滤波假设系统是线性的即状态转移矩阵和观测矩阵都是常系数矩阵。但我们的状态方程里(m) 出现在分母上(\sin\theta) 出现在驱动项中状态量和扰动量相互乘在一起属于典型的非线性系统。如果强行做线性化近似在坡度较大或质量变化明显的工况下模型误差会超过滤波噪声导致估计值发散或振荡。这并不是调大过程噪声就能解决的因为误差是模型本身的线性化误差而不是随机扰动。扩展卡尔曼滤波的核心思想是在每一个时间步把非线性方程在当前状态估计值附近做一阶泰勒展开求出雅可比矩阵然后套用卡尔曼滤波的递推框架。这个思路对本文的场景很合适因为车辆质量和坡度虽然存在非线性耦合但它们的动态特性相对平稳在一阶近似下已经能获得很好的精度。更复杂的无迹卡尔曼滤波对非线性程度的容忍度更高但在实际工程中计算量更大调参更繁琐对嵌入式控制器的算力也是负担。我做对比测试时发现在6%以上的大坡道、满载变工况场景下EKF和UKF的估计精度差距在3%以内完全在工程接受范围内所以EKF是性价比最高的选择。3. EKF状态方程构建与雅可比矩阵推导这一节是整个方案的核心步骤。我会从状态量选取开始逐步写出状态方程、观测方程再到雅可比矩阵的每一行每一列最后给出噪声矩阵设置的参考思路。代码层面大家可以直接照着这段推导写。3.1 状态量与离散状态方程选择状态向量为[ x \begin{bmatrix} v m \theta \end{bmatrix}^T ]其中 (v) 是纵向车速单位为m/s(m) 是车辆总质量单位kg(\theta) 是道路坡度角单位rad。连续时间状态方程为[ \dot{v} \frac{F_t - F_{aero} - F_{roll} - m g \sin\theta}{m} ][ \dot{m} 0 ][ \dot{\theta} 0 ]质量 (m) 和坡度 (\theta) 的变化率设为零表示它们在一次估计周期内视为缓慢变化的常量真正的变化由过程噪声来吸收。这是参数估计类问题中常见的做法相当于让滤波器自己去跟随参数的变化趋势有变化时通过新息修正来调整估计值。离散化采用欧拉法采样周期为 (T_s)[ v_{k} v_{k-1} T_s \frac{F_t - \frac{1}{2}\rho C_d A v_{k-1}^2 - m_{k-1} g f_r - m_{k-1} g \sin\theta_{k-1}}{m_{k-1}} ][ m_k m_{k-1} ][ \theta_k \theta_{k-1} ]观测方程为[ y_k v_k r_k ]这里的观测只有一个量车速。采样周期我建议取10ms到50ms。太短了状态方程中的过程噪声影响会被放大太长了坡度和质量变化的响应会迟钝。商用车控制器里我一般取20ms这是一个平衡值。3.2 雅可比矩阵的逐一推导EKF和普通KF的唯一区别就在于用雅可比矩阵代替了常系数的状态转移矩阵。我们需要计算状态转移函数对状态向量的偏导数即[ F_k \frac{\partial f}{\partial x}\bigg|{\hat{x}{k-1}} ]对三个状态分量分别求偏导。第一个状态是车速 (v)它对自身求偏导来自空气阻力项。因为 (F_{aero}) 与 (v^2) 成正比所以[ \frac{\partial \dot{v}}{\partial v} -\frac{\rho C_d A v}{m} ]对质量 (m) 求偏导。注意质量出现在分母上且滚动阻力和坡道阻力都包含质量需要分项求导[ \frac{\partial \dot{v}}{\partial m} \frac{F_{aero}}{m^2} - \frac{g f_r}{m} - \frac{g \sin\theta}{m} ]这里有一个正项 (F_{aero}/m^2)来源于驱动力减去空气阻力后除以质量的复合函数求导实际数值一般很小但严谨推导时必须带上。对坡度 (\theta) 求偏导[ \frac{\partial \dot{v}}{\partial \theta} -g \cos\theta ]第二个状态 (m) 和第三个状态 (\theta) 的导数恒为零所以它们对应的雅可比行只有对角元为1。最终离散后的雅可比矩阵为[ F_k \begin{bmatrix} 1 - T_s \frac{\rho C_d A \hat{v}}{m} T_s\left(\frac{F_{aero}}{m^2} - \frac{g f_r}{m} - \frac{g \sin\theta}{m}\right) -T_s g \cos\theta \ 0 1 0 \ 0 0 1 \end{bmatrix} ]观测矩阵是[ H_k \begin{bmatrix} 1 0 0 \end{bmatrix} ]因为观测方程是线性的所以观测矩阵直接就是常系数向量。推导过程中最容易出错的是第一行第二列的符号。因为 (F_t) 本身不显含 (m)只有阻力项含质量所以对 (m) 求偏导时需要同时处理 (F_{aero}/m)、(g f_r) 和 (g \sin\theta) 三个项。我一开始写代码时漏掉了空气阻力那一项仿真结果在120km/h高速工况下质量估计会小幅度持续偏移后来逐项核对雅可比才发现问题。3.3 噪声协方差矩阵与初值设置EKF还需要对角设置过程噪声协方差矩阵 (Q) 和观测噪声协方差矩阵 (R)。这三者的物理含义要搞清楚过程噪声描述状态方程模型误差的大小观测噪声描述传感器测量误差的大小。我的建议是[ Q \begin{bmatrix} q_v^2 0 0 \ 0 q_m^2 0 \ 0 0 q_\theta^2 \end{bmatrix} ]初值参考(q_v 0.1) m/s²(q_m) 取1到10 kg/s(q_\theta) 取0.0001到0.001 rad/s。(q_m) 太小会导致质量收敛后难以跟随装卸货造成的真实质量跳变太大会导致质量在平路上来回摆。我实际标定时通常把 (q_m) 调到在平路匀速工况下质量估计的波动幅度不超过满载质量的2%。观测噪声 (R) 直接取轮速传感器折算车速后的噪声方差。轮速信号分辨率较高一般取 (R 0.01) 到 0.05 ((m/s)^2)。如果混入了GPS速度数据要特别注意GPS在城市峡谷环境下有粗差可以把GPS作为辅助观测但不要把它的噪声方差设得太小否则容易把离散点引入质量估计。初值的设置也有讲究。质量初值可以直接用空载质量和满载质量的平均值坡度初值设0。初值误差由滤波器自身收敛过程消除但如果初值偏差过大收敛时间会明显拉长且在坡道上会先出现一段质量快速变化再逐步回调的过程。工程上可以把上一次停车前的估计值保存下来下次上电直接作为初值加载这样每次车辆的载荷变化不会导致滤波器从零开始。4. 三种典型工况下的估计结果拆解仿真验证是EKF方案落地的必经步骤。我根据实际驾驶中频率最高的三种场景分别构造了平路匀加速、固定坡度上坡、变坡度变踏板三个工况来观察质量和坡度的估计表现。下面逐个拆解结果和背后反映出的算法特性。4.1 平路匀加速质量估计的收敛表现设一辆质量12吨的车辆在平直道路上以0.5 m/s²匀加速行驶30秒车速从10m/s升至25m/s。滤波器中质量初值设为20吨坡度初值0.01rad。期望看到的是质量快速从20吨向12吨收敛坡度向0收敛。仿真结果显示质量在10秒内从20吨降到15吨20秒后接近12.5吨30秒时稳定在12.1吨左右。收敛过程是单调的没有超调。坡度基本在前5秒就归零到0附近。这个结果验证了一个关键性质平路工况下质量的可观测性较强。因为加速度直接反映了驱动力与总阻力之差而总阻力中的坡道阻力为零质量是唯一影响加速度-驱动力比例的因素。滤波器可以比较准确地分离出质量。这里有一个值得注意的细节质量收敛速度与加速度大小相关。加速度越大驱动力中加速分量占比越高质量估计的收敛越快。如果车辆只在匀速巡航驱动力全部用于平衡阻力此时坡度和质量都不可观测估计结果会停留在最近一次加速工况的数值直到进入新的加速或爬坡工况。这在实际使用中不是问题因为整车的质量在一次行驶周期内本就不变只要每次行程中有一次明显加速或爬坡就能刷新估计值。4.2 固定坡度上坡质量与坡度如何纠缠设同一辆车以恒定驱动力在5%坡度上坡初始车速20m/s质量为12吨。滤波器的质量初值设为15吨坡度初值0。车辆在坡道上持续行驶120秒。仿真结果表明质量和坡度在前20秒同时调整质量从15吨下降并向12吨靠近坡度从0上升并逐渐接近0.05rad。最终两端稳定在质量12.3吨、坡度0.049rad误差都在可接受范围内。这个工况是EKF的核心优势所在。如果只估质量不估坡度15吨的初值在坡道阻力下会被持续拉高而联合估计框架让滤波器内部自动分配误差来源有一部分新息归入坡度修正有一部分归入质量修正最终收敛结果能同时逼近真实值。不过固定坡度上坡工况也暴露了一个软肋如果在整个过程中车辆始终保持匀速那么质量和坡度之间存在真实的比例混淆无法唯一分离。只有车辆在坡道上有加速度变化油门变化、换挡或坡度本身变化时才能提供足够的激励来同时锁死两个参数。这一点对于实际标定非常关键坡道上质量估计要准不能光靠一段匀速爬坡最好是有加速爬坡和减速爬坡交替。4.3 变坡度变踏板可观测性与收敛速度的极限测试第三个工况模拟一段真实山路前60秒平路加速中间60秒进入6%上坡并伴随小幅度油门调整最后60秒变成4%下坡并减小油门。这一步主要是验证滤波器的动态跟踪能力。结果中质量在前60秒平路段完成初步收敛到12.5吨左右进入上坡段后因为坡度迅速增大质量先出现了一个约0.5吨的短暂偏移随后在坡度和质量之间重新分配约10秒后恢复到12.2吨。坡度估计在坡道切换时有约1秒的滞后最大误差约0.01rad。下坡段由于驱动力小动力方程中坡道阻力的占比更大坡度跟踪更快质量保持稳定。这个工况就是中可观测性理论在仿真中的直观体现。出现质量短暂偏移时不用怀疑算法坏了而是车辆动力学本身正处于信息不足的状态。要想减少这种偏移一个有效的办法是我在5.2节里讲的——引入IMU信号做前馈让坡度的变化信息更早进入滤波器。5. 实车落地中最容易踩的五个坑做完仿真只是第一步。把EKF从仿真代码搬到实车控制器里才是考验真正开始的地方。下面这五个坑我基本都踩过一遍每一个都花了不少时间排查写出来给大家省点时间。5.1 轮速信号在低速和颠簸路面上会让状态方程发疯轮速传感器在低速时脉冲计数不稳定在颠簸路面上轮胎与地面产生滑移和弹跳这些都会让速度观测量里混入异常的可信的虚假信息。EKF的新息对这一类异常值非常敏感一个离群点就能让质量估计产生明显的跳变。我在实车测试初期车辆过减速带时质量估计会突然往下掉一大截然后花十几秒慢慢恢复。排查后确认是轮速信号的瞬时跳动直接进入了观测方程。解决办法是加一层测量值预检计算轮速速度的变化率如果超过车辆物理能到达的最大加速度就认为该点异常用上一拍的观测值代替或者把这个异常点的新息权重置为零。这一层预检不到十行代码但对滤波稳定性贡献巨大。5.2 IMU纵向加速度如果不用等于浪费了一半的观测量我在2.2节里提到最小观测量方案只用车速实车验证后发现仅有车速信号时质量初值误差过大的场景下收敛时间有些偏长。后来我把IMU的纵向加速度信号引入进来作为第二个观测量效果立刻好了很多。IMU的纵向加速度输出 (\tilde{a}_{imu}) 与车辆真实速度之间存在如下关系[ \tilde{a}_{imu} \dot{v} g \sin\theta ]根据动力学方程(\dot{v} \frac{F_t - F_{aero} - F_{roll}}{m} - g\sin\theta)所以[ \tilde{a}{imu} \frac{F_t - F{aero} - F_{roll}}{m} ]从这里可以直观看出IMU信号的奥妙它直接反映了驱动力减去空气阻力和滚动阻力后的净比力不包含坡道阻力项。这意味着IMU加速度观测为质量估计提供了一个直接的线性观测通道而坡度则由车速和IMU信号的差值决定。这样质量与坡度之间的耦合被大幅度解耦了。但在实际数据同步上要特别注意IMU安装角偏差。如果IMU在车身上倾斜安装它的纵向敏感轴会和车辆纵轴有一个固定夹角这个夹角会导致常值偏置必须通过静置标定获取安装角补偿值。我不止一次看到有人忽略这个标定结果坡度估计始终存在一个固定偏差。5.3 驱动力计算误差是最大的隐性误差源模型里驱动力 (F_t) 是通过扭矩查询和传动比折算得到的但传动效率 (\eta) 并不是恒定值。液力变矩器的不同工况下效率波动很大甚至在低速大扭矩工况效率可以降到70%。如果在计算驱动力时用一个固定效率在低速爬坡时驱动力就会被系统性低估质量估计随之偏大。我的做法是在控制器里根据涡轮泵轮转速比查效率表而不是用一个恒定值。如果没有现成的效率表可以让整车厂提供实验数据或者用反推法在空载平路工况下标定传动效率把驱动力计算模型里所有固定参数整合成一个等效参数用实验数据拟合它随车速和挡位变化的曲线。这类隐式标定做得好不好直接决定了EKF估计结果的精度上限。另外别忘了刹车拖滞扭矩。在实测中即使没有踩刹车制动器由于热变形和液压残留也会有少量拖滞扭矩。这个量在小驱动力工况下占比很大会让质量估计产生明显偏置。建议在驱动力计算中加上一个与制动压力相关的修正项如果制动压力为零拖滞扭矩也并非零。5.4 时间戳不同步在坡顶坡谷处会放大误差控制器里CAN信号、GPS信号和IMU信号往往走不同的采样通道到达算法模块的时间不一样。如果直接使用未对齐的信号坡度和质量估计会在坡顶坡谷处出现明显的瞬态偏差。我遇到的实际情况是IMU数据更新频率为50HzCAN速度信号为10ms周期两者之间的固定时间偏置约60ms。在完整的一次山路测试中这60ms偏移产生的坡度估计误差在路上正常起伏下尚不明显但在快速变化的坡顶坡谷区域坡度估计会出现一个峰峰值接近1%的振荡。排查思路简单对每个信号记录硬件时间戳在进入滤波器之前按主时间轴对齐。工程上最实用的做法是把IMU信号做线性插值到CAN信号的采样时间点或者反过来保证滤波器的输入是同步后的数据。5.5 输出不做限幅与平滑控制策略根本不敢用EKF输出的质量估计值虽然整体收敛但在某些工况下会有高频抖动坡度输出也会有短时尖峰。底层控制器如果直接消费这些原始估计值换挡策略会在目标挡位之间来回摇摆巡航控制也会出现油门波动。我的处理分两步。第一步是限幅质量的变化速率限制在实际物理极限内比如满载40吨的车单次采样周期质量变化不超过0.5吨坡度变化速率限制在每秒0.02rad以内。第二步是平滑在限幅之后再通过一阶低通滤波截止频率设在0.5Hz左右。这样既保留了真实变化趋势又去掉了影响控制的抖动。注意平滑不能过度否则换挡策略会感到质量变化响应太慢达不到实时性要求。在输出限幅的背后还有一个工程细节把EKF的估计结果和整车其他信号源的结果做一个仲裁。比如整车如果装配了空气悬架可以直接通过悬架高度传感器和空气压力传感器换算出轴荷得到精度较高的质量参考值。EKF结果可以向这个参考值做缓慢修正而不是完全依赖动力学估计。这种多信息源融合的思路比单靠EKF一条路走到底可靠得多。我在实车测试里得到的体感是这套方案在硬化路面、一般天气条件下完全够用而且不依赖额外的GPS高程数据只靠车上的CAN信号和IMU就能稳定工作。它也不是万能的在极低附着路面冰雪、湿滑钢板、急刹车和碰撞等极端动态场景下轮胎力饱和会让纵向动力学模型失效估计结果自然不可信。做工程应用时一定要给算法配一个置信度输出告诉上层控制策略当前估计值是否值得信任而不是让上层控制盲目依赖它。
基于扩展卡尔曼滤波的车辆质量与道路坡度联合估计
1. 同时估计质量和坡度这个账到底难算在哪里做车辆控制特别是商用车动力系统标定和智能驾驶决策算法的朋友基本都绕不过两个看不见的量车辆质量和道路坡度。AMT变速箱在坡道上换挡得提前预判车辆能不能加速冲上去这需要质量来算加速度巡航系统在上坡前决定是否降挡需要坡度来判断坡道阻力电动车的续航估算和制动能量回收策略也需要质量来推算剩余里程和回收扭矩。可以说一堆上层功能都在等这两个数但这两个数偏偏是整车状态里最难直接测量的。我最早做质量估计时方案很单纯只估质量用普通卡尔曼滤波把坡度当干扰项。结果一上3%的坡估计出来的质量直接胖了十几吨逻辑其实很简单坡道阻力被滤波器当成了一部分惯性力算进质量里去了。反过来如果只估坡度而忽略质量偏差空载或满载状态下的坡度估计值同样会偏因为这个坡度里掺了真实的加速度误差。这就是为什么要用基于扩展卡尔曼滤波的方案把质量和坡度放在同一个状态向量里联合估计让滤波器在解算过程中自己去权衡哪个量对应质量、哪个量对应坡度。文章里我会把这套方案的纵向动力学建模、状态方程与雅可比矩阵推导、三个典型仿真工况的结果拆解以及实车落地时我自己踩过的几个坑一次性讲清楚。适合正在做车辆状态估计、商用车整车控制器开发、智能驾驶决策或相关课题研究的朋友参考在校生做课题选型时也可以拿这篇当作入门路线图。1.1 换挡策略、坡道辅助和续航估算都在等这两个数先说结论质量主要影响驱动力里有多少分给了加速度坡度主要影响驱动力里有多少分给了坡道阻力。这两者的数学表达在纵向动力学方程里天然耦合。具体到应用场景AMT换挡策略需要实时预判车辆在当前挡位下的最大可达到加速度。如果质量估计值偏大系统会认为车辆动力不足该升挡的时候不升挡如果偏小又会过早升挡导致拖挡甚至熄火。坡度估计值则直接决定上坡辅助功能在坡道上释放刹车的时机以及巡航系统在坡顶坡谷处的油门和挡位预调节。智能驾驶的纵向规划也会依赖这两个参数做加速度边界计算尤其商用车满载四十吨和空载十二吨时制动距离和加速能力的差异是数量级的模型里必须有一个能跟随装载变化的质量参数。这里有一个容易被忽视的细节质量和坡度不是稳定不变的。质量在每次装卸货后发生跳变在一次行驶周期内基本恒定坡度则随着道路起伏持续变化但变化频率很低。这两个量在时间尺度上的差异正好给了联合估计一个突破口。把变化极慢的质量和缓变的坡度都作为待估计状态把速度和驱动力作为可测输入就可以在一个统一的滤波框架里同时完成辨识和跟踪。1.2 为什么不能先估坡度再估质量或者反过来如果把问题拆开先单独估计坡度再用坡度修正质量听起来似乎可行实际上会遇到鸡生蛋蛋生鸡的问题。坡度估计通常可以基于GPS高程差分但在高程精度只有十几米的民用GPS信号下几十米的距离内差分出来的坡度误差可能达到百分之二到百分之三这个误差对坡道阻力计算的影响已经足够让质量估计跑偏。反过来如果先估质量把坡度当噪声处理滤波器的噪声阵就得调得很大结果就是质量估计对真实质量变化的响应变迟钝。举个具体的例子一辆重载卡车在6%坡道上匀速爬坡。此时加速度为零纵向动力学方程里只剩下驱动力、空气阻力、滚动阻力和坡道阻力。如果有滤波方案误把坡度忽略掉驱动器输出的驱动力会被全部归因于质量算出来的质量将是真实质量的数倍。这不是算法调参能解决的问题而是模型结构决定的。只要把坡度和质量一起放进状态向量滤波器就可以通过动力学方程的内在约束在加速度和坡度阻力之间做概率意义上的合理分配。2. 纵向动力学模型把受力平衡写成卡尔曼滤波能吃的形式在写任何滤波算法之前先把车辆纵向受力图画清楚。整个估计方案的精度上限在模型这一步就已经定了后端的滤波器只能尽力逼近这个上限不能突破它。2.1 车辆从发动机到路面的力传递链车辆纵向动力学方程可以写成如下形式[ m \dot{v} F_t - F_{aero} - F_{roll} - F_{grade} ]其中 (F_t) 是驱动力(F_{aero}) 是空气阻力(F_{roll}) 是滚动阻力(F_{grade}) 是坡道阻力。驱动力 (F_t) 的来源需要根据车辆类型区分。传统燃油车或混合动力车驱动力等于发动机扭矩经过变速器传动比、主减速比和传动效率折算到轮端再除以轮胎滚动半径[ F_t \frac{T_{engine} \cdot i_g \cdot i_0 \cdot \eta}{r_w} ]在这个公式里(T_{engine}) 来自发动机或电机控制器上报的实时扭矩(i_g) 是当前挡位传动比(i_0) 是主减速比(\eta) 是传动效率(r_w) 是车轮滚动半径。对于纯电动车式子里的 (T_{engine}) 换成电机扭矩(i_g) 视为固定减速比即可。这里的每一项都是已知量或可以近似为已知量是整个模型里精度较高的环节。空气阻力 (F_{aero}) 为[ F_{aero} \frac{1}{2} \rho C_d A v^2 ](\rho) 是空气密度(C_d) 是风阻系数(A) 是迎风面积(v) 是车速。空气阻力只随车速平方变化在高速行驶时占比很大低速时可以忽略。滚动阻力 (F_{roll}) 为[ F_{roll} m g f_r ](f_r) 是滚动阻力系数一般取0.008到0.012与轮胎类型和路面附着有关。这一项里出现了质量 (m)但系数相对稳定不会对估计造成病态影响。坡道阻力 (F_{grade}) 为[ F_{grade} m g \sin\theta ](\theta) 是道路坡度角(\sin\theta) 在实际坡度小于10%时可以近似等于坡度百分比。这一项同样包含质量 (m)这就是耦合的根源。2.2 可测项与不可测项的划分在卡尔曼滤波的框架里我们需要把式子里的量分为三类。第一类是已知输入包括驱动力 (F_t) 和车速 (v)。驱动力由控制器上报扭矩计算车速由轮速传感器或GPS测得。第二类是待估计状态即车辆质量 (m) 和道路坡度 (\theta)。第三类是模型噪声包括路面颠簸、风阻扰动、扭矩波动等无法建模的随机误差。值得注意的是车速 (v) 本身也作为状态量放进去。原因在于速度的微分加速度是动力学方程的核心输出把速度直接作为状态量可以利用滤波的预测-校正机制对原始轮速信号做平滑同时为质量和坡度提供间接观测。最终的观测方程只需要一个传感器量车速 (v)。这是一种最小观测量方案只依赖轮速信号就能工作。不过我在实际项目中很快发现这种最小方案在部分工况下可观测性很弱收敛速度也偏慢。所以工程上更稳妥的做法是引入IMU的纵向加速度信号作为第二个观测量。IMU测量的加速度包含重力沿坡道的分量可以提供一个和坡度直接相关的信息。要注意的是IMU测量的是加速度计敏感轴方向的加速度包含重力分量和车体真实加速度两个分量耦合在一起需要利用车辆纵向动力学方程来约束。这一点在第五节里我会专门展开。2.3 为什么是扩展卡尔曼滤波而不是普通卡尔曼滤波普通卡尔曼滤波假设系统是线性的即状态转移矩阵和观测矩阵都是常系数矩阵。但我们的状态方程里(m) 出现在分母上(\sin\theta) 出现在驱动项中状态量和扰动量相互乘在一起属于典型的非线性系统。如果强行做线性化近似在坡度较大或质量变化明显的工况下模型误差会超过滤波噪声导致估计值发散或振荡。这并不是调大过程噪声就能解决的因为误差是模型本身的线性化误差而不是随机扰动。扩展卡尔曼滤波的核心思想是在每一个时间步把非线性方程在当前状态估计值附近做一阶泰勒展开求出雅可比矩阵然后套用卡尔曼滤波的递推框架。这个思路对本文的场景很合适因为车辆质量和坡度虽然存在非线性耦合但它们的动态特性相对平稳在一阶近似下已经能获得很好的精度。更复杂的无迹卡尔曼滤波对非线性程度的容忍度更高但在实际工程中计算量更大调参更繁琐对嵌入式控制器的算力也是负担。我做对比测试时发现在6%以上的大坡道、满载变工况场景下EKF和UKF的估计精度差距在3%以内完全在工程接受范围内所以EKF是性价比最高的选择。3. EKF状态方程构建与雅可比矩阵推导这一节是整个方案的核心步骤。我会从状态量选取开始逐步写出状态方程、观测方程再到雅可比矩阵的每一行每一列最后给出噪声矩阵设置的参考思路。代码层面大家可以直接照着这段推导写。3.1 状态量与离散状态方程选择状态向量为[ x \begin{bmatrix} v m \theta \end{bmatrix}^T ]其中 (v) 是纵向车速单位为m/s(m) 是车辆总质量单位kg(\theta) 是道路坡度角单位rad。连续时间状态方程为[ \dot{v} \frac{F_t - F_{aero} - F_{roll} - m g \sin\theta}{m} ][ \dot{m} 0 ][ \dot{\theta} 0 ]质量 (m) 和坡度 (\theta) 的变化率设为零表示它们在一次估计周期内视为缓慢变化的常量真正的变化由过程噪声来吸收。这是参数估计类问题中常见的做法相当于让滤波器自己去跟随参数的变化趋势有变化时通过新息修正来调整估计值。离散化采用欧拉法采样周期为 (T_s)[ v_{k} v_{k-1} T_s \frac{F_t - \frac{1}{2}\rho C_d A v_{k-1}^2 - m_{k-1} g f_r - m_{k-1} g \sin\theta_{k-1}}{m_{k-1}} ][ m_k m_{k-1} ][ \theta_k \theta_{k-1} ]观测方程为[ y_k v_k r_k ]这里的观测只有一个量车速。采样周期我建议取10ms到50ms。太短了状态方程中的过程噪声影响会被放大太长了坡度和质量变化的响应会迟钝。商用车控制器里我一般取20ms这是一个平衡值。3.2 雅可比矩阵的逐一推导EKF和普通KF的唯一区别就在于用雅可比矩阵代替了常系数的状态转移矩阵。我们需要计算状态转移函数对状态向量的偏导数即[ F_k \frac{\partial f}{\partial x}\bigg|{\hat{x}{k-1}} ]对三个状态分量分别求偏导。第一个状态是车速 (v)它对自身求偏导来自空气阻力项。因为 (F_{aero}) 与 (v^2) 成正比所以[ \frac{\partial \dot{v}}{\partial v} -\frac{\rho C_d A v}{m} ]对质量 (m) 求偏导。注意质量出现在分母上且滚动阻力和坡道阻力都包含质量需要分项求导[ \frac{\partial \dot{v}}{\partial m} \frac{F_{aero}}{m^2} - \frac{g f_r}{m} - \frac{g \sin\theta}{m} ]这里有一个正项 (F_{aero}/m^2)来源于驱动力减去空气阻力后除以质量的复合函数求导实际数值一般很小但严谨推导时必须带上。对坡度 (\theta) 求偏导[ \frac{\partial \dot{v}}{\partial \theta} -g \cos\theta ]第二个状态 (m) 和第三个状态 (\theta) 的导数恒为零所以它们对应的雅可比行只有对角元为1。最终离散后的雅可比矩阵为[ F_k \begin{bmatrix} 1 - T_s \frac{\rho C_d A \hat{v}}{m} T_s\left(\frac{F_{aero}}{m^2} - \frac{g f_r}{m} - \frac{g \sin\theta}{m}\right) -T_s g \cos\theta \ 0 1 0 \ 0 0 1 \end{bmatrix} ]观测矩阵是[ H_k \begin{bmatrix} 1 0 0 \end{bmatrix} ]因为观测方程是线性的所以观测矩阵直接就是常系数向量。推导过程中最容易出错的是第一行第二列的符号。因为 (F_t) 本身不显含 (m)只有阻力项含质量所以对 (m) 求偏导时需要同时处理 (F_{aero}/m)、(g f_r) 和 (g \sin\theta) 三个项。我一开始写代码时漏掉了空气阻力那一项仿真结果在120km/h高速工况下质量估计会小幅度持续偏移后来逐项核对雅可比才发现问题。3.3 噪声协方差矩阵与初值设置EKF还需要对角设置过程噪声协方差矩阵 (Q) 和观测噪声协方差矩阵 (R)。这三者的物理含义要搞清楚过程噪声描述状态方程模型误差的大小观测噪声描述传感器测量误差的大小。我的建议是[ Q \begin{bmatrix} q_v^2 0 0 \ 0 q_m^2 0 \ 0 0 q_\theta^2 \end{bmatrix} ]初值参考(q_v 0.1) m/s²(q_m) 取1到10 kg/s(q_\theta) 取0.0001到0.001 rad/s。(q_m) 太小会导致质量收敛后难以跟随装卸货造成的真实质量跳变太大会导致质量在平路上来回摆。我实际标定时通常把 (q_m) 调到在平路匀速工况下质量估计的波动幅度不超过满载质量的2%。观测噪声 (R) 直接取轮速传感器折算车速后的噪声方差。轮速信号分辨率较高一般取 (R 0.01) 到 0.05 ((m/s)^2)。如果混入了GPS速度数据要特别注意GPS在城市峡谷环境下有粗差可以把GPS作为辅助观测但不要把它的噪声方差设得太小否则容易把离散点引入质量估计。初值的设置也有讲究。质量初值可以直接用空载质量和满载质量的平均值坡度初值设0。初值误差由滤波器自身收敛过程消除但如果初值偏差过大收敛时间会明显拉长且在坡道上会先出现一段质量快速变化再逐步回调的过程。工程上可以把上一次停车前的估计值保存下来下次上电直接作为初值加载这样每次车辆的载荷变化不会导致滤波器从零开始。4. 三种典型工况下的估计结果拆解仿真验证是EKF方案落地的必经步骤。我根据实际驾驶中频率最高的三种场景分别构造了平路匀加速、固定坡度上坡、变坡度变踏板三个工况来观察质量和坡度的估计表现。下面逐个拆解结果和背后反映出的算法特性。4.1 平路匀加速质量估计的收敛表现设一辆质量12吨的车辆在平直道路上以0.5 m/s²匀加速行驶30秒车速从10m/s升至25m/s。滤波器中质量初值设为20吨坡度初值0.01rad。期望看到的是质量快速从20吨向12吨收敛坡度向0收敛。仿真结果显示质量在10秒内从20吨降到15吨20秒后接近12.5吨30秒时稳定在12.1吨左右。收敛过程是单调的没有超调。坡度基本在前5秒就归零到0附近。这个结果验证了一个关键性质平路工况下质量的可观测性较强。因为加速度直接反映了驱动力与总阻力之差而总阻力中的坡道阻力为零质量是唯一影响加速度-驱动力比例的因素。滤波器可以比较准确地分离出质量。这里有一个值得注意的细节质量收敛速度与加速度大小相关。加速度越大驱动力中加速分量占比越高质量估计的收敛越快。如果车辆只在匀速巡航驱动力全部用于平衡阻力此时坡度和质量都不可观测估计结果会停留在最近一次加速工况的数值直到进入新的加速或爬坡工况。这在实际使用中不是问题因为整车的质量在一次行驶周期内本就不变只要每次行程中有一次明显加速或爬坡就能刷新估计值。4.2 固定坡度上坡质量与坡度如何纠缠设同一辆车以恒定驱动力在5%坡度上坡初始车速20m/s质量为12吨。滤波器的质量初值设为15吨坡度初值0。车辆在坡道上持续行驶120秒。仿真结果表明质量和坡度在前20秒同时调整质量从15吨下降并向12吨靠近坡度从0上升并逐渐接近0.05rad。最终两端稳定在质量12.3吨、坡度0.049rad误差都在可接受范围内。这个工况是EKF的核心优势所在。如果只估质量不估坡度15吨的初值在坡道阻力下会被持续拉高而联合估计框架让滤波器内部自动分配误差来源有一部分新息归入坡度修正有一部分归入质量修正最终收敛结果能同时逼近真实值。不过固定坡度上坡工况也暴露了一个软肋如果在整个过程中车辆始终保持匀速那么质量和坡度之间存在真实的比例混淆无法唯一分离。只有车辆在坡道上有加速度变化油门变化、换挡或坡度本身变化时才能提供足够的激励来同时锁死两个参数。这一点对于实际标定非常关键坡道上质量估计要准不能光靠一段匀速爬坡最好是有加速爬坡和减速爬坡交替。4.3 变坡度变踏板可观测性与收敛速度的极限测试第三个工况模拟一段真实山路前60秒平路加速中间60秒进入6%上坡并伴随小幅度油门调整最后60秒变成4%下坡并减小油门。这一步主要是验证滤波器的动态跟踪能力。结果中质量在前60秒平路段完成初步收敛到12.5吨左右进入上坡段后因为坡度迅速增大质量先出现了一个约0.5吨的短暂偏移随后在坡度和质量之间重新分配约10秒后恢复到12.2吨。坡度估计在坡道切换时有约1秒的滞后最大误差约0.01rad。下坡段由于驱动力小动力方程中坡道阻力的占比更大坡度跟踪更快质量保持稳定。这个工况就是中可观测性理论在仿真中的直观体现。出现质量短暂偏移时不用怀疑算法坏了而是车辆动力学本身正处于信息不足的状态。要想减少这种偏移一个有效的办法是我在5.2节里讲的——引入IMU信号做前馈让坡度的变化信息更早进入滤波器。5. 实车落地中最容易踩的五个坑做完仿真只是第一步。把EKF从仿真代码搬到实车控制器里才是考验真正开始的地方。下面这五个坑我基本都踩过一遍每一个都花了不少时间排查写出来给大家省点时间。5.1 轮速信号在低速和颠簸路面上会让状态方程发疯轮速传感器在低速时脉冲计数不稳定在颠簸路面上轮胎与地面产生滑移和弹跳这些都会让速度观测量里混入异常的可信的虚假信息。EKF的新息对这一类异常值非常敏感一个离群点就能让质量估计产生明显的跳变。我在实车测试初期车辆过减速带时质量估计会突然往下掉一大截然后花十几秒慢慢恢复。排查后确认是轮速信号的瞬时跳动直接进入了观测方程。解决办法是加一层测量值预检计算轮速速度的变化率如果超过车辆物理能到达的最大加速度就认为该点异常用上一拍的观测值代替或者把这个异常点的新息权重置为零。这一层预检不到十行代码但对滤波稳定性贡献巨大。5.2 IMU纵向加速度如果不用等于浪费了一半的观测量我在2.2节里提到最小观测量方案只用车速实车验证后发现仅有车速信号时质量初值误差过大的场景下收敛时间有些偏长。后来我把IMU的纵向加速度信号引入进来作为第二个观测量效果立刻好了很多。IMU的纵向加速度输出 (\tilde{a}_{imu}) 与车辆真实速度之间存在如下关系[ \tilde{a}_{imu} \dot{v} g \sin\theta ]根据动力学方程(\dot{v} \frac{F_t - F_{aero} - F_{roll}}{m} - g\sin\theta)所以[ \tilde{a}{imu} \frac{F_t - F{aero} - F_{roll}}{m} ]从这里可以直观看出IMU信号的奥妙它直接反映了驱动力减去空气阻力和滚动阻力后的净比力不包含坡道阻力项。这意味着IMU加速度观测为质量估计提供了一个直接的线性观测通道而坡度则由车速和IMU信号的差值决定。这样质量与坡度之间的耦合被大幅度解耦了。但在实际数据同步上要特别注意IMU安装角偏差。如果IMU在车身上倾斜安装它的纵向敏感轴会和车辆纵轴有一个固定夹角这个夹角会导致常值偏置必须通过静置标定获取安装角补偿值。我不止一次看到有人忽略这个标定结果坡度估计始终存在一个固定偏差。5.3 驱动力计算误差是最大的隐性误差源模型里驱动力 (F_t) 是通过扭矩查询和传动比折算得到的但传动效率 (\eta) 并不是恒定值。液力变矩器的不同工况下效率波动很大甚至在低速大扭矩工况效率可以降到70%。如果在计算驱动力时用一个固定效率在低速爬坡时驱动力就会被系统性低估质量估计随之偏大。我的做法是在控制器里根据涡轮泵轮转速比查效率表而不是用一个恒定值。如果没有现成的效率表可以让整车厂提供实验数据或者用反推法在空载平路工况下标定传动效率把驱动力计算模型里所有固定参数整合成一个等效参数用实验数据拟合它随车速和挡位变化的曲线。这类隐式标定做得好不好直接决定了EKF估计结果的精度上限。另外别忘了刹车拖滞扭矩。在实测中即使没有踩刹车制动器由于热变形和液压残留也会有少量拖滞扭矩。这个量在小驱动力工况下占比很大会让质量估计产生明显偏置。建议在驱动力计算中加上一个与制动压力相关的修正项如果制动压力为零拖滞扭矩也并非零。5.4 时间戳不同步在坡顶坡谷处会放大误差控制器里CAN信号、GPS信号和IMU信号往往走不同的采样通道到达算法模块的时间不一样。如果直接使用未对齐的信号坡度和质量估计会在坡顶坡谷处出现明显的瞬态偏差。我遇到的实际情况是IMU数据更新频率为50HzCAN速度信号为10ms周期两者之间的固定时间偏置约60ms。在完整的一次山路测试中这60ms偏移产生的坡度估计误差在路上正常起伏下尚不明显但在快速变化的坡顶坡谷区域坡度估计会出现一个峰峰值接近1%的振荡。排查思路简单对每个信号记录硬件时间戳在进入滤波器之前按主时间轴对齐。工程上最实用的做法是把IMU信号做线性插值到CAN信号的采样时间点或者反过来保证滤波器的输入是同步后的数据。5.5 输出不做限幅与平滑控制策略根本不敢用EKF输出的质量估计值虽然整体收敛但在某些工况下会有高频抖动坡度输出也会有短时尖峰。底层控制器如果直接消费这些原始估计值换挡策略会在目标挡位之间来回摇摆巡航控制也会出现油门波动。我的处理分两步。第一步是限幅质量的变化速率限制在实际物理极限内比如满载40吨的车单次采样周期质量变化不超过0.5吨坡度变化速率限制在每秒0.02rad以内。第二步是平滑在限幅之后再通过一阶低通滤波截止频率设在0.5Hz左右。这样既保留了真实变化趋势又去掉了影响控制的抖动。注意平滑不能过度否则换挡策略会感到质量变化响应太慢达不到实时性要求。在输出限幅的背后还有一个工程细节把EKF的估计结果和整车其他信号源的结果做一个仲裁。比如整车如果装配了空气悬架可以直接通过悬架高度传感器和空气压力传感器换算出轴荷得到精度较高的质量参考值。EKF结果可以向这个参考值做缓慢修正而不是完全依赖动力学估计。这种多信息源融合的思路比单靠EKF一条路走到底可靠得多。我在实车测试里得到的体感是这套方案在硬化路面、一般天气条件下完全够用而且不依赖额外的GPS高程数据只靠车上的CAN信号和IMU就能稳定工作。它也不是万能的在极低附着路面冰雪、湿滑钢板、急刹车和碰撞等极端动态场景下轮胎力饱和会让纵向动力学模型失效估计结果自然不可信。做工程应用时一定要给算法配一个置信度输出告诉上层控制策略当前估计值是否值得信任而不是让上层控制盲目依赖它。
