CN117629204A - 定位方法和设备 - Google Patents

定位方法和设备 Download PDF

Info

Publication number
CN117629204A
CN117629204A CN202210979139.XA CN202210979139A CN117629204A CN 117629204 A CN117629204 A CN 117629204A CN 202210979139 A CN202210979139 A CN 202210979139A CN 117629204 A CN117629204 A CN 117629204A
Authority
CN
China
Prior art keywords
pose
robot
positioning
fusion
state vector
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.)
Pending
Application number
CN202210979139.XA
Other languages
English (en)
Inventor
胡佳欣
郎小明
任科飞
蔡剑成
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Sankuai Online Technology Co Ltd
Original Assignee
Beijing Sankuai Online Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Sankuai Online Technology Co Ltd filed Critical Beijing Sankuai Online Technology Co Ltd
Priority to CN202210979139.XA priority Critical patent/CN117629204A/zh
Priority to PCT/CN2023/109080 priority patent/WO2024037295A1/zh
Publication of CN117629204A publication Critical patent/CN117629204A/zh
Pending legal-status Critical Current

Links

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
    • 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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Numerical Control (AREA)

Abstract

本发明实施例涉及定位技术领域，尤其涉及一种定位方法和设备。所述方法包括：根据VIO算法估计机器人的局部定位位姿；根据地图特征点匹配算法估计机器人的全局定位位姿；确定全局定位位姿是否与局部定位位姿估计一致；若估计一致，则基于融合状态向量执行机器人的位姿融合更新。其中，融合状态向量包括VIO局部定位变量和全局定位变量，局部定位变量包括机器人的速度和传感器偏置，速度和传感器偏置保持施密特状态；若估计不一致，则确定全局定位位姿的观测误差，根据观测误差对融合状态向量进行坐标系转换，根据坐标系转换后的融合状态向量执行机器人的位姿更新。本发明实施例方案能够提高机器人定位结果的准确性和稳定性。

Description

定位方法和设备
技术领域
本发明实施例涉及定位技术领域，尤其涉及一种定位方法和设备。
背景技术
定位和导航是机器人研究领域的核心问题。其中，定位主要是在机器人移动过程中确定机器人的实时位置。目前机器人的定位方法包括基于机器人传感器的局部定位方法，还包括基于GPS等的全局定位方法。但基于机器人传感器的局部定位方法容易存在误差的累计漂移，基于GPS等的全局定位方法比较容易受到信号干扰。因此，如何使机器人定位结果更加准确、稳定成为需要解决的问题。
发明内容
有鉴于此，本发明实施例提供了一种定位方法和设备，提出了一种融合局部定位和全局定位的机器人定位方案，且根据局部定位和全局定位结果的一致性提出了不同的融合策略，由此以提高机器人定位结果的准确性和稳定性。
第一方面，本发明实施例提供了一种定位方法，包括：
根据视觉惯性里程计(Visual-Inertial Odometry，VIO)算法估计机器人的局部定位位姿；
根据地图特征点匹配算法估计所述机器人的全局定位位姿；
确定所述全局定位位姿是否与所述局部定位位姿估计一致；
若估计一致，则基于融合状态向量执行所述机器人的位姿融合更新，其中，所述融合状态向量包括VIO局部定位变量和全局定位变量，所述局部定位变量包括所述机器人的速度和传感器偏置，所述速度和所述传感器偏置保持施密特状态；
若估计不一致，则确定所述全局定位位姿的观测误差，根据所述观测误差对所述融合状态向量进行坐标系转换，根据所述坐标系转换后的融合状态向量执行所述机器人的位姿更新。
可选的，所述全局定位位姿与所述局部定位位姿一致的条件，包括以下一项或多项的组合：
求解所述全局定位位姿时的内点指标参数满足设定条件；
所述全局定位位姿与所述局部定位位姿之间的误差值小于第一阈值；
所述机器人的相机拍摄到的用于确定所述局部定位位姿的多帧图像之间的相关系数大于第二阈值。
可选的，所述全局定位位姿与所述局部定位位姿之间的误差值包括：位置误差值和姿态角误差值；
所述内点指标参数包括：内点个数、内点率和内点的重投影平均误差。
可选的，所述VIO局部定位变量还包括：所述机器人的旋转矩阵、位置；
所述全局定位变量包括：所述机器人在全局地图的位置和姿态角。
可选的，所述基于融合状态向量执行所述机器人的位姿融合更新，包括：
当在所述机器人的相机拍摄的图像帧识别到新的地图特征点或者已识别到的地图特征点消失时，使所述速度和所述传感器偏置保持施密特状态并基于所述融合状态向量执行所述机器人的位姿融合更新。
可选的，所述方法还包括：
确定卡尔曼增益影响因子；
基于所述卡尔曼增益影响因子计算卡尔曼增益，所述卡尔曼增益用于所述机器人的位姿融合更新，所述卡尔曼增益影响因子用于在所述位姿融合更新过程中约束融合状态向量包含的横摇角和俯仰角更新前后保持不变。
可选的，所述确定所述全局定位位姿的观测误差，包括：
根据所述卡尔曼增益计算所述全局定位位姿的观测误差。
可选的，所述根据所述观测误差对所述融合状态向量进行坐标系转换，包括：
根据所述观测误差对所述局部定位位姿进行位姿更新，以得到更新后的局部定位位姿；
根据所述更新后的局部定位位姿确定世界坐标系变化矩阵；
根据所述世界坐标系变化矩阵对所述融合状态向量和所述融合状态向量对应的协方差矩阵进行坐标系转换。
第二方面，本发明实施例提供了一种定位装置，包括：
VIO模块，用于根据视觉惯性里程计VIO算法估计机器人的局部定位位姿；
全局定位模块，用于根据地图特征点匹配算法估计所述机器人的全局定位位姿；
定位融合模块，用于确定所述全局定位位姿是否与所述局部定位位姿估计一致；若估计一致，则基于融合状态向量执行所述机器人的位姿融合更新，其中，所述融合状态向量包括VIO局部定位变量和全局定位变量，所述局部定位变量包括所述机器人的速度和传感器偏置，所述速度和所述传感器偏置保持施密特状态；若估计不一致，则确定所述全局定位位姿的观测误差，根据所述观测误差对所述融合状态向量进行坐标系转换，根据所述坐标系转换后的融合状态向量执行所述机器人的位姿更新。
第三方面，本发明实施例提供了一种电子设备，包括：至少一个处理器；以及与所述处理器通信连接的至少一个存储器，其中：所述存储器存储有可被所述处理器执行的程序指令，所述处理器调用所述程序指令能够执行如上述第一方面或者第一方面任一项所述的方法。
第四方面，本发明实施例提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机可读存储介质所在设备执行如上述第一方面或者第一方面任一项所述的方法。
本发明实施例方案中，基于VIO算法获取机器人的局部定位位姿，基于地图特征点匹配算法获取机器人的全局定位位姿。并且根据局部定位位姿和全局定位位姿的一致性提出了不同的融合策略。其中当全局定位位姿与局部定位位姿一致时，将速度和传感器偏置设置为施密特变量，确保全局位姿更新时速度和传感器偏置更新前后不发生跳变。当全局定位位姿与局部定位位姿不一致时，基于观测误差对融合状态向量进行坐标系转换，由此尽量避免机器人位姿在更新过程中变差。因此通过本发明实施例方案，可以提高机器人定位结果的准确性和稳定性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种定位方法的框架示意图；
图2为本发明实施例提供的一种定位方法的流程图；
图3为本发明实施例提供的一种定位装置的结构示意图；
图4为本发明实施例提供的一种电子设备的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
参见图1，为本发明实施例提供的一种定位方法的框架示意图。图1所示方法可以应用于机器人，也可以应用于与机器人通信连接的终端设备或者服务器等。可选的，所述机器人可以是具有行走功能的设备，如扫地机器人、配送机器人或者无人驾驶车辆等。可选的，所述机器人还可以是飞行器等。如图1所示，机器人设置有惯性测量单元(InertialMeasureMent Unit，IMU)和相机。IMU包括加速计和陀螺仪，分别用于采集机器人的加速度和角速度。本发明实施例将IMU采集的加速度和角速度简称为IMU数据。相机用于拍摄图像帧。根据IMU数据和图像帧可以执行VIO算法得到机器人的局部定位位姿。根据图像帧和预先配置的全局地图可以执行地图特征点匹配算法得到机器人的全局定位位姿。本发明实施例中，根据全局定位位姿与局部定位位姿的一致性，可以执行全局定位位姿和局部定位位姿的融合更新策略，以此得到机器人的定位输出。
VIO算法是以视觉和IMU融合实现机器人定位的里程计算法。其中，IMU返回IMU数据的频率较高，相机返回图像帧的频率较低。在执行VIO算法时，可以使用IMU数据执行较高频率的位姿估计，并利用相机采集的图像帧进行位姿更新，以得到机器人的局部定位位姿。具体的，IMU和相机的融合定位可以采用松耦合方案，也可以采用紧耦合方案。其中，松耦合方案是指将基于IMU数据估计的位姿与基于相机图像帧估计的位姿直接进行融合，融合过程对IMU数据和图像帧数据不产生影响。在一些实施例中，可以采用卡尔曼滤波器执行IMU和相机的融合定位。紧耦合是指将IMU数据和图像帧的特征数据进行融合，并基于融合数据进行机器人的位姿估计，融合过程对IMU数据和图像帧数据产生影响。在一些实施例中，可以采用多状态约束卡尔曼滤波器(Multi-State Constraint Kalman Filter，MSCKF)、鲁棒视觉惯性里程计(Robust Visual Inertial Odometry，ROVIO)或者扩展卡尔曼滤波(Extended Kalman Filter，EKF)执行IMU和相机的融合定位。
地图特征点匹配算法包括：从相机拍摄的图像帧中提取图像特征点。将图像特征点与全局地图的特征点匹配，根据匹配结果可以估计机器人的全局定位位姿。机器人的全局定位位姿可以包括机器人在全局地图的六个自由度位姿，具体包括机器人在世界坐标系的三轴位置坐标以及环绕三个坐标轴的姿态角，即偏航角(yaw)、横摇角(roll)和俯仰角(pitch)。
在一些实施例中，全局定位位姿估计为低频测量，相对局部定位位姿更新频率较低，若根据全局定位位姿强行对局部定位位姿更新，可能会使定位结果产生跳变。因此，本发明实施例在计算出机器人的全局定位位姿之后，进一步判断全局定位位姿与局部定位位姿的一致性。根据一致性判断结果，可以采取不同的位姿融合更新策略，由此保证定位结果的准确性和温度性。
参见图2，为本发明实施例提供的一种定位方法的流程图。如图2所示，该方法的处理步骤包括：
101，根据VIO算法估计机器人的局部定位位姿。
102，根据地图特征点匹配算法估计机器人的全局定位位姿。
103，确定全局定位位姿是否与局部定位位姿估计一致。
104，若估计一致，则采用策略一进行全局定位位姿和局部定位位姿的融合更新。具体的，所述策略一包括：基于融合状态向量X执行机器人的位姿融合更新。可选的，可以基于施密特-卡尔曼滤波算法对全局定位位姿和局部定位位姿进行融合更新。具体的，确定融合状态向量X。融合状态向量X包括VIO局部定位变量和全局定位变量。可选的，VIO局部定位变量包括：机器人的速度和传感器偏置，速度和传感器偏置保持施密特状态。全局定位位姿估计为低频测量，相对局部定位位姿更新频率较低。若在获取到全局定位位姿后强行基于融合状态向量对局部定位位姿更新，可能会使局部定位位姿中的速度和传感器偏置发生突变。因此当全局定位位姿与局部定位位姿具有一致性时，可以令速度和传感器偏置保持施密特状态，由此在基于融合状态向量进行位姿更新时可以使更新前后的速度和传感器偏置不变。在一些实施例中，VIO局部定位变量还可以包括机器人的旋转矩阵和位置。全局定位变量包括机器人在全局地图的位置和姿态角。
在一些实施例中，机器人的局部定位姿态包括姿态角参数。姿态角参数包括偏航角(yaw)、横摇角(roll)和俯仰角(pitch)。在VIO算法中横摇角(roll)和俯仰角(pitch)是可观的，横摇角(roll)和俯仰角(pitch)不存在累计误差。为此本发明实施例中确定卡尔曼增益影响因子。根据卡尔曼增益影响因子可以计算卡尔曼增益，卡尔曼增益用于机器人的位姿融合更新。卡尔曼增益影响因子用于在机器人的位姿融合更新过程中约束融合状态向量包含的横摇角(roll)和俯仰角(pitch)更新前后保持不变。
105，若估计不一致，则采用策略二进行全局定位位姿和局部定位位姿的融合更新。具体的，所述策略二包括：确定全局定位位姿的观测误差。根据观测误差对融合状态向量X进行坐标系转换。根据坐标系转换后的融合状态向量执行机器人的位姿更新。
在一些实施例中，上述确定全局定位位姿与局部定位位姿估计一致的条件，包括以下一项或多项的组合：(1)求解全局定位位姿时的内点指标参数满足设定条件。可选的，可以采用ransac算法求解机器人的全局定位位姿。通过ransac算法求解机器人的全局定位位姿可以获取内点指标参数。所述内点指标参数可以包括：内点个数、内点率和内点的重投影平均误差。内点指标参数满足设定条件包括：内点个数大于设定个数、内点率大于设定内点率以及内点的重投影平均误差小于设定误差值。(2)全局定位位姿与局部定位位姿之间的误差值小于第一阈值。可选的，全局定位位姿和局部定位位姿均包含位置参数和姿态角参数。相应的，全局定位位姿与局部定位位姿之间的误差值可以包括位置误差值和姿态角误差值。对应上述位置误差值和姿态角误差值可以分别设置第一子阈值和第二子阈值。当全局定位位姿与局部定位位姿的位置误差值小于第一子阈值，且姿态角误差值小于第二子阈值时，确定全局定位位姿与局部定位位姿之间的误差值小于第一阈值。(3)机器人的相机拍摄到的用于确定局部定位位姿的多帧图像之间的相关系数大于第二阈值。如果上述(1)-(3)中的任一项或者多项组合的判断结果为是，则确定全局定位位姿与局部定位位姿一致，否则确定全局定位位姿与局部定位位姿不一致。
以下将对本发明实施例涉及的定位方法进行说明。在一些实施例中，IMU的状态向量可以表示为XI：
其中，表示k时刻从世界坐标系到IMU坐标系的旋转矩阵的四元数。
表示k时刻IMU在世界坐标系的速度。
表示k时刻IMU在世界坐标系的位置。
表示k时刻IMU的角速度偏差。
表示k时刻IMU的加速度偏差。
和/>统称为传感器偏置。
在一些实施例中，根据IMU连续采集的IMU数据可以对XI进行预测和更新，得到基于IMU的预估位姿。
在一些实施例中，根据相机的图像帧确定的状态向量可以表示为XC，XC可以包含图像帧的图像特征点，或者XC可以表示根据图像特征点估计的姿态角和位置。可选的，根据XC可以对IMU的预估位姿进行更新，实现相机和IMU的定位融合。其中基于VIO输出的局部定位位姿的状态向量表示为X′I。
在一些实施例中，基于地图特征点匹配算法估计的全局定位位姿的状态向量可以表示为XS，XS包括机器人在全局地图的位置和姿态角。
在一些实施例中，对全局定位位姿和局部定位位姿融合可以包括：建立融合状态变量X，融合状态变量X包括VIO局部定位变量和全局定位变量。具体的，融合状态变量X＝[X′I，XS]。采用施密特-卡尔曼滤波算法对全局定位位姿和局部定位位姿进行融合更新时，可以将全局定位变量XS作为融合状态向量X的干扰参数。即在全局定位位姿未更新的时间内，基于VIO对融合状态向量X进行更新。在X更新的过程中全局定位变量XS保持在施密特状态，直至全局定位位姿更新。其中，全局定位位姿更新包括在执行所述地图特征点匹配算法的过程中，当在机器人的相机拍摄的图像帧识别到新的地图特征点或者已识别到的地图特征点消失时，确定全局定位位姿更新，相应的全局定位变量XS更新。当全局定位变量XS更新时，基于XS对融合状态变量X更新。
进一步，对于上述融合更新策略一：全局定位位姿估计为低频测量，相对局部定位位姿更新频率较低。当全局定位变量XS更新时，若根据全局定位变量XS强行对VIO局部定位变量X′I更新，可能会使X′I中的速度和传感器偏置产生跳变。因此采用策略一更新融合状态变量X时，将融合状态变量X包含的速度和传感器偏置设置为施密特变量。当在机器人相机拍摄的图像帧识别到新的地图特征点或者已识别到的图像特征点消失时，即需要通过全局定位变量XS对融合状态变量X更新时，令融合状态变量X包含的速度和传感器偏置保持为施密特状态。即速度和传感器偏置更新前后保持不变。
在一些实施例中，VIO的roll(横摇角)和pitch(俯仰角)是可观的，不存在累计误差。因此在融合状态变量X更新时不需要对roll和pitch更新。可选的，为了在更新过程中约束融合状态变量X包含的roll和pitch不更新，本发明实施例设置了卡尔曼增益影响因子。基于卡尔曼影响因子计算卡尔曼增益，此时得到的卡尔曼增益可以约束roll和pitch在融合状态变量X更新前后保持不变。
在一些实施例中，卡尔曼影响因子可以表示为Kyaw，e3＝[0，0，1]T，/>表示旋转矩阵。卡尔曼增益/>
其中，根据K可以对融合状态变量X的估计值和估计值的协方差进行更新。
对于上述融合更新策略二：由于全局定位位姿与局部定位定位相差较大，此时如果采用策略一进行位姿更新可能会使机器人位姿变差。因此本发明实施例策略二提出了坐标系更新策略。即将当前位姿更新转换为坐标系变化。具体的，基于策略一计算的卡尔曼增益K计算全局定位位姿的观测误差δx。其中，δx＝Kr，r为全局定位位姿的残差。根据观测误差δx可以对局部定位位姿进行更新，得到更新后的局部定位位姿其中，更新前的局部定位位姿表示为Ti G。根据/>和Ti G可以确定世界坐标系变化矩阵/>根据世界坐标系变化矩阵/>可以对融合状态向量X以及融合状态向量X对应的协方差矩阵P进行坐标系转换。/>协方差矩阵Pnew＝JPJT。其中J表示/>相对XG的雅克比(jacobian)矩阵。
本发明实施例提出了基于滤波框架的融合视觉地图和VIO的方位方法。该方法实施例基于视觉地图定位的测量特性与VIO位姿估计进行融合，提高了机器人定位的鲁棒性和高效性。在提高定位精度的同时可以保证定位效率，避免全局测量精度对VIO的负面影响。
对应上述定位方法，本发明实施例提供了一种定位装置。参见图3，为本发明实施例提供的一种定位装置的结构示意图。如图3所示，该定位装置包括：
VIO模块201，用于根据视觉惯性里程计VIO算法估计机器人的局部定位位姿；
全局定位模块202，用于根据地图特征点匹配算法估计所述机器人的全局定位位姿；
定位融合模块203，用于确定所述全局定位位姿是否与所述局部定位位姿估计一致；若估计一致，则基于融合状态向量执行所述机器人的位姿融合更新，其中，所述融合状态向量包括VIO局部定位变量和全局定位变量，所述局部定位变量包括所述机器人的速度和传感器偏置，所述速度和所述传感器偏置保持施密特状态；若估计不一致，则确定所述全局定位位姿的观测误差，根据所述观测误差对所述融合状态向量进行坐标系转换，根据所述坐标系转换后的融合状态向量执行所述机器人的位姿更新。
本发明实施例的定位装置可以执行图2所示实施例涉及的定位方法。本实施例未详细描述的部分，可以参考对图2所示实施例的相关说明。该技术方案的执行过程和技术效果参见图2所示实施例中的描述，在此不再赘述。
应理解，图3所示定位装置的各个模块的划分仅仅是一种逻辑功能的划分，实际实现时可以全部或部分集成到一个物理实体上，也可以物理上分开。且这些模块可以全部以软件通过处理元件调用的形式实现；也可以全部以硬件的形式实现；还可以部分模块以软件通过处理元件调用的形式实现，部分模块通过硬件的形式实现。例如，VIO模块201和全局定位模块202可以为单独设立的处理元件，也可以集成在电子设备的某一个芯片中实现。其它模块的实现与之类似。此外这些模块全部或部分可以集成在一起，也可以独立实现。在实现过程中，上述方法的各步骤或以上各个模块可以通过处理器元件中的硬件的集成逻辑电路或者软件形式的指令完成。
例如，以上这些模块可以是被配置成实施以上方法的一个或多个集成电路，例如：一个或多个特定集成电路(Application Specific Integrated Circuit；以下简称：ASIC)，或，一个或多个微处理器(Digital Singnal Processor；以下简称：DSP)，或，一个或者多个现场可编程门阵列(Field Programmable Gate Array；以下简称：FPGA)等。再如，这些模块可以集成在一起，以片上系统(System-On-a-Chip；以下简称：SOC)的形式实现。
图4为本发明实施例提供的一种电子设备的结构示意图。该电子设备可以用于执行上述的定位方法。如图4所示，该电子设备以通用计算设备的形式表现。电子设备的组件可以包括但不限于：一个或者多个处理器410，通信接口420，存储器430以及连接不同系统组件(包括处理器410、通信接口420和存储器430)的通信总线440。
通信总线440表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器，外围总线，图形加速端口，处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说，这些体系结构包括但不限于工业标准体系结构(Industry StandardArchitecture；以下简称：ISA)总线，微通道体系结构(Micro Channel Architecture；以下简称：MAC)总线，增强型ISA总线、视频电子标准协会(Video Electronics StandardsAssociation；以下简称：VESA)局域总线以及外围组件互连(Peripheral ComponentInterconnection；以下简称：PCI)总线。
电子设备典型地包括多种计算机系统可读介质。这些介质可以是任何能够被电子设备访问的可用介质，包括易失性和非易失性介质，可移动的和不可移动的介质。
存储器430可以包括易失性存储器形式的计算机系统可读介质，例如随机存取存储器(Random Access Memory；以下简称：RAM)和/或高速缓存存储器。电子设备可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。存储器430可以包括至少一个程序产品，该程序产品具有一组(例如至少一个)程序模块，这些程序模块被配置以执行本发明实施例图2所示实施例涉及的定位方法。
具有一组(至少一个)程序模块的程序/实用工具，可以存储在存储器430中，这样的程序模块包括——但不限于——操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块通常执行本发明实施例图2所示实施例涉及的定位方法。
处理器410通过运行存储在存储器430中的程序，从而执行各种功能应用以及数据处理，例如实现本说明书图2所示实施例涉及的定位方法。
具体实现中，本申请还提供一种计算机存储介质，其中，该计算机存储介质可存储有程序，该程序执行时可包括本申请提供的各实施例中的部分或全部步骤。所述的存储介质可为磁碟、光盘、只读存储记忆体(英文：read-only memory，简称：ROM)或随机存储记忆体(英文：random access memory，简称：RAM)等。
具体实现中，本发明实施例还提供了一种计算机程序产品，所述计算机程序产品包含可执行指令，当所述可执行指令在计算机上执行时，使得计算机执行上述方法实施例中的部分或全部步骤。
本发明实施例中，“至少一个”是指一个或者多个，“多个”是指两个或两个以上。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示单独存在A、同时存在A和B、单独存在B的情况。其中A，B可以是单数或者复数。字符“/”一般表示前后关联对象是一种“或”的关系。“以下至少一项”及其类似表达，是指的这些项中的任意组合，包括单项或复数项的任意组合。例如，a，b和c中的至少一项可以表示：a,b,c,a-b,a-c,b-c,或a-b-c，其中a,b,c可以是单个，也可以是多个。
本领域普通技术人员可以意识到，本文中公开的实施例中描述的各单元及算法步骤，能够以电子硬件、计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本发明所提供的几个实施例中，任一功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(read-only memory，简称ROM)、随机存取存储器(random access memory，简称RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本发明的具体实施方式，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。本发明的保护范围应以所述权利要求的保护范围为准。

Claims (10)

1.一种定位方法，其特征在于，包括：
根据视觉惯性里程计VIO算法估计机器人的局部定位位姿；
根据地图特征点匹配算法估计所述机器人的全局定位位姿；
确定所述全局定位位姿是否与所述局部定位位姿估计一致；
若估计一致，则基于融合状态向量执行所述机器人的位姿融合更新，其中，所述融合状态向量包括VIO局部定位变量和全局定位变量，所述局部定位变量包括所述机器人的速度和传感器偏置，所述速度和所述传感器偏置保持施密特状态；
若估计不一致，则确定所述全局定位位姿的观测误差，根据所述观测误差对所述融合状态向量进行坐标系转换，根据所述坐标系转换后的融合状态向量执行所述机器人的位姿更新。
2.根据权利要求1所述的方法，其特征在于，所述全局定位位姿与所述局部定位位姿一致的条件，包括以下一项或多项的组合：
求解所述全局定位位姿时的内点指标参数满足设定条件；
所述全局定位位姿与所述局部定位位姿之间的误差值小于第一阈值；
所述机器人的相机拍摄到的用于确定所述局部定位位姿的多帧图像之间的相关系数大于第二阈值。
3.根据权利要求2所述的方法，其特征在于，所述全局定位位姿与所述局部定位位姿之间的误差值包括：位置误差值和姿态角误差值；
所述内点指标参数包括：内点个数、内点率和内点的重投影平均误差。
4.根据权利要求1所述的方法，其特征在于，所述VIO局部定位变量还包括：所述机器人的旋转矩阵、位置；
所述全局定位变量包括：所述机器人在全局地图的位置和姿态角。
5.根据权利要求1所述的方法，其特征在于，所述基于融合状态向量执行所述机器人的位姿融合更新，包括：
当在所述机器人的相机拍摄的图像帧识别到新的地图特征点或者已识别到的地图特征点消失时，使所述速度和所述传感器偏置保持施密特状态并基于所述融合状态向量执行所述机器人的位姿融合更新。
6.根据权利要求1所述的方法，其特征在于，所述方法还包括：
确定卡尔曼增益影响因子；
基于所述卡尔曼增益影响因子计算卡尔曼增益，所述卡尔曼增益用于所述机器人的位姿融合更新，所述卡尔曼增益影响因子用于在所述位姿融合更新过程中约束融合状态向量包含的横摇角和俯仰角更新前后保持不变。
7.根据权利要求6所述的方法，其特征在于，所述确定所述全局定位位姿的观测误差，包括：
根据所述卡尔曼增益计算所述全局定位位姿的观测误差。
8.根据权利要求7所述的方法，其特征在于，所述根据所述观测误差对所述融合状态向量进行坐标系转换，包括：
根据所述观测误差对所述局部定位位姿进行位姿更新，以得到更新后的局部定位位姿；
根据所述更新后的局部定位位姿确定世界坐标系变化矩阵；
根据所述世界坐标系变化矩阵对所述融合状态向量和所述融合状态向量对应的协方差矩阵进行坐标系转换。
9.一种电子设备，其特征在于，包括：
至少一个处理器；以及
与所述处理器通信连接的至少一个存储器，其中：
所述存储器存储有可被所述处理器执行的程序指令，所述处理器调用所述程序指令能够执行如权利要求1至8任一所述的方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机可读存储介质所在设备执行权利要求1至8任一所述的方法。
CN202210979139.XA 2022-08-16 2022-08-16 定位方法和设备 Pending CN117629204A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202210979139.XA CN117629204A (zh) 2022-08-16 2022-08-16 定位方法和设备
PCT/CN2023/109080 WO2024037295A1 (zh) 2022-08-16 2023-07-25 定位

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210979139.XA CN117629204A (zh) 2022-08-16 2022-08-16 定位方法和设备

Publications (1)

Publication Number Publication Date
CN117629204A true CN117629204A (zh) 2024-03-01

Family

ID=89940686

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210979139.XA Pending CN117629204A (zh) 2022-08-16 2022-08-16 定位方法和设备

Country Status (2)

Country Link
CN (1) CN117629204A (zh)
WO (1) WO2024037295A1 (zh)

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10371530B2 (en) * 2017-01-04 2019-08-06 Qualcomm Incorporated Systems and methods for using a global positioning system velocity in visual-inertial odometry
KR102226846B1 (ko) * 2019-09-02 2021-03-11 경북대학교 산학협력단 Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템
CN110706279B (zh) * 2019-09-27 2021-09-07 清华大学 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111136660B (zh) * 2020-02-19 2021-08-03 清华大学深圳国际研究生院 机器人位姿定位方法及系统
CN112734852B (zh) * 2021-03-31 2021-06-29 浙江欣奕华智能科技有限公司 一种机器人建图方法、装置及计算设备
CN114001733B (zh) * 2021-10-28 2024-03-15 浙江大学 一种基于地图的一致性高效视觉惯性定位算法

Also Published As

Publication number Publication date
WO2024037295A1 (zh) 2024-02-22

Similar Documents

Publication Publication Date Title
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
CN110517324B (zh) 基于变分贝叶斯自适应算法的双目vio实现方法
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
WO2020221307A1 (zh) 一种运动物体的追踪方法和装置
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
US10757333B2 (en) Method for determining bias in an inertial measurement unit of an image acquisition device
CN112113582A (zh) 时间同步处理方法、电子设备及存储介质
EP4220086A1 (en) Combined navigation system initialization method and apparatus, medium, and electronic device
CN108592917B (zh) 一种基于失准角的卡尔曼滤波姿态估计方法
CN112116651A (zh) 一种基于无人机单目视觉的地面目标定位方法和系统
CN114013449A (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN114136315A (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
CN114419109B (zh) 一种基于视觉和气压信息融合的飞行器定位方法
CN115540860A (zh) 一种多传感器融合位姿估计算法
CN115855048A (zh) 一种多传感器融合位姿估计方法
CN113218389B (zh) 一种车辆定位方法、装置、存储介质及计算机程序产品
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN112985391B (zh) 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN114812601A (zh) 视觉惯性里程计的状态估计方法、装置、电子设备
CN109917644B (zh) 一种提高视觉惯导系统鲁棒性的方法、装置和机器人设备
CN113327270A (zh) 视觉惯导方法、装置、设备及计算机可读存储介质
CN117629204A (zh) 定位方法和设备
CN112902957B (zh) 一种弹载平台导航方法及系统
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN113503872A (zh) 一种基于相机与消费级imu融合的低速无人车定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication