Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（四）

3. Joint optimization

3.3 IMU preintegration

这篇论文中采用了IMU-LiDAR紧耦合的方法，其实主要参考了VINS的理论和方法，核心思想其实就是IMU预积分。应该说IMU预积分也不是什么很新的东西了，关于IMU预积分的博客也有很多。但笔者刚开始接触这个东西的时候觉得，很多博客只是照着VINS中的推导简单描述了一下，但是笔者刚开始看VINS论文的时候就觉得里面的理论推导省略的地方有点多，总有种隔靴搔让的感觉。为了更加透彻的理解IMU预积分方法，笔者参考VINS的论文重新自己推导了一遍。希望推导过程能尽可能详实，同时便于理解。水平所限，如有纰漏，还请各位小伙伴帮忙指正。

3.3.1 IMU integration

一般一个IMU包括三个正交的陀螺仪以及三个正交的加速度计，三个陀螺仪可以分别测量绕自身$x,y,z$轴的角速度$w_x,w_y,w_z$，而三个加速度计则可以分别测量沿着三轴的加速度$a_x,a_y,a_z$，其测量模型一般可以建立为：
$$\begin{gathered} \tilde{w}=w+{\eta }_w+b_w \\ \tilde{a}=R^T(a-g)+{\eta }_a+b_a \end{gathered}$$
根据牛顿运动定律，我们根据简单的运动学关系来推算当前的位姿，接下来我们详细推导一下这个运动学关系。
涉及到IMU的SLAM算法，一般会把系统状态建模为：$R_{WB},p_w,v_w$，分别对应IMU本体坐标系到世界坐标系的姿态、IMU在世界坐标系下的位置和速度。关于位置和速度的运动学关系，比较容易得到：
$${\dot{p}}_w=v_w,{\dot{v}}_w=a_w$$
但是关于旋转矩阵的求导怎么表示呢？我刚开始看IMU预积分的时候，觉得角速度都知道了，求姿态那不是很简单吗，先根据角速度积分成欧拉角，再将欧拉角转换成旋转矩阵，然后再不断地乘啊乘，不就得到了最后的旋转矩阵吗？好像还是这么回事。事实上第一个做IMU预积分的还真是这样做的。但是这样做的话，会有一些问题，比如说欧拉角出现死锁啊之类的。而且看VINS的时候发现好像有些细节不太一样。那接下来我们就好好说说这个姿态的运动学。

如上图所示，假设现在有两个坐标系，一个是用实线表示的本体坐标系$B$，另一个是用虚线表示的世界坐标系$W$，两个坐标系之间只存在旋转不存在位移，且本体坐标系到世界坐标系的旋转矩阵为$R_{WB}$。在本体坐标系中存在一个点$p$，且点$p$在本体坐标系中的位置$p_B$是不变的，则点p在世界坐标系下的坐标就是：
$$p_W=R_{WB}{p}_B$$
现在假设本体坐标系一直在按照某种方式旋转，也即到世界坐标系下变换矩阵$R$一直在变$R(t)$，则可以得到：
$$p_W=R(t)p_B$$
现在如果我们想求点$p$在世界坐标系下的速度，则可以利用牛顿运动定律得到如下关系：
$${\dot{p}}_W=\frac{d}{dt}{p}_W=\frac{dR(t)}{dt}{p}_B=\dot{R}{p}_B$$
终于把这个旋转矩阵的导数引入进来了，那么这个旋转矩阵的导数该怎么求呢？我们可以从上式几何意义的角度来考虑，上式计算的其实是点$p$在世界坐标系下的线速度对吧？我们之前高中以及大学学的理论力学中好像学过这么一个公式：
$$\vec{v}=\vec{w}\times \vec{r}$$
也就是说，线速度等于角速度向量与半径向量的叉乘，根据这个想法我们可以重新计算一下点$p$在世界坐标系下的速度。首先，我们根据IMU测量出来的角速度$\vec{w}$以及点$p$在本体坐标系下的位置$\overrightarrow{p_B}$，可以求出来点p在本体坐标系下的速度向量$\vec{v}$，也即：
$$v_p^B=\vec{w}\times \overrightarrow{p_B}$$
为了求该点在世界坐标系下的速度，我们只需要将速度$v_p^B$投影到世界坐标系下，即：
$$v_p^W=R{v}_P^B=R\vec{w}\times \overrightarrow{p_B}=\dot{R}{p}_B$$
这样就得到了旋转矩阵导数的表达式：
$$\dot{R}=R\vec{w}\wedge$$
Good Job!！

接下来，就要真正的进入到IMU积分阶段了，首先我们利用刚刚推导的公式重新写一下IMU的运动学关系。
$$\dot{R}=R\overrightarrow{w_b}\wedge ,{\dot{p}}_w=v_w,{\dot{v}}_w=a_w$$
容易发现，咦好像就是以前大学学过的微分方程啊，那来求一下吧，盘它。首先是旋转矩阵，
$$\dot{R}=\frac{dR}{dt}=R{\vec{w}}_b\wedge$$
学过微分方程的对这种公式应该很熟悉，不就是把$dt$挪到等式的右边，然后把$R$挪到等式的左边，即：
$$\frac{1}{R}dR=\overrightarrow{w_b}\wedge dt$$
等式两边同时对积分：
$${\int }_t^{t+\Delta t}\frac{1}{R(t)}dR(t)={\int }_t^{t+\Delta t}\overrightarrow{w_b}\wedge dt$$
哪个函数的导数是$\frac{1}{x}$来着，应该是$\ln x$吧，因此可以进一步化简得到：
$$\ln R(t){|}_t^{t+\Delta t}={\int }_t^{t+\Delta t}\overrightarrow{w_b}\wedge dt$$
即：
ln ⁡ R ( t + Δ t ) = ln ⁡ R ( t ) + ∫ t t + Δ t w b ⃗ ∧ d t = l n { R ( t ) E x p ( ∫ t t + Δ t w b ⃗ ∧ d t ) } \ln R(t+\Delta t)=\ln R(t)+\int_{t}^{t+\Delta t}\vec{w_b}\wedge dt=ln\{R(t)Exp(\int_{t}^{t+\Delta t}\vec{w_b}\wedge dt)\} lnR(t+Δt)=lnR(t)+∫tt+Δt​wb​ ​∧dt=ln{R(t)Exp(∫tt+Δt​wb​ ​∧dt)}
也就能得到：
$$R(t+\Delta t)=R(t)Exp({\int }_t^{t+\Delta t}\overrightarrow{w_b}\wedge dt)$$
咦居然有一个指数函数，还记得上一篇博客中我们说的切空间$so(3)$到$SO(3)$群的指数映射吗？忘记了的，可以再去回顾回顾哦。总之，这个公式的意思呢就是，首先要根据利用角速度(陀螺仪)积分得到旋转角，然后利用叉乘运算符得到该旋转角对应的切空间内的反对称阵，最后利用指数映射得到对应的$SO(3)$群旋转矩阵，然后再去乘前一时刻的旋转矩阵不就是当前时刻的旋转矩阵吗？现在有没有觉得切空间、$SO(3)$群、指数映射这些听起来玄乎乎的东西也没那么难理解了。Beautiful！！

有了旋转矩阵的积分推导，剩余的关于位置和速度的推导就简单很多了。
速度：
$$\begin{array}{c}\frac{d{v}_w}{dt}=a\\ d{v}_w=adt\\ {\int }_t^{t+\Delta t}d{v}_w={\int }_t^{t+\Delta t}adt\\ v{|}_t^{t+\Delta t}={\int }_t^{t+\Delta t}adt\\ v(t+\Delta t)=v(t)+{\int }_t^{t+\Delta t}adt\end{array}$$
位置：
$$\begin{gathered} \frac{dp}{dt}=v \\ dp=vdt \\ {\int }_t^{t+\Delta t}dp={\int }_t^{t+\Delta t}v(\tau )d\tau \\ p(t+\Delta t)=p(t)+{\int }_t^{t+\Delta t}(v(t)+{\int }_t^{\tau }a(T)dT)d\tau \\ p(t+\Delta t)=p(t)+v(t)\Delta t+{\int }_t^{t+\Delta t}{\int }_t^{\tau }a(T)dT)d\tau \end{gathered}$$

3.3.2 IMU preintegration

把上面三个东西写到一起就是：
$$\begin{array}{c}R(t+\Delta t)=R(t)Exp({\int }_t^{t+\Delta t}\overrightarrow{w_b}\wedge dt)\\ v(t+\Delta t)=v(t)+{\int }_t^{t+\Delta t}adt\\ p(t+\Delta t)=p(t)+v(t)\Delta t+{\int }_t^{t+\Delta t}{\int }_t^{\tau }a(T)dT)d\tau \end{array}$$
但是还有一个问题啊，上式都是定义在连续时间内的。而我们的传感器采样的数据都是离散的，因此呢需要把这东西离散掉，可是这个角速度和加速度都是时变的，还不知道是怎么个变法，这怎么玩？这个时候啊就需要来假设了，一般我们假设加速度和角速度在一小段时间内是不会发生变换的，则上式就可以进一步简化为：
$$\begin{gathered} R(t+\Delta t)=R(t)Exp(w\Delta t\wedge ) \\ v(t+\Delta t)=v(t)+a\Delta t \\ p(t+\Delta t)=p(t)+v(t)\Delta t+\frac{1}{2}a(t)\Delta t^2 \end{gathered}$$
接下来将IMU的测量模型带入上式可得到：
$$\begin{gathered} R(t+\Delta t)=R(t)Exp((\tilde{w}-{\eta }_w-b_w)\Delta t\wedge ) \\ v(t+\Delta t)=v(t)+(R(\tilde{a}-{\eta }_a-b_a)+g)\Delta t=v(t)+g\Delta t+R(t)(\tilde{a}-{\eta }_a-b_a)\Delta t \\ p(t+\Delta t)=p(t)+v(t)\Delta t+\frac{1}{2}g\Delta t^2+\frac{1}{2}R(t)(\tilde{a}-{\eta }_a-b_a)\Delta t^2 \end{gathered}$$
现在给定两个关键帧$i$和$j$，则可以根据上式，利用IMU在这段时间内的测量量，计算两帧的相对运动，即：
$$\begin{gathered} R_j=R_i\prod _{k=i}^{j-1}Exp((\widetilde{w_k}-{\eta }_w^k-b_w^k)\Delta t\wedge ) \\ {v}_j=v_i+g\Delta t_{ij}+\sum _{k=i}^{j-1}(R_k(\widetilde{a_k}-{\eta }_a^k-b_a^k)\Delta t) \\ {p}_j=p_i+\sum _{k=i}^{j-1}(v_k\Delta t+\frac{1}{2}g\Delta t^2+\frac{1}{2}{R}_k({\tilde{a}}_k-{\eta }_a^k-b_a^k)\Delta t^2) \end{gathered}$$
那么现在我们可以利用这个公式推算两个关键帧之间的相对运动了，但是一般在图slam中优化过程中，需要不断的优化图中每一个节点的状态(位置，姿态等)。这一优化不要紧，利用IMU预测运动时，$j$时刻的状态是依赖于$i$时刻的状态的，也就是说只要有其中一个节点的状态发生了变化，后面节点的状态都需要重新积分计算，这计算量谁顶得住啊，而且重复计算一个东西真的cpu也感觉很无聊的啊！
聪敏的预积分作者开始想啊，如果能从这纷繁复杂的公式中抽出一部分不变量，然后先把这部分不变量计算出来存储下来，不就可以避免重复计算了吗？不得不佩服这个作者，实在是太机智了。那么怎么才能抽取出来不变量呢？我们首先来看第一个旋转矩阵的公式，这个看起来好像比较简单，等式右边的连乘项好像和$i$时刻系统的状态(位置、姿态、速度)无关啊，于是等式两边同乘$R_i$的逆矩阵，就可以得到：
$$R_i^T{R}_j=\prod _{k=i}^{j-1}Exp((\widetilde{w_k}-{\eta }_w^k-b_w^k)\Delta t\wedge )=\Delta R_{ij}$$
现在来看第二个，第二个看起来就不那么友善了，等式右边不仅存在$v_i$还存在旋转矩阵$R_k$，但是等式右边的连和里面，除了这个旋转矩阵，好像还都与系统的状态无关，因此我们先把$v_i$和重力加速度这两项挪到等式的左边，就可以得到:
$$v_j-v_i-g\Delta t_{ij}=\sum _{k=i}^{j-1}(R_k(\widetilde{a_k}-{\eta }_a^k-b_a^k)\Delta t)$$
那么现在怎么把这个旋转矩阵$R_k$挪掉呢？还记得之前的$\Delta R_{ij}$吗，那这里把$R_k$换成$\Delta R_{ik}$不就行了吗？于是等式两边同时乘以$R_i$的逆矩阵，就得到：
$$R_i^T(v_j-v_i-g\Delta t_{ij})=\sum _{k=i}^{j-1}(\Delta R_{ik}(\widetilde{a_k}-{\eta }_a^k-b_a^k)\Delta t)=\Delta v_{ij}$$
现在最后一个好像更复杂，我们一点一点来看。熟悉了之前的套路，这肯定得把等式右边的$p_i$和$\frac{1}{2}g\Delta t^2$挪到等式左边，也即：
$$p_j-p_i-\frac{1}{2}\sum _{k=i}^{j-1}g\Delta t^2=\sum _{k=i}^{j-1}(v_k\Delta t+\frac{1}{2}{R}_k({\tilde{a}}_k-{\eta }_a^k-b_a^k)\Delta t^2)$$
这个$R_k$比较好处理，这个$v_k$怎么办嘞？也是类似的啊，之前不是求了个什么$\Delta v_{ij}$吗，现在可以利用起来啊。
$$\begin{gathered} (v_k-v_i-g\Delta t_{ik})=R_i\Delta v_{ik} \\ {v}_k=v_i+g\Delta t_{ik}+R_i\Delta v_{ik} \end{gathered}$$
再带进去看一下：
$$p_j-p_i-\frac{1}{2}\sum _{k=i}^{j-1}g\Delta t^2=\sum _{k=i}^{j-1}((v_i+g\Delta t_{ik}+R_i\Delta v_{ik})\Delta t+\frac{1}{2}{R}_k({\tilde{a}}_k-{\eta }_a^k-b_a^k)\Delta t^2)$$
再化简一下：
$$p_j-p_i-\frac{1}{2}\sum _{k=i}^{j-1}g\Delta t^2-v_i\Delta t_{ij}-\sum _{k=i}^{j-1}g\Delta t\Delta t_{ik}=\sum _{k=i}^{j-1}(R_i\Delta v_{ik}\Delta t+\frac{1}{2}{R}_k({\tilde{a}}_k-{\eta }_a^k-b_a^k)\Delta t^2)$$
等式两边再乘以$R_i$的逆矩阵，就可以得到：
$$\begin{gathered} R_i^T(p_j-p_i-\frac{1}{2}\sum _{k=i}^{j-1}g\Delta t^2-v_i\Delta t_{ij}-\sum _{k=i}^{j-1}g\Delta t\Delta t_{ik})=\sum _{k=i}^{j-1}(\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}({\tilde{a}}_k-{\eta }_a^k-b_a^k)\Delta t^2)=\Delta p_{ij} \\ {R}_i^T(p_j-p_i-\frac{1}{2}g\Delta t_{ij}^2-v_i\Delta t_{ij})=\sum _{k=i}^{j-1}(\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}({\tilde{a}}_k-{\eta }_a^k-b_a^k)\Delta t^2)=\Delta p_{ij} \end{gathered}$$
Oh my god!!先喘口气，因为到这还每结束。
篇幅过长，下篇博客见。