CN111325801B - 一种激光雷达和相机的联合标定方法 - Google Patents

Abstract

本发明涉及一种激光雷达和相机的联合标定方法，将传感器对准立方体靶标进行一次拍摄，激光雷达拍摄得到立方体靶标的三维点云，得到激光雷达坐标系到立方体靶标所在的世界坐标系之间的旋转矩阵R_LW和平移矩阵T_LW；相机拍摄得到立方体靶标的图像，得到相机坐标系到立方体靶标所在的世界坐标系之间的旋转矩阵R_CW和平移矩阵T_CW；最后根据对前两对旋转平移矩阵进行坐标系变换得到激光雷达和相机之间的旋转矩阵R_LC和平移矩阵T_LC。

Description

一种激光雷达和相机的联合标定方法

技术领域

本发明属于传感器融合标定技术领域，具体涉及标定激光雷达和相机之间的外参。

背景技术

激光雷达基于ToF(Time of Flight)原理，通过给目标连续发送激光脉冲，然后用传感器接收物体返回的光，通过探测发射激光和接收激光的往返飞行时间来实现探测距离的目的。视觉测量是利用摄像头采集测量图像，利用图像信息与物方空间内几何信息间精确映射关系实现测量。采取激光雷达和相机融合的方案，激光雷达能够准确获取物体的三维信息，而相机能够获取物体丰富的纹理和色彩信息，将二者的优点结合能最大程度的提取物体信息。

激光雷达与视觉传感器的联合标定是传感器能准确测量的基础。对于激光雷达和视觉传感器的外参标定,分为在线方法和离线方法。在线标定是指在系统使用过程中对传感器进行标定，离线标定是指在使用前对传感器进行标定。当车辆或者机器人不能方便地进行离线标定时，就可以使用在线方法。然而，在可行的情况下，离线标定方法可以提供更准确的结果。

激光雷达和视觉传感器的离线标定一般采用张氏标定法,打印一张平面棋盘格靶标,将其放到不同的位置下分别用激光雷达和相机进行拍摄,根据激光雷达和相机之间的相对位置关系固定为几何约束,来找到激光雷达和相机之间的外参。但是这种方法需要反复移动靶标的位置，在多个位置下都要进行测量，才能得到比较准确的标定结果，操作比较繁琐，效率低下。

鉴于传统标定方法的不足，本发明提出一种激光雷达和相机之间的三维标定方法，只需一次测量即可得到激光雷达和相机之间的外参矩阵，操作简便，效率高。

发明内容

本发明的目的是解决传统激光雷达和相机标定效率低下的问题，提出一种激光雷达和相机标定的新方法，该方法只需一次测量即可得到激光雷达和相机之间的外参矩阵，标定效率高，准确度高。技术方案如下：

一种激光雷达和相机的联合标定方法，将传感器对准立方体靶标进行一次拍摄，激光雷达拍摄得到立方体靶标的三维点云，得到激光雷达坐标系到立方体靶标所在的世界坐标系之间的旋转矩阵R_LW和平移矩阵T_LW；相机拍摄得到立方体靶标的图像，得到相机坐标系到立方体靶标所在的世界坐标系之间的旋转矩阵R_CW和平移矩阵T_CW；最后根据对前两对旋转平移矩阵进行坐标系变换得到激光雷达和相机之间的旋转矩阵R_LC和平移矩阵T_LC。包括如下步骤:

(1)制作一个立方体盒子作为立方体靶标,盒子的三个相邻的面π₁、π₂、π₃布置一样的黑白棋盘格靶标,每个棋盘格的边长和棋盘格的数量已知；

(2)将激光雷达和相机固定，立方体靶标置于激光雷达和相机的视场中央，并且带有棋盘格的三个面朝向激光雷达和相机；

(3)激光雷达拍摄靶标获得靶标的深度图；

(4)根据小孔成像原理将深度图转化为激光雷达坐标系下的三维点云；

(5)利用随机采样一致性(RANSAC)算法拟合出π₁、π₂、π₃三个平面在激光雷达坐标系下的方程：

a₁x+b₁y+c₁z+d₁＝0

a₂x+b₂y+c₂z+d₂＝0

a₃x+b₃y+c₃z+d₃＝0

根据上述方程得到π₁、π₂、π₃的单位法向量，方向选取符合右手坐标系的方向：

(6)计算出平面π₁、π₂、π₃的交点O坐标：

(7)计算出激光雷达坐标系到世界坐标系的旋转矩阵R_LW和平移矩阵T_LW：

R_LW＝[^Ln₁,^Ln₂,^Ln₃]^-1

T_LW＝-[^Lx_O ^Ly_O ^Lz_O]^T

(8)相机拍摄得到一张立方体靶标的图像，将其中的三个棋盘格的图像拆分为三部分，每部分都只包含一张棋盘格的图像；

(9)根据张氏标定法计算出平面π₁、π₂、π₃到相机坐标系的旋转矩阵R₁、R₂、R₃和平移矩阵T₁、T₂、T₃；

(10)计算出平面π₁、π₂、π₃在相机坐标系下的的法向量；

^Cn₁＝R₁·(0，0，1)^T

^Cn₂＝R₂·(0，0，1)^T

^Cn₃＝R₃·(0，0，1)^T

(11)计算出平面π₁、π₂、π₃在相机坐标系下的方程：

([x,y,z]-T₁ ^T)·^Cn₁＝0

([x,y,z]-T₂ ^T)·^Cn₂＝0

([x,y,z]-T₃ ^T)·^Cn₃＝0

(12)计算出平面π₁、π₂、π₃的交点O在相机坐标系下的坐标：

(^Cx_O,^Cy_O,^Cz_O)＝[^Cn₁,^Cn₂,^Cn₃]^-1[T₁ ^TCn₁,CT₂ ^Tn₂,^CT₃ ^Tn₃]

(13)计算出相机坐标系到世界坐标系的旋转矩阵R_CW和平移矩阵T_CW：

R_CW＝[^Cn₁,^Cn₂,^Cn₃]^-1

T_CW＝-[^Cx_O,^Cy_O,^Cz_O]^T

(14)计算出激光雷达坐标系到相机坐标系的旋转矩阵R_LC和平移矩阵T_LC：

附图说明

图1是本发明采取的激光雷达和相机联合标定外参的流程图。

图2是本发明所使用的立方体标定靶标示意图。

图3是本发明标定过程中激光雷达坐标系到世界坐标系之间的坐标变换示意图。

图4是本发明标定过程中相机坐标系到世界坐标系之间的坐标系变换示意图。

具体实施方式

本发明提出了一个激光雷达和相机的联合标定方法，通过激光雷达和相机分别对本发明提出的立方体靶标进行拍摄一次，得到激光雷达和相机之间的外参矩阵。其中靶标是一个立方体,边长不需要知道,在立方体的三个相邻的面上,分别布置三个相同的棋盘格靶标,棋盘格的格子边长和数量已知。整个靶标如图2所示。

具体来说，本发明所使用的标定靶标为一个立方体靶标，其中靶标的三个相邻的面π₁、π₂、π₃上分别布置一个黑白相间的棋盘格靶标，每一个格子的边长和格子的数量已知，其余的均不需指定大小。

本发明所使用的标定方法，在用激光雷达和相机进行拍摄过程中，需要标定靶标大致在激光雷达和相机的视场中央，相机能够拍摄到带有棋盘格靶标的三个面，以获得较好的图像。

本发明所述的立方体靶标所在的世界坐标系，是以立方体的三个平面π₁、π₂、π₃分别作为空间直角坐标系O_W-X_WY_WZ_W的平面X_WO_WY_W、平面Y_WO_WZ_W、平面Z_WO_WX_W的右手空间直角坐标系。

激光雷达拍摄立方体靶标，得到靶标的深度图，根据激光雷达的内参和小孔成像原理可以将深度图转化为立方体靶标的三维点云。根据随机采样一致性(RANSAC)算法在点云中拟合出立方体靶标的三个面在激光雷达坐标系下的方程，根据坐标系变换求出激光雷达坐标系到世界坐标系之间的旋转矩阵R_LW和平移矩阵T_LW。

相机拍摄立方体靶标，得到靶标的图像，该图像内同时包括立方体靶标的π₁、π₂、π₃三个面的棋盘格图像，将其拆分为三份，每份都只包含一个棋盘格的图像。根据张氏标定法，可以获得π₁、π₂、π₃三个平面在相机坐标系下的方程，根据坐标系变换求出相机坐标系到世界坐标系之间的旋转矩阵R_CW和平移矩阵T_CW。

最后，根据坐标变换，求出激光雷达坐标系到相机坐标系之间的旋转矩阵R_LC和平移矩阵T_LC。

标定的具体过程如下：

1.将激光雷达和相机固定好，标定过程中不能移动激光雷达和相机。

2.将立方体靶标放到激光雷达和相机的视场中央，使激光雷达和相机能够清晰的拍到靶标，将带有棋盘格靶标的三个平面π₁、π₂、π₃对准激光雷达和相机的镜头，使其能够拍摄得到清晰的图像。

3.打开激光雷达和相机，分别对靶标拍摄一次，得到稳定的数据。激光雷达拍摄得到的是靶标的深度图，相机拍摄得到的是靶标的彩色图像。

4.利用激光雷达的内参，根据小孔成像原理，将激光雷达拍摄得到的深度图转化为靶标的三维点云。

其中(^Lx_P,^Ly_P,^Lz_P)为转化后的三维点云中任意一点在激光雷达坐标系的坐标，

是深度图任意像素的深度值，m和n是深度图像素的位置，f是激光雷达的焦距，d_x和d_y是激光雷达感光芯片的每个像素的实际尺寸，u₀和v₀是激光雷达的光学中心的位置。

5.在靶标的三维点云中，利用随机采样一致性(RANSAC)算法，拟合出靶标的三个平面π₁、π₂、π₃，得到三个平面在激光雷达坐标系下的方程。

a₁x+b₁y+c₁z+d₁＝0

a₂x+b₂y+c₂z+d₂＝0

a₃x+b₃y+c₃z+d₃＝0

6.计算出平面π₁、π₂、π₃的单位法向量，方向选取符合右手坐标系的方向。如图3所示。

7.计算出平面π₁、π₂、π₃的交点坐标(^Lx_O,^Ly_O,^Lz_O)。

8.计算出激光雷达坐标系到世界坐标系的旋转矩阵R_LW和平移矩阵T_LW。

R_LW＝[^Ln₁,^Ln₂,^Ln₃]^-1

T_LW＝-[^Lx_O ^Ly_O ^Lz_O]^T

9.相机拍摄得到一张立方体靶标的图像，其中包含三个棋盘格的图像，将每个棋盘格的大致区域拆分开，得到三张棋盘格靶标的图像。

10.利用Harris角点探测法，检测出三张棋盘格图像的所有角点的像素坐标。

11.将每张棋盘格的尺寸和数量，相机内参，以及每张棋盘格上的所有角点像素坐标输入到张氏标定法的函数中，计算出平面π₁、π₂、π₃到相机坐标系的旋转矩阵R₁、R₂、R₃和平移矩阵T₁、T₂、T₃。

12.令平面π₁、π₂、π₃所在的世界坐标系下的平面方程均为Z＝0，因此单位法向量均为n_Z＝(0，0，1)，计算出平面π₁、π₂、π₃的法向量在相机坐标系下的表示^Cn₁,^Cn₂,^Cn₃。

^Cn₁＝R₁·(0，0，1)^T

^Cn₂＝R₂·(0，0，1)^T

^Cn₃＝R₃·(0，0，1)^T

13.计算出平面π₁、π₂、π₃在相机坐标系下的方程。

14.联立上述三个方程，计算出平面π₁、π₂、π₃的交点O在相机坐标系下的坐标。

(^Cx_O,^Cy_O,^Cz_O)＝[^Cn₁,^Cn₂,^Cn₃]^-1[T₁ ^TCn₁,^CT₂ ^Tn₂,^CT₃ ^Tn₃]

15.计算出相机坐标系到世界坐标系的旋转矩阵R_CW和平移矩阵T_CW。

R_CW＝[^Cn₁,^Cn₂,^Cn₃]^-1

T_CW＝-[^Cx_O,^Cy_O,^Cz_O]^T

16.计算出激光雷达坐标系到相机坐标系的旋转矩阵R_LC和平移矩阵T_LC。

Claims (1)

1.一种激光雷达和相机的联合标定方法，将传感器对准立方体靶标进行一次拍摄，激光雷达拍摄得到立方体靶标的三维点云，得到激光雷达坐标系到立方体靶标所在的世界坐标系之间的旋转矩阵R_LW和平移矩阵T_LW；相机拍摄得到立方体靶标的图像，得到相机坐标系到立方体靶标所在的世界坐标系之间的旋转矩阵R_CW和平移矩阵T_CW；最后根据对前两对旋转平移矩阵进行坐标系变换得到激光雷达和相机之间的旋转矩阵R_LC和平移矩阵T_LC，包括如下步骤:

(1)制作一个立方体盒子作为立方体靶标,盒子的三个相邻的面π₁、π₂、π₃布置一样的黑白棋盘格靶标,每个棋盘格的边长和棋盘格的数量已知；

(2)将激光雷达和相机固定，立方体靶标置于激光雷达和相机的视场中央，并且带有棋盘格的三个面朝向激光雷达和相机；

(3)激光雷达拍摄靶标获得靶标的深度图；

(4)根据小孔成像原理将深度图转化为激光雷达坐标系下的三维点云；

(5)根据上述方程得到π₁、π₂、π₃的单位法向量，方向选取符合右手坐标系的方向：

(6)计算出平面π₁、π₂、π₃的交点坐标：

(7)计算出激光雷达坐标系到世界坐标系的旋转矩阵R_LW和平移矩阵T_LW：

R_LW＝[^Ln₁,^Ln₂,^Ln₃]^-1

T_LW＝-[^Lx_O ^Ly_O ^Lz_O]^T

(8)相机拍摄得到一张立方体靶标的图像，将其中的三个棋盘格的图像拆分为三部分，每部分都只包含一张棋盘格的图像；

(9)根据张氏标定法计算出平面π₁、π₂、π₃到相机坐标系的旋转矩阵R₁、R₂、R₃和平移矩阵T₁、T₂、T₃；

(10)计算出平面π₁、π₂、π₃在相机坐标系下的的法向量；

^Cn₁＝R₁·(0，0，1)^T

^Cn₂＝R₂·(0，0，1)^T

^Cn₃＝R₃·(0，0，1)^T

(11)计算出平面π₁、π₂、π₃在相机坐标系下的方程：

(12)计算出平面π₁、π₂、π₃的交点0在相机坐标系下的坐标：

(13)计算出相机坐标系到世界坐标系的旋转矩阵R_CW和平移矩阵T_CW：

R_CW＝[^Cn₁,^Cn₂,^Cn₃]^-1

T_CW＝-[^Cx_O,^Cy_O,^Cz_O]^T

(14)计算出激光雷达坐标系到相机坐标系的旋转矩阵R_LC和平移矩阵T_LC：

Images