CN112556719B - 一种基于cnn-ekf的视觉惯性里程计实现方法 - Google Patents

Abstract

本发明涉及一种基于CNN‑EKF的视觉惯性里程计实现方法，先通过卷积神经网络对图像信息进行特征提取，将神经网络输出的相对位姿与惯性传感器的位姿采用基于拓展卡尔曼算法进行融合，最终得到估计出的相对位姿后转换为原始的轨迹。在保障视觉惯性里程计系统的功能前提下，充分发挥深度学习算法的良好优势，并结合多传感器信息，通过卡尔曼滤波算法将神经网络的位姿信息和惯性传感器的位姿信息进行融合，弥补惯性传感器在采集图像信息和惯性位姿信息中存在的不足，进而提升视觉里程计系统鲁棒性和泛化能力。

Description

一种基于CNN-EKF的视觉惯性里程计实现方法

技术领域

本发明涉及无人机自主巡检中基于视觉的定位与导航领域，更具体地，涉及一种基于CNN-EKF的视觉惯性里程计实现方法。

背景技术

近年来，无人机自主导航、自动驾驶、虚拟现实(VR)以及增强现实(AR)等领域蓬勃发展，极大的丰富了人们的生产生活需求。在以上领域中，目前主流的定位导航方法主要分为两类：1)绝对位置坐标定位法：即北斗导航系统、全球定位系统(Global PositionSystem，GPS)等方法。这种定位方法可获取绝对位置，且发展较为成熟，但定位效果直接受信号质量影响，天气条件及障碍物遮挡都可能造成信号接收不成功而无法有效定位，因此也无法用于室内、隧道等场景；2)相对位置坐标定位法：即主体运动时根据获取的传感器信息计算相对位姿变化的技术。该技术有着广泛的应用需求，如家庭服务机器人、VR(VirtualReality，虚拟现实)、扫地机器人、室内自动导引运输车等。该技术的核心为信息获取、信息处理的精确度以及后续误差的消除。

视觉SLAM(VSLAM)是一种自主式导航定位手段，它利用自身视觉传感器采集图像序列，可以获取类似于人眼观察到的丰富的外部场景信息。视觉SLAM系统可以在未知环境先验信息的前提下，根据一系列图像序列通过多视图几何以及扩展卡尔曼滤波或非线性优化的方式来估计相机自身的六自由度运动状态，同时根据工程需要完成构建场景地图的任务。通常情况下视觉SLAM系统包括了视觉里程计(VO)系统。

公开号为“CN110490928A”，公开日为2019年11月22日的中国专利申请文件公开了一种基于深度神经网络的相机姿态估计方法，1)构建相机姿态估计网络；2)构建无监督训练方案，利用估计的深度图、帧间相对位姿及光流从输入的前后帧图像中分别重建出相应的图像，利用输入图像和重建图像之间的光度误差构建网络的损失函数；3)位姿估计模块与光流估计模块共享特征提取部分，加强特征对于帧间的几何联系；4)输入待训练单视点视频，输出对应帧间相对位姿，通过最优化手段降低损失函数来训练模型，以至网络达到收敛。本发明所提出的模型通过输入单视点视频序列输出对应序列的相机位姿，训练过程以一种端到端无监督的方式进行，通过光流与位姿联合训练，提高位姿估计性能。

但是上述的方案中，将该相机姿态估计方法用于视觉里程计系统，会对图像信息高度依赖，在场景光照不足、纹理缺失或者相机快速运动时，定位效果不佳，误差较大，导致里程计系统的鲁棒性和定位准确率低。

发明内容

本发明为克服上述现有技术中里程计系统的鲁棒性和定位准确率低的问题，提供一种基于CNN-EKF的视觉惯性里程计实现方法，在保障视觉惯性里程计系统的功能前提下，充分发挥深度学习算法的良好优势，并结合多传感器信息，弥补传感器在采集图像信息和惯性位姿信息中存在的不足，进而提升视觉里程计系统鲁棒性和泛化能力。

为解决上述技术问题，本发明采用的技术方案是：一种基于CNN-EKF的视觉惯性里程计实现方法，包括如下步骤：

步骤一：图像序列中的相邻两帧输入卷积神经网络之后，由卷积神经网络进行提取图像特征；

步骤二：在不断的迭代优化所述卷积神经网络后，由全连接层输出估计的相对位姿；

步骤三：将全连接层输出的相对位姿与惯性传感器的位姿采用基于拓展卡尔曼算法(EKF)进行融合，最终得到估计出的相对位姿；

步骤四：对步骤三中的估计出的相对位姿经过矩阵变换后恢复出原始的轨迹。

优选的，在所述步骤一中，在图像输入卷积神经网络之前，先对不同图像对应的实际绝对位姿数据进行预处理，获得相对位姿；相对位姿数据表示物体沿着X，Y，Z轴的移动距离，旋转角度表示物体绕X，Y，Z轴旋转的角度，转角可由四元数，欧拉角，旋转矩阵表示；通过相对位姿进行姿态结算得到每张图像的绝对姿态，绝对姿态的生成标签表示为：

P＝[x,y,z,ψ,χ,γ]

式中：x、y、z分别表示为物体在X，Y，Z轴方向的移动距离；后三个参数表示物体在X，Y，Z轴方向的转动角度。

生成单张图片的绝对姿态可以为后面的两张图片序列的相对姿态提供数据标签，相对姿态可以通过姿态解算可以得到每张图片的绝对姿态，从而恢复出原始位姿的轨迹图，并与原始的真实数据值比较计算出误差并优化网络。

优选的，所述相对位姿采用欧拉角表示：

ΔP＝[Δx,Δy,Δz,Δψ,Δχ,Δγ]

式中，ΔP中前三个物理量分别表示物体在t时刻至t+1时刻沿X，Y，Z轴的位移差；后三个物理量表示绕X，Y，Z轴旋转的角度差。

优选的，所述卷积神经网络由十个卷积层、激活函数层和Max-pooling层构成，将原始输入数据映射到抽象特征空间。

优选的，每个卷积层后面连接了一个激活函数ReLU，其中有六层卷积步长为2，得到的特征图谱相应的缩减为原来的两倍；卷积核的大小从7*7，逐渐减小为5*5，最终变为3*3。先从整体宏观提取特征，随着卷积核的减小，有利于局部特征的获取。随着卷积滤波器的层数不断增加，网络的权重W也在增大，使得输出的图像深度也随之增加，即获得特征图谱的数目增加，以增加提取的抽象化特征。

优选的，卷积神经网络的卷积层后面设计了三层全连接层，隐藏单元数依次为4096、1024、128，每层全连接层后面都加了一个非线性激活函数Relu；得到128维的特征后，进行最后一次特征浓缩，得到6维特征，表示输入的两张图片之间的相对姿态：

[Δx,Δy,Δz,Δψ,Δχ,Δγ]

式中：ΔP中前三个物理量分别表示物体在t时刻至t+1时刻沿X，Y，Z轴的位移差；后三个物理量表示绕X，Y，Z轴旋转的角度差；

最后由估计得到的6维姿态与原始真是数据值进行误差计算并优化网络参数。

优选的，通过网络误差优化函数对卷积神经网络进行迭代优化，函数具体为：

式中：P_k和Φ_k是第k对图片序列的真实相对位姿；

和

是第k对图片序列的预测相对位姿；β是相对位姿的尺度因子；

表示为二范数。

上述误差函数的获得过程如下：

假设给定的第k组相邻的图片序列为X_k＝(X_k,X_k+1)，从概率的角度出发，这两张连续时间序列的RGB图片的相对姿态Y_k的概率为：

P(Y_k|X_k)

要求得此概率的最大值的关键是求解出最优化网络参数

的参数表示如下公式所示：

最终，网络误差优化函数使用均方误差函数(MSE)，最终可得上述的优化函数。

优选的，在所述步骤三中，基于拓展卡尔曼滤波器算法来融合连接层输出的相对位姿和惯性传感器的位姿信息，具体如下：

由于状态预测由IMU状态函数进行驱动测量，核心状态函数如下：

式中，

代表由惯性传感器获得的相对位姿、速度和角速度；b_a和b_w分别为惯性传感器加速度和陀螺仪的偏置。

优选的，通过拓展卡尔曼滤波器更新系统，定义核心状态函数的各个元素的误差状态向量为：

公式中

代表估计量

和真值x之间的差值，

也可以用四元数定义为

除此之外，每个传感器还会增加一个数字辅助状态将测量的量与原始状态进行比较。因此，完整的EKF状态由原始状态

和一系列附加状态

定义为如下公式：

在视觉惯性里程计系统中要估计的量是尺度λ和视觉系统中位姿的漂移

和姿态角度

世界坐标系以及相机—IMU的旋转

由于在自主导航期间不太可能发生重大变化，因此本发明中不考虑相机到IMU的平移偏置

获得最终的具体状态函数方程如下：

式中：Δλ为系统的尺度差值；

为系统中位姿漂移的差值。

优选的，建立状态函数后，融合位姿更新过程为：

S1：通过相对测量值，可以关联的过去状态：

S2：建立状态

的完整协方差矩阵：

式中，

代表线性化系统动态矩阵的级联，存储状态转移矩阵于F_k；用p_k|k表示进行相对位姿计算的积累过程；

所有在时间跨度内到达其他测量值通过相对测量来考虑和改进F_k的各个线性化点，残差r_k+m和相对测量的协方差

可以由以下方程表示：

式中，R_r是来自的相对姿势的协方差，并且

包括两个对应的测量雅可比行列式；

卡尔曼增益的具体计算为：

S3:校正状态和给定残差r_k+m，在时间t_k+m处的协方差：

给定更好的

估计值，可以重新应用相对测量之后到达的所有测量值，并使用新的线性化方法重新预测状态点。

与现有技术相比，本发明的有益效果是：

1、本发明选择卷积神经网络提取相邻帧图像的全部信息，恢复出原始位姿，同时有利于迁移到不同环境中，提升了系统的鲁棒性的同时并能获得良好的精度。

2、本发明选择端到端的训练学习的系统框架，相比于传统的视觉里程计算法，省略各个模块中微调参数的过程，大大提高视觉里程计系统的效率。

附图说明

图1是本发明的一种基于CNN-EKF的视觉惯性里程计实现方法的流程图；

图2是本发明的卷积神经网络EfficientNetB0的网络框架图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”“长”“短”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

下面通过具体实施例，并结合附图，对本发明的技术方案作进一步的具体描述：

实施例

如图1-2所示为一种基于CNN-EKF的视觉惯性里程计实现方法的实施例，包括如下步骤：

包括如下步骤：

步骤一：图像序列中的相邻两帧输入卷积神经网络之后，由卷积神经网络进行提取图像特征；

步骤二：在不断的迭代优化所述卷积神经网络后，由全连接层输出估计的相对位姿；

步骤三：将全连接层输出的相对位姿与惯性传感器的位姿采用基于拓展卡尔曼算法进行融合，最终得到估计出的相对位姿；

步骤四：对步骤三中的估计出的相对位姿经过矩阵变换后恢复出原始的轨迹。

在本实施例中，进行步骤一前，在图像输入卷积神经网络之前，先对不同图像对应的实际绝对位姿数据进行预处理，获得相对位姿；相对位姿数据表示物体沿着X，Y，Z轴的移动距离，旋转角度表示物体绕X，Y，Z轴旋转的角度，转角可由四元数，欧拉角，旋转矩阵表示；通过相对位姿进行姿态结算得到每张图像的绝对姿态，绝对姿态的生成标签表示为：

P＝[x,y,z,ψ,χ,γ]

式中：x、y、z分别表示为物体在X，Y，Z轴方向的移动距离；后三个参数表示物体在X，Y，Z轴方向的转动角度。

生成单张图片的绝对姿态可以为后面的两张图片序列的相对姿态提供数据标签，相对姿态可以通过姿态解算可以得到每张图片的绝对姿态，从而恢复出原始位姿的轨迹图，并与原始的真实数据值比较计算出误差并优化网络。

为与绝对误差标签一致，相对位姿采用欧拉角表示：

ΔP＝[Δx,Δy,Δz,Δψ,Δχ,Δγ]

式中，ΔP中前三个物理量分别表示物体在t时刻至t+1时刻沿X，Y，Z轴的位移差；后三个物理量表示绕X，Y，Z轴旋转的角度差。

在本实施例中，所述卷积神经网络由十个卷积层、激活函数层和Max-pooling层构成，将原始输入数据映射到抽象特征空间。

具体的，每个卷积层后面连接了一个激活函数ReLU，其中有六层卷积步长为2，得到的特征图谱相应的缩减为原来的两倍；卷积核的大小从7*7，逐渐减小为5*5，最终变为3*3。先从整体宏观提取特征，随着卷积核的减小，有利于局部特征的获取。随着卷积滤波器的层数不断增加，网络的权重W也在增大，使得输出的图像深度也随之增加，即获得特征图谱的数目增加，以增加提取的抽象化特征。

其中，卷积神经网络的卷积层后面设计了三层全连接层，隐藏单元数依次为4096、1024、128，每层全连接层后面都加了一个非线性激活函数Relu；得到128维的特征后，进行最后一次特征浓缩，得到6维特征，表示输入的两张图片之间的相对姿态：

[Δx,Δy,Δz,Δψ,Δχ,Δγ]

式中：ΔP中前三个物理量分别表示物体在t时刻至t+1时刻沿X，Y，Z轴的位移差；后三个物理量表示绕X，Y，Z轴旋转的角度差；

最后由估计得到的6维姿态与原始真是数据值进行误差计算并优化网络参数。

在步骤二中，通过网络误差优化函数对卷积神经网络进行迭代优化，函数具体为：

式中：P_k和Φ_k是第k对图片序列的真实相对位姿；

和

是第k对图片序列的预测相对位姿；β是相对位姿的尺度因子；

表示为二范数。

上述误差函数的获得过程如下：

假设给定的第k组相邻的图片序列为X_k＝(X_k,X_k+1)，从概率的角度出发，这两张连续时间序列的RGB图片的相对姿态Y_k的概率为：

P(Y_k|X_k)

要求得此概率的最大值的关键是求解出最优化网络参数

的参数表示如下公式所示：

最终，网络误差优化函数使用均方误差函数(MSE)，最终可得上述的优化函数。

在步骤三中，基于拓展卡尔曼滤波器算法来融合连接层输出的相对位姿和惯性传感器的位姿信息，具体如下：

由于状态预测由IMU状态函数进行驱动测量，核心状态函数如下：

式中，

代表由惯性传感器获得的相对位姿、速度和角速度；b_a和b_w分别为惯性传感器加速度和陀螺仪的偏置。

具体的，通过拓展卡尔曼滤波器更新系统，定义核心状态函数的各个元素的误差状态向量为：

公式中

代表估计量

和真值x之间的差值，

也可以用四元数定义为

除此之外，每个传感器还会增加一个数字辅助状态将测量的量与原始状态进行比较。因此，完整的EKF状态由原始状态

和一系列附加状态

定义为如下公式：

在视觉惯性里程计系统中要估计的量是尺度λ和视觉系统中位姿的漂移

和姿态角度

世界坐标系以及相机—IMU的旋转

由于在自主导航期间不太可能发生重大变化，因此本发明中不考虑相机到IMU的平移偏置

获得最终的具体状态函数方程如下：

式中：λ为系统的尺度差值；

为系统中位姿漂移的差值。

进一步的，建立状态函数后，融合位姿更新过程为：

S1：通过相对测量值，可以关联的过去状态：

S2：建立状态

的完整协方差矩阵：

式中，

代表线性化系统动态矩阵的级联，存储状态转移矩阵于F_k；用p_k|k表示进行相对位姿计算的积累过程；

所有在时间跨度内到达其他测量值通过相对测量来考虑和改进F_k的各个线性化点，残留r_k+m和相对测量的协方差

可以由以下方程表示：

式中，R_r是来自的相对姿势的协方差，并且

包括两个对应的测量雅可比行列式；

卡尔曼增益的具体计算为：

S3:校正状态和给定残r_k+m，在时间t_k+m处的协方差：

给定更好的

估计值，可以重新应用相对测量之后到达的所有测量值，并使用新的线性化方法重新预测状态点。

还提供对本实施例方法的验证过程，包括以下步骤：

1.数据准备

本发明中提出关于解决视觉里程计的方案，在开源数据集上进行训练测试，能够基本完成视觉里程计相关功能。Euroc数据集包含两个场景：一个是苏黎世联邦理工学院的ETH机器大厅，该场景数据包含通过激光追踪精确到毫米级别定位的真值数据。第二个场景是Vicon环境下的普通房间。在本部分实验中本发明采用的是Euroc数据集的第一个场景：ETH机器大厅，该场景包含五个数据集序列(Machine Hall 01-05)。其中，Machine Hall 01(MH-01)和Machine Hall 02(MH-02)的数据集评估等级为简单模式；Machine Hall 03(MH-03)为中等模式；Machine Hall 04(MH-04)，Machine Hall 05(MH-05)为困难模式，并且每个数据集序列都提供了真值数据。

2.验证实验结果

本发明选择绝对位姿误差(APE，单位：米)作为最终的评价指标，误差数值越小说明效果越好，具体实验结果如表格1中所示。通过实验验证，本方案中提出的视觉里程计方案能够基本实现系统功能，并且采用本发明提出方法中有3条序列优于视觉方法。

表格1 不同序列的评估结果

本实施例的有益效果是：1、本发明选择卷积神经网络提取相邻帧图像的全部信息，恢复出原始位姿，同时有利于迁移到不同环境中，提升了系统的鲁棒性的同时并能获得良好的精度。2、本发明选择端到端的训练学习的系统框架，相比于传统的视觉里程计算法，省略各个模块中微调参数的过程，大大提高视觉里程计系统的效率。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (7)

1.一种基于CNN-EKF的视觉惯性里程计实现方法，其特征在于，包括如下步骤：

步骤一：图像序列中的相邻两帧输入卷积神经网络之后，由卷积神经网络进行提取图像特征；

步骤二：在不断的迭代优化所述卷积神经网络后，由全连接层输出相对位姿；

步骤三：将全连接层输出的相对位姿与惯性传感器的位姿采用基于拓展卡尔曼滤波器算法进行融合，最终得到估计出的相对位姿；

基于拓展卡尔曼滤波器算法来融合全连接层输出的相对位姿和惯性传感器的位姿信息，具体如下：

由于状态预测由IMU状态函数进行驱动测量，状态函数如下：

式中，

代表由惯性传感器获得的相对位姿、速度和角速度；b_a和b_w分别为惯性传感器加速度和陀螺仪的偏置；

通过拓展卡尔曼滤波器更新系统，定义拓展卡尔曼滤波器算法的具体状态函数如下：

式中：Δλ为系统的尺度差值；

为系统中位姿漂移的差值；

建立状态函数后，融合位姿更新过程为：

S1：通过相对测量值，可以关联的过去状态：

S2：建立状态

的完整协方差矩阵：

式中，

代表线性化系统动态矩阵的级联，存储状态转移矩阵于F_k；用p_k|k表示进行相对位姿计算的积累过程；

所有在时间跨度内到达其他测量值通过相对测量来考虑和改进F_k的各个线性化点，残差r_k+m和相对测量的协方差

可以由以下方程表示：

式中，R_r是来自相对位姿的协方差，并且

包括两个对应的测量雅可比行列式；

卡尔曼增益的具体计算为：

S3:校正状态和给定残差r_k+m，在时间t_k+m处的协方差：

给定更好的

估计值，可以重新应用相对测量之后到达的所有测量值，并使用新的线性化方法重新预测状态点；

步骤四：对步骤三中的估计出的相对位姿经过矩阵变换后恢复出原始的轨迹。

2.根据权利要求1所述基于CNN-EKF的视觉惯性里程计实现方法，其特征在于，在所述步骤一中，在图像输入卷积神经网络之前，先对不同图像对应的实际绝对位姿数据进行预处理，获得相对位姿；通过相对位姿进行姿态结算得到每张图像的绝对姿态，绝对姿态的生成标签表示为：

P＝[x,y,z,ψ,χ,γ]

式中：x、y、z分别表示为物体在X，Y，Z轴方向的移动距离；后三个参数表示物体在X，Y，Z轴方向的转动角度。

3.根据权利要求2所述基于CNN-EKF的视觉惯性里程计实现方法，其特征在于，所述相对位姿采用欧拉角表示：

ΔP＝[Δx,Δy,Δz,Δψ,Δχ,Δγ]

式中，ΔP中前三个物理量分别表示物体在t时刻至t+1时刻沿X，Y，Z轴的位移差；后三个物理量表示绕X，Y，Z轴旋转的角度差。

4.根据权利要求2所述基于CNN-EKF的视觉惯性里程计实现方法，其特征在于，所述卷积神经网络由十个卷积层、激活函数层和Max-pooling层构成，将原始输入数据映射到抽象特征空间。

5.根据权利要求4所述基于CNN-EKF的视觉惯性里程计实现方法，其特征在于，每个卷积层后面连接了一个激活函数ReLU，其中有六层卷积步长为2，得到的特征图谱相应的缩减为原来的两倍；卷积核的大小从7*7，逐渐减小为5*5，最终变为3*3。

6.根据权利要求4所述基于CNN-EKF的视觉惯性里程计实现方法，其特征在于，卷积神经网络的卷积层后面设计了三层全连接层，隐藏单元数依次为4096、1024、128，每层全连接层后面都加了一个非线性激活函数Relu；得到128维的特征后，进行最后一次特征浓缩，得到6维特征，表示输入的两张图片之间的相对位姿：

ΔP＝[Δx,Δy,Δz,Δψ,Δχ,Δγ]

式中：欧拉角ΔP中前三个物理量分别表示物体在t时刻至t+1时刻沿X，Y，Z轴的位移差；后三个物理量表示绕X，Y，Z轴旋转的角度差；

最后由估计得到的6维姿态与原始真实数据值进行误差计算并优化网络参数。

7.根据权利要求6所述基于CNN-EKF的视觉惯性里程计实现方法，其特征在于，通过网络误差优化函数对卷积神经网络进行迭代优化，函数具体为：

式中：P_k和Φ_k是第k对图片序列的真实相对位姿；

和

是第k对图片序列的预测相对位姿；β是相对位姿的尺度因子；

表示为二范数。

Images