SLAM从入门到精通（lidar的运动畸变矫正）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        前面我们说过，很多时候传感器的数据并不能直接拿过来使用。这里面除了噪声的原因之外，另外一部分原因就是传感器数据本身也有可能是脏数据，存在运动畸变的可能性。以激光雷达为例，假设一个机器人往前行走，速度是1m/s，lidar旋转的速度是10次/s。一开始的时候，lidar检测前面1m处有一个物体。那么100ms之后，同样的物体，其实这个时候距离只有0.9m了。如果lidar的裸数据没有经过畸变矫正的话，那么不管制图还是定位，本身都是一个错误的结果。

1、处理方法

        目前主要的处理办法，就是利用其它传感器数据对lidar数据进行修正。由于其他传感器的频率，本身要比lidar快的多，所以默认短时间内其他传感器的数据是可靠、有效的。通常实践中，一般会用imu对lidar的theta进行修正，而用odom对lidar的x、y进行修正。

2、参考blog

        网上有一篇针对运动畸变非常好的文章，非常适合拿来一读，它的地址如下所示，

https://blog.csdn.net/tiancailx/article/details/114924197

        相关的代码可以参考这个链接，

https://github.com/xiangli0608/Creating-2D-laser-slam-from-scratch

3、收集imu和odom数据

        在处理lidar数据之前，一般需要对imu和odom的数据进行收集。收集的方法通常就是用queue保存接收到的imu和odom数据。主要目的，则是在处理lidar数据之前找到合适的imu和odom区间，这样才能对lidar数据进行调优处理。

4、lidar数据处理

        lidar数据主要分成这几个步骤。首先，需要缓存一帧lidar数据。这帧数据的结束时间，也就是下一帧数据的开始时间。另外，lidar上电的时候，一般也是机器人刚刚工作的时候。此时机器人还没有开始工作，也就没有移动，所以缓存一帧，一般问题不大。

        其次，对imu和odom数据分别进行裁剪。裁剪的目的主要是挑出合适的区间数据，然后对这些数据进行掐头去尾的操作，留出合适的数据范围。对每一个范围，计算出对应的角度偏移量和x、y偏移量。

        接着就是用imu和odom的数据对lidar数据进行修正。先确认当前数据的时间点，找出对应的imu和odom区间，通过角度偏移、x&y偏移以及插值的方法计算出总的偏移量，最后再用这些总的偏移量对lidar数据一个一个进行矫正。

        剩下来的工作就比较简单了，只需要发布数据和重置参数就可以了。发布lidar数据比较好理解，重置参数则是对imu和odom中过时的数据进行清空处理。

5、注意事项

        对lidar数据的矫正处理本质上需要有一定的soc算力做支撑。如果机器人本身行走的很慢，不做x和y方向的矫正也可以的。但是机器人旋转的话，对于lidar数据的影响还是蛮大的，最好借助于imu对于lidar的角度数据进行一下矫正。

        此外，最好不要用odom的角度数据对lidar数据进行矫正，因为odom的角度也是近似求解出来的，本身精度其实也比较差。