自车运动模型：恒定速度模型（CV）、恒定加速度模型（CA）、恒定转弯率和速度模型（CTRV）

好，咱们接着聊自车运动补偿。上一节我们讲了为什么要做这件事，说白了就是让传感器看到的世界更真实。那怎么补偿？你得先知道自车是怎么动的。

自车运动模型，就是用来描述车辆在短时间内怎么走的数学工具。我这些年做感知，接触过不下十种模型，但真正在工程里高频使用的，其实就三个：CV模型、CA模型和CTRV模型。今天我把它们掰开揉碎了讲清楚。

1. 恒定速度模型（CV）

CV模型是最简单的。它假设车辆在极短的时间间隔内，速度保持不变。你想想看，在10毫秒或者20毫秒的采样周期里，车速能变多少？基本可以忽略。

数学表达也很直观：

状态向量：x = [px, py, vx, vy]^T
状态转移方程：
px' = px + vx * dt
py' = py + vy * dt
vx' = vx
vy' = vy

这里dt是时间间隔。我刚开始做项目时，觉得这模型太简单了，能行吗？后来发现，在高速场景下，CV模型反而很稳。为什么？因为高速时加速度本来就小，你强行用复杂模型反而容易引入噪声。

2. 恒定加速度模型（CA）

CA模型比CV多了一个维度——加速度。它假设加速度在短时间内不变。说白了，就是认为车辆在做匀变速运动。

状态向量：x = [px, py, vx, vy, ax, ay]^T
状态转移方程：
px' = px + vx*dt + 0.5*ax*dt^2
py' = py + vy*dt + 0.5*ay*dt^2
vx' = vx + ax*dt
vy' = vy + ay*dt
ax' = ax
ay' = ay

嗯，这里要注意。CA模型的状态向量是6维的，比CV多了两个加速度分量。维度高了，计算量自然就上去了。但好处也很明显——它能更准确地描述目标的动态变化。

我曾经在城区场景中吃过亏。当时用CV模型跟踪一个正在起步的车辆，结果位置误差越来越大。后来换成CA模型，问题就解决了。因为起步阶段加速度很大，CV模型根本hold不住。

3. 恒定转弯率和速度模型（CTRV）

CTRV模型就更有意思了。它专门处理转弯场景。假设车辆在转弯时，速度大小不变，但方向在变——也就是转弯率恒定。

这个模型的状态向量是：

状态向量：x = [px, py, v, θ, ω]^T
其中：
v 是速度大小（标量）
θ 是航向角
ω 是转弯率（角速度）

状态转移方程稍微复杂一点：

如果 ω ≠ 0（正在转弯）：
px' = px + (v/ω) * [sin(θ + ω*dt) - sin(θ)]
py' = py + (v/ω) * [cos(θ) - cos(θ + ω*dt)]
θ' = θ + ω*dt
v' = v
ω' = ω

如果 ω = 0（直线行驶）：
px' = px + v * cos(θ) * dt
py' = py + v * sin(θ) * dt
θ' = θ
v' = v
ω' = ω

你看，CTRV模型分了两段处理。为什么？因为当ω=0时，分母不能为0，必须单独处理直线情况。这个细节我当年写代码时踩过坑，一不留神就除零崩溃了。

4. 三个模型的对比

我整理了一个表格，方便你对比：

模型	状态维度	假设条件	适用场景	计算复杂度
CV	4	速度恒定	高速直线、高采样率	低
CA	6	加速度恒定	起步、刹车、变速	中
CTRV	5	速度大小恒定，转弯率恒定	弯道、匝道、十字路口	中高

5. 实际工程中的选择策略

你可能会问：那我到底该用哪个模型？

我的做法是——不要死磕一个模型。在实际的自动驾驶系统中，我们通常会维护多个模型并行运行，然后根据场景动态切换。

举个例子：

• 在高速上，我优先用CV模型，偶尔切CTRV处理弯道
• 在城区，我同时跑CA和CTRV，哪个残差小就用哪个
• 在停车场，我甚至会用更复杂的模型（比如CTRA，带加速度的转弯模型）

好了，关于自车运动模型的三个经典选择，我就讲到这里。下一节我们会聊怎么把这些模型用到自车运动补偿里，到时候我会给出完整的代码实现。