IMU状态预积分的定义
- IMU状态预积分的定义
在ESKF中,将两个GNSS观测之间的IMU数据进行积分,作为ESKF的预测过程。
这种做法把IMU数据看成某种一次性的使用方式:将它们积分到当前估计值上,然后用观测数据更新当时的估计值。
这种做法和此时的状态估计值有关。如果状态量发送了改变,能否重复利用这些IMU数据呢?
从物理意义上看,IMU反映的是两个时刻间机器人的角度变化量和速度变化量。如果希望IMU的计算与当时的状态估计无关,那么在算法上应该如何处理呢?则需要用到预积分了。
预积分:是一种十分常见的IMU数据处理方法。
与传统的IMU运动学积分不同,预积分可以将一段时间内的IMU测量数据累积,建立预积分测量,同时还能保证测量值与状态变量无关。
如果以吃饭来比喻的话,ESKF像是一口口地吃菜,而预积分则是从锅里先把菜一块块夹到碗里,再把碗里的菜一口气吃掉。
无论是LIO系统还是VIO系统,预积分已经称为诸多紧耦合IMU系统的标准方法。
IMU状态预积分的定义
在一个IMU系统里,考虑它的五个变量:
- 旋转R
- 平移p
- 角速度w
- 线速度v
- 加速度a
这些变量的运动学关系可以写成:
在t到
t
+
Δ
t
t+ \Delta t
t+Δt 时间内,对上式进行欧拉积分,可得:
其中,角速度和加速度可以被IMU测量到,但受噪声与重力影响。
令测量值为
ω
~
\tilde{\omega }
ω~ 和
a
~
\tilde{a}
a~ ,则有噪声与零偏与重力的测量公式如下:
其中
b
g
b_{g}
bg和
b
a
b_{a}
ba为陀螺仪和加速度零偏
η
g
\eta_{g}
ηg和
η
a
\eta_{a}
ηa为测量的高斯噪声
用测量值表示t到
t
+
Δ
t
t+ \Delta t
t+Δt 的运动关系,即测量值与状态变量的关系:
其中,
η
g
d
\eta_{gd}
ηgd和
η
a
d
\eta_{ad}
ηad是离散化后的随机游走噪声:
可以用这种约束来构建图优化,对IMU相关问题进行求解。但是这组方程刻画的时间太短,仅包含单个IMU数据。
IMU的测量频率太高,并不希望优化过程随着IMU数据进行调用,那样太浪费计算资源。
更希望将这些IMU测量值组合在一起处理。
下面介绍如何在关键帧之间对IMU进行预积分。
假设离散时间i和j之间的IMU数据被累积,这个过程可以持续几秒。这种被累积的观测被称为预积分。
在i至j的过程中,可以把变量累计得到
其中
△
t
i
j
=
∑
k
=
i
j
−
1
△
t
\triangle t_{ij}= {\textstyle \sum_{k=i}^{j-1}\triangle t}
△tij=∑k=ij−1△t,为累积的时间。
在已知i时刻状态和所有测量时,该式可以用于推断j时刻的状态。这就是传统意义上的直接积分。
直接积分的缺点是,它描述的过程和状态量有关。如果对i时刻状态进行优化,那么i+1,i+2,…j-1时刻的状态也会跟着发送改变,这个积分就必须重新计算,这是非常不便的。
将上式间改变,将IMU读数放在一侧,状态量放到另一侧,定义相对的运动量为
这种改变实际上只是计算了某种从i到j的“差值”。它们虽然写作p.v.R的形式,但并不直接是位移、速度、旋转的物理量,而是人为定义的变量。最左边的就是预积分量
这个定义在计算上有一下性质:
- 不妨考虑从i时刻出发,此时这三个量都为零。在i+1时刻,计算出计算出 △ R i , i + 1 , △ v i , i + 1 , △ p i , i + 1 \triangle R_{i,i+1},\triangle v_{i,i+1},\triangle p_{i,i+1} △Ri,i+1,△vi,i+1,△pi,i+1。在i+2时刻,由于这三个式子都是累乘或累加的形式,只需在i,i+1时刻的结果上,加上第i+2时刻的测量值即可。这个计算层面带来了很大的便利。还会发现这种性质便于后续计算各种雅克比矩阵。
- 从等号最右侧来看,上述所有计算都和R,v,p的取值无关。即使它们的估计值发送改变,也无须重新计算IMU的积分量。
- 如果零偏 b a , k b_{a,k} ba,k或 b g , k b_{g,k} bg,k发送变化,那么上式理论上还需要重新计算。然后,也可以通过修正而非重新计算的思路调整预积分量。
- 预积分量并没有直接的物理含义。尽管符合上用了 △ v \triangle v △v, △ p \triangle p △p之类的表示,但它并不表示某两个速度或位置上的偏差。它只是如此定义而已。当然,从量纲上来说,应该与角度、速度、位移对应。
- 由于预积分量不是直接的物理量,这种测量模型的噪声必须从原始的IMU噪声推导而来。