天津网站建设价格,云主机如何建网站,外贸公司系统管理软件,项目网络计划软件教程多传感器融合定位十二-基于图优化的建图方法其二3.4 预积分方差计算3.4.1 核心思路3.4.2 连续时间下的微分方程3.4.3 离散时间下的传递方程3.5 预积分更新4. 典型方案介绍4.1 LIO-SAM介绍5. 融合编码器的优化方案5.1 整体思路介绍5.2 预积分模型设计Reference:
深蓝学院-多传感…
多传感器融合定位十二-基于图优化的建图方法其二3.4 预积分方差计算3.4.1 核心思路3.4.2 连续时间下的微分方程3.4.3 离散时间下的传递方程3.5 预积分更新4. 典型方案介绍4.1 LIO-SAM介绍5. 融合编码器的优化方案5.1 整体思路介绍5.2 预积分模型设计Reference:
深蓝学院-多传感器融合多传感器融合定位理论基础
文章跳转
多传感器融合定位一-3D激光里程计其一ICP多传感器融合定位二-3D激光里程计其二NDT多传感器融合定位三-3D激光里程计其三点云畸变补偿多传感器融合定位四-3D激光里程计其四点云线面特征提取多传感器融合定位五-点云地图构建及定位多传感器融合定位六-惯性导航原理及误差分析多传感器融合定位七-惯性导航解算及误差分析其一多传感器融合定位八-惯性导航解算及误差分析其二多传感器融合定位九-基于滤波的融合方法Ⅰ其一多传感器融合定位十-基于滤波的融合方法Ⅰ其二多传感器融合定位十一-基于滤波的融合方法Ⅱ多传感器融合定位十二-基于图优化的建图方法其一多传感器融合定位十三-基于图优化的建图方法其二多传感器融合定位十四-基于图优化的定位方法多传感器融合定位十五-多传感器时空标定(综述)
3.4 预积分方差计算
3.4.1 核心思路
在融合时需要给不同信息设置权重而权重由方差得来因此对于IMU积分也要计算其方差。方差的计算形式与第四章相同即 Pi,k1FkPi,kFk⊤BkQBk⊤\boldsymbol{P}_{i, k1}\mathbf{F}_k \boldsymbol{P}_{i, k} \mathbf{F}_k^{\top}\mathbf{B}_k \boldsymbol{Q} \mathbf{B}_k^{\top} Pi,k1FkPi,kFk⊤BkQBk⊤但需注意的是此处 Fk\boldsymbol{F}_kFk 和 Gk\boldsymbol{G}_kGk 是离散时间下的状态传递方程中的矩阵而我们一般是在连续时间下推导微分方程再用它计算离散时间下的传递方程。
3.4.2 连续时间下的微分方程
连续时间下的微分方程一般写为如下形式 x˙FtxBtwx[δαtbkδθtbkδβtbkδbatδbwt]w[nanwnbanbw]\begin{gathered} \dot{\boldsymbol{x}}\boldsymbol{F}_t \boldsymbol{x}\boldsymbol{B}_t \boldsymbol{w} \\ \boldsymbol{x}\left[\begin{array}{l} \delta \boldsymbol{\alpha}_t^{b_k} \\ \delta \boldsymbol{\theta}_t^{b_k} \\ \delta \boldsymbol{\beta}_t^{b_k} \\ \delta \boldsymbol{b}_{\boldsymbol{a}_t} \\ \delta \boldsymbol{b}_{w_t} \end{array}\right] \\ \boldsymbol{w}\left[\begin{array}{l} \boldsymbol{n}_a \\ \boldsymbol{n}_w \\ \boldsymbol{n}_{b_a} \\ \boldsymbol{n}_{b_w} \end{array}\right] \end{gathered} x˙FtxBtwxδαtbkδθtbkδβtbkδbatδbwtwnanwnbanbw以上变量排列顺序是为了与lio/vio等常见系统顺序保持一致。
推导方法已经在第6讲介绍此处直接给出结果 δθ˙tbk\delta \dot{\boldsymbol{\theta}}_t^{b_k}δθ˙tbk 的微分方程 δθ˙tbk−[ωt−bωt]×δθtbknω−δbωt\delta \dot{\boldsymbol{\theta}}_t^{b_k}-\left[\boldsymbol{\omega}_t-\boldsymbol{b}_{\omega_t}\right]_{\times} \delta \boldsymbol{\theta}_t^{b_k}\boldsymbol{n}_\omega-\delta \boldsymbol{b}_{\omega_t} δθ˙tbk−[ωt−bωt]×δθtbknω−δbωtδβ˙tbk\delta \dot{\boldsymbol{\beta}}_t^{b_k}δβ˙tbk 的微分方程 δβ˙tbk−Rt[at−bat]×δθtbkRt(na−δbat)\delta \dot{\boldsymbol{\beta}}_t^{b_k}-\boldsymbol{R}_t\left[\boldsymbol{a}_t-\boldsymbol{b}_{a_t}\right]_{\times} \delta \boldsymbol{\theta}_t^{b_k}\boldsymbol{R}_t\left(\boldsymbol{n}_a-\delta \boldsymbol{b}_{a_t}\right) δβ˙tbk−Rt[at−bat]×δθtbkRt(na−δbat)δα˙tbk\delta \dot{\boldsymbol{\alpha}}_t^{b_k}δα˙tbk 的微分方程 δα˙tbkδβtbk\delta \dot{\boldsymbol{\alpha}}_t^{b_k}\delta \boldsymbol{\beta}_t^{b_k} δα˙tbkδβtbk
3.4.3 离散时间下的传递方程
得到连续时间微分方程以后就可以计算离散时间的递推方程了表示为 xk1FkxkBkwk\boldsymbol{x}_{k1}\boldsymbol{F}_k \boldsymbol{x}_k\boldsymbol{B}_k \boldsymbol{w}_k xk1FkxkBkwk其中 xk1[δαk1δθk1δβk1δbak1δbωk1]xk[δαkδθkδβkδbakδbωk]wk[naknwknak1nwk1nbanbw]\boldsymbol{x}_{k1}\left[\begin{array}{l} \delta \boldsymbol{\alpha}_{k1} \\ \delta \boldsymbol{\theta}_{k1} \\ \delta \boldsymbol{\beta}_{k1} \\ \delta \boldsymbol{b}_{a_{k1}} \\ \delta \boldsymbol{b}_{\omega_{k1}} \end{array}\right] \quad \boldsymbol{x}_k\left[\begin{array}{c} \delta \boldsymbol{\alpha}_k \\ \delta \boldsymbol{\theta}_k \\ \delta \boldsymbol{\beta}_k \\ \delta \boldsymbol{b}_{a_k} \\ \delta \boldsymbol{b}_{\omega_k} \end{array}\right] \quad \boldsymbol{w}_k\left[\begin{array}{c} \boldsymbol{n}_{a_k} \\ \boldsymbol{n}_{w_k} \\ \boldsymbol{n}_{a_{k1}} \\ \boldsymbol{n}_{w_{k1}} \\ \boldsymbol{n}_{b_a} \\ \boldsymbol{n}_{b_w} \end{array}\right] xk1δαk1δθk1δβk1δbak1δbωk1xkδαkδθkδβkδbakδbωkwknaknwknak1nwk1nbanbw δθk1\delta \boldsymbol{\theta}_{k1}δθk1 的求解 由于连续时间下有 δθ˙−[ωt−bωt]×δθnω−δbωt\delta \dot{\boldsymbol{\theta}}-\left[\boldsymbol{\omega}_t-\boldsymbol{b}_{\omega_t}\right]_{\times} \delta \boldsymbol{\theta}\boldsymbol{n}_\omega-\delta \boldsymbol{b}_{\omega_t} δθ˙−[ωt−bωt]×δθnω−δbωt则离散时间下应该有 因此有 δθ˙k−[ωkωk12−bωt]×δθknωknωk12−δbωkδθk1[I−[ωkωk12−bωk]×δt]δθkδtnωknωk12−δtδbωk\delta \dot{\boldsymbol{\theta}}_k-\left[\frac{\boldsymbol{\omega}_k\boldsymbol{\omega}_{k1}}{2}-\boldsymbol{b}_{\omega_t}\right]_{\times} \delta \boldsymbol{\theta}_k\frac{\boldsymbol{n}_{\omega_k}\boldsymbol{n}_{\omega_{k1}}}{2}-\delta \boldsymbol{b}_{\omega_k}\\ \delta \boldsymbol{\theta}_{k1}\left[\boldsymbol{I}-\left[\frac{\boldsymbol{\omega}_k\boldsymbol{\omega}_{k1}}{2}-\boldsymbol{b}_{\omega_k}\right]_{\times} \delta t\right] \delta \boldsymbol{\theta}_k\delta t \frac{\boldsymbol{n}_{\omega_k}\boldsymbol{n}_{\omega_{k1}}}{2}-\delta t \delta \boldsymbol{b}_{\omega_k} δθ˙k−[2ωkωk1−bωt]×δθk2nωknωk1−δbωkδθk1[I−[2ωkωk1−bωk]×δt]δθkδt2nωknωk1−δtδbωk令 ω‾ωkωk12−bωk\overline{\boldsymbol{\omega}}\frac{\omega_k\omega_{k1}}{2}-\boldsymbol{b}_{\omega_k}ω2ωkωk1−bωk 则上式可以重新写为 δθk1[I−[ω‾]×δt]δθkδt2nωkδt2nωk1−δtδbωk\delta \boldsymbol{\theta}_{k1}\left[\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right] \delta \boldsymbol{\theta}_k\frac{\delta t}{2} \boldsymbol{n}_{\omega_k}\frac{\delta t}{2} \boldsymbol{n}_{\omega_{k1}}-\delta t \delta \boldsymbol{b}_{\omega_k} δθk1[I−[ω]×δt]δθk2δtnωk2δtnωk1−δtδbωk δβk1\delta \boldsymbol{\beta}_{k1}δβk1 的求解 由于连续时间下有 δβ˙−Rt[at−bat]×δθRt(na−δbat)\delta \dot{\boldsymbol{\beta}}-\boldsymbol{R}_t\left[\boldsymbol{a}_t-\boldsymbol{b}_{a_t}\right]_{\times} \delta \boldsymbol{\theta}\boldsymbol{R}_t\left(\boldsymbol{n}_a-\delta \boldsymbol{b}_{a_t}\right) δβ˙−Rt[at−bat]×δθRt(na−δbat) 则离散时间下应该有 δβ˙k−12Rk[ak−bak]×δθk−12Rk1[ak1−bak]×δθk112Rknak12Rk1nak1−12(RkRk1)δbak\begin{aligned} \delta \dot{\boldsymbol{\beta}}_k -\frac{1}{2} \boldsymbol{R}_k\left[\boldsymbol{a}_k-\boldsymbol{b}_{a_k}\right]_{\times} \delta \boldsymbol{\theta}_k \\ -\frac{1}{2} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \delta \boldsymbol{\theta}_{k1} \\ \frac{1}{2} \boldsymbol{R}_k \boldsymbol{n}_{a_k} \\ \frac{1}{2} \boldsymbol{R}_{k1} \boldsymbol{n}_{a_{k1}} \\ -\frac{1}{2}\left(\boldsymbol{R}_k\boldsymbol{R}_{k1}\right) \delta \boldsymbol{b}_{a_k} \end{aligned} δβ˙k−21Rk[ak−bak]×δθk−21Rk1[ak1−bak]×δθk121Rknak21Rk1nak1−21(RkRk1)δbak把前面求得的 δθk1\delta \boldsymbol{\theta}_{k1}δθk1 的表达式代入上式可得 δβ˙k−12Rk[ak−bak]×δθk−12Rk1[ak1−bak]×{[I−[ω‾]×δt]δθkδt2nωkδt2nωk1−δtδbωk}12Rknak12Rk1nak1−12(RkRk1)δbak\begin{aligned} \delta \dot{\boldsymbol{\beta}}_k -\frac{1}{2} \boldsymbol{R}_k\left[\boldsymbol{a}_k-\boldsymbol{b}_{a_k}\right]_{\times} \delta \boldsymbol{\theta}_k \\ -\frac{1}{2} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times}\left\{\left[\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right] \delta \boldsymbol{\theta}_k\frac{\delta t}{2} \boldsymbol{n}_{\omega_k}\frac{\delta t}{2} \boldsymbol{n}_{\omega_{k1}}-\delta t \delta \boldsymbol{b}_{\omega_k}\right\} \\ \frac{1}{2} \boldsymbol{R}_k \boldsymbol{n}_{a_k} \\ \frac{1}{2} \boldsymbol{R}_{k1} \boldsymbol{n}_{a_{k1}} \\ -\frac{1}{2}\left(\boldsymbol{R}_k\boldsymbol{R}_{k1}\right) \delta \boldsymbol{b}_{a_k} \end{aligned} δβ˙k−21Rk[ak−bak]×δθk−21Rk1[ak1−bak]×{[I−[ω]×δt]δθk2δtnωk2δtnωk1−δtδbωk}21Rknak21Rk1nak1−21(RkRk1)δbak经过一系列合并同类项以后最终的合并结果为 δβ˙k−12[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω‾]×δt)]δθk−δt4Rk1[ak1−bak]×nωk−δt4Rk1[ak1−bak]×nωk1δt2Rk1[ak1−bak]×δbωk12Rknak12Rk1nak1−12(RkRk1)δbak\begin{aligned} \delta \dot{\boldsymbol{\beta}}_k -\frac{1}{2}\left[\boldsymbol{R}_k\left[\boldsymbol{a}_k-\boldsymbol{b}_{a_k}\right]_{\times}\boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \delta \boldsymbol{\theta}_k \\ -\frac{\delta t}{4} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \boldsymbol{n}_{\omega_k} \\ -\frac{\delta t}{4} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \boldsymbol{n}_{\omega_{k1}} \\ \frac{\delta t}{2} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \delta \boldsymbol{b}_{\omega_k} \\ \frac{1}{2} \boldsymbol{R}_k \boldsymbol{n}_{a_k} \\ \frac{1}{2} \boldsymbol{R}_{k1} \boldsymbol{n}_{a_{k1}} \\ -\frac{1}{2}\left(\boldsymbol{R}_k\boldsymbol{R}_{k1}\right) \delta \boldsymbol{b}_{a_k} \end{aligned} δβ˙k−21[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω]×δt)]δθk−4δtRk1[ak1−bak]×nωk−4δtRk1[ak1−bak]×nωk12δtRk1[ak1−bak]×δbωk21Rknak21Rk1nak1−21(RkRk1)δbak由导数形式可以得到递推形式如下 δβk1δβk−δt2[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω‾]×δt)]δθk−δt24Rk1[ak1−bak]×nωk−δt24Rk1[ak1−bak]×nωk1δt22Rk1[ak1−bak]×δbωkδt2Rknakδt2Rk1nak1−δt2(RkRk1)δbak\begin{aligned} \delta \boldsymbol{\beta}_{k1} \delta \boldsymbol{\beta}_k \\ -\frac{\delta t}{2}\left[\boldsymbol{R}_k\left[\boldsymbol{a}_k-\boldsymbol{b}_{a_k}\right]_{\times}\boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \delta \boldsymbol{\theta}_k \\ -\frac{\delta t^2}{4} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \boldsymbol{n}_{\omega_k} \\ -\frac{\delta t^2}{4} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \boldsymbol{n}_{\omega_{k1}} \\ \frac{\delta t^2}{2} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \delta \boldsymbol{b}_{\omega_k} \\ \frac{\delta t}{2} \boldsymbol{R}_k \boldsymbol{n}_{a_k} \\ \frac{\delta t}{2} \boldsymbol{R}_{k1} \boldsymbol{n}_{a_{k1}} \\ -\frac{\delta t}{2}\left(\boldsymbol{R}_k\boldsymbol{R}_{k1}\right) \delta \boldsymbol{b}_{a_k} \end{aligned} δβk1δβk−2δt[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω]×δt)]δθk−4δt2Rk1[ak1−bak]×nωk−4δt2Rk1[ak1−bak]×nωk12δt2Rk1[ak1−bak]×δbωk2δtRknak2δtRk1nak1−2δt(RkRk1)δbak δαk1\delta \boldsymbol{\alpha}_{k1}δαk1 的求解 由于连续时间下有 δα˙tδβt\delta \dot{\boldsymbol{\alpha}}_t\delta \boldsymbol{\beta}_tδα˙tδβt 则离散时间下应该有 δα˙k12δβk12δβk1δβk−δt4[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω‾]×δt)]δθk−δt28Rk1[ak1−bak]×nωk−δt28Rk1[ak1−bak]×nωk1δt24Rk1[ak1−bak]×δbωkδt4Rknakδt4Rk1nak1−δt4(RkRk1)δbak\begin{aligned} \delta \dot{\boldsymbol{\alpha}}_k \frac{1}{2} \delta \boldsymbol{\beta}_k\frac{1}{2} \delta \boldsymbol{\beta}_{k1} \\ \delta \boldsymbol{\beta}_k \\ -\frac{\delta t}{4}\left[\boldsymbol{R}_k\left[\boldsymbol{a}_k-\boldsymbol{b}_{a_k}\right]_{\times}\boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \delta \boldsymbol{\theta}_k \\ -\frac{\delta t^2}{8} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{\boldsymbol{a}_k}\right]_{\times} \boldsymbol{n}_{\omega_k} \\ -\frac{\delta t^2}{8} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{\boldsymbol{a}_k}\right]_{\times} \boldsymbol{n}_{\omega_{k1}} \\ \frac{\delta t^2}{4} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{\boldsymbol{a}_k}\right] \times \delta \boldsymbol{b}_{\omega_k} \\ \frac{\delta t}{4} \boldsymbol{R}_k \boldsymbol{n}_{a_k} \\ \frac{\delta t}{4} \boldsymbol{R}_{k1} \boldsymbol{n}_{a_{k1}} \\ -\frac{\delta t}{4}\left(\boldsymbol{R}_k\boldsymbol{R}_{k1}\right) \delta \boldsymbol{b}_{a_k} \end{aligned} δα˙k21δβk21δβk1δβk−4δt[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω]×δt)]δθk−8δt2Rk1[ak1−bak]×nωk−8δt2Rk1[ak1−bak]×nωk14δt2Rk1[ak1−bak]×δbωk4δtRknak4δtRk1nak1−4δt(RkRk1)δbak由导数形式可以写出递推形式 δαk1δαkδtδβk−δt24[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ωˉ]×δt)]δθk−δt38Rk1[ak1−bak]×nωk−δt38Rk1[ak1−bak]×nωk1δt34Rk1[ak1−bak]×δbωkδt24Rknakδt24Rk1nak1−δt24(RkRk1)δbak\begin{aligned} \delta \boldsymbol{\alpha}_{k1} \delta \boldsymbol{\alpha}_k \\ \delta t \delta \boldsymbol{\beta}_k \\ -\frac{\delta t^2}{4}\left[\boldsymbol{R}_k\left[\boldsymbol{a}_k-\boldsymbol{b}_{a_k}\right]_{\times}\boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times}\left(\boldsymbol{I}-[\bar{\omega}]_{\times} \delta t\right)\right] \delta \boldsymbol{\theta}_k \\ -\frac{\delta t^3}{8} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \boldsymbol{n}_{\omega_k} \\ -\frac{\delta t^3}{8} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \boldsymbol{n}_{\omega_{k1}} \\ \frac{\delta t^3}{4} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \delta \boldsymbol{b}_{\omega_k} \\ \frac{\delta t^2}{4} \boldsymbol{R}_k \boldsymbol{n}_{a_k} \\ \frac{\delta t^2}{4} \boldsymbol{R}_{k1} \boldsymbol{n}_{a_{k1}} \\ -\frac{\delta t^2}{4}\left(\boldsymbol{R}_k\boldsymbol{R}_{k1}\right) \delta \boldsymbol{b}_{a_k} \end{aligned} δαk1δαkδtδβk−4δt2[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ωˉ]×δt)]δθk−8δt3Rk1[ak1−bak]×nωk−8δt3Rk1[ak1−bak]×nωk14δt3Rk1[ak1−bak]×δbωk4δt2Rknak4δt2Rk1nak1−4δt2(RkRk1)δbak由以上的推导结果便可以写出 xk1FkxkBkwk\boldsymbol{x}_{k1}\boldsymbol{F}_k \boldsymbol{x}_k\boldsymbol{B}_k \boldsymbol{w}_kxk1FkxkBkwk 中的矩阵 Fk[If12Iδt−14(RkRk1)δt2f150I−[ω‾]×δt00−Iδt0f32I−12(RkRk1)δtf35000I00000I]Bk[14Rkδt2g1214Rk1δt2g1400012Iδt012Iδt0012Rkδtg3212Rk1δtg34000000Iδt000000Iδt]\begin{aligned} \mathbf{F}_k {\left[\begin{array}{cccccc} \mathbf{I} \mathbf{f}_{12} \mathbf{I} \delta t -\frac{1}{4}\left(\boldsymbol{R}_k\boldsymbol{R}_{k1}\right) \delta t^2 \mathbf{f}_{15} \\ \mathbf{0} \mathbf{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t \mathbf{0} \mathbf{0} -\mathbf{I} \delta t \\ \mathbf{0} \mathbf{f}_{32} \mathbf{I} -\frac{1}{2}\left(\boldsymbol{R}_k\boldsymbol{R}_{k1}\right) \delta t \mathbf{f}_{35} \\ \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{I} \mathbf{0} \\ \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{I} \end{array}\right] } \\ \mathbf{B}_k\left[\begin{array}{cccccc} \frac{1}{4} \boldsymbol{R}_k \delta t^2 \mathbf{g}_{12} \frac{1}{4} \boldsymbol{R}_{k1} \delta t^2 \mathbf{g}_{14} \mathbf{0} \mathbf{0} \\ \mathbf{0} \frac{1}{2} \mathbf{I} \delta t \mathbf{0} \frac{1}{2} \mathbf{I} \delta t \mathbf{0} \mathbf{0} \\ \frac{1}{2} \boldsymbol{R}_k \delta t \mathbf{g}_{32} \frac{1}{2} \boldsymbol{R}_{k1} \delta t \mathbf{g}_{34} \mathbf{0} \mathbf{0} \\ \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{I} \delta t \mathbf{0} \\ \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{0} \mathbf{I} \delta t \end{array}\right] \end{aligned} FkI0000f12I−[ω]×δtf3200Iδt0I00−41(RkRk1)δt20−21(RkRk1)δtI0f15−Iδtf350IBk41Rkδt2021Rkδt00g1221Iδtg320041Rk1δt2021Rk1δt00g1421Iδtg3400000Iδt00000Iδt上面的矩阵中有 f12−δt24[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω‾]×δt)]f15δt34Rk1[ak1−bak]×δbωkf32−δt2[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω‾]×δt)]f35δt22Rk1[ak1−bak]×g12−δt38Rk1[ak1−bak]×g14−δt38Rk1[ak1−bak]×g32−δt24Rk1[ak1−bak]×g34−δt24Rk1[ak1−bak]×\begin{aligned} \boldsymbol{f}_{12}-\frac{\delta t^2}{4}\left[\boldsymbol{R}_k\left[\boldsymbol{a}_k-\boldsymbol{b}_{a_k}\right]_{\times}\boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \\ \boldsymbol{f}_{15}\frac{\delta t^3}{4} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \delta \boldsymbol{b}_{\omega_k} \\ \boldsymbol{f}_{32}-\frac{\delta t}{2}\left[\boldsymbol{R}_k\left[\boldsymbol{a}_k-\boldsymbol{b}_{a_k}\right]_{\times}\boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times}\left(\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right)\right] \\ \boldsymbol{f}_{35}\frac{\delta t^2}{2} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \\ \boldsymbol{g}_{12}-\frac{\delta t^3}{8} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \\ \boldsymbol{g}_{14}-\frac{\delta t^3}{8} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \\ \boldsymbol{g}_{32}-\frac{\delta t^2}{4} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \\ \boldsymbol{g}_{34}-\frac{\delta t^2}{4} \boldsymbol{R}_{k1}\left[\boldsymbol{a}_{k1}-\boldsymbol{b}_{a_k}\right]_{\times} \end{aligned} f12−4δt2[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω]×δt)]f154δt3Rk1[ak1−bak]×δbωkf32−2δt[Rk[ak−bak]×Rk1[ak1−bak]×(I−[ω]×δt)]f352δt2Rk1[ak1−bak]×g12−8δt3Rk1[ak1−bak]×g14−8δt3Rk1[ak1−bak]×g32−4δt2Rk1[ak1−bak]×g34−4δt2Rk1[ak1−bak]×
3.5 预积分更新
回到bias变化时预积分结果怎样重新计算的 问题再次给出它的泰勒展开形式 αbibjα‾bibjJbiaαδbiaJbigαδbigβbibjβ‾bibjJbiaβδbiaJbigβδbigqbibjq‾bibj⊗[112Jbiqqδbig]\begin{gathered} \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{gathered} αbibjαbibjJbiaαδbiaJbigαδbigβbibjβbibjJbiaβδbiaJbigβδbigqbibjqbibj⊗[121Jbiqqδbig]这里雅可比没有明确的闭式解但是在推导 方差的更新时我们得到了 xk1FkxkBkwk\boldsymbol{x}_{k1}\boldsymbol{F}_k \boldsymbol{x}_k\boldsymbol{B}_k \boldsymbol{w}_k xk1FkxkBkwk通过该递推形式可以知道 Jk1FkJk\mathbf{J}_{k1}\mathbf{F}_k \mathbf{J}_k Jk1FkJk即预积分结果的雅可比可以通过这种迭代方式计算。 Jj\boldsymbol{J}_jJj 中关于 bias 的项就是预积分泰勒展开时各 bias 对应的雅可比。
4. 典型方案介绍
4.1 LIO-SAM介绍
论文名称LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping 代码地址https://github.com/TixiaoShan/LIO-SAM 最大特点分两步完成先通过点云特征计算出相对位姿再利用相对位姿、IMU预积分和GPS做融合。
相比于直接一步做紧耦合大大提高了效率而且实测性能也很优异。
5. 融合编码器的优化方案
5.1 整体思路介绍
理论上只要是在载体系下测量且频率比关键帧的提取频率高就都可以做预积分。 编码器与IMU基于预积分的融合有多种方法此处选相对简单的一种 把它们当做一个整体的传感器来用IMU提供角速度编码器提供位移增量且不考虑编码器的误差。 此时对预积分模型输入为 角速度 ωk[ωxkωykωzk]\omega_k\left[\begin{array}{l}\omega_{x k} \\ \omega_{y k} \\ \omega_{z k}\end{array}\right] \quadωkωxkωykωzk 位移增量: ϕk[ϕxk00]\phi_k\left[\begin{array}{c}\phi_{x k} \\ 0 \\ 0\end{array}\right]ϕkϕxk00 输出中包含位置、姿态但不包含速度。
5.2 预积分模型设计
连续时间下从 iii 时刻到 jjj 时刻 IMU的积分结果为 pwbjpwbi∫t∈[i,j]qwbtϕbtδtqwbj∫t∈[i,j]qwbt⊗[012ωbt]δt\begin{aligned} \mathbf{p}_{w b_j} \mathbf{p}_{w b_i}\int_{t \in[i, j]} \mathbf{q}_{w b_t} \boldsymbol{\phi}^{b_t} \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{aligned} pwbjqwbjpwbi∫t∈[i,j]qwbtϕbtδt∫t∈[i,j]qwbt⊗[021ωbt]δt把 qwbtqwbi⊗qbibt\mathbf{q}_{w b_t}\mathbf{q}_{w b_i} \otimes \mathbf{q}_{b_i b_t}qwbtqwbi⊗qbibt 代入上式可得 pwbjpwbiqwbi∫t∈[i,j](qbibtϕbt)δtqwbjqwbi[∫t∈[i,j]qbibt⊗[012ωbt]δt\mathbf{p}_{w b_j}\mathbf{p}_{w b_i}\mathbf{q}_{w b_i} \int_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \boldsymbol{\phi}^{b_t}\right) \delta t \\ \mathbf{q}_{w b_j}\mathbf{q}_{w b_i}\left[\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\right. pwbjpwbiqwbi∫t∈[i,j](qbibtϕbt)δtqwbjqwbi[∫t∈[i,j]qbibt⊗[021ωbt]δt为了整理公式把积分相关的项用下面的式子代替 αbibj∫t∈[i,j](qbibtϕbt)δtqbibj∫t∈[i,j]qbibt⊗[012ωbt]δt\begin{aligned} \boldsymbol{\alpha}_{b_i b_j}\int_{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \boldsymbol{\phi}^{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](qbibtϕbt)δtqbibj∫t∈[i,j]qbibt⊗[021ωbt]δt使用中值积分方法把连续方法改成离散形式如下 ωb12[(ωbk−bkg)(ωbk1−bkg)]ϕw12(qbibkϕbkqbibk1ϕbk1)\begin{aligned} \boldsymbol{\omega}^b \frac{1}{2}\left[\left(\boldsymbol{\omega}^{b_k}-\mathbf{b}_k^g\right)\left(\boldsymbol{\omega}^{b_{k1}}-\mathbf{b}_k^g\right)\right] \\ \boldsymbol{\phi}^w \frac{1}{2}\left(\mathbf{q}_{b_i b_k} \boldsymbol{\phi}^{b_k}\mathbf{q}_{b_i b_{k1}} \boldsymbol{\phi}^{b_{k1}}\right) \end{aligned} ωbϕw21[(ωbk−bkg)(ωbk1−bkg)]21(qbibkϕbkqbibk1ϕbk1)那么预积分的离散形式可以表示为 αbibk1αbibkϕwδtqbibk1qbibk⊗[112ωbδt]\begin{aligned} \boldsymbol{\alpha}_{b_i b_{k1}}\boldsymbol{\alpha}_{b_i b_k}\boldsymbol{\phi}^w \delta t \\ \mathbf{q}_{b_i b_{k1}}\mathbf{q}_{b_i b_k} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \boldsymbol{\omega}^b \delta t \end{array}\right] \end{aligned} αbibk1αbibkϕwδtqbibk1qbibk⊗[121ωbδt]此时状态更新的公式可以整理为 [pwbjqwbjbjgj][pwbiqwbiαbibjqwbiqbibjbig]\left[\begin{array}{c} \mathbf{p}_{w b_j} \\ \mathbf{q}_{w b_j} \\ \mathbf{b}_j^{g_j} \end{array}\right]\left[\begin{array}{c} \mathbf{p}_{w b_i}\mathbf{q}_{w b_i} \boldsymbol{\alpha}_{b_i b_j} \\ \mathbf{q}_{w b_i} \mathbf{q}_{b_i b_j} \\ \mathbf{b}_i^g \end{array}\right] pwbjqwbjbjgjpwbiqwbiαbibjqwbiqbibjbig残差形式可以写为 [rprqrbg][qwbi∗(pwbj−pwbi)−αbbbbj2[qbibj∗⊗(qwbi∗⊗qwbj)]xyzbjg−big]\left[\begin{array}{c} \mathbf{r}_p \\ \mathbf{r}_q \\ \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}\right)-\boldsymbol{\alpha}_{b_{b^b} 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{b}_j^g-\mathbf{b}_i^g \end{array}\right] rprqrbgqwbi∗(pwbj−pwbi)−αbbbbj2[qbibj∗⊗(qwbi∗⊗qwbj)]xyzbjg−big其余部分(方差递推、残差对状态量雅可比、bias更新等)的推导留作作业(仅优秀标准需要做)。
