场端激光雷达的标定方法、装置、存储介质及终端设备与流程

1.本发明涉及自动驾驶及激光雷达技术领域，尤其涉及一种场端激光雷达的标定方法、装置、计算机可读存储介质及终端设备。


背景技术：

2.随着车辆自动驾驶技术的迅猛发展，激光雷达(lightdetection and ranging，lidar)作为一种主要的感知设备被广泛应用于自动驾驶系统中，在交通运输、国防安全等领域有着重要的应用需求。
3.在实际应用中，激光雷达可以布置在车端，也可以布置在场端，但是均需要对其进行位姿标定，其中，激光雷达的外部参数标定的作用，主要是将激光雷达检测到的目标信息坐标系转换到统一的车辆坐标系中，外部参数包括坐标系空间旋转参数(yaw angle、pitchangle、roll angle)以及坐标系空间平移矩阵参数(x、y、z)，包括激光雷达在内的所有传感器的参数标定一直都是自动驾驶领域功能实现的基础，其重要性不言而喻。
4.但是，目前常用的对于激光雷达的外部参数进行标定的方法，大都具有复杂的标定流程，并且标定的精度较低。


技术实现要素：

5.本发明实施例所要解决的技术问题在于，提供一种场端激光雷达的标定方法、装置、计算机可读存储介质及终端设备，能够简化激光雷达的外参标定流程，并且提高标定精度。
6.为了解决上述技术问题，本发明实施例提供了一种场端激光雷达的标定方法，包括：
7.获取场端待标定的激光雷达的ros点云数据，并进行显示；
8.根据点云显示结果，选取地面点云；其中，所述地面点云中包括至少三个地面雷达点；
9.获取每一个地面雷达点在世界坐标系下的位姿与在雷达坐标系下的位姿对应的第一残差方程；
10.根据预设的旋转顺序提取欧拉角和旋转矩阵的对应关系；
11.根据所述对应关系对每一个地面雷达点对应的第一残差方程进行处理，获得每一个地面雷达点对应的第二残差方程；
12.根据所有地面雷达点对应的第二残差方程进行残差最小化优化处理，获得优化变量；
13.根据所述优化变量获得所述激光雷达的外参标定结果。
14.进一步地，第i个地面雷达点对应的第一残差方程表示为：
15.rz(i)＝[pw]z＝[rp
l
t]z；其中，rz(i)表示第i个地面雷达点在世界坐标系下的高度，pw表示第i个地面雷达点在世界坐标系下的位姿，r表示旋转矩阵，p
l
表示第i个地面雷达
点在雷达坐标系下的位姿，t表示平移矩阵，i＞0。
[0016]
进一步地，所述根据预设的旋转顺序提取欧拉角和旋转矩阵的对应关系，具体包括：
[0017]
采用xyz顺规提取欧拉角和旋转矩阵的对应关系；其中，所述对应关系表示为r
x
(ψ)表示绕x轴的旋转结果，ry(θ)表示绕y轴的旋转结果，表示绕z轴的旋转结果。
[0018]
进一步地，第i个地面雷达点对应的第二残差方程表示为：
[0019]rz
(i)＝-x
l
sinθ y
l
sinψcosθ z
l
cosψcosθ z
t
；其中，p
l
＝(x
l
,y
l
,z
l
)表示第i个地面雷达点在雷达坐标系下的位姿，z
t
表示z轴平移参数。
[0020]
进一步地，所述根据所有地面雷达点对应的第二残差方程进行残差最小化优化处理，获得优化变量，具体包括：
[0021]
获取每一个地面雷达点对应的第二残差方程的雅克比矩阵；
[0022]
根据最小二乘法对所有地面雷达点对应的雅克比矩阵进行求解，获得所述优化变量。
[0023]
进一步地，第i个地面雷达点对应的第二残差方程的雅克比矩阵表示为：
[0024][0025]
进一步地，所述优化变量包括θ、ψ和z
t
；则，所述根据所述优化变量获得所述激光雷达的外参标定结果，具体包括：
[0026]
根据所述优化变量θ、ψ和z
t
获得所述激光雷达的外部参数roll(θ)、pitch(ψ)和z(z
t
)。
[0027]
为了解决上述技术问题，本发明实施例还提供了一种场端激光雷达的标定装置，包括：
[0028]
点云数据处理模块，用于获取场端待标定的激光雷达的ros点云数据，并进行显示；
[0029]
地面点云选择模块，用于根据点云显示结果，选取地面点云；其中，所述地面点云中包括至少三个地面雷达点；
[0030]
第一残差获取模块，用于获取每一个地面雷达点在世界坐标系下的位姿与在雷达坐标系下的位姿对应的第一残差方程；
[0031]
对应关系获取模块，用于根据预设的旋转顺序提取欧拉角和旋转矩阵的对应关系；
[0032]
第二残差获取模块，用于根据所述对应关系对每一个地面雷达点对应的第一残差方程进行处理，获得每一个地面雷达点对应的第二残差方程；
[0033]
变量优化模块，用于根据所有地面雷达点对应的第二残差方程进行残差最小化优
化处理，获得优化变量；
[0034]
标定结果获取模块，用于根据所述优化变量获得所述激光雷达的外参标定结果。
[0035]
本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序；其中，所述计算机程序在运行时控制所述计算机可读存储介质所在的设备执行上述任一项所述的场端激光雷达的标定方法。
[0036]
本发明实施例还提供了一种终端设备，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器在执行所述计算机程序时实现上述任一项所述的场端激光雷达的标定方法。
[0037]
与现有技术相比，本发明实施例提供了一种场端激光雷达的标定方法、装置、计算机可读存储介质及终端设备，通过获取场端待标定的激光雷达的ros点云数据，并进行显示；根据点云显示结果，选取地面点云；其中，所述地面点云中包括至少三个地面雷达点；获取每一个地面雷达点在世界坐标系下的位姿与在雷达坐标系下的位姿对应的第一残差方程；根据预设的旋转顺序提取欧拉角和旋转矩阵的对应关系；根据所述对应关系对每一个地面雷达点对应的第一残差方程进行处理，获得每一个地面雷达点对应的第二残差方程；根据所有地面雷达点对应的第二残差方程进行残差最小化优化处理，获得优化变量；根据所述优化变量获得所述激光雷达的外参标定结果；从而能够简化激光雷达的外参标定流程，并且提高标定精度。
附图说明
[0038]
图1是本发明提供的一种场端激光雷达的标定方法的一个优选实施例的流程图；
[0039]
图2是本发明提供的一种场端激光雷达的标定装置的一个优选实施例的结构框图；
[0040]
图3是本发明提供的一种终端设备的一个优选实施例的结构框图。
具体实施方式
[0041]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本技术领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0042]
本发明实施例提供了一种场端激光雷达的标定方法，参见图1所示，是本发明提供的一种场端激光雷达的标定方法的一个优选实施例的流程图，所述方法包括步骤s11至步骤s17：
[0043]
步骤s11、获取场端待标定的激光雷达的ros点云数据，并进行显示；
[0044]
步骤s12、根据点云显示结果，选取地面点云；其中，所述地面点云中包括至少三个地面雷达点；
[0045]
步骤s13、获取每一个地面雷达点在世界坐标系下的位姿与在雷达坐标系下的位姿对应的第一残差方程；
[0046]
步骤s14、根据预设的旋转顺序提取欧拉角和旋转矩阵的对应关系；
[0047]
步骤s15、根据所述对应关系对每一个地面雷达点对应的第一残差方程进行处理，
获得每一个地面雷达点对应的第二残差方程；
[0048]
步骤s16、根据所有地面雷达点对应的第二残差方程进行残差最小化优化处理，获得优化变量；
[0049]
步骤s17、根据所述优化变量获得所述激光雷达的外参标定结果。
[0050]
具体的，本发明实施例适用于布置在场端的激光雷达的外参标定，并且默认激光雷达的外部参数为x＝0，y＝0，z＝0，roll＝0，pitch＝0，yaw＝0；在将场端待标定的激光雷达固定安装之后，首先，启动roscore，运行激光雷达驱动，通过激光雷达扫描地面，获取激光雷达的ros点云数据，并在rviz工具中进行显示；接着，根据获得的ros点云数据的点云显示结果，选取地面点云，所选择的地面点云中包括至少三个地面雷达点；然后，获取每一个地面雷达点在世界坐标系下的位姿与在雷达坐标系下的位姿所对应的第一残差方程，并根据预先设置的旋转顺序提取欧拉角和旋转矩阵的对应关系，以根据获得的欧拉角和旋转矩阵的对应关系对每一个地面雷达点所对应的第一残差方程进行相应处理，对应获得每一个地面雷达点所对应的第二残差方程；最后，根据获得的所有地面雷达点对应的第二残差方程进行残差最小化优化处理，即最小化残差相应获得优化变量，其中，rz(i)表示第i个地面雷达点在世界坐标系下的高度，n表示所选择的地面点云中包括的地面雷达点的个数，n≥3，从而根据获得的优化变量获得激光雷达的外参标定结果。
[0051]
需要说明的是，本发明实施例实际可以通过标定程序进行实现，在启动roscore和标定程序之后，根据点云显示结果，可以通过使用工具进行地面点云的手动选择，例如，使用点云选取工具selectpointcloudpublishaction，选择相应的地面点云的发布名称，并点击selectpointcoudpublishaction按钮，将选取的点云以ros消息的形式发送给标定程序，即可得到标定结果；此外，标定结果可以通过终端进行查看，还可以在rviz上可视化标定后的点云。
[0052]
可以理解的，在使用工具进行地面点云的手动选择时，只需要选择一定数量的是地面的点云就可以，这是能够将点云和地面对应起来的前提，并且通过地面点云的手动选择，能够人工过滤地面点，使得过滤地面点的精度更高。
[0053]
本发明实施例所提供的一种场端激光雷达的标定方法，通过获取场端待标定的激光雷达的ros点云数据，并进行显示；根据点云显示结果，选取地面点云；其中，所述地面点云中包括至少三个地面雷达点；获取每一个地面雷达点在世界坐标系下的位姿与在雷达坐标系下的位姿对应的第一残差方程；根据预设的旋转顺序提取欧拉角和旋转矩阵的对应关系；根据所述对应关系对每一个地面雷达点对应的第一残差方程进行处理，获得每一个地面雷达点对应的第二残差方程；根据所有地面雷达点对应的第二残差方程进行残差最小化优化处理，获得优化变量；根据所述优化变量获得所述激光雷达的外参标定结果；从而能够简化激光雷达的外参标定流程，实现快速标定，并且提高标定精度。
[0054]
在另一个优选实施例中，第i个地面雷达点对应的第一残差方程表示为：
[0055]rz
(i)＝[pw]z＝[rp
l
t]z；其中，rz(i)表示第i个地面雷达点在世界坐标系下的高度，pw表示第i个地面雷达点在世界坐标系下的位姿，r表示旋转矩阵，p
l
表示第i个地面雷达点在雷达坐标系下的位姿，t表示平移矩阵，i＞0。
[0056]
具体的，结合上述实施例，每一个地面雷达点所对应的第一残差方程的获取方式
相同，以第i个(i＞0)地面雷达点为例，获得的第i个地面雷达点所对应的第一残差方程表示为：rz(i)＝[pw]z＝[rp
l
t]z，其中，rz(i)表示第i个地面雷达点在世界坐标系下的高度，pw表示第i个地面雷达点在世界坐标系下的位姿，r表示雷达位姿旋转矩阵，p
l
表示第i个地面雷达点在雷达坐标系下的位姿，t表示雷达坐标的平移矩阵。
[0057]
需要说明的是，世界坐标采用东北天，世界坐标系的原点在地面上，此时，所选择的地面雷达点在世界坐标系下的z值为0，相应的，最小化残差时，残差的优化值为0。
[0058]
在又一个优选实施例中，所述根据预设的旋转顺序提取欧拉角和旋转矩阵的对应关系，具体包括：
[0059]
采用xyz顺规提取欧拉角和旋转矩阵的对应关系；其中，所述对应关系表示为r
x
(ψ)表示绕x轴的旋转结果，ry(θ)表示绕y轴的旋转结果，表示绕z轴的旋转结果。
[0060]
具体的，结合上述实施例，在根据预先设置的旋转顺序提取欧拉角和旋转矩阵的对应关系时，可以采用xyz顺规，来获取欧拉角和旋转矩阵的对应关系，使用r
x
(ψ)表示绕x轴的旋转结果，ry(θ)表示绕y轴的旋转结果，表示绕z轴的旋转结果，则获得的欧拉角和旋转矩阵的对应关系表示为
[0061]
在又一个优选实施例中，第i个地面雷达点对应的第二残差方程表示为：
[0062]rz
(i)＝-x
l
sinθ y
l
sinψcosθ z
l
cosψcosθ z
t
；其中，p
l
＝(x
l
,y
l
,z
l
)表示第i个地面雷达点在雷达坐标系下的位姿，z
t
表示z轴平移参数。
[0063]
具体的，结合上述实施例，在获得每一个地面雷达点所对应的第一残差方程以及欧拉角和旋转矩阵的对应关系之后，可以将欧拉角和旋转矩阵的对应关系的表达式对应代入每一个地面雷达点所对应的第一残差方程中，相应获得每一个地面雷达点所对应的第二残差方程；其中，每一个地面雷达点所对应的第二残差方程的获取方式相同，仍以第i个地面雷达点为例，第i个地面雷达点所对应的第一残差方程表示为：rz(i)＝[pw]z＝[rp
l
t]z，对单个地面雷达点进行展开有：则第i个地面雷达点所对应的第一残差方程表示为：rz(i)＝zw＝r
31
x
l
r
32yl
r
33zl
z
t
，结合获得的欧拉角和旋转矩阵的对应关系可知：将该公式代入第i个地面雷达点所对应的第一残差方程，获得第i个地面雷达点所对应的第二残差方程表示为：rz(i)＝-x
l
sinθ y
l
sinψcosθ z
l
cosψcosθ z
t
，其中，p
l
＝(x
l
,y
l
,z
l
)表示第i个地面雷达点在雷达坐标系下的位姿，z
t
表示
z轴平移参数。
[0064]
在又一个优选实施例中，所述根据所有地面雷达点对应的第二残差方程进行残差最小化优化处理，获得优化变量，具体包括：
[0065]
获取每一个地面雷达点对应的第二残差方程的雅克比矩阵；
[0066]
根据最小二乘法对所有地面雷达点对应的雅克比矩阵进行求解，获得所述优化变量。
[0067]
作为上述方案的改进，第i个地面雷达点对应的第二残差方程的雅克比矩阵表示为：
[0068][0069]
具体的，结合上述实施例，在最小化残差获得优化变量时，可以根据每一个地面雷达点所对应的第二残差方程分别获取每一个地面雷达点所对应的雅克比矩阵，其中，每一个地面雷达点所对应的雅克比矩阵的获取方式相同，仍以第i个地面雷达点为例，第i个地面雷达点所对应的第二残差方程表示为：rz(i)＝-x
l
sinθ y
l
sinψcosθ z
l
cosψcosθ z
t
，则第i个地面雷达点所对应的雅克比矩阵表示为：根据最小二乘法对雅克比矩阵进行求解，相应获得优化变量θ、ψ和z
t
。
[0070]
在又一个优选实施例中，所述优化变量包括θ、ψ和z
t
；则，所述根据所述优化变量获得所述激光雷达的外参标定结果，具体包括：
[0071]
根据所述优化变量θ、ψ和z
t
获得所述激光雷达的外部参数roll(θ)、pitch(ψ)和z(z
t
)。
[0072]
具体的，结合上述实施例，获得的优化变量包括θ、ψ和z
t
，其中，θ可以表示激光雷达的roll角，ψ可以表示激光雷达的pitch角，z
t
可以表示激光雷达的z轴方向的高度，则，在获得优化变量θ、ψ和z
t
之后，即可根据优化变量θ、ψ和z
t
对应获得激光雷达的优化后的外部参数roll(θ)、pitch(ψ)和z(z
t
)。
[0073]
本发明实施例还提供了一种场端激光雷达的标定装置，参见图2所示，是本发明提供的一种场端激光雷达的标定装置的一个优选实施例的结构框图，所述装置包括：
[0074]
点云数据处理模块11，用于获取场端待标定的激光雷达的ros点云数据，并进行显示；
[0075]
地面点云选择模块12，用于根据点云显示结果，选取地面点云；其中，所述地面点云中包括至少三个地面雷达点；
[0076]
第一残差获取模块13，用于获取每一个地面雷达点在世界坐标系下的位姿与在雷达坐标系下的位姿对应的第一残差方程；
[0077]
对应关系获取模块14，用于根据预设的旋转顺序提取欧拉角和旋转矩阵的对应关系；
[0078]
第二残差获取模块15，用于根据所述对应关系对每一个地面雷达点对应的第一残差方程进行处理，获得每一个地面雷达点对应的第二残差方程；
[0079]
变量优化模块16，用于根据所有地面雷达点对应的第二残差方程进行残差最小化优化处理，获得优化变量；
[0080]
标定结果获取模块17，用于根据所述优化变量获得所述激光雷达的外参标定结果。
[0081]
优选地，第i个地面雷达点对应的第一残差方程表示为：
[0082]rz
(i)＝[pw]z＝[rp
l
t]z；其中，rz(i)表示第i个地面雷达点在世界坐标系下的高度，pw表示第i个地面雷达点在世界坐标系下的位姿，r表示旋转矩阵，p
l
表示第i个地面雷达点在雷达坐标系下的位姿，t表示平移矩阵，i＞0。
[0083]
优选地，所述对应关系获取模块14具体用于：
[0084]
采用xyz顺规提取欧拉角和旋转矩阵的对应关系；其中，所述对应关系表示为r
x
(ψ)表示绕x轴的旋转结果，ry(θ)表示绕y轴的旋转结果，表示绕z轴的旋转结果。
[0085]
优选地，第i个地面雷达点对应的第二残差方程表示为：
[0086]rz
(i)＝-x
l
sinθ y
l
sinψcosθ z
l
cosψcosθ z
t
；其中，p
l
＝(x
l
,y
l
,z
l
)表示第i个地面雷达点在雷达坐标系下的位姿，z
t
表示z轴平移参数。
[0087]
优选地，所述变量优化模块16具体包括：
[0088]
雅克比矩阵获取单元，用于获取每一个地面雷达点对应的第二残差方程的雅克比矩阵；
[0089]
变量优化单元，用于根据最小二乘法对所有地面雷达点对应的雅克比矩阵进行求解，获得所述优化变量。
[0090]
优选地，第i个地面雷达点对应的第二残差方程的雅克比矩阵表示为：
[0091][0092]
优选地，所述优化变量包括θ、ψ和z
t
；则，所述标定结果获取模块17具体用于：
[0093]
根据所述优化变量θ、ψ和z
t
获得所述激光雷达的外部参数roll(θ)、pitch(ψ)和z(z
t
)。
[0094]
需要说明的是，本发明实施例所提供的一种场端激光雷达的标定装置，能够实现上述任一实施例所述的场端激光雷达的标定方法的所有流程，装置中的各个模块、单元的
作用以及实现的技术效果分别与上述实施例所述的场端激光雷达的标定方法的作用以及实现的技术效果对应相同，这里不再赘述。
[0095]
本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的计算机程序；其中，所述计算机程序在运行时控制所述计算机可读存储介质所在的设备执行上述任一实施例所述的场端激光雷达的标定方法。
[0096]
本发明实施例还提供了一种终端设备，参见图3所示，是本发明提供的一种终端设备的一个优选实施例的结构框图，所述终端设备包括处理器10、存储器20以及存储在所述存储器20中且被配置为由所述处理器10执行的计算机程序，所述处理器10在执行所述计算机程序时实现上述任一实施例所述的场端激光雷达的标定方法。
[0097]
优选地，所述计算机程序可以被分割成一个或多个模块/单元(如计算机程序1、计算机程序2、
······
)，所述一个或者多个模块/单元被存储在所述存储器20中，并由所述处理器10执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述终端设备中的执行过程。
[0098]
所述处理器10可以是中央处理单元(central processing unit，cpu)，还可以是其他通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现场可编程门阵列(field-programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等，通用处理器可以是微处理器，或者所述处理器10也可以是任何常规的处理器，所述处理器10是所述终端设备的控制中心，利用各种接口和线路连接所述终端设备的各个部分。
[0099]
所述存储器20主要包括程序存储区和数据存储区，其中，程序存储区可存储操作系统、至少一个功能所需的应用程序等，数据存储区可存储相关数据等。此外，所述存储器20可以是高速随机存取存储器，还可以是非易失性存储器，例如插接式硬盘，智能存储卡(smart media card，smc)、安全数字(secure digital，sd)卡和闪存卡(flash card)等，或所述存储器20也可以是其他易失性固态存储器件。
[0100]
需要说明的是，上述终端设备可包括，但不仅限于，处理器、存储器，本领域技术人员可以理解，图3结构框图仅仅是上述终端设备的示例，并不构成对终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件。
[0101]
综上，本发明实施例所提供的一种场端激光雷达的标定方法、装置、计算机可读存储介质及终端设备，通过获取场端待标定的激光雷达的ros点云数据，并进行显示；根据点云显示结果，选取地面点云；其中，所述地面点云中包括至少三个地面雷达点；获取每一个地面雷达点在世界坐标系下的位姿与在雷达坐标系下的位姿对应的第一残差方程；根据预设的旋转顺序提取欧拉角和旋转矩阵的对应关系；根据所述对应关系对每一个地面雷达点对应的第一残差方程进行处理，获得每一个地面雷达点对应的第二残差方程；根据所有地面雷达点对应的第二残差方程进行残差最小化优化处理，获得优化变量；根据所述优化变量获得所述激光雷达的外参标定结果；从而能够简化激光雷达的外参标定流程，并且提高标定精度。
[0102]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人
员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。