一种双单线激光雷达的离线外参标定方法

1.本发明涉及自动驾驶及机器人技术领域，具体涉及一种双单线激光雷达的离线外参标定方法。


背景技术：

2.双单线激光雷达能够弥补单线激光雷达无法获取第三维度信息的缺点，实现三维数据采集，为后续机器人自主导航、同时定位与建图(slam)等任务提供更多有效信息。与三维激光雷达相比，双单线激光雷达的价格更低、能耗更少，符合低成本和高效益的民用市场竞争趋势。然而目前缺少一种能够确定两个单线激光雷达相对位姿的标定方法，这极大地制约了双单线激光雷达的实用性。因此为克服双单线激光雷达缺少相对位姿信息，无法实现三维数据采集的缺点，本发明提出了一种双单线激光雷达的离线外参标定方法，使用一个半径已知的球体，为求解两个二维激光雷达的相对位姿提供约束，进而求解二者的相对位姿信息，从而解决了双单线激光雷达采集三维数据的难题，为低成本、低能耗的激光定位系统提供了可靠基础。


技术实现要素：

3.本发明的目的在于针对现有技术的不足，提供一种双单线激光雷达的离线外参标定方法，能够在两个激光雷达同时工作时，准确标定出二者中心的相对位姿信息。
4.本发明的目的是通过以下技术方案实现的：一种双单线激光雷达的离线外参标定方法，该方法包括如下步骤：
5.步骤(1)：选择一个半径为r的球体，在空旷环境下设有两个单线激光雷达l1、l2，l1以姿态t相对l2摆放，l1为主雷达，l2为次雷达，两雷达中心的直线距离不超出球体直径，且保证球体能够同时被l1、l2扫描；
6.步骤(2)：开启数据采集，获得两个单线激光雷达对同一球体扫描若干次的激光点云数据分别为单线激光雷达采用极坐标系，单线激光雷达采用极坐标系，分别为极坐标系中的半径和角度；
7.步骤(3)：将点云数据变换到笛卡尔坐标系下，之后对点云数据进行高斯滤波，降低随机噪声对点云数据的干扰，获得式(1)表示的激光点云坐标：
[0008][0009]
其中i代表激光雷达扫描的点数，j代表点云数据属于l1或者l2，代表笛卡尔坐标系下的点云坐标；
[0010]
步骤(4)：根据步骤(3)得到的笛卡尔坐标系下的点云数据，分别估计出以l1、l2中心为坐标系原点下的球心位置，移动球体到另一不同位置，重复步骤(2)-(4)共m次，得到m
对球心坐标：
[0011][0012]
步骤(5)：使用icp(iterative closest point)算法估计出两个单线激光雷达的相对位姿信息。
[0013]
进一步地，步骤(4)中对球心坐标的拟合过程为：
[0014]
针对l1单线激光雷达，由于点云数据来源于对同一球体的扫描，因此点云都处于同一圆上，根据圆的函数定义式有：
[0015][0016]
其中x1、y1代表当前点云数据扫描的圆所对应的圆心，r1代表当前点云数据扫描的圆所对应的半径，代表l1单线激光雷达当前扫描结果变换到笛卡尔坐标系下的点云坐标；从而可以构建非线性最小二乘优化问题，目标损失函数为：
[0017][0018]
在更新半径r1时，选择将看做是一个整体执行梯度更新，从而能够避免梯度消失的问题；求解式(4)所表述的非线性最小二乘优化问题，拟合得到该组点云数据所对应的圆，以该圆面建立xyz三垂直坐标系，且z轴垂直于圆面；在扫描的球体半径r已知的条件下，相对于单线激光雷达坐标系来说，扫描球体的球心和拟合出来的圆心在同一条z轴上，即x轴、y轴坐标相等，当前扫描得到的点云数据都处于同一球面上，考虑到单线激光雷达只能获取二维坐标，为了能够拟合出球心坐标，设定雷达所在位置为z轴原点处，即所有点云坐标变换为根据球体函数的定义式有：
[0019][0020]
其中(x1,y1,z1)代表球心的三维坐标；当获取了当前点云数据所在圆的圆心后，进而可以构建关于球体球心的目标损失函数：
[0021][0022]
设定参数初值，对这部分目标损失函数进行非线性最小二乘优化后，可以得到当前球体摆放位置下球心相对于激光雷达的三维坐标。
[0023]
进一步地，步骤(5)中icp配准估计位姿信息的过程为：
[0024]
在获取m对球心的三维坐标后，分别计算l1、l2雷达坐标系下m个球心坐标所对应的质心坐标
[0025][0026]
对所有球心坐标去质心后得到去质心球心坐标
[0027][0028]
计算icp算法中的特征矩阵m：
[0029][0030]
利用svd(singular value decomposition)方法分解特征矩阵m得到：
[0031]
m＝u∑v
t
ꢀꢀ
(10)
[0032]
其中u,v表示正交矩阵，∑为特征对角矩阵；
[0033]
求解得到两个单线激光雷达间的旋转矩阵q和相对位移t为：
[0034][0035]
最终估计得到以齐次表达式表达的两个单线激光雷达的相对位姿信息为：
[0036][0037]
本发明的有益效果在于：本发明利用一个半径已知的球体，通过非线性最小二乘优化方法，先拟合圆心，再拟合球心，将原本在二维平面上的点云数据映射到了属于三维空间的球体的几何球心，经过多次数据拟合，结合icp算法，最终得到双单线激光雷达的相对位姿信息，克服了双单线激光雷达因缺少相对位姿信息而无法进行三维数据采集的缺点，提高了双单线激光雷达的民用市场竞争地位，且标定后的位姿信息精度高、准确性好，提高了双单线激光雷达采集三维数据的可信度。
附图说明
[0038]
为了更清楚的说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。
[0039]
图1为双单线激光雷达离线外参标定方法的流程图；
[0040]
图2为双单线激光雷达离线外参标定方法使用过程中的球心拟合具体方法流程图；
[0041]
图3为双单线激光雷达离线外参标定方法使用过程中的位姿标定具体方法流程图；
[0042]
图4a为双单线激光雷达离线外参标定结果的rpe误差曲线；
[0043]
图4b为双单线激光雷达离线外参标定结果在笛卡尔坐标系中的三个坐标轴方向误差曲线。
具体实施方式
[0044]
下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
[0045]
通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0046]
在下文中，将更全面地描述本公开的各种实施例。本公开可具有各种实施例，并且可在其中做出调整和改变。然而，应理解：不存在将本公开的各种实施例限于在此公开的特定实施例的意图，而是应将本公开理解为涵盖落入本公开的各种实施例的精神和范围内的所有调整、等同物和/或可选方案。
[0047]
在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
[0048]
此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
[0049]
除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。
[0050]
图1为一个实施例中提供的一种双单线激光雷达的离线外参标定方法的流程示意图，该方法包括：
[0051]
步骤110：选择一个半径为r＝1.5m的球体，在空旷环境下设有两个单线激光雷达l1、l2，l1以位姿t相对l2摆放，位姿t以齐次表达式的形式，本实施例中t的具体数值为：
[0052][0053]
其中l1为主雷达，l2为次雷达，两雷达中心的直线距离不超出球体直径，且保证球体能够同时被l1、l2扫描。
[0054]
步骤120：开启数据采集，获得两个雷达对同一球体扫描10次的点云数据分别为单线激光雷达采用极坐标系，单线激光雷达采用极坐标系，分别为极坐标系中的半径和角度。
[0055]
步骤130：将点云数据变换到笛卡尔坐标系下，之后对点云数据进行高斯滤波，降低随机噪声对数据的干扰，获得式(1)表示的激光点云坐标：
[0056]
[0057]
其中i代表激光雷达扫描的点数，j代表点云数据属于l1或者l2，代表笛卡尔坐标系下的点云坐标。
[0058]
步骤140：根据步骤130得到的笛卡尔坐标系下的点云数据，分别估计出以l1、l2中心为坐标系原点下的球心位置，移动球体到另一不同位置，重复步骤(2)-(4)共4次，得到4对球心坐标：
[0059][0060]
具体步骤如图2所示。
[0061]
步骤141，初始化两雷达扫描数据对应的两个圆心坐标为(0.5,0.5)，圆半径为0.5，球心z轴坐标为0.5。
[0062]
步骤142，当未达到规定次数时，需要返回步骤120改变球体位置，重新进行数据采集与预处理。
[0063]
步骤143，以单线激光雷达中心为坐标系原点，估计圆心坐标位置以及圆半径的过程为：
[0064]
以l1单线激光雷达为例，由于点云数据来源于对同一球体的扫描，因此点云都处于同一圆上，根据圆的函数定义式有：
[0065][0066]
其中x1、y1代表当前点云数据扫描的圆所对应的圆心，r1代表当前点云数据扫描的圆所对应的半径，代表l1单线激光雷达当前扫描结果变换到笛卡尔坐标系下的点云坐标。从而可以构建非线性最小二乘优化问题，目标损失函数为：
[0067][0068]
在更新半径r1时，选择将看做是一个整体执行梯度更新，从而能够避免梯度消失的问题。
[0069]
步骤144，以单线激光雷达中心为坐标系原点，估计球心三维坐标，具体过程为：
[0070]
扫描的球体半径r已知，相对于单线激光雷达坐标系来说，扫描球体的球心和拟合出来的圆心在同一条z轴上，即x轴、y轴坐标相等，当前扫描得到的点云数据都处于同一球面上，考虑到单线激光雷达只能获取二维坐标，为了能够拟合出球心坐标，设定雷达所在位置为z轴原点处，即所有点云坐标变换为根据球体函数的定义式有：
[0071][0072]
其中(x1,y1,z1)代表球心的三维坐标。当获取了当前点云数据所在圆的圆心后，进而可以构建关于球体球心的目标损失函数：
[0073][0074]
设定参数初值，对这部分目标损失函数进行非线性最小二乘优化后，可以得到当前球体摆放位置下球心相对于激光雷达的三维坐标。
[0075]
步骤145，存储本次球心拟合结果，返回步骤142重新开始执行，直到执行4次后停
止。
[0076]
步骤150，使用icp算法估计出两个单线激光雷达的相对位姿信息，其具体步骤如图3所示。
[0077]
步骤151，分别计算l1、l2雷达坐标系下4对球心位置所对应的质心坐标
[0078][0079]
步骤152，对所有球心坐标去质心后得到去质心球心坐标
[0080][0081]
步骤153，计算icp算法中的特征矩阵m：
[0082][0083]
步骤154，利用svd方法分解特征矩阵m得到：
[0084]
m＝u∑v
t
ꢀꢀ
(10)
[0085]
其中u,v表示正交矩阵，∑为特征对角矩阵；
[0086]
步骤155，求解得到两个单线激光雷达间的旋转矩阵q和相对位移t为：
[0087][0088]
最终估计得到以齐次表达式表达的两个单线激光雷达的相对位姿信息为：
[0089][0090]
考虑到激光雷达在采集过程中存在随机噪声的干扰，使用rpe评价指标与相对位移在笛卡尔坐标系三个坐标轴方向上的偏差来评价在不同噪声等级下本发明的位姿标定精度，如图4a、图4b所示，在噪声标准差小于0.08m时，误差大小维持在毫米级，贴近实际使用时受到的环境随机噪声影响，进一步将噪声水平提高，本发明的标定误差仍然维持在厘米级，具有较高的精度，验证了本发明方法的有效性。
[0091]
以上所述，仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。