专栏首页科学计算VSLAM:预积分公式推导(一)

VSLAM:预积分公式推导(一)

VSLAM:预积分公式推导(一)

一、基本预备知识

1.1 预备知识:

 传统的递推算法是根据上一时刻的IMU状态量,利用当前时刻测量得到的加速度与角速度,进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中,各个状态量都是估计值,并且会不断调整,每次调整都会重新进行积分,传递IMU测量值。预积分的目的是将相对测量量与据对位姿解耦合,避免优化时重复进行积分。四元数的表示方法有两种:一种是Hamilton(右手系)表示,另一种是JPL(左手系)表示。读者对公式推导时一定注意。

1.2 IMU模型

 我们将以前文章中的IMU公式拿过来:

\hat{a_t}=a_t+b_a+n_a+R_w^tg^w \\ \hat{w_t}=w_t+b_w+n_w
t

表示在body坐标系下(IMU坐标系),随机游走及噪声不再进行解释。我们将图像帧记作

k

k+1

,body坐标系下记作

b_k

b_{k+1}

,我们将位置,速度和旋转在时间

t_k

t_{k+1}

内进行积分,其世界坐标系下的公式可以写为:

p_{b_{k+1}}^w=p_{b_k}^w+v_{b_k}^w\Delta t_k+\iint_t(R_t^w(\hat{a_t}-b_a-n_a)-g^w)dt^2 \\ v_{b_{k+1}}^w=v_{b_k}^w+\int_t(R_t^w(\hat{a_t}-b_a-n_a)-g^w)dt \\ q_{b_{k+1}}^w=q_{b_k}^w \otimes \int_t \frac{1}{2}q_t^{b_k} \otimes \begin{bmatrix} (\hat w_t -b_w -n_w) \\ 0 \end{bmatrix} dt \\ =q_{b_k}^w \otimes \int \frac{1}{2} \Omega(\hat w_t -b_w -n_w)q_t^{b_k}dt

 注意这里的四元数虚部在前,实部在后,其中:

\Omega(w)=\begin{bmatrix} -[w]_\times & w \\ -w^T & 0\end{bmatrix}, [w]_\times=\begin{bmatrix} 0 & -w_z & w_y \\ w_z & 0 & -w_x \\ -w_y & w_x & 0\end{bmatrix}

 旋转矩阵的推导:我们设

q(t)

是单位四元数,

w

是由四元数确定的角速度,则单位四元数的导数可以用左乘及右乘来表示:

\dot q= \frac{1}{2}\begin{bmatrix} w \\ 0\end{bmatrix} \otimes q=\frac{1}{2}q \otimes \begin{bmatrix} w \\ 0 \end{bmatrix}

 所以:

q_{b_{k+1}}^w=q_{b_k}^w \otimes q_{b_{k+1}}^{b_k}=q_{b_k}^w \otimes \int_t \dot q_t^{b_k}dt=q_{b_k}^w \otimes \int_t \frac{1}{2}q_t^{b_k} \otimes \begin{bmatrix} w_t^{b_k} \\ 0 \end{bmatrix} dt \\ =q_{b_k}^w \otimes \int_t \frac{1}{2}q_t^{b_k} \otimes \begin{bmatrix} (\hat w_t -b_w -n_w) \\ 0 \end{bmatrix}dt

1.3 离散表示

 我们以中值积分给出离散表示L:

p_{b_{k+1}}^w=p_{b_k}^w+v_{b_k}^w\Delta t_k+\frac{1}{2} \overline{\hat a_t}\delta t^2 \\ v_{b_{k+1}}^w=v_{b_k}^w+\overline{\hat a_t}\delta t \\ q_{b_{k+1}}^w=q_{b_k}^w \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\overline{\hat w_t}\delta t\end{bmatrix}\\ \overline{\hat a_t}=\frac{1}{2}[q_i(\hat a_t-b_a)-g^w+q_{i+1}(\hat a_{t+1}-b_a)-g^w] \\ \overline{\hat w_t}=\frac{1}{2}(\hat w_t + \hat w_{t+1}-b_w)

二、IMU预积分

2.1 连续形式

 基本思想就是将参考坐标系从

w

转到第

k

帧的body坐标系下,相当于两边同时乘

R_w^{b_k}

,我们直接用论文中的公式来表示:

 其中:

 上述可以理解为

b_{k+1}

b_k

的相对运动量,其中

b_k

状态的改变并不会对其产生影响,可以将其作为非线性优化变量,避免重复计算。实际到这里,只要求解出积分,我们就完成了预积分的计算,我们的目标也就是在此。实际当中随机游走也是发生改变的,所以我们将上述变量再次进行一阶近似,我们再次使用论文中的公式进行表示:

2.2 离散形式

 我们同样以中值积分的形式给出:

 其中位置、速度初始值为0,旋转为单位四元数,噪声视为0。

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

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

原始发表时间:2021-04-19

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

我来说两句

0 条评论
登录 后参与评论

相关文章

  • VSLAM:IMU预积分公式推导

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

    猫叔Rex
  • 高仙完成A轮千万级美元融资,自主移动导航系统独角兽浮现

    新智元
  • vSLAM开发指南:从技术框架、开源算法到硬件选型!

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

    小白学视觉
  • 专栏 | 对比激光SLAM与视觉SLAM:谁会成为未来主流趋势?

    机器之心
  • 机器视觉公司速感科技完成千万美元B轮融资,或为下一个视觉行业独角兽

    【新智元导读】专注于计算机视觉的AI领跑企业速感科技正式宣布完成千万美元B轮融资。至此,速感科技成立至今3年已累计完成4轮次12家国内外一线投资机构超过1亿元人...

    新智元
  • 【SLAM】开源 | Good Graph提高了VSLAM估计的准确性和鲁棒性,及基于VSLAM的闭环导航系统的轨迹跟踪性能!

    论文地址: http://arxiv.org/pdf/2008.10123v1.pdf

    CNNer
  • 视觉/视觉惯性SLAM最新综述:领域进展、方法分类与实验对比

    Visual and Visual-Inertial SLAM: State of the Art, Classification,and Experiment...

    3D视觉工坊
  • 视觉/视觉惯性SLAM最新综述:领域进展、方法分类与实验对比

    Visual and Visual-Inertial SLAM: State of the Art, Classification,and Experiment...

    计算机视觉
  • CES 2019|VR/AR/MR头显、眼镜、一体机、配件、模组、技术与解决方案,超全汇总!

    CES 2019,正在拉斯维加斯火热进行中。前两日,小编先对本届CES所涵盖的VR/AR、无人驾驶、芯片等多个领域的最新动态,进行了“终极前瞻”;后又对三星、L...

    VRPinea
  • 速感科技陈震:以机器视觉为核心,让低成本、高性价比成为机器人行业关键词 | 镁客请讲

    镁客网
  • vSLAM技术综述

    SLAM是“Simultaneous Localization And Mapping”的缩写,可译为同步定位与建图。概率 SLAM 问题 (the proba...

    SIGAI学习与实践平台
  • vSLAM技术综述

    SLAM是“Simultaneous Localization And Mapping”的缩写,可译为同步定位与建图。概率 SLAM 问题 (the proba...

    小白学视觉
  • 谷歌与iRobot开展合作,利用机器人吸尘器收集的空间数据改进智能家居设备

    谷歌和iRobot宣布建立战略合作伙伴关系,利用iRobot机器人吸尘器收集的空间数据改进智能家居设备。

    AiTechYun
  • 储备一些微分与积分公式

    看、未来
  • 软硬件一体化渐成趋势,视觉产业市场的“新蓝海”在哪里?

    智能时代,“计算机视觉+普通相机”已经不能满足需求,作为“硬件主体”的相机是时候升级了。

    镁客网
  • 3D视觉从入门到精通系统学习教程

    随着我们队伍的不断壮大,目前星球嘉宾及合伙人,主要包括多个大厂的计算机视觉算法工程师、深度学习算法工程师、结构光三维重建算法工程师、资深VSLAM算法工程师、3...

    BBuf
  • 书单|24本书带你入门配送机器人

    2017年10月,在获得40亿美元融资后,美团点评CEO王兴表示会将业务重心放在无人配送和人工智能上。

    美团无人配送
  • 一个狠招|如何高效学习3D视觉

    有的读者可能对于计算机视觉中2D和3D视觉的区别仍然较为模糊,此处根据某篇论文中的解释,介绍如下:

    OpenCV学堂
  • 一分钟详解initUndistortRectifyMap函数bug修复方法

    在上一篇文章(OpenCV中initUndistortRectifyMap函数存在bug原因探究)的结尾处,留给大家思考三个问题:

    3D视觉工坊

扫码关注云+社区

领取腾讯云代金券