4. 障碍物检测与跟踪：点云聚类、目标检测算法、卡尔曼滤波跟踪、匈牙利匹配

各位同学，欢迎来到第四讲。今天聊的话题，是自动驾驶感知系统里最“硬核”的一环——障碍物检测与跟踪。

你想想看，车在路上跑，周围有车、有人、有自行车，还有各种奇奇怪怪的障碍物。系统必须知道“那里有个东西”、“它是什么”、“它接下来要去哪”。这三个问题，就是我们今天要解决的。

我个人习惯把这一整套流程叫做“感知-跟踪-预测”的闭环。说白了，就是先找到障碍物，然后盯住它，最后猜它下一步想干嘛。今天我们先讲前两步。

4.1 点云聚类：从海量点中找出“物体”

激光雷达扫一圈，回来几十万个点。这些点密密麻麻，但系统不知道哪些点属于同一辆车。所以第一步，就是做聚类。

我常用的方法是基于距离的聚类，比如DBSCAN。它的核心思想很简单：如果两个点挨得够近，就认为它们属于同一个物体。

嗯，这里要注意一个参数——邻域半径。设得太小，一辆大卡车会被切成好几块；设得太大，旁边的人行道和行人就粘在一起了。我在项目中遇到过这个问题，当时调试了好久，最后发现要根据激光雷达的线束密度动态调整半径。

给你们看一段伪代码，理解一下思路：

// 点云聚类伪代码
function clusterPointCloud(points, radius, minPoints):
    clusters = []
    visited = set()
    
    for each point in points:
        if point in visited:
            continue
        neighbors = findNeighbors(point, points, radius)
        if len(neighbors) < minPoints:
            markAsNoise(point)
            continue
        // 核心点，开始生长
        newCluster = expandCluster(point, neighbors, radius, minPoints)
        clusters.append(newCluster)
        visited.update(newCluster)
    
    return clusters

除了DBSCAN，还有欧几里得聚类、基于体素的聚类。我个人偏爱DBSCAN，因为它不需要预先知道有多少个物体，而且能处理任意形状的障碍物。

实战小技巧： 在高速场景下，远处的点云很稀疏。我建议对远距离点云使用更大的聚类半径，或者先做一下体素滤波，把点云均匀化再聚类。

4.2 目标检测算法：给障碍物“贴标签”

聚类之后，我们得到了一堆“点云团”。但系统还不知道这些团是什么。是车？是行人？还是路边的垃圾桶？

这就轮到目标检测算法上场了。在自动驾驶领域，主流方案分两派：

  • 基于视觉的检测： 用摄像头图像，跑深度学习模型，比如YOLO、Faster R-CNN。优点是信息丰富，能识别颜色、纹理；缺点是受光照影响大，没有深度信息。
  • 基于激光雷达的检测： 直接用点云数据，跑PointNet、VoxelNet这类模型。优点是三维信息准确，不受光照影响；缺点是点云稀疏，远距离识别能力弱。

我建议的做法是——融合。用激光雷达做定位和测距，用摄像头做分类和识别。两者互补，效果最好。

举个例子，我曾经在一个项目中，只用激光雷达检测行人，结果经常把路边的消防栓误检成人。后来加了摄像头图像做二次验证，误检率直接降了80%。

核心观点： 目标检测不是“选一个最好的算法”，而是“如何让多个传感器互相兜底”。多模态融合，才是量产车的王道。

4.3 卡尔曼滤波跟踪：让障碍物“动起来”

检测到障碍物之后，下一帧它可能移动了位置。我们需要一个方法，来估计它每一时刻的状态（位置、速度、加速度）。

卡尔曼滤波，就是干这个的。它像一个“智能平滑器”，能把有噪声的观测数据，变成平滑的运动轨迹。

它的核心思想就两步：

  1. 预测： 根据上一帧的状态，猜这一帧它应该在哪。
  2. 更新： 用这一帧的观测数据，修正刚才的猜测。

我给你们画个简单的状态方程：

// 卡尔曼滤波状态方程
// 状态向量: [x, y, vx, vy]  位置和速度
// 预测步骤:
x_pred = F * x_prev + B * u
P_pred = F * P_prev * F^T + Q

// 更新步骤:
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_upd = x_pred + K * (z - H * x_pred)
P_upd = (I - K * H) * P_pred

这里面的Q和R，是调参的关键。Q代表你对运动模型的信任程度，R代表你对传感器观测的信任程度。我曾经在一个项目中，因为Q设得太大，导致跟踪轨迹一直在抖动；后来把Q调小，轨迹就平滑多了。

注意： 卡尔曼滤波假设噪声是高斯分布。如果你的传感器噪声不是高斯分布（比如有突发跳变），建议先用一个中值滤波器预处理一下，否则卡尔曼滤波会“懵掉”。

4.4 匈牙利匹配：解决“谁是谁”的问题

现在，我们有了上一帧的跟踪列表，和这一帧的检测列表。问题来了：这一帧检测到的障碍物，对应上一帧的哪一个？

这就是数据关联问题。匈牙利匹配算法，就是用来解决这个“一对一匹配”问题的。

它的思路很巧妙：

  • 计算每个检测框和每个跟踪框的“代价”，比如位置距离、IOU（交并比）等。
  • 然后找一个全局最优的匹配方案，使得总代价最小。

我给你们一个简单的匹配流程：

// 匈牙利匹配伪代码
function hungarianMatch(detections, tracks):
    // 构建代价矩阵
    costMatrix = []
    for each detection in detections:
        row = []
        for each track in tracks:
            cost = computeIOU(detection, track)  // 或者马氏距离
            row.append(cost)
        costMatrix.append(row)
    
    // 执行匈牙利算法，找到最小代价匹配
    matches = hungarianAlgorithm(costMatrix)
    
    // 处理未匹配的检测（新出现的障碍物）
    // 处理未匹配的跟踪（消失的障碍物）
    return matches

这里有个坑，我踩过。如果两辆车并排行驶，它们的IOU可能很大，匈牙利匹配容易把ID搞混。后来我加了一个“马氏距离”的约束，结合运动方向，才把这个问题解决。

我的经验： 匈牙利匹配虽然好用，但别忘了加一个“最大距离阈值”。如果两个障碍物距离太远，即使代价最小，也不应该匹配。否则会出现“瞬移”这种离谱的跟踪结果。

4.5 总结与思考

好了，今天的内容就到这里。我们走了一遍完整的流程：

  • 点云聚类：从无序点云中找出物体。
  • 目标检测：给物体贴上类别标签。
  • 卡尔曼滤波：让物体“动起来”，平滑跟踪。
  • 匈牙利匹配：解决帧与帧之间的“谁是谁”问题。

这四个模块环环相扣，缺一不可。你想想看，如果聚类没做好，后面检测就会漏掉目标；如果跟踪没做好，规划层就没法预测障碍物的未来轨迹。

下一讲，我们会深入讲一下“运动预测与行为意图估计”，也就是怎么猜障碍物接下来要干什么。到时候，今天讲的跟踪结果就是输入。

好，今天就到这里。有问题随时问我。

← 上一章 📖 返回目录 下一章 →