CN114325667A - 组合导航设备与激光雷达的外参标定方法及装置 - Google Patents

Abstract

本发明提供一种组合导航设备与激光雷达的外参标定方法及装置，本发明提供的组合导航设备与激光雷达的外参标定方法及装置，通过获取较为理想的外参初值，可以有效提高最优外参标定解的求取效率和结果精度，通过位姿插值的方式估计激光雷达的位姿信息，相比于通过跑SLAM算法进行雷达位姿估计的方式更加便捷、高效，进而降低了整个外参标定过程的耗时，使得外参标定过程更加高效，外参标定结果更加准确。

Description

组合导航设备与激光雷达的外参标定方法及装置

技术领域

本发明涉及智能驾驶技术领域，尤其涉及一种组合导航设备与激光雷达的外参标定方法及装置。

背景技术

高精度的组合导航设备和激光雷达是作业机械高精度定位与感知技术中常用的传感器，在实际应用过程中需要获得组合导航设备与激光雷达的位姿转换关系，但仅根据这两种传感器在作业机械上的安装位置，采用外参测量手段是很难获得外参标定参数的。

目前，组合导航设备与激光雷达的外参标定，主要通过将激光雷达采集的数据转换到组合导航设备的坐标系下实现。一方面，可以利用外界传感器直接进行外参测量，但由于组合导航设备与激光雷达的安装位置不同，直接测量过程操作复杂，且测量误差较大。

另一方面，还可以利用激光雷达跑SLAM算法的方式获得激光雷达的里程信息，再结合组合导航设备自身计算的里程信息，进行两坐标系间的变换矩阵计算，即可得到两传感器间的外参标定结果，然而，上述方法需要进行大量的计算，耗时长，且外参标定结果准确度较低。

发明内容

本发明提供一种组合导航设备与激光雷达的外参标定方法及装置，用以解决现有技术中组合导航设备与激光雷达的外参标定方法操作繁琐、耗时长且结果准确度低的缺陷，实现高效、准确的组合导航设备与激光雷达的外参标定。

第一方面，本发明提供一种组合导航设备与激光雷达的外参标定方法，该方法包括：

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

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

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

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

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

根据本发明提供的一种组合导航设备与激光雷达的外参标定方法，根据所述时间同步后的时间戳信息以及所述原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果，包括：

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

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

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

根据本发明提供的一种组合导航设备与激光雷达的外参标定方法，根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值，包括：

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

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

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

根据本发明提供的一种组合导航设备与激光雷达的外参标定方法，基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解，包括：

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

根据本发明提供的一种组合导航设备与激光雷达的外参标定方法，基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解，包括：

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

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

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

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

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

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

根据本发明提供的一种组合导航设备与激光雷达的外参标定方法，基于所述外参初值，以所述原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解，包括：

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

根据赋值后的待求外参矩阵和所述变换点坐标表达式，获取所述原始点云数据中当前扫描帧内每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第一变换点；

确定当前扫描帧内每个雷达点在下一扫描帧内距离最近的雷达点，并获取所述距离最近的雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第二变换点；

将所述第一变换点和所述第二变换点作为最近距离点对，并将所述原始点云数据中所有最近距离点对组合为最近距离点对集合；

计算所述最近距离点对集合中所有最近距离点对的距离之和，得到最近距离点的距离之和；

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

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

第二方面，一种组合导航设备与激光雷达的外参标定装置，该装置包括：

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

第一处理模块，用于将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；

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

第三处理模块，用于根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；

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

第三方面，本发明还提供一种作业机械，该作业机械使用上述任一种所述的组合导航设备与激光雷达的外参标定方法。

第四方面，本发明还提供一种组合导航设备与激光雷达的外参标定系统，该系统包括：

待标定激光雷达，用于采集目标物的原始点云数据；

待标定组合导航设备，用于采集目标物的原始位姿数据；

控制器，用于分别获取所述原始位姿数据以及所述原始点云数据；将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据所述时间同步后的时间戳信息以及原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果；根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；并基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。

本发明提供的组合导航设备与激光雷达的外参标定方法及装置，通过获取较为理想的外参初值，可以有效提高最优外参标定解的求取效率和结果精度，通过位姿插值的方式估计激光雷达的位姿信息，相比于通过跑SLAM算法进行雷达位姿估计的方式更加便捷、高效，进而降低了整个外参标定过程的耗时，使得外参标定过程更加高效，外参标定结果更加准确。

附图说明

为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明提供的组合导航设备与激光雷达的外参标定方法的流程示意图；

图2是外参初值的计算流程示意图；

图3是本发明提供的组合导航设备与激光雷达的外参标定装置的结构示意图；

图4是实施过程中搭建的外参标定系统的硬件架构示意图；

图5是外参标定系统的工作流程示意图；

图6是本发明提供的电子设备的结构示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1示出了本发明实施例提供的组合导航设备与激光雷达的外参标定方法，该方法包括：

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

本实施例中组合导航设备主要针对的是RTK/IMU组合导航设备，RTK(Real TimeKinematic，实时动态定位)，是一种卫星导航技术，用于提高从基于卫星的定位系统获得的位置数据的精度，除了信号所包含的信息之外，RTK还使用载波信号相位的测量值，并依靠单个参考站或内插虚拟站进行实时校正，以提供高达厘米级的定位精度。

IMU(Inertial Measurement Unit，惯性测量单元)，是主要用来检测和测量加速度与旋转运动的传感器。

因此，RTK/IMU组合导航设备可以同时获取卫星导航数据以及惯性测量数据，为后续外参标定过程提供可靠的数据支持。

本实施例中激光雷达主要针对的是3D激光雷达，3D激光雷达可以获取三维深度信息和点云数据，具有高分辨率和低功耗的优点，是作业机械上使用较为普遍的感知设备。

步骤120：将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息。

在时间同步环节，先将原始点云数据与原始位姿数据进行时间对齐，实现激光雷达与组合导航设备传输延迟的时间差补偿，本实施例设需要补偿的时间差为Δt₁。

然后基于需要补偿的时间差Δt₁、激光雷达初始时刻与组合导航设备初始时刻的时间差Δt₂、每个扫描帧的时间戳t_h、每个扫描帧中每个点到该点所在的扫描帧开始时刻的时间戳t_of，最终计算出每个扫描帧中每个点相对于组合导航设备初始时刻的时间戳t_p，计算公式如下：

t_p＝Δt₁+Δt₂+t_h+t_of (1)

不难看出，本实施例中上述时间同步的过程，主要计算更新的是激光雷达的时间戳，目的是将激光雷达的时间戳尽量与组合导航设备的时间戳同步，以保证后续数据处理过程有效进行。

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

在示例性实施例中，根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果的过程，具体包括：

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

本实施例具体通过对组合导航设备的位姿点做积分变换，通过IMU积分变换，计算每个位姿点当前时间t_k时刻到初始时刻t₀的位姿变换矩阵T_t₀_t_k。雷达点在组合导航设备相应时刻坐标系下对应的虚拟点，可以通过雷达点的坐标乘以预设的待求外参矩阵获得。

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

假设时间同步后激光雷达采集原始点云数据中某雷达点p_i的时间戳为t_p，在组合导航设备中与时间戳t_p最近的两个时间戳为t_k和t_k+1且满足t_k≤t_p<t_k+1，相应的可以获知t_k和t_k+1时刻对应的位姿点，即相邻位姿点，同时，能够获知t_k时刻对应位姿点当前时刻到初始时刻的位姿变换矩阵T_t₀_t_k，以及t_k+1时刻对应位姿点当前时刻到初始时刻的位姿变换矩阵T_t₀_t_k+1。

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

以上述第二步假设内容为例，该步中虚拟点到组合导航设备初始时刻点的位姿变换矩阵，即虚拟位姿变换矩阵T_t₀_t_p的计算公式如下：

可以理解的是，以上述假设内容为例，本实施例提到的虚拟点，指的是组合导航坐标系下，t_p时刻对应的点，由于该点在组合导航坐标系下实际并不存在，因此将该点称为虚拟点。

步骤140：根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值。

为了实现组合导航设备与激光雷达的外参标定，本实施例预先构建了一个待求外参矩阵，该待求外参矩阵中包含旋转角度和平移量。

该待求外参矩阵具体可以包含三个旋转角度和三个平移量，比如旋转角度可以是航向角α、横滚角β和偏航角γ，平移量可以是X坐标轴上的平移量b₁、Y坐标轴上的平移量b₂以及Z坐标轴上的平移量b₃，这样，待求外参矩阵T可以表示为：

其中，R表示旋转矩阵，t表示平移矩阵，a₁至a₉表示旋转矩阵中的元素，b₁至b₃表示平移矩阵中的元素。

因此，该待求外参矩阵T包含六个未知数，通过求解上述六个未知数可以得到该待求外参矩阵的求解结果，即可获得外参标定解。

这样，基于上述位姿插值结果、预先构建的待求外参矩阵以及雷达点坐标，可以构建变换点坐标表达式，该变换点坐标表达式用于描述变换点坐标是通过虚拟点到组合导航设备初始时刻点的位姿变换矩阵、待求外参矩阵以及雷达点坐标的乘积得到的。

可以理解的是，由于每个雷达点到组合导航设备初始时刻坐标系的位姿变换矩阵可以由待求解的外参矩阵左乘对应的虚拟位姿变换矩阵得到，因此，在计算每个雷达点在组合导航设备初始时刻坐标系下对应的变换点的坐标时，可以由雷达点坐标左乘其到组合导航设备初始时刻坐标系的位姿变换矩阵得到。

具体地，以上述雷达点p_i在组合导航设备初始时刻坐标系下的变换点q_i的坐标表达式为例进行说明，即：

其中，

表示激光雷达点p_i在组合导航设备初始时刻坐标系下的位姿变换矩阵；T·p_i表示将激光雷达点p_i转换到当前时刻组合导航设备坐标系下的虚拟点坐标；T_t₀_t_p·T·p_i表示将激光雷达点p_i转换到组合导航设备初始时刻坐标系下的变换点坐标。

参见附图2，根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值的过程，具体包括：

首先，从原始位姿数据中选取多个位姿点，并从原始点云数据中选取与每个位姿点对应的雷达点。

本实施例在完成时间同步和位姿插值后，一方面，从组合导航设备采集的原始位姿数据中提取空间位置距离分别大于d₁、d₂、d₃、d₄、d₅的五个位姿点q₁、q₂、q₃、q₄、q₅，基于已知的初始位姿点q₀，可以得到六个已知的位姿点，即图2所示的步骤210和220；

另一方面，在激光雷达采集的原始点云数据中提取与五个位姿点q₁、q₂、q₃、q₄、q₅对应的五个雷达点p₁、p₂、p₃、p₄、p₅，基于已知的初始雷达点p₀，可以得到六个已知的雷达点，即图2所示的步骤230和240。

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

参见附图2中步骤250，基于六个已知雷达点{p₀，p₁，p₂，p₃，p₄，p₅}、六个已知位姿点{q₀，q₁，q₂，q₃，q₄，q₅}以及上述变换点坐标表达式，可以构建六个以待求外参矩阵为未知量的方程，联立六个方程可以求解待求外参矩阵内的六个未知数，从而得到待求外参矩阵的解，将该解将作为外参初始T₀。

步骤150：基于外参初值进行以及预先构建的优化目标函数，求解得到最优外参标定解。

具体地，本实施例通过基于外参初值，以原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，进而求解得到最优外参标定解。

为了获得最优的外参标定解，本实施例通过非线性优化的方式以外参初值为初始值逐步寻优，最终获得最优外参标定解。

在示例性实施例中，构建优化目标函数并求解最优外参标定解的过程，可以通过如下两种方式实现：

第一种方式：可以获取原始点云数据中当前帧内各个雷达点在下一帧内的最近距离点，构建多个最近距离点对，进而求取每个最近距离点对的距离，最后求和即可得到最近距离点的距离之和。

具体地，该种求解最优外参标定解的方式，具体包括：

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

然后，根据赋值后的待求外参矩阵和变换点坐标表达式，获取原始点云数据中当前扫描帧内每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第一变换点；

确定当前扫描帧内每个雷达点在下一扫描帧内距离最近的雷达点，并获取距离最近的雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第二变换点；

接着，将第一变换点和第二变换点作为最近距离点对，并将原始点云数据中所有最近距离点对组合为最近距离点对集合；

之后，计算最近距离点对集合中所有最近距离点对的距离之和，得到最近距离点的距离之和；

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

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

该种方式中，需要计算激光雷达采集的原始点云数据中每个扫描帧内每个雷达点与其对应的下一扫描帧内距离最近的雷达点，并利用预先赋值的待求外参矩阵以及变换点坐标表达式，分别获取每个雷达点与其最近距离点在组合导航设备初始时刻坐标系下的变换点，将当前雷达点对应的变换点与其最近距离点对应的变换点记为最近距离点对，将原始点云数据中所有的最近距离点对组合，构建最近距离点对集合。

后续计算最近距离点对集合中所有最近距离点对的距离之和，可以得到原始点云数据中最近距离点的距离之和，具体可以表示为：

其中，x_i,y_i,z_i表示雷达点转到组合导航设备初始时刻t₀时刻坐标系下的点集合∑P中的第i个点的坐标，x′_i,y′_i,z′_i为点集合∑P中距离该点最近的点坐标。

以最近距离点的距离之和D_error最小为优化目标，可以构建如下优化目标函数，即：

其中，n表示点集合中点的数量。

利用非线性优化算法进行不断迭代与优化，直到某次迭代后的D_error小于设定的距离阈值D_th或在当前迭代次数大于最大迭代次数(即预设次数阈值)时，停止迭代，此时的T就是最优的外参标定解。

第二种方式：可以求取原始点云数据中各雷达点对应的变换点，构建变换点集合，然后求取变换点集合中各点距离最近的两个点，并求取集合中各点到其距离最近的两个点(可能包含当前点本身)的距离，最后将各个点到距离最近的两个点的距离求和，即可得到最近距离点的距离之和。

该种方式在实际应用过程中，具体流程如下：

首先，利用外参初值T₀，对待求外参矩阵进行赋值；

然后，根据赋值后的待求外参矩阵、时间同步后的时间戳以及变换点坐标表达式，将激光雷达采集的原始点云数据中每帧内每个雷达点变换到t₀初始时刻的组合导航设备坐标系下，即获取原始点云数据中每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，得到变换点集合。

接着，分别在变换点集合中获取每个变换点对应的至少两个距离最近的邻近变换点，并计算每个变换点与其对应的至少两个邻近变换点的距离；本实施例中每个变换点对应的两个邻近变换点中，其中一个可能为该点本身，另一个为实际的最近距离点，在距离的求取过程中，如果其中一个邻近变换点为该点本身，则两点间距离为零，并不影响最近距离的求解。

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

最后，以最近距离点的距离之和最小为优化目标，构建优化目标函数，通过连续赋值迭代寻优；直至最近距离点的距离之和小于预设距离阈值或赋值迭代次数达到预设次数阈值，将当前赋值后的待求外参矩阵作为最优外参标定解。

上述两种方式中，可以利用现有的邻近搜索算法来求取最近距离点，比如K-D树(K-Dimensional Tree的简称)。

为了进一步提高数据处理效率和准确性，在将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息之前，还可以包括：

对原始点云数据进行滤波处理。

本实施例中可以采用体素滤波和高斯统计滤波，具体地，在进行滤波处理时，可以先进行体素滤波处理，对点云数据进行降采样，然后滤去距离值大于预设距离阈值以外的点，再利用Statistical Outlier Removal算法(即统计学异常点移除算法)移除部分离群点，比如，可以对每个点最近的50个相邻点距离进行考察，如果一个点的距离超出平均距离一个标准差以上，则该点被标记为离群点并移除。对原始点云数据滤波后可大大减少后续最近距离点对的计算量，提高计算效率。

下面对本发明提供的组合导航设备与激光雷达的外参标定装置进行描述，下文描述的组合导航设备与激光雷达的外参标定装置与上文描述的组合导航设备与激光雷达的外参标定方法可相互对应参照。

图3示出了本发明实施例提供的组合导航设备与激光雷达的外参标定装置，该装置包括：

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

第一处理模块320，用于将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；

第二处理模块330，用于根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；

第三处理模块340，用于根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；

第四处理模块350，用于基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。

在示例性实施例中，第二处理模块330，具体用于：

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

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

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

在示例性实施例中，第三处理模块340，具体用于：

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

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

需要说明的是，变换点坐标表达式用于描述变换点坐标是通过虚拟点到组合导航设备初始时刻点的位姿变换矩阵、待求外参矩阵以及雷达点坐标的乘积得到的。

需要说明的是，上述提到的待求外参矩阵可以包括旋转角度和平移量。

在示例性实施例中，第四处理模块350具体用于基于外参初值，以原始点云数据中最近距离点的距离之和最小为优化目标构建优化目标函数，求解得到最优外参标定解。

进一步地，上述第四处理模块350具体用于：

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

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

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

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

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

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

进一步地，上述第四处理模块350具体用于：

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

根据赋值后的待求外参矩阵和变换点坐标表达式，获取原始点云数据中当前扫描帧内每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第一变换点；

确定当前扫描帧内每个雷达点在下一扫描帧内距离最近的雷达点，并获取距离最近的雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第二变换点；

将第一变换点和第二变换点作为最近距离点对，并将原始点云数据中所有最近距离点对组合为最近距离点对集合；

计算最近距离点对集合中所有最近距离点对的距离之和，得到最近距离点的距离之和；

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

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

在示例性实施例中，上述组合导航设备与激光雷达的外参标定装置还可以包括滤波模块，该滤波模块具体用于对原始点云数据进行滤波处理。

由此可见，本发明实施例提供的组合导航设备与激光雷达的外参标定装置，通过第三处理模块计算出了一个较为理想的外参初值，能够有效提高标定效率与标定精度；同时，该装置通过第二处理模块直接用插值法估计激光雷达的位姿，提高了标定效率，降低了计算复杂量；此外，通过滤波模块对激光雷达采集的点云数据进行滤波处理，有效提高了标定效率，降低了标定时间成本。

另外，本发明实施例还提供一种作业机械，该作业机械使用上述任一种的组合导航设备与激光雷达的外参标定方法，可以实现作业过程的高精度定位和感知功能。

可以理解的是，本实施例提到的作业机械可以是挖掘机，当然也可以是其他需要安装高精度定位和感知设备的作业机械。

此外，本发明实施例还提供一种组合导航设备与激光雷达的外参标定系统，该系统包括：

待标定激光雷达，用于采集目标物的原始点云数据；

待标定组合导航设备，用于采集目标物的原始位姿数据；

控制器，用于分别获取原始位姿数据以及原始点云数据；将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；并基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。

下面以挖掘机为例，对组合导航设备与激光雷达的外参标定系统的实施方案进行详细说明。

本实施例搭建的标定系统的硬件结构架构如图4所示，硬件包括挖掘机电源、控制器(型号可以为Nuvo-7160GC)、RTK/IMU组合导航设备(型号可以为CGI-610)、3D激光雷达(型号可以为VLP-32C)、稳压器、降压器、鼠标键盘、显示屏等。该标定系统的软件部分包括Ubuntu18.04以及ROS melodic的操作系统、底层驱动程序软件以及c++编程语言软件等。

标定系统硬件在安装时，将RTK组合导航设备CGI-610的机身和控制器Nuvo-7160GC安装于挖掘机驾驶室内座位的后方位置，将组合导航设备的RTK天线装于挖掘机上车体后方位置，两个RTK天线的距离可以设置的尽可能大些，以获取到信号轻度更优的数据；将激光雷达VLP-32C安装于驾驶室顶部或通过支架安装于驾驶室顶部上方位置；将鼠标、键盘和显示屏通过安装支架装于驾驶室内座位的前方。

如图4所示，挖掘机电源通过稳压器输出24V稳压，给降压器、控制器Nuvo-7160GC以及鼠标和键盘供电；降压器输出12V电压给RTK组合导航设备CGI-610、激光雷达VLP-32C以及显示屏供电。

硬件安装完毕后，启动挖掘机匀速行走，并将上述搭建的系统开启。操作挖掘机进行一些非平面运动以获取姿态上的变化数据，具体地，可操控挖掘机铲斗撑地进行俯仰、侧翻动作，使得俯仰角、翻滚角以及航向角有很大的角度变化。

然后将RTK组合导航设备CGI-610采集到的原始位姿数据和激光雷达VLP-32C采集的原始点云数据输入到控制器Nuvo-7160GC，利用标定技术实现RTK组合导航设备CGI-610和激光雷达VLP-32C的离线外参标定，外参标定过程的算法流程如图5所示。

参见附图5，首先，读取RTK组合导航设备CGI-610采集的原始数据，即CGI-610数据，并对其进行GPCHC数据协议解析，从而获取CGI-610的位置(即WGS84坐标)与姿态数据，包括时间戳、经纬高度(即经度、纬度和高度)以及三个姿态角(即俯仰角、翻滚角和航向角)，并将解析出来的WGS84坐标进行坐标转换，以获取t₀初始时刻CGI-610坐标系下的位姿数据。

然后，读取激光雷达VLP-32C采集的原始数据，即VLP-32C数据，并进行数据解析以获取pointcloud2格式的点云数据。之后对获得的点云数据进行滤波处理。滤波处理时，先进行体素滤波处理，对点云数据进行降采样；然后滤去距离值大于50m以外的点；再利用Statistical Outlier Removal算法移除一部分离群点，本实施例对每个点最近的50个相邻点距离进行考察，如果一个点的距离超出平均距离一个标准差以上，则该点被标记为离群点并移除。滤波后可大大减少后续最近距离点对的计算量，提高计算效率。

接着，先通过时间对齐消除两传感器之间的延迟时间差，再对RTK组合导航设备CGI-610和激光雷达VLP-32C进行时间同步，由于激光雷达VLP-32C内部只有一个代表TOH(Top Of Hour)时间的计数器，TOH时间由两部分组成，一部分是从TOH时间开始的分钟数+秒数，另一部分是微秒数，且激光雷达不能表达整小时以上的时间。

而RTK组合导航设备CGI-610中的NMEA GPGGA/GPRMC语句提供了UTC时间的分钟部分和秒部分，因此可通过读取这部分信息来进行时间戳同步。对于激光雷达使用RTK/IMU组合导航pps时间对齐功能获取精确的时间戳，从硬件层面来说，需要通过串口为激光雷达VLP-32C接入RTK组合导航设备CGI-610的pps信号，并考虑激光雷达VLP-32C底层驱动和激光雷达VLP-32C点云点的时间戳计算机制来计算单个雷达点的时间戳。

之后，进行位姿插值操作，记RTK组合导航设备CGI-610初始时刻t₀下的坐标系为世界坐标系，计算RTK组合导航设备CGI-610每个位姿点当前时刻t_k到初始时刻t₀的位姿变换矩阵T_t₀_t_k；

假设激光雷达每个点p_i相对于RTK组合导航设备CGI-610初始时刻的时间戳为t_p(即时间同步后的时间戳)；

RTK组合导航设备CGI-610中与t_p最近的两个时间戳为t_k、t_k+1，且满足t_k≤t_p<t_k+1，则RTK组合导航设备CGI-610中t_k时刻到初始时刻t₀的变换为T_t₀_t_k，t_k+1时刻到初始时刻t₀的变换为T_t₀_t_k+1；

基于上述信息，可以得到虚拟点到组合导航设备初始时刻点的位姿变换矩阵T_t₀_t_p，以实现位姿插值。

基于t_p、t_k、t_k+1、T_t₀_t_k、T_t₀_t_k+1以及位姿插值结果T_t₀_t_p，可以估计激光雷达每个雷达点p_i在RTK/IMU组合导航设备初始时刻坐标系下的位姿点q_i。即为每个扫描帧中每个雷达点通过计算的时间戳去匹配对应的更准确的位置和姿态值。

再然后，计算外参初值，完成时间同步与位姿插值后，计算出一个相对理想的外参初值。该外参初值能够有效提高后续外参优化的效率，并获得精度更高的外参解。结合附图2，本实施例中5个距离d₁、d₂、d₃、d₄、d₅可以分别设置为5米、10米、15米、20米、25米。位姿点q_i的计算公式如下式所示：

其中，i＝0,1,…,5，T₀为外参初值，

表示激光雷达点p_i在组合导航设备初始时刻坐标系下的位姿变换矩阵；T·p_i表示将激光雷达点p_i转换到当前时刻组合导航设备坐标系下的虚拟点坐标；T_t₀_t_p·T·p_i表示将激光雷达点p_i转换到组合导航设备初始时刻坐标系下的位姿点坐标。

因此，用6个等式求解6个未知数，从而可以求出外参初值T₀(α₀,β₀,γ₀,b₀₁,b₀₂,b₀₃)。

最后，以最近距离点的距离之和最小为优化目标，构建优化目标函数模型，并通过迭代优化求解，得到最优外参标定解T。

本实施例求得的最优外参标定解T可以使得激光雷达的点云数据变换到RTK/IMU组合导航设备初始时刻t₀时刻的坐标系(即世界坐标系)后自我重合度最高。

图6示例了一种电子设备的实体结构示意图，如图6所示，该电子设备可以包括：处理器(processor)610、通信接口(Communications Interface)620、存储器(memory)630和通信总线640，其中，处理器610，通信接口620，存储器630通过通信总线640完成相互间的通信。处理器610可以调用存储器630中的逻辑指令，以执行组合导航设备与激光雷达的外参标定方法，该方法包括：分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。

此外，上述的存储器630中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述各方法所提供的组合导航设备与激光雷达的外参标定方法，该方法包括：针对同一目标物，分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。

又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各提供的组合导航设备与激光雷达的外参标定方法，该方法包括：针对同一目标物，分别获取组合导航设备采集的原始位姿数据以及激光雷达采集的原始点云数据；将原始点云数据与原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据时间同步后的时间戳信息以及原始点云数据，对原始位姿数据进行位姿插值，得到位姿插值结果；根据原始位姿数据、原始点云数据以及位姿插值结果，计算得到外参初值；基于外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

根据赋值后的待求外参矩阵和所述变换点坐标表达式，获取所述原始点云数据中当前扫描帧内每个雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第一变换点；

确定当前扫描帧内每个雷达点在下一扫描帧内距离最近的雷达点，并获取所述距离最近的雷达点在组合导航设备初始时刻坐标系下对应的变换点，作为第二变换点；

将所述第一变换点和所述第二变换点作为最近距离点对，并将所述原始点云数据中所有最近距离点对组合为最近距离点对集合；

计算所述最近距离点对集合中所有最近距离点对的距离之和，得到最近距离点的距离之和；

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

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

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

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

第一处理模块，用于将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；

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

第三处理模块，用于根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；

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

9.一种作业机械，其特征在于，该作业机械使用如权利要求1至7任一项所述的一种组合导航设备与激光雷达的外参标定方法。

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

待标定激光雷达，用于采集目标物的原始点云数据；

待标定组合导航设备，用于采集目标物的原始位姿数据；

控制器，用于分别获取所述原始位姿数据以及所述原始点云数据；将所述原始点云数据与所述原始位姿数据进行时间同步，得到时间同步后的时间戳信息；根据所述时间同步后的时间戳信息以及原始点云数据，对所述原始位姿数据进行位姿插值，得到位姿插值结果；根据所述原始位姿数据、所述原始点云数据以及所述位姿插值结果，计算得到外参初值；并基于所述外参初值以及预先构建的优化目标函数，求解得到最优外参标定解。

Images