lidar和imu/gnss/ins 标定方法总结

前言

现在lidar和imu 标定主要有以下两个分支：（1）lidar 和 纯imu标定（2）lidar和组合惯导（gnss）标定
后者一般会采用rtk技术来获得odom，定位精度高，所以二者的标定技术其实不太一样，大多数人都容易搞混。一般来说，纯imu标定还需要标定其内参（游走噪声之类的），可以根据这点来快速区分是否为纯imu标定技术。

技术总结

以下是对于开源标定代码的一些总结，有关于lidar和纯imu的标定的，也有关于lidar和gnss/imu标定的。

可观测性lidar和imu内外参标定：
论文：最大的贡献点其实是，在非线性最小二乘优化中的可观测性感知状态更新机制仅更新具有截断奇异值分解的状态空间中的可识别方向，这使得即使在信息数据段不可用的退化情况下也能够获得准确的校准结果。
通俗点说就是：在优化lidar和imu外参的时候，通常要求必须激励各个轴，在其它论文中，通常都是小型实验室设备，这一点可以简单满足，但是在实际使用中，在车载设备上，通常只能进行平面运动，这情况对于z轴的拟合就会陷入局部优化当中，进而影响全部的参数拟合。而这篇论文通过在每次优化时，引入一个truncated singular value （截断奇异值）的概念，这个值可以判断哪个变量充分激励可以进行更新，哪个变量是退化的不进行更新。回到上述例子，这样就可以不对z轴进行更新（或者更新程度较小），避免对x和y轴拟合的参数进行干扰。虽然z轴的外参还是初始值，但是其不会影响对其它轴的标定。
代码：至于代码只能说半开源吧，作者将最重要的Observability-Aware 模块没有开源，在论文中，关于这部分的一些细节东西也没有体现，只能说相当可惜了。
（1）具体的环境要求跟接下来的一篇几乎一致，其 优缺点也在下面一并讲解。
（2）引入了lidar的内在模型，对lidar进行线性矫正，这点可能导致其不适用于固态激光lidar。
论文：如果上面的论文陌生，那么这篇你应该一定知道，这篇就是大家常说的浙大
lidar_IMU_calib标定工具箱，其实两篇论文都是一个作者，上面的内容有一大半跟这篇论文一致。论文部分，就不讲了，大家可以自己去看一下。
代码：已开源
（1）只支持vlp_16，但是根据作者所描述可以自己改接口，所以也可以兼容一些其它lidar类型.
（2）必须是要有组织的点云，对于一些国产lidar，只能输出无组织点云，作者说也可以，但是需要改一些代码，应该不太麻烦，但是必须要有每个lidar点云的反射强度和时间戳。
（3）最重要的就是要充分激励，否则自己会停止优化，当然可以修改阈值，但是不建议修改，如果不充分激励得到的外参也不会太精确，这点就不讲了。
（4）周围要有足够多的平面特征，因为用到了ndt，还有就是周围不要有会动的物体。
（5）时间戳，lidar和imu频率可以不一一对应。
（7）优化时间：由于会对所有点云进行失真矫正，所以优化速度会慢，特别是使用线束较多的lidar。
（6）这点是我的疑问：论文中对于初始值的求解，只进行了R的闭环求解，对于平移矩阵默认为0，对于如果lidar和imu平移位置相差较大时是否会对优化造成影响，这点暂不明确，不过以我的经验来看，问题倒是应该也不大（应该也不会有人把imu和lidar放到十几米远吧），只不过文中没有给出实验证明。

论文：这篇就是著名opencalib中关于lidar2imu部分的详细论文，论文将优化整体分为3个步骤：粗标定、细标定及z轴标定。其中主要贡献在于使用了八叉树的结构将点云重新组织，加快了优化过程，以及正对平面运动的车辆lidar-imu标定的z轴标定做了单独的优化标定。
代码：也是半开源。其中最主要的细标定和z轴标定部分没有开源。
（1）需要户外平面较多的地方，不可以有运动的人和车。
（2）支持其它线束的lidar，（固态lidar理论上也支持），需要微改代码中的参数设置。
（3）虽然名字叫lidar2imu 但是只有imu不行，需要gnss/ins，因为需要输出精确的姿态变化，单靠imu误差太大。
（3）优化速度快
（4）由于论文中没有对初始值进行封闭求解，且进行优化时对假设初始值的精确要求较高。因此需要有一个较为准确的初始值，而初始值的获得在论文中却没有提及，最后实验部分表示，在旋转10°和平移0.5m之内可以进行外参优化。因此推测，手工测量外参作为初始值，大概率是可以的。此外，代码中还提供了一个手动标定工具，可以获得外参的初始值，推测论文中应该是使用其来获得外参初始值，但其具体使用方法不明。
（5）最可惜的Z轴标定没有开源，不过看其论文其实现难度不大，但对其精度0.015m保有疑问。不知道其基准点是如何进行精确测量的，rtk的误差也有1到3cm误差。

论文：没有
代码：
（1）只支持gnss，不支持纯imu标定
（2）需要有平面较多
（3）作者说不支持车载平面运动标定，issue里有人做了，看起来建图结果还不错。
（4）没有实验数据来进行具体分析误差，倒是可以上一篇文章中有对比，可以看一下，误差大概平移在20-30cm之间（在平面运动下）。
（5）是否支持固态lidar目前不清楚，是否支持无组织点云不清楚？从实现逻辑上看貌似不需要，但实际代码细节并未深入了解，暂不清楚。
（6）但是我根据其他人的博客，根据他的思路实现复现了一下， 在其优化算法复现的时候，我的实现效果一般（可能是我对具体算法理解有误），不如opencalib中的效果好。

使用手眼标定进行lidar和rtk之间的标定。
论文：无
代码：半开源，其中rs_mapping 工具包部分缺失。
（1）也是需要较多平面特征（猜测），由于代码不全推测使用icp或者ndt进行求取lidar_pose
（2）对于固态lidar可能不适合，因为要保证计算全部的lidar_pose,当lidar发生旋转时计算可能较为困难。
（3）不支持固态lidar。

论文：就是很有名的opencalib论文关于lidar2imu的详细介绍补充，可以看出其名字已经改为a 3D-LiDAR and a Pose Sensor ，说明之前对lidar2imu起名是有一些偏差的，其是不太支持纯imu标定，未对imu进行内部参数标定，但是其提出的方法十分新颖，根据八叉树拟合平面，最小化其空间上对应平面的点，其优化速度快，效果明显，值得大家去学习一下，而且还提出了一种方法，对于自动驾驶中，车辆只能沿平面运动标定，z轴较大误差的情况，进行Z轴标定。
代码: 同样是半开源，其中细标定和Z轴标定部分不开源。
（1）用于自动驾驶领域，只能进行平面标定的场景
（2）该优化算法依赖于其初始值的精确程度，尤其z轴的平移，但是文章中和代码中未详细提到初始值的获取。
（3）需要有足够多的平面建筑物，而且其粗优化过程中粗暴限制了z轴的更新，因此也要尽可能保证地面是平坦的。
（4）要有ring，但可以是无组织点云，固态激光lidar大概率也没有问题。
（4）该方法好像前面已经写过了，哈哈，忘记了，就当补充了。

总结

总结，现在网上开源的一共也就怎么5-6种（在展望中我又补充了两种，请往后翻），（如果还有其它的方法请告诉我），但其效果或多或少都有一些不尽如人意，要么需要初始值尽可能的精确、要么无法兼容固态lidar和无组织点云、要么需要大量的平面特征物、以及对于车载只能进行平面运动的Z轴标定误差大且其误差程度难以衡量。
由于笔者的需求是进行车载固态lidar_rtk的标定，且输出无组织点云数据，因此，面对此问题，对上述开源代码取其长处，基于matlab开发了一款轻量型标定程序。
优点：
该代码无需有组织，ring和强度等点云信息，只需要点云的坐标位置，对于固态还是机械已经4D成像毫米波理论上都兼容，而rtk数据与opencail类似包括rtk对于某一固定坐标系的旋转矩阵和平移向量，无需初始值（默认单位矩阵即可）。
主要思路就是先利用icp进行求取点云位姿变换关系，之后进行手眼标定求解初始值 ，思路与后续提到的target-free Extrinsic Calibration of 3D-Lidar and Imu论文大体上类似，求出初始值后，针对其初始值进一步细化其位姿关系，迭代求取其最佳变换（建议：如果细优化过程，嫌弃太慢，可以去利用opencalib工具箱细化），此外采取差分式位姿求取，对于纯imu可能也有较好的表现（并未作过测试，只是理论上，但标定效果肯定是不如rtk的）。最后借鉴第一篇论文的可观测性概念，对结果给出误差分析，根据误差结果可以分析出那个方向激励未充分，进而做出改进。
如果车载平面标定，其Z轴误差很大，利用opencalib的思路进行Z轴标定（代码中稍有改动，根据上述误差分析，进行选择更新优化，作为最后的优化结果）避免做非平面运动时对之前优化结果的影响。
最后输出配准和定位建图。
缺点：耗时长。（没有办法，😦 细优化过程中，包含大量对点云迭代操作，如果介意可以使用opencalib工具箱细化）；
以及需要特征明显的建筑物(非平面特征物也可以)，这一点无可厚非，减少运动的行人和车辆。对于缺少特征无法匹配时程序会给出相应提醒。

程序运行视频

接下来是程序运行情况演示：数据集来自opencalib lidar2imu示例数据集

lidar_rtk_效果演示

最后录制结果时，不小心将之前配准的窗口放大了，导致建图的效果有些叠加混乱，见谅，可以看到效果图大致和opencalib粗标定官网给出的效果图类似，但是对于其细标定效果图，还是有一些差距。不过在项目使用中已经足够了。
大概运行时间：300s

如何运行

lidar_file_name='C:\Users\Administrator\Downloads\data(1)\data\top_center_lidar';
rtk_file_name='C:\Users\Administrator\Downloads\data(1)\data\NovAtel-pose-lidar-time.txt';
format long g
step=20;%帧间采样频率，20张采集一张
min_Threshold=[4,3,3,2,1];%icp的阈值设置
ratio=[5,1,0.5,0.1,0.1]; %降采样的比例，设置五次优化，最小的比例为0.1，理论越小越精确，但是运行时间会变慢
%  R_imu2lidar=[  0.000534077,0.999956, 0.00939283 ;  -0.999853,0.00037313, 0.0171284;   0.0171242,-0.0094006,0.999809]; % imu2lidr 初始值旋转矩阵，按照按照位置，给出大致方向即可，该值不参与任何计算。
%  t_imu2lidar= [-1.20793;   -0.0217731;-1.37729]; % 常量平移向量  imu2lidr 初始值平移矩阵，如果不知道默认为0，尽量将Z轴进行手动测量，该值不参与任何计算，只做显示。
Z=1.2;% 常量平移向量  lidar2imu ,Z值手动测量。
% 构建常量变换矩阵 T_c

T_lidar2imu_init=[1 0 0 0; 0 1 0 0; 0 0 1 0 ; 0 0 0 1];%无需更改
%开始标定
lidar_imu_cali(lidar_file_name,rtk_file_name,step,min_Threshold,ratio,Z,T_lidar2imu_init);

结果报告：

优化完成,下面是结果报告：
T_lidar2imu:
       0.00600551511330221        -0.999838017387414         0.016966813932771         0.108020483540084
         0.999919938597846       0.00581529865711408       -0.0112382692621306          1.21307938920761
        0.0111377817676359        0.0170329471417585         0.999792892817789          4.67393526885325
                         0                         0                         0                         1

误差分析：
       0.00146346215008086
         0.140534153343232
        0.0126123099843301
        0.0965488891989472
       0.00153766572927945
          364.303220016957

说明：该值说明估计值的一致性关系，越接近零代表估计结果越准确，依次代表：绕Z轴旋转,绕Y轴旋转，绕X轴旋转，x轴方向平移误差，y轴方向平移误差，z轴方向平移误差
从该值可以判断哪个方向未充分激励，请增加该方向的数据集
最终结果为
       0.00600551511330221        -0.999838017387414         0.016966813932771         0.108020483540084
         0.999919938597846       0.00581529865711408       -0.0112382692621306          1.21307938920761
        0.0111377817676359        0.0170329471417585         0.999792892817789                       1.2
                         0                         0                         0                         1

建图结果

存在问题

目前进展对于Z轴标定还停留在半自动标定上，需要人工选点，可能后期会增加固定特征物识别，自动选点，实现自动标定。这点留作以后更新。

开源问题

开源问题：目前用作项目，暂不打算开放源码，后续可能会以matlab工具箱app或者p文件的形式发布出来（如果没有发出来，就是懒，欢迎大家狠狠私信评论催更），方便大家可以直接使用和调用。
并且在最后会给大家推荐一些个人感觉好用的开源lidar-imu\gnss标定工具。

展望

• 对于lidar的运动失真畸变没有考虑，因为考虑到移植性，目前只是需要最少的信息来完成标定，如果考虑其失真，还需要假如其点云中每个点的时间戳。因此对于太剧烈的运动，尽可能的避免，请缓慢旋转&平移。
• 对于纯imu的适配：目前难点，由于其存在偏置和积分误差，在于如何获得其准确位姿，目前还在学习当中。
  补充：最近在学习target-free Extrinsic Calibration of 3D-Lidar and Imu 这篇论文时，发现其实现主体思路跟我的实现类似，有需要的小伙伴可以去看看，代码也是开源的，而且对于我展望的内容也做了很好的实现。
  其次在补充一篇论文GRIL-Calib: Targetless Ground Robot IMU-LiDAR Extrinsic Calibration Method using Ground Plane Motion Constraints 这篇论文对于imu-lidar 只做平面运动时，如何获得Z轴精确位姿，利用平面拟合地面，也给出了一定的优化思路，不过对于固态lidar明确表明不支持，对于是否需要有组织点云，暂不清楚。

最后

推荐环节：
可以充分激励下：
imu-lidar ：推荐 target-free Extrinsic Calibration of 3D-Lidar and Imu 开源代码
gnss-lidar ：推荐 上面几篇效果都差不多，其中opencalib 论文效果比较不错，可惜没有开源细标定部分，因此lidar_align也都可以。
平面运动下：
imu-lidar ：推荐 GRIL-Calib: Targetless Ground Robot IMU-LiDAR Extrinsic Calibration Method using Ground Plane Motion Constraints 开源代码
gnss-lidar ： 推荐opencalib

最重要的

上述涉及很多个人主观性评价，如有疏漏或错误，在所难免，请及时告诉我，保证在第一时间更改删除