1. 单目运动估计的困局与破局思路1.1 为什么单目目标运动估计是个“老大难”做视觉感知的同行都有个共识单目相机便宜、轻量、功耗低但要用它来估计目标的运动状态尤其是三维空间里的速度和轨迹难度比双目和激光雷达高出不止一个量级。核心原因就一个——深度不可直接观测。一个目标在图像上变大可能是因为它在靠近你也可能是因为它本身在膨胀一个目标在图像上横向移动可能是它在横穿也可能是相机自己在动。这种“一个观测对应多个物理解释”的现象就是典型的可观性不足。我在实际项目中反复遇到过这种情况无人机追一个移动目标视觉端给出的距离估计一会儿跳一会儿飘滤波器收敛慢控制环跟着抖。后来排查下来问题往往不在滤波器本身而在于观测模型给的信息不够导致状态估计的可观性矩阵秩亏某些状态分量根本不可观测。Bearing-Box 这个工作瞄准的就是这个痛点。它的核心思路是既然单目图像本身信息有限那就把三维检测框这个中间表示利用起来再叠加动力学约束从两个方向同时增强系统的可观性。这不是简单的“多加几个观测”而是从系统可观测性的角度重新设计了观测模型和约束结构。1.2 Bearing-Box 到底做了什么用一句话概括Bearing-Box 把目标的三维检测框的几何信息角点、边、面与传统的方位向量Bearing观测结合起来构建了一个增强型观测模型同时引入目标运动的动力学先验比如匀速、匀加速假设作为约束使得原本不可观测或弱可观测的状态分量变得可观测或强可观。具体来说传统单目 Bearing-only 方法只利用目标在图像上的方位角信息观测维度低深度方向几乎不可观。Bearing-Box 则从三维检测框的八个角点中提取出尺度信息、朝向信息、位置信息把这些都纳入观测方程。再加上动力学约束对方程的补充整个系统的可观性矩阵的秩得到提升条件数也显著改善。这个思路的价值在于它不需要额外的传感器不需要改变硬件配置纯粹从算法层面把现有信息的利用率拉满。对于 MAV微型飞行器这类对载荷和功耗极度敏感的平台来说这是非常务实的方案。1.3 适合谁来读这篇内容如果你正在做以下任何一件事这篇内容都值得你花时间用单目相机做目标跟踪或运动估计被深度漂移和滤波器不收敛折磨过在无人机或移动机器人上做视觉伺服、目标跟随需要稳定的相对运动估计对可观性分析感兴趣想看看怎么从系统理论角度指导观测模型设计做三维检测但还没想好怎么把检测结果用到状态估计里我下面会从设计思路、核心原理、实操细节、问题排查几个层面展开尽量把“为什么这么设计”和“具体怎么做”都讲透。2. 核心原理拆解可观性增强的底层逻辑2.1 可观性到底在说什么先把这个概念说清楚。一个动态系统如果给定足够长时间的观测输出能够唯一确定系统的初始状态那它就是可观的。对于线性系统看可观性矩阵的秩就行对于非线性系统要看李导数生成的可观性矩阵的秩。秩亏意味着某些状态方向上的信息在观测中“消失”了滤波器只能靠先验去猜猜久了就飘。单目 Bearing-only 跟踪的经典问题就是目标距离深度和目标的绝对尺度之间存在耦合观测只给方位角深度方向的可观性极弱。你从图像上只能看到目标在视野中的方向变化但目标沿视线方向靠近还是远离图像上的变化非常微小信噪比极低。Bearing-Box 的切入点很聪明三维检测框的表观尺寸直接与深度相关。框越大目标越近框越小目标越远。虽然单帧的尺寸-深度关系受目标真实尺寸未知的影响但在多帧时序中尺寸变化率提供了深度方向运动的强约束。再加上框的朝向信息目标的姿态运动也变得可观测。2.2 三维检测框提供了哪些额外观测一个三维检测框在图像上的投影本质上是一个透视投影后的二维框。但如果我们能从二维框反推出三维框的八个角点在图像上的位置那每个角点都提供了一组方位观测。八个角点就是八组方位观测信息量远超单点 Bearing。更重要的是三维框的几何结构本身携带约束对边平行、角点共面、框的尺寸在短时间内的变化受刚体运动约束。这些约束可以写成等式形式直接加入观测方程或作为伪观测。我在实践中发现框的朝向角Yaw对可观性的贡献经常被低估。当目标做横向运动时朝向角的变化率与横向速度强相关当目标做旋转运动时朝向角的变化更是直接观测。把朝向角纳入观测向量后滤波器对目标角速度的估计收敛速度明显加快。2.3 动力学约束怎么补上最后一块拼图光有几何观测还不够。如果目标做匀速直线运动但观测噪声大滤波器仍然可能给出抖动的速度估计。动力学约束的作用是限制状态转移的可行空间把不可能的轨迹排除掉。Bearing-Box 里用的动力学约束根据我的理解至少包括以下几类匀速模型CV位置的一阶导为速度速度的一阶导为零。这是最基础的约束适用于大多数平稳运动场景。匀加速模型CA允许加速度存在但假设其为常数或慢变。对机动目标更友好。运动学一致性约束比如目标不能侧滑非完整约束或者速度方向与朝向角对齐车类目标。这些约束在滤波框架里通常体现为过程模型的设计但在 Bearing-Box 中它们还被用来增强可观性——通过约束把不可观测的状态分量与可观测分量耦合起来间接使其可观测。举个例子如果目标做匀速运动那么深度方向的速度可以通过横向运动与朝向角的几何关系间接推断出来。这个推断链条就是动力学约束提供的“信息通道”。2.4 可观性增强的数学直觉不用太复杂的推导我用一个直观的例子说明。假设状态向量是 [px, py, pz, vx, vy, vz]观测只有方位角 [azimuth, elevation]。方位角只与位置有关与速度无关。那么速度分量的可观性从何而来只能通过位置随时间的变化间接推断。如果位置本身的可观性就弱速度就更不可观。现在加入三维框的尺寸观测。尺寸 s 与深度 pz 的关系大致是 s ∝ 1/pz。这个非线性关系提供了 pz 的直接观测。pz 可观测了vz 通过时序差分也变得可观测。再加入朝向角观测目标的角速度也变得可观测。整个系统的可观性矩阵的秩从原来的 3 或 4 提升到 6条件数也大幅下降。这就是 Bearing-Box 的核心贡献不是发明新的滤波器而是重新设计观测模型让滤波器能“看到”更多信息。3. 实操落地从检测框到运动估计的完整链路3.1 系统架构与数据流一个典型的 Bearing-Box 实现链路大致如下图像采集单目相机常规帧率 30fps 或更高。三维目标检测可以用任何能输出三维框的方法比如基于单目的 3D 检测网络或者基于已知目标尺寸的几何反推。观测提取从三维框的投影中提取角点方位、框尺寸、朝向角等观测。动力学建模根据目标类型选择 CV 或 CA 模型设定过程噪声。滤波器更新用扩展卡尔曼滤波EKF或无迹卡尔曼滤波UKF融合观测与动力学约束。输出目标的位置、速度、加速度估计以及协方差。这个链路里观测提取和动力学建模是两个最关键的环节直接决定可观性增强的效果。3.2 三维检测框的获取与预处理如果你用的是现成的三维检测网络输出通常是框的中心、尺寸、朝向。但 Bearing-Box 需要的是角点级别的信息。所以第一步是把框参数转成八个角点在相机坐标系下的坐标再投影到图像平面。这里有个细节检测框的朝向角通常有周期性模糊Yaw 差 π 等价。在时序滤波中这个模糊会导致观测跳变。我的处理方式是在观测方程里用朝向角的连续表示比如用 sin/cos 双分量或者在数据关联阶段做朝向一致性检查。另一个坑是框的尺寸抖动。检测网络输出的框尺寸往往有噪声直接用来估计深度会引入高频抖动。我通常会对框尺寸做低通滤波或滑动平均但要注意不能过度平滑否则会引入相位滞后影响速度估计。3.3 观测方程的构建观测向量 z 可以设计为z [u1, v1, u2, v2, ..., u8, v8, yaw, s]其中 (ui, vi) 是八个角点的图像坐标yaw 是朝向角s 是框的表观尺寸比如对角线长度或面积平方根。观测方程 h(x) 把状态 x 映射到 z。对于角点h 就是透视投影对于 yaw需要根据状态中的朝向角计算对于 s需要根据状态中的位置和已知的目标真实尺寸计算。这里的关键是目标真实尺寸是否已知。如果已知s 的观测方程很直接如果未知需要把尺寸也加入状态向量或者用其他方式估计。Bearing-Box 的场景里通常假设目标尺寸已知或可在线估计。3.4 动力学约束的嵌入方式动力学约束有两种嵌入方式硬约束作为等式约束加入滤波比如用投影法或拉格朗日乘子法。软约束作为伪观测加入给一个很大的权重。我倾向于用软约束因为硬约束在数值上容易出问题尤其是当约束之间不一致时。软约束的权重需要调太大会导致滤波器忽略真实观测太小则约束不起作用。具体操作上可以把动力学约束写成vx - v * cos(yaw) 0 vy - v * sin(yaw) 0其中 v 是目标速度大小。这两个等式作为伪观测加入权重根据对约束的置信度设定。3.5 滤波器选型与参数整定EKF 是最常用的选择计算量小工程实现成熟。但观测方程里有透视投影和三角函数线性化误差可能较大。UKF 通过 sigma 点传播对非线性处理更好但计算量增加。我的经验是如果帧率高、目标运动平稳EKF 够用如果目标机动频繁或帧率低UKF 更稳。参数整定方面过程噪声 Q 和观测噪声 R 的比值决定了滤波器对模型和观测的信任程度。Bearing-Box 的观测信息量大R 可以设得相对小一些让滤波器更信任观测。但要注意R 不能设得太小否则检测框的异常值会直接污染状态估计。我通常会用马氏距离做异常检测超过阈值的观测直接丢弃或降权。4. 常见问题与排查技巧实录4.1 滤波器不收敛或收敛慢这是最常见的问题。排查思路检查可观性矩阵的条件数。如果条件数过大说明某些状态方向仍然弱可观需要增加观测或调整约束。检查观测噪声 R 是否设得过大导致观测信息被淹没。检查过程噪声 Q 是否设得过小导致滤波器对模型过于自信拒绝观测修正。我遇到过一次滤波器对深度估计始终不收敛后来发现是三维检测框的尺寸观测被错误地关联到了另一个目标。数据关联错误是隐蔽性很强的问题建议加门控机制。4.2 深度估计漂移深度漂移通常是因为深度方向的可观性仍然不足。Bearing-Box 虽然增强了可观性但如果目标沿视线方向运动缓慢深度变化在图像上仍然不明显。应对方法增加尺度先验比如已知目标大致尺寸范围作为软约束。在滤波器里加入深度平滑项抑制高频漂移。如果场景允许让相机做主动运动比如小幅平移通过视差增强深度可观性。4.3 朝向角跳变朝向角的周期性模糊是老大难。我的处理方式在观测方程里用 sin/cos 表示避免角度跳变。在数据关联时检查朝向连续性如果跳变超过阈值尝试加 π 修正。如果检测网络输出的朝向本身噪声大考虑用时序滤波平滑。4.4 计算资源紧张MAV 上的计算资源有限EKF 的矩阵运算虽然不大但如果状态维度高、观测维度高仍然可能成为瓶颈。优化方向用稀疏矩阵存储可观性矩阵和雅可比。降低观测维度比如只选四个角点而不是八个。用固定点迭代代替完整的 UKF。4.5 常见问题速查表问题现象可能原因排查方法解决思路滤波器不收敛可观性不足检查可观性矩阵秩和条件数增加观测或调整约束深度估计漂移深度方向弱可观分析深度观测信噪比加尺度先验或主动运动朝向角跳变周期性模糊检查朝向角时序连续性用 sin/cos 表示或加 π 修正速度估计抖动观测噪声大检查 R 矩阵和检测框抖动低通滤波或增大 R计算超时矩阵运算量大分析各模块耗时降维或稀疏化5. 工程落地中的经验与建议5.1 仿真验证先行在真机上跑之前强烈建议先在仿真环境里验证。Gazebo 或 AirSim 都可以把目标运动、相机模型、检测噪声都建进去。仿真的好处是真值已知可以定量评估估计误差快速迭代参数。我在仿真里通常会做几组对比实验Bearing-only vs Bearing-BoxCV vs CAEKF vs UKF。这样能清楚地看到可观性增强带来的收益。5.2 检测框质量决定上限Bearing-Box 的效果高度依赖三维检测框的质量。如果检测框本身抖动大、朝向不准再好的滤波器也救不回来。所以检测网络的训练和调优不能省。如果检测框质量上不去可以考虑用时序检测或跟踪-检测联合优化。5.3 参数整定要有依据Q 和 R 的整定不要靠试凑。我的做法是先根据传感器手册和检测网络在验证集上的误差统计给出 R 的初始值再根据目标运动的典型加速度给出 Q 的初始值。然后在这个基础上微调。5.4 在线监控可观性工程系统里建议在线计算可观性矩阵的条件数作为健康指标。如果条件数突然变大说明系统进入了弱可观状态可以触发降级策略比如增大过程噪声、切换到更保守的控制模式。5.5 后续扩展方向Bearing-Box 的思路可以扩展到多目标场景每个目标维护一个滤波器共享相机运动信息。也可以扩展到多传感器融合把 IMU 的加速度观测加进来进一步增强可观性。如果目标类型已知还可以加入形状先验或运动模式库让动力学约束更精准。我个人在实际操作中的体会是单目运动估计的瓶颈往往不在滤波器而在观测模型的设计。Bearing-Box 给了很好的示范——把检测框的几何信息用足把动力学约束用对可观性自然就上来了。这个思路值得在更多场景里尝试。