Cartographer——Lidar数据初始化

1.Tim561激光雷达

  雷达都是扫描一圈360°,Tim561固定是270°范围的点, 扫描频率15Hz,则相邻点云的时间步长为time_step=1/(15*360*3)。
如上图左,箭头对应右图的90°方向,激光有效扫描范围(-45°,225°),角度步进0.3345719°(参考西克的官方手册),扫描一帧得到连续的808个点的距离值,即雷达传过来的数据。
 

2.NAV310雷达

  雷达有效扫描范围可以自行根据频率设置,与Tim561的区别在于:

1.NAV310可以设置扫描范围为360°,和更快的频率
2.驱动:Tim561返回的distance信息,16位字符串转无符号int类型后。是以毫米为单位,除以1000即为以米为单位的distance,而NAV310,高八位以米为单位,低八位需除以256再和高八位的整数相加,最后得到以米为单位的distance。
注NAV310的最终distance计算方式:D=distance/256+(distance%256)/256 。(参考西克的官方手册)
 

3.数据初始化

  下面以Tim561雷达为例,介绍一帧雷达数据转成点云的过程。
将一帧激光雷达数据存在数组里,如Range[808],实际使用时,机器人坐标系的x轴在雷达的90°方向,我们以x轴为正方向,首先要得到的是每个点云的distance及它相对机器人坐标系x轴的角度。由于雷达内的转轴是正时针旋转,那么第一个点云的角度为-135°,后面每个点云的角度=前一个点云的角度+角度步进值。这步操作可以和第1点介绍的distance解码同时进行,那么在雷达驱动部分,就可以得到每个点云的distance及相对x轴角度的二维数组。
  接下来将二维数组转成点云,每个点云包含在世界坐标系下的x,y,z坐标及点的时间信息。关键代码如下:
  方法就是:1.当前点的distance*(1,0,0),即距离乘以x轴的单位向量,设得到坐标G(distance,0,0)
                    2.将当前点的角度换算成旋转矩阵R,坐标G左乘旋转矩阵R,得到的就是当前点云相对雷达的坐标
                    3.计算当前点的时间:一帧点云生成,这帧点云的时间为当前的系统时间(orUTC时间)设为T,那么每个点的时间就是往后递推。如,设当前为第i个点,那么它的时间为t=T-i*time_step。为什么要有每个点的点云时间?这个会在后续点云畸变滤除中讲到,因为雷达帧率太低,在这个time_step下会滤掉很多正常的点(可以先思考下,点云同步会讲(会涉及多雷达融合)),我们设置time_step的值非常小,以至于可以认为一帧雷达的所有点都是同时生成的,那么这就直接干掉了点云的运动畸变去除的功能(多传感器融合会讲),但是在AGV小车上运行正常,应该还是车速不快的缘故(才3m/s左右)。
 
  通过以上3步,得到了一组点云,但这些点云是相对于雷达坐标系的点云,最后一步:设机器人坐标系到雷达坐标系的旋转矩阵pose(含平移t和旋转R),遍历每个点云,用点云坐标左乘pose。
  这个时候的点云数据ranges(x,y,z,time),就是最终喂给算法的数据结构,但是它是和当前帧点云时间,初始位姿合成一个数据结构TimedPointCloudData喂给了算法,即:
,其中
 

4.数据流

  • 2
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值