从零开始手写VIO 第5期(Lesson 2)

从零开始手写VIO 第5期(Lesson 2)

本文是VIO课程的第二节课，权当自己的总结，由于github可能比较慢，所以放在了gitee，链接参见之前的博文，欢迎大家参考，更新不易，如需转载，请著名出处，谢谢。

课程内容回顾

   第二节主要讲解了VIO中用到的IMU噪声相关的知识以及标定方法

1 作业1

设置 IMU 仿真代码中的不同的参数，生成 Allen 方差标定曲线:

作业采用kalibr_allan标定工具

1.1 参数1

gyro_bias_sigma = 0.00005 \quad rad/s^2sqrt(HZ)
acc_bias_sigma = 0.0005 \quad m/s^3sqrt(HZ)

gyro_noise_sigma = 0.015\quad rad/s^1sqrt(HZ)
acc_noise_sigma = 0.019\quad m/s^2sqrt(HZ)

加速度	角速度

1.2 参数2

gyro_bias_sigma = 0.0001\quad rad/s^2sqrt(HZ)
acc_bias_sigma = 0.005\quad m/s^3sqrt(HZ)

gyro_noise_sigma = 0.025\quad rad/s^1sqrt(HZ)
acc_noise_sigma = 0.03\quad m/s^2sqrt(HZ)

加速度	角速度

可以看到两次标定结果和设置的参数很接近

2 作业2

将 IMU 仿真代码中的欧拉积分替换成中值积分

2.1 中值积分部分代码

//上一时刻数据
MotionData imupose_pre = imudata[i - 1];
//计算均值
Eigen::Vector3d mid_omega = (imupose_pre.imu_gyro + imupose.imu_gyro) * 0.5;

//计算角度变化
Eigen::Vector3d dtheta_half = mid_omega * dt / 2.0;
Eigen::Quaterniond dq(1, dtheta_half.x(), dtheta_half.y(), dtheta_half.z());
dq.normalize();

Eigen::Quaterniond Qwb_newx = Qwb * dq;
//IMU一次积分
Eigen::Vector3d acc_w = 0.5 * (Qwb * (imupose.imu_acc) + gw + Qwb_newx * (imupose_pre.imu_acc) + gw);
Vw = Vw + acc_w * dt;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
Qwb = Qwb_newx;

2.2 结果展示

​ 将gt轨迹，欧拉积分轨迹以及中值积分轨迹画在一起，其中，蓝色为gt值，绿色为欧拉积分值，红色为中值积分值，可以看到的是，中值积分的结果更接近gt轨迹。

3 作业3

阅读从已有轨迹生成 imu 数据的论文，撰写总结推导

​ 本文提出了一种通用的视觉-惯性SLAM和标定的连续时间框架，核心在于提供了一个连续轨迹生成的方法，选择了一种形式，能够提供以下几点功能: (1)局部控制，使得系统能够在线或者捆绑批量运行,(2)$C^2$连续，从而可以预测IMU的测量，(3)估计最小弯矩的轨迹。三次样条插值在${\mathbb{R}}^3$的轨迹上是一种普遍方法，但是对于3D的旋转却不是简单，比如在$\mathbb{S}\mathbb{O}3$上进行插值，不一定能够保持$C^2$连续。所以本文采取了基于李代数的累积基函数方式去参数化一条连续的轨迹。这样的方式不仅能够提供保持$C^2$连续，而且提供了一个很简单的二阶导数公式。

3.1 B-样条累积基函数表示

​ 标准的k-1自由度的B-样条曲线的基函数可以表示为:
$$\mathbf{p}(t)=\sum _{i=0}^n{\mathbf{p}}_i{B}_{i,k}(t)\text{\hspace{1em}(1)}$$
其中${\mathbf{p}}_i\in {\mathbb{R}}^{\mathbb{N}}$是在$t_i$时刻的控制点，$i\in [0,...,n]$，$B_{i,k}(t)$是基函数，可以通过deboor-Cox递推公式来计算，所以公式$(1)$可以重写成累加的形式:
$$\mathbf{p}(t)={\mathbf{p}}_0{\tilde{B}}_{0,k}(t)+\sum _{i=0}^n({\mathbf{p}}_i-{\mathbf{p}}_{i-1}){\tilde{B}}_{i,k}(t)\text{\hspace{1em}(2)}$$
其中$B_{i,k}(t)={\sum }_{j=i}^n{B}_{j,k}(t)$是累加基函数。采用对数和指数映射的关系，可以在$\mathbb{S}\mathbb{E}3$的空间下表示$(2)$进而代替控制点的导数，$\Omega_i = log(\boldsymbol T_{w,i-1}^{-1}\boldsymbol T_{w,i})\in \mathfrak{se}3 $，然后多次指数相乘。
$${\mathbf{T}}_{w,s}(t)=exp({\tilde{B}}_{0,k}(t)log({\mathbf{T}}_{w,0}))\prod _{i=1}^{n}exp({\tilde{B}}_{i,k}(t){\Omega }_i),\text{\hspace{1em}(3)}$$
其中${\mathbf{T}}_{w,s}(t)\in \mathbb{S}\mathbb{E}3$在$t$时刻的样条插值点，${\mathbf{T}}_{w,i}(t)\in \mathbb{S}\mathbb{E}3$是控制点。$w$表示是在世界坐标系下的。

3.2 累加三次B-样条

​ 在本文中，集中讨论三次B-样条$(k=4)$的情况。假设控制点是分布在统一的时间间隔$\Delta t$上。在一个三次样条插值中，在t时刻有四个点影响B-样条曲线。定义一个集合为$[t_{i-1},t_i,t_{i+1},t_{i+2}]$，对于$t\in [t_i,t_{i+1}]$。可以进一步简化，使用统一的时间表示$s(t)=(t-t_0)/\Delta t$。这样就将控制点的时间$t_i$转换成$s_i\in [0,1,...,n]$，给定时间$s_i<=s(t)<s_{i+1}$，有$u(t)=s(t)-s(i)$；利用这种表达式，结合deboor-Cox递推公式，可以写出累积基函数$\tilde{\mathbf{B}}(u)$和它的导数$\dot{\tilde{\mathbf{B}}}(u)$，$\ddot{\tilde{\mathbf{B}}}(u)$的矩阵表达式：
$$\tilde{\mathbf{B}}(u)=\mathbf{C}\left[\begin{array}{c}1\\ u\\ u^2\\ u^3\end{array}\right],\dot{\tilde{\mathbf{B}}}(u)=\frac{1}{\Delta t}\mathbf{C}\left[\begin{array}{c}0\\ 1\\ 2u\\ 3u^2\end{array}\right],\ddot{\tilde{\mathbf{B}}}(u)=\frac{1}{\Delta t^2}\mathbf{C}\left[\begin{array}{c}0\\ 0\\ 2\\ 6u\end{array}\right],\mathbf{C}=\frac{1}{6}\left[\begin{array}{ccccccc}6& & 0& & 0& & 0\\ 5& & 3& & -3& & 1\\ 1& & 3& & 3& & -2\\ 0& & 0& & 0& & 1\end{array}\right]$$
在spline轨迹的pose可以表示为：
$${\mathbf{T}}_{w,s}(u)=T_{w,i-1}\prod _{j=1}^{3}exp(\tilde{\mathbf{B}}(u{)}_j{\Omega }_{i+j})\text{\hspace{1em}(4)}$$
其中下标$i$是从$u(t)$中获取，$\tilde{\mathbf{B}}(u{)}_j$表示$\tilde{\mathbf{B}}(u)$的第$j$个元素(从0开始)，$\Omega$就是之前定义的。并且，对任意的$u$都有，$\tilde{\mathbf{B}}(u{)}_0=1$。所以位姿的导数可以写成：
$${\dot{\mathbf{T}}}_{w,s}(u)={\mathbf{T}}_{w,i-1}({\dot{\mathbf{A}}}_0{\mathbf{A}}_1{\mathbf{A}}_2+{\mathbf{A}}_0{\dot{\mathbf{A}}}_1{\mathbf{A}}_2+{\mathbf{A}}_0{\mathbf{A}}_1{\dot{\mathbf{A}}}_2)\text{\hspace{1em}(5)}$$

$${\ddot{\mathbf{T}}}_{w,s}(u)={\mathbf{T}}_{w,i-1}({\ddot{\mathbf{A}}}_0{\mathbf{A}}_1{\mathbf{A}}_2+{\mathbf{A}}_0{\ddot{\mathbf{A}}}_1{\mathbf{A}}_2+{\mathbf{A}}_0{\mathbf{A}}_1{\ddot{\mathbf{A}}}_2+2({\dot{\mathbf{A}}}_0{\dot{\mathbf{A}}}_1{\mathbf{A}}_2+{\dot{\mathbf{A}}}_0{\mathbf{A}}_1{\dot{\mathbf{A}}}_2+{\mathbf{A}}_0{\dot{\mathbf{A}}}_1{\dot{\mathbf{A}}}_2)\text{\hspace{1em}(6)}$$

$$\begin{gathered} {\mathbf{A}}_j=exp({\Omega }_{i+j}\tilde{\mathbf{B}}(u{)}_j),{\dot{\mathbf{A}}}_j={\mathbf{A}}_j{\Omega }_{i+j}\dot{\tilde{\mathbf{B}}}(u{)}_j, \\ {\ddot{\mathbf{A}}}_j={\dot{\mathbf{A}}}_j{\Omega }_{i+j}\dot{\tilde{\mathbf{B}}}(u{)}_j+{\mathbf{A}}_j{\Omega }_{i+j}\ddot{\tilde{\mathbf{B}}}(u{)}_j \end{gathered}$$

​ 累加 B 样条参数化轨迹可以利用上式计算对时间的导数，因此可以用来计算加速度和角速度的测量：
$$Gyro(u)={\mathbf{R}}_{w,s}^T(u)\ast {\dot{\mathbf{R}}}_{w,s}(u)+bias,\text{\hspace{1em}(8)}$$

$$Accel(u)={\mathbf{R}}_{w,s}^T(u)\ast (\ddot{{\mathbf{s}}_w}(u)+g_w)+bias,\text{\hspace{1em}(9)}$$

​ 其中${\dot{\mathbf{R}}}_{w,s}(u)$和$\ddot{{\mathbf{s}}_w}(u)$分别表示$\dot {\boldsymbol T}{w,s}(u) $和$\ddot {\boldsymbol T}{w,s}(u)$的旋转、平移子矩阵，$g_w$表示世界坐标系的重力加速度。

4 Allan Variance总结

4.1 前言

陀螺仪的随即误差包括: 量化噪声，角度随机游走，零偏不稳定性，角速率随机游走，速率斜坡等。

a. 量化噪声(Q): 由AD转换过程中产生，是随机的，互不相关，是白噪声序列。

b. 角度随机游走(N): 陀螺仪输出的角速率(加速度计的线加速度)白噪声，时间积分后为角度(线速度)随机游走。

c. 零偏不稳定性(B): 零偏稳定性是指低频零偏抖动，由于内部电路随机波动引起，代表零偏随时间变化的情况。

d. 角速率随机游走(K): 相关时间较长的一种噪声，是一种起源不确定的随机过程，

e. 速率斜坡®: 漂移角速率斜坡是一种确定性系统误差,而不是随机项。

4.2 计算过程

4.2.1 采集数据

​ 首先将陀螺仪静置,采集陀螺仪的角速度序列为$\Omega$(t)，设采集到了N个数据,采样频率为。

4.2.2 选择因子m

​ 选择一个平均的时间长度为$\tau =m{\tau }_0$，其中m是平均时间因子，m的值可以随便选取，只要满足$m<(N-1)/2$。

4.2.3 分堆

​ 根据采集的数据及m的值，将数据分堆，每一堆的时间长度都为$\tau =m{\tau }_0$。这里是有重叠区域的划分，得到的结果如图所示:
这张图是参考一个文献的，具体忘记了，后续找到之后更新上来

4.2.4 计算阿伦方差

4.2.4.1 累加计算每个时刻的$\theta$角

​ 根据公式$\theta (t)={\int }^t\Omega (t^{\prime })d(t^{\prime })$来计算每个时刻对应的角，其中$ t = k \tau_0(\tau_0,2\tau_0,3\tau_0…)$，k从1到N，对于离散的数据来说，累加和也可以用来计算出N个角度值，比如有三个角速度为:10,12,15,那么有:$\theta_1 = 10 \tau_0,\theta_2 = 22 \tau_0,\theta_3 = 37 \tau_0$。

4.2.4.2 利用下式计算Allan方差

$${\sigma }^2(\theta )=\frac{1}{2{\tau }^2(N-2m)}{\Sigma }_{k=1}^{N-2m}({\theta }_{k+2m}-2{\theta }_{k+m}+{\theta }_k{)}^2$$

4.2.5 重复4.2.2-4.2.4

4.3 计算各个项方差

​ 忽略其它噪声的影响，Allan方差可近似为以上五种噪声的和，化间可得:
$${\sigma }_{total}^2(\tau )=\frac{3Q^2}{{\tau }^2}+\frac{N^2}{\tau }+B^2[\frac{2}{\pi }]ln2+\frac{K^2\tau }{3}+\frac{R^2{\tau }^2}{2}={\Sigma }_{n=-2}^2{A}_n^2{\tau }^n$$
为了求解以上5个方差，可以拟合出相应的曲线:
$${\sigma }_{total}(\tau )=\sqrt{\frac{3Q^2}{{\tau }^2}}+\sqrt{\frac{N^2}{\tau }}+\sqrt{B^2[\frac{2}{\pi }]ln2}+\sqrt{\frac{K^2\tau }{3}}+\sqrt{\frac{R^2{\tau }^2}{2}}={\Sigma }_{n=-2}^2{A}_n{\tau }^{n/2}$$
根据系数来计算对应的结果，这里有两种方式来计算。假如有m个因子,每个因子对应着一个${\tau }^{\prime }=\sqrt{\tau }$，对应的系数为C，则有:
$$\left[\begin{array}{ccccc}{\tau }_1^{\prime -2}& {\tau }_1^{\prime -1}& {\tau }_1^{\prime 0}& {\tau }_1^{\prime 1}& {\tau }_1^{\prime 2}\\ ...& ...& ...& ...& ...\\ {\tau }_m^{\prime -2}& {\tau }_m^{\prime -1}& {\tau }_m^{\prime 0}& {\tau }_m^{\prime 1}& {\tau }_m^{\prime 2}\end{array}\right]\ast \left[\begin{array}{c}{C}_1\\ C_2\\ C_3\\ C_4\\ C_5\end{array}\right]=\left[\begin{array}{c}{\sigma }_1\\ ...\\ {\sigma }_m\end{array}\right]$$

$$\begin{gathered} AX=b \\ X=(A^{T}A{)}^{-1}{A}^{T}b \end{gathered}$$

这是一个超定方程，这样就可以求出对应的系数，利用对应的系数就可以计算出上述5个方差的值了，也可采用优化的方法来求解出对应的系数，从而计算出对应的方差项。

​ 像imu_utils就用的ceres来进行优化。

4.4 单位分析

​ 根据量纲的依据,可以知道C的各项系数的单位:

​ 由于$A$的单位为$\left[\begin{array}{ccccc}{s}^{-1}& s^{-\frac{1}{2}}& s^0& s^{\frac{1}{2}}& s^1\end{array}\right]$，而$b$的单位是$deg/h$，所以$C$对应的单位是$\left[\begin{array}{ccccc}deg/h\ast s& deg/h\ast \sqrt{s}& deg/h& deg/h/\sqrt{s}& deg/h/s\end{array}\right]$。根据时间的关系可以换算成相对应的$h$或$s$的单位。

噪声类型	参数	单位
量化噪声	Q	$deg/h\ast s$
角度随机游走	N	$deg/\sqrt{h}$
零偏不稳定性	B	$deg/h$
角速率随机游走	K	$deg/h/\sqrt{h}$
速率斜坡	R	$deg/h/h$

同理可以计算出加速度计的各项误差及单位

噪声类型	参数	单位
量化噪声	Q	$m/s$
速度随机游走	N	$m/s/\sqrt{s}$
零偏不稳定性	B	$m/s^2$
加速度随机游走	K	$m/s^2/\sqrt{s}$
速率斜坡	R	$m/s^2/s$

总结

参考链接

[1] 深蓝学院SLAM/VIO课程
[2] Allan variance文献(待确认)