IMU Preintegration

0. 为什么需要 IMU 预积分

要解释 IMU 预积分的话,首先需要先了解正常情况下 IMU 积分与所谓的 IMU 预积分的区别。这里直接给出 IMU 积分过程(其中涉及到的 IMU 测量模型与运动学模型先不展开)。IMU 积分是指根据 IMU 传感器获取得到的加速度 at 和角速度 wt ,与给定 IMU bias bat,bgt ,从第 i 时刻的 pbiw,vbiw,qbiw 基础上不断积分得到第 j 时刻的 pbjw,vbjw,qbjw 。如下图所示,

imu integration

IMU 积分公式如下: (0.1)pbjw=pbiw+viwΔtk+t[i,j](qbtw(atbatna)gw)dt2vbjw=vbiw+t[i,j](qbtw(atbatna)gw)dtqbjw=t[i,j]qbtw[012(wtbgtng)]dt 观察积分公式我们发现 pbjw,vbjw,qbjw 中的积分项依赖于初值 pbiw,vbiw,qbiw 。 如果时刻 bi 的状态发生改变,时刻 bj 的状态必须重新计算。在基于优化的 Visual Inertial Odometry 算法中,状态每次迭代都会改变,如果每次迭代都重新积分,那么计算量会非常大。而预积分的目的就是想将这部分预先计算,且不受每次迭代更新重新计算,由此,我们需要想方法去掉基于 World 坐标系下的依赖项 pbiw,vbiw,qbiw

IMU 预积分技术最早由 T Lupton 于 12 年提出1,以欧拉角为基础。C Forster 于 15 年234将其进一步拓展到李代数上,形成了一套优雅的理论体系。这套 IMU 预积分理论在开源因子图优化库 GTSAM 中进行实现,且被广泛应用于图优化框架的 VIO 中。其中包括 SVO2、ORB-SLAM3、VINS-Mono/Fusion、ICE-BA 等。由于这套理论的概念是在李群流行上的推导的,因此其中涉及到许多李群李代方面的知识,这部分知识可参考567(因此下文中使用到的一些公式、定义将直接给出,不再加以说明)。这套理论的后续发展有,Guoquan Huang 等8910,在欧拉积分/二阶龙格库塔积分(Runge-Kutta 即,中值积分)基础上引入群空间闭式 (closed-form) 积分。

1. IMU 器件测量模型与运动学模型

IMU 通常包括一个三轴加速度计和一个三轴陀螺仪,能够测量传感器相对于惯性系的旋转速率和加速度。测量模型定义如下: (1.1)bw~(t)=bw(t)+bg(t)+ηg(t)ba~(t)=bwRT(wa(t)wg)+ba(t)+ηa(t) 其中 bw~(t),ba~(t) 分别为陀螺仪与加速度计的测量值,b 为随时间缓慢变化的 bias,η 是白噪声。该模型利用了 Static World Assumption,由于 SLAM 的运行场景通常比较小(由此重力变化不大),且使用的 IMU 器件一般为 MEMS 器件(精度较低,陀螺仪静置时无法敏感到地球自转)。因此在上式 (1.1) 测量模型中忽略了地球自转、重力矢量变化等影响因素,且假设运行区域水平面是一个平面。在此假设下的 VINS 中,世界坐标系 w 被认为是一个惯性系,由此,下面的运动模型都是基于该假设进行推导。因此有运动模型的微分方程形式如下: (1.2)bwR˙=bwR bw,wv˙=wa,wp˙=wv 根据上式,t+Δt 时刻的状态可以通过积分得: (1.3)bwR(t+Δt)=bwR(t) Exp(tt+Δtbw(τ)dτ)wv(t+Δt)=wv(t)+tt+Δtwa(τ)dτwp(t+Δt)=wp(t)+tt+Δtwv(τ)dτ+tt+Δtwa(τ)dτ 当假设在时间 [t,t+Δt]wabw 是恒定的,使用欧拉积分,可以得到上式的离散形式如下: (1.4)bwR(t+Δt)=bwR(t) Exp(bw(t)Δt)wv(t+Δt)=wv(t)+wa(t)Δtwp(t+Δt)=wp(t)+wv(t)Δt+12wa(t)Δt2 为了公式书写方便与符号简明,下面省略一些上下标记号,约定如下 RbwR,wbw,aba,vwv,pwp,gwg 。将测量模型(式 (1.1) )带入离散运动模式(式 (1.4) )易得: (1.5)R(t+Δt)=R(t) Exp((w~(t)bg(t)ηgd(t))Δt)v(t+Δt)=v(t)+R(t)(a~(t)ba(t)ηad(t))Δt+gΔtp(t+Δt)=p(t)+v(t)Δt+12gΔt2+12R(t)(a~(t)ba(t)ηad(t))Δt2 上述公式中,我们注意到噪声采用的 ηgd,ηad ,其中下标 d 表示为 discrete ,即使用了离散时间噪声,它们与连续噪声项 ηg,ηa 是不同的,离散噪声与连续噪声的协方差有如下关系(Nikolas Trawny and Stergios I. Roumeliotis. Indirect Kalman Filter for 3D Attitude Estimation 2.1 有详细推导过程): (1.6)Cov(ηgd(t))=1ΔtCov(ηg(t))Cov(ηad(t))=1ΔtCov(ηa(t))

2. IMU 预积分

下面我们推导如何将两个关键帧 k=ik=j 之间的所有 IMU 测量数据进行积分,由 k=i 时刻的 Ri,vi,pi 直接更新得到 k=j 时刻的 Rj,vj,pj 。过程如下图:

Different rates for IMU and camera

假设 IMU 的采样频率不变,即时间间隔 Δt 恒定,每个离散时刻由 k=0,1,2, 表示,由此将式 (1.5) 简化符号如下:

(2.1)Rk+1=Rk Exp((w~kbgkηgdk)Δt)vk+1=vk+Rk(a~kbakηadk)Δt+gΔtpk+1=pk+vkΔt+12gΔt2+12Rk(a~kbakηadk)Δt2

在关键帧 k=ik=j 之间迭代所有 Δt 时间间隔的 IMU 积分有:

(2.2)Rj=Rik=ij1Exp((w~kbgkηgdk)Δt)vj=vi+gΔtij+k=ij1Rk(a~kbakηadk)Δtpj=pi+k=ij1vkΔt+ji2gΔt2+12k=ij1Rk(a~kbakηadk)Δt2=pi+k=ij1(vkΔt+12gΔt2+12Rk(a~kbakηadk)Δt2)其中,  Δtij=k=ij1Δt=(ji)Δt

为了避免每次更新初始的 Rivipi 都需要从头积分求解 Rjvjpj 。由此引出相对运动的预积分项(理想值)以避免依赖于 Rivipi

(2.3)ΔRijRiTRj=k=ij1Exp((w~kbgkηgdk)Δt)ΔvijRiT(vjvigΔtij)=k=ij1ΔRik(a~kbakηadk)ΔtΔpijRiT(pjpiviΔtij12gΔtij2)=k=ij1[ΔvikΔt+12ΔRik(a~kbakηadk)Δt2]其中, ΔRikRiTRk,  ΔvikRiT(vkvigΔtik)

需要注意的是虽然所称为 ‘‘delta’’ 量,但与 ΔRij 相比, ΔvijΔpij 都不对应于速度和位置的真实物理变化,而是以一种使式 (2.3) 的右侧独立于时刻 i 的状态量和重力效应的方式定义的。使得我们能从两个关键帧之间的惯性测量直接计算式 (2.3) 的右侧。上述三个预积分公式中,前两个式子的推导是显而易见的。而对于 Δpij 的推导如下(为了节省篇幅,令 fk=a~kbakηadk),

(2.3a)pjpiviΔtij12gΔtij2=k=ij1(vkΔt+12gΔt2+12RkfkΔt2)k=ij1viΔt(ji)22gΔt2=k=ij1(vkvi)Δt+[ji2(ji)22]利用ji2(ji)22=(ji)[j(i+1)]2=k=ij1(ki)等差数列求和gΔt2+12k=ij1RkfkΔt2=k=ij1(vkvi)Δt+k=ij1(ki)gΔt2+12k=ij1RkfkΔt2=k=ij1{[vkvi(ki)gΔt]Δt+12RkfkΔt2}=k=ij1[(vkvigΔtik)Δt+12RkfkΔt2]=k=ij1(RiΔvikΔt+12RkfkΔt2)式左右两边同时左乘RiTRiT(pjpiviΔtij12gΔtij2)=k=ij1(RiTRiΔvikΔt+12RiTRkfkΔt2)=k=ij1(ΔvikΔt+12RikfkΔt2)=k=ij1(ΔvikΔt+12Rik(a~kbakηadk)Δt2)

我们注意到,即使采用式 (2.3) 进行预积分计算,但是它依旧依赖于 bias 的估计。为了决绝这个问题,预积分算法将之分解与两个步骤:1. 首先假设两个关键帧之间的 bias 是一致的,即 bgi=bgi+1==bgj1,bai=bai+1==baj1 先进行预积分 IMU 测量值([2.1 章节](#2.1 预积分 IMU 测量值));2. 当 bias 更新时,采用其他方式避免重复积分地更新预积分值([2.3 章节](#2.3 伴随 bias 迭代更新的预积分测量更新))。

2.1 预积分 IMU 测量值

(2.3) 将关键帧 ij 的状态(等式左侧)与测量值(等式右侧)联系起来。在这个意义上,可以被当初一个测量模型。不幸的是,它对测量噪声的依赖相当复杂,这使得 MAP 估计的直接应用也变得复杂。为了使得求解容易,需要将噪声项从惯性测量量中进行分离,使得预积分项变成 “=” 的形式。

2.1.1 ΔRij

(2.4)ΔRij=k=ij1Exp((w~kbgiηgdk)Δt)利用 Exp(ϕ+δϕ)Exp(ϕ)Exp(Jr(ϕ)δϕ),  且令 JrkJr((w~kbgi)Δt)k=ij1Exp((w~kbgi)Δt)Exp(JrkηgdiΔt)利用 Exp(ϕ) R=R Exp(RTϕ) 将噪声项移至最后ΔR~ijk=ij1Exp(ΔR~k+1jTJrkηgdkΔt)ΔR~ijExp(δϕij)

其中 ΔR~ijk=ij1Exp((w~kbgi)Δt) 即为预积分旋转测量值,它由陀螺仪测量值和对陀螺仪 bias 的估计计算得到。而 δϕij 为其测量噪声。

2.1.2 Δvij

将式 (2.4) ,即 ΔRijΔR~ijExp(δϕij) 代入 Δvij 的公式中,有

(2.5)Δvij=k=ij1ΔRik(a~kbaiηadk)Δtk=ij1ΔR~ikExp(δϕik)(a~kbaiηadk)Δtexp(δϕ)I+δϕ k=ij1ΔR~ik(Iδϕik)(a~kbaiηadk)Δt δϕηadk k=ij1[ΔR~ik(Iδϕik)(a~kbai)ΔtΔR~ikηadkΔt] αβ=βα k=ij1[ΔR~ik(a~kbai)Δt+ΔR~ik(a~kbai)δϕikΔtΔR~ikηadkΔt]=k=ij1[ΔR~ik(a~kbai)Δt]+k=ij1[ΔR~ik(a~kbai)δϕikΔtΔR~ikηadkΔt]Δv~ijδvij

其中 Δv~ijk=ij1[ΔR~ik(a~kbai)Δt] 即为预积分速度测量值,它由 IMU 测量值和对 bias 的估计计算得到。而 δvij 为其测量噪声。

2.1.3 Δpij

将式 (2.4) 和式 (2.5) ,即 ΔRijΔR~ijExp(δϕij)ΔvijΔv~ijδvij 代入 Δpij 的公式中,有

(2.6)Δpij=k=ij1[ΔvikΔt+12ΔRik(a~kbaiηadk)Δt2]k=ij1[(Δv~ikδvik)Δt+12ΔR~ikExp(δϕik)(a~kbaiηadk)Δt2] exp(δϕ)I+δϕ , δϕηadkk=ij1[(Δv~ikδvik)Δt+12ΔR~ik(Iδϕik)(a~kbai)Δt212ΔR~ikηadkΔt2]  αβ=βαk=ij1[Δv~ikΔt+12ΔR~ik(a~kbai)Δt2δvikΔt+12ΔR~ik(a~kbai)δϕikΔt212ΔR~ikηadkΔt2]Δp~ijδpij

其中 Δp~ij=k=ij1[Δv~ikΔt+12ΔR~ik(a~kbai)Δt2] 即为预积分位置测量值,它由 IMU 测量值和对 bias 的估计计算得到。而 δpij 为其测量噪声。

2.1.4 IMU 预积分测量模型

通过 2.1.1~2.1.3 我们有 ΔRijΔR~ijExp(δϕij)ΔvijΔv~ijδvijΔpijΔp~ijδpij 将之代入式 (2.3) 既能获取 IMU 预积分测量模型,即

(2.7)ΔR~ij=ΔRijExp(δϕij)T=RiTRjExp(δϕij)Δv~ij=Δvij+δvij=RiT(vjvigΔtij)+δvijΔp~ij=Δpij+δpij=RiT(pjpiviΔtij12gΔtij2)+δpij

2.2 预积分项的噪声传递

根据式 (2.7) ,将 IMU 预积分测量模型的噪声定义为 [δϕijT,δvijT,δpijT]T 。通常我们将噪声向量近似为零均的正态分布,但是准确地建模噪声的协方差是至关重要的。由于在优化过程用的是马氏距离,需要用到协方差的逆对优化各项进行加权。由此需要对 IMU 预积分测量模型的噪声进行分析(目的是给出协方差的计算表达式)。假设 IMU 预积分测量模型的噪声满足如下:

(2.8)ηijΔ[δϕijTδvijTδpijT]TN(09×1,Σij)

为了证实该假设,首先我们来看 IMU 预积分测量旋转噪声 δϕij ,由式 (2.4) 可得

(2.9)Exp(δϕij)=k=ij1Exp(ΔR~k+1jTJrkηgdkΔt):  δϕij=Log[k=ij1Exp(ΔR~k+1jTJrkηgdkΔt)] xk=ΔR~k+1jTJrkηgdkΔt , ηgdk,Jr(xk)I,Jr1(xk)Iδϕij=Log[k=ij1Exp(xk)]=Log[Exp(xi)k=i+1j1Exp(xk)]   Log(Exp(ϕ)Exp(δϕ))ϕ+Jr1δϕ(xi+I Log[k=i+1j1Exp(xk)])=xiLog[k=i+1j1Exp(xk)]=xiLog[Exp(xi+1)k=i+2j1Exp(xk)]xi+xi+1Log[k=i+2j1Exp(xk)]xi+xi+1++xj1=k=ij1ΔR~k+1jTJrkηgdkΔt

由此易得 δϕij (一阶近似)满足零均高斯分布,因为它可以通过零均高斯噪声 ηgdk 的线性组合构成。

接下来我们来看 IMU 预积分测量速度噪声 δvij 和测量位置噪声 δpij ,由式 (2.5) 和式 (2.6) 易得:

(2.10)δvij=k=ij1[ΔR~ik(a~kbai)δϕikΔt+ΔR~ikηadkΔt]δpij=k=ij1[δvijΔt12ΔR~ik(a~kbai)δϕikΔt2+12ΔR~ikηadkΔt2]

通过上式易看出 δvijδpij 由加速度噪声 ηadk 和预积分测量旋转噪声 δϕij 线性组合而成,由于其两者都是零均的高斯噪声( δϕij 在式 (2.9) 已证明),所以 δvijδpij 满足零均高斯分布。

通过式 (2.9) 和式 (2.10) 可知 IMU 预积分测量噪声 ηijΔ 可由 IMU 测量噪声 ηkd[ηgdk,ηadk],k=i,,j1 线性组合构成。由于 IMU 测量噪声 ηkd 的协方差已知(可以通过标定或查阅数据手册),因此只需通过一系列线性组合就可以 IMU 预积分测量噪声 ηijΔ 的协方差 Σij 了。为了满足实时性避免重复计算,下面推导预积分测量噪声协方差的递推形式。

2.2.1 δϕij 项的递推形式

根据式 (2.9)

(2.11)δϕijk=ij1ΔR~k+1jTJrkηgdkΔt=k=ij2ΔR~k+1jTJrkηgdkΔt+ΔR~jjT=I3×3Jrj1ηgdj1Δt=k=ij2(ΔR~k+1j1ΔR~j1jΔR~k+1j)TJrkηgdkΔt+Jrj1ηgdj1Δt=ΔR~j1jTk=ij2ΔR~k+1j1TJrkηgdkΔt+Jrj1ηgdj1Δt=ΔR~j1jTδϕij1+Jrj1ηgdj1Δt

2.2.2 δvij 项的递推形式

根据式 (2.10)

(2.12)δvij=k=ij1[ΔR~ik(a~kbai)δϕikΔt+ΔR~ikηadkΔt]=k=ij2[ΔR~ik(a~kbai)δϕikΔt+ΔR~ikηadkΔt]ΔR~ij1(a~j1bai)δϕij1Δt+ΔR~ij1ηadj1Δt=δvij1ΔR~ij1(a~j1bai)δϕij1Δt+ΔR~ij1ηadj1Δt

2.2.3 δpij 项的递推形式

根据式 (2.10)

(2.13)δpij=k=ij1[δvijΔt12ΔR~ik(a~kbai)δϕikΔt2+12ΔR~ikηadkΔt2]=k=ij2[δvijΔt12ΔR~ik(a~kbai)δϕikΔt2+12ΔR~ikηadkΔt2]+δvij1Δt12ΔR~ij1(a~j1bai)δϕij1Δt2+12ΔR~ij1ηadj1Δt2=δpij1+δvij1Δt12ΔR~ij1(a~j1bai)δϕij1Δt2+12ΔR~ij1ηadj1Δt2

2.2.4 总结

整合式 (2.11)(2.12)(2.13) ,可以得到 ηijΔ 的递推形式如下:

(2.14)ηijΔ=[ΔR~jj100ΔR~ij1(a~j1bai)ΔtI012ΔR~ij1(a~j1bai)Δt2ΔtII]Aj1ηij1Δ+[Jrj1Δt00ΔR~ij1Δt012ΔR~ij1Δt2]Bj1ηj1d

既有 ηijΔ=Aj1ηij1Δ+Bj1ηj1d ,由此根据协方差定义可得:

(2.15)Σij=Aj1Σij1Aj1T+Bj1ΣηBj1T

2.3 伴随 bias 迭代更新的预积分测量更新

在此之前的预积分都是假设了在两个关键帧 k=ik=j 之间 IMU 的 bias {b¯ak,b¯gk} 是恒定的基础上推导的。然而大多数情况下,IMU 的 bias 在优化过程中将伴随一个微小的增量 δb 进行变化。如果当 bias 变化时重新进行预积分 IMU 测量值,这将耗费巨大的计算量。因此,为了决绝这个问题,提出了利用线性化来进行 bias 变化时预积分测量项的一阶近似更新方式(b^b¯+δb):

(2.16)ΔR~ij(b^gi)ΔR~ij(b¯gi)Exp(ΔR¯ijbgδbgi)Δv~ij(b^gi,b^ai)Δv~ij(b¯gi,b¯ai)+Δv¯ijbgδbgi+Δv¯ijbaδbaiΔp~ij(b^gi,b^ai)Δp~ij(b¯gi,b¯ai)+Δp¯ijbgδbgi+Δp¯ijbaδbai

其中 b¯ 为旧 bias (原始假定) ,b^ 为新 bias (迭代更新所得) ,而 δb 是微小的增量。为了简化符号,假设已经在给定的偏差估计下的预积分变量为 ΔR¯ijΔR~ij(b¯gi), Δv¯ijΔv~ij(b¯gi,b¯ai), Δp¯ijΔp~ij(b¯gi,b¯ai) 。为了执行 (2.16) 的更新,需要计算其中的雅克比项。

2.3.1 ΔR~ij 项关于 bias 的雅克比

根据式 (2.4) 有:

(2.17)ΔR~ij(b^gi)=k=ij1Exp((w~kb^gi)Δt)=k=ij1Exp((w~k(b¯gi+δbgi))Δt)=k=ij1Exp((w~kb¯gi)ΔtδbgiΔt)利用 Exp(ϕ+δϕ)Exp(ϕ)Exp(Jr(ϕ)δϕ),  且令 J¯rkJr((w~kb¯gi)Δt)=k=ij1Exp((w~kb¯gi)Δt)Exp(J¯rkδbgiΔt)利用 Exp(ϕ) R=R Exp(RTϕ)  δbgi 噪声项移至最后=ΔR¯ijk=ij1Exp(ΔR¯k+1jJ¯rkδbgiΔt)利用 Log(Exp(ϕ)Exp(δϕ))ϕ+Jr(ϕ)1δϕ, 当 ϕ 也是小量时有Jr(ϕ)1I由于 δbgi ΔR¯k+1jJ¯rkδbgiΔt 也是小量, 令 αk=ΔR¯k+1jJ¯rkδbgiΔt=ΔR¯ijExp(αi)Exp(αi+1)k=i+2j1Exp(αk)ΔR¯ijExp(αi+αi+1)Exp(αi+2)Exp(αi+3)k=i+4j1Exp(αk)ΔR¯ijExp(k=ij1αi)=ΔR¯ijExp(k=ij1(ΔR¯k+1jJ¯rkδbgiΔt))

于是根据 (2.16) 的定义有

(2.18)ΔR¯ijbg=k=ij1(ΔR¯k+1jJ¯rkΔt) , 其中 J¯rkJr((w~kb¯gi)Δt)

2.3.2 Δv~ij 项关于 bias 的雅克比

根据式 (2.5) 有:

(2.19)Δv~ij(b^gi,b^ai)=k=ij1[ΔR~ik(b^gi)(a~kb^ai)Δt]=k=ij1[ΔR~ik(b^gi)(a~k(b¯ai+δbai))Δt]结合式(2.16)(2.17)=k=ij1[ΔR¯ikExp(ΔR¯ikbgδbgi)(a~kb¯aiδbai)Δt]利用exp(δϕ)I+δϕk=ij1[ΔR¯ik(I+(ΔR¯ikbgδbgi))(a~kb¯aiδbai)Δt]忽略高阶小项 (ΔR¯ijbgδbgi)δbai ,利用 αβ=βα , Δv¯ijk=ij1[ΔR¯ik(a~kb¯ai)Δt]Δv¯ijk=ij1ΔR¯ikΔtδbai+k=ij1ΔR¯ik(ΔR¯ikbgδbgi)(a~kb¯ai)ΔtΔv¯ijk=ij1ΔR¯ikΔtδbaik=ij1ΔR¯ik(a~kb¯ai)ΔR¯ikbgΔtδbgi

于是根据 (2.16) 的定义有

(2.20)Δv¯ijbg=k=ij1ΔR¯ik(a~kb¯ai)ΔR¯ikbgΔtΔv¯ijba=k=ij1ΔR¯ikΔt

2.3.3 Δp~ij 项关于 bias 的雅克比

根据式 (2.6) 有:

(2.21)Δp~ij(b^g,b^a)=k=ij1[Δv~ik(b^g,b^a)Δt+12ΔR~ik(b^gi)(a~kb^ai)Δt2]=k=ij1[Δv~ik(b^g,b^a)Δt]1+12k=ij1[ΔR~ik(b^gi)(a~kb^ai)Δt2]21=k=ij1[(Δv¯ik+Δv¯ikbgδbgi+Δv¯ikbaδbai)Δt]=k=ij1[Δv¯ikΔt+Δv¯ikbgΔtδbgi+Δv¯ikbaΔtδbai]2=12k=ij1[ΔR¯ikExp(ΔR¯ikbgδbgi)(a~k(b¯ai+δbai))Δt2]忽略高阶小项 (ΔR¯ijbgδbgi)δbai ,利用 exp(δϕ)I+δϕ,  αβ=βαΔt22k=ij1[ΔR¯ik(I+(ΔR¯ikbgδbgi))(a~kb¯aiδbai)]Δt22k=ij1[ΔR¯ik(a~kb¯ai)ΔR¯ikδbaiΔR¯ik(a~kb¯ai)ΔR¯ikbgδbgi]1+2=k=ij1[[Δv¯ikΔt+12ΔR¯ik(a~kb¯ai)Δt2]+  [Δv¯ikbgΔt12ΔR¯ik(a~kb¯ai)ΔR¯ikbgΔt2]δbgi+  [Δv¯ikbaΔt12ΔR¯ikΔt2]δbai]

于是根据 (2.16) 的定义有

(2.22)Δp¯ij=k=ij1[Δv¯ikΔt+12ΔR¯ik(a~kb¯ai)Δt2]Δp¯ijbg=k=ij1[Δv¯ikbgΔt12ΔR¯ik(a~kb¯ai)ΔR¯ikbgΔt2]Δp¯ijba=k=ij1[Δv¯ikbaΔt12ΔR¯ikΔt2]

通过 2.3.1~2.3.3 我们可知,当接收到新的测量数据时这些雅克比可以通过递增方式及时计算。

3. IMU 预积分残差

通过预积分测量模型 (式 (2.7)) 和测量噪声在一阶 (式 (2.8)) 上协方差为 Σij 的零均高斯噪声,因此可以得到关于 IMU 预积分因子的残差 rIij[rΔRijT, rΔvijT, rΔpijT]R9 ,有:

(3.1)rΔRijLog((ΔR~ij(b¯gi)Exp(ΔR¯ijbgδbgi))TRiTRj)rΔvijRi(vjvigΔtij)[Δv~ij(b¯gi,b¯ai)+Δv¯ijbgδbgi+Δv¯ijbaδbai]rΔpijRiT(pjpiviΔtij12gΔtij2)[Δp~ij(b¯gi,b¯ai)+Δp¯ijbgδbgi+Δp¯ijbaδbai]

其中包含前面推导的伴随 bias 迭代更新的预积分测量如式 (2.16) ,这种近似的修正方式避免了积分的重新计算,是预积分技术降低计算量的关键。

首先,需要先理解 IMU 预积分因子中的残差指的是预计预积分计算值 (由非 IMU 的其他方式估计的预积分值) 与 IMU 预积分测量值的差。在进行优化时,我们通常采用 “lift-solve-retract” 的方式,而每一次迭代过程将利用

(3.2)RiRiExp(δϕi),RjRjExp(δϕj),pipi+Riδpi,pjpj+Rjδpj,vivi+δvi,vjvj+δvj,δbgiδbgi+δ~bgi,δbaiδbbi+δ~bai

重新进行参数化式 (3.1) ,通过增量来更新状态进而 retract 估计量,而 solve 步骤需要围绕当前估计的结果成本进行线性化,而线性化展开的目的是便于解析计算残差的雅克比行列表达式。这个过程通常被称之为 lifting ,它使得残差成为定义在易于计算雅克比行列式的向量空间上的函数。因此我们接下来将推导残差关于向量 δϕi,δpi,δvi,δϕj,δpj,δvj,δ~bgiδ~bai 的雅克比行列式。

3.1 残差 rΔRij 的雅克比

由于 rΔRij 中不含 pi,pj,vi,vj,δbai ,因此 rΔRij 关于这些状态增量的雅克比都为 0 。在 3.1.1~3.1.3 的推导中采用 EExp(R¯ijbgδbgi) 来进行速记。

3.1.1 残差 rΔRij 关于 δϕi 的雅克比

(3.3)rΔRij(RiExp(δϕi))=Log((ΔR~ij(b¯gi)E)T(RiExp(δϕi))TRj)=Log((ΔR~ij(b¯gi)E)TExp(δϕi)RiTRj)利用 Exp(ϕ)R=RExp(RTϕ)=Log((ΔR~ij(b¯gi)E)TRiTRjExp(RjTRiδϕi))=Log(Exp(Log((ΔR~ij(b¯gi)E)TRiTRj))Exp(RjTRiδϕi))利用 Log(Exp(ϕ)Exp(δϕ))ϕ+Jr1(ϕ)δϕrΔRij(Ri)Jr1(rΔRij(Ri))RjTRiδϕi

3.1.2 残差 rΔRij 关于 δϕj 的雅克比

(3.4)rΔRij(RjExp(δϕj))=Log((ΔR~ij(b¯gi)E)TRiTRjExp(δϕj))=Log(Exp(Log((ΔR~ij(b¯gi)E)TRiTRj))Exp(δϕj))利用 Log(Exp(ϕ)Exp(δϕ))ϕ+Jr1(ϕ)δϕrΔRij(Rj)+Jr1(rΔRij(Rj))δϕj

3.1.3 残差 rΔRij 关于 δ~bgi 的雅克比

(3.5)rΔRij(δbgi+δ~bgi)=Log((ΔR~ij(b¯gi)Exp(ΔR¯ijbg(δbgi+δ~bgi)))TRiTRj)利用 Exp(ϕ+δϕ)Exp(ϕ)Exp(Jr(ϕ)δϕ),  JrbJr(ΔR¯ijbgδbgi)=Log((ΔR~ij(b¯gi)E Exp(JrbΔR¯ijbgδ~bgi))TRiTRj)=Log(Exp(JrbΔR¯ijbgδ~bgi)T(ΔR~ij(b¯gi)E)TRiTRj)=Log(Exp(JrbΔR¯ijbgδ~bgi)Exp(rΔRij(δbgi)))利用 Exp(ϕ)R=RExp(RTϕ)=Log(Exp(rΔRij(δbgi))  Exp(Exp(rΔRij(δbgi))TJrbΔR¯ijbgδ~bgi))利用 Log(Exp(ϕ)Exp(δϕ))ϕ+Jr1(ϕ)δϕrΔRij(δbgi)Jr(rΔRij(δbgi))Exp(rΔRij(δbgi))TJrbΔR¯ijbgδ~bgi

3.1.4 总结

根据上述的推导,残差 rΔRij 关于 δϕi,δpi,δvi,δϕj,δpj,δvj,δ~bgiδ~bai 的雅克比为:

(3.6)rΔRijδϕi=Jr1(rΔRij(Ri))RjTRirΔRijδpi=0rΔRijδvi=0rΔRijδϕj=Jr1(rΔRij(Rj))rΔRijδpj=0rΔRijδvj=0rΔRijδ~bai=0rΔRijδ~bgi=Jr(rΔRij(δbgi))Exp(rΔRij(δbgi))TJrbΔR¯ijbg

3.2 残差 rΔvij 的雅克比

由于 rΔvij 中不含 Rj,pi,pj ,因此 rΔvij 关于这些状态增量的雅克比都为 0 。

3.2.1 残差 rΔvij 关于 δϕi 的雅克比

(3.7)rΔvij(RiExp(ϕi))=(RiExp(ϕi))T(vjvigΔtij)Δv~ij(b^gi,b^ai)=Exp(ϕi)TRiT(vjvigΔtij)Δv~ij(b^gi,b^ai)=Exp(ϕi)RiT(vjvigΔtij)Δv~ij(b^gi,b^ai)利用 Exp(δϕ)I+δϕ(Iδϕi)RiT(vjvigΔtij)Δv~ij(b^gi,b^ai)=rΔvij(Ri)δϕi(RiT(vjvigΔtij))利用 αβ=βα=rΔvij(Ri)+(RiT(vjvigΔtij))δϕi

3.2.3 残差 rΔvij 关于 δviδvj 的雅克比

(3.8)rΔvij(vi+δvi)=RiT(vjviδvigΔtij)Δv~ij(b^gi,b^ai)=rΔvij(vi)RiTδvirΔvij(vj+δvj)=RiT(vj+δvjvigΔtij)Δv~ij(b^gi,b^ai)=rΔvij(vi)+RiTδvj

3.2.3 残差 rΔvij 关于 δ~baiδ~bgi 的雅克比

由于 rΔvij 关于 δbaiδbgi 是线性的,因此 rΔvij 关于 δ~baiδ~bgi 的雅克比可以直接依据线性系数得到,依赖于式 (2.20) 易得:

(3.9)rΔvij(δbai+δ~bai)=rΔvij(δbai)Δv¯ijbaδ~bairΔvij(δbgi+δ~bgi)=rΔvij(δbgi)Δv¯ijbgδ~bgi

3.2.4 总结

根据上述的推导,残差 rΔvij 关于 δϕi,δpi,δvi,δϕj,δpj,δvj,δ~bgiδ~bai 的雅克比为:

(3.10)rΔvijδϕi=(RiT(vjvigΔtij))rΔvijδpi=0rΔvijδvi=RiTrΔvijδϕj=0rΔvijδpj=0rΔvijδvj=RiTrΔvijδ~bai=Δv¯ijbarΔvijδ~bgi=Δv¯ijbg

3.3 残差 rΔpij 的雅克比

由于 rΔpij 中不含 Rj,vj ,因此 rΔpij 关于这些状态增量的雅克比都为 0 。

3.3.1 残差 rΔpij 关于 δϕi 的雅克比

(3.11)rΔpij(RiExp(δϕi))=(RiExp(δϕi))T(pjpiviΔtij12gΔtij2)Δp~(b^gi,b^ai)=Exp(δϕi)TRiT(pjpiviΔtij12gΔtij2)Δp~(b^gi,b^ai)=Exp(δϕi)RiT(pjpiviΔtij12gΔtij2)Δp~(b^gi,b^ai)利用 Exp(δϕ)I+δϕ(Iδϕi)(RiT(pjpiviΔtij12gΔtij2))Δp~(b^gi,b^ai)=rΔpij(Ri)δϕi(RiT(pjpiviΔtij12gΔtij2))利用 αβ=βα=rΔpij(Ri)(RiT(pjpiviΔtij12gΔtij2))δϕi

3.3.2 残差 rΔpij 关于 δvi 的雅克比

(3.12)rΔpij(vi+δvi)=RiT(pjpiviΔtijδviΔtij12gΔtij2)Δp~(b^gi,b^ai)=rΔpij(vi)RiTΔtijδvi

3.2.3 残差 rΔpij 关于 δpiδpi 的雅克比

(3.13)rΔpij(pi+Riδpi)=RiT(pjpiRiδpiviΔtij12gΔtij2)Δp~(b^gi,b^ai)=rΔpij(pi)I3×3δpirΔpij(pj+Rjδpj)=RiT(pj+RjδpjpiviΔtij12gΔtij2)Δp~(b^gi,b^ai)=rΔpij(pi)+RiTRjδpi

3.3.4 残差 rΔpij 关于 δ~baiδ~bgi 的雅克比

由于 rΔvij 关于 δbaiδbgi 是线性的,因此 rΔvij 关于 δ~baiδ~bgi 的雅克比可以直接依据线性系数得到,依赖于式 (2.22) 易得:

(3.14)rΔpij(δbai+δ~bai)=rΔpij(δbai)Δp¯ijbaδ~bairΔpij(δbgi+δ~bgi)=rΔpij(δbgi)Δp¯ijbgδ~bgi

3.3.5 总结

根据上述的推导,残差 rΔpij 关于 δϕi,δpi,δvi,δϕj,δpj,δvj,δ~bgiδ~bai 的雅克比为:

(3.15)rΔpijδϕi=(RiT(pjpiviΔtij12gΔtij2))rΔpijδpi=I3×3rΔpijδvi=RiTΔtijrΔpijδϕj=0rΔpijδpj=RiTRjrΔpijδvj=0rΔpijδ~bai=Δp¯ijbarΔpijδ~bgi=Δp¯ijbg

4. Bias 模型

在介绍 IMU 模型时,式 (1.1) ,我们说 biases 式随时间缓慢变化的量。因此,采用 “布朗运动 (Brownian motion)” 进行建模(补充:物理学中的布朗运动与数学中的 “维纳过程 (Wiener process)” 密切相关)。biases 白噪运动学的微分方程形式如下:

(4.1)b˙g(t)=ηbg,b˙a(t)=ηba

根据上式,在两个连续关键帧 i 和 j 之间的时间间隔 [ti,tj] 上积分得:

(4.2)bgj=bgi+ηbgd,baj=bai+ηbad

其中,上式采用例如 bgibg(ti) 进行速记,且定义带有零均值和协方差分别为 ΣbgdΔtijCov(ηbg)ΣbadΔtijCov(ηba) 的离散噪声 ηbgdηbad 。根据式 (4.2) 可以简单被包含到因子图优化中,作为 IMU 预积分因子的附加项。

5. IMU 噪声的形式与单位

当我们在使用 IMU 的时候,离不开对 IMU Allan 方差的标定。这时候,我们通常会采用 kalibr_allanallan_variance_ros 工具进行标定。根据式 (1.6) 和第 4 节的内容,可知 IMU 的噪声形式分为连续形式和离散形式。而不同形式下其单位不同。结合前面的内容与对应的参考文献,下面直接给出不同形式下名称与单位。

名称符号单位
gyroscope white noise (or gyro noise density)σgrads1Hz,=rads
gyroscope random work (or gyro’s bias instability)σbgrads21Hz,=rads3
accelerator white noise (or accel noise density)σams21Hz
accelerator random work (or accel’s bias instability)σbams31Hz
discrete gyroscope white noiseσgdrads
discrete gyroscope random workσbgdrads
discrete accelerator white noiseσadms2
discrete accelerator random workσbadms2



Reference