专栏首页科学计算VIO中的IMU积分

VIO中的IMU积分

VIO中的IMU积分

一、数值积分原理

 对于一个给定的微分方程

\dot y(t)=f(t, y(t))

,假设已经知道了初值

y(t_0)

,则其

\Delta t

时刻后的数值积分为:

y(t+\Delta t)=y(t_0)+\int_{t_0}^{t_0+\Delta t} \dot y(t)dt

 实际当中我们通常无法获得

\dot y(t)

的表达式,只能对其进行离散采样,然后使用离散积分逼近真实的连续积分。通常近似的思路为:认为

t

t+\Delta t

的微分

\dot y(t)

用恒定常数来表示,则积分表达式可改写为:

y_{n+1}=y_n+\Delta t \cdot k

 从上述公式可以看出,减少积分误差的主要方式有两种:

 1. 减少采样时间间隔

\Delta t

,时间间隔越小,则离散积分就越接近于连续积分,采样时间间隔取决于sensor的采样率,所以采样率越高积分精度越高;

 2. 计算精确的恒定常数

k

,针对

k

的通常有三种积分方法:欧拉积分、中值积分和4阶龙格-库塔积分。

二、积分方法

 2.1 欧拉积分

 欧拉积分假设在倒数区间内的斜率是恒定的,其取

t_n

时刻的斜率作为

t_n

t_{n+1}

时间段的斜率,即:

k=\dot y(t_n)=f(t_n,y_n)

 从公式可以看出,欧拉积分是最简单的一种积分方式,其逼近误差较大,但计算量很小。

 2.2 中值积分

 中值积分是在欧拉积分的基础上进行改善。先使用欧拉积分逼近时间间隔

\Delta t

的中点,即

t_n+ \frac{1}{2}\Delta t

的斜率,然后使用中点斜率作为整个时间段内的近似斜率。

 首先使用欧拉积分来近似时间段内的中点斜率:

k_1=f(t_n,y_n) \\ y(t_n+\frac{1}{2}\Delta t)=y_n+\frac{1}{2}\Delta t \cdot k_1

 然后我们使用得到的时间段中点斜率进一步近似整个时间段的斜率:

k = \dot y(t_n + \frac{1}{2}\Delta t)=f(t_n+\frac{1}{2}\Delta t,y(t_n+\frac{1}{2}\Delta t))=f(t_n+\frac{1}{2}\Delta t, y_n+\frac{1}{2}\Delta t k_1)

 显而易见,中值积分比欧拉积分更合理一些。

 2.2 4阶龙格-库塔积分

 我们直接给出4阶龙格-库塔积分的公式:

k_1=f(t_n,f_n)
k_2=f(t_n+\frac{1}{2}\Delta t,y_n+\frac{1}{2}\Delta t \cdot k_1)
k_3=f(t_n+\frac{1}{2}\Delta t,y_n+\frac{1}{2}\Delta t \cdot k_2)
k_4=f(t_n+\Delta t, y_n+\Delta t \cdot k_3)
k=\frac{1}{6}(k_1+2k_2+2k_3+k_4)
k_1

是起点的斜率,

k_2

是中点的斜率,

k_3

是中点的中点斜率,

k_4

是终点的斜率。实际上4阶龙格-库塔积分就是斜率的加权结果,

k_2

k_3

的斜率权重为2,其余为1。显而易见,这种方法的近似精度是最高的。其中

k_1

就是欧拉积分当中的斜率,

k_2

就是中值积分当中的斜率。

 4阶龙格-库塔积分的代码如下:

//Indirect Kalaman Filter for 3D Attitude Estimation的第11页
// Note we use Vector4f to represent quaternion instead of quaternion
// because it is better do not normalize q during integration
inline Eigen::Vector4f XpQuaternionDerivative(
    const Eigen::Vector4f &q,
    const Eigen::Vector3f &omega) {

  return -0.5 * XpComposeOmega(omega) * q;
}

//k时刻角速度,k+1时刻角速度,k时刻旋转,dalta_t,k+1时刻旋转
void IntegrateQuaternion(
    const Eigen::Vector3f &omega_begin, const Eigen::Vector3f &omega_end,
    const XpQuaternion &q_begin, const float dt,
    XpQuaternion *q_end,
    XpQuaternion *q_mid,
    XpQuaternion *q_fourth,
    XpQuaternion *q_eighth) {
  CHECK_NOTNULL(q_end);
  XpQuaternion q_0 = q_begin;

  // divide dt time interval into num_segment segments
  // TODO(mingyu): Reduce to 2 or 4 segments as 8 segments may overkill
  const int num_segment = 8;//步数

  // the time duration in each segment
  const float inv_num_segment = 1.0 / num_segment;//步长
  const float dt_seg = dt * inv_num_segment;

  Eigen::Vector3f delta_omega = omega_end - omega_begin;
  for (int i = 0; i < num_segment; ++i) {
    // integrate in the region: [i/num_segment, (i+1)/num_segment]
   //这里利用陀螺仪的测量RK4积分算出前后帧一个imu测量出的旋转,JPL格式四元数
   // 那么XpQuaternionDerivative(q_0, omega_tmp)应该是 - 0.5 * omega(w) × qcur_pre(t)
    Eigen::Vector3f omega_tmp = omega_begin + (i * inv_num_segment) * delta_omega;//当前步长内的imu角速度
    Eigen::Vector4f k1 = XpQuaternionDerivative(q_0, omega_tmp);
    omega_tmp = omega_begin + 0.5 * (2 * i + 1) * inv_num_segment * delta_omega;
    Eigen::Vector4f k2 = XpQuaternionDerivative(q_0 + 0.5 * k1 * dt_seg, omega_tmp);
    Eigen::Vector4f k3 = XpQuaternionDerivative(q_0 + 0.5 * k2 * dt_seg, omega_tmp);
    omega_tmp = omega_begin + (i + 1) * inv_num_segment * delta_omega;
    Eigen::Vector4f k4 = XpQuaternionDerivative(q_0 + k3 * dt_seg, omega_tmp);
    (*q_end) = q_0 + (dt_seg / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);

    // store the start point the next region of integration
    q_0 = (*q_end);

    // store the intermediate attitude as output
    if (i == 0 && q_eighth) {
      (*q_eighth) = (*q_end);
    } else if (i == 1 && q_fourth) {
      (*q_fourth) = (*q_end);
    } else if (i == 3 && q_mid) {
      (*q_mid) = (*q_end);
    }
  }
}

本文分享自微信公众号 - 科学计算technomania(Quant_Times),作者:大亮

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2021-03-14

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • Kimera实时重建的语义SLAM系统

    Kimera是C++实现的一个具有实时度量的语义SLAM系统,使用的传感器有相机与IMU惯导数据来构建环境语义标注的3D网格,Kimera支持ROS运行在CPU...

    点云PCL博主
  • 2019年视觉里程计VIO新进展

    1 2019年,Visual-Inertial Mapping with Non-Linear Factor Recovery.

    小白学视觉
  • 问答 | 怎么评价基于深度学习的deepvo,VINet?

    1.DeepVO: A Deep Learning approach for Monocular Visual Odometry;

    AI研习社
  • Paper Reading | VINet 深度神经网络架构

    2017年3月10日,Momenta老司机带你读Paper,第三趟车已出发!你,跟得上吗?

    用户1908973
  • 用于机器人导航辅助的6自由度姿态估计的平面辅助视觉惯性里程计

    Plane-Aided Visual-Inertial Odometry for 6-DOF Pose Estimation of a Robotic Navi...

    计算机视觉
  • 立体视觉+惯导+激光雷达SLAM系统

    标题:Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping

    点云PCL博主
  • ICRA 2021| 聚焦距离的Camera-IMU-UWB融合定位方法

    Range-Focused Fusion of Camera-IMU-UWB for Accurate and Drift-Reduced Localizati...

    3D视觉工坊
  • slam标定(三) vio系统

    vio由双目相机及imu构成,两者分别对应camera坐标系与body坐标系,我们需要对两个坐标系之间的位姿矩阵和图像与imu数据时间偏移进行标定。

    猫叔Rex
  • VSLAM:预积分公式推导(一)

     传统的递推算法是根据上一时刻的IMU状态量,利用当前时刻测量得到的加速度与角速度,进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中,各个状态量都...

    猫叔Rex
  • R3LIVE:一个实时鲁棒、带有RGB颜色信息的激光雷达-惯性-视觉紧耦合系统(香港大学)

    R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled ...

    计算机视觉
  • CodeVIO:基于可学习优化密集深度的视觉惯性里程计(ICRA2021)

    (Xingxing Zuo, Nate Merrill, Wei Li, Yong Liu, Marc Pollefeys, and GuoquanHuang....

    3D视觉工坊
  • 计算机视觉方向简介 | 视觉惯性里程计(VIO)

    Visual-Inertial Odometry(VIO)即视觉惯性里程计,有时也叫视觉惯性系统(VINS,visual-inertial system),是融...

    3D视觉工坊
  • CodeVIO:紧耦合神经网络与视觉惯导里程计的稠密深度重建(ICRA2021 Best Paper Finalist)

    大家好!在这篇文章里我将为大家简要介绍我们在ICRA2021上发表的论文"CodeVIO: Visual-Inertial Odometry with Lear...

    3D视觉工坊
  • 双目视觉惯性里程计的在线初始化与自标定算法

    标题:An Online Initialization and Self-Calibration Method for Stereo Visual-Inerti...

    3D视觉工坊
  • 基于关键帧的RGB-D视觉惯性里程计

    论文信息:Chu C , Yang S . Keyframe-Based RGB-D Visual-Inertial Odometry and Camera E...

    3D视觉工坊
  • 基于关键帧的RGB-D视觉惯性里程计

    论文信息:Chu C , Yang S . Keyframe-Based RGB-D Visual-Inertial Odometry and Camera E...

    计算机视觉
  • VSLAM:IMU预积分公式推导

     传统的递推算法是根据上一时刻的IMU状态量,利用当前时刻测量得到的加速度与角速度,进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中,各个状态量都...

    猫叔Rex
  • vSLAM开发指南:从技术框架、开源算法到硬件选型!

    出品 | 智东西公开课 讲师 | 小觅智能 CTO 杨瑞翾 编辑 | 王鑫

    小白学视觉
  • Ultimate SLAM?利用事件相机解锁高速运动、高动态范围场景

    Rosinol Vidal, A., Rebecq, H., Horstschaefer, T., Scaramuzza, D., Ultimate SLAM?...

    3D视觉工坊

扫码关注云+社区

领取腾讯云代金券