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

525
0
2020年9月3日 11时38分

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

1 四元数基础

四元数,英文名称为:quaternion。正如一个单位复数可以表示在复平面上的旋转一样,单位四元数也可以用于表示三维空间中的旋转,并且由于其在更新过程中不会发生奇异现象,因此在惯性导航中被广泛使用。一般来说,四元数的表示方式有许多,其中最常用的是两种,一种是Hamilton,另一种是JPL。由于,在机器人领域中,无论是ROS,Eigen,Ceres等等项目中都用到的是Hamilton四元数,因此,这里后续文章中使用以及介绍的也是这种。

 

以下介绍的都是在IMU运动方程以及ESKF中必要的关于四元数的知识,因此可能会过于简略,读者可暂时先跳转至第二部分阅读,遇到不明的四元数定义时,再回来查阅。

 

(1)四元数定义

 

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

其中,{a,b,c,d}R 是系数,{i,j,k}是虚部单位。

 

一个四元数可以表示为实部和虚部,如下:

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

 

记作向量形式则为:

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

(2)单位四元数

 

单位四元数可以作为一种旋转的表示方式,它可以转换为欧拉角,也可以转换为旋转矩阵。

 

单位四元数的定义:

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

 

其中,机器人运动估计——IMU运动方程与ESKF原理介绍(上)插图(4)是单位向量,而 θ 为一个标量。

 

由此可知,单位四元数的模长为1。

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

 

(3)乘法

 

两个四元数乘法的记号为:⊗,乘法的具体定义为:

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

值得注意的是,两个单位四元数相乘还是单位四元数。

 

(4)纯四元数的指数函数

 

纯四元数的定义为,实部为0的四元数,即:

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

 

定义纯四元数的指数函数如下:

 

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

其中:机器人运动估计——IMU运动方程与ESKF原理介绍(上)插图(9)u 为单位向量。

 

(5)单位四元数转旋转矩阵

 

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

注意,必须是单位四元数!!!

 

(6)单位四元数转欧拉角

 

由于欧拉角的定义方式也有很多,这里说明的是依据Z-Y-X定义的欧拉角,其中欧拉角与旋转矩阵的变换关系是:

 

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

 

其中:机器人运动估计——IMU运动方程与ESKF原理介绍(上)插图(12) 为欧拉角,分别为横滚角(X),俯仰角(Y),偏航角(Z)。

 

所以,四元数转欧拉角的关系为:

 

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

 

注意,必须是单位四元数!!!

 

2 IMU运动方程

2.1 关于IMU测量数据

 

在聊到IMU测量数据的时候,我们首先需要明白两个坐标系的定义。

 

一是惯性系,二是IMU坐标系。

 

这里的惯性系就是指静止或者其速度的改变可以忽略不记的坐标系。通常在机器人的应用中,由于所用的IMU都是低成本IMU,对于地球自转角速度不够敏感,所以可以认为与地面固连的坐标系都是惯性坐标系。然而对于,航空航天应用而言,用到的IMU精度很高,对于惯性导航的要求也很严格,则需要考虑地球自转角速度,这是惯性坐标系为地球惯性坐标系。

 

IMU测量的实际上就是IMU坐标系相对于惯性系的加速度和角速度。值得注意的是,这里的相对于惯性系的加速度不仅包括载体的运动加速度,还包括重力加速度。

 

所以,我们需要特别注意,加速度计测量的不是载体的运动加速度,而是载体相对惯性空间的绝对加速度和引力加速度之和,称作“比力”。

 

通常,IMU的测量模型如下:

 

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

 

其中,机器人运动估计——IMU运动方程与ESKF原理介绍(上)插图(15)表示从惯性系到IMU坐标系的旋转矩阵。机器人运动估计——IMU运动方程与ESKF原理介绍(上)插图(16)代表随机游走噪声,可以理解为加速度计和陀螺仪的零漂。an, ωn 为零均值高斯白噪声。gt 代表重力加速度,表示在惯性系下。这里关于 gt 的符号不用特别在意,这个负号并不是一定的,也可以写成正号,取决于 gt 的定义。

 

2.2 IMU运动方程

 

IMU的运动方程主要描述了物体速度,位置以及姿态在IMU测量数据的激励下产生的变化。除了关于四元数的部分以外,用到的知识就是小学二年级学过的物理知识。比如,位置的导数是速度,速度的导数是加速度。
我们定义我们关心的状态量为:

 

  • :表示IMU系相对惯性系的位置,在惯性系下表示的,默认是三维的
  • vt:表示IMU系相对惯性系的速度,同样是在惯性系下表示的,默认是三维的
  • qt:表示从IMU系到惯性系的四元数,表示从IMU系到惯性系的旋转变换

 

于是,IMU的运动方程可以表示如下:

 

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

 

其中,Rt= R{ qt}为基于 qt 得到的旋转矩阵,表示了从IMU系到惯性系的旋转变换。aw , ωw为零均值高斯白噪声。

 

值得注意的是,在这组运动方程中,把 gt 也视作变化的量,后续基于此运动方程构造的ESKF也会对 gt 进行更新。这在许多IMU运动方程中是不常见到的,这样做有什么好处呢?

 

首先,我们要明白 g并不等于 的值取决于IMU的初始姿态。这是因为,IMU运动方程是递推的方程,计算出来的位置和速度都是相对于初始状态的,而不是绝对的位置和速度。因此为了方便起见,我们通常都会定义初始时刻的IMU坐标系为惯性系,这样我们后面计算出来的虽然是相对于初始IMU系的位置和速度,但也可以作为绝对的位置和速度了。于是, g既然代表了惯性系下的重力加速度,也就是初始的IMU坐标系下表示的重力加速度,所以当初始IMU坐标系不是水平的时候, g自然也就不等于 了。

 

进一步地,我们也可以知道,估计 g,也就是估计初始时刻IMU的姿态。既然我们通过可以 g估计了初始IMU的姿态,那么初始时刻的四元数就可以直接定义为 q0=(1,0,0,0)这种操作实际上就是,我们先假设初始时刻四元数为 q0=(1,0,0,0),然后在此基础上估计 g当然,还有另外一种方法就是,先假设 gt ,然后估计初始时刻的IMU相距水平姿态相差多少。这两种方法都可以,然后估计 g的话可以使模型的线性化程度更好。

 

为了更好的阅读体验,本文分为上下两文,关于ESKF的内容以及MATLAB代码,请移步下一篇文章。

发表评论

后才能评论