cartographer模块篇2 - 位姿插值器

SLAM||DUNK

已于 2023-02-11 19:51:04 修改

阅读量2.1k

点赞数 5

分类专栏：激光SLAM 文章标签：算法

于 2021-03-06 23:12:57 首次发布

本文链接：https://blog.csdn.net/qq_42700518/article/details/110913070

版权

激光SLAM 专栏收录该内容

13 篇文章 13 订阅

订阅专栏

PoseExtrapolator

类框图如下

在这里插入图片描述

总结:
PoseExtrapolator 的作用——
主要用与前端里程计：

进行畸变去除
点与处理时估计重力 EstimateGravityOrientation()
提供scanMatcher位姿的预测值 extrapolator_->ExtrapolatePose() 。

主要思想: 采用匀速运动学模型预测未来时间time处的pose.
而匀速运动学模型的平移速度 是由1.里程计或者2.两次scanmatch结果的微分得到,并优先采用里程计的数据.
角速度 是由1.采用两次scan-match的航向进行微分获得；2.里程计；3.直接用IMU的角速度测量，优先级最高；

核心任务函数有两个：
在这里插入图片描述

文件: cartographer/mapping/pose_extrapolator.h , cartographer/mapping/pose_extrapolator.cc

一、信息更新

一共接收laser scan match pose、odom、imu的数据进行更新。
PoseExtrapolator中 laser, odom，imu的作用如下：
laser: 在没有IMU、odom的情况下估计运动速度，以及提供基准pose。
IMU:估计姿态。
ODOM：ODOM的测量数据并没有全部使用，只是提供关键时刻的线速度和角速度，。

1.1 接收scan match pose

调用AddPose()
该函数添加激光匹配的结果来更新运动模型.

图：AddPose()

1.2 接收IMU数据

AddImuData(const sensor::ImuData& imu_data)

图：AddImuData()

二、位姿插值

ExtrapolatePose(const common::Time time)

分为平移插值和旋转插值，
这里平移插值就是简单的依据当前线速度信息，然后根据匀速运动学模型提推下一时刻，如果有odom数据，可以采用精度更高的方法，就是直接用所有odom的数据递推到time时刻，

旋转插值：在有IMU的情况下，会用IMU所有数据递推time时刻pose，这个很正常，没IMU而有ODOM时，就近似的用ODOM观测角速度进行匀速运动学模型递推，虽然此时若能将ODOM的全部数据直接参与递推理论上会精度更高，但考虑到ODOM角速度本身就不高的测量精度，这样的近似操作也可以。
在这里插入图片描述
其中

图：AddPose()

三、估计姿态

这个比较简单，就用了一个ImuTracker。

/**
 * @brief 计算重力向量,用imu-tracker来进行计算.
 * @param[in] time 
 * @return Eigen::Quaterniond 
 */
Eigen::Quaterniond PoseExtrapolator::EstimateGravityOrientation(
    const common::Time time) {
  ImuTracker imu_tracker = *imu_tracker_;
  AdvanceImuTracker(time, &imu_tracker);
  return imu_tracker.orientation();
}

ImuTracker类分析在后。

ImuTracker类

ImuTracker类负责根据IMU的加速度与角速度测量,去估计姿态.
初始化时姿态以及重力都被设置为与地面垂直:
主要思想是:

通过 Advance()函数根据角速度测量, 预测时间time处的姿态以及重力.

求解出姿态预测orientation_：

// 上一时刻的角速度乘以时间,得到当前时刻相对于上一时刻的预测的姿态变化量,再转换成四元数
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));
  // 使用上一时刻的姿态 orientation_ 乘以姿态变化量, 得到当前时刻的预测出的姿态
  orientation_ = (orientation_ * rotation).normalized();    // Qw_bt * Qbt_bt+1 = Qw_bt+1

根据姿态预测，求解出对应的重力向量：

gravity_vector_ = rotation.conjugate() * gravity_vector_; // Qbt_bt+1^-1 * g_b = Qbt+1_b * g_b = g_b+1

当加速度信息到来后, 利用加速度测量校正姿态,如下函数
void ImuTracker::AddImuLinearAccelerationObservation( const Eigen::Vector3d& imu_linear_acceleration)
首先将最新的加速度测量与预测的重力gravity_vector_通过滤波器进行融合, 然后对预测的姿态 orientation_ 进行校正.

先计算权重

/********* 指数平均滤波 ***********/
  // 计算权重    即 时间间隔越久  alpha 越接近1  那么 重力越倾向于 最新测量的结果 
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);

将预测的重力和观测重力进行融合，得到优化重力。

  // 一阶低通滤波  融合 IMU测量与后验重力
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;          // gravity_vector_ 是预测结果    重力在IMU系下的表示

这里要注意，由于仅有重力的观测会导致yaw姿态不可观，因此，不可直接用这个融合后的重力gravity_vector_ 去恢复姿态，注意到我们在预测环节已经求解了预测姿态orientation_，我们可以先根据融合后的重力和预测重力求解出旋转校正量：

// 求解出旋转矫正量
  const Eigen::Quaterniond rotation = FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());

rotation将gravity_vector_转换到观测重力向量，即
rotation * gravity_vector_ = orientation_.conjugate() * Eigen::Vector3d::UnitZ()
那么rotation 实际上是融合校正后IMU坐标系 -> 预测IMU坐标系的旋转 $Q^{b_{predict}}_{b_{update}}$ 。
然后用这个微调旋转去矫正预测的姿态：

  orientation_ = (orientation_ * rotation).normalized();

即 $Q^{n}_{b_{update}} = Q^{n}_{b_{predict}}Q^{b_{predict}}_{b_{update}}$
姿态更新便完成了。

代码里，delta_t等于与上次加速度校正的时间差, 从这段代码可以看出,
距离加速度校正的时间间隔越久, 就越不相信角速度预测的姿态, 而更相信下一次加速度测量出来的姿态.
由于角速度信息短时间比较准, 因此短时间内更偏向于角速度预测出来的姿态, 而由于累计误差的存在,长时间角速度积累的误差较大, 而加速度测量与时间无关,所以长时间更偏向与加速度的测量.
总之是一种非常朴素简单的思想, 精度肯定不高,但是对于机器人来说足够了.