• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

激光SLAM:LOAM-Livox 算法研究(3)–前端处理与后端迭代位姿优化

人工智能 月照银海似蛟龙 1325次浏览 0个评论

上一篇中对 LOAM-Livox 算法做了整体的总结。 最后给出了 算法的整体框图 如下:   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   本篇主要对LOAM-Livox 算法进行 前端处理 和 迭代位姿优化 的 总结  

1 前端处理

  LOAM-Livox 算法 的 前端 处理 也就是红框的部分   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   主要包含两个环节  

  • good points 选取
  • 特征点提取

 

1.1 good points 选取

  考虑到激光雷达低等级的物理特性(点云光斑大小、信噪比),通过点的等级机制来选取出“优点”  

1.1.1 距离

  激光雷达直接输出的点的格式可以选择 点云格式   X、Y、Z 分别代表雷达的前、左、上的方向坐标轴,雷达输出的三维坐标点P则可以用(Px,Py,Pz)表示   测量点P到雷达 的 距离计算公式如下   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   筛选:去除掉过近或过远的点。  

1.1.2 偏转角

  雷达的偏转角是雷达射线和X轴的夹角,计算公式如下   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   筛选:接近边缘的点,通过偏转角的大小可以判断是否接近边缘 对于 Livox MID40,偏转角大于17,则舍去。  

1.1.3 强度

  雷达直接返回量 如果雷达直接返回的是反射率,则通过下面公式计算   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   R是物体的反射率,通过雷达传感器测量得到(一些雷达比如, Velodyne Puck,直接返回强度而不是反射率。   筛选:去除掉强度过大或过小的点。强度较小意味着测量点离雷达较远,或者物体的反射率很低。强度过大怎距离较近。  

1.1.4 入射角

  入射角,是雷达射线和测量点周围的平面的夹角   其中 a点和c点是b点相邻的两个,关系如下图所示   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   计算公式如下   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   公式的含义就是通过向量点乘的方式,计算向量ca和ob的夹角。   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   筛选:入射角接近π或者0,去除。 Livox MID40的范围为5°-175°,像图中的f点,因为这样角度入射的激光点会被拉长。  

1.1.5 遮挡点

  说白了就是藏在物体后面的点,想上图的e点。   通过下面的公式判断 这种点   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   筛选:去除这种点,这样的点很容易造成边缘特征的误识别。  

1.1.6 总结

  去除掉如下的点:  

  1. 距离过大和过小的点
  2. 接近边缘的点
  3. 强度过大的点和过小的点
  4. 入射角接近π或者0
  5. 藏在物体后面的点

 

2 后端迭代位姿优化

  由于固态雷达为非重复式扫描,提取的特征点不能持续的被跟踪。 即使雷达静止不动,扫描的轨迹和特征点和前一帧比也是不同的。 LOAM-Livox 算法 用了一种迭代式位姿优化方法来计算雷达的位姿。 实现了实时里程计和建图,频率20HZ  

2.1 边缘到边缘的残差

  εk 表示 当前帧的所有边缘特征点 εm 表示 地图中所有边缘特征点   对于εk中的每一个点,从εm中找5个和这个点最近的点,向下面那样   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   为了提高搜索的速度,建立εm(地图所有边缘特征点)的KD-tree, KD-tree由另一个线程维护,由最新的雷达数据帧实时更新 这样当新的帧来时,KD-tree可以立即被使用。   Pl 为当前帧(k帧)中的边缘特征点(εk )中的一个点 注意Pl是在雷达坐标系下的点,然而εm中的点是地图坐标系下的点。 为了在εm中找到和Pl最近的点,需要将Pl转换到地图坐标系下,通过下面的方式   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   (Rk, Tk)是 当前帧中最后一个被采集的点 时刻的 雷达的位姿,需用通过位姿优化得到。 在这使用这个雷达位姿作为当做帧所有点的雷达位姿 用这个位姿,当前帧所有点就可以投影到地图坐标系下。   Pi表示第i个离Pw最近的点在εk中, 为了保证Pi在一条直线上,计算m个离Pw最近点的平局值µ和协方差矩阵Σ,设置m为5 如果协方差矩阵Σ的最大特征值比第二大特征值大三倍以上,那么最近的这个几个点就在一条直线上 残差的计算公式如下:   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   这个公式的含义: 通过向量叉乘求点到线的距离 假设给出三个点,A,B和C,你想找出点C到点A、B定出的直线间距离。 就是d = (AB x AC)/|AB|  

2.2 面到面的残差

  和边缘到边缘的残差类似,对于一个点在面的特征点计里面,以Pk为当前帧的一个面的特征点,从地图坐标系下找5个最近的点以Pm表示。 假设这5个最近的点在一个平面上,通过计算他们协方差矩阵。 如果最小的特征值要小于第2小的特征值的3倍。那么则认为在一个平面上。 通过下面的公式计算残差   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   公式的含义就是计算点到面的距离(法向量法)  

2.3 运动畸变校正

  像之前提及的, 当雷达一直在移动,三维点将会在不同时刻不同位置被采集。 减小运动畸变的影响,做如下两种方法的处理  

2.3.1 分段处理

  一种简单但是有效的方法来减小运动畸变的影响就是通过分段处理。 将输入的一个帧分成3个连续的子帧。 然后,这个三个子帧 和 当前建立的地图 分别 做匹配 在子帧扫描匹配的过程中,通过子帧中最后一个点时刻雷达的位姿,子帧里面的所有点转换到地图坐标系下 通过这样做,每一个子帧的时间间隔是原始帧的三分之一 分段处理在算法中通过多线程的方式并行处理子帧匹配,相当于算法加速。  

2.3.2 线性差值

  经常用的进行运动畸变校正的方法就是通过线性差值的方式。 用(Rk,Tk)代表当前帧的最后一点的雷达位姿 (Rk-1,Tk-1) 是 前一帧的最后一点的雷达位姿   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   是前一帧和当前帧的旋转和平移关系 当前帧的最后一点的雷达位姿和 前一帧的最后一点的雷达位姿 则存在如下关系   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   设t(k-1)是前一帧雷达里面的最后一个点的采样时刻 对于当前帧的每一个点都存在采样时间t t∈[t(k-1),t(k)] s是时间的百分比   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   可以算出在t时刻的线性插值的位姿 先求出和前一帧最后一点的旋转和平移关系   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   其中θ   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   ω 是 其旋转向量 ω ^是ω 的斜对称矩阵   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   有另一种更快速的计算方式   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   上面式子仅有 sin(sθ) 和cos(sθ)需要计算,其它的都是固定值,这样计算可以减小计算时间 最终在t时刻雷达的位姿为   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   将这个点投影到地图坐标系下   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化  

3 迭代位姿优化算法流程

  在执行迭代位姿优化的每次迭代中,重新找每个特征点的最近邻点,并且在目标函数加入边缘残差和平面距离残差。首先迭代2次,用结果计算残差,剔除最大的20%点云,然后做完全的位置优化。   激光SLAM:LOAM-Livox 算法研究(3)--前端处理与后端迭代位姿优化   下面一篇做 LOAM-Livox 算法 代码研究


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明激光SLAM:LOAM-Livox 算法研究(3)–前端处理与后端迭代位姿优化
喜欢 (0)

您必须 登录 才能发表评论!

加载中……