CN115046545A - 一种深度网络与滤波结合的定位方法 - Google Patents

一种深度网络与滤波结合的定位方法 Download PDF

Info

Publication number
CN115046545A
CN115046545A CN202210348112.0A CN202210348112A CN115046545A CN 115046545 A CN115046545 A CN 115046545A CN 202210348112 A CN202210348112 A CN 202210348112A CN 115046545 A CN115046545 A CN 115046545A
Authority
CN
China
Prior art keywords
time
network
state
axis
frame
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210348112.0A
Other languages
English (en)
Inventor
徐博
戴秋阳
赵玉新
李浩泽
吴磊
周广涛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202210348112.0A priority Critical patent/CN115046545A/zh
Publication of CN115046545A publication Critical patent/CN115046545A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Image Processing (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种深度网络与滤波结合的定位方法，步骤一：建议视觉惯性里程计运动学模型；步骤二：将时序上相邻k+1帧图像视频序列输入堆叠网络，组合相邻帧的图像得到k组图像对；步骤三：k组图像对输入CNN‑LSTM‑ATT‑VO网络后输出6‑dof的位姿估计Δx,Δy,Δz,
Figure DDA0003571648430000011
Δλ,Δψ，其中Δx,Δy,Δz分别为x轴、y轴和z轴平移量，
Figure DDA0003571648430000012
Δλ,Δψ分别为绕x轴、y轴和z轴旋转量；所述网络依次包括卷积神经网络、CBAM模块、LSTM网络和全连接层；步骤四：通过基于李群的以机器人为中心的Kalman滤波算法进行视觉惯性里程计的融合定位。本发明提高了***的鲁棒性并提高了协同定位精度，避免了传统EKF算法的不一致性。

Description

一种深度网络与滤波结合的定位方法
技术领域
本发明属于视觉惯性传感器VIO导航领域，涉及一种深度网络与滤波结合的定位方法， 特别是一种基于视觉惯性里程计的深度网络与滤波结合的定位方法。
背景技术
传统的视觉惯性融合框架中的VO位姿估计部分基本上都是手工数学建模并根据时间情 况进行参数调节，在很多复杂情况尤其是快速运动以及外界图像信息找不到特征点时，容易 在特征匹配环节就出现故障。经过深度学习神经网络训练的VO具有参数学习和非线性模型 拟合的能力，本质上就是根据输入的场景图像与导航参数之间的关系进行建模，经过数据的 训练来优化整个网络模型的参数量。同时由于IMU信息与图像信息的复杂程度相比属于低维 信息，因此利用基于滤波的松耦合方法将深度学习的VO结合IMU的经典框架进行组合导航， 不仅能减少状态向量的维度，同时也能增强***的鲁棒性。通过研究基于深度学习的VO通 过端到端学习方法在相邻帧图像之间完成位姿估计。在考虑视觉惯性融合方面传统滤波算法 更具有实际物理意义，因此将深度学习的VO估计的位姿信息与经过物理建模的IMU模型通 过经典的传统滤波算法融合并加以改进使***整体框架具有将两全其美的优势结合在一起的 潜力。
发明内容
针对上述现有技术，本发明要解决的技术问题是提供一种基于视觉惯性里程计的深度网 络与滤波结合的定位方法，采用基于李群的以机器人为中心的滤波算法(R-EKF)，避免了传 统EKF算法的不一致性。通过建立***整体的状态模型，并接受来自深度网络CNN-LSTM-ATT-VO网络的量测信息完成量测更新。
为解决上述技术问题，本发明的一种深度网络与滤波结合的定位方法，包括：
步骤一：建议视觉惯性里程计运动学模型；
步骤二：将时序上相邻k+1帧图像视频序列输入堆叠网络，组合相邻帧的图像得到k组 图像对；
步骤三：k组图像对输入CNN-LSTM-ATT-VO网络后输出6-dof的位姿估计 △x,△y,△z,
Figure BDA0003571648410000011
△λ,△ψ，其中△x,△y,△z分别为x轴、y轴和z轴平移量，
Figure BDA0003571648410000012
△λ,△ψ分别为绕 x轴、y轴和z轴旋转量；所述网络依次包括卷积神经网络、CBAM模块、LSTM网络和全连 接层；
步骤四：通过基于李群的以机器人为中心的Kalman滤波算法进行视觉惯性里程计的融 合定位。
进一步的，步骤一具体为：
IMU***状态方程为：
Figure BDA0003571648410000021
Figure BDA0003571648410000022
其中，am为测量到的加速度计值，ωm为陀螺仪测量值，是基于载体B系在τ时刻定义的，分别受到噪声na和nω、随机游走ba和bω的影响；加速度计和陀螺仪上的随机噪声以及随机游走在偏差上的导数为零均值高斯分，i为惯性坐标系，定义局部参考坐标系rk，rk是k 时刻的参考系，其中时间k,k+1,...对应于接收图像的时间点，载体坐标系记为B系，Bτ为 τ时刻的载体系，其中τ,τ+1,...对应于时间k和k+1之间接收IMU测量的时间；
Figure BDA0003571648410000023
为k时 刻的局部参考坐标系相对于τ时刻的载体系的旋转矩阵，
Figure BDA0003571648410000024
为重力矢量，
Figure BDA0003571648410000025
是τ时刻的载 体系相对于惯性系的加速度在载体系上的表示；
在惯性状态保持恒定的情况下，载体的标准状态在连续时间上演变为：
Figure BDA0003571648410000026
进一步的，卷积神经网络对图像对进行几何关系特征提取，输出表示相邻帧之间几何关 系的张量序列。
进一步的，CBAM模块对特征图进行操作为：
F′＝σ(MLP(AP(F))+MLP(MP(F)))⊙F
F″＝σ(f7×7[AP(F′),MP(F′)])⊙F′
Figure BDA0003571648410000027
表示输入特征图，
Figure BDA0003571648410000028
表示经过通道注意力后的一维特征图，
Figure BDA0003571648410000029
表示最终经过空间注意力机制后的特征图，σ表示sigmoid函数，MLP为FC层，MP表示最 大池化，AP表示平均池化。
进一步的，LSTM网络包含两层LSTM，每个LSTM中含有1024个隐藏单元，LSTM 中三个门默认激活函数为Relu函数。
进一步的，全连接层含有128个隐藏单元并在其后加入一个Relu激活函数，对接受特征 进行降维。
进一步的，步骤四具体为：
定义状态为：
Figure BDA00035716484100000210
分别为惯性状态
Figure BDA00035716484100000211
和载体状态
Figure BDA00035716484100000212
具体为：
Figure BDA00035716484100000213
Figure BDA00035716484100000214
对于惯性状态，
Figure BDA0003571648410000031
包括I系在参考系r系中k时刻中的姿态，其中
Figure BDA0003571648410000032
是I 系相对于参考系r系中k时刻的旋转矩阵，
Figure BDA0003571648410000033
是I系相对于参考系r系中k时刻的平移在参 考系中的表示，
Figure BDA0003571648410000034
是用k时刻r系表示的重力矢量；对于载体状态，
Figure BDA0003571648410000035
包括B系在k时刻的参考是r系中的姿势，
Figure BDA0003571648410000036
分别为载体系B系中的载体速 度、陀螺仪偏置和加速度计偏置；
标准状态的误差状态向量为：
Figure BDA0003571648410000037
旋转状态的扰动定义为：
Figure BDA0003571648410000038
其中C∈SO(3)、
Figure BDA0003571648410000039
(·)∧为反对称的操作符；
其中，两个误差状态量分别表示如下：
Figure BDA00035716484100000310
Figure BDA00035716484100000311
求得连续时间误差状态方程：
Figure BDA00035716484100000312
式中，F和G分别是经过线性化的***状态和误差的Jacobian矩阵，n为噪声且
Figure BDA00035716484100000313
将连续模型的***标准状态量与误差状态进行离散化得到，
Figure BDA00035716484100000314
Figure BDA00035716484100000315
Figure BDA00035716484100000316
其中，
Figure BDA00035716484100000317
为陀螺仪偏置，
Figure BDA00035716484100000318
为加速度偏置，
Figure BDA00035716484100000319
是B系相对于参考系r系中k时刻的旋转矩阵，
Figure BDA00035716484100000320
为k时刻B系相对于I系的速度在k时刻的参考系r系中的表示，△t＝tk+1-tk，小标s表示为定值；
将τ到τ+1时刻的状态转移矩阵Φτ+1,τ进行一阶表达:
Figure BDA00035716484100000321
式中,δt＝tτ+1-tτ；
求取中间状态的协方差矩阵为：
Figure BDA00035716484100000322
式中，将噪声假设加速计和陀螺仪上的随机噪声，以及偏置上的随机游走导数被假定为 零均值高斯白噪声Q，具体为：
Figure BDA0003571648410000041
观测量
Figure RE-GDA0003679388520000042
为深度学习CNN-LSTM-ATT-VO网络输出的相对位姿测量值，其对应的协方差矩阵R简化为对角阵，将量测信息的误差
Figure RE-GDA0003679388520000043
表示为：
Figure RE-GDA0003679388520000044
其中，
Figure RE-GDA0003679388520000045
分别表示K+1时刻的载体系相对于k时刻的参考系的旋转以及平移 的估计量；
应用一阶Baker-Campbell-Hausdorff(BCH)公式将对数映射近似为两个旋转向量的相减：
Figure RE-GDA0003679388520000046
通过上式的微分过程找到量测的jacobian矩阵
Figure BDA0003571648410000047
量测矩阵Hk+1的具体形式 为：
Figure BDA0003571648410000048
进行常规EKF的更新，得到误差
Figure BDA0003571648410000049
的估计值：
Figure BDA00035716484100000410
Figure BDA00035716484100000411
Figure BDA00035716484100000412
然后，将
Figure BDA00035716484100000413
注入到预测的标准状态中得到
Figure BDA00035716484100000414
最后，将所有状态的参考帧从帧rk向前移动到帧rk+1，得到参考帧k+1时刻的状态量
Figure BDA00035716484100000415
Figure BDA00035716484100000416
Figure BDA00035716484100000417
Figure BDA00035716484100000418
Figure BDA00035716484100000419
Figure BDA00035716484100000420
上式中，
Figure BDA00035716484100000421
为k+1时刻的载体系相对于k时刻的参考系的旋转，
Figure BDA00035716484100000422
以及
Figure BDA00035716484100000423
为 k+1时刻的载体系相对于参考系的旋转与平移；当经过误差补偿后得到当前时刻的姿态后， 进行下一次EKF迭代。s
本发明的有益效果：本发明涉及一种利用端到端的深度网络结合基于李群的robocentric Kalman滤波算法(R-EKF)完成视觉惯性之间的融合定位，提高***的鲁棒性并提高协同定 位精度的方法。本发明中提出了基于李群的以机器人为中心的滤波算法用于深度网络估计的 VO与IMU的位姿估计融合，算法通过已知的IMU的运动学模型状态信息，用深度VO网络 的估计作为量测更新。
1.与传统的视觉里程计算法相比，采用深度学习算法具有更强的鲁棒性和无纹理环境的适用 性。
2.在深度学习网络中加入注意力机制变为CNN-LSTM-ATT-VO网络，通过注意力机制优化 卷积层提取的几何关系特征图，提高定位精度。
3.通过深度学习结合基于传统滤波改进的融合算法，深度学习的VO估计的位姿信息与经过 物理建模的IMU模型通过经典的传统滤波算法融合并加以改进使***整体框架将两者算 法的优势结合在一起了。
附图说明
图1是以机器人为中心的vio参考系；
图2是CNN-LSTM-VO网络结构图；
图3是CNN-LSTM-ATT-VO网络结构图；
图4是CBAM处理过程
图5是从k到k+1的给定时间步长的***概述；
图6是KITTI序列10轨迹图；
图7是XYZ三方向上的位移图；
图8是绝对轨迹误差图；
图9是轨迹的均方根误差、中位数误差、平均误差、标准差对比。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述。
步骤一：视觉惯性里程计坐标系定义与运动学模型建立；
在本发明中设计的是视觉惯性里程计位姿估计器的目标是相对于惯性参考系来估计载体 的位姿信息。惯性坐标系此处为了方便定义为小写i，它是固定在地球上的。此处根据以机器 人为中心的滤波算法定义局部参考坐标系rk是k时刻的参考系，见图1，其中时间k；k+1；... 对应于接收图像的时间点。B系是载体坐标系，是时间τ的车辆框架，其中τ,τ+1,...对应于 时间k和k+1之间接收IMU测量的时间。上标和下标用于跟踪物理量的坐标框架。例如，
Figure BDA0003571648410000051
是B系相对于C系的速度，用A系表示，Cab是B系相对于A系的旋转矩阵。上标也被用来 区分不同类型的量.比如：
Figure BDA0003571648410000052
用于表示噪声量，
Figure BDA0003571648410000053
用于表示整个EKF预测过程中的传播量，
Figure BDA0003571648410000061
用于表示EKF更新后的修正量。
IMU***状态方程的建模根据需求表达如下式(1)，Bτ为τ时刻的载体系，
Figure BDA0003571648410000062
为k时刻的局部参考坐标系相对于τ时刻的载体系的旋转矩阵，
Figure BDA0003571648410000063
是τ时刻的载体系相对于惯性系 的加速度在载体系上的表示。测量到的加速度计值am和陀螺仪测量值ωm是基于载体B系在τ 时刻定义的，它们分别受到噪声na和nω、随机游走ba和bω的影响。加速度计的测量还结合了 一个固定的重力矢量
Figure BDA0003571648410000064
并通过旋转矩阵旋转到了B系上，假设加速度计和陀螺仪上的随机噪 声以及随机游走在偏差上的导数为零均值高斯分布。
Figure BDA0003571648410000065
Figure BDA0003571648410000066
在惯性状态保持恒定的情况下，载体的标准状态在连续时间上演变如式(2)所示：
Figure BDA0003571648410000067
步骤二：采用深度学习的方式搭建基于视觉里程计的VO网络进行位姿估计；
本发明根据目前现有的卷积网络基础先设计了一种CNN-LSTM-VO网络用于提取图像之 间的几何特征关系。针对VO输入的一系列视频序列的信息进行位姿估计，也就是说不再是 仅仅针对两张图片之间进行相对位姿估计，而是要对含时间顺序的多帧图像之间的位置姿态 进行相对位姿估计的持续输出。网络结构如图2所示，输入的为时序上相邻帧的图像，经过 堆叠网络进入CNN-LSTM-VO网络，其中为了适应网络结构事先对输入图像进行预处理，由 于不同场景下的图像尺寸不一致为了更好的调节网络结构统一调整图像尺寸大小为1280× 384。预处理后的图像由卷积网络中的卷积部分提取相邻图像帧之间的运动关系特征，假设输 入的视频序列长度为k+1，经过堆叠之后组合出相邻帧的图像得到k组图像对，将其经过卷 积网络进行特征提取，得到表示相邻帧之间几何关系的张量，并将张量序列输入两层LSTM 网络通过一个FC层来稳固特征并达到降维的目的。最后输出一个6-DOF的相对位姿估计。
(1)卷积网络
为了设计出更有效的CNN架构来学习相邻帧图像之间的几何关系特征，本发明借鉴于 Dosoviskiy提出的Flownet光流网络结构，同时也参考了DeepVO。网络结构中的CNN架构 部分，本发明设计CNN-VO时在卷积层预设十层的Conv层，每一层卷积的后面均有一个Relu 函数，同时随着网络深入，特征图谱也随之越发抽象，最后将卷积层提取的特征经过一个最 大池化后输入到全连接层得到位姿估计。
表1卷积层相关参数设置
Figure BDA0003571648410000068
Figure BDA0003571648410000071
(2)LSTM网络
本发明中考虑添加两层LSTM的主要目的是为了让整个深度网络能够通过数据自主的学 习连续多个相邻帧之间位姿的内在联系。LSTM层不仅能学习到历史数据中的状态(即前面 多个帧图像之间的位姿关系)，在对当前时刻位姿进行估计时达到传统算法上多帧图像之间的 几何约束效果；而且LSTM网络在长序列的数据学习过程中因为其门结构的特殊性，不会产 生梯度消失的问题，可以持续保持网络的学习效果。其中第一层LSTM用于接收CNN的输 出特征数据，第二层则用于接收第一层的输出并将信息传递给全连接层输出位姿估计的结果。 每个LSTM中含有1024个隐藏单元。根据本发明实际需要，将LSTM中三个门默认激活函 数从Sigmoid函数改成Relu函数用于更高级的位姿信息表示。
(3)全连接层
在本发明中全连接层的目的主要起强化几何关系特征信息并且对特征进行降维的作用， 从LSTM层输出的是高维的特征表示，因此需要将特征信息通过输出到全连接层进行降维操 作。本发明中在整个网络的最后只设计了一层FC层，其中含有128个隐藏单元并在其后加 入了一个Relu激活函数，最后将降维后的特征信息输出映射为6-dof的位姿估计表示。
△x,△y,△z,
Figure BDA0003571648410000072
△λ,△ψ
其中△x,△y,△z为平移量，
Figure BDA0003571648410000073
△λ,△ψ为旋转量。最后经过对输出的6-DOF的位姿信息 进行损失函数的设计与优化。
(4)损失函数设计与优化
根据输入时序视频信息，在本发明中，假设输入有n+1组图片，则网络输入的数据为 X＝(X1,X2,...,Xn+1)，而输出的位姿估计结果为Y＝(Y1,Y2,...,Yn)，因此在得知输入数据为视频 序列的情况下，求得位姿估计结果的概率为下式(3)：
P(Y|X)＝P(Y1,Y2,...,Yn|X1,X2,...,Xn+1) (3)
因此整个网络的需要解决的问题就是求出最优的参数w*要将上式的概率最大化，即如下 式(4)所示：
Figure BDA0003571648410000074
其中w为需要学习的参数，为了找到针对VO位姿估计问题的最优参数w*，将损失函数 设计为基于网络估计位姿与真实轨迹的均方误差MSE的最小化表达如式(5)所示：
Figure BDA0003571648410000081
其中
Figure BDA0003571648410000082
表示真实轨迹的位置与姿态，β为加权平衡参数用于平衡位置与姿态之间的 关系，||·||2为2范数。
为了更适应VO的深度网络位姿估计问题，从现行的方法中选取关注图像区域特征的空 间自注意力以及注重于学习特征通道来提高表达特征能力的SENet，以及同时关注空间和通 道特征学习的CBMA。经过一些论文的验证简单的利用resnet网络与两者结合分别进行注意 力标注，早期的论文研究中实验结果表明ResNet加入注意力之后识别率明显提升，且CBAM 比SENet的识别率在图像的特征识别上有更好的性能，因此本发明基于更好的提取相邻帧图 像特征的几何关系，决定在原有网络的基础上加入CBAM注意力机制以求得更精确的特征图 从而求出精度更高的位姿估计结果，网络结构如图4所示：
CBAM的主要原理就是对特征图进行如式(6)和式(7)所示的操作：
F′＝σ(MLP(AP(F))+MLP(MP(F)))⊙F (6)
F″＝σ(f7×7[AP(F′),MP(F′)])⊙F′ (7)
CBAM对于图像特征的处理过程。其中通道注意力机制为式(6)的处理过程,空间注意 力为式(7)的处理过程。
Figure BDA0003571648410000083
表示输入特征图，
Figure BDA0003571648410000084
表示经过通道注意力后的 一维特征图，
Figure BDA0003571648410000085
最终经过空间注意力机制后的特征图。σ表示sigmoid函数，MLP为 FC层，MP表示最大池化，AP表示平均池化，两者均是为了降低参数量而设计。
将CBAM机制加入CNN-LSTM-VO中，加入注意力机制后网络本身并没有在整体结构上有较大的变化，主要是对每一个conv层的输出进行特征优化从而使整个CNN网络预测出来的特征图更符合后续位姿估计的真实精度。
步骤三：通过基于李群的以机器人为中心的Kalman滤波算法(R-EKF算法)进行视觉 惯性里程计的融合定位并避免传统kalman不一致性问题。
通过融合视觉和惯性信息该估计器的核心是在提出的以机器人为中心的EKF公式，以机 器人为中心的EKF公式相对于局部参考系定义了其所有状态。这包括相对于最新参考状态作 为“特征”进行跟踪的惯性状态。在EKF预测期间，车辆状态会在本地参考系中传递。在随 着每个新图像的到来执行EKF更新之后，以机器人为中心的EKF在构图步骤中将参考帧从 一个时间步移动到下一个时间步。然后，该过程在新的参考帧中重复。预测，更新和组成的 三步过程如图5右侧所示。
以机器人为中心的基于李群的误差状态扩展卡尔曼滤波(R-EKF)公式采用了机器人为中 心的视觉惯性里程计(VIO)。与其他具有与地球参考惯性系相关的状态的常见公式不同， R-EKF的状态是与与机器人一起移动的参考坐标系相关的。这个公式允许我们融合测量，在 本质上是相对的，而不是使用在惯性系中参考的绝对测量。R-EKF也是一个误差状态EKF。 误差状态EKF将状态分为标准状态和误差状态。标准状态跟踪全局数据的动态。误差状态跟 踪由噪声引起的误差，它们被表述为标准状态的扰动。这意味着误差状态总是在0附近运行， 这减少了远离任何奇点的误差，并减少了线性化和离散化相关的误差。
定义状态如(8)所示，分为惯性状态
Figure BDA0003571648410000091
和载体状态
Figure BDA0003571648410000092
对于惯性状态
Figure BDA0003571648410000093
包 括I系在参考系r系中k时刻中的姿态，其中
Figure BDA0003571648410000094
是I系相对于参考系r系中k时刻的旋转矩 阵，
Figure BDA0003571648410000095
是I系相对于参考系r系中k时刻的平移在参考系中的表示，
Figure BDA0003571648410000096
是用k时刻r 系表示的重力矢量。对于载体状态，
Figure BDA0003571648410000097
包括B系在k时刻的参考是r系中的 姿势，
Figure BDA0003571648410000098
分别假设为载体系B系中的载体速度、陀螺仪偏置和加速度计偏置。
Figure BDA0003571648410000099
式中两个状态量分别表示如式(9)所示：
Figure BDA00035716484100000910
标准状态对应的误差状态向量如式(10)所示。误差状态定义为对标称状态的扰动，除旋 转外，其余均定义在欧几里得空间。旋转状态的扰动定义为式(11),其中C∈SO(3)、
Figure BDA00035716484100000911
而 (·)^是反对称的操作符。
Figure BDA00035716484100000912
Figure BDA00035716484100000913
式中两个误差状态量分别表示如下：
Figure BDA00035716484100000914
将定义的误差扰动代入(11)和(12)求解误差状态，经过一阶Taylor展开并去除所有二阶项， 可得连续时间误差状态方程如式(13)所示：
Figure BDA00035716484100000915
式中F和G分别是经过线性化的***状态和误差的Jacobian矩阵，n为噪声且
Figure BDA00035716484100000916
Figure BDA00035716484100000917
其中
Figure BDA00035716484100000918
为陀螺仪偏置，
Figure BDA00035716484100000919
为加速度偏置，
Figure BDA00035716484100000920
是B系相对于参考系r系中k时刻的旋 转矩阵，
Figure BDA00035716484100000921
为k时刻B系相对于I系的速度在k时刻的参考系r系中的表示。
为了进行R-EKF的预测将上式13展示得到连续模型的***标准状态量与误差状态进行 离散化可得如式(14)，其中△t＝tk+1-tk，小标s表示为定值。
根据EKF的一般步骤求状态转移矩阵，R-EKF将τ到τ+1时刻的状态转移矩阵Φτ+1,τ进 行一阶进行表达如下(15)所示，式中δt＝tτ+1-tτ。
Figure BDA0003571648410000101
然后我们根据上式的状态转移矩阵求取中间状态的协方差矩阵如式(16)所示，式中我 们将噪声假设加速计和陀螺仪上的随机噪声，以及偏置上的随机游走导数被假定为零均值高 斯白噪声Q如式(17)。
Figure BDA0003571648410000102
Figure BDA0003571648410000103
量测模型：
观测量
Figure RE-GDA0003679388520000104
来自于深度学习CNN-LSTM-ATT-VO网络输出的相对位姿测 量值，其对应的协方差矩阵R假设深度学习输出的6-DOF之间的量测噪声不相关可以简化为 对角阵，将量测信息的误差
Figure RE-GDA0003679388520000105
表示为下式(18)所示：
Figure RE-GDA0003679388520000106
其中
Figure BDA0003571648410000107
分别表示K+1时刻的载体系相对于k时刻的参考系的旋转以及平移的 估计量。
因为当φ→π时φ＝ln(C)不可微。为了避免这个问题，本发明中应用式(19)所示的一 阶Baker-Campbell-Hausdorff(BCH)公式，该公式将对数映射近似为两个旋转向量的相减。这 个近似是有效的，因为两个旋转都是极小量。
Figure BDA0003571648410000108
通过上式的微分过程我们可以找到量测的jacobian矩阵
Figure BDA0003571648410000109
推导过程如式(20) 与(21)所示，量测矩阵Hk+1的具体形式如式(22)所示。
Figure BDA00035716484100001010
Figure BDA00035716484100001011
Figure BDA00035716484100001012
量测更新：
按照式(23)进行常规EKF的更新，得到误差
Figure BDA0003571648410000111
的估计值。
Figure BDA0003571648410000112
然后，根据上述定义的扰动，将
Figure BDA0003571648410000113
注入到预测的标准状态中，得到
Figure BDA0003571648410000114
最后，将所有状态的参考帧从帧rk向前移动到帧rk+1，得到参考帧k+1时刻的状态量
Figure BDA0003571648410000115
如式24所示。当经过误差补偿后得到当前时刻的姿态后，就可以进行下一次EKF迭代了。
Figure BDA0003571648410000116
上式中
Figure BDA0003571648410000117
为k+1时刻的载体系相对于k时刻的参考系的旋转，
Figure BDA0003571648410000118
以及
Figure BDA0003571648410000119
为k+1 时刻的载体系相对于参考系的旋转与平移。
同时状态协方差矩阵也必须被传播到下一时刻来反映上式中的操作。
为进一步说明本发明的有益效果，完成了以下仿真对比验证。将KITTI数据集中的序列 10进行算法的验证，IMU的频率为10HZ，相机的频率为100HZ。深度网络训练数据采用序 列00-06进行训练。经过算法得出了这些序列的位姿信息，下面是这些序列经过EVO评估工 具画出来的轨迹，并与传统视觉里程计单目算法VINS作为比较。从图6、图7可以看出本发 明提出的定位算法能够准确估计载体的位置信息。图8、图9给出了本发明定位提出R-EKF 算法估计的轨迹与真实估计之间求取绝对轨迹误差APE，并分别计算轨迹的均方根误差、中 位数误差、平均误差、标准差，同时与单目VINS算法对比。可以看出本发明的定位精度更 高，因此本发明提供的方法能够比传统的单目视觉惯性里程计算法提高更高的定位精度。

Claims (7)

1.一种深度网络与滤波结合的定位方法，其特征在于，包括：
步骤一：建议视觉惯性里程计运动学模型；
步骤二：将时序上相邻k+1帧图像视频序列输入堆叠网络，组合相邻帧的图像得到k组图像对；
步骤三：k组图像对输入CNN-LSTM-ATT-VO网络后输出6-dof的位姿估计
Figure FDA0003571648400000011
其中Δx,Δy,Δz分别为x轴、y轴和z轴平移量，
Figure FDA0003571648400000012
分别为绕x轴、y轴和z轴旋转量；所述网络依次包括卷积神经网络、CBAM模块、LSTM网络和全连接层；
步骤四：通过基于李群的以机器人为中心的Kalman滤波算法进行视觉惯性里程计的融合定位。
2.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：步骤一具体为：
IMU***状态方程为：
Figure FDA0003571648400000013
Figure FDA0003571648400000014
其中，am为测量到的加速度计值，ωm为陀螺仪测量值，是基于载体B系在τ时刻定义的，分别受到噪声na和nω、随机游走ba和bω的影响；加速度计和陀螺仪上的随机噪声以及随机游走在偏差上的导数为零均值高斯分，i为惯性坐标系，定义局部参考坐标系rk，rk是k时刻的参考系，其中时间k,k+1,...对应于接收图像的时间点，载体坐标系记为B系，Bτ为τ时刻的载体系，其中τ,τ+1,...对应于时间k和k+1之间接收IMU测量的时间；
Figure FDA0003571648400000015
为k时刻的局部参考坐标系相对于τ时刻的载体系的旋转矩阵，
Figure FDA0003571648400000016
为重力矢量，
Figure FDA0003571648400000017
是τ时刻的载体系相对于惯性系的加速度在载体系上的表示；
在惯性状态保持恒定的情况下，载体的标准状态在连续时间上演变为：
Figure FDA0003571648400000018
3.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：所述卷积神经网络对图像对进行几何关系特征提取，输出表示相邻帧之间几何关系的张量序列。
4.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：所述CBAM模块对特征图进行操作为：
F′＝σ(MLP(AP(F))+MLP(MP(F)))⊙F
Figure FDA0003571648400000019
Figure FDA00035716484000000110
表示输入特征图，
Figure FDA00035716484000000111
表示经过通道注意力后的一维特征图，
Figure FDA00035716484000000112
表示最终经过空间注意力机制后的特征图，σ表示sigmoid函数，MLP为FC层，MP表示最大池化，AP表示平均池化。
5.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：所述LSTM网络包含两层LSTM，每个LSTM中含有1024个隐藏单元，LSTM中三个门默认激活函数为Relu函数。
6.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：所述全连接层含有128个隐藏单元并在其后加入一个Relu激活函数，对接受特征进行降维。
7.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：步骤四具体为：
定义状态为：
Figure RE-FDA0003679388510000021
分别为惯性状态
Figure RE-FDA0003679388510000022
和载体状态
Figure RE-FDA0003679388510000023
具体为：
Figure RE-FDA0003679388510000024
Figure RE-FDA0003679388510000025
对于惯性状态，
Figure RE-FDA0003679388510000026
包括I系在参考系r系中k时刻中的姿态，其中
Figure RE-FDA0003679388510000027
是I系相对于参考系r系中k时刻的旋转矩阵，
Figure RE-FDA0003679388510000028
是I系相对于参考系r系中k时刻的平移在参考系中的表示，
Figure RE-FDA0003679388510000029
是用k时刻r系表示的重力矢量；对于载体状态，
Figure RE-FDA00036793885100000210
包括B系在k时刻的参考是r系中的姿势，
Figure RE-FDA00036793885100000211
分别为载体系B系中的载体速度、陀螺仪偏置和加速度计偏置；
标准状态的误差状态向量为：
Figure RE-FDA00036793885100000212
旋转状态的扰动定义为：
Figure RE-FDA00036793885100000213
其中C∈SO(3)、
Figure RE-FDA00036793885100000214
(·)^为反对称的操作符；
其中，两个误差状态量分别表示如下：
Figure RE-FDA00036793885100000215
Figure RE-FDA00036793885100000216
求得连续时间误差状态方程：
Figure RE-FDA00036793885100000217
式中，F和G分别是经过线性化的***状态和误差的Jacobian矩阵，n为噪声且
Figure RE-FDA00036793885100000218
将连续模型的***标准状态量与误差状态进行离散化得到，
Figure RE-FDA0003679388510000031
Figure RE-FDA0003679388510000032
Figure RE-FDA0003679388510000033
其中，
Figure RE-FDA0003679388510000034
为陀螺仪偏置，
Figure RE-FDA0003679388510000035
为加速度偏置，
Figure RE-FDA0003679388510000036
是B系相对于参考系r系中k时刻的旋转矩阵，
Figure RE-FDA0003679388510000037
为k时刻B系相对于I系的速度在k时刻的参考系r系中的表示，Δt＝tk+1-tk，小标s表示为定值；
将τ到τ+1时刻的状态转移矩阵Φτ+1,τ进行一阶表达:
Figure RE-FDA0003679388510000038
式中,δt＝tτ+1-tτ；
求取中间状态的协方差矩阵为：
Figure RE-FDA0003679388510000039
式中，将噪声假设加速计和陀螺仪上的随机噪声，以及偏置上的随机游走导数被假定为零均值高斯白噪声Q，具体为：
Figure RE-FDA00036793885100000310
观测量
Figure RE-FDA00036793885100000311
为深度学习CNN-LSTM-ATT-VO网络输出的相对位姿测量值，其对应的协方差矩阵R简化为对角阵，将量测信息的误差
Figure RE-FDA00036793885100000312
表示为：
Figure RE-FDA00036793885100000313
其中，
Figure RE-FDA00036793885100000314
分别表示K+1时刻的载体系相对于k时刻的参考系的旋转以及平移的估计量；
应用一阶Baker-Campbell-Hausdorff(BCH)公式将对数映射近似为两个旋转向量的相减：
Figure RE-FDA00036793885100000315
通过上式的微分过程找到量测的jacobian矩阵
Figure RE-FDA00036793885100000316
量测矩阵Hk+1的具体形式为：
Figure RE-FDA00036793885100000317
进行常规EKF的更新，得到误差
Figure RE-FDA00036793885100000318
的估计值：
Figure RE-FDA0003679388510000041
Figure RE-FDA0003679388510000042
Figure RE-FDA0003679388510000043
然后，将
Figure RE-FDA0003679388510000044
注入到预测的标准状态中得到
Figure RE-FDA0003679388510000045
最后，将所有状态的参考帧从帧rk向前移动到帧rk+1，得到参考帧k+1时刻的状态量
Figure RE-FDA0003679388510000046
Figure RE-FDA0003679388510000047
Figure RE-FDA0003679388510000048
Figure RE-FDA0003679388510000049
Figure RE-FDA00036793885100000410
Figure RE-FDA00036793885100000411
上式中，
Figure RE-FDA00036793885100000412
为k+1时刻的载体系相对于k时刻的参考系的旋转，
Figure RE-FDA00036793885100000413
以及
Figure RE-FDA00036793885100000414
为k+1时刻的载体系相对于参考系的旋转与平移；当经过误差补偿后得到当前时刻的姿态后，进行下一次EKF迭代。
CN202210348112.0A 2022-03-29 2022-03-29 一种深度网络与滤波结合的定位方法 Pending CN115046545A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210348112.0A CN115046545A (zh) 2022-03-29 2022-03-29 一种深度网络与滤波结合的定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210348112.0A CN115046545A (zh) 2022-03-29 2022-03-29 一种深度网络与滤波结合的定位方法

Publications (1)

Publication Number Publication Date
CN115046545A true CN115046545A (zh) 2022-09-13

Family

ID=83157424

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210348112.0A Pending CN115046545A (zh) 2022-03-29 2022-03-29 一种深度网络与滤波结合的定位方法

Country Status (1)

Country Link
CN (1) CN115046545A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111709323A (zh) * 2020-05-29 2020-09-25 重庆大学 一种基于李群和长短时记忆网络的手势识别方法
CN116295507A (zh) * 2023-05-26 2023-06-23 南京师范大学 一种基于深度学习的激光惯性里程计优化方法、***
CN117152546A (zh) * 2023-10-31 2023-12-01 江西师范大学 一种遥感场景分类方法、***、存储介质及电子设备

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111709323A (zh) * 2020-05-29 2020-09-25 重庆大学 一种基于李群和长短时记忆网络的手势识别方法
CN111709323B (zh) * 2020-05-29 2024-02-02 重庆大学 一种基于李群和长短时记忆网络的手势识别方法
CN116295507A (zh) * 2023-05-26 2023-06-23 南京师范大学 一种基于深度学习的激光惯性里程计优化方法、***
CN116295507B (zh) * 2023-05-26 2023-08-15 南京师范大学 一种基于深度学习的激光惯性里程计优化方法、***
CN117152546A (zh) * 2023-10-31 2023-12-01 江西师范大学 一种遥感场景分类方法、***、存储介质及电子设备
CN117152546B (zh) * 2023-10-31 2024-01-26 江西师范大学 一种遥感场景分类方法、***、存储介质及电子设备

Similar Documents

Publication Publication Date Title
CN115046545A (zh) 一种深度网络与滤波结合的定位方法
CN110030994B (zh) 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN110702107A (zh) 一种单目视觉惯性组合的定位导航方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
Hasberg et al. Simultaneous localization and mapping for path-constrained motion
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN111780781B (zh) 基于滑动窗口优化的模板匹配视觉和惯性组合里程计
CN108731700B (zh) 一种视觉惯性里程计中的加权欧拉预积分方法
CN111578937A (zh) 同时优化外参数的视觉惯性里程计***
CN107883965A (zh) 基于光学信息交互多模型强跟踪容积卡尔曼滤波导航方法
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN113483755B (zh) 一种基于非全局一致地图的多传感器组合定位方法及***
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
Lamon et al. 3D position tracking in challenging terrain
Zhang et al. Dido: Deep inertial quadrotor dynamical odometry
Sun et al. An improved monocular visual-inertial navigation system
CN113340324B (zh) 一种基于深度确定性策略梯度的视觉惯性自校准方法
CN112991400B (zh) 一种无人艇的多传感器辅助定位方法
CN113029173A (zh) 车辆导航方法及装置
CN117073720A (zh) 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备
CN111982126A (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
Luo et al. End‐Effector Pose Estimation in Complex Environments Using Complementary Enhancement and Adaptive Fusion of Multisensor
CN116295342A (zh) 一种用于飞行器勘测的多传感状态估计器
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN114046800A (zh) 一种基于双层滤波框架的高精度里程估计方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination