CN113607160B - 视觉定位恢复方法、装置、机器人和可读存储介质 - Google Patents
视觉定位恢复方法、装置、机器人和可读存储介质 Download PDFInfo
- Publication number
- CN113607160B CN113607160B CN202110973056.5A CN202110973056A CN113607160B CN 113607160 B CN113607160 B CN 113607160B CN 202110973056 A CN202110973056 A CN 202110973056A CN 113607160 B CN113607160 B CN 113607160B
- Authority
- CN
- China
- Prior art keywords
- pose
- map
- visual positioning
- new
- navigation
- 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
- 230000000007 visual effect Effects 0.000 title claims abstract description 90
- 238000011084 recovery Methods 0.000 title claims abstract description 47
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000013507 mapping Methods 0.000 claims abstract description 16
- 238000012216 screening Methods 0.000 claims description 22
- 238000004590 computer program Methods 0.000 claims description 8
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000004904 shortening Methods 0.000 abstract 1
- 230000006870 function Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 7
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction 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/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
- G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
- G01C11/04—Interpretation of pictures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/29—Geographical information databases
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30241—Trajectory
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30244—Camera pose
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Databases & Information Systems (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种视觉定位恢复方法、装置、机器人和可读存储介质,该视觉定位恢复方法包括:获取视觉图像计算位姿以及相应的位姿评分,将位姿以及位姿评分关联存储至轨迹集;当确定定位跟踪失败时,按照预设规则从轨迹集中选取一个位姿作为新导航目标;以轨迹集中最近的上一位姿为坐标原点创建恢复地图,将新导航目标映射至恢复地图上进行视觉定位导航并重定位;在重定位成功后,将恢复地图与全局地图合并为新的全局地图,并恢复初始导航目标,在新的全局地图上进行视觉定位导航。本发明在定位追踪失败时,可以通过获取最优的历史位姿进行重定位路线的导航,从而缩短重定位的时间,提高重定位的成功率。
Description
技术领域
本发明涉及机器人视觉领域,具体而言,涉及一种视觉定位恢复方法、装置、机器人和可读存储介质。
背景技术
现有的机器人视觉导航针对跟踪失败时,采用重定位技术进行恢复,要求机器人回到已生成地图的区域,再次获得机器人之前地图中的位姿,才能执行重定位。而现有的重定位过程中,机器人只能盲目的运动以尝试重新定位成功,因此重定位的时间较长,并且成功率较低。
发明内容
鉴于上述问题,本发明提供了一种视觉定位恢复方法、装置、机器人和可读存储介质,以缩短重定位的时间,提高重定位的成功率。
为了实现上述目的,本发明采用如下的技术方案:
一种视觉定位恢复方法,包括:
获取视觉图像计算位姿以及相应的位姿评分,将所述位姿以及所述位姿评分关联存储至轨迹集;
当确定定位跟踪失败时,按照预设规则从所述轨迹集中选取一个位姿作为新导航目标;
以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图,将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位;
在重定位成功后,将所述恢复地图与全局地图合并为新的全局地图,并恢复初始导航目标,在所述新的全局地图上进行视觉定位导航。
优选地,所述的视觉定位恢复方法中,所述按照预设时间间隔获取视觉图像计算当前位姿以及相应的位姿评分包括:
获取当前视觉图像,对所述当前视觉图像进行特征点提取,获得多个图像特征点;
利用所述多个图像特征点与全局地图的地图特征点进行匹配运算,获得当前位姿以及相应的位姿评分。
优选地,所述的视觉定位恢复方法中,所述按照预设规则从所述轨迹集中选取一个位姿作为新导航目标包括:
从所述轨迹集中筛选出位姿评分大于预设阈值的至少一个初筛位姿;
根据位姿距离算法从至少一个所述初筛位姿中筛选出与所述恢复地图的原点的位姿距离最小的位姿,作为所述新导航目标。
优选地,所述的视觉定位恢复方法中,所述位姿距离算法的算式包括:
dist(Ti,Tj)=α|xi-xj,yi-yj,zi-zj|2+β|ai-aj,bi-bj,ci-cj|2;
式中,dist()为位姿距离,Ti为第i个位姿,Tj为第j个位姿;xi、yi、zi为Ti位姿的坐标,ai、bi、ci为Ti位姿的欧拉角;xj、yj、zj为Tj位姿的坐标,aj、bj、cj为Tj位姿的欧拉角;α为直线距离权重,β为转动角度权重。
优选地,所述的视觉定位恢复方法中,还包括:
启动视觉定位导航后,创建所述全局地图并接收所述导航目标,在所述全局地图上向所述导航目标进行视觉定位导航。
优选地,所述的视觉定位恢复方法中,所述将所述新导航目标映射至所述恢复地图的算式包括:
recovergoal=T-1 lastTrecover;
式中,Tlast为所述上一位姿,Trecover为所述新导航目标,recovergoal为映射后所述新导航目标。
优选地,所述的视觉定位恢复方法中,还包括:
通过运动追踪传感器获取上一位姿与当前位姿之间的位姿变换,利用所述上一位姿与所述位姿变换计算所述坐标原点,根据所述坐标原点创建所述恢复地图;
所述坐标原点的计算算式包括:T=TlastTdiff,式中,T为所述坐标原点,Tlast为所述上一位姿,Tdiff为所述位姿变换。
本发明还提供一种视觉定位恢复装置,包括:
位姿计算模块,用于获取视觉图像计算位姿以及相应的位姿评分,将所述位姿以及所述位姿评分关联存储至轨迹集;
导航目标选取模块,用于当确定定位跟踪失败时,按照预设规则从所述轨迹集中选取一个位姿作为新导航目标;
重定位模块,用于以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图,将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位;
导航恢复模块,用于在重定位成功后,将所述恢复地图与全局地图合并为新的全局地图,并恢复初始导航目标,在所述新的全局地图上进行视觉定位导航。
本发明还提供一种机器人,包括存储器以及处理器,所述存储器存储有计算机程序,所述计算机程序在所述处理器上运行时执行所述的视觉定位恢复方法。
本发明还提供一种可读存储介质,其存储有计算机程序,所述计算机程序在处理器上运行时执行所述的视觉定位恢复方法。
本发明提供一种视觉定位恢复方法,该视觉定位恢复方法包括:获取视觉图像计算位姿以及相应的位姿评分,将所述位姿以及所述位姿评分关联存储至轨迹集;当确定定位跟踪失败时,按照预设规则从所述轨迹集中选取一个位姿作为新导航目标;以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图,将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位;在重定位成功后,将所述恢复地图与全局地图合并为新的全局地图,并恢复初始导航目标,在所述新的全局地图上进行视觉定位导航。本发明的视觉定位恢复方法,在视觉导航计算出当前机器人的位姿的同时,还计算出相应的位姿评分进行关联存储至轨迹集,在定位追踪失败时,可以通过获取最优的历史位姿进行重定位路线的导航,从而缩短重定位的时间,提高重定位的成功率。
为使本发明的上述目的、特征和优点能更明显易懂,下文特举较佳实施例,并配合所附附图,作详细说明如下。
附图说明
为了更清楚地说明本发明的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本发明的某些实施例,因此不应被看作是对本发明保护范围的限定。在各个附图中,类似的构成部分采用类似的编号。
图1是本发明实施例1提供的一种视觉定位恢复方法的流程图;
图2是本发明实施例2提供的一种计算位姿及评分的流程图;
图3是本发明实施例3提供的一种轨迹集中选取位姿的流程图;
图4是本发明实施例4提供的一种视觉定位恢复方法的流程图;
图5是本发明实施例4提供的一种视觉定位恢复装置的结构示意图。
具体实施方式
下面将结合本发明实施例中附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。
通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此,以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围,而是仅仅表示本发明的选定实施例。基于本发明的实施例,本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例,都属于本发明保护的范围。
在下文中,可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合,并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。
此外,术语“第一”、“第二”、“第三”等仅用于区分描述,而不能理解为指示或暗示相对重要性。
除非另有限定,否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义,除非在本发明的各种实施例中被清楚地限定。
实施例1
图1是本发明实施例1提供的一种视觉定位恢复方法的流程图,该方法包括如下步骤:
步骤S11:获取视觉图像计算位姿以及相应的位姿评分,将所述位姿以及所述位姿评分关联存储至轨迹集。
本发明实施例中,在机器人启动视觉定位导航后,可以创建全局地图并接收导航目标,在全局地图上向导航目标进行视觉定位导航,即通过视觉同步定位与建图系统进行视觉导航。在通过视觉同步定位与建图系统进行视觉导航的场景中,可以按照预设时间间隔获取机器人行驶时前方的视觉图像,输入至系统,以计算出机器人在全局地图上的当前位姿。在计算出当前位姿的同时,还可以对该当前位姿进行评分,例如通过当前位姿的特征点与全局地图中的地图特征点进行匹配,其匹配的程度越高,则当前的位姿评分越高。
本发明实施例中,在通过视觉图像计算出机器人的当前位姿以及相应的位姿评分后,可以关联存储至预先设定的轨迹集中,通过轨迹集来表示机器人在视觉导航行驶过程中的轨迹,为重定位提供所需的位姿。
步骤S12:当确定定位跟踪失败时,按照预设规则从所述轨迹集中选取一个位姿作为新导航目标。
本发明实施例中,视觉导航在复杂的环境中可能难以获得机器人的当前位姿,即所谓的定位跟踪失败。在定位跟踪失败后,可以从轨迹集中按照预设规则选取出一个最优的位姿作为新导航目标,以便通过视觉导航在导航至该新导航目标期间使用重定位功能,使视觉导航重新定位回全局地图。
步骤S13:以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图,将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位。
本发明实施例中,由于定位跟踪失败,将导致当前的全局地图可能处于失效状态,在重定位过程中为了避免出现更大的误差,因此以上一位姿作为坐标原点创建恢复地图,由于相邻两位姿之间的时间差非常小,因此重定位过程中带来的误差也非常小,对于重定位的影响非常小。
本发明实施例中,由于轨迹集中的位姿是以初始的全局地图的坐标生成的,因此在应用在恢复地图时,需要进行坐标的映射。所述将所述新导航目标映射至所述恢复地图的算式包括:
recovergoal=T-1 lastTrecover;
式中,Tlast为所述上一位姿,Trecover为所述新导航目标,recovergoal为映射后所述新导航目标。其中,在机器人中可以预先设置有基于上述算式的应用程序,在获取到新导航目标以及上一位姿后,可以输入至该应用程序,以获得在恢复地图上映射的新导航目标。
步骤S14:在重定位成功后,将所述恢复地图与全局地图合并为新的全局地图,并恢复初始导航目标,在所述新的全局地图上进行视觉定位导航。
本发明实施例中,在恢复地图上通过视觉导航向新导航目标前进的过程中,同时开启重定位功能,以便找到在之前全局地图中的位姿。一旦找到在之前全局地图中的位姿,也即说明恢复地图与初始的全局地图已经出现重合部分,这时可以将恢复地图与全局地图合并为新的全局地图,在更新地图后恢复初始导航目标,重新在全局地图上向初始导航目标进行导航。
本发明实施例中,在视觉导航计算出当前机器人的位姿的同时,还计算出相应的位姿评分进行关联存储至轨迹集,在定位追踪失败时,可以通过获取最优的历史位姿进行重定位路线的导航,从而缩短重定位的时间,提高重定位的成功率。
实施例2
图2是本发明实施例2提供的一种计算位姿及评分的流程图,包括如下步骤:
步骤S21:获取当前视觉图像,对所述当前视觉图像进行特征点提取,获得多个图像特征点。
步骤S22:利用所述多个图像特征点与全局地图的地图特征点进行匹配运算,获得当前位姿以及相应的位姿评分。
本发明实施例中,在机器人中可以预先设置有进行匹配运算的应用程序,在获取到当前视觉图像的图像特征点后,可以将该图像特征点以及全局地图的地图特征点输入至该应用程序,以获得相应的位姿评分。
实施例3
图3是本发明实施例3提供的一种轨迹集中选取位姿的流程图,包括如下步骤:
步骤S31:从所述轨迹集中筛选出位姿评分大于预设阈值的至少一个初筛位姿。
本发明实施例中,从轨迹集中选取作为新导航目标的位姿时,首先从轨迹集中筛选出所有位姿评分大于预设阈值的相应的位姿,作为初筛位姿,初筛位姿可能会存在多个,因此需要根据位姿距离进行进一步的筛选。其中,上述筛选的过程也即位姿评分的对比过程,例如可以从轨迹集中按逆时间顺序获取位姿评分与预设阈值进行对比,在确定位姿评分大于预设阈值时从轨迹集中提取出相应的初筛位姿。
步骤S32:根据位姿距离算法从至少一个所述初筛位姿中筛选出与所述恢复地图的原点的位姿距离最小的位姿,作为所述新导航目标。
本发明实施例中,所述位姿距离算法的算式包括:
dist(Ti,Tj)=α|xi-xj,yi-yj,zi-zj|2+β|ai-aj,bi-bj,ci-cj|2;
式中,dist()为位姿距离,Ti为第i个位姿,Tj为第j个位姿;xi、yi、zi为Ti位姿的坐标,ai、bi、ci为Ti位姿的欧拉角;xj、yj、zj为Tj位姿的坐标,aj、bj、cj为Tj位姿的欧拉角;α为直线距离权重,β为转动角度权重。其中,在获得多个初筛位姿后,将利用每个初筛位姿分别于恢复地图的原点的位姿按照上述位姿距离算法的算式计算出相应的位姿距离,在筛选出位姿距离最小的相应的初筛位姿,作为最终新导航目标。其中,该恢复地图的原点的位姿即轨迹集中最近的上一位姿。
实施例4
图4是本发明实施例4提供的一种视觉定位恢复方法的流程图,该方法包括如下步骤:
步骤S41:按照预设时间间隔获取视觉图像计算位姿以及相应的位姿评分,将所述位姿以及所述位姿评分关联存储至轨迹集。
此步骤与上述步骤S11一致,在此不再赘述。
步骤S42:当无法计算当前位姿时确定定位跟踪失败时,按照预设规则从所述轨迹集中选取一个位姿作为新导航目标。
此步骤与上述步骤S12一致,在此不再赘述。
步骤S43:通过运动追踪传感器获取上一位姿与当前位姿之间的位姿变换,利用所述上一位姿与所述位姿变换计算所述坐标原点,根据所述坐标原点创建所述恢复地图,将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位。
本发明实施例中,所述坐标原点的计算算式包括:T=TlastTdiff,式中,T为所述坐标原点,Tlast为所述上一位姿,Tdiff为所述位姿变换。
本发明实施例中,上述运动追踪传感器包括惯性传感器和轮式里程计中至少一种传感器。
步骤S44:在重定位成功后,将所述恢复地图与全局地图合并为新的全局地图,并恢复初始导航目标,在所述新的全局地图上进行视觉定位导航。
此步骤与上述步骤S14一致,在此不再赘述。
实施例5
图5是本发明实施例4提供的一种视觉定位恢复装置的结构示意图。
该视觉定位恢复装置500包括:
位姿计算模块510,用于获取视觉图像计算位姿以及相应的位姿评分,将所述位姿以及所述位姿评分关联存储至轨迹集;
导航目标选取模块520,用于当确定定位跟踪失败时,按照预设规则从所述轨迹集中选取一个位姿作为新导航目标;
重定位模块530,用于以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图,将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位;
导航恢复模块540,用于在重定位成功后,将所述恢复地图与全局地图合并为新的全局地图,并恢复初始导航目标,在所述新的全局地图上进行视觉定位导航。
本发明实施例中,上述各个模块更加详细的功能描述可以参考前述实施例中相应部分的内容,在此不再赘述。
此外,本发明还提供了一种机器人,该机器人包括存储器和处理器,存储器可用于存储计算机程序,处理器通过运行所述计算机程序,从而使机器人执行上述方法或者上述视觉定位恢复装置中的各个模块的功能。
存储器可包括存储程序区和存储数据区,其中,存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等;存储数据区可存储根据机器人的使用所创建的数据(比如音频数据、电话本等)等。此外,存储器可以包括高速随机存取存储器,还可以包括非易失性存储器,例如至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
本发明还提供了一种计算机存储介质,用于储存上述机器人中使用的计算机程序。
在本申请所提供的几个实施例中,应该理解到,所揭露的装置和方法,也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的,例如,附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分,所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意,在作为替换的实现方式中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个连续的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。也要注意的是,结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合,可以用执行规定的功能或动作的专用的基于硬件的系统来实现,或者可以用专用硬件与计算机指令的组合来实现。
另外,在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分,也可以是各个模块单独存在,也可以两个或更多个模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应所述以权利要求的保护范围为准。
Claims (10)
1.一种视觉定位恢复方法,其特征在于,包括:
获取视觉图像计算位姿以及相应的位姿评分,将所述位姿以及所述位姿评分关联存储至轨迹集;
当确定定位跟踪失败时,按照预设规则从所述轨迹集中选取一个位姿作为新导航目标;
以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图,将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位;
在重定位成功后,将所述恢复地图与全局地图合并为新的全局地图,并恢复初始导航目标,在所述新的全局地图上进行视觉定位导航。
2.根据权利要求1所述的视觉定位恢复方法,其特征在于,所述获取视觉图像计算当前位姿以及相应的位姿评分包括:
获取当前视觉图像,对所述当前视觉图像进行特征点提取,获得多个图像特征点;
利用所述多个图像特征点与全局地图的地图特征点进行匹配运算,获得当前位姿以及相应的位姿评分。
3.根据权利要求1所述的视觉定位恢复方法,其特征在于,所述按照预设规则从所述轨迹集中选取一个位姿作为新导航目标包括:
从所述轨迹集中筛选出位姿评分大于预设阈值的至少一个初筛位姿;
根据位姿距离算法从至少一个所述初筛位姿中筛选出与所述恢复地图的原点的位姿距离最小的位姿,作为所述新导航目标。
4.根据权利要求3所述的视觉定位恢复方法,其特征在于,所述位姿距离算法的算式包括:
dist(Ti,Tj)=α|xi-xj,yi-yj,zi-zj|2+β|ai-aj,bi-bj,ci-cj|2;
式中,dist()为位姿距离,Ti为第i个位姿,Tj为第j个位姿;xi、yi、zi为Ti位姿的坐标,ai、bi、ci为Ti位姿的欧拉角;xj、yj、zj为Tj位姿的坐标,aj、bj、cj为Tj位姿的欧拉角;α为直线距离权重,β为转动角度权重。
5.根据权利要求1所述的视觉定位恢复方法,其特征在于,还包括:
启动视觉定位导航后,创建所述全局地图并接收所述初始导航目标,在所述全局地图上向所述初始导航目标进行视觉定位导航。
6.根据权利要求1所述的视觉定位恢复方法,其特征在于,所述将所述新导航目标映射至所述恢复地图的算式包括:
recovergoal=T-1 lastTrecover;
式中,Tlast为所述上一位姿,Trecover为所述新导航目标,recovergoal为映射后所述新导航目标。
7.根据权利要求1所述的视觉定位恢复方法,其特征在于,还包括:
通过运动追踪传感器获取上一位姿与当前位姿之间的位姿变换,利用所述上一位姿与所述位姿变换计算所述坐标原点,根据所述坐标原点创建所述恢复地图;
所述坐标原点的计算算式包括:T=TlastTdiff,式中,T为所述坐标原点,Tlast为所述上一位姿,Tdiff为所述位姿变换。
8.一种视觉定位恢复装置,其特征在于,包括:
位姿计算模块,用于获取视觉图像计算位姿以及相应的位姿评分,将所述位姿以及所述位姿评分关联存储至轨迹集;
导航目标选取模块,用于当确定定位跟踪失败时,按照预设规则从所述轨迹集中选取一个位姿作为新导航目标;
重定位模块,用于以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图,将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位;
导航恢复模块,用于在重定位成功后,将所述恢复地图与全局地图合并为新的全局地图,并恢复初始导航目标,在所述新的全局地图上进行视觉定位导航。
9.一种机器人,其特征在于,包括存储器以及处理器,所述存储器存储有计算机程序,所述计算机程序在所述处理器上运行时执行根据权利要求1至7中任一项所述的视觉定位恢复方法。
10.一种可读存储介质,其特征在于,其存储有计算机程序,所述计算机程序在处理器上运行时执行权利要求1至7中任一项所述的视觉定位恢复方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110973056.5A CN113607160B (zh) | 2021-08-24 | 2021-08-24 | 视觉定位恢复方法、装置、机器人和可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110973056.5A CN113607160B (zh) | 2021-08-24 | 2021-08-24 | 视觉定位恢复方法、装置、机器人和可读存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113607160A CN113607160A (zh) | 2021-11-05 |
CN113607160B true CN113607160B (zh) | 2023-10-31 |
Family
ID=78309214
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110973056.5A Active CN113607160B (zh) | 2021-08-24 | 2021-08-24 | 视觉定位恢复方法、装置、机器人和可读存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113607160B (zh) |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2017005532A (ja) * | 2015-06-11 | 2017-01-05 | 富士通株式会社 | カメラ姿勢推定装置、カメラ姿勢推定方法およびカメラ姿勢推定プログラム |
CN109084732A (zh) * | 2018-06-29 | 2018-12-25 | 北京旷视科技有限公司 | 定位与导航方法、装置及处理设备 |
CN109141437A (zh) * | 2018-09-30 | 2019-01-04 | 中国科学院合肥物质科学研究院 | 一种机器人全局重定位方法 |
CN110246182A (zh) * | 2019-05-29 | 2019-09-17 | 深圳前海达闼云端智能科技有限公司 | 基于视觉的全局地图定位方法、装置、存储介质和设备 |
CN110533722A (zh) * | 2019-08-30 | 2019-12-03 | 的卢技术有限公司 | 一种基于视觉词典的机器人快速重定位方法及系统 |
CN111750864A (zh) * | 2020-06-30 | 2020-10-09 | 杭州海康机器人技术有限公司 | 一种基于视觉地图的重定位方法和装置 |
CN112991447A (zh) * | 2021-03-16 | 2021-06-18 | 华东理工大学 | 一种动态环境下视觉定位与静态地图构建方法及系统 |
CN113034582A (zh) * | 2021-03-25 | 2021-06-25 | 浙江商汤科技开发有限公司 | 位姿优化装置及方法、电子设备及计算机可读存储介质 |
CN113129379A (zh) * | 2021-06-17 | 2021-07-16 | 同方威视技术股份有限公司 | 自动移动设备的全局重定位方法以及装置 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111105454B (zh) * | 2019-11-22 | 2023-05-09 | 北京小米移动软件有限公司 | 一种获取定位信息的方法、装置及介质 |
CN111024100B (zh) * | 2019-12-20 | 2021-10-29 | 深圳市优必选科技股份有限公司 | 一种导航地图更新方法、装置、可读存储介质及机器人 |
-
2021
- 2021-08-24 CN CN202110973056.5A patent/CN113607160B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2017005532A (ja) * | 2015-06-11 | 2017-01-05 | 富士通株式会社 | カメラ姿勢推定装置、カメラ姿勢推定方法およびカメラ姿勢推定プログラム |
CN109084732A (zh) * | 2018-06-29 | 2018-12-25 | 北京旷视科技有限公司 | 定位与导航方法、装置及处理设备 |
CN109141437A (zh) * | 2018-09-30 | 2019-01-04 | 中国科学院合肥物质科学研究院 | 一种机器人全局重定位方法 |
CN110246182A (zh) * | 2019-05-29 | 2019-09-17 | 深圳前海达闼云端智能科技有限公司 | 基于视觉的全局地图定位方法、装置、存储介质和设备 |
CN110533722A (zh) * | 2019-08-30 | 2019-12-03 | 的卢技术有限公司 | 一种基于视觉词典的机器人快速重定位方法及系统 |
CN111750864A (zh) * | 2020-06-30 | 2020-10-09 | 杭州海康机器人技术有限公司 | 一种基于视觉地图的重定位方法和装置 |
CN112991447A (zh) * | 2021-03-16 | 2021-06-18 | 华东理工大学 | 一种动态环境下视觉定位与静态地图构建方法及系统 |
CN113034582A (zh) * | 2021-03-25 | 2021-06-25 | 浙江商汤科技开发有限公司 | 位姿优化装置及方法、电子设备及计算机可读存储介质 |
CN113129379A (zh) * | 2021-06-17 | 2021-07-16 | 同方威视技术股份有限公司 | 自动移动设备的全局重定位方法以及装置 |
Non-Patent Citations (3)
Title |
---|
A Visual Relocation Algorithm With Illumination Changing Robustness;Xu Duo et al.;《2019 6th International Conference on Systems and Informatics (ICSAI)》;127-132 * |
一种基于半直接视觉里程计的RGB-D SLAM算法;谷晓琳等;《机器人》;第42卷(第1期);39-48 * |
单目同时定位与建图中的地图恢复融合技术;张剑华;王燕燕;王曾媛;陈胜勇;管秋;;中国图象图形学报(第03期);74-85 * |
Also Published As
Publication number | Publication date |
---|---|
CN113607160A (zh) | 2021-11-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110631554B (zh) | 机器人位姿的确定方法、装置、机器人和可读存储介质 | |
CN104756155B (zh) | 合并多个地图以用于基于计算机视觉的跟踪的系统及方法 | |
EP2840528B1 (en) | Method and apparatus for tracking object | |
JP5774226B2 (ja) | 方位センサに基づくホモグラフィ分解の曖昧性の解決 | |
US11866067B2 (en) | System and method for generating and controlling driving paths in autonomous vehicle | |
JP2007303886A (ja) | 位置推定装置、位置推定方法及びプログラム記録媒体 | |
CN111445526A (zh) | 一种图像帧之间位姿的估计方法、估计装置和存储介质 | |
KR101941852B1 (ko) | 그래프 slam을 위한 키프레임 추출방법 및 이를 이용한 slam 장치 | |
JP2019132664A (ja) | 自車位置推定装置、自車位置推定方法、及び自車位置推定プログラム | |
US10242281B2 (en) | Hybrid orientation system | |
US9396538B2 (en) | Image processing system, image processing method, and program | |
US20160219215A1 (en) | Survey data processing device, survey data processing method, and program therefor | |
CN105824592A (zh) | 倒车轨迹的显示方法及装置 | |
CN114445453A (zh) | 一种自动驾驶中实时多目标跟踪方法及系统 | |
US20220398825A1 (en) | Method for generating 3d reference points in a map of a scene | |
JP2008122093A (ja) | 多目標追尾装置 | |
CN113607160B (zh) | 视觉定位恢复方法、装置、机器人和可读存储介质 | |
JPWO2018159467A1 (ja) | 移動体検知装置、移動体検知方法、及びプログラム | |
WO2018114244A1 (en) | Method for assisting a driver of a motor vehicle when parking using a driver assistance device, corresponding computer program product and driver assistance device | |
CN111292292B (zh) | 管道障碍物检测方法、装置、管道机器人和可读存储介质 | |
CN112330727A (zh) | 图像匹配方法、装置、计算机设备及存储介质 | |
CN110119649B (zh) | 电子设备状态跟踪方法、装置、电子设备及控制系统 | |
CN111814114B (zh) | 车道定位校验的方法、设备、电子设备、车辆及存储介质 | |
CN113762129A (zh) | 实时2d人体姿势估计系统中的姿势稳定系统和方法 | |
JP4279697B2 (ja) | 目標追尾装置 |
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 |