[发明专利]组合导航设备与激光雷达的外参标定方法及装置

权利要求书

1.一种组合导航设备与激光雷达的外参标定方法，其特征在于，包括：

分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；

将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；

根据所述时间同步后的时间戳信息以及所述原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果；

根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；

基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。

2.根据权利要求1所述的一种组合导航设备与激光雷达的外参标定方法，其特征在于，根据所述时间同步后的时间戳信息以及所述原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果，包括：

分别获取所述原始位姿数据中每个位姿点当前时刻到初始时刻的位姿变换矩阵以及相应的时间戳信息，并获取所述每个雷达点在组合导航设备相应时刻坐标系下对应的虚拟点；

根据所述时间同步后的时间戳信息，从所述原始位姿数据中获取与所述原始点云数据中每个雷达点对应的时间戳相邻的相邻位姿点；

基于所述相邻位姿点的时间戳信息以及所述相应的位姿变换矩阵，计算所述虚拟点到组合导航设备初始时刻点的位姿变换矩阵，作为位姿插值结果。

3.根据权利要求1所述的一种组合导航设备与激光雷达的外参标定方法，其特征在于，根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值，包括：

从所述原始位姿数据中选取多个位姿点，并从所述原始点云数据中选取与每个所述位姿点对应的雷达点；

分别将各个位姿点与相应的雷达点代入预先构建的变换点坐标表达式，获得多个以待求外参矩阵为未知量的待求解方程，将多个所述待求解方程联立，计算得到外参初值。

4.根据权利要求3所述的一种组合导航设备与激光雷达的外参标定方法，其特征在于，所述变换点坐标表达式用于描述变换点坐标是通过所述虚拟点到组合导航设备初始时刻点的位姿变换矩阵、预先构建的待求外参矩阵以及雷达点坐标计算得到的。

5.根据权利要求3所述的一种组合导航设备与激光雷达的外参标定方法，其特征在于，基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解，包括：

基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解。

6.根据权利要求5所述的一种组合导航设备与激光雷达的外参标定方法，其特征在于，基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解，包括：

基于所述外参初值，对待求外参矩阵进行赋值；

根据赋值后的待求外参矩阵和所述变换点坐标表达式，获取所述原始点云数据中每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，得到变换点集合；

分别在所述变换点集合中获取每个变换点对应的至少两个距离最近的邻近变换点，并计算每个变换点与其对应的至少两个邻近变换点的距离；

将各个所述变换点与其对应的至少两个邻近变换点的距离求和，得到最近距离点的距离之和；

以所述最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；

直至所述最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。