前言

不知不觉自己也到了秋招的年纪了，虽然感觉自己还是个菜鸡，总感觉自己理论上和实践上都会一点，但又会的不多。。相比于V-SLAM，自己在激光SLAM这块真的是徒有一点理论毫无工程经验，所以为了巩固一下自己这可怜的卢纶基础，画了差不多两周多一点，把之前在深蓝学院上的网课《自动驾驶与机器人中的SLAM技术》又过了一遍，真是不复习不知道，一复习吓一跳，虽然自己当时擦着边拿到了优秀学员，但基础知识还真是忘了不少。。 写这篇文章的目的就是为了再从整本书的角度出发，把每章节大概讲了什么以及我认为的重点再大概记录一遍，这样自己以后再忘看这一篇文章再结合点博客应该差不多能想起来。当然也希望能帮助到和我一样在准备秋招的同学，如果和我一样在激光SLAM上没有什么工程经验，那就一起来巩固下基础知识吧！（PS：这里章节的顺序按照小章节顺序来了）

注：本文大部分图表来自于深蓝学院《自动驾驶与机器人中的SLAM技术》课程材料，如有侵删

第一章 自动驾驶基础知识

事实上，虽然这两年自动驾驶领域在飞速发展，但关于自动驾驶的终极目标，依旧是没有改变的：让汽车拥有自动驾驶的能力。现如今。端到端的自动驾驶似乎是研究热门，"传统"的自动驾驶系统采用模块化部署策略，而端到端的自动驾驶系统可定义为完全可微的过程，以原始传感器数据为输入，并产生规划和/或低级控制动作作为输出。 由于这部分本人也不是很了解，所以这里不做过多赘述，感兴趣的可以参考上海AI Lab的综述文章：End-to-end Autonomous Driving: Challenges and Frontiers。不过关于自动驾驶的分级，依旧是L1-L5

之后高博引入了高精地图和定位在L4级别的自动驾驶中的重要地位， 高精定位和地图代表着一种严格、准确的理念。高精地图可以明显地减轻感知的负担，因此目前L4 级别自动驾驶方案中，大部分任务元素都是和地图绑定的。而高精地图本质上就是结构化的矢量数据，基本元素就是现实世界中的车道，像在之前看autoware的时候，里面用到的地图其实最重要的无非就是两个部分：车道线和红绿灯，当然还有诸如停车场、建筑物一类的标记物在地图上，在一些特殊的场景下（如自动泊车）也会发挥相应的作用。
高精地图的主要获取方式可以分为遥感、低空摄影测量以及车载设备扫描三种，遥感和无人机飞（无人机感觉其实也可以做SLAM，但不如倾斜摄影测量重建来的快了）这两种手段可能可以更快地得到一个比较大尺度的地图，但是普适性较差（感觉更偏向于建模而非建图），因为这本书就是针对车载设备建图这一问题，对其中的定位和建图两个部分分别进行了介绍。

第二章 基础数学知识回顾

事实上这里有很大的部分是基于SLAM十四讲或者说与十四讲的内容相同的，这里还是做简单总结

旋转的表示

所有的三维旋转矩阵组成特殊正交群，也就是SO(3) ，旋转矩阵是一个输出矩阵，满足正交(转置等于逆)和行列式为1两个性质，事实上这两个性质在程序里的体现也是最多的，对旋转的处理往往添加的就是这两个约束。关于旋转的表示还可以由四元数和旋转矢量两种方式来表达：

1. 旋转矢量，及角轴
   旋转矢量其实本身是SO(3)对应的李代数，即小so(3)，它是可以表示为SO(3)的切空间，所以也可以用它来描述角速度，旋转矢量与旋转矩阵的转换关系是罗德里格斯公式和指数映射：
   
   当然后文中主要讲exp(^)这种形式记为了Exp()，如果从旋转矩阵获得旋转矢量的话，其实就是把上边的步骤反着来了
   
   这里还有一个性质需要注意一下，就是之前讨论的旋转矢量大多是针对旋转角度这个部分展开的，事实上轴n也是旋转中的一部分，关于旋转轴n，它对应的就是旋转矩阵特征值为1的单位特征向量，即Rn=n

2. 四元数
   四元数的概念其实还是很复杂的，简单来说可以看作更高维度的复数，但性质还是有不少区别的，四元数最重要的部分就是乘法，主要需要掌握以下两种形式
   
   
   第一种是最标准的四元数乘法形式，第二种则是用的更多的左乘右乘形式，然后还有一个关键点就是四元数表示旋转的时候，是在左右两边都乘了四元数的，也就是p’=qpq-1这种形式，这个对后边理解四元数表示旋转非常重要，因为左右两边都乘了一遍，所以我们简单理解为四元数表示旋转的时候要对角度处理两次，所以最后的角度都带着二倍的关系（简记，而非真实原理）
   

SO(3)的BCH近似

这里也是很多里理论的基础知识，把旋转看作原有姿态矩阵发生的扰动，BCH公式主要就是展示在李群空间和李代数空间这个扰动具体是怎么表现的

当然这里暂时用的还是左扰动模型，以及后面很多地方会将括号内的Φ省略掉

运动学表示

这个主要就是根据旋转矩阵是正交阵展开推导的，最终的目的是得到关于旋转矩阵的微分方程，也被称作泊松方程

这里的ω表示的就是瞬时角速度，对上述微分方程求解，事实上我们就能直接得到指数映射关系，因此离散时间下的形式一般记作

当然后边更常用的做法是对这个再进行一节泰勒展开

同理，我们可以从相同的出发点利用四元数展开，也就是qq*=1，通过这里的展开我们就可以表示出来四元数视角下的运动学方程，这里具体的展开没有什么特殊的，不过其中涉及到了纯虚单位四元数的两个性质

那么对纯虚单位四元数我们可以得到

再把四元数和李代数结合起来看，我们就发现了四元数表达的角速度正好是SO(3) 李代数的一半，那么四元数的运动方程也就可以记为

而四元数的更新公式也是类似的手段进行积分处理，最后简化为

这里还有一个运动学的表示方式比较常用，就是SO(3)+t的运动学表示，其实就是把线速度也考虑进来，是非常简单直观的方式

线速度与加速度的处理

这里其实在VIO的课程里面也有介绍过，出于简化考虑，只考虑带旋转的情况，我们可以得到两个速度之间的转换关系

然后我们再对这个式子求导，其实就是不停地展开求导，就可以得到两个加速度之间地关系了

事实上，后三项其实都可以作为小量忽略，也就是保留第一项，所以这也就是后面所有加速度展开都会带一个R在前面的理论基础

一些常见的雅可比

其实主要是对旋转矩阵的雅可比计算，下面这个式子是最基础的，但也用的最多的，这里是基于右扰动模型

然后复杂一点的情况就是对旋转进行复合了

这里用的近似后边也很常用

滤波器和最优化理论

这里的其实是整个SLAM问题的核心部分，SLAM问题最后可以简单概括为状态估计问题，典型的离散时间状态估计问题可以概括为运动方程+观测方程的形式，先从线性问题出发来考虑，对方程加入高斯分布的随机噪声

这里的A就是状态转移矩阵，C就是观测矩阵，在不同的文献中表达可能不同，核心思想是一致的。至于Kalman滤波器，因为给出证明的文献和材料太多了，主要分为三种：增益最优，贝叶斯滤波器和最大后验估计，像我比较熟悉的可能是第一种。 这里简单记录下最终的公式（还是一样的，不同的公式表达可能不一样，但是核心思想一样）


然而线性系统终究只是理想情况，在实际问题中面对的问题大多数还是非线性的，这个时候就涉及到非线性系统的线性化，一般用到的矩阵就是一阶导数的雅可比矩阵J和二阶导的海塞矩阵H

当然很多问题其实线性化到一阶就够了。这种把非线性问题进行线性化的情况，再加入到Kalman滤波器中，就得到了扩展卡尔曼滤波器，即EKF，这个之前在autoware中的EKF模块里有简单记录过，其实和线性系统没什么大区别，无非就是把非线性系统线性化了。

当然除了滤波器，另一种手段就是批量最小二乘，我觉得其实也就是图优化了，这里我们把看问题的角度都换成了残差角度出发

所以最优估计也就可以看成是误差项的最小二乘问题了

这里涉及到的优化理论其实就是高斯牛顿，LM方法这些了，这里这些具体的就不再展开，关于L-M和GN之间，其实L-M算法可以看作高斯牛顿法的引申，看出高斯牛顿法是在x附近进行近似二阶泰勒展开来简化计算，所以这决定了∆x必须在x附近才有较高的置信度，L-M算法便是在此基础上产生，在增量方程中添加了一个动态拉格朗日乘子λ来改善高斯牛顿方法。这里简单记录下G-N方法的流程

这里的Hx=b形式在后边会被反复的用到，关于滤波和图优化二者之间的区别，简单记录

第三章 惯性导航与组合导航

IMU系统运动学

关于IMU的基础概念和运动学表示其实是任何一种形势下组合导航的基础，当然由于目的不同，可能有以严老师、牛老师为代表的传统积分方法以及以预积分为代表的简单积分方法两种角度，前一种方法精度更高，我本人也只是粗略了解，如果想深入学习要花的时间、精力成本完全不亚于学习SLAM，而第二种是在SLAM领域用的更多的一种，包括这本书里对IMU的处理也是从这种简单角度出发的。这里简单记录下本书中的IMU观测数据表示

当然上式这种表示是不考虑噪声的情况下的表示，IMU的噪声分为测量噪声和零偏，噪声其实往往影响没那么大，零偏对定位结果的影响其实是更大的，可以说零偏估计的结果也直接影响了最终的定位精度，在数学上，我们将它建模出来，认为零偏也是系统的状态量，而且随着时间发生随机的改变。但是，这只是一种数学建模，而非系统本质。那么考虑误差后，IMU的测量方程就可以得到更全面的表示了

这里我们认为零偏是一个维纳过程（Wiener process），或称布朗运动（Brownian motion）或随机游走（random walk）。它们都是常见的、经典的随机过程。 基于此对零偏进行建模

当然上述的IMU模型都是连续时间下的，在离散时间下IMU的模型表达可能会更加简单一些，这里需要注意的是加速度和陀螺仪噪声离散化时候和零偏里酸化时候的区别

在程序里，我们往往使用更为简单的表达方式

IMU航迹推算

航迹推算其实就是简单积分，当然积分有中值积分，欧拉积分，LK等等，这个可以根据具体情况和个人喜好来定，针对低精度的mems-IMU我认为是没什么区别的，在不考虑噪声的影响下，我们对观测值进行直接积分即可

卫星导航

事实上，高博的书中对GNSS只是做了简单的介绍，作为测绘专业的人，这些知识还是十分基础的。高博在这里，或者在整本书中用到的都是RTK，当然这里也给出了一个双天线方案，比起单天线，双天线其实就是可以根据天线之间的安装关系来确定车辆的一个绝对方向

当然这里还有另外一个概念，就是世界坐标系，其实比起来各种什么ECEF,WGS-84,ENU等等，这里就是一个最简单的UTM投影坐标系，也就是将地球视为一个椭球体，投影至横躺的圆柱体上，然后展开进行分区后得到的。在每个分区内，UTM 坐标以正东，正北的米制坐标来表达车辆位置。当然这里为了让x为正数，还加了个500000，然后坐标的定义的话就是比较常用的东北天定义了

这里还需要明确下书中关于RTK的外参，书中用到了安装偏角和安装偏移来描述，这种方法比较直观，有了外参矩阵，就可以得到车体B系到世界系W的转换了

基于ESKF的简单组合导航

书中的状态变量是18维的，也就是把重力向量g也加到了状态变量中，如果我们不在状态方程里写出重力变量，那么必须事先确定初始时刻的IMU 朝向R(0)，才可以执行后续的计算。此时IMU 的姿态是相对于初始的水平面来描述的。而如果把重力写出来，就可以设IMU 的初始姿态为单位矩阵R = I，把重力方向作为IMU 当前姿态相比于水平面的一个度量。 至于状态变量的运动方程还是很好表示的

在流形意义下，EKF的更新量应是位于切空间中的矢量，中间的加法应为流形与切空间指数映射的广义加法，将更新量（或者称为误状态）视为滤波器的状态变量，来推导运动和观测模型。这就引出了误差状态卡尔曼滤波器，也就是ESKF，关于ESKF的优点总结如下


ESKF里，原来的状态变量被拆分成了两个部分：名义状态变量和误差状态变量，而他们的和就是真值，处理中认为名义状态变量是不含噪声的，这样就可以基于简单的运动学方程得到名义状态变量的方程，关于ESKF的推导，核心就是利用真值、名义状态变量和误差变量之间的关系，不停对相同的东西进行展开，首先我们考虑真值的

然后我们把名义状态变量和误差状态变量分离开

其中名义状态变量的方程和真值是相同的（因为都是不考虑噪声的情况下），这里位置、零偏以及g的的误差状态变量求导很简单，就是线性的关系，所以重点要掌握的是旋转和速度的，当然这里就是上面所提到的了，针对同一个等式不停展开以及替换


当然速度的误差项推导也差不多，不过这里要注意把旋转项的误差要记得带进去

最终可以得到误差变量的整体运动方程，当然实际上我们要用到的是离散时间情况下，总结如下

为了表达方便与统一，我们把误差变量的运动方程记为如下形式

然后就可以根据之前的式子得到线性化后的形式了

基于此，我们就可以进行EKF中的预测部分了，当然这里预测的其实是误差状态
至于更新的话，因为我们的预测是针对误差状态变量的，所以我们当然希望更新也是针对误差状态变量的了，所以要求的雅可比就变成观测矩阵对误差状态的雅可比了，至于这个雅可比的求导，要用到链式法则，其实只有旋转的部分需要单独处理下

这里的旋转又用一个右乘就好，也就是
这样我们整个ESKF的更新就可以表示为

由于我们之前一直在处理的都是误差状态变量，所以这里还有个很重要的步骤就是对ESKF的后处理，其实就是把误差状态加到名义状态就好了。不过由于我们描述的协方差发生了变化，尤其对于旋转变量来说，其实切空间的零点发生了变化，所以这里还要单独处理下

其实看的就是误差状态重置后，发生了多少的改变

最后关于组合导航的实现，当然要考虑对RTK的处理了，这里是出于双天线，也就是有姿态观测值的情况下考虑，我们认为观测是对角度变化的直接观测，这样它的雅可比就是I了，后边使用就会很方便；当然位移因为是线性的，更是直接是I了，就更没什么好说的了

最后高博给出了一段ESKF和图优化之间的区别和相似之处，我觉得总结的还是蛮好的，这里记录下

速度观测量

因为IMU自己会飞，GNSS又经常不靠谱，所以这里引入了轮速计这种简单方便的传感器，当然这里的轮速计用的也是非常的简单，直接看作是载体坐标系下X方向的速度观测，这种约束我也用到过VIO里面，确实是有效的

第四章 预积分

预积分，作为传感器融合中不可绕开的一部分，高博这里带来了一种新的推导形式，当然高博在自己的知乎上也有分享过这种推导形式，具体链接在简明预积分推导，这里只做思路上的总结和流程上的梳理

什么是预积分

预积分，我个人感觉就可以简单理解为把数据积分起来当作新的处理对象，如前文中所提到的IMU具体的运动模型，虽然已经是不考虑地球自转的简化版本了，但依旧还是很复杂的一个形式，因此为了更一步简化的表达，当然也是因为通常IMU的观测频率太高了，比其他的传感器都要高，所以把i、j两个时刻内的IMU数据累积起来进行处理，我认为就是预积分的初衷。这个处理的手段就是尽可能把IMU的读书放在一侧，状态量放在另一侧，这样我们就得到了最基础的预积分形式

预积分的测量模型

预积分的一大处理基础就是先短暂地认为零偏是不变的，不然处理起来实在是太麻烦了。最后在当零偏估计发生改变时，用线性模型来修正预积分即可。这里首先就是要知道预积分的噪声有多大，还是从旋转开始推导，因为可以看到p和v其实里面是包含旋转的

我们这里引入预积分观测量，其实就是不考虑噪声和零偏变化情况下的观测值简单积分

然后就是不停对第一个式子展开，伴随转换

其实如果还对ESKF有印象的话可以发现，这里噪声的形式和ESKF里的也几乎一样了。至于速度部分也差不多，当然要记得把旋转的形式带进去


平移部分的操作也是类似的

然后我们就可以得到一个简单的预积分模型了

噪声是干什么的？噪声模型！

之前的推导知识单纯把旋转、速度和位移的噪声量从整体方程中分离了出来，进一步地我们应该分析这个噪声具体多大，以及噪声是怎么传递的，还是从旋转噪声开始看，用上面得到的表达方式，两边进行Log计算，这里要注意，因为高博对噪声的定义里面是加了符号的，所以前面自然而然就有负号了

然后因为噪声其实是小量，所以这里的BCH直接近似为I，就得到了一个简单形式

之后就可以根据这个公式来得到噪声的递推公式了，其实本质上就是把上一时刻的提取出来

那么误差的递推就可以表示为

同理，速度和位移的误差模型也是这样一点点推导出来的，不过形式上要比旋转的简单不少


把噪声项总结在一起，就得到整个系统的噪声递推关系了

零偏怎么更新

前文中提到了，我们是默认零偏先不变进行的整个预积分过程，但是零偏变了的话，必不可少要根据新的零偏进行一波更新了，这里有一个原则，就是假定预积分观测是随零偏线性变化的，这样我们就只是在原先的观测量上进行修正，注意，这里修正的是观测量

所以这个问题又变成求Jacobian的问题了，旋转这里还是不停地用伴随矩阵换，当然最后又用到了小量的BCH展开接近单位阵

然后我们也写成递推的形式

速度和位移也是同理


最后整体的递推形式是 (4.38)

因为我自己推过几遍了，所以这里主要是梳理流程，如果是第一次推的话，还是要耐心推完为好

图优化模型怎么建

这里其实主要就是残差怎么定义的一个问题，高博相当于是减去观测值看作残差这种做法

然后要考虑的就是残差对各个状态变量的求导了，这里其实比起之前的求导要简单不少（注：这里的旋转都是右扰动的）

这里对零偏的求导则要复杂一些，因为本身带着零偏修正，所以这里的零偏就要再多加一项

也就是

速度的话则会简单一些

平移的话也差不多

至此，我们跟着高博的思路完成了一遍预积分的推导，当然预积分的推导还有像VINS那种一些其他的方法，其实区别的话更多还是一些细节以及符号表达上的区别，大体思路上是差不多的，我认为预积分更应该掌握这些每一步都是在干嘛的以及为什么要怎么干，后边具体实现的话，反而相对简单一些，用G2O的话就是把对应的顶点啊、边啊什么的构造好

总结

到这里其实是把这本书激光部分前的基础部分梳理了一遍，这部分的知识对多源融合部分十分重要，无论是激光，还是视觉，还是单纯的GINS，其实这里都涉及到了一些基础知识，因为要重点掌握才是