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

不会算法的阿召

已于 2022-05-27 15:05:19 修改

阅读量967

点赞数 2

文章标签： c++

于 2022-04-06 17:52:23 首次发布

本文链接：https://blog.csdn.net/qq_41916893/article/details/123995626

版权

在IMU在slam系统中的应用(二)中给出了IMU预积分计算推导以及代码，这里给出IMU误差递推方程的推导。

（一）一段时间多个IMU数据预积分量协方差

单个IMU数据作为测量值，其噪声方差我们可以标定，但是一段时间内多个IMU数据积分形成的预积分量的方差应该怎么求解呢？

协方差传递公式

当 $y=Ax$ ，并且 $x$ 服从分布 $x\in N(0,\Sigma_x )$ ，那么 $y$ 的协方差为：

$\Sigma_y=A \Sigma_x A^T$

相邻时刻误差线性传递方程

误差包括两部分，上一时刻的误差传递给当前时刻，上一时刻的噪声传递给当前时刻。

假设上一时刻的状态量误差为 $\eta_{ik-1}=[\delta \theta_{ik-1}, \delta v_{ik-1}, \delta p_{ik-1}]$ ，上一时刻得测量噪声为 $n_{k-1}=[n^g_{k-1},n^a_{k-1}]$ ，那么，传递给当前时刻误差线性传递方程为：

$\eta_{ik}=F_{k-1}\eta_{k-1}+G_{k-1}n_{k-1}$

协方差传递公式

所以，协方差传递方程为：

$\Sigma_{ik}=F_{k-1}\Sigma_{ik-1}F^T_{k-1}+G_{k-1}\Sigma_{k-1}G^T_{k-1}$

（二）非线性方程的误差递推方程推导

通常，状态量之间的递推关系是非线性的方程，如 $x_{k+1}=f(x_k,u_k)$ ，其中 $x$ 为状态量， $u$ 为观测量。可以用两种方式来推导状态误差传递的线性递推关系。

基于一阶泰勒展开的误差传递方程

令状态量为 $\hat{x}=x+\delta x$ ，其中 $\hat{x}$ 为计算得到的预测状态量， $x$ 为实际真值， $\delta x$ 为误差，观测量的噪声为 $n$ ，所以，对非线性状态方程进行一阶泰勒展开

$\hat{x}_{k+1}=f(\hat{x}_k,\hat{u}_k)$

代入误差和噪声得：

$\hat{x}_{k+1}=f({x}_k+\delta x_k,{u}_k+n_k)$

所以展开后：

${x}_{k+1}+\delta x_{k+1}=f({x}_k,{u}_k)+F\delta x_k+G n_k$

其中 $F$ 为状态量 $x_{k+1}$ 对状态量 $x_k$ 的雅可比矩阵， $G$ 为状态量 $x_{k+1}$ 对观测量 $u_{k}$ 的雅可比矩阵。

因此状态误差的递推方程为：

$\delta x_{k+1}= F\delta x_k+G n_k$

基于误差随时间变化的递推方程(论文中的推导)

如果可以推导状态误差随时间变化的导数关系，如：

$\dot{\delta}x=A\delta x + Bn$

那么误差状态的传递方程为：

$\delta x_{k+1}=\delta x_k+ \dot{\delta}x_k\Delta t$

$\Rightarrow \delta x_{k+1}=(I+A\Delta t)\delta x_k+B \delta n_k$

代入到一阶泰勒展开的误差递推方程中，可以推导：

$F=I+A\Delta t$ ， $G=B\Delta t$

基于泰勒展开和基于误差随时间变化方法对比

为什么要用误差随时间的变化来进行误差递推方程的构建呢？

在应用（一）（二）的推导中，我们已经知道状态的导数和状态之间的关系，比如已经知道速度和状态量之间的关系：

$\dot{v}^w=R_b^wa^b+g^w$

因此可以推导速度的误差导数和状态误差之间的关系，在上式中加上各自误差则有

$\dot{v}^w+\dot{\delta}v^w=R^w_b exp([\delta \theta]_{\times})(a^b+\delta a^b)+g+\delta g$ $\dot{v}^w+\dot{\delta}v^w=R^w_b (I + [\delta \theta]_{\times})(a^b+\delta a^b)+g+\delta g$

$\dot{v}^w+\dot{\delta}v^w=R^w_b a^b+g+R^w_b \delta a^b +R^w_b [\delta \theta]_{\times}(a^b+\delta a^b)+\delta g$

$\dot{\delta}v^w= R^w_b \delta a^b +R^w_b [\delta \theta]_{\times}(a^b+\delta a^b)+\delta g$

$\dot{\delta}v^w= R^w_b \delta a^b -R^w_b [a^b]_{\times}\delta \theta+\delta g$

由此，就可以推导写出整个 $A$ 和 $B$ 和其他方程了。

（三）IMU的PVQ增量误差、协方差以及雅可比矩阵的递推方程

我们已经建立起IMU预积分测量值的推导，在前置知识下，需要将IMU预积分运用到非线性优化中，需要建立线性高斯误差状态递推方程，由线性高斯系统的协方差，推导协方差矩阵，并求解对应的雅可比矩阵。

首先，由于四元数 $\gamma_t^{b_k}$ 被参数化过，所以将其误差定义为围绕其均值的扰动：

$\gamma_t^{b_k}\approx \hat{\gamma}_t^{b_k}\bigotimes \left[\begin{array}{c} 1 \\ \frac{1}{2}{ \delta \theta_t^{b_k}} \end{array}\right]$

由预积分的连续形式建立的运动模型，以一段时间内IMU构建的预积分量作为测量值，对两时刻之间的状态量进行约束，可以得到 $k$ 时刻和 $k+1$ 时刻下的误差项。

之前已经推导过世界坐标系中的预积分和PVQ状态的关系，从世界坐标系转换到本体坐标系，该关系可以写成：

$\mathbf{R}_{w}^{b_{k}} \mathbf{p}_{b_{k+1}}^{w}=\mathbf{R}_{w}^{b_{k}}\left(\mathbf{p}_{b_{k}}^{w}+\mathbf{v}_{b_{k}}^{w} \Delta t_{k}-\frac{1}{2} \mathbf{g}^{w} \Delta t_{k}^{2}\right)+\boldsymbol{\alpha}_{b_{k+1}}^{b_{k}}$

$\mathbf{R}_{w}^{b_{k}} \mathbf{v}_{b_{k+1}}^{w}=\mathbf{R}_{w}^{b_{k}}\left(\mathbf{v}_{b_{k}}^{w}-\mathbf{g}^{w} \Delta t_{k}\right)+\boldsymbol{\beta}_{b_{k+1}}^{b_{k}}$

$\mathbf{q}_{w}^{b_{k}} \otimes \mathbf{q}_{b_{k+1}}^{w}=\gamma_{b_{k+1}}^{b_{k}}$

以预积分作为测量量，可以得到误差项为：

$\left[\begin{array}{c} \delta \boldsymbol{\alpha}_{b_{k+1}}^{b_{k}} \\ \delta \boldsymbol{\beta}_{b_{k+1}}^{b_{k}} \\ \delta \boldsymbol{\theta}_{b_{k+1}}^{b_{k}} \\ \delta \mathbf{b}_{a} \\ \delta \mathbf{b}_{g} \end{array}\right] =\left[\begin{array}{c} \mathbf{R}_{w}^{b_{k}}\left(\mathbf{p}_{b_{k+1}}^{w}-\mathbf{p}_{b_{k}}^{w}+\frac{1}{2} \mathbf{g}^{w} \Delta t_{k}^{2}-\mathbf{v}_{b_{k}}^{w} \Delta t_{k}\right)-\hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}} \\ \mathbf{R}_{w}^{b_{k}}\left(\mathbf{v}_{b_{k+1}}^{w}+\mathbf{g}^{w} \Delta t_{k}-\mathbf{v}_{b_{k}}^{w}\right)-\hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}} \\ 2\left[\mathbf{q}_{b_{k}}^{w^{-1}} \otimes \mathbf{q}_{b_{k+1}}^{w} \otimes\left(\hat{\gamma}_{b_{k+1}}^{b_{k}}\right)^{-1}\right]_{x y z} \\ \mathbf{b}_{a b_{k+1}}-\mathbf{b}_{a b_{k}} \\ \mathbf{b}_{w b_{k+1}}-\mathbf{b}_{w b_{k}} \end{array}\right]$

因此根据误差递推方程的推导，可以给出 $t$ 时刻误差项的线性化递推方程：

$\begin{gathered} {\left[\begin{array}{l} \delta \dot{\alpha}_{t}^{b k} \\ \delta \dot{\beta}_{t}^{b k} \\ \delta \dot{\theta}_{t}^{b k} \\ \delta \dot{b}_{a t} \\ \delta \dot{b}_{w t} \end{array}\right]=\left[\begin{array}{ccccc} 0 & I & 0 & 0 & 0 \\ 0 & 0 & -R_{t}^{b k}\left[\hat{a}_{t}-b_{a t}\right]_{\times} & -R_{t}^{b k} & 0 \\ 0 & 0 & -\left[\hat{w}-b_{w t}\right]_{\times} & 0 & -I \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \end{array}\right]\left[\begin{array}{c} \delta \alpha_{t}^{b k} \\ \delta \beta_{t}^{b k} \\ \delta \theta_{t}^{b k} \\ \delta b_{a t} \\ \delta b_{w t} \end{array}\right]+\left[\begin{array}{cccc} 0 & 0 & 0 & 0 \\ -R_{t}^{b k} & 0 & 0 & 0 \\ 0 & -I & 0 & 0 \\ 0 & 0 & I & 0 \\ 0 & 0 & 0 & I \end{array}\right]\left[\begin{array}{c} n_{a} \\ n_{w} \\ n_{b a} \\ n_{b w} \end{array}\right]} \\ \\ =F_{t} \delta z_{t}^{b k}+G_{t} n_{t} \end{gathered}$

简写为：

$\delta \dot{z}_{t}^{b k}=F_{t} \delta z_{t}^{b k}+G_{t} n_{t}$

进一步的，根据导数的定义，

$\delta \dot{z}_{t}^{b k}=\lim _{\delta t \rightarrow 0} \frac{\delta z_{t+\delta t}^{b k}-\delta z_{t}^{b k}}{\delta t}$

即可得到：

$\delta z_{t+\delta t}^{b k}=\delta z_{t}^{b k}+\delta \dot{z}_{t}^{b k} \delta t=\delta z_{t}^{b k}+\left(F_{t} \delta z_{t}^{b k}+G_{t} n_{t}\right) \delta t$

$=\left(I+F_{t} \delta t\right) \delta z_{t}^{b k}+G_{t} \delta t n_{t}$

得到与误差随时间的变化来进行误差递推方程类似的方程，令 $F=\left(I+F_{t} \delta t\right)$ ， $V=G_{t} \delta t$ 可以得到非线性系统的线性化递推误差方程：

$\delta z_{t+\delta t}^{b_{k}}=F \delta z_{t}^{b_{k}}+V n_{t}$

再，对于下一时刻的协方差，根据协方差递推公式，由高斯分布线性变换协方差的传递规律，可以预测下一时刻的协方差：

$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}$

其中 $P^{b_k}_{t}$ 表示 $t$ 时刻的协方差， $Q$ 表示噪声项的对角协方差矩阵，初始协方差为 $0$ ：

$Q^{12 \times 12}=\left[\begin{array}{cccc}\sigma_{a}^{2} & 0 & 0 & 0 \\ 0 & \sigma_{w}^{2} & 0 & 0 \\ 0 & 0 & \sigma_{b a}^{2} & 0 \\ 0 & 0 & 0 & \sigma_{b w}^{2}\end{array}\right]$

同时也可以根据递推方程得到对应的雅可比矩阵迭代公式：

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

其中初始化雅可比矩阵为单位阵 $I$ 。

不会算法的阿召

关注

2
点赞
踩
20

收藏

觉得还不错? 一键收藏
0
评论
IMU在slam系统中的应用(三)

在IMU在slam系统中的应用(二)中给出了IMU预积分计算推导以及代码，这里给出IMU误差状态方程的推导。单个IMU数据作为测量值，其噪声方差我们可以标定，但是一段时间内多个IMU数据积分形成的预积分量的方差...
复制链接

扫一扫