踩坑/Robotics Stack Exchange·2015-08-09

视觉位移只有方向时如何估计相机间位置

在已知初始距离和航向的前提下，用正弦定理求距离，再用卡尔曼滤波融合 IMU 与 GPS。

AI 整理

• 感知与导航

本文由 AI 根据公开内容整理，可能有误，请以原文为准：Kalman filter for estimating position with “direction” measurements（Robotics Stack Exchange #7832）

状态：已解决

问题

两台相机各带 GPS/IMU，相机2相对相机1的视觉位姿估计较准，但平移只有归一化方向、没有绝对尺度，难以直接得到更准确的公制位移。

环境

两台相机；相机1固定稳定；相机2为 3D 噪声位姿；各自带 GPS/IMU；视觉相对位姿尺度不确定。

解决办法

1. 假设相机2初始时与相机1的距离和初始航向已知，将初始距离记为 a。
2. 相机2移动后，用 IMU 给出的行进距离作为 b，用相机1输出通过反正切得到初始位置到当前位置的航向变化角 B。
3. 根据 a、b、B 使用正弦定理计算相机1与相机2的新距离 c。
4. 用一个基本卡尔曼滤波器滤波 IMU 距离输出以改进 b 和 c，再用另一个滤波器将该结果与有噪声的 GPS 位置融合；短期优于 GPS，长期由 GPS 修正漂移。

原问题作者：HighVoltage；采纳答案作者：Chuck。原内容依 CC BY-SA 4.0 授权，本页摘要同样依 CC BY-SA 4.0 授权。

来源

本页事实对应的公开页面，以原文为准。

1. Robotics Stack Exchange #7832：Kalman filter for estimating position with “direction” measurements访问 2026-10-22