VIO残差函数的构建以及IMU预积分和协方差传递

最新推荐文章于 2022-08-15 18:16:28 发布

秃头队长

最新推荐文章于 2022-08-15 18:16:28 发布

阅读量1.8k

点赞数 3

分类专栏： SLAM 文章标签： VIO 残差

本文链接：https://blog.csdn.net/qq_39266065/article/details/115348824

版权

SLAM 专栏收录该内容

55 篇文章 42 订阅

订阅专栏

该文详细介绍了基于滑动窗口的视觉惯性里程计（VIO）优化中，如何利用预积分减少计算量。预积分通过积分IMU测量值减少了重复计算，同时引入了误差传递矩阵F和G来考虑状态误差和测量噪声。文中还讨论了预积分的离散形式以及协方差传递性质，并展示了如何构建VIO残差函数，用于系统状态的优化更新。

摘要由CSDN通过智能技术生成

在这里插入图片描述
基于滑动窗口的 VIO Bundle Adjustment，为了节约计算量采用滑动窗口形式的 Bundle Adjustment，在 i 时刻，
滑动窗口内待优化的系统状态量定义如下：
$\chi = [X_n,X_{n+1},...,X_{n+N},\lambda{m},\lambda{m+1},...,\lambda{m+M}]$ $X_i=[p_{wb_i},q_{wb_i},v_i^w,b_a^{b_i},b_g^b{_i}],i \in [n,n+N]$
其中:

$x_i$ 包含 $i$ 时刻 IMU 机体的在惯性坐标系中的位置，速度，姿态，以及 IMU 机体坐标系中的加速度和角速度的偏置量估计。
$m, n$ 分别是机体状态量，路标在滑动窗口里的起始时刻。
$N$ 滑动窗口中关键帧数量。
$M$ 是被滑动窗口内所有关键帧观测到的路标数量。
$\lambda$ 是逆深度。

IMU的真实值为 $w, a$ ，测量值为 $\tilde{w},\tilde{a}$ ，则有：
$\tilde{w}^b=w^b+b^g+n^g$ $\tilde{a}^b=q_{bw}(a^w+g^w)+b^a+n^a$

P(位置)，V(速度)，Q(位姿) 对时间的导数可写成：
$\dot{p}_{wb_t}=v^w_t,\dot{v}^w_t=a^w_t,\dot{q}_{wb_t}=q_{wb_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix}$
从第 $i$ 时刻的 PVQ 对 IMU 的测量值进行积分得到第 $j$ 时刻的 PVQ：
$p_{wb_j}=p_{wb_i}+v_i^w\Delta t +\iint_{t \in [i,j]}(q_{wb_t}a^{b_t}-g^w)\delta t^2$ $v_i^w=v_i^w+\int_{t \in [i,j]}(q_{wb_t}a^{b_t}-g^w)\delta t$ $q_{wb_j}=\int_{t \in [i,j]}q_{wb_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix}\delta t$
每次 $q_{wb_t}$ 优化更新后，都需要重新进行积分，运算量较大。因此提出IMU预积分：
$q_{wb_t}=q_{wb_i}⊗q_{b_ib_t}$
那么，PVQ 积分公式中的积分项则变成相对于第 $i$ 时刻的姿态，而不是相对于世界坐标系的姿态：
$p_{wb_j}=p_{wb_i}+v_i^w\Delta t +\iint_{t \in [i,j]}(q_{wb_t}a^{b_t}-g^w)\delta t^2 =p_{wb_i}+v_i^w\Delta t- {1\over 2}g^w \Delta t^2+\iint_{t \in [i,j]}(q_{wb_t}a^{b_t})\delta t^2$ $\implies \color{red}{p_{wb_j}=p_{wb_i}+v_i^w\Delta t- {1\over 2}g^w \delta t^2+q_{wb_i}\iint_{t \in [i,j]}(q_{b_ib_t}a^{b_t})\delta t^2}$ $v_i^w=v_i^w+\int_{t \in [i,j]}(q_{wb_t}a^{b_t}-g^w)\delta t \implies \color{red}{ v_i^w=v_i^w-g^w\Delta t+q_{wb_i}\int_{t \in [i,j]}(q_{b_ib_t}a^{b_t})\delta t}$ $q_{wb_j}=\int_{t \in [i,j]}q_{wb_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix}\delta t \implies \color{red}{q_{wb_j}=q_{wb_i}\int_{t \in [i,j]}q_{b_ib_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix}\delta t}$

预积分量仅仅跟 IMU 测量值有关，它将一段时间内的 IMU 数据直接积分起来就得到了预积分量：
$\alpha_{b_ib_j}=\iint_{t \in [i,j]}(q_{b_ib_t}a^{b_t})\delta t^2$ $\beta_{b_ib_j}=\int_{t \in [i,j]}(q_{b_ib_t}a^{b_t})\delta t$ $q_{b_ib_j}=\int_{t \in [i,j]}q_{b_ib_t}⊗ \begin{bmatrix} 0 \\ {1\over 2}w^{b_t} \\ \end{bmatrix}\delta t$ 将 $\alpha_{b_ib_j},\beta_{b_ib_j},q_{b_ib_j}$ 代入整理可得到最终的PVQ积分公式：
$\begin{bmatrix} p_{wb_j}\\v_j^w\\q_{wb_j}\\b_j^a\\b_j^g\\ \end{bmatrix}=\begin{bmatrix} p_{wb_i}+v_i^w\Delta t- {1\over 2}g^w \Delta t^2+q_{wb_i}\color{red}{\alpha_{b_ib_j}}\\v_i^w-g^w\Delta t+q_{wb_i}\color{red}{\beta_{b_ib_j}} \\q_{wb_i}\color{red}{q_{b_ib_j}}\\b_i^a\\b_i^g\\ \end{bmatrix}$

预积分误差：一段时间内 IMU 构建的预积分量作为测量值，对两时刻之间的状态量进行约束
$\begin{bmatrix} r_p\\r_q\\r_v\\r_{ba}\\r_{bg}\\ \end{bmatrix}=\begin{bmatrix} p_{wb_j} - p_{wb_i}-v_i^w\Delta t+ {1\over 2}g^w \Delta t^2-q_{wb_i}\color{red}{\alpha_{b_ib_j}}\\ 2[{\color{red}{q_{b_jb_i}}}⊗(q_{b_iw} ⊗ q_{wb_j})]_{xyz} \\v_j^w - v_i^w+g^w\Delta t-q_{wb_i}\color{red}{\beta_{b_ib_j}} \\b_j^a-b_i^a\\b_j^g-b_i^g\\ \end{bmatrix}=\begin{bmatrix} q_{b_iw}(p_{wb_j} - p_{wb_i}-v_i^w\Delta t+ {1\over 2}g^w \Delta t^2)-\color{red}{\alpha_{b_ib_j}}\\ 2[{\color{red}{q_{b_jb_i}}}⊗(q_{b_iw} ⊗ q_{wb_j})]_{xyz} \\q_{b_iw}(v_j^w - v_i^w+g^w\Delta t)-\color{red}{\beta_{b_ib_j}} \\b_j^a-b_i^a\\b_j^g-b_i^g\\ \end{bmatrix}$
上面误差中位移，速度，偏置都是直接相减得到。第二项是关于四元数的旋转误差，其中 $[\cdot]_{xyz}$ 表示只取四元数的虚部 $(x, y, z)$ 组成的三维向量。

预积分的离散形式
以上是预积分的连续形式，也可以使用离散形式，这里使用 $m i d - p o i n t$ 方法，即两个相邻时刻 $k$ 到 $k + 1$ 的位姿是用两个时刻的测量值 $a, ω$ 的平均值来计算： $\over 2}((w^{b_k}-b_k^g)+(w^{b_{k+1}}-b_k^g))$ $a={1\over 2}(q_{b_ib_k}(a^{b_k}-b_k^a)+q_{b_ib_{k+1}}(a^{b_{k+1}}-b_k^a))$

预积分量的方差
协方差传递性质：在一个线性系统中，已知一个变量 $\mathcal N (0, Σ_x)$ , 则有 $Σ_y = AΣ_xA^⊤$
$Σ_y = E((Ax)(Ax)^⊤) = E(Axx^⊤A^⊤) = AΣ_xA^⊤$ 要推导预积分量的协方差，我们需要知道 imu 噪声和预积分量之间的线性递推关系。

假设已知了相邻时刻误差的线性传递方程： $η_{ik} = {\color{red}F_{k−1}η_{ik−1}} + {\color{blue}G_{k−1}n_{k−1}}$ 比如：状态量误差为 $η_{ik} = [δθ_{ik}, δv_{ik}, δp_{ik}]$ ，测量噪声为 $n_k = [n^g_k, n^a_k]$ 。
误差的传递由两部分组成：1.当前时刻的误差传递给下一时刻，2.当前时刻测量噪声传递给下一时刻。
协方差矩阵可以通过递推 $Σ_y = AΣ_xA^⊤$ 计算得到： $Σ_{ik} = {\color{red} F_{k−1}Σ_{ik−1}F^⊤_{k−1}} +{\color{blue} G_{k−1}Σ_nG^⊤_{k−1}}$ 其中， $Σ_n$ 是测量噪声的协方差矩阵，方差从 $i$ 时刻开始进行递推， $Σ_{ii} = 0$ 。

状态误差线性递推公式的推导(线性化)
协方差传递性质只对于线性化系统，通常对于状态量之间的递推关系是非线性的方程如 $x_k = f(x_{k−1}, u_{k−1})$ ，其中状态量为 $x ， u$ 为系统的输入量。

我们可以用两种方法来推导状态误差传递的线性递推关系：

一种是基于一阶泰勒展开的误差递推方程（应用于 EKF 的协方差预测）。
令状态量为 $\hat{x} + δx$ ，其中，真值为 $\hat{x}$ ，误差为 $δ x$ 。另外，输入量 $u$ 的噪声为 $n$ 。
非线性系统 $x_k = f(x_{k−1}, u_{k−1})$ 方程进行一阶泰勒展开有： $x_k = f(x_{k−1}, u_{k−1})$ $\hat{x}_k +δx_k= f(\hat{x}_{k−1}+δx_{k-1}, \hat{u}_{k−1}+n_{k-1})$ $\hat{x}_k +δx_k= f(\hat{x}_{k−1}, \hat{u}_{k−1})+Fδx_{k-1}+Gn_{k-1}$ ${\color{red}δx_k= Fδx_{k-1}+Gn_{k-1}}$ 其中，F 是状态量 $x_k$ 对状态量 $x_{k−1}$ 的雅克比矩阵，G 是状态量 $x_k$ 对输入量 $u_{k−1}$ 的雅克比矩阵。
一种是基于误差随时间变化的递推方程。
如果我们能够推导状态误差随时间变化的导数关系，比如： $δ\dot{x} = Aδx + Bn$ 则误差状态的传递方程为：
$δx_k = δx_{k−1} + δ\dot{x}_{k−1}∆t → δx_k = (I + A∆t)δx_{k−1} + B∆tn_{k−1}$

这两种推导方式的可以看出有：
$F = I + A ∆ t, G = B ∆ t$

回顾预积分(离散形式)的误差递推公式，将测量噪声也考虑进模型：
$\over 2}((w^{b_k}+ {\color{red}n_k^g}-b_k^g)+(w^{b_{k+1}}+ {\color{red}n_{k+1}^g}-b_k^g))$ $a={1\over 2}(q_{b_ib_k}(a^{b_k}+ {\color{red}n_k^a}-b_k^a)+q_{b_ib_{k+1}}(a^{b_{k+1}}+ {\color{red}n_{k+1}^a}-b_k^a))$ 确定误差传递的状态量，噪声量，然后开始构建传递方程。用前面一阶泰勒展开的推导方式，我们能推导出如下的形式： $\begin{bmatrix} δα_{b_{k+1}b′_{k+1}} \\ δθ_{b_{k+1}b′_{k+1}} \\ δβ_{b_{k+1}b′_{k+1}} \\δb^a_{k+1}\\ δb^g_{k+1}\\ \end{bmatrix}=F\begin{bmatrix} δα_{b_{k}b′_{k}} \\ δθ_{b_{k}b′_{k}} \\ δβ_{b_{k}b′_{k}} \\δb^a_{k}\\ δb^g_{k}\\ \end{bmatrix}+G\begin{bmatrix} n^a_k\\n^g_k\\n^a_{k+1}\\n^g_{k+1}\\n_{b^a_a}\\n_{b^g_k}\\ \end{bmatrix}$ F, G 为两个时刻间的协方差传递矩阵。