[发明专利]一种激光雷达-相机-惯导联合标定方法及系统

权利要求书

1.一种激光雷达-相机-惯导联合标定方法，其特征在于，包括：

步骤1、通过激光雷达、单目相机和GPS/IMU同时进行数据采集，获取激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息；

步骤2、根据采集的数据，分别获得激光雷达位姿估计结果单目相机位姿估计结果以及GPS/IMU设备自身在全局坐标系下的位姿

步骤3、将获取的三种位姿估计结果进行数据关联，确定位姿之间的对应关系；

步骤4、将关联后的三个位姿序列两两之间构建手眼标定问题，并将手眼标定问题转化为优化问题；

步骤5、求解优化问题，将不同传感器轨迹转换到同一坐标系下；

所述步骤3中，采用位姿插值的方式进行数据关联：在三种位姿序列中任选两种位姿序列进行插值，获得同一时间戳下两个位姿序列中对应的位姿；然后在具有同一时间戳的两个位姿序列中任选一个位姿序列与余下的位姿序列再次进行插值，完成数据关联；

所述的位姿插值包括以下步骤：

步骤31、输入两个位姿序列，并判断其中第一位姿序列是否遍历完成，若完成，则输出与第二位姿序列的插值结果，反之，则进入下一步；

步骤32、取第一位姿序列中的时间戳，判断其是否落在第二位姿序列中，若落入，则进入下一步，反之，则返回步骤31重新判断第一位姿序列是否遍历完成；

步骤33、将第二位姿序列的平移部分和旋转部分分别进行插值，由此获得同一时间戳下位姿序列和位姿序列中对应的位姿。

2.如权利要求1所述联合标定方法，其特征在于，所述步骤2中，采用激光SLAM算法获得激光雷达位姿估计结果采用视觉SLAM算法获得单目相机位姿估计结果采用惯性导航设备获得自身在全局坐标系下的位姿

3.如权利要求2所述联合标定方法，其特征在于，所述单目相机位姿估计结果

为第i帧单目相机相对于自身起点的位姿；表示视觉里程计估计出的旋转部分位姿；表示视觉里程计估计出的平移部分位姿，缺乏尺度；C₀表示单目相机起始位置坐标系，C_i表示第i帧单目相机坐标系。

4.如权利要求3所述联合标定方法，其特征在于，所述视觉里程计估计出的平移部分位姿缺乏尺度，需要通过估计尺度s恢复其实际位姿

5.如权利要求1所述联合标定方法，其特征在于，所述步骤4中，构建方法包括以下步骤：

步骤41、计算位姿增量；

步骤42、根据位姿增量确定手眼标定问题；

步骤43、将手眼标定问题转化为优化问题。

6.如权利要求5所述联合标定方法，其特征在于，所述步骤43中，单目相机-激光雷达之间尺度单目相机-惯性导航设备之间的尺度之间应当满足如下关系：

引入新的代价函数项：

完成优化问题构建；其中，为激光雷达到相机的外参矩阵，为激光雷达到惯性导航设备的外参矩阵，为相机到惯性导航设备的外参矩阵。

7.如权利要求1所述联合标定方法，其特征在于，所述步骤5中，通过非线性优化的方式求解优化问题，获得激光雷达、单目相机、惯性导航设备之间的外参以及单目相机运行中的尺度利用外参和尺度将不同传感器轨迹转换到同一坐标系下。

8.一种激光雷达-相机-惯导联合标定系统，其特征在于，包括：数据采集模块、位姿估计模块、数据关联模块、优化问题构建模块和优化求解模块；

所述数据采集模块，通过激光雷达、单目相机和GPS/IMU同时进行数据采集，获取激光雷达点云、单目相机图像以及GPS/IMU设备位姿信息；

所述位姿估计模块，根据采集的数据，分别获得激光雷达位姿估计结果单目相机位姿估计结果以及GPS/IMU设备自身在全局坐标系下的位姿

所述数据关联模块，将获取的三种位姿估计结果进行数据关联，确定位姿之间的对应关系；

所述优化问题构建模块，将关联后的三个位姿序列两两之间构建手眼标定问题，并将手眼标定问题转化为优化问题；

所述优化求解模块，求解优化问题，将不同传感器轨迹转换到同一坐标系下；

所述数据关联模块中，采用位姿插值的方式进行数据关联：在三种位姿序列中任选两种位姿序列进行插值，获得同一时间戳下两个位姿序列中对应的位姿；然后在具有同一时间戳的两个位姿序列中任选一个位姿序列与余下的位姿序列再次进行插值，完成数据关联；

所述的位姿插值包括以下步骤：

步骤31、输入两个位姿序列，并判断其中第一位姿序列是否遍历完成，若完成，则输出与第二位姿序列的插值结果，反之，则进入下一步；

步骤32、取第一位姿序列中的时间戳，判断其是否落在第二位姿序列中，若落入，则进入下一步，反之，则返回步骤31重新判断第一位姿序列是否遍历完成；

步骤33、将第二位姿序列的平移部分和旋转部分分别进行插值，由此获得同一时间戳下位姿序列和位姿序列中对应的位姿。