激光slam：2激光雷达运动畸变去除

里程计模型与标定

激光雷达传感器介绍

1.测距原理

已知α、β、L,其中α、β由标定得到，L由投影得到。求d。
已知α、β、L，角角边，可以唯一确定一个三角形。

时间t转换为求相位差，提高精度。

2.数学模型

推导过程

①正态分布—均值d0
②激光被遮挡----指数分布d1
③噪声—出现max—d2
④均值误差----d3
varytracking得到d,则d0=d,d1<d
①②涵盖大部分的情况，③④可以忽略不计


为score,是位姿zk相对于地图的重合程度。

突变：
发生突变时score相对于位姿不平滑。
amcl算法----bean model

有障碍物环境------消除传感器误差和位姿误差-----不需要期望值
传感器误差和位姿误差等误差形成高斯分布（0，Σ），远离障碍物时值为0，靠近障碍物时值为1.

运动畸变介绍

①激光帧率较低时，机器人的运动不能忽略
5-10hz----畸变很大—相邻位姿角度相差很大，甚至同一激光，有不同的位姿
50hz-畸变很小
②激光测量时伴随着机器人的运动
③激光点数据不是瞬时获得
如图，激光点数据不是瞬时获得，使真实测量d变短（blue），小于理想值（red），并且走直线时会有所倾斜。

畸变去除

1.ICP方法

（R,T）为欧式变换

图1为点与点之间的对应（SVM法），图2为ICP的结果

EM算法：A→B，B→A，A→B，B→A，A→B，…
A为（R，T）
B 为point consponse ----隐藏变量
固定A，调整B；固定B，调整A；…

2.VICP方法

VICP----加了速度

①矫正为运动矫正
②机器人的速度Vi实际为Vi = log(Ti-1-1Ti）

3.里程计辅助方法

数据预处理与状态估计过程解耦-----两者分开
数据预处理：定位、建图
状态估计：导航
Loam和VICP类似

里程计的角度测量精度：500HZ
IMU的测量精度：≥1000HZ

处理器:CPU
在单片机上处理：



发布或者显示出去

假设匀速运动，5-10ms的误差

分段数大于3:二次曲线上取2个点

转换前后的坐标：激光前后的坐标
激光数据发布出去：laserscan