CN115661269A - 相机与激光雷达的外参标定方法、装置及存储介质 - Google Patents

Abstract

本发明公开了一种相机与激光雷达的外参标定方法、装置及存储介质，其中，该方法包括：获取激光雷达扫描本体前方垂直面的第一点云数据、第一ToF相机扫描本体前方水平面以及前方垂直面的第二点云数据、第二ToF相机扫描本体前方垂直面的第三点云数据；根据第一点云数据拟合得到第一直线；根据第二点云数据拟合得到第一水平面和第一垂直平面；根据第三点云数据拟合得到第二垂直平面；将拟合的直线和平面转换至本体坐标系下，在本体坐标系下构建面与线的几何约束关系对第一ToF相机、第二ToF相机的外参进行标定，通过本发明，解决了相关技术中多相机融合外参标定不方便的技术问题，使得多相机外参标定更快速，多相机融合感知更加精确。

Description

相机与激光雷达的外参标定方法、装置及存储介质

技术领域

本发明涉及传感器标定技术领域，尤其涉及一种相机与激光雷达的外参标定方法、装置及存储介质。

背景技术

在移动机器人系统中，感知模块是整个系统安全、高效运行的核心要素，它就像机器人的“眼睛”，不断获取周围物体信息，并分析和理解所处的环境，给规划与控制模块提供实时精准的障碍物信息，使得移动机器人能够自主高效安全地完成各种复杂的任务。

在移动机器人上获取周围物体信息的途径就是搭载各种不同的传感器，常用的有2D激光雷达、3D激光雷达、超声波、红外线传感器、单目相机、双目相机、ToF相机以及结构光相机等。确定在移动机器人上搭载哪些传感器，需要从用途、成本、安全、效率以及运行场景等多个方面综合考虑，室内移动机器人通常会选择2D激光雷达与多ToF相机的组合。其中，2D激光雷达也称单线激光雷达，其工作原理是通过激光发射器发射出一束激光，光束遇到物体后，经过漫反射，返回至激光接收器，雷达模块根据发送和接收信号的时间差乘以光速再除以2，即可计算出发射器与物体的距离。2D激光雷达具有结构简单、体积小、功耗低、扫描快、角分辨率高、数据量小以及成本低等优点，在移动机器人上搭载2D激光雷达除了给感知模块提供周围物体信息之外，还可以结合IMU以及轮速计等其他机载设备，通过定位模块给系统提供实时的位置信息。ToF相机是深度相机的一种，ToF是“Time of Fligth”的缩写，直译为“飞行时间”，其工作原理是通过给目标连续发送光脉冲，然后用传感器接收从物体返回的光，通过光线发射和接收时间差或相位差来计算目标物体的距离。这种技术跟激光传感器原理基本类似，只不过激光传感器是逐点扫描，而TOF相机则是同时得到整幅图像的深度信息。相对于普通相机，ToF相机生成的图像通过距离信息能有效区分前景与后景，方便快速完成对目标图像的分割、标记、识别以及跟踪，但是它的测量距离较短，一般不超过10m，因此一般会在移动机器人上搭载多个ToF相机，常见的就是搭载两个ToF相机，一个倾斜一定角度照向地面，用于识别近距离的低矮障碍物，另一个照向正前方，用于识别正前方以及悬空的障碍物。

移动机器人采用2D激光雷达与多ToF相机融合感知能使得整个系统长时间稳定安全地运行，但前提是需要精准地标定2D激光雷达与ToF相机之间的转换关系。目前，2D激光雷达与相机之间的标定算法主要分为基于运动的方法和基于特征的方法，基于运动的方法不要求传感器之间具备共视区域，可以视为手眼标定问题，但需要特定装置，标定过程复杂且耗时。基于特征的方法要求传感器之间具备共视区域，同时需要借助特定的标定板，构建点到面、点到线以及线到面等各种不同的约束，通过非线性最小二乘法进行求解，该方法对环境光照比较敏感，同时需要可靠的初值。因此，相关技术中的两种方法都不方便快速二次标定。

发明内容

本发明实施例提供了一种相机与激光雷达的外参标定方法、装置及存储介质，以解决相关技术中多相机融合外参标定不方便的技术问题。

根据本发明实施例的一个方面，提供了一种相机与激光雷达的外参标定方法，所述方法包括：获取激光雷达扫描本体前方垂直面的第一点云数据、第一ToF相机扫描本体前方水平面以及前方垂直面的第二点云数据、第二ToF相机扫描本体前方垂直面的第三点云数据，其中，所述第二点云数据中包括第一ToF相机前方水平的第一平面点云数据以及前方垂直的第二平面点云数据，其中，所述激光雷达和所述第一ToF相机，第二ToF相机设置在所述本体上；根据所述第一点云数据拟合得到所述本体正前方的第一直线；根据所述第二点云数据拟合得到所述本体前方的第一水平面和第一垂直平面；根据所述第三点云数据拟合得到所述本体前方的第二垂直平面；采用初始外参将所述第一直线、所述第一水平面、所述第一垂直平面、所述第二垂直平面转换成本体坐标系下的第二直线、第二水平面、第三垂直平面、第四垂直平面；构建所述第二直线、所述第二水平面、所述第三垂直平面、所述第四垂直平面之间的线面几何约束关系，并根据所述线面几何约束关系计算得到标定外参，其中，所述标定外参用于指示所述第一ToF相机或所述第二ToF相机相对所述激光雷达的位置关系。

可选地，根据所述第一点云数据拟合得到所述本体正前方的第一直线，包括：从所述第一点云数据中的起始点开始，选择所述起始点以及所述起始点之后预设数量的点组成第一点集合；根据所述第一点集合拟合生成第一直线方程；判断所述第一直线方程是否符合拟合要求；若所述第一直线方程符合拟合要求，则执行以下步骤，直到执行到不符合拟合要求的中间点或终止点：分别向前和向后遍历所述第一点集合的相邻点，其中，所述相邻点包括向前遍历的前相邻点和向后遍历的后相邻点；判断所述第一点集合的相邻点是否符合拟合要求；若所述相邻点符合拟合要求，则采用所述相邻点加入所述第一点集合组成新的第一点集合，并重新拟合生成新的直线方程；针对不符合拟合要求的中间点，保留所述中间点之前的直线方程，并将向前遍历终止的中间点作为起始点，继续拟合生成直线方程，直到终止点；对得到的若干个直线方程进行合并，得到合并后的直线方程集合；从合并后的直线方程集合中选择所述激光雷达正前方激光点所在的直线方程作为第一直线。

可选地，判断所述第一直线方程是否符合拟合要求，包括：判断所述第一点集合中的各点到所述第一直线方程的第一距离是否均小于或等于第一预设阈值；判断所述第一点集合中的各点到交点的第二距离是否均小于或等于第二预设阈值，其中，所述交点是经过对应点的激光射线与所述第一直线方程的交点；判断所述第一点集合中任意相邻点的第三距离是否均小于或等于第三预设阈值；若所述第一距离均小于或等于第一预设阈值，所述第二距离均小于或等于第二预设阈值，且所述第三距离均小于或等于第三预设阈值，则认为所述第一直线方程符合拟合要求。

可选地，对得到的若干个直线方程进行合并，包括：遍历所述若干个直线方程，若所述若干个直线方程中第一相邻两直线存在重合点，则获取所述第一相邻两直线的斜率；判断所述第一相邻两直线的斜率的差值是否小于第四预设阈值；若所述第一相邻两直线的斜率的差值小于第四预设阈值，则采用所述第一相邻两直线的所有点重新拟合生成新的直线方程，并采用新的直线方程替换所述第一相邻两直线，得到第一直线方程集合；遍历所述第一直线方程集合，若所述第一直线方程集合中第二相邻两直线存在重合点，则遍历所述第二相邻两直线的重合点；获取第二相邻两直线各重合点与第二相邻两直线中前一直线方程的第四距离，以及与后一直线方程的第五距离；若第四距离小于或等于第五距离，则将重合点从后一直线方程的点集中删除，直至遍历到第四距离大于第五距离的第一重合点，则将第一重合点以及第一重合点之后的重合点从前一直线方程的点集中删除，得到第二直线方程集合；采用RANSAC方法重新拟合第二直线方程集合中各直线上的点集，得到合并后的第三直线方程集合。

可选地，根据所述第二点云数据拟合得到所述本体前方的第一水平面和第一垂直平面，包括：针对第二点云数据中的每个点，获取每个点的邻近点集；根据每个点的邻近点集计算每个点的法向量，得到包含有法向量信息的第三点云数据；采用八叉树数据结构对所述第三点云数据递归细分空间，直至所述八叉树数据结构的每个叶子节点中包含的点个数小于预设个数；从所述八叉树数据结构的叶子节点开始，对每个叶子节点中包含的点集合进行平面检测；针对每个叶子节点，若叶子节点中的点集合平面检测成功，则根据所述叶子节点中的点集合拟合得到平面表达式；根据得到的多个叶子节点的平面表达式，得到平面片集合；从所述平面片集合中选择符合预设条件的多个平面片进行合并；从合并后的平面片集合中获取点个数最多且超过第五预设阈值的第一平面表达式以及第二平面表达式；采用初始外参将所述第一平面表达式以及所述第二平面表达式转换为本体坐标系下的第三平面表达式、第四平面表达式；采用预设辅助向量校正第三平面表达式、第四平面表达式的法向量方向；根据校正方向后的法向量从所述第一平面表达式以及所述第二平面表达式中分别确定第一水平面以及第一垂直平面。

可选地，对每个叶子节点中包含的点集合进行平面检测包括：针对每个叶子节点中包含的点集合进行平面拟合，采用所述点集合中各点位置中位数以及法向量中位数作为拟合平面的中心点以及法向量；计算所述点集合中的各点到所述拟合平面的第六距离；根据所述第六距离统计所述第六距离的中位数绝对偏差；根据所述第六距离的中位数绝对偏差计算最大距离偏差；根据所述最大距离偏差计算最大距离偏差角度；判断所述最大距离偏差角度是否小于第六预设阈值；若所述最大距离偏差角度小于第六预设阈值，则平面检测失败；若所述最大距离偏差角度大于或等于第六预设阈值，则计算所述点集合中每个点的法向量与所述拟合平面的法向量之间的第一夹角；根据所述第一夹角统计所述第一夹角的中位数绝对偏差；根据所述第一夹角的中位数绝对偏差计算最大法向量偏差；判断所述最大法向量偏差是否大于第七预设阈值；若所述最大法向量偏差大于第七预设阈值，则平面检测失败；若所述最大法向量偏差小于或等于第七预设阈值，则统计所述第六距离不在第一预设区间范围的第一个数百分比，以及统计所述第一夹角不在第二预设区间范围的第二个数百分比；若所述第一个数百分比大于第八预设阈值，或者所述第二个数百分比大于第九预设阈值，则平面检测失败；否则，则平面检测成功。

可选地，从所述平面片集合中选择符合预设条件的多个平面片进行合并包括：计算所述平面片集合中各平面片的最大法向量偏差；根据各平面表达式的最大法向量偏差对各平面片进行排序，得到第一平面片集合；遍历第一平面片集合中的各平面片，针对平面片π_i，获取π_i点集合中各点p_i的邻近点集；针对邻近点集中的各邻近点p_j，判断p_j是否属于第一平面片集合中的任一平面片；若p_j不属于第一平面片集合中的任一平面片，则计算p_j到平面片π_i的距离；若p_j到平面片π_i的距离小于第十预设阈值，则计算p_j法向量与平面片π_i法向量的夹角；若p_j法向量与平面片π_i法向量的夹角小于第十一预设阈值，则确认p_j属于平面片π_i；采用p_j更新平面片π_i中的点集合，得到第二平面片集合；针对第二平面片集合中各平面片，计算任一平面片π_i中各点到另一平面片π_j中各点的距离；若π_i和π_j存在距离小于第十二预设阈值的两点，则计算π_i法向量和π_j法向量的夹角；若π_i法向量和π_j法向量的夹角小于π_i最大法向量偏差且小于π_j最大法向量偏差，则采用π_i和π_j中的所有点重新拟合生成新的平面表达式，并采用新的平面表达式替换平面片π_i和π_j，得到合并后的第三平面片集合。

可选地，构建所述第二直线、所述第二水平面、所述第三垂直平面、所述第四垂直平面之间的线面几何约束关系，并根据所述线面几何约束关系计算得到标定外参，包括：将所述第二水平面的法向量旋转至与本体坐标系Z轴平行，并测量第一旋转角度；将所述第三垂直平面的法向量旋转至与所述第二直线的方向向量垂直，并测量第二旋转角度；在本体坐标系下将所述第三垂直平面的中心点沿坐标轴方向平移至所述第二直线上，并测量得到第一平移距离；根据所述第一旋转角度、第二旋转角度、第一平移距离得到第一ToF相机的第一标定外参；将所述第四垂直平面的法向量旋转至与本体坐标系Z轴垂直，并测量第三旋转角度；将所述第四垂直平面的法向量旋转至与所述第二直线的方向向量垂直，并测量第四旋转角度；在本体坐标系下将所述第四垂直平面的中心点沿坐标轴方向平移至所述第二直线上，并测量得到第二平移距离；根据所述第三旋转角度、第四旋转角度、第二平移距离得到第二ToF相机的第二标定外参。

根据本发明实施例的另一个方面，提供了一种相机与激光雷达的外参标定装置，包括：第一获取模块，用于获取激光雷达扫描本体前方垂直面的第一点云数据；第二获取模块，用于获取第一ToF相机扫描本体前方水平面以及前方垂直面的第二点云数据，其中，所述第二点云数据中包括第一ToF相机前方水平的第一平面点云数据以及前方垂直的第二平面点云数据；第三获取模块，用于获取第二ToF相机扫描本体前方垂直面的第三点云数据，其中，所述激光雷达和所述第一ToF相机，第二ToF相机设置在所述本体上；第一拟合模块，用于根据所述第一点云数据拟合得到所述本体正前方的第一直线；第二拟合模块，用于根据所述第二点云数据拟合得到所述本体前方的第一水平面和第一垂直平面；第三拟合模块，用于根据所述第三点云数据拟合得到所述本体前方的第二垂直平面；转换模块，用于采用初始外参将所述第一直线、所述第一水平面、所述第一垂直平面、所述第二垂直平面转换成本体坐标系下的第二直线、第二水平面、第三垂直平面、第四垂直平面；构建模块，用于构建所述第二直线、所述第二水平面、所述第三垂直平面、所述第四垂直平面之间的线面几何约束关系；计算模块，用于根据所述线面几何约束关系计算得到标定外参，其中，所述标定外参用于指示所述第一ToF相机或所述第二ToF相机相对所述激光雷达的位置关系。

根据本发明实施例的另一方面，还提供了一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的相机与激光雷达的外参标定方法的步骤。

本发明实施例还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述方法中的步骤。

本发明实施例通过激光雷达、第一ToF相机、第二ToF相机采集本体前方垂直面或者水平面的点云数据，对采集的点云数据进行直线或者平面的拟合，之后根据本体坐标系下直线和平面的几何约束关系，对第一ToF相机、第二ToF相机的外参进行标定，使得多相机融合的外参更加精确，本发明实施例不需要特定装置，通过从激光雷达中提取出的线特征以及从ToF相机中提取出的面特征，构建线与面之间的几何约束即可快速准确完成激光雷达与相机之间的外参标定，使得标定更方便高效。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明实施例的一种计算机的硬件结构框图；

图2是根据本发明实施例的一种相机与激光雷达的外参标定方法的流程图；

图3是根据本发明实施例一种相机与激光雷达的外参标定方法应用的场景示意图；

图4为根据本发明实施例的一种外参标定坐标系转换示意图；

图5是根据本发明实施例的一种相机与激光雷达的外参标定装置。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

实施例1

本申请实施例所提供的方法实施例可以在手机、计算机、平板、服务器或者类似的运算装置中执行。以运行在计算机上为例，图1是本发明实施例的一种计算机的硬件结构框图。如图1所示，计算机可以包括一个或多个（图1中仅示出一个）处理器102（处理器102可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置）和用于存储数据的存储器104，可选地，上述计算机还可以包括用于通信功能的传输设备106以及输入输出设备108。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述计算机的结构造成限定。例如，计算机还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。

存储器104可用于存储计算机程序，例如，应用软件的软件程序以及模块，如本发明实施例中的一种相机与激光雷达的外参标定方法对应的计算机程序，处理器102通过运行存储在存储器104内的计算机程序，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至计算机。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

传输设备106用于经由一个网络接收或者发送数据。上述的网络具体实例可包括计算机的通信供应商提供的无线网络。在一个实例中，传输设备106包括一个网络适配器（Network Interface Controller，简称为NIC），其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输设备106可以为射频（Radio Frequency，简称为RF）模块，其用于通过无线方式与互联网进行通讯。

在本实施例中提供了一种相机与激光雷达的外参标定方法，图2是根据本发明实施例的一种相机与激光雷达的外参标定方法的流程图，如图2所示，该流程包括如下步骤：

S1，获取激光雷达扫描本体前方垂直面的第一点云数据、第一ToF相机扫描本体前方水平面以及前方垂直面的第二点云数据、第二ToF相机扫描本体前方垂直面的第三点云数据，其中，所述第二点云数据中包括第一ToF相机前方水平的第一平面点云数据以及前方垂直的第二平面点云数据，其中，所述激光雷达和所述第一ToF相机，第二ToF相机设置在所述本体上；

参照图3，图3是根据本发明实施例一种相机与激光雷达的外参标定方法应用的场景示意图，如图3所示，该场景中移动机器人本体上搭载了一个2D激光雷达以及两个ToF相机，其中，第一ToF相机倾斜一定角度照向地面，用于识别移动机器人前下方近距离的低矮障碍物，第二ToF相机照向正前方，用于识别正前方以及悬空的障碍物。本实施场景中机器人被放置在一堵墙前，其中，墙面与水平面垂直，通过控制机器人与墙面的距离，使得机器人上倾斜一定角度照向地面安装的第一ToF相机可以同时观测到地面和墙面，而照向正前方安装的第二ToF相机可以观测到正前方的墙面。可理解地，本示例中第一ToF相机、第二ToF相机和2D激光雷达扫描的垂直面为同一个垂直面，但扫描区域可能存在差异，第一ToF相机扫描的点云数据中既包括地面的点云数据也包括墙面的点云数据。

S2，根据所述第一点云数据拟合得到所述本体正前方的第一直线；

激光雷达扫描本体前方垂直面的第一点云数据为各激光射线依次水平照射前面墙面得到的相对于激光雷达坐标的直角坐标（x_i，y_i）值，根据各激光点的（x_i，y_i）值可以采用最小二乘法、梯度下降法，或者其它的拟合方法进行直线拟合，从而得到激光雷达水平扫描前方垂直面的第一直线的直线方程式。

在一示例中，获取激光雷达扫描本体前方垂直面的第一点云数据之前，还包括对激光雷达原始数据进行预处理，具体地，一帧2D激光的原始数据通常包含时间戳t、扫描起始角度θ_s，扫描终止角度θ_e、角度增量d_θ、时间增量d_t、扫描周期t_s、最大视距r_max、最小视距r_min以及距离值集合{…r_i…}。每个点的角度值θ_i可以通过θ_s、θ_e和d_θ得到，每个激光点的直角坐标（x_i，y_i）可以通过距离值r_i与角度值θ_i得到。由于不同激光雷达的扫描起止角度可能有所不同，扫描方向也可能不相同，因此需要按照固定顺序对激光点进行重排，经过数据预处理得到激光点集合第一点云数据Scan{…s_i…}，其中，

θ_i=θ_s+i*d_θ，θ_i＜θ_e；

x_i=r_i*cos（θ_i）；

y_i=r_i*sin（θ_i）；

Scan{…s_i（i，r_i，θ_i，x_i，y_i）…}，θ_i＜θ_i+1。

S3，根据所述第二点云数据拟合得到所述本体前方的第一水平面和第一垂直平面；

TOF相机可以同时得到整幅图像的深度信息。第二点云数据中包括墙面的点云数据，还包括地面的点云数据，因此，需要在第二ToF相机的第二点云数据中拟合出两个平面。

S4，根据所述第三点云数据拟合得到所述本体前方的第二垂直平面；

第三点云数据为第二ToF相机扫描前方垂直墙面的数据，根据第三点云数据拟合出一个平面。

其中，根据点云数据拟合得到平面的方式可以是通过最小二乘法或者其它的拟合方法实现。

S5，采用初始外参将所述第一直线、所述第一水平面、所述第一垂直平面、所述第二垂直平面转换成本体坐标系下的第二直线、第二水平面、第三垂直平面、第四垂直平面；

本示例中传感器（激光雷达以及ToF相机）安装在机器人上，初始外参指各传感器坐标系相对机器人本体坐标系的位置和姿态，即通过初始外参可以将传感器坐标系调整至机器人的本体坐标系，初始外参一般是在设计机器人的结构时就设计出来的，但由于生产、加工、安装等因素导致传感器相对机器人的位置和姿态与设计值有所偏差，并且，机器人上安装有多个传感器时，多个传感器需要融合感知，而在机器人使用过程中，对传感器或者机器人进行拆装不可避免地会导致传感器的外参发生改变，因此，直接使用初始外参可能会导致严重的误差。

本实施例中先采用各相机的初始外参将上述步骤中拟合得到的第一直线、第一水平面、第一垂直平面、第二垂直平面转换到机器人本体坐标系下的第二直线、第二水平面、第三垂直平面、第四垂直平面，之后，根据第二直线、第二水平面、第三垂直平面、第四垂直平面的线面关系计算得到标定外参，通过标定外参对机器人上的各传感器进行更精确的标定。

S6，构建所述第二直线、所述第二水平面、所述第三垂直平面、所述第四垂直平面之间的线面几何约束关系，并根据所述线面几何约束关系计算得到标定外参，其中，所述标定外参用于指示所述第一ToF相机或所述第二ToF相机相对所述激光雷达的位置关系。

根据图3实施场景的部署，ToF相机与2D激光雷达扫描的为同一垂直墙面，并且第一ToF相机扫描的地面与墙面垂直，因此，机器人本体坐标系下的第二直线、第二水平面、第三垂直平面、第四垂直平面存在如第二水平面的法向量与第二直线的方向向量垂直，第三垂直平面或者第四垂直平面的法向量与第二直线的方向向量垂直，第三垂直平面或者第四垂直平面与第二直线的距离为0等的线面几何约束关系，但是，由于初始外参存在误差，上述的线面几何约束关系一般不满足，因此，本示例中构建第二直线、第二水平面、第三垂直平面、第四垂直平面之间的线面几何约束关系，通过构建的线面几何约束关系来调整各传感器的位置姿态，比如，通过测量将第二水平面的法向量旋转至与本体坐标系Z轴平行的旋转角度来标定第一ToF相机的外参，通过测量将第四垂直面的法向量旋转至与第二直线方向向量垂直的旋转角度来标定第二ToF相机的外参等。

本发明实施例通过激光雷达、第一ToF相机、第二ToF相机采集本体前方垂直面或者水平面的点云数据，对采集的点云数据进行直线或者平面的拟合，之后根据本体坐标系下直线和平面的几何约束关系，对第一ToF相机、第二ToF相机的外参进行标定，使得相机的外参更加精确，并且，不需要特定装置，扫描同一垂直面即可快速完成标定，标定更方便高效。

在一示例中，根据所述第一点云数据拟合得到所述本体正前方的第一直线，S2，包括：

S21，从所述第一点云数据中的起始点开始，选择所述起始点以及所述起始点之后预设数量的点组成第一点集合；

S22，根据所述第一点集合拟合生成第一直线方程；

本示例从激光雷达的第一点云数据Scan{…s_i…}中选择一个起始点s_i，从起始点s_i开始，利用当前起始点s_i及起始点后续3个点作为初始种子点组成第一点集合（s_i，s_i+1，s_i+2，s_i+3），根据第一点集合（s_i，s_i+1，s_i+2，s_i+3）拟合直线ax+by+c=0。

其中，利用N个点{…（x_i，y_i）…}拟合一条直线的公式如下：

S23，判断所述第一直线方程是否符合拟合要求；

一般地，可以借助相关系数，比如皮尔逊相关系数，或者标准偏差来判断拟合的第一直线方程是否符合拟合要求，比如相关系数越趋近于1，标准偏差越趋近于0表示第一直线方程越符合拟合要求。

在本发明实施例一示例中，判断所述第一直线方程是否符合拟合要求，S23，包括：

判断所述第一点集合中的各点到所述第一直线方程的第一距离是否均小于或等于第一预设阈值；判断所述第一点集合中的各点到交点的第二距离是否均小于或等于第二预设阈值，其中，所述交点是经过对应点的激光射线与所述第一直线方程的交点；判断所述第一点集合中任意相邻点的第三距离是否均小于或等于第三预设阈值；若所述第一距离均小于或等于第一预设阈值，所述第二距离均小于或等于第二预设阈值，且所述第三距离均小于或等于第三预设阈值，则认为所述第一直线方程符合拟合要求。

本示例中，通过判断第一点集合中的各种子点是否在拟合的直线上来判断拟合的第一直线方程是否符合拟合要求。其中，判断第一点集合中的各种子点是否在拟合的直线上的判断条件有三个，一是各种子点到拟合直线的距离d₁是否满足要求；二是种子点到该点激光射线与拟合直线的交点的距离d₂是否满足要求；三是相邻两个种子点的距离d₃是否满足要求，如果d₁、d₂、d₃均满足要求，即均小于或等于预设的参考阈值，则认为种子点均在拟合直线上，所述第一直线方程符合拟合要求。

其中，第一距离d₁、第二距离d₂、第三距离d₃通过如下公式计算得到：

在一实施方式中，d₁、d₂、d₃预设的参考阈值可以取0.03，0.1，0.03，可理解地，不同激光雷达精度不同可能需要微调，在具体实施方式中，本领域技术人员可根据实际情况设置参考阈值。

若第一直线方程不符合拟合要求，则将起始点之后的下一点更新为起始点，并采用更新的起始点继续拟合得到直线方程。

S24，若所述第一直线方程符合拟合要求，则执行以下步骤，直到执行到不符合拟合要求的中间点或终止点：分别向前和向后遍历所述第一点集合的相邻点，其中，所述相邻点包括向前遍历的前相邻点和向后遍历的后相邻点；判断所述第一点集合的相邻点是否符合拟合要求；若所述相邻点符合拟合要求，则采用所述相邻点加入所述第一点集合组成新的第一点集合，并重新拟合生成新的直线方程；

S25，针对不符合拟合要求的中间点，保留所述中间点之前的直线方程，并将向前遍历终止的中间点作为起始点，继续拟合生成直线方程，直到终止点；

若初始点拟合的第一直线方程符合拟合要求，则进一步提取完整直线，分别向前和向后遍历相邻激光点，若相邻激光点到拟合的第一直线方程的距离d₁满足要求，即小于或等于预设的阈值，就加入该相邻点重新拟合新的直线，若相邻激光点不满足拟合要求则终止遍历，并记录向前遍历终止时的激光点位置j，从第j个激光点开始，将j点作为起始点，继续执行S21，直到遍历完整个Scan{…s_i…}，输出拟合的直线方程集合L{…l_i：ax+by+c=0…}。

S26，对得到的若干个直线方程进行合并，得到合并后的直线方程集合；

在得到直线方程集合L后，本示例还需要对直线方程集合L中的直线进行合并。其中，对得到的直线方程集合L中若干个直线方程进行合并，S26，包括：

S261，遍历所述若干个直线方程，若所述若干个直线方程中第一相邻两直线存在重合点，则获取所述第一相邻两直线的斜率；

S262，判断所述第一相邻两直线的斜率的差值是否小于第四预设阈值；

S263，若所述第一相邻两直线的斜率的差值小于第四预设阈值，则采用所述第一相邻两直线的所有点重新拟合生成新的直线方程，并采用新的直线方程替换所述第一相邻两直线，得到第一直线方程集合；

本示例中先合并斜率差异小存在重合点的相邻两直线。通过遍历直线集合L，当相邻直线存在重合的激光点且两条直线的斜率差异满足要求时，则把两个直线合并为一条直线，同时合并两条直线的激光点并重新拟合新的直线，最终输出新的第一直线方程集合L_m。

S264，遍历所述第一直线方程集合，若所述第一直线方程集合中第二相邻两直线存在重合点，则遍历所述第二相邻两直线的重合点；

S265，获取第二相邻两直线各重合点与第二相邻两直线中前一直线方程的第四距离，以及与后一直线方程的第五距离；

S266，若第四距离小于或等于第五距离，则将重合点从后一直线方程的点集中删除，直至遍历到第四距离大于第五距离的第一重合点，则将第一重合点以及第一重合点之后的重合点从前一直线方程的点集中删除，得到第二直线方程集合；

S267，采用RANSAC方法重新拟合第二直线方程集合中各直线上的点集，得到合并后的第三直线方程集合。

在合并存在重合点且斜率差异小的相邻两直线后，本示例还包括对相邻两直线上的重合点的归属进行划分。遍历第一直线方程集合L_m，当L_m中相邻两直线存在重合的激光点时，则依次遍历重合的激光点，当重合激光点距离相邻两直线中上一条直线更近时，则将重合激光点从下一条直线中删除，将其归并到上一条直线中，直到某个重合激光点距离下一条直线更近时则停止遍历，并将距离下一条直线更近的重合激光点以及后续重合激光点均从前一直线方程中删除，归并到下一条直线中，使得每个重合点归属于一个直线方程，输出新的直线集合L_g，之后，遍历直线集合L_g，根据直线上的激光点集合，利用RANSAC方法重新拟合直线并去除外点，最终输出新的直线集合L_r。

S27，从合并后的直线方程集合中选择所述激光雷达正前方激光点所在的直线方程作为第一直线。

本示例从2D激光雷达提取的拟合直线方程集合L_r中选择正前方的激光点所处的直线l作为根据第一点云数据拟合的第一直线。

通过本示例，实现了对第一点云数据进行直线拟合，相较于直接采用全部的点云数据拟合直线，本发明实施例先构建第一点集合拟合初始直线，之后向前向后加入相邻点扩展初始拟合直线，然后对得到的若干个直线方程进行合并，本示例中直线拟合更加精细准确，误差更小。

在本发明一示例中，根据所述第二点云数据拟合得到所述本体前方的第一水平面和第一垂直平面，S3，包括：

S31，针对第二点云数据中的每个点，获取每个点的邻近点集；

S32，根据每个点的邻近点集计算每个点的法向量，得到包含有法向量信息的第三点云数据；

ToF相机能直接提供点云信息P_r，对点云P_r进行体素滤波得到第二点云数据P_v。通过knn（K-NearestNeighbor，K-临近）方法搜索第二点云数据P_v中每个点的邻近点集G{…P_j…}，并使用G{…P_j…}计算每个点的法向量（n_x，n_y，n_z），最终输出带法向量信息的点云P_n：

S33，采用八叉树数据结构对所述第三点云数据递归细分空间，直至所述八叉树数据结构的每个叶子节点中包含的点个数小于预设个数；

使用八叉数对P_n递归细分空间，直到每个叶子节点包含的点数小于阈值ϵ，其中，ϵ可以取总点云数量的0.1%。

S34，从所述八叉树数据结构的叶子节点开始，对每个叶子节点中包含的点集合进行平面检测；

从最底层的叶子节点向根节点遍历，对每个叶子节点中的点集S{…P_i…}进行平面检测，其中，对点集合进行平面检测可以是通过计算点集合中每个点到平面的距离的标准差等方式来实现。

在本发明一示例中，对每个叶子节点中包含的点集合进行平面检测，S34，包括：

S341，针对每个叶子节点中包含的点集合进行平面拟合，采用所述点集合中各点位置中位数以及法向量中位数作为拟合平面的中心点以及法向量；

根据该叶子节点中的点集合S{…P_i…}拟合平面（C，N），C为平面中心点，N为平面法向量，其中，

式中，median表示对样本集合取中位数，S_{x|y|z|nx|ny|nz}分别表示点p_i不同属性各自组成的样本集合。

S342，计算所述点集合中的各点p_i到所述拟合平面的第六距离d_i；

计算点集S{…P_i…}中每个点p_i到拟合平面（C，N）的距离D{…d_i…}，其中，

d_i=|(p_i(x，y，z)-C)·N|；

S343，根据所述第六距离d_i统计所述第六距离的中位数绝对偏差MAD（D）；

MAD（D{…d_i…}）=median（{…|d_i-median(D)|…}）;

式中，MAD（D{…d_i…}）表示对样本集合D{…d_i…}求中位数绝对偏差。

S344，根据所述第六距离的中位数绝对偏差MAD（D）计算最大距离偏差σ_d；

σ_d=3MAD（D）；

S345，根据所述最大距离偏差σ_d计算最大距离偏差角度θ_d；

其中，N_x，N_y，N_z为N的三个分量，U、V和N两两垂直，l和w分别表示拟合平面的长和宽。

S346，判断所述最大距离偏差角度θ_d是否小于第六预设阈值；

S347，若所述最大距离偏差角度θ_d小于第六预设阈值，则平面检测失败；若所述最大距离偏差角度θ_d大于或等于第六预设阈值，则计算所述点集合中每个点的法向量n_i与所述拟合平面的法向量N之间的第一夹角φ_i；

在具体实施例中，第六预设阈值可根据需要进行设置。比如，当θ_d＜75°时，点集S{…P_i…}平面检测失败。若最大距离偏差角度θ_d大于或等于第六预设阈值，则继续计算点集中每个点P_i的法向量n_i（n_x，n_y，n_z）与拟合平面的法向量N之间的夹角Φ{…φ_i…}，其中，φ_i=cos^-1（|n_i·N|）。

S348，根据所述第一夹角φ_i统计所述第一夹角的中位数绝对偏差MAD（Φ）；

MAD（Φ{…φ_i…}）=median（{…|φ_i-median（φ）|…}）；

S349，根据所述第一夹角的中位数绝对偏差MAD（Φ）计算最大法向量偏差σ_φ；

σ_φ=3MAD（Φ）；

S3410，判断所述最大法向量偏差σ_φ是否大于第七预设阈值；

S3411，若所述最大法向量偏差σ_φ大于第七预设阈值，则平面检测失败；若所述最大法向量偏差σ_φ小于或等于第七预设阈值，则统计所述第六距离d_i不在第一预设区间范围的第一个数百分比，以及统计所述第一夹角φ_i不在第二预设区间范围的第二个数百分比；

第七预设阈值可根据需要进行设置。比如，当σ_φ＞60°时，点集平面S{…P_i…}检测失败。若最大法向量偏差小于或等于第七预设阈值，则继续统计点集S{…P_i…}中每个点的距离偏差落在第一预设区间范围δ_d之外的第一个数百分比ρ_d以及法向量角度偏差落在第二预设区间范围δ_φ之外的第二个数百分比ρ_φ，其中，

δ_d=[median（D）-σ_d，median（D）+σ_d]；

δ_φ=[median（D）-σ_φ，median（D）+σ_φ]；

S3412，若所述第一个数百分比大于第八预设阈值，或者所述第二个数百分比大于第九预设阈值，则平面检测失败；否则，则平面检测成功。

当ρ_d＞25%或者ρ_φ＞25%时，点集平面S{…P_i…}检测失败，否则平面检测成功。

通过上述步骤S331至S3312，实现了对每个叶子节点中包含的点集合进行平面检测。

S35，针对每个叶子节点，若叶子节点中的点集合平面检测成功，则根据所述叶子节点中的点集合拟合得到平面表达式；

如果叶子节点中包含的点集合平面检测成功，则拟合得到平面表达式（C，N），将该叶子节点中的点集合添加到平面片集合∏中，并停止从该叶子节点向上遍历。

在一示例中，若一个父节点下所有的叶子节点的点集合平面检测均不成功，则删除所述父节点下的所有叶子节点，该父节点退化为叶子节点继续向上遍历，重新进行平面检测，直到叶子节点中的点集合平面检测成功。

S36，根据得到的多个叶子节点的平面表达式，得到平面片集合；

S37，从所述平面片集合中选择符合预设条件的多个平面片进行合并；

遍历平面片集合∏{…π_i…}，对于任意两个平面片π_i和π_j，如果它们满足合并条件，比如π_i的点集与π_j的点集之间至少有一对点距离小于预设阈值，π_i的法向量与π_j的法向量夹角小于预设阈值，则将π_i和π_j合并。

在本发明实施例一示例中，从所述平面片集合中选择符合预设条件的多个平面片进行合并，S37，包括：计算所述平面片集合中各平面片的最大法向量偏差；根据各平面表达式的最大法向量偏差对各平面片进行排序，得到第一平面片集合；遍历第一平面片集合中的各平面片，针对平面片π_i，获取π_i点集合中各点p_i的邻近点集；针对邻近点集中的各邻近点p_j，判断p_j是否属于第一平面片集合中的任一平面片；若p_j不属于第一平面片集合中的任一平面片，则计算p_j到平面片π_i的距离；若p_j到平面片π_i的距离小于第十预设阈值，则计算p_j法向量与平面片π_i法向量的夹角；若p_j法向量与平面片π_i法向量的夹角小于第十一预设阈值，则确认p_j属于平面片π_i；采用p_j更新平面片π_i中的点集合，得到第二平面片集合；针对第二平面片集合中各平面片，计算任一平面片π_i中各点到另一平面片π_j中各点的距离；若π_i和π_j存在距离小于第十二预设阈值的两点，则计算π_i法向量和π_j法向量的夹角；若π_i法向量和π_j法向量的夹角小于π_i最大法向量偏差且小于π_j最大法向量偏差，则采用π_i和π_j中的所有点重新拟合生成新的平面表达式，并采用新的平面表达式替换平面片π_i和π_j，得到合并后的第三平面片集合。

本示例中根据平面片的最大法向量偏差σ_φ，对平面片集合∏{…π_i…}进行排序。遍历排序后的平面片集合∏{…π_i…}，对于每个平面片π_i，遍历它的点集S{…p_i…}，对于每个点p_i，遍历它的邻近点集G{…p_j…}，对于每个邻近点p_j，如果它满足内点条件：

(1)它不属于任何平面片；

(2)它到π_i的距离小于σ_d；

(3)它与π_i法向量夹角小于σ_φ；

则判定p_j属于π_i。

遍历平面片集合∏{…π_i…}，对于任意两个平面片π_i和π_j，如果它们满足合并条件：

(1) π_i的点集与π_j的点集之间至少有一对点距离小于阈值；

(2) π_i的法向量与π_j的法向量夹角同时小于π_i和π_j的最大法向量偏差σ_φ；

则将它们进行合并。

S38，从合并后的平面片集合中获取点个数最多且超过第五预设阈值的第一平面表达式以及第二平面表达式；

S39，采用初始外参将所述第一平面表达式以及所述第二平面表达式转换为本体坐标系下的第三平面表达式、第四平面表达式；

S310，采用预设辅助向量校正第三平面表达式、第四平面表达式的法向量方向；

S311，根据校正方向后的法向量从所述第一平面表达式以及所述第二平面表达式中分别确定第一水平面以及第一垂直平面。

对于第一ToF相机，从合并后的平面片集合∏¹（上标“

”表示第一ToF相机）中获取其中点集数目满足要求的最大的两个平面π¹ _i和π¹ _j，如果没有满足要求的两个平面，则不能进行第一ToF相机的标定。利用初始外参T_bt1，将π¹ _i和π¹ _j转换到本体坐标系下得到^bπ¹ _i和^bπ¹ _j，其中，T代表转换关系，由旋转关系R和平移关系t组成，T_bt1中的下标“bt1”表示从第一ToF相机坐标系转换到本体坐标系。

预设辅助向量为^bCH，表示本体坐标系下平面中心点^bC与^bH之间的向量，其中，^bH为（0，0，h），h为机器人高度。通过平面法向量^bN与向量^bCH点积的符号，调整^bπ¹ _i和^bπ¹ _j的法向量朝向，使其朝向坐标轴正方向。根据调整方向后^bπ¹ _i和^bπ¹ _j的法向量^bN与向量^bH的夹角，其中，确定夹角小于30°的为水平面，记为^bπ¹ ₁，夹角大于60°且小于120°记为垂直平面，记为^bπ¹ ₂，其中，^bπ¹ ₁转换前的π¹ ₁为第一水平面，^bπ¹ ₂转换前的π¹ ₂为第一垂直平面。

针对第一直线l，本发明实施例利用初始的外参T_bl将第一直线l转换到本体坐标系下，得到第二直线^bl，其中，T_bl中的T代表转换关系，下标“bl”代表从激光坐标系转换到本体坐标系，^bl左上标“b”表示在本体坐标系下，记第二直线^bl的直线方程为ax+by+c=0，第二直线的方向向量为^bv(-b，a，0)，直线上的点为^bp(p_x，p_y，0)。

针对第一ToF相机，利用初始的外参T_btl将第一水平面π¹ ₁以及第一垂直平面π¹ ₂转换到本体坐标系下，得到第二水平面^bπ¹ ₁、第三垂直平面^bπ¹ ₂。

针对第二ToF相机，参考第一ToF相机的平面拟合方法从第三点云数据提取平面集合∏²，获取其中点集数目满足要求的最大的平面π² ₁，利用初始的外参T_bt2将第二垂直平面π² ₁转换到本体坐标系下，得到第四垂直平面^bπ² ₁。并参考上述示例中第一ToF相机S39调整^bπ² ₁的法向量朝向。

在得到第二直线、第二水平面、第三垂直平面、第四垂直平面后，根据场景先验几何关系，可知：

（1）^bπ¹ ₁平面为地面，平面^bπ¹ ₂和^bπ² ₁均为正前方与地面垂直的墙面，^bl为该墙面上的直线；

（2）^bπ¹ ₁的法向量^bN¹ ₁与z轴平行；

（3）^bπ¹ ₂的法向量与^bl垂直；

（4）^bl到平面^bπ¹ ₂的距离为0；

（5）^bπ² ₁的法向量与z轴垂直；

（6）^bπ² ₁的法向量与^bl垂直；

（7）^bl到平面^bπ² ₁的距离为0。

而由于初始外参存在误差，先验几何关系(2)-(7)一般都不满足，因此初始外参描述的并不是与真正的本体坐标系之间的关系，记2个ToF相机初始外参描述的本体坐标系分别为b¹ ₁和b² ₁，相应的外参分别表示为

和

。

在一示例中，构建所述第二直线、所述第二水平面、所述第三垂直平面、所述第四垂直平面之间的线面几何约束关系，并根据所述线面几何约束关系计算得到标定外参，S6，包括：

S61，将所述第二水平面的法向量旋转至与本体坐标系Z轴平行，并测量第一旋转角度；

根据先验关系（2）将第二水平面的法向量旋转至与本体坐标系Z轴平行，如图4所示，图4为根据本发明实施例的一种外参标定坐标系转换示意图，参照图4-①，可以求得坐标系b¹ ₁与中间坐标系b¹ ₂之间的旋转关系

，其中，

S62，将所述第三垂直平面的法向量旋转至与所述第二直线的方向向量垂直，并测量第二旋转角度；

根据几何关系(3)将第三垂直平面的法向量旋转至与第二直线的方向向量垂直，参照图4-②可以求得中间坐标系b¹ ₂与中间坐标系b¹ ₃之间的旋转关系

，其中，

S63，在本体坐标系下将所述第三垂直平面的中心点沿坐标轴方向平移至所述第二直线上，并测量得到第一平移距离；

根据几何关系(4)将第三垂直平面的中心点沿坐标轴（x轴）方向平移至第二直线上，使第三垂直平面的中心点与第二直线的距离为0，参照图4-③可以求得坐标系t₁与中间坐标系b¹ ₃之间的平移关系

，其中，

S64，根据所述第一旋转角度、第二旋转角度、第一平移距离得到第一ToF相机的第一标定外参；

第一TOF相机的第一标定外参为

，其中，

。

S65，将所述第四垂直平面的法向量旋转至与本体坐标系Z轴垂直，并测量第三旋转角度；

根据先验关系（5）将第四垂直面的法向量旋转至与本体坐标系Z轴垂直，同理参照

，可以求得坐标系b² ₁与中间坐标系b² ₂之间的旋转关系

，其中，

。

S66，将所述第四垂直平面的法向量旋转至与所述第二直线的方向向量垂直，并测量第四旋转角度；

根据先验关系（6）将第四垂直面的法向量旋转至与第二直线的方向向量垂直，同理参照

，可以求得坐标系b² ₂与中间坐标系b² ₃之间的旋转关系

，其中，

。

S67，在本体坐标系下将所述第四垂直平面的中心点沿坐标轴方向平移至与所述第二直线上，并测量得到第二平移距离；

根据先验关系（7）将第四垂直面的中心点沿坐标轴（x轴）方向平移至第二直线，使第四垂直平面的中心点与第二直线的距离为0，同理参照

，可以求得坐标系t₂与中间坐标系b² ₃之间的平移关系

，其中，

S68，根据所述第三旋转角度、第四旋转角度、第二平移距离得到第二ToF相机的第二标定外参。

第二TOF相机的第二标定外参为

，其中，

。

本发明实施例提出了一种基于线面几何约束的多ToF相机与2D激光雷达外参标定方法，利用机器人所处环境的一些先验线面几何关系，通过从2D激光雷达中提取出的线特征以及从ToF相机中提取出的面特征，构建线特征方向向量与面特征法向量之间的几何约束，通过这些约束以解析的形式快速准确地求得2D激光雷达与ToF相机之间的外参。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质（如ROM/RAM、磁碟、光盘）中，包括若干指令用以使得一台终端设备（可以是手机，计算机，服务器，或者网络设备等）执行本发明各个实施例所述的方法。

实施例2

在本实施例中还提供了一种操作系统的升级装置，用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图5是根据本发明实施例的一种相机与激光雷达的外参标定装置，如图5所示，该装置包括：第一获取模块100、第二获取模块200、第三获取模块300、第一拟合模块400、第二拟合模块500、第三拟合模块600、转换模块700、构建模块800、计算模块900，其中：

第一获取模块100，用于获取激光雷达扫描本体前方垂直面的第一点云数据；

第二获取模块200，用于获取第一ToF相机扫描本体前方水平面以及前方垂直面的第二点云数据，其中，所述第二点云数据中包括第一ToF相机前方水平的第一平面点云数据以及前方垂直的第二平面点云数据；

第三获取模块300，用于获取第二ToF相机扫描本体前方垂直面的第三点云数据，其中，所述激光雷达和所述第一ToF相机，第二ToF相机设置在所述本体上；

第一拟合模块400，用于根据所述第一点云数据拟合得到所述本体正前方的第一直线；

第二拟合模块500，用于根据所述第二点云数据拟合得到所述本体前方的第一水平面和第一垂直平面；

第三拟合模块600，用于根据所述第三点云数据拟合得到所述本体前方的第二垂直平面；

转换模块700，用于采用初始外参将所述第一直线、所述第一水平面、所述第一垂直平面、所述第二垂直平面转换成本体坐标系下的第二直线、第二水平面、第三垂直平面、第四垂直平面；

构建模块800，用于构建所述第二直线、所述第二水平面、所述第三垂直平面、所述第四垂直平面之间的线面几何约束关系；

计算模块900，用于根据所述线面几何约束关系计算得到标定外参，其中，所述标定外参用于指示所述第一ToF相机或所述第二ToF相机相对所述激光雷达的位置关系。

本实施例中相机与激光雷达的外参标定装置应用相机与激光雷达的外参标定方法的具体示例可以参考上述实施例及可选实施方式中所描述的示例，在此不再赘述。

需要说明的是，上述各个模块是可以通过软件或硬件来实现的，对于后者，可以通过以下方式实现，但不限于此：上述模块均位于同一处理器中；或者，上述各个模块以任意组合的形式分别位于不同的处理器中。

在本申请提供的又一实施例中，还提供了一种计算机可读存储介质，该计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述实施例中任一所述的操作系统的升级方法。

在本申请提供的又一实施例中，还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述实施例中任一所述的操作系统的升级方法。

在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本申请实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线（例如同轴电缆、光纤、数字用户线（DSL））或无线（例如红外、无线、微波等）方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质（例如固态硬盘Solid State Disk (SSD)）等。

以上所述仅为本申请的较佳实施例而已，并非用于限定本申请的保护范围。凡在本申请的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本申请的保护范围内。

以上所述仅是本申请的具体实施方式，使本领域技术人员能够理解或实现本申请。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种相机与激光雷达的外参标定方法，其特征在于，所述方法包括：

获取激光雷达扫描本体前方垂直面的第一点云数据、第一ToF相机扫描本体前方水平面以及前方垂直面的第二点云数据、第二ToF相机扫描本体前方垂直面的第三点云数据，其中，所述第二点云数据中包括第一ToF相机前方水平的第一平面点云数据以及前方垂直的第二平面点云数据，其中，所述激光雷达和所述第一ToF相机，第二ToF相机设置在所述本体上；

根据所述第一点云数据拟合得到所述本体正前方的第一直线；

根据所述第二点云数据拟合得到所述本体前方的第一水平面和第一垂直平面；

根据所述第三点云数据拟合得到所述本体前方的第二垂直平面；

采用初始外参将所述第一直线、所述第一水平面、所述第一垂直平面、所述第二垂直平面转换成本体坐标系下的第二直线、第二水平面、第三垂直平面、第四垂直平面；

构建所述第二直线、所述第二水平面、所述第三垂直平面、所述第四垂直平面之间的线面几何约束关系，并根据所述线面几何约束关系计算得到标定外参，其中，所述标定外参用于指示所述第一ToF相机或所述第二ToF相机相对所述激光雷达的位置关系。

2.如权利要求1所述的相机与激光雷达的外参标定方法，其特征在于，根据所述第一点云数据拟合得到所述本体正前方的第一直线，包括：

从所述第一点云数据中的起始点开始，选择所述起始点以及所述起始点之后预设数量的点组成第一点集合；

根据所述第一点集合拟合生成第一直线方程；

判断所述第一直线方程是否符合拟合要求；

若所述第一直线方程符合拟合要求，则执行以下步骤，直到执行到不符合拟合要求的中间点或终止点：分别向前和向后遍历所述第一点集合的相邻点，其中，所述相邻点包括向前遍历的前相邻点和向后遍历的后相邻点；判断所述第一点集合的相邻点是否符合拟合要求；若所述相邻点符合拟合要求，则采用所述相邻点加入所述第一点集合组成新的第一点集合，并重新拟合生成新的直线方程；

针对不符合拟合要求的中间点，保留所述中间点之前的直线方程，并将向前遍历终止的中间点作为起始点，继续拟合生成直线方程，直到终止点；

对得到的若干个直线方程进行合并，得到合并后的直线方程集合；

从合并后的直线方程集合中选择所述激光雷达正前方激光点所在的直线方程作为第一直线。

3.如权利要求2所述的相机与激光雷达的外参标定方法，其特征在于，判断所述第一直线方程是否符合拟合要求，包括：

判断所述第一点集合中的各点到所述第一直线方程的第一距离是否均小于或等于第一预设阈值；判断所述第一点集合中的各点到交点的第二距离是否均小于或等于第二预设阈值，其中，所述交点是经过对应点的激光射线与所述第一直线方程的交点；判断所述第一点集合中任意相邻点的第三距离是否均小于或等于第三预设阈值；

若所述第一距离均小于或等于第一预设阈值，所述第二距离均小于或等于第二预设阈值，且所述第三距离均小于或等于第三预设阈值，则认为所述第一直线方程符合拟合要求。

4.如权利要求2所述的相机与激光雷达的外参标定方法，其特征在于，对得到的若干个直线方程进行合并包括：

遍历所述若干个直线方程，若所述若干个直线方程中第一相邻两直线存在重合点，则获取所述第一相邻两直线的斜率；

判断所述第一相邻两直线的斜率的差值是否小于第四预设阈值；

若所述第一相邻两直线的斜率的差值小于第四预设阈值，则采用所述第一相邻两直线的所有点重新拟合生成新的直线方程，并采用新的直线方程替换所述第一相邻两直线，得到第一直线方程集合；

遍历所述第一直线方程集合，若所述第一直线方程集合中第二相邻两直线存在重合点，则遍历所述第二相邻两直线的重合点；

获取第二相邻两直线各重合点与第二相邻两直线中前一直线方程的第四距离，以及与后一直线方程的第五距离；

若第四距离小于或等于第五距离，则将重合点从后一直线方程的点集中删除，直至遍历到第四距离大于第五距离的第一重合点，则将第一重合点以及第一重合点之后的重合点从前一直线方程的点集中删除，得到第二直线方程集合；

采用RANSAC方法重新拟合第二直线方程集合中各直线上的点集，得到合并后的第三直线方程集合。

5.如权利要求1所述的相机与激光雷达的外参标定方法，其特征在于，根据所述第二点云数据拟合得到所述本体前方的第一水平面和第一垂直平面，包括：

针对第二点云数据中的每个点，获取每个点的邻近点集；

根据每个点的邻近点集计算每个点的法向量，得到包含有法向量信息的第三点云数据；

采用八叉树数据结构对所述第三点云数据递归细分空间，直至所述八叉树数据结构的每个叶子节点中包含的点个数小于预设个数；

从所述八叉树数据结构的叶子节点开始，对每个叶子节点中包含的点集合进行平面检测；

针对每个叶子节点，若叶子节点中的点集合平面检测成功，则根据所述叶子节点中的点集合拟合得到平面表达式；

根据得到的多个叶子节点的平面表达式，得到平面片集合；

从所述平面片集合中选择符合预设条件的多个平面片进行合并；

从合并后的平面片集合中获取点个数最多且超过第五预设阈值的第一平面表达式以及第二平面表达式；

采用初始外参将所述第一平面表达式以及所述第二平面表达式转换为本体坐标系下的第三平面表达式、第四平面表达式；

采用预设辅助向量校正第三平面表达式、第四平面表达式的法向量方向；

根据校正方向后的法向量从所述第一平面表达式以及所述第二平面表达式中分别确定第一水平面以及第一垂直平面。

6.如权利要求5所述的相机与激光雷达的外参标定方法，其特征在于，对每个叶子节点中包含的点集合进行平面检测包括：

针对每个叶子节点中包含的点集合进行平面拟合，采用所述点集合中各点位置中位数以及法向量中位数作为拟合平面的中心点以及法向量；

计算所述点集合中的各点到所述拟合平面的第六距离；

根据所述第六距离统计所述第六距离的中位数绝对偏差；

根据所述第六距离的中位数绝对偏差计算最大距离偏差；

根据所述最大距离偏差计算最大距离偏差角度；

判断所述最大距离偏差角度是否小于第六预设阈值；

若所述最大距离偏差角度小于第六预设阈值，则平面检测失败；若所述最大距离偏差角度大于或等于第六预设阈值，则计算所述点集合中每个点的法向量与所述拟合平面的法向量之间的第一夹角；

根据所述第一夹角统计所述第一夹角的中位数绝对偏差；

根据所述第一夹角的中位数绝对偏差计算最大法向量偏差；

判断所述最大法向量偏差是否大于第七预设阈值；

若所述最大法向量偏差大于第七预设阈值，则平面检测失败；若所述最大法向量偏差小于或等于第七预设阈值，则统计所述第六距离不在第一预设区间范围的第一个数百分比，以及统计所述第一夹角不在第二预设区间范围的第二个数百分比；

若所述第一个数百分比大于第八预设阈值，或者所述第二个数百分比大于第九预设阈值，则平面检测失败；否则，则平面检测成功。

7.如权利要求5所述的相机与激光雷达的外参标定方法，其特征在于，

从所述平面片集合中选择符合预设条件的多个平面片进行合并包括：

计算所述平面片集合中各平面片的最大法向量偏差；

根据各平面表达式的最大法向量偏差对各平面片进行排序，得到第一平面片集合；

遍历第一平面片集合中的各平面片，针对平面片π_i，获取π_i点集合中各点p_i的邻近点集；

针对邻近点集中的各邻近点p_j，判断p_j是否属于第一平面片集合中的任一平面片；

若p_j不属于第一平面片集合中的任一平面片，则计算p_j到平面片π_i的距离；

若p_j到平面片π_i的距离小于第十预设阈值，则计算p_j法向量与平面片π_i法向量的夹角；

若p_j法向量与平面片π_i法向量的夹角小于第十一预设阈值，则确认p_j属于平面片π_i；

采用p_j更新平面片π_i中的点集合，得到第二平面片集合；

针对第二平面片集合中各平面片，计算任一平面片π_i中各点到另一平面片π_j中各点的距离；

若π_i和π_j存在距离小于第十二预设阈值的两点，则计算π_i法向量和π_j法向量的夹角；

若π_i法向量和π_j法向量的夹角小于π_i最大法向量偏差且小于π_j最大法向量偏差，则采用π_i和π_j中的所有点重新拟合生成新的平面表达式，并采用新的平面表达式替换平面片π_i和π_j，得到合并后的第三平面片集合。

8.如权利要求1所述的相机与激光雷达的外参标定方法，其特征在于，构建所述第二直线、所述第二水平面、所述第三垂直平面、所述第四垂直平面之间的线面几何约束关系，并根据所述线面几何约束关系计算得到标定外参，包括：

将所述第二水平面的法向量旋转至与本体坐标系Z轴平行，并测量第一旋转角度；

将所述第三垂直平面的法向量旋转至与所述第二直线的方向向量垂直，并测量第二旋转角度；

在本体坐标系下将所述第三垂直平面的中心点沿坐标轴方向平移至所述第二直线上，并测量得到第一平移距离；

根据所述第一旋转角度、第二旋转角度、第一平移距离得到第一ToF相机的第一标定外参；

将所述第四垂直平面的法向量旋转至与本体坐标系Z轴垂直，并测量第三旋转角度；

将所述第四垂直平面的法向量旋转至与所述第二直线的方向向量垂直，并测量第四旋转角度；

在本体坐标系下将所述第四垂直平面的中心点沿坐标轴方向平移至所述第二直线上，并测量得到第二平移距离；

根据所述第三旋转角度、第四旋转角度、第二平移距离得到第二ToF相机的第二标定外参。

9.一种相机与激光雷达的外参标定装置，其特征在于，包括：

第一获取模块，用于获取激光雷达扫描本体前方垂直面的第一点云数据；

第二获取模块，用于获取第一ToF相机扫描本体前方水平面以及前方垂直面的第二点云数据，其中，所述第二点云数据中包括第一ToF相机前方水平的第一平面点云数据以及前方垂直的第二平面点云数据；

第三获取模块，用于获取第二ToF相机扫描本体前方垂直面的第三点云数据，其中，所述激光雷达和所述第一ToF相机，第二ToF相机设置在所述本体上；

第一拟合模块，用于根据所述第一点云数据拟合得到所述本体正前方的第一直线；

第二拟合模块，用于根据所述第二点云数据拟合得到所述本体前方的第一水平面和第一垂直平面；

第三拟合模块，用于根据所述第三点云数据拟合得到所述本体前方的第二垂直平面；

转换模块，用于采用初始外参将所述第一直线、所述第一水平面、所述第一垂直平面、所述第二垂直平面转换成本体坐标系下的第二直线、第二水平面、第三垂直平面、第四垂直平面；

构建模块，用于构建所述第二直线、所述第二水平面、所述第三垂直平面、所述第四垂直平面之间的线面几何约束关系；

计算模块，用于根据所述线面几何约束关系计算得到标定外参，其中，所述标定外参用于指示所述第一ToF相机或所述第二ToF相机相对所述激光雷达的位置关系。

10.一种存储介质，其特征在于，所述存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至8中任一项所述的相机与激光雷达的外参标定方法的步骤。

Images