机器人运动估计——IMU运动方程与ESKF原理介绍(下)

646
0
2020年9月4日 10时25分

Hello! 欢迎来到我的博客
今天的内容关于机器人中常用的传感器IMU,我们用它来实现机器人姿态、速度、位置的估计。
今天将会介绍使用低成本IMU进行机器人运动估计的一个常用方法——ESKF。

 

3 Error State Kalman Filter(ESKF介绍)

3.1 动机与流程

使用IMU的初衷是为了求解载体的位置、速度、姿态等系统状态,然而由于IMU测量数据(包括加速度计和陀螺仪的测量数据)都带有大量噪声,直接利用IMU运动方程进行积分会随着时间发生显著的漂移。因此,为了避免漂移我们需要融合其他传感器的信息(如GPS、视觉),对IMU运动积分的结果进行修正。

 

Error-state Kalman Filter(ESKF)就是一种传感器融合的算法,它的基础仍然是卡尔曼滤波。它的核心思想是把系统的状态分为三类:

  • true state:实际状态,系统实际的运行状态
  • nominal state:名义状态,描述了运动状态的主要趋势,主导成分。(large-signal,非线性)
  • error state:误差状态,实际状态与名义状态之间的差值(small-signal,近似线性,适合线性高斯滤波)

 

基于以上状态分类,我们可以将关心的true-state,分为两部分,分别进行估计,即nominal state和error state,然后再进行二者叠加。

 

ESKF的全过程可以描述为如下步骤:

  • 对高频率的IMU数据um\mathbf{u}_m进行积分,获得nomimal state x\mathbf{x}。(注意:nominal state里面不考虑噪声,因此势必会累积误差。这部分误差可依据error state进行修正)
  • 利用Kalman Filter估计error state,这里同样也包括时间更新和量测更新两部分。(注意:这个过程考虑了噪声,并且由于误差状态方程是近似线性的,所以可以直接用卡尔曼滤波)
  • 利用error state修正nominal state,获取true state
  • 重置error state

 

使用ESKF的优势:

  1. error state 中的参数数量与运动自由度是相等的,避免了过参数化(over-parameterization or redundancy)引起的协方差矩阵奇异的风险。
  2. error state 总是接近于0,Kalman Filter工作在原点附近。因此,远离奇异值、万向节锁,并且保证了线性化的合理性和有效性。
  3. error state 总是很小,因此二阶项都可以忽略,因此雅可比矩阵的计算会很简单,很迅速。
  4. error state 的变化平缓,因此KF修正的频率不需要太高。

3.2 状态定义

所有在ESKF中用到的变量定义如下:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图

 

图片中定义了所以状态,包括true state,nominal state以及error state。以及error state和nominal state组合获得true state的方式。

 

另外,图中的各个运算符在上一篇文章中均已有了定义。除了一个叉乘矩阵的符号,其定义如下:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(1)

 

图片来自于参考文献[1]的P31。

 

3.3 Nominal State 积分更新(每个时间步都要执行)

3.3.1 Nominal state 方程(连续时间)

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(2)

 

其中,R=R{q}是基于q成的旋转矩阵,表示从IMU系到惯性系的旋转。

 

注意到,这组方程完全没有考虑噪声。这组方程的推导是显而意见的,只需要在IMU运动方程的基础上,假设噪声项全部为0即可。

 

3.3.2 Nominal state 方程(离散时间,用于写程序)

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(3)

 

其中,定义机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(4) ,所以:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(5)

 

其中,机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(6)

 

实际上,上面这个操作就是把角度增量改造成单位四元数,两个单位四元数相乘仍然是单位四元数,这样才能保证 q 始终为单位四元数,保证迭代是收敛的。

 

3.4 Error State 时间更新(每个时间步都要执行)

3.4.1 Error state 方程(连续时间):

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(7)

 

这个误差方程,考虑了噪声等不确定性。其形式看起来似乎不太直观,但实际上推导也很简单。

 

比如,以位置为例:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(8)

 

所以其导数为:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(9)

 

再比如以速度为例:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(10)

 

所以其导数为:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(11)

 

其中,用到了机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(12)。此外,还用到了两个小量相乘可以忽略的假设。

 

其它方程的推导,就不在此说明了,可以参考文献[1]的P33页。

 

3.4.2 Error state 方程(离散时间,用于写程序):

 

定义状态向量:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(13)

 

于是误差状态方程可以写作:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(14)

 

其中:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(15)

 

中,vi,θi,ai,ωi 代表随机脉冲,他们的均值为 0,协方差矩阵为对角阵。

 

另外,定义:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(16)

 

这是一个微分方程的闭合解,利用这个解进行递推会更精确。当然,也可以只取前面两项。

 

3.4.3 对于Error state的卡尔曼滤波器的时间更新(用于写程序)

 

误差状态更新

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(17)

 

协方差矩阵更新

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(18)

 

这个就是很基础的卡尔曼滤波方程了,不了解的可以移步我的博客:机器人运动估计系列(番外篇)——从贝叶斯滤波到卡尔曼(上)

 

3.5 Error State 量测更新(只有测量值到来的时间步需要执行)

由于IMU的数据中夹带了大量噪声,我们需要利用更多的传感器的信息,来对状态进行修正。

 

而这些传感器中常用的组合包括:GPS+IMU,单目视觉+IMU,双目视觉+IMU等。

 

3.5.1 量测方程

 

通常,量测方程的基本形式如下:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(19)

 

其中 h( ) 是量测函数,与传感器有关。v是高斯白噪声,为机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(20)

 

为了实现量测更新,我们需要求 h δx 的导数,所以有:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(21)

 

其中机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(22)是量测函数的雅可比矩阵,根据传感器的不同而具有不同形式。

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(23)则可以表示为:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(24)

 

关于上式的详细推导,可以参考文献[1]的P39页。

 

3.5.2 对于Error state的卡尔曼滤波器的量测更新(用于写程序)

 

卡尔曼增益的计算

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(25)

 

误差状态量测更新

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(26)

 

协方差矩阵更新

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(27)

 

这是卡尔曼滤波的基础方程,就不做介绍了。

3.6 状态合并以及Error State 重置(在量测更新过后执行)

3.6.1 状态合并

 

这一步是要把error state与nominal state合并,即修正nominal state随时间的漂移。

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(28)

 

即:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(29)

 

3.6.2 Error state 重置

 

由于nominal state的误差已经修正了,所以error state要置零,同时协方差矩阵也要相应的修改。

 

通常来说,重置的方法如下:

 

机器人运动估计——IMU运动方程与ESKF原理介绍(下)插图(30)

 

参考文献

[1] Joan Sol`a. Quaternion kinematics for the error-state KF. Sep 12, 2016.

 

关于<IMU运动方程与ESKF原理介绍>的上下两篇文章主要内容出自上面这篇参考文献,我只是进行了翻译和重新梳理逻辑顺序的工作。因此,大家如果使用到本博客内容,也烦请引用上述参考文献。谢谢~

 

发表评论

后才能评论