激光slam课程学习笔记－－第５课：传感器数据处理II：激光雷达运动畸变去除

前言：这系列笔记是学习曾书格老师的激光slam课程所得，这里分享只是个人理解，有误之处，望大佬们赐教。这节课主要介绍激光的运动畸变去除相关知识。

1.概念介绍 1.1激光雷达传感器介绍

1.1.1　测距原理

三角测距、飞行时间

[比较便宜的，或者探测距离10米内的，激光雷达测距原理一般选择三角测距]

[远距离使用飞行时间比较合适，但是想要短距离使用且保证精度，是非常有挑战性的][个人理解，短距离如1米范围，使用飞行时间原理的雷达测距，效果非常差]

1.1.2 特点

1）三角测距：

a、中近距离精度较高[距离越近精度越高，在10到15米范围，误差在10到20cm之间]
b、价格便宜

c、远距离精度较差
d、易受干扰
e、一般在室内使用

2）飞行时间（TOF）

a、测距范围广
b、测距精度高[这里在25米或30米情况说的，对于在0.5米范围内，得到的精度非常差]

c、抗干扰能力强
d、价格昂贵
e、室内外都可使用

[当雷达的的线数达到300线时，可以理解为一个深度相机了。固态雷达就是企图到达300线]

1.1.3　三角测距原理

[个人理解，已知两角一边，可以计算任意高]

[a,L是标定值，B可以通过传感器内参计算获取]

[由于直接计量时间非常困难，所以转换为相位的方式，计算出时间]

1.2激光雷达数学模型介绍 1.2.1光束模型

[老师推荐书籍《probabilistic robotics》,里面有介绍激光雷达的原理]
[一束激光打到物体上，一般的到上面四种状况][距离信息，左上属于高斯分布，属于理想情况；右上属于指数分布，由于物体有东西挡住所造成的；左下，属于噪声导致的，右下属于距离分布误差导致的；一般情况而言，只考虑上面两种情况即可]
[p（zt|xt，m），给定机器人位置xt，地图m，得到zt（测量值）值的概率p]

[图中p都是表示概率，后面称之为score得分;得分表示的是与地图重合程度]

光束模型的缺点：

1）期望值的计算需要用raytracing，每一个位姿需要进行N次raytracing，N为一帧激光的激素束数量
[使用画线算法进行raytracing，导致计算量非常大]

2）在非结构化环境中（clutter），位姿微小的改变会造成期望值的巨大变化，从而导致得分进行突变
[这种得分的突变，会导致得分相对位姿变化而言，属于不平滑的]

[由于存在这两个缺陷，现在实际使用时，光束模型基本没有人使用][上图是前面四个图合成的结果]

[ros里面的amcl源码就是激光束模型，个人看法，自我仿真学习还行，但是实际使用，还是不合适的]

1.2.2 似然场模型

1）对图像进行高斯平滑，在任何环境中期望值对于位姿都是平滑的

2）得分的计算不需要经过raytracking，直接通过查表即可得到，计算量低

3）同时适合结构化环境和非结构环境中

[右图是通过对左图进行高斯分布模糊得到的，其白色表示模糊过程]
[由于采用的是高斯分布的似然场模型，没有了期望值这一说法，也就是不需要进行得分计算;而是，直接进行得分查表即可]

[高斯平滑过程可通过离线进行，然后进行查表 *** 作]
[由于该模型是对环境进行改变，所有对结构化或非结构环境都适用，如右侧框框处表示结构化环境，点点处表示非结构化环境]

1.3 运动畸变介绍

产生原理：
1）激光点数据都不是临时获得

[激光数据程序处理时视为统一时刻获取，而实际情况并不是;每一帧激光数据获取，需要一个决策时间的，如有如100ms]

2）激光测量时伴随机器人的运动

3）激光帧率较低时，机器人的运动不能忽略
[每个激光点，都有不同的基准位置，但是数据处理时，均视为统一的基准位置进行的]

2.畸变去除 2.1 纯估计方法

[纯估计方式，采用ICP方法进行]

[原理，找到一个欧式变换E(R,t)，使得公式右侧的距离误差尽可能小]
[个人还是不了解]

1）icp对于已知对应点的求解方法
[已知xi是和pi对应的]［下图的Ｒ＝Ｕ（ＶＴ）有误，应该为Ｒ＝Ｖ（ＵＴ）的］

2） icp对应未知对应点的求解方法

背景：
a、实际中，不知道对应点的匹配
b、不能一步到位计算出R和t

c、需要进行迭代计算
d、ICP算法是EM算法的一个特例

[EM算法，已知A，第一步，固定A求解B，第二步固定B求解A，第三步固定A求解B，如此循环迭代，最后收敛得到一个所求位姿]

算法流程：
a、寻找对应点
b、根据对应点，计算R和t

c、对应点云进行转换，计算误差
d、不断迭代，直至误差小于某一个值
[可以参看图中下方的三个图过程]

3）ICP方法在激光匹配中的缺点

a、其没有考虑激光的运动畸变
b、（没有考虑到情况）当前的激光数据是错误的

VICP：
a、其是ICP的变种
b、考虑了机器人的运动
c、匀速运动
d、进行匹配的同时估计机器人的速度

4）VICP 方法介绍

[右上公式涉及李代数知识]
[在右下公式里，由于其是运动导致的，由于畸变是运动带来的，所以矫正时不考虑Ti（由于其在运动过程校正，又称运动校正）]
[有点迷惑，矫正过程，是不是因为在运动过程中实现了，所有不再考虑Ti？]

５）VICP矫正过程实现

[算法流程没理解，需要仔细查看]
[for循环 *** 作是去畸变 *** 作，end for是进行icp *** 作][T是两帧之间的相对位姿]
[算法流程最后的公式编写有误，应该为Vi=(1/At)logT]

2.3里程计辅助方法

[属于外部传感器辅助的方法]

1)背景意义

VICP缺点：
低帧率（5hz），匀速运动假设不成立；数据预处理了和状态估计过程耦合

解决方法：
尽可能准确的反映运动情况；实现预处理和状态估计的解偶

传感器辅助方法（Odom/IMU）：
极高的位姿更新频率（200HZ），可以比较准确的反应运动情况；较高精度的局部位姿估计；跟状态估计完全解偶

2）传感器类型辅助：

惯性测量单元（IMU）：
a、直接测量角速度和线加速度
b、具有较高的角速度测量精度
c、测量频率极高（1kz-8kz）
d、线加速度精度太差，二次积分在局部的精度依然很差

轮式里程计（odom）：
a、直接测量机器人的位移和角度
b、具有较高的局部角度测量精度
c、具有较高的局部位置测量精度
d、更新速度较高（100hz-200hz）

3）轮式里程计处理方式

３－１）单片机上处理
ａ　用单片机读取激光数据，每次读取激光点数据时都可以获取当前机器人的位姿，根据机器人位姿消除运动畸变．得到一帧完整数据后，上传处理器

ｂ．在单片机层消除运动畸变

ｃ．无需考虑时间同步问题

ｄ．需要对数据进行压缩，否则会产生较大延时

３－２）在处理器上处理

ａ　用ＣＰＵ读取激光数据，同时单片机上传里程计积分数据，两者同时进行时间同步．在ＣＰＵ同一进行运动畸变去除

ｂ　体系清晰，不会产生延时

ｃ　需要进行时间同步

ｄ　需要进行位姿插值

４）运动畸变－－轮式里程计

[图片中左侧最后一行，表示的是　最迟　的里程计数据时间戳]

４－１）求解位姿

［第一种情况概率基本为０，常见的情况是第二种］
［线性插值时，假设帧内机器人做的是匀速状态的］

４－２）二次插值

［二次插值，假设帧内机器人做的是匀加速运动］

４－３）二次曲线的近似

［对二次曲线取离散点，做分段线段，对二次曲线进行再次近似］

４－３）坐标系统－激光数据发布

[range,理解为距离；angle理解为角度，若机械式激光雷达转速恒定，那么通过此可推导出激光点的时间戳]

#####################
图片版权归原作者所有
致谢曾老师的付出

不积硅步，无以至千里
好记性不如烂笔头
感觉有点收获的话，麻烦大大们点赞收藏哈