cartographer位姿估计整理



 

cartographer在扫描匹配阶段（Ceres 非线性优化）之前，使用 优化后的位姿、IMU、码盘 三个数据源进行融合，估计出最新时刻的位姿，用来作为下次非线性优化的初值。下面，针对这部分数据融合模块进行分析整理，如下图红圈部分。

GitHub - cartographer-project/cartographer: Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.

cartographer/mapping/pose_extrapolator.cc中，

1数据源

AddPose中传入的pose，保存在timed_pose_queue_中

①	获取最新优化的位姿并删除历史数据
②	由最新优化的位姿解算出角速度和线速度（angular_velocity_from_poses_、linear_velocity_from_poses_）
③	使用IMU数据进行姿态估计（imu_tracker_），并将此时估计出的姿态用于计算 码盘线速度 和最新估计姿态 的初值（odometry_imu_tracker_、extrapolation_imu_tracker_）
④	删除最新优化的位姿时刻之前的码盘和IMU数据

AddImuData中传入的imu_data，保存在imu_data_中

①	获取IMU数据
②	删除 最新优化的位姿时刻 之前的IMU数据

AddOdometryData传入的odometry_data，保存在odometry_data_中

①	获取码盘数据
②	删除最新优化的位姿时刻之前的码盘数据
③	由码盘数据解算出角速度和线速度（angular_velocity_from_odometry_、linear_velocity_from_odometry_）

2数据融合

ExtrapolatePose	使用码盘计算出的线速度进行位置估计，使用IMU计算出的角度增量进行姿态估计
AdvanceImuTracker	使用IMU的陀螺仪和加速度计进行姿态估计，估计方法是比较简单的互补滤波算法。用此函数进行的姿态估计共有三个，分别是 imu_tracker_、odometry_imu_tracker_和extrapolation_imu_tracker_ imu_tracker_ 直接使用IMU数据进行，只进行一次初始化 odometry_imu_tracker_ 表示AddPose到AddOdometryData之间（即 最新优化的位姿时刻 到 最新码盘数据时刻 之间的时间段）的姿态变化 extrapolation_imu_tracker_ 表示AddPose到ExtrapolatePose之间（即 最新优化的位姿时刻 到 最新估计位姿时刻 之间的时间段）的姿态变化
ExtrapolatePose	获取融合完成的位姿数据

总结：	使用这种方法，对三个数据源做了严格的时间对齐，能够较为精确的获得估计后的位姿信息，用来作为后续非线性优化的初值。

                                                                                                                                                                                                                                                                                 

旧版本UKF算法整理

在旧版本的CartoGrapher中，针对上述数据融合使用了UKF算法，整理如下。

GitHub - slam-code/cartographer: 关于谷歌slam地图库cartographer的源码注释

cartographer/kalman_filter/pose_tracker.cc中，

1数据源

imu_tracker_	由IMU的加速度计和陀螺仪，使用简单的互补滤波方式解算出的姿态信息
odometry_state_tracker_	使用两次相邻时刻的码盘增量来计算新位姿
pose_observation	在外部（kalman_local_trajectory_builder.cc）经过非线性优化后得到的位姿信息

2状态量

此算法使用的状态量为9维向量，如下：

三维位置（PX PY PZ）

三维姿态误差（OX OY OZ）

三维速度（VX VY VZ）

说明：三维姿态误差是指IMU解算的姿态和真实姿态之间的误差

3状态方程

状态方程在ModelFunction中做了定义：

PX（k+1）= PX（k）+ VX（k）* dt

PY（k+1）= PY（k）+ VY（k）* dt

PZ（k+1）= PZ（k）+ VZ（k）* dt

OX（k+1）= OX（k）

OY（k+1）= OY（k）

OZ（k+1）= OZ（k）

VX（k+1）= VX（k）

VY（k+1）= VY（k）

VZ（k+1）= VZ（k）

4观测方程

ZPx（k）= PX（k）

ZPy（k）= PY（k）

ZPz（k）= PZ（k）

Rz（k）= R（OX（k），OY（k），OZ（k））* Rimu（k）

为了便于计算，代码中将观测方程做了归零处理，即

0 = PX（k）- ZPx（k）

0 = PY（k）- ZPy（k）

0 = PZ（k）- ZPz（k）

Diag（[1，1，1]）= Rz-1（k）* R（OX（k），OY（k），OZ（k））* Rimu（k）

5数据融合

数据融合完全按照UKF的迭代方式：

①	Predict 通过状态方程不断进行状态预测，获得最新时刻的位姿估计；
②	AddOdometerPoseObservation AddPoseObservation 当码盘或优化后的位姿数据到来时，则进行状态更新（注：单独的IMU数据到来，只进行预测、不进行更新）；
③	GetPoseEstimateMeanAndCovariance 获取融合后的位姿信息

总结：	使用这种方法，将IMU、码盘、优化后的位姿进行数据融合，得到一个较为精确的融合位姿数据。 相比最新代码中使用的融合方法，此UKF存在如下问题： ①三个数据源的时间戳对准做的不精细 ②IMU姿态融合过程完全依赖 码盘数据 或 优化后的位姿 的到来时刻一起做状态更新（AddPoseObservation），会引起比较大的姿态估计滞后 ③姿态估计时直接使用IMU互补滤波计算出的姿态，会引入累计误差 ④位置估计时主要使用两次相邻时刻码盘的位置增量，而不是由线速度计算，精度较差 ⑤UKF本身存在延迟且计算量过大