从零手写VIO第二讲——旋转运动学

目录结构如下：

一、旋转运动学

二、IMU测量模型及运动模型

​ MEMS加速度计工作原理

​ MEMS陀螺工作原理

三、IMU误差模型

​ 确定性误差

​ 确定性误差的标定

​ 随机误差

​ IMU数学模型

运动模型离散时间处理

​ 欧拉法

​ 中值法

5、IMU数据仿真

一、旋转运动学

1、

其中a是半径，左边的矩阵是反对称矩阵。

​ 求导就是求得这个东西的速度。

​ 最后一个公式其实就是线速度=角速度×半径（只不过这里是在矢量坐标系下）

线速度和角速度的计算公式如下：

2、
现在我们考虑两个坐标系下的运动，一个是静止的坐标系（惯性系），一个是运动 的坐标系（车体IMU的坐标系）。分别对应下图的inertial frame body frame

第二个公式其实就是body坐标系和惯性inertial坐标系的转化，就是旋转矩阵×body坐标系下的坐标。

最终求得的是body坐标系下的速度如何转化到惯性inertial坐标系下。并不是简简单单的乘以做左乘旋转矩阵。

重点看一下在惯性系inertial下的速度如何由body（IMU）坐标系表示出来。

旋转坐标系下的运动学计算如下：

二、IMU测量模型及运动模型

看下面这个公式就可得到IMU其实测量的是那个弹簧的加速度，这个把m往右边乘过去就知道了。

IMU测量模型及运动模型如下图：

这个其实也正常，这个IMU输出的是弹簧的力的话，那也就是说静止不动的时候，他要抵消地球的加速度，所以是G。

自由落体的时候，加速度本来就是G，由地球全部提供了，所以IMU的输出为0。

IMU不是只要静止放在地球上输出就为0，比如放在一个斜面上，它是有分量的。所以这个ENU（东北天坐标系）是一个向量。

加速度计测量原理如下图：

这个陀螺仪的原理就是根据科氏力来测的，前面的PPT中有科氏力的公式，2w×v就是科氏力，所以装置必须有产生科氏力的条件，也就是需要一个主动轴，比如在x轴上给质量块一个力，让其做高速的正弦运动，所以在y轴旋转的时候，就会产生相应的科氏力，这里采用了两个质量块，做相反的正弦运动，产生两个科氏力。

w是IMU的body坐标系下的旋转在Inertial坐标系下的表示。

用两个科氏力的原因有两个，一是可以让科氏力更大，更外一个原因是y轴的一些干扰可以抵消掉。

音叉震动陀螺原理如下图：

三、IMU误差模型（有IMU的标定方法）

IMU误差模型如下：

确定性误差模型如下：

下面的这些误差其实也是bias的一些影响，比如刚上电bias有一个值，再次上电bias有另外一个值，这造成了一些确定的误差。

1、IMU的确定性误差的标定

标定的话，首先要进行加速度计的一个标定：

首先必须确定加速度计的一个公式是：

​ L = Sa + b

其中这个L是IMU的读数，S是尺度，b是偏置，我们标定需要确定的就是这两个数，具体原理就是应用L多次测量，求几个方程。

下面是第一种情况：就是假设三个轴都是垂直的情况：

如果遇见三个轴不一定垂直的情况：

之后要进行陀螺仪的一个标定：

这个真实值不再是IMU读取的，是高精度转台提供的，然后读取到真实值以后根据上述的过程一样，列个方程式就可以了。

其他因素的测定补偿标定：

如温度相关的标定补偿如下：

2、IMU随机误差的标定：

实际上我们的IMU是离散时间的采样，下面是IMU采样如何从连续到离散时间

下面介绍一种实际的随机误差的标定方法：艾伦方差标定：

首先这个是随机误差的一个标定：

艾伦方差标的是高斯白噪声和随机游走的方差的大小。

一般来说，至少采集两个小时以上的数据，然后将这里的数据按照时间段平均分段，然后球平均，计算方差，绘制曲线，具体绘制方法如下：


下面是allan方差的验证：

具体的就是制作一个仿真数据集a，然后假设一个高斯白噪声n和b的随机游走，之后就是a = a’（理论值） + n + b。

使用kalibr_allan这个标定的开源工具。

艾伦方差的验证步骤如下：

首先，加速度计的数学模型如下：**

下面这个第一个公式首先就是aG-gG这个就是惯性坐标系（ground）下面的一个加速度的表示，乘以旋转矩阵以后，变为在IMU（body）坐标系的一个表示。

这里有一个旋转矩阵的变换，所以这里注意下。

PVQ就是pose（位置），Velocity（速度），Quaternion（旋转）

VIO中的IMU模型如下：

连续时间下的IMU模型如下：

求PVQ（位移，速度，旋转）进行积分的方法有两种，第一种是欧拉法：

运动模型的离散积分——欧拉法：

下面是进行积分的第二种方法中值法：

我们这里比较关心欧拉角的变化：

下面这些是一个点绕着轴怎么旋转的方法：

旋转的基础知识如下：

这里这个是惯性坐标系下的欧拉角速度怎么旋转到body坐标系下面：

这个公式32其实就是之前的那个公式30的E：

欧拉及速度和body加速度的转换：

关于作业的一些思考：最后这个作业是使用rosbag包生成数据以后，使用allen方差标定的方法进行白噪声和随机游走误差的标定，使用这个标定好的参数进行欧拉积分和中值积分，从积分的结果来看，中值积分好于欧拉积分。