沉默杀手HomeArchivesTagsCategories
PoseExtrapolator 1
|激光SLAMCartographer源码解读|
Word count: 1.6k|Reading time: 5 min

位姿估计器就是用一段时间的位姿数据估计线速度和角速度，进而预测运动。

位姿估计器在Node中有一个（一条轨迹一个），在LocalTrajectoryBuilder也有一个。 Node中的估计器只有在Node::PublishTrajectoryStates有用，也就只有执行发布数据的时候才会将local_slam_pose添加到其中。所以重点看后者。

看LocalTrajectoryBuilder::AddRangeData函数，发现估计器会在两种情况下初始化：如果没在lua里启用IMU，就会在LocalTrajectoryBuilder2D里直接初始化；如果在lua里启用了，会在收到IMU数据后再初始化，否则一直等待，此时也不会处理里程计数据。

从Cartographer流程图可以看出，PoseExtrapolator输入为scan match反馈的位姿、 IMU、里程计三个数据源，估计出下一时刻的位姿，用来作为下次scan match的初值。

框架图

scan match可以获取较高精度的位姿，但是两次扫描匹配之间的运动只有里程计数据，如果里程计误差严重，将影响定位。

InitializeExtrapolator

初始化需要一个pose_queue_duration_(1ms) 和 重力加速度时间常数(10s)。
初始化之后立刻添加一个位姿为0的位姿。添加位姿的过程:实例化imu_tracker_，更新速度信息；处理imu和 odom 数据；初始化odometry_imu_tracker_和extrapolator_imu_tracker_都指向 imu_tracker_

1
2
3
4
5
6
7
8
9
10
11
if(extrapolator_ != nullptr)
    return;
CHECK(!options_.pose_extrapolator_options().use_imu_based());
// 创建一个PoseExtrapolator类型的位姿推理器
// pose queue 用于速度估计，pose_queue_duration是估计所用的时间
extrapolator_ = absl::make_unique<PoseExtrapolator>(
    ::cartographer::common::FromSeconds(
    options_.pose_extrapolator_options().constant_velocity().pose_queue_duration() ),
    options_.pose_extrapolator_options().constant_velocity().imu_gravity_time_constant()  );
// 添加一个初始位姿，Rigid3d::Identity()对变换矩阵赋值，位移为0，旋转为1
extrapolator_->AddPose(time, transform::Rigid3d::Identity() );

PoseExtrapolator 的三条线

PoseExtrapolator中管理了三个队列：
  1. imu_data_： 保存imu的双向队列 std::deque
  2. odometry_data_： 保存里程计数据的双向队列
  3. timed_pose_queue_: 保存估计的位姿的双向队列 std::deque。只保存时间在 pose_queue_duration_范围内的位姿，旧的数据要删除掉。 位姿包括从IMU测量得到的Pose，以及从Scan Match输出的PoseObservation。

流程图

AddImuData 和 AddOdometryData

PoseExtrapolator管理里程计数据和经过ImuTracker的IMU数据，这两个函数的调用的最终源头是LocalTrajectoryBuilder2D::AddOdometryData和LocalTrajectoryBuilder2D::AddImuData

AddImuData比较简单，主要是把IMU数据加入imu_data_队列，清除旧的imu数据，保证队列中第2个数据的时间大于最后一个位姿的时间。AddOdometryData除了添加里程计数据，删除比最新位姿旧的odom数据，还根据首尾的odometry数据，计算tracking 坐标系下的线速度和角速度。

AddPose, ExtrapolatePose, EstimateGravityOrientation，这三个函数在LocalTrajectoryBuilder2D::AddRangeData中调用

ExtrapolatePose

目的是预测time时刻tracking坐标系在local坐标系下的位姿。假设传感器数据之间，机器人以恒定线速度和恒定角速度移动( local坐标系 )

估计的线速度有2种方式组成，优先级参见ExtrapolateTranslation：

  1. 由于里程计的更新速度更快，优先使用里程计获得线速度
  2. 采用两次scan-match位置进行微分获得

估计的角速度有3种方式，按优先级(参见AdvanceImuTracker开始部分)：

  1. 采优先使用IMU数据获取角速度
  2. 采用里程计进行微分
  3. 采用两次scan-match的朝向进行微分获得，频率最低

以上估计线速度和角速度所用的两次scan mathch的位姿的处理都在UpdateVelocitiesFromPoses。如果出现了报警： Queue too short for velocity estimation，可以尝试降低odometry_sampling_ratio 和 imu_sampling_ratio，先从1降到0.5

最后针对线速度和角速度进行实时积分，输出当前时间的融合的估计位姿

AddPose

存入的是前端一帧一帧激光匹配的位姿，就是两个node之间的位姿。由于pose是由激光匹配产生的，而激光的频率是比IMU和odom要低的，所以这里pose的传入频率比IMU、odom低，比如传入5帧IMU，才插入一个pose。

位姿估计器十分相信scan match得到的位姿，如果遇到长走廊等环境相似的区域，这个位姿就会有问题

只在Node::PublishTrajectoryStates和LocalTrajectoryBuilder2D::AddAccumulatedRangeData调用。重点是后者，这个pose是Scan Match反馈给估计器以修正估计值, 也就是添加到timed_pose_queue_队列

函数大致流程:

  • 确保ImuTracker实例化：实例化时需要用到重力加速度常数，初始时间。重力常数主要用来做距离的修正。

  • 将该位姿pose加入到timed_pose_queue_中，删除其中比加入时间早 pose_queue_duration_的位姿。

  • 根据timed_pose_queue_中的位姿估计速度：找出队列中最久和最新的位姿和时间， 通过除以时间得到线速度(linear_velocity_from_poses_)和角速度(angular_velocity_from_poses_)。 计算角速度的时候是使用四元数old_q.inverse() * new_q 得到old_q 到 new_q 的角位移，然后除以时间得到的。队列中的位姿时间要求如下：
    timed_pose_queue队列
    一般选择1ms以内的姿态来估计，而且旧的数据要删除掉。

  • 进一步处理ImuTracker：如果到该时间之前没有imu数据，那么使用从poses中推断的角速度和一个假的重力来保障2d的稳定。如果有IMU，使用IMU数据进行姿态估计（imu_tracker_），并将此时估计出的姿态用于计算 里程计线速度 和最新估计姿态 的初值（odometry_imu_tracker_ extrapolation_imu_tracker_）

GetLastPoseTime

1
2
3
4
5
6
common::Time PoseExtrapolator::GetLastPoseTime() const {
  if (timed_pose_queue_.empty()) {
    return common::Time::min();
  }
  return timed_pose_queue_.back().time;
}

就是返回队列最后一个pose对应的时间

在位姿融合中,因为IMU和里程计的采集速率比scan match快很多，因而在一定时间段内，可能没有scan match估计的位姿数据，这时需要通过 IMU 和里程计数据进行推断，但因为这两个传感器存在相对激光扫描较大的累积误差，所以需要对两者进行一些更新和初始化消除累积误差

最后以里程计数据为例，补上整个的处理流程

为什么每次新建地图的角度不同：每次开始的方向不同，从第一帧雷达数据到来时，前端子图的坐标原点来自ExtrapolatePose，此时机器人发生旋转，ExtrapolatePose输出的位姿朝向会不同；另外还有IMU的数据问题。