SLAM 激光里程计

里程计面临的挑战：

6 DOF，精确，无偏的里程计信息
实时高速，硬件结构小巧
对抖动等鲁棒
对传感器限制（高速公路等重复度高的场景精确度低）鲁棒

Point-to-point ICP

步骤：

扫描两个相关点的集合
做最近匹配
使用牛顿法或者LM迭代优化

缺陷：精度高度依赖于初始化情况

图中的错误匹配没有办法remove，于是有人提出了Point-to-plane ICP

Point-to-plane ICP

扫描两个相关点的集合
做最近匹配
计算集合中的点的法向量
使用牛顿法或者LM迭代优化

法向量求解：

加入了方向和法向量之间的夹角，可以remove图中兔子耳朵部分的错误匹配
缺陷：图中类似于兔子嘴巴部分的错误匹配依然无法被remove
于是有人提出了Plane-to-plane ICP

Plane-to-plane ICP

扫描两个相关点的集合
做最近匹配
计算集合中的点的法向量
计算另一个集合中的点的法向量和二者的方向向量
使用牛顿法或者LM迭代优化

设红色点云：B，蓝色点云：M

服从高斯分布

比较：

Point-to-point ICP：快，但是不鲁棒
Point-to-plane ICP：速度、鲁棒性中等
Plane-to-plane ICP：最慢，精度最高

激光里程计 LiDAR Odometry

setting：里程计信息+地图信息
假设：运动过程相对匀速，抖动不明显

改进：

激光扫描可以很容易的获得点的曲率（利用当前点和周围点近似估计）

X：集合中的点，N：激光的线数，n：第n根线
将点分成两类点：平面的点（surf points）、角上的点（Corner points）

计算距离：
Corner points：

蓝色点：上一时刻的线扫
黄色点：本时刻的线扫

点到点的计算：

surf points：

点到面的计算：

激光的插值：

目的：提高激光的精度（激光在tk时刻发射，等收集到一定数据之后再将数据打包回传，但此时已经是tk+1时刻，传感器已经运动了一段距离，但传感器依旧以tk时刻作为参照，要通过差值提高精度）

因此已知tk时刻的位置信息和tk+1时刻的位置信息，中间时刻的位置信息由以下插值函数计算：

求解旋转平移矩阵
上一时刻点： X(k-1,i)
此时刻点： X(k,i)
旋转矩阵：R(k,i) ，平移矩阵：τ(k,i)
得到：X(k,i) = R(k,i) × X(k-1,i) + τ(k,i)

距离：
F(Tk(t)) = d
求解旋转平移矩阵Tk(t)

平移：τ：x，y，z
旋转矩阵表达方式：Rodrigues‘ Formula（易求导）

当前位置：q，旋转轴：w
角速度：
叉乘运算，求角速度，利用反对称矩阵将叉乘计算转换为点乘计算

整理得：

旋转一定角度：

定理1：

假设：

可得：

exp(wt)可以转换为：

化成此形式最大的优点就是求导简易：

优化方法：

假定r是向量矩阵，形式：

雅可比矩阵：

一阶导数、二阶导数，求导

梯度下降法：

缺点：
梯度下降法在陡峭的部分下降快，但在平缓的部分十分慢

牛顿法：
改进梯度下降法（依赖于一阶导），更依赖于二阶导

优点：速度快
缺点：对初值的要求高，初值选的好，会非常快，初值选的不好，效果很一般

于是有人提出了LM算法

LM：
梯度下降法和牛顿法的融合，不仅考虑了二阶导，还考虑了一阶导

缺点：系数只能手动调整，不能动态调整

改进：

优点：
在平滑的区域，H矩阵发挥作用较大，趋向于使用牛顿法
在陡峭的区域，即远端处，起更大作用的是系数，趋向于使用梯度下降法

激光里程计步骤：

激光扫描计算曲率
通过插值的方法优化每个点的信息
分别计算surf点和corner点的距离关系
LM优化（在实际工程中，更倾向于使用高斯牛顿优化）

Local Mapping

local map被储存到kd-tree中（可快速查找）
后面的点被丢弃（为了保证效率，kd-tree不能过大）
此时提取特征点是之前的十倍，精度更高

已达到目标：6-DOF，精度高，低漂移，实时，速度快
但是对抖动剧烈的情况不鲁棒
改进：

Visual LiDAR Odometry

以激光得到的深度信息补充视觉里程计中的深度信息

Visual Odometry：

将Xk改为：

得到：

若是有深度信息的特征点（恰巧这个点的位置也有激光扫过得到的深度信息）
构建约束：

没有深度信息构建约束：

残差约束：

若有深度信息特征点n个，没有深度信息特征点m个，则约束2n+m+1(残差约束)

视觉雷达里程计步骤：

feature tracking
获取depth信息
利用2n+m+1个约束构建loss function
激光里程计

已达到目标：6-DOF，精度高，低漂移，实时，速度快，克服aggressive motion（剧烈抖动）
但是对传感器的limitation不鲁棒
改进：

Sensor Fusion

顺序问题很重要

基于图优化：

重点是各传感器数据之间的关系，构建各个节点的场景约束

只能在小场景中使用

改进：

Hierarchical

粗糙到精确：IMU估计->visual估计（以IMU估计为前端）->激光

每一个模块都基于前一个模块

IMU：

w(t)是角速度，bw(t)贝叶斯偏置，nw(t)噪声
加速度a(t)，-Rw(t)g必须要减去，否则机器人的抖动影响过大，比如无人机的高空飞行

当视觉里程计失效的时候，直接跨过，用IMU来补足激光需要的信息（）夜晚
当激光里程计失效的时候，直接跨过，用视觉来输出（狭长的隧道，结构相似）

已达到目标：6-DOF，精度高，低漂移，实时，速度快，克服aggressive motion（剧烈抖动），对传感器的limitation鲁棒