惯性测量单元预积分原理与实现

这篇具有很好参考价值的文章主要介绍了惯性测量单元预积分原理与实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

IMU器件

IMU的全称是惯性测量单元,包括一个三轴的加速度计以及一个三轴的陀螺仪,分别测量出物体的加速度和角速度信息,不受周围环境结构、光照等外界因素影响。通常IMU的输出频率在100-1000hz之间,远高于相机或者激光雷达的输出频率,一方面可以提高整体系统的输出频率,另一方面,可以在视觉或者激光雷达短期失效的时候提供一段时间的位姿推算。

在大多数的LIO或者VIO中,关于IMU的输出的建模方式如下

角速度部分:
惯性测量单元预积分原理与实现
其中
惯性测量单元预积分原理与实现
为t时刻测量值
惯性测量单元预积分原理与实现
为t时刻真实角速度值
惯性测量单元预积分原理与实现
为t时刻零偏

惯性测量单元预积分原理与实现
为t时刻噪声

加速度部分
惯性测量单元预积分原理与实现
除了零偏和噪声外,也有重力加速度影响
at和g是在世界坐标系下的值

通过上面的模型可知,输出的加速度计和陀螺仪的数据受零偏及高斯白噪声的影响,因此紧耦合的LIO或者VIO都会实时估计IMU的零偏,以实现IMU数据的最大利用率。

实际的IMU数据并没有这么理想,除了零偏和高斯白噪声,还有可能受到刻度误差尺度因子轴向偏差等影响。如果把这些因素通过建模方式考虑进来,就显得过于复杂。
通常的做法是在IMU选型的时候就最大化避免上述误差的影响。也就是说,我们选择IMU型号的时候除了关注价格(当然价格很多时候也是非常重要甚至是决定性的因素),还要关心其出厂标定情况,是否做过温度补偿之类的。

IMU状态传递方程

IMU如何进行位姿估计

IMU可以获得当前时刻的角速度和加速度值,通过该值可以对系统状态(位置,速度,姿态)进行推算

连续时间两个关键帧bk和bk+1 之间的状态传递公式如下:

平移:

  • 公式原理
    惯性测量单元预积分原理与实现
  • 用积分的形式表示如下
    惯性测量单元预积分原理与实现

速度:

  • 公式原理
    惯性测量单元预积分原理与实现
  • 用积分的形式表示如下
    惯性测量单元预积分原理与实现

旋转

  • 用积分的形式表示如下
    对四元数求导,相当于积分里面的内容
    惯性测量单元预积分原理与实现
    其中
    惯性测量单元预积分原理与实现

在实际用用的时候,计算机接受的的传感器数据都是离散形式的,所以需要对上面进行离散化

每当收到一帧新的imu数据后,系统状态变化为:

将上面的k和k+1,换成了i和i+1,其中i是IMU的第i帧,k是IMU的第k关键帧。k和k+1帧直接有n帧imu数据。i和i+1帧是没有的

位置离散化为:
惯性测量单元预积分原理与实现
速度离散化为:
惯性测量单元预积分原理与实现
旋转离散化为:
惯性测量单元预积分原理与实现
四元数的模长为1,矩阵里的实部为1,后面的虚部时间非常短,近似为0

IMU预积分

IMU预积分的作用

当k时刻的状态发生变化时,则通过imu积分得到的k+1时刻的状态也会发生相应的变化,而在基于滑窗的后端优化或者因子图的优化中,对一些状态量进行调整是必然发生的,此时,如果每次状态发生调整时,都对imu的积分过程重新执行一遍(但是IMU数据并未发生变换),则实时性必然无法得到保证,因此,预积分理论就是为解决这个问题而提出的(避免重复积分)。
惯性测量单元预积分原理与实现

IMU预积分的核心思想

其核心思想就是对IMU积分的结果和上一时刻系统的状态无关,这样,当系统状态在优化过程中发生调整的时候,就不需要对下一时刻的系统状态重新积分。

IMU预积分方法

根据上面连续时间IMU积分的公式,等号两边乘上世界坐标系到机体坐标系的变换
惯性测量单元预积分原理与实现

惯性测量单元预积分原理与实现
其中
惯性测量单元预积分原理与实现
上面的三个变量即预积分量,这三个预积分量都和k时刻或k+1时刻状态无关,因此当k时刻状态发生变化时,不需要对IMU数据重新进行积分

仅和k时刻的机体坐标系有关

关于零偏的建模

通常来说,IMU的零偏会随着时间的变化而偏移,因此为了系统的准确性,零偏也是系统的优化变量之一,预积分量虽然和两帧的具体位姿和速度等状态无关,但是和零偏相关,因此当零偏作为优化变量被优化后,预积分量也会发生相应的变化,那么如果重新积分,预积分的作用就没有了。

为了避免零偏的变化导致预积分量重新积分,考虑到通常零偏的变化在短时间(100ms)非常小,因此,我们可以使用一阶泰勒展开来进行近似,具体为:
惯性测量单元预积分原理与实现
这里预积分量关于零偏的雅可比矩阵会在预积分计算的时候一并计算,因此,当零偏被优化调整后,只需要根据事先计算号的雅可比矩阵对预积分量进行更新即可。

离散时间的预积分更新

同样,实际系统是离散得IMU数据,目的是得到两个关键帧(视觉或者激光雷达)之间的预积分结果,而获得的IMU数据是离散的,因此,常见的做法就是每收到一帧新的IMU数据更新一次预积分量,同样,这是一个求和而非连续积分的过程

当收到新的IMU数据后,预积分量更新公式如下:

惯性测量单元预积分原理与实现

IMU预积分实现

  • IMU预积分根据上面的理论推导,便可以手动进行代码的实现。可以参考 orb3 或者VINS-MONO中的代码
  • IMU预积分在LIO-SAM中是通过GT-SAM库实现的。

重点部分文章来源地址https://www.toymoban.com/news/detail-421905.html

    Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
                                          const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
    {//get IMU residuals   Qi transform coordinate from i to w; Vi volosity at time i in w coordinate
        Eigen::Matrix<double, 15, 1> residuals;
//    O_P = 0,O_R = 3, O_V = 6, O_BA = 9, O_BG = 12
        Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);//0,9
        Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);//0,12

        Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);//3,12

        Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);//6,9
        Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);//6,12

        Eigen::Vector3d dba = Bai - linearized_ba;
        Eigen::Vector3d dbg = Bgi - linearized_bg;
        //cuihuakun p6-(6)  // IMU预积分的结果,消除掉acc bias和gyro bias的影响, 对应IMU model中的\hat{\alpha},\hat{\beta},\hat{\gamma}
        Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
        Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
        Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
        //ppt ch3-p69 // IMU项residual计算,输入参数是状态的估计值, 上面correct_delta_*是预积分值, 二者求'diff'得到residual.
        residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
        residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
        residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
        residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
        residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
        return residuals;
    }

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{   //获取当前时间
    double t = imu_msg->header.stamp.toSec();
    //首帧判断
    if (init_imu)
    {latest_time = t;init_imu = 0;return;}
    //获取dt并传递时间
    double dt = t - latest_time;
    latest_time = t;
    //获取当前时刻的IMU采样数据
    double dx = imu_msg->linear_acceleration.x;
    double dy = imu_msg->linear_acceleration.y;
    double dz = imu_msg->linear_acceleration.z;
    Eigen::Vector3d linear_acceleration{dx, dy, dz};
    double rx = imu_msg->angular_velocity.x;
    double ry = imu_msg->angular_velocity.y;
    double rz = imu_msg->angular_velocity.z;
    Eigen::Vector3d angular_velocity{rx, ry, rz};
    //注意,以下数据都是世界坐标系下的
    Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
    tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
    Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.g;
    Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
    tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
    tmp_V = tmp_V + dt * un_acc;
    //信息传递
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

到了这里,关于惯性测量单元预积分原理与实现的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • MCU自动测量单元:自动化数据采集的未来

    随着科技的飞速发展,自动化技术在各个领域中的应用日益广泛。其中,MCU(微控制器)自动测量单元以其高效、精准的特性,成为自动化数据采集领域的佼佼者,引领着未来数据采集技术的革新。本文将深入探讨MCU自动测量单元的原理、优势以及在自动化数据采集领域的应用

    2024年04月29日
    浏览(40)
  • 积分球原理及积分球类型介绍

    积分球原理:由于球体内整涂有白色漫反射材料的空腔球体,球壁上开有采样口,当待测样品光源进入积分球的光经过内壁涂层多次反射,在内壁上形成均匀照度,可用于测试光源的光通量,色温,光效等参数。 功能:测试光源的光通量,色温,光效等参数,均匀光源。 检测对

    2024年02月11日
    浏览(29)
  • 理解机制,再探单元工厂的实现原理

    最近有点忙,好久没更新文章了,今天继续再研究一下单元工厂的实现机制。为什么我们要这么重视这一块的内容呢?因为用计算机的目的是为了处理大量数据,如果数据量不大,大多情况下用纸就好了,专门用个计算设备的便捷性也就体现不出来。而大量数据的呈现方式的

    2024年02月11日
    浏览(27)
  • AD原理图器件镜像翻转

    在原理图选中想要镜像或者翻转的器件,使器件处于可拖动的状态。 鼠标左键选中器件拖动,英文输入法状态下 按 Y 键,可以实现器件上下翻转。 同理,按 X 键可以实现左右镜像翻转。 可以的话别忘记个赞👍

    2024年02月12日
    浏览(32)
  • IMU惯性里程计解算(附代码实现)

    一、系统概述 IMU是机器人常用的传感器之一,IMU对机器人的定位功能实现非常重要,其优点在于是内源传感器对外部环境变化不明显,输出频率高,缺点在于存在累积误差。本文主要记录一下在机器人定位中对IMU的使用和对惯性导航里程计的理解和实现。 本文代码主要依赖

    2023年04月15日
    浏览(43)
  • AD21原理图----图纸修改、栅格设置、元器件(添加库、元器件属性)

    总图1 总图2 图纸修改  栅格设置  元器件 添加库 元器件属性 尺寸、背景色、边框(总图1)      颜色、单位、捕获半径、捕获距离(总图1)   第一步   选择    添加库 (总图2)  第二步 库安装

    2024年02月15日
    浏览(59)
  • (转载)基本粒子群算法及惯性权重分析(matlab实现)

            粒子群算法(particle swarm optimization,PSO)是计算智能领域,除了蚁群算法、鱼群算法之外的一种群体智能的优化算法。该算法最早由Kennedy和Eberhart在1995年提出的。PSO算法源于对鸟类捕食行为的研究,鸟类捕食时,找到食物最简单有效的策略就是搜寻当前距离食物最近

    2024年02月07日
    浏览(38)
  • AD(Altium Designer )一体多个器件部件原理图创建

    有时候一个元器件含有多个相同功能的电器结构组成,如多运放,多门,多三极管等等封装到一个电子元件上,而为了画原理图时简单明了,所以想把它拆分成A B C等分部件,例如下图1.1多门级电路74HC00的常规原理图封装想画成下图1.2的分布式直观图封装。 图1.1:常规封装

    2023年04月10日
    浏览(45)
  • 半导体器件基础08:MOS管结构和原理(1)

    如果要用一个词来形容泡利,那就是聪明,聪明绝顶。不过他的教父更牛,是哲学大神马赫。泡利1918年刚刚中学毕业,就带着父亲写的推荐信来找索末菲;他要求不上本科了,直接读研究生,索末菲吓了一跳,再一问,原来泡利在高中就已经把大学课程给自学了(想想我自己

    2024年02月06日
    浏览(48)
  • 【嘉立创EDA】器件原理图引脚标识颜色更改

    1️⃣ 在嘉立创EDA专业版中,很多时候需要自行建立元器件并绘制对应的器件原理图。对于一些引脚极其丰富的元件,引脚颜色千篇一律(均为黑色)时,难以辨认,增加了连线时的枯燥感。本文主要讲述如何修改器件原理图引脚标识颜色,本文将此过程记录,以供有需要的

    2024年02月11日
    浏览(170)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包