一种机器人的定位方法及其定位装置与流程

[0001]
本发明涉及机器人技术领域，尤指一种机器人的定位方法及其定位装置。


背景技术：

[0002]
随着对制造灵活性的需求增加、产品周期缩短和加快、人力成本上升、以及人类安全趋势的需求增加，全球移动机器人的市场正在快速发展，技术需求越来越高，例如，自主导引车（automated guided vehicles agv）已广泛应用于自动化仓库、工厂物料传输系统、物流拣配系统、柔性装配系统和其他智能运输站点。相对于基于磁条、反光板、二维码等agv，自主移动机器人（autonomous mobile robot, amr）的技术难度更高和使用场景更广，amr导航的核心技术即slam（simultaneous localization and mapping），也即使得机器人知道环境地图，以及知道自己实时位置，这是全程不需要人干预、高效智能移动到达目标的基础。
[0003]
目前，单传感器slam的定位精度不高，在退化环境中容易发生丢失，导致无法对机器人的位置进行定位，从而导致机器人无法正常有效地工作。
[0004]
基于此，如何提高机器人的定位的精度，是本领域技术人员亟待解决的技术问题。


技术实现要素：

[0005]
本发明实施例提供了一种机器人的定位方法及其定位装置，用以提高机器人的定位的精度。
[0006]
第一方面，本发明实施例提供了一种机器人的定位方法，包括：根据当前采集到的惯性测量单元imu数据、轮速里程计数据和图像数据，对确定出的当前关键帧进行滑窗优化处理，确定所述当前关键帧的位姿信息；其中，所述图像数据包括：图像中特征点的匹配信息、以及在所述图像中具有预设标签时所述预设标签的索引和坐标信息；根据当前处于滑窗中多个关键帧的位姿信息、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧，对所述当前关键帧进行位姿图优化处理，得到处理后的所述当前关键帧的位姿信息；根据处理后的所述当前关键帧的位姿信息、以及在确定出所述当前关键帧之后最近时刻采集到的一帧图像的位姿信息，确定所述机器人的位姿。
[0007]
第二方面，本发明实施例提供了一种机器人的定位装置，包括：第一单元，用于根据当前采集到的惯性测量单元imu数据、轮速里程计数据和图像数据，对确定出的当前关键帧进行滑窗优化处理，确定所述当前关键帧的位姿信息；其中，所述图像数据包括：图像中特征点的匹配信息、以及在所述图像中具有预设标签时所述预设标签的索引和坐标信息；第二单元，用于根据当前处于滑窗中多个关键帧的位姿信息、以及确定出的与所述当前关键帧处于闭环匹配状态的图像闭环关键帧，对所述当前关键帧进行位姿图优化处理，
得到处理后的所述当前关键帧的位姿信息；第三单元，用于根据处理后的所述当前关键帧的位姿信息、以及在确定出所述当前关键帧之后最近时刻采集到的一帧图像的位姿信息，确定所述机器人的位姿。
[0008]
本发明有益效果如下：本发明实施例提供的一种机器人的定位方法及其定位装置，在确定机器人的位姿时，通过引入预设标签，再将预设标签技术与imu技术、轮速里程计技术、以及视觉技术相结合，基于滑窗优化处理，实现了基于紧耦合的标签视觉惯性轮速里程计的融合定位；并且，通过将预设标签的坐标信息融入至滑窗优化处理过程中，可以大幅提高全局定位精度，从而实现机器人精准有效地定位。
附图说明
[0009]
图1为本发明实施例中提供的一种机器人的定位方法的流程图；图2为本发明实施例中提供的另一种机器人的定位方法的流程图；图3为本发明实施例中提供的一种预设标签的示意图；图4为本发明实施例中提供的一种机器人的定位装置的结构示意图；图5为本发明实施例中提供的一种机器人的结构示意图。
具体实施方式
[0010]
下面将结合附图，对本发明实施例提供的一种机器人的定位方法及其定位装置的具体实施方式进行详细地说明。需要说明的是，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0011]
说明一点，在本申请文件中，预设标签和tag表示相同的含义，二者可以进行互换使用。
[0012]
本发明实施例提供了一种机器人的定位方法，如图1所示，可以包括：s101、根据当前采集到的惯性测量单元imu数据、轮速里程计数据和图像数据，对确定出的当前关键帧进行滑窗优化处理，确定当前关键帧的位姿信息；其中，图像数据包括：图像中特征点的匹配信息、以及在图像中具有预设标签时预设标签的索引和坐标信息；其中，在确定具有预设标签的图像中预设标签的索引和坐标信息时，可以基于预先设置的包括预设标签信息的地图确定。
[0013]
s102、根据当前处于滑窗中多个关键帧的位姿信息、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧，对当前关键帧进行位姿图优化处理，得到处理后的当前关键帧的位姿信息；s103、根据处理后的当前关键帧的位姿信息、以及在确定出当前关键帧之后最近时刻采集到的一帧图像的位姿信息，确定机器人的位姿。
[0014]
如此，在确定机器人的位姿时，通过引入预设标签，再将预设标签技术与imu技术、轮速里程计技术、以及视觉技术相结合，基于滑窗优化处理，实现了基于紧耦合的标签视觉惯性轮速里程计的融合定位；并且，通过将预设标签的坐标信息融入至滑窗优化处理过程中，可以大幅提高全局定位精度，从而实现机器人精准有效地定位。
[0015]
可选地，在本发明实施例中，预设标签的坐标信息包括：在用于表示预设标签在所在图像中位置的角点具有多个时，各角点的坐标信息。
[0016]
如此，可以将预设标签中观测到的角点信息加入至滑窗优化处理过程中，可以大幅提高全局定位精度，从而实现机器人精准有效地定位。
[0017]
可选地，在本发明实施例中，预设标签的坐标信息还包括：在用于形成预设标签的图案的内点具有多个时，各内点的坐标信息。
[0018]
如此，不仅将角点信息加入至滑窗优化处理过程中，还可以将内点信息加入至滑窗优化处理过程中，能够充分利用预设标签的信息，进而能够使得预设标签可以提供更多的视觉重投影误差的约束，从而更大幅度地全局定位精度和定位结果的稳定性。
[0019]
可选地，在本发明实施例中，根据当前采集到的imu数据、轮速里程计数据、图像数据，对确定出的当前关键帧进行滑窗优化处理，具体包括：采用如下公式，对确定出的当前关键帧进行滑窗优化处理：χ
*
=argmin（res1+res2+res3+res4+res5）=argmin（res1+res2+res3+res4+res5）=argmin（res1+res2+res3+res4+res5）=argmin（res1+res2+res3+res4+res5）=argmin（res1+res2+res3+res4+res5）其中，表示被边缘化的图像帧的先验残差，表示imu数据的预积分约束，b表示具备imu数据的预积分约束的图像帧的列表，表示imu数据的预积分状态增量对应的协方差矩阵，表示imu数据的预积分结果，表示滑窗中第j个特征点在第l个图像帧中的重投影残差，表示第j个特征点在第l个数据帧中的观测值，表示视觉观测的协方差，c表示滑窗中所有图像帧的列表，表示轮速里程计数据的预积分残差，e表示具备轮速里程计预积分约束的图像帧的列表，表示轮速里程计预积分对应的协方差矩阵，表示轮速里程计数据的预积分结果，表示预设标签的重投影误差，表示预设标签的观测噪声的协方差，k表示第k个数据帧，t
表示观测到的第t个预设标签，表示预设标签的坐标信息，t表示观测到的预设标签的列表，χ
*
表示滑窗优化后的最优状态，argmin表示使目标误差函数（目标误差函数即res1+res2+res3+res4+res5）取最小值时的变量值。
[0020]
其中，滑窗优化的状态可以包括：机器人的位姿、机器人的速度、imu偏置、特征点深度、以及外参等信息。
[0021]
如此，可以实现对当前关键帧进行滑窗优化处理，确定出当前关键帧的位姿信息，从而有利于后续对该位姿信息进行优化，实现机器人定位。
[0022]
可选地，在本发明实施例中，预设标签的重投影误差的确定方法，包括：根据当前滑窗的优化状态，计算预设标签包括的全部标签点在所在图像坐标系中的二维坐标的预测值；其中，全部标签点包括：用于表示预设标签在所在图像中位置的多个角点，和/或用于形成预设标签的图案的多个内点；计算观测到的标签点在所在图像坐标系中的二维坐标、以及该标签点的二维坐标的预测值之间的差值；根据计算后得到的各标签点的差值，确定预设标签的重投影误差。
[0023]
如此，可以确定出预设标签的重投影误差，进而有利于进行滑窗优化处理，从而确定出当前关键帧的位姿信息。
[0024]
具体地，在本发明实施例中，在预设标签占据所在图像中的面积比例大于预设阈值时，全部标签点包括各角点和内点。
[0025]
具体地，在计算预设标签包括的各标签点在所在图像坐标系中的二维坐标的预测值时，可以采用以下公式确定：其中，c表示图像采集器（例如但不限于为相机）坐标系，i表示imu坐标系，w表示世界坐标系，p表示第p个标签点，表示imu坐标系相对于图像采集器坐标系的坐标转换变量，表示世界坐标系相对于imu坐标系的坐标转换变量，r表示旋转矩阵，t表示平移向量。
[0026]
并且，在计算观测到的标签点在所在图像坐标系中的二维坐标、以及计算出的该标签点的二维坐标的预测值之间的差值时，可以采用以下公式计算：其中，e表示计算出的差值，π
c
表示将某个标签点从图像采集器坐标系（也可以称之为相机坐标系）中的三维坐标转换至图像坐标系中的二维坐标的映射函数，p
c
表示根据当前滑窗的优化状态确定出的该标签点在相机坐标系下的三维坐标，表示观测到的该标签点在所在图像坐标系中的二维坐标。
[0027]
可选地，在本发明实施例中，对确定出的当前关键帧进行滑窗优化处理，具体包括：若判断出当前处于滑窗中所有关键帧满足预设的优化状态固定切换条件时，将所有关键帧中采集时间最早的关键帧定义为最老关键帧，针对所有关键帧中除最老关键帧之外的
其他关键帧和当前关键帧，进行滑窗优化处理；其中，优化状态固定切换条件包括：所有关键帧对应图像中均不包括预设标签；或，所有关键帧对应图像中至少一张图像包括预设标签，且所述图像中包括的预设标签在所述滑窗中均约束无效。
[0028]
例如，如果当前滑窗中有10帧关键帧，且这10帧关键帧对应的图像有10张图像，其中这10张图像有3张图像包括预设标签，且这3张图像中的预设标签在滑窗中的约束均无效，即可以确认满足优化状态固定切换条件。
[0029]
由于在进行滑窗优化处理时，存在全局状态不可观的特点，且预设标签的观测约束属于全局约束的性质，所以在进行滑窗优化处理时引入了固定切换策略，从而避免滑窗优化过程中出现偏移，提高确定出的位姿信息的准确度。
[0030]
可选地，在本发明实施例中，在对确定出的当前关键帧进行滑窗优化处理之前，还包括：确定预设标签在滑窗中的约束有效。
[0031]
如此，通过在进行滑窗优化处理之前确定出预设标签的约束有效，可以避免因预设标签误检测或预设标签中标签点的检测误差而造成的滑窗优化精度和稳定性降低，从而提高确定出的位姿信息的准确度，提高机器人的定位精度。
[0032]
可选地，在本发明实施例中，判断预设标签在滑窗中的约束是否有效，具体包括：确定预设标签在当前处于滑窗中的各关键帧对应图像中的观测次数；其中，观测次数为：根据当前处于滑窗中包括预设标签的图像对应的关键帧的帧数、以及包括预设标签的图像包括的各目图像中观测到预设标签的次数确定；判断观测次数是否大于第一预设值；若是，则确定预设标签在滑窗中的约束有效；若否，则确定预设标签在滑窗中的约束无效。
[0033]
其中，关于观测次数如何确定的过程，可参见后续内容中的介绍，在此不再详述。
[0034]
同时，对于第一预设值的设置，可以根据实际需要进行设置，在此并不限定。
[0035]
如此，可以确定出预设标签在滑窗中的约束是否有效，进而避免因预设标签误检测或预设标签中标签点的检测误差而造成的滑窗优化精度和稳定性降低，从而提高确定出的位姿信息的准确度，提高机器人的定位精度。
[0036]
可选地，在本发明实施例中，在对确定出的当前关键帧进行滑窗优化处理之前，还包括：在机器人的初始化阶段，按照预设规则，确定滑窗中的m个数据帧的初始位姿信息；其中m为大于1的整数；对接收到的第m+1个数据帧和滑窗中的m个数据帧进行滑窗优化处理，得到m+1个数据帧优化后的位姿信息；预设规则包括：采集到的首个数据帧的初始位姿信息为：根据首个数据帧对应图像中具有预设标签的坐标信息和索引确定；m+1个数据帧中除首个数据帧之外的其他任一数据帧的初始位姿信息为：在该数据帧
对应图像中具有预设标签时，根据预设标签的坐标信息和索引确定；或，根据该数据帧相对于首个数据帧的imu预积分数据确定。
[0037]
其中，机器人的初始化阶段可以理解为：机器人刚开机，各传感器（包括imu、轮速里程计和图像采集器）开始采集数据，且采集到的数据的帧数还没有达到滑窗的最大容量（如最大容量为m帧）的阶段。
[0038]
如此，可以提高在初始化阶段确定出的各数据帧的初始位姿信息的精度，避免后期滑窗优化处理时出现误差较大的情况，进而避免出现局部收敛造成滑窗优化结果不稳定，有利于提高当前关键帧的位姿信息的准确度，从而提高机器人的定位精度。
[0039]
可选地，在本发明实施例中，根据处理后的当前关键帧的位姿信息、以及在确定出当前关键帧之后最近时刻采集到的一帧图像的位姿信息，确定机器人的位姿，具体包括：确定当前关键帧在进行位姿图优化处理前后产生的位姿增量；将位姿增量与在确定出当前关键帧之后最近时刻采集到的一帧图像的位姿信息进行叠加，确定机器人的位姿。
[0040]
如此，可以确定出机器人的位姿，进而实现机器人的定位，为机器人的移动和控制提供有效地数据参考。
[0041]
可选地，在本发明实施例中，对确定出的当前关键帧进行滑窗优化处理，确定当前关键帧的位姿信息，具体包括：对确定出的当前关键帧进行滑窗优化处理后，得到优化后的系统状态；其中，系统状态包括：imu、轮速里程计和图像采集器之间的外参；根据优化后的系统状态，确定出当前关键帧的位姿。
[0042]
需要说明的是，在本发明实施例中，在实现机器人定位时，是基于外参（即各传感器之间的外参）在线优化实现的机器人定位，且在上述内容提及的滑窗优化处理过程和位姿图优化过程中，在实现对位姿优化的同时，也均会对外参进行优化，以使得机器人定位的准确性更高更有效。
[0043]
在具体实施，在本发明实施例中，结合图2所示，上述定位方法可以包括三大部分，分别为：数据预处理部分、滑窗优化处理部分、以及闭环检测和优化部分，其中，数据预处理部分和滑窗优化处理部分可以看作是前端处理过程，闭环检测、优化部分和确定机器人位置可以看作是后端处理过程。
[0044]
下面对三大部分分别进行介绍。
[0045]
一、数据预处理部分。
[0046]
1、预设标签。
[0047]
1.1、预设标签可以理解为一种二进制图像编码标记，其可以采用一个宽的黑边和一个内部的二进制矩阵组成，内部的矩阵（或者可以理解为用于构成预设标签的图像）排布决定了预设标签的编号（或称之为索引，可以简写为id），且黑边有利于快速检测到预设标签，二进制矩阵可以用于确定预设标签的id。
[0048]
当然，二进制图像编码标记还可以为不具有黑边且仅具有二进制矩阵的标记，在实际情况中，可以根据实际需要设置预设标签，所以对于预设标签的具体实现形式在此并不限定。
[0049]
在实际应用时，在机器人所在环境的地面上，可以按照某种间距和密度，在地面
和/或墙面等三维空间中的其他位置之上贴预设标签。
[0050]
1.2、利用图像采集器（例如但不限于相机）可以对预设标签进行拍照，得到具有预设标签的图像；其中，得到的图像可以包括左目图像和右目图像，且二者具有相同的时间戳信息，二者组合可以作为一帧图像的信息；说明一点，可选地，在本发明实施例中，相机可以为单目相机、多目相机。
[0051]
对于单目相机而言，具有体积小、制作成本低等优势；对于多目相机而言，可以通过单帧图像，确定出图像中特征点的尺度，并且随着相机目数的增加，确定出的图像尺度也就越准确；此外，多目相机还可以拍到更多的预设标签和特征点，有利于提高后续定位结果的准确度和稳定性。
[0052]
其中，图中特征点的尺度可以理解为：图像中的某个特征点距离相机的远近程度或该特征点三维坐标的同比放大系数。
[0053]
对于本发明实施例提供的上述定位方法而言，不仅可以兼容上述两类相机，还可以弥补单目相机中尺度模糊/漂移、以及多目相机难以获取相机之间的外参的问题，从而实现尺度准确可靠，实时在线估计出各相机之间的外参。
[0054]
下面的内容均是以双目相机为例进行说明。
[0055]
1.3、利用预先设置的预设标签检测算法，可以从具有预设标签的图像中确定出预设标签所在的候选区域，并识别出预设标签的id和坐标信息，如图2中所示的“预设标签的识别”和“标签点提取”。
[0056]
其中，预设标签一般可以但不限于为四边形，所以预设标签的坐标信息可以包括：四边形的四个角点的二维坐标、用于形成预设标签的图案的多个内点的二维坐标、预设标签的在地图坐标系下的6dof（即6自由度）的位姿信息等。
[0057]
例如，如图3所示，图中示出了预设标签，但在实际情况中，预设标签的形状和图案并不限于图3所示，此处只是举例说明而已；其中，标记为k1的四个圈均表示角点，标记为k2的圈表示其中一个内点。
[0058]
并且，可选地，在利用预设标签检测算法确定预设的id和坐标信息时，具体过程可以包括：步骤1.3.1、确定预设标签所在图像中的候选区域；其中，该步骤主要用于在整个图像区域中找到那些可能是预设标签的专有轮廓，例如但不限于方形轮廓。
[0059]
具体地，可以利用自适应性阈值算法分割预设标签，然后从阈值化的图像中提取外形轮廓，并且舍弃那些非凸多边形的轮廓，以及那些不是方形的轮廓。其中，在提取外形轮廓时，可以使用滤波操作来剔除那些过小或者过大的轮廓、过于相近的凸多边形等。
[0060]
步骤1.3.2、确定候选区域中预设标签的类型；其中，在检测得到预设标签所在的候选区域后，可以通过分析其内部图案来确定该候选区域中是否存在指定类型的预设标签，具体可以包括以下三步：第一步，对图像进行投影变换，得到对应规范的形态（例如但不限于正视图）；第二步，对规范后的图像采用ossu阈值化算法，分离图像中的白色位和黑色位；其中，在此第二步中，规范后的图像可以根据预设标签的大小和边界划分出多个不同的格子，再通过统计落在每个格子中黑白像素的数目，确定该格子为黑色位还是白色位。
[0061]
第三步，根据分离出的白色位和黑色位，确定该预设标签在指定字典中对应的类型。
[0062]
其中，指定字典中可以存储有标志位（包括白色位和黑色位）、预设标签的类型以及预设标签的id之间的对应关系。
[0063]
步骤1.3.3、确定预设标签的id和坐标信息。
[0064]
其中，对于预设标签的id而言：可以根据指定字典中存储的对应关系进行确定；对于预设标签的坐标信息而言：针对每一个预设标签而言，根据预设标签的在所在图像中的边界，可以按指定顺序确定出各角点的二维坐标；根据自适应阈值算法后调整后的图像，判断是否需要输出内点的二维坐标；如果需要，则可以根据该预设标签的内点的排布信息，按指定顺序输出各内点的二维坐标。
[0065]
具体地，在本发明实施例中，角点和内点在世界坐标系中的三维坐标的确定方法，可以包括：根据预设的标签字典中包括的点与所在图像之间的坐标对应关系，确定角点和内点在所在图像（即具有预设标签的二维图像）坐标系中的二维坐标；确定角点和内点在预设标签三维坐标系中的三维坐标；根据预设的标签地图中包括的预设标签的id、以及预设标签在世界坐标系中的位置信息和角度信息，确定预设标签三维坐标系和世界坐标系之间的转换关系（即6dof的位姿信息）；根据预设标签三维坐标系和世界坐标系之间的转换关系，确定角点和内点在世界坐标系中的三维坐标。
[0066]
具体地，在根据预设标签三维坐标系和世界坐标系之间的转换关系，确定角点和内点在世界坐标系中的三维坐标时，可以采用以下公式执行：其中，w表示世界坐标系，p表示角点或内点，tag表示预设标签三维坐标系，p
tag
表示角点或内点在预设标签三维坐标系中的位置，p
w
表示角点或内点在世界坐标系中的位置，表示预设标签三维坐标系相对于世界坐标系的旋转矩阵，表示预设标签三维坐标系相对于世界坐标系的平移向量。
[0067]
2、图像中特征点处理。
[0068]
说明一点，图像中特征点处理时，针对的图像可以与上述“预设标签”中涉及到的预设标签所在图像可以为同一张图像，只是在上述“预设标签”部分，针对的是与图像中预设标签相关的信息，而在此部分内容中，针对的是特征点的信息。
[0069]
并且，具有预设标签的图像中的某个点，可以既为上述预设标签内容中提及的角点或内点，也可以同时为下述内容中提及的待匹配的特征点。
[0070]
可选地，在对图像中的特征点进行处理时，可以为：根据采集到的每一帧图像包括的图像1（即在采用双目相机时其中的左目相机（也即相机1）采集到的左目图像）和图像2（即在采用双目相机时其中的右目相机（也即相机2）采集到的右目图像）中特征点的匹配情
况、以及与上一帧特征点的匹配情况，确定每一帧图像中的特征点匹配信息。
[0071]
其中，特征点匹配信息可以包括：特征点的编号、特征点的坐标信息。
[0072]
在确定每一帧图像中的特征点匹配信息时，可以采用如下步骤：步骤2.1、对每一帧图像中的左目图像（如图2中的利用“相机1”采集到的图像）和相邻的上一帧图像中的左目图像进行特征点匹配处理（如图2中的“特征点提取”和“特征点匹配”）、以及对每一帧图像中的右目图像和相邻的上一帧图像中的右目图像（如图2中的利用“相机2”采集到的图像）进行特征点匹配处理（如图2中的“特征点提取”和“特征点匹配”），得到每一帧图像中左目图像中的特征点、以及每一帧图像中右目图像中的特征点；步骤2.2、对左目图像或右目图像进行特征点补充处理；说明一点，执行该步骤2.2的目的在于：以左目图像为例，对于第i帧图像而言，在执行上述步骤2.1之前，假设第i帧图像中左目图像中的角点具有150个，在执行上述步骤2.1之后，假设确定出的第i帧图像中左目图像中的特征点为100个；那么，对于第i+1帧图像而言，如果将具有100个特征点的第i帧图像中左目图像与第i+1帧图像中左目图像进行特征点匹配处理时，在执行上述步骤2.1之后，确定出的第i+1帧图像中左目图像中的特征点的数量很可能会少于100个，假设为50个时，那么对于第i+2帧图像而言，经过特征点匹配处理后，确定出的第i+2帧图像中左目图像中的特征点的数量会更少，即少于50个；如此循环下去，在后续获取到的图像中，确定出的特征点数量越少，随着获取的图像的增加，确定出的特征点数量可能会为零，显然，这是与实际不相符的。
[0073]
因此，为了避免出现上述情况，需要执行步骤2.2，即对特征点进行补充处理，也即，在确定出的第i帧图像中左目图像中的特征点为100个的基础上，可以对执行完上述步骤2.1的第i帧图像中左目图像中的特征点进行补充，假设可以补充50个特征点，即补充到150个，以保证后续获取到的图像中，可以确定出足够数量的特征点。
[0074]
其中，补充的特征点可以从执行完上述步骤2.1的第i帧图像中左目图像中重新提取，且提取的过程，可以采用现有技术，只要能够提取出特征点即可，对于提取的过程在此并不限定。
[0075]
步骤2.3、如图2所示，对执行完上述步骤2.1和步骤2.2的每一帧图像的左目图像和右目图像进行特征点匹配处理（如图2中的“特征匹配”），得到每一帧图像的左目图像和右目图像中特征点的匹配关系；步骤2.4、根据得到的匹配关系，确定每一帧图像中特征点的匹配信息。
[0076]
说明一点，上述步骤2.1至步骤2.4的匹配过程，可以理解为采用描述子匹配算法来实现，但在实际情况中，并不限于此种算法，还可以采用其他算法来实现匹配过程，例如但不限于：光流跟踪匹配算法。
[0077]
3、对imu数据的处理。
[0078]
在此内容中，主要目的用于：基于imu数据，确定采集到的连续两帧图像之间的状态增量信息。
[0079]
其中，状态增量信息可以包括：位置增量信息、角度增量信息和速度增量信息。
[0080]
说明一点，状态增量信息表示的是两帧图像之间的关系，并不是两帧imu数据之间的关系。
[0081]
可选地，在确定采集到的连续两帧图像之间的状态增量信息时，具体可以采用以下步骤：步骤3.1、确定获取到的连续两帧图像之间的惯性测量单元（inertial measurement unit, imu）数据（如图2中的“惯性测量单元”），并存储至预设的缓存列表中；步骤3.2、将imu数据的积分起始时刻和结束时刻与对应的一帧图像的时刻信息进行对齐处理；步骤3.3、根据中值法，将对齐处理后的imu数据进行迭代积分处理（如图2中所示的“预积分处理”），得到连续两帧图像之间的状态增量信息；步骤3.4、根据确定出的状态增量信息，确定对应的雅克比矩阵和方差矩阵。
[0082]
具体地，在确定采集到的连续两帧图像之间的状态增量信息时，所采用的过程并不限于上述步骤3.1至步骤3.4的具体实现过程，还可以为其他实现过程，在此并不限定。
[0083]
4、对轮速里程计数据的处理。
[0084]
在此内容中，主要目的用于：基于轮速里程计数据，确定采集到的连续两帧图像之间的状态增量信息。
[0085]
说明一点，状态增量信息表示的是两帧图像之间的关系，并不是两帧轮速里程计数据之间的关系。
[0086]
并且，可选地，在基于轮速里程计数据，确定采集到的连续两帧图像之间的状态增量信息时，具体可以采用以下步骤：步骤4.1、确定获取到的连续两帧图像之间的轮速里程计数据（如图2中的“轮速里程计”），并存储至预设的缓存列表中；步骤4.2、将轮速里程计数据的积分起始时刻和结束时刻与对应的一帧图像的时刻信息进行对齐处理；步骤4.3、根据中值法，将对齐处理后的轮速里程计数据进行迭代积分处理（如图2中所示的“预积分处理”），得到连续两帧图像之间的状态增量信息；步骤4.4、根据确定出的状态增量信息，确定对应的雅克比矩阵和方差矩阵。
[0087]
具体地，在确定采集到的连续两帧图像之间的状态增量信息时，所采用的过程并不限于上述步骤4.1至步骤4.4的具体实现过程，还可以为其他实现过程，在此并不限定。
[0088]
二、滑窗优化处理（如图2中所示的“滑窗优化”）部分。
[0089]
1、确定关键帧。
[0090]
在筛选关键帧时，可以具体依据帧与帧之间的平均视差进行确定。
[0091]
在筛选关键帧时，需要保证在后续滑窗优化处理过程中具有快速性和鲁棒性等特点，以提高滑窗处理结果的有效性。
[0092]
2、进行ba优化处理，确定当前关键帧的位姿信息。
[0093]
滑窗的主要目的为：在多个关键帧中，采用ba优化处理，优化关键帧的位姿信息、速度、imu偏置、外参（也可以称之为转换关系）和特征点逆深度等信息。并且，在滑窗中，每个关键帧与其邻域的关键帧都包含足够的视差。
[0094]
可选地，在滑窗中，各关键帧对应图像、以及各关键帧对应图像包含的各特征点的状态，也即滑窗需要优化的状态，可以表示为：
其中，表示第k个关键帧的位姿信息（k从1到n，其中n为当前滑窗中关键帧的数量），w表示世界坐标系，表示第k个关键帧在世界坐标系中的位移量，表示第k个关键帧在世界坐标系中的速度信息，表示第k个关键帧在世界坐标系中的角度信息（或者称之为旋转量信息），b
a
表示加速度的偏置，b
g
表示陀螺仪的偏置，表示图像采集器（如摄像头或称之为相机）的坐标系相对于imu坐标系之间的转换关系（即外参），λ
i
表示第i个特征点，m代表滑窗中观测到的特征点数量，表示左目摄像头（即上述内容提及的相机1）的坐标系相对于imu坐标系的位置转换关系，表示左目摄像头的坐标系相对于imu坐标系的相对旋转关系，表示右目摄像头（即上述内容提及的相机2）的坐标系相对于imu坐标系的位置转换关系，表示右目摄像头的坐标系相对于imu坐标系的相对旋转关系，表示轮速里程计坐标系相对于imu坐标系之间的外参，表示该外参的位置分量，表示该外参的旋转分量。
[0095]
可选地，在进行ba优化处理时，具体可以包括：采用如下公式，对确定出的当前关键帧进行滑窗优化处理：χ
*
=argmin（res1+res2+res3+res4+res5）=argmin（res1+res2+res3+res4+res5）=argmin（res1+res2+res3+res4+res5）=argmin（res1+res2+res3+res4+res5）=argmin（res1+res2+res3+res4+res5）其中，表示被边缘化的图像帧的先验残差， 表示imu数据的预
积分约束，b表示具备imu数据的预积分约束的图像帧的列表，表示imu数据的预积分状态增量对应的协方差矩阵，表示imu数据的预积分结果，表示滑窗中第j个特征点在第l个图像帧中的重投影残差，表示第j个特征点在第l个数据帧中的观测值，表示视觉观测的协方差，c表示滑窗中所有图像帧的列表，表示轮速里程计数据的预积分残差，e表示具备轮速里程计预积分约束的图像帧的列表，表示轮速里程计预积分对应的协方差矩阵，表示轮速里程计数据的预积分结果，表示预设标签的重投影误差，表示预设标签的观测噪声的协方差，k表示第k个数据帧，t表示观测到的第t个预设标签，表示预设标签的坐标信息，t表示观测到的预设标签的列表，χ
*
表示滑窗优化后的最优状态，argmin表示使目标误差函数（目标误差函数即res1+res2+res3+res4+res5）取最小值时的变量值。
[0096]
并且，在上述公式中，大括号中的五项分别为：边缘化的先验残差约束、预设标签中角点和/或内点的重投影约束、轮速里程计测量约束、imu测量约束、以及视觉特征点重投影约束。其中，这五种约束均可以但不限于用马氏距离表示。
[0097]
具体地，先验信息约束可以具体表示：滑窗被边缘化的信息对现有滑窗状态的约束；预设标签中角点和/或内点重投影约束可以具体表示：滑窗中各帧左右目观测到的各预设标签中所有角点和/或内点的三维坐标向二维坐标投影时产生的误差约束；轮速里程计测量约束可以具体表示：滑窗中相邻两帧的轮速里程计预积分的测量约束；imu测量约束可以具体表示：滑窗中相邻两帧的imu预积分的测量约束；视觉特征点的投影约束可以具体表示：滑窗中各帧左右目观测到的视觉特征点重投影约束，例如：针对滑窗某个关键帧对应图像中的第k个特征点，在从第一次观看到第k个特征点的第i个相机坐标系转换至当前的第j个相机坐标系时，确定第k个特征点在当前的第j个相机坐标系中的二维坐标，进而确定出该二维坐标与第k个特征点在第j个相机坐标系下观测到的二维坐标之间的误差，将该误差确定为第k个特征点的重投影误差（还可以称之为第k个特征点的重投影约束）。
[0098]
说明一点，在构建预设标签重投影约束时，可以仅采用角点信息进行构建；当然，为了尽可能地利用预设标签的信息，所以还可以采用角点和内点构建预设标签重投影误差。
[0099]
其中，为了确保内点的提取质量，可以根据tag占据图像像素面积的比例来确定。
[0100]
例如，图像的像素大小为n
×
m，tag占据的像素大小i
×
j，那么只有像素面积比值（即前述内容提及的预设标签占据所在图像中的面积比例）大于某个阈值时才可以利用内点构建预设标签重投影误差；
也即：在(i
×
j)/(n
×
m)>n时，n表示阈值（例如但不限于0.02）。
[0101]
并且，阈值n的大小与不同类型的tag包括的内点的数量成反比。
[0102]
由于内点的数量可以明显大于4个角点的数量，实验表明加入内点后，可以增加预设标签重投影约束的个数，从而可以明显提高位姿估计的精度和稳定性。
[0103]
此外，通过内点是否提取的判断准则，可以有效地避免距离tag过远（此时tag观测不清晰/模糊）时内点过于密集而导致内点提取效果不佳带来的不利影响。
[0104]
具体地，预设标签重投影误差对应的雅克比矩阵（即jacobian矩阵）可以包括有多个，具体的确定方法可以包括以下内容：根据链式求导法则可知：tag（即预设标签）重投影误差对所在图像帧位置的jacobian矩阵（即）可以为：其中，表示误差对处于相机坐标系中的角点或内点的三维坐标（即p
c
）的导数，表示p
c
对该图像帧位置扰动的导数，且该导数可以具体表示为:。
[0105]
tag重投影误差对该图像帧姿态的jacobian矩阵（即）可以为：其中，表示p
c
对该图像帧姿态扰动的导数，且该导数具体可以表示为：其中，上述公式中等号右边右上角的
×
表示向量的叉乘反对称矩阵。
[0106]
tag重投影误差对待优化外参位置的jacobian矩阵（即）可以为：其中，表示p
c
对待优化外参位置扰动的导数。
[0107]
tag重投影误差对待优化外参姿态的jacobian矩阵（即）可以为：其中，表示p
c
对待优化外参姿态扰动的导数。
[0108]
此外，为了充分利用预设标签的信息，提高ba优化的精度和可靠性，可选地，可以采用如下改进策略。
[0109]
2.1、滑窗中优化状态的固定切换策略（也即上述内容中提及的优化状态固定切换条件）主要分为以下两种情况：情况1：滑窗中不存在预设标签约束。
[0110]
其中，滑窗中不存在预设标签约束可以理解为：假设滑窗中包括10帧数据，那么在进行ba优化处理时，处理的是获取到的最新帧数据（记为第n帧数据）和滑窗中的10帧数据，那么，这11帧数据对应图像中均不存在预设标签，或者这11帧数据对应的图像中存在预设标签但预设标签约束无效。
[0111]
此时，在进行ba优化处理时，可以包括：方式1：仅对11帧中的后10帧的位姿进行优化。
[0112]
具体地，将11帧中的第1帧的位姿信息设置固定值，且保持第1帧与其他帧之间的约束关系，也即：ba优化处理可以理解为对11帧数据中的各变量进行优化，这些变量中不包括第1帧数据相关的变量。
[0113]
因此，经过此方式1进行优化后，除11帧中的第1帧的位姿信息保持不变之外，其他后10帧的位姿信息均会被优化（优化后的结果可以看作是优化过程中得到的位姿增量与原位姿的叠加）。
[0114]
方式2：对11帧的位姿进行正常的ba优化，并确定出第1帧的位姿增量，然后保持将1帧的位姿在优化前后保持不变，将后10帧的位姿分别减去第1帧的位姿增量，得到后10帧的位姿优化结果。
[0115]
情况2：滑窗中存在预设标签约束。
[0116]
其中，滑窗中存在预设标签约束可以理解为：假设滑窗中包括10帧数据，那么在进行ba优化处理时，处理的是获取到的最新帧数据（记为第n帧数据）和滑窗中的10帧数据，那么，这11帧数据对应的图像中至少一帧图像存在预设标签，且该预设标签的约束有效（具体解释可参见下述相关内容）。
[0117]
此时，在进行ba优化处理时，可以保持原ba优化过程不变。
[0118]
2.2、初始化阶段的初值选择策略其中，此部分内容涉及到的是定位装置的初始化阶段，下面举例说明。
[0119]
例如，假设滑窗中包括10帧数据。
[0120]
在定位装置刚开机且采集到第1帧数据时，可以采用两种方式确定第1帧数据的初始位姿，分别为：根据角点和/或内点的索引和坐标信息、以及预设的标签地图中三维坐标，按照pnp（pespective-n-point）算法，确定第1帧数据的初始位姿；或者，利用图像数据预处
理后得到的特征点的匹配信息，按照pnp算法，确定第1帧数据的初始位姿；在采集到第2帧数据至第11帧数据时，可以采用三种方式确定第2帧数据至第11帧数据的数据，分别为：利用角点和/或内点的索引和坐标信息、以及预设的标签地图中三维坐标，按照pnp算法，确定初始位姿；或者，利用图像数据预处理后得到的特征点的匹配信息，按照pnp算法，确定初始位姿；又或者，采用imu递推方法确定初始位姿；在确定出连续的11帧数据的初始位姿之后，可以对这11帧数据进行ba优化处理，得到第11帧优化后的位姿信息；在采集到第12帧数据以及之后的各帧数据时，均可以根据imu递推方法确定初始位姿。
[0121]
2.3、预设标签（即tag）约束有效性判断策略为了避免tag误检测或tag中的角点/内点的检测误差，进而避免造成ba优化时精度或稳定性降低，可选地，在本发明实施例提供的上述定位方法中，引入了tag约束有效性判断策略，也即：在确定出预设标签在滑窗中的约束有效时，根据当前确定出的惯性测量单元imu数据、轮速里程计数据、图像数据，对确定出的当前关键帧进行ba优化处理，且图像数据包括：图像中特征点的匹配信息、以及在图像中具有预设标签时预设标签的索引和坐标信息。
[0122]
也就是说，通过判断某一tag在滑窗中连续观测的稳定性，判断是否需要将该tag的约束加入ba优化处理过程中；具体地，如果约束无效，即连续观测的稳定性较差，则不需要将tag的约束加入ba优化处理过程中，使得在进行ba优化处理时无需考虑tag的重投影约束的影响；如果约束有效，即连续观测的稳定性较好，则需要将tag的约束加入ba优化处理过程中，使得在进行ba优化处理时需要考虑tag的重投影约束的影响。
[0123]
其中，在确定预设标签在滑窗中的约束是否有效时，依据的是：预设标签在当前处于滑窗中的各关键帧对应图像中的观测次数，其中，观测次数为：根据当前处于滑窗中包括预设标签的图像对应的关键帧的帧数、以及包括预设标签的图像包括的各目图像中观测到预设标签的次数确定；并且，观测次数与当前处于滑窗中包括预设标签的图像对应的关键帧的帧数、以及包括预设标签的图像包括的各目图像中观测到预设标签的次数成正比。
[0124]
假设当前处于滑窗中的关键帧有m帧，那么滑窗中各关键帧对应图像同样具有m帧，且每一帧图像可以包括左目图像和右目图像，那么该观测次数可以具体为：若将预设标签在m帧左目图像中的观测次数定义为第一观测次数，将预设标签在m帧右目图像中的观测次数定义为第二观测次数时：1）该观测次数为第一观测次数和第二观测次数之和；此时，如果第一观测次数和第二观测次数之和大于第一预设值时，则说明预设标签在滑窗中的约束有效。
[0125]
2）该观测次数为第一观测次数；此时，如果第一观测次数大于第一预设值时，则说明预设标签在滑窗中的约束有效。
[0126]
3）该观测次数为第二观测次数；此时，如果第二观测次数大于第一预设值时，则说明预设标签在滑窗中的约束有效。
[0127]
4）该观测次数为第一观测次数和第二观测次数中的较小值；
此时，如果第一观测次数和第二观测次数中的较小值大于第一预设值时，则说明预设标签在滑窗中的约束有效。
[0128]
当然，在实际情况中，在确定观测次数时，可以根据实际情况选择上述四种方式中的任一种，在此并不限定。
[0129]
说明一点，对于m帧图像而言，假设通过观测可知有k（其中，k小于或等于m）帧图像中具有预设标签，确定这k帧图像中的每一帧中的角点或内点的马氏距离，如果马氏距离大于某个设定的阈值，说明此次观测无效，从而确定出预设标签约束无效。
[0130]
3、对滑窗进行边缘化处理，将当前关键帧增加至滑窗中。
[0131]
其中，可以假设滑窗中最多可以包括m个数据帧，若在边缘化处理之前，滑窗中已经包括m个数据帧时，此时将当前关键帧加入至滑窗中后，会导致滑窗中包括的数据帧的数量超出预设的m个，所以在将当前关键帧加入至滑窗中之前，需要对滑窗进行边缘化处理。
[0132]
具体地，可以通过边缘化策略，从滑窗中包括的m个数据帧中删除掉一个数据帧，然后再将当前关键帧加入中滑窗中，使得滑窗中始终包括m个数据帧。
[0133]
其中，边缘化策略包括：对于滑窗中的m个数据帧，分别占据第1位置、第2位置、
…
、第m位置；根据占据第m-1位置的数据帧与占据第m位置的数据帧的视差量，确定占据第m-1位置的数据帧是否为关键帧；其中，如果视差量较小，说明占据第m-1位置的数据帧是关键帧，如果视差量较大，说明占据第m-1位置的数据帧不是关键帧。因此：若占据第m-1位置的数据帧是关键帧，则将占据第1位置的数据帧剔除掉（即剔除掉最早获取的数据帧），并将占据第2位置的数据帧移动至第1位置、将占据第3位置的数据帧移动至第2位置、
…
、将占据第m位置的数据帧移动至第m-1位置，使得滑窗中第m位置被空出来；若占据第m-1位置的数据帧不是关键帧，则将占据第m-1位置的数据帧剔除掉，此时说明当前关键帧与剔除掉的这一数据帧很相似，即当前关键帧与路标点之间的约束与剔除的这一数据帧与路标点之间的约束很接近，直接剔除掉并不会导致整个约束关系丢失很多信息，所以可以直接将这一数据帧剔除掉；说明一点，虽然剔除掉了这一数据帧，但需要保留这一数据帧的预积分数据，从而保证下一帧预积分的连贯性；同时，将占据第m位置的数据帧移动至第m-1位置，使得滑窗中第m位置被空出来。
[0134]
因此，通过上述边缘化过程，可以空出滑窗中的第m位置，再将当前关键帧加入至滑窗中时，可以使得当前关键帧占据滑窗中的第m位置，使得滑窗中始终保持m个数据帧。
[0135]
并且，使得滑窗中始终包含m帧数据帧，且保证滑窗中的数据均为最新的数据帧图像，以有利于保证后续定位更加精准；并且，还可以减少计算量，降低计算成本。
[0136]
此外，对滑窗进行边缘化处理，利用边缘化处理可以去掉滑窗中的最老帧或者次新帧，目的是希望不再计算这一帧，以减少计算量，同时通过舒尔补操作等方式，保留该帧对滑窗内其他帧状态的约束关系。
[0137]
三、闭环处理部分。
[0138]
其中，该部分内容主要用于：移除前端处理过程中位姿误差的累积，从而进一步提高机器人定位的精度。
[0139]
1、视觉闭环检测（如图2中所示的“视觉闭环检测”）。
[0140]
对于此部分内容，主要用于：确定出与当前关键帧处于闭环匹配状态的图像闭环关键帧；其中，在确定当前关键帧处于闭环匹配状态的图像闭环关键帧时，具体过程可以包括：根据预设的视觉闭环检测方法，分别对当前关键帧与预设的数据库（如图2中所示的“数据库”）中存储的每一个图像关键帧进行视觉闭环检测，确定出与当前关键帧处于候选闭环匹配关系的候选关键帧；其中，候选关键帧可以理解为：可能与当前关键帧存在闭环匹配关系的图像关键帧；确定当前关键帧与候选关键帧之间的第一相对位姿；将第一相对位姿小于第二预设值对应的候选关键帧，确定为与当前关键帧处于闭环匹配状态的图像闭环关键帧。
[0141]
其中，第二预设值的具体设置，可以根据实际情况进行设置，在此并不限定。
[0142]
并且，在计算第一相对位姿时，可以采用预设pnp算法进行计算，且具体的计算过程可参见现有技术，在此不再详述。
[0143]
2、闭环优化（即位姿图优化，如图2中所示的“位姿图优化”）。
[0144]
其中，在视觉闭环检测结束之后，可以开启位姿图优化过程。
[0145]
可选地，在本发明实施例中，对当前关键帧进行位姿图优化处理，具体包括：采用如下公式，对当前关键帧进行位姿图优化处理：；其中，s表示具有滑窗约束的关键帧列表，l表示具有闭环约束的关键帧列表，i表示第i帧关键帧，j表示第j帧关键帧，r
i,j
表示第i帧关键帧与第j帧关键帧之间的残差，表示从滑窗中得到的相邻两个数据帧之间的相对位姿约束的协方差，表示视觉闭环约束的不确定性协方差，ρ(.)表示鲁棒核函数，x*表示经过位姿图优化后的状态。
[0146]
也就是说，为了达到输出位姿的稳定性，后端处理过程中维护了一个小规模的位姿图来约束和平滑全局姿态，从而可以有效避免因误匹配闭环检测而引起的姿态跳变。
[0147]
具体地，可以将一个关键帧视为姿势图中的一个顶点，每两个顶点（由关键帧构成）由一个约束边连接，约束边的类型主要有两种，分别为：顺序约束边和闭环约束边，其中，顺序约束边可以表示前端处理（包括数据预处理过程和滑窗优化处理过程）结果中的两个顶点之间的相对转换约束，且关键帧可以使用顺序约束边连接到几个邻近已经确定出的关键帧之上；闭环约束边可以表示关键帧和对应的闭环关键帧之间的相对转换约束，且闭环关键帧可以通过闭环约束边与对应的关键帧进行连接。
[0148]
因此，在确定关键帧与对应闭环关键帧满足预设的闭环条件时，说明该关键帧与对应闭环关键帧之间接受闭环，所以对闭环关键帧进行位姿图优化处理时，需要考虑闭环约束边的残差和顺序约束边的残差这两个因素。
[0149]
并且，随着amr的移动，位姿图的大小会逐渐增加，这会影响定位的实时性，因此，在执行位姿图优化处理之前，或者在执行位姿图优化处理之后，当amr的运动轨迹仍然处于
已构建的地图范围内时，可以仅保留n个最新的关键帧，以减少计算量。
[0150]
3、高频递推。
[0151]
在实际情况中，在前端处理部分，位姿信息的输出频率取决于图像帧的采集频率。
[0152]
为了进一步提高位姿信息的输出频率，并降低计算量，可以采取以下措施：在前端处理部分解算后，且在新的图像帧来临之前，采用imu预积分相对位姿，在上一个图像帧的位姿信息的基础上，进行递推得到imu频率的高频位姿信息。
[0153]
其中，imu预积分相对位姿可以理解为：先对imu数据进行预积分，然后计算预积分结果与上一个图像帧的位姿信息之间的相对位姿。
[0154]
或者，如果imu不存在或失效时，还可以采用以下方式：采用轮速里程计的预积分相对位姿，在上一个图像帧的位姿信息的基础上，进行递推得到轮速里程计频率的高频位姿信息。
[0155]
其中，轮速里程计的预积分相对位姿可以理解为：先对角速度/线速度进行预积分，然后计算预积分结果与上一个图像帧的位姿信息之间的相对位姿。
[0156]
也就是说，可以将输出相邻两个数据帧之间的时间定义为增加时间，然后根据增加时间内imu预积分相对位姿或轮速里程计的预积分的相对位姿、以及输出的上一个数据帧的位置信息，确定增加时间内补充数据帧的位置信息；最后，将补充数据帧插入至待输出的两个数据帧之间时，将插入补充数据帧之后的结果输出，从而提高了数据帧的输出频率，进而提高了位姿信息的输出频率。
[0157]
4、确定机器人的位姿。
[0158]
具体地，在确定机器人的位姿时，可以根据适用场景的不同，采用以下两种方式：方式1：适用于低频输出场景。
[0159]
其中，可以根据以下两种数据确定机器人的最终位姿，两种数据包括：a、当前关键帧在经过位姿图优化后的位姿信息；b、在采集到当前关键帧之后最近时刻采集到的图像帧的位姿信息。
[0160]
其中，最近时刻采集到的图像帧的位姿信息，具体为：最近时刻采集到的图像帧在经过滑窗优化处理后得到的位姿信息。
[0161]
在根据上述两种数据确定最终位姿时，具体过程可以但不限于为：通过当前关键帧在经过位姿图优化后的位姿信息，可以确定出在进行位姿图优化处理前后产生的位姿增量；将产生的位姿增量与最近时刻采集到的图像帧的位姿信息进行叠加，得到机器人的最终位姿信息，从而实现机器人定位。
[0162]
方式2：适用于高频输出场景。
[0163]
其中，可以根据以下三种数据确定机器人的最终位姿，三种数据包括：a、当前关键帧在经过位姿图优化后的位姿信息；b、在采集到当前关键帧之后最近时刻采集到的图像帧的位姿信息；c、在采集到当前关键帧之后最近采集到的imu数据与最近采集到的图像数据之间的预积分结果。
[0164]
其中，在c中，因imu的采集频率高于轮速里程计的采集频率大于图像的采集频率，所以利用预积分结果，可以增加位姿信息的输出频率，从而实现高频输出，使得最终确定出的机器人的位姿信息更加准确和实时。
[0165]
说明一点，最近采集imu数据的时间晚于最近采集图像数据的时间。
[0166]
也就是说，在此方式2中，将方式1中得到的最终位姿信息与高频递推过程中得到的增加时间内补充数据帧的位姿信息进行组合，从而输出高频的位姿信息，确定机器人的位姿，提高机器人定位的实时性和有效性。
[0167]
基于同一发明构思，本发明实施例提供了一种定位装置，该定位装置的具体实现原理与前述一种定位方法的实现原理类似，该定位装置的具体实施方式可参见前述定位方法的具体实施例，重复之处不再赘述。
[0168]
具体地，本发明实施例提供了一种机器人的定位装置，如图4所示，包括：第一单元401，用于根据当前采集到的惯性测量单元imu数据、轮速里程计数据和图像数据，对确定出的当前关键帧进行滑窗优化处理，确定当前关键帧的位姿信息；其中，图像数据包括：图像中特征点的匹配信息、以及在图像中具有预设标签时预设标签的坐标信息；第二单元402，用于根据当前处于滑窗中多个关键帧的位姿信息、以及确定出的与当前关键帧处于闭环匹配状态的图像闭环关键帧，对当前关键帧进行位姿图优化处理，得到处理后的当前关键帧的位姿信息；第三单元403，用于根据处理后的当前关键帧的位姿信息、以及在确定出当前关键帧之后最近时刻采集到的一帧图像的位姿信息，确定机器人的位姿。
[0169]
可选地，在本发明实施例中，第一单元401具体用于：根据当前滑窗的优化状态，计算预设标签包括的全部标签点在所在图像坐标系中的二维坐标的预测值；其中，全部标签点包括：用于表示预设标签在所在图像中位置的多个角点，和/或用于形成预设标签的图案的多个内点；计算观测到的标签点在所在图像坐标系中的二维坐标、以及该标签点的二维坐标的预测值之间的差值；根据计算后得到的各标签点的差值，确定预设标签的重投影误差。
[0170]
可选地，在本发明实施例中，第一单元401具体用于：若判断出当前处于滑窗中所有关键帧满足预设的优化状态固定切换条件时，将所有关键帧中采集时间最早的关键帧定义为最老关键帧，针对所有关键帧中除最老关键帧之外的其他关键帧和当前关键帧，进行滑窗优化处理；其中，优化状态固定切换条件包括：所有关键帧对应图像中均不包括预设标签；或，所有关键帧对应图像中至少一张图像包括预设标签，且图像中包括的预设标签在滑窗中均约束无效。
[0171]
可选地，在本发明实施例中，第一单元401还用于：在对确定出的当前关键帧进行滑窗优化处理之前，确定预设标签在滑窗中的约束有效。
[0172]
可选地，在本发明实施例中，第一单元401具体用于：确定预设标签在当前处于滑窗中的各关键帧对应图像中的观测次数；其中，观测次数
为：根据当前处于滑窗中包括预设标签的图像对应的关键帧的帧数、以及包括预设标签的图像包括的各目图像中观测到预设标签的次数确定；判断观测次数是否大于第一预设值；若是，则确定预设标签在滑窗中的约束有效；若否，则确定预设标签在滑窗中的约束无效。
[0173]
可选地，在本发明实施例中，第一单元401还用于：在对确定出的当前关键帧进行滑窗优化处理之前，在机器人的初始化阶段，按照预设规则，确定滑窗中的m个数据帧的初始位姿信息；其中m为大于1的整数；对接收到的第m+1个数据帧和滑窗中的m个数据帧进行滑窗优化处理，得到m+1个数据帧优化后的位姿信息；预设规则包括：采集到的首个数据帧的初始位姿信息为：根据首个数据帧对应图像中具有预设标签的坐标信息和索引确定；m+1个数据帧中除首个数据帧之外的其他任一数据帧的初始位姿信息为：在该数据帧对应图像中具有预设标签时，根据预设标签的坐标信息和索引确定；或，根据该数据帧相对于首个数据帧的imu预积分数据确定。
[0174]
可选地，在本发明实施例中，第三单元403具体用于：确定当前关键帧在进行位姿图优化处理前后产生的位姿增量；将位姿增量与在确定出当前关键帧之后最近时刻采集到的一帧图像的位姿信息进行叠加，确定机器人的位姿。
[0175]
基于同一发明构思，本发明实施例提供了一种机器人，如图5所示，可以包括：如本发明实施例提供的上述定位装置501。
[0176]
可选地，在本发明实施例中，机器人除了可以包括定位装置501之外，还可以包括其他用于实现机器人功能的结构，具体可根据实际需要进行设计，在此不再详述。
[0177]
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。