位姿推测器-cartographer
Kong Liangqian Lv6
 2021-11-15 18:38:29  

AddPose

对整个位姿估计器进行校准，本身估计器就是IMU加上里程计进行预测，预测肯定就有不准的情况，因此需要校准。就需要整个匹配后的Pose进行校准

首先是IMU_tracker 没有初始化则初始化

把Pose加入到

1
2
// 在timed_pose_queue_中保存pose
timed_pose_queue_.push_back(TimedPose{time, pose});

把这一帧Pose加入到timed_pose_queue，同时也要裁剪

1
2
3
// 将imu_tracker_更新到time时刻,
// 本来是跟新到上一帧的姿态，现在把可以预测的时间拉到现在这个时间
AdvanceImuTracker(time, imu_tracker_.get());

AdvanceImuTracker

把imu_tracker 更新到time时刻

整个函数完成了一个，根据IMU数据队列，利用imu_tracker对时间点time的imu姿态进行预测，然后校准，再预测，再校准,

此函数将imutracker更新到time时刻

还有一个情况，如果不适用imu数据，或者需要预测的时刻都还没有imu的数据

那么就角速度就取里程计的，如果不适用里程计，那么就使用angularvelocity_from_poses ，校准的位姿

因此即便是没有使用IMU，计算的方式其实也是一样的，只不过线性加速度永远不会变，角速度来自于odometry

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
/**
 * @brief 更新imu_tracker的状态, 并将imu_tracker的状态预测到time时刻
 * 
 * @param[in] time 要预测到的时刻
 * @param[in] imu_tracker 给定的先验状态
 */
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                         ImuTracker* const imu_tracker) const {
  // 检查指定时间是否大于等于 ImuTracker 的时间
  CHECK_GE(time, imu_tracker->time());

  // 不使用imu 或者 预测时间之前没有imu数据的情况
  if (imu_data_.empty() || time < imu_data_.front().time) {
    // There is no IMU data until 'time', so we advance the ImuTracker and use
    // the angular velocities from poses and fake gravity to help 2D stability.
    // 在time之前没有IMU数据, 因此我们推进ImuTracker, 并使用姿势和假重力产生的角速度来帮助2D稳定
    
    // 预测当前时刻的姿态与重力方向
    imu_tracker->Advance(time);
    // 使用 假的重力数据对加速度的测量进行更新
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    // 只能依靠其他方式得到的角速度进行测量值的更新
    imu_tracker->AddImuAngularVelocityObservation(
        odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    return;
  }

  // imu_tracker的时间比imu数据队列中第一个数据的时间早, 就先预测到imu数据队列中第一个数据的时间
  // 比imu最早的数据的时间，都可以预测，方便后面开始循环
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of 'imu_data_'.
    imu_tracker->Advance(imu_data_.front().time);
  }

  // c++11: std::lower_bound() 是在区间内找到第一个大于等于 value 的值的位置并返回, 如果没找到就返回 end() 位置
  // 在第四个参数位置可以自定义比较规则,在区域内查找第一个 **不符合** comp 规则的元素

  // 在imu数据队列中找到第一个时间上 大于等于 imu_tracker->time() 的数据的索引
  auto it = std::lower_bound(
      imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
      [](const sensor::ImuData& imu_data, const common::Time& time) {
        return imu_data.time < time;
      });

  // 然后依次对imu数据进行预测, 以及添加观测, 直到imu_data_的时间大于等于time截止
  // 这里相当于就是先预测，然后再校准（两个速度）
  while (it != imu_data_.end() && it->time < time) {
    // 预测出当前time时刻的姿态与重力方向
    imu_tracker->Advance(it->time);
    // 根据线速度的观测,更新重力的方向,并根据重力的方向对上一时刻预测的姿态进行校准
    imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
    // 更新角速度观测
    imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
    ++it;
  }
  // 最后将imu_tracker的状态预测到time时刻
  imu_tracker->Advance(time);
}

注意，这个advance，就是去变更imu_tracker的在某一个时刻time的四元数位姿，变更的依据为用于校准的角速度。即旋转情况。

EstimateGravityOrientation

预测得到time时刻 tracking frame 在 local 坐标系下的姿态

这个数据传过来的时候就已经被传到local坐标系下了

1
2
3
4
5
6
7
8
9
// 预测得到time时刻 tracking frame 在 local 坐标系下的姿态
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(
    const common::Time time) {
  ImuTracker imu_tracker = *imu_tracker_;
  // 使得 imu_tracker 预测到time时刻
  AdvanceImuTracker(time, &imu_tracker);
  // 返回 imu_tracker 预测到的time时刻 的姿态
  return imu_tracker.orientation();
}

ExtrapolatePose

有一个缓存变量，如果cachedextrapolated_pose 时间已经算过了就不需要再一次计算了

这里extrapolationimu_tracker 是用于旋转的预测，

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// 预测得到time时刻 tracking frame 在 local 坐标系下的位姿
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  CHECK_GE(time, newest_timed_pose.time);
  // 如果本次预测时间与上次计算时间相同 就不再重复计算
  if (cached_extrapolated_pose_.time != time) {
    // 预测tracking frame在local坐标系下time时刻的位置
    const Eigen::Vector3d translation =
        ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
    // 预测tracking frame在local坐标系下time时刻的姿态
    const Eigen::Quaterniond rotation =
        newest_timed_pose.pose.rotation() *
        ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
    cached_extrapolated_pose_ =
        TimedPose{time, transform::Rigid3d{translation, rotation}};
  }
  return cached_extrapolated_pose_.pose;
}

ExtrapolateTranslation

预测一下time时刻的位移，通过 线性加速度乘以时间差，就可以得到位移差

1
2
3
4
5
6
7
8
9
10
11
12
13
// 返回从最后一个位姿的时间 到time时刻 的tracking frame在local坐标系下的平移量
Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  const double extrapolation_delta =
      common::ToSeconds(time - newest_timed_pose.time);
      
  // 使用tracking frame 在 local坐标系下的线速度 乘以时间 得到平移量的预测
  if (odometry_data_.size() < 2) {
    return extrapolation_delta * linear_velocity_from_poses_;
  }
  // 如果不使用里程计就使用通过pose计算出的线速度
  return extrapolation_delta * linear_velocity_from_odometry_;
}

ExtrapolateRotation

先通过imu_tracker 获取上一次位姿校准的姿态，然后乘上现在的orientation，求出姿态变化量。

注意：

姿态变化量=上一次位姿的逆 * 这一次的位姿

1
2
3
4
5
6
7
8
9
10
11
12
13
// 计算从imu_tracker到time时刻的姿态变化量
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
    const common::Time time, ImuTracker* const imu_tracker) const {
  CHECK_GE(time, imu_tracker->time());

  // 更新imu_tracker的状态到time时刻
  AdvanceImuTracker(time, imu_tracker);

  // 通过imu_tracker_获取上一次位姿校准时的姿态
  const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
  // 求取上一帧到当前时刻预测出的姿态变化量：上一帧姿态四元数的逆 乘以 当前时刻预测出来的姿态四元数
  return last_orientation.inverse() * imu_tracker->orientation();
}

ExtrapolatePosesWithGravity

其返回结果为N个pose ， 当前的速度和一个位姿

1
2
3
4
5
6
7
8
9
10
11
12
13
/**
   * @brief 
   * 包含 请求时间内索引为0到N-1的位姿, 请求时间内索引N处的位姿
   * 当前线速度与当前重力方向
   */
  struct ExtrapolationResult {
    // The poses for the requested times at index 0 to N-1.
    std::vector<transform::Rigid3f> previous_poses;
    // The pose for the requested time at index N.
    transform::Rigid3d current_pose;
    Eigen::Vector3d current_velocity;
    Eigen::Quaterniond gravity_from_tracking;
  };

按照时间逐个预测位姿，存入vector

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
// 获取一段时间内的预测位姿的结果
PoseExtrapolator::ExtrapolationResult
PoseExtrapolator::ExtrapolatePosesWithGravity(
    const std::vector<common::Time>& times) {
  std::vector<transform::Rigid3f> poses;

  // c++11: std::prev 获取一个距离指定迭代器 n 个元素的迭代器,而不改变输入迭代器的值
  // 默认 n 为1,当 n 为正数时, 其返回的迭代器将位于 it 左侧；
  // 反之, 当 n 为负数时, 其返回的迭代器位于 it 右侧

  // 获取 [0, n-1] 范围的预测位姿
  for (auto it = times.begin(); it != std::prev(times.end()); ++it) {
    poses.push_back(ExtrapolatePose(*it).cast<float>());
  }

  // 进行当前线速度的预测
  const Eigen::Vector3d current_velocity = odometry_data_.size() < 2
                                               ? linear_velocity_from_poses_
                                               : linear_velocity_from_odometry_;
  return ExtrapolationResult{poses, ExtrapolatePose(times.back()),
                             current_velocity,
                             EstimateGravityOrientation(times.back())};
}

总结

PoseExtrapolator 这个位姿推测器，他就是用来预测位姿，位姿又分为平移和旋转（姿态），姿态通过IMUtraker预测，平移通过线速度进行预测，使用里程计的话就用里程计的线速度进行预测，没有的画 就用Pose计算出来的线速度进行预测

旋转：首选考虑的IMU，先预测，然后进行通过线性加速度进行校准，imu_tracker一个是确定俯仰角和翻滚角，还有一个就是对yaw角进行推测，偏航角，这个角就是用角速度进行推测，首先考虑imu的角速度，次之使用里程计的角速度，再次之使用pose的角速度

cartographer总是认为他的位姿推测器是准确的，因此如果IMU不准，有加速度却没有动的话，他会以为你的机器人在移动

使用里程计的时候，最好还是要加上imu，因为 里程计的角度是通过其他方式进行预测的，不如就自己搞雷达

 Comments