CN116147618B - 一种适用动态环境的实时状态感知方法及*** - Google Patents
一种适用动态环境的实时状态感知方法及*** Download PDFInfo
- Publication number
- CN116147618B CN116147618B CN202310061217.2A CN202310061217A CN116147618B CN 116147618 B CN116147618 B CN 116147618B CN 202310061217 A CN202310061217 A CN 202310061217A CN 116147618 B CN116147618 B CN 116147618B
- Authority
- CN
- China
- Prior art keywords
- dynamic
- frame
- points
- feature points
- real
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 94
- 238000001514 detection method Methods 0.000 claims abstract description 56
- 230000003068 static effect Effects 0.000 claims abstract description 42
- 230000009466 transformation Effects 0.000 claims abstract description 30
- 230000010354 integration Effects 0.000 claims abstract description 18
- 238000012216 screening Methods 0.000 claims abstract description 15
- 238000011156 evaluation Methods 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 32
- 230000004044 response Effects 0.000 claims description 21
- 238000013519 translation Methods 0.000 claims description 20
- 238000000605 extraction Methods 0.000 claims description 15
- 230000008859 change Effects 0.000 claims description 10
- 238000013507 mapping Methods 0.000 claims description 9
- 238000005192 partition Methods 0.000 claims description 6
- 238000012795 verification Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 230000007480 spreading Effects 0.000 claims description 3
- 238000003892 spreading Methods 0.000 claims description 3
- 238000003780 insertion Methods 0.000 claims description 2
- 230000037431 insertion Effects 0.000 claims description 2
- 230000008030 elimination Effects 0.000 abstract description 5
- 238000003379 elimination reaction Methods 0.000 abstract description 5
- 238000004422 calculation algorithm Methods 0.000 description 24
- 230000033001 locomotion Effects 0.000 description 17
- 230000000007 visual effect Effects 0.000 description 17
- 238000010276 construction Methods 0.000 description 12
- 238000005457 optimization Methods 0.000 description 12
- 238000004364 calculation method Methods 0.000 description 10
- 238000013135 deep learning Methods 0.000 description 10
- 230000000694 effects Effects 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 8
- 230000011218 segmentation Effects 0.000 description 8
- 230000006872 improvement Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000005259 measurement Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 230000004927 fusion Effects 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 4
- 238000009826 distribution Methods 0.000 description 3
- 238000012549 training Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 241000282472 Canis lupus familiaris Species 0.000 description 1
- 241000282326 Felis catus Species 0.000 description 1
- 206010034719 Personality change Diseases 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000012512 characterization method Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 239000012014 frustrated Lewis pair Substances 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 230000014759 maintenance of location Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000013450 outlier detection Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 230000000644 propagated effect Effects 0.000 description 1
- 230000006833 reintegration Effects 0.000 description 1
- 238000013341 scale-up Methods 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
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
- G01C21/1656—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 with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C3/00—Measuring distances in line of sight; Optical rangefinders
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/77—Processing image or video features in feature spaces; using data integration or data reduction, e.g. principal component analysis [PCA] or independent component analysis [ICA] or self-organising maps [SOM]; Blind source separation
- G06V10/774—Generating sets of training patterns; Bootstrap methods, e.g. bagging or boosting
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
- G06V10/82—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using neural networks
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V2201/00—Indexing scheme relating to image or video recognition or understanding
- G06V2201/07—Target detection
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Artificial Intelligence (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Health & Medical Sciences (AREA)
- Computing Systems (AREA)
- Databases & Information Systems (AREA)
- General Health & Medical Sciences (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种适用动态环境的实时状态感知方法及***，该方法包括：对RGB‑D相机采集的图像信息基于目标检测模型进行识别，生成先验动态框；对IMU传感器采集的姿态数据进行IMU预积分，得到帧间位姿相对变换关系；对先验动态框在各层图像金字塔上生成目标框掩码，经八叉树筛选提取特征点；根据帧间位姿相对变换关系判断每个匹配特征点的真实动态性；基于深度图上区域生长法生成动态区域掩码和静态区域掩码，基于动态特征点剔除策略，保留静态特征点并进行初始位姿估计；在***关键帧策略中加入对匹配动态点数量以及内点数量的评价，输出关键帧；进行局部建图以及回环检测，通过存储的数据关联进行修正，实现动态场景的导航与定位。
Description
技术领域
本发明属于陆地及航空导航技术领域，尤其涉及一种适用动态环境的实时状态感知方法及***。
背景技术
视觉SLAM(Simultaneous Localization and Mapping,即时定位与地图构建)是指以视觉传感器为中心的同步定位与地图构建算法，经典视觉SLAM框架由传感器、前端视觉里程计、后端优化、回环检测以及建图构成。现有主流视觉SLAM算法假定环境中物体是静止的或低运动的，但实际应用中，场景大多具有复杂性和动态性，RANSAC算法(RandomSample Consensus,随机采样一致性)等方法能够在一定程度上解决占据画面较小且数量较少的动态目标带来的问题，但是当动态目标上特征点数量较多时，这种方法会失效，SLAM***分不清载体与物体的运动，导致定位不准确、建图出现严重偏差，***的鲁棒性和精度大大降低。为了解决这个问题，大量动态鲁棒性SLAM算法被提出，主要针对主流视觉SLAM算法的前端进行改进，前端粗略的估计相邻两帧图像之间相机的运动，主要可以分为特征点法和直接法。
如何有效区分并精准剔除动态特征点，消除动态物体对位姿估计的影响，是状态感知***能否在动态环境中为运动平台提供高效、稳定技术支撑的关键。分割图像中的动态目标主要可以分为基于几何的方法、基于光流的方法和结合深度学习的方法。基于几何的方法利用对极几何特性来分割静态和动态特征，动态要素将违反多视图几何中为静态场景定义的标准约束，这种方法具有较小的计算量以及较好的实时性，但是无法区分由运动物体引起的残差和错误匹配引起的残差。基于光流的方法通过描述像素在不同图像帧之间的运动状态追踪动态目标上像素的运动过程，这种方法不需要先验场景信息，对轻微运动敏感，但是易受光照影响且缺乏实际数据以及客观的评价指标。结合深度学习的方法是主流动态SLAM方案，又可以分为基于语义分割和目标检测的方法，通过设计先验目标动态信息，检测并去除潜在动态物体特征，对于特定场景的特定目标十分有效，但是出现未训练物体时鲁棒性会大大降低，其中基于语义分割的方法精度较高但很难达到实时要求，基于目标检测的方法能够实时工作但不能实现精确分割，可能会因特征点过少而影响定位精度。大部分动态SLAM方案会结合其中的两种方法在动态复杂环境中实现更加鲁棒的状态感知效果。
2018年，清华大学的Yu等人提出的DS-SLAM将语义分割网络SegNet和移动一致性检测方法相结合，滤出场景中的动态部分，相比于ORB-SLAM2，DS-SLAM增加了语义分割和稠密的语义八叉树地图构建线程。DS-SLAM存在的问题有：1)语义分割时间开销为37.6ms，与跟踪线程大致相当，实时性很难保证；2)在语义分割网络中可以识别的对象类型受到限制，限制了其应用范围。3)极线约束的外点检测方法并不能找到所有外点，当物体沿极线方向运动时这种方法会失效。2018年被提出的DynaSLAM同样以ORB-SLAM2为基础，增加了动态物体检测和背景修复的能力，使用RGB-D相机时通过多视图几何和深度学习结合的方法来检测运动物体，使用单目或双目相机时仅使用深度学习的方法。该方法的缺点在于：1)将所有潜在动态点去除，可能会导致剩余静止特征点过少而影响SLAM效果。2)DynaSLAM不是实时的，耗时主要是因为Region Growing算法。2020年被提出的RDS SLAM建立在ORB-SLAM3的基础上，添加了一个语义线程和一个基于语义的优化线程，利用移动概率来更新和传播语义信息，并使用数据关联算法来去除跟踪中的异常值，RDS SLAM解决了基于语义的方法的实时性问题，但是仅针对关键帧剔除异常值，对于大型剧烈运动的动态目标的扰动剔除存在局限性。
大部分现有的动态SLAM方法存在的问题包括：1)许多使用深度学***衡实时性与准确性成为动态感知领域亟待解决的问题。
发明内容
本发明的目的在于克服现有技术缺陷，提出了一种适用动态环境的实时状态感知方法及***。
为了实现上述目的，本发明提出了一种适用动态环境的实时状态感知方法，基于RGB-D相机和IMU传感器实现，所述方法包括：
步骤1)对RGB-D相机采集的图像信息基于预先训练好的目标检测模型进行动态物体的识别，生成先验动态框；
步骤2)对IMU传感器采集的姿态数据进行IMU预积分，得到当前帧与前一帧的帧间位姿相对变换关系；
步骤3)对先验动态框在各层图像金字塔上生成目标框掩码，经八叉树筛选提取特征点；
步骤4)对当前帧和前一帧的特征点进行匹配，根据帧间位姿相对变换关系判断每个匹配特征点的真实动态性；
步骤5)结合匹配特征点的真实动态性，基于深度图上区域生长法生成动态区域掩码和静态区域掩码，基于动态特征点剔除策略，剔除动态特征点并保留静态特征点，再基于静态特征点进行初始位姿估计；
步骤6)在***关键帧策略中加入对匹配动态点数量以及内点数量的评价，输出关键帧；
步骤7)对关键帧进行局部建图以及回环检测，通过存储的数据关联进行修正，实现动态场景的导航与定位。
作为上述方法的一种改进，所述步骤1)的目标检测模型为PP-PicoDet网络，所述PP-PicoDet网络采用ESNet作为骨干网络，颈部采用CSP-PAN结构。
作为上述方法的一种改进，所述步骤2)的帧间位姿相对变换关系具体包括：位移变化、速度变化和姿态变化的变换关系。
作为上述方法的一种改进，所述步骤3)的图像金字塔层数为8层，所有层级提取到的特征点数之和为1000，图像金字塔相邻层级间的缩放系数为1.2。
作为上述方法的一种改进，所述步骤3)具体包括：
将每一个图像金字塔层级均匀分为若干个区域，遍历每个区域，进行以下处理：
使用设定的高响应阈值搜索特征点，如果找到特征点，则记录；如果没有找到特征点，使用设定的低响应阈值继续搜索，如果找到特征点，则记录，如果没找到，则移动到下一个区域；
当遍历完所有区域，将先验动态目标框掩码标记区域提取到的特征点响应值降低为原来的0.8倍，对得到的所有特征点进行八叉树筛选，不断将存在特征点的区域进行4等分，直到分出设定数目的足够多分区，每个分区内只保留响应值最大的特征点，所述特征点为ORB特征。
作为上述方法的一种改进，所述步骤4)具体包括：
根据IMU预积分数据得到相邻两帧I1、I2间的刚体姿态变换矩阵为包括旋转矩阵/>和平移向量/>根据I1载体系在世界坐标系下的旋转矩阵/>平移向量/>速度向量/>帧间IMU采样时间间隔之和△t、重力向量g，计算I2在世界坐标系下的位姿：
其中，分别为I2载体系在世界坐标系下的旋转矩阵和平移向量；
使用Kalibr标定工具进行相机和IMU的联合标定，得到的相机系与IMU载体系之间的旋转变换Rbc和平移变换tbc，根据下式得到I2相机系在世界坐标系下的旋转矩阵和平移向量/>
根据图像I1中的像素点p1在图像I2中的匹配点p2的像素坐标为(u2,v2)、标定得到的相机内参fx、fy、cx、cy以及RGB-D相机确定的像素点p2对应空间点的深度由下式得到p2对应空间点在I2相机坐标系下的坐标/>
根据下式计算p2对应空间点在世界坐标系下的坐标
已知p1对应空间点在世界坐标系下的坐标根据下式计算p1、p2对应空间点的3D距离误差δ：
计算出每个匹配点对的3D距离误差δ后，根据相邻两帧之间的IMU预积分量的协方差以及置信概率设定阈值，判断特征点的真实动态性。
作为上述方法的一种改进，所述步骤5)具体包括：
采用深度图上的自适应区域生长法，以匹配特征点作为种子，通过区域生长法蔓延得到动态区域与静态区域；
根据步骤1)得到的目标框掩码以及各目标框对应物体的类别，步骤4)得到的匹配特征点的真实动态性，以及步骤5)得到的动态区域掩码和静态区域掩码进行判断，基于特征点剔除策略得到静态特征点。
另一方面，本发明提出了一种适用动态环境的实时状态感知***，基于RGB-D相机和IMU传感器实现，所述***包括部署在前端的目标检测模块、预积分模块、特征提取模块、动态性验证模块、剔除模块和关键帧输出模块，以及部署在后端的建图回环检测模块；其中，
所述目标检测模块，用于对RGB-D相机采集的图像信息基于预先训练好的目标检测模型进行动态物体的识别，生成先验动态框；
所述预积分模块，用于对IMU传感器采集的姿态数据进行IMU预积分，得到当前帧与前一帧的帧间位姿相对变换关系；
所述特征提取模块，用于对先验动态框在各层图像金字塔上生成目标框掩码，经八叉树筛选提取特征点；
所述动态性验证模块，用于对当前帧和前一帧的特征点进行匹配，根据帧间位姿相对变换关系判断每个匹配特征点的真实动态性；
所述剔除模块，用于结合匹配特征点的真实动态性，基于深度图上区域生长法生成动态区域掩码和静态区域掩码，基于动态特征点剔除策略，剔除动态特征点并保留静态特征点，再基于静态特征点进行初始位姿估计；
所述关键帧输出模块，用于在***关键帧策略中加入对匹配动态点数量以及内点数量的评价，输出关键帧；
建图回环检测模块，用于对关键帧进行局部建图以及回环检测，通过存储的数据关联进行修正，实现动态场景的导航与定位。
与现有技术相比，本发明的优势在于：
1、现有主流视觉SLAM算法假定环境中物体是静止的或低运动的，但实际应用中，场景大多具有动态性，此时***的鲁棒性和精度会大大降低，本发明针对该问题提出一种利用视觉/惯性传感器，适用动态环境的状态感知***及感知方法，通过基于深度学习的目标检测以及基于IMU(Inertial Measurement Unit,惯性测量单元)信息的几何方法，有效识别环境中的动态物体并剔除动态特征点，仅保留静态特征点用于跟踪、优化以及地图构建，提高复杂环境条件下状态感知***的可靠性，为机器人在真实动态环境中的路径规划、动作控制等提供鲁棒、稳定的位姿和导航信息，也可为无人驾驶、无人机导航与控制等领域在真实动态环境下的导航难题提供解决方案；
2、本申请针对主流基于视觉SLAM的状态感知方法在实际应用的复杂动态环境中鲁棒性和精度大大降低的问题，提出了一种利用视觉/惯性传感器，适用动态环境的状态感知***及感知方法，针对传统动态SLAM方案在精度与速度平衡上存在的问题，提出了基于深度学习的目标检测和基于IMU信息的几何方法结合的动态SLAM方案，本申请采用了基于目标检测生成动态先验的特征提取方法，在剔除动态特征点的同时保证剩余特征点的数量和质量，带来了更加稳定可靠的追踪效果；
3、本申请采用轻量级目标检测模型PP-PicoDet，获得的语义先验动态信息应用于动态SLAM方案的多个阶段，满足实时性的同时带来了丰富的语义信息；
4、本申请采用3D距离误差判断特征点真实动态性的移动一致性检测方法，选择能够直接反映3D路标点P在各轴方向上动态程度的参数，更加直观、精确地反映特征点真实动态性。
附图说明
图1是本发明的适用动态环境的实时状态感知方法整体架构图；
图2是RGB-D相机+IMU传感器融合的视觉/惯性组合导航技术结构图；
图3是RGB-D图像特征提取示意图；
图4是简化金字塔结构示意图；
图5是各层图像金字塔上特征提取流程；
图6是PP-PicoDet网络架构；
图7是对极几何约束示意图；
图8是3D距离误差判断特征点真实动态性示意图；
图9是深度图上的区域生长法流程图；
图10是特征点剔除策略。
具体实施方式
技术方案中首先说明了本发明的整体算法框架，再对其中的关键技术进行说明，包括RGB-D相机+IMU传感器融合的视觉/惯性组合导航技术、针对边缘计算场景的轻量级目标检测网络、基于IMU预积分信息与几何约束的移动一致性检测技术、基于深度图上区域生长法的动态特征点剔除策略。
算法整体框架：
本发明采用基于RGB-D相机的视觉惯性紧耦合SLAM算法框架，算法整体架构由前端和后端组成，前端识别环境中的动态物体并剔除动态特征点，根据剩余的静态信息与IMU信息融合估计出粗略的相机运动，给后端提供较好的没有动态物体影响的初始值，后端通过局部BA(Bundle Adjustment,光束法平差)、回环检测和全局BA等方法再次优化位姿、地图点，最终获得精度较高的位姿估计结果。为了实现在动态环境中的稳定定位，在算法前端加入轻量级目标检测网络和基于几何约束的特征点真实动态性判断方法，本发明的算法整体架构如图1所示。
该发明在当前帧创建后首先运用预训练的目标检测算法快速检测出画面中“车”“人”等潜在运动物体，生成先验动态框，同时进行IMU预积分得到当前帧与前一帧的帧间位姿相对变换关系。
然后，在特征点提取阶段利用先验动态框在各层图像金字塔上生成目标框掩码，减小目标框掩码标记区域内提取到特征点的响应值，以便在八叉树筛选时保留更多框外潜在静止区域特征点。
接下来，验证特征点的真实动态性：当前帧与前一帧进行特征点匹配，得到匹配点对，根据IMU预积分的得到的帧间位姿相对变换关系求得预测点位与匹配点位之间的差值，根据动态阈值判断每个匹配特征点的真实动态性。
接下来，以匹配特征点的真实动态性为辅助信息，利用深度图上的区域生长法分割动态区域。
接下来，根据先验动态框信息、匹配特征点动态信息以及区域生长法生成的动态区域和静态区域掩码设计特征点剔除策略，最大限度剔除动态特征点并保留静态特征点。
接下来，利用剩余特征点进行初始位姿估计后，在***关键帧策略中加入对匹配动态点数量以及内点数量的评价，提高用于建图和回环检测的关键帧质量。
最后输出关键帧进行局部建图以及回环检测，通过中期数据关联和长期数据关联修正视觉惯性里程计累计误差和建图误差，实现中、大型动态场景的高精度导航与定位。
下面结合附图和实施例对本发明的技术方案进行详细的说明。
实施例1
本发明的实施例1提出了一种适用动态环境的实时状态感知方法，基于RGB-D相机和IMU传感器实现，所述方法包括：
步骤1)对RGB-D相机采集的图像信息基于预先训练好的目标检测模型进行动态物体的识别，生成先验动态框；
步骤2)对IMU传感器采集的姿态数据进行IMU预积分，得到当前帧与前一帧的帧间位姿相对变换关系；
步骤3)对先验动态框在各层图像金字塔上生成目标框掩码，经八叉树筛选提取特征点；
步骤4)对当前帧和前一帧的特征点进行匹配，根据帧间位姿相对变换关系判断每个匹配特征点的真实动态性；
步骤5)结合匹配特征点的真实动态性，基于深度图上区域生长法生成动态区域掩码和静态区域掩码，基于动态特征点剔除策略，剔除动态特征点并保留静态特征点，再基于静态特征点进行初始位姿估计；
步骤6)在***关键帧策略中加入对匹配动态点数量以及内点数量的评价，输出关键帧；
步骤7)对关键帧进行局部建图以及回环检测，通过存储的数据关联进行修正，实现动态场景的导航与定位。
下面对具体的步骤加以详细阐述。
1RGB-D相机+IMU传感器融合的视觉/惯性组合导航技术
惯性导航与视觉导航的优劣势有互补性，本发明采用的视觉/惯性组合导航技术融合了双方的优势，IMU能够提供快速运动时的定位，视觉能够减少IMU发散和累计误差。RGB-D相机+IMU传感器融合的视觉/惯性组合导航技术的结构如图2所示。
1.1基于ORB特征的前端视觉里程计技术
本发明的视觉前端图像处理技术采用特征法中的点特征，RGB-D相机提取特征的流程如图3所示。
好的追踪效果需要特征点的数量充足并且在画面中尽可能均匀分布，大部分动态SLAM算法在得到图像帧后立刻提取特征，然后再根据语义分割或目标检测或几何约束结果剔除动态特征点，特征提取算法在一帧图像中提取的特征点数目是固定的，当动态目标占据画面面积较大且特征较好时，大量特征点被剔除并且可能导致图像中某一个方向区域特征点数目极少或为零，进而导致追踪失败并影响后续定位与建图。本发明充分考虑该问题，在得到RGB图像帧后首先通过一个轻量级目标检测网络得到“车”、“人”等先验动态目标的检测框，根据检测框生成目标框掩码，然后在八叉树筛选特征点阶段控制目标框掩码标记区域的响应值，抑制该区域特征点的提取，以保证剔除动态特征点后剩余特征点的数量和质量，进而保证跟踪以及后续定位与建图的效果。
(1)ORB特征
特征点由关键点和描述子两部分组成，关键点是指特征点在图像中的位置，描述子通常是人为设计的向量，描述了该关键点周围像素的信息，本发明采用的ORB特征(Oriented FAST and Rotated BRIEF)的关键点为Oriented FAST，描述子为BRIEF。
Oriented FAST是一种改进的FAST角点，FAST角点认为与邻域像素差别较大的像素为角点，计算较为简单因此速度很快。FAST角点的检测需要设定一个阈值，对于每一个像素，判断其周围半径为3的圆上的16个像素亮度是否在设定阈值之外，若有连续N个像素的亮度在阈值之外则该像素被认为是角点，根据N的设置不同，可以分为FAST-12、FAST-11、FAST-9等，本发明中N的设定为12。
Oriented FAST相比于FAST增加了方向信息，添加了尺度和旋转的描述，为BRIEF描述子增加了旋转不变特性，其中尺度信息通过构建图像金字塔并在每一层检索角点得到。将输入图片逐级缩放得到图像金字塔，金字塔底层为原始图像，金字塔层级越高，图像分辨率越低。本发明设定的金字塔层数为8层，所有层级提取到的特征点数之和为1000，图像金字塔相邻层级间的缩放系数为1.2，简化金字塔结构示意图如图4所示，使用金字塔可以匹配不同缩放倍率下的图像，从而实现了尺度不变性。
Oriented FAST角点的旋转信息通过计算特征点附近的图像灰度质心得到，本发明使用特征点周围半径19大小的圆作为图像块计算其灰度质心方向，计算过程如下：
1)定义图像块的所有像素的灰度值为I，x为所有像素的x坐标，y为所有像素的y坐标，定义图像块的矩为：
2)计算图像块的质心为：
3)连接图像块几何中心O与质心C，得到方向向量即为特征点的方向：
(2)特征点提取与八叉树筛选
特征点响应值描述特征点的区分度大小，响应值越大的点越应该被保留，本发明的特征点响应值定义为中心点和周围16个像素的绝对偏差之和，
为了在动态环境下依然实现特征点均匀分布的目标，本发明使用了三个技巧：
1)将每一个图像金字塔层级均匀分为若干个区域，在每个区域搜索特征点，若该区域特征点响应值普遍较小则降低标准再次搜索。
2)对得到的所有特征点进行八叉树筛选，避免某区域特征点过于密集，选取其中响应值最大的点。
3)进行八叉树筛选前，将先验动态目标框掩码标记区域提取到的特征点响应值降低为原来的0.8倍，以抑制掩码标记区域特征点的保留。
特征点提取流程如图5所示。
八叉树筛选特征点即对特征点进行非极大值抑制，不断将存在特征点的区域进行4等分，直到分出足够多的分区，每个分区内只保留响应值最大的特征点。
1.2IMU预积分
IMU频率比图像频率高很多，两个图像之间会存在很多个IMU数据，在迭代优化过程中，历史状态的更新会导致IMU重新积分，给优化带来较大计算负担，因此本发明采用IMU预积分的方法，将图像帧间测量点的IMU信息整合为一个值，获得一个不依赖前一帧的帧间位姿相对变换关系。
IMU的误差可以分为确定性误差和随机误差，确定性误差包括bias(零偏)、scale(比例因子误差)等，随机误差包括高斯白噪声、bias随机游走等，确定性误差可以通过事先标定确定。仅考虑高斯白噪声n和bias随机游走b的IMU模型为：
IMU的真实值为ω、α，测量值为上标g表示gyro(陀螺仪)，a表示acc(加速度计)，w表示世界坐标系，b表示IMU机体坐标系。
P(pose，位置),V(velocity，速度),Q(quaternion，四元数)对时间的导数为：
根据该求导关系，根据i时刻的PVQ可以求得j时刻的PVQ：
每次优化更新后，都需要重新进行积分，带来非常大的计算负担，因此将积分模型转换为预积分模型：
转换后，积分公式中的积分项变为相对于i时刻的姿态：
预积分量仅仅与IMU测量值有关，位移变化、速度变化、姿态变化的预积分量为：
1.3滑动窗口算法
本发明中的SLAM问题采用基于图优化的优化方法，构建误差函数，利用最小二乘得到状态量的最优估计。随着***不断探索新环境，会有新的相机姿态以及新的环境特征，最小二乘残差会越来越多，信息矩阵越来越大，计算量不断增加，为了控制优化变量的规模，保证计算的实时性，使用滑动窗口算法动态增加或移除优化变量，并引入边缘化和舒尔补来解决变量增加和移除时信息矩阵的变化问题。滑动窗口算法的大致流程为：1)增加新的变量进入最小二乘***优化；2)如果变量数目达到一定维度，则移除老的变量；3)不断循环以上两步。
滑动窗口算法存在两个问题：
1)在移除老的变量时不能直接丢弃变量和对应的测量值，会损失信息，应采用边际概率，将丢弃变量所携带的信息传递给剩余变量，但这样会使信息矩阵变得稠密，原本条件独立的变量可能变得相关，而稀疏性是SLAM能够实时优化的重要原因。
在k时刻，最小二乘优化完以后，被边缘化的状态量记为xm，窗口内剩余的状态量记为xr，新加入的状态量记为xn。边缘化发生前，变量xm和其对应的测量Sm构建的最小二乘信息矩阵为：
边缘化后，变量xm的测量信息传递给xr：
bp(k)＝bmr(k)-Λrm(k)Λmm(k)-1bmm(k)
Λp(k)＝Λrr(k)-Λrm(k)Λmm(k)-1Λmr(k)
下标p表示先验，即这些信息将构成一个关于xr的先验信息。可以从bp(k)，Λp(k)中反解出一个残差rp(k)和对应的雅克比矩阵Jp(k)，然后构建一个先验残差的最小二乘问题，即边缘化先验残差。通过这一方式，将之前的状态的信息通过先验的方式也加入了最小二乘优化问题，这个问题的规模并不会随时间的推移而使得规模增大。而其余的窗口内的测量则仍然具有稀疏性，这样也没有影响到窗口内优化求解的效率。
在k′通过构建下面的最小二乘问题进行含先验信息的优化：
其中用于将矩阵维度扩张到剩余变量数目的维度，Sa表示剩余的残差集。
2)进行边缘化操作确定先验之后，由于被边缘化的变量测量已经被丢弃，先验信息Λp(k)中关于xr的雅克比在后续求解中无法更新，但xr中的部分变量还和其它残差有联系，这些残差将会随着更新在新的线性化点下进行求解计算。这导致先验的雅可比矩阵的线性化点与新残差的线性化点不同，两者的叠加有可能带来错误信息，从而使得矩阵的零空间发生变化，使得本来有多个最优解变为了仅有一个最优解，不可观的变量变成了可观的。
为了解决这一问题，滑动窗口法往往采取FEJ算法，其核心思想是：与边缘化的状态相关的状态，在后续的雅可比求解过程中，必须使用边缘化时的值，不同残差对同一状态求雅克比时，线性化点必须一致，这样才能保证不会引起矩阵零空间的变化。
2针对边缘计算场景的轻量级目标检测网络
为了实现算法在边缘计算机上的部署并保证实时性，采用轻量级目标检测模型PP-PicoDet，PP-PicoDet针对模型的速度、精度和部署友好性等做出优化，本发明采用三种型号版本中的最小型PicoDet-S，该模型仅用0.99M参数和1.08G FLOPs就实现了30.6％的mAP(mean Average Precision)，输入大小为320，在移动ARM CPU上实现了150FPS的速度，精度、速度和网络体积都能够满足本发明的目标检测任务需求。
PP-PicoDet的网络架构如图6所示，采用ESNet作为骨干网络(Backbone)，它将C3-C5特征图输出到颈部(Neck)，颈部采用CSP-PAN，输入三个特征图，输出四个特征图。
PP-PicoDet采用了百度自研的超轻量、高精度骨干网络--ESNet(EnhancedShuffleNet)，ESNet在ShuffleNetV2的基础上引入了SE模块和GhostNet中的Ghost模块，并新增深度可分离卷积，同时还使用神经网络搜索(NAS)搜索更高效的模型结构，进一步提升模型性能。在Neck部分，PP-PicoDet提出了CSP-PAN结构，使用1×1的卷积来统一通道数，从而减少网络参数，并增强特征提取能力。采用更精准的SimOTA动态标签分配策略，随训练过程动态变换标签分配方式，通过在目标区域采集高质量的样本来有效加速模型收敛。
本发明的目标检测模型使用Pascal VOC数据集进行训练，首先进行数据集的准备，将标注好的图片与对应标签分开存放，并划分训练和验证集，数据准备完成后开始训练。修改数据配置文件中的分类数、路径等参数，修改运行文件中的迭代间隔、模型保存间隔等参数，修改优化参数配置文件中的学习率与优化器配置参数，修改数据读取器配置文件中的数据增强算子、数据读取大小等参数，在单卡GPU上训练并进行评估与测试。
3基于特征点几何约束的移动一致性检测技术
目标检测能够检测物体的先验动态性，除此之外，我们还需要判断特征点的真实动态性，本发明利用对极几何特性来分割静态和动态特征，动态要素将违反多视图几何中为静态场景定义的标准约束。对极几何约束示意图如图7所示。
对于相邻的两帧图像I1、I2，两个相机中心分别为O1、O2，两帧图像之间的运动为R、t，对于空间中的点P，在图像I1中对应像素p1，在图像I2中对应像素p2。连线O1p1和O2p2在三维空间中会相交于空间点P，这时候O1、O2、P三个点可以确定一个平面，称为极平面，O1O2连线与像平面I1、I2的交点分别为e1、e2，e1、e2称为极点，O1O2被称为基线，称极平面与两个像平面之间的相交线l1、l2为极线。
令P的空间坐标为：
P＝[X,Y,Z]T
根据针孔模型得到像素p1、p2的像素位置为：
s1p1＝KP
s2p2＝K(RP+t)
其中K为相机内参矩阵。取：
x1＝K-1p1，x2＝K-1p2
通过推导可以得到对极约束：
x2 Tt^Rx1＝0
p2 TK-Tt^RK-1p1＝0
定义基础矩阵E和本质矩阵F简化对极约束：
E＝t^R
F＝K-TEK-1
x2 TEx1＝p2 TFp1＝0
大部分基于几何的真实动态性检验方法从极线、三角剖分、基础矩阵估计、重投影误差等式等中构建约束，不同的约束构建方法均有其局限性，如通过匹配点到其对应的极线距离来判断真实动态性的方法在载体沿极线方向运动时会失效，通过在成像平面上计算重投影误差的方法会损失深度方向的动态信息。
因此本发明提出了一种3D距离误差判断特征点真实动态性的方法，基于特征点属于动态物体时两帧间由相机光心与投影点相连的反投影射线不能相交于一点的约束构建方法，找到一个能够直接反映3D路标点P在各轴方向上动态程度的参数作为判断动态性的依据，利用RGB-D相机能够直接获取深度的特性，根据IMU获得的帧间转换矩阵、RGB-D深度相机确定的p2深度以及p2的像素坐标计算p2在世界坐标系下的坐标，同p1在世界坐标系下的坐标比较计算出3D距离误差，更加准确的判断特征点真实动态性的同时几乎不增加计算量。
3D距离误差判断特征点真实动态性的示意图如图8所示，当匹配点对p1p2为动态特征点时，观测的3D路标点为同个动态目标上的同个特征，但是在世界坐标系下的坐标不同，为了方便计算，记为P1、P2。
根据IMU预积分数据得到两帧间的刚体姿态变换矩阵为包括旋转矩阵/>和平移向量/>已知I1载体系在世界坐标系下的旋转矩阵/>平移向量/>速度向量/>帧间IMU采样时间间隔之和△t，重力向量g，计算I2载体系在世界坐标系下的坐标：
其中，分别为I2载体系在世界坐标系下的旋转矩阵和平移向量；
使用Kalibr进行相机和IMU的联合标定，得到的相机系与IMU载体系之间的旋转变换Rbc和平移变换tbc，可知：
分别为I2相机系在世界坐标系下的旋转矩阵和平移向量。
已知p2的像素坐标(u2,v2)、标定得到的相机内参fx、fy、cx、cy以及RGB-D相机确定的P2的深度由如下公式计算P2在I2相机坐标系下的坐标/>
计算P2在世界坐标系下的坐标
已知P1在世界坐标系下的坐标根据下式计算P1、P2对应空间点的3D距离误差δ：
计算出每个匹配点对的3D距离误差δ后，根据相邻两帧之间的IMU预积分量协方差以及合适的置信概率设定阈值，判断特征点的真实动态性。
4基于深度图上区域生长法的动态特征点剔除策略
4.1深度图上区域生长法
RGB-D相机得到的深度图保存的是三维空间点的深度值，它的每个像素值是传感器距离物体的实际距离，RGB图像和深度图像的像素点之间具有一对一的对应关系。本发明采用深度图上的自适应区域生长法，以匹配特征点作为种子，通过区域生长法蔓延得到动态与静态区域。区域生长是指从某个像素出发，按照一定的准则，逐步加入邻近像素，当满足一定的条件时，区域生长终止，最后得到整个区域，进而实现目标的提取。区域生长的好坏决定于种子点的选取、生长准则、终止条件，本发明中深度图上区域生长法种子点选取为特征点真实动态性判断中得到的动态和静态特征点；相邻像素的深度值之差满足相应的阈值要求即可划分在一个图斑之中；图像中的每一个匹配动态像素点和匹配静态像素点都有归属作为停止的条件，同时不合并动态性相反的匹配特征点作为停止的附加条件(如以匹配动态点为种子开始的区域生长不合并匹配静态特征点)。深度图上的区域生长法的流程如图9所示：
4.2动态特征点剔除策略
本发明在进行动态特征点的剔除前已经得到的信息有：1)目标检测得到的目标框掩码以及各目标框对应物体的类别；2)根据IMU预积分的对极几何约束得到的匹配特征点的真实动态性；3)深度图上的区域生长法得到的动态区域掩码和静态区域掩码；利用以上信息制定完善的特征点剔除策略，最大程度的剔除动态特征点，保留静态特征点。
整张图像可以分为目标框掩码区域、动态掩码区域、静态掩码区域以及其他区域，因为“人”“猫”“狗”等生物虽然可能保持静止状态，但具有非常强的不确定性，因此在特征点剔除策略中剔除属于这些目标的特征点，具体的特征点剔除策略如图10所示。
创新点：
1、基于深度学习的目标检测和基于IMU信息的几何方法结合的动态SLAM方案
传统基于深度学习的动态SLAM方案根据事先设定的先验动态信息判断动态物体，一方面当环境中出现未知动态物体时无法识别动态目标，另一方面当检测出的物体与先验动态性不符时动态目标识别错误，形象的说就是会对“静止的汽车，移动的椅子”这类物体的动态性判断错误。本发明在目标检测得到先验动态信息的基础上采用基于IMU信息的几何方法进一步判断特征点的真实动态性，一方面能够检测出没有纳入先验动态性信息的动态物体，另一方面避免了对“静止的汽车，移动的椅子”这类物体的误检，获得丰富的语义信息用于灵活的特征点剔除策略，并且更加精准的剔除动态特征点避免特征点质量影响追踪效果。
2、基于目标检测生成动态先验的特征点提取方法
在复杂环境下实现良好的定位与建图效果的基础是数量充足、分布均匀、属于静态物体的特征点，传统动态SLAM算法仅考虑动态特征点的剔除，而没有考虑剔除后剩余特征点的质量与数量问题，进而影响追踪效果并影响后续定位与建图。本发明充分考虑该问题，在得到一帧图像后首先通过一个轻量级目标检测网络得到先验动态目标的目标框掩码，然后在八叉树筛选特征点阶段控制目标框掩码区域的响应值，抑制该区域特征点的提取，以保证剔除动态特征点后剩余特征点的数量和质量，进而保证跟踪以及后续定位与建图的效果，提供在复杂动态环境下的稳定可靠的导航信息。
3、3D距离误差判断特征点真实动态性的移动一致性检测方法
传统的基于几何的移动一致性检测方法从极线、三角剖分、基础矩阵估计、重投影误差等式等中构建约束，不同的约束构建方法均有其局限性，如通过匹配点到其对应的极线距离来判断真实动态性的方法在载体沿极线方向运动时会失效，通过在成像平面上计算重投影误差的方法会损失深度方向的动态信息。为了解决该问题，本发明提出了一种3D距离误差判断特征点真实动态性的方法，该约束构建方法基于特征点属于动态物体时两帧间由相机光心与投影点相连的反投影射线不能相交于一点的特性，3D距离误差是能够直接反映3D路标点P在各轴方向上动态程度的参数，因此不会损失各个方向的动态信息，更加直观、精确地反映特征点动态性，同时几乎不增加计算量。
最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。
Claims (7)
1.一种适用动态环境的实时状态感知方法，基于RGB-D相机和IMU传感器实现，所述方法包括：
步骤1)对RGB-D相机采集的图像信息基于预先训练好的目标检测模型进行动态物体的识别，生成先验动态框；
步骤2)对IMU传感器采集的姿态数据进行IMU预积分，得到当前帧与前一帧的帧间位姿相对变换关系；
步骤3)对先验动态框在各层图像金字塔上生成目标框掩码，经八叉树筛选提取特征点；
步骤4)对当前帧和前一帧的特征点进行匹配，根据帧间位姿相对变换关系判断每个匹配特征点的真实动态性；
步骤5)结合匹配特征点的真实动态性，基于深度图上区域生长法生成动态区域掩码和静态区域掩码，基于动态特征点剔除策略，剔除动态特征点并保留静态特征点，再基于静态特征点进行初始位姿估计；
步骤6)在***关键帧策略中加入对匹配动态点数量以及内点数量的评价，输出关键帧；
步骤7)对关键帧进行局部建图以及回环检测，通过存储的数据关联进行修正，实现动态场景的导航与定位；
所述步骤4)具体包括：
根据IMU预积分数据得到相邻两帧I1、I2间的刚体姿态变换矩阵为包括旋转矩阵/>和平移向量/>根据I1载体系在世界坐标系下的旋转矩阵/>平移向量/>速度向量帧间IMU采样时间间隔之和△t、重力向量g，计算I2在世界坐标系下的位姿：
其中，分别为I2载体系在世界坐标系下的旋转矩阵和平移向量；
使用Kalibr标定工具进行相机和IMU的联合标定，得到的相机系与IMU载体系之间的旋转变换Rbc和平移变换tbc，根据下式得到I2相机系在世界坐标系下的旋转矩阵和平移向量/>
根据图像I1中的像素点p1在图像I2中的匹配点p2的像素坐标为(u2,v2)、标定得到的相机内参fx、fy、cx、cy以及RGB-D相机确定的像素点p2对应空间点的深度由下式得到p2对应空间点在I2相机坐标系下的坐标/>
根据下式计算p2对应空间点在世界坐标系下的坐标
已知p1对应空间点在世界坐标系下的坐标根据下式计算p1、p2对应空间点的3D距离误差δ：
计算出每个匹配点对的3D距离误差δ后，根据相邻两帧之间的IMU预积分量的协方差以及置信概率设定阈值，判断特征点的真实动态性。
2.根据权利要求1所述的适用动态环境的实时状态感知方法，其特征在于，所述步骤1)的目标检测模型为PP-PicoDet网络，所述PP-PicoDet网络采用ESNet作为骨干网络，颈部采用CSP-PAN结构。
3.根据权利要求1所述的适用动态环境的实时状态感知方法，其特征在于，所述步骤2)的帧间位姿相对变换关系具体包括：位移变化、速度变化和姿态变化的变换关系。
4.根据权利要求1所述的适用动态环境的实时状态感知方法，其特征在于，所述步骤3)的图像金字塔层数为8层，所有层级提取到的特征点数之和为1000，图像金字塔相邻层级间的缩放系数为1.2。
5.根据权利要求4所述的适用动态环境的实时状态感知方法，其特征在于，所述步骤3)具体包括：
将每一个图像金字塔层级均匀分为若干个区域，遍历每个区域，进行以下处理：
使用设定的高响应阈值搜索特征点，如果找到特征点，则记录；如果没有找到特征点，使用设定的低响应阈值继续搜索，如果找到特征点，则记录，如果没找到，则移动到下一个区域；
当遍历完所有区域，将先验动态目标框掩码标记区域提取到的特征点响应值降低为原来的0.8倍，对得到的所有特征点进行八叉树筛选，不断将存在特征点的区域进行4等分，直到分出设定数目的足够多分区，每个分区内只保留响应值最大的特征点，所述特征点为ORB特征。
6.根据权利要求1所述的适用动态环境的实时状态感知方法，其特征在于，所述步骤5)具体包括：
采用深度图上的自适应区域生长法，以匹配特征点作为种子，通过区域生长法蔓延得到动态区域与静态区域；
根据步骤1)得到的目标框掩码以及各目标框对应物体的类别，步骤4)得到的匹配特征点的真实动态性，以及步骤5)得到的动态区域掩码和静态区域掩码进行判断，基于特征点剔除策略得到静态特征点。
7.一种适用动态环境的实时状态感知***，基于RGB-D相机和IMU传感器实现，其特征在于，所述***包括部署在前端的目标检测模块、预积分模块、特征提取模块、动态性验证模块、剔除模块和关键帧输出模块，以及部署在后端的建图回环检测模块；其中，
所述目标检测模块，用于对RGB-D相机采集的图像信息基于预先训练好的目标检测模型进行动态物体的识别，生成先验动态框；
所述预积分模块，用于对IMU传感器采集的姿态数据进行IMU预积分，得到当前帧与前一帧的帧间位姿相对变换关系；
所述特征提取模块，用于对先验动态框在各层图像金字塔上生成目标框掩码，经八叉树筛选提取特征点；
所述动态性验证模块，用于对当前帧和前一帧的特征点进行匹配，根据帧间位姿相对变换关系判断每个匹配特征点的真实动态性；
所述剔除模块，用于结合匹配特征点的真实动态性，基于深度图上区域生长法生成动态区域掩码和静态区域掩码，基于动态特征点剔除策略，剔除动态特征点并保留静态特征点，再基于静态特征点进行初始位姿估计；
所述关键帧输出模块，用于在***关键帧策略中加入对匹配动态点数量以及内点数量的评价，输出关键帧；
建图回环检测模块，用于对关键帧进行局部建图以及回环检测，通过存储的数据关联进行修正，实现动态场景的导航与定位；
所述动态性验证模块的处理过程具体包括：
根据IMU预积分数据得到相邻两帧I1、I2间的刚体姿态变换矩阵为包括旋转矩阵和平移向量/>根据I1载体系在世界坐标系下的旋转矩阵/>平移向量/>速度向量帧间IMU采样时间间隔之和△t、重力向量g，计算I2在世界坐标系下的位姿：
其中，分别为I2载体系在世界坐标系下的旋转矩阵和平移向量；
使用Kalibr标定工具进行相机和IMU的联合标定，得到的相机系与IMU载体系之间的旋转变换Rbc和平移变换tbc，根据下式得到I2相机系在世界坐标系下的旋转矩阵和平移向量/>
根据图像I1中的像素点p1在图像I2中的匹配点p2的像素坐标为(u2,v2)、标定得到的相机内参fx、fy、cx、cy以及RGB-D相机确定的像素点p2对应空间点的深度由下式得到p2对应空间点在I2相机坐标系下的坐标/>
根据下式计算p2对应空间点在世界坐标系下的坐标
已知p1对应空间点在世界坐标系下的坐标根据下式计算p1、p2对应空间点的3D距离误差δ：
计算出每个匹配点对的3D距离误差δ后，根据相邻两帧之间的IMU预积分量的协方差以及置信概率设定阈值，判断特征点的真实动态性。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310061217.2A CN116147618B (zh) | 2023-01-17 | 2023-01-17 | 一种适用动态环境的实时状态感知方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310061217.2A CN116147618B (zh) | 2023-01-17 | 2023-01-17 | 一种适用动态环境的实时状态感知方法及*** |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116147618A CN116147618A (zh) | 2023-05-23 |
| CN116147618B true CN116147618B (zh) | 2023-10-13 |
Family
ID=86372997
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310061217.2A Active CN116147618B (zh) | 2023-01-17 | 2023-01-17 | 一种适用动态环境的实时状态感知方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116147618B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116399350B (zh) * | 2023-05-26 | 2023-09-01 | 北京理工大学 | 一种融合YOLOv5的半直接法视觉里程计的确定方法 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
| CN111583136A (zh) * | 2020-04-25 | 2020-08-25 | 华南理工大学 | 一种救援场景下自主移动平台同时定位与建图方法 |
| CN112132897A (zh) * | 2020-09-17 | 2020-12-25 | 中国人民解放军陆军工程大学 | 一种基于深度学习之语义分割的视觉slam方法 |
| WO2022002150A1 (zh) * | 2020-06-30 | 2022-01-06 | 杭州海康机器人技术有限公司 | 一种视觉点云地图的构建方法、装置 |
| WO2022002039A1 (zh) * | 2020-06-30 | 2022-01-06 | 杭州海康机器人技术有限公司 | 一种基于视觉地图的视觉定位方法、装置 |
| CN114199259A (zh) * | 2022-02-21 | 2022-03-18 | 南京航空航天大学 | 一种基于运动状态与环境感知的多源融合导航定位方法 |
| CN115115672A (zh) * | 2022-08-29 | 2022-09-27 | 东南大学 | 基于目标检测和特征点速度约束的动态视觉slam方法 |
| CN115355901A (zh) * | 2022-08-19 | 2022-11-18 | 北京化工大学 | 一种融合动态目标感知的多机联合建图方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11199414B2 (en) * | 2016-09-14 | 2021-12-14 | Zhejiang University | Method for simultaneous localization and mapping |
| CN109903330B (zh) * | 2018-09-30 | 2021-06-01 | 华为技术有限公司 | 一种处理数据的方法和装置 |
-
2023
- 2023-01-17 CN CN202310061217.2A patent/CN116147618B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
| CN111583136A (zh) * | 2020-04-25 | 2020-08-25 | 华南理工大学 | 一种救援场景下自主移动平台同时定位与建图方法 |
| WO2022002150A1 (zh) * | 2020-06-30 | 2022-01-06 | 杭州海康机器人技术有限公司 | 一种视觉点云地图的构建方法、装置 |
| WO2022002039A1 (zh) * | 2020-06-30 | 2022-01-06 | 杭州海康机器人技术有限公司 | 一种基于视觉地图的视觉定位方法、装置 |
| CN112132897A (zh) * | 2020-09-17 | 2020-12-25 | 中国人民解放军陆军工程大学 | 一种基于深度学习之语义分割的视觉slam方法 |
| CN114199259A (zh) * | 2022-02-21 | 2022-03-18 | 南京航空航天大学 | 一种基于运动状态与环境感知的多源融合导航定位方法 |
| CN115355901A (zh) * | 2022-08-19 | 2022-11-18 | 北京化工大学 | 一种融合动态目标感知的多机联合建图方法 |
| CN115115672A (zh) * | 2022-08-29 | 2022-09-27 | 东南大学 | 基于目标检测和特征点速度约束的动态视觉slam方法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于视觉惯导融合的智能车辆定位技术研究;周亚洲;周亚洲(第5期);C035-374 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116147618A (zh) | 2023-05-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
| CN110223348B (zh) | 基于rgb-d相机的机器人场景自适应位姿估计方法 | |
| US11830216B2 (en) | Information processing apparatus, information processing method, and storage medium | |
| US20230258455A1 (en) | Simultaneous location and mapping (slam) using dual event cameras | |
| CN113436260B (zh) | 基于多传感器紧耦合的移动机器人位姿估计方法和*** | |
| CN112785702A (zh) | 一种基于2d激光雷达和双目相机紧耦合的slam方法 | |
| CN112233177B (zh) | 一种无人机位姿估计方法及*** | |
| CN112304307A (zh) | 一种基于多传感器融合的定位方法、装置和存储介质 | |
| CN111288989B (zh) | 一种小型无人机视觉定位方法 | |
| CN114018236B (zh) | 一种基于自适应因子图的激光视觉强耦合slam方法 | |
| JP7479324B2 (ja) | 情報処理装置、情報処理方法、及びプログラム | |
| CN113985445A (zh) | 一种基于相机与激光雷达数据融合的3d目标检测算法 | |
| CN112419497A (zh) | 基于单目视觉的特征法与直接法相融合的slam方法 | |
| CN112752028A (zh) | 移动平台的位姿确定方法、装置、设备和存储介质 | |
| CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
| CN116449384A (zh) | 基于固态激光雷达的雷达惯性紧耦合定位建图方法 | |
| WO2024114119A1 (zh) | 一种基于双目相机引导的传感器融合方法 | |
| CN116147618B (zh) | 一种适用动态环境的实时状态感知方法及*** | |
| CN114004894A (zh) | 基于三个标定板的激光雷达与双目相机空间关系确定方法 | |
| CN114723811A (zh) | 一种非结构化环境四足机器人立体视觉定位建图方法 | |
| Shao | A Monocular SLAM System Based on the ORB Features | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及*** | |
| Buck et al. | Capturing uncertainty in monocular depth estimation: Towards fuzzy voxel maps | |
| CN115511970B (zh) | 一种面向自主泊车的视觉定位方法 | |
| CN114608522B (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |