视觉轮速滤波融合1讲：理论推导

本文主要是介绍视觉轮速滤波融合1讲：理论推导，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

视觉轮速滤波融合理论推导

文章目录

视觉轮速滤波融合理论推导
1 坐标系
2 轮速计
- 2.1 运动学模型
- 2.2 外参
3 状态和协方差矩阵
- 3.1 状态
- 3.2 协方差矩阵
4 Wheel Propagation
- 4.1 连续运动学
- 4.2 离散积分
- - 4.2.1 状态均值递推
  - 4.2.2 协方差递推
5 Visual update
- 5.1 视觉残差与雅可比
- 5.2 左零空间投影
- 5.3 降低计算量
- 5.4 边缘化
6 平面约束Update
- 6.1 平面约束本质
- 6.2 如何进行平面约束

1 坐标系

在这里插入图片描述

2 轮速计

2.1 运动学模型

这里介绍下轮速计的一个运动学模型，以阿克曼为例。如果一个小车，它沿着直线运动，那么它的左右轮速相等。如果不相等，意味着发生了旋转：
$V_{X}=\frac{V_{L}+V_{R}}{2}，ω = V_{X}/R$

在这里插入图片描述

左右轮速的速度我们是可以计算得到的，但是半径R一开始肯定未知，所以利用下面公式计算，其中W是轮距，H是轴距！
$V_{X}=\frac{V_{L}+V_{R}}{2},ω =\frac{V_{R}-V_{L}}{W}$
在这里插入图片描述

有了角速度和线速度，就可以计算出旋转半径R，然后利用轴距、轮距、R来计算左右前轮的偏角（θ=ωt）
$\begin{aligned}Angle_L&=\tan^{-1}\big(\frac{H}{R-0.5W}\big)\\\\Angle_R&=\tan^{-1}\big(\frac{H}{R+0.5W}\big)\end{aligned}$

2.2 外参

与相机的姿态变换外参
$_O^CR\text{ 与平移 }^Cp_O$

3 状态和协方差矩阵

3.1 状态

类似于MSCKF，状态由两部分组成，一个是Odometry的位姿：
$_O^GT=\{_O^GR,^Gp_O\}\in\text{SE}3$
另一个是相机位姿（多帧）：
$_C^GT=\{_C^GR,^Gp_C\}\in\text{SE}3$
滑窗内状态变量即当前时刻Odometry位姿+N帧的相机位姿
$KaTeX parse error: Got group of unknown type: 'internal'$

3.2 协方差矩阵

里程计状态（姿态）协方差矩阵维度6*6，相机状态协方差矩阵维度6N*6N
$P_{k:k}=\begin{bmatrix}P_{OO_{k:k}}&P_{OC_{k:k}}\\P_{OC_{k:k}}^T&P_{CC_{k:k}}\end{bmatrix}$

1 卡尔曼预测阶段更新里程计状态协方差，同时更新里程计与相机交叉部分状态协方差

其中， $\mathbf{P}_{OO_{k+1|k}} = \Phi P_{OO_{k:k}}\Phi^T+FQF^T$
$\left.\mathbf{P}_{k+1|k}=\left(\begin{matrix}\mathbf{P}_{OO_{k+1|k}}&\mathbf{\Phi}_{k}\mathbf{P}_{OC_{k|k}}\\\mathbf{P}_{OC_{k|k}}^\top\mathbf{\Phi}_{k}^\top&\mathbf{P}_{CC_{k|k}}\end{matrix}\right.\right)$

2 状态增广：当新的frame到来，利用里程计位姿，通过外参预测相机位姿，更新 $P_{k:k}$ , 整体维度（6+6N + 6）*（6+6N + 6）

$\begin{array}{c}_C^GR={}_O^GR{}_C^OR\\^Gp_C={}^Gp_O+{}_O^GR{}^Op_C\end{array}$

$\begin{gathered} P^{(6+6(N+1))\times(6+6(N+1))} \left.=\left[\begin{array}{c}I_{6+6N}\\J_{6\times(6+6N)}\end{array}\right.\right]P^{(6+6N)\times(6+6N)}\left[\begin{array}{c}I_{6+6N}\\J_{6\times(6+6N)}\end{array}\right]^T \\ \left.=\left[\begin{array}{cc}P&PJ^T\\JP&JPJ^T\end{array}\right.\right] \end{gathered}$

其中，雅可比是相机姿态对状态扰动量的雅可比，因为odometry预测相机姿态，所以和之前N帧相机无关，导数为0！
$J=\begin{bmatrix}^O_CR^T&0_{3\times3}&0_{6N\times6N}\\-_O^GR{\left[^Op_C\right]}_\times&I&0_{6N\times6N}\end{bmatrix}$

重点补充下第一列的雅可比求解推导（要搞清楚是左/右扰动）

1 平移 $^Gp_C$ 对 $_O^GR$ 的雅可比，注意是右扰动！若按左扰动求导，结果应该是 $-{\left[_O^GR^Op_C\right]}$

推导过程如下

在这里插入图片描述

4 Wheel Propagation

论文Localization for Ground Robots: On Manifold Representation, Integration

4.1 连续运动学

VIO里面使用IMU去做预测这部分，这里是利用轮速计去做propagation。

$\begin{aligned}\dot{_O^GR}&= ^G_OR[^O\omega_O]_\times\\^G\dot{p_O}&={}^Gv_O={}_O^GR^Ov_O\end{aligned}$

其中 $^O\omega_O$ 是轮速坐标系下的顺时角速度，等同IMU测量的角速度。但是因为轮速计只有2D方面的旋转，一般只有偏航角，即只能测到绕z轴的角速度。下面式子中b是轮距， $k_l,k_r$ 分别是左右轮速系数。
$\left.^O\omega_O=\left[\begin{array}{c}0\\0\\\frac{k_r\cdot v_r-k_l\cdot v_l}b\end{array}\right.\right]$
$^Ov_O$ 是瞬时速度，轮速本身只能测量沿x轴方向的速度（在世界系并不是）。
$\left.^Ov_O=\left[\begin{array}{c}\frac{k_l\cdot v_l+k_r\cdot v_r}{2}\\0\\0\end{array}\right.\right]$

4.2 离散积分

4.2.1 状态均值递推

对上面得到的常微分方程进行离散化，使用欧拉积分，认为dt时间内变量未改变，根据定积分公式，以平移p为例，可以得到下式：

常见的一种运动学积分公式（欧拉积分）

$\begin{gathered} _{O}^{G}R_{k+1} ={}_O^GR_k\underbrace{\mathrm{Exp}({}^O\omega_O\Delta t)}_{\Delta R} \\ ^Gp_{O_{k+1}} ={}^Gp_{Ok}+{}_O^GR_k\underbrace{^Ov_O\Delta t}_{\Delta p} \end{gathered}$

4.2.2 协方差递推

对于协方差的Propgation，我们先求雅克比。

这里先引出论文中的公式，貌似顺序写反了，δp在前，δθ在后，更多详细推导可以参考论文

在这里插入图片描述

我们这里先不管后面那个m，因为这个递推公式是连续的，所以我们把它离散化。在MSCKF中，利用泰勒展开去近似Fc得到状态转移矩阵Φ，简单起见，用一阶泰勒展开δx' = (I + Fc*dt)δx + G,Q是噪声矩阵。下图展示了状态转移矩阵的推导。

在这里插入图片描述
$\Phi=\begin{bmatrix}\operatorname{Exp}(^O\omega_O\Delta t)^T&0\\-_O^GR_k[^Ov_O\Delta t]_\times&I\end{bmatrix}$

$\left.G'=\left[\begin{array}{cc}I&0\\0&{}_{O}^{G}R_{k}\end{array}\right.\right]$

卡尔曼预测阶段协方差更新

$\mathbf{P}_{OO_{k+1|k}} = \Phi P_{OO_{k:k}}\Phi^T+G'QG'^T$

5 Visual update

5.1 视觉残差与雅可比

MSCKF的边缘化操作：没有把特征点放到状态向量里面，降低了计算量。

当特征点丢失的时候，才拿来进行更新。特征点丢失有两种情况：

一种是跟踪丢失，也就是当前帧跟踪不到的那些特征点；
另一种是边缘化的时候，也就是把最后一帧滑出窗口的时候，把在这一帧里面新建的特征点都丢弃，也就是都拿来做更新。

这里更详细的推导，可以参考之前MSCKF的博客

$\mathbf{r}_i^j=\mathbf{z}_i^j-\hat{\mathbf{z}}_i^j=\mathbf{H}_{C_i}^j\tilde{\mathbf{x}}_{C_i}+\mathbf{H}_{f_i}^j{}^G\tilde{\mathbf{p}}_j+\mathbf{n}_i^j$

5.2 左零空间投影

左零空间投影—QR分解

$\begin{aligned}r_0^{(2M-3M_L)\times1}&=\mathrm{A}^Tr^{2M\times1}\cong\underbrace{\mathrm{A}^TH_X^{2M\times(15+6N)}}_{H_0}\tilde{X}^{(15+6N)\times1}+\underbrace{\mathrm{A}^Tn^{2M\times1}}_{n_0}\\&={H_0}^{(2M-3)\times(15+6N)}\tilde{X}^{(15+6N)\times1}+{n_0}^{(2M-3)\times1}\end{aligned}$

5.3 降低计算量

QR分解降低计算量

投影完之后，我们就可以得到新的残差和相应的雅可比矩阵 $H_{0}$ 。但是 $H_{0}$ 矩阵通常维度很大，为了降低计算量，对 $H_{0}$ 进行QR分解：

QR分解会得到一个正交矩阵和一个上三角矩阵！正交矩阵的转置和其本身的乘积是单位矩阵，且该矩阵的所有列向量彼此正交（内积为0）
${H_0}^{(2M-3)\times(15+6N)}=[Q_1\quad Q_2]\begin{bmatrix}T_H\\0\end{bmatrix}$
对上面新的残差左边乘以 $\left.\left[\begin{array}{cc}\mathbf{Q}_1^T\\\mathbf{Q}_2^T\end{array}\right.\right]$ ,得到
$\begin{bmatrix}Q_1^Tr_0\\Q_2^Tr_0\end{bmatrix}=\begin{bmatrix}T_H\\0\end{bmatrix}\tilde{X}+\begin{bmatrix}Q_1^Tn_0\\Q_2^Tn_0\end{bmatrix}$

5.4 边缘化

边缘化，就是说如何删除滑窗里的状态，这里使用最简单的策略，把最老的一帧去掉，删掉协方差矩阵中对应块，去掉的这帧里的所有特征点都被拿来做更新。

6 平面约束Update

参考论文：VINS on wheels," 2017 (ICRA)

当车辆在在平面上运动时，在更新的时候，我们引入一个平面约束.

6.1 平面约束本质

为了解释这个问题，首先解释下旋转矩阵的含义，更多请参考之前博客

刚体坐标系B相对于世界坐标系A的的姿态—旋转矩阵。分别把B下的三个轴分别投影到世界坐标系下的三个轴，下面矩阵给出了对应的投影分量。
$}^A _B{R}=\left[\begin{array}{ccc} \mid & \mid & \mid \\ { }^A \widehat{X}_B & { }^A \widehat{Y}_B & { }^A \hat{Z}_B \\ \mid & \mid & \mid \end{array}\right]=\left[\begin{array}{ccc} \hat{X}_B \cdot \hat{X}_A & \hat{Y}_B \cdot \hat{X}_A & \hat{Z}_B \cdot \hat{X}_A \\ \hat{X}_B \cdot \hat{Y}_A & \hat{Y}_B \cdot \hat{Y}_A & \hat{Z}_B \cdot \hat{Y}_A \\ \hat{X}_B \cdot \hat{Z}_A & \hat{Y}_B \cdot \hat{Z}_A & \hat{Z}_B \cdot \hat{Z}_A \end{array}\right]$
也就是说，矩阵 $^A_BR$ 的第3列就是坐标系B的z轴在A系下的投影。对于 $^G_OR$ 来讲，前面定义世界系与初始轮速坐标系重合，轮速只有绕z轴的旋转，所以坐标系{G}和坐标系{O}的z轴是平行的。平面约束的本质就是让 $^G_OR$ 的右上角2*1向量为0。