一种用于双桥转向矿卡的多雷达外参快速标定方法及装置

1.本发明属于传感器和激光雷达建图与定位领域，涉及一种用于双桥转向矿卡的多雷达外参快速标定方法及装置，具体涉及一种适用于无驾驶室的双桥转向矿卡的多雷达外参快速标定方法、装置及存储介质。


背景技术：

2.矿用卡车体型巨大，适用于其的感知系统需要考虑车身总体范围内的视野尽可能大、死角尽可能小的问题，以保障自动驾驶过程中的安全性。激光雷达具有精度高、全天候工作的特点，考虑到激光雷达的扫描范围，通常在车身周围安装多个激光雷达来实现对周边环境的全覆盖感知。然而，由于各个激光雷达设备的安装位置不同，各个激光雷达采集到的数据存在一定偏移。因此，对各个激光雷达的外部安装参数(外参)进行准确标定，是同时使用多个激光雷达必不可少的步骤。
3.目前，对激光雷达进行外参标定主要采用传统的人工标定法或者标定物测量法。其中，人工标定采用手动测量或仪器测量的方式，测量激光雷达坐标系和参考坐标系之间的变换关系，这种方法虽然原理简单，但是对操作者的操作精细度以及测量仪器的精度要求高，因此可推广性不高，且标定精度不稳定，会随着操作者的水平高低变化。标定物测量法通过对特定标定物的匹配来确定激光雷达设备的外参，这种方法也存在一定弊端：由于激光雷达对于不同颜色和材质的物体反射率不一样，标定物的查找存在一定困难，同时移动标定物的过程中需要大量人力参与，以及标定精度依赖于其他传感器与标定物之间的参数，因此这种标定方法的标定效率低、精度也不高。
4.中国专利cn114047487a，名称为《雷达和车体的外参标定方法、装置、电子设备和存储介质》提供了一种标定车载激光雷达外参的方法，其通过独立于车体的辅助雷达和标定板，将参考点在辅助雷达坐标系下的坐标转换成车载雷达坐标系下的坐标，并根据参考点在车体坐标系下的位置坐标，标定车载雷达坐标系到车体坐标系的外部参数。该方法仅仅降低了对标定场地的要求，并且在需标定的雷达个数较多时，操作简便性也未得到提高。
5.中国专利cn113702931a，名称为《一种车载雷达的外参标定方法、装置及存储介质》提供了一种利用多种雷达的特性，利用第一对象集合信息和第二对象集合信息，在线、实时、自动地标定雷达之间的外参的方法。该方法需要多个激光雷达之间有足够的重复视野，并且可同时标定的雷达数目较少。
6.对于无驾驶室的双桥转向矿卡，其在实际运行时可在前向行驶和后向行驶之间切换，要获得全面的感知视野，其前后需要相同能力的激光雷达感知设备。显然这些激光雷达不可能同时存在重复的感知视野，而如果采用两两标定的方法效率又不高，并且由于多次标定，还会累积误差，无法得到理想精度。


技术实现要素：

7.无驾驶室的双桥转向矿卡需要多个激光雷达来感知周围环境，而目前的标定方法
难以快速准确的得到多个雷达之间的安装参数。为此，本发明提供了一种多激光雷达外参快速标定方法，能够对无驾驶室的双桥转向矿车上的多个激光雷达快速标定，保障融合点云的坐标统一性，为后续的感知任务提供精准的位置信息。
8.为实现上述目的，本发明提供了一种用于双桥转向矿卡的多雷达外参快速标定方法，包括如下步骤：
9.s1：当矿卡处于标定场景时，采集矿卡车体上的多个激光雷达的点云数据并测量各激光雷达的离地安装高度，将位于矿卡车体中部的激光雷达作为主激光雷达；
10.s2：基于各激光雷达的离地安装高度，将所述点云数据分割为地面点云数据和非地面点云数据，并对所述非地面点云数据添加语义类别信息；
11.s3：对所述地面点云数据进行平面参数拟合，获得地面点云平面参数，基于所述地面点云平面参数构建各激光雷达之间的多个自由度约束关系；
12.s4：基于所述主激光雷达的带语义类别信息的非地面点云数据，构建点云地图；
13.s5：基于所构建的各激光雷达之间的多个自由度约束关系和所构建的点云地图，对步骤s1采集到的各激光雷达的点云数据进行点云地图配准，得到各激光雷达的坐标系相对所述点云地图的坐标系的位姿，进而得到各激光雷达的坐标系的6自由度位姿关系；
14.s6：基于各激光雷达的坐标系的6自由度位姿关系计算矿卡车体坐标系的原点位置，并求出各激光雷达相对矿卡车体坐标系的外参。
15.进一步，步骤s1中，采用使矿卡原地转向的方式，分别采集矿卡车体前向和后向上的激光雷达的点云数据。
16.进一步，步骤s2具体过程为：
17.根据各激光雷达的离地安装高度，将各激光雷达的点云数据分割为地面点云数据和非地面点云数据，并将非地面点云数据进行语义分割，使得非地面点云数据中的每个点都具有除空间几何信息以外的语义类别信息，生成多个类比点云。
18.进一步，步骤s3具体过程为：
19.利用ransac方法对各激光雷达的地面点云数据进行平面参数拟合，获得各激光雷达的地面点云平面参数，构建各激光雷达在空间位置中的多个自由度约束关系。
20.进一步，步骤s4具体过程为：
21.利用所述主激光雷达的非地面点云数据，进行线面特征提取，并在特征匹配阶段加入语义类别信息约束，计算相邻帧点云的位姿关系，并叠加构建点云地图。
22.进一步，步骤s5具体过程为：
23.基于所构建的各激光雷达之间的多个自由度约束关系，在所构建的点云地图上对各激光雷达的点云数据进行点云地图匹配，不断迭代后得到各激光雷达相对所述点云地图的定位参数，最终获得各激光雷达的坐标系相对所述点云地图的坐标系的x,y,z,roll,pitch,yaw信息，进而得到各激光雷达的坐标系的6自由度位姿关系；其中x,y,z表示各激光雷达的坐标系相对所述点云地图的坐标系的x轴，y轴，z轴方向的偏移量，roll表示其他各激光雷达的坐标系相对所述点云地图的坐标系绕x轴旋转角度，pitch表示各激光雷达的坐标系相对所述点云地图的坐标系绕y轴旋转角度，yaw表示各激光雷达的坐标系相对所述点云地图的坐标系绕z轴旋转角度。
24.进一步，步骤s6具体过程为：
25.基于各激光雷达的坐标系的6自由度位姿关系，计算出矿卡车体坐标系原点位置，并将各激光雷达的坐标系的6自由度位姿关系转换为矿卡车体坐标系下的参数，得到各激光雷达相对矿卡车体坐标系的外参。
26.本发明还提供了一种用于双桥转向矿卡的多雷达外参快速标定装置，包括：
27.采集模块，用于采集矿卡车体上的多个激光雷达的点云数据；
28.点云分割模块，用于将所述采集模块采集的点云数据分割为地面点云数据和非地面点云数据；同时用于对非地面点云数据添加语义类别信息；
29.平面拟合模块，用于对各激光雷达的地面点云数据进行平面参数拟合，得到地面点云平面参数后构建各激光雷达之间的多个自由度约束关系，并传输给重定位模块；
30.建图模块，用于基于主激光雷达的带语义类别信息的非地面点云数据构建点云地图，所述主激光雷达为位于矿卡车体中部的激光雷达；
31.重定位模块，用于接收所述平面拟合模块构建的各激光雷达之间的多个自由度约束，对来自所述采集模块的各激光雷达的点云数据进行点云地图匹配，得到各激光雷达的坐标系的6自由度位姿关系；
32.外参优化模块，用于基于各激光雷达的坐标系的6自由度位姿关系，计算出矿卡车体坐标系的原点位置，并求出各激光雷达相对矿卡车体坐标系的外参。
33.本发明还提供了一种电子设备，包括：
34.存储器、处理器和存储在所述存储器上并可在所述处理器上运行的程序，所述处理器执行所述程序，以实现上述方法。
35.本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质上存储有程序，该程序被处理器执行，以实现上述方法。
36.本发明的有益效果：
37.1)本发明无需其它标定设备，不需要和其它传感器相互配合，只依靠激光雷达的自身特性、就能实现室内室外环境的多激光雷达外参标定；
38.2)本发明结合slam建图原理，可以很好的解决多个激光雷达之间无较多重复视野问题，并能够同时对多个激光雷达进行快速准确标定，大幅提高标定效率；
39.3)本发明利用地面点云数据信息为定位提供多个自由度约束，在一定程度上提高了定位精度和定位速度；
40.4)本发明最终将多个激光雷达的外部参数标定到矿卡车体中间，相当于在车体中间安装了一个能全视角范围感知的虚拟雷达，无前后向感知切换步骤，更加符合双桥转向无明显前后向区别的特征。
附图说明
41.图1为本发明实施例1的双桥转向矿卡的多雷达外参标定方法流程图；
42.图2为本发明实施例2的双桥转向矿卡的多雷达外参标定装置模块图。
具体实施方式
43.下面结合附图和实施例对本发明作进一步详细说明。
44.实施例1
45.参照图1，本实施例的双桥转向矿卡的多雷达外参快速标定方法包括如下步骤：
46.步骤1：在无驾驶室的双桥转向矿卡的前向和后向各安装3个激光雷达，并测量6个激光雷达的离地安装高度。
47.本实施例将安装在矿卡车体前向和后向之间的中间位置处的2个激光雷达作为主激光雷达，最终目的是要将6个激光雷达的点云数据变换到以这2个主激光雷达的中心位置为原点、车头方向为x
车
轴、车体正左方向为y
车
轴，车体正上方向为z
车
轴的车体坐标系中。
48.步骤2：将双桥转向矿卡置于合适的标定场景中，采集6个激光雷达的点云数据。由于双桥转向的前后向的感知能力相同，特别地在采集数据时为了尽量保证前后向环境差异不大，可以采用原地转向的方式分别采集矿卡车体前向和后向上的激光雷达的点云数据，保障前后向激光雷达采集数据的一致性。
49.步骤3：基于各激光雷达的离地安装高度，将激光雷达的点云数据分割为地面点云数据和非地面点云数据；利用rangnet++深度学习框架，将非地面点云数据进行球映射，将3d点云映射为2d的深度图像，映射公式如下：
[0050][0051]
式中：u,v代表像素坐标系坐标；x
′
,y
′
,z
′
为激光雷达中3d点云的坐标值；r为点到激光雷达的欧式距离；w,h为深度图像的宽度和高度；f为激光雷达垂直方向的总的视角范围，f
up
为垂直方向上的上视角范围。
[0052]
通过映射公式(1)能够将激光雷达的点云数据中的所有点都映射到深度图像中的每个像素点上。由于为提高卷积速度，深度图像的宽度和高度w,h不能设定太大，导致深度图像上的任一像素点会对应原始非地面点云数据中的多个点。在每个像素位置设置5个通道，每个通道值分别对应3d点的3d坐标(x
′
,y
′
,z
′
)、反射率、距离，最终形成[5
×h×
w]的深度图像。之后对映射后的深度图像进行rangnet53网络训练，根据训练结果得到每个像素点的语义标签，每个像素点对应的多个3d点都被赋予相同的语义标签，然后将2d像素点根据保存的通道值重建为3d点云。最后针对3d点云中被遮挡物体(障碍物)的边缘位置，进行后处理修正，即根据每个3d点的邻居点类别来决定该3d点的类别，最终可以将非地面点云数据的每个点划分出不同的语义类别信息，生成多个类比点云，如行人点云，车辆点云，交通标志点云等等。
[0053]
步骤4：利用ransac平面拟合方法对各个激光雷达的地面点云数据进行平面参数拟合，获得各激光雷达的地面点云参数，并据此得到各激光雷达之间在空间位置上的坐标变换关系方程，构建各激光雷达之间的多个自由度约束关系。
[0054]
在本实施例中，利用ransac平面拟合方法对各激光雷达的地面点云数据进行平面参数拟合，具体包括如下步骤：
[0055]
1)在地面点云数据中集中随机选择3个点作为初始点，可以计算出地面点云的地面平面模型方程：
[0056]
ax'+by'+cz'+d＝0
ꢀꢀꢀꢀ
(2)
[0057]
其中，a、b、c、d表示地面平面模型中的系数。
[0058]
2)计算地面点云数据中的所有点到该地面平面模型的欧式距离di＝|ax
′i+by
′i+cz
′i+d|，其中x
′i,y
′i,z
′i表示第i点的坐标。设定距离阈值d
t
，如果计算出某点的欧式距离di≤d
t
，则该点认为是地面平面模型内的点，否则认为该点为地面平面模型外的点，即该点不在当前地面平面模型内。
[0059]
3)重复步骤2)，记录当前地面平面模型内点的个数，选择内点数量最多的地面平面模型作为最优地面平面模型，输出最佳模型参数作为最终的地面平面模型参数估计值。
[0060]
4)构建地面约束：
[0061]
首先求解出各个激光雷达的点云的最终的地面平面模型参数估计值，并根据该模型参数估计值确定约束方程如下：
[0062]
将地面平面模型方程(2)写为矩阵形式，即：
[0063][0064]
对于任意两个激光雷达的坐标系(x,y,z)，假设其之间的变换参数分别为tx,ty,tz,θr,θy,θ
p
，其中tx为x轴方向的平移量，ty为y轴方向的平移量，tz为z轴方向的平移量，θr为两个激光雷达的翻滚角差值，θy为两个激光雷达的偏航角差值，θz为两个激光雷达俯仰角差值。
[0065]
根据刚体表换特性，两个激光雷达坐标系中任意两个对应点(x
′
1,y
′
1,z
′
1)和(x
′
2,y
′
2,z
′
2)之间的对应关系为
[0066][0067]
根据地面平面模型方程的矩形形式(3)得到：
[0068][0069]
将式(4)代入地面平面模型方程的矩形形式(3)中，得到：
[0070]
[0071]
所以
[0072][0073]
式中(a
1 b
1 c
1 d1)和(a
2 b
2 c
2 d2)已知，tx,ty,tz,θr,θy,θ
p
为未知量。
[0074]
由上述方程(6)可以确定tx,ty,tz,θr,θy,θ
p
这几个未知量之间的约束关系，即可确定两个激光雷达坐标系的多自由度约束关系，进而可以确定多个激光雷达的多自由度约束关系。
[0075]
步骤5：利用2个主激光雷达的带语义信息的非地面点云数据，根据非地面点云数据中的大曲率点作为角点，小曲率点作为平面点进行线面特征提取，并根据语义信息进行上下帧点云配准，即在进行相邻帧对应特征点匹配时，先判断相邻帧对应特征点是否具有相同语义信息才进行匹配，如果语义信息匹配失败，则该帧特征点会在上一帧匹配失败的特征点邻近特征点进行语义信息匹配，如果找到语义相同特征点，则匹配成功，计算相邻帧点云的位姿关系，并叠加构建点云地图。
[0076]
其中，非地面点云数据中点的曲率计算公式如下：
[0077][0078]
式中：ρi为第i点的曲率，xi为第i点的坐标(x'i,y'i,z'i)，xj为第j点的坐标，m为邻近点个数，为设定值。
[0079]
步骤6：基于所构建的各激光雷达之间的多个自由度约束关系，对各激光雷达的实时点云数据在所构建的点云地图进行点云地图配准，不断迭代后得到满足精度要求的定位参数，得到各激光雷达坐标系的6自由度位姿。可以利用如下精度评估方法进行精度评估：
[0080]
假设初次配准后的实时点云中任一点a坐标为(x
′a,y
′a,z
′a)，点云地图中找到的对应特征点b坐标为(x
′b,y
′b,z
′b)，ab之间的欧式距离d
ab
为
[0081][0082]
之后计算所有对应点之间的距离，求欧式距离和作为评估指标，n为激光雷达的点云总个数。如果d满足设定阈值，则满足精度要求，否则对配准后的实时点云继续进行迭代配准。
[0083]
步骤7：基于各激光雷达坐标系的位姿关系，计算出矿卡车体坐标系原点位置，并将各激光雷达的位姿关系转换为矿卡车体坐标系下的参数，得到各激光雷达相对矿卡车体坐标系的外参。具体过程如下：
[0084]
通过步骤6计算出各个激光雷达之间的6自由度位姿，根据两个主激光雷达的位姿(x1,y1,z1,roll1,pitch1,yaw1)和(x2,y2,z2,roll2,pitch2,yaw2)，计算出车体坐标系原点为
将其余各激光雷达位姿转变到该车体坐标系中，得到各个激光雷达相对车体坐标系的外参，最终成功标定。
[0085]
实施例2：
[0086]
如图2所示，本实施例的双桥转向矿卡的多雷达外参快速标定装置包括采集模块、分割模块、平面拟合模块、建图模块、重定位模块和外参优化模块。其中：
[0087]
采集模块，用于采集矿卡车体上的多个激光雷达的点云数据。本实施例2在无驾驶室的双桥转向矿卡的前向和后向各安装3个激光雷达，采集模块通过采集6个激光雷达的离线数据，用于后续的离线建图标注。
[0088]
点云分割模块，用于将采集模块采集的点云数据分割为地面点云数据和非地面点云数据；同时用于对非地面点云数据添加语义信息。具体地，点云分割模块通过使用6个激光雷达的离地安装高度信息，将采集模块采集的原始点云数据分割为地面点云数据和非地面点云数据，并对非地面点云数据使用rangenet++网络结构，将其进行球坐标投影、网络训练、重映射、语义分割处理。使得非地面点云数据不仅具有x，y，z等空间几何信息，还具有能表示每个点的所代表的具体含义的语义信息，即能获取每个点是属于行人点云还是车辆点云等，对后续的建图阶段的前后帧配准以及重定位阶段的定位都具有重要作用。
[0089]
平面拟合模块，用于对各激光雷达的地面点云数据进行平面参数拟合，得到地面点云平面参数后构建所述多个激光雷达之间的多个自由度约束关系，并传输给重定位模块。具体地，平面拟合模块通过使用ransac方法将地面点云进行平面拟合，得到各个激光雷达中地面点云的地面平面模型参数，由于各个激光雷达之间的坐标变换为刚体变换，在已知地面平面模型方程的情况下，即可根据地面的不同地面平面模型参数，确定各个激光雷达坐标系之间的多个自由度约束关系，对后续的定位具有一定的矫正作用。
[0090]
建图模块，用于基于主激光雷达的带语义信息的非地面点云数据构建点云地图。具体地，建图模块基于loam建图原理，基于主激光雷达的非地面点云数据，提取出其角点和平面点作为特征点，在进行里程计计算时，加入语义信息作为先验信息，前后帧特征点的匹配必须具有相同的语义信息，使得能够快速准确的找出对应的特征点，对减小里程计误差有重要作用。
[0091]
重定位模块，用于接收平面拟合模块构建的6个激光雷达之间的多个自由度约束，对来自采集模块的多个激光雷达的点云数据进行点云地图配准，得到6个激光雷达坐标系的6自由度位姿关系。具体地，重定位模块根据各个激光雷达之间的即时点云数据在地图上进行icp匹配，同时也加入语义信息作为辅助，能够得到各个激光雷达在点云地图上的相对位姿，即能确定各激光雷达坐标系相对点云地图坐标系的x,y,z,roll,pitch,yaw信息，最终得到各激光雷达坐标系的6自由度位姿关系；其中x,y,z表示各激光雷达坐标系相对点云地图坐标系的x轴，y轴，z轴方向的偏移量，roll表示各激光雷达坐标系相对点云地图坐标系绕x轴旋转角度，pitch表示各激光雷达坐标系相对点云地图坐标系绕y轴旋转角度，yaw表示各激光雷达坐标系相对点云地图坐标系绕z轴旋转角度。
[0092]
外参优化模块，用于基于6个激光雷达之间的6自由度位姿关系，计算出矿卡车体坐标系的原点位置，并求出各激光雷达相对矿卡车体坐标系的外参。外参优化模块主要是将各个激光雷达的相对位姿变换到相对车体坐标系的位姿。由于无驾驶室的双桥转向矿车
在行驶时并不具有明显的前后端区分，将激光雷达数据标定到以车体中心为坐标原点的车体坐标系中，此时的感知数据不再具有明显的前后向区别，从而能够在双桥转向矿卡进行行驶方向切换时，感知障碍物距离快速切换，满足无驾驶室的双桥转向矿卡的行驶特征。
[0093]
对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以对本发明的实施例作出若干变型和改进，这些都属于本发明的保护范围。