CN108596979A

CN108596979A - 一种用于激光雷达和深度相机的标定装置和方法

Info

Publication number: CN108596979A
Application number: CN201810255765.8A
Authority: CN
Inventors: 唐文名; 谷也; 盛卫华
Original assignee: Shenzhen Intelligent Robot Research Institute
Current assignee: Shenzhen Intelligent Robot Research Institute
Priority date: 2018-03-27
Filing date: 2018-03-27
Publication date: 2018-09-28

Abstract

本发明公开了一种用于激光雷达和深度相机的标定装置和方法。所述装置包括标定物、标定背景和主机；所述方法包括采集得到二维激光雷达数据和三维模拟激光雷达数据，然后得到二维模拟激光雷达数据，根据二维模拟激光雷达数据和二维激光雷达数据得到一维模拟激光雷达数据和一维激光雷达数据，根据一维模拟激光雷达数据和一维激光雷达数据得到第一标定数据和第二标定数据，从而求解旋转矩阵和平移向量等步骤。本发明可以快速、高精度地调整深度相机与激光雷达之间的相对位置关系，完成对深度相机和激光雷达的标定，从而提高深度相机与激光雷达所采集数据的转换和融合的精度，使最终生成的全局地图的精度更高。本发明应用于图像识别处理技术领域。

Description

一种用于激光雷达和深度相机的标定装置和方法

技术领域

本发明涉及图像识别处理技术领域，尤其是一种用于激光雷达和深度相机的标定装置和方法。

背景技术

SLAM(Simultaneous localization and mapping)是即时定位与地图构建的英文简称，也称为CML(Concurrent Mapping and Localization)，并发建图与定位。SLAM要解决的问题是：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边逐步描绘出此环境完全的地图，同时一边决定机器人应该往哪个方向行进。

机器人SLAM技术是机器人领域的基础核心技术，基于激光雷达的SLAM技术是被广泛应用的一种方案，其中基于2D激光雷达的SLAM技术被广泛应用在家庭服务机器人领域。但现有基于2D激光雷达的SLAM技术的一个缺点是只能探测2D平面的障碍物，而且对障碍物的要求较高，一些特殊的障碍物，如含有镂空结构的桌椅等障碍物则不能通过现有2D激光雷达有效探测出来。这样，基于2D激光雷达所建立的全局地图有可能遗漏了大量的障碍物，这极大地限制了SLAM技术的应用。

为了解决对含有镂空结构的障碍物的探测，可以在2D激光雷达的基础上，同时使用深度相机进行探测。深度相机可以探测空间的深度信息，形成三维体感，能够分辨出2D激光雷达不能探测到的含有镂空结构的桌椅等障碍物。深度相机与基于2D激光雷达所建立的地图进行融合，能够实现完整建图与导航功能。

但是，深度相机与激光雷达的工作原理存在较大差异，深度相机与激光雷达在机器人上安装时产生的误差将影响两者数据的转换和融合，这将影响最终生成的全局地图的精度，为此应对深度相机与激光雷达进行标定，但现有的标定方法复杂且精度不高，对深度相机与激光雷达的应用造成了困难。

发明内容

为了解决上述技术问题，本发明的第一目的在于提供一种用于激光雷达和深度相机的标定装置，第二目的在于提供一种用于激光雷达和深度相机的标定方法。

本发明所采取的第一技术方案是：

一种用于激光雷达和深度相机的标定装置，所述激光雷达和深度相机安装在一固定装置上，所述激光雷达和深度相机之间的旋转矩阵和平移向量可以调整，所述标定装置包括标定物、标定背景和主机，所述标定物位于固定装置和标定背景之间并且在激光雷达和深度相机的视野范围内，所述主机分别与激光雷达和深度相机连接；所述主机用于处理激光雷达对标定物采集到的二维激光雷达数据以及深度相机对标定物采集到的三维模拟激光雷达数据，具体包括：

将三维模拟激光雷达数据映射到平面，从而得到二维模拟激光雷达数据；

将二维模拟激光雷达数据降为一维，得到一维模拟激光雷达数据，将二维激光雷达数据降为一维，得到一维激光雷达数据；

对一维模拟激光雷达数据进行均值滤波处理后再利用一维拉普拉斯算子进行处理，从而得到第一标定数据，利用一维拉普拉斯算子对一维激光雷达数据进行处理，从而得到第二标定数据；

根据第一标定数据和第二标定数据，利用迭代最近点算法求解得到激光雷达和深度相机之间的旋转矩阵和平移向量。

本发明所采取的第二技术方案是：

一种用于激光雷达和深度相机的标定方法，包括以下步骤：

S1.使用激光雷达对标定物进行采集，从而得到二维激光雷达数据；

S2.使用深度相机对标定物进行采集，从而得到三维模拟激光雷达数据；

S3.将三维模拟激光雷达数据映射到平面，从而得到二维模拟激光雷达数据；

S4.将二维模拟激光雷达数据降为一维，得到一维模拟激光雷达数据，将二维激光雷达数据降为一维，得到一维激光雷达数据；

S5.对一维模拟激光雷达数据进行均值滤波处理后再利用一维拉普拉斯算子进行处理，从而得到第一标定数据，利用一维拉普拉斯算子对一维激光雷达数据进行处理，从而得到第二标定数据；

S6.根据第一标定数据和第二标定数据，利用迭代最近点算法求解得到激光雷达和深度相机之间的旋转矩阵和平移向量。

进一步地，所述步骤S6之后，还包括以下步骤：

S7.根据所述旋转矩阵和平移向量，对激光雷达和深度相机进行标定。

进一步地，所述步骤S3中，所述平面为激光雷达成像面所在平面。

进一步地，所述步骤S4中：

所述二维模拟激光雷达数据为C{p_c1,p_c2,…,p_cn}，其中p_cn为二维模拟激光雷达数据的第n个元素；

所述二维激光雷达数据为L{p_l1,p_l2,…,p_lm}，其中p_lm为二维激光雷达数据的第m个元素；

所述一维模拟激光雷达数据为C′{p_c1′,p_c2′,…,p_cn′}，其中p_cn′为一维模拟激光雷达数据的第n个元素；

所述一维激光雷达数据为L′{p_l1′,p_l2′,…,p_lm′}，其中p_lm′为一维激光雷达数据的第m个元素。

进一步地，所述步骤S5中：

所述一维拉普拉斯算子为laplace{k,-2k,k}，其中k为自然数；

所述均值滤波处理以m为基数，经过所述均值滤波处理后的一维模拟激光雷达数据为C′{p_c1′,p_c2′,…,p_cm′}；

所述第一标定数据为C″{p_c1″,p_c2″,…,p_cm″}，其中p_cm″为第一标定数据的第m个元素；

所述第二标定数据为L″{p_l1″,p_l2″,…,p_lm″}，其中p_lm″为第二标定数据的第m个元素。

进一步地，所述步骤S6具体包括：

S61.根据第一标定数据和第二标定数据，构建迭代最近点算法的目标函数，所述目标函数包含待求解的旋转矩阵和待求解的平移向量；

S62.求解目标函数，从而得到旋转矩阵和平移向量。

进一步地，所述步骤S62之后，还包括以下步骤：

S63.根据一维模拟激光雷达数据以及步骤S62求得的旋转矩阵和平移向量，生成重投影数据；

S64.将步骤S62求得的旋转矩阵和平移向量、一维激光雷达数据和重投影数据代入迭代最近点算法的判别函数中，如果判别函数收敛，则以当前旋转矩阵和平移向量作为最终求解结果，反之，修改步骤S61中目标函数的条件后返回步骤S61重新执行。

进一步地，所述目标函数为式中，p_ci′为一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cm′}的第i个元素，p_li′为一维激光雷达数据L′{p_l1′,p_l2′,…,p_lm′}的第i个元素，i为求和序号，m为一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cm′}或一维激光雷达数据L′{p_l1′,p_l2′,…,p_lm′}的元素总数，R为旋转矩阵，T为平移向量，ε为目标函数的值。

进一步地，所述重投影数据为C″′{p″′_c1,p″′_c2,…,p″′_cn}，重投影数据所述重投影数据通过下式计算得到：

p″′_ci＝Rp_ci+T，式中，p″′_ci为重投影数据的第i个元素，p_ci为一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cm′}的第i个元素，R为旋转矩阵，T为平移向量；

所述判别函数及其收敛条件为式中，p″′_ci为重投影数据的第i个元素，p_li′为一维激光雷达数据的第i个元素，β为自定义正整数，δ为任意小正数。

本发明的有益效果是：通过本发明，可以快速、高精度地调整深度相机与激光雷达之间的相对位置关系，完成对深度相机和激光雷达的标定，从而提高深度相机与激光雷达所采集数据的转换和融合的精度，使最终生成的全局地图的精度更高。本发明装置结构简单，便于获取和构建。本发明方法克服了现有标定方法精度低、步骤复杂、可操作性差的缺点。

附图说明

图1为激光雷达和深度相机的安装位置关系图；

图2为本发明装置的空间结构图；

图3为本发明装置的俯视图和坐标关系图；

图4为激光雷达和深度相机所探测到的数据的形象示意图；

图5为本发明方法的流程图；

图中1为固定装置，2为深度相机，3为激光雷达，4为标定物，5为标定背景，激光雷达探测到的标定背景数据，7为深度相机探测到的标定背景数据，8为激光雷达探测到的标定物数据，9为深度相机探测到的标定物数据。

具体实施方式

实施例1

本实施例中，一种用于激光雷达和深度相机的标定装置，如图1所示，所述激光雷达3和深度相机2安装在一固定装置1上，所述激光雷达3和深度相机2均与主机连接，所述激光雷达3和深度相机2之间的旋转矩阵和平移向量可以调整。如图2所示，所述标定装置包括标定物4、标定背景5和主机，所述标定物4位于固定装置1和标定背景5之间并且在激光雷达3和深度相机2的视野范围内，所述主机分别与激光雷达3和深度相机2连接；所述主机用于处理激光雷达对标定物采集到的二维激光雷达数据以及深度相机对标定物采集到的三维模拟激光雷达数据，具体包括：

如图1所示，由于SLAM技术一般应用于机器人技术中，因此，固定装置1可以是底盘或者是机器人的身体等，固定装置起着固定激光雷达3和深度相机2的作用。激光雷达3与深度相机2的位置关系可以用旋转矩阵和平移向量关系来描述，其中旋转矩阵描述激光雷达3与深度相机2之间的角度位移关系，平移向量描述激光雷达3与深度相机2之间的直线位移关系。例如，可以设定一个参考坐标系，则在这个坐标系中激光雷达和深度相机的视野方向均对应一定的角度，可以用旋转矩阵乘以深度相机对应的角度得到激光雷达对应的角度。对激光雷达3与深度相机2进行标定，就是用合适的旋转矩阵和平移向量对激光雷达3与深度相机2之间的位置关系进行调整，使得激光雷达3与深度相机2能够更好地实现背景技术中的对含有镂空结构的障碍物的探测结果。

在本实施例中，标定物4用于为激光雷达3与深度相机2提供探测对象，标定背景5用于为激光雷达3与深度相机2的探测提供背景。标定物4可以是长方体或者其他形状的物体，标定背景可以是墙体或者其他板状物体。

图3为本发明标定装置的俯视图。

激光雷达与深度相机所探测到的数据，传输给主机以进行处理。激光雷达与深度相机所探测到的数据可以用图4来进行形象表示，图4中6为激光雷达探测到的标定背景数据，7为深度相机探测到的标定背景数据，8为激光雷达探测到的标定物数据，9为深度相机探测到的标定物数据。

实施例2

本实施例中，一种用于激光雷达和深度相机的标定方法，如图5所示，包括以下步骤：

激光雷达可以采用2D激光雷达，其能够直接采集到二维激光雷达数据。深度相机所探测的信息中含有深度方向的信息，是三维的，而且对激光雷达的建图具有辅助作用，因此深度相机采集到的数据是三维模拟激光雷达数据.

步骤S3中，可以在图3所示的坐标系中进行。图3中，以深度相机为原点建立的坐标系，Y轴方向为深度相机成像面的法线方向，XY平面与地面平行，图中未画出的Z轴与XY平面垂直。步骤S3中将三维模拟激光雷达数据映射到平面，可以是将三维模拟激光雷达数据的Y方向数据置零，便得到XZ平面上的数据，即二维模拟激光雷达数据。还可以对所得的XZ平面上的数据做进一步处理，即以图像列编列XZ平面上的数据，取离深度相机距离最近的点。

由于所得的二维激光雷达数据和二维模拟激光雷达数据在水平方向，即X方向上一般都是均匀分布的，因此可以对其进行降维，即只保留Z方向上的数据以减少数据量。也可以只保留X方向上的数据。经过降维处理，便得到一维激光雷达数据和一维模拟激光雷达数据。

针对第一标定数据和第二标定数据所用的迭代最近点算法可以是ICP(IterativeClosest Point)算法。

进一步作为优选的实施方式，所述步骤S6之后，还包括以下步骤：

进一步作为优选的实施方式，所述步骤S3中，所述平面为激光雷达成像面所在平面。

激光雷达成像面所在平面，即图3所示的XZ平面。

进一步作为优选的实施方式，所述步骤S4中：

无论是二维的还是一维的模拟激光雷达数据，都含有n个元素；无论是二维的还是一维的激光雷达数据，都含有m个元素。因为激光雷达和深度相机的工作原理不同，同视角下深度相机所采集到的数据点一般远多于激光雷达，即n>>m。

在步骤S4中，还可以针对一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cn′}和一维激光雷达数据L′{p_l1′,p_l2′,…,p_lm′}分别做筛选处理，其具体步骤为：针对激光雷达和深度相机在共同可视范围θ内采集并处理得到的一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cn′}和一维激光雷达数据L′{p_l1′,p_l2′,…,p_lm′}，分别去掉无效点和无穷点，保留下来的数据点下标保持原来不变。

进一步作为优选的实施方式，所述步骤S5中：

所述一维拉普拉斯算子为laplace{k,-2k,k}，其中k为自然数；

k可以按照实际使用来自定义其具体值。步骤S5中，原本含有n个元素的一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cn′}，经过以m为基数的均值滤波处理后变为含有m个元素的C′{p_c1′,p_c2′,…,p_cm′}。由于n>>m，经过均值滤波处理后，一维模拟激光雷达数据所含元素大大减少，即后面步骤需要处理的数据量大大减少，而且经过处理后的一维模拟激光雷达数据所含元素数与一维激光雷达数据相同。

进一步作为优选的实施方式，所述步骤S6具体包括：

S62.求解目标函数，从而得到旋转矩阵和平移向量。

在步骤S62中，将求解目标函数所得的旋转矩阵和平移向量作为最终结果，而所求得的旋转矩阵和平移向量与目标函数所设定的条件有关。

进一步作为优选的实施方式，所述步骤S62之后，还包括以下步骤：

为了保证最终求得的旋转矩阵和平移向量的精确性，可以对步骤S62所求得的旋转矩阵和平移向量进行验证，如果符合条件，则将其作为最终求解结果，否则，重新求解旋转矩阵和平移向量，直到所求旋转矩阵和平移向量符合条件。所求旋转矩阵和平移向量是否符合条件通过判断判别函数是否收敛来判断。

进一步作为优选的实施方式，所述目标函数为式中，p_ci′为一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cm′}的第i个元素，p_li′为一维激光雷达数据L′{p_l1′,p_l2′,…,p_lm′}的第i个元素，i为求和序号，m为一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cm′}或一维激光雷达数据L′{p_l1′,p_l2′,…,p_lm′}的元素总数，R为旋转矩阵，T为平移向量，ε为目标函数的值。

可以通过优化目标函数，如使ε最小化从而求解目标函数中的旋转矩阵和平移向量。也可以设置其他求解条件，例如使ε小于某一设定的值，从而求得对应的旋转矩阵和平移向量。

进一步作为优选的实施方式，所述重投影数据为C″′{p″′_c1,p″′_c2,…,p″′_cn}，重投影数据所述重投影数据通过下式计算得到：

通过p″′_ci＝Rp_ci′+T来求得重投影数据中各个元素，式中，R为最近一次执行步骤S62求得的旋转矩阵，T为最近一次执行步骤S62求得的平移向量。重投影数据可以一共有n个元素，即最终求得的重投影数据为C″′{p″′_c1,p″′_c2,…,p″′_cn}。

判别函数的表达式为式中β为自定义正整数，β

的意义为从重投影数据C″′{p″′_c1,p″′_c2,…,p″′_cn}中所选择的用来代入判别函数进行计算的连续数据点的个数，从重投影数据中选择连续数据点的方式可以是任意的。判别函数小于任意小正整数，等价于判别函数收敛。

以上是对本发明的较佳实施进行了具体说明，但对本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims

1.一种用于激光雷达和深度相机的标定装置，所述激光雷达和深度相机安装在一固定装置上，所述激光雷达和深度相机之间的旋转矩阵和平移向量可以调整，其特征在于，包括标定物、标定背景和主机，所述标定物位于固定装置和标定背景之间并且在激光雷达和深度相机的视野范围内，所述主机分别与激光雷达和深度相机连接；所述主机用于处理激光雷达对标定物采集到的二维激光雷达数据以及深度相机对标定物采集到的三维模拟激光雷达数据，具体包括：

2.一种用于激光雷达和深度相机的标定方法，其特征在于，包括以下步骤：

3.根据权利要求2所述的一种用于激光雷达和深度相机的标定方法，其特征在于，所述步骤S6之后，还包括以下步骤：

4.根据权利要求2所述的一种用于激光雷达和深度相机的标定方法，其特征在于，所述步骤S3中，所述平面为激光雷达成像面所在平面。

5.根据权利要求2所述的一种用于激光雷达和深度相机的标定方法，其特征在于，所述步骤S4中：

6.根据权利要求5所述的一种用于激光雷达和深度相机的标定方法，其特征在于，所述步骤S5中：

所述一维拉普拉斯算子为laplace{k,-2k,k}，其中k为自然数；

7.根据权利要求6所述的一种用于激光雷达和深度相机的标定方法，其特征在于，所述步骤S6具体包括：

S62.求解目标函数，从而得到旋转矩阵和平移向量。

8.根据权利要求7所述的一种用于激光雷达和深度相机的标定方法，其特征在于，所述步骤S62之后，还包括以下步骤：

9.根据权利要求7或8所述的一种用于激光雷达和深度相机的标定方法，其特征在于：

所述目标函数为式中，p_ci′为一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cm′}的第i个元素，p_li′为一维激光雷达数据L′{p_l1′,p_l2′,…,p_lm′}的第i个元素，i为求和序号，m为一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cm′}或一维激光雷达数据L′{p_l1′,p_l2′,…,p_lm′}的元素总数，R为旋转矩阵，T为平移向量，ε为目标函数的值。

10.根据权利要求8所述的一种用于激光雷达和深度相机的标定方法，其特征在于：

所述重投影数据为C″′{p″′_c1,p″′_c2,…,p″′_cn}，重投影数据所述重投影数据通过下式计算得到：

p″′_ci＝Rp_ci′+T，式中，p″′_ci为重投影数据的第i个元素，p_ci′为一维模拟激光雷达数据C′{p_c1′,p_c2′,…,p_cm′}的第i个元素，R为旋转矩阵，T为平移向量；