福州外贸网站建设怎么建设网站容易被百度抓取

当前位置： 首页 > news >正文

福州外贸网站建设,怎么建设网站容易被百度抓取,大连全员核酸检测,门户网站开发介绍多传感器融合定位十二-基于图优化的建图方法其二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,k1​Fk​Pi,k​Fk⊤​Bk​QBk⊤​但需注意的是此处 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^{bk} \ \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}{ba} \ \boldsymbol{n}{b_w} \end{array}\right] \end{gathered} x˙Ft​xBt​wx​δαtbk​​δθtbk​​δβtbk​​δbat​​δbwt​​​​w​na​nw​nba​​nbw​​​​​以上变量排列顺序是为了与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}{\omegat}\right]{\times} \delta \boldsymbol{\theta}_t^{bk}\boldsymbol{n}\omega-\delta \boldsymbol{b}_{\omega_t} δθ˙tbk​​−[ωt​−bωt​​]×​δθtbk​​nω​−δ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}{at}\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​​]×​δθtbk​​Rt​(na​−δbat​​)δα˙tbk\delta \dot{\boldsymbol{\alpha}}_t^{b_k}δα˙tbk​​ 的微分方程 δα˙tbkδβtbk\delta \dot{\boldsymbol{\alpha}}_t^{b_k}\delta \boldsymbol{\beta}_t^{bk} δα˙tbk​​δβtbk​​ 3.4.3 离散时间下的传递方程 得到连续时间微分方程以后就可以计算离散时间的递推方程了表示为 xk1FkxkBkwk\boldsymbol{x}{k1}\boldsymbol{F}_k \boldsymbol{x}_k\boldsymbol{B}_k \boldsymbol{w}k xk1​Fk​xk​Bk​wk​其中 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}{ak} \ \delta \boldsymbol{b}{\omega_k} \end{array}\right] \quad \boldsymbol{w}k\left[\begin{array}{c} \boldsymbol{n}{ak} \ \boldsymbol{n}{wk} \ \boldsymbol{n}{a{k1}} \ \boldsymbol{n}{w{k1}} \ \boldsymbol{n}{ba} \ \boldsymbol{n}{bw} \end{array}\right] xk1​​δαk1​δθk1​δβk1​δbak1​​δbωk1​​​​xk​​δαk​δθk​δβk​δbak​​δbωk​​​​wk​​nak​​nwk​​nak1​​nwk1​​nba​​nbw​​​​ δθk1\delta \boldsymbol{\theta}{k1}δθk1​ 的求解 由于连续时间下有 δθ˙−[ωt−bωt]×δθnω−δbωt\delta \dot{\boldsymbol{\theta}}-\left[\boldsymbol{\omega}t-\boldsymbol{b}{\omegat}\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}_{\omegat}\right]{\times} \delta \boldsymbol{\theta}k\frac{\boldsymbol{n}{\omegak}\boldsymbol{n}{\omega{k1}}}{2}-\delta \boldsymbol{b}{\omegak}\ \delta \boldsymbol{\theta}{k1}\left[\boldsymbol{I}-\left[\frac{\boldsymbol{\omega}k\boldsymbol{\omega}{k1}}{2}-\boldsymbol{b}_{\omegak}\right]{\times} \delta t\right] \delta \boldsymbol{\theta}k\delta t \frac{\boldsymbol{n}{\omegak}\boldsymbol{n}{\omega{k1}}}{2}-\delta t \delta \boldsymbol{b}{\omega_k} δθ˙k​−[2ωk​ωk1​​−bωt​​]×​δθk​2nωk​​nωk1​​​−δbωk​​δθk1​[I−[2ωk​ωk1​​−bωk​​]×​δt]δθk​δt2nωk​​nωk1​​​−δtδbωk​​令 ω‾ωkωk12−bωk\overline{\boldsymbol{\omega}}\frac{\omegak\omega{k1}}{2}-\boldsymbol{b}_{\omegak}ω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}{\omegak}\frac{\delta t}{2} \boldsymbol{n}{\omega{k1}}-\delta t \delta \boldsymbol{b}{\omegak} δθk1​[I−[ω]×​δt]δθk​2δt​nωk​​2δt​nω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}{at}\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}{ak}\right]{\times} \delta \boldsymbol{\theta}k \ -\frac{1}{2} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}{ak}\right]{\times} \delta \boldsymbol{\theta}_{k1} \ \frac{1}{2} \boldsymbol{R}k \boldsymbol{n}{ak} \ \frac{1}{2} \boldsymbol{R}{k1} \boldsymbol{n}{a{k1}} \ -\frac{1}{2}\left(\boldsymbol{R}k\boldsymbol{R}{k1}\right) \delta \boldsymbol{b}_{ak} \end{aligned} δβ˙​k​​−21​Rk​[ak​−bak​​]×​δθk​−21​Rk1​[ak1​−bak​​]×​δθk1​21​Rk​nak​​21​Rk1​nak1​​−21​(Rk​Rk1​)δ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}{ak}\right]{\times} \delta \boldsymbol{\theta}k \ -\frac{1}{2} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}{ak}\right]{\times}\left{\left[\boldsymbol{I}-[\overline{\boldsymbol{\omega}}]_{\times} \delta t\right] \delta \boldsymbol{\theta}k\frac{\delta t}{2} \boldsymbol{n}{\omegak}\frac{\delta t}{2} \boldsymbol{n}{\omega{k1}}-\delta t \delta \boldsymbol{b}{\omega_k}\right} \ \frac{1}{2} \boldsymbol{R}k \boldsymbol{n}{ak} \ \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​​]×​δθk​−21​Rk1​[ak1​−bak​​]×​{[I−[ω]×​δt]δθk​2δt​nωk​​2δt​nωk1​​−δtδbωk​​}21​Rk​nak​​21​Rk1​nak1​​−21​(Rk​Rk1​)δ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}{ak}\right]{\times}\boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}_{ak}\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}{ak}\right]{\times} \boldsymbol{n}_{\omegak} \ -\frac{\delta t}{4} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}{ak}\right]{\times} \boldsymbol{n}{\omega{k1}} \ \frac{\delta t}{2} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}_{ak}\right]{\times} \delta \boldsymbol{b}_{\omega_k} \ \frac{1}{2} \boldsymbol{R}k \boldsymbol{n}{ak} \ \frac{1}{2} \boldsymbol{R}{k1} \boldsymbol{n}{a{k1}} \ -\frac{1}{2}\left(\boldsymbol{R}k\boldsymbol{R}{k1}\right) \delta \boldsymbol{b}_{ak} \end{aligned} δβ˙​k​​−21​[Rk​[ak​−bak​​]×​Rk1​[ak1​−bak​​]×​(I−[ω]×​δt)]δθk​−4δt​Rk1​[ak1​−bak​​]×​nωk​​−4δt​Rk1​[ak1​−bak​​]×​nωk1​​2δt​Rk1​[ak1​−bak​​]×​δbωk​​21​Rk​nak​​21​Rk1​nak1​​−21​(Rk​Rk1​)δ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}{ak}\right]{\times}\boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}_{ak}\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}{ak}\right]{\times} \boldsymbol{n}_{\omegak} \ -\frac{\delta t^2}{4} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}{ak}\right]{\times} \boldsymbol{n}{\omega{k1}} \ \frac{\delta t^2}{2} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}_{ak}\right]{\times} \delta \boldsymbol{b}_{\omega_k} \ \frac{\delta t}{2} \boldsymbol{R}k \boldsymbol{n}{ak} \ \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}_{ak} \end{aligned} δβk1​​δβk​−2δt​[Rk​[ak​−bak​​]×​Rk1​[ak1​−bak​​]×​(I−[ω]×​δt)]δθk​−4δt2​Rk1​[ak1​−bak​​]×​nωk​​−4δt2​Rk1​[ak1​−bak​​]×​nωk1​​2δt2​Rk1​[ak1​−bak​​]×​δbωk​​2δt​Rk​nak​​2δt​Rk1​nak1​​−2δt​(Rk​Rk1​)δ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}{ak}\right]{\times}\boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}_{ak}\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}_{\omegak} \ -\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}{ak} \ \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}_{ak} \end{aligned} δα˙k​​21​δβk​21​δβk1​δβk​−4δt​[Rk​[ak​−bak​​]×​Rk1​[ak1​−bak​​]×​(I−[ω]×​δt)]δθk​−8δt2​Rk1​[ak1​−bak​​]×​nωk​​−8δt2​Rk1​[ak1​−bak​​]×​nωk1​​4δt2​Rk1​[ak1​−bak​​]×δbωk​​4δt​Rk​nak​​4δt​Rk1​nak1​​−4δt​(Rk​Rk1​)δ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}{ak}\right]{\times}\boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}_{ak}\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}{ak}\right]{\times} \boldsymbol{n}_{\omegak} \ -\frac{\delta t^3}{8} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}{ak}\right]{\times} \boldsymbol{n}{\omega{k1}} \ \frac{\delta t^3}{4} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}_{ak}\right]{\times} \delta \boldsymbol{b}_{\omega_k} \ \frac{\delta t^2}{4} \boldsymbol{R}k \boldsymbol{n}{ak} \ \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}_{ak} \end{aligned} δαk1​​δαk​δtδβk​−4δt2​[Rk​[ak​−bak​​]×​Rk1​[ak1​−bak​​]×​(I−[ωˉ]×​δt)]δθk​−8δt3​Rk1​[ak1​−bak​​]×​nωk​​−8δt3​Rk1​[ak1​−bak​​]×​nωk1​​4δt3​Rk1​[ak1​−bak​​]×​δbωk​​4δt2​Rk​nak​​4δt2​Rk1​nak1​​−4δt2​(Rk​Rk1​)δbak​​​由以上的推导结果便可以写出 xk1FkxkBkwk\boldsymbol{x}{k1}\boldsymbol{F}_k \boldsymbol{x}_k\boldsymbol{B}_k \boldsymbol{w}_kxk1​Fk​xk​Bk​wk​ 中的矩阵 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} ​Fk​​I0000​f12​I−[ω]×​δtf32​00​Iδt0I00​−41​(Rk​Rk1​)δt20−21​(Rk​Rk1​)δtI0​f15​−Iδtf35​0I​​Bk​​41​Rk​δt2021​Rk​δt00​g12​21​Iδtg32​00​41​Rk1​δt2021​Rk1​δt00​g14​21​Iδtg34​00​000Iδt0​0000Iδ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}{ak}\right]{\times}\boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}_{ak}\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}_{ak}\right]{\times} \delta \boldsymbol{b}_{\omegak} \ \boldsymbol{f}{32}-\frac{\delta t}{2}\left[\boldsymbol{R}_k\left[\boldsymbol{a}k-\boldsymbol{b}{ak}\right]{\times}\boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}_{ak}\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}_{ak}\right]{\times} \ \boldsymbol{g}{12}-\frac{\delta t^3}{8} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}{ak}\right]{\times} \ \boldsymbol{g}{14}-\frac{\delta t^3}{8} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}{ak}\right]{\times} \ \boldsymbol{g}{32}-\frac{\delta t^2}{4} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}{ak}\right]{\times} \ \boldsymbol{g}{34}-\frac{\delta t^2}{4} \boldsymbol{R}{k1}\left[\boldsymbol{a}{k1}-\boldsymbol{b}{ak}\right]{\times} \end{aligned} ​f12​−4δt2​[Rk​[ak​−bak​​]×​Rk1​[ak1​−bak​​]×​(I−[ω]×​δt)]f15​4δt3​Rk1​[ak1​−bak​​]×​δbωk​​f32​−2δt​[Rk​[ak​−bak​​]×​Rk1​[ak1​−bak​​]×​(I−[ω]×​δt)]f35​2δt2​Rk1​[ak1​−bak​​]×​g12​−8δt3​Rk1​[ak1​−bak​​]×​g14​−8δt3​Rk1​[ak1​−bak​​]×​g32​−4δt2​Rk1​[ak1​−bak​​]×​g34​−4δt2​Rk1​[ak1​−bak​​]×​​
3.5 预积分更新 回到bias变化时预积分结果怎样重新计算的 问题再次给出它的泰勒展开形式 αbibjα‾bibjJbiaαδbiaJbigαδbigβbibjβ‾bibjJbiaβδbiaJbigβδbigqbibjq‾bibj⊗[112Jbiqqδbig]\begin{gathered} \boldsymbol{\alpha}_{b_i bj}\overline{\boldsymbol{\alpha}}{b_i bj}\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 bj}\overline{\boldsymbol{\beta}}{b_i bj}\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 bj}\overline{\mathbf{q}}{b_i bj} \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} αbi​bj​​αbi​bj​​Jbia​α​δbia​Jbig​α​δbig​βbi​bj​​β​bi​bj​​Jbia​β​δbia​Jbig​β​δbig​qbi​bj​​q​bi​bj​​⊗[121​Jbiq​q​δbig​​]​这里雅可比没有明确的闭式解但是在推导 方差的更新时我们得到了 xk1FkxkBkwk\boldsymbol{x}{k1}\boldsymbol{F}_k \boldsymbol{x}_k\boldsymbol{B}_k \boldsymbol{w}k xk1​Fk​xk​Bk​wk​通过该递推形式可以知道 Jk1FkJk\mathbf{J}{k1}\mathbf{F}_k \mathbf{J}_k Jk1​Fk​Jk​即预积分结果的雅可比可以通过这种迭代方式计算。 Jj\boldsymbol{J}_jJj​ 中关于 bias 的项就是预积分泰勒展开时各 bias 对应的雅可比。

1. 典型方案介绍 4.1 LIO-SAM介绍 论文名称LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping 代码地址https://github.com/TixiaoShan/LIO-SAM 最大特点分两步完成先通过点云特征计算出相对位姿再利用相对位姿、IMU预积分和GPS做融合。 相比于直接一步做紧耦合大大提高了效率而且实测性能也很优异。
2. 融合编码器的优化方案 5.1 整体思路介绍 理论上只要是在载体系下测量且频率比关键帧的提取频率高就都可以做预积分。 编码器与IMU基于预积分的融合有多种方法此处选相对简单的一种 把它们当做一个整体的传感器来用IMU提供角速度编码器提供位移增量且不考虑编码器的误差。 此时对预积分模型输入为 角速度 ωk[ωxkωykωzk]\omegak\left[\begin{array}{l}\omega{x k} \ \omega{y k} \ \omega{z k}\end{array}\right] \quadωk​​ωxk​ωyk​ωzk​​​ 位移增量: ϕk[ϕxk00]\phik\left[\begin{array}{c}\phi{x k} \ 0 \ 0\end{array}\right]ϕk​​ϕxk​00​​ 输出中包含位置、姿态但不包含速度。 5.2 预积分模型设计 连续时间下从 iii 时刻到 jjj 时刻 IMU的积分结果为 pwbjpwbi∫t∈[i,j]qwbtϕbtδtqwbj∫t∈[i,j]qwbt⊗[012ωbt]δt\begin{aligned} \mathbf{p}_{w bj} \mathbf{p}{w bi}\int{t \in[i, j]} \mathbf{q}_{w b_t} \boldsymbol{\phi}^{bt} \delta t \ \mathbf{q}{w bj} \int{t \in[i, j]} \mathbf{q}_{w b_t} \otimes\left[\begin{array}{c} 0 \ \frac{1}{2} \boldsymbol{\omega}^{bt} \end{array}\right] \delta t \end{aligned} pwbj​​qwbj​​​pwbi​​∫t∈[i,j]​qwbt​​ϕbt​δt∫t∈[i,j]​qwbt​​⊗[021​ωbt​​]δt​把 qwbtqwbi⊗qbibt\mathbf{q}{w bt}\mathbf{q}{w bi} \otimes \mathbf{q}{b_i bt}qwbt​​qwbi​​⊗qbi​bt​​ 代入上式可得 pwbjpwbiqwbi∫t∈i,jδtqwbjqwbi[∫t∈[i,j]qbibt⊗[012ωbt]δt\mathbf{p}{w bj}\mathbf{p}{w bi}\mathbf{q}{w bi} \int{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \boldsymbol{\phi}^{bt}\right) \delta t \ \mathbf{q}{w bj}\mathbf{q}{w bi}\left[\int{t \in[i, j]} \mathbf{q}_{b_i b_t} \otimes\left[\begin{array}{c} 0 \ \frac{1}{2} \boldsymbol{\omega}^{bt} \end{array}\right] \delta t\right. pwbj​​pwbi​​qwbi​​∫t∈[i,j]​(qbi​bt​​ϕbt​)δtqwbj​​qwbi​​[∫t∈[i,j]​qbi​bt​​⊗[021​ωbt​​]δt为了整理公式把积分相关的项用下面的式子代替 αbibj∫t∈i,jδtqbibj∫t∈[i,j]qbibt⊗[012ωbt]δt\begin{aligned} \boldsymbol{\alpha}{b_i bj}\int{t \in[i, j]}\left(\mathbf{q}_{b_i b_t} \boldsymbol{\phi}^{bt}\right) \delta t \ \mathbf{q}{b_i bj}\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} ​αbi​bj​​∫t∈[i,j]​(qbi​bt​​ϕbt​)δtqbi​bj​​∫t∈[i,j]​qbi​bt​​⊗[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}^{bk}\mathbf{q}{bi b{k1}} \boldsymbol{\phi}^{b{k1}}\right) \end{aligned} ωbϕw​21​[(ωbk​−bkg​)(ωbk1​−bkg​)]21​(qbi​bk​​ϕbk​qbi​bk1​​ϕbk1​)​那么预积分的离散形式可以表示为 αbibk1αbibkϕwδtqbibk1qbibk⊗[112ωbδt]\begin{aligned} \boldsymbol{\alpha}{bi b{k1}}\boldsymbol{\alpha}_{b_i bk}\boldsymbol{\phi}^w \delta t \ \mathbf{q}{bi b{k1}}\mathbf{q}_{b_i bk} \otimes\left[\begin{array}{c} 1 \ \frac{1}{2} \boldsymbol{\omega}^b \delta t \end{array}\right] \end{aligned} ​αbi​bk1​​αbi​bk​​ϕwδtqbi​bk1​​qbi​bk​​⊗[121​ωbδt​]​此时状态更新的公式可以整理为 [pwbjqwbjbjgj][pwbiqwbiαbibjqwbiqbibjbig]\left[\begin{array}{c} \mathbf{p}{w bj} \ \mathbf{q}{w b_j} \ \mathbf{b}_j^{gj} \end{array}\right]\left[\begin{array}{c} \mathbf{p}{w bi}\mathbf{q}{w bi} \boldsymbol{\alpha}{b_i bj} \ \mathbf{q}{w bi} \mathbf{q}{b_i b_j} \ \mathbf{b}_i^g \end{array}\right] ​pwbj​​qwbj​​bjgj​​​​​pwbi​​qwbi​​αbi​bj​​qwbi​​qbi​bj​​big​​​残差形式可以写为 [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 bi}^*\left(\mathbf{p}{w bj}-\mathbf{p}{w bi}\right)-\boldsymbol{\alpha}{b_{b^b} bj} \ 2\left[\mathbf{q}{b_i bj}^* \otimes\left(\mathbf{q}{w bi}^* \otimes \mathbf{q}{w bj}\right)\right]{x y z} \ \mathbf{b}_j^g-\mathbf{b}_i^g \end{array}\right] ​rp​rq​rbg​​​​qwbi​∗​(pwbj​​−pwbi​​)−αbbb​bj​​2[qbi​bj​∗​⊗(qwbi​∗​⊗qwbj​​)]xyz​bjg​−big​​​其余部分(方差递推、残差对状态量雅可比、bias更新等)的推导留作作业(仅优秀标准需要做)。