CN114708293A - 基于深度学习点线特征和imu紧耦合的机器人运动估计方法 - Google Patents

基于深度学习点线特征和imu紧耦合的机器人运动估计方法 Download PDF

Info

Publication number
CN114708293A
CN114708293A CN202210283858.8A CN202210283858A CN114708293A CN 114708293 A CN114708293 A CN 114708293A CN 202210283858 A CN202210283858 A CN 202210283858A CN 114708293 A CN114708293 A CN 114708293A
Authority
CN
China
Prior art keywords
line
point
imu
pose
error function
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
CN202210283858.8A
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.)
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202210283858.8A priority Critical patent/CN114708293A/zh
Publication of CN114708293A publication Critical patent/CN114708293A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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/207—Analysis of motion for motion estimation over a hierarchy of resolutions
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/045—Combinations of networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • General Health & Medical Sciences (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • Biophysics (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • Computational Linguistics (AREA)
  • Biomedical Technology (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Image Analysis (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，包括：对当前帧图像进行实例分割处理得到动态像素和静态像素；对于静态像素部分，提取预测的重复性单元和稠密描述子并进行线特征提取；获取像素级可重复性图和分块级可重复性图，采用基于可重复性的残差，构造点特征重投影误差函数，从而估计机器人位姿；利用线特征构建线特征重投影误差函数，约束位姿估计；对连续两帧图像之间的IMU数据进行预积分，构造IMU误差函数对位姿的迭代优化；最小化点特征重投影误差函数、线特征重投影误差函数以及IMU误差函数之和来优化所有状态变量，求得准确的机器人位姿信息。本发明解决了现有技术在动态环境中定位精度不高、鲁棒性差等问题。

Description

基于深度学习点线特征和IMU紧耦合的机器人运动估计方法
技术领域
本发明涉及移动机器人定位导航领域，具体涉及一种基于深度学习点线特征和IMU紧耦合的机器人运动估计方法。
背景技术
视觉里程计是移动机器人、自动驾驶等领域的重要环节，单目视觉里程计因其轻巧、便宜而得到广泛研究，但其也更具有挑战性，在面对遮挡、光照变化大、纹理弱等场景，效果并不算太好。
近年来，随着深度学习在计算机视觉领域的广泛运用，很多学者开始借助卷积神经网络强大的特征提取和抽象概括能力，根据相邻图像直接预测当前位姿信息。虽然采用深度学习技术替代传统几何SLAM的这类方法能够充分提取图像上的信息，且具有一定的泛化能力，但在物体被遮挡或存在动态物体的场景中，估计结果不够鲁棒和准确，且复杂场景下，训练和测试的难度也随之增加。
另一方面，多传感器融合也是当前机器人导航和定位的热点方向，而在多传感器融合中，IMU与视觉信息融合的方案也被广泛应用。IMU数据和单目视觉里程计数据融合能够解决单目相机缺乏尺度信息、在动态环境中定位精度不高、鲁棒性差的问题。但在复杂场景中(如环境噪声大)，不可避免会对融合数据带来干扰。
发明内容
本发明的目的是提供一种基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，用以应对现有技术中移动机器人视觉系统中的SLAM系统面临的弱纹理环境和动态环境的挑战，并解决IMU数据和单目视觉里程计数据融合能够解决单目相机缺乏尺度信息、在动态环境中定位精度不高、鲁棒性差的问题。
为了实现上述任务，本发明采用以下技术方案：
一种基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，包括：
实时采集机器人运动过程中通过相机拍摄的图像作为当前帧图像；对当前帧图像进行实例分割处理，从而区分得到当前帧图像中的动态像素和静态像素；
对于当前帧图像中的静态像素部分，提取预测的重复性单元和稠密描述子；
对当前帧图像中的静态像素部分进行线特征提取；
根据提取到的重复性单元和稠密描述子，获取像素级可重复性图和分块级可重复性图，采用基于可重复性的残差替代传统直接法的光度残差，构造点特征重投影误差函数，从而估计机器人位姿；
利用对静态像素部分提取的线特征，构建线特征重投影误差函数，约束位姿估计；
利用IMU获得机器人的IMU数据，对连续两帧图像之间的IMU数据进行预积分，构造IMU误差函数对位姿的迭代优化；
最小化点特征重投影误差函数、线特征重投影误差函数以及IMU误差函数之和来优化所有状态变量，求得准确的机器人位姿信息。
进一步地，所述的预测的重复性单元表示为
Figure BDA0003559263010000021
由特征提取网络的特征点检测层得到；稠密描述子表示为
Figure BDA0003559263010000022
由特征提取网络的描述子解码层得到；其中，
Figure BDA0003559263010000023
W表示当前帧图像的宽度，H表示当前帧图像的高度，C是网格的大小，
Figure BDA0003559263010000024
表示实数集；
对
Figure BDA0003559263010000025
归一化，得到归一化后的重复性单元
Figure BDA0003559263010000026
进一步地，所述特征提取网络采用改进后的SuperPoint网络，具体对该网络的改进如下：
采用基于GhostNet架构的GhostNet编码层替代原superpoint网络的VGG编码层，GhostNet编码层包含卷积层Conv2d+批归一化层BN+激活层ReLU及六层G-bneck，同时将网络结构宽度系数设置为1.5；其中，六层G-bneck的扩张尺寸依次增大，第四、五层G-bneck的扩张率为0.25，其余层为0；第一、三、五、六层G-bneck的步长为2，其余层为1。
进一步地，所述对当前帧图像中的静态像素部分进行线特征提取，包括：
首先计算静态像素部分图像的每个像素的水平线角度，从而得到一个单位向量场，使单位向量场中所有向量都和经过像素的水平线相切；接着再选取静态像素部分图像中方向连贯的图像像素区域，对于每个选出的所述图像像素区域，先将其视为一条线段s，若图像像素区域的梯度相互独立，则说明图像像素区域的像素之间不存在线段相关性，即该图像像素区域无线段存在；定义NFAM为像素梯度之间无相关性的警告次数：
Figure BDA0003559263010000031
其中，NLoI表示静态像素部分图像中线段可能存在的数量；γ表示归一化值；S表示存在一条可能包含n条子线段{s1,...,sn}的线段；u和v分别代表图像像素区域的长和宽；而K＝{k1,...,kn}，ki表示与子线段si(i＝1,2,…,n)方向相同的像素个数；p是在图像像素区域内的一个随机像素q与线段si的方向θs(q)相同的概率，β(|si|,ki,p)是二项分布；
若NFAM(S,K,p)越小则说明某个像素区域中的像素梯度相互关联，即该像素区域可能存在线段；设定阈值η，当NFAM(S,K,p)<η，则s视为提取出的线特征。
进一步地，所述采用基于可重复性的残差替代传统直接法的光度残差，构造点特征重投影误差函数，从而估计机器人位姿，包括：
为了估计当前帧的相机位姿Tk，构建每个通过运动恢复结构SFM得到的三维路标点pi的可重复性残差
Figure BDA0003559263010000032
在分块级可重复性图
Figure BDA0003559263010000033
上关联静态像素部分图像的点特征和三维路标点，然后使用Levenberg-Marquardt算法迭代优化误差函数至收敛，可得位姿增量ξk：
在像素级可重复性图
Figure BDA0003559263010000034
上应用半径搜索算法，寻找与世界坐标系中的三维路标点pi的描述子最接近的像素点
Figure BDA0003559263010000035
然后将三维路标点重投影回当前帧图像的像素级可重复性图
Figure BDA0003559263010000036
上，通过最小化点特征重投影误差函数求得机器人的当前位姿Tk。
进一步地，所述可重复性残差
Figure BDA0003559263010000037
表示为：
Figure BDA0003559263010000038
Figure BDA0003559263010000039
其中，
Figure BDA00035592630100000310
代表不同分辨率的像素级可重复性图
Figure BDA00035592630100000311
和分块级可重复性图
Figure BDA00035592630100000312
π(·)表示相机投影方程，ξk表示第k帧相机位姿增量，Tk表示相机的当前位姿，
Figure BDA00035592630100000313
是ξk的反对称矩阵；
使用Levenberg-Marquardt算法迭代优化以下误差函数
Figure BDA00035592630100000314
至收敛，可得位姿增量ξk：
Figure BDA0003559263010000041
其中，
Figure BDA0003559263010000042
表示是与当前帧有高共视关系的相邻帧的特征点集合，γ指鲁棒核函数；
点特征重投影误差函数为：
Figure BDA0003559263010000043
其中：
Figure BDA0003559263010000044
上式中，Rk表示机器人相机当前位姿Tk对应的旋转矩阵，tk表示平移向量，pi表示三维路标点，
Figure BDA0003559263010000045
表示是与当前帧有高共视关系的相邻帧的特征点集合，上标T表示转置，∑i,k表示像素点
Figure BDA0003559263010000046
的协方差矩阵，其决定特征点在优化中的权重；i为第i个三维路标点，k为第k帧；γ表示鲁棒核函数。
进一步地，所述利用对静态像素部分提取的线特征，构建线特征重投影误差函数，约束位姿估计，包括：
记P、Q是三维空间下的线段PQ的两个端点，pd,qd是线段端点P、Q在相机平面的投影经过位姿变换后所对应的两个端点，它们对应的齐次坐标分别为
Figure BDA0003559263010000047
Figure BDA0003559263010000048
从而构建归一化线性系数；
将线特征的重投影误差
Figure BDA0003559263010000049
定义为投影线段端点到图像平面中变换后得到线段pdqd之间的距离，即：
Figure BDA00035592630100000410
Figure BDA00035592630100000411
Figure BDA00035592630100000412
其中，
Figure BDA00035592630100000413
指世界坐标系下的路标点Pi到投影线段pdqd的距离，
Figure BDA00035592630100000414
指世界坐标系下的地图点Qi到投影线段pdqd的距离，Ii为世界坐标系下的地图点Pi对应的归一化线性系数，T表示机器人相机位姿；K表示相机内参；
则最终的线特征重投影误差函数Eline为：
Figure BDA0003559263010000051
其中，χl指第I帧图像提取到的线特征集合，∑l为线段的两个端点(i,j)的协方差矩阵，ρ为Cauchy函数。
进一步地，所述利用IMU获得机器人的IMU数据，对连续两帧图像之间的IMU数据进行预积分，构造IMU误差函数对位姿的迭代优化，包括：
对IMU的测量值进行预积分，获得连续两帧之间的相对位姿信息，从而得到状态向量信息矩阵的预积分；
构建第i帧图像和三维路标点xj的重投影误差rij；
对于给定的k+1关键帧和其相应的状态向量
Figure BDA0003559263010000052
以及给定的l个三维路标点集合和其相应的状态向量X，它们的IMU误差函数为：
Figure BDA0003559263010000053
其中，
Figure BDA0003559263010000054
表示状态向量信息矩阵的预积分，
Figure BDA0003559263010000055
为
Figure BDA0003559263010000056
对应的协方差矩阵；rij表示第i帧图像和三维路标点xj的重投影误差，Kj表示第j个三维地图点的观测关键帧集合，ρ为Cauchy函数。
进一步地，所述最小化点特征重投影误差函数、线特征重投影误差函数以及IMU误差函数之和来优化所有状态变量，求得准确的机器人位姿信息，包括：
针对视觉点线特征约束、IMU约束，构建残差目标函数：
min{ρ(Epoint)+Eline+Ei,i+1}
其中，Epoint为点特征重投影误差函数，Eline为线特征重投影误差函数，Ei,i+1为IMU误差函数，ρ为Cauchy函数；
通过Levenberg–Marquard法迭代优化出所需要的状态变量：
Figure BDA0003559263010000057
其中，Ti为相机在世界坐标系下的位姿，vi为速度，
Figure BDA0003559263010000058
为陀螺仪的偏差，
Figure BDA0003559263010000059
为加速度计的偏差；
即可得到准确的位姿信息。
进一步地，所述方法还包括：
根据提取的点特征、线特征进行回环检测，与构建的点线词袋库匹配，并对所述状态向量进行反馈矫正；其中，点线词袋库即提取的点特征和线特征构成的特征库。
与现有技术相比，本发明具有以下技术特点：
本发明利用ORB-SLAM3基础上，融合了深度学习技术、IMU数据融合、实例分割技术和传统几何推理的优点，提供了一种基于深度学习点线特征和IMU紧耦合的机器人运动估计方法来计算出机器人相机位姿，避免了特征点误匹配带来的位姿错误的问题。同时，在复杂动态环境中，比如物体被局部遮挡，动态物体过多，相机较快移动，场景中存在与待跟踪物体相似的其他物体等特殊情况，本发明能够稳定地提取特征点、特征线，并通过IMU数据以及基于深度学习网络的可重复性图进行位姿跟踪，同时添加YOLACT实例分割模型对动态环境进行处理，使得本发明能更好地适用于各种复杂场景，且鲁棒性好。
附图说明
图1为本发明方法的流程示意图。
具体实施方式
本发明在ORB-SLAM3的基础上，提供了一种基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，融合了深度学习技术、IMU数据融合、实例分割技术和传统几何推理的优点，使得本发明的单目视觉里程计在复杂场景下更具鲁棒性和准确性；本发明方法具体包括以下步骤：
步骤1，实时采集机器人运动过程中通过相机拍摄的图像作为当前帧图像；对当前帧图像进行实例分割处理，从而区分得到当前帧图像中的动态像素和静态像素。
其中，对于当前帧图像进行实例分割处理时利用YOLACT实例分割模型，具体方法如下：
将移动机器人运动过程中通过相机拍摄的当前帧图像输入YOLACT实例分割模型进行实时目标识别与检测，一边生成候选区域RoI，一边实时对这些RoI进行分类和分割，将置信分数(mask coefficients)大于0.75的RoI记为动态像素，并记录模型输出的此RoI的(x,y,w,h)；将当前帧图像中除了动态像素其它区域均记为静态像素。
其中，四维向量(x,y,w,h)分别表示窗口的中心点坐标和宽高，边框(x,y)用于移动，边框大小(w,h)用于缩放；(x,y,w,h)所构成的区域即为感兴趣区域RoI。
步骤2，对于当前帧图像中的静态像素部分，提取预测的重复性单元和稠密描述子。
所述的预测的重复性单元表示为
Figure BDA0003559263010000071
由特征提取网络的特征点检测层得到；稠密描述子表示为
Figure BDA0003559263010000072
由特征提取网络的描述子解码层得到；其中，
Figure BDA0003559263010000073
W表示当前帧图像的宽度，H表示当前帧图像的高度，C是网格的大小，例如设置为8，
Figure BDA0003559263010000074
表示实数集；则重复性单元
Figure BDA0003559263010000075
归一化为：
Figure BDA0003559263010000076
其中，hc,wc,y、hc,wc,k表示重复性单元
Figure BDA0003559263010000077
的3个图像通道；归一化后的重复性单元
Figure BDA0003559263010000078
用于后续像素级可重复性图
Figure BDA0003559263010000079
和分块级可重复性图
Figure BDA00035592630100000710
的获取。
本方案中，采用改进的SuperPoint网络提取预测的重复性单元和稠密描述子；其中，对SuperPoint网络的改进包括：
原始的SuperPoint网络采用类似于VGG的网络结构作为特征点检测网络和描述子检测网络的共用编码网络，用以对图像进行降维，提取特征，减小后续网络的计算量。
本发明采用基于GhostNet架构的GhostNet编码层替代原superpoint网络的VGG编码层，GhostNet编码层包含卷积层Conv2d+批归一化层BN+激活层ReLU及六层Ghostbottleneck(G-bneck)，同时将网络结构宽度系数设置为1.5；其中，六层G-bneck的扩张尺寸依次增大，第四、五层G-bneck的扩张率为0.25，其余层为0；第一、三、五、六层G-bneck的步长为2，其余层为1。
表1.改进的SuperPoint网络中的GhostNet编码层结构
Figure BDA00035592630100000711
Figure BDA0003559263010000081
步骤3，对当前帧图像中的静态像素部分进行线特征提取；本实施例中，线特征提取的具体方法为：
首先计算静态像素部分图像的每个像素的水平线角度，从而得到一个单位向量场，使单位向量场中所有向量都和经过像素的水平线相切；接着再选取静态像素部分图像中方向连贯的图像像素区域，对于每个选出的所述图像像素区域，先将其视为一条线段s，若图像像素区域的梯度相互独立，则说明图像像素区域的像素之间不存在线段相关性，即该图像像素区域无线段存在；定义NFAM为像素梯度之间无相关性的警告次数：
Figure BDA0003559263010000082
其中，NLoI表示静态像素部分图像中线段可能存在的数量；γ表示归一化值；S表示存在一条可能包含n条子线段{s1,...,sn}的线段；u和v分别代表图像像素区域的长和宽；而K＝{k1,...,kn}，ki表示与子线段si(i＝1,2,…,n)方向相同的像素个数；p是在图像像素区域内的一个随机像素q与线段si的方向θs(q)相同的概率，β(|si|,ki,p)是二项分布，其函数为：
Figure BDA0003559263010000083
若NFAM(S,K,p)越小则说明某个像素区域中的像素梯度相互关联，即该像素区域可能存在线段；设定阈值η，当NFAM(S,K,p)<η，则s视为提取出的线特征。
步骤4，根据提取到的重复性单元和稠密描述子，获取像素级可重复性图
Figure BDA0003559263010000084
和分块级可重复性图
Figure BDA0003559263010000085
采用基于可重复性的残差替代传统直接法的光度残差，构造点特征重投影误差函数，从而估计机器人位姿。其中：
(1)像素级可重复性图
Figure BDA0003559263010000091
和分块级可重复性图
Figure BDA0003559263010000092
分别为：
Figure BDA0003559263010000093
Figure BDA0003559263010000094
其中，‘.’表示归一化后的重复性单元
Figure BDA0003559263010000095
中省略的通道‘:’表示从
Figure BDA0003559263010000096
中的第一个通道到最后一个通道，
Figure BDA0003559263010000097
s表示
Figure BDA0003559263010000098
Figure BDA0003559263010000099
即s指从可重复性图的像素块到二维预测值的映射函数。
(2)为了估计当前帧的相机位姿Tk，对于每个通过运动恢复结构SFM(Structure-From-Motion,SFM)得到的三维路标点pi(世界坐标系下的3d点)的可重复性残差
Figure BDA00035592630100000910
为：
Figure BDA00035592630100000911
Figure BDA00035592630100000912
其中，
Figure BDA00035592630100000913
代表不同分辨率的可重复性图(像素级可重复性图
Figure BDA00035592630100000914
和分块级可重复性图
Figure BDA00035592630100000915
)，π(·)表示相机投影方程，ξk表示第k帧相机位姿增量，Tk表示相机的当前位姿，
Figure BDA00035592630100000916
是ξk的反对称矩阵。
由于我们关心的是可重复性残差
Figure BDA00035592630100000917
是如何随着相机位姿Tk变化的，故得分析它们的导数关系：残差
Figure BDA00035592630100000918
的雅克比方程为：
Figure BDA00035592630100000919
Figure BDA00035592630100000920
其中，Jrepeat是可重复性图(像素级可重复性图
Figure BDA00035592630100000921
和分块级可重复性图
Figure BDA00035592630100000922
)中投影点的像素梯度，Jpeat是相机投影方程关于相机坐标系下的三维路标点
Figure BDA00035592630100000923
的偏导数，Jpose是投影点对位姿增量ξk的偏导数。
Figure BDA00035592630100000924
为三维路标点pi在相机坐标系下的坐标，
Figure BDA00035592630100000925
为三维路标点pi在第k帧图像像素坐标系下的坐标。在后续求解点特征重投影误差函数时，采用列文伯格-马夸尔特方法计算误差增量时需要用到该雅克比矩阵。
(3)在分块级可重复性图
Figure BDA0003559263010000101
上关联静态像素部分图像的点特征(由Superpoint网络的特征点检测层得到)和三维路标点(通过SFM可估计出三维路标点的空间位置)，然后使用Levenberg-Marquardt算法迭代优化以下误差函数
Figure BDA0003559263010000102
至收敛，可得位姿增量ξk：
Figure BDA0003559263010000103
其中，
Figure BDA0003559263010000104
表示是与当前帧有高共视关系的相邻帧的特征点集合，γ指鲁棒核函数，用于抑制异常值。
(4)在像素级可重复性图
Figure BDA0003559263010000105
上应用半径搜索算法，寻找与世界坐标系中的三维路标点pi的描述子最接近的像素点
Figure BDA0003559263010000106
然后将三维路标点重投影回当前帧图像的像素级可重复性图
Figure BDA0003559263010000107
上，通过最小化点特征重投影误差函数求得机器人的当前位姿Tk。
该重投影误差为：
Figure BDA0003559263010000108
其中，π(·)表示相机投影方程，Rk表示机器人相机当前位姿Tk对应的旋转矩阵，tk表示平移向量，pi表示三维路标点。
则最终的点特征重投影误差函数为：
Figure BDA0003559263010000109
Figure BDA00035592630100001010
其中，
Figure BDA00035592630100001011
表示是与当前帧有高共视关系的相邻帧的特征点集合，上标T表示转置，∑i,k表示像素点
Figure BDA00035592630100001012
的协方差矩阵，其决定特征点在优化中的权重；i为第i个三维路标点，k为第k帧；γ表示鲁棒核函数，upeak表示像素级可重复性图中的峰值像素点；
Figure BDA00035592630100001013
表示upeak的附近的像素点集合，u表示ui和uk，而ui和uk为当前帧图像
Figure BDA0003559263010000111
像素集合中的像素点，
Figure BDA0003559263010000112
分别为像素级可重复性图
Figure BDA0003559263010000113
中ui和uk位置处的值，ρ(·)为Cauchy函数，用于抑制异常值，E(·)表示求期望。
步骤5，利用对静态像素部分提取的线特征，构建线特征重投影误差函数，约束位姿估计；具体如下：
记P、Q是三维空间下的线段PQ的两个端点，pd,qd是线段端点P、Q在相机平面的投影经过位姿变换后所对应的两个端点，它们对应的齐次坐标分别为
Figure BDA0003559263010000114
Figure BDA0003559263010000115
则归一化线性系数I为：
Figure BDA0003559263010000116
将线特征的重投影误差
Figure BDA0003559263010000117
定义为投影线段端点到图像平面中变换后得到线段pdqd之间的距离，即：
Figure BDA0003559263010000118
Figure BDA0003559263010000119
Figure BDA00035592630100001110
其中，
Figure BDA00035592630100001111
指世界坐标系下的路标点Pi到投影线段pdqd的距离，
Figure BDA00035592630100001112
指世界坐标系下的地图点Qi到投影线段pdqd的距离，Ii为世界坐标系下的地图点Pi对应的归一化线性系数，T表示机器人相机位姿；K表示相机内参；π(·)指相机投影模型。
则最终的线特征重投影误差函数Eline为：
Figure BDA00035592630100001113
其中，χl指第I帧图像提取到的线特征集合，∑l为线段的两个端点(i,j)的协方差矩阵，ρ为Cauchy函数，用于抑制异常值。
步骤6，利用IMU获得机器人的IMU数据(即运动信息)，对连续两帧图像之间的IMU数据进行预积分，获取角度变化、速度变化、位置变化数据，构造IMU误差函数对位姿的迭代优化。
在视觉惯导视觉里程计中，除了要估计相机在世界坐标系下的位姿Ti，还要估计速度vi，陀螺仪的偏差
Figure BDA0003559263010000121
和加速度计的偏差
Figure BDA0003559263010000122
等IMU信息，这些状态量构成如下状态向量：
Figure BDA0003559263010000123
对IMU的测量值进行预积分，获得连续两帧(如第i帧到第i+1帧)之间的相对位姿信息，其中包括相对旋转信息ΔRi,i+1、相对速度信息Δvi,i+1、相对位移信息Δpi,i+1以及测量向量的信息矩阵
Figure BDA0003559263010000124
给定上述预积分项、第i帧的状态向量Si和第i+1帧状态向量Si+1，可以得到状态向量信息矩阵的预积分
Figure BDA0003559263010000125
为：
Figure BDA0003559263010000126
其中:
Figure BDA0003559263010000127
Figure BDA0003559263010000128
Figure BDA0003559263010000129
其中，Ii,i+1指从第i帧到第i+1帧的预积分，Ri和Ri+1分别指第i帧和第i+1帧的相机旋转矩阵，上标T表示矩阵转置，Ri,i+1指从第i帧到第i+1帧的旋转矩阵，vi和vi+1分别指第i帧和第i+1帧IMU对应的速度，vi,i+1指从i帧到第i+1帧IMU的相对速度，pi+1、pi指第i+1帧、第i帧IMU对应的位置，Δti,i+1指从第i帧到第i+1帧所用时间，Δvi,i+1表示从第i帧到第i+1帧的速度增量，Δpi,i+1表示从第i帧到第i+1帧的位移，g为重力加速度常数。
而第i帧图像和三维路标点xj的重投影误差rij为：
Figure BDA00035592630100001210
其中，uij表示三维路标点xj在第i帧图像上的二维投影点；Π(·)表示相机投影方程；TCB表示机器人坐标到相机坐标的刚体变换矩阵；Ti为相机在世界坐标系下的位姿，⊕表示李群SE(3)在三维空间上的转换运算。
视觉惯性优化目标函数就是最小化上述重投影误差，同时使用鲁棒的Huber核ρHub函数用以降低虚假错误特征匹配的影响，则对于给定的k+1关键帧和其相应的状态向量
Figure BDA0003559263010000131
以及给定的l个三维路标点集合和其相应的状态向量
Figure BDA0003559263010000132
它们的IMU误差函数为：
Figure BDA0003559263010000133
其中，
Figure BDA0003559263010000134
表示状态向量信息矩阵的预积分，
Figure BDA0003559263010000135
为
Figure BDA0003559263010000136
对应的协方差矩阵；rij表示第i帧图像和三维路标点xj的重投影误差，Kj表示第j个三维地图点的观测关键帧集合，ρ为Cauchy函数。
在本发明中，利用运动恢复结构法(SFM)估计相机位姿和三维点的空间坐标，然后与预积分数据对齐，利用前一帧对后一帧位置、速度的预测与当前值的误差，得到每帧的重力方向、比例因子、陀螺仪偏置和相应的速度，该过程与传统的SLAM方法相同，在本发明中不做特别赘述。
步骤7，最小化点特征重投影误差函数、线特征重投影误差函数以及IMU误差函数之和来优化所有状态变量，求得准确的机器人位姿信息。
针对视觉点线特征约束、IMU约束，构建残差目标函数；具体地，残差目标函数为：
min{ρ(Epoint)+Eline+Ei,i+1}
其中，Epoint为步骤4所得的点特征重投影误差函数，Eline为步骤5所得的线特征重投影误差函数，Ei,i+1为步骤6所得的IMU误差函数，ρ为Cauchy函数，用于抑制异常值。
通过Levenberg–Marquard法迭代优化出所需要的状态变量：
Figure BDA0003559263010000137
即可得到准确的位姿信息。
最后，还可以根据提取的点特征、线特征进行回环检测，与构建的点线词袋库匹配，并对步骤7得到的状态向量(包含机器人位姿信息)进行反馈矫正。其中，点线词袋库即步骤3、4中提取的点特征和线特征构成的特征库。
以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (10)

1.一种基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，包括：
实时采集机器人运动过程中通过相机拍摄的图像作为当前帧图像；对当前帧图像进行实例分割处理，从而区分得到当前帧图像中的动态像素和静态像素；
对于当前帧图像中的静态像素部分，提取预测的重复性单元和稠密描述子；
对当前帧图像中的静态像素部分进行线特征提取；
根据提取到的重复性单元和稠密描述子，获取像素级可重复性图和分块级可重复性图，采用基于可重复性的残差替代传统直接法的光度残差，构造点特征重投影误差函数，从而估计机器人位姿；
利用对静态像素部分提取的线特征，构建线特征重投影误差函数，约束位姿估计；
利用IMU获得机器人的IMU数据，对连续两帧图像之间的IMU数据进行预积分，构造IMU误差函数对位姿的迭代优化；
最小化点特征重投影误差函数、线特征重投影误差函数以及IMU误差函数之和来优化所有状态变量，求得准确的机器人位姿信息。
2.根据权利要求1所述的基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，所述的预测的重复性单元表示为
Figure FDA0003559263000000011
由特征提取网络的特征点检测层得到；稠密描述子表示为
Figure FDA0003559263000000012
由特征提取网络的描述子解码层得到；其中，
Figure FDA0003559263000000013
W表示当前帧图像的宽度，H表示当前帧图像的高度，C是网格的大小，
Figure FDA0003559263000000014
表示实数集；
对
Figure FDA0003559263000000015
归一化，得到归一化后的重复性单元
Figure FDA0003559263000000016
3.根据权利要求2所述的基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，所述特征提取网络采用改进后的SuperPoint网络，具体对该网络的改进如下：
采用基于GhostNet架构的GhostNet编码层替代原superpoint网络的VGG编码层，GhostNet编码层包含卷积层Conv2d+批归一化层BN+激活层ReLU及六层G-bneck，同时将网络结构宽度系数设置为1.5；其中，六层G-bneck的扩张尺寸依次增大，第四、五层G-bneck的扩张率为0.25，其余层为0；第一、三、五、六层G-bneck的步长为2，其余层为1。
4.根据权利要求1所述的基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，所述对当前帧图像中的静态像素部分进行线特征提取，包括：
首先计算静态像素部分图像的每个像素的水平线角度，从而得到一个单位向量场，使单位向量场中所有向量都和经过像素的水平线相切；接着再选取静态像素部分图像中方向连贯的图像像素区域，对于每个选出的所述图像像素区域，先将其视为一条线段s，若图像像素区域的梯度相互独立，则说明图像像素区域的像素之间不存在线段相关性，即该图像像素区域无线段存在；定义NFAM为像素梯度之间无相关性的警告次数：
Figure FDA0003559263000000021
其中，NLoI表示静态像素部分图像中线段可能存在的数量；γ表示归一化值；S表示存在一条可能包含n条子线段{s1，...，sn}的线段；u和v分别代表图像像素区域的长和宽；而K＝{k1，...，kn}，ki表示与子线段si(i＝1，2，...，n)方向相同的像素个数；p是在图像像素区域内的一个随机像素q与线段si的方向θs(q)相同的概率，β(|si|，ki，p)是二项分布；
若NFAM(S，K，p)越小则说明某个像素区域中的像素梯度相互关联，即该像素区域可能存在线段；设定阈值η，当NFAM(S，K，p)＜η，则s视为提取出的线特征。
5.根据权利要求1所述的基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，所述采用基于可重复性的残差替代传统直接法的光度残差，构造点特征重投影误差函数，从而估计机器人位姿，包括：
为了估计当前帧的相机位姿Tk，构建每个通过运动恢复结构SFM得到的三维路标点pi的可重复性残差
Figure FDA0003559263000000022
在分块级可重复性图
Figure FDA0003559263000000023
上关联静态像素部分图像的点特征和三维路标点，然后使用Levenberg-Marquardt算法迭代优化误差函数至收敛，可得位姿增量ξk：
在像素级可重复性图
Figure FDA0003559263000000024
上应用半径搜索算法，寻找与世界坐标系中的三维路标点pi的描述子最接近的像素点
Figure FDA0003559263000000025
然后将三维路标点重投影回当前帧图像的像素级可重复性图
Figure FDA00035592630000000315
上，通过最小化点特征重投影误差函数求得机器人的当前位姿Tk。
6.根据权利要求5所述的基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，所述可重复性残差
Figure FDA0003559263000000031
表示为：
Figure FDA0003559263000000032
Figure FDA0003559263000000033
其中，
Figure FDA0003559263000000034
代表不同分辨率的像素级可重复性图
Figure FDA0003559263000000035
和分块级可重复性图
Figure FDA0003559263000000036
π(·)表示相机投影方程，ξk表示第k帧相机位姿增量，Tk表示相机的当前位姿，
Figure FDA0003559263000000037
是ξk的反对称矩阵；
使用Levenberg-Marquardt算法迭代优化以下误差函数
Figure FDA0003559263000000038
至收敛，可得位姿增量ξk：
Figure FDA0003559263000000039
其中，
Figure FDA00035592630000000310
表示是与当前帧有高共视关系的相邻帧的特征点集合，γ指鲁棒核函数；
点特征重投影误差函数为：
Figure FDA00035592630000000311
其中：
Figure FDA00035592630000000312
上式中，Rk表示机器人相机当前位姿Tk对应的旋转矩阵，tk表示平移向量，pi表示三维路标点，
Figure FDA00035592630000000313
表示是与当前帧有高共视关系的相邻帧的特征点集合，上标T表示转置，∑i，k表示像素点
Figure FDA00035592630000000314
的协方差矩阵，其决定特征点在优化中的权重；i为第i个三维路标点，k为第k帧；γ表示鲁棒核函数。
7.根据权利要求1所述的基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，所述利用对静态像素部分提取的线特征，构建线特征重投影误差函数，约束位姿估计，包括：
记P、Q是三维空间下的线段PQ的两个端点，pd，qd是线段端点P、Q在相机平面的投影经过位姿变换后所对应的两个端点，它们对应的齐次坐标分别为
Figure FDA0003559263000000041
Figure FDA0003559263000000042
从而构建归一化线性系数；
将线特征的重投影误差
Figure FDA0003559263000000043
定义为投影线段端点到图像平面中变换后得到线段pdqd之间的距离，即：
Figure FDA0003559263000000044
Figure FDA0003559263000000045
Figure FDA0003559263000000046
其中，
Figure FDA0003559263000000047
指世界坐标系下的路标点Pi到投影线段pdqd的距离，
Figure FDA0003559263000000048
指世界坐标系下的地图点Qi到投影线段pdqd的距离，Ii为世界坐标系下的地图点Pi对应的归一化线性系数，T表示机器人相机位姿；K表示相机内参；
则最终的线特征重投影误差函数Eline为：
Figure FDA0003559263000000049
其中，χl指第I帧图像提取到的线特征集合，∑l为线段的两个端点(i，j)的协方差矩阵，ρ为Cauchy函数。
8.根据权利要求1所述的基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，所述利用IMU获得机器人的IMU数据，对连续两帧图像之间的IMU数据进行预积分，构造IMU误差函数对位姿的迭代优化，包括：
对IMU的测量值进行预积分，获得连续两帧之间的相对位姿信息，从而得到状态向量信息矩阵的预积分；
构建第i帧图像和三维路标点xj的重投影误差rij；
对于给定的k+1关键帧和其相应的状态向量
Figure FDA00035592630000000411
以及给定的l个三维路标点集合和其相应的状态向量X，它们的IMU误差函数为：
Figure FDA00035592630000000410
其中，
Figure FDA0003559263000000051
表示状态向量信息矩阵的预积分，
Figure FDA0003559263000000052
为
Figure FDA0003559263000000053
对应的协方差矩阵；rij表示第i帧图像和三维路标点xj的重投影误差，Kj表示第j个三维地图点的观测关键帧集合，ρ为Cauchy函数。
9.根据权利要求1所述的基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，所述最小化点特征重投影误差函数、线特征重投影误差函数以及IMU误差函数之和来优化所有状态变量，求得准确的机器人位姿信息，包括：
针对视觉点线特征约束、IMU约束，构建残差目标函数：
min{ρ(Epoint)+Eline+Ei，i+1}
其中，Epoint为点特征重投影误差函数，Eline为线特征重投影误差函数，Ei，i+1为IMU误差函数，ρ为Cauchy函数；
通过Levenberg-Marquard法迭代优化出所需要的状态变量：
Figure FDA0003559263000000054
其中，Ti为相机在世界坐标系下的位姿，vi为速度，
Figure FDA0003559263000000055
为陀螺仪的偏差，
Figure FDA0003559263000000056
为加速度计的偏差；
即可得到准确的位姿信息。
10.根据权利要求1所述的基于深度学习点线特征和IMU紧耦合的机器人运动估计方法，其特征在于，所述方法还包括：
根据提取的点特征、线特征进行回环检测，与构建的点线词袋库匹配，并对所述状态向量进行反馈矫正；其中，点线词袋库即提取的点特征和线特征构成的特征库。
CN202210283858.8A 2022-03-22 2022-03-22 基于深度学习点线特征和imu紧耦合的机器人运动估计方法 Pending CN114708293A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210283858.8A CN114708293A (zh) 2022-03-22 2022-03-22 基于深度学习点线特征和imu紧耦合的机器人运动估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210283858.8A CN114708293A (zh) 2022-03-22 2022-03-22 基于深度学习点线特征和imu紧耦合的机器人运动估计方法

Publications (1)

Publication Number Publication Date
CN114708293A true CN114708293A (zh) 2022-07-05

Family

ID=82169759

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210283858.8A Pending CN114708293A (zh) 2022-03-22 2022-03-22 基于深度学习点线特征和imu紧耦合的机器人运动估计方法

Country Status (1)

Country Link
CN (1) CN114708293A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115063485A (zh) * 2022-08-19 2022-09-16 深圳市其域创新科技有限公司 三维重建方法、装置及计算机可读存储介质
CN115178944A (zh) * 2022-08-04 2022-10-14 广东工业大学 一种安全强化学习的狭窄空间机器人作业规划方法
CN116468786A (zh) * 2022-12-16 2023-07-21 中国海洋大学 一种面向动态环境的基于点线联合的语义slam方法
CN116499455A (zh) * 2023-06-19 2023-07-28 煤炭科学研究总院有限公司 定位方法及装置

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115178944A (zh) * 2022-08-04 2022-10-14 广东工业大学 一种安全强化学习的狭窄空间机器人作业规划方法
CN115178944B (zh) * 2022-08-04 2024-05-24 广东工业大学 一种安全强化学习的狭窄空间机器人作业规划方法
CN115063485A (zh) * 2022-08-19 2022-09-16 深圳市其域创新科技有限公司 三维重建方法、装置及计算机可读存储介质
CN116468786A (zh) * 2022-12-16 2023-07-21 中国海洋大学 一种面向动态环境的基于点线联合的语义slam方法
CN116468786B (zh) * 2022-12-16 2023-12-26 中国海洋大学 一种面向动态环境的基于点线联合的语义slam方法
CN116499455A (zh) * 2023-06-19 2023-07-28 煤炭科学研究总院有限公司 定位方法及装置
CN116499455B (zh) * 2023-06-19 2023-11-14 煤炭科学研究总院有限公司 定位方法及装置

Similar Documents

Publication Publication Date Title
Usenko et al. Visual-inertial mapping with non-linear factor recovery
CN111862126B (zh) 深度学习与几何算法结合的非合作目标相对位姿估计方法
CN107980150B (zh) 对三维空间建模
CN114708293A (zh) 基于深度学习点线特征和imu紧耦合的机器人运动估计方法
CN112132897A (zh) 一种基于深度学习之语义分割的视觉slam方法
He et al. Vision-based UAV flight control and obstacle avoidance
CN110717927A (zh) 基于深度学习和视惯融合的室内机器人运动估计方法
CN114424250A (zh) 结构建模
CN112115980A (zh) 基于光流跟踪和点线特征匹配的双目视觉里程计设计方法
CN102722697A (zh) 一种无人飞行器视觉自主导引着陆的目标跟踪方法
CN111797688A (zh) 一种基于光流和语义分割的视觉slam方法
CN112101160B (zh) 一种面向自动驾驶场景的双目语义slam方法
CN113516664A (zh) 一种基于语义分割动态点的视觉slam方法
Qian et al. Robust visual-lidar simultaneous localization and mapping system for UAV
Chen et al. A stereo visual-inertial SLAM approach for indoor mobile robots in unknown environments without occlusions
CN111581313A (zh) 一种基于实例分割的语义slam鲁棒性改进方法
CN110176022B (zh) 一种基于视频检测的隧道全景监控系统及方法
Saleem et al. Neural network-based recent research developments in SLAM for autonomous ground vehicles: A review
CN116619358A (zh) 一种矿山自主探测机器人自适应定位优化与建图方法
Yu et al. Accurate and robust visual localization system in large-scale appearance-changing environments
CN113345032B (zh) 一种基于广角相机大畸变图的初始化建图方法及系统
Ge et al. Vipose: Real-time visual-inertial 6d object pose tracking
Samadzadeh et al. Srvio: Super robust visual inertial odometry for dynamic environments and challenging loop-closure conditions
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN111950599A (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