CN110390695B - 一种基于ros的激光雷达、摄像头的融合标定系统及标定方法 - Google Patents

Abstract

本发明涉及一种基于ROS的激光雷达、摄像头的融合标定系统及标定方法，包括雷达标定模块，其用于获取激光雷达的外部参数，即激光雷达的雷达坐标系到世界坐标系的变换关系；相机标定模块，用于获取摄像头的内部参数；融合标定模块，用于接收激光雷达的外部参数以及相机的内部参数，求解相机的外部参数，即求解出世界坐标系到相机坐标系的变换关系；雷达标定模块、相机标定模块以及融合标定模块之间基于ROS平台实现数据的传输；雷达标定模块包括激光雷达，还包括桩桶以及两块标定板，前述的相机标定模块包括摄像头以及一块标定板；本发明实现激光雷达和摄像头数据融合的空间同步方法，解决了现有单一传感器感知存在的感知维度不足、信息缺乏等缺陷。

Description

一种基于ROS的激光雷达、摄像头的融合标定系统及标定方法

技术领域

本发明涉及一种基于ROS的激光雷达、摄像头的融合标定系统及标定方法，属于激光雷达和摄像头之间的融合技术领域。

背景技术

无人车的环境感知技术主要是借助外在传感器，例如激光雷达、摄像头、毫米波雷达等实现对周围环境的探测，保障无人车可以及时地、精准地感知路面环境中存在的安全隐患，快速采取措施，避免发生交通事故。环境感知相当于无人车的眼睛，对于保障无人驾驶汽车安全行驶具有不可替代的作用。

目前有多种环境感知的方法：视觉传感是基于机器视觉获取车辆周边环境的图像信息，通过图像处理等方式对周围环境进行感知。它能直接获取色彩信息，并且摄像机具有安装方便、体积小、能耗低的优点，它的缺点就是容易受到光线的干扰、同时没有三维信息。激光传感是通过雷达扫描车辆周围环境来获得三维距离信息，通过距离分析识别技术对行驶路况进行感知。它能够直接获取物体三维距离信息，具有很高的测量精度并且对光线明暗变化不敏感，但是它无法获得环境的色彩信息，同时目前激光雷达价格昂贵，难以在家用轿车上装备。

激光雷达与摄像头融合，相当于融合激光雷达的三维信息以及图像的色彩信息，实现两个传感器优势互补。融合包含不同传感器的时间同步和空间同步。本发明正是基于此背景，提出基于ROS的激光雷达、摄像头的融合标定系统及方法，解决激光雷达与摄像头的空间同步问题。

发明内容

本发明提供一种基于ROS的激光雷达、摄像头的融合标定系统及标定方法，实现激光雷达和摄像头数据融合的空间同步方法，解决了现有单一传感器感知存在的感知维度不足、信息缺乏等缺陷。

本发明解决其技术问题所采用的技术方案是：

一种基于ROS的激光雷达、摄像头的融合标定系统，包括雷达标定模块，其用于获取激光雷达的外部参数，即激光雷达的雷达坐标系到世界坐标系的变换关系；

相机标定模块，用于获取摄像头的内部参数；

融合标定模块，用于接收激光雷达的外部参数以及相机的内部参数，求解相机的外部参数，即求解出世界坐标系到相机坐标系的变换关系；

前述的雷达标定模块、相机标定模块以及融合标定模块之间基于ROS平台实现数据的传输；

前述的雷达标定模块包括激光雷达，还包括桩桶以及两块标定板，

前述的相机标定模块包括摄像头以及一块标定板；

作为本发明的进一步优选，

前述的雷达标定模块，通过放置在激光雷达外部不同位置处的桩桶以及两块标定板，提取桩桶的一个角点以及两块标定板中的6-8个角点，获取前述所有角点在世界坐标系中的坐标以及在雷达坐标系中的坐标，通过解方程获取雷达坐标系至世界坐标系的变换矩阵；

前述的相机标定模块，通过将标定板在摄像头中置于不同角度及位置，且基于ROS获取相机内部参数，即将相机坐标系中的物体成像至像素坐标；

前述的融合标定模块，将雷达标定模块中的7-9个角点在雷达坐标系中的坐标变换至世界坐标系中的坐标，同时通过相机标定模块获取前述7-9个角点的像素坐标，通过求解位姿估计问题，得到世界坐标系至像素坐标系的转换关系，实现三维物体与二维像素的匹配，即融合；

一种基于ROS的激光雷达、摄像头的融合标定方法，包括以下步骤：

第一步：通过相机标定模块获取摄像头的内部参数，即相机坐标系至像素坐标系的转换关系；

第二步：通过雷达标定模块获取激光雷达坐标与世界坐标之间的外部参数，即雷达坐标系与世界坐标系之间的转换关系；

第三步：在融合模块中，获取雷达标定模块中桩桶以及两块标定板的7-9个角点作为特征点，其中选取9个特征点，分别获取前述9个特征点分别在世界坐标系、像素坐标系中的坐标；

第四步：通过融合标定模块求解位姿估计问题，即求解出上述9个特征点世界坐标系到像素坐标系的转换关系，前述转换关系的实现通过DLS、UPnP、P3P以及EPnP算法实现，完成两者的融合；

第五步：将激光雷达实时获取的点云数据通过转换关系即变换矩阵投影至摄像头获取的图像中，验证是否对应，若两者匹配有误则继续重复上述融合步骤；

第六步：若两者匹配无误，则证明已经获取世界坐标系至像素坐标系的转换关系，完成融合工作；

作为本发明的进一步优选，

前述的第一步中通过相机标定模块获取摄像头的内部参数，具体包括以下步骤：

第11步，准备好带有黑板棋盘格的标定板；

第12步：利用基于ROS平台的摄像头调用可见光摄像头，并通过内置或编写显示图像的ROS包，完成摄像头数据的调用以及实时显示；

第13步：启动ROS平台中的单目相机标定工具；

第14步：移动标定板到画面的最左、最右，最上方、最下方，移动标定板到视野的最近处和最远处，移动标定板使其充满整个画面，同时保持标定板倾斜状态，获取标定板在画面各个位置对应的数据，完成相机内参标定；

作为本发明的进一步优选，

前述标定方法中的第二步，具体包括以下步骤：

第21步：将激光雷达供电，激光雷达发射器发射激光束，激光雷达接收器接收反射回来的点云数据；

第22步：利用ROS平台打开工控机网口，接收激光雷达点云数据，将消息转化为基于ROS平台下的点云标准数据；

第23步：通过在激光雷达前方人为设置具有角点的装置，即一块桩桶及两块标定板，获取其6-8个角点的世界坐标系中的坐标以及雷达坐标系坐标；

第24步：求解位姿变换问题，得到雷达坐标系相对于世界坐标系的变换关系；

作为本发明的进一步优选，

前述标定方法中的第三步以及第四步，具体包括以下步骤：

第31步：获取相机内摄像头的内部参数，对图像进行去畸变；

第32步：将去畸变的图像基于ROS平台进行编辑，同时雷达标定模块接收去畸变的图像消息；

第33步：显示去畸变的图像，在图像最左侧以及最右侧各放置一块标定板，同时保证标定板的上沿以及下沿在激光线束的密集区，在图像中间位置放置一个桩桶；

第34步：提取桩桶的一个角点以及两块标定板中的8个角点，共计9个角点的雷达坐标系中的坐标，以及通过相机标定模块获取9个角点的像素坐标系中的坐标；

第35步：通过上述第二步中获取雷达坐标系相对于世界坐标系的变换关系，得到9个角点在世界坐标系中的坐标，通过相机标定模块获取9个角点在像素坐标系中的坐标；

第41步：通过DLS、UPnP、P3P以及EPnP算法实现9个角点在世界坐标系中的坐标和像素坐标系中的坐标的融合，最终获取9个角点的世界坐标系到像素坐标系的变换关系；

作为本发明的进一步优选，

前述标定方法中的第五步，具体包括以下步骤：

第51步：将去畸变的图像基于ROS平台进行编辑，同时激光雷达通过ROS平台发布点云数据；

第52步：将激光雷达实时获取的点云图通过激光雷达坐标系到世界坐标系的变换、世界坐标系到像素坐标系的变换关系，投影至图像中，验证是否对应，如两者匹配有误则重复上述融合操作；

作为本发明的进一步优选，

前述的世界坐标到像素坐标的变换矩阵为：设某点在{world}即世界坐标系中的坐标为P_w＝(x_w,y_w,z_w)^T，在{camera}即相机坐标系中的坐标为P_c＝(x_c,y_c,z_c)，则

T＝[t_x t_y t_z]^T；

而上述相机坐标系至像素坐标的最终转换，是将相机的内部参数分为内参矩阵和畸变矩阵两部分，

设某点在{camera}相机坐标系中的坐标为P_c＝(x_c,y_c,z_c)，则像素坐标{u,v}为

/>

中间k成为相机内参矩阵，c_x和c_y表示相机光轴在像素坐标系中的偏移量，f_x和f_y为u轴和v轴上的归一化焦距；

平面上的任意一点p可以用笛卡尔坐标表示为[x,y]^T，也可以用极坐标r,θ表示

其中r表示点p离坐标系原点的距离，θ表示和水平轴的夹角,

由于具有畸变的存在，畸变矩阵用[k₁,k₂,k₃,p₁,p₂]表示

[x,y]^T是被纠正的点的坐标，[x_corrected,y_corrected]纠正后的点的坐标将纠正后的点通过内参数矩阵投影到像素平面，

最终像素坐标[u,v]可表示为：

通过以上技术方案，相对于现有技术，本发明具有以下有益效果：

本发明通过基于ROS平台下协调摄像头以及激光雷达，通过将雷达坐标系变换到世界坐标系，手动提取特征点求解雷达-世界-相机三者变换矩阵，实现激光雷达到摄像头的标定从而达到激光雷达点云数据与相机彩色图片的精准融合；该方法不需借助复杂设备，同时系统运行在ROS平台中，便于嵌入式开发及使用。

附图说明

下面结合附图和实施例对本发明进一步说明。

图1是本发明的优选实施例的标定系统的整体框架图。

具体实施方式

现在结合附图对本发明作进一步详细的说明。这些附图均为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。

如图1所示，本发明的一种基于ROS的激光雷达、摄像头的融合标定系统，包括雷达标定模块，其用于获取激光雷达的外部参数，即激光雷达的雷达坐标系到世界坐标系的变换关系；

相机标定模块，用于获取摄像头的内部参数；

融合标定模块，用于接收激光雷达的外部参数以及相机的内部参数，求解相机的外部参数，即求解出世界坐标系到相机坐标系的变换关系；

前述的雷达标定模块、相机标定模块以及融合标定模块之间基于ROS平台实现数据的传输；

前述的雷达标定模块包括激光雷达，还包括桩桶以及两块标定板，

前述的相机标定模块包括摄像头以及一块标定板；

实施例1：

前述的雷达标定模块，通过放置在激光雷达外部不同位置处的桩桶以及两块标定板，提取桩桶的一个角点以及两块标定板中的6-8个角点，获取前述所有角点在世界坐标系中的坐标以及在雷达坐标系中的坐标，通过解方程获取雷达坐标系至世界坐标系的变换矩阵；

前述的相机标定模块，通过将标定板在摄像头中置于不同角度及位置，且基于ROS获取相机内部参数，即将相机坐标系中的物体成像至像素坐标；

前述的融合标定模块，将雷达标定模块中的7-9个角点在雷达坐标系中的坐标变换至世界坐标系中的坐标，同时通过相机标定模块获取前述7-9个角点的像素坐标，通过求解位姿估计问题，得到世界坐标系至像素坐标系的转换关系，实现三维物体与二维像素的匹配，即融合；

实施例2：

一种基于ROS的激光雷达、摄像头的融合标定方法，包括以下步骤：

第一步：通过相机标定模块获取摄像头的内部参数，即相机坐标系至像素坐标系的转换关系；

第二步：通过雷达标定模块获取激光雷达坐标与世界坐标之间的外部参数，即雷达坐标系与世界坐标系之间的转换关系；

第三步：在融合模块中，获取雷达标定模块中桩桶以及两块标定板的7-9个角点作为特征点，其中选取9个特征点，分别获取前述9个特征点分别在世界坐标系、像素坐标系中的坐标；

第四步：通过融合标定模块求解位姿估计问题，即求解出上述9个特征点世界坐标系到像素坐标系的转换关系，前述转换关系的实现通过DLS、UPnP、P3P以及EPnP算法实现，完成两者的融合；

第五步：将激光雷达实时获取的点云数据通过转换关系即变换矩阵投影至摄像头获取的图像中，验证是否对应，若两者匹配有误则继续重复上述融合步骤；

第六步：若两者匹配无误，则证明已经获取世界坐标系至像素坐标系的转换关系，完成融合工作；

作为本发明的进一步优选，

前述的第一步中通过相机标定模块获取摄像头的内部参数，具体包括以下步骤：

第11步，准备好带有黑板棋盘格的标定板；

第12步：利用基于ROS平台的摄像头调用可见光摄像头，并通过内置或编写显示图像的ROS包，完成摄像头数据的调用以及实时显示；

第13步：启动ROS平台中的单目相机标定工具；

第14步：移动标定板到画面的最左、最右，最上方、最下方，移动标定板到视野的最近处和最远处，移动标定板使其充满整个画面，同时保持标定板倾斜状态，获取标定板在画面各个位置对应的数据，完成相机内参标定；

作为本发明的进一步优选，

前述标定方法中的第二步，具体包括以下步骤：

第21步：将激光雷达供电，激光雷达发射器发射激光束，激光雷达接收器接收反射回来的点云数据；

第22步：利用ROS平台打开工控机网口，接收激光雷达点云数据，将消息转化为基于ROS平台下的点云标准数据；

第23步：通过在激光雷达前方人为设置具有角点的装置，即一块桩桶及两块标定板，获取其6-8个角点的世界坐标系中的坐标以及雷达坐标系坐标；

第24步：求解位姿变换问题，得到雷达坐标系相对于世界坐标系的变换关系；

作为本发明的进一步优选，

前述标定方法中的第三步以及第四步，具体包括以下步骤：

第31步：获取相机内摄像头的内部参数，对图像进行去畸变；

第32步：将去畸变的图像基于ROS平台进行编辑，同时雷达标定模块接收去畸变的图像消息；

第33步：显示去畸变的图像，在图像最左侧以及最右侧各放置一块标定板，同时保证标定板的上沿以及下沿在激光线束的密集区，在图像中间位置放置一个桩桶；

第34步：提取桩桶的一个角点以及两块标定板中的8个角点，共计9个角点的雷达坐标系中的坐标，以及通过相机标定模块获取9个角点的像素坐标系中的坐标；

第35步：通过上述第二步中获取雷达坐标系相对于世界坐标系的变换关系，得到9个角点在世界坐标系中的坐标，通过相机标定模块获取9个角点在像素坐标系中的坐标；

第41步：通过DLS、UPnP、P3P以及EPnP算法实现9个角点在世界坐标系中的坐标和像素坐标系中的坐标的融合，最终获取9个角点的世界坐标系到像素坐标系的变换关系；

作为本发明的进一步优选，

前述标定方法中的第五步，具体包括以下步骤：

第51步：将去畸变的图像基于ROS平台进行编辑，同时激光雷达通过ROS平台发布点云数据；

第52步：将激光雷达实时获取的点云图通过激光雷达坐标系到世界坐标系的变换、世界坐标系到像素坐标系的变换关系，投影至图像中，验证是否对应，如两者匹配有误则重复上述融合操作；

作为本发明的进一步优选，

前述的世界坐标到像素坐标的变换矩阵为：设某点在{world}即世界坐标系中的坐标为P_w＝(x_w,y_w,z_w)^T，在{camera}即相机坐标系中的坐标为P_c＝(x_c,y_c,z_c)，则

T＝[t_x t_y t_z]^T；

而上述相机坐标系至像素坐标的最终转换，是将相机的内部参数分为内参矩阵和畸变矩阵两部分，

设某点在{camera}相机坐标系中的坐标为P_c＝(x_c,y_c,z_c)，则像素坐标{u,v}为

中间k成为相机内参矩阵，c_x和c_y表示相机光轴在像素坐标系中的偏移量，f_x和f_y为u轴和v轴上的归一化焦距；

平面上的任意一点P可以用笛卡尔坐标表示为[x,y]^T，也可以用极坐标r,θ表示；

其中r表示点P离坐标系原点的距离，θ表示和水平轴的夹角,

由于具有畸变的存在，畸变矩阵用[k₁,k₂,k₃,p₁,p₂]表示

[x,y]^T是被纠正的点的坐标，[x_corrected,y_corrected]纠正后的点的坐标将纠正后的点通过内参数矩阵投影到像素平面，

最终像素坐标[u,v]可表示为：

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本申请所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

本申请中所述的“和/或”的含义指的是各自单独存在或两者同时存在的情况均包括在内。

本申请中所述的“连接”的含义可以是部件之间的直接连接也可以是部件间通过其它部件的间接连接。

以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关工作人员完全可以在不偏离本项发明技术思想的范围内，进行多样的变更以及修改。本项发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。

Claims (7)

1.一种基于ROS的激光雷达、摄像头的融合标定系统，其特征在于：包括雷达标定模块，其用于获取激光雷达的外部参数，即激光雷达的雷达坐标系到世界坐标系的变换关系；

相机标定模块，用于获取摄像头的内部参数；

融合标定模块，用于接收激光雷达的外部参数以及相机的内部参数，求解相机的外部参数，即求解出世界坐标系到相机坐标系的变换关系；

前述的雷达标定模块、相机标定模块以及融合标定模块之间基于ROS平台实现数据的传输；

前述的雷达标定模块包括激光雷达，还包括桩桶以及两块标定板，

前述的相机标定模块包括摄像头以及一块标定板；

前述的雷达标定模块，通过放置在激光雷达外部不同位置处的桩桶以及两块标定板，提取桩桶的一个角点以及两块标定板中的6-8个角点，获取所有角点在世界坐标系中的坐标以及在雷达坐标系中的坐标，通过解方程获取雷达坐标系至世界坐标系的变换矩阵；

前述的相机标定模块，通过将标定板在摄像头中置于不同角度及位置，且基于ROS获取相机内部参数，即将相机坐标系中的物体成像至像素坐标；

前述的融合标定模块，将雷达标定模块中的7-9个角点在雷达坐标系中的坐标变换至世界坐标系中的坐标，同时通过相机标定模块获取前述7-9个角点的像素坐标，通过求解位姿估计问题，得到世界坐标系至像素坐标系的转换关系，实现三维物体与二维像素的匹配，即融合。

2.一种基于ROS的激光雷达、摄像头的融合标定方法，用于如权利要求1所述的融合标定系统，其特征在于：包括以下步骤：

第一步：通过相机标定模块获取摄像头的内部参数，即相机坐标系至像素坐标系的转换关系；

第二步：通过雷达标定模块获取激光雷达坐标与世界坐标之间的外部参数，即雷达坐标系与世界坐标系之间的转换关系；

第三步：在融合模块中，获取雷达标定模块中桩桶以及两块标定板的7-9个角点作为特征点，其中选取9个特征点，分别获取前述9个特征点分别在世界坐标系、像素坐标系中的坐标；

第四步：通过融合标定模块求解位姿估计问题，即求解出上述9个特征点世界坐标系到像素坐标系的转换关系，前述转换关系的实现通过DLS、UPnP、P3P以及EPnP算法实现，完成两者的融合；

第五步：将激光雷达实时获取的点云数据通过转换关系即变换矩阵投影至摄像头获取的图像中，验证是否对应，若两者匹配有误则继续重复第一步至第四步步骤；

第六步：若两者匹配无误，则证明已经获取世界坐标系至像素坐标系的转换关系，完成融合工作。

3.根据权利要求2所述的基于ROS的激光雷达、摄像头的融合标定方法，其特征在于：前述的第一步中通过相机标定模块获取摄像头的内部参数，具体包括以下步骤：

第11步，准备好带有黑板棋盘格的标定板；

第12步：利用基于ROS平台的摄像头调用可见光摄像头，并通过内置或编写显示图像的ROS包，完成摄像头数据的调用以及实时显示；

第13步：启动ROS平台中的单目相机标定工具；

第14步：移动标定板到画面的最左、最右，最上方、最下方，移动标定板到视野的最近处和最远处，移动标定板使其充满整个画面，同时保持标定板倾斜状态，获取标定板在画面各个位置对应的数据，完成相机内参标定。

4.根据权利要求2所述的基于ROS的激光雷达、摄像头的融合标定方法，其特征在于：前述标定方法中的第二步，具体包括以下步骤：

第21步：将激光雷达供电，激光雷达发射器发射激光束，激光雷达接收器接收反射回来的点云数据；

第22步：利用ROS平台打开工控机网口，接收激光雷达点云数据，将消息转化为基于ROS平台下的点云标准数据；

第23步：通过在激光雷达前方人为设置具有角点的装置，即一块桩桶及两块标定板，获取其6-8个角点的世界坐标系中的坐标以及雷达坐标系坐标；

第24步：求解位姿变换问题，得到雷达坐标系相对于世界坐标系的变换关系。

5.根据权利要求2所述的基于ROS的激光雷达、摄像头的融合标定方法，其特征在于：前述标定方法中的第三步以及第四步，具体包括以下步骤：

第31步：获取相机内摄像头的内部参数，对图像进行去畸变；

第32步：将去畸变的图像基于ROS平台进行编辑，同时雷达标定模块接收去畸变的图像消息；

第33步：显示去畸变的图像，在图像最左侧以及最右侧各放置一块标定板，同时保证标定板的上沿以及下沿在激光线束的密集区，在图像中间位置放置一个桩桶；

第34步：提取桩桶的一个角点以及两块标定板中的8个角点，共计9个角点的雷达坐标系中的坐标，以及通过相机标定模块获取9个角点的像素坐标系中的坐标；

第35步：通过上述第二步中获取雷达坐标系相对于世界坐标系的变换关系，得到9个角点在世界坐标系中的坐标，通过相机标定模块获取9个角点在像素坐标系中的坐标；

第41步：通过DLS、UPnP、P3P以及EPnP算法实现9个角点在世界坐标系中的坐标和像素坐标系中的坐标的融合，最终获取9个角点的世界坐标系到像素坐标系的变换关系。

6.根据权利要求2所述的基于ROS的激光雷达、摄像头的融合标定方法，其特征在于：前述标定方法中的第五步，具体包括以下步骤：

第51步：将去畸变的图像基于ROS平台进行编辑，同时激光雷达通过ROS平台发布点云数据；

第52步：将激光雷达实时获取的点云图通过激光雷达坐标系到世界坐标系的变换、世界坐标系到像素坐标系的变换关系，投影至图像中，验证是否对应，如两者匹配有误则重复第一步至第四步操作。

7.根据权利要求2所述的基于ROS的激光雷达、摄像头的融合标定方法，其特征在于：前述的世界坐标到像素坐标的变换矩阵为：设某点在世界坐标系中的坐标为P_w＝(x_w,y_w,z_w)^T，在相机坐标系中的坐标为P_c＝(x_c,y_c,z_c)，则

/>

T＝[t_x t_y t_z]^T；

而上述相机坐标系至像素坐标的最终转换，是将相机的内部参数分为内参矩阵和畸变矩阵两部分，

设某点在相机坐标系中的坐标为P_c＝(x_c,y_c,z_c)，则像素坐标{u,v}为

中间k成为相机内参矩阵，c_x和c_y表示相机光轴在像素坐标系中的偏移量，f_x和f_y为u轴和v轴上的归一化焦距

平面上的任意一点p可以用笛卡尔坐标表示为[x,y]^T，也可以用极坐标r,θ表示，

其中r表示点p离坐标系原点的距离，θ表示和水平轴的夹角,

由于具有畸变的存在，畸变矩阵用[k₁,k₂,k₃,p₁,p₂]表示

[x,y]^T是被纠正的点的坐标，[x_corrected,y_corrected]纠正后的点的坐标将纠正后的点通过内参数矩阵投影到像素平面,

最终像素坐标[u,v]可表示为：

/>

Images