技术领域
[0001] 本
发明涉及运动估计技术领域，具体涉及一种基于深度学习和视惯融合的室内机器人运动估计方法。
背景技术
[0002] 智能机器人一般是通过安装感应设备来
感知周围的环境，并根据感应设备所获取的信息，实现自身的
定位与建图，再通过后续的规划路径，最终安全可靠的到达目的地。实时准确的运动估计是
移动机器人智能化的
基础，是保障机器人完成自主行为的前提。随着视觉技术不断发展，视觉
里程计技术已被广泛应用，比如无人机、无人驾驶、工场AGV等等，同时也越来越多的被应用于智能机器人的自主定位和运动估计。
[0003] 在纯视觉里程计系统中，系统利用摄像头
传感器获取周围环境的图像信息，通过对图像进行分析，估计系统的运动状态。但是在单目视觉系统中存在着尺度问题，即系统无法得知所获得的运动过程的实际尺度，只能获取相对的长度信息。针对纯视觉系统无法解决的尺度问题，视觉惯性里程计系统在纯视觉里程计系统的基础上添加了惯性传感器信息，其可以通过利用惯性信息进行尺度对准，顺利的获取系统的尺度信息。然而，由于惯性传感器存在累积误差，随着时间的推移，误差会越来越大，因此在连续工作时会存在累积漂移的问题。
发明内容
[0004] 本发明所要解决的是现有智能机器人的运动估计系统存在
精度不高的问题，提供一种基于深度学习和视惯融合的室内机器人运动估计方法。
[0005] 为解决上述问题，本发明是通过以下技术方案实现的：
[0006] 基于深度学习和视惯融合的室内机器人运动估计方法，包括步骤如下：
[0007] 步骤1、利用单目相机获取机器人的视觉图像；
[0008] 步骤2、将步骤1所获取的视觉数据送入到
卷积神经网络和长短期记忆网络所组成的三维投影几何训练模型中，对视觉图像进行特征提取与
跟踪，以及关键点预测；
[0009] 步骤3、利用惯性测量单元获得机器人的IMU数据；
[0010] 步骤4、采用预积分的方法对步骤3所获取的IMU数据进行处理，得到IMU数据的预积分值；
[0011] 步骤5、通过系统增设的布尔类型判断的入口判断系统是否初始
化成功，如果成功，则执行步骤7；如果未成功，则执行步骤6；
[0012] 步骤6、对系统进行纯视觉初始化和视觉惯
导联合初始化；
[0013] 步骤7、初始化完成后，将步骤2和步骤4所得的数据一并送入紧耦合的视觉惯导里程计中，对数据进行基于滑动窗口的视觉惯导里程计的优化；
[0014] 步骤8、根据当前
帧的
位姿估计结果,判断当前帧是否为关键帧,关键帧的选取依据为当前帧与上一帧关键帧运动距离超过设定
阈值大小：若当前帧不为关键帧,说明系统与上一关键帧运动距离较短,则将当前帧数据直接从滑动窗口中剔除；若当前帧为关键帧,则将滑动窗口中时间最早的一帧从滑动窗口中剔除,将当前帧作为新的关键帧；
[0015] 步骤9、每隔N个关键帧提取1个关键帧到关键帧
数据库，并将当前提取的关键帧与关键帧数据库内存储的历史关键帧进行比较，即对于当前提取的关键帧和历史关键帧；通过对当前视觉帧提取的特征与历史关键帧中的特征点做匹配，判断是否回环，即判断出运动的起点是否是终点，如果判断出机器人已经走过完整连续的一圈，系统输出机器人正确的位姿，如果未判断出回环，即未完成闭环的运动，或者已完成闭环的运动，系统未检测出，系统输出未矫正的机器人的位姿，并返回步骤1，直至检测出回环，系统输出正确的位姿。
[0016] 上述步骤3所采集的IMU数据的数据量要多于步骤1所采集的视觉的数据量。
[0017] 上述步骤5中，系统初始化成功的依据是：当解出绝对尺度，
陀螺仪偏置，
加速度偏置，
重力加速度和每个IMU时刻的速度时，则判断出系统初始化成功。
[0018] 上述步骤6中所述纯视觉初始化的过程为：每当进来一个新的图像帧的时候，首先判断它与滑动窗口里面存储的之前的一帧的的相对位移，用特征点的相对位移表示；接着利用对极
几何模型求解单目相机的相对变换；然后三
角化得到相应的观测的点到物体的点即3D点的坐标；最后利用这些3D点和滑动窗口中其他的帧的2D点即可以进行pnp求解获得滑动窗口中的所有的位姿和特征点3D坐标，从而完成纯视觉初始化。
[0019] 上述步骤6中所述的视觉惯导联合初始化的过程为：首先根据滑动窗口中连续图像的相对旋转算出相机和IMU间的外参；接着根据纯视觉的初始化得到的各帧图像的相对旋转来计算出陀螺仪的偏置；最后利用前一帧对后一帧
位置、速度的预测与当前值的误差，实现速度、重力、尺度的对齐，最终完成视觉惯导的联合初始化。
[0020] 上述步骤7的具体过程如下：每当进来一个新的图像帧的时候，首先判断它与滑动窗口里面存储的之前的一帧的的相对位移，用特征点的相对位移表示；接着利用滑动窗口中的各帧的位置，
姿态，速度，偏移，相机的外参，以及每一个特征点在它被观察到的第一帧的深度，拟合前后帧之间的IMU的预积分值，前后帧之间的偏移的差的约束，每一个特征点的重投影位置这三部分，最终完成基于滑动窗口的视觉惯导里程计的优化。
[0021] 与
现有技术相比，本发明利用深度学习的方法通过设计的GCN网络来对视觉数据进行特征提取，并将视觉惯导信息相融合，构建一个鲁棒的SLAM系统，其不仅可以极大的提高系统的鲁棒性，还可以在计算能力受限的嵌入式设备上快速部署，通过对惯性信息的预积分处理，形成视觉信息的帧间约束，采用联合优化器对视觉惯性输出数据进行融合优化，通过预积分的视觉惯性里程计系统比单独视觉里程计系统更高的定位精度，基于预积分
算法有效的将惯性信息用到里程计系统中，并抑制系统的噪声传播，降低惯性零漂对里程计定位精度的影响。此外，本发明还可部署到低功耗平台Jetson tx2上，用于室内机器人的控制。
附图说明
[0022] 图1为基于深度学习和视惯融合的室内机器人运动估计方法的
流程图。
[0023] 图2为GCN网络示意图。
[0024] 图3为图1中预积分的流程图。
具体实施方式
[0025] 为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实例，对本发明进一步详细说明。
[0026] 一种基于深度学习和视惯融合的室内机器人运动估计方法，如图1所示，其具体包括步骤如下：
[0027] 步骤1、利用单目相机获取机器人的视觉数据(即视觉图像)。
[0028] 步骤2、利用三维投影几何训练模型对视觉图像进行特征提取与跟踪，同时进行了关键帧的选取。
[0029] (1)利用CNN和LSTM组成一个深度学习网络，即三维投影几何训练模型，它是一个三维投影几何训练的网络，用于生成关键点和描述子。
[0030] 三维投影几何训练模型如图2所示，上面部分是CNN(卷积神经网络)，它是一个50层的CNN，表示为resnet-50，它以密集的方式提取特征。它包括用于多尺度特征表示的全卷积部分和用于细化特征细节的反卷积部分。下半部分是一个浅双向递归结构，用于预测两幅图像中关键点的位置。实际网络的
隐藏层比图中的要多。训练的目标函数分别是三重损失函数和交叉熵。
[0031] (2)三维投影几何训练模型中，resnet-50的权重是通过ImageNet对象分类数据集预先训练的，该数据集有120万个标记图像。
[0032] 训练网络时，首先提取第一幅图像中的关键点，并使用已知的帧到帧转换，将其转换到下一帧，把这些关键点作为真值。通过用刚体变换来计算下一帧的关键点，而不是通过在第二帧中的外观来找到点的对应关系，避免了训练两幅图像中具有相似外观但实际上不是几何点的点，如不同深度的交叉边缘形成的线。
[0033] 当图像被送入resnet-50时，通过
池化操作，减少维度。之后为了将特征恢复到原来的大小，利用反卷积对特征进行上
采样。
[0034] (3)将CNN的输出反馈到LSTM中，LSTM是一个循环神经网络，利用时间信息和空间信息同时预测两个输入图像的关键点位置。定义如下：
[0035]
[0036]
[0037]
[0038]
[0039] 其中 表示卷积操作，f是在循环网络之前最后一层的特征信息，h和v是用于存储信息，属于隐藏层。o是给定位置的输出，其中用一个sigmoid函数来测量是生成关键点的概率，sigmoid函数也叫Logistic函数，用于隐藏层神经元输出，它可以将一个实数映射到(0,1)的区间，可以用来做二分类。Wi2h和Wh2o是学习的权重，分别表示
输入层到隐藏层和隐藏层到
输出层的权重，在本
实施例，分别取为1和0.9。
[0040] (4)采用FCN进行密集特征提取，采用双向网络进行时间关键点预测。在特征提取部分，为了加速匹配过程并与ORB进行特征匹配，网络的特征训练为二进制，为了对这些特性进行二值化，在最终输出的顶部添加了一个二进制激活层。为方便表示，把训练的网络简称为GCN。二进制激活层定义为：
[0041] 前向：
[0042] 反向：
[0043] 其中，x是图像中的坐标，f(·)是给定位置的
特征向量。Lmetric是度量学习的损失函数，度量学习将输入样本映射到一个特征空间，在该空间中，相似样本越近，不同样本越远。1是指示函数。
像素度量学习用于以最近邻方式训练描述子。二值化特征的三重损失如下：
[0044]
[0045]
[0046] 其中，m是截断距离边距，d等于32字节描述符的平方汉明距离。
[0047] (xi,xi,+)是使用地面真值相机从以下
训练数据中获取的匹
配对：
[0048] xi,+＝π-1(Rgt·π(xi,di)+tgt)
[0049] 其中，R是旋转矩阵，t是平移向量。
[0050] (xi,xi,-)是负样本挖掘的非匹配对。
[0051] 掩膜上关键点检测的训练与原始网络相同，可以使用以下两个连续帧计算：
[0052] Lmask＝Lce(o1,x)+Lce(o2,x+)
[0053]
[0054] 其中Lce为交叉熵函数，c为图像帧。
[0055] 在训练过程中，三重损失和交叉熵都被加权以实现标准化。用于训练的自适应
梯度下降法的学习率从10-4开始，每过40个周期就减半，总共有100个训练周期。GCN网络的权重是随机初始化的，将平方汉明距离映射到L2单位球体，以执行快速最近邻匹配，三重损失的裕度设置为1。加权交叉熵的权重α1，α2分别设为0.1和1.0。
[0056] 步骤3、利用惯性测量单元(IMU)获得机器人的IMU数据(即运动信息)。
[0057] 步骤4、采用预积分的方法对IMU据进行处理，得到预积分值，流程图如图3所示。
[0058] 对两帧视觉图像之间的所有IMU进行积分处理，计算定义的预积分变量，从当前时刻的状态量，根据IMU数据，得到下一时刻的状态量。具体包括：
[0059] 对惯性测量过程建模：
[0060]
[0061]
[0062] 其中B表示惯性
坐标系，W表示世界坐标系。
[0063] 惯性单元的运动模型：
[0064]
[0065]
[0066]
[0067] R表示角度，V表示速度，P表示位置。
[0068] 在t+Δt时间内积分，离散化，结合惯性测量过程和图像的第i和第j两帧数据，得到下式：
[0069]
[0070]
[0071]
[0072] 其中η为噪声项，b为偏置项，d表示离散状态，a表示加速度，g表示重力。
[0073] 将上式中只与惯性测量有关的变量汇聚一起，把IMU信息转为跟世界坐标系下关键帧状态无关的帧间约束，将参考帧从世界坐标系转为重力坐标系下，获得预积分结果；
[0074] 定义预积分变量ΔRij，Δvij，Δpij如下：
[0075]
[0076]
[0077]
[0078] 在重力坐标系下，IMU的预积分值可以由传感器直接获得。在获得预积分变量后，通过下式计算获得预积分的估计位姿：
[0079] Rj＝RiΔRij
[0080] vj＝vi-wgΔtij+RiΔvij
[0081]
[0082] 所述进行误差传播更新，获得两帧视觉图像之间的运动约束关系，具体包括：
[0083] 将噪声项从预积分理想值中分离出来：
[0084]
[0085]
[0086]
[0087] 其中， δvij，δpij是其测量噪声。
[0088] 通过进一步计算，得到三个噪声项的分布形式：
[0089]
[0090]
[0091]
[0092] 在本发明中，IMU预积分的作用是计算出IMU数据的观测值(就是IMU预积分值)以及残差的协方差矩阵和雅各比矩阵，这三个量为后面的视觉惯性校准部分提供初值以及VIO优化部分提供IMU的约束关系。
[0093] 步骤5、判断系统是否初始化成功，在系统中加入一个布尔类型判断的入口，当求解出绝对尺度，陀螺仪偏置，加速度偏置，重力加速度和每个IMU时刻的速度时，则判断出系统初始化成功，接着执行步骤7，如果未初始化成功，执行步骤4。
[0094] 步骤6、对系统进行初始化。初始化的过程分为纯视觉初始化和视觉惯导联合初始化。
[0095] (1)纯视觉初始化
[0096] 首先构建一个滑动窗口，设置相对固定的窗口，大小设置为10，每当进来一个新的图像帧的时候，首先判断它与窗口里面存储的之前的一帧的的相对位移，用特征点的相对位移表示。窗口中只包含视觉数据帧。接着利用对极几何模型求解单目相机的相对变换，然后三角化得到相应的3D点(观测的点到物体的点)坐标，有这些3D点和滑动窗口中其他的帧的2D点就可以进行pnp(Perspective-n-Point，透视N点)求解获得滑动窗口中的所有的位姿和特征点3D坐标，完成纯视觉初始化。
[0097] (2)视觉惯导联合初始化
[0098] 根据滑动窗口中连续图像的相对旋转算出相机和IMU间的外参，再根据纯视觉的初始化得到的各帧图像的相对旋转来计算出陀螺仪的偏置。利用前一帧对后一帧位置、速度的预测与当前值的误差，实现速度、重力、尺度的对齐，最终完成视觉惯导的联合初始化。
[0099] 步骤7、初始化过程完成，所有这些度量值都将被输入到一个紧耦合的视觉惯导里程计中，完成基于滑动窗口的视觉惯导里程计的优化。
[0100] 为了平衡计算量，依旧通过设置相对固定的窗口大小来实现对待优化参数的数量进行限制，窗口大小设置为10，每当进来一个新的图像帧的时候，首先判断它与窗口里面存储的之前的一帧的的相对位移，用特征点的相对位移表示。优化由之前步骤得到的滑动窗口中的各帧的位置，姿态，速度，偏移，相机的外参，以及每一个特征点在它被观察到的第一帧的深度。拟合前后帧之间的IMU的预积分值，前后帧之间的偏移的差的约束，每一个特征点的重投影位置这三部分，最终完成基于滑动窗口的视觉惯导里程计的优化。
[0101] 滑动窗口法将系统的计算量控制在一定限度内,进而保证系统运行的实时性,将需要进行优化的视觉和惯性测量值放入滑动窗口中,而不需要进行优化的则剔除出滑动窗口,保证视觉惯性里程计系统的总计算量恒定,在每次联合位姿优化结束后,都需要根据定位结果进行滑动窗口调整,使得获取新的数据时仍能进行优化，以相机运动的位姿速度和漂移作为状态变量，利用数据图像初步估计和惯性运动约束对系统状态进行优化。定义视觉惯导里程计系统的状态向量如下：
[0102] x＝[p,v,q,ba,bg]
[0103] 系统的状态向量为15维向量，包含相机运动的平移、运动、旋转角度、惯性加速度零漂和
角速度零漂，对所述状态向量进行图优化，定义如下优化函数：
[0104]
[0105] 这是对视觉观测方程和IMU运动方程同时进行优化，最终实现紧耦合的视觉惯导里程计(VIO)。
[0106] 步骤8、对关键帧进行选取，即：确定当前帧是否为关键帧。首先根据当前帧的位姿估计结果,判断当前帧是否为关键帧,关键帧的选取依据为当前帧与上一帧关键帧运动距离超过设定阈值大小：若当前帧不为关键帧,说明系统与上一关键帧运动距离较短,则将当前帧数据直接从滑动窗口中剔除；若当前帧为关键帧,则将滑动窗口中时间最早的一帧从滑动窗口中剔除,将当前帧作为新的关键帧。
[0107] 在滑动窗口中，图像插入
频率过高会导致信息
冗余度快速增加，会消耗更多的计算资源，在所述滑动窗口中均采用关键帧策略，选取依据为当前帧与上一帧关键帧运动距离超过设定阈值，如果特征点的平均相对位移大于某一个阈值，就认为是一个新的关键帧，本实施列中的阈值设置为10。就把这个新的关键帧放入窗口里面，然后其它的关键帧都往前移动。第一个位置的关键帧被移出去，边缘化。如果不是新的关键帧，就把之前的第10帧边缘化掉，这个新的一帧替换成为第10帧。
[0108] 步骤9、回环检测。每3个关键帧，提取一个到关键帧数据库里。关键帧数据库里面的每一帧都跟之前的进行检测，看是否有回环。
[0109] 回环检测模
块能够有效的消除系统的累积误差，优化模块结束后加入一个用来判断回环的布尔类型。利用神经网络训练出的新的字典，字典中的不同的单词对应着不同的特征，利用关键帧与数据库之间的比较，来判断是否回到过历史位置。相似度较高的图像能获得较高的评分，本实施列，评分超过0.6就认为出现了回环，如果低于评分将一直持续判断。
[0110] 本发明利用深度学习的方法创建新的词袋模型进行检测。在关键帧上，用ORB找出的新的特征点和它在之前被跟踪到的特征点，提取描述子，与历史描述子匹配。这帧的描述子以及对应的特征点，与历史上的描述子以及对应的特征点进行匹配，得到匹配上的特征点。如果有回环，等待对应帧从窗口里滑出。
[0111] 通过观察到的关键帧，检测机器人是否回环。利用词袋模型的思想，本发明用标准的最近邻搜索替换了匹配算法，通过之前深度学习的训练图像的数据集，计算它们来重新生成词袋词汇表以适应设计网络特征描述符。向网络输入一个灰色图像帧，输出两个矩阵：1×320×240关键点矩阵和256×320×240特征描述矩阵。关键点掩码被阈值化，以获得一组关键点位置、它们的
置信度以及它们相应的256位特征描述符。采用非最大抑制，网格尺寸为8×8。由于无法知道检测到的特征的方向，将角度设置为零。
[0112] 通过对图像数据的训练，生成字典之后，根据给定的任意的图像特征，然后在字典中逐层查找，找出与之对应的单词，判断是否回环。
[0113] 下面通过具体数据对本发明的性能进行说明。
[0114] 表1为本发明的特征提取方法与传统的特征提取方法的误差对比：
[0115] 表1
[0116]
[0117]
[0118] 通过表1可以看出，本发明的特征提取方法相对于传统特征提取方法误差更小。
[0119] 表2为本发明的最终运动估计结果与经典ORB-SLAM2方法的最终运动估计结果的误差对比：
[0120] 表2
[0121]Dataset GCN ORBSLAM2
fr1_floor 0.038m 0.036m
fr1_desk 0.029m 0.016m
fr1_360 0.069m 0.213m
fr3_long_office 0.040m 0.010m
fr3_large_cabinet 0.073m -
fr3_nst 0.020m 0.019m
fr3_nnf 0.064m -
[0122] 通过表2可以看出，本发明方法也具有良好的可靠性，在某些场景下比传统的ORBSLAM2具有更高的精度。
[0123] 需要说明的是，尽管以上本发明所述的实施例是说明性的，但这并非是对本发明的限制，因此本发明并不局限于上述具体实施方式中。在不脱离本发明原理的情况下，凡是本领域技术人员在本发明的启示下获得的其它实施方式，均视为在本发明的保护之内。
