如何利用imu计算位姿
Kong Liangqian Lv6
 2021-12-28 18:38:29  

背景

本文介绍如何通过imu的数据来求得位姿

在Imu返回的数据中，包含了如下内容

  • angular_velocity：角速度
  • linear_acceleration：线性加速度，包含重力加速度
  • orientation：位姿，他是通过积分求得，不准确（为什么？）

因为他本身的不准确，因此下面介绍cartographer中是如何进行位姿的预测的

计算方法

cartographer是计算每一个imu的数据，从而得到每一个数据所对应的位姿信息，因此称为imu_tracker，即不停的追踪每一个imu的数据。

cartographer会把imu数据保存在一个imudata的双端队列中，会实时的裁剪，以致于不会队列不会太长

再预测位姿的时候，我们通过不断的遍历imudata中的imu数据，做如下三个事情

  • 来预测一下该imu数据下的位姿，顺便更新线性加速度
  • 添加线性加速度的观测，校正一下位姿，顺便更新线性加速度
  • 更新一下角速度的值

第一个事情Advance

每一次预测，都会记录当前预测的时间，今后就作为上一次预测的时间，

通过【（当前时间-上一次预测时间）* 上一次观测到的角速度 】 可以得到一个旋转量，直接把这个旋转量作用到上一次预测的位姿上，就是最新的位姿

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

第二个事情 AddImuLinearAccelerationObservation

把观测到底线性加速度的值和上一个线性加速度做一个简单的融合（指数滑动平均法），使得线性加速度更为平滑

1
2
3
4
5
6
7
8
9
10
// Step: 2 求alpha, alpha=1-e^(-delta_t/10)
// delta_t越大, alpha越大
const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);

// Step: 3 将之前的线加速度与当前传入的线加速度进行融合, 这里采用指数滑动平均法

// 指数来确定权重, 因为有噪声的存在, 时间差越大, 当前的线性加速度的权重越大
// 这里的gravity_vector_改成线性加速度更清晰一些
gravity_vector_ =
    (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;

再计算新的线性加速度和旧的线性加速度的一个差，相当于是看看重力方向差了多少旋转，然后把这个旋转作用到之前的位姿上进行更新，得到最新的位姿

第三个事情更新一下角速度的值

这里只是简答的更新角速度值，不做其他操作。更新的角速度可以在预测的时候用上

 Comments