第四节：车辆运动学建模——单车运动模型与轨迹预测基础
各位同学，今天我们聊点硬核的——车辆运动学建模。
说实话，做V2X交叉路口优化，你绕不开一个问题：车接下来会怎么走？ 你想想看，如果连车下一秒在哪儿都算不准，那什么协同、什么优化都是空谈。所以，运动学建模就是整个算法体系的“地基”。
我个人习惯把这一节分成两块来讲：单车运动模型和轨迹预测基础。前者是物理规律，后者是工程应用。咱们一个一个来。
4.1 单车运动模型：匀加速与恒定曲率
先问个问题：一辆车在路上跑，它的运动状态怎么描述？
嗯，最朴素的想法——位置、速度、加速度。但实际工程中，我们还需要考虑转向。所以，我一般把单车模型拆成两个子模型：纵向运动模型和横向运动模型。
4.1.1 匀加速运动模型
这个最简单，也最常用。说白了，就是假设车辆在短时间内加速度不变。
公式长这样：
s(t) = s₀ + v₀ * t + 0.5 * a * t²
v(t) = v₀ + a * t其中：
- s₀：初始位置
- v₀：初始速度
- a：加速度（恒定）
- t：时间
我在项目中遇到过一个问题：直接用这个模型预测3秒后的位置，结果偏差很大。为什么？因为真实道路上的加速度不可能一直不变。所以，我建议预测周期不要超过2秒，超过之后误差会指数级增长。
4.1.2 恒定曲率运动模型
车辆转弯时，光用匀加速模型就不够了。这时候需要引入曲率的概念。
恒定曲率模型假设：车辆在转弯过程中，转向角速度不变。说白了，就是方向盘角度固定。
公式稍微复杂一点：
x(t) = x₀ + (v/ω) * [sin(θ₀ + ω*t) - sin(θ₀)]
y(t) = y₀ + (v/ω) * [cos(θ₀) - cos(θ₀ + ω*t)]
θ(t) = θ₀ + ω * t其中：
- ω：角速度（恒定）
- θ₀：初始航向角
- v：线速度
这里有个坑——当ω趋近于0时，公式会除以0。我曾经在代码里忘了处理这个边界情况，结果仿真直接崩了。所以，实际工程中，我会加一个判断：如果|ω| < 1e-6，就退化为匀加速直线模型。
4.2 轨迹预测基础
有了运动模型，下一步就是预测轨迹。说白了，就是用数学模型推演车辆未来的位置序列。
我个人把轨迹预测分为三个层次：
- 物理层预测：基于运动学模型，不考虑环境因素
- 行为层预测：考虑车道、交通规则等
- 交互层预测：考虑其他车辆的影响
今天我们先讲第一层——物理层预测。这是后面所有预测的基础。
4.2.1 预测流程
我习惯的预测流程是这样的：
- 获取当前状态：位置、速度、航向角、加速度、角速度
- 选择模型：根据当前状态判断用匀加速还是恒定曲率
- 迭代计算：以固定时间步长（比如0.1秒）向前推算
- 生成轨迹：输出未来N个时间点的位置序列
代码实现大概长这样：
def predict_trajectory(state, dt, horizon):
"""
state: [x, y, v, theta, a, omega]
dt: 时间步长 (s)
horizon: 预测步数
"""
trajectory = []
x, y, v, theta, a, omega = state
for _ in range(horizon):
# 判断模型
if abs(omega) < 1e-6:
# 匀加速直线
x += v * dt * cos(theta)
y += v * dt * sin(theta)
v += a * dt
else:
# 恒定曲率转弯
x += (v/omega) * (sin(theta + omega*dt) - sin(theta))
y += (v/omega) * (cos(theta) - cos(theta + omega*dt))
theta += omega * dt
v += a * dt
trajectory.append([x, y, v, theta])
return trajectory4.2.2 误差分析与处理
你可能会问：预测出来的轨迹，到底有多准？
嗯，这个问题很关键。我根据经验总结了一个误差表：
| 预测时长 | 匀加速模型误差 | 恒定曲率模型误差 | 建议使用场景 |
|---|---|---|---|
| 0.5秒内 | < 0.3m | < 0.2m | 紧急制动、碰撞预警 |
| 1.0秒内 | < 1.0m | < 0.8m | 跟车、变道辅助 |
| 2.0秒内 | < 3.0m | < 2.5m | 交叉口通行优化 |
| 3.0秒以上 | > 5.0m | > 4.0m | 仅作参考，不建议直接使用 |
我曾经在一个项目中，直接用2秒的预测结果做路径规划，结果车辆在路口差点撞上护栏。后来排查发现，是预测误差累积导致的。从那以后，我定了一个规矩：超过2秒的预测，必须加置信度标签。
4.3 小结与思考
今天的内容，说白了就是两件事：怎么描述车的运动，以及怎么用这个描述去猜车接下来会去哪儿。
匀加速模型适合直道，恒定曲率模型适合弯道。两者结合，基本能覆盖90%的日常驾驶场景。但记住，模型永远是模型的，真实世界的噪声、延迟、不确定性，都需要你在工程中去处理。
最后留个思考题：如果车辆在弯道中突然加速，这时候应该用哪个模型？或者，有没有一种模型能同时处理纵向和横向的耦合？
嗯，这个问题，我们下一节讲自行车模型的时候再细聊。