第四章：环境感知与状态表示——障碍物检测、状态预测、不确定性建模
各位同学，欢迎来到第四章。这一章，我们聊聊自动驾驶和机器人领域里最核心、也最头疼的问题——环境感知。
说白了，就是让机器“看懂”周围的世界。你想想看，如果连障碍物在哪、它要往哪走都搞不清楚，那后面的轨迹规划、博弈决策全是空谈。我在项目里见过太多“规划算法写得漂亮，一上路就撞”的案例，问题往往就出在感知这一层。
4.1 障碍物检测——从传感器到结构化数据
障碍物检测，就是把传感器原始数据变成“这里有个东西”的过程。常用的传感器有激光雷达、摄像头、毫米波雷达。我个人习惯用激光雷达做主要检测，摄像头做辅助分类。
检测流程大致分三步：
- 数据预处理：去噪、滤波、坐标变换
- 目标分割：把点云或图像中的物体分离出来
- 目标分类与跟踪：判断是车、人还是其他东西
举个例子，激光雷达点云的处理，我常用欧几里得聚类：
# 伪代码：欧几里得聚类
def euclidean_clustering(points, tolerance=0.5):
clusters = []
visited = set()
for i, p in enumerate(points):
if i in visited:
continue
cluster = [p]
visited.add(i)
# 用KD树加速近邻搜索
neighbors = kd_tree.query(p, radius=tolerance)
for n in neighbors:
if n not in visited:
visited.add(n)
cluster.append(points[n])
clusters.append(cluster)
return clusters
嗯，这里要注意：tolerance参数很敏感。设大了会把两个物体连在一起，设小了会把一个物体切碎。我曾经在高速场景下吃过这个亏——一辆大卡车被分成了三块，规划器以为有三个障碍物，直接急刹。
4.2 状态预测——障碍物下一步会去哪？
检测到障碍物只是第一步。更关键的是：它接下来会怎么动？
状态预测，就是根据历史轨迹，推测未来几秒的位置。常用的方法有：
- 匀速/匀加速模型：简单，但不够准
- 卡尔曼滤波：线性系统下的最优估计
- 扩展卡尔曼滤波（EKF）：处理非线性运动
- 粒子滤波：处理多模态分布
我个人最常用的是EKF。为什么？因为车辆的运动本质上是非线性的——转弯、加减速，线性模型根本扛不住。
EKF的核心思想很简单：用泰勒展开把非线性函数线性化。状态方程长这样：
x_k = f(x_{k-1}, u_k) + w_k
z_k = h(x_k) + v_k
其中f是运动模型，h是观测模型，w和v是噪声。EKF就是在这两个方程上做“线性近似+卡尔曼滤波”。
4.3 不确定性建模——承认“我不知道”
这是很多初学者容易忽略的部分。你想想看，传感器有噪声，预测有误差，如果规划器把这些当成“确定值”来处理，那结果一定很糟糕。
不确定性建模，就是给每个状态估计加上一个“置信度”。常用的表示方式：
| 表示方法 | 适用场景 | 优缺点 |
|---|---|---|
| 高斯分布 | 单峰、对称的不确定性 | 计算简单，但表达能力有限 |
| 高斯混合模型 | 多模态分布（如：障碍物可能左转或右转） | 更灵活，但计算量大 |
| 粒子集 | 任意分布，非线性非高斯 | 最通用，但需要大量粒子 |
我在项目中遇到过这样一个场景：十字路口，一辆车打着左转向灯，但实际直行了。如果只用单峰高斯预测，规划器会认为它肯定左转，结果差点撞上。后来改用高斯混合模型，同时保留“左转”和“直行”两种可能性，才安全通过。
4.4 知识体系总览
下面这张图，是我自己总结的本章知识结构。你可以把它当成一个“思维导图”来用：
这张图把本章的三个核心模块串起来了。从左到右，从原始数据到结构化信息，再到对未来行为的预测和置信度评估。每一步都环环相扣。
4.5 实战中的坑与经验
最后，分享几个我在实际项目中踩过的坑：
- 传感器标定：激光雷达和摄像头的外参标定，差1厘米，融合结果就完全不对。我建议每季度重新标定一次。
- 延迟补偿：传感器有处理延迟，预测时要考虑“当前帧其实是100ms前的数据”。不做补偿，规划器永远在“看后视镜开车”。
- 动态障碍物的“消失”：遮挡导致障碍物短暂消失，不要立刻删除轨迹。我习惯保留3-5帧的“记忆”，用恒速模型外推。
好了，这一章就到这里。记住：感知是规划的眼睛，不确定性是安全的底线。下一章我们会聊“行为预测与意图估计”，到时候见。