CN112734852A - 一种机器人建图方法、装置及计算设备 - Google Patents

一种机器人建图方法、装置及计算设备 Download PDF

Info

Publication number
CN112734852A
CN112734852A CN202110348706.7A CN202110348706A CN112734852A CN 112734852 A CN112734852 A CN 112734852A CN 202110348706 A CN202110348706 A CN 202110348706A CN 112734852 A CN112734852 A CN 112734852A
Authority
CN
China
Prior art keywords
pose
preset
robot
image
frame
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.)
Granted
Application number
CN202110348706.7A
Other languages
English (en)
Other versions
CN112734852B (zh
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.)
Zhejiang Sineva Intelligent Technology Co ltd
Original Assignee
Zhejiang Sineva Intelligent 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 Zhejiang Sineva Intelligent Technology Co ltd filed Critical Zhejiang Sineva Intelligent Technology Co ltd
Priority to CN202110348706.7A priority Critical patent/CN112734852B/zh
Publication of CN112734852A publication Critical patent/CN112734852A/zh
Application granted granted Critical
Publication of CN112734852B publication Critical patent/CN112734852B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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
    • 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
    • G01C21/165—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 combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Image Analysis (AREA)
  • Manipulator (AREA)

Abstract

本申请公开一种机器人建图方法、装置及计算设备，机器人可将预设时间段内采集的连续多帧图像数据、惯性测量单元IMU数据以及轮速里程计数据进行集束优化，获取当前帧图像中预设标签在机器人的相机坐标系的优化位姿及优化位姿的方差值；对当前帧图像对应的IMU数据、轮速里程计数据进行扩展卡尔曼EKF的状态递推，确定机器人的位姿和机器人的位姿的方差值；之后将优化位姿、优化位姿的方差值、机器人的位姿和机器人的位姿的方差值进行EKF测量的迭代更新，获取预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿和目标位姿的方差值。该方式通过集束优化和EKF的松紧融合来进行机器人建图，可以提高建图精度。

Description

一种机器人建图方法、装置及计算设备
技术领域
本申请涉及计算机视觉技术领域，尤其涉及一种机器人建图方法、装置及计算设备。
背景技术
随着制造灵活性需求的提高、产品周期缩短或加快、人力成本上升以及安全的需求增加，移动机器人正在快速发展，以满足人们日益增长的需求。如今，自主导引车（automated guided vehicles，AGV）已广泛应用于自动化仓库，工厂物料传输***，物流拣配***，柔性装配***和其他智能运输站点。自主导引车导航的核心技术为同步定位与建图（simultaneous localization and mapping，SLAM），通过SLAM导引车可以获悉环境地图，以及导引车自身的实时位置，以便在没有人为干预的情况下，高效智能移动到达目的地。
目前主要采用激光SLAM和视觉SLAM。但是，视觉SLAM的摄像头成本远低于激光传感器。此外，相对于2维激光SLAM而言，视觉SLAM能获得待定位目标对象的6自由度位姿。故此，成本相对低廉的视觉SLAM得以快速发展。
但是，单纯依赖视觉SLAM，存在累计误差，基于视觉特征点的视觉SLAM精度会大幅降低，为了提高机器人定位的鲁棒性，在机器人落地时可加入二维码标签进行辅助定位。相关技术需要预先获取二维码标签的全局位姿及其方差，但通过测绘等方式无法精确获取标签的全局位姿及其方差。此外，针对单次二维码标签的观测出现的图像模糊、位姿歧义、噪声等位姿估计不稳定性情况，基于传统松耦合算法，无法准确的计算二维码位姿。在多传感器融合过程，各传感器的外参精度和时间偏置会极大影响SLAM效果。
发明内容
本申请提供了一种机器人建图方法、装置及计算设备，用以提高视觉建图的可靠性和准确度。
第一方面，本申请实施例提供一种机器人建图方法，该方法可通过机器人来执行，也可通过与机器人通讯连接的服务器（机器人采集数据后传输给服务器）来执行，本申请在此不作具体限定。在此仅以执行主体为机器人为例进行说明，机器人可将预设时间段内采集的连续多帧图像数据、惯性测量单元（inertial measurement，IMU）数据以及轮速里程计数据进行集束优化，获取当前帧图像中预设标签在机器人的相机坐标系的优化位姿及优化位姿的方差值；对当前帧图像对应的所述IMU数据、轮速里程计数据进行扩展卡尔曼（extended kalman filter，EKF）的状态递推，确定机器人的位姿和机器人的位姿的方差值；将优化位姿、优化位姿的方差值、机器人的位姿和机器人的位姿的方差值进行EKF测量的迭代更新，获取预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿和目标位姿的方差值。
本申请实施例中，机器人在建图时，在采用连续多帧的图像的同时，参考了IMU数据以及轮速里程计数据，通过该方式也可以保证建图过程中预设标签位姿的精确度。另外，本申请在建图时还融合了集束优化与EKF的状态预测的方法，使得机器人建图精确度更高，且有助于更快地计算出预设标签在世界坐标系中的目标位姿和目标位姿的方差值。
在一种可能的实现方式中，机器人在EKF的测量迭代更新过程中对各传感器之间的外参和时间偏置进行在线优化，并将优化后的外参和时间偏置用于集束优化；所述外参包括：IMU与相机的相对位姿，以及轮速里程计与相机的相对位姿。
本申请实施例中，在集束优化时加入外参和时间偏置，可以使得获取的当前帧图像中预设标签在机器人的相机坐标系的优化位姿更加可靠，进而使得机器人建图的定位精确度更高。
在一种可能的实现方式中，机器人可通过机器人的相机获取预设时间段内采集的连续N帧图像，并对N帧图像进行筛选，确定包括预设标签的M帧图像；N、M为整数；M大于或等于2，小于或等于N；通过对M帧图像中预设标签的角点的位置信息、预设标签的角点在机器人的相机所在坐标系的投影信息、IMU数据以及轮速里程计数据进行集束优化，确定预设标签在机器人的相机所在坐标系的优化位姿、优化位姿的方差值、预设标签的投影误差、雅克比矩阵和协方差矩阵。
本申请实施例中，机器人通过对M帧图像中预设标签的角点的位置信息、预设标签的角点在机器人的相机所在坐标系的投影信息、IMU数据以及轮速里程计数据进行集束优化，可确定预设标签在机器人的相机所在坐标系的优化位姿、优化位姿的方差值、预设标签的投影误差、雅克比矩阵和协方差矩阵。通过该方式可提高机器人建图精确度。
在一种可能的实现方式中，集束优化的滑动窗口中连续多帧图像通过如下方式确定：
若滑动窗口中图像帧的帧数小于或等于滑窗阈值，则将所有的图像帧进行集束优化；若滑动窗口中图像帧的帧数大于滑窗阈值，则将所有的图像帧进行集束优化之后要筛选出预设帧图像，并通过舒尔补方法保留对被剔除图像的滑动窗口的约束。
本申请实施例中，在滑动窗口对图像帧进行筛选，使得机器人建图的定位精确度更高。
在一种可能的实现方式中，筛选出预设帧图像进行集束优化通过以下方式中的一种或多种确定：
获取当前帧图像中所有预设标签的标识列表；
剔除多帧图像中不包括预设标签的图像；若在第N帧图像中检测的预设标签的标识与第M帧图像中预设标签的标识不同，则剔除第N帧图像；若在预设时间段内任一帧图像中预设标签的标识与第M帧图像中预设标签的标识均相同，则对除第M帧图像以外的图像按照信息度进行打分，剔除分数最低的帧图像；其中，M、N为整数，M为预设时间段内采集的最后一帧图像的数目。
本申请实施例中，通过筛选预设帧图像进行技术优化，使得预设标签的优化位姿更加可靠。
在一种可能的实现方式中，预设标签的投影误差及雅可比矩阵可通过如下方式确定：
通过透视N点投影PNP算法对所述M帧图像中所述预设标签的角点的二维坐标信息进行计算，确定所述预设标签在机器人的相机坐标系的初始位姿；根据所述预设标签在机器人的相机坐标系的初始位姿以及所述机器人的初始位姿，确定所述预设标签的各个角点的在世界坐标系的三维坐标信息；将预设标签的角点的三维坐标信息投影至相机所在坐标系中，得到投影后的预设标签的角点的二维坐标信息；将投影后的预设标签的角点的二维坐标信息与投影前预设标签的角点的二维坐标信息进行差值计算，确定预设标签的投影误差；确定预设标签的投影误差相对滑动窗口中优化状态量的雅克比矩阵。
本申请实施例中，通过透视N点投影（pespective-n-point，PNP）算法，根据第i个预设标签中角点在预设标签坐标系下的三维坐标，以及首次观测第i个预设标签的图像帧M中预设标签的二维像素坐标，确定第i个预设标签在第M帧图像中相机坐标系的初始位姿；根据第i个预设标签在第M帧图像中相机坐标系的初始位姿以及第M帧图像中相机的全局位姿，确定预设标签的角点的全局三维坐标信息，之后通过确定预设标签的投影误差可以保证机器人建图的定位精确度。
在一种可能的实现方式中，目标位姿的方差值与滑动窗口各预设标签的角点观测不确定性相关；其中，任一预设标签的角点在任一图像帧中观测不确定性与以下参数存在函数关系：
预设标签的角点的二维坐标信息、预设标签所在图像帧的图像采集装置的线速度、角速度、加速度以及预设标签在图像帧中的像素面积。
本申请实施例中，预设标签的目标位姿的方差值与多种参数存在函数关系，故此可知，本申请获取的目标位姿的方差值更符合实际场景。
在一种可能的实现方式中，机器人可将预设标签的先验信息作为约束项加入集束优化和测量更新；其中，所述预设标签的先验信息为预先确定的任意两个预设标签的相对位姿及不确定性方差，或任一预设标签在世界坐标系的全局位姿及不确定性方差。
本申请实施例中，机器人还可基于先验信息调整预设标签在世界坐标系的位姿，通过该方式使得机器人建图的精确度更高，建图的结果更加可靠。
在一种可能的实现方式中，机器人可对预设标签的投影约束、预设标签的先验相对位姿约束、预设标签的先验全局位姿约束、轮速里程计的测量约束以及时间偏置约束进行EKF测量的迭代更新，得到更新后的EKF状态。
本申请实施例中，通过更新预设标签的投影约束、预设标签的位姿约束以及时间偏置等，可以提高机器人建图的精确度。
在一种可能的实现方式中，所述EKF状态包括：所述预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿、各传感器之间的外参、各传感器之间的时间偏置、当前帧机器人的位姿和速度以及IMU的零偏。
第二方面，本申请实施例还提供一种机器人建图装置，包括：集束优化单元、状态预测单元以及测量更新单元。
其中，集束优化单元，用于将预设时间段内采集的连续多帧图像数据、惯性测量单元IMU数据以及轮速里程计数据进行集束优化，获取当前帧图像中预设标签在机器人的相机坐标系的优化位姿及所述优化位姿的方差值；状态预测单元，用于对当前帧图像对应的所述IMU数据、所述轮速里程计数据进行扩展卡尔曼EKF的状态递推，确定所述机器人的位姿和所述机器人的位姿的方差值；测量更新单元，用于将所述预设标签的优化位姿、所述预设标签优化位姿的方差值、所述机器人的位姿和所述机器人的位姿的方差值进行EKF测量的迭代更新，获取预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿和目标位姿的方差值。
在一种可能的实现方式中，测量更新单元还可在EKF的测量的迭代更新过程中对各传感器之间的外参和时间偏置进行在线优化，并将优化后的外参和时间偏置用于集束优化；所述外参包括：IMU与相机的相对位姿，以及轮速里程计与相机的相对位姿。
在一种可能的实现方式中，集束优化单元具体用于：通过机器人的相机获取预设时间段内采集的连续N帧图像，并对N帧图像进行筛选，确定包括预设标签的M帧图像；N、M为整数；M大于或等于2，小于或等于N；通过对M帧图像中预设标签的角点的位置信息、预设标签的角点在机器人的相机所在坐标系的投影信息、IMU数据以及轮速里程计数据进行集束优化，确定预设标签在机器人的相机所在坐标系的优化位姿、优化位姿的方差值、预设标签的投影误差、雅克比矩阵和协方差矩阵。
在一种可能的实现方式中，集束优化的滑动窗口中连续多帧图像通过如下方式确定：
若滑动窗口中图像帧的帧数小于或等于滑窗阈值，则将所有的图像帧进行集束优化；若所述滑动窗口中图像帧的帧数大于滑窗阈值，则将所有的图像帧进行集束优化之后筛选出预设帧图像，并通过舒尔补方法保留对被剔除图像的滑动窗口的约束。
在一种可能的实现方式中，筛选出预设帧图像进行集束优化通过以下方式中的一种或多种确定：
获取当前帧图像中所有预设标签的标识列表；
剔除多帧图像中不包括预设标签的图像；若在第N帧图像中检测的预设标签的标识与第M帧图像中预设标签的标识不同，则剔除第N帧图像；若在预设时间段内任一帧图像中预设标签的标识与第M帧图像中预设标签的标识均相同，则对除第M帧图像以外的图像按照信息度进行打分，剔除分数最低的帧图像；其中，M、N为整数，M为预设时间段内采集的最后一帧图像的数目。
在一种可能的实现方式中，预设标签的投影误差及雅可比矩阵可通过如下方式确定：
通过透视N点投影PNP算法对所述M帧图像中所述预设标签的角点的二维坐标信息进行计算，确定所述预设标签在机器人的相机坐标系的初始位姿；根据所述预设标签在机器人的相机坐标系的初始位姿以及所述机器人的初始位姿，确定所述预设标签的各个角点的在世界坐标系的三维坐标信息；将所述预设标签的角点的三维坐标信息投影至所述相机所在坐标系中，得到投影后的预设标签的角点的二维坐标信息；将所述投影后的预设标签的角点的二维坐标信息与投影前所述预设标签的角点的二维坐标信息进行差值计算，确定所述预设标签的投影误差；确定所述预设标签的投影误差相对滑动窗口中优化状态量的雅克比矩阵。
在一种可能的实现方式中，目标位姿的方差值与滑动窗口各预设标签的角点观测不确定性相关；其中任一预设标签的角点在任一图像帧中观测不确定性与以下参数存在函数关系：
预设标签的角点的二维坐标信息、预设标签所在图像帧的图像采集装置的线速度、角速度、加速度以及预设标签在图像帧中的像素面积。
在一种可能的实现方式中，集束优化单元具体用于：将预设标签的先验信息作为约束项加入集束优化和测量更新；其中，所述预设标签的先验信息为预先确定的任意两个预设标签的相对位姿及不确定性方差，或任一预设标签在世界坐标系的全局位姿及不确定性方差。
在一种可能的实现方式中，机器人可对预设标签的投影约束、预设标签的先验相对位姿约束、预设标签的先验全局位姿约束、轮速里程计的测量约束以及时间偏置约束进行EKF测量的迭代更新，得到更新后的EKF状态。
在一种可能的实现方式中，所述EKF状态包括：所述预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿、各传感器之间的外参、各传感器之间的时间偏置、当前帧机器人的位姿和速度以及IMU的零偏。
第三方面，本申请实施例还提供一种计算设备，包括：存储器以及处理器；存储器，用于存储程序指令；处理器，用于调用所述存储器中存储的程序指令，按照获得的程序指令第一方面任一项所述的机器人建图方法。
第四方面，本申请实施例还提供一种计算机存储介质，其中，所述计算机存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行本申请实施例中的任一机器人建图方法。
本申请的其它特征和优点将在随后的说明书中阐述，并且部分地从说明书中变得显而易见，或者通过实施本申请而了解。本申请的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图作简单地介绍，显而易见地，下面所介绍的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的机器人建图方法的应用场景的示意图；
图2为本申请实施例提供的机器人建图方法的流程示意图；
图3为本申请实施例提供的机器人建图方法的流程示意图；
图4为本申请实施例提供的舒尔补方法的流程示意图；
图5为本申请实施例提供的机器人建图装置的结构示意图；
图6为本申请实施例提供的计算设备的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
需要说明的是，本申请中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应所述理解这样使用的数据在适当情况下可以互换，以便这里描述的本公开的实施例能够以除了在这里图示或描述的那些以外的顺序实施。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。
接下来参照图1来介绍本申请可能适用的应用场景，图1中包括机器人、预设定位标签以及服务器。在实际应用时，具体执行可根据机器人计算粒度的设定是否需要服务器，若机器人计算粒度较高，机器人可自主完成复杂的计算，则无需服务器辅助计算；若机器人计算粒度较低，机器人则可将待计算数据发送至服务器，通过服务器计算，通过该方式可以提高数据的处理效率。在应用场景中包括服务器时，机器人与服务器通信连接，机器人也可与服务器共同计算待计算数据，之后通过服务器协助机器人建图，通过该方式可以提高数据的处理效率。
需要说明的是，预设标签张贴在机器人建图的空间环境中如：工厂天花板、办公室屋顶等位置基本保持不变的物体上。本申请中预设标签可以为任何可以标识信息的图案，如：二维码、条形码等，本申请在此不具体限定。本申请以预设标签为二维码为例进行说明，其中二维码是一个二进制平方标记，其采用一个宽的黑边和一个内部的二进制矩阵组成，内部的矩阵排布决定了二维码的身份标识（identity document，ID）。黑色的边界有利于快速检测到图像，二进制编码可以验证ID，并且允许错误检测和矫正技术的应用。
在本申请中，机器人通过相机采集连续多帧图像，图像中包括二维码，通过对IMU数据、轮速里程计数据进行集束优化以及EKF的测量更新确定二维码在世界坐标系的位姿，基于二维码在世界坐标系的位姿来进行建图。
在实际应用时，机器人建图可通过上述图1所示的应用场景中的机器人来执行，也可通过服务器执行，还可以通过机器人与服务器的相互配合来执行，本申请在此并不具体限定执行主体。在此仅以执行主体为机器人为例进行说明，如图2所示，机器人在建图时，可参照如下步骤来执行：
步骤201，机器人将预设时间段内采集的连续多帧图像数据、IMU数据以及轮速里程计数据进行集束优化，获取当前帧图像中预设标签在机器人的相机坐标系的优化位姿及优化位姿的方差值。
在实际执行时，机器人的相机可能是单目的相机也可能是双目的相机，还可能是多目的相机，本申请在此不作具体限定。机器人根据获取最新一帧的各目图像，将具有相同时间戳的各目图像，作为当前帧图像信息。需要说明的是，在按照图2所示的机器人建图流程进行建图时，预设标签在世界坐标系下的位姿是未知的，之后调用二维码检测程序，依次检测当前帧各目图像二维码所在的局部区域，并返回图像中检测到的所有二维码的ID和二维坐标。
其中，二维码检测过程由以下步骤构成：
（1）检测二维码的候选区域。该步骤主要是在整个图像区域找到哪些可能是二维码的专有轮廓（例如：方形）。利用自适应性阈值来分割二维码，然后从阈值化的图像中提取外形轮廓，并舍弃非凸多边形的，以及那些不是方形的外形轮廓。在此过程中可通过滤波操作来剔除那些过小或者过大的轮廓、过于相近的凸多边形等。
（2）分析候选区域确定二维码类型。机器人检测得到二维码候选区域后，通过分析其内部编码来确定该候选区域中是否存在指定类型的二维码，之后对图像进行投影变换，得到其规范的形态（正视图），对规范的图像用阈值化以分离白色位和黑色位。根据二维码大小和边界大小分成为不同格子，统计落在每个格子中的黑白像素数目来确定是黑色位还是白色位，最后根据这些标志位来确定二维码是否属于指定字典的所属类型。
（3）输出二维码的相关数据信息，将上一步检测出的二维码类型和指定字典的匹配，确定出各二维码的ID。对每一个二维码，根据二维码外框区域，确定出四个角点的二维坐标。
步骤202，机器人对当前帧图像对应的IMU数据、轮速里程计数据进行EKF的状态递推，确定机器人的位姿和机器人的位姿的方差值。
需要说明的是，步骤202与步骤201在实际执行时可不区分先后顺序，可以先执行步骤202再执行步骤201，也可以同时执行，本申请在此不作具体限定。
步骤203，机器人将优化位姿、优化位姿的方差值、机器人的位姿和机器人的位姿的方差值进行EKF测量的迭代更新，获取预设时间段内采集的连续多帧图像中预设标签在世界坐标系的位姿和目标位姿的方差值。
其中，目标位姿的方差值可用于表征当前帧图像中预设标签在世界坐标系的位姿可信度；目标位姿的方差值与滑动窗口各预设标签的角点观测不确定性相关；其中，任一预设标签的角点在任一图像帧中观测不确定性与以下参数存在函数关系：预设标签的角点的二维坐标信息、预设标签所在图像帧的图像采集装置的线速度、角速度、加速度以及预设标签在图像帧中的像素面积。
本申请实施例中，机器人在建图时，在采用连续多帧的图像的同时且参考了IMU数据以及轮速里程计数据，通过该方式也可以保证建图的精确度。另外，本申请在建图时还融合了集束优化与EKF的状态预测的方法，使得机器人建图的定位精确度更高，有助于更快地计算出预设标签在世界坐标系中的位姿和目标方差值。
在一个实施例中，机器人在EKF的测量的迭代更新过程中，可对各传感器之间的外参和时间偏置进行在线优化，并将优化后的外参和时间偏置用于集束优化；外参包括：IMU与相机的相对位姿，以及轮速里程计与相机的相对位姿。本申请中，机器人在建图时将外参进行集束优化，可以确保建图的精确度。
接下来通过图3所示的执行流程图来对本申请的方案进行具体说明，具体如下：
1.1、传感器数据的预处理
在一个可选的实施例中，机器人可通过机器人的多个相机也即图3中的相机模组获取预设时间段内采集的连续N帧图像，并对N帧图像进行筛选，确定包括预设标签的M帧图像；N、M为整数；M大于或等于2，小于或等于N；通过对M帧图像中预设标签的角点的位置信息、预设标签的角点在机器人的相机所在坐标系的投影信息、IMU数据、轮速里程计数据进行集束优化，确定预设标签在机器人的相机所在坐标系的优化位姿、优化位姿的方差值、预设标签的投影误差、雅克比矩阵和协方差矩阵。具体如下：
A．图片中二维码的预处理
机器人可先获取最新一帧的各目图像，将具有相同时间戳的各目图像，作为当前帧图像信息。之后调用二维码检测程序，依次检测当前帧各目图像二维码所在的局部区域，并返回图像中检测到的所有二维码的ID和二维坐标。之后输出当前帧中各目图像的所有二维码观测信息，二维码观测信息可包括：二维码的ID、二维码所在图像帧的ID或时间戳、观测到二维码的相机ID、图像四个角点的二维坐标，可为像素uv值或归一化坐标（四个角点按照指定的顺序输出：例如从二维码左上角开始顺时针遍历）若第一次观测到该二维码时，可根据PNP计算该二维码和该相机的相对位姿，进而再基于当前帧相机的全局位姿，可计算出该二维码全局位姿的初始值 。
B．IMU预积分
获取IMU数据：将连续两帧图像之间的IMU数据存放到缓存列表。
对齐时间戳：基于IMU和相机的时间偏置，通过插值，将上述IMU数据的积分起始时刻和结束时刻与图像帧时刻对齐。
计算预积分：利用中值法，迭代积分得出该两图像帧间的位置、速度、角度的状态增量。
计算雅克比和方差矩阵：计算两帧图像间状态量的雅克比矩阵和方差矩阵，用于后续集束优化过程。
C．轮速里程计预积分
获取轮速里程计数据：将连续两帧图像之间的轮速里程计数据存放到缓存列表。
时间戳对齐：基于轮速计和相机的时间偏置，通过插值，将上述轮速里程计数据的积分起始时刻和结束时刻与图像帧时刻对齐。
计算预积分：利用中值法，迭代积分得出该两图像帧间的位置、角度的状态增量。
计算雅克比和方差矩阵：计算两帧图像间状态量的雅克比矩阵和方差矩阵，用于后续集束优化过程。
1.2、集束优化
集束优化的主要目的是根据多帧多目中观测到二维码角点重投影误差及其观测不确定性、IMU预积分、轮速里程计预积分对当前窗口***状态进行优化，从而得到更鲁棒、更精确的二维码与当前机器人坐标系的相对位姿的观测值。
1.2.1、***的优化状态
当前窗口中待优化的状态XS包含：窗口内的n+1帧的惯性状态（包括位置、朝向、速度、加速度计偏置bias和陀螺仪偏置bias）、各目相机到IMU的外参、轮速里程计到IMU的外参、已观测到的所有二维码的全局位姿：
Figure 139431DEST_PATH_IMAGE001
其中，
Figure 135200DEST_PATH_IMAGE002
代表第k帧的惯性状态；其中
Figure 521795DEST_PATH_IMAGE003
代表第k帧的IMU全局位姿（也即在世界坐 标系下的位姿）；
Figure 713742DEST_PATH_IMAGE004
分别代表第k帧的IMU全局位置和姿态；
Figure 341163DEST_PATH_IMAGE005
代表第k帧的IMU全局速 度；
Figure 773413DEST_PATH_IMAGE006
代表加速度计bias；
Figure 102763DEST_PATH_IMAGE007
代表陀螺仪bias；
Figure 849133DEST_PATH_IMAGE008
代表第i个二维码的全局位姿；
Figure 377066DEST_PATH_IMAGE009
分别代表第i个二维码的全局位置和姿态；
Figure 180550DEST_PATH_IMAGE010
代表各相机与IMU的外参；其中
Figure 731617DEST_PATH_IMAGE011
代表该外参的位置分量；
Figure 78416DEST_PATH_IMAGE012
代表该外参的角度分量；
Figure 414850DEST_PATH_IMAGE013
代表轮速里程计与IMU的外参；
Figure 641432DEST_PATH_IMAGE014
代表该外参的位置分量；
Figure 696107DEST_PATH_IMAGE015
代表该外参的角度分量。
1.2.2、***的优化约束构建
滑动窗口的集束优化过程可构造为非线性最小二乘问题：
Figure 33547DEST_PATH_IMAGE016
其中， X表示滑窗优化的状态；
Figure 283876DEST_PATH_IMAGE017
表示边缘化的先验信息约束；
Figure 415780DEST_PATH_IMAGE018
表示预设标签的重投影约束；T表示观测到的预设标签的列表；
Figure 754488DEST_PATH_IMAGE019
表示 第i个预设标签的角点在滑动窗口第k个图像帧的观测数据；
Figure 646352DEST_PATH_IMAGE020
表示第i个预设标签的角点 在滑动窗口第k个图像帧的不确定性方差矩阵；
Figure 941067DEST_PATH_IMAGE021
表示预设标签的相对位 姿约束；
Figure 322501DEST_PATH_IMAGE022
表示第i个预设标签和第j个预设标签的相对位姿观测；
Figure 335456DEST_PATH_IMAGE023
表示第i个预设标 签和第j个预设标签的相对位姿不确定性方差矩阵；
Figure 28081DEST_PATH_IMAGE024
表示预设标签的全局 位姿约束；
Figure 442882DEST_PATH_IMAGE025
表示第i个预设标签的全局位姿观测；
Figure 198480DEST_PATH_IMAGE026
表示第i个预设标签的全局位姿不 确定性方差矩阵；
Figure 433152DEST_PATH_IMAGE027
表示IMU数据的预积分约束；B表示具备IMU数据的预积分 约束的图像帧的列表；
Figure 666818DEST_PATH_IMAGE028
表示IMU数据的预积分状态增量对应的协方差矩阵；
Figure 749175DEST_PATH_IMAGE029
表示 IMU数据的预积分结果；
Figure 659362DEST_PATH_IMAGE030
表示轮速里程计数据的预积分残差；E表示具 备轮速里程计预积分约束的图像帧的列表；
Figure 394713DEST_PATH_IMAGE031
表示轮速里程计预积分对应的协方差矩 阵；
Figure 478075DEST_PATH_IMAGE032
表示轮速里程计数据的预积分结果。此外，考虑到轮速里程计打滑的情况，针对轮 速里程计的约束可采用鲁棒核函数ρ形式。
其中，先验信息约束代表滑窗被边缘化的信息对现有滑窗状态的约束；二维码的 重投影约束代表滑窗中各帧各目观测到的各二维码中所有角点的全局三维点向二维点投 影误差约束；轮速里程计测量约束代表滑窗中相邻两帧的轮速里程计预积分的测量约束； IMU测量约束代表滑窗中相邻两帧的IMU预积分的测量约束。二维码的相对位姿约束代表滑 窗中所有二维码已知的相对位姿及其不确定性对窗口中待优化的状态XS的约束，例如第i 个二维码和第j个二维码的已知相对位姿为
Figure 352621DEST_PATH_IMAGE033
，且该相对位姿的不确定性方差为
Figure 433710DEST_PATH_IMAGE034
，该约 束项是可选的。同理，二维码的全局位姿约束代表滑窗中所有二维码已知的全局位姿
Figure 659286DEST_PATH_IMAGE035
及 其不确定性
Figure 828230DEST_PATH_IMAGE036
对窗口中待优化的状态XS的约束。
1.2.3、二维码的重投影约束构建
二维码重投影约束的构建是遍历滑窗中所有图像帧，将每一个图像帧各目观测到的每一个二维码上的所有角点，基于每一个二维码的待求的全局位姿状态，得到各角点的三维全局坐标，并将其投影到该图像帧二维平面上，进而和该角点的二维坐标观测值求差，得到残差约束。
在一个实施例中，预设标签的投影误差及雅可比矩阵可通过如下方式确定：
通过PNP算法，根据对第i个预设标签中角点在预设标签坐标系下的三维坐标，以及首次观测第i个预设标签的图像帧M中预设标签的二维像素坐标，确定第i个预设标签在机器人第M帧图像中的相机坐标系的初始位姿；根据第i个预设标签在第M帧图像中相机坐标系的初始位姿以及第M帧图像中相机的全局位姿，确定预设标签的角点的全局三维坐标信息；将预设标签的角点的三维坐标信息投影至相机所在坐标系中，得到投影后的预设标签的角点的二维坐标信息；将投影后的预设标签的角点的二维坐标信息与投影前预设标签的角点的二维坐标信息进行差值计算，确定该预设标签在第M帧图像中的投影误差；确定该预设标签投影误差对滑动窗口中优化状态量的雅克比矩阵。
需要说明的是，本申请中二维码的重投影约束的构建流程，主要分如下几步：
（1）二维码位姿初始化
由于建图阶段支持二维码任意张贴，故无法提前获取二维码的全局位姿。本申请 在第一次观测到某个二维码时，需要初始化该二维码的全局位姿。本申请采用第一次观测 到二维码的角点坐标，通过PNP方法，得到该二维码和相机的相对位姿
Figure 72130DEST_PATH_IMAGE037
，再叠加上相机的 全局位姿，得到二维码全局位姿的初值
Figure 829780DEST_PATH_IMAGE038
：
Figure 339390DEST_PATH_IMAGE039
其中，
Figure 967818DEST_PATH_IMAGE040
表示该二维码的全局位置初值；
Figure 613694DEST_PATH_IMAGE041
表示全局旋转初值。
（2）二维码各角点的全局坐标
A、获取角点的局部三维坐标。根据每一个二维码的物理尺寸，可知该二维码中各 角点在该二维码坐标系下的局部三维坐标
Figure 771006DEST_PATH_IMAGE042
。
B、计算角点的全局三维坐标。根据每一个二维码的全局位姿
Figure 971174DEST_PATH_IMAGE043
和该二维码上角 点的局部三维坐标，通过计算可得到该二维码上所有角点的全局三维坐标。当第一次观测 到某个二维码时，全局位姿
Figure 947833DEST_PATH_IMAGE043
取上一步初始化得到的二维码全局位姿的初值
Figure 431904DEST_PATH_IMAGE044
。若 不是第一次观测到某个二维码时，全局位姿
Figure 307587DEST_PATH_IMAGE045
取上一次集束优化后状态中的二维码全局 位姿。全局三维坐标的计算公式如下：
Figure 526210DEST_PATH_IMAGE046
其中，
Figure 762019DEST_PATH_IMAGE047
表示全局三维坐标；
Figure 320171DEST_PATH_IMAGE048
表示该二维码的全局位置初值；
Figure 894984DEST_PATH_IMAGE049
表示全局 旋转初值。
（3）二维码重投影误差的计算
将上一步得到二维码中各角点的三维坐标，基于观测到该二维码的图像帧的全局位姿，投影到该图像帧二维平面上，进而和该角点的二维坐标观测值求差，得到残差约束。即：
将每一个二维码上所有角点的三维全局坐标转换到该帧的相机坐标系上的三维坐标：
Figure 53433DEST_PATH_IMAGE050
计算每个角点的二维重投影误差：
Figure 312507DEST_PATH_IMAGE051
其中，
Figure 36749DEST_PATH_IMAGE052
物理意义为：将相机坐标系的三维点转换到归一化坐标的映射函数矩 阵。
（4）二维码重投影约束的雅克比（Jacobian）矩阵的计算
为了求取二维码重投影误差对滑窗中优化状态量（该帧位姿、外参和二维码全局位姿）的Jacobian矩阵，包含二维码重投影误差对滑窗各关键帧位姿的Jacobian矩阵、二维码重投影误差对外参的Jacobian矩阵、二维码重投影误差对二维码全局位姿的Jacobian矩阵。不同于定位阶段（已知二维码全局位姿），本步骤需要额外求取对二维码全局位姿的Jacobian矩阵。根据链式求导法则可知：
A、二维码重投影误差对图像帧位姿的位置分量的Jacobian矩阵
Figure 723077DEST_PATH_IMAGE053
为：
Figure 916292DEST_PATH_IMAGE054
其中，
Figure 962745DEST_PATH_IMAGE055
代表误差对相机坐标系上3D点
Figure 289297DEST_PATH_IMAGE056
的导数；
Figure 943263DEST_PATH_IMAGE057
代表
Figure 873042DEST_PATH_IMAGE058
对该图像帧位置 扰动的导数：
Figure 473919DEST_PATH_IMAGE059
。
二维码重投影误差对该关键帧位姿的姿态分量的Jacobian矩阵
Figure 923486DEST_PATH_IMAGE060
为：
Figure 466462DEST_PATH_IMAGE061
其中，
Figure 303444DEST_PATH_IMAGE062
代表
Figure 19596DEST_PATH_IMAGE063
对该图像帧姿态扰动的导数：
Figure 261353DEST_PATH_IMAGE064
B、二维码重投影误差对待优化外参的位置分量的Jacobian矩阵
Figure 522701DEST_PATH_IMAGE065
为：
Figure 630334DEST_PATH_IMAGE066
其中，
Figure 369751DEST_PATH_IMAGE067
代表P C 对待优化外参位置扰动的导数；二维码重投影误差对待优化外参 姿态的Jacobian矩阵
Figure 715282DEST_PATH_IMAGE068
为：
Figure 410181DEST_PATH_IMAGE069
Figure 224684DEST_PATH_IMAGE070
代表P C 对待优化外参姿态扰动的导数：
Figure 282639DEST_PATH_IMAGE071
C、二维码重投影误差对二维码全局位姿
Figure 233409DEST_PATH_IMAGE072
的Jacobian矩阵：
其中，对位置分量的Jacobian矩阵
Figure 102139DEST_PATH_IMAGE073
为：
Figure 653206DEST_PATH_IMAGE074
其中
Figure 997075DEST_PATH_IMAGE075
代表
Figure 317198DEST_PATH_IMAGE076
对二维码角点坐标P W 的导数，
Figure 356829DEST_PATH_IMAGE077
代表二维码角点坐标P W 对位 置分量的导数。
其中，对姿态分量的Jacobian矩阵
Figure 145925DEST_PATH_IMAGE078
为：
Figure 748944DEST_PATH_IMAGE079
其中，
Figure 939885DEST_PATH_IMAGE080
代表二维码角点坐标P W 对姿态分量的导数。
（5）二维码中各角点的观测不确定性方差计算
为了量化对单二维码中各角点的单次观测不确定性，根据大量的实际测试结论，配置二维码的不确定方差函数，即：
Figure 134106DEST_PATH_IMAGE081
上式中u和v代表该二维码中角点在图像中的二维坐标，linear_vel代表该二维码所在图像帧的相机移动线速度，angular_vel代表该二维码所在图像帧的相机旋转角速度，acc代表该二维码所在图像帧的相机的加速度，S代表该二维码在图像中的像素面积。
在根据实际场景，根据传感器的配置，可将上式简化为：
Figure 204306DEST_PATH_IMAGE082
其中，u_max代表图像像素坐标中横坐标的最大值，v_max代表图像像素坐标中纵坐标的最大值。
（6）二维码约束有效性判断策略
为了避免二维码误检测或二维码中二维点检测误差，造成的滑窗优化精度或稳定性降低的情况，本申请设置了二维码约束有效性判断策略，即通过判断某一二维码在滑窗连续观测的稳定性，来判断是否要将该二维码的约束加入滑窗优化函数中。例如将统计某一二维码在滑窗中各帧中左右目的有效观测次数，如果有效观测次数大于N次，则认为该二维码在滑窗中的观测约束有效。根据各角点的单次观测不确定性方差来进行单次观测的有效观测性判断。例如预设标签角点的单次观测不确定性方差小于阈值，即认为是有效观测。
1.2.4、边缘化策略的设计
在一个实施例中，集束优化的滑动窗口中连续多帧图像通过如下方式确定：若滑动窗口中图像帧的帧数小于或等于滑窗阈值，则将所有的图像帧进行集束优化；若滑动窗口中图像帧的帧数大于滑窗阈值，则将所有的图像帧进行集束优化之后要筛选预设帧图像，并通过舒尔补方法保留被剔除图像对滑动窗口的约束。例如，滑窗阈值为32，滑动窗口中图像帧的数量若为18，则将18帧图像全部进行集束优化；若滑动窗口中图像帧的数量为33，则对该33帧进行集束优化，优化后将基于预设标签观测信息的通过边缘化策略剔除掉多个图像帧。被剔除的图像帧将通过舒尔补方法构建边缘化先验约束，将被用于集束优化。边缘化过程是将联合概率分布分解为边缘概率分布和条件概率分布的过程，这样可以将集束优化窗口中较旧的状态边缘化出滑动窗口，同时保留其信息，并且保证了对应的信息矩阵的稀疏性，从而实现计算资源和计算精度的平衡，通过对矩阵进行舒尔补操作可以实现边缘化的功能。为了清楚说明舒尔补的方法，通过图4进行示意，在图4中（a）指示连续4帧的图像，Xp1、Xp2、Xp3以及Xp4，其中，Xp1帧中可以观测到预设标签Xm1、Xm2、Xm3。但是Xp1帧的图像在进行集数优化时被剔除了，舒尔补的方法如图4中（b）所示，预设标签Xm1、Xm2、Xm3之间的关联关系依然被保留。
在一个实施例中，筛选策略通过以下方式中的一种或多种确定：
需要说明的是，机器人首先需获取当前帧图像中所有预设标签的标识列表，之后剔除多帧图像中不包括预设标签的图像。
方式1：若在第N帧图像中检测的预设标签的标识与第M帧图像中预设标签的标识不同，则剔除第N帧图像。其中，M、N为整数，M为预设时间段内采集的最后一帧图像的索引。
方式2：若在预设时间段内任一帧图像中预设标签的标识与第M帧图像中预设标签的标识均相同，则对除第M帧图像以外的图像按照信息度进行打分，剔除分数最低的帧图像。其中，M、N为整数，M为预设时间段内采集的最后一帧图像的索引。
具体地，若滑动窗口中图像帧在通过方式1进行筛选后，滑窗内图像帧的数量依然大于滑窗阈值，则可对帧图像按照信息度打分，从低到高地剔除图像帧，直到满足滑窗阈值为止。
需要说明的是，上述滑动窗口中图像帧的筛选操作可理解为边缘化操作，通过边缘化，可维持一个固定数量图像帧的滑窗，从而控制运算量，并通过舒尔补的方式以边缘化约束的形式，保留被边缘化数据对滑窗中状态的约束。
不同于基于特征点跟踪的滑窗边缘化策略，本申请提出基于二维码观测信息的边缘化策略。该策略主要根据最新帧各目中观测到所有二维码的ID列表{IDs}对当前集束优化窗口中所有图像帧进行边缘化。
如果窗口中图像帧的帧数小于等于阈值N，则保留窗口中所有图像帧。如果窗口中图像帧的帧数大于阈值N，则遍历窗口中所有图像帧，不限于根据如下三种情况进行图像帧边缘化：
情况1：如果某图像帧没有检测到二维码，则丢弃该图像帧。并将该帧的IMU和轮速里程计的预积分约束保留至下一图像帧。
情况2：如果某图像帧检测到二维码，但检测到的二维码均不属于最新帧{IDs}列表，则丢弃该图像帧。并将该帧的IMU和轮速里程计的预积分约束保留至下一图像帧。同时通过舒尔补的操作，来保留该帧对窗口内其他帧的约束关系。
情况3：如果窗口中所有图像帧均检测到二维码，且检测到的二维码部分或全部属于{IDs}列表，则保留最新图像帧，并将其他图像帧按信息度打分后，丢弃信息度最低的图像帧。并将该帧的IMU和轮速里程计的预积分约束保留至下一图像帧。同时通过舒尔补的操作，来保留该帧对窗口内其他帧的约束关系。图像帧信息度与该帧观测到隶属于{IDs}列表的二维码数量成正比，与二维码的观测方差成反比。
1.2.5、基于滑窗优化状态的二维码观测输出
基于滑窗优化后的状态，遍历最新帧各目中观测到所有二维码，得到各二维码相对于最新帧各目的相对位姿，即：
Figure 96170DEST_PATH_IMAGE083
式中
Figure 390885DEST_PATH_IMAGE084
代表基于滑窗优化后状态计算出的第i个二维码相对于最新帧图像坐标 系的相对位姿；
Figure 506739DEST_PATH_IMAGE085
代表滑窗优化后的外参；
Figure 519695DEST_PATH_IMAGE086
代表的滑窗优化后的位姿；
Figure 215250DEST_PATH_IMAGE087
代表滑窗优 化后的第i个二维码的全局位姿。
二、状态预测
本申请采用基于EKF的多传感器融合框架，能实现自动补偿时间/空间标定误差。
2.1、EKF优化的状态
EKF状态可包括：预设时间段内采集的连续多帧图像中预设标签在世界坐标系的 目标位姿、各传感器之间的外参、各传感器之间的时间偏置、当前帧机器人的位姿和速度以 及IMU的零偏。如：EKF的***状态向量
Figure 10528DEST_PATH_IMAGE088
包含：当前惯性导航状态
Figure 749814DEST_PATH_IMAGE089
、二维码的全局位姿、外 参、时间偏置，即：
Figure 797536DEST_PATH_IMAGE090
其中，
Figure 14890DEST_PATH_IMAGE091
代表当前帧的惯性状态；
Figure 97247DEST_PATH_IMAGE092
代表当前帧的IMU全局位姿；
Figure 758167DEST_PATH_IMAGE093
分别代 表当前帧的IMU全局位置和姿态；
Figure 480135DEST_PATH_IMAGE094
代表当前帧的IMU全局速度；
Figure 514563DEST_PATH_IMAGE095
代表加速度计bias；
Figure 700693DEST_PATH_IMAGE096
代表陀螺仪bias；
Figure 798093DEST_PATH_IMAGE097
代表第i个二维码的全局位姿；
Figure 85986DEST_PATH_IMAGE098
分别代表第i个二维码的全 局位置和姿态；
Figure 645144DEST_PATH_IMAGE099
代表各相机与IMU的外参；
Figure 639776DEST_PATH_IMAGE100
代表该外参的位置分量；
Figure 701885DEST_PATH_IMAGE101
代表该外参的角 度分量；
Figure 664025DEST_PATH_IMAGE102
代表轮速里程计与IMU的外参；
Figure 777606DEST_PATH_IMAGE103
代表该外参的位置分量；
Figure 876012DEST_PATH_IMAGE104
代表该外参的角 度分量；
Figure 111952DEST_PATH_IMAGE105
代表相机与IMU的时间偏置；
Figure 295809DEST_PATH_IMAGE106
代表轮速里程计与IMU的时间偏置。其中外参
Figure 744239DEST_PATH_IMAGE107
和
Figure 507271DEST_PATH_IMAGE108
的初值取自集束优化后的优化结果，二维码的全局位姿
Figure 101063DEST_PATH_IMAGE109
初值取自集束优化后的优 化结果。
2.2、状态递推公式
当收到运动测量信息时，EKF会对状态进行递推。根据传感器配置和运行场景，可分为三种状态递推方式：
A、IMU的三维递推
该递推流程为：当收到一帧IMU测量信息时，利用采集到的加速度计线加速度和陀螺仪角速度，根据IMU的动力学模型，从k-1时刻递推到k时刻的状态和协方差如下：
Figure 319686DEST_PATH_IMAGE110
其中
Figure 40648DEST_PATH_IMAGE111
代表k时刻的状态XE预测值；
Figure 910384DEST_PATH_IMAGE112
代表k-1时刻状态值；
Figure 425810DEST_PATH_IMAGE113
代表IMU的 加速度计测量值；
Figure 584259DEST_PATH_IMAGE114
代表IMU的陀螺仪测量值；
Figure 637141DEST_PATH_IMAGE115
代表k时刻状态的协方差；
Figure 580958DEST_PATH_IMAGE116
代表 k-1时刻状态的协方差；
Figure 313290DEST_PATH_IMAGE117
是对***状态的一阶Jacobian矩阵；
Figure 709768DEST_PATH_IMAGE118
是IMU测量噪声的协 方差。
B、轮速里程计的三维递推
该递推流程为：当收到一帧轮速里程计测量信息时，利用采集到的线速度和角速度，根据轮速里程计的动力学模型，从k-1时刻递推到k时刻的状态和协方差如下：
Figure 756221DEST_PATH_IMAGE119
其中
Figure 554544DEST_PATH_IMAGE120
代表k时刻的状态XE预测值；
Figure 471160DEST_PATH_IMAGE121
代表k-1时刻状态值；
Figure 604201DEST_PATH_IMAGE122
代表轮速 里程计的线速度测量值；
Figure 205078DEST_PATH_IMAGE123
代表轮速里程计的角速度测量值；
Figure 638333DEST_PATH_IMAGE124
代表k时刻状态的协方 差；
Figure 932042DEST_PATH_IMAGE125
代表k-1时刻状态的协方差；
Figure 834271DEST_PATH_IMAGE126
是对***状态的一阶Jacobian矩阵；
Figure 753685DEST_PATH_IMAGE127
是轮速 里程计测量噪声的协方差。
C、轮速里程计和IMU的混合递推
该递推流程为：当收到一帧轮速里程计和IMU测量信息时，利用采集到的轮速里程计线速度和IMU陀螺仪角速度，根据轮速里程计和陀螺仪的动力学模型，从k-1时刻递推到k时刻的状态和协方差如下：
Figure 992512DEST_PATH_IMAGE128
其中
Figure 503128DEST_PATH_IMAGE129
代表k时刻的状态XE预测值；
Figure 95914DEST_PATH_IMAGE130
代表k-1时刻状态值；
Figure 553440DEST_PATH_IMAGE131
代表轮速 里程计的线速度测量值；
Figure 712020DEST_PATH_IMAGE132
代表IMU的陀螺仪测量值；
Figure 347532DEST_PATH_IMAGE133
代表k时刻状态的协方差；
Figure 676882DEST_PATH_IMAGE134
代表k-1时刻状态的协方差；
Figure 217061DEST_PATH_IMAGE135
是对***状态的一阶Jacobian矩阵；
Figure 682677DEST_PATH_IMAGE136
是轮速里 程计和陀螺仪测量噪声的协方差。
三、状态更新
机器人可采用预设标签的投影约束、预设标签的先验相对位姿约束、预设标签的先验全局位姿约束、轮速里程计的测量约束以及时间偏置约束对EKF状态进行迭代更新，得到更新后的EKF状态。更新后的各传感器外参和时间偏置将实时传递给前述的集束优化环节，其中更新后的外参用于集束优化中外参优化的初值，更新后的时间偏置用于集束优化中各测量约束的时间对齐和插值。状态更新环节具体内容如下：
3.1、通用非线性测量函数
为了能兼容多模态的传感器输入测量，本申请采用通用的非线性测量函数，即：
Figure 754669DEST_PATH_IMAGE137
其中，
Figure 853206DEST_PATH_IMAGE138
代表第k帧的测量数据；
Figure 918114DEST_PATH_IMAGE139
代表第k帧的非线性测量函数；
Figure 254549DEST_PATH_IMAGE140
代表第k 帧时的***状态；
Figure 481131DEST_PATH_IMAGE141
代表第k帧时的测量噪声。
本申请进而对非线性测量函数进行线性化后采用EKF滤波框架进行更新。在每一次更新前，校验测量是否能通过95%的卡方分布校验，从而避免错误的测量输入对***状态估计的干扰。
3.2、二维码的重投影约束测量更新遍历最新帧各目中观测到所有二维码，得到各二维码相对于最新帧各目的重投影位姿约束的状态方程，即：
Figure 556314DEST_PATH_IMAGE142
其中，
Figure 159333DEST_PATH_IMAGE143
代表基于当前EKF状态计算出的第i个二维码相对于最新帧图像坐标系 的相对位姿。
对上述状态方程线性化后，得到如下的误差状态方程：
Figure 84695DEST_PATH_IMAGE144
其中，H代表一阶Jacobian矩阵；~代表该状态的误差；
Figure 747758DEST_PATH_IMAGE145
代表第i个二维码相对 于最新帧图像坐标系的相对位姿误差。
3.3、二维码的位置和姿态的先验相对约束更新
在一个实施例中，机器人可将先验二维码约束信息进行集束优化；其中，先验二维码约束信息为预先确定的任意两个预设标签的相对位姿，或任一预设标签在世界坐标系的位姿。也即：
机器人遍历最新帧各目中观测到所有二维码，如果第i个二维码和第j个二维码存在已知先验的朝向和姿态的相对约束（例如先验信息为：第i个二维码和第j个二维码的相对姿态的均值为5度，方差为3度，和或相对位置的均值5米，方差为0.3米），则在测量更新模型中构建二维码的位置和姿态的先验相对约束，其状态约束方程为：
Figure 24149DEST_PATH_IMAGE146
其中，
Figure 227597DEST_PATH_IMAGE147
代表基于当前EKF状态计算出的第i个二维码相对于第j个二维码的相 对位姿。
对上述状态方程线性化后，得到如下的误差状态方程：
Figure 538624DEST_PATH_IMAGE148
其中，H代表一阶Jacobian矩阵；~代表该状态的误差；
Figure 841430DEST_PATH_IMAGE149
代表第i个二维码相对 于最新帧图像坐标系的相对位姿误差。
3.4、二维码的位置和姿态的先验全局约束更新
遍历最新帧各目中观测到所有二维码，如果第i个二维码存在已知先验的朝向和姿态的全局约束（例如：第i个二维码在世界坐标系下的姿态均值为5度，其方差为3度，和或全局位置的均值100米，方差为1米），则在测量更新模型中构建二维码的位置和姿态的先验全局约束，其状态方程和误差方差类似3.3中所述。
3.5、轮速里程计的测量约束更新
当预测模型中，不采用轮速里程计测量进行递推时，在状态更新时可将轮速里程计预积分结果作为测量约束进行更新。
3.6、时间偏置在线标定
本申请将真实时钟为相机的采集时钟，故图像时钟下的状态可近似建模为当前帧IMU或轮速里程计状态递推估计值叠加上一个误差项，该误差项对应的时间即近似为时间偏置。以IMU和相机的时间偏置为例，其状态方程如下：
Figure 867767DEST_PATH_IMAGE150
其中，
Figure 360059DEST_PATH_IMAGE151
为当前帧IMU状态递推估计值；
Figure 509281DEST_PATH_IMAGE152
为时间偏置的估计误 差。
四、二维码地图输出
经过上述EKF状态更新后，可得到优化后的各二维码的全局位姿以及全局位姿的协方差。其中， 各二维码的全局位姿可作为机器人定位过程的已知量，各二维码全局位姿的协方差可作为机器人定位过程中二维码位姿可信度或不确定性。
本申请在预设标签的观测获取中，针对多帧多目观测采用了紧耦合的滑窗集束优化，在得到鲁棒高精度的预设标签观测后，采用松耦合的EKF优化得到预设标签的地图。EKF更新后的各传感器间外参和时间偏置会被实时反馈给集束优化。故本申请，采用松紧融合的方式，既兼顾预设标签观测精度和稳定性，又兼顾了计算算力要求。
本申请支持预设标签的随意张贴，从而极大简化预设标签粘贴过程对现场部署要求，有利于视觉移动机器人的大规模推广。为避免根据单次预设标签的观测出现的图像模糊、位姿歧义现象、噪声等位姿估计不稳定性情况，本申请采用将多帧多目观测到预设标签信息，作为重投影误差约束，加入非线性集束优化，得到优化后预设标签的位姿作为加入EKF的观测。为尽可能利用的预设标签粘贴过程的先验信息，本申请还可将预设标签之间的相对或全局的朝向和距离约束，作为可选的先验信息，用来提高预设标签建图精度。
此外，在多传感器融合建图过程中，各传感器的外参精度会极大影响建图效果，本申请在EKF优化过程中加入了各传感器外参的在线优化。为了解决多传感器之间的时钟不同步问题，本申请中还加入了各传感器间时间偏置的在线估计。另外，在多帧多目的预设标签的集束优化中，提出了基于预设标签观测信息的边缘化测量。
基于同样的构思，本申请实施例提供一种机器人建图装置，如图5所示，包括：集束优化单元51、状态预测单元52以及测量更新单元53。
其中，集束优化单元51，用于将预设时间段内采集的连续多帧图像数据、惯性测量单元IMU数据以及轮速里程计数据进行集束优化，获取当前帧图像中预设标签在机器人的相机坐标系的优化位姿及所述优化位姿的方差值；状态预测单元52，用于对当前帧图像对应的所述IMU数据、所述轮速里程计数据进行扩展卡尔曼EKF的状态递推，确定所述机器人的位姿和所述机器人的位姿的方差值；测量更新单元53，用于将所述预设标签的优化位姿、所述预设标签优化位姿的方差值、所述机器人的位姿和所述机器人的位姿的方差值进行EKF测量的迭代更新，获取预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿和目标位姿的方差值。
本申请实施例中，机器人在建图时，在采用连续多帧的图像的同时且参考了IMU数据以及轮速里程计数据，通过该方式也可以保证建图的精确度。另外，本申请在建图时还融合了集束优化与EKF的状态预测的方法，使得机器人建图的定位精确度更高，且有助于更快地计算出预设标签在世界坐标系中的目标位姿和目标位姿的方差值。
在一种可能的实现方式中，测量更新单元53还可在EKF的测量的迭代更新过程中对各传感器之间的外参和时间偏置进行在线优化，并将优化后的外参和时间偏置用于集束优化；所述外参包括：IMU与相机的相对位姿，以及轮速里程计与相机的相对位姿。
本申请实施例中，在集束优化时加入外参和时间偏置，可以使得获取的当前帧图像中预设标签在机器人的相机坐标系的优化位姿更加可靠，进而使得机器人建图的定位精确度更高。
在一种可能的实现方式中，集束优化单元51具体用于：通过机器人的相机获取预设时间段内采集的连续N帧图像，并对N帧图像进行筛选，确定包括预设标签的M帧图像；N、M为整数；M大于或等于2，小于或等于N；通过对M帧图像中预设标签的角点的位置信息、预设标签的角点在机器人的相机所在坐标系的投影信息、IMU数据以及轮速里程计数据进行集束优化，确定预设标签在机器人的相机所在坐标系的优化位姿、优化位姿的方差值、预设标签的投影误差、雅克比矩阵和协方差矩阵。
本申请实施例中，机器人通过对M帧图像中预设标签的角点的位置信息、预设标签的角点在机器人的相机所在坐标系的投影信息、IMU数据以及轮速里程计数据进行集束优化，可确定预设标签在机器人的相机所在坐标系的优化位姿、优化位姿的方差值、预设标签的投影误差、雅克比矩阵和协方差矩阵。通过该方式可提高机器人建图精确度。
在一种可能的实现方式中，集束优化的滑动窗口中连续多帧图像通过如下方式确定：
若滑动窗口中图像帧的帧数小于或等于滑窗阈值，则将所有的图像帧进行集束优化；若所述滑动窗口中图像帧的帧数大于滑窗阈值，则将所有的图像帧进行集束优化之后筛选出预设帧图像，并通过舒尔补方法保留对被剔除图像的滑动窗口的约束。
本申请实施例中，在滑动窗口对图像帧进行筛选，使得机器人建图的定位精确度更高。
在一种可能的实现方式中，筛选出预设帧图像进行集束优化通过以下方式中的一种或多种确定：
获取当前帧图像中所有预设标签的标识列表；
剔除多帧图像中不包括预设标签的图像；若在第N帧图像中检测的预设标签的标识与第M帧图像中预设标签的标识不同，则剔除第N帧图像；若在预设时间段内任一帧图像中预设标签的标识与第M帧图像中预设标签的标识均相同，则对除第M帧图像以外的图像按照信息度进行打分，剔除分数最低的帧图像；其中，M、N为整数，M为预设时间段内采集的最后一帧图像的数目。
本申请实施例中，通过筛选预设帧图像进行技术优化，使得预设标签的优化位姿更加可靠。
在一种可能的实现方式中，预设标签的投影误差及雅可比矩阵可通过如下方式确定：
通过透视N点投影PNP算法对所述M帧图像中所述预设标签的角点的二维坐标信息进行计算，确定所述预设标签在机器人的相机坐标系的初始位姿；根据所述预设标签在机器人的相机坐标系的初始位姿以及所述机器人的初始位姿，确定所述预设标签的各个角点的在世界坐标系的三维坐标信息；将所述预设标签的角点的三维坐标信息投影至所述相机所在坐标系中，得到投影后的预设标签的角点的二维坐标信息；将所述投影后的预设标签的角点的二维坐标信息与投影前所述预设标签的角点的二维坐标信息进行差值计算，确定所述预设标签的投影误差；确定所述预设标签的投影误差相对滑动窗口中优化状态量的雅克比矩阵。
本申请实施例中，通过透视N点投影（pespective-n-point，PNP）算法，根据第i个预设标签中角点在预设标签坐标系下的三维坐标，以及首次观测第i个预设标签的图像帧M中预设标签的二维像素坐标，确定第i个预设标签在第M帧图像中相机坐标系的初始位姿；根据第i个预设标签在第M帧图像中相机坐标系的初始位姿以及第M帧图像中相机的全局位姿，确定预设标签的角点的全局三维坐标信息，之后通过确定预设标签的投影误差可以保证机器人建图的定位精确度。
在一种可能的实现方式中，目标位姿的方差值与滑动窗口各预设标签的角点观测不确定性相关；其中任一预设标签的角点在任一图像帧中观测不确定性与以下参数存在函数关系：
预设标签的角点的二维坐标信息、预设标签所在图像帧的图像采集装置的线速度、角速度、加速度以及预设标签在图像帧中的像素面积。
本申请实施例中，预设标签的目标位姿的方差值与多种参数存在函数关系，故此可知，本申请获取的目标位姿的方差值更符合实际场景。
在一种可能的实现方式中，集束优化单元51具体用于：将预设标签的先验信息作为约束项加入集束优化和测量更新；其中，所述预设标签的先验信息为预先确定的任意两个预设标签的相对位姿及不确定性方差，或任一预设标签在世界坐标系的全局位姿及不确定性方差。
本申请实施例中，机器人还可基于先验信息调整预设标签在世界坐标系的位姿，通过该方式使得机器人建图的精确度更高，建图的数据更加可靠。
在一种可能的实现方式中，机器人可对对预设标签的投影约束、预设标签的先验相对位姿约束、预设标签的先验全局位姿约束、轮速里程计的测量约束以及时间偏置约束进行EKF测量的迭代更新，得到更新后的EKF状态。
本申请实施例中，通过更新预设标签的投影约束、预设标签的位姿约束以及时间偏置等，可以提高机器人建图的精确度。
在一种可能的实现方式中，所述EKF状态包括：所述预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿、各传感器之间的外参、各传感器之间的时间偏置、当前帧机器人的位姿和速度以及IMU的零偏。
在介绍了本申请示例性实施方式中的机器人建图方法、装置之后，接下来，介绍本申请的另一示例性实施方式的计算设备。
所属技术领域的技术人员能够理解，本申请的各个方面可以实现为***、方法或程序产品。因此，本申请的各个方面可以具体实现为以下形式，即：完全的硬件实施方式、完全的软件实施方式（包括固件、微代码等），或硬件和软件方面结合的实施方式，这里可以统称为“电路”、“模块”或“***”。
在一些可能的实施方式中，根据本申请的计算设备可以至少包括至少一个处理器、以及至少一个存储器。其中，存储器存储有计算机程序，当计算机程序被处理器执行时，使得处理器执行本说明书上述描述的根据本申请各种示例性实施方式的机器人建图方法中的步骤。例如，处理器可以执行如图2中所示的步骤201-步骤203。
下面参照图6来描述根据本申请的这种实施方式的计算设备130。图6显示的计算设备130仅仅是一个示例，不应对本申请实施例的功能和使用范围带来任何限制。如图6所示，计算设备130以通用智能终端的形式表现。计算设备130的组件可以包括但不限于：上述至少一个处理器131、上述至少一个存储器132、连接不同***组件（包括存储器132和处理器131）的总线133。
总线133表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器、***总线、处理器或者使用多种总线结构中的任意总线结构的局域总线。存储器132可以包括易失性存储器形式的可读介质，例如随机存取存储器（RAM）1321和/或高速缓存存储器1322，还可以进一步包括只读存储器（ROM）1323。存储器132还可以包括具有一组（至少一个）程序模块1324的程序/实用工具1325，这样的程序模块1324包括但不限于：操作***、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。
计算设备130也可以与一个或多个外部设备134（例如键盘、指向设备等）通信，和/或与使得该计算设备130能与一个或多个其它智能终端进行通信的任何设备（例如路由器、调制解调器等等）通信。这种通信可以通过输入/输出（I/O）接口135进行。并且，智能终端130还可以通过网络适配器136与一个或者多个网络（例如局域网（LAN），广域网（WAN）和/或公共网络，例如因特网）通信。如图所示，网络适配器136通过总线133与用于计算设备130的其它模块通信。应当理解，尽管图中未示出，可以结合计算设备130使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理器、外部磁盘驱动阵列、RAID***、磁带驱动器以及数据备份存储***等。
在一些可能的实施方式中，本申请提供的三维视觉重定位方法的各个方面还可以实现为一种程序产品的形式，其包括计算机程序，当程序产品在计算机设备上运行时，计算机程序用于使计算机设备执行本说明书上述描述的根据本申请各种示例性实施方式的机器人建图方法中的步骤。例如，处理器可以执行如图2中所示的步骤201-步骤203。
程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的***、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子（非穷举的列表）包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
本申请的实施方式的用于机器人建图的程序产品可采用便携式紧凑盘只读存储器(CD-ROM)并包括计算机程序，并可在智能终端上运行。但本申请的程序产品不限于此，在本文件中，可读存储介质可以是任何包含或存储程序的有形介质，该程序可被指令执行***、装置或者器件使用或者与其结合使用。
可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了可读计算机程序。这种传播的数据信号可以采用多种形式，包括——但不限于——电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质，该可读介质可以发送、传播或者传输用于由指令执行***、装置或者器件使用或者与其结合使用的程序。
应当注意，尽管在上文详细描述中提及了装置的若干单元或子单元，但是这种划分仅仅是示例性的并非强制性的。实际上，根据本申请的实施方式，上文描述的两个或更多单元的特征和功能可以在一个单元中具体化。反之，上文描述的一个单元的特征和功能可以进一步划分为由多个单元来具体化。
此外，尽管在附图中以特定顺序描述了本申请方法的操作，但是，这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。
本申请是参照根据本申请实施例的方法、设备（***）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本申请的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本申请范围的所有变更和修改。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (13)

1.一种机器人建图方法，其特征在于，包括：
将预设时间段内采集的连续多帧图像数据、惯性测量单元IMU数据以及轮速里程计数据进行集束优化，获取当前帧图像中预设标签在机器人的相机坐标系的优化位姿及所述优化位姿的方差值；
对当前帧图像对应的所述IMU数据、所述轮速里程计数据进行扩展卡尔曼EKF的状态递推，确定所述机器人的位姿和所述机器人的位姿的方差值；
将所述预设标签的优化位姿、所述预设标签优化位姿的方差值、所述机器人的位姿和所述机器人的位姿的方差值进行EKF测量的迭代更新，获取预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿和目标位姿的方差值。
2.根据权利要求1所述的方法，其特征在于，还包括：
在EKF的测量的迭代更新过程中对各传感器之间的外参和时间偏置进行在线优化，并将优化后的外参和时间偏置用于集束优化；所述外参包括：IMU与相机的相对位姿，以及轮速里程计与相机的相对位姿。
3.根据权利要求1所述的方法，其特征在于，所述将预设时间段内采集的连续多帧图像、惯性测量单元IMU数据、轮速里程计数据进行集束优化，获取当前帧图像中各个预设标签在机器人的相机坐标系的优化位姿及所述优化位姿的方差值，包括：
通过机器人的相机获取预设时间段内采集的连续N帧图像，并对所述N帧图像进行筛选，确定包括预设标签的M帧图像；所述N、M为整数；所述M大于或等于2，小于或等于N;
通过对所述M帧图像中所述预设标签的角点的位置信息、所述预设标签的角点在机器人的相机所在坐标系的投影信息、所述IMU数据以及所述轮速里程计数据进行集束优化，确定所述预设标签在机器人的相机所在坐标系的优化位姿、所述优化位姿的方差值、所述预设标签的投影误差、雅克比矩阵和协方差矩阵。
4.根据权利要求3所述的方法，其特征在于，所述集束优化的滑动窗口中连续多帧图像通过如下方式确定：
若所述滑动窗口中图像帧的帧数小于或等于滑窗阈值，则将所有的图像帧进行集束优化；
若所述滑动窗口中图像帧的帧数大于滑窗阈值，则将所有的图像帧进行集束优化之后筛选出预设帧图像，并通过舒尔补方法保留对被剔除图像的滑动窗口的约束。
5.根据权利要求4所述的方法，其特征在于，所述筛选出预设帧图像通过以下方式中的一种或多种确定：
获取当前帧图像中所有预设标签的标识列表；
剔除所述多帧图像中不包括所述预设标签的图像；
若在第N帧图像中检测的预设标签的标识与第M帧图像中预设标签的标识不同，则剔除所述第N帧图像；
若在所述预设时间段内任一帧图像中预设标签的标识与所述第M帧图像中预设标签的标识均相同，则对除所述第M帧图像以外的图像按照信息度进行打分，剔除分数最低的帧图像；
其中，所述M、N为整数，所述M为所述预设时间段内采集的最后一帧图像的数目。
6.根据权利要求3所述的方法，其特征在于，所述预设标签的投影误差及雅可比矩阵可通过如下方式确定：
通过透视N点投影PNP算法对所述M帧图像中所述预设标签的角点的二维坐标信息进行计算，确定所述预设标签在机器人的相机坐标系的初始位姿；
根据所述预设标签在机器人的相机坐标系的初始位姿以及所述机器人的初始位姿，确定所述预设标签的各个角点的在世界坐标系的三维坐标信息；
将所述预设标签的角点的三维坐标信息投影至所述相机所在坐标系中，得到投影后的预设标签的角点的二维坐标信息；
将所述投影后的预设标签的角点的二维坐标信息与投影前所述预设标签的角点的二维坐标信息进行差值计算，确定所述预设标签的投影误差；
确定所述预设标签的投影误差相对滑动窗口中优化状态量的雅克比矩阵。
7.根据权利要求1-6中任一所述的方法，其特征在于，所述目标位姿的方差值与滑动窗口各预设标签的角点观测不确定性相关；其中，任一预设标签的角点在任一图像帧中观测不确定性与以下参数存在函数关系：
所述预设标签的角点的二维坐标信息、所述预设标签所在图像帧的图像采集装置的线速度、角速度、加速度以及所述预设标签在图像帧中的像素面积。
8.根据权利要求1-6中任一所述的方法，其特征在于，还包括：
将预设标签的先验信息作为约束项加入集束优化和测量更新；其中，所述预设标签的先验信息为预先确定的任意两个预设标签的相对位姿及不确定性方差，或任一预设标签在世界坐标系的全局位姿及不确定性方差。
9.根据权利要求8所述的方法，其特征在于，还包括：
对预设标签的投影约束、预设标签的先验相对位姿约束、预设标签的先验全局位姿约束、轮速里程计的测量约束以及时间偏置约束进行EKF测量的迭代更新，得到更新后的EKF状态。
10.根据权利要求9所述的方法，其特征在于，所述EKF状态包括：所述预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿、各传感器之间的外参、各传感器之间的时间偏置、当前帧机器人的位姿和速度以及IMU的零偏。
11.一种机器人建图装置，其特征在于，包括：
集束优化单元，用于将预设时间段内采集的连续多帧图像数据、惯性测量单元IMU数据以及轮速里程计数据进行集束优化，获取当前帧图像中预设标签在机器人的相机坐标系的优化位姿及所述优化位姿的方差值；
状态预测单元，用于对当前帧图像对应的所述IMU数据、所述轮速里程计数据进行扩展卡尔曼EKF的状态递推，确定所述机器人的位姿和所述机器人的位姿的方差值；
测量更新单元，用于将所述预设标签的优化位姿、所述预设标签优化位姿的方差值、所述机器人的位姿和所述机器人的位姿的方差值进行EKF测量的迭代更新，获取预设时间段内采集的连续多帧图像中预设标签在世界坐标系的目标位姿和目标位姿的方差值。
12.一种计算设备，其特征在于，包括：存储器以及处理器；
存储器，用于存储程序指令；
处理器，用于调用所述存储器中存储的程序指令，按照获得的程序执行权利要求1-10任一项所述的方法。
13.一种计算机存储介质，存储有计算机可执行指令，其特征在于，所述计算机可执行指令用于执行如权利要求1-10中任一权利要求所述的方法。
CN202110348706.7A 2021-03-31 2021-03-31 一种机器人建图方法、装置及计算设备 Active CN112734852B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110348706.7A CN112734852B (zh) 2021-03-31 2021-03-31 一种机器人建图方法、装置及计算设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110348706.7A CN112734852B (zh) 2021-03-31 2021-03-31 一种机器人建图方法、装置及计算设备

Publications (2)

Publication Number Publication Date
CN112734852A true CN112734852A (zh) 2021-04-30
CN112734852B CN112734852B (zh) 2021-06-29

Family

ID=75596233

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110348706.7A Active CN112734852B (zh) 2021-03-31 2021-03-31 一种机器人建图方法、装置及计算设备

Country Status (1)

Country Link
CN (1) CN112734852B (zh)

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111401445A (zh) * 2020-03-16 2020-07-10 腾讯科技（深圳）有限公司 一种图像识别模型的训练方法、图像识别的方法及装置
CN113052855A (zh) * 2021-02-26 2021-06-29 苏州迈思捷智能科技有限公司 一种基于视觉-imu-轮速计融合的语义slam方法
CN113379915A (zh) * 2021-07-05 2021-09-10 广东工业大学 一种基于点云融合的行车场景构建方法
CN113390408A (zh) * 2021-06-30 2021-09-14 深圳市优必选科技股份有限公司 一种机器人定位方法、装置、机器人及存储介质
CN113658260A (zh) * 2021-07-12 2021-11-16 南方科技大学 机器人位姿计算方法、***、机器人及存储介质
CN113792564A (zh) * 2021-09-29 2021-12-14 北京航空航天大学 基于不可见投影二维码的室内定位方法
CN113838129A (zh) * 2021-08-12 2021-12-24 高德软件有限公司 一种获得位姿信息的方法、装置以及***
CN114018284A (zh) * 2021-10-13 2022-02-08 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114523471A (zh) * 2022-01-07 2022-05-24 中国人民解放军海军军医大学第一附属医院 基于关联标识的误差检测方法及机器人***
CN114638886A (zh) * 2022-03-18 2022-06-17 杭州唯精医疗机器人有限公司 一种分体式微创手术机器人高精度位姿关系标定方法
CN115388902A (zh) * 2022-10-28 2022-11-25 苏州工业园区测绘地理信息有限公司 室内定位方法和***、ar室内定位导航方法和***
EP4130924A1 (en) * 2021-08-04 2023-02-08 Tata Consultancy Services Limited Method and system of dynamic localization of a telepresence robot based on live markers
WO2024037295A1 (zh) * 2022-08-16 2024-02-22 北京三快在线科技有限公司 定位
CN117824667A (zh) * 2024-03-06 2024-04-05 成都睿芯行科技有限公司 一种基于二维码和激光的融合定位方法及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080144925A1 (en) * 2006-08-15 2008-06-19 Zhiwei Zhu Stereo-Based Visual Odometry Method and System
CN107145578A (zh) * 2017-05-08 2017-09-08 深圳地平线机器人科技有限公司 地图构建方法、装置、设备和***
CN108180913A (zh) * 2018-01-03 2018-06-19 深圳勇艺达机器人有限公司 一种基于3d摄像头的二维码定位***
CN110163909A (zh) * 2018-02-12 2019-08-23 北京三星通信技术研究有限公司 用于获得设备位姿的方法、装置和存储介质
CN110702107A (zh) * 2019-10-22 2020-01-17 北京维盛泰科科技有限公司 一种单目视觉惯性组合的定位导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080144925A1 (en) * 2006-08-15 2008-06-19 Zhiwei Zhu Stereo-Based Visual Odometry Method and System
CN107145578A (zh) * 2017-05-08 2017-09-08 深圳地平线机器人科技有限公司 地图构建方法、装置、设备和***
CN108180913A (zh) * 2018-01-03 2018-06-19 深圳勇艺达机器人有限公司 一种基于3d摄像头的二维码定位***
CN110163909A (zh) * 2018-02-12 2019-08-23 北京三星通信技术研究有限公司 用于获得设备位姿的方法、装置和存储介质
CN110702107A (zh) * 2019-10-22 2020-01-17 北京维盛泰科科技有限公司 一种单目视觉惯性组合的定位导航方法

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111401445A (zh) * 2020-03-16 2020-07-10 腾讯科技（深圳）有限公司 一种图像识别模型的训练方法、图像识别的方法及装置
CN113052855A (zh) * 2021-02-26 2021-06-29 苏州迈思捷智能科技有限公司 一种基于视觉-imu-轮速计融合的语义slam方法
CN113052855B (zh) * 2021-02-26 2021-11-02 苏州迈思捷智能科技有限公司 一种基于视觉-imu-轮速计融合的语义slam方法
CN113390408A (zh) * 2021-06-30 2021-09-14 深圳市优必选科技股份有限公司 一种机器人定位方法、装置、机器人及存储介质
CN113379915A (zh) * 2021-07-05 2021-09-10 广东工业大学 一种基于点云融合的行车场景构建方法
CN113658260A (zh) * 2021-07-12 2021-11-16 南方科技大学 机器人位姿计算方法、***、机器人及存储介质
EP4130924A1 (en) * 2021-08-04 2023-02-08 Tata Consultancy Services Limited Method and system of dynamic localization of a telepresence robot based on live markers
CN113838129A (zh) * 2021-08-12 2021-12-24 高德软件有限公司 一种获得位姿信息的方法、装置以及***
CN113838129B (zh) * 2021-08-12 2024-03-15 高德软件有限公司 一种获得位姿信息的方法、装置以及***
CN113792564A (zh) * 2021-09-29 2021-12-14 北京航空航天大学 基于不可见投影二维码的室内定位方法
CN113792564B (zh) * 2021-09-29 2023-11-10 北京航空航天大学 基于不可见投影二维码的室内定位方法
CN114018284B (zh) * 2021-10-13 2024-01-23 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114018284A (zh) * 2021-10-13 2022-02-08 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114523471B (zh) * 2022-01-07 2023-04-25 中国人民解放军海军军医大学第一附属医院 基于关联标识的误差检测方法及机器人***
CN114523471A (zh) * 2022-01-07 2022-05-24 中国人民解放军海军军医大学第一附属医院 基于关联标识的误差检测方法及机器人***
CN114638886A (zh) * 2022-03-18 2022-06-17 杭州唯精医疗机器人有限公司 一种分体式微创手术机器人高精度位姿关系标定方法
WO2024037295A1 (zh) * 2022-08-16 2024-02-22 北京三快在线科技有限公司 定位
CN115388902A (zh) * 2022-10-28 2022-11-25 苏州工业园区测绘地理信息有限公司 室内定位方法和***、ar室内定位导航方法和***
CN117824667A (zh) * 2024-03-06 2024-04-05 成都睿芯行科技有限公司 一种基于二维码和激光的融合定位方法及介质
CN117824667B (zh) * 2024-03-06 2024-05-10 成都睿芯行科技有限公司 一种基于二维码和激光的融合定位方法及介质

Also Published As

Publication number Publication date
CN112734852B (zh) 2021-06-29

Similar Documents

Publication Publication Date Title
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
Cvišić et al. SOFT‐SLAM: Computationally efficient stereo visual simultaneous localization and mapping for autonomous unmanned aerial vehicles
CN112634451B (zh) 一种融合多传感器的室外大场景三维建图方法
CN110555901B (zh) 动静态场景的定位和建图方法、装置、设备和存储介质
CN113168717B (zh) 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达
CN111210477B (zh) 一种运动目标的定位方法及***
CN110587597B (zh) 一种基于激光雷达的slam闭环检测方法及检测***
JP2018124787A (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
Chien et al. Visual odometry driven online calibration for monocular lidar-camera systems
CN111623773A (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN112541423A (zh) 一种同步定位与地图构建方法和***
CN116977628A (zh) 一种应用于动态环境的基于多模态语义框架的slam方法及***
CN117218350A (zh) 一种基于固态雷达的slam实现方法及***
CN116643291A (zh) 一种视觉与激光雷达联合剔除动态目标的slam方法
CN115077519A (zh) 基于模板匹配与激光惯导松耦合的定位建图方法和装置
CN115147344A (zh) 一种增强现实辅助汽车维修中的零件三维检测与跟踪方法
Wei et al. Novel robust simultaneous localization and mapping for long-term autonomous robots
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及***
CN116958452A (zh) 三维重建方法和***
Andersson et al. Simultaneous localization and mapping for vehicles using ORB-SLAM2
Ye et al. Robust and efficient vehicles motion estimation with low-cost multi-camera and odometer-gyroscope
CN111862146A (zh) 一种目标对象的定位方法及装置
CN114862953A (zh) 一种基于视觉特征和3d激光的移动机器人重定位方法及装置
Xia et al. YOLO-Based Semantic Segmentation for Dynamic Removal in Visual-Inertial SLAM

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