4. 目标检测与跟踪：基于卡尔曼滤波的多目标跟踪（MOT）基础
多目标跟踪，简称 MOT，是自动驾驶感知里绕不开的一环。说白了，就是让车知道「谁是谁」，并且持续盯住它们。
你想想看，如果每一帧都重新做目标检测，那结果会怎样？检测框会疯狂抖动，ID 会频繁切换。车根本没法判断前面那辆车是不是刚才那辆。所以我们需要跟踪——把检测结果串起来，给每个目标一个稳定的身份。
4.1 为什么需要卡尔曼滤波？
我刚开始做跟踪时，也想过直接用检测框的坐标。但现实很骨感——检测器会漏检、会抖动、甚至会突然跳一下。卡尔曼滤波就是来解决这个问题的。
它的核心思想很简单：用历史信息来预测当前状态，再用观测值来修正预测。说白了，就是「猜一个，再改一改」。
卡尔曼滤波的两大步骤：
- 预测（Predict）：根据上一帧的状态，估计当前帧的位置
- 更新（Update）：用当前帧的检测结果，修正预测值
我在项目中遇到过一个问题：如果只用检测框中心点做跟踪，车辆转弯时跟踪会跟丢。后来我加入了速度和加速度状态量，效果就好多了。嗯，这里要注意——状态量的选择直接影响跟踪精度。
4.2 状态向量与观测模型
对于自动驾驶场景，我习惯用 7 维状态向量：
状态向量: [x, y, w, h, vx, vy, s]
- x, y: 检测框中心坐标
- w, h: 检测框宽高
- vx, vy: 中心点速度
- s: 面积变化率（用于处理尺度变化）观测向量就简单了，就是检测器输出的框：
观测向量: [x_det, y_det, w_det, h_det]为什么观测比状态少？因为速度和面积变化率是隐式的，我们没法直接观测到，只能通过卡尔曼滤波来估计。
4.3 卡尔曼滤波的数学核心
别被公式吓到。我拆开来讲，其实就 5 个公式：
| 步骤 | 公式 | 含义 |
|---|---|---|
| 预测状态 | x' = F·x + B·u | 用上一帧状态预测当前帧 |
| 预测协方差 | P' = F·P·F^T + Q | 预测的不确定性 |
| 卡尔曼增益 | K = P'·H^T·(H·P'·H^T + R)^(-1) | 预测和观测的权重分配 |
| 更新状态 | x = x' + K·(z - H·x') | 用观测修正预测 |
| 更新协方差 | P = (I - K·H)·P' | 修正后的不确定性 |
你可能会问：这些矩阵怎么设？我直接给经验值：
- F（状态转移矩阵）：匀速运动模型，时间步长 dt 取 0.1s（10Hz 传感器）
- Q（过程噪声协方差）：设小一点，比如 0.01，让滤波更相信模型
- R（观测噪声协方差）：设大一点，比如 0.1，因为检测器本身有误差
我的经验：Q 和 R 的比例决定了滤波器的「性格」。Q 小 R 大，轨迹更平滑但响应慢；Q 大 R 小，跟踪更灵敏但容易抖动。我一般先设 Q=0.01, R=0.1，然后根据实车数据微调。
4.4 数据关联：谁是谁？
卡尔曼滤波只能跟踪单个目标。多目标跟踪的核心难点在于——怎么把检测框和已有的跟踪轨迹配对。
我常用的方法是匈牙利算法 + 马氏距离：
- 计算代价矩阵：每个检测框和每个预测框之间的马氏距离
- 匈牙利匹配：找到总代价最小的匹配方案
- 阈值过滤：距离太大的匹配直接丢弃，防止误匹配
我曾经踩过的坑：只用马氏距离做匹配，结果两辆车并排行驶时 ID 频繁互换。后来我加入了外观特征（比如 ReID 特征向量），用余弦距离辅助匹配，问题就解决了。
4.5 跟踪的生命周期管理
每个跟踪目标都有生命周期。我一般这样管理：
| 状态 | 条件 | 操作 |
|---|---|---|
| 未确认 | 新检测框，连续匹配成功 < 3 帧 | 用卡尔曼滤波跟踪，但不输出 |
| 已确认 | 连续匹配成功 ≥ 3 帧 | 输出跟踪结果 |
| 丢失 | 连续匹配失败 > 5 帧 | 删除跟踪器 |
为什么要设「未确认」状态？因为检测器偶尔会误检。如果一出现就输出，那路上会多出很多幽灵目标。等 3 帧确认了再说，这是我在实际项目里学到的教训。
4.6 完整代码示例
下面是一个简化版的卡尔曼滤波跟踪器。我把它写成了 Python 类，方便你直接复用：
import numpy as np
from scipy.optimize import linear_sum_assignment
class KalmanTracker:
def __init__(self):
# 状态转移矩阵（匀速模型）
dt = 0.1
self.F = np.array([
[1, 0, 0, 0, dt, 0, 0],
[0, 1, 0, 0, 0, dt, 0],
[0, 0, 1, 0, 0, 0, dt],
[0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 1]
])
# 观测矩阵
self.H = np.array([
[1, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0]
])
# 噪声协方差
self.Q = np.eye(7) * 0.01
self.R = np.eye(4) * 0.1
def predict(self, x, P):
x_pred = self.F @ x
P_pred = self.F @ P @ self.F.T + self.Q
return x_pred, P_pred
def update(self, x_pred, P_pred, z):
y = z - self.H @ x_pred # 残差
S = self.H @ P_pred @ self.H.T + self.R
K = P_pred @ self.H.T @ np.linalg.inv(S) # 卡尔曼增益
x = x_pred + K @ y
P = (np.eye(7) - K @ self.H) @ P_pred
return x, P
# 数据关联示例
def associate_detections_to_trackers(detections, trackers, iou_threshold=0.3):
if len(trackers) == 0:
return np.empty((0, 2)), np.arange(len(detections)), np.empty((0, 2))
# 计算 IoU 代价矩阵
cost_matrix = np.zeros((len(detections), len(trackers)))
for d, det in enumerate(detections):
for t, trk in enumerate(trackers):
cost_matrix[d, t] = 1 - compute_iou(det, trk)
# 匈牙利匹配
row_ind, col_ind = linear_sum_assignment(cost_matrix)
# 过滤低 IoU 匹配
matches = []
unmatched_detections = []
unmatched_trackers = []
for d in range(len(detections)):
if d not in row_ind:
unmatched_detections.append(d)
for t in range(len(trackers)):
if t not in col_ind:
unmatched_trackers.append(t)
for r, c in zip(row_ind, col_ind):
if cost_matrix[r, c] < 1 - iou_threshold:
matches.append([r, c])
else:
unmatched_detections.append(r)
unmatched_trackers.append(c)
return np.array(matches), np.array(unmatched_detections), np.array(unmatched_trackers)4.7 卡尔曼滤波的局限性
说实话，卡尔曼滤波不是万能的。我总结了几点：
- 线性假设：卡尔曼滤波假设运动是线性的。车辆急转弯时，预测会偏差很大。这时候可以考虑扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）
- 高斯噪声假设：实际传感器噪声不一定是高斯分布。遇到突发噪声（比如 Lidar 点云被遮挡），滤波会失效
- 计算量：目标数量多时（比如 50+ 个），矩阵运算会变慢。我一般用 C++ 实现，或者用并行计算加速
一句话总结：卡尔曼滤波是 MOT 的基石，但不是终点。它解决的是「平滑跟踪」问题，而「长期稳定跟踪」还需要结合 ReID、运动模型、甚至深度学习特征。
4.8 本章知识体系
下面这张图展示了本章的核心逻辑：
这张图把整个流程串起来了。从传感器输入开始，经过目标检测、卡尔曼滤波预测更新、数据关联、生命周期管理，最后输出稳定的跟踪轨迹。每个环节都环环相扣，缺一不可。
好了，这一章就到这里。卡尔曼滤波是 MOT 的基础，但真正落地时还有很多细节要处理。比如怎么处理遮挡、怎么融合多传感器、怎么在嵌入式平台上跑实时——这些我们后面会慢慢聊。
可可精品资料库