CN110780285B - 激光雷达与组合惯导的位姿标定方法、系统及介质 - Google Patents

Abstract

本发明实施例公开了一种激光雷达与组合惯导的位姿标定方法、系统及介质。其中，该方法包括：获取激光雷达和组合惯导在两个采集位置对多个标定板采集的两个点云数据集和两个惯导数据集，所述激光雷达和所述组合惯导刚性连接；根据所述两个点云数据集，确定各个标定板的参考点在所述两个点云数据集中对应的点云坐标；根据所述两个惯导数据集和各个标定板的参考点在所述两个点云数据集中对应的点云坐标，确定所述激光雷达到所述组合惯导的位姿转换矩阵。本发明的技术方案无需人工测量，可快速准确的标定出激光雷达和组合惯导之间的相对位姿。

Description

激光雷达与组合惯导的位姿标定方法、系统及介质

技术领域

本发明实施例涉及计算机技术领域，尤其涉及一种激光雷达与组合惯导的位姿标定方法、系统及介质。

背景技术

随着传感器技术的发展，在室外的3D建图与定位领域，多线激光雷达与组合惯导联合使用已成为多传感器融合中不可缺少的方案。而两者之间的相对位姿的标定，对传感器的使用至关重要，直接关系到传感器测量结果的准确性。

目前，在进行激光雷达与组合惯导的位姿标定时，采用的方法是人工测量呈刚性连接的激光雷达与组合惯导之间的角度值和位移值，进而根据测量结果实现激光雷达与组合惯导的位姿标定。但是通过人工测量标定激光雷达与组合惯导之间的相对位姿，成本高、误差大，严重影响位姿标定的准确性。

发明内容

本发明实施例提供一种激光雷达与组合惯导的位姿标定方法、系统及介质，无需人工测量，可快速准确的标定出激光雷达和组合惯导之间的相对位姿。

第一方面，本发明实施例提供了一种激光雷达与组合惯导的位姿标定方法，包括：

获取激光雷达和组合惯导在两个采集位置对多个标定板采集的两个点云数据集和两个惯导数据集，所述激光雷达和所述组合惯导刚性连接；

根据所述两个点云数据集，确定各个标定板的参考点在所述两个点云数据集中对应的点云坐标；

根据所述两个惯导数据集和各个标定板的参考点在所述两个点云数据集中对应的点云坐标，确定所述激光雷达到所述组合惯导的位姿转换矩阵。

第二方面，本发明实施例还提供了一种激光雷达与组合惯导的位姿标定装置，该装置包括：

数据集获取模块，用于获取激光雷达和组合惯导在两个采集位置对多个标定板采集的两个点云数据集和两个惯导数据集，所述激光雷达和所述组合惯导刚性连接；

点云坐标确定模块，用于根据所述两个点云数据集，确定各个标定板的参考点在所述两个点云数据集中对应的点云坐标；

位姿转换矩阵确定模块，用于根据所述两个惯导数据集和各个标定板的参考点在所述两个点云数据集中对应的点云坐标，确定所述激光雷达到所述组合惯导的位姿转换矩阵。

第三方面，本发明实施例还提供了一种测绘系统，该测绘系统包括激光雷达、组合惯导和控制装置；所述控制装置分别与所述激光雷达和所述组合惯导连接，所述控制装置包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序；

当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现第一方面所述的激光雷达与组合惯导的位姿标定方法。

第四方面，本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现第一方面所述的激光雷达与组合惯导的位姿标定方法。

本发明实施例提供的激光雷达与组合惯导的位姿标定方法、系统及介质，获取呈刚性连接的激光雷达和组合惯导在两个不同位置处针对多个标定板采集的两个点云数据集和两个惯导数据集，通过获取的两个点云数据集，确定出每个标定板的参考点在各点云数据集中对应的点云坐标，进而结合两个惯导数据集确定激光雷达到组合惯导的位姿转换矩阵，从而能够准确标定出激光雷达和组合惯导之间的相对位姿。本发明实施例的方案标定激光雷达和组合惯导的相对位姿的整个过程无需人工手动测量，极大的提高了标定结果的准确性，降低了标定成本。为激光雷达和组合惯导之间相对位姿的标定提供了一种新思路。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1A是本发明实施例一中的一种激光雷达与组合惯导的位姿标定方法的流程图；

图1B是本发明实施例中的激光雷达与组合惯导的数据采集过程示意图；

图2A是本发明实施例二中的一种激光雷达与组合惯导的位姿标定方法的流程图；

图2B是本发明实施例中的一种标定板结构及其二维点云数据的可视化图像效果图；

图3是本发明实施例三中的一种激光雷达与组合惯导的位姿标定方法的流程图；

图4是本发明实施例四中的一种激光雷达与组合惯导的位姿标定装置的结构示意图；

图5A为本发明实施例五提供的一种测绘系统的结构示意图；

图5B是本发明实施例五中的一种测绘系统的控制装置的结构示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

实施例一

图1A是本发明实施例一提供的一种激光雷达与组合惯导的位姿标定方法的流程图，图1B是本发明实施例中的激光雷达与组合惯导的数据采集过程示意图；本实施例适用于如何准确标定出激光雷达与组合惯导之间的相对位姿的情况。该方法可以由本发明实施例的测绘系统中的控制装置执行，该控制装置可采用软件和/或硬件的方式实现。如图1A-1B所示，该方法具体包括如下步骤：

S101，获取激光雷达和组合惯导在两个采集位置对多个标定板采集的两个点云数据集和两个惯导数据集。

其中，激光雷达是以发射激光束探测物体位置的雷达系统，可选的，本实施例中的激光雷达可以是多线激光雷达。组合惯导可以是指由至少两个具有定位功能的单元或系统等组合而成，例如，可以由惯性测量单元(Inertial measurement unit，IMU)、惯性导航系统(Inertial Navigation System，INS)中的至少一个，以及全球定位系统(GlobalPositioning System，GPS)和北斗卫星导航系统(BeiDou Navigation Satellite System，BDS)中的至少一个组合而成。需要说明的是，本发明实施例由多个具有定位功能的单元或系统组合而成的组合惯导的定位精度相比于单独的定位单元或系统更高。可选的，本实施例中，激光雷达和组合惯导呈刚性连接，且可固定在可移动载体上，如可以固定在无人驾驶车辆上。点云数据集是激光雷达采集的一组由包含三维坐标的点所组成的点集，可以用来表征一个物体的外表面形状。其中，每个点的三维空间几何位置信息可用(x，y，z)表示，除此之外点云数据还可以表示一个点的光强值。惯导数据集是组合惯导采集的表征刚性连接的激光雷达和组合惯导构成的刚体结构的位置和姿态的数据集，包括：x、y、z轴方向的线加速度和角加速度。

可选的，本实施例在进行激光雷达与组合惯导标定的过程中，需要使用到多个标定板，其中，标定板的个数越多，对应的标定结果越准确，可选的，为了保证标定结果的准确性，本实施例可以至少选择4个标定板完成本次标定过程。多个标定板的形状和大小可以相同也可以不同。多个标定板的具体放置位置本实施例不进行限定，只需保证在本实施例选择的两个采集位置处，激光雷达采集的点云数据集中包括所有标定板的点云数据即可。可选的，本实施例中的标定板可以是由一个标定板构成，也可以是由多个不同的标定板按照一定的方式组合而成。例如，当标定板为两个子标定板组合而成时，该标定板可以由反射率不同、中心重合的一组主标定板和辅标定板构成，每组主标定板和辅标定板平行设置，且所述每组主标定板在辅标定板上的投影轮廓包含在所述辅标定板的轮廓内。

可选的，本步骤的具体可实施方式可以是将激光雷达和组合惯导构成的刚体结构11设置在第一采集位置处，获取激光雷达在该第一采集位置针对多个标定板采集的第一点云数据集，以及组合惯导在该第一采集位置采集的第一惯导数据集；然后保持多个标定板不动，将激光雷达和组合惯导构成的刚体结构移动至第二采集位置处，获取激光雷达在该第二采集位置针对该多个标定板采集的第二点云数据集，以及组合惯导在该第二采集位置采集的第二惯导数据集。需要说明的是，本步骤中，呈刚性连接的激光雷达和组合惯导在第一采集位置和第二采集位置针对多个标定板采集数据时，多个标定板的位置固定不变，只是呈刚性连接的激光雷达和组合惯导位置发生变化。示例性的，如图1B所示，本步骤可以是将承载激光雷达和组合惯导构成的刚体结构11的可移动载体10移动到第一采集位置处，获取激光雷达针对标定板1至标定板4采集的第一点云数据集，以及组合惯导在该第一采集位置处采集的第一惯导数据集；然后四个标定板的位置保持不变，将可移动载体10移动到第二采集位置处，获取激光雷达针对标定板1至标定板4采集的第二点云数据集，以及组合惯导在该第二采集位置处采集的第二惯导数据集。

需要说明的是，本发明实施例中激光雷达和组合惯导是呈刚性连接的，在激光雷达采集某一场景的点云数据集的同时，组合惯导系统也会对相同场景进行定位获取该场景对应的惯导数据集，也就是说，第一点云数据集和第一惯导数据集是同步采集的；第二点云数据集和第二惯导数据集是同步采集的。

S102，根据两个点云数据集，确定各个标定板的参考点在两个点云数据集中对应的点云坐标。

其中，标定板的参考点可以是标定板上易于识别的特征点，各个标定板的参考点可以相同，也可以不同，对此本实施例不进行限定。可选的，对于标定板的参考点的选择，可以是工作人员根据实际需求设置的，还可以系统默认的，例如，系统可以默认将各标定板的中心点作为各标定板的参考点。需要说明的是，由于本步骤需要根据点云数据集，自动确定各个标定板的参考点对应的点云坐标，所以为了保证参考点坐标确定的准确性，在选择各个标定板的参考点时，优选便于求解坐标的参考点，如标定板的中心点，或标定板的边角点等易于识别的特征点。

可选的，由于本实施例中激光雷达在该第一采集位置采集的第一点云数据集和第二采集位置采集的第二点云数据集中都包括多个标定板的点云数据，但是由于采集位置的不同，标定板的参考点在第一点云数据集和第二点云数据集中对应的点云坐标也不同。本步骤需要分别根据每个点云数据集，计算出该点云数据集中各个标定板的参考点对应的点云坐标。

可选的，本步骤根据两个点云数据集，确定各个标定板的参考点在两个点云数据集中对应的点云坐标的方法有很多，对此本实施例不进行限定。一种可实施方式可以是：针对每个点云数据集，由该点云数据集生成三维的可视化图像，该三维可视化图像中包含有每个标定板对应的图像区域，进而对三维可视化图像中各个标定板对应的图像区域进行分析处理，确定各个标定板的参考点在该三维可视化图像中对应的三维空间几何位置坐标(x，y，z)作为各个标定板的参考点在点云数据集中对应的点云坐标。

由于针对三维可视化图像确定区域内某一点的坐标过程复杂，难度较大，所以本步骤的另一种可实施方式可以是：针对每个点云数据集，将点云数据集对应的三维平面转换为二维平面，并确定转换过程中点云数据集对应的空间转换矩阵；确定各个标定板的参考点在二维平面内对应的二维坐标；根据空间转换矩阵和二维坐标，确定各个标定板的参考点在点云数据集的三维平面中对应的点云坐标。具体的，该可实施方式可以针对每一个点云数据集，由该点云数据集生成三维的可视化图像，将该三维可视化图像进行旋转平移操作，以将其转换至直角坐标系的某一二维平面，例如，转换到直角坐标系的x-y二维平面，并将转换过程中的旋转角度和平移距离，生成该可视化图像从三维空间转换到二维空间的空间旋转矩阵。可选的，该空间旋转矩阵中可以包括：本次旋转平移过程沿x、y、z轴上的旋转角度和平移距离。针对二维空间的可视化图像可以根据标定板的特性以及参考点的特征，在二维空间的可视化图像中拟合出标定板的参考点的二维坐标。由于点云数据集中的点云坐标是三维坐标，所以还需要根据空间旋转矩阵将该二维坐标转换回三维空间，得到各个标定板的参考点在点云数据集的三维平面中对应的点云坐标。需要说明的是，该可实施方式的详细执行过程将在后续实施例进行详细介绍。

S103，根据两个惯导数据集和各个标定板的参考点在两个点云数据集中对应的点云坐标，确定激光雷达到组合惯导的位姿转换矩阵。

可选的，本实施例在根据两个惯导数据集和各个标定板的参考点在两个点云数据集中对应的点云坐标，确定激光雷达到组合惯导的位姿转换矩阵时，可以是根据两个惯导数据集和各个标定板的参考点在两个点云数据集中对应的点云坐标采用两种不同的坐标求解算法，计算同一参考点在同一采集位置处的点云坐标从雷达坐标系转换到世界坐标系的坐标表达式。两种不同的方法可以得到两种不同的坐标表达式，其中，一种求解算法生成的坐标表达式中包含未知矩阵，即激光雷达到组合惯导的位姿转换矩阵。由于两种不同的坐标表达式对应的其实是世界坐标系下的同一点，所以可以根据这两个坐标表达式建立方程式，求解该激光雷达到组合惯导的位姿转换矩阵。

可选的，本步骤计算同一参考点在同一采集位置处的点云坐标从雷达坐标系转换到世界坐标系的坐标表达式的方法本实施例不进行限定。例如，可以是根据两个惯导数据集和待确定的所述激光雷达到所述组合惯导的位姿转换矩阵，采用第一求解算法，计算出某一参考点在第二采集位置处的点云坐标从雷达坐标系转换到世界坐标系的第一世界坐标表达式；根据各个标定板的参考点在两个点云数据集中对应的点云坐标，采用第二求解算法，计算出该参考点在第二采集位置处的点云坐标从雷达坐标系转换到世界坐标系的第二世界坐标表达式。需要说明的是，具体的求解过程将在后续实施例进行详细介绍。

本发明实施例提供的激光雷达与组合惯导的位姿标定方法，获取呈刚性连接的激光雷达和组合惯导在两个不同位置处针对多个标定板采集的两个点云数据集和两个惯导数据集，通过获取的两个点云数据集，确定出每个标定板的参考点在各点云数据集中对应的点云坐标，进而结合两个惯导数据集确定激光雷达到组合惯导的位姿转换矩阵，从而能够准确标定出激光雷达和组合惯导之间的相对位姿。本发明实施例的方案标定激光雷达和组合惯导的相对位姿的整个过程无需人工手动测量，极大的提高了标定结果的准确性，降低了标定成本。为激光雷达和组合惯导之间相对位姿的标定提供了一种新思路。

实施例二

图2A是本发明实施例二中的一种激光雷达与组合惯导的位姿标定方法的流程图，图2B是本发明实施例中的一种标定板结构及其二维点云数据的可视化图像效果图；本实施例以上述实施例为基础，进行了进一步的优化，具体给出了如何根据所述两个点云数据集，确定各个标定板的参考点在两个点云数据集中对应的点云坐标的具体情况介绍。如图2A-2B所示，本实施例的方法具体包括如下步骤：

S201，获取激光雷达和组合惯导在两个采集位置对多个标定板采集的两个点云数据集和两个惯导数据集。

其中，所述激光雷达和所述组合惯导刚性连接。

S202，针对每个点云数据集，将点云数据集对应的三维平面转换为二维平面，并确定转换过程中点云数据集对应的空间转换矩阵。

可选的，由于激光雷达采集的点云数据是三维空间下的点云数据，且在三维空间下求解各个标定板的参考点的点云坐标难度较大，所以本实施例可以将点云数据集从三维空间转换到二维空间，通过二维空间推算参考点的点云坐标，这样设置的好处是极大的简化了求解标定板的参考点坐标的过程。

可选的，本步骤可以是针对S201获取的每一个点云数据集，都将其从三维平面转换到二维平面，具体的转换过程可以包括如下三个子步骤：

S2021,拟合点云数据集对应的三维平面方程。

可选的，本子步骤可以是从点云数据集中选择至少四个点的点云数据，并将其代入公式ax+by+cz+d＝0中，求解出上述公式中的参数变量a、b、c和d，代回公式ax+by+cz+d＝0，得到点云数据集对应的三维平面方程。可选的，为了提高拟合得到的三维平面方程的准确性，还可以是采用预设的平面拟合算法，例如，可以是ransac平面拟合算法，根据点云数据集中的点云数据，拟合出该点云数据集对应的三维平面方程。

S2022，根据三维平面方程，确定三维平面方程的法向量。

其中，三维平面方程的法向量可以是垂直于该三维平面方程对应平面的直线所对应的向量。

可选的，在S2021拟合出点云数据集对应的三维平面方程后，求解该平面方程的法向量的过程可以是：先在该三维平面方程所在平面内选择两个不共线的向量，由于一个平面的法向量与该平面内的任意向量的乘积为零，所以根据待求的法向量和这两个不共线的向量构建法向量为未知向量的方程组，求解该方程组得到待求的三维平面方程的法向量。

S2023，旋转三维平面方程对应的三维平面，以使三维平面的法向量与直角坐标系中的预选坐标轴重合。

其中，直角坐标系中预选坐标轴可以是直角坐标系的x、y、z轴中的任一坐标轴。通常情况下，我们选择z轴作为预选坐标轴。在明确了预选坐标轴后，旋转三维平面方程对应的三维平面，直到该三维平面的法向量与预选坐标轴重合。具体的，为了保证旋转过程的准确性，本子步骤可以是先计算三维平面的法向量与预选坐标轴之间的夹角和距离，然后按照求解出来的夹角和距离，将法向量沿x轴方向移动该方向对应的角度和距离、沿y轴方向移动该方向对应的角度和距离、沿z轴方向移动该方向对应的角度和距离，此时三维平面的法向量与直角坐标系中的预选坐标轴重合。例如，若预选坐标轴为z轴，旋转三维平面方程对应的三维平面，旋转后的得到的二维平面分布在直角坐标系中的x-y平面上。

可选的，由于后续需要将求解出来的参考点的二维坐标转换为三维空间下的点云坐标，所以在将点云数据集对应的三维平面转换为二维平面后，需要确定出本次从三维平面转换到二维平面的过程对应的空间转换矩阵。具体的确定过程可以是：计算三维平面方程的法向量与直角坐标系中的预选坐标轴之间在x、y、z三个方向上的夹角和距离，然后根据这三个方向的夹角和距离，进一步计算出点云数据集从三维平面转换到二维平面的过程中对应的空间转换矩阵。

S203，确定各个标定板的参考点在二维平面内对应的二维坐标。

可选的，本步骤确定各个标定板的参考点在二维平面内对应的二维坐标的过程可以是根据转换到二维平面的点云数据，拟合出该二维平面内各个标定板的区域或轮廓，进而根据拟合出的各个标定板的区域或轮廓，确定出各个标定板的参考点在该二维平面内对应的二维坐标。

可选的，为了保证标定板参考点坐标确定的准确性。本实施例的每一个标定板都可以是由反射率不同、中心重合的一组主标定板和辅标定板构成，每组主标定板和辅标定板平行设置，且每组主标定板在辅标定板上的投影轮廓包含在所述辅标定板的轮廓内。需要说明的是，本实施例中的主标定板和辅标定板的行状可以有很多，例如，可以是四边形、三角形、圆形、椭圆等易于拟合的图形形状，且主标定板和辅标定板的行状可以相同也可以不同。示例性的，图2B所示的标定板可以包括两个四边形的主标定板和辅标定板构成，其中，主标定板的反射率为低反射率，辅标定板的反射率为高反射率，主标定板与辅标定板平行设置、中心重合，此外，主标定板的尺寸小于辅标定板，且主标定板可以完整的投影在辅标定板的轮廓内。可选的，在另一实施例中，也可以是将一个标定板的中间进行镂空处理，并将镂空处理后的板面区域作为辅标定板，将镂空处理后的镂空区域作为主标定板。

可选的，当本实施例的标定板是由上述介绍的主标定板和辅标定板构成，且参考点为标定板的中心点(即主标定板和辅标定板共同的中心)，则本步骤确定各个标定板的参考点在二维平面内对应的二维坐标，包括如下两个子步骤：

S2031，根据二维平面内各个标定板的点云数据，确定各个标定板的主标定板在二维平面内的轮廓。

可选的，由于本实施例中主标定板和辅标定板的反射率不同。反射率越高，标定板将照射在其表面的光线反射出去的能力就越强，对应的激光雷达采集到的该标定板区域的点云数据的光强值就越低，即该标定板区域对应到二维图像上的点的亮度也就越低。示例性的，如图2B所示，图2B左边示出了某一个标定板的结构示意图，其中主标定板的反射率较高，辅标定板的反射率较低；图2B右边示出了该标定板对应的点云数据的二维图像的效果示意图，本子步骤可以根据二维平面对应的二维点云图像，结合主标定板的形状，采用相应的形状拟合算法，拟合出主标定板的轮廓，例如，如图2B所示，已知主标定板的行状为四边形，可以根据二维点云图像中的光强值的跳变，将主标定板和辅标定板的交接处的直线拟合出来，进而拟合出四条相交直线构成的四边形的轮廓(见图2B中二维图像内的白色虚线框)。

需要说明的是，由于一个点云数据集对应的二维平面中包括所有标定板对应的点云数据，所以，先将二维平面中各个标定板对应的区域进行划分，其中，划分后的每个区域都包含有该标定板的主标定板和辅标定板。然后分别针对每个划分区域的点云数据，按照上述介绍的方法，拟合出该区域对应的标定板中的主标定板在二维平面内的轮廓。

S2032，根据各个标定板的主标定板在二维平面内的轮廓，拟合各个标定板的中心点在二维平面内对应的二维坐标。

可选的，当S2031拟合出每个标定板中的主标定板在二维平面内的轮廓后，接下来可以根据每个标定板中的主标定板在二维平面内的轮廓的点云坐标，拟合出每个轮廓的中心点在二维平面内对应的二维坐标，作为该轮廓对应的主标定板所属标定板的中心点在二维平面内对应的二维坐标。例如，可以是计算主标定板的轮廓点云坐标均值，作为该主标定板所属标定板的中心点在二维平面内对应的二维坐标；还可以采用其它方法拟合轮廓的中心点的二维坐标，对此本实施例不进行限定。

需要说明的是，若各标定板的参考点为标定板的边角点，则本实施例可以是根据二维平面的轮廓，拟合出边角点对应的两条边的直线方程，进而求解两直线方程的交点坐标作边角点的点云坐标。

S204，根据空间转换矩阵和二维坐标，确定各个标定板的参考点在点云数据集的三维平面中对应的点云坐标。

可选的，为了简化参考点坐标的计算，S203确定出的是各个标定板的参考点在二维平面内对应的二维坐标，此时还需要将S203确定出的各个参考点的二维坐标转换到三维空间下。此时可以是根据S202确定的三维平面转换到二维平面的空间转换矩阵，逆向推算各个参考点的二维坐标转换到三维空间下的三维点云坐标。具体的，可以是将各个参考点的二维坐标乘以空间转换矩阵的逆矩阵，得到各个标定板的参考点在点云数据集的三维平面中对应的点云坐标。

需要说明的是，本实施例中，针对获取的每一个点云数据集，依次执行上述S202-S204的操作，确定出每一个点云数据中各个标定板的参考点对应的点云坐标。

S205，根据两个惯导数据集和各个标定板的参考点在两个点云数据集中对应的点云坐标，确定激光雷达到组合惯导的位姿转换矩阵。

本发明实施例提供的激光雷达与组合惯导的位姿标定方法，获取呈刚性连接的激光雷达和组合惯导在两个不同的位置处针对多个标定板采集的两个点云数据集和两个惯导数据集，针对每一个点云数据集，先将其转换到二维平面确定各个标定板的参考点的二维坐标，然后再将二维坐标按照点云数据集进行平面转换过程对应的空间转换矩阵逆推为三维平面的点云坐标，进而结合两个惯导数据集确定激光雷达到组合惯导的位姿转换矩阵，从而能够准确标定出激光雷达和组合惯导之间的相对位姿。本发明实施例的方案通过将参考点三维点云坐标的求解过程转换到求解二维坐标后逆推三维点云坐标，极大的降低了参考点三维点云坐标确定过程的复杂度，且提高了参考点三维点云坐标确定的准确性。为后续精准标定激光雷达和组合惯导之间相对位姿提供了保证，也为激光雷达和组合惯导之间相对位姿的标定提供了一种新思路。

实施例三

图3为本发明实施例三中的一种激光雷达与组合惯导的位姿标定方法的流程图，本实施例以上述实施例为基础，进行了进一步的优化，具体给出了如何根据两个惯导数据集和各个标定板的参考点在两个点云数据集中对应的点云坐标，确定激光雷达到组合惯导的位姿转换矩阵的具体情况介绍。如图3所示，本实施例的方法具体包括如下步骤：

S301，获取激光雷达和组合惯导在两个采集位置对多个标定板采集的两个点云数据集和两个惯导数据集。

其中，激光雷达和组合惯导刚性连接。

S302，根据两个点云数据集，确定各个标定板的参考点在两个点云数据集中对应的点云坐标。

S303，根据两个惯导数据集和待确定的激光雷达到组合惯导的位姿转换矩阵，生成目标标定板的参考点在第二采集位置对应的第一世界坐标表达式。

其中，目标标定板为多个标定板中的任一标定板；第二采集位置为S301中的两个采集位置中第二次采集点云数据集和惯导数据集的位置。

可选的，本步骤可以根据两个惯导数据集和待确定的激光雷达到组合惯导的位姿转换矩阵，采用第一种世界坐标求解算法，生成目标标定板的参考点在第二采集位置对应的第一世界坐标表达式。具体的执行过程可以包括如下两个子步骤：

S3031，根据两个惯导数据集，确定组合惯导从第一采集位置到第二采集位置的惯导转换矩阵。

可选的，本子步骤计算组合惯导从第一采集位置到第二采集位置的惯导转换矩阵时，可以是先对两个惯导数据集中的x、y、z轴方向的线加速度和角加速度进行二重积分运算，得到各惯导数据集对应的采集位置处的x、y、z轴方向的位移值和角度值，然后分别计算x、y、z轴方向上第二采集位置处相对于第一采集位置处的角度变换值和位移变换值，进而根据x、y、z轴方向上角度变换值和位移变换值，生成组合惯导从第一采集位置到第二采集位置的惯导转换矩阵C。

S3032，根据惯导转换矩阵、待确定的所述激光雷达到所述组合惯导的位姿转换矩阵和目标标定板的参考点在第二采集位置采集的点云数据集中对应的点云坐标，生成目标标定板的参考点在第二采集位置从雷达坐标系转换到世界坐标系的第一世界坐标表达式。

可选的，S3031中已确定出惯导转换矩阵C，若目标标定板的参考点在第二采集位置采集的点云数据集中对应的点云坐标为S，则根据位姿变换可以得到该目标标定板的参考点在第二采集位置的点云坐标S从雷达坐标系转换到世界坐标系的第一世界坐标表达式为：(RT)^-1C(RT)S；其中(RT)为待确定的激光雷达到组合惯导的位姿转换矩阵。

S304，根据各个标定板的参考点在两个点云数据集中对应的点云坐标，生成目标标定板的参考点在第二采集位置对应的第二世界坐标表达式。

可选的，本步骤可以根据各个标定板的参考点在两个点云数据集中对应的点云坐标，采用第二种世界坐标求解算法，生成目标标定板的参考点在第二采集位置对应的第二世界坐标表达式。具体的执行过程可以包括如下两个子步骤：

S3041，根据各个标定板的参考点在两个点云数据集中对应的点云坐标，确定点云数据从第一采集位置到第二采集位置的点云变换矩阵。

可选的，本子步骤可以是将各个标定板在第一采集位置采集的点云数据集中对应的点云坐标，与各个标定板在第二位置采集的点云数据集中对应的点云坐标进行匹配，确定点云数据从第一采集位置到第二采集位置的点云变换矩阵。具体的，可以是将各个标定板在第一采集位置采集的点云数据集中对应的点云坐标和第二采集位置采集的点云数据集中对应的点云坐标用迭代最近点算法(Iterative Closest Points，ICP)算法或遗传迭代最近点算法(Genetic Iterative Closest Points，GICP)算法等进行离线配准，在两者之间的距离、距离均值或距离方差中任意一个最小的情况下，所确定的点云数据从第一采集位置到第二采集位置的点云数据变换值，作为点云数据从第一采集位置到第二采集位置的点云变换矩阵T。

S3042，根据点云变换矩阵和目标标定板的参考点在第二采集位置采集的点云数据集中对应的点云坐标，生成目标标定板的参考点在第二采集位置从雷达坐标系转换到世界坐标系的第二世界坐标表达式。

可选的，S3041中已确定出点云变换矩阵T，若目标标定板的参考点在第二采集位置采集的点云数据集中对应的点云坐标为S，则根据位姿变换可以得到该目标标定板的参考点在第二采集位置的点云坐标S从雷达坐标系转换到世界坐标系的第二世界坐标表达式为：TS。

S305，根据第一世界坐标表达式和第二世界坐标表达式，确定激光雷达到组合惯导的位姿转换矩阵。

可选的，由于S303已经确定出了第一世界坐标表达式(RT)^-1C(RT)S，S304已经确定出了第二世界坐标表达式TS，所以本步骤根据这两个世界坐标表达式，确定激光雷达到组合惯导的位姿变换矩阵时，可以是根据第一世界坐标表达式和第二世界坐标表达式，构建目标方程；求解目标方程，得到激光雷达到组合惯导的位姿转换矩阵。具体的，根据第一世界坐标表达式(RT)^-1C(RT)S和第二世界坐标表达式TS，构建目标方程(RT)^-1C(RT)S＝TS，由于惯导转换矩阵C、目标标定板的参考点在第二采集位置采集的点云数据集中对应的点云坐标S以及点云变换矩阵T都已确定，所以可以通过求解该目标方程，得到未知变量(RT)，即激光雷达到组合惯导的位姿转换矩阵。

本发明实施例提供的激光雷达与组合惯导的位姿标定方法，获取呈刚性连接的激光雷达和组合惯导在两个不同的位置处针对多个标定板采集的两个点云数据集和两个惯导数据集；通过两个惯导数据集以及确定的激光雷达到组合惯导的位姿转换矩阵，确定目标标定板的参考点在第二采集位置对应的第二世界坐标表达式；通过获取的两个点云数据集，确定出每个标定板的参考点在各点云数据集中对应的点云坐标，进而根据其确定目标标定板的参考点在第二采集位置对应的第一世界坐标表达式；进而根据第一世界坐标表达式和第二世界坐标表达式，确定激光雷达到组合惯导的位姿转换矩阵。极大的提高了标定结果的准确性，为激光雷达和组合惯导之间相对位姿的标定提供了一种新思路。

实施例四

图4为本发明实施例四提供的一种激光雷达与组合惯导的位姿标定装置的结构示意图。该装置可执行本发明任意实施例所提供的激光雷达与组合惯导的位姿标定方法，具备执行方法相应的功能模块和有益效果。如图4所示，该装置具体包括：

数据集获取模块401，用于获取激光雷达和组合惯导在两个采集位置对多个标定板采集的两个点云数据集和两个惯导数据集，所述激光雷达和所述组合惯导刚性连接；

点云坐标确定模块402，用于根据所述两个点云数据集，确定各个标定板的参考点在所述两个点云数据集中对应的点云坐标；

位姿转换矩阵确定模块403，用于根据所述两个惯导数据集和各个标定板的参考点在所述两个点云数据集中对应的点云坐标，确定所述激光雷达到所述组合惯导的位姿转换矩阵。

本发明实施例提供的激光雷达与组合惯导的位姿标定装置，获取呈刚性连接的激光雷达和组合惯导在两个不同位置处针对多个标定板采集的两个点云数据集和两个惯导数据集，通过获取的两个点云数据集，确定出每个标定板的参考点在各点云数据集中对应的点云坐标，进而结合两个惯导数据集确定激光雷达到组合惯导的位姿转换矩阵，从而能够准确标定出激光雷达和组合惯导之间的相对位姿。本发明实施例的方案标定激光雷达和组合惯导的相对位姿的整个过程无需人工手动测量，极大的提高了标定结果的准确性，降低了标定成本。为激光雷达和组合惯导之间相对位姿的标定提供了一种新思路。

进一步的，上述点云坐标确定模块402具体包括：

平面空间转换单元，用于针对每个点云数据集，将所述点云数据集对应的三维平面转换为二维平面，并确定转换过程中所述点云数据集对应的空间转换矩阵；

二维坐标确定单元，用于确定各个标定板的参考点在所述二维平面内对应的二维坐标；

点云坐标确定单元，用于根据所述空间转换矩阵和所述二维坐标，确定各个标定板的参考点在所述点云数据集的三维平面中对应的点云坐标。

进一步的，上述平面空间转换单元执行将所述点云数据集对应的三维平面转换为二维平面时，具体用于：

拟合所述点云数据集对应的三维平面方程；

根据所述三维平面方程，确定所述三维平面方程的法向量；

旋转所述三维平面方程对应的三维平面，以使所述三维平面的法向量与直角坐标系中的预选坐标轴重合。

进一步的，每个标定板由反射率不同、中心重合的一组主标定板和辅标定板构成，每组主标定板和辅标定板平行设置，且所述每组主标定板在辅标定板上的投影轮廓包含在所述辅标定板的轮廓内；

相应的，若参考点为标定板的中心点，则二维坐标确定单元具体用于：

根据所述二维平面内各个标定板的点云数据，确定各个标定板的主标定板在所述二维平面内的轮廓；

根据所述各个标定板的主标定板在所述二维平面内的轮廓，拟合所述各个标定板的中心点在所述二维平面内对应的二维坐标。

进一步的，上述位姿转换矩阵确定模块403包括：

第一表达式确定单元，用于根据所述两个惯导数据集和待确定的所述激光雷达到所述组合惯导的位姿转换矩阵，生成目标标定板的参考点在第二采集位置对应的第一世界坐标表达式；其中，目标标定板为所述多个标定板中的任一标定板；

第二表达式确定单元，用于根据各个标定板的参考点在所述两个点云数据集中对应的点云坐标，生成所述目标标定板的参考点在所述第二采集位置对应的第二世界坐标表达式；

位姿转换矩阵确定单元，用于根据所述第一世界坐标表达式和所述第二世界坐标表达式，确定所述激光雷达到所述组合惯导的位姿转换矩阵；

其中，第二采集位置为所述两个采集位置中第二次采集点云数据集和惯导数据集的位置。

进一步的，上述第一表达式确定单元具体用于：

根据所述两个惯导数据集，确定所述组合惯导从第一采集位置到第二采集位置的惯导转换矩阵；

根据所述惯导转换矩阵、待确定的所述激光雷达到所述组合惯导的位姿转换矩阵和目标标定板的参考点在所述第二采集位置采集的点云数据集中对应的点云坐标，生成所述目标标定板的参考点在所述第二采集位置从所述雷达坐标系转换到世界坐标系的第一世界坐标表达式。

进一步的，上述第二表达式确定单元具体用于：

根据各个标定板的参考点在所述两个点云数据集中对应的点云坐标，确定点云数据从第一采集位置到第二采集位置的点云变换矩阵；

根据所述点云变换矩阵和目标标定板的参考点在所述第二采集位置采集的点云数据集中对应的点云坐标，生成所述目标标定板的参考点在所述第二采集位置从所述雷达坐标系转换到世界坐标系的第二世界坐标表达式。

进一步的，上述位姿转换矩阵确定单元具体用于：

根据所述第一世界坐标表达式和所述第二世界坐标表达式，构建目标方程；

求解所述目标方程，得到激光雷达到所述组合惯导的位姿转换矩阵。

实施例五

图5A为本发明实施例五提供的一种测绘系统的结构示意图，图5B为本发明实施例四提供的一种测绘系统的控制装置的结构示意图。图5A所示的测绘系统5包括激光雷达51、组合惯导52以及控制装置50。其中，控制装置50分别与激光雷达51和组合惯导52连接，激光雷达51和组合惯导52刚性连接。图5B示出了适于用来实现本发明实施方式的示例性控制装置50的框图。图5B显示的控制装置50仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。如图5B所示，该控制装置50以通用计算设备的形式表现。该控制装置50的组件可以包括但不限于：一个或者多个处理器或者处理单元501，系统存储器502，连接不同系统组件(包括系统存储器502和处理单元501)的总线503。

总线503表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器，外围总线，图形加速端口，处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说，这些体系结构包括但不限于工业标准体系结构(ISA)总线，微通道体系结构(MAC)总线，增强型ISA总线、视频电子标准协会(VESA)局域总线以及外围组件互连(PCI)总线。

控制装置50典型地包括多种计算机系统可读介质。这些介质可以是任何能够被控制装置50访问的可用介质，包括易失性和非易失性介质，可移动的和不可移动的介质。

系统存储器502可以包括易失性存储器形式的计算机系统可读介质，例如随机存取存储器(RAM)504和/或高速缓存存储器505。控制装置50可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。仅作为举例，存储系统506可以用于读写不可移动的、非易失性磁介质(图5B未显示，通常称为“硬盘驱动器”)。尽管图5B中未示出，可以提供用于对可移动非易失性磁盘(例如“软盘”)读写的磁盘驱动器，以及对可移动非易失性光盘(例如CD-ROM,DVD-ROM或者其它光介质)读写的光盘驱动器。在这些情况下，每个驱动器可以通过一个或者多个数据介质接口与总线503相连。系统存储器502可以包括至少一个程序产品，该程序产品具有一组(例如至少一个)程序模块，这些程序模块被配置以执行本发明各实施例的功能。

具有一组(至少一个)程序模块507的程序/实用工具508，可以存储在例如系统存储器502中，这样的程序模块507包括但不限于操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块507通常执行本发明所描述的实施例中的功能和/或方法。

控制装置50也可以与一个或多个外部设备509(例如键盘、指向设备、显示器510等)通信，还可与一个或者多个使得用户能与该设备交互的设备通信，和/或与使得该控制装置50能与一个或多个其它计算设备进行通信的任何设备(例如网卡，调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口511进行。并且，控制装置50还可以通过网络适配器512与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。如图5B所示，网络适配器512通过总线503与控制装置50的其它模块通信。应当明白，尽管图中未示出，可以结合控制装置50使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。

处理单元501通过运行存储在系统存储器502中的程序，从而执行各种功能应用以及数据处理，例如实现本发明实施例所提供激光雷达与组合惯导的位姿标定方法。

实施例六

本发明实施例六还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时可实现上述实施例所述的激光雷达与组合惯导的位姿标定方法。

本发明实施例的计算机存储介质，可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是但不限于：电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。

计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络，包括局域网(LAN)或广域网(WAN)，连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

上述实施例序号仅仅为了描述，不代表实施例的优劣。

本领域普通技术人员应该明白，上述的本发明实施例的各模块或各操作可以用通用的计算装置来实现，它们可以集中在单个计算装置上，或者分布在多个计算装置所组成的网络上，可选地，他们可以用计算机装置可执行的程序代码来实现，从而可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或操作制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件的结合。

本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间的相同或相似的部分互相参见即可。

以上所述仅为本发明的优选实施例，并不用于限制本发明，对于本领域技术人员而言，本发明可以有各种改动和变化。凡在本发明的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种激光雷达与组合惯导的位姿标定方法，其特征在于，包括：

获取激光雷达和组合惯导在两个采集位置对多个标定板采集的两个点云数据集和两个惯导数据集，所述激光雷达和所述组合惯导刚性连接；

根据所述两个点云数据集，确定各个标定板的参考点在所述两个点云数据集中对应的点云坐标；

根据所述两个惯导数据集和各个标定板的参考点在所述两个点云数据集中对应的点云坐标，确定所述激光雷达到所述组合惯导的位姿转换矩阵；

根据所述两个惯导数据集和各个标定板的参考点在所述两个点云数据集中对应的点云坐标，确定所述激光雷达到所述组合惯导的位姿转换矩阵，包括：

根据所述两个惯导数据集和待确定的所述激光雷达到所述组合惯导的位姿转换矩阵，生成目标标定板的参考点在第二采集位置对应的第一世界坐标表达式；其中，目标标定板为所述多个标定板中的任一标定板；

根据各个标定板的参考点在所述两个点云数据集中对应的点云坐标，生成所述目标标定板的参考点在所述第二采集位置对应的第二世界坐标表达式；

根据所述第一世界坐标表达式和所述第二世界坐标表达式，确定所述激光雷达到所述组合惯导的位姿转换矩阵；其中，第二采集位置为所述两个采集位置中第二次采集点云数据集和惯导数据集的位置。

2.根据权利要求1所述的方法，其特征在于，根据所述两个点云数据集，确定各个标定板的参考点在所述两个点云数据集中对应的点云坐标，包括：

针对每个点云数据集，将所述点云数据集对应的三维平面转换为二维平面，并确定转换过程中所述点云数据集对应的空间转换矩阵；

确定各个标定板的参考点在所述二维平面内对应的二维坐标；

根据所述空间转换矩阵和所述二维坐标，确定各个标定板的参考点在所述点云数据集的三维平面中对应的点云坐标。

3.根据权利要求2所述的方法，其特征在于，将所述点云数据集对应的三维平面转换为二维平面，包括：

拟合所述点云数据集对应的三维平面方程；

根据所述三维平面方程，确定所述三维平面方程的法向量；

旋转所述三维平面方程对应的三维平面，以使所述三维平面的法向量与直角坐标系中的预选坐标轴重合。

4.根据权利要求2所述的方法，其特征在于，每个标定板由反射率不同、中心重合的一组主标定板和辅标定板构成，每组主标定板和辅标定板平行设置，且所述每组主标定板在辅标定板上的投影轮廓包含在所述辅标定板的轮廓内；

相应的，若所述参考点为标定板的中心点，则确定各个标定板的参考点在所述二维平面内对应的二维坐标，包括：

根据所述二维平面内各个标定板的点云数据，确定各个标定板的主标定板在所述二维平面内的轮廓；

根据所述各个标定板的主标定板在所述二维平面内的轮廓，拟合所述各个标定板的中心点在所述二维平面内对应的二维坐标。

5.根据权利要求1所述的方法，其特征在于，根据所述两个惯导数据集，和待确定的所述激光雷达到所述组合惯导的位姿转换矩阵，生成目标标定板的参考点在第二采集位置对应的第一世界坐标表达式，包括：

根据所述两个惯导数据集，确定所述组合惯导从第一采集位置到第二采集位置的惯导转换矩阵；

根据所述惯导转换矩阵、待确定的所述激光雷达到所述组合惯导的位姿转换矩阵和目标标定板的参考点在所述第二采集位置采集的点云数据集中对应的点云坐标，生成所述目标标定板的参考点在所述第二采集位置从雷达坐标系转换到世界坐标系的第一世界坐标表达式。

6.根据权利要求1所述的方法，其特征在于，根据各个标定板的参考点在所述两个点云数据集中对应的点云坐标，生成所述目标标定板的参考点在所述第二采集位置对应的第二世界坐标表达式，包括：

根据各个标定板的参考点在所述两个点云数据集中对应的点云坐标，确定点云数据从第一采集位置到第二采集位置的点云变换矩阵；

根据所述点云变换矩阵和目标标定板的参考点在所述第二采集位置采集的点云数据集中对应的点云坐标，生成所述目标标定板的参考点在所述第二采集位置从雷达坐标系转换到世界坐标系的第二世界坐标表达式。

7.根据权利要求1所述的方法，其特征在于，根据所述第一世界坐标表达式和所述第二世界坐标表达式，确定所述激光雷达到所述组合惯导的位姿转换矩阵，包括：

根据所述第一世界坐标表达式和所述第二世界坐标表达式，构建目标方程；

求解所述目标方程，得到激光雷达到所述组合惯导的位姿转换矩阵。

8.一种测绘系统，其特征在于，包括激光雷达、组合惯导和控制装置；所述控制装置分别与所述激光雷达和所述组合惯导连接，所述控制装置包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序；

当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-7中任一所述的激光雷达与组合惯导的位姿标定方法。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一所述的激光雷达与组合惯导的位姿标定方法。

Images