视觉定位恢复方法、装置、机器人和可读存储介质.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113607160A(43)申请公布日2021.11.05(21)申请号202110973056.5(22)申请日2021.08.24(71)申请人湖南国科微电子股份有限公司地址410000湖南省长沙市长沙经济技术开发区泉塘街道东十路南段9号(72)发明人周震袁涛(74)专利代理机构北京超凡宏宇专利代理事务所(特殊普通合伙)11463代理人梁韬(51)Int.Cl.G01C21/00(2006.01)G01C11/04(2006.01)G06T7/73(2017.01)G06F16/29(2019.01)权利要求书2页说明书7页附图3页(54)发明名称视觉定位恢复方法、装置、机器人和可读存储介质(57)摘要本发明提供一种视觉定位恢复方法、装置、机器人和可读存储介质，该视觉定位恢复方法包括：获取视觉图像计算位姿以及相应的位姿评分，将位姿以及位姿评分关联存储至轨迹集；当确定定位跟踪失败时，按照预设规则从轨迹集中选取一个位姿作为新导航目标；以轨迹集中最近的上一位姿为坐标原点创建恢复地图，将新导航目标映射至恢复地图上进行视觉定位导航并重定位；在重定位成功后，将恢复地图与全局地图合并为新的全局地图，并恢复初始导航目标，在新的全局地图上进行视觉定位导航。本发明在定位追踪失败时，可以通过获取最优的历史位姿进行重定位路线的导航，从而缩短重定位的时间，提高重定位的成功率。CN113607160ACN113607160A权利要求书1/2页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所述的视觉定位恢复方法，其特征在于，所述将所述新导航目标映射至所述恢复地图的算式包括：‑1recovergoal＝TlastTrecover；式中，Tlast为所述上一位姿，Trecover为所述新导航目标，recovergoal为映射后所述新导航目标。7.根据权利要求1所述的视觉定位恢复方法，其特征在于，还包括：通过运动追踪传感器获取上一位姿与当前位姿之间的位姿变换，利用所述上一位姿与所述位姿变换计算所述坐标原点，根据所述坐标原点创建所述恢复地图；所述坐标原点的计算算式包括：T＝TlastTdiff，式中，T为所述坐标原点，Tlast为所述上一位姿，Tdiff为所述位姿变换。8.一种视觉定位恢复装置，其特征在于，包括：2CN113607160A权利要求书2/2页位姿计算模块，用于获取视觉图像计算位姿以及相应的位姿评分，将所述位姿以及所述位姿评分关联存储至轨迹集；导航目标选取模块，用于当确定定位跟踪失败时，按照预设规则从所述轨迹集中选取一个位姿作为新导航目标；重定位模块，用于以所述轨迹集中最近的上一位姿为坐标原点创建恢复地图，将所述新导航目标映射至所述恢复地图上进行视觉定位导航并重定位；导航恢复模块，用于在重定位成功后，将所述恢复地图与全局地图