室外移动机器人RTK恢复后连续轨迹修正方案咨询

问题背景 #

室外移动机器人采用RTK+IMU+Odom融合定位方案：RTK信号正常时定位精度可靠；RTK信号丢失阶段切换为IMU+Odom推算定位，但该阶段轨迹与实际轨迹存在偏差，需在RTK恢复后修正这段轨迹，保证整条轨迹连续无断裂。

可用数据：

• RTK信号正常时段的真实轨迹坐标(x,y)
• 全时段（含RTK丢失、恢复后）IMU+Odom推算的完整轨迹坐标(x,y)

已尝试方案：VSLAM的sim3算法，效果存疑，需其他可行方案。

可行修正方案 #

1. 位姿对齐式分段轨迹拼接法 #

• 核心逻辑：利用RTK丢失前最后一个有效真实位姿、RTK恢复后第一个有效真实位姿，计算IMU+Odom丢失阶段轨迹的2D刚体变换（旋转+平移），将这段轨迹整体对齐到真实轨迹坐标系。
• 操作步骤：
  1. 提取RTK丢失前最后一组对应位姿：真实位姿(x_prev, y_prev, yaw_prev)、IMU+Odom推算位姿(x'_prev, y'_prev, yaw'_prev)
  2. 提取RTK恢复后第一组对应位姿：真实位姿(x_curr, y_curr, yaw_curr)、IMU+Odom推算位姿(x'_curr, y'_curr, yaw'_curr)
  3. 通过两组位姿对求解2D刚体变换矩阵T（包含旋转角θ、平移量tx/ty）
  4. 将RTK丢失阶段所有IMU+Odom推算位姿应用T变换，得到修正后轨迹
• 适用场景：短时间RTK丢失，能保证轨迹首尾与真实轨迹无缝衔接，计算效率高。

2. 多约束全局轨迹优化法 #

• 核心逻辑：以RTK正常时段的真实位姿为关键约束点，对IMU+Odom整条轨迹做全局优化，既保证约束点处与真实轨迹重合，又保留IMU+Odom轨迹的局部运动平滑性。
• 操作步骤：
  1. 整理所有约束点：RTK正常时段的真实位姿(x_i, y_i)与对应时刻IMU+Odom推算位姿(x'_i, y'_i)
  2. 构建优化目标：最小化约束点的位姿误差，同时加入相邻帧位姿变化的平滑约束（保证运动连续性）
  3. 用g2o或Ceres Solver等非线性优化器求解最优轨迹
• 适用场景：长时间RTK丢失，修正后的轨迹兼顾全局准确性与局部合理性，平滑度高。

3. 系统误差标定补偿法 #

• 核心逻辑：利用RTK正常时段的真实轨迹与IMU+Odom推算轨迹的差异，标定IMU（姿态零偏、加速度零偏）和轮式里程计（刻度系数）的系统误差，再用标定后参数重新推算丢失阶段轨迹。
• 操作步骤：
  1. 在RTK正常时段，采集足量真实位姿与推算位姿的对应数据
  2. 建立误差模型，完成IMU与里程计的参数标定
  3. 用标定后的参数重新运行IMU+Odom融合算法，生成修正后的丢失阶段轨迹
• 优势：从根源降低系统误差，既能修正历史轨迹，也能提升后续RTK丢失时的推算精度。

4. 时间对齐式轨迹插值法 #

• 核心逻辑：将两条轨迹按时间戳严格对齐，RTK正常时段用真实轨迹覆盖推算轨迹；丢失时段以前后真实位姿为端点，对推算轨迹做非线性插值修正，保证轨迹连续光滑。
• 操作步骤：
  1. 对真实轨迹与推算轨迹做时间戳同步，缺失时刻的位姿用相邻帧插值补充
  2. 确定RTK丢失的时间窗口t_start至t_end
  3. 以t_start、t_end时刻的真实位姿为端点，用三次样条插值修正丢失时段的推算轨迹
• 适用场景：对实时性要求高的场景，实现简单，无需复杂优化或标定。

内容的提问来源于stack exchange，提问作者dfagg