基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统
阅读说明：本技术 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统 (Poor texture tunnel modeling method and system based on vision-laser radar coupling ) 是由 何斌 朱琪琪 李刚 沈润杰 程斌 王志鹏 陆萍 朱忠攀 周艳敏 于 2021-08-17 设计创作，主要内容包括：本发明涉及一种基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统,包括以下步骤：获取深度相机采集的点云信息、激光雷达采集的激光信息以及无人机的运动信息；基于激光信息生成栅格地图,基于运动信息得到无人机的位姿信息；采用贝叶斯融合方法将点云信息、栅格地图和位姿信息进行融合,得到地图模型；基于上一时刻的地图模型,通过特征匹配对最新的地图模型进行修正。与现有技术相比,本发明融合深度相机和激光雷达进行SLAM建图,充分利用激光雷达较大的范围信息和深度相机较为丰富的局部信息,互补提高了信息的精确度,使地图模型的建立更加接近于真实隧洞环境。(The invention relates to a poor texture tunnel modeling method and system based on vision-laser radar coupling, which comprises the following steps: acquiring point cloud information acquired by a depth camera, laser information acquired by a laser radar and motion information of an unmanned aerial vehicle; generating a grid map based on the laser information, and obtaining pose information of the unmanned aerial vehicle based on the motion information; fusing point cloud information, a grid map and pose information by adopting a Bayesian fusion method to obtain a map model; and correcting the latest map model through feature matching based on the map model at the previous moment. Compared with the prior art, the method provided by the invention integrates the depth camera and the laser radar to carry out SLAM mapping, makes full use of the larger range information of the laser radar and the richer local information of the depth camera, complements and improves the accuracy of the information, and enables the establishment of a map model to be closer to a real tunnel environment.)
技术领域
本发明涉及无人机隧洞建模技术领域，尤其是涉及一种基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统。
背景技术
隧洞的检修和建模工作是地质工程安全中必不可少的一部分，关系到整个工程的正常运转。传统的隧洞建模方法一般要先到隧洞实地采集数据信息，在人无法进入的狭小区域使用无人机采集数据信息，然后利用3D建模软件将信息转换为3D模型。但是，在隧洞纹理缺失的条件下，信息的采集变得十分困难。另外，在某些狭小黑暗区域，信息采集与建模的难度进一步上升，而3D建模的质量完全依赖于原地采集的信息，因此这种方法很可能导致建模与实际场景有较大差距，建模的精确度有待提高。
SLAM(simultaneous localization and mapping)即即时定位与地图构建，可以理解为搭载特定传感器的主体，如机器人和无人机系统，在未知的环境中估计自身姿态同时建立周围环境地图。SLAM主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。使用无人机在隧洞中进行建模主要使用的就是SLAM算法。常用的SLAM算法有视觉SLAM、激光SLAM等，基本原理是通过相邻帧中的特征点匹配进行位姿估算和建图，在纹理缺失、画面重复性高的隧洞环境下，建模精度有待提高。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统，融合深度相机和激光雷达进行SLAM建图，充分利用激光雷达较大的范围信息和深度相机较为丰富的局部信息，互补提高了信息的精确度，使地图模型的建立更加接近于真实隧洞环境。
本发明的目的可以通过以下技术方案来实现：
一种基于视觉-激光雷达耦合的贫纹理隧洞建模方法，通过搭载深度相机和激光雷达的无人机进行建模，包括以下步骤：
S1、获取深度相机采集的点云信息、激光雷达采集的激光信息以及无人机的运动信息；
S2、对激光信息进行滤波处理后生成栅格地图，基于运动信息得到无人机的位姿信息；
S3、采用贝叶斯融合方法将点云信息、栅格地图和位姿信息进行融合，得到地图模型；
S4、重复步骤S1至步骤S3，得到新的地图模型，基于上一时刻的地图模型，通过特征匹配对最新的地图模型进行修正，重复步骤S4，直至完成地图模型的构建。
进一步的，步骤S1之前还包括：根据深度相机和激光雷达在无人机上的位置关系，确定点云信息和激光信息之间的相对变换关系。
进一步的，激光雷达坐标系下的点云至深度相机坐标系的变换关系为：
其中，(X，Y，Z)T表示激光雷达坐标系下的坐标，(Xc，Yc，Yc)T表示深度相机坐标系下的坐标，(u，v，1)T表示深度相机成像平面上的像素坐标，r是基于深度相机和激光雷达在无人机上的位置确定的旋转矩阵，t是基于深度相机和激光雷达在无人机上的位置确定的平移矩阵，K表示深度相机的内参数矩阵。
进一步的，无人机的运动信息是由惯性测量单元IMU和里程计测量得到的，包括速度、加速度和距离；由于隧洞中GPS信号差甚至丢失，因此基于惯性测量单元IMU和里程计测量得到的运动信息，通过卡尔曼滤波融合运动信息得到无人机的位姿估计。
进一步的，步骤S4中，基于上一时刻的地图模型，通过特征匹配对最新的地图模型进行修正具体为：
S41、获取上一时刻的地图模型，作为参考帧；获取最新的地图模型，自最新的地图模型中找到与上一时刻的地图模型所对应的区域，作为当前帧；
S42、参考帧中的特征点使用{Pi}表示，当前帧中的特征点使用{Qi}表示，当前帧和参考帧中的特征点数量相同；
S43、建立帧间变化模型：
{Qi}＝R{Pi}+T
其中，R表示旋转参数，T表示平移参数；
S44、代入参考帧中的特征点和当前帧中的特征点，迭代求解旋转参数和平移参数；
S45、基于旋转参数和平移参数得到上一时刻的地图模型与最新的地图模型之间的匹配关系，对最新的地图模型进行修正。
进一步的，由于地图模型是在无人机飞行过程中构建的，两个时刻构建得到地图模型一定会存在误差。理论上，如果旋转参数和平移参数准确，那么参考帧中的每一个特征点都会与当前帧中的特征点重合，但是考虑到噪声和误差的存在，不可能做到百分百重合，因此定义目标函数；步骤S44中，迭代求解旋转参数和平移参数具体为：
将参考帧中的特征点和当前帧中的特征点代入帧间变化模型，基于帧间变化模型建立目标函数，令目标函数的函数值取最小值的旋转参数和平移参数即最终求得的旋转参数和平移参数，所述目标函数的公式为：
其中，L表示目标函数的函数值，pi表示参考帧中的一个特征点，qi表示当前帧中的一个特征点，N表示特征点的数量。
一种基于视觉-激光雷达耦合的贫纹理隧洞建模系统，采用如上所述的基于视觉-激光雷达耦合的贫纹理隧洞建模方法，包括无人机本体，其上搭载有深度相机、激光雷达、计算单元和控制器，所述控制器与深度相机、激光雷达和计算单元通信连接，在无人机飞行过程中执行以下步骤：
T1、控制器获取深度相机采集的点云信息、激光雷达采集的激光信息以及无人机的运动信息，发送至计算单元；
T2、计算单元对激光信息进行滤波处理后生成栅格地图，基于运动信息得到无人机的位姿信息；
T3、采用贝叶斯融合方法将点云信息、栅格地图和位姿信息进行融合，得到地图模型；
T4、重复步骤T1至步骤T3，得到新的地图模型，基于上一时刻的地图模型，通过特征匹配对最新的地图模型进行修正，重复步骤T4，直至完成地图模型的构建。
进一步的，步骤T4中，基于上一时刻的地图模型，通过特征匹配对最新的地图模型进行修正具体为：
T41、获取上一时刻的地图模型，作为参考帧；获取最新的地图模型，自最新的地图模型中找到与上一时刻的地图模型所对应的区域，作为当前帧；
T42、参考帧中的特征点使用{Pi}表示，当前帧中的特征点使用{Qi}表示，当前帧和参考帧中的特征点数量相同；
T43、建立帧间变化模型：
{Qi}＝R{Pi}+T
其中，R表示旋转参数，T表示平移参数；
T44、代入参考帧中的特征点和当前帧中的特征点，迭代求解旋转参数和平移参数；
T45、基于旋转参数和平移参数得到上一时刻的地图模型与最新的地图模型之间的匹配关系，对最新的地图模型进行修正。
进一步的，步骤T44中，迭代求解旋转参数和平移参数具体为：
将参考帧中的特征点和当前帧中的特征点代入帧间变化模型，基于帧间变化模型建立目标函数，令目标函数的函数值取最小值的旋转参数和平移参数即最终求得的旋转参数和平移参数，所述目标函数的公式为：
其中，L表示目标函数的函数值，pi表示参考帧中的一个特征点，qi表示当前帧中的一个特征点，N表示特征点的数量。
进一步的，所述无人机本体上还搭载有存储单元，所述存储单元与控制单元和计算单元连接，用于存储构建的地图模型。
与现有技术相比，本发明具有以下有益效果：
(1)融合深度相机和激光雷达进行SLAM建图，充分利用激光雷达较大的范围信息和深度相机较为丰富的局部信息，互补提高了信息的精确度，使地图模型的建立更加接近于真实隧洞环境。
(2)使用贝叶斯融合方法将点云信息、栅格地图和位姿信息进行融合，得到地图模型，适用于具有可加高斯噪声的不确定性信息，得到的地图模型的误差更小。
(3)最新的地图模型与前一时刻的地图模型之间通过特征点匹配的方法，计算旋转参数和平移参数，从而进行地图模型修正，进一步提升了精确度。
附图说明
图1为基于视觉-激光雷达耦合的贫纹理隧洞建模方法的流程图；
图2为SLAM的框架示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
在附图中，结构相同的部件以相同数字标号表示，各处结构或功能相似的组件以相似数字标号表示。附图所示的每一组件的尺寸和厚度是任意示出的，本发明并没有限定每个组件的尺寸和厚度。为了使图示更清晰，附图中有些地方适当夸大了部件。
实施例1：
一种基于视觉-激光雷达耦合的贫纹理隧洞建模系统，包括无人机本体，其上搭载有深度相机、激光雷达、计算单元和控制器，控制器与深度相机、激光雷达和计算单元通信连接；该建模系统采用基于视觉-激光雷达耦合的贫纹理隧洞建模方法，通过搭载深度相机和激光雷达的无人机进行建模，其基本流程如图1所示，包括以下步骤：
S1、获取深度相机采集的点云信息、激光雷达采集的激光信息以及无人机的运动信息；
S2、对激光信息进行滤波处理后生成栅格地图，基于运动信息得到无人机的位姿信息；
S3、采用贝叶斯融合方法将点云信息、栅格地图和位姿信息进行融合，得到地图模型；
S4、重复步骤S1至步骤S3，得到新的地图模型，基于上一时刻的地图模型，通过特征匹配对最新的地图模型进行修正，重复步骤S4，直至完成地图模型的构建。
无人机本体上还搭载有存储单元，存储单元与控制单元和计算单元连接，用于存储构建的地图模型。
激光雷达具有精度高、稳定性好以及获取信息范围大等特点，但获取的数据信息不够丰富；视觉传感器具有价格低、重量轻、获取环境信息丰富以及易于数据关联等优点，但存在深度估计能力差的特点，由于视觉传感器对光照变化或低纹理环境敏感，在缺乏光照和纹理特征的环境中，视觉SLAM表现较差，甚至无法完成任务。由于激光SLAM和视觉SLAM在单独使用的条件下存在一定的局限性，因此本申请考虑将二者融合进行建图，使用深度相机和激光雷达融合建图，弥补二者各自的不足，最终提高了SLAM建图的精度和鲁棒性，提高了建图的精确度和速度。
以视觉SLAM为例，SLAM框架基本如图2所示，传感器数据->前端视觉里程计->后端非线性优化->回环检测->建图。传感器数据即接收到的数据；在视觉SLAM中主要是图像信息，前端视觉里程计的任务是从获取的图像信息中，根据相邻时间上的图像信息计算出相机的运动信息，构建局部地图；后端非线性优化主要是减小视觉里程计构建的地图的误差；回环检测判断是否之前曾经达到过这个位置，主要解决位置估计随时间漂移。
由于深度相机和激光雷达的位置不一样，采集到的点云信息和激光信息处于不同的坐标系下，因此需要进行坐标变化以统一坐标。可以在无人机飞行前，通过标定的方法确定坐标的相对变化关系。即根据深度相机和激光雷达在无人机上的位置关系，确定点云信息和激光信息之间的相对变换关系。
激光雷达坐标系下的点云至深度相机坐标系的变换关系为：
其中，(X，Y，Z)T表示激光雷达坐标系下的坐标，(Xc，Yc，Yc)T表示深度相机坐标系下的坐标，(u，v，1)T表示深度相机成像平面上的像素坐标，r是基于深度相机和激光雷达在无人机上的位置确定的旋转矩阵，t是基于深度相机和激光雷达在无人机上的位置确定的平移矩阵，K表示深度相机的内参数矩阵，是固定值。
无人机的运动信息是由惯性测量单元IMU和里程计测量得到的，包括速度、加速度和距离；由于隧洞中GPS信号差甚至丢失，因此基于惯性测量单元IMU和里程计测量得到的运动信息，再通过卡尔曼滤波融合运动信息得到无人机的位姿估计。
贝叶斯估计是基于Bayes定理的条件或后验概率的统计数据融合算法，它适用于具有可加高斯噪声的不确定性信息，可以通过已知的向量，估计出未知状态下n维向量。本申请考虑了深度相机采集的点云信息、基于激光雷达构建的栅格地图以及基于运动相机得到的位姿信息，使用贝叶斯融合得到地图模型，综合考虑了深度相机和激光雷达的优缺点，并融合了无人机的运动，进一步缩小了误差。
在步骤S4中，基于上一时刻的地图模型，通过特征匹配对最新的地图模型进行修正具体为：
S41、获取上一时刻的地图模型，作为参考帧；获取最新的地图模型，自最新的地图模型中找到与上一时刻的地图模型所对应的区域，作为当前帧；
S42、参考帧中的特征点使用{Pi}表示，当前帧中的特征点使用{Qi}表示，当前帧和参考帧中的特征点数量相同；
S43、建立帧间变化模型：
{Qi}＝R{Pi}+T
其中，R表示旋转参数，T表示平移参数；
S44、代入参考帧中的特征点和当前帧中的特征点，迭代求解旋转参数和平移参数；
S45、基于旋转参数和平移参数得到上一时刻的地图模型与最新的地图模型之间的匹配关系，对最新的地图模型进行修正。
由于地图模型是在无人机飞行过程中构建的，两个时刻构建得到地图模型一定会存在误差。理论上，如果旋转参数和平移参数准确，那么参考帧中的每一个特征点都会与当前帧中的特征点重合，但是考虑到噪声和误差的存在，不可能做到百分百重合，因此定义目标函数；步骤S44中，迭代求解旋转参数和平移参数具体为：
将参考帧中的特征点和当前帧中的特征点代入帧间变化模型，基于帧间变化模型建立目标函数，令目标函数的函数值取最小值的旋转参数和平移参数即最终求得的旋转参数和平移参数，目标函数的公式为：
其中，L表示目标函数的函数值，pi表示参考帧中的一个特征点，qi表示当前帧中的一个特征点，N表示特征点的数量。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。