IMU在slam系统中的应用(四)

        在IMU在slam系统中的应用(三)中我们推导了IMU的雅可比矩阵迭代公式协方差矩阵的传递公式,那么,这两个迭代公式在耦合IMU的slam中的用处是什么呢?这里我们来推导一下他们的作用。

(一)雅可比矩阵迭代公式

        之前已经推到过,雅可比矩阵迭代公式为:

J_{t+\delta t}=\left(I+F_{t} \delta t\right) J_{t}

        因此,在预积分中,可以有另一种求法,预积分的公式可以写成(符号稍微与前文有点变化,实际含义不变):

\boldsymbol{\alpha}_{b_ib_j}=\iint_{t\in [i,j]}^{}(\mathbf{q}_{b_ib_t}\mathbf{a}^{bt})\delta t^2 =\iint_{t\in [i,j]}^{}\mathbf{q}_{b_ib_t}(\mathbf{\hat{a}}^{bt} - \mathbf{b}^{at}-\mathbf{n}^a)\delta t^2

\boldsymbol{\beta}_{b_ib_j} = \int_{t\in [i,j]}^{}(\mathbf{q}_{b_ib_t}\mathbf{a}^{b_t})\delta t = \int_{t\in [i,j]}^{}\mathbf{q}_{b_ib_t}(\mathbf{\hat{a}}^{bt} - \mathbf{b}^{at}-\mathbf{n}^a)\delta t

\mathbf{\gamma }_{b_ib_j}=\int_{t\in [i,j]}^{}\mathbf{\gamma }_{b_ib_t} \bigotimes\left[\begin{array}{c} 0 \\ \frac{1}{2}\vec{ \omega}^{bt} \end{array}\right]\delta t =\int_{t\in [i,j]}^{}\mathbf{\gamma }_{b_ib_t} \bigotimes\left[\begin{array}{c} 0 \\ \frac{1}{2}\vec{ \omega}^{bt}-b^w-n^g \end{array}\right]\delta t

        对上式进行一阶泰勒展开(噪声是保持不变的)近似,雅可比矩阵可以根据上一个时刻递推而得:

\boldsymbol{\alpha}_{b_ib_j}\approx \hat{\boldsymbol{\alpha}}_{b_ib_j}+ J^\alpha_{b^a}\delta b^a+J^\alpha_{b^\omega}\delta b^\omega

\boldsymbol{\beta }_{b_ib_j}\approx \hat{\boldsymbol{\beta }}_{b_ib_j}+ J^\beta _{b^a}\delta b^a+J^\beta _{b^\omega}\delta b^\omega

\boldsymbol{\gamma }_{b_ib_j}\approx \hat{\boldsymbol{ \gamma }}_{b_ib_j}\bigotimes\left[\begin{array}{c} 1 \\ \frac{1}{2}J^\gamma _{b^w}\delta b^w\end{array}\right]

(二)协方差矩阵的传递公式

        协方差矩阵传递的作用是用于后端优化的IMU信息矩阵的迭代以及边缘化先验信息的误差函数。

(1)补充知识:H矩阵的稀疏结构以及边缘化操作

        在非线性优化的高斯牛顿方法中,我们在迭代求解增量\Delta x,是根据高斯牛顿方程来求解:

H\Delta x = g

        高斯牛顿方法中用JJ^T作为牛顿法中的二阶Hessian矩阵的近似求解增量方程。

        考虑一个简单的观测模型:误差项e_{ij}描述相机以T_i姿态对路标点j进行观测的误差,那么整体的代价函数可以表示为:

\dfrac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{m}\left \| e_{ij }\right \|^2=\dfrac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{m}\left \| z_{ij}-h(T_i,p_j)\right \|^2

        其中,z_{ij}为相机观测的像素点,h(\cdot )表示路标p_j在位姿T_i时的投影模型。

        考虑e_{ij}只涉及到了第i个相机位姿和第j个路标点,对其余部分的导数为0,把位姿变量放在误差项的前面,路标变量放在误差项的后头,所以,可以得到该误差项对应的雅可比矩阵:

J_{ij}(x) = (0_{2\times 6},\dots,\frac{\partial e_{ij}}{\partial T_i},\dots,0_{2\times 6},\dots,0_{2\times 3},\dots,\frac{\partial e_{ij}}{\partial p_j},\dots,0_{2\times 3})

        0_{2\times3}0_{2\times3}表示0矩阵,对相机姿态的偏导维度为2\times 6,对路标点的偏导维度为2\times 3,该误差项对应的雅可比矩阵对H矩阵的贡献为J_{ij}J_{ij}^T,因此J_{ij}J_{ij}^T只在(i,j)(j,i)(i,i)(j,j)存在非零块。

        对于整体的H矩阵,令i在所有的相机位姿中取值,j在所有的路标点中取值,那么,对矩阵按照位姿和路标分块,H可以表示为:

H=\sum_{i,j}^{}J_{ij}J_{ij}^T=\begin{bmatrix} H_{11} & H_{12}\\ H_{21}& H_{22} \end{bmatrix}

        很容易可以看出H_{11}H_{22}对角阵,H_{12}H_{21}稀疏还是稠密取决于具体的观测数据。这显示了H矩阵的稀疏结构,对线性方程的加速求解也利用了矩阵H的稀疏性。

        现在将模型变复杂一点,考虑一个场景实例,假设一个场景内有两个相机位姿(C_1C_2)和六个路标点(P_1P_2P_3P_4P_5P_6),对应的变量为T_iP_j,因此很容易写出该场景下的误差函数为:

\frac{1}{2}\left(\left\|\boldsymbol{e}_{11}\right\|^{2}+\left\|\boldsymbol{e}_{12}\right\|^{2}+\left\|\boldsymbol{e}_{13}\right\|^{2}+\left\|\boldsymbol{e}_{14}\right\|^{2}+\left\|\boldsymbol{e}_{23}\right\|^{2}+\left\|\boldsymbol{e}_{24}\right\|^{2}+\left\|\boldsymbol{e}_{25}\right\|^{2}+\left\|\boldsymbol{e}_{26}\right\|^{2}\right)

        现在把所有变量按照位姿在前,路标点在后的顺序排列可得变量x为:

x=(T_1,T_2,p_1,p_2,p_3,p_4,p_5,p_6)

        因此可以得到e_{11}对应的雅可比矩阵J_{11}为:

J_{11} = (\frac{\partial e_{11}}{\partial T_1},0_{2\times 6},\frac{\partial e_{11}}{\partial p_1},0_{2\times 3},0_{2\times 3},0_{2\times 3},0_{2\times 3},0_{2\times 3})

        用带颜色方块将该矩阵可视化为下图(较宽的黑块表示较大的维度):

        进一步可视化表示整体目标函数对应的雅可比矩阵:

        所以相应的H矩阵的系数情况也可以可视化出来,H矩阵中的非对角部分的非零矩阵块表示变量之间存在约束:

        现在继续考虑更一般的情况,在实际的slam系统中,假设存在m个相机位姿和n个路标点,相机位姿的数量远远小于路标点的数量,即n\gg m,所以对应的H矩阵左上角显得特别小,可视化以后如下图:

        对于具有这种稀疏结构的H,线性方程H\Delta x = g的求解通常采用边缘化Marg)的操作,也被称为Schur消元。

        首先把H阵分成四个区域,如下图,记为BEE^TC

        所以对应的线性方程组H\Delta x = g可以变为如下形式:

\left[\begin{array}{ll} \boldsymbol{B} & \boldsymbol{E} \\ \boldsymbol{E}^{\mathrm{T}} & \boldsymbol{C} \end{array}\right]\left[\begin{array}{l} \Delta \boldsymbol{x}_{\mathrm{c}} \\ \Delta \boldsymbol{x}_{p} \end{array}\right]=\left[\begin{array}{l} \boldsymbol{v} \\ \boldsymbol{w} \end{array}\right]

        由于BC是对角矩阵,对对角矩阵求逆是十分方便的,考虑到这个特性,对方程组高斯消元,目标是消去右上的非对角矩阵E,得到:

\left[\begin{array}{cc} \boldsymbol{I} & -\boldsymbol{E} \boldsymbol{C}^{-1} \\ \mathbf{0} & \boldsymbol{I} \end{array}\right]\left[\begin{array}{cc} \boldsymbol{B} & \boldsymbol{E} \\ \boldsymbol{E}^{\mathrm{T}} & \boldsymbol{C} \end{array}\right]\left[\begin{array}{l} \Delta \boldsymbol{x}_{\mathrm{c}} \\ \Delta \boldsymbol{x}_{p} \end{array}\right]=\left[\begin{array}{cc} \boldsymbol{I} & -\boldsymbol{E} \boldsymbol{C}^{-1} \\ \mathbf{0} & \boldsymbol{I} \end{array}\right]\left[\begin{array}{l} \boldsymbol{v} \\ \boldsymbol{w} \end{array}\right]

        即处理后得到:

\left[\begin{array}{cc} \boldsymbol{B}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{E}^{\mathrm{T}} & \mathbf{0} \\ \boldsymbol{E}^{\mathrm{T}} & \boldsymbol{C} \end{array}\right]\left[\begin{array}{c} \Delta \boldsymbol{x}_{\mathrm{c}} \\ \Delta \boldsymbol{x}_{p} \end{array}\right]=\left[\begin{array}{c} \boldsymbol{v}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{w} \\ \boldsymbol{w} \end{array}\right]

        所以可以得到方程组:

\left[\boldsymbol{B}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{E}^{\mathrm{T}}\right] \Delta \boldsymbol{x}_{\mathrm{c}}=\boldsymbol{v}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{w}

\Delta \boldsymbol{x}_{p}=\boldsymbol{C}^{-1}\left(\boldsymbol{w}-\boldsymbol{E}^{\mathrm{T}} \Delta \boldsymbol{x}_{\mathrm{c}}\right)

        该过程即Schur消元,或者为slam中的边缘化,加速了线性方程中\Delta x的求解。

(2)VINS中的目标函数

        VINS是视觉和IMU紧耦合的slam系统,使用滑动窗口的方法进行滤波和优化。在后端非线性优化中,优化的是包括滑动窗口在内的n+1个相机的状态(包括位置,朝向,速度,加速度计的零偏和陀螺仪的零偏),相机到IMU的外参,以及m+13D点的逆深度:

\begin{gathered} X=\left[x_{0}, x_{1}, \cdots x_{n}, x_{c}^{b}, \lambda_{0}, \lambda_{1}, \cdots \lambda_{m}\right] \\ x_{k}=\left[p_{b_{k}}^{w}, v_{b_{k}}^{w}, q_{b_{k}}^{w}, b_{a}, b_{g}\right] \\ x_{c}^{b}=\left[p_{c}^{b}, q_{c}^{b}\right] \end{gathered}

        所以给出滑动窗口优化的目标函数(见VINS论文):

\min _{X}\left\{\left\|r_{p}-H_{p} X\right\|^{2}+\sum_{k \in B}\left\|r_{B}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X\right)\right\|_{P_{b_{k+1}}^{b_{k}}}^{2}+\sum_{(l, j) \in C}\rho (\left\|r_{C}\left(\hat{z}_{l}^{C_{j}}, X\right)\right\|_{P_{l}}^{c_{j}})\right\}

        这个目标函数中的三个残差项即误差项分别为边缘化的先验信息,IMU测量残差和视觉的重投影残差,三种残差都是使用马氏距离表示,这里只讨论与IMU信息矩阵(协方差的逆)有关的前两个残差,第三个残差会另外写文章讨论。

        由高斯牛顿法我们知道,以IMU测量残差为例子,当优化变量有一个微小增量时,对其进行一阶泰勒展开可以得到:

\min _{\delta X}\left\|r_{B}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X+\Delta X\right)\right\|_{P_{b_{k+1}}^{b_{k}}}^{2}=\min _{\delta X}\left\|r_{B}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X\right)+J_{b_{k+1}}^{b_{k}} \Delta X\right\|_{P_{b_{k+1}}^{b_{k}}}^{2}

        同视觉14讲中,展开目标函数的平方项,并对\Delta x求导,令其为零,可以得到增量\Delta x

J_{b_{k+1}}^{b_{k}}{ }^{T} P_{b_{k+1}}^{b_{k}}{ }^{-1} J_{b_{k+1}}^{b_{k}} \Delta \mathrm{X}=-J_{b_{k+1}}^{b_{k}}{ }^{T} P_{b_{k+1}}^{b_{k}}{ }^{-1} r_{B}

        同理,整体目标函数的增量方程可以写成:

\begin{aligned} &\left(H_{p}+\sum J_{b_{k+1}}^{b_{k}}{ }^{T} P_{b_{k+1}}^{b_{k}^{-1}} J_{b_{k+1}}^{b_{k}}+\sum J_{l}^{C_{j}^{T}} P_{l}^{C_{j}^{-1}} J_{l}^{C_{j}}\right) \Delta X\\ &=b_{p}+\sum J_{b_{k+1}}^{b_{k}}{ }^{T} P_{b_{k+1}}^{b_{k}^{-1}}{ } r_{B}+\sum J_{l}^{C_{j}^{T}} P_{l}^{C_{j}^{-1}} r_{C} \end{aligned}

        这里P_{b_{k+1}}^{b_{k}}{ }为IMU预积分噪声项的协方差误差,P_{l}^{C_{j}}为观测的协方差误差,二者描述的是观测的可靠性,所以其绝对值没有意义,考虑的是相对性,进一步将上式简化:

\left(\Lambda_{p}+\Lambda_{B}+\Lambda_{C}\right) \Delta X=b_{p}+b_{B}+b_{C}

(3)IMU测量残差

        之前已经推导过IMU残差矩阵,这里直接相邻两帧的PVQ和bias变化量的差写出:

r_{B}^{15 \times 1}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X\right)=\left[\begin{array}{c} \delta \alpha_{b_{k+1}}^{b_{k}} \\ \delta \theta_{b_{k+1}}^{b_{k}} \\ \delta \beta_{b_{k+1}}^{b_{k}} \\ \delta b_{a} \\ \delta b_{g} \end{array}\right]=\left[\begin{array}{c} R_{w}^{b_{k}}\left(p_{b_{k+1}}^{w}-p_{b_{k}}^{w}-v_{b_{k}}^{w} \Delta t_{k}+\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)-\alpha_{b_{k+1}}^{b_{k}} \\ 2\left[\gamma_{b_{k+1}}^{b_{k}}\otimes q_{b_{k}}^{w} \otimes q_{b_{k+1}}^{w}\right]_{x y z} \\ R_{w}^{b_{k}}\left(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w} \Delta t_{k}\right)-\beta_{b_{k+1}}^{b_{k}} \\ b_{a_{b_{k+1}}}-b_{a_b_{k}} \\ b_{\omega_{b_{k+1}}}-b_{\omega_{b_k}} \end{array}\right]

        从全局目标函数优化变量可以看出,这里优化变量的是相邻两帧的IMU姿态:

\left[p_{b_{k}}^{w}, q_{b_{k}}^{w}\right],\left[v_{b_{k}}^{w}, b_{a_{k}}, b_{\omega_{k}}\right],\left[p_{b_{k+1}}^{w}, q_{b_{k+1}}^{w}\right],\left[v_{b_{k+1}}^{w}, b_{a_{k+1}}, b_{\omega_{k+1}}\right]

        接下来给出残差矩阵对于这些优化变量的雅可比矩阵(采用扰动的方式求解,推导过程后续补充):

J[0]^{15 \times 7}=\left[\frac{\partial r_{B}}{\partial p_{b_{k}}^{w}}, \frac{\partial r_{B}}{\partial q_{b_{k}}^{w}}\right]=\left[\begin{array}{cc} -R_{w}^{b_{k}} & \left.\left[R_{w}^{b_{k}}\left(p_{b_{k+1}}^{w}-p_{b_{k}}^{w}-v_{b_{k}}^{w} \Delta t_{k}+\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)\right]^{\wedge}\right] \\ 0 & -\mathcal{L}\left[q_{b_{k+1}}^{w^{-1}} \otimes q_{b_{k}}^{w}\right] \mathcal{R}\left[\gamma_{b_{k+1}}^{b_{k}}\right] \\ 0 & {\left[R_{w}^{b_{k}}\left(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w} \Delta t_{k}\right)\right]^{\wedge}} \\ 0 & 0 \\ 0 & 0 \end{array}\right]

J[1]^{15 \times 9}=\left[\frac{\partial r_{B}}{\partial v_{b_{k}}^{W}}, \frac{\partial r_{B}}{\partial b_{a_{k}}}, \frac{\partial r_{B}}{\partial b_{w_{k}}}\right]=\begin{bmatrix} -R_{w}^{b_{k}} \Delta t& -J_{b_{a}}^{\alpha} & -J_{b_{\omega}}^{\alpha}\\ 0 & 0& -\mathcal{L}\left[q_{b_{k+1}}^{w^{-1}} \otimes q_{b_{k}}^{w} \otimes \gamma_{b_{k+1}}^{b_{k}}\right] J_{b_{\omega}}^{\gamma}\\ -R_{w}^{b_{k}}& -J_{b_{a}}^{\beta} & -J_{b_{\omega}}^{\beta}\\ 0&-I &0 \\ 0 &0 &-I \end{bmatrix}

J[2]^{15 \times 7}=\left[\frac{\partial r_{B}}{\partial p_{b_{k+1}}^{w}}, \frac{\partial r_{B}}{\partial q_{b_{k+1}}^{w}}\right]=\begin{bmatrix} R_{w}^{b_{k}} & 0\\ 0 & \mathcal{L}\left[\gamma_{b_{k+1}}^{b_{k}^{-1}} \otimes q_{b_{k}}^{w^{-1}} \otimes q_{b_{k+1}}^{w}\right]\\ 0 & 0\\ 0&0 \\ 0 & 0 \end{bmatrix}

J[3]^{15 \times 9}=\left[\frac{\partial r_{B}}{\partial v_{b_{k+1}}^{w}}, \frac{\partial r_{B}}{\partial b_{a_{k+1}}}, \frac{\partial r_{B}}{\partial b_{w_{k+1}}}\right]=\left[\begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & 0 \\ R_{w}^{b_{k}} & 0 & 0 \\ 0 & I & 0 \\ 0 & 0 & I \end{array}\right]

        另外,协方差P_{b_{k+1}}^{b_{k} }由预积分中的协方差迭代公式得到,为:

P_{t+\delta t}^{b_{k}}=\left(I+F_{t} \delta t\right) P_{t}^{b k}\left(I+F_{t} \delta t\right)^{T}+(G \delta t) Q\left(G_{t} \delta t\right)^{T}

        到这里IMU残差部分的推导已经结束,之后根据高斯牛顿方法求解优化变量即可。

(4)边缘化的先验信息

        重点:VINS中的边缘化和利用H矩阵的稀疏结构边缘化虽然处理方法相同,但是意义不同。

        利用H矩阵的稀疏结构边缘化主要是在全局BA中先消去路标点变量,先求解相机位姿,然后利用相机位姿来求解路标点变量,目的是为了加速求解速度

        而VINS中的边缘化操作的目的把即将剔出滑动窗口的帧中的路标点点和位姿给边缘化,这样使得求解过程中只出现滑动窗口里的帧的优化变量,但是又保留了该剔出帧对滑动窗口内其他帧的约束关系。(按我的理解是在求解过程中引入了历史信息,即引入了先验)。

        边缘化的操作上文已经详细推导过,这里不在赘述,仅仅详细说一下为什么边缘化可以引入先验。考虑这么一个观测模型,有四个相机姿态x_{pi}和六个路标点x_{mk},相机和路标点的连线表示相机对路标点的观测,相机之间的连线表示IMU约束,用以下示意图表示:

      我们可以画出关于上述观测模型的H矩阵,即初始矩阵:

 下面我们先把位姿x_{p1}给marg,marg的操作就是把跟x_{p1}有关的部分移到左上角,见下图:

         然后边缘化x_{p1},边缘化的操作即把x_{p1}高斯消元,这里x_{p1}是在上方,与上文中描述的边缘化消元略有区别,上文中描述的消元是消去下方的变量,但是方法一样,可以自行推导公式,利用边缘化公式为\boldsymbol{C}-\boldsymbol{E}^{\mathrm{T}} \boldsymbol{B}^{-1} \boldsymbol{E},得到:

         上图中我们发现矩阵变稠密了,有三部分被fill-in,这时候图关系产生了新的约束,变为

         同样的道理,marg掉x_{m1}之后的H为:

         对应的图关系为:

        这里marg掉x_{m1}并没有使得矩阵更稠密,原因是x_{m1}并未与其他的姿态产生观测关系,即并没有被观测到,所以如果marg未被其他的相机姿态观测到的路标点,是不会使得H矩阵变得稠密。

   

  待更 //补充一下VINS的边缘化策略,FEJ以及新测量信息和旧测量信息构建信息矩阵所存在的问题

  • 3
    点赞
  • 15
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值