多传感器融合定位十二-基于图优化的建图方法其一

news2024/11/6 3:16:20

多传感器融合定位十二-基于图优化的建图方法其一

  • 1. 基于预积分的融合方案流程
    • 1.1 优化问题分析
    • 1.2 预积分的作用
    • 1.3 基于预积分的建图方案流程
  • 2. 预积分模型设计
  • 3. 预积分在优化中的使用
    • 3.1 使用方法
    • 3.2 残差设计
    • 3.3 残差雅可比的推导
      • 3.3.1 姿态残差的雅可比
      • 3.3.2 速度残差的雅可比
      • 3.3.3 位置残差的雅可比

Reference:

  1. 深蓝学院-多传感器融合
  2. 多传感器融合定位理论基础

文章跳转:

  1. 多传感器融合定位一-3D激光里程计其一:ICP
  2. 多传感器融合定位二-3D激光里程计其二:NDT
  3. 多传感器融合定位三-3D激光里程计其三:点云畸变补偿
  4. 多传感器融合定位四-3D激光里程计其四:点云线面特征提取
  5. 多传感器融合定位五-点云地图构建及定位
  6. 多传感器融合定位六-惯性导航原理及误差分析
  7. 多传感器融合定位七-惯性导航解算及误差分析其一
  8. 多传感器融合定位八-惯性导航解算及误差分析其二
  9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
  10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
  11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
  12. 多传感器融合定位十二-基于图优化的建图方法其一
  13. 多传感器融合定位十三-基于图优化的建图方法其二
  14. 多传感器融合定位十四-基于图优化的定位方法
  15. 多传感器融合定位十五-多传感器时空标定(综述)

1. 基于预积分的融合方案流程

1.1 优化问题分析

优化问题可以等效为如下形式:
在这里插入图片描述
三种约束分别通过以下方式获得:

  1. 激光里程计约束:使用激光里程计,计算每个关键帧位姿,进而得到相对位姿;
  2. IMU约束:在上一个关键帧位姿基础上,进行惯性积分,从而得到两关键帧相对位姿;
  3. RTK约束:直接测量得到。

1.2 预积分的作用

在这里插入图片描述

问题:位姿每次优化后会发生变化,其后的IMU惯性积分就要重新进行,运算量过大。
解决思路:直接计算两帧之间的相对位姿,而不依赖初始值影响,即所谓的预积分

预积分不是为了提高精度(与粗暴方法比,而非不融合方法),而是为了提高效率。关于预积分的文章,这一篇从理解上讲的非常清晰明了:imu预积分原理的个人理解

1.3 基于预积分的建图方案流程

在这里插入图片描述
由于此处讨论的优化方案包含组合导航系统,且认为外参已标定,因此会和常见的lio/vio中的方案有所不同,它不包含以下内容:

  1. 初始化lidar和IMU之间的外参(有外参的配置文件,已经标定好了,有了这个配置文件,外参的估计就没有意义了);
  2. 初始化速度、陀螺仪bias等(因为这里有组合导航,组合导航解决了大部分的问题,组合导航完成后有初始速度、初始速度和初始姿态,同时一般的组合导航会告诉初始的bias是多少,所以在这种情况下这两项也变成了已知的问题了,不需要做初始化来估计了);
  3. 初始化重力(如果知道经纬度就知道了精确的重力,只有在不知道经纬度的情况下才需要初始化去优化重力,因为组合导航经纬度都有了,经纬度是可以通过直接计算得到的而不需要通过优化得到);
  4. 世界坐标系对齐(组合导航已经对齐)(组合导航有了姿态,这个姿态就是在世界坐标系下的一个姿态)。

2. 预积分模型设计

在第8讲中,已知导航的微分方程如下:
p ˙ w b t = v t w v ˙ t w = a t w q ˙ w b t = q w b t ⊗ [ 0 1 2 ω b t ] \begin{aligned} & \dot{\mathbf{p}}_{w b_t}=\mathbf{v}_t^w \\ & \dot{\mathbf{v}}_t^w=\mathbf{a}_t^w \\ & \dot{\mathbf{q}}_{w b_t}=\mathbf{q}_{w b_t} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{array}\right] \end{aligned} p˙wbt=vtwv˙tw=atwq˙wbt=qwbt[021ωbt]根据该微分方程,可知从 i i i 时刻到 j j j 时刻 IMU的积分结果为
p w b j = p w b i + v i w Δ t + ∬ t ∈ [ i , j ] ( q w b t a b t − g w ) δ t 2 v j w = v i w + ∫ t ∈ [ i , j ] ( q w b t a b t − g w ) δ t q w b j = ∫ t ∈ [ i , j ] q w b t ⊗ [ 0 1 2 ω b t ] δ t \begin{gathered} \mathbf{p}_{w b_j}=\mathbf{p}_{w b_i}+\mathbf{v}_i^w \Delta t+\iint_{t \in[i, j]}\left(\mathbf{q}_{w b_t} \mathbf{a}^{b_t}-\mathbf{g}^w\right) \delta t^2 \\ \mathbf{v}_j^w=\mathbf{v}_i^w+\int_{t \in[i, j]}\left(\mathbf{q}_{w b_t} \mathbf{a}^{b_t}-\mathbf{g}^w\right) \delta t \\ \mathbf{q}_{w b_j}=\int_{t \in[i, j]} \mathbf{q}_{w b_t} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{array}\right] \delta t \end{gathered} pwbj=pwbi+viwΔt+t[i,j](qwbtabtgw)δt2vjw=viw+t[i,j](qwbtabtgw)δtqwbj=t[i,j]qwbt[021ωbt]δt根据预积分的要求,需要求相对结果,而且不依赖于上一时刻位姿,因此需要对上式做转换。
由于 q w b t = q w b i ⊗ q b i b t \mathbf{q}_{w b_t}=\mathbf{q}_{w b_i} \otimes \mathbf{q}_{b_i b_t} qwbt=qwbiqbibt,把它带入(4)-(6)式可得
p w b j = p w b i + v i w Δ t − 1 2 g w Δ t 2 + q w b i [ ∬ t ∈ [ i , j ] ( q b i b t a b t ) δ t 2 v j w = v i w − g w Δ t + q w b i ∫ t ∈ [ i , j ] ( q b i b t a b t ) δ t ] q w b j = q w b i ∫ t ∈ [ i , j ] q b i b t ⊗ [ 0 1 2 ω b t ] δ t \begin{gathered} \mathbf{p}_{w b_j}=\mathbf{p}_{w b_i}+\mathbf{v}_i^w \Delta t-\frac{1}{2} \mathbf{g}^w \Delta t^2+\mathbf{q}_{w b_i}\left[\iint_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \mathbf{a}^{b_t}\right) \delta t^2\right. \\ \left.\mathbf{v}_j^w=\mathbf{v}_i^w-\mathbf{g}^w \Delta t+\mathbf{q}_{w b_i} \int_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \mathbf{a}^{b_t}\right) \delta t\right] \\ \mathbf{q}_{w b_j}=\mathbf{q}_{w b_i} \int_{t \in[i, j]} \mathbf{q}_{b_i b_t} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{array}\right] \delta t \end{gathered} pwbj=pwbi+viwΔt21gwΔt2+qwbi[t[i,j](qbibtabt)δt2vjw=viwgwΔt+qwbit[i,j](qbibtabt)δt]qwbj=qwbit[i,j]qbibt[021ωbt]δt可见,此时需要积分的项,就完全和 i i i 时刻的状态无关了。

为了整理公式,把积分相关的项用下面的式子代替:
α b i b j = ∬ t ∈ [ i , j ] ( q b i b t a b t ) δ t 2 β b i b j = ∫ t ∈ [ i , j ] ( q b i b t a b t ) δ t q b i b j = ∫ t ∈ [ i , j ] q b i b t ⊗ [ 0 1 2 ω b t ] δ t \begin{aligned} & \boldsymbol{\alpha}_{b_i b_j}=\iint_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \mathbf{a}^{b_t}\right) \delta t^2 \\ & \boldsymbol{\beta}_{b_i b_j}=\int_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \mathbf{a}^{b_t}\right) \delta t \\ & \mathbf{q}_{b_i b_j}=\int_{t \in[i, j]} \mathbf{q}_{b_i b_t} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega}^{b_t} \end{array}\right] \delta t \end{aligned} αbibj=t[i,j](qbibtabt)δt2βbibj=t[i,j](qbibtabt)δtqbibj=t[i,j]qbibt[021ωbt]δt实际使用中使用离散形式,而非连续形式,由于在解算中,一般采用中值积分方法,即:
ω = 1 2 [ ( ω b k − b k g ) + ( ω b k + 1 − b k g ) ] a = 1 2 [ q b i b k ( a b k − b k a ) + q b i b k + 1 ( a b k + 1 − b k a ) ] \begin{aligned} & \boldsymbol{\omega}=\frac{1}{2}\left[\left(\boldsymbol{\omega}^{b_k}-\mathbf{b}_k^g\right)+\left(\boldsymbol{\omega}^{b_{k+1}}-\mathbf{b}_k^g\right)\right] \\ & \mathbf{a}=\frac{1}{2}\left[\mathbf{q}_{b_i b_k}\left(\mathbf{a}^{b_k}-\mathbf{b}_k^a\right)+\mathbf{q}_{b_i b_{k+1}}\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_k^a\right)\right] \end{aligned} ω=21[(ωbkbkg)+(ωbk+1bkg)]a=21[qbibk(abkbka)+qbibk+1(abk+1bka)]那么预积分的离散形式可以表示为:
α b i b k + 1 = α b i b k + β b i b k δ t + 1 2 a δ t 2 β b i b k + 1 = β b i b k + a δ t q b i b k + 1 = q b i b k ⊗ [ 1 1 2 ω δ t ] \begin{aligned} & \boldsymbol{\alpha}_{b_i b_{k+1}}=\boldsymbol{\alpha}_{b_i b_k}+\boldsymbol{\beta}_{b_i b_k} \delta t+\frac{1}{2} \mathbf{a} \delta t^2 \\ & \boldsymbol{\beta}_{b_i b_{k+1}}=\boldsymbol{\beta}_{b_i b_k}+\mathbf{a} \delta t \\ & \mathbf{q}_{b_i b_{k+1}}=\mathbf{q}_{b_i b_k} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \boldsymbol{\omega} \delta t \end{array}\right] \end{aligned} αbibk+1=αbibk+βbibkδt+21aδt2βbibk+1=βbibk+aδtqbibk+1=qbibk[121ωδt]经过以上的推导,此时状态更新的公式可以整理为:
[ p w b j v j w q w b j b j a b j g ] = [ p w b i + v i w Δ t − 1 2 g w Δ t 2 + q w b i α b i b j v i w − g w Δ t + q w b i β b i b j q w b i q b i b j b i a b i g ] \left[\begin{array}{c} \mathbf{p}_{w b_j} \\ \mathbf{v}_j^w \\ \mathbf{q}_{w b_j} \\ \mathbf{b}_j^a \\ \mathbf{b}_j^g \end{array}\right]=\left[\begin{array}{c} \mathbf{p}_{w b_i}+\mathbf{v}_i^w \Delta t-\frac{1}{2} \mathbf{g}^w \Delta t^2+\mathbf{q}_{w b_i} \boldsymbol{\alpha}_{b_i b_j} \\ \mathbf{v}_i^w-\mathbf{g}^w \Delta t+\mathbf{q}_{w b_i} \boldsymbol{\beta}_{b_i b_j} \\ \mathbf{q}_{w b_i} \mathbf{q}_{b_i b_j} \\ \mathbf{b}_i^a \\ \mathbf{b}_i^g \end{array}\right] pwbjvjwqwbjbjabjg = pwbi+viwΔt21gwΔt2+qwbiαbibjviwgwΔt+qwbiβbibjqwbiqbibjbiabig 需要注意的是,陀螺仪和加速度计的模型为:
b k + 1 a = b k a + n b k a δ t b k + 1 g = b k g + n b k g δ t \begin{array}{r} \mathbf{b}_{k+1}^a=\mathbf{b}_k^a+\mathbf{n}_{\mathbf{b}_k^a} \delta t \\ \mathbf{b}_{k+1}^g=\mathbf{b}_k^g+\mathbf{n}_{\mathbf{b}_k^g} \delta t \end{array} bk+1a=bka+nbkaδtbk+1g=bkg+nbkgδt即认为bias是在变化的,这样便于估计不同时刻的bias值,而不是整个系统运行时间内都当做常值对待。这更符合低精度mems的实际情况。但在预积分时,由于两个关键帧之间的时间较短,因此认为 i i i j j j 时刻的bias相等。

需要注意的一点是,预积分的结果中包含了bias,在优化过程中,bias作为状态量也会发生变化,从而引起预积分结果变化。
为了避免bias变化后,重新做预积分,可以把预积分结果在bias处泰勒展开,表达成下面的形式,这样就可以根据bias的变化量直接算出新的预积分结果。
α b i b j = α ‾ b i b j + J b i a α δ b i a + J b i g α δ b i g β b i b j = β ‾ b i b j + J b i a β δ b i a + J b i g β δ b i g q b i b j = q ‾ b i b j ⊗ [ 1 1 2 J b i q q δ b i g ] \begin{aligned} & \boldsymbol{\alpha}_{b_i b_j}=\overline{\boldsymbol{\alpha}}_{b_i b_j}+\mathbf{J}_{b_i^a}^\alpha \delta \mathbf{b}_i^a+\mathbf{J}_{b_i^g}^\alpha \delta \mathbf{b}_i^g \\ & \boldsymbol{\beta}_{b_i b_j}=\overline{\boldsymbol{\beta}}_{b_i b_j}+\mathbf{J}_{b_i^a}^\beta \delta \mathbf{b}_i^a+\mathbf{J}_{b_i^g}^\beta \delta \mathbf{b}_i^g \\ & \mathbf{q}_{b_i b_j}=\overline{\mathbf{q}}_{b_i b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_i^q}^q \delta \mathbf{b}_i^g \end{array}\right] \end{aligned} αbibj=αbibj+Jbiaαδbia+Jbigαδbigβbibj=βbibj+Jbiaβδbia+Jbigβδbigqbibj=qbibj[121Jbiqqδbig]其中:
J b i a α = ∂ α b i b j ∂ δ b i a J b i g α = ∂ α b i b j ∂ δ b i j J b i a β = ∂ β b i b j ∂ δ b i a J b i g β = ∂ β b i b j ∂ δ b i a J b i g q = q b i b j ∂ b i q \begin{aligned} \mathbf{J}_{b_i^a}^\alpha & =\frac{\partial \alpha_{b_i b_j}}{\partial \delta \mathbf{b}_i^a} \\ \mathbf{J}_{b_i^g}^\alpha & =\frac{\partial \boldsymbol{\alpha}_{b_i b_j}}{\partial \delta \mathbf{b}_i^j} \\ \mathbf{J}_{b_i^a}^\beta & =\frac{\partial \beta_{b_i b_j}}{\partial \delta \mathbf{b}_i^a} \\ \mathbf{J}_{b_i^g}^\beta & =\frac{\partial \beta_{b_i b_j}}{\partial \delta \mathbf{b}_i^a} \\ \mathbf{J}_{b_i^g}^q & =\frac{\mathbf{q}_{b_i b_j}}{\partial \mathbf{b}_i^q} \end{aligned} JbiaαJbigαJbiaβJbigβJbigq=δbiaαbibj=δbijαbibj=δbiaβbibj=δbiaβbibj=biqqbibj注: 此处暂时不直接给出以上各雅可比的结果,它的推导放在后面进行。

3. 预积分在优化中的使用

3.1 使用方法

  1. 凸优化回顾
    损失函数由残差函数组成:
    min ⁡ x F ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 \min _x F(x)=\frac{1}{2}\|f(x)\|_2^2 xminF(x)=21f(x)22当考虑方差时,可以写为:
    min ⁡ x F ( x ) = f ( x ) T Ω f ( x ) \min _x F(x)=f(x)^T \Omega f(x) xminF(x)=f(x)TΩf(x)利用高斯牛顿方法,求解该优化问题,需要解下面的方程:
    J T Ω J ⏟ H Δ x = − J T Ω f ( x ) ⏟ g \underbrace{J^T \Omega J}_H \Delta x=\underbrace{-J^T \Omega f(x)}_g H JTΩJΔx=g JTΩf(x)即,在优化中使用一项信息,需要设计残差, 并推导它的雅可比和方差。
  2. 预积分的使用
    按照优化的套路,在优化中使用预积分,需要:
    a. 设计残差
    b. 推导残差关于待优化变量的雅可比
    c. 计算残差的方差
    除此以外,预积分中还多一项计算,即推导bias变化时,预积分量重新计算的方法。

3.2 残差设计

在优化时,需要知道残差关于状态量的雅可比。由于已知姿态位姿更新的方法如下:
[ p w b j q w b j v j w b j a b j g ] = [ p w b i + v i w Δ t − 1 2 g w Δ t 2 + q w b i α b i b j q w b i q b i b j v i w − g w Δ t + q w b i β b i b j b i a b i g ] \left[\begin{array}{c} \mathbf{p}_{w b_j} \\ \mathbf{q}_{w b_j} \\ \mathbf{v}_j^w \\ \mathbf{b}_j^a \\ \mathbf{b}_j^g \end{array}\right]=\left[\begin{array}{c} \mathbf{p}_{w b_i}+\mathbf{v}_i^w \Delta t-\frac{1}{2} \mathbf{g}^w \Delta t^2+\mathbf{q}_{w b_i} \boldsymbol{\alpha}_{b_i b_j} \\ \mathbf{q}_{w b_i} \mathbf{q}_{b_i b_j} \\ \mathbf{v}_i^w-\mathbf{g}^w \Delta t+\mathbf{q}_{w b_i} \boldsymbol{\beta}_{b_i b_j} \\ \mathbf{b}_i^a \\ \mathbf{b}_i^g \end{array}\right] pwbjqwbjvjwbjabjg = pwbi+viwΔt21gwΔt2+qwbiαbibjqwbiqbibjviwgwΔt+qwbiβbibjbiabig 因此,可以很容易写出一种残差形式如下:
[ r p r q r v r b a r b g ] = [ p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 − q w b i α b i b j 2 [ q b i b j ∗ ⊗ ( q w b i ∗ ⊗ q w b j ) ] x y z v j w − v i w + g w Δ t − q w b i β b i b j b j a − b i a b j g − b i g ] \left[\begin{array}{c} \mathbf{r}_p \\ \mathbf{r}_q \\ \mathbf{r}_v \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b g} \end{array}\right]=\left[\begin{array}{c} \mathbf{p}_{w b_j}-\mathbf{p}_{w b_i}-\mathbf{v}_i^w \Delta t+\frac{1}{2} \mathbf{g}^w \Delta t^2-\mathbf{q}_{w b_i} \boldsymbol{\alpha}_{b_i b_j} \\ 2\left[\mathbf{q}_{b_i b_j}^* \otimes\left(\mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right)\right]_{x y z} \\ \mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t-\mathbf{q}_{w b_i} \boldsymbol{\beta}_{b_i b_j} \\ \mathbf{b}_j^a-\mathbf{b}_i^a \\ \mathbf{b}_j^g-\mathbf{b}_i^g \end{array}\right] rprqrvrbarbg = pwbjpwbiviwΔt+21gwΔt2qwbiαbibj2[qbibj(qwbiqwbj)]xyzvjwviw+gwΔtqwbiβbibjbjabiabjgbig 但是和预积分相关的量,仍然与上一时刻的姿态有关,无法直接加减,因此,把残差修正为以下形式:
[ r p r q r v r b a r b g ] = [ q w b i ∗ ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j 2 [ q q b j ∗ ⊗ ( q w i ∗ ⊗ q w b j ) ] x y z q w b i ∗ ( v j w − v i w + g w Δ t ) − β b i b j b j a − b i a b j g − b i g ] \left[\begin{array}{c} \mathbf{r}_p \\ \mathbf{r}_q \\ \mathbf{r}_v \\ \mathbf{r}_{b a} \\ \mathbf{r}_{b g} \end{array}\right]=\left[\begin{array}{c} \mathbf{q}_{w b_i}^*\left(\mathbf{p}_{w b_j}-\mathbf{p}_{w b_i}-\mathbf{v}_i^w \Delta t+\frac{1}{2} \mathbf{g}^w \Delta t^2\right)-\boldsymbol{\alpha}_{b_i b_j} \\ 2\left[\mathbf{q}_{\mathbf{q}_{b_j}^*} \otimes\left(\mathbf{q}_{w_i}^* \otimes \mathbf{q}_{w b_j}\right)\right]_{x y z} \\ \mathbf{q}_{w b_i}^*\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)-\boldsymbol{\beta}_{b_i b_j} \\ \mathbf{b}_j^a-\mathbf{b}_i^a \\ \mathbf{b}_j^g-\mathbf{b}_i^g \end{array}\right] rprqrvrbarbg = qwbi(pwbjpwbiviwΔt+21gwΔt2)αbibj2[qqbj(qwiqwbj)]xyzqwbi(vjwviw+gwΔt)βbibjbjabiabjgbig 待优化的变量是 [ p w b i q w b i v i w b i a b i g ] [ p w b j q w b j v j w b j a b j g ] \left[\begin{array}{lllll}\mathbf{p}_{w b_i} & \mathbf{q}_{w b_i} & \mathbf{v}_i^w & \mathbf{b}_i^a & \mathbf{b}_i^g\end{array}\right]\left[\begin{array}{lllll}\mathbf{p}_{w b_j} & \mathbf{q}_{w b_j} & \mathbf{v}_j^w & \mathbf{b}_j^a & \mathbf{b}_j^g\end{array}\right] [pwbiqwbiviwbiabig][pwbjqwbjvjwbjabjg]
但在实际使用中,往往都是使用扰动量,因此实际是对以下变量求雅可比
[ δ p w b i δ θ w b i δ v i w δ b i a δ b i g ] [ δ p w b j δ θ w b j δ v j w δ b j a δ b j g ] \begin{aligned} & {\left[\begin{array}{lllll} \delta \mathbf{p}_{w b_i} & \delta \theta_{w b_i} & \delta \mathbf{v}_i^w & \delta \mathbf{b}_i^a & \delta \mathbf{b}_i^g \end{array}\right]} \\ & {\left[\begin{array}{lllll} \delta \mathbf{p}_{w b_j} & \delta \theta_{w b_j} & \delta \mathbf{v}_j^w & \delta \mathbf{b}_j^a & \delta \mathbf{b}_j^g \end{array}\right]} \end{aligned} [δpwbiδθwbiδviwδbiaδbig][δpwbjδθwbjδvjwδbjaδbjg]此处只对几个比较复杂的雅可比进行推导,其余比较简单,感兴趣的可自行完成。

3.3 残差雅可比的推导

3.3.1 姿态残差的雅可比

  1. i i i 时刻姿态误差的雅可比:
    ∂ r q ∂ δ θ b i b i ′ = ∂ 2 [ q b j b i ⊗ ( q b i w ⊗ q w b j ) ] x y z ∂ δ θ b i b i ′ = ∂ 2 [ q b i , b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ′ ] ) ∗ ⊗ q w b j ] x y z ∂ δ θ b i b i ′ = ∂ − 2 [ ( q b i b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ′ ] ) ∗ ⊗ q w b j ) ∗ ] x y z ∂ δ θ b i b i = ∂ − 2 [ q w b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ′ ] ) ⊗ q b i b j ] x y z ∂ δ θ b i b i ′ \begin{aligned} \frac{\partial \mathbf{r}_q}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} & =\frac{\partial 2\left[\mathbf{q}_{b_j b_i} \otimes\left(\mathbf{q}_{b_i w} \otimes \mathbf{q}_{w b_j}\right)\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial 2\left[\mathbf{q}_{b_i, b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_j}\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial-2\left[\left(\mathbf{q}_{b_i b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i} b_i^{\prime} \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_j}\right)^*\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i}} \\ & =\frac{\partial-2\left[\mathbf{q}_{w b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]\right) \otimes \mathbf{q}_{b_i b_j}\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \end{aligned} δθbibirq=δθbibi2[qbjbi(qbiwqwbj)]xyz=δθbibi2[qbi,bj(qwbi[121δθbibi])qwbj]xyz=δθbibi2[(qbibj(qwbi[121δθbibi])qwbj)]xyz=δθbibi2[qwbj(qwbi[121δθbibi])qbibj]xyz上式可以化简为:
    ∂ r q ∂ δ θ b i b i ′ = − 2 [ 0 I ] ∂ q w b j ∗ ⊗ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ′ ] ) ⊗ q b i b j ∂ δ θ b i b i ′ = − 2 [ 0 I ] ∂ [ q w b j ∗ ⊗ q w b i ] L [ q b i b j ] R [ 1 1 2 δ θ b i b i ′ ] ∂ δ θ b i b i ′ = − 2 [ 0 I ] [ q w b j ∗ ⊗ q w b i ] L [ q b i b j ] R [ 0 1 2 I ] \begin{aligned} \frac{\partial \mathbf{r}_q}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right] \frac{\partial \mathbf{q}_{w b_j}^* \otimes\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]\right) \otimes \mathbf{q}_{b_i b_j}}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right] \frac{\partial\left[\mathbf{q}_{w b_j}^* \otimes \mathbf{q}_{w b_i}\right]_L\left[\mathbf{q}_{b_i b_j}\right]_R\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i^{\prime}} \end{array}\right]}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{w b_j}^* \otimes \mathbf{q}_{w b_i}\right]_L\left[\mathbf{q}_{b_i b_j}\right]_R\left[\begin{array}{c} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{array}\right] \end{aligned} δθbibirq=2[0I]δθbibiqwbj(qwbi[121δθbibi])qbibj=2[0I]δθbibi[qwbjqwbi]L[qbibj]R[121δθbibi]=2[0I][qwbjqwbi]L[qbibj]R[021I]

  2. j j j 时刻姿态误差的雅可比:
    ∂ r q ∂ δ θ b j b j ′ = ∂ 2 [ q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ⊗ [ 1 1 2 δ θ b j b j ′ ] ] x y z ∂ δ θ b j b j ′ = ∂ 2 [ [ q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ] L [ 1 1 2 δ θ b j b j ′ ] ] x y z ∂ δ θ b j b j ′ = 2 [ 0 I ] [ q b i b j ∗ ⊗ q w b i ∗ ⊗ q w b j ] L [ 0 1 2 I ] \begin{aligned} \frac{\partial \mathbf{r}_q}{\partial \delta \boldsymbol{\theta}_{b_j b_j^{\prime}}} & =\frac{\partial 2\left[\mathbf{q}_{b_i b_j}^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_j b_j^{\prime}} \end{array}\right]\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_j b_j^{\prime}}} \\ & =\frac{\partial 2\left[\left[\mathbf{q}_{b_i b_j}^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right]_L\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_j b_j^{\prime}} \end{array}\right]\right]_{x y z}}{\partial \delta \boldsymbol{\theta}_{b_j b_j^{\prime}}} \\ & =2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{b_i b_j}^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right]_L\left[\begin{array}{c} \mathbf{0} \\ \frac{1}{2} \mathbf{I} \end{array}\right] \end{aligned} δθbjbjrq=δθbjbj2[qbibjqwbiqwbj[121δθbjbj]]xyz=δθbjbj2[[qbibjqwbiqwbj]L[121δθbjbj]]xyz=2[0I][qbibjqwbiqwbj]L[021I]

  3. i i i 时刻陀螺仪bias误差的雅可比:
    ∂ r q ∂ δ b i g = ∂ 2 [ ( q b i b j ⊗ [ 1 1 2 J b i q δ b i g q ] ) ∗ ⊗ q w b i ∗ ⊗ q w b j ] x y z ∂ δ b i g = ∂ − 2 [ ( ( q b i b j ⊗ [ 1 1 2 J b i q q δ b i g ] ) ∗ ⊗ q w b i ∗ ⊗ q w b j ) ∗ ] x y z ∂ δ b i g = ∂ − 2 [ q w b j ∗ ⊗ q w b i ⊗ ( q b i b j ⊗ [ 1 1 2 J b i o δ b i g ] ) ] x y z ∂ δ b i g = − 2 [ 0 I ] [ q w b j ∗ ⊗ q w b i ⊗ q b i b j ] L [ 0 1 2 J b i q q ] \begin{aligned} & \frac{\partial \mathbf{r}_q}{\partial \delta \mathbf{b}_i^g}=\frac{\partial 2\left[\left(\mathbf{q}_{b_i b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_i^q \delta \mathbf{b}_i^g}^q \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right]_{x y z}}{\partial \delta \mathbf{b}_i^g} \\ & =\frac{\partial-2\left[\left(\left(\mathbf{q}_{b_i b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_i^q}^q \delta \mathbf{b}_i^g \end{array}\right]\right)^* \otimes \mathbf{q}_{w b_i}^* \otimes \mathbf{q}_{w b_j}\right)^*\right]_{x y z}}{\partial \delta \mathbf{b}_i^g} \\ & =\frac{\partial-2\left[\mathbf{q}_{w b_j}^* \otimes \mathbf{q}_{w b_i} \otimes\left(\mathbf{q}_{b_i b_j} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_i^o} \delta \mathbf{b}_i^g \end{array}\right]\right)\right]_{x y z}}{\partial \delta \mathbf{b}_i^g} \\ & =-2\left[\begin{array}{ll} \mathbf{0} & \mathbf{I} \end{array}\right]\left[\mathbf{q}_{w b_j}^* \otimes \mathbf{q}_{w b_i} \otimes \mathbf{q}_{b_i b_j}\right]_L\left[\begin{array}{c} 0 \\ \frac{1}{2} \mathbf{J}_{b_i^q}^q \end{array}\right] \\ & \end{aligned} δbigrq=δbig2[(qbibj[121Jbiqδbigq])qwbiqwbj]xyz=δbig2[((qbibj[121Jbiqqδbig])qwbiqwbj)]xyz=δbig2[qwbjqwbi(qbibj[121Jbioδbig])]xyz=2[0I][qwbjqwbiqbibj]L[021Jbiqq]

3.3.2 速度残差的雅可比

  1. i i i 时刻姿态误差的雅可比:
    ∂ r v ∂ δ θ b i b i = ∂ ( q w b i ⊗ [ 1 1 2 δ θ b i b i ] ) − 1 ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = ∂ ( R w b i exp ⁡ ( [ δ θ b i b i ′ ] × ) ) − 1 ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = ∂ exp ⁡ ( [ − δ θ b i b i ] × ) R b i w ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = ∂ ( I − [ δ θ b i b i ] × ) R b i w ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = ∂ − [ δ θ b i b i ] × R b i w ( v j w − v i w + g w Δ t ) ∂ δ θ b i b i ′ = [ R b i w ( v j w − v i w + g w Δ t ) ] × \begin{aligned} \frac{\partial \mathbf{r}_v}{\partial \delta \boldsymbol{\theta}_{b_i b_i}} & =\frac{\partial\left(\mathbf{q}_{w b_i} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta}_{b_i b_i} \end{array}\right]\right)^{-1}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial\left(\mathbf{R}_{w b_i} \exp \left(\left[\delta \boldsymbol{\theta}_{b_i b_i^{\prime}}\right]_{\times}\right)\right)^{-1}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial \exp \left(\left[-\delta \boldsymbol{\theta}_{b_i b_i}\right]_{\times}\right) \mathbf{R}_{b_i w}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i} b_i^{\prime}} \\ & =\frac{\partial\left(\mathbf{I}-\left[\delta \boldsymbol{\theta}_{b_i b_i}\right]_{\times}\right) \mathbf{R}_{b_i w}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\frac{\partial-\left[\delta \boldsymbol{\theta}_{b_i b_i}\right]_{\times} \mathbf{R}_{b_i w}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)}{\partial \delta \boldsymbol{\theta}_{b_i b_i^{\prime}}} \\ & =\left[\mathbf{R}_{b_i w}\left(\mathbf{v}_j^w-\mathbf{v}_i^w+\mathbf{g}^w \Delta t\right)\right]_{\times} \end{aligned} δθbibirv=δθbibi(qwbi[121δθbibi])1(vjwviw+gwΔt)=δθbibi(Rwbiexp([δθbibi]×))1(vjwviw+gwΔt)=δθbibiexp([δθbibi]×)Rbiw(vjwviw+gwΔt)=δθbibi(I[δθbibi]×)Rbiw(vjwviw+gwΔt)=δθbibi[δθbibi]×Rbiw(vjwviw+gwΔt)=[Rbiw(vjwviw+gwΔt)]×
  2. i i i 时刻速度误差的雅可比:
    ∂ r v ∂ δ v i w = − R b i w \frac{\partial \mathbf{r}_v}{\partial \delta \mathbf{v}_i^w}=-\mathbf{R}_{b_i w} δviwrv=Rbiw
  3. i i i 时刻加速度biasi吴差的雅可比:
    ∂ r v ∂ δ b i a = − ∂ β b i b j ∂ δ b i a = − J b i a β \frac{\partial \mathbf{r}_v}{\partial \delta \mathbf{b}_i^a}=-\frac{\partial \boldsymbol{\beta}_{b_i b_j}}{\partial \delta \mathbf{b}_i^a}=-\mathbf{J}_{b_i^a}^\beta δbiarv=δbiaβbibj=Jbiaβ

3.3.3 位置残差的雅可比

由于位置残差的形式与速度残差极其相似,因此不再重复推导。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/338290.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

Python3.10新特性之match语句示例详解

这篇文章主要为大家介绍了Python3.10新特性之match语句示例详解,有需要的朋友可以借鉴参考下,希望能够有所帮助,祝大家多多进步,早日升职加薪正文在Python 3.10发布之前,Python是没有类似于其他语言中switch语句的&…

Clip-path实现按钮流动边框动画

前言 &#x1f44f;Clip-path实现按钮流动边框动画&#xff0c;速速来Get吧~ &#x1f947;文末分享源代码。记得点赞关注收藏&#xff01; 1.实现效果 2.实现步骤 添加div标签 <div>苏苏_icon</div>添加样式 div {position: relative;width: 220px;height: 6…

1947抓住那头牛(队列 广度优先搜索)

目录 题目描述 解析 解题思路 代码部分 代码部分 运行结果 看看len数组中各个位置的标记值 为什么这样做一定是最短路径&#xff1a; 题目描述 农夫知道一头牛的位置&#xff0c;想要抓住它。农夫和牛都位于数轴上&#xff0c;农夫起始位于点N(0<N<100000)&…

Java八股文(Java面试题)

JDK、JRE、JVM 三者之间的关系&#xff1f;JDK&#xff08;Java Development Kit&#xff09;&#xff1a;是Java开发工具包&#xff0c;是整个Java的核心&#xff0c;包括了Java运行环境JRE、Java工具和Java基础类库。它能够创建和编译程序。JRE&#xff08;Java Runtime Envi…

MySQL-字符集和比较规则

在计算机中只能存储二进制数据&#xff0c;那该怎么存储字符串呢&#xff1f;当然是建立字符与二进制数据的映射关系 了&#xff0c;建立这个关系最起码要搞清楚两件事&#xff1a; 界定清楚字符范围&#xff1a;需要把哪些字符映射成二进制数据&#xff1f;编码与解码&#x…

九龙证券|外资强势出手!这只科创板百元股,被疯狂加仓

本周&#xff0c;北上资金净买入29.32亿元&#xff0c;连续第13周加仓A股。分商场看&#xff0c;北上资金加仓重点倾向于沪市的白马蓝筹股&#xff0c;沪股通取得50.34亿元&#xff0c;深股通则被净卖出21.02亿元。 食品饮料本周取得逾23亿元的增持&#xff0c;居职业首位&…

leaflet 读取上传的geojson文件,转换为wkt文件(057)

第057个 点击查看专栏目录 本示例的目的是介绍演示如何在vue+leaflet中上传geojson文件,解析geojson文件并转换为WKT,并在地图上显示图片。 直接复制下面的 vue+openlayers源代码,操作2分钟即可运行实现效果. 文章目录 示例效果配置方式示例源代码(共128行)安装 @terraf…

软件测试/自动化测试/测试开发/性能测试经典好书推荐

目录 前言 测试理论与实践 性能测试 安全测试 自动化测试 编程语言与开发技能 管理技能 前言 软件测试入行容易进阶难。从测试小白成长为测试经理、总监级别人才&#xff0c;要跨越长长的技术栈升级之路&#xff0c;挑战非常高的综合能力要求。 大牛都是相似的&#xf…

五分钟看懂Java字节码:极简手册

字节码新手很容易被厚厚的 JVM 书籍劝退&#xff0c;即使我看过相关书籍&#xff0c;工作真正用到时也全忘了&#xff0c;还得现学。 等我有了一定的字节码阅读经验&#xff0c;才发现字节码其实非常简单&#xff0c;只需要三步就能快速学会&#xff1a; 先了解 JVM 的基本结…

【设计模式之美 设计原则与思想:面向对象】14 | 实战二(下):如何利用面向对象设计和编程开发接口鉴权功能?

在上一节课中&#xff0c;针对接口鉴权功能的开发&#xff0c;我们讲了如何进行面向对象分析&#xff08;OOA&#xff09;&#xff0c;也就是需求分析。实际上&#xff0c;需求定义清楚之后&#xff0c;这个问题就已经解决了一大半&#xff0c;这也是为什么我花了那么多篇幅来讲…

创建Django项目

创建Django项目 步骤 创建Django项目 django-admin startproject name 创建子应用 python manager.py startapp name创建工程 在使用Flask框架时&#xff0c;项目工程目录的组织与创建是需要我们自己手动创建完成的。 在django中&#xff0c;项目工程目录可以借助django提供…

嵌软工程师要掌握的硬件知识2:一文看懂什么开漏和推挽电路(open-drain / push-pull)

想了解开漏和推挽,就要先了解一下三极管和场效应管是什么,在其他章节有详细介绍,本文就不再进行赘述。 1 推挽(push pull)电路 1.1 理解什么是推挽电路 - 详细介绍 如图所示,Q3是个NPN型三极管,Q4是个PNP型三极管。 1)当Vin电压为正时,上面的N型三极管控制端有电…

ccc-Classification-李宏毅(4)

文章目录Classification 概念Example ApplicationHow to do ClassificationWhy not RegesssionProbability from Class - FeatureProbability from ClassHow’s the results?Modifying ModelThree StepsProbability DistributionClassification 概念 本质是找一个函数&#x…

电商导购CPS,淘宝联盟如何跟单实现用户和订单绑定

前言 大家好&#xff0c;我是小悟 做过自媒体的小伙伴都知道&#xff0c;不管是发图文还是发短视频&#xff0c;直播也好&#xff0c;可以带货。在你的内容里面挂上商品&#xff0c;你自己都不需要囤货&#xff0c;如果用户通过这个商品下单成交了&#xff0c;自媒体平台就会…

基于 MySQL 排它锁实现分布式可重入锁解决方案

一、MySQL 排它锁和共享锁 在进行实验前&#xff0c;先来了解下MySQL 的排它锁和共享锁&#xff0c;在 MySQL 中的锁分为表锁和行锁&#xff0c;在行锁中锁又分成了排它锁和共享锁两种类型。 1. 排它锁 排他锁又称为写锁&#xff0c;简称X锁&#xff0c;是一种悲观锁&#x…

【C++】模板初阶STL简介

今天&#xff0c;你内卷了吗&#xff1f; 文章目录一、泛型编程二、函数模板&#xff08;显示实例化和隐式实例化&#xff09;1.函数模板格式2.单参数模板3.多参数模板4.模板参数的匹配原则三、类模板&#xff08;没有推演的时机&#xff0c;统一显示实例化&#xff09;1.类模…

RTOS之二环境搭建初识RTOS

参考&#xff1a;https://blog.csdn.net/kouxi1/article/details/123650688RTOS本质就是切换线程栈&#xff0c;栈换了环境就换了&#xff0c;一个重要的结构tcb&#xff08;linux叫PCB或thread_info&#xff09;&#xff1a;struct tcb{int32_t *sp; // 重要的sp指针&#xff…

seata【SAGA模式】代码实践(细节未必完全符合saga的配置,仅参考)

seata SAGA模式&#xff1a; 代码仍然是上一篇AT模式的代码&#xff1a;AT模式 不需要undo_log表 下面开始&#xff1a; 首先&#xff0c;saga模式依靠状态机的json文件来执行整个流程&#xff0c;其中的开始节点的服务即TM&#xff0c;然后状态机需要依靠三张表&#xff0…

【大数据】Hadoop-HA-Federation-3.3.1集群高可用联邦安装部署文档(建议收藏哦)

背景概述 单 NameNode 的架构使得 HDFS 在集群扩展性和性能上都有潜在的问题&#xff0c;当集群大到一定程度后&#xff0c;NameNode 进程使用的内存可能会达到上百 G&#xff0c;NameNode 成为了性能的瓶颈。因而提出了 namenode 水平扩展方案-- Federation。 Federation 中…

C语言---字符串函数总结

&#x1f680;write in front&#x1f680; &#x1f4dd;个人主页&#xff1a;认真写博客的夏目浅石. &#x1f381;欢迎各位→点赞&#x1f44d; 收藏⭐️ 留言&#x1f4dd; &#x1f4e3;系列专栏&#xff1a;夏目的C语言宝藏 &#x1f4ac;总结&#xff1a;希望你看完之…