CN113093155A - 一种激光雷达联合标定方法及系统 - Google Patents

Abstract

本发明提供了一种激光雷达联合标定方法及系统，包括步骤：分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据；分别对两组点云数据进行地面与标志物分割，将地面点拟合成平面方程，将标志物点拟合成直线方程；分别计算标志物的朝向及标志物与地面的交点；分别根据标志物的朝向及对应的标志物与地面的交点计算标志物上预设一点的坐标；分别将第一激光雷达和第二激光雷达观测到标志物与地面的交点，以及标志物上预设一点的坐标组合成一组三维点集；根据两组三维点集计算联合标定结果。该方案的自动化程度更高，标定精度更高，且在标定时不需要看到标志物全部，适用于各种视场的激光雷达的标定。

Description

一种激光雷达联合标定方法及系统

技术领域

本发明涉及雷达标定技术领域，尤指一种激光雷达联合标定方法及系统。

背景技术

激光雷达，是激光探测及测距系统的简称。激光雷达是激光技术与雷达技术相结合的产物，由发射机、天线、接收机、跟踪架及信息处理等部分组成。发射机是各种形式的激光器，如二氧化碳激光器、掺钕钇铝石榴石激光器、半导体激光器及波长可调谐的固体激光器等；天线是光学望远镜；接收机采用各种形式的光电探测器，如光电倍增管、半导体光电二极管、雪崩光电二极管、红外和可见光多元探测器件等。

为了实现机器人的障碍物感知、定位和建图的功能，需要对无人驾驶车辆的双激光雷达传感器进行联合标定，即估计激光雷达2到激光雷达1的位置和旋转的变化，以对准两个激光雷达。现有的激光雷达联合标定方法大多自动化程度低，需要手动标定，且在标定时，需要激光雷达看到标志物全部，不利于小视场的激光雷达的标定。因此，需要一种自动化程度更高，且不需要看到标志物全部，适用范围更广，精度更高的联合标定方法。

发明内容

本发明的目的是提供一种激光雷达联合标定方法及系统，该方案的自动化程度更高，标定精度更高，且在标定时不需要看到标志物全部，适用于各种视场的激光雷达的标定。

本发明提供的技术方案如下：

本发明提供一种激光雷达联合标定方法，包括步骤：

分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据；

分别对两组所述点云数据进行地面与标志物分割，并分别将地面点拟合成平面方程，将标志物点拟合成直线方程；

分别根据所述平面方程和其对应的所述直线方程计算所述标志物的朝向及所述标志物与地面的交点；

分别根据所述标志物的朝向及对应的所述标志物与地面的交点计算所述标志物上预设一点的坐标；

将所述第一激光雷达观测到所述标志物与地面的交点，以及所述标志物上预设一点的坐标组合成一组三维点集，将所述第二激光雷达观测到的所述标志物与地面的交点，以及所述标志物上预设一点的坐标组合成另一组三维点集；

根据两组所述三维点集计算联合标定结果。

分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据，并对两组点云数据进行地面与标志物分割，能够将地面点拟合成平面方程，将标志物点拟合成直线方程，通过平面方程和其对应的直线方程能够计算获得标志物的朝向及标志物与地面的交点，进而获得标志物上预设一点的坐标，将第一激光雷达观测到标志物与地面的交点，以及标志物上预设一点的坐标组合成一组三维点集，将第二激光雷达观测到的标志物与地面的交点，以及标志物上预设一点的坐标组合成另一组三维点集，便能够根据两组三维点集计算联合标定结果。由于该方案选用棍棒状的标志物，在采集数据时，对数据的要求较低，仅需要看到部分标志物，便能进行数据计算，适用于各种视场的激光雷达的标定，且数据的分割、拟合和计算都能够自动进行，使得标定的自动化程度更高，有利于提高标定精度。

另外，本方案是用于两个激光雷达的联合标定，在激光雷达的数量超过三个时，可以按照上述方法两两进行标定，得到全部的联合标定结果，从而使得标定更为灵活通用。

具体的，在本方案中，由于对标志物是进行直线拟合，因此，对标志物的设置没有具体要求，可以竖直设置，也可以倾斜设置。

进一步地，所述的分别对两组所述点云数据进行地面与标志物分割，并分别将地面点拟合成平面方程，将标志物点拟合成直线方程，具体包括：

分别对所述第一激光雷达和所述第二激光雷达采集的所述点云数据进行ROI过滤；

采用随机采样一致算法致(RANSAC)分割所述点云数据的地面点和标志物点；

将地面点拟合成平面方程：n^T(r-p)＝0，

将标志物点拟合成直线方程：r＝λd+q，

其中，p＝(p_x,p_y,p_z)为地面所在的平面上的一点，n＝(n_x,n_y,n_z)为平面法向量，q＝(q_x,q_y,q_z)为标志物所在的直线上的一点，d＝(d_x,d_y,d_z)为标志物所在的直线的朝向，r＝(r_x,r_y,r_z)为地面所在的平面与标志物所在的直线的交点。

通过对点云数据进行ROI过滤，能够便于选择确定两个激光雷达的感兴趣区域(ROI)，保证区域内有较多的地面点和标志物点，从而方便后续的数据分割和计算。

另外，若分割失败，则抛弃本次点云数据，待车辆位置改变后，再次尝试分割。

进一步地，根据所述平面方程和其对应的所述直线方程计算所述第一激光雷达观测到的所述标志物的朝向d₁，所述标志物与地面的交点r₁，以及所述第二激光雷达观测到的所述标志物的朝向d₂，所述标志物与地面的交点r₂，其中：

进一步地，根据所述第一激光雷达观测到的所述标志物与地面的交点r₁，计算所述标志物上的预设三维点r₁+kd₁，根据所述第二激光雷达观测到的所述标志物与地面的交点r₂，计算所述标志物上的预设三维点r₂+kd₂，其中k为预设参数，用于控制预设点的位置，k的具体数值可以根据实际测量需求进行调整，且为了保证标定的精准度，通过第一激光雷达和第二激光雷达计算的标志物上的预设点应为同一点，即两个预设三维点的k值相等。

进一步地，将当前标定获得的所述第一激光雷达观测到的所述标志物与地面的交点r₁，以及所述标志物上的预设三维点r₁+kd₁，记为x₁，将当前标定获得的所述第二激光雷达观测到的所述标志物与地面的交点r₂，以及所述标志物上的预设三维点r₂+kd₂，记为y₁；

重复标定过程，获得若干组所述第一激光雷达和所述第二激光雷达分别观测到的所述标志物与地面的交点，以及对应的所述标志物上的三维点；

将所有所述第一激光雷达观测到的所述标志物与地面的交点，以及对应的所述标志物上的三维点组成三维点集X，将所有所述第二激光雷达观测到的所述标志物与地面的交点，以及对应的所述标志物上的三维点组成三维点集Y，其中，X＝{x₁,x₂,…,x_N}，Y＝{y₁,y₂,...,y_N}，N≥4，如果N小于4，那么本次标定失败，可以亮起红色标定指示灯并启动蜂鸣器发声3秒，以提醒工作人员。

分别计算X和Y的平均值和

和

其中，

根据

和

获得协方差矩阵H，其中，

对所述协方差矩阵进行SVD分解，获得H＝UΛV^T；

计算联合标定的旋转矩阵R，其中，R＝VU^T∈O(3)；

计算平移向量t，其中，

存储所述旋转矩阵和所述平移向量，将它们和所标定的两个激光雷达的ID号保存起来，并亮起绿色标定指示灯。

由于旋转矩阵R在正交群中，即R∈O(3)，需要进一步检验R的行列式，如果det(R)＝+1，则表示标定成功，R在特殊正交群中R∈SO(3)，即不存在翻转变换；如果det(R)＝-1，则表示本次标定失败，亮起红色标定指示灯并启动蜂鸣器发声3秒。

进一步地，所述的重复标定过程，具体包括：

移动车辆，从而调整所述第一激光雷达和所述第二激光雷达的相对位置，获得预设数量的标定数据，或在预设时间段内获得若干组标定数据，如在本实施例中，可采集400组标定数据，或在预设时间段300秒内，采集若干标定数据，在其它实施例中，可以根据实际需求调整数据数量或预设时间段。

另外，本发明还提供一种激光雷达联合标定系统，包括：

第一激光雷达；

第二激光雷达；

处理器，所述处理器与所述第一激光雷达、所述第二激光雷达通信连接；

其中，所述处理器包括：

点云采集模块，用于分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据；

特征分割及拟合模块，用于分别对两组所述点云数据进行地面与标志物分割，并分别将地面点拟合成平面方程，将标志物点拟合成直线方程；

交点及朝向计算模块，用于分别根据所述平面方程和其对应的所述直线方程计算所述标志物的朝向及所述标志物与地面的交点；

三维点集存储模块，分别根据所述标志物的朝向及对应的所述标志物与地面的交点计算所述标志物上预设一点的坐标，并将所述第一激光雷达观测到所述标志物与地面的交点，以及所述标志物上预设一点的坐标组合成一组三维点集，将所述第二激光雷达观测到的所述标志物与地面的交点，以及所述标志物上预设一点的坐标组合成另一组三维点集；

联合标定计算模块，用于根据两组所述三维点集计算联合标定结果。

分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据，并对两组点云数据进行地面与标志物分割，能够将地面点拟合成平面方程，将标志物点拟合成直线方程，通过平面方程和其对应的直线方程能够计算获得标志物的朝向及标志物与地面的交点，进而获得标志物上预设一点的坐标，将第一激光雷达观测到标志物与地面的交点，以及标志物上预设一点的坐标组合成一组三维点集，将第二激光雷达观测到的标志物与地面的交点，以及标志物上预设一点的坐标组合成另一组三维点集，便能够根据两组三维点集计算联合标定结果。由于该方案选用棍棒状的标志物，在采集数据时，对数据的要求较低，仅需要看到部分标志物，便能进行数据计算，适用于各种视场的激光雷达的标定，且数据的分割、拟合和计算都能够自动进行，使得标定的自动化程度更高，有利于提高标定精度。

进一步地，所述处理器还包括ROI过滤模块，用于对所述点云数据进行ROI过滤。

通过对点云数据进行ROI过滤，能够便于选择确定两个激光雷达的感兴趣区域(ROI)，保证区域内有较多的地面点和标志物点，从而方便后续的数据分割和计算

进一步地，所述标志物竖直设置或倾斜设置。

进一步地，还包括：

标定指示灯，用于在标定成功时点亮绿灯，或在标定失败时点亮红灯；

标定蜂鸣器，用于在标定失败时进行发声。

具体的，第一激光雷达和第二激光雷达采集的数据可以通过路由器与处理器连接，在本方案中，处理器可以为工控机处理器，标定指示灯和标定蜂鸣器与路由器连接，用于在工控机处理器的控制下工作。

根据本发明提供的一种激光雷达联合标定方法及系统，分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据，并对两组点云数据进行地面与标志物分割，能够将地面点拟合成平面方程，将标志物点拟合成直线方程，通过平面方程和其对应的直线方程能够计算获得标志物的朝向及标志物与地面的交点，进而获得标志物上预设一点的坐标，将第一激光雷达观测到标志物与地面的交点，以及标志物上预设一点的坐标组合成一组三维点集，将第二激光雷达观测到的标志物与地面的交点，以及标志物上预设一点的坐标组合成另一组三维点集，便能够根据两组三维点集计算联合标定结果。由于该方案选用棍棒状的标志物，在采集数据时，对数据的要求较低，仅需要看到部分标志物，便能进行数据计算，适用于各种视场的激光雷达的标定，且数据的分割、拟合和计算都能够自动进行，使得标定的自动化程度更高，有利于提高标定精度。

附图说明

下面将以明确易懂的方式，结合附图说明优选实施方式，对本方案的上述特性、技术特征、优点及其实现方式予以进一步说明。

图1是本发明实施例的整体流程示意图；

图2是本发明实施例的标志物设置示意图；

图3是本发明另一个实施例的流程示意图；

图4是本发明实施例的标志物朝向示意图；

图5是本发明实施例的系统结构示意图；

图6是本发明实施例的处理器结构示意图。

图中标号：1-第一激光雷达；2-第二激光雷达；3-处理器；31-点云采集模块；32-特征分割及拟合模块；33-交点及朝向计算模块；34-三维点集存储模块；35-联合标定计算模块；36-ROI过滤模块；4-标定指示灯；5-标定蜂鸣器。

具体实施方式

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

为使图面简洁，各图中只示意性地表示出了与本发明相关的部分，它们并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部件，仅示意性地绘示了其中的一个，或仅标出了其中的一个。在本文中，“一个”不仅表示“仅此一个”，也可以表示“多于一个”的情形。

实施例1

本发明的一个实施例，如图1所示，本发明提供一种激光雷达联合标定方法，包括步骤：

S1、分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据。

具体的，在本方案中，由于对标志物是进行直线拟合，因此，对标志物的设置没有具体要求，可以竖直设置，也可以倾斜设置。第一激光雷达、第二激光雷达以及标志物的设置方式可以如图2所示。

S2、分别对两组点云数据进行地面与标志物分割，并分别将地面点拟合成平面方程，将标志物点拟合成直线方程。

S3、分别根据平面方程和其对应的直线方程计算标志物的朝向及标志物与地面的交点。

S4、分别根据标志物的朝向及对应的标志物与地面的交点计算标志物上预设一点的坐标。

S5、将第一激光雷达观测到标志物与地面的交点，以及标志物上预设一点的坐标组合成一组三维点集，将第二激光雷达观测到的标志物与地面的交点，以及标志物上预设一点的坐标组合成另一组三维点集。

S6、根据两组三维点集计算联合标定结果。

分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据，并对两组点云数据进行地面与标志物分割，能够将地面点拟合成平面方程，将标志物点拟合成直线方程，通过平面方程和其对应的直线方程能够计算获得标志物的朝向及标志物与地面的交点，进而获得标志物上预设一点的坐标，将第一激光雷达观测到标志物与地面的交点，以及标志物上预设一点的坐标组合成一组三维点集，将第二激光雷达观测到的标志物与地面的交点，以及标志物上预设一点的坐标组合成另一组三维点集，便能够根据两组三维点集计算联合标定结果。由于该方案选用棍棒状的标志物，在采集数据时，对数据的要求较低，仅需要看到部分标志物，便能进行数据计算，适用于各种视场的激光雷达的标定，且数据的分割、拟合和计算都能够自动进行，使得标定的自动化程度更高，有利于提高标定精度。

另外，本方案是用于两个激光雷达的联合标定，在激光雷达的数量超过三个时，可以按照上述方法两两进行标定，得到全部的联合标定结果，从而使得标定更为灵活通用。

实施例2

本发明的一个实施例，如图3所示，在实施例1的基础上，分别对两组点云数据进行地面与标志物分割，并将地面点拟合成平面方程，将分别标志物点拟合成直线方程，具体包括：

S21、分别对第一激光雷达和第二激光雷达采集的点云数据进行ROI过滤。

通过对点云数据进行ROI过滤，能够便于选择确定两个激光雷达的感兴趣区域(ROI)，保证区域内有较多的地面点和标志物点，从而方便后续的数据分割和计算。

S22、采用随机采样一致算法致(RANSAC)分割点云数据的地面点和标志物点。

S23、将地面点拟合成平面方程：n^T(r-p)＝0，将标志物点拟合成直线方程：r＝λd+q，其中，p＝(p_x,p_y,p_z)为地面所在的平面上的一点，n＝(n_x,n_y,n_z)为平面法向量，q＝(q_x,q_y,q_z)为标志物所在的直线上的一点，d＝(d_x,d_y,d_z)为标志物所在的直线的朝向，标志物朝向示意如图4所示，r＝(r_x,r_y,r_z)为地面所在的平面与标志物所在的直线的交点。

另外，若分割失败，则抛弃本次点云数据，待车辆位置改变后，再次尝试分割。

优选的，根据平面方程和其对应的直线方程计算第一激光雷达观测到的标志物的朝向d₁，标志物与地面的交点r₁，以及第二激光雷达观测到的标志物的朝向d₂，标志物与地面的交点r₂，其中：

优选的，根据第一激光雷达观测到的标志物与地面的交点r₁，计算标志物上的预设三维点r₁+kd₁，根据第二激光雷达观测到的标志物与地面的交点r₂，计算标志物上的预设三维点r₂+kd₂，其中k为预设参数，用于控制预设点的位置，k的具体数值可以根据实际测量需求调整，且为了保证标定的精准度，通过第一激光雷达和第二激光雷达计算的标志物上的预设点应为同一点，即两个预设三维点的k值相等。

进一步优选的，将当前标定获得的第一激光雷达观测到的标志物与地面的交点r₁，以及标志物上的预设三维点r₁+kd₁，记为x₁，将当前标定获得的第二激光雷达观测到的标志物与地面的交点r₂，以及标志物上的预设三维点r₂+kd₂，记为y₁。

重复标定过程，获得若干组第一激光雷达和第二激光雷达分别观测到的标志物与地面的交点，以及对应的标志物上的三维点。

优选的，重复标定过程，具体包括：移动车辆，从而调整所述第一激光雷达和所述第二激光雷达的相对位置，获得预设数量的标定数据，或在预设时间段内获得若干组标定数据，如在本实施例中，可采集400组标定数据，或在预设时间段300秒内，采集若干标定数据，在其它实施例中，可以根据实际需求调整数据数量或预设时间段。

具体的，将所有所述第一激光雷达观测到的所述标志物与地面的交点，以及对应的所述标志物上的三维点组成三维点集X，将所有所述第二激光雷达观测到的所述标志物与地面的交点，以及对应的所述标志物上的三维点组成三维点集Y，其中，X＝{x₁,x₂,…,x_N}，Y＝{y₁,y₂,…,y_N}，N≥4，如果N小于4，那么本次标定失败，可以亮起红色标定指示灯并启动蜂鸣器发声3秒，以提醒工作人员。

分别计算X和Y的平均值和

和

其中，

根据

和

获得协方差矩阵H，其中，

对协方差矩阵进行SVD分解，获得H＝UΛV^T。

计算联合标定的旋转矩阵R，其中，R＝VU^T∈O(3)。

计算平移向量t，其中，

存储旋转矩阵和平移向量，将它们和所标定的两个激光雷达的ID号保存起来，并亮起绿色标定指示灯。

由于旋转矩阵R在正交群中，即R∈O(3)，需要进一步检验R的行列式，如果det(R)＝+1，则表示标定成功，R在特殊正交群中R∈SO(3)，即不存在翻转变换；如果det(R)＝-1，则表示本次标定失败，亮起红色标定指示灯并启动蜂鸣器发声3秒。

实施例3

本发明的一个实施例，如图5和图6所示，本发明还提供一种激光雷达联合标定系统，包括：第一激光雷达(即图中的激光雷达1)1、第二激光雷达(即图中的激光雷达2)2和处理器3，处理器3与第一激光雷达1、第二激光雷达2通信连接。

具体的，第一激光雷达1和第二激光雷达2采集的数据可以通过路由器与处理器3连接，在本方案中，处理器可以为工控机处理器。

优选的，该系统还包括标定指示灯4和标定蜂鸣器5。

标定指示灯4用于在标定成功时点亮绿灯，或在标定失败时点亮红灯；标定蜂鸣器5用于在标定失败时进行发声。标定指示灯4和标定蜂鸣器5与路由器连接，用于在工控机处理器的控制下工作。

其中，处理器3包括点云采集模块31、特征分割及拟合模块32、交点及朝向计算模块33、三维点集存储模块34和联合标定计算模块35。

点云采集模块31用于分别通过第一激光雷达1和第二激光雷达2采集棍棒状的标志物附近的点云数据。

具体的，在本方案中，由于对标志物是进行直线拟合，因此，对标志物的设置没有具体要求，可以竖直设置，也可以倾斜设置。第一激光雷达、第二激光雷达以及标志物的设置方式可以如图2所示。

特征分割及拟合模块32用于分别对两组点云数据进行地面与标志物分割，并分别将地面点拟合成平面方程，将标志物点拟合成直线方程。

交点及朝向计算模块33用于分别根据平面方程和其对应的直线方程计算标志物的朝向及标志物与地面的交点。

三维点集存储模块(即图中的3维点集存储模块)34分别根据标志物的朝向及对应的标志物与地面的交点计算标志物上预设一点的坐标，并将第一激光雷达观测到标志物与地面的交点，以及标志物上预设一点的坐标组合成一组三维点集，将第二激光雷达观测到的标志物与地面的交点，以及标志物上预设一点的坐标组合成另一组三维点集。

联合标定计算模块(即图中的联合标定SVD计算模块)35用于根据两组三维点集计算联合标定结果。

优选的地，处理器3还包括ROI过滤模块36，用于对点云数据进行ROI过滤。

通过对点云数据进行ROI过滤，能够便于选择确定两个激光雷达的感兴趣区域(ROI)，保证区域内有较多的地面点和标志物点，从而方便后续的数据分割和计算。

分别通过第一激光雷达1和第二激光雷达2采集棍棒状的标志物附近的点云数据，并对两组点云数据进行地面与标志物分割，能够将地面点拟合成平面方程，将标志物点拟合成直线方程，通过平面方程和其对应的直线方程能够计算获得标志物的朝向及标志物与地面的交点，进而获得标志物上预设一点的坐标，将第一激光雷达1观测到标志物与地面的交点，以及标志物上预设一点的坐标组合成一组三维点集，将第二激光雷达2观测到的标志物与地面的交点，以及标志物上预设一点的坐标组合成另一组三维点集，便能够根据两组三维点集计算联合标定结果。由于该方案选用棍棒状的标志物，在采集数据时，对数据的要求较低，仅需要看到部分标志物，便能进行数据计算，适用于各种视场的激光雷达的标定，且数据的分割、拟合和计算都能够自动进行，使得标定的自动化程度更高，有利于提高标定精度。

具体的，联合标定的具体计算方式如实施例2所述，在此不累赘叙述。

另外，本方案是用于两个激光雷达的联合标定，在激光雷达的数量超过三个时，可以按照上述方法两两进行标定，得到全部的联合标定结果，从而使得标定更为灵活通用。

应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种激光雷达联合标定方法，其特征在于，包括步骤：

分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据；

分别对两组所述点云数据进行地面与标志物分割，并分别将地面点拟合成平面方程，将标志物点拟合成直线方程；

分别根据所述平面方程和其对应的所述直线方程计算所述标志物的朝向及所述标志物与地面的交点；

分别根据所述标志物的朝向及对应的所述标志物与地面的交点计算所述标志物上预设一点的坐标；

将所述第一激光雷达观测到所述标志物与地面的交点，以及所述标志物上预设一点的坐标组合成一组三维点集，将所述第二激光雷达观测到的所述标志物与地面的交点，以及所述标志物上预设一点的坐标组合成另一组三维点集；

根据两组所述三维点集计算联合标定结果。

2.根据权利要求1所述的一种激光雷达联合标定方法，其特征在于，所述的分别对两组所述点云数据进行地面与标志物分割，并分别将地面点拟合成平面方程，将标志物点拟合成直线方程，具体包括：

分别对所述第一激光雷达和所述第二激光雷达采集的所述点云数据进行ROI过滤；

采用随机采样一致算法分割所述点云数据的地面点和标志物点；

将地面点拟合成平面方程：n^T(r-p)＝0，

将标志物点拟合成直线方程：r＝λd+q，

其中，p＝(p_x,p_y,p_z)为地面所在的平面上的一点，n＝(n_x,n_y,n_z)为平面法向量，q＝(q_x,q_y,q_z)为标志物所在的直线上的一点，d＝(d_x,d_y,d_z)为标志物所在的直线的朝向，r＝(r_x,r_y,r_z)为地面所在的平面与标志物所在的直线的交点。

3.根据权利要求2所述的一种激光雷达联合标定方法，其特征在于：

根据所述平面方程和其对应的所述直线方程计算所述第一激光雷达观测到的所述标志物的朝向d₁，所述标志物与地面的交点r₁，以及所述第二激光雷达观测到的所述标志物的朝向d₂，所述标志物与地面的交点r₂。

4.根据权利要求3所述的一种激光雷达联合标定方法，其特征在于：根据所述第一激光雷达观测到的所述标志物与地面的交点r₁，计算所述标志物上的预设三维点r₁+kd₁，根据所述第二激光雷达观测到的所述标志物与地面的交点r₂，计算所述标志物上的预设三维点r₂+kd₂，其中k为预设参数。

5.根据权利要求4所述的一种激光雷达联合标定方法，其特征在于：

将当前标定获得的所述第一激光雷达观测到的所述标志物与地面的交点r₁，以及所述标志物上的预设三维点r₁+kd₁，记为x₁，将当前标定获得的所述第二激光雷达观测到的所述标志物与地面的交点r₂，以及所述标志物上的预设三维点r₂+kd₂，记为y₁；

重复标定过程，获得若干组所述第一激光雷达和所述第二激光雷达分别观测到的所述标志物与地面的交点，以及对应的所述标志物上的三维点；

将所有所述第一激光雷达观测到的所述标志物与地面的交点，以及对应的所述标志物上的三维点组成三维点集X，将所有所述第二激光雷达观测到的所述标志物与地面的交点，以及对应的所述标志物上的三维点组成三维点集Y，其中，X＝{x₁,x₂,…,x_N}，Y＝{y₁,y₂,…,y_N}，N≥4，

分别计算X和Y的平均值和

和

其中，

根据

和

获得协方差矩阵H，其中，

对所述协方差矩阵进行SVD分解，获得H＝UΛV^T；

计算联合标定的旋转矩阵R，其中，R＝VU^T∈O(3)；

计算平移向量t，其中，

存储所述旋转矩阵和所述平移向量。

6.根据权利要求5所述的一种激光雷达联合标定方法，其特征在于，所述的重复标定过程，具体包括：

移动车辆，从而调整所述第一激光雷达和所述第二激光雷达的相对位置，获得预设数量的标定数据，或在预设时间段内获得若干组标定数据。

7.一种激光雷达联合标定系统，其特征在于，包括：

第一激光雷达；

第二激光雷达；

处理器，所述处理器与所述第一激光雷达、所述第二激光雷达通信连接；

其中，所述处理器包括：

点云采集模块，用于分别通过第一激光雷达和第二激光雷达采集棍棒状的标志物附近的点云数据；

特征分割及拟合模块，用于分别对两组所述点云数据进行地面与标志物分割，并分别将地面点拟合成平面方程，将标志物点拟合成直线方程；

交点及朝向计算模块，用于分别根据所述平面方程和其对应的所述直线方程计算所述标志物的朝向及所述标志物与地面的交点；

三维点集存储模块，用于分别根据所述标志物的朝向及对应的所述标志物与地面的交点计算所述标志物上预设一点的坐标，并将所述第一激光雷达观测到所述标志物与地面的交点，以及所述标志物上预设一点的坐标组合成一组三维点集，将所述第二激光雷达观测到的所述标志物与地面的交点，以及所述标志物上预设一点的坐标组合成另一组三维点集；

联合标定计算模块，用于根据两组所述三维点集计算联合标定结果。

8.根据权利要求7所述的一种激光雷达联合标定系统，其特征在于，所述处理器还包括ROI过滤模块，用于对所述点云数据进行ROI过滤。

9.根据权利要求7所述的一种激光雷达联合标定系统，其特征在于：所述标志物竖直设置或倾斜设置。

10.根据权利要求7所述的一种激光雷达联合标定系统，其特征在于，还包括：

标定指示灯，用于在标定成功时点亮绿灯，或在标定失败时点亮红灯；

标定蜂鸣器，用于在标定失败时进行发声。

Images