CN115046545A - 一种深度网络与滤波结合的定位方法 - Google Patents
一种深度网络与滤波结合的定位方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000001914 filtration Methods 0.000 title claims abstract description 27
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 25
- 230000000007 visual effect Effects 0.000 claims abstract description 20
- 238000013519 translation Methods 0.000 claims abstract description 13
- 238000013527 convolutional neural network Methods 0.000 claims abstract description 9
- 230000004927 fusion Effects 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 31
- 238000005259 measurement Methods 0.000 claims description 22
- 230000006870 function Effects 0.000 claims description 17
- 239000013598 vector Substances 0.000 claims description 13
- 230000008569 process Effects 0.000 claims description 11
- 238000005295 random walk Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 8
- 238000010586 diagram Methods 0.000 claims description 8
- 230000007246 mechanism Effects 0.000 claims description 8
- 230000004913 activation Effects 0.000 claims description 6
- 238000011176 pooling Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 5
- 230000009467 reduction Effects 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 5
- 125000004432 carbon atom Chemical group C* 0.000 claims description 2
- 239000000284 extract Substances 0.000 claims description 2
- 238000013135 deep learning Methods 0.000 description 11
- 230000014616 translation Effects 0.000 description 8
- 230000008901 benefit Effects 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000012795 verification Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000012549 training Methods 0.000 description 2
- CLSIFQGHPQDTHQ-DTWKUNHWSA-N (2s,3r)-2-[(4-carboxyphenyl)methyl]-3-hydroxybutanedioic acid Chemical compound OC(=O)[C@H](O)[C@@H](C(O)=O)CC1=CC=C(C(O)=O)C=C1 CLSIFQGHPQDTHQ-DTWKUNHWSA-N 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000008034 disappearance Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000002372 labelling Methods 0.000 description 1
- 239000007788 liquid Substances 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000000644 propagated effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
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)
- Navigation (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
本发明公开了一种深度网络与滤波结合的定位方法，步骤一：建议视觉惯性里程计运动学模型；步骤二：将时序上相邻k+1帧图像视频序列输入堆叠网络，组合相邻帧的图像得到k组图像对；步骤三：k组图像对输入CNN‑LSTM‑ATT‑VO网络后输出6‑dof的位姿估计Δx,Δy,Δz,
Δλ,Δψ，其中Δx,Δy,Δz分别为x轴、y轴和z轴平移量，
Δλ,Δψ分别为绕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,
△λ,△ψ，其中△x,△y,△z分别为x轴、y轴和z轴平移量，
△λ,△ψ分别为绕 x轴、y轴和z轴旋转量；所述网络依次包括卷积神经网络、CBAM模块、LSTM网络和全连 接层；
步骤四：通过基于李群的以机器人为中心的Kalman滤波算法进行视觉惯性里程计的融 合定位。
进一步的，步骤一具体为：
IMU系统状态方程为：
其中，am为测量到的加速度计值，ωm为陀螺仪测量值，是基于载体B系在τ时刻定义的，分别受到噪声na和nω、随机游走ba和bω的影响；加速度计和陀螺仪上的随机噪声以及随机游走在偏差上的导数为零均值高斯分，i为惯性坐标系，定义局部参考坐标系rk，rk是k 时刻的参考系，其中时间k,k+1,...对应于接收图像的时间点，载体坐标系记为B系，Bτ为 τ时刻的载体系，其中τ,τ+1,...对应于时间k和k+1之间接收IMU测量的时间；
为k时 刻的局部参考坐标系相对于τ时刻的载体系的旋转矩阵，
为重力矢量，
是τ时刻的载 体系相对于惯性系的加速度在载体系上的表示；
在惯性状态保持恒定的情况下，载体的标准状态在连续时间上演变为：
进一步的，卷积神经网络对图像对进行几何关系特征提取，输出表示相邻帧之间几何关 系的张量序列。
进一步的，CBAM模块对特征图进行操作为：
F′＝σ(MLP(AP(F))+MLP(MP(F)))⊙F
F″＝σ(f7×7[AP(F′),MP(F′)])⊙F′
进一步的，LSTM网络包含两层LSTM，每个LSTM中含有1024个隐藏单元，LSTM 中三个门默认激活函数为Relu函数。
进一步的，全连接层含有128个隐藏单元并在其后加入一个Relu激活函数，对接受特征 进行降维。
进一步的，步骤四具体为：
定义状态为：
分别为惯性状态
和载体状态
具体为：
对于惯性状态，
包括I系在参考系r系中k时刻中的姿态，其中
是I 系相对于参考系r系中k时刻的旋转矩阵，
是I系相对于参考系r系中k时刻的平移在参 考系中的表示，
是用k时刻r系表示的重力矢量；对于载体状态，
包括B系在k时刻的参考是r系中的姿势，
分别为载体系B系中的载体速 度、陀螺仪偏置和加速度计偏置；
标准状态的误差状态向量为：
旋转状态的扰动定义为：
其中C∈SO(3)、
(·)∧为反对称的操作符；
其中，两个误差状态量分别表示如下：
求得连续时间误差状态方程：
式中，F和G分别是经过线性化的系统状态和误差的Jacobian矩阵，n为噪声且
将连续模型的系统标准状态量与误差状态进行离散化得到，
其中，
为陀螺仪偏置，
为加速度偏置，
是B系相对于参考系r系中k时刻的旋转矩阵，
为k时刻B系相对于I系的速度在k时刻的参考系r系中的表示，△t＝tk+1-tk，小标s表示为定值；
将τ到τ+1时刻的状态转移矩阵Φτ+1,τ进行一阶表达:
式中,δt＝tτ+1-tτ；
求取中间状态的协方差矩阵为：
式中，将噪声假设加速计和陀螺仪上的随机噪声，以及偏置上的随机游走导数被假定为 零均值高斯白噪声Q，具体为：
观测量
为深度学习CNN-LSTM-ATT-VO网络输出的相对位姿测量值，其对应的协方差矩阵R简化为对角阵，将量测信息的误差
表示为：
其中，
分别表示K+1时刻的载体系相对于k时刻的参考系的旋转以及平移 的估计量；
应用一阶Baker-Campbell-Hausdorff(BCH)公式将对数映射近似为两个旋转向量的相减：
通过上式的微分过程找到量测的jacobian矩阵
量测矩阵Hk+1的具体形式 为：
进行常规EKF的更新，得到误差
的估计值：
然后，将
注入到预测的标准状态中得到
最后，将所有状态的参考帧从帧rk向前移动到帧rk+1，得到参考帧k+1时刻的状态量 
上式中，
为k+1时刻的载体系相对于k时刻的参考系的旋转，
以及
为 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测量的时间。上标和下标用于跟踪物理量的坐标框架。例如，
是B系相对于C系的速度，用A系表示，Cab是B系相对于A系的旋转矩阵。上标也被用来 区分不同类型的量.比如：
用于表示噪声量，
用于表示整个EKF预测过程中的传播量， 
用于表示EKF更新后的修正量。
IMU系统状态方程的建模根据需求表达如下式(1)，Bτ为τ时刻的载体系，
为k时刻的局部参考坐标系相对于τ时刻的载体系的旋转矩阵，
是τ时刻的载体系相对于惯性系 的加速度在载体系上的表示。测量到的加速度计值am和陀螺仪测量值ωm是基于载体B系在τ 时刻定义的，它们分别受到噪声na和nω、随机游走ba和bω的影响。加速度计的测量还结合了 一个固定的重力矢量
并通过旋转矩阵旋转到了B系上，假设加速度计和陀螺仪上的随机噪 声以及随机游走在偏差上的导数为零均值高斯分布。
在惯性状态保持恒定的情况下，载体的标准状态在连续时间上演变如式(2)所示：
步骤二：采用深度学习的方式搭建基于视觉里程计的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卷积层相关参数设置
(2)LSTM网络
本发明中考虑添加两层LSTM的主要目的是为了让整个深度网络能够通过数据自主的学 习连续多个相邻帧之间位姿的内在联系。LSTM层不仅能学习到历史数据中的状态(即前面 多个帧图像之间的位姿关系)，在对当前时刻位姿进行估计时达到传统算法上多帧图像之间的 几何约束效果；而且LSTM网络在长序列的数据学习过程中因为其门结构的特殊性，不会产 生梯度消失的问题，可以持续保持网络的学习效果。其中第一层LSTM用于接收CNN的输 出特征数据，第二层则用于接收第一层的输出并将信息传递给全连接层输出位姿估计的结果。 每个LSTM中含有1024个隐藏单元。根据本发明实际需要，将LSTM中三个门默认激活函 数从Sigmoid函数改成Relu函数用于更高级的位姿信息表示。
(3)全连接层
在本发明中全连接层的目的主要起强化几何关系特征信息并且对特征进行降维的作用， 从LSTM层输出的是高维的特征表示，因此需要将特征信息通过输出到全连接层进行降维操 作。本发明中在整个网络的最后只设计了一层FC层，其中含有128个隐藏单元并在其后加 入了一个Relu激活函数，最后将降维后的特征信息输出映射为6-dof的位姿估计表示。
△x,△y,△z,
△λ,△ψ
其中△x,△y,△z为平移量，
△λ,△ψ为旋转量。最后经过对输出的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)所示：
其中w为需要学习的参数，为了找到针对VO位姿估计问题的最优参数w*，将损失函数 设计为基于网络估计位姿与真实轨迹的均方误差MSE的最小化表达如式(5)所示：
其中
表示真实轨迹的位置与姿态，β为加权平衡参数用于平衡位置与姿态之间的 关系，||·||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)的处理过程。
表示输入特征图，
表示经过通道注意力后的 一维特征图，
最终经过空间注意力机制后的特征图。σ表示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)所示，分为惯性状态
和载体状态
对于惯性状态
包 括I系在参考系r系中k时刻中的姿态，其中
是I系相对于参考系r系中k时刻的旋转矩 阵，
是I系相对于参考系r系中k时刻的平移在参考系中的表示，
是用k时刻r 系表示的重力矢量。对于载体状态，
包括B系在k时刻的参考是r系中的 姿势，
分别假设为载体系B系中的载体速度、陀螺仪偏置和加速度计偏置。
式中两个状态量分别表示如式(9)所示：
标准状态对应的误差状态向量如式(10)所示。误差状态定义为对标称状态的扰动，除旋 转外，其余均定义在欧几里得空间。旋转状态的扰动定义为式(11),其中C∈SO(3)、
而 (·)^是反对称的操作符。
式中两个误差状态量分别表示如下：
将定义的误差扰动代入(11)和(12)求解误差状态，经过一阶Taylor展开并去除所有二阶项， 可得连续时间误差状态方程如式(13)所示：
式中F和G分别是经过线性化的系统状态和误差的Jacobian矩阵，n为噪声且
其中
为陀螺仪偏置，
为加速度偏置，
是B系相对于参考系r系中k时刻的旋 转矩阵，
为k时刻B系相对于I系的速度在k时刻的参考系r系中的表示。
为了进行R-EKF的预测将上式13展示得到连续模型的系统标准状态量与误差状态进行 离散化可得如式(14)，其中△t＝tk+1-tk，小标s表示为定值。
根据EKF的一般步骤求状态转移矩阵，R-EKF将τ到τ+1时刻的状态转移矩阵Φτ+1,τ进 行一阶进行表达如下(15)所示，式中δt＝tτ+1-tτ。
然后我们根据上式的状态转移矩阵求取中间状态的协方差矩阵如式(16)所示，式中我 们将噪声假设加速计和陀螺仪上的随机噪声，以及偏置上的随机游走导数被假定为零均值高 斯白噪声Q如式(17)。
量测模型：
观测量
来自于深度学习CNN-LSTM-ATT-VO网络输出的相对位姿测 量值，其对应的协方差矩阵R假设深度学习输出的6-DOF之间的量测噪声不相关可以简化为 对角阵，将量测信息的误差
表示为下式(18)所示：
其中
分别表示K+1时刻的载体系相对于k时刻的参考系的旋转以及平移的 估计量。
因为当φ→π时φ＝ln(C)不可微。为了避免这个问题，本发明中应用式(19)所示的一 阶Baker-Campbell-Hausdorff(BCH)公式，该公式将对数映射近似为两个旋转向量的相减。这 个近似是有效的，因为两个旋转都是极小量。
通过上式的微分过程我们可以找到量测的jacobian矩阵
推导过程如式(20) 与(21)所示，量测矩阵Hk+1的具体形式如式(22)所示。
量测更新：
按照式(23)进行常规EKF的更新，得到误差
的估计值。
然后，根据上述定义的扰动，将
注入到预测的标准状态中，得到
最后，将所有状态的参考帧从帧rk向前移动到帧rk+1，得到参考帧k+1时刻的状态量 
如式24所示。当经过误差补偿后得到当前时刻的姿态后，就可以进行下一次EKF迭代了。
上式中
为k+1时刻的载体系相对于k时刻的参考系的旋转，
以及
为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的位姿估计
其中Δx,Δy,Δz分别为x轴、y轴和z轴平移量，
分别为绕x轴、y轴和z轴旋转量；所述网络依次包括卷积神经网络、CBAM模块、LSTM网络和全连接层；
步骤四：通过基于李群的以机器人为中心的Kalman滤波算法进行视觉惯性里程计的融合定位。
2.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：步骤一具体为：
IMU系统状态方程为：
其中，am为测量到的加速度计值，ωm为陀螺仪测量值，是基于载体B系在τ时刻定义的，分别受到噪声na和nω、随机游走ba和bω的影响；加速度计和陀螺仪上的随机噪声以及随机游走在偏差上的导数为零均值高斯分，i为惯性坐标系，定义局部参考坐标系rk，rk是k时刻的参考系，其中时间k,k+1,...对应于接收图像的时间点，载体坐标系记为B系，Bτ为τ时刻的载体系，其中τ,τ+1,...对应于时间k和k+1之间接收IMU测量的时间；
为k时刻的局部参考坐标系相对于τ时刻的载体系的旋转矩阵，
为重力矢量，
是τ时刻的载体系相对于惯性系的加速度在载体系上的表示；
在惯性状态保持恒定的情况下，载体的标准状态在连续时间上演变为：
3.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：所述卷积神经网络对图像对进行几何关系特征提取，输出表示相邻帧之间几何关系的张量序列。
4.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：所述CBAM模块对特征图进行操作为：
F′＝σ(MLP(AP(F))+MLP(MP(F)))⊙F
5.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：所述LSTM网络包含两层LSTM，每个LSTM中含有1024个隐藏单元，LSTM中三个门默认激活函数为Relu函数。
6.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：所述全连接层含有128个隐藏单元并在其后加入一个Relu激活函数，对接受特征进行降维。
7.根据权利要求1所述的一种深度网络与滤波结合的定位方法，其特征在于：步骤四具体为：
定义状态为：
分别为惯性状态
和载体状态
具体为：
对于惯性状态，
包括I系在参考系r系中k时刻中的姿态，其中
是I系相对于参考系r系中k时刻的旋转矩阵，
是I系相对于参考系r系中k时刻的平移在参考系中的表示，
是用k时刻r系表示的重力矢量；对于载体状态，
包括B系在k时刻的参考是r系中的姿势，
分别为载体系B系中的载体速度、陀螺仪偏置和加速度计偏置；
标准状态的误差状态向量为：
旋转状态的扰动定义为：
其中C∈SO(3)、
(·)^为反对称的操作符；
其中，两个误差状态量分别表示如下：
求得连续时间误差状态方程：
式中，F和G分别是经过线性化的系统状态和误差的Jacobian矩阵，n为噪声且
将连续模型的系统标准状态量与误差状态进行离散化得到，
其中，
为陀螺仪偏置，
为加速度偏置，
是B系相对于参考系r系中k时刻的旋转矩阵，
为k时刻B系相对于I系的速度在k时刻的参考系r系中的表示，Δt＝tk+1-tk，小标s表示为定值；
将τ到τ+1时刻的状态转移矩阵Φτ+1,τ进行一阶表达:
式中,δt＝tτ+1-tτ；
求取中间状态的协方差矩阵为：
式中，将噪声假设加速计和陀螺仪上的随机噪声，以及偏置上的随机游走导数被假定为零均值高斯白噪声Q，具体为：
观测量
为深度学习CNN-LSTM-ATT-VO网络输出的相对位姿测量值，其对应的协方差矩阵R简化为对角阵，将量测信息的误差
表示为：
其中，
分别表示K+1时刻的载体系相对于k时刻的参考系的旋转以及平移的估计量；
应用一阶Baker-Campbell-Hausdorff(BCH)公式将对数映射近似为两个旋转向量的相减：
通过上式的微分过程找到量测的jacobian矩阵
量测矩阵Hk+1的具体形式为：
进行常规EKF的更新，得到误差
的估计值：
然后，将
注入到预测的标准状态中得到
最后，将所有状态的参考帧从帧rk向前移动到帧rk+1，得到参考帧k+1时刻的状态量
上式中，
为k+1时刻的载体系相对于k时刻的参考系的旋转，
以及
为k+1时刻的载体系相对于参考系的旋转与平移；当经过误差补偿后得到当前时刻的姿态后，进行下一次EKF迭代。
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)
| 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 | 江西师范大学 | 一种遥感场景分类方法、系统、存储介质及电子设备 |
-
2022
- 2022-03-29 CN CN202210348112.0A patent/CN115046545A/zh active Pending
Cited By (6)
| 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) | 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 | |
| CN111156987B (zh) | 基于残差补偿多速率ckf的惯性/天文组合导航方法 | |
| CN110702107A (zh) | 一种单目视觉惯性组合的定位导航方法 | |
| CN109991636A (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
| CN111780781B (zh) | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| Hasberg et al. | Simultaneous localization and mapping for path-constrained motion | |
| CN109343550B (zh) | 一种基于滚动时域估计的航天器角速度的估计方法 | |
| CN111578937A (zh) | 同时优化外参数的视觉惯性里程计系统 | |
| CN108731700B (zh) | 一种视觉惯性里程计中的加权欧拉预积分方法 | |
| CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
| CN111156997A (zh) | 一种基于相机内参在线标定的视觉/惯性组合导航方法 | |
| CN112991400B (zh) | 一种无人艇的多传感器辅助定位方法 | |
| CN113483755B (zh) | 一种基于非全局一致地图的多传感器组合定位方法及系统 | |
| CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
| CN114529576A (zh) | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 | |
| Wang et al. | Adaptive optimization online IMU self-calibration method for visual-inertial navigation systems | |
| Zhang et al. | Dido: Deep inertial quadrotor dynamical odometry | |
| Sun et al. | An improved monocular visual-inertial navigation system | |
| CN113340324B (zh) | 一种基于深度确定性策略梯度的视觉惯性自校准方法 | |
| CN114111772A (zh) | 一种基于数据手套的水下机器人软体作业手位置跟踪方法 | |
| Luo et al. | End‐Effector Pose Estimation in Complex Environments Using Complementary Enhancement and Adaptive Fusion of Multisensor | |
| CN113029173A (zh) | 车辆导航方法及装置 | |
| CN113091754A (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 |