CN111780763B - 一种基于视觉地图的视觉定位方法、装置 - Google Patents
一种基于视觉地图的视觉定位方法、装置 Download PDFInfo
- Publication number
- CN111780763B CN111780763B CN202010618519.1A CN202010618519A CN111780763B CN 111780763 B CN111780763 B CN 111780763B CN 202010618519 A CN202010618519 A CN 202010618519A CN 111780763 B CN111780763 B CN 111780763B
- Authority
- CN
- China
- Prior art keywords
- positioning
- current frame
- frame
- current
- matching
- 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 71
- 230000000007 visual effect Effects 0.000 title claims abstract description 51
- 238000007781 pre-processing Methods 0.000 claims description 21
- 238000004364 calculation method Methods 0.000 claims description 20
- 230000009466 transformation Effects 0.000 claims description 19
- 239000011159 matrix material Substances 0.000 claims description 16
- 238000005457 optimization Methods 0.000 claims description 16
- 238000001914 filtration Methods 0.000 claims description 15
- 238000000605 extraction Methods 0.000 claims description 13
- 238000006243 chemical reaction Methods 0.000 claims description 12
- 238000012545 processing Methods 0.000 claims description 8
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 7
- 238000012216 screening Methods 0.000 claims description 7
- 238000013507 mapping Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 230000004044 response Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000001514 detection method Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 abstract description 14
- 230000006870 function Effects 0.000 description 17
- 238000010586 diagram Methods 0.000 description 16
- 235000001275 Bouea macrophylla Nutrition 0.000 description 4
- 240000001160 Bouea macrophylla Species 0.000 description 4
- 230000008859 change Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000012790 confirmation Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012417 linear regression Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000012360 testing method 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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Image Analysis (AREA)
Abstract
本申请公开了一种基于视觉地图的视觉定位方法,该方法包括,采集当前图像,得到当前帧;基于当前帧,进行特征点提取,得到当前帧的特征点;根据当前定位状态确定定位策略,基于定位策略,将当前帧中的特征点与地图中的地图点进行匹配,得到匹配特征点,当匹配特征点满足用于当前帧位姿解算的条件时,根据匹配特征点计算当前帧的位姿,得到定位结果。本申请不需要设定位置精确的标定点,对未建图区域或定位失败时,具有较佳的鲁棒性,定位过程中可以持续与地图进行匹配而不会存在大幅跳变。
Description
技术领域
本发明涉及视觉导航领域,特别地,涉及一种基于视觉地图的视觉定位方法。
背景技术
视觉导航是通过摄像装置对周围环境进行图像采集,并基于图像进行计算,完成位置确定和路径识别,通常需要依赖视觉地图来完成,其中,视觉地图可利用图像特征点以及描述子构建环境地图。以基于地面纹理的地图为例,移动机器人移动经过某纹理点的时候,根据当前图像与地图的特征配准,解算出当前移动机器人的位姿,即可进行基于地面纹理的定位导航。
现有的基于视觉地图的定位方法,使用一帧图像的匹配结果进行定位,导致定位鲁棒性差。
发明内容
本发明提供了一种基于视觉地图的视觉定位方法,以提高视觉定位的鲁棒性。
本发明提供的一种基于视觉地图的视觉定位方法是这样实现的:
采集当前图像,得到当前帧;
基于当前帧,进行特征点提取,得到当前帧的特征点;
根据当前定位状态确定定位策略,
基于定位策略,将当前帧中的特征点与地图中的地图点进行匹配,得到匹配特征点,
当匹配特征点满足用于当前帧位姿解算的条件时,根据匹配特征点计算当前帧的位姿,得到定位结果。
较佳地,所述得到匹配特征点之后,进一步包括,
基于匹配特征点,采用随机抽样一致性算法确定最佳匹配特征点集合,
判断最佳匹配特征点集合是否满足用于位姿解算的条件,当匹配特征点不满足用于当前帧位姿解算的条件时,判定当前帧定位失败;
所述定位状态根据上一帧的定位结果、当前连续定位失败帧数确定;所述定位状态包括,未初始化状态,定位成功状态,以及定位丢失状态;
所述定位策略包括,未初始化状态下的初始化定位,定位成功状态下的正常定位,定位丢失状态下的重定位,以及各个定位状态之间转换关系;
其中,所述各个定位状态之间转换关系包括,
当前状态为未初始化状态时,如果初始化定位成功,则转换至定位成功状态,否则保持当前未初始化状态;
当前状态为定位成功状态时,如果正常定位成功,则保持当前定位成功状态,否则,转换至定位丢失状态;
当前状态为定位丢失状态时,如果重定位成功,则转换至定位成功状态,否则,判断连续定位失败的帧数是否超过设定的帧数阈值,如果是,则转换至未初始化状态,否则,则保持当前定位丢失状态;
所述根据当前定位状态确定定位策略包括,根据各个定位状态以及各个定位状态之间转换关系,生成定位逻辑;
所述基于定位策略,将当前帧中的特征点与候选地范围内的地图点进行匹配,包括,基于定位逻辑,按照各个状态下的定位策略,为当前帧确定地图中候选地图范围,将当前帧中的特征点与候选地图范围内的地图点进行匹配。
较佳地,所述根据各个定位状态以及各个定位状态之间转换关系,生成定位逻辑,包括,
判断当前帧是否是第一帧,
如果当前帧是第一帧,则对当前帧进行初始化定位,
如果当前帧不是第一帧,则判断上一帧是否定位成功,
如果上一帧定位成功,则对当前帧进行正常定位;
如果上一帧定位不成功,则判断当前连续定位失败帧数是否超过设定的帧数阈值,或者,当前帧与最近一次定位成功的位姿之间的距离是否超过设定的第一距离阈值;
如果当前连续定位失败帧数超过帧数阈值,则对当前帧进行初始化定位,
如果当前连续定位失败帧数未超过帧数阈值,则对当前帧进行重定位。
较佳地,所述如果当前帧是第一帧,则对当前帧进行初始化定位,进一步包括,
根据当前帧是第一帧,判定当前状态为未初始化状态,
如果当前帧初始化定位成功,则记录当前帧定位成功,转换至定位成功状态,否则,记录当前帧定位失败,保持当前未初始化状态;
所述如果上一帧定位成功,则对当前帧进行正常定位,进一步包括,
根据上一帧定位成功,判定当前状态为正常定位状态,
如果当前帧正常定位成功,则记录当前帧定位成功,保持当前定位成功状态,否则,记录当前帧定位失败,转换至定位丢失状态;
所述如果当前连续定位失败帧数超过帧数阈值,则对当前帧进行初始化定位,包括,
根据当前连续定位失败帧数超过帧数阈值,判定当前状态为未初始化状态,
如果当前帧初始化定位成功,则记录当前帧定位成功,转换至定位成功状态,否则,记录当前帧定位失败,保持当前未初始化状态;
所述如果当前连续定位失败帧数未超过帧数阈值,则对当前帧进行重定位,进一步包括,
根据当前连续定位失败帧数未超过帧数阈值,判定当前状态为重定位状态,
如果重定位定位成功,则记录当前帧定位成功,转换至定位成功状态,否则,记录当前帧定位失败,返回所述判断当前连续定位失败帧数是否超过设定的帧数阈值步骤。
较佳地,所述基于定位逻辑,按照各个状态下的定位策略,为当前帧确定地图中候选地图范围,包括,
对当前帧进行初始化定位时,将地图中的所有地图点作为第一候选地图范围,或者,利用辅助信息获得第一候选地图范围;对第一候选地图范围内的地图点采用暴力匹配进行筛选,如果每两个地图点特征匹配度超过设定的第二匹配阈值,则随机删掉其中的一个地图点;
对当前帧进行正常定位时,根据上一帧到当前帧的帧间运动信息,对当前帧进行位姿预测,得到当前帧预测位姿;根据当前帧预测位姿,确定第二候选地图范围;
对当前帧进行重定位时,根据定位成功的历史帧,向前追溯历史帧中与当前帧最近的一帧并作为参考帧,将该参考帧作为上一帧,根据该上一帧到当前帧的帧间运动信息,对当前帧进行位姿预测,得到当前帧预测位姿;根据当前帧预测位姿,确定第三候选地图范围。
较佳地,所述根据上一帧到当前帧的帧间运动信息,对当前帧进行位姿预测,包括,
第一方式:通过轮式里程计或惯性测量元件获得上一帧到当前帧的帧间位姿变换,基于帧间位姿变换、以及上一帧的定位结果,求得当前帧的预测位姿;
或者
第二方式:通过视觉里程计获得上一帧到当前帧的帧间位姿变换,基于帧间位姿变换、以及上一帧的定位结果,求得当前帧的预测位姿;
或者
第三方式:根据已获得定位结果的历史帧,预测上一帧到当前帧的帧间位姿变换,基于帧间位姿变换、以及上一帧的定位结果,求得当前帧的预测位姿;
或者,
第四方式:至少采用第一方式、第二方式、第三方式中的两方式,分别求得当前帧的第一预测位姿,得到至少两个第一预测位姿;
采用卡尔曼滤波器对所述至少两个第一预测位姿进行滤波,得到滤波后的第二预测位姿,将第二预测位姿作为当前帧最终的预测位姿;或者,采用非线性优化的方法,基于所述至少两个第一预测位姿进行优化,得到优化后的第二预测位姿,将第二预测位姿作为当前帧最终的预测位姿;
其中,非线性优化的目标函数为根据各个方式所得到的各个误差项子和,将上一帧的位姿、不同方式所得到的当前帧j的第一预测位姿、以及帧间位姿变换作为初始值,代入目标函数,求解使得目标函数取得最小值时的位姿,作为第二预测位姿。
较佳地,所述根据当前帧预测位姿,确定第二候选地图范围,包括,
以当前帧的预测位姿所确定的地图位置为中心,将该中心的第一邻域确定为第二候选地图范围,
或者
对于当前帧的每个特征点,根据当前帧的预测位姿和该特征点的像素坐标,计算当前帧特征点投影到地图中的投影点位置,以该投影点位置为中心的第一邻域作为该特征点第二候选地图范围;
所述根据当前帧预测位姿,确定第三候选地图范围,包括,
以当前帧的预测位姿所确定的地图位置为中心,将该中心的第二邻域确定为第三候选地图范围,
或者
对于当前帧的每个特征点,根据当前帧的预测位姿和该特征点的像素坐标,计算当前帧特征点投影到地图中的投影点位置,以该投影点位置为中心的第二邻域作为该特征点第三候选地图范围;
所述第二邻域范围大于第一领域范围。
较佳地,所述基于匹配特征点,采用随机抽样一致性算法确定最佳匹配特征点集合,包括,
从匹配特征点所构成的匹配特征点集合中,随机选择用于计算当前帧位姿估计的匹配特征点,得到当前匹配特征点子集;
基于匹配特征点子集中的匹配特征点所建立的空间位置信息与像素位置信息的映射,计算当前位姿,得到该匹配特征点子集的拟合位姿估计,
根据拟合位姿估计以及相机内参,获取当前帧中所有的特征点的空间位置,得到所有特征点的投影点空间位置;
对于匹配特征点集合中的每个匹配特征点,根据该匹配特征点的投影点空间位置,判断当前帧中该匹配特征点的投影点与地图中该匹配特征点的距离是否小于设定的第二距离阈值,如果是,则判定该匹配特征点为内点;反复执行所述判断当前帧中该匹配特征点的投影点与地图中该匹配特征点的距离是否小于设定的第二距离阈值的步骤,直至匹配特征点集合中所有匹配特征点都被进行了内点的判定;
统计当前内点数量,判断当前统计的内点数量是否为历次迭代中的最大,如果是,则将当前内点所构成的集合作为当前最佳匹配特征点集合;
判断是否达到结束条件,如果是,则将当前最佳匹配特征点集合作为最终的最佳匹配特征点集合,否则,返回所述从匹配特征点所构成的匹配特征点集合中随机选择用于计算当前帧位姿估计的匹配特征点的步骤;
所述判断最佳匹配特征点集合是否满足用于位姿解算的条件,包括,
根据匹配特征点的数量判断最佳匹配特征点集合是否满足用于位姿解算的条件;
或者,
对至少两个以上最佳匹配特征点集合分别赋予衡量当前帧与候选地图范围匹配程度的权值;所述权值根据最佳匹配特征点集合中的匹配特征点数量、当前帧特征点提取数量,特征点分布情况,匹配特征点初始数量与最佳匹配特征点集合中的匹配特征点数量之一或其任意组合确定,
根据设定的权值阈值和最大权值,判断最佳匹配特征点集合是否满足用于位姿解算的条件。
较佳地,所述根据匹配特征点计算当前帧的位姿,得到定位结果,进一步包括,
对计算得到当前帧的位姿采用基于滑动窗口的非线性优化;优化的变量为滑动窗口内各图像帧的位姿,该滑动窗口包括当前帧,优化的约束为当前帧特征点与上一关键帧特征点之间的帧间匹配约束、和/或当前帧特征点与地图中的地图点之间的地图匹配约束,
利用最小二乘法,最小化帧间匹配误差和/或地图匹配误差,得到优化后的当前帧位姿,作为定位结果;
其中,
地图匹配约束为:第一匹配地图点反投影到当前帧上的像素位置与当前帧中与该地图点匹配的第一匹配特征点的像素位置之间的误差,或者,当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与该第一匹配特征点匹配的第一匹配地图点在世间坐标系中的空间位置之间的误差;所述第一匹配特征点为:当前帧中的特征点与地图中的地图点进行匹配所得到匹配成功的特征点;第一匹配地图点为:被第一匹配特征点匹配成功的地图点;
帧间匹配约束为:当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与当前帧的上一关键帧中与该第一匹配特征点匹配的第二匹配特征点投影至世界坐标系中的空间位置之间的误差,或者,与第二匹配特征点匹配的第二匹配地图点反投影到当前帧的像素位置与该第二匹配地图点反投影到上一关键帧的像素位置之间的误差;
所述采集当前图像,得到当前帧,进一步包括,对当前帧进行图像预处理。
较佳地,所述利用最小二乘法,最小化帧间匹配误差和/或地图匹配误差,包括,
构建目标函数,该函数为:以第一权值加权当前滑动窗口中所有帧的所有第一匹配特征点的地图匹配误差之和所得到的第一结果,以及,以第二权值加权当前滑动窗口中各帧与其上一关键帧之间所有帧间的所有第二匹配地图点的帧间匹配误差之和所得到的第二结果,所累加的第一结果与第二结果之和;
以根据当前帧的位姿、第一匹配地图点的空间位置信息、相机内参、当前帧中的与第一匹配地图点匹配的第一匹配特征点的像素坐标所得到的地图匹配误差,作为地图匹配误差初始值,
以根据当前帧位姿、第二匹配地图点的空间位置信息、上一关键帧的位姿、相机内参矩阵所得到帧间匹配误差,作为帧间匹配误差初始值,
迭代求解使得目标函数取得最小值时当前帧的位姿;
所述得到优化后的当前帧位姿之后,进一步包括,
满足以下条件之一时,判定当前帧为关键帧:
当前帧中第一匹配特征点的数量大小于第一阈值,
当前帧中第二匹配特征点的数量大小于第二阈值;
若当前帧为非关键帧时,则在滑动窗口中删除该当前帧,
若当前帧为关键帧,则判断当前滑动窗口中帧数量是否达到设定的第一帧阈值,如果是,则删除滑动窗口中最早添加的关键帧,否则,则不删除滑动窗口中最早添加的关键帧;
所述对当前帧进行图像预处理包括,
根据相机的畸变系数对当前帧进行去畸变处理,得到去畸变图像,
判断去畸变图像中各个像素点的像素值是否大于第一像素阈值,如果是,则将像素值大于第一像素阈值的像素点进行取反操作,然后进行图像滤波,得到背景图像,否则,将去畸变图像进行图像滤波,得到背景图像,
用去畸变图像减去背景图像,得到前景图像,
判断前景图像中的像素值是否分布均匀,如果均匀,则将该前景图像作为图像预处理后的当前帧,否则,对前景图像进行拉伸处理,得到图像预处理后的当前帧。
较佳地,所述前景图像进行拉伸处理包括,
若前景图像像素值小于等于设定的最小灰度值时,将该前景图像像素值取值为像素取值范围内的最小值;
若前景图像像素值大于最小灰度值、且小于设定的最大灰度值时,按照与像素最大值成一定比例的像素值作为该前景图像像素值;所述比例为前景图像像素值与最小灰度值之差与最大灰度值与最小灰度值之差的比值;
若前景图像像素值大于等于最大灰度值时,将该前景图像像素值取值为像素取值范围内的最大值;
所述基于当前帧,进行特征点提取,得到当前帧的特征点,包括,
对当前帧进行特征检测,得到特征点,
将当前帧划分成一定数量的网格,
对于任一网格中的特征点,将网格内的特征点按特征点响应值降序排列,保留前Q个特征点,得到筛选后的特征点;其中,Q根据该目标图像帧中特征点的数量和设定的特征点总数上限、以及该网格特征点总数确定;
对筛选后的各特征点,分别计算特征描述子。
本发明提供的一种基于视觉地图的视觉定位装置,该装置包括,
图像采集模块,采集当前图像,得到当前帧;
特征提取模块,基于当前帧,进行特征点提取,得到当前帧的特征点;
定位模块,根据当前定位状态确定定位策略,基于定位策略,将当前帧中的特征点与地图中的地图点进行匹配,得到匹配特征点,当匹配特征点满足用于当前帧位姿解算的条件时,根据匹配特征点计算当前帧的位姿,得到定位结果。
本发明还提供一种计算机可读存储介质,所述存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现上述任一所述基于视觉地图的视觉定位方法的步骤。
本发明提供的基于视觉地图的视觉定位方法,根据当前定位状态所确定的定位策略来进行当前帧的特征匹配和位姿计算,不需要设定位置精确的标定点,对未建图区域或定位失败时,具有较佳的鲁棒性,定位过程中可以持续与地图进行匹配而不会存在大幅跳变。
附图说明
图1是本实施例视觉定位的一种流程示意图。
图2为特征点筛选示意图。
图3为定位状态之间转换关系的一种示意图。
图4为定位逻辑的一种示意图。
图5为初始化定位的一种流程示意图。
图6为地图点筛选的一种示意图。
图7为正常定位的一种流程示意图。
图8为重定位的一种流程示意图。
图9a和图9b为确定候选地图范围的一种示意图。
图10为本发明的基于视觉地图的视觉定位装置的一种示意图。
图11为图像预处理模块的一种示意图。
图12为图像坐标系下当前帧的地图匹配约束、帧间匹配约束的一种示意图。
图13为世界坐标系下当前帧的地图匹配约束、帧间匹配约束的一种示意图。
具体实施方式
为了使本申请的目的、技术手段和优点更加清楚明白,以下结合附图对本申请做进一步详细说明。
本申请根据不同定位状态所采用不同的定位策略而确定定位逻辑,使得定位状态在满足定位逻辑的情形下进行转换,其中,定位策略在位姿预测、特征匹配的处理上不同,从而使得定位整体上的鲁棒性得以增加。
以下将以基于视觉地图的视觉定位来进行说明,其中,视觉地图为已预先构建好的特征地图,该地图中具有三维空间信息的地图点,即,地图中保存有特征点的世界坐标和描述子信息。
为便于理解,实施例中,视觉地图为基于采集的地面纹理信息所构建的纹理地图,可以是二维或三维带有特征描述子的点云地图,该纹理地图覆盖范围可以是连续覆盖的,也可以是离散覆盖的。以下以三维点云地图为例。
参见图1所示,图1为本实施例视觉定位的一种流程示意图。移动机器人在加载有纹理地图后,执行如下步骤:
步骤101,采集当前图像,得到当前帧;
步骤102,对当前帧进行图像预处理,以使得图像中的纹理显著,包括且不限于,图像去畸变、图像滤波、图像增强等可选的处理。该步骤为基于图像质量的可选步骤。例如,根据当前帧是否去过畸变、纹理是否显著确定是否加入图像预处理。
较佳地,对当前帧进行图像预处理的一种流程示意图,包括如下步骤:
步骤1021,根据相机的畸变系数对当前帧进行去畸变处理,得到去畸变图像I(u,v),其中,u、v表示像素坐标。
步骤1022,判断去畸变图像中各个像素点的像素值是否大于设定的第一像素阈值,如果是,则将像素值大于第一像素阈值的像素点进行取反操作,然后对进行了取反操作后的去畸变图像进行滤波,否则,直接对去畸变图像I(u,v),进行图像滤波,得到背景图像Ib(u,v)。
步骤1023,用去畸变图像减去背景图像,得到前景图像If(u,v),其中,前景图像的数学式表达为:
If(u,v)=I(u,v)-Ib(u,v)
步骤1024,判断前景图像If(u,v)的像素值是否分布均匀,如果是,则将前景图像作为预处理后的当前帧,否则,则对前景图像进行拉伸,得到预处理后的当前帧;
其中,对前景图像进行拉伸处理可以为:
当前景图像像素值小于等于最小灰度值时,将该前景图像像素值取值为像素取值范围内的最小值,即像素值为0;
当前景图像像素值大于最小灰度值、且小于最大灰度值时,将增加该前景图像像素值的对比度;较佳地,按照与像素最大值成一定比例的像素值作为该前景图像像素值;较佳地,所述比例为:前景图像像素值与最小灰度值之差与最大灰度值与最小灰度值之差的比值。
当前景图像像素值大于等于最大灰度值时,将该前景图像像素值取值为像素取值范围内的最大值,例如,像素最大值为255。
用数学式表达为:
拉伸后的前景图像If'(u,v)表示为:
其中,Imin为最小灰度值,Imax为最大灰度值,在上式中,像素取值范围为0~255。
步骤103,提取预处理后的当前帧中的图像特征点,并基于特征点计算描述子,得到当前帧的特征点和描述子。其中,描述子形式与地图中地图点的描述子形式相同。可采用ORB(Oriented FAST and Rotated BRIEF)、尺度不变特征变换(SIFT)、SURF(Speeded UpRobust Features)等特征。例如,地图点的描述子为ORB(Oriented FAST and RotatedBRIEF)特征点,则当前帧的描述子也为ORB特征点。
较佳地,按照与所构建地图的特征点形成方式,提取预处理后的当前帧中的图像特征点。
在本实施例中,采用ORB特征点。
鉴于所采集的地面纹理图像通常尺度稳定,无需多尺度的特征进行构建,以增强特征的尺度不变性,故而可放弃金字塔图像的构建,而基于当前帧图像进行特征提取,以相当于基于源图像进行特征提取,从而减小特征提取需要的计算量,提高计算效率。
鉴于均匀而又显著的特征点能减少相机位姿计算时的定位误差,为了提高定位的性能和效率,可对提取的特征点进行筛选。图2为特征点筛选示意图。因此在提取FAST(Features from Accelerated Segment Test)特征后,可以将当前帧划分为一定数量的网格,每个网格筛选出Q个FAST响应值最高的特征点予以保留。其中,Q根据一帧目标图像中特征点的数量和所设特征点总数上限、网格中的帧点总数确定,不同网格所保留的特征点数不同。例如,一帧目标图像中,特征点总数的上限设定为100个,当前帧特征点的数量为2000个,则该帧目标图像中每20个特征点选出一个,如果某网格中有20个特征点,则该网格保留的特征点为1,即,Q=1。用数学式表达为:
步骤104,鉴于在定位的过程中,如果遇到当前位置的地面纹理未曾建图,或地面纹理发生改变等情况,那么当前帧与地图点不能正常匹配,这时就会出现某一帧定位丢失的情况。因此,在定位过程中根据当前不同的定位状态,在当前帧采用不同的定位策略进行定位。即:
根据当前的定位状态,确定与定位状态对应的定位策略,其中,定位状态包括未初始化状态,定位成功状态,以及定位丢失状态,各个定位状态分别对应的定位策略包括,未初始化状态下的初始化定位,定位成功状态下的正常定位,定位丢失状态下的重定位,以及各个状态之间的转换关系。
参见图3所示,图3为定位状态之间转换关系的一种示意图。其中,
当前状态为未初始化状态时,如果初始化定位成功,则转换至定位成功状态,否则保持当前未初始化状态;
当前状态为定位成功状态时,如果正常定位成功,则保持当前定位成功状态,否则,转换至定位丢失状态;
当前状态为定位丢失状态时,如果重定位成功,则转换至定位成功状态,否则,判断连续定位失败的帧数是否超过设定的帧数阈值,如果是,则转换至未初始化状态,否则,则保持当前定位丢失状态。
基于定位状态之间转换关系,可以得到定位逻辑流程。参见图4所示,图4为定位逻辑的一种示意图,包括,
步骤1041,判断当前帧是否是第一帧,
如果是,则说明没有上一帧的定位结果,判定当前状态为未初始化状态,执行初始化定位,如果定位成功,则记录当前帧定位成功,转换至定位成功状态,否则,记录当前帧定位失败,保持当前未初始化状态;
否则,执行步骤1042,
步骤1042,判断上一帧是否定位成功,
如果成功,则判定当前状态为正常定位状态,执行正常定位,如果定位成功,则记录当前帧定位成功,保持当前定位成功状态,否则,记录当前帧定位失败,转换至定位丢失状态;
否则,执行步骤1043,
步骤1043,判断当前连续定位失败帧数是否超过帧数阈值,或者,当前帧与最近一次定位成功的位姿之间的距离是否超过设定的第一距离阈值;
如果是,则判定当前状态为未初始化状态,执行初始化定位,如果定位成功,则记录当前帧定位成功,转换至定位成功状态,否则,记录当前帧定位失败,保持当前未初始化状态;
否则,判定当前状态为定位丢失状态,执行重定位,如果定位成功,则记录当前帧定位成功,转换至定位成功状态,否则,记录当前帧定位失败,返回执行步骤1043;
其中,
初始化定位在第一帧或连续定位失败帧数超过阈值的情况下进行。由于此时没有上一帧的定位结果,因此无法进行准确的位姿预测。通常采用搜索全局地图中的地图点或利用辅助信息等方法获得第一候选地图范围,将当前帧的特征点与全局地图中的地图点或第一候选地图范围中的地图点进行匹配,如果匹配成功,则根据匹配特征点计算当前帧的位姿,从而得到当前帧的定位结果。
正常定位是在上一帧定位成功的情况下进行的定位策略。在上一帧定位成功的情况下,通过位姿预测可以较准确的预测当前帧位姿,因此在进行特征匹配时,可以根据预测位姿的第一邻域确定第二候选地图范围,将当前帧的特征点与第二候选地图范围中的地图点进行匹配,如果匹配成功,则根据匹配特征点计算当前帧的位姿,从而得到当前帧的定位结果。
重定位是在上一帧定位丢失的情况下进行的定位策略。在上一帧定位丢失的情况下,向前追溯定位未丢失与当前帧的最近一帧作为参考帧,根据参考帧预测出当前帧的位姿。在进行特征匹配时,根据预测位姿的第二邻域确定第三候选地图范围,并将当前帧的特征点与第三候选地图范围中的地图点采用暴力匹配的方法进行特征匹配,这样可以保证重定位的鲁棒性。如果重定位匹配成功,则根据匹配特征点计算当前帧的位姿,从而得到当前帧的定位结果。
较佳地,所述第一邻域范围小于第二邻域范围。
步骤105,对定位成功的当前帧位姿进行图优化,以对定位结果进行优化,提高定位的准确性。
定位优化的目的是使输出的定位结果更准确,平滑,提升整个系统的准确性和鲁棒性。较佳地,定位优化采用基于滑动窗口的非线性优化,优化的变量为滑动窗口内各图像帧的位姿,所述当前帧被增加至滑动窗口中,优化的约束为帧间匹配约束、和/或地图匹配约束,利用LM方法最小化帧间匹配误差和/或地图投影误差,得到优化后的当前帧位姿。
其中,
地图匹配约束为:第一匹配地图点反投影到当前帧上的像素位置与当前帧中与该地图点匹配的第一匹配特征点的像素位置之间的误差,或者,当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与该第一匹配特征点匹配的第一匹配地图点在世间坐标系中的空间位置之间的误差;所述第一匹配特征点为:当前帧中的特征点与地图中的地图点进行匹配所得到匹配成功的特征点;第一匹配地图点为:被第一匹配特征点匹配成功的地图点;
帧间匹配约束为:当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与当前帧的上一关键帧中与该第一匹配特征点匹配的第二匹配特征点投影至世界坐标系中的空间位置之间的误差,或者,与第二匹配特征点匹配的第二匹配地图点反投影到当前帧的像素位置与该第二匹配地图点反投影到上一关键帧的像素位置之间的误差。
参见图12所示,图12为当前帧的地图匹配约束、帧间匹配约束的一种示意图。其中,第二匹配特征点是第一匹配特征点集合的子集,与第二匹配特征点匹配的第二匹配地图点是与第一匹配特征点匹配的第一匹配地图点集合的子集。第一匹配地图点反投影至当前帧的像素位置与该第一地图点匹配的第一匹配特征点的像素位置之间的误差构成地图匹配约束;第二匹配地图点反投影至当前帧的像素位置与该第二匹配地图点反投影当前帧的上一关键帧的像素位置之间的误差构成帧间匹配约束。
所应理解的是,尽管本申请实施例所述地图匹配约束、帧间匹配约束是在图像坐标系下确定的,地图匹配约束和帧间匹配约束也可以在世界坐标系下确定。
参见图13所示,图13为世界坐标系下当前帧的地图匹配约束、帧间匹配约束的一种示意图。其中,当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与该第一匹配特征点匹配的第一匹配地图点在世间坐标系中的空间位置之间的误差构成地图匹配约束;当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与当前帧的上一关键帧中与该第一匹配特征点匹配的第二匹配特征点投影至世界坐标系中的空间位置之间的误差构成帧间匹配约束。所述匹配特征点投影至世界坐标系中的空间位置按照相机模型,根据相机内参、匹配特征点的像素位置、匹配特征点所在的帧的位姿得到。
较佳地,所述第一匹配特征点为最佳匹配特征点集合中的。
在该步骤中,构建目标函数,该函数为:以当前滑动窗口中所有帧的所有第一匹配特征点的地图匹配误差之和所得到的第一结果,和/或,以当前滑动窗口中所有帧与其上一关键帧的所有第二匹配地图点的帧间匹配误差之和所得到的第二结果;
以优化的约束为帧间匹配约束和地图匹配约束为例,目标函数数学式表示为:
其中,
eik-map=pik-(K(RiXk+ti)),eijm-frame=(K(RiXm+ti))-(K(RjXm+tj))
eik-map为第一匹配地图点k反投影到当前帧i上的像素位置与当前帧i中与该地图点匹配的第一匹配特征点k的像素位置之间的误差地图匹配误差;pik为当前帧i中第一匹配特征点k的像素坐标,K为相机内参矩阵,Xk为地图中的第一匹配地图点k的三维坐标,Ri、ti为通过第一匹配特征点得到的当前帧i的位姿。
eijm-frame为第二匹配地图点m在当前帧i和上一关键帧j的反投影误差,Xm为当前帧i和上一关键帧j中的第二匹配特征点所匹配的第二匹配地图点m的三维坐标,Ri、ti为当前帧i的位姿,Rj、tj为上一关键帧j的位姿,可通过第二匹配特征点得到。
I为滑动窗口中帧的总数,K为当前帧内第一匹配特征点的总数,j为滑动窗口中各帧的上一关键帧,M为当前帧中所反投影的第二匹配地图点总数。
在目标函数中,
代入根据当前帧i的位姿、地图中的第一匹配地图点k的三维坐标、相机内参矩阵K、当前帧i中的第一匹配特征点k的像素坐标所得到的地图匹配约束,作为地图匹配约束初始值;
代入根据当前帧i的位姿、地图中第二匹配地图点m的三维坐标、上一关键帧的位姿、相机内参矩阵K所得到帧间匹配约束,作为帧间匹配约束初始值,
迭代求解使得目标函数取得最小值时的当前帧位姿,将该位姿作为当前定位结果。
进一步地,还可以赋以地图匹配约束和帧间匹配约束不同的权值,来构建所述目标函数,以进一步提高定位的准确性,该函数为:以第一权值加权当前滑动窗口中所有帧的所有第一匹配特征点的地图匹配误差之和所得到的第一结果,以及,以第二权值加权当前滑动窗口中各帧与其上一关键帧之间所有帧间的所有第二匹配地图点的帧间匹配误差之和所得到的第二结果,所累加的第一结果与第二结果之和;用数学式表示为:
其中,γ1、γ2为权值。
此外,在迭代求解当前帧位姿时,还可以将约束帧中所有各帧的位姿作为待优化的变量来进行优化。
在优化结束后,对当前滑动窗口进行优化,以进一步提高定位的准确性。具体为:
满足以下条件之一时,判定当前帧为关键帧:
当前帧中第一匹配特征点的数量小于第一阈值,
当前帧中第二匹配特征点的数量小于第二阈值;
若当前帧为非关键帧时,则在滑动窗口中删除该当前帧,
若当前帧为关键帧,则判断当前滑动窗口中帧数量是否达到设定的第一帧阈值,如果是,则删除滑动窗口中最早添加的关键帧,否则,则不删除滑动窗口中最早添加的关键帧。
以下对步骤104中所涉及的各个定位策略进行说明。
参见图5所示,图5为初始化定位的一种流程示意图。当前帧在图像预处理和特征点提取之后,初始化定位包括,
步骤501,将当前帧的特征点与地图中的地图点进行匹配,匹配成功时,得到第一匹配特征点,所得到的第一匹配特征点构成第一匹配特征点集合;这样,对于第一匹配特征点而言,将与第一匹配特征点匹配的第一匹配地图点的空间位置信息确定为该第一匹配特征点的空间位置信息,从而获得当前帧的二维特征点与地图中三维特征点的匹配。
所述匹配可以是这样进行的:对于当前帧的任一特征点,计算当前帧中的特征点与地图中地图点描述子之间的匹配度是否小于设定的第一匹配阈值,如果是,则判定匹配,否则,判定不匹配;其中,匹配度可以用汉明距离来描述,匹配阈值为汉明距离阈值;
在该步骤中,地图中的地图点可以是地图中所有的地图点,这时采用暴力匹配的方法进行匹配,即,进行两两匹配时每一种匹配的可能性都进行计算;
较佳地,为了减少匹配计算的计算量,利用辅助信息获得候选地图范围,例如,设置易辨识的位置作为候选地图范围,如路径起终点位置,转弯位置,十字交叉位置。以网格路径地图为例,可设置所有路径的交点位置和角点位置作为候选地图范围,
进一步地,由于实际下视纹理场地情况不一,极有可能出现相似纹理、重复纹理的情况,例如,地毯、地砖等,鉴于此,可以对候选地图范围内的地图点进行筛选,以防止存在特征相似或相同的地图点。地图点筛选可采用暴力匹配的方法进行,即对每两个地图点特征匹配,如果匹配度超过设定的第二匹配阈值,则说明两个地图点之间非常相似,需要随机删掉一个地图点,以防止在定位初始化的过程中存在误匹配的风险。参见图6所示,图6为地图点筛选的一种示意图,其中,虚线圈为被筛选去除掉的地图点,实线圈为候选地图位置。
重复执行该步骤,直至当前帧的所有特征点都进行了匹配。
步骤502,鉴于纯描述子匹配可能存在一些误匹配点,较佳地,可以采用一定的方法筛选最佳第一匹配特征点,以提高匹配特征点的准确性,从而提高当前帧的位姿的准确性。故而,在该步骤中,基于第一匹配特征点,采用随机抽样一致性(RANSAC)算法确定最佳匹配特征点集合,具体包括,
步骤5021,从步骤501所得到第一匹配特征点集合中,随机选择用于计算当前帧位姿估计的匹配特征点,得到当前匹配特征点子集;
步骤5022,基于匹配特征点子集中的匹配特征点所建立的空间位置信息与像素位置信息的映射,计算当前位姿,从而获得该匹配特征点子集的拟合位姿估计;
该步骤中,计算当前位姿包括且不限于以下方法:透视N点定位PNP(2D-3D)方法,2维最邻近迭代2D-ICP(2D-2D),3维最邻近迭代3D-ICP(3D-3D),单应矩阵H(2D-2D)。
以通过单应矩阵计算位姿为例。
由于移动机器人是在平面中运动,空间位置坐标在同一平面z=0中,则单应矩阵与空间位置坐标矩阵的乘积对应为像素坐标矩阵,用数学式表达为:
其中单应矩阵的自由度为8,通过4个第一匹配特征点的空间位置和像素位置的对应关系,可以求出单应矩阵中每一个元素的值。对单应矩阵进行奇异值(SVD)分解,可以求得相应的旋转矩阵R和平移向量t,得到拟合位姿估计。
步骤5023,由于拟合位姿估计是由子集中的匹配特征点得到的,为了考量第一匹配特征集合中的其他匹配特征点是否也符合当前计算出来的拟合位姿估计,因此,需要计算内点率。在该步骤中,根据拟合位姿估计以及相机内参,获取当前帧中所有的特征点的空间位置,得到投影点的空间位置。
利用相机投影模型,可以将二维像素坐标点映射到三维坐标点,在本文中,这种映射称之为投影;反之,由三维坐标点映射到二维坐标点,这种映射称之为反投影。
在该步骤中,将当前帧中的所有第一匹配特征点投影至三维空间位置,即,投影至地图中,作为投影点的空间位置。故而,对于当前帧中的任一第一匹配特征点i,可以求得其三维空间坐标。
数学式可以表达为:
pi=K(RiXi+ti)
其中,pi为当前帧中第一匹配特征点i的像素坐标,Ri、ti为当前拟合位姿估计,Xi为第一匹配特征点i投影至地图的三维空间坐标,即投影点的三维空间坐标。
步骤5024,对于第一匹配特征点集合中的每个第一匹配特征点,判断当前帧中该第一匹配特征点的投影点与地图中该第一匹配特征点的距离是否小于设定的第二距离阈值,如果是,则判定该第一匹配特征点为内点,
反复执行该步骤,直至所有第一匹配特征点都被进行了是否是内点的判定。
步骤5025,统计当前内点数量,将当前内点数量与第一匹配特征点数量的比值作为内点率;比值越大,内点率越高,则说明拟合程度越高,拟合位姿估计越佳,所随机选择的匹配特征点越佳。
步骤5026,判断当前统计的内点数量是否是历次迭代中的最大,如果是,则将当前内点所构成的集合作为当前最佳匹配特征点集合,然后执行步骤5027,否则,对当前最佳匹配特征点集合不进行更新,而直接执行步骤5027,
步骤5027,判断是否达到结束条件,如果是,则执行步骤503,否则,返回步骤5021,以便重新随机选择匹配特征点子集来进行拟合位姿估计,从而进行估计-确认的循环;
其中,结束条件包括至少以下条件之一:
1)内点率满足预设的条件,
2)迭代次数满足预设的条件;为了在置信度η的条件下,在迭代循环过程中,至少有一次随机选择,使得选择的m个点均为内点,这样有利于在循环的过程中,至少有一次能取得拟合位姿估计的最佳值。因此,迭代次数α应该满足以下条件:
其中,m为子集的大小,即,子集中匹配特征点的数量;置信度一般设置在0.95~0.99的范围内。ε为内点率,在一般情况下,ε通常是未知的,因此可以取最坏条件下内点的比例,或者在初始状态下设置为最坏条件下的比例,然后随着迭代次数,不断更新为当前最大的内点率。
3)子集全部是内点的概率满足要求设定的置信度的要求,具体为,将选取的子集看作为“全部是内点”或“不全部是内点”这两种结果的二项分布,而前者的概率为p=1/εm。对于p足够小的情况下,可以将其看作为一种泊松分布,因此,在i次循环中,有θ个“子集全部是内点”的概率可以表达为:
其中,λ表示在i次循环中,“子集全都是内点”的选取次数的期望。
例如,希望在这i次迭代循环中所选子集“没有一个全是内点”的概率小于某个置信度,即:p(0,λ)=e-λ<1-η,以置信度为95%为例,λ约等于3,表示在95%置信度下,在i次循环中,平均可以选到3次“好”的子集。
步骤503,判断最佳匹配特征点集合是否满足用于位姿解算的条件,
如果是,则进行当前帧的位姿计算,计算当前位姿包括且不限于以下方法:透视N点定位PNP(2D-3D)方法,2维最邻近迭代2D-ICP(2D-2D),3维最邻近迭代3D-ICP(3D-3D),单应矩阵H(2D-2D);
否则,判定当前帧定位失败。
在该步骤中,实施方式之一,鉴于不同的位姿计算方法所要求的位姿解算的条件是不同的,以单应矩阵H(2D-2D)为例,解算出位姿至少要4个匹配特征点,故而,判断最佳匹配特征点集合中匹配特征点的数量是否满足用于位姿解算的条件。
实施方式之二,当由于多个候选地图范围使得最佳匹配特征点集合有多个时,需要通过确定唯一的最佳匹配特征点集合,以便基于当前帧与多个候选地图范围的匹配情况来判断是否有某一候选地图范围可以满足匹配成功的条件。具体是:
根据最佳匹配特征点集合中的匹配特征点数量,也就是根据内点数量,对各个最佳匹配特征点集合赋予衡量当前帧与候选地图范围匹配程度的权值;该权值还可以根据当前帧特征点提取数量,特征点分布情况,匹配特征点初始数量与最佳匹配特征点集合中的匹配特征点数量(筛选得到的内点数量)之一或其任意组合,来确定权值。
根据设定的权值阈值和最大权值,确定唯一最佳匹配特征点集合,从而判定最佳匹配特征点集合满足用于位姿解算的条件;例如,采用权值阈值与唯一最大权值相结合,即,筛选出权值大于权值阈值的最佳匹配特征点集合,按照最大权值与次大权值差异最大的原则,从筛选出的最佳匹配特征点集合中选择唯一的最佳匹配特征点集合。
参见图7所示,图7为正常定位的一种流程示意图。当前帧在图像预处理和特征点提取之后,正常定位包括,
步骤701,鉴于上一帧的定位结果已被获取,根据上一帧到当前帧的帧间运动信息,对当前帧进行位姿预测,得到预测位姿,以便利用该预测位姿确定第二候选地图范围,从而提高匹配的效率;
位姿预测的方法包括,
实施方式一,通过轮式里程计或惯性测量元件(IMU)获得上一帧到当前帧的帧间位姿变换,基于上一帧的定位结果、以及帧间位姿变换,求得当前帧的预测位姿。
实施方式二,通过视觉里程计(VO)获得上一帧到当前帧的帧间位姿变换,基于上一帧的定位结果、以及帧间位姿变换,求得当前帧的预测位姿。该实施方式仅需要图像信息,不需要额外的惯性信息。
实施方式三,根据若干已获得定位结果的历史帧,预测上一帧到当前帧的帧间位姿变换,基于上一帧的定位结果、以及帧间位姿变换,求得当前帧的预测位姿。该实施方式不依赖当前帧的任何信息。
实施方式四,至少采用实施方式一至三中的两实施方式,分别求得当前帧的第一预测位姿,得到至少两个第一预测位姿;采用卡尔曼滤波器对所述至少两个第一预测位姿进行滤波,得到滤波后的第二预测位姿,将第二预测位姿作为当前帧最终的预测位姿,或者,采用非线性优化的方法,基于所述至少两个第一预测位姿进行优化,得到优化后的第二预测位姿,将第二预测位姿作为当前帧最终的预测位姿;
例如,根据各个实施方式所得到的各个误差项,构建非线性优化的目标函数,数学表达式为:
其中,eijs表示采用实施方式s时的误差项,Ti表示上一帧i的位姿,即上一帧i的定位结果,Tj表示当前帧j的第一预测位姿,ΔTij表示上一帧i到当前帧j之间的帧间位姿变换,ξj为当前帧预测位姿的李代数表示,S为所采用的实施方式的总数。
将上一帧i的位姿Ti、不同实施方式所得到的当前帧j的第一预测位姿Tj以及帧间位姿变换ΔTij作为初始值代入目标函数,求解使得目标函数取得最小值时的位姿。
步骤702,根据当前帧的预测位姿,确定第二候选地图范围,将当前帧中的特征点与第二候选地图范围内的地图点进行匹配,当匹配成功时,得到第三匹配特征点,然后执行步骤703,
在该步骤中,一种实施方式是,参见图9a所示,以当前帧的预测位姿所确定的地图位置为中心,将该中心的第一邻域确定为第二候选地图范围;匹配的方式可以采用暴力匹配,即暴力计算当前帧所有特征点与候选地图范围内的特征点的描述子距离,选取描述子距离最小的特征点作为匹配特征点。该方式适合预测位姿的置信度较低或待匹配的特征点数量较少时使用。
另一种实施方式是,参见图9b所示,对于当前帧的每个特征点,根据该特征点的投影范围,确定该特征点的第二候选地图范围,即,根据当前帧的预测位姿和该特征点的像素坐标,计算当前帧特征点投影到地图中的投影点位置,以该投影点位置为中心的第一邻域作为该特征点第二候选地图范围,计算当前帧特征点与第二候选地图范围内的地图点的匹配,选取描述子距离最小的特征点对作为匹配特征点。
较佳地,第一邻域可以是以中心为圆心、设定半径所覆盖的地图范围。
重复执行该步骤,直至当前帧的所有特征点都进行了匹配。
步骤703,基于第三匹配特征点,采用随机抽样一致性算法确定最佳匹配特征点集合,该步骤与步骤502相同,
步骤704,判断最佳匹配特征点集合中匹配特征点的数量是否满足用于位姿解算的条件,如果是,则进行当前帧的位姿计算,否则,判定当前帧定位失败。该步骤与步骤503相同。
参见图8所示,图8为重定位的一种流程示意图。当前帧在图像预处理和特征点提取之后,重定位包括,
步骤801,由于上一帧定位失败,根据定位成功的历史帧,追溯历史帧中最近的一帧作为参考帧,根据参考帧到当前帧的帧间运动信息,对当前帧进行位姿预测,得到预测位姿,以便利用该预测位姿确定第三候选地图范围,从而提高匹配的效率;
其中,位姿预测方法与步骤701中的位姿预测方法相同,只需将参考帧当作上一帧即可。
步骤802,根据当前帧的预测位姿,确定第三候选地图范围,将当前帧中的特征点与第三候选地图范围内的地图点进行匹配,当匹配成功时,得到第四匹配特征点,然后执行步骤803,
在该步骤中,一种实施方式是,以当前帧的预测位姿所确定的地图位置为中心,将该中心的第二邻域确定为第三候选地图范围;匹配的方式可以采用暴力匹配,以提高重定位的鲁棒性。
另一种实施方式是,对于当前帧的每个特征点,根据该特征点的投影范围,确定该特征点的第三候选地图范围,即,根据当前帧的预测位姿和该特征点的像素坐标,计算当前帧特征点投影到地图中的投影点位置,以该投影点位置为中心的第二邻域作为该特征点第三候选地图范围,计算当前帧特征点与第三候选地图范围内的地图点的匹配,选取描述子距离最小的特征点对作为匹配特征点。
较佳地,第二邻域可以是以中心为圆心、设定半径所覆盖的地图范围。
重复执行该步骤,直至当前帧的所有特征点都进行了匹配。
步骤803,基于第四匹配特征点,采用随机抽样一致性算法确定最佳匹配特征点集合,该步骤与步骤502相同,
步骤804,判断最佳匹配特征点集合中匹配特征点的数量是否满足用于位姿解算的条件,如果是,则进行当前帧的位姿计算,否则,判定当前帧定位失败。
在重定位成功计算当前帧位姿时,由于最佳匹配特征点集合是通过参考帧来确定的,存在较大误差的概率较高,较佳地,采用LM优化的方法来求解。
本实施例提供的视觉定位方法,对于采用纹理地图的定位而言,相比于传统二维码定位,无需对地面进行改造,利用自然纹理即可定位,成本较低;通过不同定位状态下的定位策略而确定的定位逻辑,增强了局部纹理变化或局部纹理缺失情形下的适应性,无论是纹理地图还是非纹理的视觉地图,都不需要预先设定精确地标定点,图像获取简便,并且定位过程中可以持续与地图进行匹配而不会存在大幅跳变,在部分建图的区域内,可以任意路径定位,而不局限于建图的路径上,提高了定位过程中的鲁棒性;不同定位策略在位姿预测、特征匹配处理上的不同,既提高了定位的准确性,又有利于提高定位的效率。
参见图10所示,图10为本发明的基于视觉地图的视觉定位装置的一种示意图。该装置包括,
图像采集模块1001,采集当前图像,得到当前帧;
特征提取模块1002,基于当前帧,进行特征点提取,得到当前帧的特征点;
定位模块1003,根据当前定位状态确定定位策略;基于定位策略,为当前帧确定地图中候选地图范围,将当前帧中的特征点与候选地范围内的地图点进行匹配,得到匹配特征点,当匹配特征点满足用于当前帧位姿解算的条件时,根据匹配特征点计算当前帧的位姿,得到定位结果。
其中,所述定位模块1003包括,
定位状态子模块1004,根据上一帧的定位结果、当前连续定位失败帧数确定当前定位状态,
定位策略子模块1005,根据当前定位状态确定定位策略,所述定位策略包括,未初始化状态下的初始化定位,定位成功状态下的正常定位,定位丢失状态下的重定位,以及各个定位状态之间转换关系;
定位逻辑子模块1006,根据各个定位状态以及各个定位状态之间转换关系,生成定位逻辑;
匹配定位子模块1007,基于定位逻辑,按照各个状态下的定位策略,为当前帧确定地图中候选地图范围,将当前帧中的特征点与候选地范围内的地图点进行匹配,得到匹配特征点,根据匹配特征点计算当前帧的位姿;
位姿图优化子模块1008,对当前帧位姿进行图优化。
所述匹配定位子模块1007包括,
候选地图确定单元1009,基于定位逻辑,按照各个状态下的定位策略,为当前帧确定地图中候选地图范围,
特征匹配单元1010,将当前帧中的特征点与候选地范围内的地图点进行匹配,
位姿计算单元1011,根据匹配特征点计算当前帧的位姿。
为了提高定位的效率,视觉定位装置还包括图像预处理模块1012,用于对图像进行预处理;较佳地,参见图11所示,图11为图像预处理模块的一种示意图。该图像预处理模块1012包括,
图像去畸变子模块,根据相机的畸变系数对源图像帧进行去畸变处理,得到去畸变图像,
图像滤波子模块,将去畸变图像进行图像滤波,得到背景图像,
图像差分子模块,用去畸变图像减去背景图像,得到前景图像,
图像拉伸子模块,对前景图像进行拉伸处理,得到目标图像帧。
其中,当视觉地图为纹理地图时,图像滤波子模块,图像差分子模块,图像拉伸子模块可用于增强图像纹理。
本发明还提供一种移动机器人,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器被配置执行所述计算机程序,以实现上述基于视觉地图的视觉定位方法的步骤。
存储器可以包括随机存取存储器(Random Access Memory,RAM),也可以包括非易失性存储器(Non-Volatile Memory,NVM),例如至少一个磁盘存储器。可选的,存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(Network Processor,NP)等;还可以是数字信号处理器(Digital SignalProcessing,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
本发明实施例还提供了一种计算机可读存储介质,所述存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现上述基于视觉地图的视觉定位方法的步骤。
对于装置/网络侧设备/存储介质实施例而言,由于其基本相似于方法实施例,所以描述的比较简单,相关之处参见方法实施例的部分说明即可。
在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明保护的范围之内。
Claims (13)
1.一种基于视觉地图的视觉定位方法,其特征在于,该方法包括,
采集当前图像,得到当前帧;
基于当前帧,进行特征点提取,得到当前帧的特征点;
根据上一帧的定位结果、以及当前连续定位失败帧数确定当前定位状态;其中,所述定位状态包括,未初始化状态,定位成功状态,以及定位丢失状态中的至少之一;
根据当前定位状态确定定位策略,
基于定位策略,将当前帧中的特征点与地图中的地图点进行匹配,得到匹配特征点,
当匹配特征点满足用于当前帧位姿解算的条件时,根据匹配特征点计算当前帧的位姿,得到定位结果;
其中,所述定位策略包括,未初始化状态下的初始化定位,定位成功状态下的正常定位,定位丢失状态下的重定位,以及各个定位状态之间转换关系;
所述各个定位状态之间转换关系包括,
当前状态为未初始化状态时,如果初始化定位成功,则转换至定位成功状态,否则保持当前未初始化状态;
当前状态为定位成功状态时,如果正常定位成功,则保持当前定位成功状态,否则,转换至定位丢失状态;
当前状态为定位丢失状态时,如果重定位成功,则转换至定位成功状态,否则,判断连续定位失败的帧数是否超过设定的帧数阈值,如果是,则转换至未初始化状态,否则,则保持当前定位丢失状态。
2.如权利要求1所述的方法,其特征在于,所述得到匹配特征点之后,进一步包括,
基于匹配特征点,采用随机抽样一致性算法确定最佳匹配特征点集合,
判断最佳匹配特征点集合是否满足用于位姿解算的条件,当匹配特征点不满足用于当前帧位姿解算的条件时,判定当前帧定位失败。
3.如权利要求2所述的方法,其特征在于,所述根据当前定位状态确定定位策略包括,根据各个定位状态以及各个定位状态之间转换关系,生成定位逻辑;
所述基于定位策略,将当前帧中的特征点与候选地范围内的地图点进行匹配,包括,基于定位逻辑,按照各个状态下的定位策略,为当前帧确定地图中候选地图范围,将当前帧中的特征点与候选地图范围内的地图点进行匹配;
其中,所述定位逻辑,包括:
判断当前帧是否是第一帧,
如果当前帧是第一帧,则对当前帧进行初始化定位,
如果当前帧不是第一帧,则判断上一帧是否定位成功,
如果上一帧定位成功,则对当前帧进行正常定位;
如果上一帧定位不成功,则判断当前连续定位失败帧数是否超过设定的帧数阈值,或者,当前帧与最近一次定位成功的位姿之间的距离是否超过设定的第一距离阈值;
如果当前连续定位失败帧数超过帧数阈值,则对当前帧进行初始化定位,
如果当前连续定位失败帧数未超过帧数阈值,则对当前帧进行重定位。
4.如权利要求3所述的方法,其特征在于,所述如果当前帧是第一帧,则对当前帧进行初始化定位,进一步包括,
根据当前帧是第一帧,判定当前状态为未初始化状态,
如果当前帧初始化定位成功,则记录当前帧定位成功,转换至定位成功状态,否则,记录当前帧定位失败,保持当前未初始化状态;
所述如果上一帧定位成功,则对当前帧进行正常定位,进一步包括,
根据上一帧定位成功,判定当前状态为正常定位状态,
如果当前帧正常定位成功,则记录当前帧定位成功,保持当前定位成功状态,否则,记录当前帧定位失败,转换至定位丢失状态;
所述如果当前连续定位失败帧数超过帧数阈值,则对当前帧进行初始化定位,包括,
根据当前连续定位失败帧数超过帧数阈值,判定当前状态为未初始化状态,
如果当前帧初始化定位成功,则记录当前帧定位成功,转换至定位成功状态,否则,记录当前帧定位失败,保持当前未初始化状态;
所述如果当前连续定位失败帧数未超过帧数阈值,则对当前帧进行重定位,进一步包括,
根据当前连续定位失败帧数未超过帧数阈值,判定当前状态为定位丢失状态,
如果重定位定位成功,则记录当前帧定位成功,转换至定位成功状态,否则,记录当前帧定位失败,返回所述判断当前连续定位失败帧数是否超过设定的帧数阈值步骤。
5.如权利要求4所述的方法,其特征在于,所述基于定位逻辑,按照各个状态下的定位策略,为当前帧确定地图中候选地图范围,包括,
对当前帧进行初始化定位时,将地图中的所有地图点作为第一候选地图范围,或者,利用辅助信息获得第一候选地图范围;对第一候选地图范围内的地图点采用暴力匹配进行筛选,如果每两个地图点特征匹配度超过设定的第二匹配阈值,则随机删掉其中的一个地图点;
对当前帧进行正常定位时,根据上一帧到当前帧的帧间运动信息,对当前帧进行位姿预测,得到当前帧预测位姿;根据当前帧预测位姿,确定第二候选地图范围;
对当前帧进行重定位时,根据定位成功的历史帧,向前追溯历史帧中与当前帧最近的一帧并作为参考帧,将该参考帧作为上一帧,根据该上一帧到当前帧的帧间运动信息,对当前帧进行位姿预测,得到当前帧预测位姿;根据当前帧预测位姿,确定第三候选地图范围。
6.如权利要求5所述的方法,其特征在于,所述根据上一帧到当前帧的帧间运动信息,对当前帧进行位姿预测,包括,
第一方式:通过轮式里程计或惯性测量元件获得上一帧到当前帧的帧间位姿变换,基于帧间位姿变换、以及上一帧的定位结果,求得当前帧的预测位姿;
或者
第二方式:通过视觉里程计获得上一帧到当前帧的帧间位姿变换,基于帧间位姿变换、以及上一帧的定位结果,求得当前帧的预测位姿;
或者
第三方式:根据已获得定位结果的历史帧,预测上一帧到当前帧的帧间位姿变换,基于帧间位姿变换、以及上一帧的定位结果,求得当前帧的预测位姿;
或者,
第四方式:至少采用第一方式、第二方式、第三方式中的两方式,分别求得当前帧的第一预测位姿,得到至少两个第一预测位姿;
采用卡尔曼滤波器对所述至少两个第一预测位姿进行滤波,得到滤波后的第二预测位姿,将第二预测位姿作为当前帧最终的预测位姿;或者,采用非线性优化的方法,基于所述至少两个第一预测位姿进行优化,得到优化后的第二预测位姿,将第二预测位姿作为当前帧最终的预测位姿;
其中,非线性优化的目标函数为根据各个方式所得到的各个误差项之和,将上一帧的位姿、不同方式所得到的当前帧j的第一预测位姿、以及帧间位姿变换作为初始值,代入目标函数,求解使得目标函数取得最小值时的位姿,作为第二预测位姿。
7.如权利要求5或6所述的方法,其特征在于,所述根据当前帧预测位姿,确定第二候选地图范围,包括,
以当前帧的预测位姿所确定的地图位置为中心,将该中心的第一邻域确定为第二候选地图范围,
或者
对于当前帧的每个特征点,根据当前帧的预测位姿和该特征点的像素坐标,计算当前帧特征点投影到地图中的投影点位置,以该投影点位置为中心的第一邻域作为该特征点第二候选地图范围;
所述根据当前帧预测位姿,确定第三候选地图范围,包括,
以当前帧的预测位姿所确定的地图位置为中心,将该中心的第二邻域确定为第三候选地图范围,
或者
对于当前帧的每个特征点,根据当前帧的预测位姿和该特征点的像素坐标,计算当前帧特征点投影到地图中的投影点位置,以该投影点位置为中心的第二邻域作为该特征点第三候选地图范围;
其中,第二邻域范围大于第一邻域范围。
8.如权利要求2所述的方法,其特征在于,所述基于匹配特征点,采用随机抽样一致性算法确定最佳匹配特征点集合,包括,
从匹配特征点所构成的匹配特征点集合中,随机选择用于计算当前帧位姿估计的匹配特征点,得到当前匹配特征点子集;
基于匹配特征点子集中的匹配特征点所建立的空间位置信息与像素位置信息的映射,计算当前位姿,得到该匹配特征点子集的拟合位姿估计,
根据拟合位姿估计以及相机内参,获取当前帧中所有的特征点的空间位置,得到所有特征点的投影点空间位置;
对于匹配特征点集合中的每个匹配特征点,根据该匹配特征点的投影点空间位置,判断当前帧中该匹配特征点的投影点与地图中该匹配特征点的距离是否小于设定的第二距离阈值,如果是,则判定该匹配特征点为内点;反复执行所述判断当前帧中该匹配特征点的投影点与地图中该匹配特征点的距离是否小于设定的第二距离阈值的步骤,直至匹配特征点集合中所有匹配特征点都被进行了内点的判定;
统计当前内点数量,判断当前统计的内点数量是否为历次迭代中的最大,如果是,则将当前内点所构成的集合作为当前最佳匹配特征点集合;
判断是否达到结束条件,如果是,则将当前最佳匹配特征点集合作为最终的最佳匹配特征点集合,否则,返回所述从匹配特征点所构成的匹配特征点集合中随机选择用于计算当前帧位姿估计的匹配特征点的步骤;
所述判断最佳匹配特征点集合是否满足用于位姿解算的条件,包括,
根据匹配特征点的数量判断最佳匹配特征点集合是否满足用于位姿解算的条件;
或者,
对至少两个以上最佳匹配特征点集合分别赋予衡量当前帧与候选地图范围匹配程度的权值;所述权值根据最佳匹配特征点集合中的匹配特征点数量、当前帧特征点提取数量,特征点分布情况,匹配特征点初始数量与最佳匹配特征点集合中的匹配特征点数量之一或其任意组合确定,
根据设定的权值阈值和最大权值,判断最佳匹配特征点集合是否满足用于位姿解算的条件。
9.如权利要求1所述的方法,其特征在于,所述根据匹配特征点计算当前帧的位姿,得到定位结果,进一步包括,
对计算得到当前帧的位姿采用基于滑动窗口的非线性优化;优化的变量为滑动窗口内各图像帧的位姿,该滑动窗口包括当前帧,优化的约束为当前帧特征点与上一关键帧特征点之间的帧间匹配约束、和/或当前帧特征点与地图中的地图点之间的地图匹配约束,
利用最小二乘法,最小化帧间匹配误差和/或地图匹配误差,得到优化后的当前帧位姿,作为定位结果;
其中,
地图匹配约束为:第一匹配地图点反投影到当前帧上的像素位置与当前帧中与该地图点匹配的第一匹配特征点的像素位置之间的误差,或者,当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与该第一匹配特征点匹配的第一匹配地图点在世界坐标系中的空间位置之间的误差;所述第一匹配特征点为:当前帧中的特征点与地图中的地图点进行匹配所得到匹配成功的特征点;第一匹配地图点为:被第一匹配特征点匹配成功的地图点;
帧间匹配约束为:当前帧中的第一匹配特征点投影至世界坐标系中的空间位置与当前帧的上一关键帧中与该第一匹配特征点匹配的第二匹配特征点投影至世界坐标系中的空间位置之间的误差,或者,与第二匹配特征点匹配的第二匹配地图点反投影到当前帧的像素位置与该第二匹配地图点反投影到上一关键帧的像素位置之间的误差;
所述采集当前图像,得到当前帧,进一步包括,对当前帧进行图像预处理。
10.如权利要求9所述的方法,其特征在于,所述利用最小二乘法,最小化帧间匹配误差和/或地图匹配误差,包括,
构建目标函数,该函数为:以第一权值加权当前滑动窗口中所有帧的所有第一匹配特征点的地图匹配误差之和所得到的第一结果,以及,以第二权值加权当前滑动窗口中各帧与其上一关键帧之间所有帧间的所有第二匹配地图点的帧间匹配误差之和所得到的第二结果,所累加的第一结果与第二结果之和;
以根据当前帧的位姿、第一匹配地图点的空间位置信息、相机内参、当前帧中的与第一匹配地图点匹配的第一匹配特征点的像素坐标所得到的地图匹配误差,作为地图匹配误差初始值,
以根据当前帧位姿、第二匹配地图点的空间位置信息、上一关键帧的位姿、相机内参矩阵所得到帧间匹配误差,作为帧间匹配误差初始值,
迭代求解使得目标函数取得最小值时当前帧的位姿;
所述得到优化后的当前帧位姿之后,进一步包括,
满足以下条件之一时,判定当前帧为关键帧:
当前帧中第一匹配特征点的数量小于第一阈值,
当前帧中第二匹配特征点的数量小于第二阈值;
若当前帧为非关键帧时,则在滑动窗口中删除该当前帧,
若当前帧为关键帧,则判断当前滑动窗口中帧数量是否达到设定的第一帧阈值,如果是,则删除滑动窗口中最早添加的关键帧,否则,则不删除滑动窗口中最早添加的关键帧;
所述对当前帧进行图像预处理包括,
根据相机的畸变系数对当前帧进行去畸变处理,得到去畸变图像,
判断去畸变图像中各个像素点的像素值是否大于第一像素阈值,如果是,则将像素值大于第一像素阈值的像素点进行取反操作,然后进行图像滤波,得到背景图像,否则,将去畸变图像进行图像滤波,得到背景图像,
用去畸变图像减去背景图像,得到前景图像,
判断前景图像中的像素值是否分布均匀,如果均匀,则将该前景图像作为图像预处理后的当前帧,否则,对前景图像进行拉伸处理,得到图像预处理后的当前帧。
11.如权利要求10所述的方法,其特征在于,所述前景图像进行拉伸处理包括,
若前景图像像素值小于等于设定的最小灰度值时,将该前景图像像素值取值为像素取值范围内的最小值;
若前景图像像素值大于最小灰度值、且小于设定的最大灰度值时,按照与像素最大值成一定比例的像素值作为该前景图像像素值;所述比例为前景图像像素值与最小灰度值之差与最大灰度值与最小灰度值之差的比值;
若前景图像像素值大于等于最大灰度值时,将该前景图像像素值取值为像素取值范围内的最大值;
所述基于当前帧,进行特征点提取,得到当前帧的特征点,包括,
对当前帧进行特征检测,得到特征点,
将当前帧划分成一定数量的网格,
对于任一网格中的特征点,将网格内的特征点按特征点响应值降序排列,保留前Q个特征点,得到筛选后的特征点;其中,Q根据当前帧中特征点的数量和设定的特征点总数上限、以及该网格特征点总数确定;
对筛选后的各特征点,分别计算特征描述子。
12.一种基于视觉地图的视觉定位装置,其特征在于,该装置包括,
图像采集模块,采集当前图像,得到当前帧;
特征提取模块,基于当前帧,进行特征点提取,得到当前帧的特征点;
定位模块,根据上一帧的定位结果、以及当前连续定位失败帧数确定当前定位状态;根据当前定位状态确定定位策略,基于定位策略,将当前帧中的特征点与地图中的地图点进行匹配,得到匹配特征点,当匹配特征点满足用于当前帧位姿解算的条件时,根据匹配特征点计算当前帧的位姿,得到定位结果;
其中,
所述定位状态包括,未初始化状态,定位成功状态,以及定位丢失状态中的至少之一,
所述定位策略包括,未初始化状态下的初始化定位,定位成功状态下的正常定位,定位丢失状态下的重定位,以及各个定位状态之间转换关系;
所述各个定位状态之间转换关系包括,
当前状态为未初始化状态时,如果初始化定位成功,则转换至定位成功状态,否则保持当前未初始化状态;
当前状态为定位成功状态时,如果正常定位成功,则保持当前定位成功状态,否则,转换至定位丢失状态;
当前状态为定位丢失状态时,如果重定位成功,则转换至定位成功状态,否则,判断连续定位失败的帧数是否超过设定的帧数阈值,如果是,则转换至未初始化状态,否则,则保持当前定位丢失状态。
13.一种计算机可读存储介质,其特征在于,所述存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现如权利要求1至11任一所述基于视觉地图的视觉定位方法的步骤。
Priority Applications (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010618519.1A CN111780763B (zh) | 2020-06-30 | 2020-06-30 | 一种基于视觉地图的视觉定位方法、装置 |
PCT/CN2021/103073 WO2022002039A1 (zh) | 2020-06-30 | 2021-06-29 | 一种基于视觉地图的视觉定位方法、装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010618519.1A CN111780763B (zh) | 2020-06-30 | 2020-06-30 | 一种基于视觉地图的视觉定位方法、装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111780763A CN111780763A (zh) | 2020-10-16 |
CN111780763B true CN111780763B (zh) | 2022-05-06 |
Family
ID=72759967
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010618519.1A Active CN111780763B (zh) | 2020-06-30 | 2020-06-30 | 一种基于视觉地图的视觉定位方法、装置 |
Country Status (2)
Country | Link |
---|---|
CN (1) | CN111780763B (zh) |
WO (1) | WO2022002039A1 (zh) |
Families Citing this family (29)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111524192B (zh) * | 2020-04-20 | 2023-10-03 | 阿波罗智能技术(北京)有限公司 | 车载相机外参的标定方法、装置、系统及存储介质 |
CN111780763B (zh) * | 2020-06-30 | 2022-05-06 | 杭州海康机器人技术有限公司 | 一种基于视觉地图的视觉定位方法、装置 |
CN112509027B (zh) * | 2020-11-11 | 2023-11-21 | 深圳市优必选科技股份有限公司 | 重定位方法、机器人及计算机可读存储介质 |
CN112488007B (zh) * | 2020-12-04 | 2023-10-13 | 深圳市优必选科技股份有限公司 | 视觉定位方法、装置、机器人及存储介质 |
CN112950710A (zh) * | 2021-02-24 | 2021-06-11 | 广州极飞科技股份有限公司 | 位姿确定方法、装置、电子设备和计算机可读存储介质 |
CN113160270A (zh) * | 2021-02-24 | 2021-07-23 | 广州视源电子科技股份有限公司 | 视觉地图生成方法、装置、终端及存储介质 |
CN112950715B (zh) * | 2021-03-04 | 2024-04-30 | 杭州迅蚁网络科技有限公司 | 无人机的视觉定位方法、装置、计算机设备和存储介质 |
CN112990003B (zh) * | 2021-03-11 | 2023-05-19 | 深圳市无限动力发展有限公司 | 图像序列重定位判断方法、装置和计算机设备 |
CN113034595B (zh) * | 2021-03-19 | 2024-06-07 | 浙江商汤科技开发有限公司 | 视觉定位的方法和相关装置、设备、存储介质 |
CN113239072B (zh) * | 2021-04-27 | 2024-09-06 | 华为技术有限公司 | 一种终端设备定位方法及其相关设备 |
CN113252033B (zh) * | 2021-06-29 | 2021-10-15 | 长沙海格北斗信息技术有限公司 | 基于多传感器融合的定位方法、定位系统及机器人 |
CN114111803B (zh) * | 2022-01-26 | 2022-04-19 | 中国人民解放军战略支援部队航天工程大学 | 一种室内卫星平台的视觉导航方法 |
CN114485653A (zh) * | 2022-02-23 | 2022-05-13 | 广州高新兴机器人有限公司 | 基于视觉与轮式里程计融合的定位方法、装置、介质及设备 |
CN114577215B (zh) * | 2022-03-10 | 2023-10-27 | 山东新一代信息产业技术研究院有限公司 | 一种移动机器人的特征地图更新方法、设备及介质 |
CN117036663B (zh) * | 2022-04-18 | 2024-07-09 | 荣耀终端有限公司 | 视觉定位方法、设备和存储介质 |
CN114782499A (zh) * | 2022-04-28 | 2022-07-22 | 杭州电子科技大学 | 一种基于光流和视图几何约束的图像静态区域提取方法及装置 |
CN114782495B (zh) * | 2022-06-16 | 2022-10-18 | 西安中科立德红外科技有限公司 | 一种多目标跟踪方法、系统及计算机存储介质 |
CN115049847B (zh) * | 2022-06-21 | 2024-04-16 | 上海大学 | 一种基于orb描述子的特征点局部邻域特征匹配方法 |
CN117346650A (zh) * | 2022-06-28 | 2024-01-05 | 中兴通讯股份有限公司 | 视觉定位的位姿确定方法、装置以及电子设备 |
CN115267796B (zh) * | 2022-08-17 | 2024-04-09 | 深圳市普渡科技有限公司 | 定位方法、装置、机器人和存储介质 |
CN115439536B (zh) * | 2022-08-18 | 2023-09-26 | 北京百度网讯科技有限公司 | 视觉地图更新方法、装置及电子设备 |
CN115451996B (zh) * | 2022-08-30 | 2024-03-29 | 华南理工大学 | 一种面向室内环境的单应视觉里程计方法 |
CN115493612A (zh) * | 2022-10-12 | 2022-12-20 | 中国第一汽车股份有限公司 | 一种基于视觉slam的车辆定位方法及装置 |
CN118052867A (zh) * | 2022-11-15 | 2024-05-17 | 中兴通讯股份有限公司 | 定位方法、终端设备、服务器及存储介质 |
CN115752476B (zh) * | 2022-11-29 | 2024-06-18 | 重庆长安汽车股份有限公司 | 一种基于语义信息的车辆地库重定位方法、装置、设备和介质 |
CN116147618B (zh) * | 2023-01-17 | 2023-10-13 | 中国科学院国家空间科学中心 | 一种适用动态环境的实时状态感知方法及系统 |
CN116778141B (zh) * | 2023-08-28 | 2023-12-22 | 深圳联友科技有限公司 | 一种基于orb算法的图片快速识别定位的方法 |
CN117419690B (zh) * | 2023-12-13 | 2024-03-12 | 陕西欧卡电子智能科技有限公司 | 一种无人船的位姿估计方法、装置及介质 |
CN117710469B (zh) * | 2024-02-06 | 2024-04-12 | 四川大学 | 一种基于rgb-d传感器的在线稠密重建方法及系统 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106885574A (zh) * | 2017-02-15 | 2017-06-23 | 北京大学深圳研究生院 | 一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法 |
CN107990899A (zh) * | 2017-11-22 | 2018-05-04 | 驭势科技(北京)有限公司 | 一种基于slam的定位方法和系统 |
CN109544636A (zh) * | 2018-10-10 | 2019-03-29 | 广州大学 | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 |
CN109633725A (zh) * | 2018-10-31 | 2019-04-16 | 百度在线网络技术(北京)有限公司 | 定位初始化的处理方法、装置及可读存储介质 |
CN110108258A (zh) * | 2019-04-09 | 2019-08-09 | 南京航空航天大学 | 一种单目视觉里程计定位方法 |
CN110361005A (zh) * | 2019-06-26 | 2019-10-22 | 深圳前海达闼云端智能科技有限公司 | 定位方法、定位装置、可读存储介质及电子设备 |
CN111156984A (zh) * | 2019-12-18 | 2020-05-15 | 东南大学 | 一种面向动态场景的单目视觉惯性slam方法 |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104732518B (zh) * | 2015-01-19 | 2017-09-01 | 北京工业大学 | 一种基于智能机器人地面特征的ptam改进方法 |
US10054445B2 (en) * | 2016-05-16 | 2018-08-21 | Northrop Grumman Systems Corporation | Vision-aided aerial navigation |
US10162362B2 (en) * | 2016-08-29 | 2018-12-25 | PerceptIn, Inc. | Fault tolerance to provide robust tracking for autonomous positional awareness |
CN107390245B (zh) * | 2017-07-13 | 2020-07-10 | 广东小天才科技有限公司 | 一种定位方法、装置、设备及存储介质 |
CN111322993B (zh) * | 2018-12-13 | 2022-03-04 | 杭州海康机器人技术有限公司 | 一种视觉定位方法和装置 |
CN111750864B (zh) * | 2020-06-30 | 2022-05-13 | 杭州海康机器人技术有限公司 | 一种基于视觉地图的重定位方法和装置 |
CN111780763B (zh) * | 2020-06-30 | 2022-05-06 | 杭州海康机器人技术有限公司 | 一种基于视觉地图的视觉定位方法、装置 |
CN112541970A (zh) * | 2020-11-30 | 2021-03-23 | 北京华捷艾米科技有限公司 | 一种集中式协同SlAM中的重定位方法及装置 |
-
2020
- 2020-06-30 CN CN202010618519.1A patent/CN111780763B/zh active Active
-
2021
- 2021-06-29 WO PCT/CN2021/103073 patent/WO2022002039A1/zh active Application Filing
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106885574A (zh) * | 2017-02-15 | 2017-06-23 | 北京大学深圳研究生院 | 一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法 |
CN107990899A (zh) * | 2017-11-22 | 2018-05-04 | 驭势科技(北京)有限公司 | 一种基于slam的定位方法和系统 |
CN109544636A (zh) * | 2018-10-10 | 2019-03-29 | 广州大学 | 一种融合特征点法和直接法的快速单目视觉里程计导航定位方法 |
CN109633725A (zh) * | 2018-10-31 | 2019-04-16 | 百度在线网络技术(北京)有限公司 | 定位初始化的处理方法、装置及可读存储介质 |
CN110108258A (zh) * | 2019-04-09 | 2019-08-09 | 南京航空航天大学 | 一种单目视觉里程计定位方法 |
CN110361005A (zh) * | 2019-06-26 | 2019-10-22 | 深圳前海达闼云端智能科技有限公司 | 定位方法、定位装置、可读存储介质及电子设备 |
CN111156984A (zh) * | 2019-12-18 | 2020-05-15 | 东南大学 | 一种面向动态场景的单目视觉惯性slam方法 |
Non-Patent Citations (2)
Title |
---|
Hyungjin Kim ; Kwangyik Jung ; Taekjun Oh ; Hyun Myung.Image-based localization using image database and local 3D maps.《2015 12th International Conference on Ubiquitous Robots and Ambient Intelligence》.2015,全文. * |
复杂场景下视觉先验信息的地图恢复SLAM;刘盛,张宇翔,等;《中国图象图形学报》;20200131;第25卷(第1期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN111780763A (zh) | 2020-10-16 |
WO2022002039A1 (zh) | 2022-01-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111780763B (zh) | 一种基于视觉地图的视觉定位方法、装置 | |
CN111750864B (zh) | 一种基于视觉地图的重定位方法和装置 | |
CN111780764B (zh) | 一种基于视觉地图的视觉定位方法、装置 | |
CN111795704B (zh) | 一种视觉点云地图的构建方法、装置 | |
US8503760B2 (en) | System and method for real-time object recognition and pose estimation using in-situ monitoring | |
CN109815770B (zh) | 二维码检测方法、装置及系统 | |
JP5940453B2 (ja) | 画像のシーケンス内のオブジェクトのリアルタイム表現のハイブリッド型追跡のための方法、コンピュータプログラム、および装置 | |
CN101398934B (zh) | 对图像中的对象进行定位的方法和系统 | |
CN105654548B (zh) | 一种基于大规模无序图像的多起点增量式三维重建方法 | |
CN108537876A (zh) | 基于深度相机的三维重建方法、装置、设备及存储介质 | |
CN109977997B (zh) | 基于卷积神经网络快速鲁棒的图像目标检测与分割方法 | |
CN107633226B (zh) | 一种人体动作跟踪特征处理方法 | |
CN110084299B (zh) | 基于多头融合注意力的目标检测方法和装置 | |
EP2811424A2 (en) | Method and apparatus for training an estimator for estimating a pose of an articulated object | |
JP2010238226A (ja) | 物体を追跡するための方法及びシステム | |
WO2019196476A1 (zh) | 基于激光传感器生成地图 | |
CN105224935A (zh) | 一种基于Android平台的实时人脸关键点定位方法 | |
CN112163588A (zh) | 基于智能进化的异源图像目标检测方法、存储介质及设备 | |
CN113673400A (zh) | 基于深度学习的实景三维语义重建方法、装置及存储介质 | |
KR20120112293A (ko) | 이동체의 위치 판단 장치 및 위치 판단 방법 | |
CN115457492A (zh) | 目标检测方法、装置、计算机设备及存储介质 | |
JP2019185787A (ja) | 地理的地域内のコンテナのリモート決定 | |
CN114067128A (zh) | 一种基于语义特征的slam回环检测方法 | |
CN117576303A (zh) | 三维图像生成方法、装置、设备及存储介质 | |
CN113361400B (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 | ||
CP01 | Change in the name or title of a patent holder |
Address after: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province Patentee after: Hangzhou Hikvision Robot Co.,Ltd. Address before: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province Patentee before: HANGZHOU HIKROBOT TECHNOLOGY Co.,Ltd. |
|
CP01 | Change in the name or title of a patent holder |