六轴IMU估计自身在世界坐标系的位姿

一般的IMU包含三轴的加速度记和三轴的陀螺仪，成为六轴IMU，可以提供IMU坐标系下三轴加速度的测量以及IMU坐标系下三轴角速度的测量，而有的IMU包含三轴的磁力计，可以提高IMU在世界坐标系下的姿态。
世界坐标系是指惯性系，即重力方向与Z轴重合。
但是六轴IMU由于可以测量出重力方向，可以通过自身线加速度和角速度测量数据，估计自身在世界坐标系下的姿态。

六轴IMU估计IMU在世界坐标系下的姿态

IMU可以通过线加速度测量数据估计IMU在世界坐标系下roll和pitch角，但是世界坐标系下IMU的yaw角对于重力方向上不可观的，因此将IMU初始化时的朝向作为yaw角的初始值，然后根据IMU的角速度测量数据，通过积分的位姿变化量更新IMU在世界坐标系下的位姿。
因此需要估计IMU坐标系下的重力向量gravity_vector_，以及世界坐标系下IMU的姿态orientation_。

初始化

世界坐标系下IMU的yaw角对于重力方向上不可观的，因此将IMU初始化时的朝向作为yaw角的初始值。

ImuTracker::ImuTracker(const double imu_gravity_time_constant,
                       const common::Time time)
    : imu_gravity_time_constant_(imu_gravity_time_constant),
      time_(time),
      last_linear_acceleration_time_(common::Time::min()),
      // 世界坐标系下IMU的yaw角对于重力方向上不可观的，因此将IMU初始化时的朝向作为yaw角的初始值
      orientation_(Eigen::Quaterniond::Identity()),
      gravity_vector_(Eigen::Vector3d::UnitZ()),
      imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
1
2
3
4
5
6
7
8
9

通过重力向量估计IMU在世界坐标系下的姿态

由于IMU的线加速度测量会受到摇晃颠簸的影响，线加速度测量会出现大幅度的震荡，因此需要指数滤波，在更新重力方向的时候，时间越久，线加速度测量的权重越大，反之越小，这样可以从线加速度测量中获取重力向量。

  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_;
  // 根据时间变化进行指数滤波，过滤IMU由于车身摆动造成的晃动，保存重力向量。`imu_gravity_time_constant_`越大，重力向量保持得越好
  const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
  gravity_vector_ =
      (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
1
2
3
4
5
6
7
8
9

在IMU坐标系下，根据重力方向到世界坐标Z轴的转角更新IMU在世界坐标系下的坐标。

  // 由于IMU观测到重力方向指向世界坐标系的Z轴，`gravity_vector_`表示当前时刻IMU坐标系下世界坐标系的Z轴，
  // `orientation_.inverse() * Eigen::Vector3d::UnitZ()`表示上一时刻IMU坐标系下世界坐标系的Z轴
  // 因此求出Rwi,wi-1
  const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
      gravity_vector_, orientation_.inverse() * Eigen::Vector3d::UnitZ());
  // 根据IMU重力推测的增量，更新IMU在世界坐标系下的姿态
  orientation_ = (orientation_ * rotation).normalized();
1
2
3
4
5
6
7

根据IMU角速度积分，更新IMU在坐标系下的姿态

IMU的角速度测量也可以估计出IMU的姿态，并且对IMU在世界坐标系下的yaw角是可观的。
因此对IMU的角速度测量进行积分，通过位姿变化量对IMU在世界坐标系下的位姿以及在IMU坐标系下的重力向量进行更新。

  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.inverse() * gravity_vector_;
  time_ = time;
1
2
3
4
5
6
7