CN109001711B - 多线激光雷达标定方法 - Google Patents

Abstract

本发明涉及一种多线激光雷达标定方法，包括：通过解耦α、β、Δz和γ、Δx、Δy，分别标定两部分参数。通过提取水平地面特征，标定参数α、β、Δz；通过GPS提供的旋转矩阵^vR和平移矩阵^vT，构建约束方程，求解标定参数γ、Δx、Δy。本发明提供的方法，无需人工测量标定参数，不需要特殊的标定物，操作便捷，标定效率高；利用平地特征和高精度GPS信息，可以提高标定精度。

Description

多线激光雷达标定方法

技术领域

本发明涉及激光雷达技术领域，尤其涉及一种多线激光雷达标定方法。

背景技术

近年来,随着智能驾驶技术的飞速发展，激光雷达具有精度高、测距范围大，以及不受光线影响等特点，被广泛应用于智能驾驶车辆的障碍物检测、即时定位与地图构建等环境感知领域。

在智能驾驶***中，激光雷达一般安装于智能驾驶车辆的顶部，激光雷达获取的位置数据信息是基于雷达自身局部坐标系。在激光雷达的实际使用过程中，为了统一多传感器的信息，需要将激光雷达获取到的自身局部坐标系下的数据转换到汽车本体坐标系。数据转换过程中通过变换矩阵，将激光雷达坐标系中的点云数据，转换到汽车本体坐标系下。因此在实际使用过程中需要事先对激光雷达进行标定，即获取雷达坐标系与汽车本体坐标系之间的变换关系。

现有技术中,为了实现坐标系的标定，研究者提出了多种方案。常见的标定方法主要有传统人工标定和标定物测量方法。

传统人工标定采用手动测量或者仪器测量的方式，测量激光雷达坐标系和车体坐标系之间的变换关系。首先确定车体坐标系的原点位置和坐标轴方向，确定激光雷达坐标系原点位置，利用卡尺或者卷尺测量激光雷达坐标系原点与车体坐标系原点之间的距离向量。之后确定激光雷达坐标轴的方向，用特殊测试仪器，例如三维角度测量仪测量激光雷达坐标轴和车体坐标轴之间的欧拉角。

标定物测量方法，通过对特定标定物的匹配来求解三维激光雷达的外参数。常见的标定物有平面靶标、三角面靶标和三面靶标。这类方法利用了激光雷达扫描点不可见、单线式扫描等工作特点，以基于空间向量的三维坐标系转换模型为基础，对靶标进行三维重建，采用坐标系转换模型对标定参数求解。

手动测量标定方法虽然原理简单，但是对操作者的操作精细度以及测量仪器的精度要求高，因此其可推广性不高，且测量精度的不稳定，会随着操作者的水平高低变化。标定物测量方法也存在一定弊端，由于激光雷达对于不同颜色和材质的物体反射率不一样，因此控制点的查找存在一定困难，同时移动控制点的过程中需要大量人力参与，其标定的精度也依赖于其他传感器与控制点之间的参数，所以这类标定方法效率低、精度不高。

发明内容

本发明的目的是针对现有技术中存在的缺陷，提供一种多线激光雷达标定方法，不需要人工测量标定参数，也不需要特定的标定物，提高了标定效率以及标定精度。

为实现上述目的，本发明提供了一种多线激光雷达标定方法，包括：

激光雷达采集车辆所在地平面的点云数据；

根据所述点云数据计算激光雷达坐标系下所述地平面的平面方程的法向量n′；

设定激光雷达坐标系到车体坐标系的旋转矩阵R中的参数α、β、γ的初始值；

根据所述参数α、β、γ的初始值计算旋转矩阵R的初始值；

利用所述平面方程的法向量n′和旋转矩阵R的初始值构造坐标轴z的平行度函数；

将多个点云数据的平行度函数求和，得到第一优化目标函数；

根据所述参数α、β的初始值对所述第一优化目标函数进行迭代处理，当满足预设条件时，迭代终止，得到α、β的最终值；

根据所述α、β的最终值得到激光雷达坐标系到车体坐标系的z轴平移量Δz；

设定激光雷达坐标系下固定参考点的位置坐标；

根据车辆GPS数据获取所述固定参考点在车体坐标系下不同时刻的第一位姿变换数据；

根据所述第一位姿数据获取所述固定参考点在激光雷达坐标系下不同时刻的第二位姿变换数据；

根据所述第一位姿变换数据和第二位姿变换数据得到误差函数；

将多组连续帧数据对应的多个所述误差函数求和，得到第二优化目标函数；

根据α、β的最终值、z轴平移量Δz和第二优化目标函数得到参数γ、x轴平移量Δx和y轴平移量Δy。

进一步的，所述预设条件具体包括：

所述优化目标函数的值小于第一预设阈值或者迭代次数达到第二预设阈值。

进一步的，所述利用所述平面方程的法向量n′和旋转矩阵R的初始值构造坐标轴z的平行度函数具体包括：

利用公式

构造法向量n′坐标轴z的平行度函数。

进一步的，所述将多个点云数据的平行度函数求和，得到第一优化目标函数具体包括：

在地平面上采集m个采样点，计算每个点的法向量n_i′；

利用公式

计算得到第一优化目标函数。

进一步的，所述根据所述参数α、β、γ的初始值计算旋转矩阵R的初始值具体包括：

根据参数α、β的预设估计值和参数γ＝0利用公式

计算旋转矩阵R的初始值。

进一步的，所述设定激光雷达坐标系下固定参考点的位置坐标具体包括：

设定激光雷达坐标系原点为固定参考点P。

进一步的，所述根据车辆GPS数据获取固定参考点在车体坐标系下不同时刻的第一位姿变换数据具体包括：

在任意时刻t，根据车辆GPS数据获取固定参考点在车体坐标系C₀(t)下的坐标为^vP_t＝T＝[Δx Δy Δz]^T；

在时刻t+1，在车体坐标系c₀(t+1)下,固定参考点坐标为

其中，^vR_t为车体坐标系变化的旋转矩阵，^vT_t为车体坐标系变化的平移矩阵。

进一步的，所述根据所述第一位姿数据获取所述固定参考点在激光雷达坐标系下不同时刻的第二位姿变换数据具体包括：

在任意时刻t，激光雷达坐标系C_l(t)下，固定参考点P的坐标为^lP_t＝[0 0 0]^T；

在时刻t+1，激光雷达坐标系C_l(t+1)下，固定参考点的坐标为

其中，^lR_t为激光雷达坐标系变化的旋转矩阵，^lT_t为激光雷达坐标系变化的平移矩阵。

进一步的，所述根据所述第一位姿变换数据和第二位姿变换数据得到误差函数具体包括：

激光雷达坐标系和车体坐标系之间坐标，满足如下关系：

^vP_t+1＝R*^lP_t+1+T；

利用公式

得到误差函数e。

进一步的，所述将多组连续帧数据对应的多个所述误差函数求和，得到第二优化目标函数具体包括：

利用公式

得到第二优化目标函数。

本发明提供的多线激光雷达标定方法，通过解耦α、β、Δz和γ、Δx、Δy，分别标定两部分参数。通过提取水平地面特征，标定参数α、β、Δz；通过GPS提供的旋转矩阵^vR和平移矩阵^vT，构建约束方程，求解标定参数γ、Δx、Δy。本发明提供的方法，无需人工测量标定参数，不需要特殊的标定物，操作便捷，标定效率高；利用平地特征和高精度GPS信息，可以提高标定精度。

附图说明

图1为本发明实施例提供的坐标系相对位置关系示意图；

图2为本发明实施例提供的多线激光雷达标定方法流程图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

图1为本发明实施例提供的坐标系相对位置关系示意图。如图1所示，激光雷达坐标系x′y′z′记为C_l，车体坐标系xyz记作C₀，车体坐标系的原点设置为车头中心点在地面的投影点，车体坐标系的xy平面与地面重合。α、β、γ分别为两个坐标系沿x、y、z轴方向的旋转角度。Δx、Δy、Δz是激光雷达坐标系C_l相对车体坐标系C₀分别沿x、y、z轴方向的平移量。

激光雷达坐标系和车体坐标系之间的变换关系为：

求出上述参数α、β、γ、Δx、Δy、Δz，就可以求出激光雷达坐标系C_l相对于车体坐标系C₀的变换关系。本发明技术方案通过解耦α、β、Δz和γ、Δx、Δy，分别标定两部分参数。通过提取水平地面特征，标定参数α、β、Δz；通过NDT算法得到的旋转矩阵^lR和平移矩阵^lT，以及GPS提供的旋转矩阵^vR平移矩阵^vT，构建约束方程，求解标定参数γ、Δx、Δy。

图2为本发明实施例提供的多线激光雷达标定方法流程图。如图2所示，具体包括如下步骤：

步骤101，激光雷达采集车辆所在地平面的点云数据；

固定在车辆顶部的激光雷达采集激光雷达坐标系下车辆所在地平面的点云数据。

步骤102，根据点云数据计算激光雷达坐标系下地平面的平面方程的法向量；

以地面为参考平面，根据采集到的点云数据，利用RANSAC(Random SampleConsensus)算法计算地平面在激光雷达坐标系C_l下的平面方程：

z′＝Ax′+By′+C (4)

平面方程的法向量为：n′＝[-A -B 1]^T (5)

步骤103，设定激光雷达坐标系到车体坐标系的旋转矩阵R中的参数的初始值；

设定参数α、β的预设估计值为初始值，γ的初始值为0。

步骤104，根据参数的初始值计算旋转矩阵的初始值；

将步骤103中设定的参数α、β、γ的初始值代入公式

计算得到旋转矩阵R的初始值。

步骤105，利用平面方程的法向量和旋转矩阵的初始值构造坐标轴z的平行度函数；

平面法向量与车体坐标系中的z轴，满足方程：

构造法向量n′和坐标轴z的平行度函数：

其中，f的值越小，说明地面与车体坐标系的xy平面平行度越好。

步骤106，将多个点云数据的平行度函数求和，得到第一优化目标函数；

在地平面上采集m个采样点，计算每个点的法向量n_i′；

利用公式

计算所有采样点平行度函数的和，得到第一优化目标函数。

步骤107，根据参数的初始值对第一优化目标函数进行迭代处理，当满足预设条件时，迭代终止，得到第一参数、第二参数的最终值；

其中，预设条件具体包括：优化目标函数的值小于第一预设阈值或者迭代次数达到第二预设阈值。

具体的，采用粒子群优化PSO(Particle swarm optimization)算法，以等式(9)作为优化目标函数。

将参数α、β的预设估计值作为初始值，γ的初始值为0。PSO算法以平行度函数F作为优化目标函数，更新一组α、β的值，根据更新的α、β值计算平行度函数F，判断F的值是否小于第一预设阈值∈，或者迭代的次数是否达到预设第二预设阈值K，如果满足上述任意一个条件，迭代终止，得到第一参数α、第二参数β的最终值。

本步骤中的粒子群优化算法也可以替换为人工免疫***算法、分布估计算法等多目标优化算法。

步骤108，根据第一参数、第二参数的最终值得到激光雷达坐标系到车体坐标系的z轴平移量；

将优化得到的α、β的最终值代入投影方程，得到地平面在激光雷达坐标系下的平面方程，方程中的常数项C为平移量Δz。

另外，对于标定参考平面，还可以采用正对车体并且与地平面垂直的平整的墙面作为参平面，则相应的公式(7)替换为以下公式：

相应的公式(8)替换为以下公式：

相应的公式(9)替换为以下公式：

至此，得到了激光雷达标定参数中的α、β、Δz。

下面以标定的结果α、β、Δz作为基础，利用车体的全球定位***(GlobalPositioning SystemGPS数据和正态分布变换NDT(Normal Distribution Transform)算法得到的数据，推算约束方程，构造优化函数，计算偏航角γ和平面偏移量Δx、Δy。

由公式(1)可知，激光雷达坐标系C_l相对于车体坐标系C₀的关系，可以用旋转矩阵R，平移向量T来描述。

车载GPS***固定在车体上，GPS可以得到车体坐标系在时刻t到时刻t+1之间的位姿变换，利用NDT算法可以推算时刻t到时刻t+1之间，激光雷达坐标系的位姿变换。激光雷达固定在车体上，因此，在t时刻和t+1时刻，激光雷达坐标系相对于车体坐标系之间的变换关系固定不变。利用上述约束关系，可以构造约束方程。

步骤109，设定激光雷达坐标系下固定参考点的位置坐标；

设定激光雷达坐标系原点，作为固定参考点P。

步骤110，根据车辆GPS数据获取固定参考点在车体坐标系下不同时刻的第一位姿变换数据；

车载GPS***固定在车体上，GPS可以得到车体坐标系在时刻t到时刻t+1之间的位姿变换：

在任意时刻t，根据车辆GPS数据获取固定参考点在车体坐标系C₀(t)下的坐标为：

^vP_t＝T＝[Δx Δy Δz]^T (10)

在时刻t+1，在车体坐标系C₀(t+1)下,固定参考点满足方程：

^vP_t＝^vR_t*^vP_t+1+^vT_t (11)

因此，固定参考点在车体坐标系C₀(t+1)下的坐标为：

其中，^vR_t为车体坐标系变化的旋转矩阵，^vT_t为车体坐标系变化的平移矩阵。

步骤111，根据第一位姿数据获取固定参考点在激光雷达坐标系下不同时刻的第二位姿变换数据；

在任意时刻t，激光雷达坐标系C_l(t)下，固定参考点P的坐标为^lP_t＝[0 0 0]^T；

在时刻t+1，激光雷达坐标系C_l(t+1)下，固定参考点满足方程：

^lP_t＝^lR_t*^lP_t+1+^lT_t (13)

因此，固定参考点在激光雷达坐标系C₀(t+1)下的坐标为：

其中，^lR_t为激光雷达坐标系变化的旋转矩阵，^lT_t为激光雷达坐标系变化的平移矩阵。

步骤112，根据第一位姿变换数据和第二位姿变换数据得到误差函数；

激光雷达坐标系和车体坐标系之间坐标，满足如下关系：

^vP_t+1＝R*^lP_t+1+T (15)

将公式(12)和公式(14)代入公式(15)，最小化等式两边的误差，

得到误差函数：

步骤113，将多组连续帧数据对应的多个误差函数求和，得到第二优化目标函数；

采集多组连续帧之间的数据，将得到的多个误差函数求和，得到第二优化目标函数：

得到第二优化目标函数。

另外，点云匹配使用的算法NDT算法，也可以替换成迭代最近点ICP(Iterativecloset point)算法。

步骤114，根据第一参数、第二参数的最终值、z轴平移量和第二优化目标函数得到第三参数、x轴平移量和y轴平移量。

将第一参数α、第二参数β的最终值、z轴平移量Δz代入公式(16)，并将公式(16)进行最小化，得到第三参数γ、Δx和Δy。

最后，将最终得到的α、β、γ、Δx、Δy、Δz带入旋转矩阵R、T中，得到两个坐标系之间的变换关系。

本发明提供的多线激光雷达标定方法，通过解耦α、β、Δz和γ、Δx、Δy，分别标定两部分参数。通过提取水平地面特征，标定参数α、β、Δz；通过GPS提供的旋转矩阵^vR和平移矩阵^vT，构建约束方程，求解标定参数γ、Δx、Δy。本发明提供的方法，无需人工测量标定参数，不需要特殊的标定物，操作便捷，标定效率高；利用平地特征和高精度GPS信息，可以提高标定精度。

专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种多线激光雷达标定方法，其特征在于，所述方法包括：

激光雷达采集车辆所在地平面的点云数据；

根据所述点云数据计算激光雷达坐标系下所述地平面的平面方程的法向量n′；

设定激光雷达坐标系到车体坐标系的旋转矩阵R中的参数α、β、γ的初始值；其中，所述α为所述激光雷达坐标系沿x轴方向到所述车体坐标系的旋转角度；所述β为所述激光雷达坐标系沿y轴方向到所述车体坐标系的旋转角度；所述γ为所述激光雷达坐标系沿z轴方向到所述车体坐标系的旋转角度；根据所述参数α、β、γ的初始值计算旋转矩阵R的初始值；

利用所述平面方程的法向量n′和旋转矩阵R的初始值构造坐标轴z的平行度函数；所述坐标轴z的平行度函数为：

将多个点云数据的平行度函数求和，得到第一优化目标函数；

根据所述参数α、β的初始值对所述第一优化目标函数进行迭代处理，当满足预设条件时，迭代终止，得到α、β的最终值；

根据所述α、β的最终值得到激光雷达坐标系到车体坐标系的z轴平移量Δz；

设定激光雷达坐标系下固定参考点的位置坐标；

根据车辆GPS数据获取所述固定参考点在车体坐标系下不同时刻的第一位姿变换数据；

根据所述第一位姿数据获取所述固定参考点在激光雷达坐标系下不同时刻的第二位姿变换数据；

根据所述第一位姿变换数据和第二位姿变换数据得到误差函数；

将多组连续帧数据对应的多个所述误差函数求和，得到第二优化目标函数；

根据α、β的最终值、z轴平移量Δz和第二优化目标函数得到参数γ、x轴平移量Δx和y轴平移量Δy。

2.根据权利要求1所述的方法，其特征在于，所述预设条件具体包括：

所述第一优化目标函数的值小于第一预设阈值或者迭代次数达到第二预设阈值。

3.根据权利要求1所述的方法，其特征在于，所述将多个点云数据的平行度函数求和，得到第一优化目标函数具体包括：

在地平面上采集m个采样点，计算每个点的法向量n_i′；

利用公式

计算得到第一优化目标函数。

4.根据权利要求1所述的方法，其特征在于，所述根据所述参数α、β、γ的初始值计算旋转矩阵R的初始值具体包括：

根据参数α、β的预设估计值和参数γ＝0利用公式

计算旋转矩阵R的初始值。

5.根据权利要求1所述的方法，其特征在于，所述设定激光雷达坐标系下固定参考点的位置坐标具体包括：

设定激光雷达坐标系原点为固定参考点P。

6.根据权利要求1所述的方法，其特征在于，所述根据车辆GPS数据获取固定参考点在车体坐标系下不同时刻的第一位姿变换数据具体包括：

在任意时刻t，根据车辆GPS数据获取固定参考点在车体坐标系C₀(t)下的坐标为^vP_t＝T＝[Δx Δy Δz]^T；

在时刻t+1，在车体坐标系C₀(t+1)下,固定参考点坐标为

其中，^vR_t为车体坐标系变化的旋转矩阵，^vT_t为车体坐标系变化的平移矩阵。

7.根据权利要求1所述的方法，其特征在于，所述根据所述第一位姿数据获取所述固定参考点在激光雷达坐标系下不同时刻的第二位姿变换数据具体包括：

在任意时刻t，激光雷达坐标系C_l(t)下，固定参考点P的坐标为^lP_t＝[0 0 0]^T；

在时刻t+1，激光雷达坐标系C_l(t+1)下，固定参考点的坐标为

其中，^lR_t为激光雷达坐标系变化的旋转矩阵，^lT_t为激光雷达坐标系变化的平移矩阵。

8.根据权利要求6或7所述的方法，其特征在于，所述根据所述第一位姿变换数据和第二位姿变换数据得到误差函数具体包括：

激光雷达坐标系和车体坐标系之间坐标，满足如下关系：^vP_t+1＝R*^lP_t+1+T；

利用公式

得到误差函数e。

9.根据权利要求1所述的方法，其特征在于，所述将多组连续帧数据对应的多个所述误差函数求和，得到第二优化目标函数具体包括：

利用公式

得到第二优化目标函数。

Images