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

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

1 坐标系

2 轮速计

2.1 运动学模型

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

  左右轮速的速度我们是可以计算得到的，但是半径R一开始肯定未知，所以利用下面公式计算，其中W是轮距，H是轴距！
$$V_X=\frac{V_L+V_R}{2},\omega =\frac{V_R-V_L}{W}$$

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

2.2 外参

  与相机的姿态变换外参
$${}_O^{C}R{\text{ 与平移 }}^C{p}_O$$

3 状态和协方差矩阵

3.1 状态

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

3.2 协方差矩阵

  里程计状态（姿态）协方差矩阵维度6*6，相机状态协方差矩阵维度6N*6N
$$P_{k:k}=\left[\begin{array}{cc}{P}_{O{O}_{k:k}}& P_{O{C}_{k:k}}\\ P_{O{C}_{k:k}}^T& P_{C{C}_{k:k}}\end{array}\right]$$

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

其中，${\mathbf{P}}_{O{O}_{k+1|k}}=\Phi P_{O{O}_{k:k}}{\Phi }^T+FQ{F}^T$
$${\mathbf{P}}_{k+1|k}=(\begin{array}{cc}{\mathbf{P}}_{O{O}_{k+1|k}}& {\mathbf{\Phi }}_k{\mathbf{P}}_{O{C}_{k|k}}\\ {\mathbf{P}}_{O{C}_{k|k}}^{\top }{\mathbf{\Phi }}_k^{\top }& {\mathbf{P}}_{C{C}_{k|k}}\end{array})$$

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

$$\begin{array}{c}{}_C^{G}R={}_O^{G}R{}_C^{O}R\\ {}^G{p}_C={}^G{p}_O+{}_O^{G}R{}^O{p}_C\end{array}$$

$$\begin{array}{c}{P}^{(6+6(N+1))\times (6+6(N+1))}=[\begin{array}{c}{I}_{6+6N}\\ J_{6\times (6+6N)}\end{array}]P^{(6+6N)\times (6+6N)}{\left[\begin{array}{c}{I}_{6+6N}\\ J_{6\times (6+6N)}\end{array}\right]}^T\\ =[\begin{array}{cc}P& P{J}^T\\ JP& JP{J}^T\end{array}]\end{array}$$

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

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

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

推导过程如下

4 Wheel Propagation

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

4.1 连续运动学

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

$$\begin{array}{rl}\dot{{}_O^{G}R}& {=}_O^{G}R{[}^O{\omega }_O{]}_{\times }\\ {}^G\dot{p_O}& ={}^G{v}_O={}_O^G{R}^O{v}_O\end{array}$$

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

4.2 离散积分

4.2.1 状态均值递推

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

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

$$\begin{array}{c}{}_O^G{R}_{k+1}={}_O^G{R}_k\underset{\Delta R}{\underbrace{\mathrm{Exp}({}^O{\omega }_O\Delta t)}}\\ {}^G{p}_{O_{k+1}}={}^G{p}_{Ok}+{}_O^G{R}_k\underset{\Delta p}{\underbrace{{}^O{v}_O\Delta t}}\end{array}$$

4.2.2 协方差递推

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

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

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

$$\Phi =\left[\begin{array}{cc}\mathrm{Exp}{(}^O{\omega }_O\Delta t{)}^T& 0\\ {-}_O^G{R}_k{[}^O{v}_O\Delta t{]}_{\times }& I\end{array}\right]$$

$$G^{\prime }=[\begin{array}{cc}I& 0\\ 0& {}_O^G{R}_k\end{array}]$$

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

$${\mathbf{P}}_{O{O}_{k+1|k}}=\Phi P_{O{O}_{k:k}}{\Phi }^T+G^{\prime }Q{G}^{\prime T}$$

5 Visual update

5.1 视觉残差与雅可比

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

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

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

这里更详细的推导，可以参考之前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{array}{rl}{r}_0^{(2M-3M_L)\times 1}& ={\mathrm{A}}^T{r}^{2M\times 1}\cong \underset{H_0}{\underbrace{{\mathrm{A}}^T{H}_X^{2M\times (15+6N)}}}{\tilde{X}}^{(15+6N)\times 1}+\underset{n_0}{\underbrace{{\mathrm{A}}^T{n}^{2M\times 1}}}\\ & ={H_0}^{(2M-3)\times (15+6N)}{\tilde{X}}^{(15+6N)\times 1}+{n_0}^{(2M-3)\times 1}\end{array}$$

5.3 降低计算量

QR分解降低计算量

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

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

5.4 边缘化

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

6 平面约束Update

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

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

6.1 平面约束本质

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

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

6.2 如何进行平面约束

  平面约束是Odometry的坐标的X-O-Y面与一个平面重合。我们这里设定平面 {Π} 为初始时刻Odometry坐标系的X-O-Y平面，即世界系。

$$0_{2\times 1}=\Lambda \underset{{}_O^{\pi }R}{\underbrace{{}_G^{\pi }{R}_O^{G}R}}{e}_3={\Lambda }_O^{G}R{e}_3$$

  其中，$\Lambda$是为了将一个三维列向量取出前面2*1向量，$e_3$就是Odometry坐标系z下轴单位向量，投影到世界系后，取出前面二维，应该是一个0向量！

$$\Lambda =\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right],e_3=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$$

本文参考博客