一种可移动载体的LiDAR-GPS/IMU自标定方法与流程

本发明涉及传感器标定，尤其涉及一种可移动载体的lidar-gps/imu自标定方法。

背景技术：

1、在无人驾驶领域和机器人领域中，常用的传感器有lidar和相机，gps/imu以及轮速计，其中gps/imu是惯性测量单元(imu)与global positioning system(gps)相结合的综合系统，可以提供厘米级的绝对定位精度，单独的imu根据其硬件性能只能在短时间内获得比较准确的运动估计，但是定位误差会随着时间急速增加；lidar作为无人车最主要的测距传感器，可以通过点云配准实时估计出车辆的相对运动位姿，但是基于lidar的slam在几何特征信息稀疏的地方会存在退化的现象。相机是将三维场景投影到二维图像上，再通过这些图像估计出视觉里程计的运动状态，但是视觉里程计存在尺度不确定性，图像也易受曝光率和光线明暗的影响。

2、为了各个传感器取长补短，许多的任务都需要不止一种的传感器来完成，多传感器融合已经成为了自动驾驶的一个趋势。但是由于各个传感器的坐标系是不同的，所以来源于各个传感器的数据需要统一到同一坐标系下才能进行融合，这就需要计算出单个传感器的内参和多个传感器的外参。由于imu和lidar的内参通常由商家出厂时提供，一般需要解决lidar与gps/imu之间的外参标定问题。关于lidar与gps/imu的外参标定，目前面临许多挑战，例如自动驾驶中车辆的运动并不像机器臂的运动，他的运动主要是三自由度的平面运动，对其他自由度约束较小，所以其他三个自由度的误差相对较大。由于多线激光雷达和gps/imu在测量原理上存在较大差异，为了保证不同传感器能检测出相同的物体用于标定他们之间的外参，许多标定方法需要特定的车辆运行方式及人工标定标记场景，导致成本大、自动化程度低下。

3、在论文《vision meets robotics:the kitti dataset》中将通过点到面的lidar里程计与gps/imu里程计进行手眼标定的思想，其并没有一个初始外参的计算，所以它的标定结果更依赖于lidar里程计的配准精度。论文《3d lidar-gps/imu calibration basedon hand-eye calibration model for unmanned vehicle》中使用地面点优化求解z平移、roll、pitch角变换，然后将三维问题转为二维问题，通过手眼标定估计剩下的三个自由度。该方法可以视为将地面点优化出的三个自由度作为初值，然后进行手眼标定，其所展示的初值工作过于简陋和粗暴，导致最终得到的精度也相对较低。百度apollo项目中将标定分为两步，首先通过轨迹计算出外参初值，然后通过基于特征的方法进行外参的细化，但具体的实施策略并不完善。论文《improved vehicle lidar calibration with trajectory-based hand-eye method》采用大轨迹解决平面运动对其他自由度约束不强的问题，但是对于一个标定来讲，录制公里级的数据的要求是比较苛刻的。

技术实现思路

1、鉴于上述现有技术的不足，本发明的目的在于提供一种可移动载体的lidar-gps/imu自标定方法，即通过外参初始化和外参细化，及采用更多的策略，以得到标定精度更高、策略更加完善的可移动载体的lidar-gps/imu自标定方法。

2、本发明的技术方案如下：

3、一种可移动载体的lidar-gps/imu自标定方法，其特征在于，所述方法包括外参初始化和外参细化；其中：

4、所述外参初始化包括以下步骤：

5、s1：获取基于特征的lidar里程计和以lidar时间戳为基准插值后的gps/imu里程计，以构建手眼标定方程；

6、s2：根据所述手眼标定方程，解耦出可靠的旋转外参初值和平移外参初值，以得到初始外参值；

7、s3：通过所述初始外参值将lidar与imu紧耦合，再使用因子图优化消除lidar里程计的累积误差，完成全局地图的构建；

8、所述外参细化包括以下步骤：

9、s4：对所述外参初始化过程构建的所述全局地图进行滤波得到局部地图；

10、s5：通过将实时扫描点云与所述局部地图进行配准，得到lidar里程计的绝对位姿；

11、s6：将所述绝对位姿与所述初始外参值构建代价函数，进行非线性优化求解，从而得到更精确的细化外参值。

12、进一步地，所述步骤s1的具体过程包括：

13、使用恒速模型的六个自由度与地面点优化的三个自由度进行融合得到预测位姿；

14、计算每个点的俯仰角，根据俯仰角小于阈值进行粗提取，然后由随机采样一致性ransac算法进行精提取；

15、通过曲率的大小提取特征点，基于特征点构建残差进行配准得到帧间的lidar里程计，将帧间的lidar里程计用于后端帧到局部地图的配准的初值，从而对lidar里程计进行细化得到基于特征的lidar里程计。

16、进一步的，所述步骤s1的具体过程还包括：

17、同时根据lidar时间戳为基准对相邻的高频gps/imu做线性插值得到gps/imu的里程计；

18、由上述得到的lidar里程计和插值后的gps/imu里程计的相对位姿约束构建手眼标定方程ax＝xb：

19、

20、式中，{lk}和{ik}分别为第k帧的lidar位姿与此时插值得到的gps/imu位姿；为{ik}到{ik+1}的相对位姿；是从{lk}到{lk+1}的相对位姿。

21、进一步地，所述步骤s2的具体过程为：

22、将由式(1)建立的手眼标定方程分解为旋转项和平移项两个部分：

23、

24、

25、式(2)旋转项写成四元数的形式后并使用左右四元数乘法矩阵转化为矩阵乘法得到如下公式：

26、

27、其中是四元数乘法运算符，而和分别是左乘和右乘四元数乘法的矩阵表示形式。

28、进一步地，所述步骤s2具体过程还包括：

29、在不同时间累积测量数据之后，得到如下的超定方程：

30、

31、其中k是超定方程中旋转对的数量，是为了更好地处理离群值而使用的鲁棒权重；

32、当前旋转对在角度轴上的差异角度由式(4)计算，并作为huber损失的参数，其导数即为权重：

33、

34、其中，ρ()表示huber损失，qω是四元数q的实部，()*表示取四元数的逆。

35、进一步地，所述步骤s3具体为：

36、通过所述lidar-gps/imu初始化旋转外参初值和平移外参初值，将lidar和imu紧耦合，对两个连续帧的点云进行配准，再将所述相对位姿与通过地面点优化获得的z平移、roll角和pitch角结合，以此回环检测优化相对位姿，从而完成全局地图的构建。

37、进一步的，所述局部地图是由时间相邻的关键帧和位置相邻的关键帧所组成。

38、进一步地，所述步骤s4还包括：

39、判断所述关键帧；所述关键帧的判断只要当前帧相对于上一帧的相对位移或者相对旋转角度大于设定的阈值就可以视为新的关键帧。

40、进一步地，所述步骤s5具体包括：

41、通过与所述局部地图进行配准获得lidar里程计，lidar里程计和imu由初始外参参数紧密耦合，利用预积分的方法对lidar点云进行去畸变，并作为lidar里程计的初始猜测位姿，imu的偏差通过基于因子图的优化lidar里程计进行校正。

42、进一步地，所述步骤s6具体包括：

43、通过以下公式构建lidar里程计与外参的约束：

44、

45、其中是与当前lidar时间戳相对应的gps/imu位姿；

46、解耦成新的旋转部分和平移部分，即：

47、

48、由公式(12)构建绝对位姿与外参的代价函数实现外参参数的细化优化：

49、

50、通过levenberg-marquardt算法优化代价函数，当迭代收敛时，可获得最终精确的外参参数。

51、有益效果：本发明公开了一种可移动载体的lidar-gps/imu自标定方法，用于可移动的载体上，例如无人驾驶车辆和机器人等，相比于现有技术，本发明实施例提供了一种自动化程度更高，成本更低的外参自标定方法，通过采用更多的策略，例如回环检测和紧耦合策略，从而得到更高的标定精度、误差更小、策略更加完善的可移动载体的多个传感器的外参。