CN115900710A

CN115900710A - 基于视觉信息的动态环境导航方法

Info

Publication number: CN115900710A
Application number: CN202211366835.XA
Authority: CN
Inventors: 王搏; 程曦娜; 张亦弛; 焦李成; 焦建斌
Original assignee: Xidian University
Current assignee: Xidian University
Priority date: 2022-11-01
Filing date: 2022-11-01
Publication date: 2023-04-04

Abstract

本发明提出了一种基于视觉信息的动态环境导航方法，主要解决现有技术对室内动态目标处理及自动导航定位差的问题，其方案为：建立人类动态模型，并对其进行参数化；使用DeeplabV3+神经网络对室内的场景图像进行区域分割获取场景分割模型；基于神经网络训练好的场景分割模型进行多视图几何操作；将多视图几何操作后的图像回归ORBSLAM系统，利用该系统中的视觉里程计VIO模块进行相机的位姿估计；使用回环检测优化位姿并输出一个全局位姿，根据全局位姿重建3D网格；利用重建好的3D网格生成3D点云，在3D点云上构建全局地图，机器人利用全局地图在室内拥挤的动态环境中自主导航，本发明提高了导航性能，可用于室内定位。

Description

基于视觉信息的动态环境导航方法

技术领域

本发明属于计算机视觉领域，特别涉及一种动态环境导航方法，可用于室内定位。

背景技术

近年来，位置服务的相关技术和产业正向室内发展以提供无所不在的基于位置的服务，其主要推动力是室内定位所能带来的巨大的应用和商业潜能。在室内和室外的环境下，连续可靠地提供位置信息可以为用户带来更好的用户体验。室外定位和基于位置的服务已经成熟，目前，全球卫星导航系统GPS是最常用的定位导航系统，但是由于卫星信号无法穿透建筑物，室内定位无法利用GPS实现。室内环境的很多独特特征使得现有的很多方案受到不同的局限。实用的室内定位解决方案需要满足以下七个方面的需求：精度、覆盖范围、可靠性、成本、功耗、可扩展性和响应时间。

现有的室内定位技术主要分为以下六类：

第一类为红外线定位技术，红外线的技术已经非常成熟，用于室内定位精度相对较高，但是由于红外线只能视距传播，穿透性极差，当标识被遮挡时就无法正常工作，也极易受灯光、烟雾环境因素的影响。加上红外线的传输距离不长，使其在布局上，无论哪种方式，都需要在每个遮挡背后、甚至转角都安装接收端，布局复杂且定位效果有限。

第二类为射频识别RFID室内定位技术，这类技术利用射频方式，固定天线把无线电信号调成电磁场，使得附着于物品的标签经过磁场后生成感应电流把数据传送出去，以多对双向通信交换数据以达到识别和三角定位的目的。但其不具有通信能力，抗干扰能力较差，不便于整合到其他系统之中。

第三类为蓝牙室内定位技术，蓝牙室内定位技术最大的优点是设备体积小、短距离、低功耗，容易集成在手机等移动设备中。虽然蓝牙传输不受视距的影响，但对于复杂的空间环境，蓝牙系统的稳定性稍差，受噪声信号干扰大且在于蓝牙器件和设备的价格比较昂贵。

第四类为Wi-Fi室内定位技术，Wi-Fi定位技术有两种，一种是通过移动设备和三个无线网络接入点的无线信号强度，通过差分算法，来比较精准地对人和车辆的进行三角定位；另一种是事先记录巨量的确定位置点的信号强度，通过用新加入的设备的信号强度对比拥有巨量数据的数据库，来确定位置。但是这类技术用于室内定位的精度只能达到2米左右，无法做到精准定位。

第五类为ZigBee室内定位技术，ZigBee是一种新兴的短距离、低速率无线网络技术，这些传感器只需要很少的能量，以接力的方式通过无线电波将数据从一个节点传到另一个节点，作为一个低功耗和低成本的通信系统，ZigBee的工作效率非常高。但ZigBee的信号传输受多径效应和移动的影响都很大，而且定位精度取决于信道物理品质、信号源密度、环境和算法的准确性，造成定位软件的成本较高，有待提高的空间还很大。

第六类为UWB超宽带室内定位技术，超宽带定位技术是一种全新的、与传统通信定位技术有极大差异的新技术，其利用事先布置好的已知位置的锚节点和桥节点，与新加入的盲节点进行通讯，并利用三角定位或者“指纹”定位方式来确定位置，但该技术由于新加入的盲节点也需要主动通信使得功耗较高，而且事先也需要布局，使得成本还无法降低。

以上六种技术都存在抗噪能力差或者成本的问题，因此需要一种新的技术来实现在室内环境尤其是动态环境中的导航。

随着最近几年机器人、无人机、无人驾驶、VR/AR的火爆，同步定位与地图构建SLAM技术也为大家熟知，该技术也因为高精度、低成本等优点逐渐被引进室内定位中。同步定位与地图构建技术最早在机器人领域提出，它指的是：机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的环境特征定位自身位置和姿态，再根据自身位置构建周围环境的增量式地图，从而达到同时定位和地图构建的目的，如图1所示。由于SLAM的重要学术价值和应用价值，一直以来都被认为是实现全自主移动机器人的关键技术。

SLAM作为机器人定位导航的核心技术，正在不断引起业内重视，但在实际应用中SLAM技术并不完全等同于机器人自主定位导航。SLAM核心过程主要包括三大步骤：预处理、匹配及地图融合。其中，预处理是通过激光雷达或其他传感器获取所在位置的环境信息，然后对激光雷达原始数据进行优化，剔除一些有问题的数据，或者进行滤波；匹配是个非常关键的步骤，主要是将当前局部环境的点云数据在已建立的地图上寻找对应的位置，匹配的好坏对SLAM构建地图的精度有直接的影响，在定位和建图的过程中，需要将激光雷达当前采集的点云匹配拼接到原有地图中；地图融合就是将这一轮来自激光雷达的新数据拼接到原始地图当中，最终完成地图的更新。

基于SLAM技术，许多学者提出很多多线程框架，将SLAM技术应用于室内定位技术中，并取得很好的效果。

ORB-SLAM是一种基于ORB特征的三维定位与地图构建算法。该算法由Raul Mur-Artal，J.M.M.Montiel和Juan D.Tardos于2015年发表在IEEE Transactions onRobotics。ORB-SLAM一开始基于单目传感器，后来扩展到双目和RGB-D传感器上。ORB-SLAM的系统框架基于PTAM架构，增加了地图初始化和闭环检测的功能，优化了关键帧选取和地图构建的方法，在处理速度、追踪效果和地图精度上都取得了很好的效果，ORB-SLAM构建的地图是稀疏的。ORB-SLAM算法的一大特点是在所有步骤统一使用图像的ORB特征。ORB特征是一种非常快速的特征提取方法，具有旋转不变性，并可以利用金字塔构建出尺度不变性。使用统一的ORB特征有助于SLAM算法在特征提取与追踪、关键帧选取、三维重建、闭环检测等步骤具有内生的一致性。但是ORB-SLAM系统仅仅是在静态的室内环境中取得很好的导航效果，而实际生活场景往往是动态的，室内不断移动的人类和其他动态物体都是不可忽略的导航影响因素，因此需要一些更好的算法能消除这些影响因素，让SLAM系统更具有鲁班性。

人工神经网络是计算机智能研究领域发展迅速的一门理论和技术,具有从环境中学习知识的能力和以类似生物的交互方式适应环境的能力。语义分割是图像理解的基石技术，它可以给出每类对象对应的准确像素，但不能区分同一类型的不同个体。因为深度神经网络强大的学习能力，室内一些非刚性的动态物体，如人类、宠物等能够被深度模型很好地识别出来，如果能利用好神经网络良好的场景分割能力，就能很好的解决环境中的动态噪声干扰问题。目前语义分割方法基本上都是基于深度学习的方法，例如U-Net、SegNet、PSPNet。DeepLab网络是结合了深度卷积神经网络DCNNs和概率图模型稠密条件随机场的方法。

语义SLAM是一种能够同时估计场景的3D几何形状并将语义标签附加到对象和结构之上的算法。Odest Chadwicke Jenkins,Karthik Desingh等人于2018年发表了“Semantic Mapping with Simultaneous Object Detection and Localization”，文中提出了语义SLAM的技术框架，大致为语义提取、SLAM定位与建图两部分，不同的语义SLAM问题区别在于语义提取的方法、语义信息的应用。语义提取主要体现为图像的处理，对图像进行识别分割，加语义标签，而识别与分割图像的方法目前主要为深度学习方法，网络模型训练的结果也决定着语义SLAM的最终效果。对于语义信息的应用，主要体现在辅助定位与语义地图，其中辅助定位又可以分为SLAM特征提取、回环检测、BA这几方面，以提高SLAM本身的定位精度；语义地图主要是构建含语义信息的可用于导航的地图，为机器人层面理解提供丰富的信息。但是语义SLAM也存在一个明显的缺点，那就是不同的算法模型对训练学习数据库依赖较强，往往在不同的场景需要单独训练分割模型，这就导致了成本的巨幅提升。

如今，几何方法在动态滤除领域也逐渐发展起来，比如利用重投影误差过滤动态目标已发展成熟，且成本低、场景适应性好。但是几何方法的缺点就是精度不高，评判标准也不统一。例如重投影误差并不是一个理想的评判标准，这是因为评判的结果与相机的像素有关，所以重投影误差小并不一定代表判断结果就是精确的。

以上這些现有技术的不足，均影响SLAM系统在室内导航的性能，不能满足实际应用需求。

发明内容

本发明的目的在于克服上述现有技术存在的缺陷，提出一种基于视觉信息的动态环境导航方法，以将深度学习与几何优化算法结合到基于ORB特征的三维定位与地图构建系统ORBSLAM系统中，通过室内导航模型基本复现室内的整体结构，减小影响机器人导航的动态元素因子，提高SLAM系统在室内导航的性能，满足实际应用需求。

本发明的技术方案是这样实现的：

一.技术原理

3D环境和对它的空间感知是现实世界中机器人执行高级任务的关键推动因素。为了执行高级指令，例如“在高层建筑的二楼搜索幸存者”，机器人首先要利用深度神经网络对室内环境进行学习，并输出室内环境的结构，物品，动态目标等的相关信息，然后需要将语义信息组合成度量地图，从而形成度量语义空间表示。此外，为了将低级避障和运动规划与高级任务规划联系起来，还需要构建一个世界模型来捕捉不同抽象层次的现实场景。例如机器人可以按照运动规划有效完成到达建筑入口、走楼梯、进入每个房间这一系列动作，但运动规划通常依赖于细粒度的地图表示，例如网格或体积模型。当自治导航系统部署在室内拥挤的环境中时，这个问题变得更加具挑战性。

本发明通过建立基于语义信息的动态对象模型和基于deeplabV3+语义分割的SLAM框架，实现机器人在室内拥挤的环境中自主导航并实时构建环境地图。其中：

建立动态对象模型，是将动态对象分为椅子、杯子等可移动对象和人类两类，对于可移动对象，通过对输入场景图像进行语义分割，再根据不同区域内的语义特征建立对应模型；对于人类，首先使用基于图像卷积神经网络Graph-CNN的方法给出带有全景分割标签的RGB图片，再结合Unity中的person模型组合构建人体3D SMPL模型；

基于deeplabV3+语义分割的SLAM框架，是在建立ORB-SLAM2的基础上，增加目标检测和背景绘制的能力，以通过多视图几何、深度学习或者两者组合检测运动物体，并对于拥有场景的静态地图允许不绘制已被这些动态对象遮挡的帧背景，可估计出场景中静态部分的地图，以满足现实环境中长期应用所的需要。

二.实现方案

根据上述技术原理，本发明的实现步骤如下：

(1)建立动态对象模型：

1a)基于优化的方法先将3D网格匹配到2D图像关键点，再基于深度学习的方法匹配语义图像关键点与2D图像关键点，建立语义图像像素点到3D网格的联系；

1b)根据语义图像像素点到3D网格的联系，对皮肤多人线性模型SMPL进行参数化，通过其紧凑的姿态和形状描述，获得具有6890个顶点和23个关节的3D人体网格模型；

(2)采用双目相机拍摄的图像和RBG-D相机拍摄的深度图像作为输入，使用DeeplabV3+神经网络对室内的场景图像进行区域分割，得到场景分割模型；

(3)基于DeeplabV3+神经网络的场景分割模型分割图片帧，基于分割后的结果去除潜在动态区域内部和周围像素，再将静态区域的特征点投影到图片帧中，并最小化重投影误差，实现多视图几何操作；

(4)基于现有的ORBSLAM系统，输入单/双目图像、RGB-D深度图像，融合DeeplabV3+神经分割网络和人体3DSMPL模型，并利用步骤(2)中区域分割的初结果对场景的图片帧进行多视图几何操作，再将几何操作后的图像回归到ORBSLAM系统进行基于视觉的相机位姿估计；

(5)获取相机与室内环境相对的全局位姿：

(5a)基于ORBSLAM系统中的视觉里程计VIO模块，利用惯性测量IMU数据计算局部相机位姿估计值，并使用现有的非线性优化方法对该相机局部位姿估计值和步骤(4)中的视觉位姿估计值进行综合，得到优化后的局部相机位姿估计值；

(5b)利用回环检测法对由多次跟踪所造成优化局部相机位姿估计值的累计漂移进行消除，输出相机与室内环境相对的全局位姿；

(6)重建3D网格：

(6a)利用得到的全局位姿，不断调整机器人的跟踪轨迹，在相机跟踪过程中的关键帧上获取图像上2D特征点，并在该特征点上进行Delaunay三角剖分，形成帧图像上的2D三角网格；

(6b)对2D三角网格做反向投影操作，生成一个3D网格；

(7)语义建图：

(7a)使用基于体素TSDF算法去除重建好的3D网格噪声并提取该全局3D网格，再将步骤(1)的3D人体模型网格加入到该全局3D网格中；

(7b)对加入3D人体模型网格后的全局3D网格使用稠密双目立体算法获得来自于当前双目图片对应的3D点云；

(7c)对已经获取的包含人体模型的3D点云地图使用捆集光线投射方法，构建一个全局地图，机器人根据全局地图在室内动态的环境中进行导航。

本发明与现有技术相比，具有以下优点：

1.本发明利用CNN分割的RGB图片和Unity中的person模型，从中预测人体3D模型，并对皮肤多人线性模型SMPL进行参数化，能实现随时给出时变的3D模型表面形状顶点信息的需求，为位姿获取和3D地图的构建提供了不可缺少的动态信息。

2.本发明通过多视图几何、深度学习两者兼用检测运动物体，可检测出室内场景中像椅子这样没有先验信息的半动态物体，为相机的跟踪提供了更加可靠的静态特征点，减少了动态物体对机器人在室内自主导航的干扰。

3.本发明提出的新回环检测方法PCM，即利用PCM的邻接矩阵实现的检测的在线操作，保证了系统运作的实时性，同时由于在一致性观测中添加了高斯牛顿优化方法，保证了回环的鲁棒性，可使机器人的导航实现长期自主。

4.本发明由于对完成视角匹配对应的双目图像融合3D网格生成了3D点云地图，可使机器人能根据双目图像在3D真实世界中更加精确地实现自我定位及后续导航，提高了SLAM系统在室内导航的性能。

附图说明

图1是本发明的实现流程图：

图2是本发明中构建的线性人体模型SMPL实例图；

图3是本发明中3D人体模型位姿跟踪示意图；

图4是本发明中的DeeplabV3+网络结构图；

图5是本发明中的空洞卷积原理图；

图6是本发明中的深度可分离卷积示意图；

图7是本发明中基于DeeplabV3+网络对室内场景进行分割的语义实例图；

图8是本发明中利用新帧和关键帧之间的平移和旋转计算帧间重合度示意图；

图9是本发明中基于多视图几何操作生成的动态区域分割实例图；

图10是本发明中搭建的定位与建图框架示意图。

具体实施方式

以下结合附图对本发明的实施例和效果作进一步的详细描述。

参照图1，本实例的实现步骤如下：

步骤1，建立动态对象模型。

本步骤基于优化的方法，将三维网格匹配到2D图像关键点，再基于深度学习的方法，直接从像素信息推断网格，最后根据网格和全景分割图构建动态模型，具体实现如下：

1.1)从自带语义标签的3D网格中选出预先设置人类对象的所有网格，并使用欧几里得聚类进行实例分割，再将分割好的人类簇投影到2D图像之中，从人类簇的网格顶点计算2D图像中对应对象的质心和方向；

1.2)从卷积神经网络CNNs中获取带2D场景地图的语义图，利用OpenCV提供的Brute-Force匹配函数将语义图像与2D图像的对应关键点进行匹配，建立出语义图像像素点到3D网格的联系；

1.3)根据语义图像像素点到3D网格的联系先将左侧原始相机图像裁剪到语义图中每个被检测到的人附近的边界框中，再从边界框回归一个SMPL网格模型顶点的3D位置，然后根据具体的场景3D位置获得一个3D人体模型SMPL，如图2所示，其中图2(a)是动态场景中的3D人类对象，图2(b)是针对对象构建好的3D人体模型SMPL；

1.4)使用N点视角对应算法PNP提取相机原始视角图像帧中人类的完整位姿，如图3所示。

步骤2，对室内场景进行区域分割。

本步骤采用双目相机拍摄的图像和RBG-D相机拍摄的深度图像作为输入，使用DeeplabV3+神经网络对室内的场景图像进行区域分割，得到场景分割模型，具体实现如下：

参照图4,所述DeeplabV3+神经网络，包括下采样模块Encoder和上采样模块Decoder，其中Encoder负责特征提取，Decoder通过插值，转置卷积的方式负责恢复特征图大小，Encoder和Decoder这两个模块在结构上是对称的，通过Encoder-Decoder结构得到高分辨率的特征，其具体实现如下：

2.1)下采样模块Encoder进行特征提取：

2.1.1)下采样模块Encoder先对压缩四次的初步有效特征层使用并行的空洞卷积Atrous Convolution来扩大感受野和捕获多尺度图片的环境信息：

参照图5，空洞卷积就是在卷积核元素之间加入一些空格来进行卷积扩张，原来卷积核卷积后的感受野大小为3，进行扩张后的卷积核卷积后的感受野大小为5。因为神经元感受野的值越大表示其能接触到的原始图像范围就越大，也意味着它可能蕴含更为全面、语义层次更高的特征，因此需要利用扩大后的感受野来捕获多尺度图片的环境信息，且由于卷积核中空格的存在，只要最后的卷积输出与扩张前的卷积输出大小一致，就不会造成成本浪费；

2.1.2)分别用不同采样率的Atrous空间金字塔池化结构ASPP进行特征提取；再将特征提取结果进行concat合并，得到合并特征层；

2.1.3)将合并特征层进行1x1逐点卷积得到绿色特征图，完成对场景图的高分辨率特征初步提取；

2.2)上采样模块Decoder恢复特征图的大小：

2.2.1)上采样模块Decoder对压缩两次的初步有效特征层利用1x1逐点卷积调整通道数，再将调整后的有效特征层和Encoder中经过ASPP处理的有效特征层进行堆叠；

2.2.2)对堆叠特征层进行两次深度可分离卷积获得一个最终的有效特征层，并利用一个1x1逐点卷积对这个最终有效特征层进行通道调整并调整成类别总数目：

深度可分离卷积主要分为两个过程：逐通道卷积Depthwise Convolution和逐点卷积Pointwise Convolution。普通神经网络利用的就是逐通道卷积，但是逐通道卷积后的特征图数量与输入层的通道数相同，无法扩展对特征图进行后续扩展，并且这种运算对输入层的每个通道独立进行卷积运算，没有有效的利用不同通道在相同空间位置上的特征信息，因此需要逐点卷积来将这些特征图进行重新组合来获取更多特征信息。图6给出了深度可分离卷积的三种卷积形式，其中图6(a)是现有逐通道卷积，图6(b)是现有逐点卷积，图6(c)是本发明融合空洞卷积的逐通道卷积；

2.2.3)经过调整的有效特征层输出预测的特征图后，利用图片调整resize使输出预测图恢复原图大小；

2.3)基于DeeplabV3+神经网络的场景分割模型分割图片帧：

利用该网络先对场景图片数据集进行模型迭代训练，直到获得场景分割模型:再该利用分割模型对场景图片进行分割预测，输出与原场景图大小一致的语义分割结果图，如图7所示，其中图7(a)是室内3D场景的原始灰度图，图7(b)是与室内场景对应生成的语义分割结果图。

步骤3，多视图几何操作。

3.1)基于DeeplabV3+神经网络的场景分割模型分割后的结果，去除图片帧动态区域内部和周围像素，再将静态区域的特征点投影到图片帧中；

3.2)设置重合度评判准则：

3.2.1)将最大重叠关键帧的数量被设置为5，作为在动态目标检测的计算成本和精度之间的折中；

3.2.2)参考图8，计算新输入的图片帧和每个投影帧之间的旋转视角误差和深度误差：

3.2.2.1)计算旋转视角误差：根据图8(a)所示旋转视角误差的计算原理，将静态区域投影后的投影帧设为关键帧CF，当新输入图片帧KF时，计算其中每个特征点x到关键帧的投影X，并利用相机参数矩阵计算获得新特征点x′，以及特征点x到投影点X的投影深度z和新特征点x′到投影点X的投影深度z_proj：

即用x和x’分别到投影点X的欧氏距离表示二者的深度z、z_proj；

利用投影深度z、z_proj计算x和x′的的视差角α作为旋转视角误差，设置误差阈值为30°，如果α大于30。，则这个点就会被标记为动态，从这时起就会被删除。

3.2.2.2)计算深度误差：根据图8(b)所示的深度误差计算原理，通过实验数据分析，对于视差角度α大于30。的静态物体，很有可能由于它们的视点差异而被认为是动态的，因此本步骤将利用刚性平移矩阵测量新特征点x′与投影点X的平移距离作为深度z′，并比较由相机参数矩阵计算获得的投影深度z_proj，得到误差函数：

Δz＝z_proj-z′，其中Δz表示两个深度的差值

通过手动标记数据集中30张图像的动态对象设定3个阈值τ分别为：0.4m、0.Sm、0.6m，并根据这些阈值得到的精度和召回率建立一个评估函数以此确认最佳阈值：

F＝0.7*Precision+0.3*Recall

其中F是评估函数，Precision是三个阈值得到的精度，Recall是三个阈值得到的召回率。

本实例通过最大化评估函数F确认了最佳阈值τ为0.4m，如果Δz小于0.4m，这个被标记为动态的点则被恢复为静态。

3.2.3)综合旋转视角误差和深度误差作为一种重合度评判准则；

3.3)利用该评判准则去除没有先验信息的动态区域获得动态滤除结果图，完成多视图几何操作，分割效果如照图9，其中图9(a)是原动态场景的RGB图，图9(b)多视图几何操作后的分割结果图。由于椅子在深度学习模型中是没有先验语义的，通过多视图几何操作后，对其也进行了分割。

步骤4，搭建跟踪与定位框架。

本步骤基于现有的0RBSLAM系统，输入单/双目图像、RGB-D深度图像，融合DeeplabV3+神经分割网络，并利用语义分割的初结果对场景的图片帧进行多视图几何操作，再将几何操作后的图像回归到0RBSLAM系统形成完整的定位建图系统框架。

参考图10，具体实现如下：

4.1)在双目和单目图像管道中，利用DeeplabV3+神经网络的场景分割模型分割图片帧，去除图片帧中有先验信息的动态区域并去除潜在动态区域内部和周围像素，属于动态物体的特征点既不被跟踪也不用于建图；

4.2)在RGB-D图像管道中，增加基于多视图几何操作以实现更精确的运动分割，并基于多视图几何操作后的结果去除动态区域轮廓特征点；

4.3)将双目和单目图像管道中去除动态区域的结果和RGB-D图像管道中进行更加精确的运动分割后的结果同时加入基于ORBSLAM系统的跟踪和定位Tracking and Mapping模块，从而回归到ORBSLAM系统。

步骤5，获取相机与室内环境相对的局部位姿。

本步骤基于ORBSLAM系统中的视觉里程计VIO模块，利用惯性测量IMU数据、非线性优化方法和回环检测，获取相机与室内环境相对的全局位姿，具体实现如下：

5.1)对于已获取的惯性测量IMU数据，使用IMU流形预积分方法获取相机两个连续关键帧之间的相对位姿：

以一维空间位置估计为例，其IMU预积分是假设在相机或激光所在的全局坐标系中，将每个IMU数据从关键帧的前一个时刻t₁积分到后一个时刻t₂，即已知IMU的测量值是加速度、速度和位移，假设要优化这两个时刻的位移、速度，可以得到：

v_k＝v_k-1+(a_k-1-b_a)Δt

其中，v_k-1是上一时刻的测量速度，v_k是当前时刻的测量速度；x_k-1是上一时刻的测量位移，x_k是当前时刻的测量位移；a_k-1是上一时刻的测量加速度；b_a是IMU测量误差；Δt是t₁到t₂时刻的时间差；

如果在全局坐标系对上式直接积分，可以得到：

其中a_k是当前时刻的测量加速度；v_t1是t₁时刻优化后的速度，v_t2是t₂时刻优化后的速度；x_t1是t₁时刻优化后的位移，x_t2是t₂时刻优化后的位移；

如果是在三维空间中，由于速度是有方向的，因而每次优化时，更新全局坐标系下的v_t1都需要重新计算求和号内的结果。但如果先在局部坐标系下将：

作为前一个时刻t₁到后一个时刻t₂的速度增量，就可以定量表示速度计算求和号内的结果，故用当前时刻测量速度v_k代替v_t2，再带入x_t2的积分求和号中，得到预积分的结果γ：

再把当前时刻优化后的速度v_t2的表达式带入x_t2的计算公式中，得到当前时刻优化后的位移x_t2的新表达式：

每次更新全局坐标系下的v_t1时，只需要将代替前后两个时刻间的测量速度增量代入当前时刻优化后的速度v_t2的表达式中，再将预积分结果γ和v_t2的新表达式代入当前时刻优化后的位移x_t2的表达式中，得到新的积分结果：

v_t2＝v_t1+β

x_t2＝x_t1+v_t1(t₂-t₁)+γ

其中，β是前后时刻间的测量速度增量的定量表示，γ是局部坐标下优化位移的新预积分结果，v_t2是当前时刻定量表示后的优化速度表达式，x_t2是当前时刻添加新预积分后的优化位移表达式；

利用上述新的积分结果获取两个连续关键帧之间的相对位姿；

5.2)使用Shi-Tomasi角点作为特征点并使用Lukas-kanade追踪器对特征点进行追踪，再基于已经获取的两帧之间的相对位姿对特征点进行跨帧追踪，获取连续的位姿估计；

5.3)使用随机抽样一致算法RANSAC对位姿估计结果进行几何验证，输出最终的局部相机位姿估计值；

5.4)对神经网络分割后的视觉结果使用直接线性变换DLT估计动态区域特征点的3D位置，并从相机跟踪状态中消除相应的3D点，以获取无结构视觉模型；

5.5)在每一个关键帧中将无结构视觉模型和前端流形预积分后的IMU数据结果添加到一个固定延迟平滑器中，使其在结构上形成一个因子图；

5.6)使用因子图求解库GTSAM中的联合树iSAM2求解因子图：

将因子图与贝叶斯定理相结合，把固定延迟平滑器中的每个融合数据记为因子X_i，将问题描述成一个最大后验概率问题，得到目标函数X^*：

对目标函数X^*的右边取负对数，将最大化因子的乘积问题转化成一个非线性最小二乘问题，即将目标函数X^*转换成如下形式：

求解这个非线性最小二乘问题，得到因子图的最优解，其中，φ_i(X_i)是后验概率函数，-log是对后验概率函数取的负对数，argmaxΠi是后验概率的乘积最大化，f_i(X_i)是误差函数；

5.7)相机在移动的过程每获取一个关键帧时，iSAM2就进入一次迭代，根据因子图的最优解删除关键帧中相机后面的点、没有足够视差进行三角化的点和具有较大重投影误差的点，再跟踪关键帧中的剩余特征点得到一个位姿估计值；

5.8)利用位姿估计值计算出的协方差矩阵，利用该协方差矩阵对相邻帧变换较大的相机位姿进行平滑缩小，得到最终的位姿估计值。

步骤6，回环检测。

本步骤利用回环检测法对由多次跟踪所造成优化局部相机位姿估计值的累计漂移进行消除，具体实现如下：

6.1)根据相机跟踪的轨迹设置多个回环，并使用词袋文件库DBoW2中带有ORB特征描述符的词袋对这些回环进行快速检测，得到初步筛选的回环；

6.2)对筛选后的回环使用随机抽样一致算法RANSAC进行几何验证，以拒绝离群点较多的回环；

6.3)将剩余的回环传递给离群点拒绝器PCM选择最优的连续回环，完成对累计漂移的消除：

6.3.1)在回环添加一个里程计一致性检验，里程计检验是为了确保每一个回环是与里程计始终保持一致，即把沿着回积累的的误差与使用卡方检验得到的观测误差不一致的回环标记为上外点的标签，如果一个在当前时刻检测到的回环通过了里程计检验，则会测试它是否成对与之前的回环一致；

6.3.2)构建一个回环邻接单位矩阵A(1X1)，以启用在线操作，其中“1”代表回环的个数,每当检测到一个新的回环，仅给这个矩阵增加一个行和一个列；

6.3.3)对最后的回环矩阵使用快速最大子图方法得到一个连续的最大一致回环集合。

至此多次跟踪所造成的相机位姿估计值的累计漂移得到消除，输出一个具有鲁棒性和长期性的全局位姿。

步骤7，重建3D网格。

本步骤利用得到的优化后的全局位姿，不断调整机器人的跟踪轨迹，在相机跟踪过程中的单张关键帧上获取图像上2D特征点，利用每个特征点生成对应网格得到单帧的3D网格，然后将单帧网格整理融合成一个能够跨越多帧的网格，最后对网格进行正则化，输出一个多帧全局网格。具体实现如下：

7.1)利用得到的优化后的全局位姿，不断调整机器人的跟踪轨迹，再实时根据纠正后的轨迹再次获取关键帧上的静态2D特征点，并在该特征点上进行Delaunay三角剖分，从而形成帧图像上的2D三角网格；再对该2D网格做反向投影操作生成一个3D网格，并将每个关键帧的所有静态特征点对应生成的3D网格合并为一个单帧网格；

7.2)利用符号距离函数TSDF的空间距离限制去除3D网格中的空间体素离群值，从而去除掉单帧网格中的噪声；

7.2)利用多帧网格生成器把去噪后的单帧网格收集起来并融合成一个多帧网格；

7.3)分别对单帧网格和多帧网格编码，生成带有节点ID的单帧网格三角列表和多帧网格三角列表，查看并添加所有仅在单帧网格三角列表里面而不在多帧网格三角列表的节点，同时，实时更新节点对应的的3D网格位置，实现多帧网格的正则化，最后输出一个多帧全局3D网格。

步骤8，语义建图。

8.1)利用多帧全局3D网格和双目图像生成3D点云：

8.1.1)利用相机已知的畸变参数对图像畸变进行单目校正，即校正图像边缘的成像畸变曲线；

8.1.2)根据相机的内参数和外参数，调用已有的视觉opencv中的UndistortRectifyMap函数和remap函数对双目图像进行立体畸变矫正，使得左右视角的匹配对应；

8.1.3)对完成视角匹配对应的双目图像使用深度点云库LIBELAS计算其视差，并基于该视差结果对全局3D网格进行融合修正，将修正结果代入3D点云库ELAS中进行处理，最终获得与双目图像对应的3D点云图；

8.2)对3D点云地图使用捆集光线投射方法，构建一个全局地图：

8.2.1)在3D点云地图打上与场景语义分割图对应的语义标签；

8.2.2)用捆集光线投射方法对3D点云进行光线投射形成点云捆集光线束，利用光束集里能观测到静态语义标签的频率计算标签概率，利用点云捆集光线束集里面的每一束光线构建一个储存标签概率的向量；

8.2.3)仅在符号距离函数TSDF的截断距离里面沿着带向量的光线遍历整个空间体素，并给这些空间体素赋予对应的标签概率；

8.2.4)使用贝叶斯方法更新每一个体素的概率，并提取概率最大标签对应的体素；

8.2.5)利用已知的坐标文件排列所有空间体素，构成一个全局地图，机器人利用该全局地图在室内拥挤的动态环境中自主导航，且全局地图也能在以后的导航系统构建中提供重定位帮助和建图背景信息。

以上描述仅是本发明的一个具体实例，不构成对本发明的任何限制，显然对于本领域的专业人员来说，在了解了本发明内容和原理后，都可能在不背离本发明原理、结构的情况下，进行形式和细节上的各种修正和改变，但是这些基于本发明思想的修正和改变仍在本发明的权利要求的保护范围之内。

Claims

1.一种基于视觉信息的动态环境导航方法，其特证在于，包括如下步骤：

(1)建立动态对象模型：

1b)据语义图像像素点到3D网格的联系，对皮肤多人线性模型SMPL进行参数化，通过其紧凑的姿态和形状描述，获得具有6890个顶点和23个关节的3D人体网格模型；

(4)于现有的ORBSLAM系统，输入单/双目图像、RGB-D深度图像，融合DeeplabV3+神经分割网络和人体3DSMPL模型，并利用步骤(2)中区域分割的初结果对场景的图片帧进行多视图几何操作，再将几何操作后的图像回归到ORBSLAM系统进行基于视觉的相机位姿估计；

(5)获取相机与室内环境相对的全局位姿：

(5a)于ORBSLAM系统中的视觉里程计VIO模块，利用惯性测量IMU数据计算局部相机位姿估计值，并使用现有的非线性优化方法对该相机局部位姿估计值和步骤(4)中的视觉位姿估计值进行综合，得到优化后的局部相机位姿估计值；

(6)重建3D网格：

(6b)对2D三角网格做反向投影操作，生成一个3D网格；

(7)语义建图：

2.根据权利要求1所述的方法，其特征在于，步骤1a)中基于优化的方法先将3D网格匹配到2D图像关键点，再基于深度学习的方法匹配语义图像关键点与2D图像关键点，实现如下：

1a1)从自带语义标签的3D网格中选出预先设置人类对象的所有网格，并使用欧几里得聚类进行实例分割，再将分割好的人类簇投影到2D图像之中，从人类簇的网格顶点计算2D图像中对应对象的质心和方向；

1a2)从卷积神经网络CNNs中获取带2D场景地图的语义图，利用OpenCV提供的Brute-Force匹配函数将语义图像与2D图像的对应关键点进行匹配，建立出语义图像像素点到3D网格的联系。

3.根据权利要求1所述的方法，其特征在于，步骤1b)中根据语义图像像素点到3D网格的联系，对皮肤多人线性模型SMPL进行参数化，实现如下：

根据语义图像像素点到3D网格的联系先将左侧原始相机图像裁剪到语义图中每个被检测到的人附近的边界框中，再从边界框回归一个SMPL网格模型顶点的3D位置；

使用图像卷积神经网络方法获得一个3DSMPL；

使用N点视角对应算法PNP提取相机原始视角图像帧中人类的完整位姿。

4.根据权利要求1所述的方法，其特征在于，步骤(3)中基于DeeplabV3+神经网络的场景分割模型分割图片帧，实现如下：

所述DeeplabV3+神经网络模型场景分割模型，包括下采样模块和上采样模块，其中：

该下采样模块用于特征提取，其先对压缩四次的初步有效特征层使用并行的空洞卷积Atrous Convolution来扩大感受野和捕获多尺度图片的环境信息；再分别用不同采样率的Atrous空间金字塔池化结构ASPP进行特征提取；再将特征提取结果进行concat合并，将合并特征层进行1x1逐点卷积得到绿色特征图，完成对场景图的高分辨率特征初步提取；

该上采样模块用于恢复特征图的大小，其通过插值、转置卷积方式先对压缩两次的初步有效特征层利用1x1逐点卷积调整通道数，再将调整后的有效特征层和Encoder中经过ASPP处理的有效特征层进行堆叠；然后利用Pytorch对堆叠特征层进行两次深度可分离卷积获了一个最终的有效特征层，并利用一个1x1逐点卷积对这个最终有效特征层进行通道调整并调整成类别总数目；最后利用resize进行上采样使输出预测图片恢复原图大小；

所述基于DeeplabV3+神经网络的场景分割模型分割图片帧，是先利用该网络对场景图片数据集进行模型迭代训练，直到获得场景分割模型；再利用分割模型对场景图片进行分割预测并输出与原场景图大小一致的语义分割结果图。

5.根据权利要求1所述的方法，其特征在于，步骤(4)将几何操作后的图像回归到ORBSLAM系统进行基于视觉的相机位姿估计，实现如下：

(4a)基于场景分割模型分割后的结果去除图片帧动态区域内部和周围像素，再将静态区域的特征点投影到图片帧中；

(4b)计算新输入的图片帧和每个投影帧之间的平移和旋转距离，并将这个平移和旋转距离作为一种重合度评判准则，利用该评判准则去除没有先验信息的动态区域，完成多视图几何操作；

(4c)先利用多视图几何操作后的结果去除动态区域轮廓特征点，再利用ORBB特征点SLAM系统中的track函数跟踪场景图中去除动态区域轮廓特征点的剩余静态特征点，以完成初步相机位姿估计。

6.根据权利要求1所述的方法，其特征在于，步骤(5a)中基于ORBSLAM系统中的视觉里程计VIO模块，利用惯性测量IMU数据计算局部相机位姿估计值，实现如下：

(5a1)对于已获取的惯性测量IMU数据，使用IMU流形预积分方法获取相机两个连续关键帧之间的相对位姿：

(5a2)使用Shi-Tomasi角点作为特征点并使用Lukas-kanade追踪器对特征点进行追踪，再基于已经获取的两帧之间的相对位姿对特征点进行跨帧追踪，获取连续的位姿估计；

(5a3)使用随机抽样一致算法RANSAC对位姿估计结果进行几何验证，输出最终的局部相机位姿估计值。

7.根据权利要求1所述的方法，其特征在于，步骤(5a)使用非线性优化方法对相机局部位姿估计值和步骤(4)中的视觉位姿估计值进行综合，实现如下：

(5a4)对神经网络分割后的视觉结果使用直接线性变换DLT估计动态区域特征点的3D位置，并从相机跟踪状态中消除相应的3D点,以获取无结构视觉模型；

(5a5)在每一个关键帧中将无结构视觉模型和前端流形预积分后的IMU数据结果添加到一个固定延迟平滑器中，使其在结构上形成一个因子图；

(5a6)使用因子图求解库GTSAM中的联合树iSAM2求解因子图：

将因子图与贝叶斯定理相结合，把固定延迟平滑器中的每个融合数据记为因子X_i,将问题描述成一个最大后验概率问题，得到目标函数X^*：

其中，

是后验概率函数，-log是对后验概率函数取的负对数，argmaxΠi是后验概率的乘积最大化，f_i(X_i)是误差函数。

求解这个非线性最小二乘问题，得到因子图的最优解；

(5a7)相机在移动的过程每获取一个关键帧时，iSAM2就进入一次迭代，根据因子图的最优解删除关键帧中相机后面的点、没有足够视差进行三角化的点和具有较大重投影误差的点，再跟踪关键帧中的剩余特征点得到一个位姿估计值；

(5a8)利用位姿估计值计算出的协方差矩阵，利用该协方差矩阵对相邻帧变换较大的相机位姿进行平滑缩小，得到最终的位姿估计值。

8.根据权利要求1所述的方法，其特征在于，步骤(5b)中利用回环检测法对由多次跟踪所造成优化局部相机位姿估计值的累计漂移进行消除，实现如下：

(5b1)根据相机跟踪的轨迹设置多个回环，并使用词袋文件库DBoW2中带有ORB特征描述符的词袋对这些回环进行快速检测，得到初步筛选的回环；

(5b2)对筛选后的回环使用随机抽样一致算法RANSAC进行几何验证，以拒绝离群点较多的回环；

(5b3)将剩余的回环传递给离群点拒绝器PCM选择最优的连续回环，完成对累计漂移的消除。

9.根据权利要求1所述的方法，其特征在于，步骤(7a)中使用基于体素TSDF算法去除重建好的3D网格噪声，是先使用Delaunay三角剖分方法提取场景3D模型表面，获得3D网格的空间体素值；再利用符号距离函数TSDF的空间距离限制去除3D网格中的空间体素离群值。

10.根据权利要求1所述的方法，其特征在于，步骤(7b)中对加入3D人体模型网格后的全局3D网格使用稠密双目立体算法获得来自于当前双目图片对应的3D点云，实现如下：

(7b1)利用相机已知的畸变参数对图像畸变进行单目校正，即校正图像边缘的成像畸变曲线；

(7b2)根据相机的内参数和外参数，调用已有的视觉opencv中的UndistortRectifyMap函数和remap函数对双目图像进行立体畸变矫正，使得左右视角的匹配对应；

(7b3)对完成视角匹配对应的双目图像使用深度点云库LIBELAS计算其视差，并于该视差结果对全局3D网格进行融合修正；

(7b4)将修正结果代入3D点云库ELAS中进行处理，最终获得与双目图像对应的3D点云图。

11.根据权利要求1所述的方法，其特征在于，步骤(7c)中对已经获取的包含人体模型的3D点云地图使用捆集光线投射方法，构建一个全局地图，实现如下：

(7c1)对3D点云地图打上与场景语义分割图对应的语义标签；

(7c2)采用捆集光线投射方法对3D点云进行光线投射形成点云捆集光线束，并利用光束集里能观测到静态语义标签的频率计算标签概率；

(7c3)利用点云捆集光线束集里面的每一束光线构建一个储存标签概率的向量；

(7c4)仅在符号距离函数TSDF的截断距离里面沿着带向量的光线遍历整个空间体素，并给这些空间体素赋予对应的标签概率；

(7c5)使用贝叶斯方法更新每一个体素的概率，并提取概率最大标签对应的体素；

(7c6)利用已知的坐标文件排列所有空间体素，构成一个全局地图。