前言:这系列笔记是学习曾书格老师的激光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对应的][下图的R=U(VT)有误,应该为R=V(UT)的]
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?]
5)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)轮式里程计处理方式
3-1)单片机上处理
a 用单片机读取激光数据,每次读取激光点数据时都可以获取当前机器人的位姿,根据机器人位姿消除运动畸变.得到一帧完整数据后,上传处理器
b.在单片机层消除运动畸变
c.无需考虑时间同步问题
d.需要对数据进行压缩,否则会产生较大延时
3-2)在处理器上处理
a 用CPU读取激光数据,同时单片机上传里程计积分数据,两者同时进行时间同步.在CPU同一进行运动畸变去除
b 体系清晰,不会产生延时
c 需要进行时间同步
d 需要进行位姿插值
4)运动畸变--轮式里程计
[图片中左侧最后一行,表示的是 最迟 的里程计数据时间戳]
4-1)求解位姿
[第一种情况概率基本为0,常见的情况是第二种]
[线性插值时,假设帧内机器人做的是匀速状态的]
4-2)二次插值
[二次插值,假设帧内机器人做的是匀加速运动]
4-3)二次曲线的近似
[对二次曲线取离散点,做分段线段,对二次曲线进行再次近似]
4-3)坐标系统-激光数据发布
[range,理解为距离;angle理解为角度,若机械式激光雷达转速恒定,那么通过此可推导出激光点的时间戳]
#####################
图片版权归原作者所有
致谢曾老师的付出
不积硅步,无以至千里
好记性不如烂笔头
感觉有点收获的话,麻烦大大们点赞收藏哈