CN114782639A - 一种基于多传感器融合的快速式差速潜伏式agv稠密三维重建方法 - Google Patents

Abstract

本发明公开的一种一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法及其工作方法，包括如下步骤：获取移动场景RGB图像，对RGB图像进行预处理，通过特征提取算法进行提取RGB图像特征，对图像帧添加关键帧，对关键帧进行筛选；通过位姿测量单元进行检测生成二维位姿；将筛选后的关键帧的二维位姿对其三维位姿进行约束，得到约束后的位姿，通过关键帧的位姿和深度信息进行三维建图，通过词袋模型对三维建图进行优化；去除离散的点云，将离散点云对应的关键帧剔除，形成最终的稠密三维重建模型；根据最终的稠密三维重建模型构建快速移动场景的实验平台。

Description

一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重 建方法

技术领域

本发明涉及多传感器融合的地图重建领域，更具体的，涉及一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法。

背景技术

近年来，随着计算机软硬件的飞速发展，人工智能已经越来越频繁的出现在人们的日常生活中。而三维重建则作为一项特殊的技术，在人工智能的领域发挥着重要的作用。这通常应用在室内机器人导航、室外无人驾驶、增强现实以及游戏场景制作等等。激光雷达和RGB-D相机(深度相机)等设备是三维重建系统中最为关键的传感器，先进的算法使得它们能够在感知自身定位的同时，构建出周围环境的三维地图与模型。

移动机器人作为机器人学中的一个重要分支，最早出现于上世纪 60年代末期，由斯坦福研究院的人工智能研究中心研制出现实意义上第一台移动机器人。其最重要的目的是将人工智能技术应用在复杂环境下，使得机器人可以感知世界，自主规划并对自身的行为进行执行与控制。

基于激光雷达的地图重建方法可以分为两种，分别是含有视觉信息和不含有视觉信息的方法。为了能够重建三维的地图信息，往往需要融合视觉才能够实现。基于单目相机的同步定位与映射技术具有显而易见的优势，传感器小巧便捷且价格便宜，除了跟踪轨迹以外也有许多方法能够满足室内外的三维重建需求。在RGB-D相机兴起之前，为了获取图像的深度信息，研究者们大多会选择双目相机。双目相机是由两个单独的RGB相机组合而成的，两个相机之间的距离被称为基线。通过基线可以估算出每个像素格的深度信息，因此能够解决单目相机固有的尺度不确定性问题。通过双目相机，可以重建出精度更高且稠密的三维地图。RGB-D相机是在2010年前后兴起的一种通过红外结构光或者Time-of-Flight(ToF)原理测出物体深度信息的设备。

与双目相机不同，RGB-D相机是通过物理手段直接测得图像的深度信息的，无需借助软件计算，也就省去了部分的硬件资源。基于 RGB-D相机的三维重建方法可以分为两种，一种是含有动态场景的三维重建，另一种是纯静态的三维重建。

发明内容

为了解决上述至少一个技术问题，本发明提出了一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法。

本发明第一方面提供了一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，包括如下步骤：

获取移动场景RGB图像，对RGB图像进行预处理，通过特征提取算法进行提取RGB图像特征，

对图像帧添加关键帧，对关键帧进行筛选；

通过位姿测量单元进行检测生成二维位姿；

将筛选后的关键帧的二维位姿对其三维位姿进行约束，得到约束后的位姿，

通过关键帧的位姿和深度信息进行三维建图，

通过词袋模型对三维建图进行优化；

去除离散的点云，将离散点云对应的关键帧剔除，形成最终的稠密三维重建模型；

根据最终的稠密三维重建模型构建快速移动场景的实验平台。

本发明一个较佳实施例中，位姿测量单元包括二维激光雷达、惯导传感器以及轮速计中的一种或两种以上的组合。

本发明一个较佳实施例中，图像特征提取具体包括ORB特征点检测算法，ORB特征点检测算法为FAST检测子和BRIEF描述子结合形成，使用FAST进行特征点检测，接着从得到的候选FAST特征点中，提取出Harris角点的最大响应特征点，其中Harris角点的响应函数如下：

R＝det M-α(traceM)²

其中，其中，定义R为角点响应函数，通过判断R的大小来判断像素是否为角点。α为一个经验常数，通常取值0.04-0.06。M矩阵为各维均值平均化的协方差矩阵。

本发明一个较佳实施例中，还包括词袋模型优化后计算Sim3，进行全局优化，去除离散的点云，将离散点云对应的关键帧剔除，形成最终的稠密三维重建模型。

本发明一个较佳实施例中，ORB特征点检测法采用了灰度质心法来进行修正，灰度质心法中包括几何中心P与灰度中心Q，将几何中心P作为该坐标系下的零点，得到灰度质心的坐标为：

其中M₁₀为所有X轴的灰度值之和，M₀₁为所有Y轴的灰度值之和，M₀₀为该像素块内所有灰度值之和；

ORB特征点的角度计算公式为：θ＝a tan 2(m₀₁,m₁₀)。

本发明一个较佳实施例中，还包括运载小车，运载小车为差速潜伏式自动引导小车，RGB-D相机和激光雷达搭载在差速潜伏式自动引导小车上。

本发明一个较佳实施例中，所述运载小车包括IMU惯导传感器、工控机与单片机，通过IMU惯导传感器将小车运动的四元数传递给工控机，同时通过二维激光雷达将二维点云传递给工控机，单片机作对电机驱动器发送控制命令，同时接收电机驱动器的轮速数据；

通过串口将轮速数据发送给工控机，进行二维拓扑地图的建立。

本发明一个较佳实施例中，运载小车携带相机从三维运动降到了二维运动，进行添加地面约束，

对于长度为单位长度的正交基(e₁,e₂,e₃)，经过一次变换后成为了平面上的(e₁′,e₂′,0)，对于三维下的旋转矩阵R：

而添加地面约束后，旋转矩阵R中的e₃′为

因此平面约束下的旋转矩阵R′为：

本发明一个较佳实施例中，在平面约束下的平移向量t′为：

因此，在平面约束下的SE′(3)如下式所示：

其中，T是变换矩阵。SE(3)指的是变换矩阵构成的特殊欧式群。 SO(3)指的是三维旋转矩阵构成的特殊正交群。通过对已优化的位姿进行地面约束处理，将相机运动整体降维。

本发明的上述技术方案相比现有技术具有以下优点：

(1)针对当前仅使用视觉传感器无法应对机器人迷失，仅使用激光雷达获取信息少难以重定位的问题，本专利提出一种视觉传感器与二维激光雷达融合的方法，将激光雷达、深度相机采集信息结合，在获取丰富特征信息的基础上具备更为精准的尺度信息，输入至三维重建系统中，大大提高传统三维重建位姿求取的速度和准确性。

(2)针对手持式三维重建方法对于采集者的高要求，避免需要对采集者进行训练并无法控制采集过程中信息的稳定连续性采集而产生不必要的误差，本专利在同一垂直线上配置广角深度相机与二维激光雷达，能够精准的控制实验平台的采集速率和运动角速度，定量控制视频流采集速率，使得系统能够在快速移动的同时稳定获取连续性采集信息。

(3)针对大多三维重建算法注重重建效果而忽视了运动位姿精度的问题，本专利提出一种将系统的维度从三维降至二维的地面约束方法，将相机运动从三维运动降至了二维运动，降低了整体系统的算法复杂度并提升计算效率，使得系统能够更好的应对快速移动的场景。

附图说明

图1是本发明优选实施例的一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法示意图。

图2是本发明优选实施例的灰度质心法原理示意图。

图3是本发明优选实施例的RGB-D相机的运载小车内部原理结构图。

图4是本发明实施例中二维激光雷达运载小车生成的大场景二维拓扑地图及其尺度。

图5是本发明实施例中运载小车多场景快速移动旋转三维重建中的墙面间距测量结果与平均误差统计表。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，包括如下步骤：

获取移动场景RGB图像，对RGB图像进行预处理，通过特征提取算法进行提取RGB图像特征，

对图像帧添加关键帧，对关键帧进行筛选；

通过位姿测量单元进行检测生成二维位姿；

将筛选后的关键帧的二维位姿对其三维位姿进行约束，得到约束后的位姿，

通过关键帧的位姿和深度信息进行三维建图，

通过词袋模型对三维建图进行优化；

去除离散的点云，将离散点云对应的关键帧剔除，形成最终的稠密三维重建模型；

根据最终的稠密三维重建模型构建快速移动场景的实验平台。

具体的，地面约束指的是三维位姿通过“地面约束”这一优化算法，来得到优化后的三维位姿。本发明三维重建方法首先计算面向快速移动场景的三维重建的位姿估计→研究RGB图像的图像特征提取算法，使用以快速性为第一要素、准确性为第二要素的特征提取算法→对图像帧添加关键帧→对关键帧进行筛选→通过局部优化对局部关键帧进行筛→通过二维激光雷达、惯导传感器以及轮速计生成二维位姿→将筛选后的关键帧的二维位姿对其三维位姿进行约束，得到约束后的位姿→对深度图进行滤波处理→通过关键帧的位姿和深度信息进行移动截止深度建图→词袋模型优化→计算 Sim3→全局优化→去除离散的点云→将离散点云对应的关键帧剔除→形成最终的稠密三维重建模型→设计一个面向快速移动场景的三维重建地图构建的实验平台→通过实验验证结果。

其中，Sim3是使用3对匹配点来进行相似变换的求解，进而解出两个坐标系之间的旋转矩阵R、平移向量t和尺度。特别的，采用了深度相机，尺度因子被设置为1。

ORB(Oriented FAST and Rotated BRIEF)特征点检测算法是有FAST和BRIEF结合形成的。该算法首先使用FAST进行特征点检测，接着从得到的候选FAST特征点中，提取出Harris角点的最大响应特征点。对于Harris角点的响应函数如下：

R＝det M-α(traceM)² (1)

其中，M矩阵为各维均值平均化的协方差矩阵。由于FAST特征点检测没有尺度不变的特性，因此在进行ORB特征点检测时，往往在高斯金字塔上进行检测角点。这样能够实现每一层图像的尺度相关，以此来达到尺度不变的结果。另一方面，FAST特征点检测法不光缺少尺度不变性，同时它还缺少方向性。针对这一缺陷，ORB 特征点检测法采用了灰度质心法来进行补全。该方法利用大部分角点的灰度质心与其几何中心之间不重合的特点，得到由几何中心指向灰度中心的方向，将其定义为ORB特征点的方向信息。由于是由几何中心P指向灰度中心Q，因此将几何中心P作为该坐标系下的零点，如图2所示。

那么得到灰度质心的坐标为：

其中M₁₀为所有X轴的灰度值之和，M₀₁为所有Y轴的灰度值之和，M₀₀为该像素块内所有灰度值之和。因此可以得到ORB特征点的角度为：

θ＝a tan 2(m₀₁,m₁₀) (3)

在BRIEF描述子中，也没有旋转不变的特性，因此我们也需要将其赋予旋转的特征信息。研究者将前文所得到的θ角度作为主方向，然后将θ离散开来，分成12个子角度。对于12个子角度，分别求解BRIEF描述子，从而能够提高ORB特征点的旋转不变的鲁棒性。

基于地面约束的降维方法如下：

对于加入了运载小车后的RGB-D三维重建系统来说，相机运动从三维运动降到了二维运动，因此我们需要将SE3转换为SE2，以此来添加地面约束。

对于长度为单位长度的正交基(e₁,e₂,e₃)，经过一次变换后成为了平面上的(e₁′,e₂′,0)，对于三维下的旋转矩阵R：

而添加地面约束后，旋转矩阵R中的e₃′为

因此平面约束下的旋转矩阵R′为：

同理，在平面约束下的平移向量t′为：

因此，在平面约束下的SE′(3)如下式所示：

通过对已优化的位姿进行地面约束处理，将相机运动整体降维，使得更加接近真实场景，减少不必要的轨迹误差，从而能够得到更加完善的三维重建。同时在三维重建的过程中，传递的位姿信息更少，能够带来更快的三维重建速度。

实验平台搭建方案如下：

针对大场景三维重建所需的灵巧便携设备的要求，本专利选取了体积小巧，并且能够满足大场景三维重建需求的RGB-D相机。本专利所选取的RGB-D相机是来自于英特尔实感的RealSense-D455 相机。该相机的有效深度范围为0.6～6m，能够满足大场景的“大”需求。同时，其最高深度和RGB帧率为30帧每秒，可以满足本专利所针对的快速式、旋转式的大场景三维重建的速度需求。同时由于其较小的体积：124mm*26mm*29mm，可将其固定在任意可活动的装置上，进行更加快速的大场景三维重建。

针对在不同场景变换下的快速旋转式大场景三维重建，比如光照变化、场景大小变化等等的情况下，会导致尺度不一。因此，本专利设计了一种将RGB-D相机和激光雷达搭载在差速潜伏式自动引导小车上，进行远程大场景的快速三维重建，该自动引导小车装有SlamOpto(司岚)二维激光雷达、IMU及轮速计，通过对电机的控制，可以定量地对图像采集的速度进行对比。同时，由于其通过激光雷达进行二维拓扑地图的建立，因此可以使整个重建系统具有更精确、平稳的快速移动。并且由于降低了运动维度，因此可以大大提高系统的运行效率。也可以通过二维拓扑地图对三维重建结果进行尺度误差对比

该运载小车的内部原理结构如图3所示。通过IMU惯导传感器将小车运动的四元数传递给工控机，同时通过二维激光雷达将二维点云传递给工控机。另一边STM32F407单片机作对电机驱动器发送控制命令，同时接收电机驱动器的轮速数据。并且通过232串口将轮速数据发送给工控机。工控机在接收到这些信息后，进行二维拓扑地图的建立。将二维拓扑地图作为三维重建模型的真实尺度依据。

硬件方面：主要使用英特尔实感的RealSense-D455相机，该相机的有效深度范围为0.6～6m，能够满足大场景需求，可直接同时获取彩色图像和深度图像；司岚二维激光雷达基于ToF测距原理，扫描频率为15Hz(15r/s)，角度分辨率均为0.33°，最远有效工作距离为25米；

软件编程：以C/C++与python为主，主要在Ubuntu系统下进行实验编程；

三维地图构建策略：借鉴当前先进的SLAM算法，设计一套面向快速移动场景的高精度三维地图构建实验平台。该实验平台集数据的实时采集、数据的实时处理、数据的后期优化处理、三维重建结果显示等于一体。该实验平台可以面向各种快速复杂场景三维重建

本专利将搭载二维激光雷达的运载小车，先将实验场景进行二维拓扑地图重建，如图4所示。经实际测量，该二维拓扑地图与实际米尺测量结果的差值仅为5mm，因此本专利在尺度对比上选取该二维拓扑地图作为真值，如图4所示。

为了实现地面约束这一条件，同时为了获得精准的速度量数据，本专利将RGB-D相机搭载在运载小车上。通过运载小车上的IMU 和轮速计，获得实时的角速度与线速度，从而能够证明本专利在大场景下的快速式运动和旋转中，都能获得较好的三维重建模型。

首先在大房间的中心，将运载小车调整至自旋式原地旋转，并将轮上速度调整为0.3m/s，

本发明技术方案具有以下有益效果：

针对当前仅使用视觉传感器无法应对机器人迷失，仅使用激光雷达获取信息少难以重定位的问题，本专利提出一种视觉传感器与二维激光雷达融合的方法，将激光雷达、深度相机采集信息结合，在获取丰富特征信息的基础上具备更为精准的尺度信息，输入至三维重建系统中，大大提高传统三维重建位姿求取的速度和准确性。

针对手持式三维重建方法对于采集者的高要求，避免需要对采集者进行训练并无法控制采集过程中信息的稳定连续性采集而产生不必要的误差，本专利在同一垂直线上配置广角深度相机与二维激光雷达，能够精准的控制实验平台的采集速率和运动角速度，定量控制视频流采集速率，使得系统能够在快速移动的同时稳定获取连续性采集信息。

针对大多三维重建算法注重重建效果而忽视了运动位姿精度的问题，本专利提出一种将系统的维度从三维降至二维的地面约束方法，将相机运动从三维运动降至了二维运动，降低了整体系统的算法复杂度并提升计算效率，使得系统能够更好的应对快速移动的场景。

轮间距为0.52m。通过IMU的实时数据反馈，运载小车的平均角速度为64deg/s。在原地64deg/s的情况下，采集RGB-D数据集，并输入至本专利算法与其他两种算法中，进行对比实验

接着为了测试对不同大场景同时重建时，地面约束对三维重建模型的优化作用，本专利将小车的轮速调整至3m/s、1m/s，角速度调整至66deg/s。其中，当小车在走廊的长直道上时，运行轮速为 3m/s；当小车在大房间内时，运行轮速为1m/s。根据驱动器的日志文件，可以获得在此期间轮速平均值为1.671m/s

最终的测量数据与真值数据如图5所示，图5运载小车多场景快速移动旋转三维重建中的墙面间距测量结果与平均误差统计表。

从图5可以看出，在本专利算法相比较改进的ORBSLAM2稠密重建算法拥有绝对的优势。同时在添加了地面约束后，本专利算法所得到的三维重建模型的精度得到了显著的提高，提高了接近一倍。特别是在1号线、2号线和8号线中，这三组真实距离较远的测量线段中，添加了地面约束的本专利算法拥有的精度优势更加显著。同时在2号线、3号线、4号线、5号线这四组真实距离较近的测量线段中，添加了地面约束的本专利算法也具有一定的优势

针对单一传感器三维重建算法难以保证位姿精度，本专利提出一种基于多传感器融合的复杂场景三维重建方法。该方法主要通过二维激光雷达与广角深度传感器融合，获取AGV坐标以及姿态角得到更为精准的尺度信息，以及彩色、深度视频流信息，以更为精准的尺度、特征信息使得系统避免陷入大多算法容易陷入的模型错层、漂移的问题，最终的三维重建模型更贴近真实尺度。

针对手持式传感器难以在移动的场景中稳定获取采集信息，本专利提出一种基于差速潜伏式AGV的快速移动场景三维重建方法。通过控制AGV能够获得精准的速度量数据，以验证系统在大场景的快速式运动中能获得精准的三维重建模型。此外，为了减少位姿转换计算量，在同一垂直线上搭载深度相机和二维激光雷达。

为了能在快速移动的复杂场景中提高运算效率，本专利提出一种添加地面约束条件的三维重建方法。由于传感器均位于同一垂直线上，系统无需对X方向和Y方向做矩阵变换，且对于Z方向不需要做旋转变换，仅需做平移变换。由于将运动维度从三维降至二维，因此可以大大降低系统的运行、计算复杂度，使得系统能够应对快速移动场景带来的的挑战

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对上述实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的上述实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (9)

1.一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，其特征在于，包括如下步骤：

获取移动场景RGB图像，对RGB图像进行预处理，通过特征提取算法进行提取RGB图像特征，

对图像帧添加关键帧，对关键帧进行筛选；

通过位姿测量单元进行检测生成二维位姿；

将筛选后的关键帧的二维位姿对其三维位姿进行约束，得到约束后的位姿，

通过关键帧的位姿和深度信息进行三维建图，

通过词袋模型对三维建图进行优化；

去除离散的点云，将离散点云对应的关键帧剔除，形成最终的稠密三维重建模型；

根据最终的稠密三维重建模型构建快速移动场景的实验平台。

2.根据权利要求1所述的一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，其特征在于，位姿测量单元包括二维激光雷达、惯导传感器以及轮速计中的一种或两种以上的组合。

3.根据权利要求1所述的一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，其特征在于，图像特征提取具体包括ORB特征点检测算法，ORB特征点检测算法为FAST检测子和BRIEF描述子结合形成，使用FAST进行特征点检测，接着从得到的候选FAST特征点中，提取出Harris角点的最大响应特征点，其中Harris角点的响应函数如下：

R＝detM-α(traceM)²其中，定义R为角点响应函数，通过判断R的大小来判断像素是否为角点。α为一个经验常数，通常取值0.04-0.06。M矩阵为各维均值平均化的协方差矩阵。

4.根据权利要求1所述的一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，其特征在于，还包括词袋模型优化后计算Sim3，进行全局优化，去除离散的点云，将离散点云对应的关键帧剔除，形成最终的稠密三维重建模型。

5.根据权利要求4所述的一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，其特征在于，ORB特征点检测法采用了灰度质心法来进行修正，灰度质心法中包括几何中心P与灰度中心Q，将几何中心P作为该坐标系下的零点，得到灰度质心的坐标为：

其中M₁₀为所有X轴的灰度值之和，M₀₁为所有Y轴的灰度值之和，M₀₀为该像素块内所有灰度值之和；

ORB特征点的角度计算公式为：θ＝atan2(m₀₁,m₁₀)。

6.根据权利要求1所述的一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，其特征在于，还包括运载小车，运载小车为差速潜伏式自动引导小车，RGB-D相机和激光雷达搭载在差速潜伏式自动引导小车上。

7.根据权利要求6所述的一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，其特征在于，所述运载小车包括IMU惯导传感器、工控机与单片机，通过IMU惯导传感器将小车运动的四元数传递给工控机，同时通过二维激光雷达将二维点云传递给工控机，单片机作对电机驱动器发送控制命令，同时接收电机驱动器的轮速数据；

通过串口将轮速数据发送给工控机，进行二维拓扑地图的建立。

8.根据权利要求7所述的一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，其特征在于，运载小车携带相机从三维运动降到了二维运动，进行添加地面约束，

对于长度为单位长度的正交基(e₁,e₂,e₃)，经过一次变换后成为了平面上的(e₁′,e₂′,0)，对于三维下的旋转矩阵R：

而添加地面约束后，旋转矩阵R中的e₃′为

因此平面约束下的旋转矩阵R′为：

9.根据权利要求8所述的一种基于多传感器融合的快速式差速潜伏式AGV稠密三维重建方法，其特征在于，在平面约束下的平移向量t′为：

因此，在平面约束下的SE′(3)如下式所示：

其中，T是变换矩阵。SE(3)指的是变换矩阵构成的特殊欧式群。SO(3)指的是三维旋转矩阵构成的特殊正交群；

通过对已优化的位姿进行地面约束处理，将相机运动整体降维。

Images