CN113534110B - 一种多激光雷达系统静态标定方法 - Google Patents

Abstract

本发明公开了一种多激光雷达系统静态标定方法，所述方法在进行多激光雷达系统标定时，只需要获取多个激光雷达在不同系统位姿下的采集的点云数据，因此可以解决现有技术中在进行多个激光雷达的坐标系标定时，需要设置复杂的检校场，导致标定成本较高且标定过程复杂的问题。

Description

一种多激光雷达系统静态标定方法

技术领域

本发明涉及测绘领域，尤其涉及的是一种多激光雷达系统静态标定方法。

背景技术

目前关于多个激光雷达的坐标系标定方法，需要设置复杂的检校场，包括建立比较严苛的标定环境，借助特征丰富的激光雷达标志板、动作捕捉系统、全站仪等，成本较高且配置麻烦，标定过程相对繁琐且标定结果易受标定装置以及操作人员影响。

因此，现有技术还有待改进和发展。

发明内容

本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种多激光雷达系统静态标定方法，旨在解决现有技术中在进行多个激光雷达的坐标系标定时，需要设置复杂的检校场，导致标定成本较高且标定过程复杂的问题。

本发明解决问题所采用的技术方案如下：

第一方面，本发明实施例提供一种多激光雷达系统静态标定方法，其中，所述方法应用于多激光雷达系统，所述多激光雷达系统包括第一激光雷达和第二激光雷达，所述方法包括：

获取所述第一激光雷达和所述第二激光雷达之间的初始标定参数；

获取所述多激光雷达系统采集的若干组静态点云数据；其中，所述若干组点云数据分别对应不同的系统位姿，且每组点云数据包括在同一系统位姿下，由所述第一激光雷达采集的一帧第一点云数据和由所述第二激光雷达采集的一帧第二点云数据；

提取所述若干组点云数据中每一数据点对应的局部平面法向量，根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量对所述初始标定参数进行调整，得到目标标定参数。

在一种实现方式中，所述提取所述若干组点云数据中每一数据点对应的局部平面法向量，包括：

针对所述若干组点云数据中的每帧点云数据，获取该帧点云数据中所有相邻数据点之间的距离信息；

根据所述距离信息确定该帧点云数据中各数据点分别对应的邻域点集合；

根据各数据点分别对应的邻域点集合，确定各数据点分别对应的局部平面法向量；其中，针对每一数据点，确定该数据点对应的邻域点集合的主方向，将所述主方向作为该数据点对应的局部平面法向量。

在一种实施方式中，所述获取该帧点云数据中所有相邻数据点之间的距离信息，包括：

提取出该帧点云数据对应的扫描线集合；

通过线简化算法确定所述扫描线集合中每一扫描线对应的特征点，得到特征点分布信息；

根据所述特征点分布信息，确定该帧点云数据中所有相邻数据点之间的距离信息。

在一种实施方式中，所述通过线简化算法确定所述扫描线集合中每一扫描线对应的特征点，得到特征点分布信息，包括：

针对所述扫描线集合中每一扫描线，通过线简化算法确定该扫描线上的所有拐点，并将该扫描线上的所有拐点作为该扫描线对应的所有特征点，得到该扫描线集合对应的特征点分布信息。

在一种实施方式中，所述根据所述特征点分布信息确定该帧点云数据中所有相邻数据点之间的距离信息，包括：

针对该帧点云数据中任意一对相邻数据点，通过比对所述特征点分布信息，判断该对相邻数据点是否属于同一平面；

若该对相邻数据点都为特征点，则将该对相邻数据点之间的距离设为第一距离；

若该对相邻数据点其中之一是特征点，且该特征点相邻不存在其他特征点，则将该对相邻数据点之间的距离设为第一距离；

若该对相邻数据点都不是特征点，则将该对相邻数据点之间的距离设为第二距离；其中，所述第一距离大于所述第二距离。

在一种实施方式中，所述根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量对所述初始标定参数进行调整，得到目标标定参数，包括：

通过所述初始标定参数，将所述若干组点云数据中由所述第一激光雷达采集的若干帧第一点云数据分别转换至所述第二激光雷达对应的目标坐标系下，得到若干帧标准点云数据；

通过所述初始标定参数将所述若干帧第一点云数据中每一数据点对应的局部平面法向量转换至所述目标坐标系下，得到所述若干帧标准点云数据中每一数据点对应的标准法向量；

根据所述若干帧标准点云数据和所述若干帧标准点云数据中每一标准数据点对应的标准法向量，确定所述初始标定参数的误差值；

若所述误差值大于预设阈值，则根据所述误差值更新所述初始标定参数，并重新执行所述根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量对所述初始标定参数进行调整的步骤，直至所述误差值小于或者等于所述预设阈值，得到所述目标标定参数。

在一种实施方式中，所述根据所述若干帧标准点云数据和所述若干帧标准点云数据中每一标准数据点对应的标准法向量，确定所述初始标定参数的误差值，包括：

获取所述若干组点云数据中由所述第二激光雷达采集的若干帧第二点云数据；

根据所述若干帧第二点云数据，确定所述若干帧标准点云数据中每一数据点对应的投影点以及投影点对应的局部平面法向量；

将所述若干帧标准点云数据每一标准数据点的坐标数据、所述若干帧标准点云数据中每一数据点对应的投影点的坐标数据以及投影点对应的局部平面法向量代入误差函数，得到所述误差值。

在一种实施方式中，所述根据所述若干帧第二点云数据，确定所述若干帧标准点云数据中每一数据点对应的投影点以及投影点对应的局部平面法向量，包括：

针对在同一系统位姿下的一帧标准点云数据和一帧第二点云数据，在该帧第二点云数据中确定该帧标准点云数据中各数据点分别对应的最邻近点集合，根据各数据点对应的最邻近点集合确定各数据点分别对应的投影点以及投影点对应的局部平面法向量。

在一种实施方式中，所述根据各数据点对应的最邻近点集合确定各数据点分别对应的投影点以及投影点对应的局部平面法向量，包括：

针对所述各数据点中的每一数据点，根据该数据点对应的最邻近点集合确定投影平面；

将该数据点投影至所述投影平面，得到该数据点对应的投影点；

将该数据点的最邻近点对应的局部平面法向量，作为该投影点的法向量。

本发明的有益效果：本发明在进行多激光雷达系统标定时，只需要获取多个激光雷达在不同系统位姿下的采集的点云数据，因此可以解决现有技术中在进行多个激光雷达的坐标系标定时，需要设置复杂的检校场，导致标定成本较高且标定过程复杂的问题。

附图说明

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

图1是本发明实施例提供的多激光雷达系统静态标定方法的流程示意图。

图2是本发明实施例提供的根据特征点分布信息确定所有相邻点云数据之间的距离信息的参考图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后……)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

激光雷达作为一种主动式探测传感器，它不依赖于外界光照条件，且具有高精度的测距信息，因此近些年来被广泛运用于地面移动测量、地形勘测以及自动驾驶等相关应用中。受到激光雷达安放位置以及扫描范围限制，单个激光雷达往往不能满足应用对点云数据密度和覆盖范围的需求，因此往往会在设备上安装多个激光雷达。

当处理多个激光雷达的数据时，由于安放位置和姿态的不同，激光雷达数据的坐标系也会有差异。在进行点云数据配准以及地图构建时，首先要把多个激光雷达的坐标系进行统一，这样才便于后期的数据处理，这就是激光雷达坐标系的标定。为了保证激光点云数据足够的覆盖范围，各个激光雷达点云数据之间重叠度一般比较低，这加大了激光雷达之间坐标系标定的难度。

目前关于多个激光雷达的坐标系标定方法，需要设置复杂的检校场，包括建立比较严苛的标定环境，借助特征丰富的激光雷达标志板、动作捕捉系统、全站仪等，成本较高且配置麻烦，标定过程相对繁琐且标定结果易受标定装置以及操作人员影响。

针对现有技术的上述缺陷，本发明提供了一种多激光雷达系统静态标定方法，通过获取所述第一激光雷达和所述第二激光雷达之间的初始标定参数；获取所述多激光雷达系统采集的若干组点云数据；其中，所述若干组点云数据分别对应不同的系统位姿，且每组点云数据包括在同一系统位姿下，由所述第一激光雷达采集的一帧第一点云数据和由所述第二激光雷达采集的一帧第二点云数据；提取所述若干组点云数据中每一数据点对应的局部平面法向量；根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量，对所述初始标定参数进行调整，得到目标标定参数。本发明在进行多激光雷达系统标定时，只需要获取多个激光雷达在不同系统位姿下的采集的点云数据，因此可以解决现有技术中在进行多个激光雷达的坐标系标定时，需要设置复杂的检校场，导致标定成本较高且标定过程复杂的问题。

如图1所示，所述方法应用于多激光雷达系统，所述多激光雷达系统包括第一激光雷达和第二激光雷达，所述方法包括如下步骤：

步骤S100、获取所述第一激光雷达和所述第二激光雷达之间的初始标定参数。

具体地，本实施例的目标是为了获得多激光雷达系统的精确的标定参数，为了实现这一目标，本实施例首先需要获取粗略的初始标定参数，再对该初始标定参数进行不断调整，以最终得到精确的目标标定参数。在一种实现方式中，可以通过多激光雷达系统的安装信息得到该初始标定参数。具体地，多激光雷达系统中的各激光雷达通常安装在具有确定结构参数的刚性框架上，因此可以通过多激光雷达的安装信息，得到系统框架结构参数，进而根据系统框架结构参数计算出各激光雷达之间精确度不高的初始标定参数。在另一种实现方式中，还可以通过标尺测量的方式获取到该初始标定参数。

需要说明的是，本实施例仅是以第一激光雷达和第二激光雷达作为示例说明本方法的具体实施方式，在实际应用中，本方法还可以应用于超过两个激光雷达的系统，只需要将该系统拆分为由多个双激光雷达系统组成的形式即可。

如图1所示，所述方法还包括如下步骤：

步骤S200、获取所述多激光雷达系统采集的若干组点云数据；其中，所述若干组点云数据分别对应不同的系统位姿，且每组点云数据包括在同一系统位姿下，由所述第一激光雷达采集的一帧第一点云数据和由所述第二激光雷达采集的一帧第二点云数据。

由于本实施例无需设置复杂的校验场，因此为了确保标定的精确性，本实施例需要获取不同系统位姿下第一激光雷达系统和第二激光雷达分别采集的点云数据，以得到足够的点云数据提供给终端进行解算。需要强调的是，本实施例中采集的点云数据为静态点云数据，在实际应用中，相关操作人员只需要将多激光雷达系统对应的设备放置在空旷的标定场内，周围无需设置复杂的标定板或者标靶，只需要每次稍微改变系统的位姿，然后使第一激光雷达和第二激光雷达每次都在同一系统位姿下同时静止采集点云数据，得到若干组点云数据即可。简言之，本步骤的目的是在不改变标定场环境的情况下，通过改变系统位姿以获得各激光雷达分别采集的同一位姿下不同的观测数据。

如图1所示，所述方法还包括如下步骤：

步骤S300、提取所述若干组点云数据中每一数据点对应的局部平面法向量，根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量，对所述初始标定参数进行调整，得到目标标定参数。

具体地，本实施例通过提取每一数据点对应的局部平面法向量，可以实现对第一激光雷达和第二激光雷达分别采集的点云数据进行配准，进而根据配准结果确定初始标定参数的误差值，从而根据该误差值对初始标定参数进行调整，以缩小初始标定参数与真实标定参数之间的差距，得到准确反映第一激光雷达和第二激光雷达的坐标转换关系的目标标定参数。

在一种实现方式中，所述提取所述若干组点云数据中每一数据点对应的局部平面法向量，具体包括如下步骤：

步骤S301、针对所述若干组点云数据中的每帧点云数据，获取该帧点云数据中所有相邻数据点之间的距离信息；

步骤S302、根据所述距离信息确定该帧点云数据中各数据点分别对应的邻域点集合；

步骤S303、根据各数据点分别对应的邻域点集合，确定各数据点分别对应的局部平面法向量；其中，针对每一数据点，确定该数据点对应的邻域点集合的主方向，将所述主方向作为该数据点对应的局部平面法向量。

具体地，由于每一数据点的局部平面法向量需要用于后续调整初始标定参数，因此为了获得每一数据点的稳定可靠的局部平面法向量计算结果，本实施例首先需要筛选出每一数据点的邻域点集合，由每一数据点的邻域点集合的主方向确定其对应的局部平面法向量。针对每一数据点，以该数据点为中心点的话，由于该中心点对应的邻域点集合中每一数据点均与该中心点处于同一平面，因此以该中心点的邻域点集合的主方向作为该中心点的局部平面法向量更准确。

需要说明的是，因为点云数据的分布密度与数据点到原点的距离呈反比例变化，即距离原点越远，数据点的密度越小，相邻点之间的距离也越远。所以本实施例采用线性分段函数对不同距离数据点的邻域增长范围F(d)做出了限定，具体函数如下：

其中，F_max和F_min分别表示最大、最小邻域范围点数，d_max和d_min分别表示最大、最小数据点距离。

针对一个数据点，本实施例提供一个确定该数据点对应的邻域点集合，并根据邻域点集合确定局部平面法向量的算法，该算法的具体流程为：

(1)建立一个空队列Q用来存储该数据点的预选邻域点，队列Q满足先进先出的原则，并将该数据点推入队尾作为初始预选邻域点；

(2)建立一个集合P存储挑选出的邻域点集合，初始将集合P置为空；

(3)当队列Q不为空且集合P元素数量小于限定的邻域增长范围F(d)时，不断推出队列Q的队头元素，并对推出的数据点元素进行以下操作：

a)当该元素属于格网数据且未被访问，另外横轴和纵轴两个方向都存在距离为第二距离的相邻点时，则将该元素加入到邻域点集合P，并标注该点已经被访问过，否则直接丢弃该元素。

b)当该元素已经判断属于邻域点集合，则认为该点有继续邻域增长的潜能，按照左右上下的顺序依次判断该点与四个相邻点的距离，若为第二距离则可将对应的相邻点推入预选邻域点队列Q。

c)当队列Q为空或者集合P元素数量大于等于限定的邻域增长范围F(d)时，则认为无法或没有必要继续寻找邻域点，此时集合P即所需的该数据点的邻域点集合。

(4)根据步骤(3)得到的邻域点集合P，进行目标点的局部平面法向量计算，当邻域点集合大小满足条件时，则认为该目标点具有局部平面法向量，用主成分分析方法获得邻域点集合的主方向，即对应的局部平面法向量。需要注意的是，如果邻域点集合协方差矩阵的最小特征值和所有特征值之和的比值过大，则认为该目标点的邻域平面凹凸不平，不具备局部平面法向量。

在一种实现方式中，为了获取到该帧点云数据中所有相邻数据点之间的距离信息，本实施例可以提取出该帧点云数据对应的扫描线集合；通过线简化算法确定所述扫描线集合中每一扫描线对应的特征点，得到特征点分布信息；根据所述特征点分布信息，确定该帧点云数据中所有相邻数据点之间的距离信息。

具体地，本实施例的该步骤是以一帧点云数据为例，可以理解的是，一组点云数据中包含有两帧点云数据，分别是第一激光雷达采集到的点云数据和第二激光雷达采集到的点云数据，对于每一帧点云数据均需要执行一次该步骤。在实际应用中，激光雷达是通过将扫描点打在物体上，获取物体表面反射的信号来采集点云数据的，因此按照激光雷达扫描顺序就可以恢复出每帧点云数据对应的扫描线集合。对于一帧点云数据而言，在其对应的扫描线集合内，该帧点云数据中的每一数据点都有一条或多条对应的扫描线，每一数据点可能属于多条扫描线。由于本实施例使用的激光雷达为旋转式或面阵式三维激光扫描仪，因此每一扫描线集合内均包括规则排列成格网的若干纵轴扫描线和若干横轴扫描线，即一帧点云数据由一幅完整的格网构成，该格网中横轴扫描线和纵轴扫描线的数量为预先设定的参数，无人为修改不会改变，对于一帧点云数据中的缺失点云数据，可以采用激光雷达扫描坐标系原点代替，以确保每一帧点云数据的完整性。对于一个扫描线集合而言，该扫描线集合中包含多条扫描线，而每一条扫描线上包含有多个点，因此本实施例通过线简化算法选择出每一条扫描线的部分点作为该扫描线的特征点。由于所有点云数据均分布在扫描线上，因此可以利用扫描线上分布的特征点，确定所有相邻数据点之间的距离信息。

在一种实现方式中，所述通过线简化算法确定所述扫描线集合中每一扫描线对应的特征点，得到特征点分布信息，具体包括：针对所述扫描线集合中每一扫描线，通过线简化算法确定该扫描线上的所有拐点，并将该扫描线上的所有拐点作为该扫描线对应的所有特征点，得到该扫描线集合对应的特征点分布信息。

具体地，本实施例通过线简化算法确定该扫描线集合中每一条扫描线的拐点，然后将该扫描线集合中每一条扫描线的拐点作为特征点，得到该扫描线集合对应的特征点分布信息。在一种实现方式中，本实施例可根据所处理扫描线的特点及需要保留的特征选择与目的最合适的算法。例如可以选择Douglas-Peucker算法：依据垂距阈值进行迭代式分割，当垂距最大值超过阈值时，在该最大点处进行分割，并分别检查分割产生的两条子线段，直至无法分割；或者选择Lang算法：依据垂距阈值进行增长和分割，当垂距大于阈值时，删除该点之前的一个点与起点间的点，并由该点之前的一个点开始继续搜索；又或者选择Visvalingam-Williamson算法：依据每相邻三个顶点组成的三角形面积，自动删去面积小于阈值且最小的三角形的中间一个顶点，并再次搜索，直至没有三角形可以被删除。由于所有点云数据均分布在扫描线上，因此可以利用扫描线上分布的特征点，确定所有相邻数据点之间的距离信息。

在一种实现方式中，所述根据所述特征点分布信息确定该帧点云数据中所有相邻数据点之间的距离信息，具体包括：针对该帧点云数据中任意一对相邻数据点，通过比对所述特征点分布信息，判断该对相邻数据点是否属于同一平面；若该对相邻数据点都为特征点，则将该对相邻数据点之间的距离设为第一距离；若该对相邻数据点其中之一是特征点，且该特征点相邻不存在其他特征点，则将该对相邻数据点之间的距离设为第一距离；若该对相邻数据点都不是特征点，则将该对相邻数据点之间的距离设为第二距离；其中，所述第一距离大于所述第二距离。

具体地，本实施例以一帧点云数据的处理过程为例，针对该帧点云数据中的每对相邻数据点，通过比对该帧点云数据对应的特征点分布信息，判断该对相邻数据点是否属于同一平面，可以理解的是，特征点信息是附着在数据点上的，并不是单独创造一个特征点数据，而是通过判断一个数据点是不是特征点从而给每一数据点添加标签。若该对相邻数据点都为特征点，则判定该对相邻数据点不属于同一平面，并将该对相邻数据点之间的距离设为第一距离(如图2所示)；若该对相邻数据点其中之一是特征点，且该特征点相邻不存在其他特征点，则同样判定该对相邻数据点不属于同一平面，并将该对相邻数据点之间的距离设为第一距离(如图2所示)；若该对相邻数据点都不是特征点，则判定该对相邻数据点属于同一平面，并将该对相邻数据点之间的距离设为第二距离(如图2所示)。值得注意的是，因为每条扫描线的端点只存在一边的相邻点，于是默认将该端点和另一边不存在的相邻点之间的距离设为第一距离。

在一种实现方式中，所述根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量对所述初始标定参数进行调整，得到目标标定参数，具体包括：通过所述初始标定参数，将所述若干组点云数据中由所述第一激光雷达采集的若干帧第一点云数据分别转换至所述第二激光雷达对应的目标坐标系下，得到若干帧标准点云数据；通过所述初始标定参数将所述若干帧第一点云数据中每一数据点对应的局部平面法向量转换至所述目标坐标系下，得到所述若干帧标准点云数据中每一数据点对应的标准法向量；根据所述若干帧标准点云数据和所述若干帧标准点云数据中每一标准数据点对应的标准法向量，确定所述初始标定参数的误差值；若所述误差值大于预设阈值，则根据所述误差值更新所述初始标定参数，并重新执行所述根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量对所述初始标定参数进行调整的步骤，直至所述误差值小于或者等于所述预设阈值，得到所述目标标定参数。

具体地，本实施例的目标就是根据初始标定参数和若干组点云数据中每一数据点对应的局部平面法向量，不断对第一激光雷达采集的点云数据和第二激光雷达采集的点云数据进行点云配准，以获得目标标定参数。

具体地，本实施例需要根据每次点云配准的结果确定当前标定参数与真实值之间的误差值，该误差值可以反映当前标定参数与真实值之间的差距，并以该误差值对当前标定参数进行调整。再以更新后的标定参数，重新对第一激光雷达采集的点云数据和第二激光雷达采集的点云数据进行点云配准，并确定当前标定参数与真实值之间的误差值，再根据该误差值对该当前标定参数进行调整，即本步骤是一个更新迭代的步骤，通过不断更新标定参数，使得更新后的标定参数不断接近真实值。若当前计算得到的标定参数的误差很小时，表示当前标定参数已经非常接近真实值，不需要再进行更新，这时候就可以停止计算，并得到最终的目标标定参数。

在一种实现方式中，所述根据所述若干帧标准点云数据和所述若干帧标准点云数据中每一标准数据点对应的标准法向量，确定所述初始标定参数的误差值，具体包括：获取所述若干组点云数据中由所述第二激光雷达采集的若干帧第二点云数据；根据所述若干帧第二点云数据，确定所述若干帧标准点云数据中每一数据点对应的投影点以及投影点对应的局部平面法向量；将所述若干帧标准点云数据每一标准数据点的坐标数据、所述若干帧标准点云数据中每一数据点对应的投影点的坐标数据以及投影点对应的局部平面法向量代入误差函数，得到所述误差值。

简单来讲，本实施例将第二激光雷达的坐标系作为参考坐标系，并将同一位姿下第一激光雷达采集的点云数据转换至该参考坐标系下，得到标准点云数据和标准点云数据中各数据点对应的标准法向量。为了方便说明，本实施例将第二激光雷达采集的点云数据作为第二点云数据，然后在第二点云数据中确定标准点云数据中各数据点对应的投影点以及对应的局部平面法向量，将各标准点云数据的坐标数据、各标准点云数据分别对应的投影点的坐标数据以及各投影点对应的局部平面法向量输入预设的误差函数中，该误差函数即可输出一个误差值，该误差值用于反映初始标定参数与真实值之间的差距。

在一种实现方式中，为了确定标准点云数据中各数据点对应的投影点以及投影点对应的局部平面法向量，本实施例针对在同一系统位姿下的一帧标准点云数据和一帧第二点云数据，在该帧第二点云数据中确定该帧标准点云数据中各数据点分别对应的最邻近点集合，根据各数据点对应的最邻近点集合确定各数据点分别对应的投影点以及投影点对应的局部平面法向量。

具体地，针对在同一系统位姿下的一帧标准点云数据和一帧第二点云数据，本实施例需要在该帧第二点云数据中确定该帧标准点云数据中每一数据点分别对应的最邻近点集合，所述最邻近点为在空间距离上与该数据点相距最近的前N点。其中，该数据点的标准法向量与该最邻近点集合的每个数据点的局部平面法向量的角度差值小于预设阈值，如不满足则需要从邻近点集合中剔除。然后根据该最邻近点集合确定该数据点在该最邻近点集合所在平面的投影点。

在一种实现方式中，所述根据各数据点对应的最邻近点集合确定各数据点分别对应的投影点，具体包括：针对所述各数据点中的每一数据点，根据该数据点对应的最邻近点集合确定投影平面；将该数据点投影至所述投影平面，得到该数据点对应的投影点。

概括地讲，对于该帧标准点云数据中的任意一个数据点，在该帧标准点云数据对应的同一姿态下的第二点云数据中确定该数据点对应的最邻近点集合，此外，还需要比较该数据点与该最邻近点集合中每个数据点之间的法向量角度差，如果法向量角度差大于或者等于预设阈值，则认为该邻近点无效，需要从集合中删除；只有当法向量角度差小于预设阈值，才认为该邻近点有效。然后利用该最邻近点集合构建一个投影平面，并将该数据点投影至该投影平面上，得到该数据点对应的投影点，需要注意的是，还需要约束该投影点的法向量与数据点的最邻近点的法向量一致。

然后，对于每一个数据点P_B(p_x,p_y,p_z)和该数据点对应的投影点P_B′(p_x′,p_y′,p_z′)以及法向量N_B′(n_x,n_y,n_z)，代入如下的线性误差方程进行求解。在一种实现方式中，所述线性误差方程如下所示：

其中，β，γ，t_x,t_y,t_z即是需要求解的标定参数误差，6个标定参数误差即可组成ΔT，其中，ΔT为矩阵形式。

具体地，根据标定参数误差矩阵ΔT对旧的标定参数T_old进行更新，更新方式如下：

T_new＝ΔT*T_old

当某次标定参数误差ΔT小于小于预设阈值，就可以认为当前的标定参数与真实值之间的误差值满足要求，迭代结束，并将当前的标定参数作为目标标定参数。

综上所述，本发明公开了一种多激光雷达系统静态标定方法，通过获取所述第一激光雷达和所述第二激光雷达之间的初始标定参数；获取所述多激光雷达系统采集的若干组点云数据；其中，所述若干组点云数据分别对应不同的系统位姿，且每组点云数据包括在同一系统位姿下，由所述第一激光雷达采集的一帧第一点云数据和由所述第二激光雷达采集的一帧第二点云数据；提取所述若干组点云数据中每一数据点对应的局部平面法向量；根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量，对所述初始标定参数进行调整，得到目标标定参数。本发明在进行多激光雷达系统标定时，只需要获取多个激光雷达在不同系统位姿下的采集的点云数据，因此可以解决现有技术中在进行多个激光雷达的坐标系标定时，需要设置复杂的检校场，导致标定成本较高且标定过程复杂的问题。

应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (8)

1.一种多激光雷达系统静态标定方法，其特征在于，所述方法应用于多激光雷达系统，所述多激光雷达系统包括第一激光雷达和第二激光雷达，所述方法包括：

获取所述第一激光雷达和所述第二激光雷达之间的初始标定参数；

获取所述多激光雷达系统采集的若干组点云数据；其中，所述若干组点云数据分别对应不同的系统位姿，且每组点云数据包括在同一系统位姿下，由所述第一激光雷达采集的一帧第一点云数据和由所述第二激光雷达采集的一帧第二点云数据；每一帧所述第一点云数据/所述第二点云数据由一幅格网数据组成，每一帧所述第一点云数据/所述第二点云数据的扫描线集合包括规则排列成格网的若干纵轴扫描线和若干横轴扫描线，每一所述数据点都有一条或者多条对应的扫描线；

提取所述若干组点云数据中每一数据点对应的局部平面法向量，根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量对所述初始标定参数进行调整，得到目标标定参数；

所述提取所述若干组点云数据中每一数据点对应的局部平面法向量，包括：

针对所述若干组点云数据中的每帧点云数据，获取该帧点云数据中所有相邻数据点之间的距离信息；

根据所述距离信息确定该帧点云数据中各数据点分别对应的邻域点集合；

根据各数据点分别对应的邻域点集合，确定各数据点分别对应的局部平面法向量；其中，针对每一数据点，确定该数据点对应的邻域点集合的主方向，将所述主方向作为该数据点对应的局部平面法向量；

每一所述数据点对应的所述邻域点集合和所述局部平面法向量的确定方法包括：

建立空白的队列Q，将该数据点推入所述队列Q的队尾作为初始预选邻域点，其中，所述队列Q用于存储该数据点的预选邻域点，并满足先进先出的原则；

建立一个集合P存储挑选出的邻域点集合，初始将集合P置为空；当队列Q不为空且集合P元素数量小于预设的邻域增长范围时，不断推出队列Q的队头元素，并对推出的元素进行以下操作：当该元素属于格网数据且未被访问，另外横轴和纵轴两个方向都存在距离为第二距离的相邻点时，则将该元素加入到集合P，并标注该元素已经被访问过，否则直接丢弃该元素；当该元素已经判断属于邻域点集合，则认为该元素有继续邻域增长的潜能，按照左右上下的顺序依次判断该元素与四个相邻点的距离，若为第二距离则可将对应的相邻点推入队列Q；

当队列Q为空或者集合P元素数量大于或者等于所述邻域增长范围时，将此时的集合P作为该数据点的邻域点集合；当邻域点集合大小满足预设条件时，则通过主成分分析方法获得邻域点集合的主方向，并将所述主方向作为该数据点对应的所述局部平面法向量，其中，所述预设条件为邻域点集合的协方差矩阵的最小特征值和所有特征值之和的比值小于预设值。

2.根据权利要求1所述的多激光雷达系统静态标定方法，其特征在于，所述获取该帧点云数据中所有相邻数据点之间的距离信息，包括：

提取出该帧点云数据对应的扫描线集合；

通过线简化算法确定所述扫描线集合中每一扫描线对应的特征点，得到特征点分布信息；

根据所述特征点分布信息，确定该帧点云数据中所有相邻数据点之间的距离信息。

3.根据权利要求2所述的多激光雷达系统静态标定方法，其特征在于，所述通过线简化算法确定所述扫描线集合中每一扫描线对应的特征点，得到特征点分布信息，包括：

针对所述扫描线集合中每一扫描线，通过线简化算法确定该扫描线上的所有拐点，并将该扫描线上的所有拐点作为该扫描线对应的所有特征点，得到该扫描线集合对应的特征点分布信息。

4.根据权利要求2所述的多激光雷达系统静态标定方法，其特征在于，所述根据所述特征点分布信息确定该帧点云数据中所有相邻数据点之间的距离信息，包括：

针对该帧点云数据中任意一对相邻数据点，通过比对所述特征点分布信息，判断该对相邻数据点是否属于同一平面；

若该对相邻数据点都为特征点，则将该对相邻数据点之间的距离设为第一距离；

若该对相邻数据点其中之一是特征点，且该特征点相邻不存在其他特征点，则将该对相邻数据点之间的距离设为第一距离；

若该对相邻数据点都不是特征点，则将该对相邻数据点之间的距离设为第二距离；其中，所述第一距离大于所述第二距离。

5.根据权利要求1所述的多激光雷达系统静态标定方法，其特征在于，所述根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量对所述初始标定参数进行调整，得到目标标定参数，包括：

通过所述初始标定参数，将所述若干组点云数据中由所述第一激光雷达采集的若干帧第一点云数据分别转换至所述第二激光雷达对应的目标坐标系下，得到若干帧标准点云数据；

通过所述初始标定参数将所述若干帧第一点云数据中每一数据点对应的局部平面法向量转换至所述目标坐标系下，得到所述若干帧标准点云数据中每一数据点对应的标准法向量；

根据所述若干帧标准点云数据和所述若干帧标准点云数据中每一标准数据点对应的标准法向量，确定所述初始标定参数的误差值；

若所述误差值大于预设阈值，则根据所述误差值更新所述初始标定参数，并重新执行所述根据所述若干组点云数据和所述若干组点云数据中每一数据点对应的局部平面法向量对所述初始标定参数进行调整的步骤，直至所述误差值小于或者等于所述预设阈值，得到所述目标标定参数。

6.根据权利要求5所述的多激光雷达系统静态标定方法，其特征在于，所述根据所述若干帧标准点云数据和所述若干帧标准点云数据中每一标准数据点对应的标准法向量，确定所述初始标定参数的误差值，包括：

获取所述若干组点云数据中由所述第二激光雷达采集的若干帧第二点云数据；

根据所述若干帧第二点云数据，确定所述若干帧标准点云数据中每一数据点对应的投影点以及投影点对应的局部平面法向量；

将所述若干帧标准点云数据每一标准数据点的坐标数据、所述若干帧标准点云数据中每一数据点对应的投影点的坐标数据以及投影点对应的局部平面法向量代入误差函数，得到所述误差值。

7.根据权利要求6所述的多激光雷达系统静态标定方法，其特征在于，所述根据所述若干帧第二点云数据，确定所述若干帧标准点云数据中每一数据点对应的投影点以及投影点对应的局部平面法向量，包括：

针对在同一系统位姿下的一帧标准点云数据和一帧第二点云数据，在该帧第二点云数据中确定该帧标准点云数据中各数据点分别对应的最邻近点集合，根据各数据点对应的最邻近点集合确定各数据点分别对应的投影点以及投影点对应的局部平面法向量。

8.根据权利要求7所述的多激光雷达系统静态标定方法，其特征在于，所述根据各数据点对应的最邻近点集合确定各数据点分别对应的投影点以及投影点对应的局部平面法向量，包括：

针对所述各数据点中的每一数据点，根据该数据点对应的最邻近点集合确定投影平面；

将该数据点投影至所述投影平面，得到该数据点对应的投影点；

将该数据点的最邻近点对应的局部平面法向量作为投影点的法向量。