CN111524194A - 一种激光雷达和双目视觉相互融合的定位方法及终端 - Google Patents

Abstract

本发明提供了一种激光雷达和双目视觉相互融合的定位方法及终端，将激光雷达和摄像头的外参数进行标定，使激光雷达测量到的第一数据和摄像头测量到的第二数据实现点对点对应；利用所述第一数据辅助估计所述第二数据的深度误差；利用所述第二数据去除所述第一数据中的畸变，得到第一点云；利用所述第一点云、第二数据和所述第二数据的深度误差，计算出激光雷达和摄像头所在载体的位姿信息；联合优化结果，确定激光雷达的位姿，利用坐标转换关系得到激光雷达和摄像头所在载体的位姿，在定位过程中高度融合激光雷达的测量数据和摄像头的测量数据，而不仅仅是做简单的点对点匹配，提高了定位精度和定位方法的鲁棒性。

Description

一种激光雷达和双目视觉相互融合的定位方法及终端

技术领域

本发明涉及图像处理领域，尤其涉及一种激光雷达和双目视觉相互融合的定位方法及终端。

背景技术

目前，在无人驾驶领域主要应用激光雷达融合视觉进行图像处理实现局部定位，常用的激光雷达与视觉融合定位技术主要分为激光辅助视觉和视觉辅助激光两类，其中，常见视觉辅助激光定位主要从三个方面进行：通过数据层面上融合由视觉估计出具有深度的稠密点云配合激光雷达的稀疏点云输出点云数据；或是视觉辅助激光雷达做回环检测；或是通过滤波方式(卡尔曼或者粒子滤波)进行状态估计。

但是现有的视觉辅助激光的定位方案中存在如下主要缺点：并未考虑到用来匹配的帧内的激光点云，因为激光雷达位置发生移动，而存在畸变；视觉和激光雷达只是数据上的融合，并没有实现紧密组合，精度往往不高；采用卡尔曼滤波的方式进行状态估计，如果估计不准，误差会快速累计，无法修复，甚至估计值发散，出现不稳定，若选用粒子滤波估计状态，则会存在粒子耗散现象。

发明内容

本发明所要解决的技术问题是：提供一种激光雷达和双目视觉相互融合的定位方法及终端，实现视觉和激光的相互辅助，提高测量精度和鲁棒性。

为了解决上述技术问题，本发明采用的一种技术方案为：

一种激光雷达和双目视觉相互融合的定位方法，包括步骤：

S1、将激光雷达和摄像头的外参数进行标定，使激光雷达测量到的第一数据和摄像头测量到的第二数据实现点对点对应；

S2、利用所述第一数据辅助估计所述第二数据的深度误差；

S3、利用所述第二数据去除所述第一数据中的畸变，得到第一点云；

S4、利用所述第一点云、第二数据和所述第二数据的深度误差，计算出激光雷达和摄像头所在载体的位姿信息。

为了解决上述技术问题，本发明采用的另一种技术方案为：

一种激光雷达和双目视觉相互融合的定位终端，包括存储器、处理器及存储在存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

S1、将激光雷达和摄像头的外参数进行标定，使激光雷达测量到的第一数据和摄像头测量到的第二数据实现点对点对应；

S2、利用所述第一数据辅助估计所述第二数据的深度误差；

S3、利用所述第二数据去除所述第一数据中的畸变，得到第一点云；

S4、利用所述第一点云、第二数据和所述第二数据的深度误差，计算出激光雷达和摄像头所在载体的位姿信息。

本发明的有益效果在于：利用激光雷达测量和摄像头测量所得数据的各自优势，互相校准数据，结合了传统视觉辅助激光和激光辅助视觉两种技术各自的优点，通过激光数据融合提高双目视觉深度信息的估计精度，另外，通过摄像头能够高频采集数据的特点，去除激光雷达的数据畸变，最后进行整体优化计算，而不选用现有技术中的滤波进行优化，可以消除累计误差，减轻激光雷达或视觉匹配错误造成的误差问题，提高了定位精度和定位方法的鲁棒性。

附图说明

图1为本发明实施例的一种激光雷达和双目视觉相互融合的定位方法的步骤流程图；

图2为本发明实施例的一种激光雷达和双目视觉相互融合的定位终端的结构示意图；

图3为本发明实施例的一种激光雷达和双目视觉相互融合的定位方法的过程示意图；

标号说明：

1、一种激光雷达和双目视觉相互融合的定位终端；2、处理器；3、存储器。

具体实施方式

为详细说明本发明的技术内容、所实现目的及效果，以下结合实施方式并配合附图予以说明。

请参照图1，一种激光雷达和双目视觉相互融合的定位方法，包括步骤：

S1、将激光雷达和摄像头的外参数进行标定，使激光雷达测量到的第一数据和摄像头测量到的第二数据实现点对点对应；

S2、利用所述第一数据辅助估计所述第二数据的深度误差；

S3、利用所述第二数据去除所述第一数据中的畸变，得到第一点云；

S4、利用所述第一点云、第二数据和所述第二数据的深度误差，计算出激光雷达和摄像头所在载体的位姿信息。

从上述描述可知，本发明的有益效果在于：利用激光雷达测量和摄像头测量所得数据的各自优势，互相校准数据，结合了传统视觉辅助激光和激光辅助视觉两种技术各自的优点，通过激光数据融合提高双目视觉深度信息的估计精度，另外，通过摄像头能够高频采集数据的特点，去除激光雷达的数据畸变，最后进行整体优化计算，而不选用现有技术中的滤波进行优化，可以消除累计误差，减轻激光雷达或视觉匹配错误造成的误差问题，提高了定位精度和定位方法的鲁棒性。

进一步的，所述S2具体为：

根据公式：

计算出p₁、p₂、p₃、p₄的值，式中，d为摄像头和激光雷达匹配上的深度信息，dep_lid为所述第一数据中的深度信息，dep_cam为所述第二数据中的深度信息；

根据公式：

估计出所述第二数据的深度误差。

由上述描述可知，根据激光雷达测量出的精度较高的深度测量值，对摄像头测量出的深度信息进行校准，估计出摄像头测量深度数据的误差值，在之后的计算中直接使用估计出的摄像头测量深度数据时的误差值对摄像头测量出的深度数据进行校准，而不用再使用雷达测量的深度数据进行校准，提高了获取定位的速度。

进一步的，取摄像头和激光雷达匹配上的深度数据中的80％作为训练集运用LM方法进行p₁、p₂、p₃、p₄的估计；

将所述摄像头和激光雷达匹配上的深度数据中剩余的20％作为验证集；

利用所述验证集验证所述第二数据的深度误差的估计精度，如果所述估计精度高于一预设值，则保留所述深度误差，否则，删除所述深度误差。

由上述描述可知，将摄像头和激光雷达匹配上的数据分为训练集和验证集，保证了得出的参数估计值的准确度，从而保证了最后计算出的位姿信息的准确度，利用LM方法对p₁、p₂、p₃、p₄的值进行估计，保证其接近真实值，为后续估值的准确度提供了保证。

进一步的，所述S3具体为：

提取所述摄像头拍摄的图像上的特征点并进行匹配；

根据所述第二数据的深度误差对匹配后的特征点相对于摄像头的深度信息进行矫正，得到特征点尺度信息；

根据所述特征点尺度信息确定所述摄像头的位姿；

根据所述摄像头的位姿定所述激光雷达的位姿变化情况；

式中，O_p＝[r p]表示t_k-1时刻到t_p时刻所述激光雷达的位姿变化，r表示t_k-1时刻到t_p时刻所述激光雷达的姿态变化，p表示t_k-1时刻到t_p时刻所述激光雷达的位置变化；R_k为在t_k时刻摄像头在激光雷达坐标系下的位姿，R_k－1为在t_k－1时刻摄像头在激光雷达坐标系下的位姿；

根据公式

得到第一点云；

式中，Pcr为第一数据中的点云数据，

为激光雷达坐标系转换为导航坐标系的方向余弦阵，

为根据r得到，表示激光雷达从t_k-1时刻到t_p时刻的姿态变化在激光雷达坐标系中的方向余弦阵，

为导航坐标系转换为激光雷达坐标系的方向余弦阵，Pc为第一点云。

由上述描述可知，利用摄像头在同一时间段内相较于激光雷达可以获取更多帧数图像的特性，利用摄像头拍摄图像时的位姿变化对激光雷达测量到的图像的点云位姿进行估计，利用视觉去除激光数据畸变，最大程度消除了因激光雷达所在载体运动而造成的点云畸变。

进一步的，所述S4包括：

根据所述第一点云进行帧间匹配，确定边界匹配点误差方程和平面匹配点误差方程；

根据所述摄像头的位姿确定所述摄像头拍摄的图像上匹配后的特征点对应的第三误差方程；

根据所述边界匹配点误差方程、平面匹配点误差方程和第三误差方程运用LM优化器进行梯度下降估计出最优的所述激光雷达和摄像头所在载体的位姿。

由上述描述可知，列出雷达帧间匹配时的误差、摄像头在确定位姿计算和图像定位时的误差，建立误差方程，考虑了多方面的误差，通过LM优化器将误差降到最低，应用联合优化，而不选用滤波(无法消除累积误差)，减轻激光雷达或者视觉的匹配错误问题，进一步提高定位精度和鲁棒性。

请参照图2，一种激光雷达和双目视觉相互融合的定位终端，包括存储器、处理器及存储在存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

S1、将激光雷达和摄像头的外参数进行标定，使激光雷达测量到的第一数据和摄像头测量到的第二数据实现点对点对应；

S2、利用所述第一数据辅助估计所述第二数据的深度误差；

S3、利用所述第二数据去除所述第一数据中的畸变，得到第一点云；

S4、利用所述第一点云、第二数据和所述第二数据的深度误差，计算出激光雷达和摄像头所在载体的位姿信息。

从上述描述可知，本发明的有益效果在于：利用激光雷达测量和摄像头测量所得数据的各自优势，互相校准数据，结合了传统视觉辅助激光和激光辅助视觉两种技术各自的优点，通过激光数据融合提高双目视觉深度信息的估计精度，另外，通过摄像头能够高频采集数据的特点，去除激光雷达的数据畸变，最后进行整体优化计算，而不选用现有技术中的滤波进行优化，可以消除累计误差，减轻激光雷达或视觉匹配错误造成的误差问题，提高了定位精度和定位方法的鲁棒性。

进一步的，所述S2具体为：

根据公式：

计算出p₁、p₂、p₃、p₄的值，式中，d为摄像头和激光雷达匹配上的深度信息，dep_lid为所述第一数据中的深度信息，dep_cam为所述第二数据中的深度信息；

根据公式：

估计出所述第二数据的深度误差。

由上述描述可知，根据激光雷达测量出的精度较高的测量值，对摄像头测量出的深度信息进行校准，估计出摄像头测量深度数据的误差值，在之后的计算中直接使用估计出的摄像头测量深度数据时的误差值对摄像头测量出的深度数据进行校准，而不用再使用雷达测量的深度数据，提高了获取定位的速度。

进一步的，取摄像头和激光雷达匹配上的深度数据中的80％作为训练集运用LM方法进行p₁、p₂、p₃、p₄的估计；

将所述摄像头和激光雷达匹配上的深度数据中剩余的20％作为验证集；

利用所述验证集验证所述第二数据的深度误差的估计精度，如果所述估计精度高于一预设值，则保留所述深度误差，否则，删除所述深度误差。

由上述描述可知，将摄像头和激光雷达匹配上的数据分为训练集和验证集，保证了得出的参数估计值的准确度，从而保证了最后计算出的位姿信息的准确度，利用LM方法对p₁、p₂、p₃、p₄的值进行估计，保证其接近真实值，为后续估值的准确度提供了保证。

进一步的，所述S3具体为：

提取所述摄像头拍摄的图像上的特征点并进行匹配；

根据所述第二数据的深度误差对匹配后的特征点相对于摄像头的深度信息进行矫正，得到特征点尺度信息；

根据所述特征点尺度信息确定所述摄像头的位姿；

根据所述摄像头的位姿定所述激光雷达的位姿变化情况；

式中，O_p＝[r p]表示t_k-1时刻到t_p时刻所述激光雷达的位姿变化，r表示t_k-1时刻到t_p时刻所述激光雷达的姿态变化，p表示t_k-1时刻到t_p时刻所述激光雷达的位置变化；R_k为在t_k时刻摄像头在激光雷达坐标系下的位姿，R_k－1为在t_k－1时刻摄像头在激光雷达坐标系下的位姿；

根据公式

得到第一点云；

式中，Pcr为第一数据中的点云数据，

为激光雷达坐标系转换为导航坐标系的方向余弦阵，

为根据r得到，表示激光雷达从t_k-1时刻到t_p时刻的姿态变化在激光雷达坐标系中的方向余弦阵，

为导航坐标系转换为激光雷达坐标系的方向余弦阵，Pc为第一点云。

由上述描述可知，利用摄像头在同一时间段内相较于激光雷达可以获取更多帧数图像的特性，利用摄像头拍摄图像时的位姿变化对激光雷达测量到的图像的点云位姿进行估计，利用视觉去除激光数据畸变，最大程度消除了因激光雷达所在载体运动而造成的点云畸变。

进一步的，所述S4包括：

根据所述第一点云进行帧间匹配，确定边界匹配点误差方程和平面匹配点误差方程；

根据所述摄像头的位姿确定所述摄像头拍摄的图像上匹配后的特征点对应的第三误差方程；

根据所述边界匹配点误差方程、平面匹配点误差方程和第三误差方程运用LM优化器进行梯度下降估计出最优的所述激光雷达和摄像头所在载体的位姿。

由上述描述可知，列出雷达帧间匹配时的误差、摄像头在确定位姿计算和图像定位时的误差，建立误差方程，考虑了多方面的误差，通过LM优化器将误差降到最低，应用联合优化，而不选用滤波(无法消除累积误差)，减轻激光雷达或者视觉的匹配错误问题，进一步提高定位精度和鲁棒性。

请参照图1及图3，本发明的实施例一为：

下文所述导航坐标系为n系列：以载体的初始坐标为原点，过所述原点且指向所述载体在所述原点时的东方向的射线为x_n轴，北方向的射线为y_n轴，过所述原点且垂直于x_ny_n平面并指向所述载体顶部的射线为z_n轴；

载体坐标系为b系：以载体的重心为原点，过所述原点且指向所述载体左侧的射线为x_b轴，过所述原点且指向所述载体前方的射线为y_b轴，过所述原点且垂直于x_by_b平面并指向所述载体顶部的射线为z_b轴；

雷达坐标系为r系：以雷达测量零点为原点，过所述原点且指向所述雷达测量零点左侧的射线为x_r轴，过所述原点且指向所述雷达测量零点前方的射线为y_r轴，过所述原点且垂直于x_ry_r平面并指向所述雷达测量零点上方的射线为z_r轴；

相机坐标系为c系：以相机重心为原点，过所述原点且指向所述相机右侧的射线为x_c轴，过所述原点且指向所述相机下方的射线为y_c轴，过所述原点且垂直于x_cy_c平面并指向所述相机前方的射线为z_c轴；

一种激光雷达和双目视觉融合的方法，具体包括步骤：

S1、将激光雷达和摄像头的外参数进行标定，使激光雷达测量到的第一数据和摄像头测量到的第二数据实现点对点对应；

所述第一数据为激光雷达扫描其所在载体周围环境得到的扫描图中提供的数据；

所述第二数据为双目摄像头拍摄其所在载体周围环境得到的图像中提供的数据；

标定完外参后，能够使激光雷达与摄像头所获得的数据3D-3D点对应；

S2、利用所述第一数据辅助估计所述第二数据的深度误差，具体为：

根据公式：

计算出p₁、p₂、p₃、p₄的值，式中，d为摄像头和激光雷达匹配上的深度信息；

具体的，激光雷达通过外参标定，将一时刻激光雷达扫描所获得的一帧图像，投影到同一时刻摄像头拍摄所获得的一帧图像上对应了周围环境中同一点的像素点上，d为摄像头能够提供深度信息，并且激光雷达也能够提供深度信息的点，去除其中误差较大或为不稳定的激光雷达或摄像头测量提供了深度信息的点，如在该点激光雷达的入射角过小，摄像头拍摄画面为纯白且无明显纹理的区域中的点；

dep_lid为所述第一数据中的深度信息，dep_cam为所述第二数据中的深度信息；

根据公式：

估计出所述第二数据的深度误差e_d；

其中，取摄像头和激光雷达匹配上的深度数据中的80％作为训练集运用LM方法进行p₁、p₂、p₃、p₄的估计，将所述摄像头和激光雷达匹配上的深度数据中剩余的20％作为验证集；

其中，LM方法所使用的LM优化器为：

(J^TJ+μI)ΔXlm＝-J^Tf withμ≥0

上式中，J为其代价(误差)函数的雅可比矩阵，f为e_d的值(深度误差的值)，ΔXlm为迭代前后时刻的变化量，μ为阻尼系数，设定初值后，一直迭代计算，μ会根据每次迭代后的ΔXlm的变化量进行调整，当ΔXlm变化值小于额定阈值，并且其值小于特定阈值后，收敛，估计出f中带估参数的值(p₁、p₂、p₃、p₄)；

利用所述验证集验证所述第二数据的深度误差的估计精度，如果所述估计精度高于一预设值，则保留所述深度误差，否则，删除所述深度误差，不应用到后面的定位算法中；

摄像头的测量深度dep_cam随深度的增大而变大，因此误差值e_d与与深度密切相关，应用激光雷达的高精度测量去辅助视觉估计深度信息，提高估计精度，可以提高通过视觉估计摄像头姿态变化的精度。

S3、利用所述第二数据去除所述第一数据中的畸变，得到第一点云；

S4、利用所述第一点云、第二数据和所述第二数据的深度误差，计算出激光雷达和摄像头所在载体的位姿信息，包括：

根据所述第一点云进行帧间匹配，确定边界匹配点误差方程和平面匹配点误差方程；

根据所述摄像头的位姿确定所述摄像头拍摄的图像上匹配后的特征点对应的第三误差方程；

根据所述边界匹配点误差方程、平面匹配点误差方程和第三误差方程运用LM优化器进行梯度下降估计出最优的所述激光雷达和摄像头所在载体的位姿。

本发明的实施例二为：

一种激光雷达和双目视觉相互融合的定位方法，与实施例一的不同在于，所述S3具体为：

提取所述摄像头拍摄的图像上的特征点并进行匹配；

图像特征提取主要是提取图像上的角点作为图像特征点，通过KLT光流法进行不同时刻的相机输出图像上同个特征点的跟踪，实现不同时刻图像上特征点的匹配；

根据所述第二数据的深度误差对匹配后的特征点相对于摄像头的深度信息进行矫正，得到特征点尺度信息；

具体的，通过双目摄像头计算匹配上的特征点在不同时刻相对于摄像头的深度信息dep_cam，并使用深度误差e_d，通过dep_cam-e_d去除误差值，

根据所述特征点尺度信息确定所述摄像头的位姿R；

优选的，通过BA(Bundle Adjustment，光束平差法)确定所述摄像头的位姿R；

摄像头的频率一般为60赫兹，而激光雷达的输出频率一般为10赫兹，即1帧激光雷达测量数据的时间段内，可以得到6帧摄像头测量数据，可以进行6次摄像头位姿的估计；但是一帧激光测量的点云都有成千上万个，大部分点云找不到对应的位姿信息，因此需要通过内插法获取位姿；通过合理假设间隔16毫秒内(1秒/60＝0.0167秒)车辆运动是线性变化，可以估计出每个激光点云获取时相对摄像头的移动速度和姿态变化，具体的，根据所述摄像头的位姿确定所述激光雷达的位姿变化情况：

式中，O_p＝[r p]表示t_k-1时刻到t_p时刻所述激光雷达的位姿变化，r表示t_k-1时刻到t_p时刻所述激光雷达的姿态变化，p表示t_k-1时刻到t_p时刻所述激光雷达的位置变化；R_k为在t_k时刻摄像头在激光雷达坐标系下的位姿，R_k－1为在t_k－1时刻摄像头在激光雷达坐标系下的位姿；

通过上述内插公式可以通过摄像头的位姿变化估计出所述激光雷达在获取两帧测量图像间隔中的位姿变化；

根据公式

得到第一点云；

式中，Pcr为第一数据中的点云数据，

为激光雷达坐标系转换为导航坐标系的方向余弦阵，

为根据r得到，表示激光雷达从t_k-1时刻到t_p时刻的姿态变化在激光雷达坐标系中的方向余弦阵，

为导航坐标系转换为激光雷达坐标系的方向余弦阵，Pc为第一点云；

激光雷达安装在运动的载体上，而激光雷达的频率较低，即其获取一帧数据所要经过的时间较长，通常在一帧数据接收开始到结束需要100ms的时间，在此期间内，位姿一直在变，不能保证雷达在此期间接收激光反射时的位置不变，这样数据就存在畸变，雷达载体运动越快，畸变越大；

利用摄像头在激光雷达单帧内的位姿变化，将其投影到激光雷达坐标系下，估计出激光雷达在单帧内的位姿变化，通过位姿转换消除激光雷达在单帧数据获取过程中位姿变化造成的畸变，保证一帧激光雷达数据收取周期内，激光雷达对周围物体的深度估计更加准确。

请参照图1，本发明的实施例三为：

一种激光雷达和双目视觉相互融合的定位方法，与实施例一、实施例二的不同之处在于，所述S4具体为：

根据所述第一点云进行帧间匹配，具体为：

提取特征点，按照预设的规则提取边界点和平面点作为特征点，考虑到特征的提取可能集中在一个区域内，将一帧点云分为六块区域，每块区域提取最多4个边界点和10个平面点；

匹配特征点，分为边缘点匹配和平面点匹配；

首先，边缘点匹配：搜寻上一帧中距离待匹配帧最近的两个边缘点，保证其不在同一激光平扫线上；

然后，平面点匹配：搜寻上一帧中距离待匹配帧最近的三个平面点，保证两个平面点在同一激光平扫线上，而另一平面点不在同一激光平扫线上；

设定边界匹配点误差方程

和平面匹配点误差方程

上式中，f_{point-to-line}应用ICP(Iterative Closest Point，迭代最近点)算法计算待匹配帧中提取的特征点集合与上一帧所提取的特征点集合中，匹配上的特征点之间的连线的距离之和的平均值，f_{point-to-plane}应用ICP算法计算待匹配帧中提取的特征点集合中一特征点与上一帧所提取的特征点集合中匹配到的三个特征点所在平面的距离之和的平均值；

当提取所述摄像头拍摄的图像上的特征点并进行匹配后，建立对应的第三误差方程；

所述第三误差方程具体为：

res_cam＝ρ(||residual_c||)；

其中，

其中，u_cj和v_cj是j时刻摄像头测量到的归一化深度的特征点相对于摄像头的位置信息，x_cj，y_cj和z_cj由j时刻摄像头测量值计算得出，与i时刻摄像头测量到的归一化深度的特征点相对于摄像头的位置信息的关系为：

由上式估计出R_bjn的值；

上式中，λ为逆深度，其值为

z即为摄像头测量出的i时刻深度信息去除深度误差后的深度信息估计值，R_cb为摄像头相对于载体坐标系的位姿矩阵，为S3中通过BA(BundleAdjustment，光束平差法)确定，R_bin和R_bjn分别为i时刻和j时刻载体相对于导航坐标系的位姿矩阵，R_bin为已估计出的i时刻载体相对于导航坐标系的位姿矩阵，若i为初始时刻，则R_bin已知；

根据所述边界匹配点误差方程、平面匹配点误差方程和第三误差方程构建目标函数E，具体为：

运用LM优化器进行梯度下降，得到误差值最小的载体在导航坐标系中的位姿估计；

进一步的，由于激光雷达和摄像头都固定在载体上，可以通过空间测量事先标定载体坐标系和激光雷达坐标系、摄像头坐标系的关系，计算得到激光雷达坐标系和载体坐标系之间、摄像头坐标系和载体坐标系之间的转换矩阵，能够通过载体的位姿得到激光雷达和摄像头在导航坐标系中的位姿。

请参照图2，本发明的实施例四为：

一种激光雷达和双目视觉相互融合的定位终端1，所述终端1包括处理器2、存储器3及存储在存储器3上并可在所述处理器2上运行的计算机程序，所述处理器2执行所述计算机程序时实现实施例一或实施例二或实施例三中的各个步骤。

综上所述，本发明提供了一种激光雷达和双目视觉相互融合的定位方法及终端，将激光雷达和摄像头的外参数进行标定，使其测量得到的数据能够进行点对点对应，利用其对应点激光雷达测量得到的深度信息对其双目摄像头测量的深度信息进行校准，得到深度误差，并且留出一部分对应点进行深度误差的校验，最大程度保证了所计算出的深度误差的准确性，在后续使用摄像头所测量的深度信息时，无需再次使用激光测量数据进行校准，可以直接使用计算好的深度误差使其接近真实值，并且在计算深度误差时考虑到随测量深度的增加，误差会加大的情况，不同的深度值计算不同的对应参数，进一步提高了深度误差的准确性，使校准后的摄像头测量出的深度信息进一步接近真实值；利用摄像头获取数据的频率高于激光雷达的特性，通过计算激光雷达获取一帧数据时对应的多帧摄像头获取的数据，计算得到摄像头的位姿变化过程，利用摄像头的位姿变化过程，估计激光雷达在获取这一帧数据时的位姿变化，从而消除激光雷达因雷达载体运动而导致测量数据畸变；最后也采用了联合优化的方法，而不是简单地将处理后的数据做点对点的对应，将去除畸变后的激光雷达点云数据进行帧间匹配，并列出帧间匹配时的误差函数，同时列出摄像头在进行测量时的误差函数，利用LM优化，得到误差值最小的载体相对于导航坐标系的位姿，坐标转换的过程也较简单，加快了对载体位姿的计算过程，不仅定位速度快，而且定位精度高，鲁棒性好。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等同变换，或直接或间接运用在相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种激光雷达和双目视觉相互融合的定位方法，其特征在于，包括步骤：

S1、将激光雷达和摄像头的外参数进行标定，使激光雷达测量到的第一数据和摄像头测量到的第二数据实现点对点对应；

S2、利用所述第一数据辅助估计所述第二数据的深度误差；

S3、利用所述第二数据去除所述第一数据中的畸变，得到第一点云；

S4、利用所述第一点云、第二数据和所述第二数据的深度误差，计算出激光雷达和摄像头所在载体的位姿信息。

2.根据权利要求1所述的一种激光雷达和双目视觉相互融合的定位方法，其特征在于，所述S2具体为：

根据公式：

计算出p₁、p₂、p₃、p₄的值，式中，d为摄像头和激光雷达匹配上的深度信息，dep_lid为所述第一数据中的深度信息，dep_cam为所述第二数据中的深度信息；

根据公式：

估计出所述第二数据的深度误差。

3.根据权利要求2所述的一种激光雷达和双目视觉相互融合的定位方法，其特征在于，取摄像头和激光雷达匹配上的深度数据中的80％作为训练集运用LM方法进行p₁、p₂、p₃、p₄的估计；

将所述摄像头和激光雷达匹配上的深度数据中剩余的20％作为验证集；

利用所述验证集验证所述第二数据的深度误差的估计精度，如果所述估计精度高于一预设值，则保留所述深度误差，否则，删除所述深度误差。

4.根据权利要求1所述的一种激光雷达和双目视觉相互融合的定位方法，其特征在于，所述S3具体为：

提取所述摄像头拍摄的图像上的特征点并进行匹配；

根据所述第二数据的深度误差对匹配后的特征点相对于摄像头的深度信息进行矫正，得到特征点尺度信息；

根据所述特征点尺度信息确定所述摄像头的位姿；

根据所述摄像头的位姿定所述激光雷达的位姿变化情况；

式中，O_p＝[r p]表示t_k-1时刻到t_p时刻所述激光雷达的位姿变化，r表示t_k-1时刻到t_p时刻所述激光雷达的姿态变化，p表示t_k-1时刻到t_p时刻所述激光雷达的位置变化；R_k为在t_k时刻摄像头在激光雷达坐标系下的位姿，R_k－1为在t_k－1时刻摄像头在激光雷达坐标系下的位姿；

根据公式

得到第一点云；

式中，Pcr为第一数据中的点云数据，

为激光雷达坐标系转换为导航坐标系的方向余弦阵，

为根据r得到，表示激光雷达从t_k-1时刻到t_p时刻的姿态变化在激光雷达坐标系中的方向余弦阵，

为导航坐标系转换为激光雷达坐标系的方向余弦阵，Pc为第一点云。

5.根据权利要求4所述的一种激光雷达和双目视觉相互融合的定位方法，其特征在于，所述S4包括：

根据所述第一点云进行帧间匹配，确定边界匹配点误差方程和平面匹配点误差方程；

根据所述摄像头的位姿确定所述摄像头拍摄的图像上匹配后的特征点对应的第三误差方程；

根据所述边界匹配点误差方程、平面匹配点误差方程和第三误差方程运用LM优化器进行梯度下降估计出最优的所述激光雷达和摄像头所在载体的位姿。

6.一种激光雷达和双目视觉相互融合的定位终端，包括存储器、处理器及存储在存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现以下步骤：

S1、将激光雷达和摄像头的外参数进行标定，使激光雷达测量到的第一数据和摄像头测量到的第二数据实现点对点对应；

S2、利用所述第一数据辅助估计所述第二数据的深度误差；

S3、利用所述第二数据去除所述第一数据中的畸变，得到第一点云；

S4、利用所述第一点云、第二数据和所述第二数据的深度误差，计算出激光雷达和摄像头所在载体的位姿信息。

7.根据权利要求6所述的一种激光雷达和双目视觉相互融合的定位终端，其特征在于，所述S2具体为：

根据公式：

计算出p₁、p₂、p₃、p₄的值，式中，d为摄像头和激光雷达匹配上的深度信息，dep_lid为所述第一数据中的深度信息，dep_cam为所述第二数据中的深度信息；

根据公式：

估计出所述第二数据的深度误差。

8.根据权利要求7所述的一种激光雷达和双目视觉相互融合的定位终端，其特征在于，取摄像头和激光雷达匹配上的深度数据中的80％作为训练集运用LM方法进行p₁、p₂、p₃、p₄的估计；

将所述摄像头和激光雷达匹配上的深度数据中剩余的20％作为验证集；

利用所述验证集验证所述第二数据的深度误差的估计精度，如果所述估计精度高于一预设值，则保留所述深度误差，否则，删除所述深度误差。

9.根据权利要求6所述的一种激光雷达和双目视觉相互融合的定位终端，其特征在于，所述S3具体为：

提取所述摄像头拍摄的图像上的特征点并进行匹配；

根据所述第二数据的深度误差对匹配后的特征点相对于摄像头的深度信息进行矫正，得到特征点尺度信息；

根据所述特征点尺度信息确定所述摄像头的位姿；

根据所述摄像头的位姿定所述激光雷达的位姿变化情况；

式中，O_p＝[r p]表示t_k-1时刻到t_p时刻所述激光雷达的位姿变化，r表示t_k-1时刻到t_p时刻所述激光雷达的姿态变化，p表示t_k-1时刻到t_p时刻所述激光雷达的位置变化；R_k为在t_k时刻摄像头在激光雷达坐标系下的位姿，R_k－1为在t_k－1时刻摄像头在激光雷达坐标系下的位姿；

根据公式

得到第一点云；

式中，Pcr为第一数据中的点云数据，

为激光雷达坐标系转换为导航坐标系的方向余弦阵，

为根据r得到，表示激光雷达从t_k-1时刻到t_p时刻的姿态变化在激光雷达坐标系中的方向余弦阵，

为导航坐标系转换为激光雷达坐标系的方向余弦阵，Pc为第一点云。

10.根据权利要求9所述的一种激光雷达和双目视觉相互融合的定位终端，其特征在于，所述S4包括：

根据所述第一点云进行帧间匹配，确定边界匹配点误差方程和平面匹配点误差方程；

根据所述摄像头的位姿确定所述摄像头拍摄的图像上匹配后的特征点对应的第三误差方程；

根据所述边界匹配点误差方程、平面匹配点误差方程和第三误差方程运用LM优化器进行梯度下降估计出最优的所述激光雷达和摄像头所在载体的位姿。

Images