基于背景点云精细化处理的激光雷达与相机联合标定方法

1.本发明涉及机器人视觉及传感器标定技术领域，特别涉及一种基于背景点云精细化处理的激光雷达与相机联合标定方法。


背景技术：

2.激光雷达和可见光相机的信息融合被广泛应用于机器人视觉中的三维重建、自主导航和定位以及无人车等领域。单个传感器具有局限性，不能满足复杂的环境感知任务，如可见光相机易受光照影响且缺少环境深度信息，激光雷达则数据点较为稀疏且缺少色彩信息，而二者的融合可以弥补各自缺陷，达到更为精准的环境感知。
3.对激光雷达和可见光相机的外部参数标定是进行外部信息融合的基础。主流的激光雷达和可见光相机的外参标定为基于标定物的标定。基于标定物的标定是通过有一个几何约束的标定物，获取三维特征点及其对应的二维特征点用于外部参数的计算。利用激光雷达在边缘处点的深度不连续性，提取距离突变的点作为边缘点，以用作标定的特征点，并和图像中提取的边缘信息进行配准。但由于激光雷达的水平角分辨率影响，位于物体边缘的点会发生距离突变，从而不能很好的定位真实的边缘点，这极大影响了后续特征点的求解精度，也使得标定的效果变差且不稳定。
4.如图1所示，由于激光雷达的水平分辨角的存在，使其不能对物体的边缘点进行很好的测量。
5.近处的直线为待测量物体，称之为前景；远处的直线为非测量物体，称之为背景；较粗的点为激光雷达的测量点，落在前景的称之为前景边缘点，落在背景的称之为背景边缘点；较细的点为待测量物体的实际边缘点，称之为真实边缘点；θ为激光雷达的水平分辨角，即最相近两条扫描射线之间的理论夹角。
6.由于水平分辨角的存在，使得激光雷达未能正确捕捉到真实边缘点。定义较粗的前景边缘点和较细的真实边缘点之间的距离为理论误差，下面对理论误差进行简要分析，如图2所示。
7.直线l1和l2为某两条相邻的激光雷达扫描线，h为激光雷达距离前景的距离，d为较远的扫描线l1到激光雷达所在的位置的水平距离，e即为理论误差。由几何关系可得，e与h、d、θ的关系为：
[0008][0009]
取d为长度500mm固定不变，h分别为3000mm、6000mm、10000mm、20000mm，θ分别为0.1
°
、0.2
°
、0.4
°
，带入上述理论误差e，得如下表格：
[0010]
表1 d＝500mm时e的值
[0011]
θ(
°
)\h(mm)3000600010000200000.15.38010.54317.49534.9270.210.75721.08334.98869.851
0.421.50142.15569.964139.69
[0012]
由于物体测量的是左右两个边缘，如图3所示，所以最大理论误差还需要再放大两倍，如下表所示。
[0013]
表2最大理论误差放大两倍
[0014]
θ(
°
)\h(mm)3000600010000200000.15.380*210.543*217.495*234.927*20.210.757*221.083*234.988*269.851*20.421.501*242.155*269.964*2139.69*2
[0015]
从上表中可以发现，激光雷达的固有属性(角分辨率)导致其不能正确的捕捉到待测物体的边缘点。另外，以上数据是建立在角分辨率固定不变(即只有0.1/0.2/0.4三个数值)的情况下，实际上激光雷达的角分辨率并非一成不变，上述最大理论误差在实际中会更大。这种由于激光雷达水平分辨角带来的误差会影响三维特征点的求解。
[0016]
因此，精确的三维特征点的求解是激光雷达和可见光相机的联合标定的关键，是实现激光雷达和可见光相机联合标定需要解决的首要问题。


技术实现要素：

[0017]
针对上述问题，本发明提供一种基于背景点云精细化处理的激光雷达与相机联合标定方法，引入标定物的背景点云，将其与前景点云共同进行精细化处来求解边缘点，得到精确的边缘点后用于求解标定物的特征点，将这些特征点以重投影误差为优化指标建立优化方程来求解联合标定参数，从而提高联合标定精度。
[0018]
为解决上述技术问题，本发明的实施例提供如下方案：
[0019]
一种基于背景点云精细化处理的激光雷达与相机联合标定方法，包括以下步骤：
[0020]
s1、利用激光雷达和可见光相机搭建实验装置，并构建用于求解标定矩阵的优化方程；
[0021]
s2、将激光雷达扫描的标定物的前景点云拟合为直线；
[0022]
s3、对于拟合出的直线，获取与之处于同一扫描线的激光雷达背景点云；
[0023]
s4、根据水平角求解标定物的边缘点；
[0024]
s5、利用得到的边缘点求解标定物的三维特征点；
[0025]
s6、将得到的三维特征点和对应的通过可见光相机得到的二维特征点带入优化方程中进行标定矩阵的求解。
[0026]
优选地，所述步骤s1中，实验装置放置于标定物前方，标定物为方形板，斜45
°
放置，位于激光雷达与可见光相机的视场范围内，标定物后方设有背景墙。
[0027]
优选地，所述步骤s1中，构建用于求解标定矩阵的优化方程包括：
[0028]
从三维特征点(x，y，z)到二维特征点(u，v)的转换表示为公式(1)：
[0029][0030]
其中s为缩放因子，k为提前获取的可见光相机内参矩阵，需要求解一个最优的标定矩阵[r
*
|t
*
]，满足如公式(2)：
[0031][0032]
其中r
*
为最佳旋转矩阵，t
*
为最佳平移矩阵，p为三维特征点集合，pi(xi，yi，zi)∈p；q为二维特征点集合，qi(ui，vi)∈q；h(
·
)为二维距离函数，如公式(3)所示：
[0033][0034]
上述公式(2)可以转化为优化问题如公式(4)：
[0035][0036]
将f(q，p)作为优化方程，求解得到标定矩阵[r
*
|t
*
]。
[0037]
优选地，所述步骤s2中，将标定物的第i帧前景点云拟合成一条直线linei；定义两个符号用于描述直线拟合的两个步骤，分别是增符号
⊕
和删符号增符号
⊕
表示合并前i帧的扫描线；删符号表示对经过
⊕
作用之后的数据进行离群点删除；上述两步骤得到的第i帧前景点云信息经过ransac直线拟合后，得到直线linei信息如下：
[0038][0039][0040][0041]
其中，为直线方向向量，直线上任意一点(x，y，z)可由(x0，y0，z0)和表示，后续在拟合出的直线linei上寻找边缘点pm(xm,ym，zm)。
[0042]
优选地，所述步骤s3中，对于拟合出的直线linei，根据激光雷达给出的每条扫描线的id信息，在背景墙上找到与之处于同一扫描线的激光雷达背景点云，用于提取背景边缘点。
[0043]
优选地，所述步骤s4中，在直线linei上分别找到水平角为α、β的点作为背景边缘点pb和前景边缘点pf的投影点；
[0044][0045]
具体地，pb的水平角α通过pb的三维坐标(xb,yb，zb)求得，再由公式(6)可求解t，将t带入式(5)中得到pb的投影点p
b'
，同理求解出pf的投影点p
f'
；将p
b'
和p
f'
的几何中点pm作为当前帧的边缘点，保存pm到边缘点集合s
edge
中；继续获取下一帧的直线line
i 1
，经过同样的处理后将新一帧的边缘点也保存到边缘点集合s
edge
中，直到达到指定帧数；求解边缘点集合s
edge
的质心点作为该扫描线的真正边缘点，保存到edgepoints中；同理，分别求解其余扫描线的边缘点，保存到edgepoints中。
[0046]
优选地，所述步骤s5中，标定物为方形板，标定物的四条边的边缘点通过上述步骤求得，全部保存到edgepoints中；将求取到的边缘点分别利用最小二乘法拟合标定物的四条边直线l
1-l4，利用两条空间直线的中垂线之中点表示三维特征点p
1-p4。
[0047]
优选地，所述步骤s6中，改变标定物的位置，以同样的方法获取更多的三维特征点，将获取的三维特征点和对应的通过可见光相机得到的二维特征点带入公式(4)所示的
优化方程中进行标定矩阵的求解。
[0048]
优选地，所述步骤s6中，通过边缘检测和/或角点检测的方法，从可见光相机获取的图像中提取二维特征点。
[0049]
本发明实施例提供的技术方案带来的有益效果至少包括：
[0050]
本发明实施例中，对激光雷达扫描标定物的背景点云加以利用，使其与前景点云共同进行精细化处理以得到精确的标定物边缘点，对边缘点进行直线拟合，可以求解得到标定物上的三维特征点，利用多视角测量得到的三维特征点及对应的二维特征点建立优化方程，最小化重投影误差得到联合标定结果。本发明能够高精度、高鲁棒性地提取激光雷达扫描在标定物上的边缘点，避免了由于激光雷达的低水平角分辨率带来的特征点精度不够的问题，通过获取到的精确边缘点，能够得到更准确的标定结果，从而提高联合标定精度。
附图说明
[0051]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0052]
图1是激光雷达的水平分辨角示意图；
[0053]
图2是激光雷达理论误差分析示意图；
[0054]
图3是物体测量左右两个边缘示意图；
[0055]
图4是本发明实施例提供的基于背景点云精细化处理的激光雷达与相机联合标定方法的流程图；
[0056]
图5是本发明实施例提供的联合标定方法的具体实施过程示意图；
[0057]
图6是本发明实施例中实验装置的立体示意图；
[0058]
图7是本发明实施例中实验装置的摆放示意图；
[0059]
图8a和图8b是本发明实施例提供的实验装置及边缘点位置的主视图和左视图；
[0060]
图9a和图9b是本发明实施例提供的特征点分布图和边缘点分布图。
[0061]
如图所示，为了能明确实现本发明的实施例的结构，在图中标注了特定的结构和器件，但这仅为示意需要，并非意图将本发明限定在该特定结构、器件和环境中，根据具体需要，本领域的普通技术人员可以将这些器件和环境进行调整或者修改，所进行的调整或者修改仍然包括在本发明的保护范围中。
具体实施方式
[0062]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0063]
本发明的实施例提供了一种基于背景点云精细化处理的激光雷达与相机联合标定方法，如图4和图5所示，所述方法包括以下步骤：
[0064]
s1、利用激光雷达和可见光相机搭建实验装置，并构建用于求解标定矩阵的优化
方程。
[0065]
本步骤中，首先搭建实验装置，使其进入稳定工作状态，如图6和图7所示，激光雷达与可见光相机刚性连接后置于标定物前方，标定物为一个平整的方形板，斜45
°
放置，位于激光雷达与可见光相机的视场范围内，标定物后方设有背景墙。激光雷达、可见光相机及标定物摆放的主视图和左视图分别如图8a和图8b所示。
[0066]
其中，构建用于求解标定矩阵的优化方程包括：
[0067]
从三维特征点(x，y，z)到二维特征点(u，v)的转换表示为公式(1)：
[0068][0069]
其中s为缩放因子，k为提前获取的可见光相机内参矩阵，需要求解一个最优的标定矩阵[r
*
|t
*
]，或称为最优刚性变换矩阵[r
*
|t
*
]，满足如公式(2)：
[0070][0071]
其中r
*
为最佳旋转矩阵，t
*
为最佳平移矩阵，p为三维特征点集合，pi(xi，yi，zi)∈p；q为二维特征点集合，qi(ui，vi)∈q；h(
·
)为二维距离函数，如公式(3)所示：
[0072][0073]
上述公式(2)可以转化为优化问题如公式(4)：
[0074][0075]
将f(q，p)作为优化方程，求解得到标定矩阵[r
*
|t
*
]。
[0076]
图9a和图9b是本发明实施例中特征点和边缘点的分布图。从图中可以看到，p
1-p4为待获取的特征点，这四个点分别为l
1-l4四条直线两两相交的点。在可见光相机图像中，通过图像的边缘检测和/或角点检测等方法可以很容易地提取到二维特征点，但是三维特征点需要借助3d空间边缘直线相交来求取。由于l
1-l4为空间中的直线，用两条空间直线的中垂线之中点表示其交点，这在point cloud library中已经有可用的函数。本发明方法的关键在于如何获取精确的边缘点来进行直线l
1-l4的拟合，进而求解三维特征点。
[0077]
s2、将激光雷达扫描的标定物的前景点云拟合为直线。
[0078]
本步骤中，将标定物的第i帧前景点云拟合成一条直线linei；定义两个符号用于描述直线拟合的两个步骤，分别是增符号
⊕
和删符号增符号
⊕
表示合并前i帧的扫描线，例如图8a中，在framei处施加
⊕
，是将前i帧的扫描线合并到一起；删符号表示对经过
⊕
作用之后的数据进行离群点删除；上述两步骤得到的第i帧前景点云信息经过ransac直线拟合后，得到直线linei信息如下：
[0079][0080][0081]
[0082]
其中，为直线方向向量，直线上任意一点(x，y，z)可由(x0，y0，z0)和表示，后续在拟合出的直线linei上寻找边缘点pm(xm,ym，zm)。
[0083]
s3、对于拟合出的直线，获取与之处于同一扫描线的激光雷达背景点云。
[0084]
本步骤中，对于拟合出的直线linei，根据激光雷达给出的每条扫描线的id信息，在背景墙上找到与之处于同一扫描线的激光雷达背景点云，用于提取背景边缘点，参考图9b。
[0085]
s4、根据水平角求解标定物的边缘点。
[0086]
本步骤中，在直线linei上分别找到水平角为α、β的点作为背景边缘点pb和前景边缘点pf的投影点；
[0087][0088]
具体地，pb的水平角α通过pb的三维坐标(xb,yb，zb)求得，再由公式(6)可求解t，将t带入式(5)中得到pb的投影点p
b'
，如图8b中圆形点；同理求解出pf的投影点p
f'
，由于p
f'
在图8b中的位置和pf相近，未标志出；将p
b'
和p
f'
的几何中点pm作为当前帧的边缘点，保存pm到边缘点集合s
edge
中；继续获取下一帧的直线line
i 1
，经过同样的处理后将新一帧的边缘点也保存到边缘点集合s
edge
中，直到达到指定帧数，例如line
20
；求解边缘点集合s
edge
的质心点作为该扫描线的真正边缘点，保存到edgepoints中；同理，分别求解其余扫描线的边缘点，保存到edgepoints中。
[0089]
上述求解边缘点的算法如下所示：
[0090][0091]
s5、利用得到的边缘点求解标定物的三维特征点。
[0092]
本步骤中，标定物的四条边的边缘点通过上述步骤求得，全部保存到edgepoints中；将求取到的边缘点分别利用最小二乘法拟合标定物的四条边直线l
1-l4，利用两条空间直线的中垂线之中点表示三维特征点p
1-p4，如图9a所示。
[0093]
s6、将得到的三维特征点和对应的通过可见光相机得到的二维特征点带入优化方
程中进行标定矩阵的求解。
[0094]
本步骤中，改变标定物的位置，以同样的方法获取更多的三维特征点，将获取的三维特征点和对应的通过可见光相机得到的二维特征点带入公式(4)所示的优化方程中进行标定矩阵的求解。
[0095]
本发明方法利用了激光雷达扫描的标定物的背景点云，将其与前景点云进行精细化边缘处理，得到更为靠近标定物边缘的点，进而拟合直线得到精确的三维特征点。通过分别在可见光相机拍摄的图像和雷达扫描的点云图中获取标定物的二维特征点和三维特征点，将这些特征点以重投影误差为优化指标建立优化方程来寻找最优旋转矩阵和最优平移矩阵。本发明能够以较高的精度获取激光雷达扫描到的标定物的边缘点并求解出精确的三维特征点，避免了由于激光雷达的水平分辨率较低这个固有属性导致的特征点精度不够的问题，从而提高了联合标定精度。
[0096]
需要说明的是，在说明书中提到“一个实施例”、“实施例”、“示例性实施例”、“一些实施例”等指示所述的实施例可以包括特定特征、结构或特性，但未必每个实施例都包括该特定特征、结构或特性。另外，在结合实施例描述特定特征、结构或特性时，结合其它实施例(无论是否明确描述)实现这种特征、结构或特性应在相关领域技术人员的知识范围内。
[0097]
通常，可以至少部分从上下文中的使用来理解术语。例如，至少部分取决于上下文，本文中使用的术语“一个或多个”可以用于描述单数意义的任何特征、结构或特性，或者可以用于描述复数意义的特征、结构或特性的组合。另外，术语“基于”可以被理解为不一定旨在传达一组排他性的因素，而是可以替代地，至少部分地取决于上下文，允许存在不一定明确描述的其他因素。
[0098]
如本文使用的，术语“标称/标称地”是指在生产或制造过程的设计阶段期间设置的针对部件或过程操作的特性或参数的期望或目标值，以及高于和/或低于期望值的值的范围。值的范围可能是由于制造过程或容限中的轻微变化导致的。如本文使用的，术语“大约”指示可以基于与主题半导体器件相关联的特定技术节点而变化的给定量的值。基于特定技术节点，术语“大约”可以指示给定量的值，其例如在值的5％-15％(例如，值的
±
5％、
±
10％或
±
15％)内变化。
[0099]
可以理解的是，本公开中的“在
……
上”、“在
……
之上”和“在
……
上方”的含义应当以最宽方式被解读，以使得“在
……
上”不仅表示“直接在”某物“上”而且还包括在某物“上”且其间有居间特征或层的含义，并且“在
……
之上”或“在
……
上方”不仅表示“在”某物“之上”或“上方”的含义，而且还可以包括其“在”某物“之上”或“上方”且其间没有居间特征或层的含义。
[0100]
此外，诸如“在
…
之下”、“在
…
下方”、“下部”、“在
…
之上”、“上部”等空间相关术语在本文中为了描述方便可以用于描述一个元件或特征与另一个或多个元件或特征的关系，如在附图中示出的。空间相关术语旨在涵盖除了在附图所描绘的取向之外的在设备使用或操作中的不同取向。设备可以以另外的方式被定向，并且本文中使用的空间相关描述词可以类似地被相应解释。
[0101]
本发明涵盖任何在本发明的精髓和范围上做的替代、修改、等效方法以及方案。为了使公众对本发明有彻底的了解，在以下本发明优选实施例中详细说明了具体的细节，而对本领域技术人员来说没有这些细节的描述也可以完全理解本发明。另外，为了避免对本
发明的实质造成不必要的混淆，并没有详细说明众所周知的方法、过程、流程、元件和电路等。
[0102]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于计算机可读取存储介质中，如：rom/ram、磁碟、光盘等。
[0103]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。