Cartographer源码阅读05-imu_tracker - 前端匹配 一阶低通融合

imu_tracker

PoseExtrapolator 调用 imu_trakcer 来利用imu数据对角速度进行计算估计，并通过求解加速度来估计重力向量。

文件在 Cartographer/mapping/imu-tracker.cc

1.什么是imu?

惯性测量单元（英文：Inertial measurement unit，简称IMU）是测量物体三轴姿态角(或角速率)以及加速度的装置。陀螺仪及加速度计是IMU的主要元件，其精度直接影响到惯性系统的精度。

在实际工作中，由于不可避免的各种干扰因素，而导致陀螺仪及加速度计产生误差，从初始对准开始，其导航误差就随时间而增长，尤其是位置误差，这是惯导系统的主要缺点。所以需要利用外部信息进行辅助，实现组合导航，使其有效地减小误差随时间积累的问题。为了提高可靠性，还可以为每个轴配备更多的传感器。一般而言IMU要安装在被测物体的重心上。imu的角速度测量精度往往比线速度好的多，因此，在工程上，经常采用imu的角速度数据。

一般情况，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在导航中有着很重要的应用价值。

利用三轴地磁解耦和三轴加速度计，受外力加速度影响很大，在运动/振动等环境中，输出方向角误差较大,此外地磁传感器有缺点，它的绝对参照物是地磁场的磁力线,地磁的特点是使用范围大，但强度较低，约零点几高斯，非常容易受到其它磁体的干扰， 如果融合了Z轴陀螺仪的瞬时角度，就可以使系统数据更加稳定。

**加速度测量的是重力方向，**在无外力加速度的情况下，能准确输出ROLL/PITCH两轴姿态角度 并且此角度不会有累积误差，在更长的时间尺度内都是准确的。但是加速度传感器测角度的缺点是加速度传感器实际上是用MEMS技术检测惯性力造成的微小形变，而惯性力与重力本质是一样的,所以加速度计就不会区分重力加速度与外力加速度，当系统在三维空间做变速运动时，它的输出就不正确了。

陀螺仪输出角速度是瞬时量，角速度在姿态平衡上不能直接使用， 需要角速度与时间积分计算角度，得到的角度变化量与初始角度相加，就得到目标角度，其中积分时间Dt越小输出的角度越精确。但陀螺仪的原理决定了它的测量基准是自身，并没有系统外的绝对参照物，加上Dt是不可能无限小的，所以积分的累积误差会随着时间的流逝迅速增加，最终导致输出角度与实际不符，所以陀螺仪只能工作在相对较短的时间尺度内。

所以在没有其它参照物的基础上，要得到较为真实的姿态角，就要**利用加权算法（一阶低通滤波）**扬长避短，结合两者的优点，摈弃其各自缺点,设计算法在短时间尺度内增加陀螺仪的权值，在更长时间尺度内增加加速度权值，这样系统输出角度就接近真实值了。

2.imu-tracker.h

用来计算重力加速度的方向,相当于一个姿态解算器.从IMU数据中计算重力加
速度的向量.

class ImuTracker {
 public:

  // 构造函数,指数滤波器的常数 和 当前时间.
  ImuTracker(double imu_gravity_time_constant, common::Time time);

  // Advances to the given 'time' and updates the orientation to reflect this.
  // 把ImuTracker更新到指定时刻time
  //得到time时刻的重力向量信息,相当于进行外插.
  void Advance(common::Time time);

  // Updates from an IMU reading (in the IMU frame).
  // 加入IMU坐标系的数据,用来更新重力向量和角速度信息.
  void AddImuLinearAccelerationObservation(
      const Eigen::Vector3d& imu_linear_acceleration);

  void AddImuAngularVelocityObservation(
      const Eigen::Vector3d& imu_angular_velocity);

  // Query the current time.
  // 得到当前的时间.
  common::Time time() const { return time_; }

  // Query the current orientation estimate.
  // 得到当前的估计.
  Eigen::Quaterniond orientation() const { return orientation_; }

 private:
  const double imu_gravity_time_constant_;
  common::Time time_;
  common::Time last_linear_acceleration_time_;
  Eigen::Quaterniond orientation_;
  Eigen::Vector3d gravity_vector_;
  Eigen::Vector3d imu_angular_velocity_;
};

3.imu-tracker.cc

3.1Advance

按照时间利用角速度进行推测–外插

求得方位角，和重力向量

void ImuTracker::Advance(const common::Time time)
{
  CHECK_LE(time_, time);
  // 求当前时间对指定时间的时间差
  const double delta_t = common::ToSeconds(time - time_);

  //进行匀角速度运动的外插.求得角度
  //这里涉及到旋转的表示方式：角轴和四元数，都是旋转的不同表示方式
  const Eigen::Quaterniond rotation =
      transform::AngleAxisVectorToRotationQuaternion(
          Eigen::Vector3d(imu_angular_velocity_ * delta_t));

  //计算方向.
  orientation_ = (orientation_ * rotation).normalized();

  //计算重力向量.
  gravity_vector_ = rotation.conjugate() * gravity_vector_;

  time_ = time;
}

3.2跟新重力向量-:AddImuLinearAccelerationObservation

增加加速度信息的函数–用来跟新重力向量.使用了一阶低通融合算法防止加速度跳动剧烈。

void ImuTracker::AddImuLinearAccelerationObservation(
    const Eigen::Vector3d& imu_linear_acceleration)
{
  // Update the 'gravity_vector_' with an exponential moving average using the
  // 'imu_gravity_time_constant'.
  // 当前时刻离上一次时刻的差.
  const double delta_t =
      last_linear_acceleration_time_ > common::Time::min()
          ? common::ToSeconds(time_ - last_linear_acceleration_time_)
          : std::numeric_limits<double>::infinity();

  last_linear_acceleration_time_ = time_;

  //计算权重,时间差越大,当前的权重越大.
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);

  //一阶低通融合.
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;

  // Change the 'orientation_' so that it agrees with the current
  // 'gravity_vector_'.
  // 根据新的重力向量,更新对应的旋转矩阵.
  const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
      gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
      
  //更新方位角
  orientation_ = (orientation_ * rotation).normalized();

  CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
  CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
}

一阶低通融合

一阶滤波，又叫一阶惯性滤波，或一阶低通滤波。是使用软件编程实现普通硬件RC低通滤波器的功能。
　　一阶低通滤波的算法公式为：
　　

式中：α=滤波系数；X(n)=本次采样值；Y(n-1)=上次滤波输出值；Y(n)=本次滤波输出值。 一阶低通滤波法采用本次采样值与上次滤波输出值进行加权，得到有效滤波值，使得输出对输入有反馈作用。
其中权重系数a：

从公式可以看出，时间差越大,当前的权重越大.其中d为滤波器的常数
当数据快速变化时，滤波结果能及时跟进（灵敏度优先）；当数据趋于稳定，在一个固定的点上下振荡时，滤波结果能趋于平稳（平稳度优先）

3.3增加角速度信息

因为imu直接测得角速度，所以直接更新即可

//更新角速度信息.
void ImuTracker::AddImuAngularVelocityObservation(
    const Eigen::Vector3d& imu_angular_velocity)
{
  imu_angular_velocity_ = imu_angular_velocity;
}

小节

imu_tracker主要作用是利用角速度对旋转进行了插值，求得重力向量。
到目前为止，前端匹配还剩CSM和ceres就基本上结束了，接下来将对扫描匹配进行介绍。