VINS是由香港科技大学秦通等人,于2018年提出的一种强大且通用的单目视觉惯性状态估计器,是初学者接触VIO 比较好的开源学习资料,作者的论文条理清晰,分为五个主要部分对该系统展开描述:测量预处理、初始化、后端非线性优化、闭环检测、闭环优化。本节介绍IMU预积分公式推导。

VINS公式推导:IMU预积分公式推导

符号约定

我们将 $(·)^w$ 视为世界坐标系。重力方向与世界坐标系的 $z$ 轴对齐。 $(·)^b$ 是body 坐标系,我们定义它和IMU 坐标系一样。$(·)^c$ 是相机坐标系。

我们使用旋转矩阵 $R$ 和 Hamilton 四元数 $q$ 来表示旋转。我们主要在状态向量中使用四元数,但旋转矩阵也用于方便地旋转三维向量。 $q^w_b , p^w_b$ 是从本体坐标系到世界坐标系的旋转和平移。$b_k$ 是拍摄第 $k$ 张图像时的本体坐标系。 $c_k$ 是拍摄第 $k$ 张图像时的相机帧。 $\otimes$ 表示两个四元数之间的乘法运算。$g^w = [0, 0, g]^T$ 是世界坐标系中的重力矢量。最后,我们将 $\hat{(·)}$ 表示为某个量的噪声测量或估计。

IMU 噪声模型

IMU 的原始陀螺仪和加速度计测量值 $\hat{w}$ 和 $\hat{a}$ 由下式给出:

$$ \begin{align*} \hat{a}_t&=a_t+b_{a_t}+R^t_wg^w+n_a\\ \hat{w}_t&=w_t+b_{w_t}+n_w \end{align*}\tag{1} $$

我们假设加速度和陀螺仪测量中的附加噪声是高斯噪声,$n_a\sim\mathcal{N}(0,\sigma^2_a),n_w\sim\mathcal{N}(0,\sigma^2_w)$。加速度偏差和陀螺仪偏差被建模为随机游走,其导数为高斯,$n_{b_a}\sim\mathcal{N}(0, \sigma^2_{b_a}),n_{b_w}\sim\mathcal{N}(0, \sigma^2_{b_w})$:

$$ \dot{b}_{a_t}=n_{b_a},\ \ \ \ \ \ \dot{b}_{w_t}=n_{b_w}\tag{2} $$

Ⅰ. 当前时刻 PVQ

A.连续形式

image-20220520195515606

将第 $k$ 帧和第 $k+1$ 帧之间的所有 IMU 进行积分,可得第 $k+1$ 帧的位置、速度和旋转
( PVQ),作为视觉估计的初始值,这里的旋转采用的四元数。

$$ \begin{align*} \mathbf{p}^w_{b_{k+1} }&=\mathbf{p}^w_{b_k}+\mathbf{v}^w_{b_k}\Delta t_k+\iint_{t\in[t_k, t_{k+1}]}(\mathbf{R}^w_t(\hat{a}_t-b_{a_t}-n_a)-g^w)dt^2\\ \mathbf{v}^w_{b_{k+1} }&=\mathbf{v}^w_{b_k}+\int_{t\in[t_k, t_{k+1}]}(\mathbf{R}^w_t(\hat{a}_t-b_{a_t}-n_a)-g^w)dt\\ \mathbf{q}^w_{b_{k+1} }&=\mathbf{q}^w_{b_k}\otimes\int_{t\in[t_k, t_{k+1}]} \frac{1}{2}\Omega(\hat{w}_t-b_{w_t}-n_w)\mathbf{q}^{b_k}_tdt \end{align*}\tag{3} $$

其中 $\hat{w}$ 和 $\hat{a}$ 分别是 IMU 的原始陀螺仪和加速度计测量值,$\Delta t_k$ 是时间间隔 $[t_k, t_{k+1}]$ 之间的持续时间。且

$$ \Omega(w)= \begin{bmatrix} -w^{\land}&w\\-w^T&0 \end{bmatrix}, w^{\land}=\begin{bmatrix} 0&-w_z&w_y\\ w_z&0&-w_x\\ -w_y&w_x&0 \end{bmatrix}\tag{4} $$

其中,$w=\hat{w}_t-b_{w_t}-n_w$

对于基于四元数的IMU积分,这里做进一步推导,

公式(1)的 IMU 连续形式下的旋转状态推导如下,首先可写成:

$$ \mathbf{q}^w_{b_{k+1} }=\mathbf{q}^w_{b_{k} }\otimes\int_{t\in[k,k+1]}\dot{\mathbf{q}}_tdt\tag{IA-1} $$

设四元数 $\mathbf{q}=[x,y,z,s]$,由四元数的左乘右乘:

$$ \begin{align*} \mathbf{q}_a\otimes\mathbf{q}_b &=\mathcal{R}(\mathbf{q}_b)\otimes\mathbf{q}_a= \begin{bmatrix} s_b&z_b&-y_b&x_b\\ -z_b&s_b&x_b&y_b\\ y_b&-x_b&s_b&z_b\\ -x_b&-y_b&-z_b&s_b\\ \end{bmatrix} \begin{bmatrix}x_a\\y_a\\z_a\\s_a\end{bmatrix} \\ &=\mathcal{L}(\mathbf{q}_a)\otimes\mathbf{q}_b= \begin{bmatrix} s_a&-z_a&y_a&x_a\\ z_a&s_a&-x_a&y_a\\ -y_a&x_a&s_a&z_a\\ -x_a&-y_a&-z_a&s_a\\ \end{bmatrix} \begin{bmatrix}x_b\\y_b\\z_b\\s_b\end{bmatrix} \end{align*}\tag{IA-2} $$

为了简化以上的形式,我们设四元数 $\mathbf{q}=[x,y,z,s]$为 $\mathbf{q}=[w,s]$,则:

$$ \begin{align*} \mathcal{R}(\mathbf{q}_a)&=\Omega(w)+sI_{4\times4} =\begin{bmatrix}-w^{\land}&w\\-w^T&0\end{bmatrix}+sI_{4\times4}\\ \mathcal{L}(\mathbf{q}_a)&=\Psi(w)+sI_{4\times4} =\begin{bmatrix}w^{\land}&w\\-w^T&0\end{bmatrix}+sI_{4\times4} \end{align*}\tag{IA-3} $$

因此,可以推导四元数的导数

$$ \begin{align*} \dot{\mathbf{q} }_t &=\underset{\delta t}{\lim}\frac{1}{\delta t} (\mathbf{q}_{t+\delta t}-\mathbf{q}_t)\\ &=\underset{\delta t}{\lim}\frac{1}{\delta t} (\mathbf{q}_t\otimes\mathbf{q}^t_{t+\delta t}-\mathbf{q}_t\otimes \begin{bmatrix}0\\1\end{bmatrix})\\ &=\underset{\delta t}{\lim}\frac{1}{\delta t} \left( \mathbf{q}_t\otimes\begin{bmatrix}\mathbf{n}\sin\frac{\theta}{2}\\\cos \frac{\theta}{2}\end{bmatrix}-\mathbf{q}_t\otimes \begin{bmatrix}0\\1\end{bmatrix} \right)\\ &\approx\underset{\delta t}{\lim}\frac{1}{\delta t} \left( \mathbf{q}_t\otimes\begin{bmatrix}\mathbf{n}\frac{\theta}{2}\\1\end{bmatrix}-\mathbf{q}_t\otimes \begin{bmatrix}0\\1\end{bmatrix} \right)\\ &=\underset{\delta t}{\lim}\frac{1}{\delta t} \left[ \mathcal{R}\left(\begin{bmatrix}\mathbf{n}\frac{\theta}{2}\\1\end{bmatrix}\right)- \mathcal{R}\left(\begin{bmatrix}0\\1\end{bmatrix}\right) \right]\mathbf{q}_t\\ &=\underset{\delta t}{\lim}\frac{1}{\delta t} \begin{bmatrix} -\frac{\theta\mathbf{n}}{2}^{\land} &\frac{\theta\mathbf{n}}{2}\\ -\frac{\theta\mathbf{n}}{2}^T&0 \end{bmatrix}\mathbf{q}_t \end{align*}\tag{IA-4} $$

其中,$\mathbf{n}$ 表示旋转轴,而角速度定义有:$w=\underset{\delta t}{\lim}\frac{\mathbf{n}\theta}{\delta t}$,则有:

$$ \dot{\mathbf{q} }_t =\frac{1}{2}\begin{bmatrix}-w^{\land}&w\\-w^T&0\end{bmatrix}\mathbf{q}_t =\frac{1}{2}\Omega(w)\mathbf{q}_t =\frac{1}{2}\mathcal{R}\left(\begin{bmatrix}w\\0\end{bmatrix}\right)\mathbf{q}_t =\frac{1}{2}\mathbf{q}_t\otimes\begin{bmatrix}w\\0\end{bmatrix}\tag{IA-5} $$

B.中值法离散形式

公式 $(1)$ 给出的是连续时刻的相机当前 PVQ 的迭代公式,实际上,IMU在工作的时候是离散采集数据,所以代码中使用的也是离散形式,为了跟代码一致,下面给出基于中值法的公式,(还有基于欧拉法的公式,但是代码中使用的是前者), 即从第 $i$ 个 IMU 时刻到第 $i+1$ 个 IMU 时刻的积分过程, 这与 Estimator::processIMU() 函数中的 Ps[j]Rs[j]Vs[j] 是一致的,(代码中的 $j$ 时刻即为此处的 $i+1$),IMU 积分出来的第 $j$ 时刻的物理量可以作为第 $j$ 帧图像的初始值。

$$ \begin{align*} \mathbf{p}^w_{b_{i+1} }&=\mathbf{p}^w_{b_i}+\mathbf{v}^w_{b_i}\delta t+\frac{1}{2}\overline{\hat{a} }_i\delta t^2\\ \mathbf{v}^w_{b_{i+1} }&=\mathbf{v}^w_{b_i}+\overline{\hat{a} }_i\delta t\\ \mathbf{q}^w_{b_{i+1} }&=\mathbf{q}^w_{b_i}\otimes \begin{bmatrix} 1\\\frac{1}{2}\overline{\hat{w} }_i\delta t \end{bmatrix} \end{align*}\tag{5} $$

其中使用 mid-point 方法,即两个相邻时刻 $i$ 到 $j$ 的位姿是用两个时刻的测量值 $ a,w $ 的平均值来计算:

$$ \begin{align*} \overline{\hat{a} }_i&=\frac{1}{2} [\mathbf{q}_i(\hat{a}_i-b_{a_i})-g^w+\mathbf{q}_{i+1}(\hat{a}_{i+1}-b_{a_i})-g^w]\\ \overline{\hat{w} }_i&=\frac{1}{2}(\hat{w}_i+\hat{w}_{i+1})-b_{w_i} \end{align*}\tag{6} $$


在滤波问题中,存在马尔科夫假设,即当前时刻的状态与上一时刻有关,与上一时刻之前的状态都没有关系,(当然,好像也存在多级的马尔科夫,网上有人提到MSCKF,它包含之前的历史状态的信息的,存在协方差 P 矩阵里。),因 此,我们使用上述积分形式,即可根据 $k$ 时刻推出 $k+1$ 时刻的PVQ,然后依次进行,就可以得出之后每一个时刻的PVQ,这里并没有使用预积分,只进行了一次近似,找到了一个fixed point,就是一个简单的线性递推过程。

image-20220709112001381

但是在优化问题中,则是一个回头看的过程,当前时刻的状态与之前所有时刻(关键帧)都有关系,优化问题会进行多次迭代,每次迭后,会把上一时刻迭代的结果作为新的fixed point,比如在优化问题再,IMU用的是帧间约束$\Delta P_{12}$,保证相邻状态量的误差不大的离谱。并且,优化问题往往由剔除外点的机制,判断数据是否误差过大,误差过大则剔除。

image-20220709112011884

显然,上面的积分形式,就是一个线性递推的过程,不存在帧间约束,因此如果需要利用帧间约束,还需要对上面的公式做进一步处理。

Ⅱ. 两帧之间 PVQ

A.连续形式

一方面,为了建立帧间约束,我们需要把公式 $(3)$ 右侧积分中的 $\mathbf{R}_{b_k}^{w}$ 转换为帧间约束(公式 $(3)$ 中都是相对世界坐标系的,不构成帧间的约束);另一方面,观察公式 $(3)$ 可知, IMU 的预积分需要依赖与第 $k$ 帧的 $\mathbf{v}$ 和 $\mathbf{R}$ ,当我们在后端进行非线性优化时,需要迭代更新第 $k$ 帧的 $\mathbf{v}$ 和 $\mathbf{R}$ ,这将导致我们需要根据每次迭代后值重新进行积分,这将非常耗时。

因此,我们考虑将优化变量从第 $k$ 帧到第 $k+1$ 帧的 IMU 预积分项中分离开来, 通过对公式 $(3)$ 左右两侧各乘 $\mathbf{R}^{b_k}_{w}$,可化简为:

$$ \begin{align*} \mathbf{R}^{b_k}_w\mathbf{p}^w_{b_{k+1} }&=\mathbf{R}^{b_k}_w(\mathbf{p}^w_{b_k}+\mathbf{v}^w_{b_k}\Delta t_k- \frac{1}{2}g^w\Delta t_k^2)+\alpha^{b_k}_{b_{k+1} }\\ \mathbf{R}^{b_k}_w\mathbf{v}^w_{b_{k+1} }&= \mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_k}-g^w\Delta t_k)+\beta^{b_k}_{b_{k+1} }\\ \mathbf{q}^{b_k}_w\otimes\mathbf{q}^w_{b_{k+1} }&=\gamma^{b_k}_{b_{k+1} } \end{align*}\tag{7} $$

其中,

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&=\iint_{t\in[t_k, t_{k+1}]}\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t}-n_a)dt^2\\ \beta ^{b_k}_{b_{k+1} }&=\int_{t\in[t_k, t_{k+1}]}\mathbf{R}^{b_k}_t(\hat{a}_t-b_{a_t}-n_a)dt\\ \gamma^{b_k}_{b_{k+1} }&= \int_{t\in[t_k, t_{k+1}]} \frac{1}{2}\Omega(\hat{w}_t-b_{w_t}-n_w)\gamma^{b_k}_tdt \end{align*}\tag{8} $$

可以看出,预积分项 $(8)$ 可以仅通过以 $b_k$ (表示第 $k$ 帧时的本体坐标系) 作为参考系的 IMU 测量来获得。

$\alpha^{b_k}_{b_{k+1} },\beta^{b_k}_{b_{k+1} },\gamma^{b_k}_{b_{k+1} }$ 与IMU的 $bais$ 有关,而 $bais$ 也是我们需要优化的变量,这将导致的问题是,当每次迭代时,我们得到一个新的 $bais$ ,还需要根据公式 $(8)$ 重新对第 $k$ 帧和第 $k+1$ 帧之间的 IMU 预积分,非常耗时。

当 $bais$ 的估计发生变化时,如果变化很小,我们通过它们对 $bais$ 的一阶近似来调整 $\alpha^{b_k}_{b_{k+1} },\beta^{b_k}_{b_{k+1} },\gamma^{b_k}_{b_{k+1} }$ ,否则我们进行重新传播。这种策略为基于优化的算法节省了大量的计算资源,因为我们不需要重复传播 IMU 测量。一阶近似如下:

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&\approx \hat{\alpha}^{b_k}_{b_{k+1} }+\mathbf{J}^{\alpha}_{b_a}\delta b_{a_k}+\mathbf{J}^{\alpha}_{b_w}\delta b_{w_k} \\ \beta^{b_k}_{b_{k+1} }&\approx \hat{\beta}^{b_k}_{b_{k+1} }+\mathbf{J}^{\beta}_{b_a}\delta b_{a_k}+\mathbf{J}^{\beta}_{b_w}\delta b_{w_k} \\ \gamma^{b_k}_{b_{k+1} }&\approx \hat{\gamma}^{b_k}_{b_{k+1} }\otimes \begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} \end{align*}\tag{9} $$

其中 $\mathbf{J}^{\alpha}_{b_a}$ 和 是 $\mathbf{J}^{\alpha}_{b_{k+1} }$ 中的子块矩阵,其位置对应于 $\frac{\delta\alpha^{b_k}_{b_{k+1} } }{\delta b_{a_k} }$ 。 $\mathbf{J}^{\alpha}_{b_w}、\mathbf{J}^{\beta}_{b_a}、\mathbf{J}^{\beta}_{b_w}、\mathbf{J}^{\gamma}_{b_w}$ 也使用相同的含义。

B.离散形式

欧拉法

下面给出离散时刻的 IMU 预积分公式, 首先按照论文中采用的欧拉法, 给出第 $i$ 个 IMU
时刻与第 $i+1$ 个 IMU 时刻的变量关系为:

$$ \begin{align*} \hat{\alpha}^{b_k}_{i+1}&=\hat{\alpha}^{b_k}_{i}+\hat{\beta}^{b_k}_{i}\delta t+\frac{1}{2}\overline{\hat{a} }_i^{\prime}\delta t^2\\ \hat{\beta}^{b_k}_{i+1}&=\hat{\beta}^{b_k}_{i}+\overline{\hat{a} }_i^{\prime}\delta t\\ \hat{\gamma}^{b_k}_{i+1}&= \hat{\gamma}^{b_k}_{i}\otimes\hat{\gamma}^{i}_{i+1} =\hat{\gamma}^{b_k}_{i}\otimes \begin{bmatrix}1\\\frac{1}{2}\overline{\hat{w} }_i^{\prime}\delta t\end{bmatrix} \\ \end{align*}\tag{10} $$

使用欧拉法,即两个相邻时刻 $i$ 到 $i+1$的位姿是用第 $i$ 时刻的测量值 $\overline{\hat{a} }_i^{\prime}, \overline{\hat{w} }_i^{\prime}$ 来计算:

$$ \begin{align*} \overline{\hat{a} }_i^{\prime}&=\mathbf{R}(\hat{\gamma}^{b_k}_{i})(\hat{a}_i-b_{a_i})\\ \overline{\hat{w} }_i^{\prime}&=\hat{w}_i-b_{w_i} \end{align*}\tag{11} $$

中值法

下面给出代码中采用的基于中值法的 IMU 预积分公式,这与 Estimator::processIMU()
数中的 IntegrationBase::push_back()上是一致的。注意这里跟公式 $(5)$ 是不一样的,这里积分出来的是前后两帧之间的 IMU 增量信息,而公式 $(5)$ 给出的当前帧时刻的物理量信息。

$$ \begin{align*} \hat{\alpha}^{b_k}_{i+1}&=\hat{\alpha}^{b_k}_{i}+\hat{\beta}^{b_k}_{i}\delta t+\frac{1}{2}\overline{\hat{a} }_i^{\prime} \delta t^2\\ \hat{\beta}^{b_k}_{i+1}&=\hat{\beta}^{b_k}_{i}+\overline{\hat{a} }_i^{\prime}\delta t\\ \hat{\gamma}^{b_k}_{i+1}&= \hat{\gamma}^{b_k}_{i}\otimes\hat{\gamma}^{i}_{i+1} =\hat{\gamma}^{b_k}_{i}\otimes \begin{bmatrix}1\\\frac{1}{2}\overline{\hat{w} }_i^{\prime}\delta t\end{bmatrix} \\ \end{align*}\tag{12} $$

其中,

$$ \begin{align*} \overline{\hat{a} }_i^{\prime}&=\frac{1}{2} [\mathbf{q}_i(\hat{a}_i-b_{a_i})+\mathbf{q}_{i+1}(\hat{a}_{i+1}-b_{a_i})]\\ \overline{\hat{w} }_i^{\prime}&=\frac{1}{2}(\hat{w}_i+\hat{w}_{i+1})-b_{w_i} \end{align*}\tag{13} $$

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/64.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 07 月 09 日
赏杯咖啡