CN117470248B - 一种移动机器人室内定位方法 - Google Patents
一种移动机器人室内定位方法 Download PDFInfo
- Publication number
- CN117470248B CN117470248B CN202311809922.2A CN202311809922A CN117470248B CN 117470248 B CN117470248 B CN 117470248B CN 202311809922 A CN202311809922 A CN 202311809922A CN 117470248 B CN117470248 B CN 117470248B
- Authority
- CN
- China
- Prior art keywords
- frame
- pose data
- optical flow
- pixel point
- point
- 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 62
- 230000000007 visual effect Effects 0.000 claims abstract description 55
- 230000002441 reversible effect Effects 0.000 claims abstract description 40
- 230000010354 integration Effects 0.000 claims abstract description 26
- 238000000605 extraction Methods 0.000 claims abstract description 11
- 230000003287 optical effect Effects 0.000 claims description 74
- 230000001133 acceleration Effects 0.000 claims description 31
- 230000033001 locomotion Effects 0.000 claims description 26
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000006073 displacement reaction Methods 0.000 claims description 20
- 230000008859 change Effects 0.000 claims description 17
- 238000005457 optimization Methods 0.000 claims description 16
- 230000008878 coupling Effects 0.000 claims description 15
- 238000010168 coupling process Methods 0.000 claims description 15
- 238000005859 coupling reaction Methods 0.000 claims description 15
- 238000005111 flow chemistry technique Methods 0.000 claims description 12
- 238000001514 detection method Methods 0.000 claims description 10
- 230000004044 response Effects 0.000 claims description 9
- 238000013507 mapping Methods 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000006243 chemical reaction Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 6
- 238000005295 random walk Methods 0.000 claims description 6
- 238000005316 response function Methods 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 description 23
- 238000010586 diagram Methods 0.000 description 16
- 230000006870 function Effects 0.000 description 10
- 238000005259 measurement Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 8
- 238000004590 computer program Methods 0.000 description 7
- 238000002474 experimental method Methods 0.000 description 5
- 238000003860 storage Methods 0.000 description 5
- 238000010276 construction Methods 0.000 description 2
- 230000037433 frameshift Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000001939 inductive effect Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 238000012795 verification Methods 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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
-
- 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/40—Extraction of image or video features
- G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种移动机器人室内定位方法，涉及视觉定位技术领域，通过对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，能够准确地得到视觉位姿数据，然后采用IMU预积分策略获取惯性位姿数据，将视觉位姿数据与惯性位姿数据进行紧耦合之后，便能够准确地实现机器人的室内定位，在室内的真实场景中能够更进一步提高动态场景特征提取的准确性，且相比于现有技术，拥有更好地移植性以及鲁棒性。
Description
技术领域
本发明涉及视觉定位技术领域，具体涉及一种移动机器人室内定位方法。
背景技术
传统GPS（Global Positioning System，全球定位系统）以及北斗导航等定位技术是通过卫星信号传递确定机器人位姿，而SLAM（Simultaneous Localization AndMapping，即时定位与地图构建）可以在某些接受不到卫星信号的特定场景下，通过机器人自身装配的传感器进行位姿估计和地图创建。
在传统的纯视觉单目SLAM中，由于传感器本身缺少尺度信息，存在无法准确测量初始位姿等局限性，导致随着时间推移，定位误差逐渐增大。在VINS(Visual InertialNavigation Systems，视觉惯性导航系统)中，将单目视觉传感器信息和IMU（Inertialmeasurement unit，惯性测量单元）测量的信息进行融合，移动机器人可以更好地解决单目视觉传感器深度缺失的问题，能够在动态场景下完成定位与建图任务。但在真实环境下，传统的VINS在实验过程中，可能会因为周围白墙太多，存在单目相机无法提取到有效特征点或者移动机器人遇到转弯时，因为帧与帧图像信息变化过大而无法提取有效特征点等问题，从而导致在运行过程中出现定位失败，最终发生运动轨迹漂移。
发明内容
本发明的目的在于提供一种移动机器人室内定位方法，解决了现有技术在定位过程中发生运动轨迹漂移的问题。
本发明通过下述技术方案实现：
一种移动机器人室内定位方法，包括：
当移动机器人在室内移动时，通过移动机器人搭载的单目相机按第一预设频率采集室内环境图像以及通过移动机器人内置的惯性传感器按第二预设频率采集IMU信息；
对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，得到视觉位姿数据；
对IMU信息进行预积分位姿估计，并将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据；
采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，并根据优化后的视觉惯性位姿数据进行稀疏建图，获取机器人移动轨迹图像；
对移动机器人的移动进行回环检测，获取回环检测结果，并根据所述回环检测结果对机器人移动轨迹图像进行全局优化，得到移动机器人的室内移动轨迹，完成移动机器人室内定位。
在一种可能的实施方式中，对室内环境图像进行特征点提取，包括：
针对任一帧室内环境图像，选取室内环境图像中的任一像素点p，并获取像素点p的亮度为I p ；
以所述像素点p为圆心，在半径为r的邻域圆上选取均匀分布的16个像素点；
以选取的16个像素点为基础，将像素点p与邻域圆上标号1至16的像素点进行比较，判断是否有连续9个像素点的亮度大于I p +L或者小于I p -L，若是，则确定像素点p为特征点，否则确定像素点p不是特征点；其中，L表示预设的亮度阈值；
遍历室内环境图像中的每个像素点，得到室内环境图像的特征点。
在一种可能的实施方式中，针对任一个像素点p，在其邻域内的特征点数若大于预设阈值m，则对室内环境图像进行特征点提取之后，还包括：
以所述像素点p为圆心，通过强度响应函数在半径为r的邻域圆内确定每个像素点p的强度响应为：
；
其中，V表示像素点p邻域内的特征点中强度响应最高的点，，/>，S bright 表示像素点p领域内特征点亮度大于I p +L的集合，x表示像素点p邻域内的特征点，I p→x 表示像素点p邻域内的特征点对应的亮度，S dark 表示像素点p邻域内特征点亮度小于I p -L的集合，I p 表示像素点p的亮度；像素点p的邻域用于表征以像素点p为中心，且预设半径的邻域圆或者邻域正多边形；
判断像素点p是否为其邻域圆内响应强度最大的像素点以及像素点p是否为特征点，若是，则保留像素点p为特征点，否则将像素点p从特征点中去除。
在一种可能的实施方式中，对提取的特征点进行正反向流光匹配，得到视觉位姿数据，包括：
对相邻两帧室内环境图像中特征点进行正向匹配光流处理，得到正向匹配光流结果；
在正向匹配光流结果上进行反向匹配光流处理，得到反向匹配光流结果；
根据所述反向匹配光流结果对正向匹配光流结果对比，反向光流计算后将特征点的正向光流匹配进行抵消，从而使反向光流矢量表征特征点的视觉位姿信息。
在一种可能的实施方式中，所述正向匹配光流处理，包括：
设第K帧室内环境图像中特征点位置为（x k ，y k ），在时间t+dt时运动到第K+1帧，则变化关系有：
I（x k +dx，y k +dy，t+dt）=I（x k ，y k ，t）；
其中，（x k ，y k ）表示图像帧中第k个特征点的位置，I表示亮度值，I（x k ，y k ，t）表示特征点在t时刻的亮度值，I（x k +dx，y k +dy，t+dt）表示t+dt时刻亮度值，d表示微分符号，dt表示对t的微分；
将上述变化关系左边进行泰勒展开保留一阶项后，根据灰度不变特性，获取特征点在平面上的位移为：
；
其中，表示特征点沿x轴方向速度，记作u；/>表示特征点沿y轴方向速度，记作v；/>表示偏导数符号；
根据高斯牛顿法，获取特征点在图像间的轴方向速度u以及y轴方向速度v为：
；
其中，雅可比矩阵J为第k个特征点的偏导数/>，T表示转置，/>表示两帧之间位置的变化量，/>表示两帧之间位置速度的变化量；
根据像素点的运动速度(u,v)，获取第K帧与第K+1帧之间的正向匹配光流结果。
在一种可能的实施方式中，所述反向匹配光流处理，包括：
从第K+1帧室内环境图像中特征点位置（x b ，y b ）到第K帧的反向匹配关系为：
-(x b -x k -dx,y b -y k -dy,t-dt)=(dx+x k -x b ,dy+y k -x b ,dt-t)；
其中，(x k ,y k )表示第k个特征点在K帧的位置，（x b -x k ,y b -y k ）表示正向匹配光流结果，（x k -x b ,y k -y b ）矢量差表示单目相机自身运动在时间范围内使得反向匹配光流接近前景运动的位移，反向光流变换增量可以表示为：；其中，/>表示特征点在K+1帧到K帧的变换增量，f(q)表示第K+1帧到第K帧的反向光流位置速度的变化量；
采用逆变换对正向匹配光流进行验证，得到反向匹配光流结果。
在一种可能的实施方式中，对IMU信息进行预积分位姿估计，包括：
获取离散时间下K+1帧相对于K帧的误差项为：
；
其中，表示第K+1帧位移偏置误差，/>表示第K+1帧旋转偏置误差，/>表示第K+1帧速度偏置误差，/>表示第K+1帧零偏加速度误差，/>表示第K+1帧零偏角速度误差，/>表示单位矩阵，/>表示第K帧到第K+1帧时间差，/>表示第K帧旋转矩阵，/>表示第K+1帧旋转矩阵，f 01表示第一中间变量，f 11表示第二中间变量，f 21表示第三中间变量，f 03表示第四中间变量，f 04表示第五中间变量，f 23表示第六中间变量，f 24表示第七中间变量，V 00表示第八中间变量，V 01表示第九中间变量，V 02表示第十中间变量，V 03表示第十一中间变量，V 21表示第十二中间变量，V 23表示第十三中间变量，/>表示第K帧位移偏置误差，/>表示第K帧旋转偏置误差，/>表示第K帧零偏加速度误差，/>表示第K帧零偏角速度误差，表示表示第K帧加速度噪声，/>表示第K帧角速度噪声，/>表示第K+1帧加速度噪声，/>表示第K+1帧角速度噪声，/>表示加速度随机游走，/>表示角速度随机游走；
根据离散时间下K+1帧相对于K帧的误差项，获取相对于初始帧的旋转、线加速度以及位移增量，得到惯性位姿数据。
在一种可能的实施方式中，所述f 01、f 03、f 04、f 11、f 21、f 23、f 24、V 00、V 01、V 02、V 03、V 21以及V 23为：
；
其中，表示第K帧IMU测得角速度，/>表示第K+1帧IMU测得角速度，/>表示第K帧零偏角速度，/>表示第K帧IMU测得加速度，/>表示第K帧零偏加速度，/>表示第K+1帧IMU测得加速度。
在一种可能的实施方式中，将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据，包括：第一紧耦合策略以及第二紧耦合策略；
所述第一紧耦合策略包括：从IMU坐标系下将第K帧惯性位姿数据b K ，旋转到第K+1帧惯性位姿数据b K+1，再旋转到相机坐标系下第K+1帧，得到转换位姿数据c K+1；根据原有的第K+1帧视觉位姿数据与旋转得到的转换位姿数据c K+1，得到视觉惯性位姿数据；
所述第二紧耦合策略包括：从IMU坐标系下将第K帧惯性位姿数据b K 先旋转到相机坐标系下的第K帧视觉位姿数据c K ，再旋转到相机坐标系下K+1帧，得到转换位姿数据c K+1；根据原有的第K+1帧视觉位姿数据与旋转得到的转换位姿数据c K+1，得到视觉惯性位姿数据。
在一种可能的实施方式中，采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，包括：第一滑动窗口策略以及第二滑动窗口策略；
第一滑动窗口策略包括：若当前第P n 帧是新关键帧，则丢弃滑动窗口中最老的图像帧P 1，同时将与最老的图像帧P 1相关联的IMU预积分约束进行边缘化处理；
其中，若最老的图像帧P 1为关键帧时，则将最老的图像帧P 1对应的深度信息转移到其对应的下一图像帧P 2中；
第二滑动窗口策略包括：若当前图像帧不是新关键帧，但滑动窗口内能容纳的图像帧已满，则丢弃滑窗中上一图像帧P n-1 ，把当前帧和前一帧的IMU预积分约束转化为当前帧P n 和第n-2图像帧P n-2 的IMU约束。
本发明提供的一种移动机器人室内定位方法，通过对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，能够准确地得到视觉位姿数据，然后采用IMU预积分策略获取惯性位姿数据，将视觉位姿数据与惯性位姿数据进行紧耦合之后，便能够准确地实现机器人的室内定位，在室内的真实场景中能够更进一步提高动态场景特征提取的准确性，且相比于现有技术，拥有更好地移植性以及鲁棒性。
附图说明
为了更清楚地说明本发明示例性实施方式的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1为本发明实施例提供的前端模块的示意图。
图2为本发明实施例提供的初始化模块的示意图。
图3为本发明实施例提供的后端模块的示意图。
图4为本发明实施例提供的一种移动机器人室内定位方法的流程图。
图5为本发明实施例提供的正反向匹配光流法示意图。
图6为本发明实施例提供的IMU预积分约束示意图。
图7为本发明实施例提供的视觉与惯性初始化示意图。
图8为本发明实施例提供的滑动窗口示意图。
图9（a）为本发明实施例提供的试验数据为MH-04序列时，本发明算法所得轨迹与真实轨迹对比图。
图9（b）为本发明实施例提供的试验数据为MH-04序列时，现有开源算法轨迹与真实轨迹对比图。
图9（c）为本发明实施例提供的试验数据为MH-04序列时，本发明算法所得轨迹与真实轨迹的平移误差对比图。
图9（d）为本发明实施例提供的试验数据为MH-04序列时，现有开源算法轨迹与真实轨迹的平移误差对比图。
图10（a）为本发明实施例提供的试验数据为MH-03序列时，本发明算法所得轨迹与真实轨迹对比图。
图10（b）为本发明实施例提供的试验数据为MH-03序列时，现有开源算法轨迹与真实轨迹对比图。
图10（c）为本发明实施例提供的试验数据为MH-03序列时，本发明算法所得轨迹与真实轨迹的平移误差对比图。
图10（d）为本发明实施例提供的试验数据为MH-03序列时，现有开源算法轨迹与真实轨迹的平移误差对比图。
图11（a）为本发明实施例提供的一种移动机器人室内定位方法在室内的建图轨迹。
图11（b）为本发明实施例提供的现有开源算法在室内的建图轨迹。
其中，现有开源算法指VINS-Fusion算法。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。
为了便于理解本发明的实施例，首先结合图1至图3对本发明实施例所适用的系统架构进行介绍。
如图1至图3共同所示，本发明实施例所适用的系统架构包括前端模块、初始化模块以及后端模块，为了解决移动机器人在室内场景下定位的准确精度较低、系统兼容性较差以及鲁棒性较差的问题，该系统架构可以基于VINS-fusion框架构建，单目相机获取图像信息后，引入FAST-9算法对图像中特征点快速提取后，将提取的特征点进行正反向匹配光流，更好地解决原系统框架不具备的旋转不变性和尺度不变性。再将视觉匹配的位姿信息与IMU预积分测算信息对齐，前端模块的输出作为后端的输入，采用滑动窗口非线性优化模型对关键帧进行判定，减轻后端的运算量。在与开源系统VINS-fusion通过数据集比较中，该系统架构的定位精度有较大提升，特征点提取与匹配改进后，能应对更复杂的情况，在室内的真实场景中能够更进一步提高动态场景特征提取的准确性，最终完成移动机器人建图任务。
如图1所示，前端模块通过携带移动机器人上的单目相机收集到图像信息后，间接测算机器人的位姿数据，最终实现在未知环境中进行定位。在图像信息中需要对特征点进行提取，并根据特征点估计相机运动。本发明提出一种基于FAST-9算法，但不采用原算法中BRIEF描述图像特征点周围信息，而是通过正反向匹配光流跟踪，目的是更好地解决单目视觉在现实场景中定位准确性较低问题。考虑到单目相机缺乏深度信息，在定位和建图过程中无法还原现实场景中三维结构，限制了机器人对环境的感知力。IMU可以提供加速度和角速度信息，通过连续测量数据，运用预积分可以转化为相机的姿态、速度和位置估计，进而改善单目视觉中SLAM系统的定位性能。
由于单目相机本身无法直接测量尺度信息，需要通过本质矩阵计算出前两帧图像之间的相对尺度，这个尺度可以用来将单目相机的位姿估计结果进行尺度校正，以获得更精确的相对尺度信息，再利用PnP算法求解出2D-3D点对匹配，估计相机的初始位姿，这个过程需要将前一帧图像中的特征点投影到3D空间中，然后与当前帧空间中的特征点进行匹配，完成三角化测量，最终实现对场景中特征点三维重建和深度信息获取。
如图2所示，在实际应用场景中，为了更准确地室内定位，初始化模块可以利用IMU与单目相机具有互补性的特点，即视觉能提供更丰富的信息测算出物体的位姿，IMU预积分后可以获得高频运动下，两帧之间的位姿测量数据，在非性优化后，可以提供较准确的位姿估计，最后将视觉初始化信息和IMU预积分进行对齐。
如图3所示，将前端模块的输出信息作为后端的输入信息，采用基于滑动窗口的优化策略对视觉单目与惯性融合的数据进行非线性优化，建立残差模型，最后通过回环检测使整个SLAM系统精度得到进一步提升。
如图4所示，本发明实施例提供一种移动机器人室内定位方法，包括：
S101、当移动机器人在室内移动时，通过移动机器人搭载的单目相机按第一预设频率采集室内环境图像以及通过移动机器人内置的惯性传感器按第二预设频率采集IMU信息；
S102、对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，得到视觉位姿数据；
S103、对IMU信息进行预积分位姿估计，并将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据；
S104、采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，并根据优化后的视觉惯性位姿数据进行稀疏建图，获取机器人移动轨迹图像；
S105、对移动机器人的移动进行回环检测，获取回环检测结果，并根据所述回环检测结果对机器人移动轨迹图像进行全局优化，得到移动机器人的室内移动轨迹，完成移动机器人室内定位。
本发明提供的一种移动机器人室内定位方法，通过对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，能够准确地得到视觉位姿数据，然后采用IMU预积分策略获取惯性位姿数据，将视觉位姿数据与惯性位姿数据进行紧耦合之后，便能够准确地实现机器人的室内定位，在室内的真实场景中能够更进一步提高动态场景特征提取的准确性，且相比于现有技术，采用FAST-9即提取像素周围第1、6、11以及16标号进行比较，且判断条件为若周围连续有9个像素点亮度大于或小于提取像素点则认定为特征点，反向光流匹配是直接计算两帧间的变换增量，通过逆变换对正向光流进行有效抵消，通过实验验证，本发明能更精准地实现相邻帧间匹配，拥有更好地移植性以及鲁棒性。
本发明实施例采用FAST-9算法加快特征点提取速度。在进行特征点匹配时，不进行BRIEF描述子匹配，因为该算法对噪声敏感，鲁棒性较差且不具备旋转不变性和尺度不变性，采用正反向匹配光流法，从而提高匹配精度和准确性。
在一种可能的实施方式中，对室内环境图像进行特征点提取，包括：
针对任一帧室内环境图像，选取室内环境图像中的任一像素点p，并获取像素点p的亮度为I p 。
以所述像素点p为圆心，在半径为r的邻域圆上选取均匀分布的16个像素点。
以选取的16个像素点为基础，将像素点p与邻域圆上标号1至16的像素点进行比较，判断是否有连续9个像素点的亮度大于I p +L或者小于I p -L，若是，则确定像素点p为特征点，否则确定像素点p不是特征点。其中，L表示预设的亮度阈值。
遍历室内环境图像中的每个像素点，得到室内环境图像的特征点。
在一种可能的实施方式中，针对任一个像素点p，在其邻域内的特征点数若大于预设阈值m，则对室内环境图像进行特征点提取之后，还包括：
以所述像素点p为圆心，通过强度响应函数在半径为r的邻域圆内确定每个像素点p的强度响应为：
；
其中，V表示像素点p邻域内的特征点中强度响应最高的点，，/>，S bright 表示像素点p领域内特征点亮度大于I p +L的集合，x表示像素点p邻域内的特征点，I p→x 表示像素点p邻域内的特征点对应的亮度，S dark 表示像素点p邻域内特征点亮度小于I p -L的集合，I p 表示像素点p的亮度；像素点p的邻域用于表征以像素点p为中心，且预设半径的邻域圆或者邻域正多边形；
判断像素点p是否为其邻域圆内响应强度最大的像素点以及像素点p是否为特征点，若是，则保留像素点p为特征点，否则将像素点p从特征点中去除。
在相邻两关键帧之间，通过FAST-9提取到特征点后，利用稀疏光流法对特征点的匹配进行跟踪，得到不同帧间对应特征点位置。
在一种可能的实施方式中，对提取的特征点进行正反向流光匹配，得到视觉位姿数据，包括：
对相邻两帧室内环境图像中特征点进行正向匹配光流处理，得到正向匹配光流结果。
在正向匹配光流结果上进行反向匹配光流处理，得到反向匹配光流结果。
根据所述反向匹配光流结果对正向匹配光流结果对比，反向光流计算后将特征点的正向光流匹配进行抵消，从而使反向光流矢量表征特征点的视觉位姿信息。提高特征点匹配稳定性，有效去除了光流法中特征点错误跟踪和单目相机抖动干扰。在得到相邻帧间特征点的位移后，建立特征点的运动轨迹。
在一种可能的实施方式中，所述正向匹配光流处理，包括：
设第K帧室内环境图像中特征点位置为（x k ，y k ），在时间t+dt时运动到第K+1帧，则变化关系有：
I（x k +dx，y k +dy，t+dt）=I（x k ，y k ，t）；
其中，（x k ，y k ）表示图像帧中第k个特征点的位置，I表示亮度值，I（x k ，y k ，t）表示特征点在t时刻的亮度值，I（x k +dx，y k +dy，t+dt）表示t+dt时刻亮度值，d表示微分符号，dt表示对t的微分。
将上述变化关系左边进行泰勒展开保留一阶项后，根据灰度不变特性，获取特征点在平面上的位移为：
；
其中，表示特征点沿x轴方向速度，记作u；/>表示特征点沿y轴方向速度，记作v；/>表示偏导数符号。
根据高斯牛顿法，获取特征点在图像间的轴方向速度u以及y轴方向速度v为：
；
其中，雅可比矩阵J为第k个特征点的偏导数/>，T表示转置，/>表示两帧之间位置的变化量，/>表示两帧之间位置速度的变化量。
根据像素点的运动速度(u,v)，获取第K帧与第K+1帧之间的正向匹配光流结果，即运动速度乘以相邻帧间隔时间，就可以得到特征点的位移。
为了估算图像相邻两帧之间特征点匹配，需要进行非线性优化，如果所有特征点都重新计算匹配，这会增加计算量，影响该进程的效率。考虑到图像帧与帧之间的特征点匹配光流是可逆的，本发明提出一种反向光流匹配法，在非线性优化迭代过程中，只需进行一次雅可比矩阵计算就能完成特征点的光流匹配。
在一种可能的实施方式中，所述反向匹配光流处理，包括：
从第K+1帧室内环境图像中特征点位置（x b ，y b ）到第K帧的反向匹配关系为：
-(x b -x k -dx,y b -y k -dy,t-dt)=(dx+x k -x b ,dy+y k -x b ,dt-t)；
其中，(x k ,y k )表示第k个特征点在K帧的位置，（x b -x k ,y b -y k ）表示正向匹配光流结果，（x k -x b ,y k -y b ）矢量差表示单目相机自身运动在时间范围内使得反向匹配光流接近前景运动的位移，反向光流变换增量可以表示为：；/>表示特征点在K+1帧到K帧的变换增量，f(q)表示第K+1帧到第K帧的反向光流位置速度的变化量。
采用逆变换对正向匹配光流进行验证，得到反向匹配光流结果。
每次迭代中，相邻帧间反向匹配光流的雅可比矩阵都相同，因此采用逆变换就能对正向匹配光流进行验证，反向光流也能一定程度上表征特征点的位置信息，提高了特征匹配的准确性。
如图5所示，设方框内分别为K、K+1、K+2共视关键帧的特征点，通过本发明提出的反向匹配光流法对正向匹配光流进行有效抵消，从而反向匹配光流在一定程度上可以表示一个特征点的运动方向和速度，进而实现更精准的相邻帧间匹配。
如图6所示，IMU通过测得线加速度和陀螺仪角速度，积分后可获得两帧之间的旋转和位移变换。但在SLAM中，随着时间的推移，通过传感器获取到的信息，需要求解机器人的位姿，如果将两帧之间的IMU测量值都进行计算和评估，会造成非常大的计算量且耗时较长。为了避免不断迭代这些状态量，采用IMU预积分策略，其主要思路是在每次迭代中把不变量提取出来，减小每次重新积分的计算量。
连续时间下，IMU积分误差项导数只与偏置有关与其他量无关，可以表示为：
；
其中，表示IMU坐标系中第K帧位姿数据b K 里位移偏置误差对时刻t的导数，表示IMU坐标系中第K帧位姿数据b K 里速度偏置误差对时刻t的导数，/>表示IMU坐标系中第K帧位姿数据b K 里旋转偏置误差对时刻t的导数，/>表示零偏加速度误差对时刻t的导数，/>表示零偏角速度误差对时刻t的导数，/>表示位移偏置误差原函数，/>表示速度偏置误差原函数，/>表示旋转偏置误差原函数，/>表示零偏加速度误差原函数，/>表示零偏角速度误差原函数，/>表示IMU坐标系下t时刻获取的线加速度测量值，表示t时刻获取的角加速度测量值，/>和/>分别表示t时刻加速计的零偏和陀螺仪的零偏，/>表示表示IMU坐标系中第K帧位姿数据b K 在t时刻的时刻到时刻旋转变量，/>、/>、/>、/>均表示零均值的高斯白噪声，/>表示反对称矩阵。
在一种可能的实施方式中，对IMU信息进行预积分位姿估计，包括：
获取离散时间下K+1帧相对于K帧的误差项为：
；
其中，表示第K+1帧位移偏置误差，/>表示第K+1帧旋转偏置误差，/>表示第K+1帧速度偏置误差，/>表示第K+1帧零偏加速度误差，/>表示第K+1帧零偏角速度误差，/>表示单位矩阵，/>表示第K帧到第K+1帧时间差，/>表示第K帧旋转矩阵，/>表示第K+1帧旋转矩阵，f 01表示第一中间变量，f 11表示第二中间变量，f 21表示第三中间变量，f 03表示第四中间变量，f 04表示第五中间变量，f 23表示第六中间变量，f 24表示第七中间变量，V 00表示第八中间变量，V 01表示第九中间变量，V 02表示第十中间变量，V 03表示第十一中间变量，V 21表示第十二中间变量，V 23表示第十三中间变量，/>表示第K帧位移偏置误差，/>表示第K帧旋转偏置误差，/>表示第K帧零偏加速度误差，/>表示第K帧零偏角速度误差，表示表示第K帧加速度噪声，/>表示第K帧角速度噪声，/>表示第K+1帧加速度噪声，/>表示第K+1帧角速度噪声，/>表示加速度随机游走，/>表示角速度随机游走；
根据离散时间下K+1帧相对于K帧的误差项，获取相对于初始帧的旋转、线加速度以及位移增量，得到惯性位姿数据。
在一种可能的实施方式中，所述f 01、f 03、f 04、f 11、f 21、f 23、f 24、V 00、V 01、V 02、V 03、V 21以及V 23为：
；
其中，表示第K帧IMU测得角速度，/>表示第K+1帧IMU测得角速度，/>表示第K帧零偏角速度，/>表示第K帧IMU测得加速度，/>表示第K帧零偏加速度，/>表示第K+1帧IMU测得加速度。
如图7所示，首先应建立不同坐标系之间的联系，计算出IMU坐标系到相机坐标系对应关键帧的旋转参数，再利用旋转参数分别计算出陀螺仪偏差、重力方向、速度以及尺度的初始值。
在一种可能的实施方式中，将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据，包括：第一紧耦合策略以及第二紧耦合策略；
所述第一紧耦合策略包括：从IMU坐标系下将第1帧惯性位姿数据b 0 开始，将第K帧惯性位姿数据b K 先旋转到第K+1帧惯性位姿数据b K+1，再最后旋转到相机坐标系下第K+1帧，得到转换旋转位姿数据c K+1。b 0 表示第一帧相机坐标系下的惯性位姿数据。根据原有的第K+1帧视觉位姿数据与旋转得到的转换位姿数据c K+1，得到视觉惯性位姿数据。
所述第二紧耦合策略包括：从IMU坐标系下将第1帧惯性位姿数据b 0 开始，将第K帧惯性位姿数据b K 先旋转到相机坐标系下的第K帧视觉位姿数据c K （如将IMU坐标系下将第1帧惯性位姿数据b 0 旋转到相机坐标系下第1帧数据c 0 ），再最后旋转到相机坐标系下第K+1帧，得到转换位姿数据c K+1；根据原有的第K+1帧视觉位姿数据与旋转得到的转换位姿数据c K+1，得到视觉惯性位姿数据。
数学表达式为：
；
其中，表示IMU坐标系下第K帧到K+1帧四元数，/>表示四元数乘法，/>表示IMU坐标系到相机坐标系四元数，/>表示相机坐标系下第K帧到K+1帧四元数。
将上式改写为四元数形式为：
；
其中，分别表示四元数的左乘和右乘，/>表示左乘矩阵和右乘矩阵相减后的结果。
若将K个帧的线性方程累加起来，可以构建一个超定方程组：
；
其中，Q K 表示中间矩阵，为每个时刻线性方程的权重。
权重函数主要通过下式得到：
；
其中，G为工程实践中设定的一个阈值。
实际在采集数据中不可避免地产生误差，在此用残差构建两条路径之间的误差，根据旋转矩阵和轴角之间的关系/>，/>计算结果为：
；
其中，表示刚体从IMU坐标系到相机坐标系的旋转逆矩阵，/>表示IMU坐标系第K帧到第K+1帧位姿数据的旋转逆矩阵，/>表示刚体从IMU坐标系到相机坐标系的旋转矩阵，/>表示相机坐标系第K帧到第K+1帧位姿数据的旋转矩阵，/>表示旋转矩阵R的迹，/>表示轴角。
本发明采用SVD分解法，其最小奇异值对应的向量即为旋转外参。通过旋转外参矩阵使单目视觉和惯性传感器进行有效融合，提高定位和建图的精度。
后端模块采用滑动窗口策略进行非线性优化。随着前端输出数据量的增加，后端输入的数据量也会增加。本发明系统需要判断确定新关键帧以及怎样去除旧关键帧是滑动窗口解决的关键问题。
如图8所示，采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，包括：第一滑动窗口策略以及第二滑动窗口策略。
第一滑动窗口策略包括：若当前第P n 帧是新关键帧，则丢弃滑动窗口中最老的图像帧P 1，同时将与最老的图像帧P 1相关联的IMU预积分约束进行边缘化处理。
其中，若最老的图像帧P 1为关键帧时，则将最老的图像帧P 1对应的深度信息转移到其对应的下一图像帧P 2中。
第二滑动窗口策略包括：若当前图像帧不是新关键帧，但滑动窗口内能容纳的图像帧已满，则丢弃滑窗中上一图像帧P n-1 ，把当前帧和前一帧的IMU预积分约束转化为当前帧P n 和第n-2图像帧P n-2 的IMU约束。因为P n 与P n-1 图像信息很相似，因此这两帧与地图路标点直接的IMU约束很接近，丢弃后并不会对这个滑窗的整体约束关系丢失信息，保证了IMU预积分的连续性。
本发明改进后的系统框架主要和现今比较流行的开源算法框架VINS-Fusion进行比较，主要分为数据集实验比较和现实场景中定位建图实验比较。
在数据集实验中，主要选用开源数据集EuRoc里MH-04序列和MH-03序列，具体实验结果如图9以及图10所示。其中图9（a）和图9（b）是本发明算法在MH-04序列中移动机器人运行的轨迹和真实轨迹的比较，开源算法指VINS-Fusion算法运行轨迹，图中虚线表示用动捕设备测算的真实轨迹，图10（a）和图10（b）是在MH-03序列中运行轨迹示意图，综合两个序列的数据集运行轨迹，可以看出经本发明改进后的运动轨迹更贴近真实轨迹，相较于开源算法的准确性更高。
图9（c）和图9（d）和图10（c）和图10（d）分别进一步比较了在MH-04和MH-03序列中，机器人运动时在x、y、z轴的平移误差，在不同轴向的精度上都优于VINS-fusion。
因为周围条件复杂，如：来往行人较多，照明灯是感应式的等，可能造成单目相机捕捉到的光环境不稳定导致提取的特征点不够。用VINS-fusion进行实验时会发生在转角处光强度变化过大，周围白墙导致了机器人定位失败，在Rviz中表现为机器人发生漂移，而本发明的算法因为对前端模块进行改进，提高了机器人在真实场景下的实时性和定位的准确性，室内运行过程和结果如图11（a）和图11（b）所示。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备（系统）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
本领域普通技术人员可以理解实现上述事实和方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，涉及的程序或者所述的程序可以存储于一计算机所可读取存储介质中，该程序在执行时，包括如下步骤：此时引出相应的方法步骤，所述的存储介质可以是ROM/RAM、磁碟、光盘等等。
以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (5)
1.一种移动机器人室内定位方法，其特征在于，包括：
当移动机器人在室内移动时，通过移动机器人搭载的单目相机按第一预设频率采集室内环境图像以及通过移动机器人内置的惯性传感器按第二预设频率采集IMU信息；
对室内环境图像进行特征点提取，对提取的特征点进行正反向流光匹配，得到视觉位姿数据；对IMU信息进行预积分位姿估计，并将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据；
采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，并根据优化后的视觉惯性位姿数据进行稀疏建图，获取机器人移动轨迹图像；
对移动机器人的移动进行回环检测，获取回环检测结果，并根据所述回环检测结果对机器人移动轨迹图像进行全局优化，得到移动机器人的室内移动轨迹，完成移动机器人室内定位；对室内环境图像进行特征点提取，包括：
针对任一帧室内环境图像，选取室内环境图像中的任一像素点p，并获取像素点p的亮度为Ip；
以所述像素点p为圆心，在半径为r的邻域圆上选取均匀分布的16个像素点；
以选取的16个像素点为基础，将像素点p与邻域圆上标号1至16的像素点进行比较，判断是否有连续9个像素点的亮度大于Ip+L或者小于Ip-L，若是，则确定像素点p为特征点，否则确定像素点p不是特征点；其中，L表示预设的亮度阈值；
遍历室内环境图像中的每个像素点，得到室内环境图像的特征点；
针对任一个像素点p，在其邻域内的特征点数若大于预设阈值m，则对室内环境图像进行特征点提取之后，还包括：
以所述像素点p为圆心，通过强度响应函数在半径为r的邻域圆内确定每个像素点p的强度响应为：
其中，V表示像素点p邻域内的特征点中强度响应最高的点，
Sbright＝{x|Ip→x≥Ip+L}，sdark＝{x|Ip→x≤Ip-L}，Sbright表示像素点p邻域内特征点亮度大于Ip+L的集合，x表示像素点p邻域内的特征点，Ip→x表示像素点p邻域内的特征点对应的亮度，Sdark表示像素点p邻域内特征点亮度小于Ip-L的集合，Ip表示像素点p的亮度；像素点p的邻域用于表征以像素点p为中心，且预设半径的邻域圆或者邻域正多边形；
判断像素点p是否为其邻域圆内响应强度最大的像素点以及像素点p是否为特征点，若是，则保留像素点p为特征点，否则将像素点p从特征点中去除；
对提取的特征点进行正反向流光匹配，得到视觉位姿数据，包括：
对相邻两帧室内环境图像中特征点进行正向匹配光流处理，得到正向匹配光流结果；
在正向匹配光流结果上进行反向匹配光流处理，得到反向匹配光流结果；
根据所述反向匹配光流结果对正向匹配光流结果对比，反向光流计算后将特征点的正向光流匹配进行抵消，从而使反向光流矢量表征特征点的视觉位姿信息；
所述正向匹配光流处理，包括：
设第K帧室内环境图像中特征点位置为(xk，yk)，在时间t+dt时运动到第K+1帧，则变化关系有：
I(xk+dx，yk+dy，t+dt)＝I(xk，yk，t)；
其中，(xk，yk)表示图像帧中第k个特征点的位置，I表示亮度值，I(xk，yk，t)表示特征点在t时刻的亮度值，I(xk+dx，yk+dy，t+dt)表示t+dt时刻亮度值，d表示微分符号，dt表示对t的微分；
将上述变化关系左边进行泰勒展开保留一阶项后，根据灰度不变特性，获取特征点在平面上的位移为：
其中，表示特征点沿x轴方向速度，记作u；/>表示特征点沿y轴方向速度，记作v；/>表示偏导数符号；
根据高斯牛顿法，获取特征点在图像间的x轴方向速度u以及y轴方向速度v为：
其中，雅可比矩阵J为第k个特征点f(xk，yk)的偏导数T表示转置，f(dx，dy)表示两帧之间位置的变化量，/>表示两帧之间位置速度的变化量；
根据像素点的运动速度(u，v)，获取第K帧与第K+1帧之间的正向匹配光流结果；
所述反向匹配光流处理，包括：
从第K+1帧室内环境图像中特征点位置(xb，yb)到第K帧的反向匹配关系为：
-(xb-xk-dx，yb-yk-dy，t-dt)＝(dx+xk-xb，dy+yk-yb，dt-t)；
其中，(xk，yk)表示特征点k在K帧的位置，(xb-xk，yb-yk)表示正向匹配光流结果，(xk-xb，yk-yb)矢量差表示单目相机自身运动在时间范围内使得反向匹配光流接近前景运动的位移，反向光流变换增量可以表示为：Δq＝-(JTJ)-1JTf(q)；Δq表示特征点在K+1帧到K帧的变换增量，f(q)表示第K+1帧到第K帧的反向光流位置速度的变化量；
采用逆变换(Δq)-1对正向匹配光流进行验证，得到反向匹配光流结果。
2.根据权利要求1所述的移动机器人室内定位方法，其特征在于，对IMU信息进行预积分位姿估计，包括：
获取离散时间下K+1帧相对于K帧的误差项为：
其中，δaK+1表示第K+1帧位移偏置误差，δθK+1表示第K+1帧旋转偏置误差，δβK+1表示第K+1帧速度偏置误差，表示第K+1帧零偏加速度误差，/>表示第K+1帧零偏角速度误差，I′表示单位矩阵，δt表示第K帧到第K+1帧时间差，RK表示第K帧旋转矩阵，RK+1表示第K+1帧旋转矩阵，f01表示第一中间变量，f11表示第二中间变量，f21表示第三中间变量，f03表示第四中间变量，f04表示第五中间变量，f23表示第六中间变量，f24表示第七中间变量，V00表示第八中间变量，V01表示第九中间变量，V02表示第十中间变量，V03表示第十一中间变量，V21表示第十二中间变量，V23表示第十三中间变量，δaK表示第K帧位移偏置误差，δθK表示第K帧旋转偏置误差，/>表示第K帧零偏加速度误差，/>表示第K帧零偏角速度误差，/>表示表示第K帧加速度噪声，/>表示第K帧角速度噪声，/>表示第K+1帧加速度噪声，/>表示第K+1帧角速度噪声，/>表示加速度随机游走，/>表示角速度随机游走；
根据离散时间下K+1帧相对于K帧的误差项，获取相对于初始帧的旋转、线加速度以及位移增量，得到惯性位姿数据。
3.根据权利要求2所述的移动机器人室内定位方法，其特征在于，所述f01、f03、f04、f11、f21、f23、f24、V00、V01、V02、V03、V21以及V23为：
其中，表示第K帧IMU测得角速度，/>表示第K+i帧IMU测得角速度，/>表示第K帧零偏角速度，/>表示第K帧IMU测得加速度，/>表示第K帧零偏加速度，/>表示第K+1帧IMU测得加速度。
4.根据权利要求2所述的移动机器人室内定位方法，其特征在于，将预积分位姿估计得到惯性位姿数据与视觉位姿数据进行紧耦合，得到视觉惯性位姿数据，包括：第一紧耦合策略以及第二紧耦合策略；
所述第一紧耦合策略包括：从IMU坐标系下将第K帧惯性位姿数据bK，旋转到第K+1帧惯性位姿数据bK+1，再旋转到相机坐标系下第K+1帧，得到转换位姿数据cK+1；根据原有的第K+1帧视觉位姿数据与旋转得到的转换位姿数据cK+1，得到视觉惯性位姿数据；
所述第二紧耦合策略包括：从IMU坐标系下将第K帧惯性位姿数据bK先旋转到相机坐标系下的第K帧视觉位姿数据cK，再旋转到相机坐标系下K+1帧，得到转换位姿数据cK+1；根据原有的第K+1帧视觉位姿数据与旋转得到的转换位姿数据cK+1，得到视觉惯性位姿数据。
5.根据权利要求4所述的移动机器人室内定位方法，其特征在于，采用滑动窗口策略对视觉惯性位姿数据进行关键帧优化，包括：第一滑动窗口策略以及第二滑动窗口策略；
第一滑动窗口策略包括：若当前第Pn帧是新关键帧，则丢弃滑动窗口中最老的图像帧P1，同时将与最老的图像帧P1相关联的IMU预积分约束进行边缘化处理；
其中，若最老的图像帧P1为关键帧时，则将最老的图像帧P1对应的深度信息转移到其对应的下一图像帧P2中；
第二滑动窗口策略包括：若当前图像帧不是新关键帧，但滑动窗口内能容纳的图像帧已满，则丢弃滑窗中上一图像帧Pn-1，把当前帧和前一帧的IMU预积分约束转化为当前帧Pn和第n-2图像帧Pn-2的IMU约束。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311809922.2A CN117470248B (zh) | 2023-12-27 | 2023-12-27 | 一种移动机器人室内定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311809922.2A CN117470248B (zh) | 2023-12-27 | 2023-12-27 | 一种移动机器人室内定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117470248A CN117470248A (zh) | 2024-01-30 |
| CN117470248B true CN117470248B (zh) | 2024-04-02 |
Family
ID=89633300
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311809922.2A Active CN117470248B (zh) | 2023-12-27 | 2023-12-27 | 一种移动机器人室内定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117470248B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108833920A (zh) * | 2018-06-04 | 2018-11-16 | 四川大学 | 一种基于光流和块匹配的dvc边信息融合方法 |
| CN111024066A (zh) * | 2019-12-10 | 2020-04-17 | 中国航空无线电电子研究所 | 一种无人机视觉-惯性融合室内定位方法 |
| CN111383247A (zh) * | 2018-12-29 | 2020-07-07 | 北京易讯理想科技有限公司 | 增强金字塔lk光流算法图像跟踪稳定性的方法 |
| CN115222974A (zh) * | 2022-07-14 | 2022-10-21 | Oppo广东移动通信有限公司 | 特征点匹配方法及装置、存储介质及电子设备 |
| CN115615424A (zh) * | 2022-08-25 | 2023-01-17 | 中国人民解放军火箭军工程大学 | 一种起重机惯性视觉组合定位方法 |
| CN116205947A (zh) * | 2023-01-03 | 2023-06-02 | 哈尔滨工业大学 | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10019637B2 (en) * | 2015-11-13 | 2018-07-10 | Honda Motor Co., Ltd. | Method and system for moving object detection with single camera |
-
2023
- 2023-12-27 CN CN202311809922.2A patent/CN117470248B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108833920A (zh) * | 2018-06-04 | 2018-11-16 | 四川大学 | 一种基于光流和块匹配的dvc边信息融合方法 |
| CN111383247A (zh) * | 2018-12-29 | 2020-07-07 | 北京易讯理想科技有限公司 | 增强金字塔lk光流算法图像跟踪稳定性的方法 |
| CN111024066A (zh) * | 2019-12-10 | 2020-04-17 | 中国航空无线电电子研究所 | 一种无人机视觉-惯性融合室内定位方法 |
| CN115222974A (zh) * | 2022-07-14 | 2022-10-21 | Oppo广东移动通信有限公司 | 特征点匹配方法及装置、存储介质及电子设备 |
| CN115615424A (zh) * | 2022-08-25 | 2023-01-17 | 中国人民解放军火箭军工程大学 | 一种起重机惯性视觉组合定位方法 |
| CN116205947A (zh) * | 2023-01-03 | 2023-06-02 | 哈尔滨工业大学 | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 |
Non-Patent Citations (9)
| Title |
|---|
| A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors;Tong Qin;《Computer Vision and Pattern Recognition》;20190111;1-7 * |
| Distance and velocity estimation usingoptical flow from a monocular camera;Hann Woei Ho;《International Journal of Micro AirVehicles》;20171231;第9卷(第3期);198-208 * |
| VINS-Mono: A Robust and Versatile MonocularVisual-Inertial State Estimator;Tong Qin;《IEEE Transactions on Robotics》;20181231;1-17 * |
| VINS论文推导及代码解析;Treememory666;《豆丁 https://www.docin.com/p-2287521664.html》;20191218;1-43 * |
| 一种基于VINS的视觉里程计改进方法;李歆;《国外电子测量技术》;20230131;第42卷(第01期);20-27 * |
| 基于机器视觉的光刻机工件台调平系统设计与实现;万东栩;《中国优秀硕士学位论文全文数据库信息科技辑》;20220115(第01期);I135-567 * |
| 李歆.一种基于VINS的视觉里程计改进方法.《国外电子测量技术》.2023,第42卷(第01期),20-27. * |
| 白话VINS-Mono之外参标定（二）;栗子;《知乎，https://zhuanlan.zhihu.com/p/413096765?utm_id=0》;20220428;1-7 * |
| 白话VINS-Mono之边缘化（五）;栗子;《知乎，https://zhuanlan.zhihu.com/p/430996372?utm_id=0》;20211117;1-13 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117470248A (zh) | 2024-01-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109993113B (zh) | 一种基于rgb-d和imu信息融合的位姿估计方法 | |
| CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
| CN109029433B (zh) | 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法 | |
| CN109166149B (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 | |
| CN110009681B (zh) | 一种基于imu辅助的单目视觉里程计位姿处理方法 | |
| Cvišić et al. | Stereo odometry based on careful feature selection and tracking | |
| CN106127739B (zh) | 一种结合单目视觉的rgb-d slam方法 | |
| CN109676604B (zh) | 机器人曲面运动定位方法及其运动定位系统 | |
| CN107909614B (zh) | 一种gps失效环境下巡检机器人定位方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
| CN103745474A (zh) | 基于惯性传感器和摄像机的图像配准方法 | |
| CN114693754B (zh) | 一种基于单目视觉惯导融合的无人机自主定位方法与系统 | |
| CN111609868A (zh) | 一种基于改进光流法的视觉惯性里程计方法 | |
| CN114529576A (zh) | 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法 | |
| CN108253962A (zh) | 一种低照度环境下新能源无人驾驶汽车定位方法 | |
| CN116205947A (zh) | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 | |
| Li et al. | A binocular MSCKF-based visual inertial odometry system using LK optical flow | |
| CN112179373A (zh) | 一种视觉里程计的测量方法及视觉里程计 | |
| Rahman et al. | Contour based reconstruction of underwater structures using sonar, visual, inertial, and depth sensor | |
| Huttunen et al. | A monocular camera gyroscope | |
| CN112731503B (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| CN112580683A (zh) | 一种基于互相关的多传感器数据时间对齐系统及其方法 | |
| CN114812601A (zh) | 视觉惯性里程计的状态估计方法、装置、电子设备 | |
| CN117470248B (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 |