SLAM-去除激光雷达运动畸变／去除激光帧畸变

1 激光雷达运动畸变

• 1）激光帧产生畸变的根源：
  　　每个激光点都具有不同的基准位姿。
• 2）产生的原因：
  　　激光测量时伴随机器人的运动，激光点的数据不是瞬时获得的，当激光帧率较低时，机器人运动带来的激光帧运动畸变不能忽略。（如５Ｈｚ的激光雷达，一帧激光的首尾时间差为２００ｍｓ，如果机器人转速为１.５７ｒａｄ／ｓ，那么首尾激光点基准坐标系位姿误差将达到１８度）

2 去除畸变原理

• 去除激光帧畸变原理：把一帧激光的每个激光点坐标变换到不同时刻的机器人里程计上
• 里程计辅助方法：ｉｍｕ、轮式里程计
• １）ＩＭＵ
  　　直接测量获得角速度和线速度，但对于机器人位移和角度需要积分
  　　更新速度高１ｋｈｚ－８ｋｈｚ
  　　具有较高的角速度测量精度，但线加速度精度差
• ２）轮式里程计
  　　更新速度高１００－２００Ｈｚ
  　　（一般机器人轮式里程计200hz，即可认为满足一定的要求，200hz一帧５ｍｓ，1.57rad/s下，误差仅为0.45度，可认为机器人没有运动。）

3 程序流程

1) main-构造函数
    订阅scan话题
    调用scancallback处理
2) scancallback
    scan话题数据提取到vector数组(包括angles,ranges) 
    vector数组保存到pcl::PointCloud<plc::PointXYZRGB>类型的点云中
    调用Lidar_Calibration进行畸变矫正
3) Lidar_Calibration
    程序对激光雷达数据进行分段线性插值*****
    调用Lidar_MotionCalibration激光雷达运动畸变去除分段函数*****
    调用getLaserPose得到激光雷达在odom中的位姿

4 程序实现显示

• scan使用ros话题显示
• 校正后的使用pcl_viewer显示
  　(后续可以把pcl改为话题发布,这样数据一块显示可以作比较)