一种激光与相机任意相对位姿关系的标定方法与流程

本发明涉及多传感器标定技术，具体地说，涉及一种激光与相机任意相对位姿关系的标定方法。

背景技术：

伴随着传感技术的快速进步，为机器人配备多传感器的能力大大提高。不同传感器捕获的关于环境的补充信息，可以通过联合标定来共同使用，多传感器融合可以提高移动机器人的建图和定位的精度。在完成视觉相机和激光传感器的联合标定后，可以将从视觉相机中获得的“场景相关对象的语义信息”与从激光传感器中获得的“几何属性”进行数据融合，实现信息的最有效利用。

相机获得的是二维图像信息，而激光传感器采集的是三维点云信息。现有的直接匹配方法将2d图像与3d点云直接匹配，会有不同模型的信息利用问题，影响标定精度。而现有的基于运动的方法，对图像与图像进行匹配求解相机轨迹，点云与点云进行匹配求解激光传感器轨迹，再将两条轨迹进行对齐求解标定外参，但是点云与点云匹配时激光的数据噪声会导致不准确的运动估计进而影响标定。

在实际的应用中，对传感器安装的位置也有了新的要求，比如安装朝前的激光传感器用于检测四周的环境，而为了克服视野盲区安装了斜朝下的相机，那么在同一时刻，激光传感器和相机没有重叠视野。现有的大部分方法并不能标定无视野重合的激光与相机。

激光传感器的频率一般在10hz左右，工业相机的频率一般在25hz左右，无论怎么处理，两者采集的数据并不能严格的时间同步。因此在移动传感器采集数据进行标定时，大部分标定算法会有时空耦合的问题，如何把时间和空间进行解耦，简化我们要解决的问题，也是一大难点。

技术实现要素：

为了克服现有技术的不足，本发明的目的在于提供一种激光与相机任意相对位姿关系的标定方法，本发明是通过以下技术方案来实现的：

本发明公开过了一种激光与相机任意相对位姿关系的标定方法，包括以下步骤：

步骤一、将相机与激光传感器刚性连接固定，并保证它们的相对位置以及相机的内部参数不发生改变，形成相机激光感知模块；定义世界坐标系ow、相机坐标系oc、激光坐标系ol；对相机的内部参数进行标定，得到相机的内部参数矩阵其中(fx，fy)为相机的等效焦距，(u0,v0)为相机光心坐标；

步骤二、在空间中围绕相机激光感知模块放置多块标定板，不同标定板之间有高低和俯仰的变化；

固定各标定板的空间位置不变，打开激光传感器，采集一帧激光传感器数据，激光坐标系ol为采集这一帧时的激光传感器位置；采集当前相机图像，移动相机激光感知模块，用相机依次近距离采集各个标定板的图像，相机坐标系oc为采集第一帧相机图像时的相机位置；采集激光传感器与相机数据，提取激光棋盘格3d点和重建视觉3d点；

步骤三、视觉3d点在相机坐标系oc下的点为pc，先经过粗糙外参转换为激光坐标系ol下表示为pcl，粗糙外参是手动量取和估计相机到激光传感器的旋转、平移，使用knn最近邻搜索n个激光坐标系ol下距离pcl最近的激光3d点pl，构造视觉点到激光平面的点到面优化方程，通过计算梯度下降方向求解激光与相机的相对位姿。

作为进一步地改进，本发明所述的标定板是使用黑白棋盘格标定板或使用任意带图案的平面板。

作为进一步地改进，本发明所述的标定板围绕相机激光分散摆放在空间中，各标定板之间要求有高低和俯仰的变换。

作为进一步地改进，本发明所述的固定各标定板的空间中位置，采集一帧激光数据，采集当前时刻的相机图像，移动相机激光感知模块，用相机依次近距离采集各个标定板的图像，使每块标定板都至少一次完整的出现在相机图像中。

作为进一步地改进，本发明所述的步骤二中，采集激光传感器与相机数据，提取激光棋盘格3d点和重建视觉3d点是利用ransac拟合或法向量聚类或人工标注的方法提取激光传感器采集的数据中的棋盘格3d点；提取相机采集的图像中的棋盘格像素点并标记，处理采集的所有相机图片作同步的建图和定位，将所标记的图像像素点重建为空间中3d点。

作为进一步地改进，本发明所述的步骤三中，构造视觉点到激光平面的点到面优化方程，通过计算梯度下降方向求解激光与相机的相对位姿，优化方程推导如下：视觉3d点在相机坐标系oc下的点为pc，先经过粗糙外参(手动量取和估计相机到激光传感器的旋转、平移)转换为激光坐标系ol下表示为pcl，使用knn最近邻搜索n个激光坐标系ol下距离pcl最近的激光3d点pl，计算pcl到pl所在平面的点到面误差：其中，pc是相机坐标系下的视觉3d点，r是相机坐标系到激光坐标系的旋转矩阵，t是相机坐标系到激光坐标系的平移矩阵，pl是激光坐标系下的激光3d点，nl是pl所在平面的法向量。

作为进一步地改进，本发明所述优化方程的求解如下：求解误差方程相对与外参r，t的雅可比矩阵：其中，^代表的是括号中向量的反对称矩阵；对于上述不方便直接求解的非线性最小二乘问题，采用的是迭代的方法，从一个初始值出发，不断地更新当前的优化变量(相机和激光的相对位姿r，t)，使目标函数下降；采用高斯牛顿法求解上述问题，j^tj决定了梯度下降的方向，在代入采集到的数据之后，可以联合优化出外参r，t。

相比现有技术，本发明的有益效果在于：

本发明公开了一种激光与相机任意相对位姿关系的标定方法，通过非线性优化方法，提取激光棋盘格平面点，提取图像棋盘格像素点并重建为空间视觉3d点，构造视觉点到激光平面的点到面优化方程，通过计算梯度下降方向求解激光与相机的相对位姿。

本发明方法的优势在于：

1、相机获得的是二维图像信息，而激光传感器采集的是三维点云信息。现有的直接匹配方法将2d图像与3d点云直接匹配，会有不同模型的信息利用问题，影响标定精度。而现有的基于运动的方法，对图像与图像进行匹配求解相机轨迹，点云与点云进行匹配求解激光传感器轨迹，再将两条轨迹进行对齐求解标定外参，但是点云与点云匹配时激光的数据噪声会导致不准确的运动估计进而影响标定。本发明中提及的标定方法，只用了一帧激光数据，没有做点云与点云之间的匹配，避免了点云的数据噪声带来的运动估计偏差，并且通过对采集的图像做同步的建图和定位，将2d图像重建为3d视觉点云，避免了不同模型的信息利用问题。而3d视觉点云与3d激光点云也没有做直接的匹配，而是通过knn的最近邻搜索筛选之后做3d视觉点到激光平面的优化，从而提升了标定精度。

2、在实际的应用中，对传感器安装的位置也有了新的要求，比如安装朝前的激光传感器用于检测四周的环境，而为了克服视野盲区安装了斜朝下的相机，那么在同一时刻，激光传感器和相机没有重叠视野。现有的大部分方法并不能标定无视野重合的激光与相机。本发明无需传感器有视野重合，激光传感器采集当前环境的一帧数据，之后移动相机采集所有标定板的图像，只要标定板固定不动，通过对图像的点云重建，可以得到同一标志物的激光点云和视觉3d点云，从而完成标定。

3、激光传感器的频率一般在10hz左右，工业相机的频率一般在25hz左右，无论怎么处理，两者采集的数据并不能严格的时间同步。因此在移动传感器采集数据进行标定时，大部分标定算法会有时空耦合的问题，如何把时间和空间进行解耦，简化我们要解决的问题，也是一大难点。本发明通过激光传感器只采集一帧数据，固定空间中的标定板，将时间和空间进行解耦，消除无法完成激光相机数据时间同步带来的误差。

4、激光的一帧数据是360°的，而相机的视野只有60°左右，这里面有很多的激光数据带来的信息是被浪费的。本发明通过在空间中围绕相机放置多个有高低、俯仰变换的标定板，并且通过移动相机采集标定板图像重建视觉3d点云，模拟出视觉相机的360°信息，从而使参与标定的信息足够的丰富，进而提高标定的精度。

本发明克服了现有的激光相机标定方法中信息利用不恰当，时空耦合，并且无法标定无视野重叠的激光相机的不足，具有可操作性好，标定精度高等优点，并且可以完成实时的标定，在机器人采集数据的同时完成标定，大大降低标定工作量。本发明实用性较高，标定结果能够应用于计算机视觉中的多传感器融合等多种领域。

附图说明

图1为本发明方法的流程示意图；

图2为本发明实施装置示意图；

图3为图1中优化过程的示意图。

具体实施方式

本发明公开了一种激光与相机任意相对位姿关系的标定方法，图1为本发明方法的流程示意图，图2为本发明实施装置示意图，包括以下步骤：

步骤一、将相机与激光传感器刚性连接固定，并保证它们的相对位置以及相机的内部参数不发生改变，对连接角度无要求，相机和激光传感器视野可以不重叠；定义世界坐标系ow、相机坐标系oc、激光坐标系ol；对相机的内部参数进行标定，得到相机的内部参数矩阵其中(fx，fy)为相机的等效焦距，(u0,v0)为相机光心坐标；

步骤二、在空间中围绕相机激光感知模块放置多块标定板，要求不同标定板之间有高低和俯仰的变化；标定板不做限制，可以使用黑白棋盘格标定板，也可以使用任意带图案的平面板；标定板围绕相机激光分散摆放在空间中，各标定板之间要求有高低和俯仰的变换。

本发明所述的数据采集流程具体为：固定各标定板的空间中位置不变，打开激光传感器，采集一帧激光数据，激光坐标系ol为采集这一帧时的激光传感器位置；采集当前时刻的相机图像，移动相机激光感知模块，用相机依次近距离采集各个标定板的图像，使每块标定板都至少一次完整的出现在相机图像中，相机坐标系oc为采集第一帧相机图像时的相机位置。

利用ransac拟合或法向量聚类或人工标注的方法提取激光传感器采集的数据中的棋盘格3d点；提取相机采集的图像中的棋盘格像素点并标记，处理采集的所有相机图片做同步的建图和定位，将所标记的图像像素点重建为空间中3d点；

步骤三、视觉3d点在相机坐标系oc下的点为pc，先经过粗糙外参(手动量取和估计相机到激光传感器的旋转、平移)转换为激光坐标系ol下表示为pcl，使用knn最近邻搜索n个激光坐标系ol下距离pcl最近的激光3d点pl，计算pcl到pl所在平面的点到面误差：

其中，pc是相机坐标系下的视觉3d点，r是相机坐标系到激光坐标系的旋转矩阵，t是相机坐标系到激光坐标系的平移矩阵，pl是激光坐标系下的激光3d点，nl是pl所在平面的法向量；

求解误差方程相对与外参r，t的雅可比矩阵：

其中，^代表的是括号中向量的反对称矩阵；

对于上述不方便直接求解的非线性最小二乘问题，采用的是迭代的方法，从一个初始值出发，不断地更新当前的优化变量(相机和激光的相对位姿r，t)，使目标函数下降；采用高斯牛顿法求解上述问题，j^tj决定了梯度下降的方向，在代入采集到的数据之后，可以联合优化出外参r，t。

图3为图1中优化过程的示意图，从左往右6个相机位姿代表相机随时间改变的移动，每个时刻相机的位姿估计靠视觉里程计约束进行确定。不同时刻的相机观测到不同的环境，环境由地图上的路标3d点表示。取其中的棋盘格视觉3d点，搜索激光坐标系下欧式距离最近的激光平面，构造视觉点到激光平面的点到面优化方程，通过计算梯度下降方向求解激光与相机的相对位姿。

下面，结合具体实施方式，对本发明的技术方案作进一步地描述：

本发明公开了一种机器人多相机视觉惯性实时定位方法，包括以下步骤：

1、将相机与激光传感器刚性连接固定，并保证它们的相对位置以及相机的内部参数不发生改变，对连接角度无要求，相机和激光传感器视野可以不重叠；定义世界坐标系ow、相机坐标系oc、激光坐标系ol；对相机的内部参数进行标定，得到相机的内部参数矩阵其中(fx，fy)为相机的等效焦距，(u0,v0)为相机光心坐标；

2、在空间中围绕相机激光感知模块放置多块标定板，要求不同标定板之间有高低和俯仰的变化；所述的标定板采用黑白棋盘格板；

3、固定各标定板的空间位置不变，打开激光传感器，采集一帧激光传感器数据，激光坐标系ol为采集这一帧时的激光传感器位置；采集当前相机图像，移动相机激光感知模块，用相机依次近距离采集各个标定板的图像，相机坐标系oc为采集第一帧相机图像时的相机位置；

4、利用ransac拟合或法向量聚类或人工标注的方法提取激光传感器采集的数据中的棋盘格3d点；提取相机采集的图像中的棋盘格像素点并标记，处理采集的所有相机图片做同步的建图和定位，将所标记的图像像素点重建为空间中3d点；

5、视觉3d点在相机坐标系oc下的点为pc，先经过粗糙外参(手动量取和估计相机到激光传感器的旋转、平移)转换为激光坐标系ol下表示为pcl，使用knn最近邻搜索n个激光坐标系ol下距离pcl最近的激光3d点pl，计算pcl到pl所在平面的点到面误差：

其中，pc是相机坐标系下的视觉3d点，r是相机坐标系到激光坐标系的旋转矩阵，t是相机坐标系到激光坐标系的平移矩阵，pl是激光坐标系下的激光3d点，nl是pl所在平面的法向量；

6、求解误差方程相对与外参r，t的雅可比矩阵：

其中，^代表的是括号中向量的反对称矩阵；

对于上述不方便直接求解的非线性最小二乘问题，采用的是迭代的方法，从一个初始值出发，不断地更新当前的优化变量(相机和激光的相对位姿r，t)，使目标函数下降；采用高斯牛顿法求解上述问题，j^tj决定了梯度下降的方向，在代入采集到的数据之后，可以联合优化出外参r，t。

本发明提出的标定方法，通过非线性优化方法，提取激光棋盘格平面点，提取图像棋盘格像素点并重建为空间视觉3d点，构造视觉点到激光平面的点到面优化方程，通过计算梯度下降方向求解激光与相机的相对位姿。本发明具有可操作性好，标定精度高等优点，并且可以完成实时的标定，在机器人采集数据的同时完成标定，大大降低标定工作量。本发明实用性较高，标定结果能够应用于计算机视觉中的多传感器融合等多种领域。

最后，还需注意的是，以上列举的仅是本发明的具体实施例。显然，本发明不限于以上实施例，还可以有许多变形。本领域的普通技术人员能从本发明公开的内容直接导出或联想到的所有变形，均应认为是本发明的保护范围。