CN110148180A - 一种激光雷达与相机融合装置与标定方法 - Google Patents

Abstract

本发明公开一种激光雷达与相机融合装置与标定方法,包括竖直升降台和标定板支架；所述竖直升降台上的工作导轨上设有工作台，所述工作台上固定安装有相机和激光雷达；所述标定板支架上固定安装有标定板；所述激光雷达在所述标定板上扫描得到一条水平直线，找到此直线在标定板边界上的左右端点，然后多次移动相同间隔，便可以得到左右边线上的多组端点，拟合便可近似得到左右边界的直线方程进而得到标定板的端点；将相机图片上端点所在位置的像素点与端点对应便可以得到相机和雷达的位姿关系表达式；运用本激光雷达与相机融合标定方法能够获取雷达点云稀疏的场景下的特征,并能够解得内外参的联合表达式，不需要提前测得相机内参。

Description

一种激光雷达与相机融合装置与标定方法

技术领域

本发明涉及到融合及标定技术领域，具体涉及到一种激光雷达与相机融合装置与标定方法。

背景技术

随着计算机技术的兴起，三维重建逐渐成为一个热门的话题，常用的三维重建传感器一般是用双目相机或者是激光雷达来完成物体的重建。对于双目相机来说，虽然具有了颜色信息，但是由于它比较复杂的图像之间的匹配会对整个***带来计算上的复杂度而且只能够进稀疏点云三维重建。对于激光雷达来说，其结果精确可靠但是由于它没有彩色信息的获取所以对于三维重建的结果无法直观意义上的感受。

所以单一传感器不可避免的存在局限性，为了提高***的稳健性，且在保证精度的情况下融合彩色信息，多采取多传感器融合的方案，融合又包含不同传感器的时间同步和空间同步。激光雷达和相机的联合标定就属于空间同步范畴。

现在用深度学习处理点云成为热点，在标注点云时，由于点云的稀疏性，单靠点云，很难判断目标和类别，如果有时间同步的图像，就可以判断出来，但是为了精确标注，还需要将点云投影到图像，观察是否重合，这里就需要做激光雷达和相机的联合标定。

激光雷达和相机的联合标定可以用来实时重建物体的表面且获取具有彩色信息的纹理表面，通过联合标定将激光雷达的点云和相机像素的信息统一起来，是多传感器融合的重要组成。

发明内容

本发明的目的是针对现有技术存在的问题，提供一种激光雷达与相机融合装置与标定方法。

为实现上述目的，本发明采用的技术方案是：

一种激光雷达与相机融合装置，包括竖直升降台和标定板支架；所述竖直升降台上设有平行的工作导轨，所述工作导轨之间设有工作台，所述工作台上固定安装有相机和激光雷达，所述激光雷达两侧设置有“T”形安装板，所述“T”形安装板上安装所述相机；所述标定板支架上固定安装有标定板，所述标定板位于所述相机视角及激光雷达扫描区域内。

本激光雷达与相机融合装置能够确保激光雷达和相机同步上下移动，结构简单，精度高，相机与激光雷达位姿相对固定；所述标定板始终位于相机视角及二维激光雷达扫描区域内，保证能够获取全面的雷达点云和像素点。

优选的，所述工作导轨之间设有滚珠丝杠，在所述滚珠丝杠上部设置所述工作台；所述滚珠丝杠端部连接有步进电机。通过步进电机配合滚珠丝杠能够精确控制工作台上升或下降的位移量，有利于提高激光雷达与相机的融合标定计算的准确性。

优选的，所述标定板为三角形标定板或方形标定板。

优选的，所述工作台上升一个行程的距离为4.82毫米，误差为10微米。此设置以确保融合和标定过程中高精度的要求。

一种使用上述融合装置的激光雷达与相机融合标定方法，包括以下步骤:

S1：将所述激光雷达与所述相机以固定位姿固定在竖直升降台的工作台上,确保所述竖直升降台竖直移动过程中,所述激光雷达与所述相机的相对位置固定；

S2：将标定板设置在所述激光雷达及所述相机能够同时获取到的位置；

S3：打开所述激光雷达，所述激光雷达在所述标定板上扫描得到一条水平直线，找到此直线在标定板边界上的左右端点，然后多次移动相同间隔，便可以得到所述标定板左右边线上的多组端点，拟合多组端点得到左右边界的直线方程，进而得到激光雷达扫描的标定板的角点；

S4：将所述相机图片上标定板的角点所在位置的像素点与所述S3中得到的标定板的角点对应便得到所述相机和所述激光雷达的位姿关系表达式；

S5：将标定板进行不同位姿的摆放可以得到多组点，进而连立方程可得到所述相机与所述激光雷达的位姿变化；

所述激光雷达扫描的角点确定步骤具体如下：

1)移动工作台，激光雷达扫描得到不同高度的扫描线段，基于RANSAC线性特征提取，获得不同点的雷达数据及多条扫描线段；

假设螺旋升降仪上升高度为z，扫描的某点的雷达数据为r,θ，则扫描点在主坐标系下可表示为：(r cosθ,r sinθ,z)。对于每一组固定位姿的标定板，通过移动固定行程的工作台来对标定板形成高度不一的扫描线。且这些扫描线的竖直方向即z方向已知，即为工作台移动的行程。对于每一次扫描得到的线端，得到每一个点均可用上述的公式表达，记第i次测量得到的第j个点的雷达数据为r_j,θ_j，螺旋仪的行程为z_i，可以得到此点的三维坐标系表达式为(r_jcosθ_j,r_jsinθ_j,z_i)。对于第i次扫描，假设已经扫描到了n个点，其中j∈(1,n)，可以利用RANSAC剔除噪声的雷达数据点，并且通过最小二乘进行拟合具体做法如下：

对于第i次扫描，可以得到雷达数据的三维坐标点(r_jcosθ_j,r_jsinθ_j,z_i)，其中j∈(1,n)，n为扫描得到的点的个数。每次选取任意的两点(r_acosθ_a,r_asinθ_a,z_i)，(r_bcosθ_b,r_bsinθ_b,z_i)，可以得到直线方程如下：

由于每一次扫描z_i值相同，便可以舍弃z_i，将直线变为二维坐标系下的点，简单记两点所产生的直线为y＝ax+b。由于舍弃了z_i值，对于任意一点(r_ccosθ_c,r_csinθ_c)，可以求点到直线的距离：

由雷达固定参数可得其精度误差为σ，若d≤σ，则此点为该次扫描的内点。从而得到此次内点的集合S₁。

重复上述过程，任意选取两点，重复上述过程次，便可以得到所有的内点集合，选取内点最大的集合为S。

对每次扫描的内点进行最小二乘拟合得到一条具有两个端点的线段。

其中最小二乘的拟合方法如下：对于直线y＝ax+b，对于每i个点存在误差：

e_i＝y_i-ax_i-b (3)

定义一个损失函数：

调整a和b，使得Q达到最小，那么曲线拟合的误差也会最小。这里，Q是a，b的函数。Q的最小值点必然是其导数为0的点。令

求解上述方程组，则可以解得a和b的值。即可得到一条具有两个端点的线段。

对于每一次固定位姿的标定板，对于每一次行程扫描，重复上述过程，便可以得到对于一次固定位姿标定板的标定，得到一组平行的线段，且每一条线段端点均已知。

2)利用RANSAC算法剔除得到的雷达数据中噪声数据点，通过最小二乘法进行曲面拟合得到一个拟合平面；

对于得到的雷达数据点去除噪声点后，设有n个数据点P_i＝(x_i，y_i，z_i)i∈(1，2，...，n).定义拟合平面方程为：

AX+BY+CZ+D＝0 (6)

其中A、B、C、D为待定系数，A、B、C、不同时为0；

数据点P_i到拟合平面AX+BY+CZ+D＝0的距离设为d_i，则：

d_i ²＝(AX_i+BY_i+CZ_i+D)²/(A²+B²+C²) (7)

设误差项L为

欲使L最小.L是关于(A、B、C、D)的函数，其中(X_i、Y_i、Z_i均已知)，当L取最小值时，可知：

将公式(7)和(8)代入公式(9)中联立可得到四个等式，四个未知数的方程，利用线性代数联立即可求解得到最小二乘拟合得到的拟合平面方程。

3)再将步骤1)中扫描线段的左右端点投影到所述拟合平面上；

找到拟合的平面AX+BY+CZ+D＝0后，在此标定角度下，对于步骤1)中拟合得到的线段，令其端点为p_k＝(X_k，Y_k，Z_k)k∈(1，2，...，n)，由于每次测量竖直工作台的量程都比较精确且每个数据点的z坐标就是工作台的行程高度，所以不优化z的数据，使得p_k到拟合平面AX+BY+CZ+D＝0的距离最短，即将拟合得到的扫描线段的左右端点投影到拟合平面上。由于z坐标不变，只要将端点水平投影到平面，我们假设p_k对应于平面上的点是

P_kr＝(X_kr，Y_kr，Z_k)k∈(1，2，...，n)。对于任意一点，可得下面关系式：

min((X_k-X_kr)²+(Y_k-Y_kr)²)，AX_kr+BY_kr+CZ_k+D＝0 (10)

将上式带入已知数据求导即可得到p_k对应于拟合平面上的点p_kr，即完成了扫描线段的左右端点投影到所述拟合平面上的步骤。

4)利用PCA算法，将3)中投影到拟合平面上的左右端点拟合直线，得到的拟合直线的交点为雷达扫描的标定板的角点。

按此方式选取不同的扫描线，便可以在同一张标定板中找到若干个交点，既可以确定出雷达扫描的标定板的每个角点，因此利用雷达数据点可测得标定板的三维空间坐标点(角点的坐标)，定义二维激光雷达坐标为(x，y，z)。

所述相机像素角点的确定步骤如下：

1)对相机获取的标定板图像进行高斯滤波，使其平滑，去除噪声点；

2)再对图像进行霍夫变换，进行直线检测，检测到相邻的两边的直线方程；

3)通过求解直线方程得到标定板的角点的像素坐标。

按此方法，便可以将所有图片上面的标定板的像素角点给检测出来，定义相机的像素角点坐标为(u，v)。

对于在不同视角下的标定板的三维空间点和对应的像素坐标点，它们存在一个转换关系：

式中：f_u和f_v分别为水平与垂直方向上的有效焦距长度；u₀和v₀为图像中心点位置坐标，这四个参数为相机内参；而R与T分别为相机与三维激光雷达间的旋转矩阵和平移矩阵，是外参数；m_ij为内外参数联合变换矩阵。

其中有12个未知参数，为获得其12个参数，至少需要获取6组方程解算未知数。

通过至少六组点便可以得到相机像素点和雷达三维点之间的转换关系，对于超过六组点，可以通过奇异值分解来得到一个最小二乘解。通常为了追求数据的优化，一般会采用超过六组点来解此内外参的联合表达式。而相机的内参并不需要提前测得。至此求得了相机以及雷达之间的数据转换关系。

优选的，还包括确定所述竖直升降台移动精度的过程。

首先利用夹具在工作台上装夹一个物体，固定工作台不动，在一端设有激光测距仪来测量激光测距仪与物体的距离l₁，然后开动电机使得螺旋仪上升一个行程，可以得到此时激光测距仪与物体的距离l₂，由此可以得到上升一个行程的距离经过多次试验数据测量分析，得到竖直升降台行程从下到上比从上到下精度要来的高。并且对于激光测距仪得到的数据，进行卡尔曼滤波可将精度误差从器械本身的30微米下降到10微米。

与现有技术相比，本发明的有益效果是：1、运用本激光雷达与相机融合标定方法能够获取雷达点云稀疏的场景下的特征；2、通过本方法可以解得内外参的联合表达式，不需要提前测得相机内参；3、本激光雷达与相机融合装置结构简单，精度高，相机与激光雷达位姿相对固定；4、装置成本低廉，不需要昂贵的三维雷达，只需要二维雷达和普通彩色相机便可以重建出三维轮廓；5、建图效果具有直观性，在原有的没有色彩信息的雷达点云中通过外参转换将相机的彩色信息融合到了点云中，使得重建后的物体更加具有辨识度。

附图说明

图1为本发明激光雷达与相机融合装置及标定示意图；

图2为本发明扫描提取特征示意图；

图中：1、竖直升降台；2、标定板支架；3、工作导轨；4、工作台；401、雷达安装座；402、“T”形安装板；5、相机；6、雷达；7、标定板；8、滚珠丝杠；9、步进电机；10、雷达扫描点；11、相机视角；12、扫描线；13、行程。

具体实施方式

下面将结合本发明中的附图，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动条件下所获得的所有其它实施例，都属于本发明保护的范围。

一种激光雷达与相机融合装置，包括竖直升降台1和标定板支架2；所述竖直升降台1上设有平行的工作导轨3，所述工作导轨3上设有工作台4，所述工作台4上固定安装有相机5和二维激光雷达6；所述标定板支架2上固定安装有标定板7，所述标定板7位于所述相机视角及二维激光雷达扫描区域内。

具体的，所述工作台4上设有弧形的雷达安装座401，所述雷达安装座401的弧形槽内上安装所述激光雷达6，所述雷达安装座401两侧固定安装有“T”形安装板402，所述“T”形安装板402上安装所述相机5。

优选的，所述工作导轨3之间设有滚珠丝杠8，在所述滚珠丝杠8上部设置所述工作台4；所述滚珠丝杠8端部连接有步进电机9。

优选的，所述标定板7为三角形标定板或方形标定板。

优选的，所述工作台4上升一个行程的距离为4.82毫米，误差为10微米。

一种使用上述融合装置的激光雷达与相机融合标定方法，包括以下步骤:

S1：将所述激光雷达6与所述相机5以固定位姿固定在竖直升降台的工作台5上,确保所述工作台5竖直移动过程中,所述激光雷达6与所述相机5的相对位置固定；

S2：将标定板7设置在所述激光雷达6及所述相机5能够同时获取到的位置；

S3：打开所述激光雷达6，所述激光雷达6在所述标定板7上扫描得到一条水平直线，找到此直线在标定板边界上的左右端点，然后多次移动相同间隔，便可以得到所述标定板左右边线上的多组端点，拟合多组端点得到左右边界的直线方程，进而得到激光雷达扫描的标定板的角点；

S4：将所述相机5图片上标定板的角点所在位置的像素点与所述S3中得到的标定板的角点对应便得到所述相机和所述激光雷达的位姿关系表达式；

S5：将标定板进行不同位姿的摆放可以得到多组点，进而连立方程可得到所述相机与所述激光雷达的位姿变化；

所述激光雷达扫描的角点确定步骤具体如下：

1)移动工作台4，激光雷达6扫描得到不同高度的扫描线段，基于RANSAC线性特征提取，获得不同点的雷达数据及多条扫描线段；

假设螺旋升降仪上升高度为z，扫描的某点的雷达数据为r,θ，则扫描点在主坐标系下可表示为：(r cosθ,r sinθ,z)。对于每一组固定位姿的标定板，通过移动固定行程的工作台来对标定板形成高度不一的扫描线。且这些扫描线的竖直方向即z方向已知，即为工作台移动的行程。对于每一次扫描得到的线端，得到每一个点均可用上述的公式表达，记第i次测量得到的第j个点的雷达数据为r_j,θ_j，螺旋仪的行程为z_i，可以得到此点的三维坐标系表达式为(r_jcosθ_j,r_jsinθ_j,z_i)。对于第i次扫描，假设已经扫描到了n个点，其中j∈(1,n)，可以利用RANSAC剔除噪声的雷达数据点，并且通过最小二乘进行拟合具体做法如下：

对于第i次扫描，可以得到雷达数据的三维坐标点(r_jcosθ_j,r_jsinθ_j,z_i)，其中j∈(1,n)，n为扫描得到的点的个数。每次选取任意的两点(r_acosθ_a,r_asinθ_a,z_i)，(r_bcosθ_b,r_bsinθ_b,z_i)，可以得到直线方程如下：

由于每一次扫描z_i值相同，便可以舍弃z_i，将直线变为二维坐标系下的点，简单记两点所产生的直线为y＝ax+b。由于舍弃了z_i值，对于任意一点(r_ccosθ_c,r_csinθ_c)，可以求点到直线的距离：

由雷达固定参数可得其精度误差为σ，若d≤σ，则此点为该次扫描的内点。从而得到此次内点的集合S₁。

重复上述过程，任意选取两点，重复上述过程次，便可以得到所有的内点集合，选取内点最大的集合为S。

对每次扫描的内点进行最小二乘拟合得到一条具有两个端点的线段。

其中最小二乘的拟合方法如下：对于直线y＝ax+b，对于每i个点存在误差：

e_i＝y_i-ax_i-b (3)

定义一个损失函数：

调整a和b，使得Q达到最小，那么曲线拟合的误差也会最小。这里，Q是a，b的函数。Q的最小值点必然是其导数为0的点。令

求解上述方程组，则可以解得a和b的值。即可得到一条具有两个端点的线段。

对于每一次固定位姿的标定板，对于每一次行程扫描，重复上述过程，便可以得到对于一次固定位姿标定板的标定，得到一组平行的线段，且每一条线段端点均已知。

2)利用RANSAC算法剔除得到的雷达数据中噪声数据点，通过最小二乘法进行曲面拟合得到一个拟合平面；

对于得到的雷达数据点去除噪声点后，设有n个数据点P_i＝(x_i，y_i，z_i)i∈(1，2，...，n).定义拟合平面方程为：

AX+BY+CZ+D＝0 (6)

其中A、B、C、D为待定系数，A、B、C、不同时为0；

数据点P_i到拟合平面AX+BY+CZ+D＝0的距离设为d_i，则：

d_i ²＝(AX_i+BY_i+CZ_i+D)²/(A²+B²+C²) (7)

设误差项L为

欲使L最小.L是关于(A、B、C、D)的函数，其中(X_i、Y_i、Z_i均已知)，当L取最小值时，可知：

将公式(7)和(8)代入公式(9)中联立可得到四个等式，四个未知数的方程，利用线性代数联立即可求解得到最小二乘拟合得到的拟合平面方程。

3)再将步骤1)中扫描线段的左右端点投影到所述拟合平面上；

找到拟合的平面AX+BY+CZ+D＝0后，在此标定角度下，对于步骤1)中拟合得到的线段，令其端点为p_k＝(X_k，Y_k，Z_k)k∈(1，2，...，n)，由于每次测量竖直工作台的量程都比较精确且每个数据点的z坐标就是工作台的行程高度，所以不优化z的数据，使得p_k到平面AX+BY+CZ+D＝0的距离最短，即将拟合得到的扫描线段的左右端点投影到拟合平面上。由于z坐标不变，只要将端点水平投影到平面，我们假设p_k对应于平面上的点是p_kr＝(X_kr，Y_kr，Z_k)k∈(1，2，...，n)。对于任意一点，可得下面关系式：

min((X_k-X_kr)²+(Y_k-Y_kr)²)，AX_kr+BY_kr+CZ_k+D＝0 (10)

将上式带入已知数据求导即可得到p_k对应于平面上的点p_kr，即完成了扫描线段的左右端点投影到所述拟合平面上的步骤。

4)利用PCA算法，将3)中投影到拟合平面上的左右端点拟合直线，得到的拟合直线的交点为雷达扫描的标定板的角点。

按此方式选取不同的扫描线，便可以在同一张标定板中找到若干个交点，既可以确定出雷达扫描的标定板的每个角点，因此利用雷达数据点可测得标定板的三维空间坐标点(角点的坐标)，定义二维激光雷达坐标为(x，y，z)。

所述相机5像素角点的确定步骤如下：

1)对相机获取的标定板图像进行高斯滤波，使其平滑，去除噪声点；

2)再对图像进行霍夫变换，进行直线检测，检测到相邻的两边的直线方程；

3)通过求解直线方程得到标定板的角点的像素坐标。

按此方法，便可以将所有图片上面的标定板的像素角点给检测出来，定义相机的像素角点坐标为(u,v)。

对于在不同视角下的标定板的三维空间点和对应的像素坐标点，它们存在一个转换关系：

式中：f_u和f_v分别为水平与垂直方向上的有效焦距长度；u₀和v₀为图像中心点位置坐标，这四个参数为相机内参；而R与T分别为相机与三维激光雷达间的旋转矩阵和平移矩阵，是外参数；m_ij为内外参数联合变换矩阵。

其中有12个未知参数，为获得其12个参数，至少需要获取6组方程解算未知数。

通过至少六组点便可以得到相机像素点和雷达三维点之间的转换关系，对于超过六组点，可以通过奇异值分解来得到一个最小二乘解。通常为了追求数据的优化，一般会采用超过六组点来解此内外参的联合表达式。而相机的内参并不需要提前测得。至此求得了相机以及雷达之间的数据转换关系，对于后续的平台实际运用提供了数据转换的支持。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (8)

1.一种激光雷达与相机融合装置，其特征在于，包括竖直升降台和标定板支架；所述竖直升降台上设有一对平行的竖直工作导轨，一对所述工作导轨之间设有工作台，所述工作台上固定安装有相机和激光雷达，所述激光雷达两侧设置有“T”形安装板，所述“T”形安装板上安装所述相机；所述标定板支架上固定安装有标定板，所述标定板位于所述相机视角及激光雷达扫描区域内。

2.根据权利要求1所述的激光雷达与相机融合装置，其特征在于，所述工作导轨之间设有滚珠丝杠，在所述滚珠丝杠上部设置所述工作台；所述滚珠丝杠端部连接有步进电机。

3.根据权利要求1所述的激光雷达与相机融合装置，其特征在于，所述标定板为三角形标定板或方形标定板。

4.根据权利要求1所述的激光雷达与相机融合装置，其特征在于，所述工作台上升一个行程的距离为4.82毫米，误差为10微米。

5.根据权利要求1所述的激光雷达与相机融合装置的激光雷达与相机融合标定方法，其特征在于，包括以下步骤:

S1：将所述激光雷达与所述相机以固定位姿固定在竖直升降台的工作台上,确保所述工作台竖直移动过程中,所述激光雷达与所述相机的相对位置固定；

S2：将标定板设置在所述激光雷达及所述相机能够同时获取到的位置；

S3：打开所述激光雷达，所述激光雷达在所述标定板上扫描得到一条水平直线，找到此直线在标定板边界上的左右端点，然后多次移动相同间隔，便可以得到所述标定板左右边线上的多组端点，拟合多组端点得到左右边界的直线方程，进而得到激光雷达扫描的标定板的角点；

S4：将所述相机图片上标定板的角点所在位置的像素点与所述S3中得到的标定板的角点对应便得到所述相机和所述激光雷达的位姿关系表达式；

S5：将标定板进行不同位姿的摆放可以得到多组点，进而连立方程可得到所述相机与所述激光雷达的位姿变化。

6.根据权利要求5所述的激光雷达与相机融合装置的激光雷达与相机融合标定方法，其特征在于，所述激光雷达扫描的角点确定步骤具体如下：

步骤1：移动工作台，激光雷达扫描得到不同高度的扫描线段，基于RANSAC线性特征提取，获得不同点的雷达数据及多条扫描线段；

步骤2：利用RANSAC算法剔除得到的雷达数据中噪声数据点，通过最小二乘法进行曲面拟合得到一个拟合平面；

步骤3：再将步骤1中扫描线段的左右端点投影到所述拟合平面上；

步骤4：利用PCA算法，将步骤3中投影到拟合平面上的左右端点拟合直线，得到的拟合直线的交点为雷达扫描的标定板的角点。

7.根据权利要求5所述的激光雷达与相机融合装置的激光雷达与相机融合标定方法，其特征在于，所述相机像素角点的确定步骤如下：

步骤5：对相机获取的标定板图像进行高斯滤波，使其平滑，去除噪声点；

步骤6：再对图像进行霍夫变换，进行直线检测，检测到相邻的两边的直线方程；

步骤7：通过求解直线方程得到标定板的角点的像素坐标。

8.根据权利要求4所述的激光雷达与相机融合装置的激光雷达与相机融合标定方法，其特征在于，还包括确定所述竖直升降台移动精度的过程。