大家做自动驾驶或者辅助驾驶感知的应该都有类似的经历模型跑通了检测框画得挺准车辆行人自行车都能框住精度指标也好看可一上车路测总觉得系统反应慢半拍。不是检测漏了而是它只告诉你那里有个东西却没告诉你这东西接下来要干嘛。前方人行横道边站了个人检测框已经打好几帧了车子还在匀速往前开等人家真的迈步了你才一脚重刹体感就是差。这时候你会意识到动态场景理解的核心其实不是检测而是从感知结果里把运动状态估出来再把状态映射成意图抢出提前量。这篇文章我想把基于感知结果做运动状态估计与意图猜测这件事从头到尾拆开讲一遍聊聊状态估计怎么做、意图怎么猜、工程落地有哪些坑以及实测下来哪些场景真的能预判成功。适合正在做感知融合、预测决策模块或者想把检测结果升级成场景理解结果的工程师参考。文章里没有花哨的理论堆砌都是能直接落到代码里的思路和实操经验。1. 从检测到预测动态场景理解要解决的核心矛盾1.1 检测框只回答在哪不回答要去哪先说一个很多人没意识到的点目标检测任务本身是空间性的它输出的是当前帧里这个物体在哪个位置、有多大、属于什么类别。对这个时刻而言这已经是完整信息了。但当你要做的是连续场景理解时单帧检测框就变成一个很小的零件——真正有价值的信息藏在帧与帧之间的关系里。举个例子你在一段连续视频里看到一辆车在靠近车道线检测框单独看每一帧都没问题但把时间维度加进来你会发现它的横向位置在缓慢变化而且变化率在增大。这个横向变化率就是检测框本身不直接给的信息需要靠多帧累积才能提取。动态场景理解的第一步就是把检测框序列转成连续的运动状态量——位置、速度、加速度、朝向角变化率——然后在这个状态量的基础上做后续判断。1.2 感知结果到预测输入之间被低估的空档我见过不少团队把精力全砸在检测模型上觉得检测准了后面什么都好说。实际上从检测框到可预测的状态之间有一大段工程空档这段空档经常被低估。检测框给的是图像坐标或传感器坐标下的矩形而预测模块真正想要的是在某个稳定坐标系下的运动矢量。这中间需要做时间戳对齐、坐标变换、状态平滑还要处理遮挡和漏检带来的状态跳变。这个空档最典型的坑是直接用检测框中心点算速度。检测框的抖动会导致中心点在静止目标上都飘来飘去算出来的速度像噪声一样根本无法支撑预测。真正可靠的做法是引入滤波器把检测测量当作带噪声的观测值用运动学模型去平滑。这一步做不做直接决定后面意图猜测是建立在干净的速度上还是噪声上。提示如果你现在还在用相邻两帧检测框中心距除以帧间隔来算速度建议尽快换成带滤波器的估计方案。这个土办法在小目标、遮挡场景下会直接让意图判断失效。2. 运动状态估计从一串检测框到可用的速度与加速度2.1 状态向量的定义与坐标系选择做状态估计第一步不是写滤波器而是想清楚状态空间怎么定义。我常用的做法是区分两类目标机动性强的行人和自行车以及机动受限的车辆。对于车辆我习惯用一个八维状态向量横向位置、纵向位置、横向速度、纵向速度、朝向角、朝向角变化率、加速度、加速度变化率。简写为 [x, y, vx, vy, θ, ω, a, a_dot]。这个状态量覆盖了直线运动、转弯、加减速的大部分情况。对于行人和自行车我会省略朝向角相关的维度因为行人的朝向和运动方向的关联性没有车辆那么强——人完全可以侧着走朝向和速度方向不一致。坐标系选择上有两点经验。第一估计模块内部最好用统一的世界坐标系通常是以路测起点或地图原点为基准的笛卡尔坐标而不是自车坐标系。原因很简单自车坐标系会随自车运动而旋转和平移目标的速度估计会被自车运动耦合进去很难剥离。第二往预测模块输出结果时再根据下游需求转成自车坐标系或道路坐标系。这个先统一再按需展开的思路能少掉很多坐标变换的坑。2.2 卡尔曼滤波的时间更新与量测更新到底在做什么对多数动态目标卡尔曼滤波依然是最稳的选择。它的两个步骤背后的物理含义值得想清楚时间更新预测步骤做的是根据运动模型外推状态。假设目标在上一个时间戳的状态是 xt速度是 vt那么在 dt 之后它大概率应该在 xt vt·dt 附近。这里模型越接近真实运动外推越准。对车辆我常用匀加速模型对行人用匀速模型加一点过程噪声调节——行人的机动性更强你要用更大的过程噪声来容忍他的突然变向。量测更新更新步骤做的是在预测值和观测值之间做个加权平均。权重由两个协方差矩阵决定预测的不确定度越大越相信观测观测噪声越大越相信预测。这个比例关系比具体的矩阵数值更关键。实际调参时我最常动的就是过程噪声矩阵 Q。Q 太小状态会过于平滑目标真的变向时你跟不上。Q 太大状态会跟着检测框抖动平滑作用又没了。我的经验是从中等量级开始用录制好的数据离线回放调参观察目标的横向速度曲线在真实变道时刻的响应延迟一般能做到 3~5 帧内跟上就能接受。2.3 数据关联检测框和轨迹ID的匹配是状态估计的上游命门估计模块做得再漂亮如果数据关联错了一切白搭。数据关联要解决的是这一帧的检测框对应的是上一帧哪条轨迹的问题。常见的做法是两步走第一步用运动模型预测每一条轨迹在当前帧的期望位置。第二步把检测框和预测位置做匹配匹配方法用成本矩阵加匈牙利算法。成本矩阵的元素我通常用马氏距离而不是欧氏距离因为马氏距离会考虑状态估计的不确定度——某条轨迹本身速度很大、不确定度也大时它附近的可匹配区域应该更宽马氏距离能体现这一点。还有一个细节容易被忽略不同类别的目标要分开做关联。行人和车辆同框交叠时如果混在一起匹配容易出现ID互换或轨迹错绑。我习惯在关联前先用类别对待匹配集合做一级过滤同类目标之间才做匹配。这个简单策略能让ID切换率下降不少。注意ID切换是预测模块最头疼的问题之一。轨迹中断再重挂状态估计的时间连续性断裂滤波器的历史信息清零,预测只能从头来过。减少ID切换是状态估计工程里优先级非常高的一件事。3. 意图猜测从运动状态到行为语义3.1 意图不是玄学而是行为先验与语义上下文的结合意图猜测听起来玄其实拆开就两点目标当前的运动状态以及目标周围能约束行为的上下文信息。两者叠加生成候选意图集合以及每个意图的概率。以路口场景为例一辆车在左侧车道减速靠近路口运动状态是减速轻微左偏。结合路网信息它的前方车道是直行带左转箭头。那么候选意图可以限定为直行等待和左转。再看转向灯信号——感知模块若能识别到转向灯状态左转的概率就要上调。如果没有转向灯信号还可以看它的轨迹对车道中心线的贴合度在直行道上压着中心线但与左转引导线也有交集两者的概率就会比较接近。关键认知是:意图猜测不需要追求唯一正确而是要输出一个带概率的候选集合。下游决策模块拿到这个集合可以做保守策略——比如预测行人可能横穿虽然概率只有40%但决策端仍然会做一个减速准备。意图猜测的价值不在于猜中而在于不遗漏低概率但高风险的可能。3.2 基于语义地图的意图候选空间我比较推崇的做法是先用语义地图约束意图候选空间。所谓语义地图就是带车道拓扑、路权规则、人行横道、停止线、路口导向信息的道路模型。每一类目标在语义地图上都有一个合法行为集合。拿行人举例行人在人行横道附近合法行为有沿人行横道横穿在人行道上静止绕行行人在非人行区域靠近车道边缘合法行为有继续沿路边行走突然横穿低概率但高风险折返。这些集合不需要模型去从零学习而是用规则从地图里生成。这一步把无限可能的意图空间压缩成有限候选后面的概率打分才有意义。这里有个实操细节语义地图的坐标必须和状态估计模块的坐标系严格对齐。我在项目里做坐标对齐时吃过亏地图车道的点列出差了十几厘米横穿行人的预测点就压在相邻车道上了下游决策基于这个信息发出误警告。建议在小项目里花时间做一个地图对齐校验的离线工具确保路测时地图坐标和感知坐标的误差在可接受范围内。3.3 行为概率的近邻法贝叶斯、学习式与规则式的取舍候选意图集合生成之后需要给每个意图打分。工程上行之有效的方案有三类规则式打分最直接定义一组特征如横向速度靠近车道线的程度、与交叉路口停止线的距离、转向灯状态每个特征对每个意图有预设的权重加权求和后归一化得到概率。优点是逻辑透明、可调试缺点是权重靠经验和试错复杂场景覆盖不全。贝叶斯式打分把问题转化成后验概率更新。先验概率来自历史统计比如这个路口左转车占比观测模型则用轨迹特征横向偏移、速度变化等构建似然函数。它的好处是能吸收历史数据自适应性比纯粹规则式好但需要花时间标定似然函数的参数。学习式打分比如轻量级梯度提升树或小型时序网络拟合能力最强能发现人眼看不出的大特征组合。代价是需要带标注的数据——标注那辆车的意图是左转这件事本身成本极高而且不同司机的驾驶风格不同同一个意图表达出的轨迹差异很大。我在实践中发现学习式更适合在积累了相当规模的真实数据之后再做增量而不是冷启动。我目前的倾向是规则式搭骨架贝叶斯调概率用语义地图和规则先把候选意图限定好再用贝叶斯打分对每个候选意图做概率估计。既保证了逻辑可解释又能从数据中持续修正。纯学习式我不反对但除非你们团队的标注管线很成熟否则前期投入产出比不一定划算。4. 工程落地的三个隐藏坑延迟、失谐与ID切换4.1 延迟补偿感知链路的死角做预测模块的人最容易忽略的问题是感知结果本身是有延迟的。从传感器曝光到检测模型推理再到结果输出给下游整个过程可能耗时30到100毫秒不等。这意味着你拿到的检测框描述的是100毫秒之前的世界而不是现在。我实际测试中发现在自车以60km/h行驶、目标以5km/h横穿的情况下100毫秒的延迟换算到空间就是约1.7米的提前量偏差。这个偏差足以让预测结果偏离真实位置一个车身宽度。解决思路有两个方向。第一尽量压缩感知链路延迟比如把模型推理从CPU挪到推理加速卡上或者砍掉不必要的预处理。第二用运动模型做时间前推在状态估计时记录每个传感器的延迟时间戳输出预测前先按当前时刻进行时间前推把目标位置推到现在再交给下游。这个方法在工程上更容易落地强烈建议做。我踩过的教训是一开始图省事所有目标都按当前系统时间统一处理理论上省了代码量路测时却总觉得预测结果飘直到把传感器时间戳单独拉出来做统计分析才发现不同传感器之间的延迟能差出70毫秒以上。4.2 传感器失谐时的退化策略多传感器融合系统的现实是总会有某个时刻卫星信号被遮挡、惯导漂移、相机曝光异常或者毫米波雷达的点云稀疏得离谱。状态估计模块必须有一套优雅的退化策略而不是在异常数据上硬算。我的做法是在状态估计前加一个数据质量检查器按传感器分别评估质量分数。相机帧率骤降时降低相机观测的权重雷达点云稀疏时对速度估计的约束减弱。退化后哪怕目标状态的不确定度变大了也不至于输出一个错得离谱的速度。举个例子某次路测经过高架桥下定位信号消失自车速度估计漂了大约3km/h。目标车辆在旁车道正常行驶如果状态估计模块还在用自车速度做坐标系转换那目标速度估计也会跟着漂。加了基于定位质量的自适应补偿后虽然定位仍然不准但目标速度的相对误差明显收窄预测模块依然能输出可用的结果。这个不硬算、先降级的思路我认为比任何花哨的算法都管用。4.3 ID切换对轨迹平滑的破坏前面说了ID切换这里展开讲它的后果和补救。ID切换意味着目标轨迹断裂。滤波器重新初始化后第一帧速度估计基本等于检测框位移除以帧间隔噪声极大。这时候预测模块如果直接消费速度信息会看到一个瞬间从静止跳到10m/s的目标下游决策很容易被误导。补救措施有两个层次。浅层的做法是轨迹重挂时做速度初始化平滑——用新轨迹的前三帧速度做指数加权平均代替直接使用第一帧瞬时速度。深层的做法是维护一个目标短期历史记忆库把已丢失目标的最后若干帧状态存下来。当检测框重新出现时先和历史记忆库做一次匹配若匹配上用历史状态做滤波器的热启动。这一套在我实测中把ID切换引起的速度估计异常事件减少了大约40%。需要说明的是即便做了这些措施ID切换也不可能完全消除。所以预测模块的设计要容忍ID切换即使轨迹ID变了预测结果也要在一帧内恢复到合理范围而不该因为轨迹断裂就输出异常值。换句话说预测模块也要有自己的防呆设计。5. 实测效果与边界哪些场景真的能预测成功5.1 典型场景实测对比我在模拟环境和真实路测里分别验证了这套检测状态估计意图猜测的管线的效果选取三类典型场景做了对比。第一类人行横道行人横穿。目标是站在路边准备横穿的行人。处理前系统只能在他进入自车行驶轨迹约0.5秒后才给出碰撞预警。加入运动状态估计和意图猜测后当他开始迈步的瞬间身体横向速度超过阈值系统就能输出横穿意图的高概率候选提前量大约拉到1.2到1.5秒。这个提前量足够自车从60km/h开始舒适减速而不是急刹。第二类城市快速路匝道汇入。旁车道车辆打转向灯准备汇入。状态估计捕捉到它的横向速度持续增大语义地图显示它前方是匝道渐变段意图模块给出汇入概率提升。实测中系统在目标车辆完全变道前约1秒就能锁定汇入意图自车可以提前松开加速踏板做备刹。第三类无保护左转的对向直行车。这是最难的场景。对向直行车速度很快意图模块给出的直行优先通过概率很高但是当它出现轻微减速迹象并伴有前轮转向时系统仍倾向于保守处理——保持警惕但不刹车。这个场景我认为目前仍属于感知能覆盖、预测不完全可靠的边界地带安全冗余还得靠决策端的保守策略兜底。从这三类场景能看出意图猜测的实用性边界它最擅长捕捉行为先兆明显、语义环境清晰的目标对意图表达模糊的目标则只能给出概率化的参考不能作为唯一依据。5.2 失败模式归纳与应对思路实测中我也累积了一些失败模式写出来供大家参考。模式一目标突然静止又突然起步。行人走到路边突然站住状态估计会将其速度降到接近零意图模块根据停留做的预测是继续等待。但行人往往下一秒就猛跑过街。对这种中止-重启型行为目前没有可靠的提前预测方法只能靠决策端对行人位置处于高风险区域这一事实做持续减速准备。模式二大曲率弯道上的目标。弯道内目标的朝向变化率很大匀速模型外推会明显偏离。我在弯道路段使用更小的时间步长同时把过程噪声调大但效果仍然有限。一个可行的思路是把弯道曲率作为先验信息输入给滤波器——如果语义地图里有车道曲率可以直接约束目标在弯道内的运动方向比单纯调参数有效得多。模式三目标被长时间完全遮挡后重新出现。比如行人在大货车后面绕行消失好几秒后出现在另一侧。这时无论是滤波器还是短期记忆库都无法处理目标相当于全新出现。正确的应对是降低优先级并扩大不确定性而不是强行利用旧意图做预测。有一次测试里系统把重新出现的行人误判为原轨迹继续直行差点和真实位置偏离一个车道事后复盘就是没有合理重置意图置信度。这几类失败模式说明一个道理预测算法的上限受制于信息完整度。算法能做的是充分利用已有信息并在信息不足时诚实地输出高不确定性而不是硬着头皮猜一个看似精确的结果。6. 状态估计与意图猜测的模块化拆分建议6.1 为什么要把估计和预测拆成两层很多团队开始做这件事时习惯把状态估计和意图猜测揉在一个模块里觉得反正都是对轨迹做文章。我在实际开发中强烈建议拆成两层估计层只负责目标现在处于什么运动状态预测层只负责目标接下来可能做什么。拆开的理由很简单——两者的数据消费方不同、时间尺度不同、评测指标也不同。估计层服务于预测层和部分决策逻辑它关心的是滤波器的收敛速度、状态量的抖动幅度、跟踪的连续性。预测层服务于规划层关心的是意图概率的稳定性、候选轨迹的合理性。如果揉在一起当预测效果不好时你很难判断到底是状态估计脏了还是意图模型错了。拆开后可以分别回放、分别定位问题调试效率高出一个量级。6.2 模块间接口可以这样设计接口设计我只说两个关键点。一是输出频率估计层建议按感知帧率输出比如10到15Hz预测层可以降低到5到10Hz因为意图变化本身是慢变量没必要跟着帧率抖。二是输出内容除了意图概率一定要附带置信度和有效期。置信度低于某阈值时下游应该把它当作未知意图处理。有效期告诉下游这个预测在多少毫秒内是可信的超时后必须丢弃避免过期的预测被当成新鲜信息。两条轨迹数据我是这么组织的状态估计输出一个结构化变量包含目标ID、类别、位置速度加速度、状态协方差、时间戳意图猜测输出一组候选意图结构体包含意图ID、概率、置信度、有效终止时间。下游逻辑只依赖这些结构体不直接访问内部状态。这套接口实现在代码层面并不复杂但对后续维护和多人协作帮助很大。7. 数据闭环让状态估计与意图猜测持续变好的关键7.1 离线回放与真值标注做感知相关的开发最重要的工具就是离线回放。你不可能每次调参都上路更不可能凭肉眼在路测时判断滤波器跟没跟上。我们的流程是路测采集数据存储完整传感器数据与感知结果回到实验室用离线工具逐帧回放观察状态估计曲线和意图概率变化。真值标注这件事我建议分两档。第一档是轨迹级真值标注这个目标在这段时间内的真实位置和速度用于评估状态估计精度。第二档是意图级真值标注这个目标在某个时刻的行为意图用于评估预测准确性。意图级标注的主观性很强两个标注员可能对同一段视频给出不同意图判断。我在项目里会让多个标注员独立标注然后用一致性指标筛选有效样本。标注一致性低的数据宁可不用否则会误导模型。7.2 从失败样本中挖掘改进点数据闭环的价值不在存数据而在用数据发现问题。我每周会做一次失败案例评审把预测错误率最高的场景挑出来归类看是状态估计的问题还是意图猜测的问题。一个典型例子某段时间我们发现在雨天傍晚场景预测错误率明显偏高回放数据发现检测框抖动加剧导致状态估计的速度噪声变大意图概率也随之波动。最终改进方向不是改预测算法而是针对雨天检测框加了一帧时间平滑。这个案例说明很多预测系统的问题根源在上游感知质量。这里还要提一个容易被忽略的细节数据闭环需要记录足够多的上下文信息不只是目标轨迹。自车车速、自车转向、天气、光照、地图区域类型等上下文对于分析预测失败的原因至关重要。建议在数据采集时把这些变量一并记录形成场景指纹后续检索和比对失败案例会非常高效。8. 写在最后的实操心得从开始做这个方向到现在我最大的感受是动态场景理解是一项系统工程。检测、状态估计、意图预测环环相扣任何一个环节的短板都会在最终的预测效果上暴露出来。很多团队把大量资源投入到检测模型的涨点却忽略了检测框到意图之间那段沉默的工程恰恰是这部分决定了一个系统从能看变成能懂。几个小建议供参考第一先别急着上深度学习预测模型。先把检测、状态估计、坐标变换、延迟补偿这条链路做到稳定很多场景的预测准确率就已经能到不错的水平。第二意图猜测的候选空间一定要用语义地图先收紧不要让模型在无限可能里瞎猜。地图信息越准意图模块的可靠性越高。第三设计任何模块时都为信息缺失留好退路。传感器会失效目标会被遮挡地图会过期你的算法应该在信息变差时优雅降级而不是硬算出离谱结果。最后动态场景理解是一个持续逼近上限的过程没有终点。每次你觉得这次应该没问题了下一次路测总会在某个刁钻场景里给你惊喜。保持记录失败案例的习惯比追求单次算法的华丽表现重要得多。