3. 运动学模型与状态估计：匀加速运动模型、恒定加速度与恒定 jerk 模型、卡尔曼滤波在速度估计中的应用
各位同学，欢迎来到第三章。
这一章，我们聊聊运动学模型和状态估计。说白了，就是怎么用数学描述一辆车是怎么动的，以及怎么从有噪声的传感器数据里，把真实速度给“猜”出来。
我个人觉得，这部分是纵向控制的基础。你连车下一秒在哪都不知道，还谈什么跟车策略？
3.1 匀加速运动模型
先看最简单的模型——匀加速运动。这个大家高中就学过，但实际工程里怎么用，还是有讲究的。
公式很简单：
s = s0 + v0 * t + 0.5 * a * t²
v = v0 + a * t其中 s 是位置，v 是速度，a 是加速度，t 是时间。
这个模型假设加速度是常数。嗯，现实世界哪有这么完美？但别急着否定它。我在项目中遇到过，当采样频率很高（比如 100Hz），两个时间步之间的加速度变化其实很小。这时候用匀加速模型，计算量小，效果也还行。
适用场景：
- 短时间预测（< 0.5s）
- 低速工况（停车场、拥堵路段）
- 计算资源受限的嵌入式平台
3.2 恒定加速度与恒定 Jerk 模型
匀加速模型不够用怎么办？加阶数。
恒定加速度模型（CA），就是我们刚才说的。它假设加速度不变。但实际驾驶中，司机会踩刹车、会松油门，加速度一直在变。
这时候就要引入 jerk（加加速度）了。jerk 是加速度的变化率，单位是 m/s³。
恒定 Jerk 模型（CJ）的公式：
s = s0 + v0*t + 0.5*a0*t² + (1/6)*j*t³
v = v0 + a0*t + 0.5*j*t²
a = a0 + j*t你看，多了一个 j（jerk）项。这个模型能描述加速度的连续变化。
我个人习惯，在高速跟车场景下，优先用恒定 jerk 模型。为什么？因为前车驾驶员的操作是连续的，加速度不会突变。用 jerk 模型能更平滑地预测前车轨迹。
| 模型 | 状态变量 | 预测精度（1s内） | 计算复杂度 |
|---|---|---|---|
| 匀加速（CA） | 位置、速度、加速度 | 中等 | 低 |
| 恒定 Jerk（CJ） | 位置、速度、加速度、jerk | 高 | 中等 |
3.3 卡尔曼滤波在速度估计中的应用
好了，模型有了，但传感器数据是带噪声的。怎么办？卡尔曼滤波登场。
卡尔曼滤波，说白了就是一个“数据融合器”。它把模型预测值和观测值加权平均，权重由各自的协方差决定。
你想想看，如果模型预测很准，我就多信模型；如果传感器测量很准，我就多信传感器。卡尔曼滤波做的就是这件事。
核心五步：
- 预测： 用运动模型算下一时刻的状态
- 预测协方差： 算预测的不确定性
- 卡尔曼增益： 算模型和传感器该信谁
- 更新： 用观测值修正预测值
- 更新协方差： 更新不确定性
下面是一个简单的卡尔曼滤波代码，用于估计纵向速度：
import numpy as np
class KalmanFilter1D:
def __init__(self):
# 状态: [位置, 速度]
self.x = np.array([[0.0], [0.0]])
# 状态协方差
self.P = np.eye(2) * 100
# 状态转移矩阵 (匀加速模型)
self.F = np.array([[1, 0.1], [0, 1]])
# 观测矩阵 (只观测位置)
self.H = np.array([[1, 0]])
# 过程噪声
self.Q = np.array([[0.01, 0], [0, 0.01]])
# 观测噪声
self.R = np.array([[1.0]])
def predict(self):
self.x = self.F @ self.x
self.P = self.F @ self.P @ self.F.T + self.Q
def update(self, z):
y = z - self.H @ self.x
S = self.H @ self.P @ self.H.T + self.R
K = self.P @ self.H.T @ np.linalg.inv(S)
self.x = self.x + K @ y
self.P = (np.eye(2) - K @ self.H) @ self.P
def get_velocity(self):
return self.x[1, 0]这段代码里，我用了匀加速模型做预测。实际项目中，你可以把 F 矩阵换成恒定 jerk 模型，效果会更好。
关键点： Q 和 R 的调参决定了滤波效果。Q 太大，滤波结果会跟着传感器噪声跳；Q 太小，滤波结果会滞后。我一般先根据传感器手册给个初值，然后实车采集数据，用试错法微调。
3.4 避坑指南
讲几个我踩过的坑：
- 初始化问题： 卡尔曼滤波的初始协方差 P 不要设太小。我曾经设成单位矩阵，结果前几帧滤波结果完全跟不上真实速度。后来改成 100*I，收敛快多了。
- 模型失配： 如果前车突然急刹，匀加速模型会严重高估速度。这时候我建议加一个“机动检测”，检测到加速度突变时，临时增大过程噪声 Q。
- 传感器延迟： 雷达和摄像头都有延迟。卡尔曼滤波默认假设观测和预测是同步的。实际中，我一般会加一个时间戳对齐模块，否则估计的速度会滞后 100-200ms。
嗯，这一章的内容就这些。运动学模型是骨架，卡尔曼滤波是血肉。两者结合，才能得到可靠的速度估计。
下一章，我们聊聊更高级的交互式多模型（IMM）算法，它能自动切换不同的运动模型，适应各种工况。敬请期待。