CN112183171A - 一种基于视觉信标建立信标地图方法、装置 - Google Patents

一种基于视觉信标建立信标地图方法、装置 Download PDF

Info

Publication number
CN112183171A
CN112183171A CN201910603494.5A CN201910603494A CN112183171A CN 112183171 A CN112183171 A CN 112183171A CN 201910603494 A CN201910603494 A CN 201910603494A CN 112183171 A CN112183171 A CN 112183171A
Authority
CN
China
Prior art keywords
visual beacon
beacon
visual
global
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.)
Pending
Application number
CN201910603494.5A
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.)
Hangzhou Hikrobot Technology Co Ltd
Original Assignee
Hangzhou Hikrobot 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 Hangzhou Hikrobot Technology Co Ltd filed Critical Hangzhou Hikrobot Technology Co Ltd
Priority to CN201910603494.5A priority Critical patent/CN112183171A/zh
Priority to EP20837645.9A priority patent/EP3995988A4/en
Priority to PCT/CN2020/100306 priority patent/WO2021004416A1/zh
Priority to KR1020227002958A priority patent/KR20220025028A/ko
Priority to JP2022500643A priority patent/JP7300550B2/ja
Publication of CN112183171A publication Critical patent/CN112183171A/zh
Pending legal-status Critical Current

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
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/383—Indoor data
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3837—Data obtained from a single source
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06K—GRAPHICAL DATA READING; PRESENTATION OF DATA; RECORD CARRIERS; HANDLING RECORD CARRIERS
    • G06K7/00—Methods or arrangements for sensing record carriers, e.g. for reading patterns
    • G06K7/10—Methods or arrangements for sensing record carriers, e.g. for reading patterns by electromagnetic radiation, e.g. optical sensing; by corpuscular radiation
    • G06K7/10544—Methods or arrangements for sensing record carriers, e.g. for reading patterns by electromagnetic radiation, e.g. optical sensing; by corpuscular radiation by scanning of the records by radiation in the optical part of the electromagnetic spectrum
    • G06K7/10712—Fixed beam scanning
    • G06K7/10722—Photodetector array or CCD scanning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/24—Aligning, centring, orientation detection or correction of the image
    • G06V10/245—Aligning, centring, orientation detection or correction of the image by locating a pattern; Special marks for positioning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/10—Terrestrial scenes
    • 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

Abstract

公开一种基于视觉信标建立信标地图的方法，获取至少包括视觉信标图像帧的视觉信标检测数据、和图像帧采集时刻机器人本体的里程计测量数据；基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果；对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得视觉信标全局位姿优化结果；基于视觉信标全局位姿优化结果，建立信标地图。本发明获得精确的信标地图，降低了对建图数据集的要求。

Description

一种基于视觉信标建立信标地图方法、装置
技术领域
本发明涉及计算视觉领域，特别地，涉及一种基于视觉信标建立信标地图的方法。
背景技术
在计算视觉中，位于空间的物体(刚体)需要用位置和姿态来描述，合称为位姿，其中，位置是指物体在三维空间坐标系下的坐标位置，姿态是指物体在该坐标系下相对三个坐标轴的旋转角度，包括俯仰、偏转、横滚。
视觉信标导航技术是常见的机器人导航方案之一。机器人基于由视觉信标所构建的信标地图来实现定位。现有的视觉信标建图通常需要事先对工作环境进行物理测量，然后在特定的位置布置视觉信标，例如二维码；再通过建图机器人获取视觉信标的图像信息进行视觉计算(该过程也是机器人视觉定位的过程)，以获得视觉信标在世界坐标系下的位姿，即视觉信标的全局位姿，基于视觉信标的全局位姿形成信标地图，最后，根据视觉信标的全局位姿和视觉信标的定位信息对建图的偏差进行修正。
现有视觉信标建图方法主要存在的问题包括：(1)环境测量及信标布置工作繁琐，消耗大量人力及时间；(2)人工布置视觉信标的精度无法保证，贴码误差、甚至错误将会导致机器人定位结果存在较大的误差，乃至定位失败。(3)视觉信标定位信息和获取的视觉信标全局位姿的准确性对于建图有直接的影响。
发明内容
本发明提供了一种基于视觉信标的建立信标地图方法，以减少对视觉信标定位信息的依赖。
本发明提供的一种基于视觉信标建立信标地图的方法，是这样实现的：
获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；
基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果；
对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得视觉信标全局位姿优化结果；其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述视觉信标全局位姿的初始值为所述视觉信标全局位姿初始化结果；
基于视觉信标全局位姿优化结果，建立信标地图。
较佳地，所述基于视觉信标检测数据，获取视觉信标全局位姿初始化结果之后，进一步包括，
获取图像帧中视觉信标的局部位置测量约束残差，
对所述图像帧的里程计约束残差、以及所述局部位置测量约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、及以视觉信标全局位置为状态变量的联合优化，获得图像帧机器人本体全局位姿的初步优化结果、和视觉信标全局位置的初步优化结果；其中，所述视觉信标全局位置的初始值为所述视觉信标全局位姿初始化结果所包括的视觉信标全局位置初始化结果；
将所述图像帧机器人本体全局位姿的初步优化结果作为所述图像帧机器人本体全局位姿初始化结果；
所述基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果，包括，
基于视觉信标首次被观测的图像帧相机初始位姿、及视觉信标检测数据，获取视觉信标全局姿态初始化结果；
将所述视觉信标全局位置的初步优化结果、以及视觉信标全局姿态初始化结果作为所述视觉信标全局位姿初始化结果。
其中，所述进行以所述图像帧机器人本体全局位姿为状态变量、及以视觉信标全局位置为状态变量的联合优化，包括：
构建融合所述里程计约束残差和所述视觉信标的局部位置测量约束残差的第一目标函数，通过以各个所述图像帧机器人本体全局位姿初始化结果、和各个所述视觉信标全局位置初始化结果为迭代初始值的迭代，获取使得所述第一目标函数为最小值时的各个图像帧机器人本体全局位姿的初步优化结果、和各个视觉信标全局位置的初步优化结果。
其中，所述进行以所述图像帧机器人本体全局位姿为状态变量、及视觉信标全局位姿为状态变量的联合优化，包括：
构建融合所述里程计约束残差和所述重投影约束残差的第二目标函数，通过以各个所述图像帧机器人本体全局位姿初始化结果、和各个所述视觉信标全局位姿初始化结果为迭代初始值的迭代，获取使得所述第二目标函数为最小值时的各个视觉信标全局位姿优化结果。
其中，所述基于所述里程计测量数据获取的图像帧的里程计约束残差，包括：
对任一所述图像帧，
基于所述里程计测量数据，获取所述图像帧与其相邻图像帧之间的机器人本体相对位姿、和所述图像帧与其相邻图像帧之间的里程计相对位姿；
由所述机器人本体相对位姿和里程计相对位姿的差异计算所述图像帧的里程计约束残差，
累计各个所述图像帧的里程计约束残差，得到所有所述图像帧的里程计约束残差。
所述获取图像帧中视觉信标的局部位置测量约束残差，包括，
对任一图像帧中的任一视觉信标，获取该视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置与该视觉信标被观测测量值之间的差异，得到该视觉信标的局部位置测量约束残差，
其中，
所述视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置通过该视觉相机外参位姿、图像帧机器人本体全局位姿当前状态变量、以及视觉信标的全局位置当前状态变量获得；所述图像帧机器人本体全局位姿当前状态变量在第一次迭代时为图像帧机器人本体全局位姿初始化结果；所述视觉信标的全局位置当前状态变量在第一次迭代时为视觉信标的全局位置初始化结果；
所述视觉信标被观测测量值通过该视觉信标相对于相机的局部位姿中的局部位置获得；
累计各个图像帧中的各个视觉信标的局部位置测量约束残差，得到所有图像帧中所有视觉信标的局部位置测量约束残差。
所述基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上的重投影约束残差，包括：
根据各个所述视觉信标全局位姿当前状态变量、以及各个所述视觉信标检测数据，获取所述图像帧中所有视觉信标的所有特征点在该图像帧上的重投影约束残差。
所述根据各个所述视觉信标全局位姿当前状态变量、以及各个所述视觉信标检测数据，获取所述图像帧中所有视觉信标的所有特征点在该图像帧上的重投影约束残差，包括，
对于任一图像帧中的任一视觉信标所具有的任一特征点，根据相机内参矩阵、图像帧机器人本体全局位姿当前状态变量、该视觉信标的全局位姿当前状态变量、以及该特征点在该视觉信标坐标系下的局部位置的投影函数与该特征点的图像坐标的测量值之差异，获得该视觉信标中该特征点在其图像帧上重投影误差，
累计各个图像帧中的各个视觉信标的各个特征点的重投影误差，得到所有图像帧中所有视觉信标的所有特征点在图像帧上的重投影约束残差。
所述基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果包括，
将各个所述图像帧机器人本体全局位姿的初始化结果直接采用对应图像帧的里程计测量数据结果；
所述基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果，包括：
对于图像帧中任一视觉信标，通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，
其中，所述视觉信标检测数据包括视觉信标被观测测量值，从视觉信标相对相机的局部位姿获得；
所述视觉信标相对于相机的局部位姿根据视觉信标中特征点的图像坐标、相机内参、以及信标参数获得。
所述通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，包括：
根据图像帧机器人本体全局位姿向量、相机外参位姿向量、以及视觉信标相对相机的局部位置向量，获得所述视觉信标全局位置初始化结果；
根据图像帧机器人本体坐标系的全局旋转向量、相机外参旋转向量、视觉信标在相机坐标系中的局部旋转向量，获得所述视觉信标全局姿态初始化结果，
其中，
所述图像帧机器人本体坐标系的全局旋转向量从所述图像帧机器人本体全局位姿初始化结果获得，
所述相机外参旋转向量从所述相机外参位姿获得，
所述视觉信标在相机坐标系中的局部旋转向量从所述视觉信标相对相机的局部位姿获得。
所述获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧被采集时刻机器人本体的里程计测量数据包括，采集布置于待建信标地图物理环境中的各个视觉信标的图像至少一次，在采集的同一时刻进行里程计的测量。
所述获取至少包括视觉信标图像帧的视觉信标检测数据、和至少包括所述视觉信标图像帧采样时刻的里程计测量数据之后，进一步包括，按照如下任一条件或其组合筛选出关键帧，从所述图像帧中至少筛选出关键帧：
当前帧里程计测量与已有关键帧里程计测量之间的位置平移偏差大于设定的平移阈值时，则将当前帧作为关键帧；
当前帧里程计测量与已有关键帧里程计测量之间的旋转偏差大于设定的旋转阈值时，则将当前帧作为关键帧；
第一帧为关键帧；
所述图像帧为筛选出的关键帧。
本发明提供的一种基于视觉信标建立信标地图的装置，
数据采集模块，获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
初始化模块，基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；基于视觉信标检测数据，获取视觉信标全局位姿初始化结果；
里程计约束与视觉信标特征点图像重投影约束联合优化模块，对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿初始化结果、和所述视觉信标全局位姿初始化结果为初始值的联合优化，获得视觉信标全局位姿优化结果；其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述视觉信标全局位姿的初始值为所述视觉信标全局位姿初始化结果；
建图模块，基于视觉信标全局位姿优化结果，建立信标地图。
本发明还提供一种基于视觉信标建立信标地图的电子设备，该电子设备包括存储器和处理器，其中，
所述存储器用于存放计算机程序；
所述处理器，用于执行所述存储器上所存放的程序，实现上述任一所述基于视觉信标建立信标地图的方法。
本发明还提供一种计算机可读存储介质，其特征在于，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行上述任一所述基于视觉信标建立信标地图的方法。
本发明利用里程计测量信息和视觉信标检测结果，通过里程计约束和信标观测约束(至少包括视觉信标的所有特征点在关键帧上的重投影约束)共同构建优化问题，并通过求解该问题获取视觉信标的全局位姿，从而获得精确的信标地图，降低了对建图数据集的要求，既不依赖视觉信标的全局位置或角度信息，也不依赖视觉信标的类型，又不依赖任何先验信息，同时提高了建图效率和精度。
附图说明
图1为本发明实施例视觉信标建图方法的一种流程图。
图2为图2为数据采集的一种示意图。
图3示出了以上述步骤101-109所涉及的图像数据的关联关系的一种示意图
图4为本发明实施例基于视觉信标建立信标地图的装置的一种示意图。
具体实施方式
为了使本申请的目的、技术手段和优点更加清楚明白，以下结合附图对本申请做进一步详细说明。
本发明利用视觉信标检测数据和里程计测量数据，通过分别构建里程计约束与视觉信标局部位置测量约束的第一目标函数、以及里程计约束与视觉信标特征点图像重投影约束的第二目标函数，并通过对第一目标函数和第二目标函数的求解联合优化问题，来获得视觉信标的全局位姿，从而构建信标地图。通过对原始数据进行筛选，获取关键帧数据，并针对关键帧进行优化计算，大幅提升了优化算法的计算效率。
为便于理解本发明实施例，对涉及的概念说明如下。
在本实施例中，涉及世界坐标系、摄像机坐标系(相机坐标系)、信标坐标系、里程计坐标系、机器人本体坐标系，其中，
世界坐标系是系统的绝对坐标系，在环境中选择一个基准坐标系来描述摄像机和环境中任何物体的位置；
摄像机坐标系是原点为摄像机光心，x轴与y轴与图像的X,Y轴平行，z轴为摄像机光轴，它与成像平面垂直，以此构成的空间直角坐标系，也称为相机坐标系，摄像机坐标系是三维坐标系。其中，光轴与图像平面的交点，即为图像坐标系的原点，与图像的X、Y轴构成的直角坐标系即为图像坐标系，图像坐标系是二维坐标系。
信标坐标系用于描述视觉信标中特征点的三维空间位置，例如，二维码中位于四个角上的定位特征点的位置。各个视觉信标分别具有各自信标坐标系。
机器人本体坐标系，与机器人中心重合，为机器人的本体的坐标系。
里程计坐标系，属于世界固定坐标系，用于描述机器人本体(机器人坐标系)在二维平面内位姿的坐标系，原点为开始计算位姿那个时刻的机器人的位置，坐标系中的数据来源于里程计。
里程计,一种针对移动机器人的累加式定位算法，根据所采用的传感器及机械结构的不同，可以有多种实现方式，其输出结果为机器人本体(机器人本体坐标系)在里程计坐标系二维平面内的位姿。
在以下文中，“局部”是与“全局”相对的概念，全局系指在世界坐标系而言，局部系指在非世界坐标系而言。
PnP(Perspective-n-Point)，又称为n点透视，通过图像中一组特征点(n个点)的坐标以及该特征点的三维位置，计算相机坐标系的位姿。其中，n至少为不相同的4个特征点。为构建信标地图，将待建的信标地图物理环境中随机布置一定数量的视觉信标，所述视觉信标包括有用于定位的特征点。
参见图1所示，图1为本发明实施例视觉信标建图方法的一种流程图。该建图方法包括，
步骤101，在视觉信标图像数据的采集过程中获取视觉信标检测数据和里程计测量数据。
参见图2所示，图2为数据采集的一种示意图。控制机器人移动，以使得机器人通过至少一个相机采集待建的信标地图物理环境中布置的各个视觉信标至少一次，获得至少包括视觉信标图像帧的视觉信标检测数据，并利用轮式编码器等方式采集里程计测量数据，该里程计测量数据至少包括一个图像帧采集时刻机器人本体测量数据。所述机器人移动轨迹没有限制，满足采集所有视觉信标至少一次即可；如图2中，共布置了4个视觉信标，机器人移动轨迹为里程计轨迹。
这样，数据采集过程中所采集的原始数据分别包括里程计测量数据和视觉信标检测数据，其中，里程计测量数据为机器人本体在里程计坐标系下的位姿；视觉信标检测数据包括采集的图像帧、信标ID、以及信标各个特征点的图像坐标。
此外，还可预先获取系统参数，包括：相机内参(光心位置、镜头焦距、畸变系数等)、相机外参(相机与机器人本体的相对位姿)、信标参数(各个特征点在信标坐标系中的局部坐标位置)。
在视觉信标图像数据的采集的过程中，通常还会采集到无视觉信标图像的图像帧以及该图像帧的里程计测量数据，这些数据也可以分别作为视觉信标检测数据和里程计测量数据，形成采集的原始数据。
步骤102，考虑到计算效率，需要对原始数据进行筛选，以便从大量的原始数据所包括的图像帧中筛选出关键帧。故而，基于所采集的原始数据，按照关键帧筛选策略，从原始数据中筛选出一定数量的关键帧。其中，关键帧筛选策略可以有多种，本实施例中的一种策略是：
当前帧中存在信标图像时，必须选择该帧为关键帧；和/或
当前帧里程计测量与已有关键帧里程计测量之间的位置平移偏差大于设定的平移阈值时，则将当前帧添加为关键帧，即，使得相邻关键帧的里程计平移偏差大于设定的平移阈值；和/或
当前帧里程计测量与已有关键帧里程计测量之间的旋转偏差大于设定的旋转阈值时，则将当前帧添加为关键帧，即，使得相邻关键帧的里程计旋转偏差大于设定的旋转阈值；和/或
第一帧为关键帧。
这样，所筛选的关键帧包括了具有信标图像的关键帧和不具有信标图像的关键帧。
上述当前帧里程计测量是指当前帧被采样时刻所对应的机器人本体在里程计坐标系下的位姿，关键帧里程计测量是指关键帧被采样时刻所对应的机器人本体在里程计坐标系下的位姿。
步骤103，根据包括有信标图像的关键帧、以及步骤101所获得的信标图像的信标参数，获得该信标与该关键帧相机的相对位姿，即，获得信标相对于相机坐标系的局部位姿，并将该局部位姿添加至信标检测数据中，作为视觉信标被观测测量值。
在该步骤中，信标关于相机的局部位姿的解算，实质是求解PnP(Perspective-n-Point)问题，即，根据信标特征点的图像坐标、相机内参、信标参数，获得信标关于相机的局部位姿。考虑到求解PnP问题对特征点数量的要求，本发明中至少包含4个特征点。
步骤104，对关键帧机器人本体全局位姿初始化、及视觉信标全局位置坐标初始化；其中，关键帧机器人本体全局位姿系关键帧被采样时刻所对应的机器人本体在世界坐标系下的位姿，
关键帧机器人本体全局位姿的初始化直接采用里程计测量数据结果，该结果为机器人本体在里程计二维坐标系平面内的位姿。视觉信标全局位置的初始化通过该信标首次被观测时的关键帧相机初始位姿、及观测结果计算获得，其中，关键帧相机位姿系指相机获取该帧时的相机位姿，信标首次被观测时所对应的关键帧相机位姿为关键帧相机初始位姿；
具体计算公式可以如下：
Figure BDA0002119958380000091
其中，符号左侧上下角标为坐标系标识，
Figure BDA0002119958380000092
为视觉信标全局位置向量(在信标坐标系m中的信标相对世界坐标系w的坐标位置)，
Figure BDA0002119958380000093
为关键帧机器人本体全局位姿向量(在机器人坐标系b中关键帧被采样时刻所对应的机器人本体相对世界坐标系w的位姿)，可表示为
Figure BDA0002119958380000094
Figure BDA0002119958380000095
为机器人本体坐标系的全局旋转向量，
Figure BDA0002119958380000096
为机器人本体坐标系的全局位置向量，该
Figure BDA0002119958380000097
通过将机器人本体全局位姿向量
Figure BDA0002119958380000098
进行从二维空间到三维空间的转换而得到，其中，机器人本体全局位姿向量可表示为
Figure BDA0002119958380000099
Figure BDA00021199583800000910
为机器人本体偏航角；所述关键帧机器人本体全局位姿向量得初始值以里程计测量数据结果为初始值，
Figure BDA00021199583800000911
为相机外参位姿向量(在相机坐标系c中相机相对机器人坐标系b的位置)，可表示为
Figure BDA00021199583800000912
Figure BDA00021199583800000913
为相机外参的旋转向量，
Figure BDA00021199583800000914
为相机外参的位置向量；
Figure BDA00021199583800000915
为视觉信标局部位置向量(在信标坐标系m中的信标相对相机坐标系c的坐标位置，即，被观测的测量值)，由步骤103获得，可表示为
Figure BDA00021199583800000916
符号⊕表示右乘，有例如，
Figure BDA00021199583800000917
以及
Figure BDA00021199583800000918
符号⊙表示投影，有例如，
Figure BDA00021199583800000919
由
Figure BDA00021199583800000920
得到视觉信标首次被观测时的关键帧相机初始位姿。
步骤105，基于关键帧机器人本体全局位姿初始化、及信标全局位置坐标初始化结果，分别计算里程计约束残差、视觉信标局部位置测量约束残差，在本申请中，视觉信标局部位置测量约束残差简称为M约束残差；其中，
对于任一关键帧i的里程计约束残差，由相邻关键帧机器人本体相对位姿与该相邻关键帧里程计相对位姿计算，实施方式之一可以是，计算相邻关键帧机器人本体相对位姿与该相邻关键帧里程计相对位姿之间差异，例如为两者之间的差值，可表示为：
Figure BDA0002119958380000101
其中，eo,<i>表示关键帧i的里程计残差项，
Figure BDA0002119958380000102
表示与关键帧i相邻的关键帧i+1机器人本体全局位姿向量，
Figure BDA0002119958380000103
表示关键帧i+1的里程计位姿向量(在机器人坐标系b相对里程计坐标系o的位姿)，从
Figure BDA0002119958380000104
得到相邻关键帧机器人本体相对位姿，从
Figure BDA0002119958380000105
)得到相邻关键帧里程计相对位姿。
对于任一关键帧i中任一视觉信标j，视觉信标局部位置测量约束残差为视觉信标在其所在关键帧相机坐标系下的局部位置与该信标的被观测测量值之间的差异，实施方式之一可以是，视觉信标在其所在关键帧相机坐标系下的局部位置与该信标的被观测测量值之间的差值，可表示为：
Figure BDA0002119958380000106
其中，em,<i,j>表示关键帧i中视觉信标j的局部位置测量约束残差，
Figure BDA0002119958380000107
为视觉信标j全局位置向量，其初始值由步骤104所获得，
Figure BDA0002119958380000108
表示关键帧i中视觉信标j的局部位置向量，由
Figure BDA0002119958380000109
得到视觉信标j在其所在关键帧i相机坐标系下的局部位置。
步骤106，对筛选出的所有关键帧的里程计约束残差、以及所有关键帧中的所有视觉信标的局部位置测量约束残差进行联合优化，以生成关键帧机器人本体全局位姿的初步优化结果和视觉信标全局位置的初步优化结果，为后续的信标特征点图像重投影约束的联合优化提供初始值；
在该步骤中，根据筛选出的所有关键帧的里程计约束残差、以及所有视觉信标局部位置测量约束残差，构建融合上述约束残差的第一目标函数，采用针对非线性优化问题的迭代式优化算法，例如，LM(Levenberg-Marquardt)算法，通过对以关键帧机器人本体全局位姿初始化结果、和视觉信标全局位置初始化结果为初始值进行多次迭代收敛，在迭代过程中以关键帧机器人本体全局位姿、和视觉信标全局位置为状态变量，即，优化的状态变量由关键帧机器人本体全局位姿、视觉信标全局位置两部分组成；求解使得第一目标函数获得最小值时的关键帧机器人本体全局位姿和视觉信标全局位置。其中，第一目标函数可表示为：
Figure BDA0002119958380000111
其中，
Figure BDA0002119958380000112
其中
Figure BDA0002119958380000113
xk为关键帧全局位姿，xm为信标全局位姿；N为所有关键帧，M为所有关键帧中的所有信标数；
Ωo,<i>为关键帧i的里程计约束信息矩阵，Ωm,<i,j>为关键帧i中视觉信标j的局部位置测量约束信息矩阵，信息矩阵的取值与系统误差模型相关，通常由人工给定。
Figure BDA0002119958380000114
表示累计筛选出的所有关键帧N的里程计约束残差，
Figure BDA0002119958380000115
表示累计集合
Figure BDA0002119958380000116
中所有视觉信标j局部位置测量约束残差，所述集合
Figure BDA0002119958380000117
由具有任一视觉信标j的关键帧i构成。在优化过程中，第一迭代时，关键帧机器人本体全局位姿状态变量初始值为步骤104获取的关键帧机器人本体全局位姿初始化结果，视觉信标全局位置状态变量初始值为步骤104获取的视觉信标全局位置初始化结果；在第一次之后的迭代过程中，则将上一次迭代所求得的状态变量作为当前状态变量，以进行各个当前迭代时所优化的状态变量的求解。
通过步骤106的联合优化，能够使得关键帧机器人本体全局位姿和视觉信标局部位置具有更好的鲁棒性，更加不容易在迭代过程中陷入局部极小值，有利于提高后续的信标特征点图像重投影约束的联合优化的效率。
步骤107，基于具有视觉信标图像的关键帧，对视觉信标的全局姿态(角度)进行初始化；
视觉信标的全局姿态(角度)初始化根据该视觉信标首次被观测的关键帧相机初始位姿、及对应信标检测结果计算获得，可以表示为：
Figure BDA0002119958380000118
其中，
Figure BDA0002119958380000119
为信标全局旋转向量(信标坐标系m相对世界坐标系w的旋转角度)，
Figure BDA00021199583800001110
为机器人本体坐标系的全局旋转向量(机器人本体坐标系b相对世界坐标系w的旋转角度)，
Figure BDA00021199583800001111
为相机外参旋转向量(相机坐标系c相对机器人本体坐标系b的旋转角度)，
Figure BDA00021199583800001112
为信标在相机坐标系中的局部旋转向量(信标坐标系m相对相机坐标系c的旋转角度)，在步骤103中从所获得的信标与该关键帧相机的相对位姿中获得。
步骤108，基于视觉信标的全局姿态初始化结果，计算视觉信标中的特征点在关键帧上的重投影约束残差，即，视觉信标特征点在关键帧相机图像上的重投影误差，在本申请中简称为V约束，可以表示为：
Figure BDA0002119958380000121
其中，ev,<i,j,k>为关键帧i中信标j所具有的特征点k在该关键帧上的重投影约束残差，proj(.)为图像投影函数，A为相机内参矩阵，
Figure BDA0002119958380000122
为特征点k在视觉信标j下的局部位置向量(在图像坐标系f下的特征点k相对于视觉信标系m中信标j的坐标位置，即信标参数)，
Figure BDA0002119958380000123
为特征点k在相机图像下的局部位置向量(图像坐标系f下的特征点k相对于相机坐标系c下关键帧i的图像坐标位置，即特征点k的图像坐标测量值)，
Figure BDA0002119958380000124
为信标j的全局位姿初始值，由步骤107中的视觉信标的全局姿态(角度)初始化结果和步骤106得到的视觉信标全局位置的初步优化结果获得。
步骤109，对筛选出的所有关键帧的里程计约束残差、以及所有信标中的所有特征点在关键帧上的重投影约束残差进行联合优化，构建融合前述两者约束残差的第二目标函数；
将步骤106获得的关键帧机器人本体全局位姿的初步优化结果作为关键帧机器人本体全局位姿迭代的初始值，将步骤106获得的视觉信标全局位置的初步优化结果和步骤107获得的视觉信标全局姿态结果作为视觉信标全局位姿的迭代初始值，采用针对非线性优化问题的迭代式优化算法，例如，LM(Levenberg-Marquardt)算法，通过关键帧机器人本体全局位姿初始值、和视觉信标全局位姿初始值进行多次迭代收敛，在迭代过程中，以关键帧机器人本体全局位姿、和视觉信标全局位姿(全局位置
Figure BDA0002119958380000125
和全局位姿
Figure BDA0002119958380000126
)为状态变量，即，优化的状态变量由关键帧机器人本体全局位姿、视觉信标全局位姿两部分组成；求解使得目标函数获得最小值时的关键帧机器人本体全局位姿和视觉信标全局位姿。其中，第二目标函数可表示为：
Figure BDA0002119958380000127
其中，
Figure BDA0002119958380000131
其中
Figure BDA0002119958380000132
里程计约束残差项与步骤106中的里程计约束残差一致，ev,<i,j,k>为关键帧i中信标j所具有的特征点k在关键帧上的重投影约束残差，Ωv,<i,j,k>为V约束残差信息矩阵，
Figure BDA0002119958380000133
表示累计所有信标j中的所有特征点k的V约束残差。
在优化过程中，第一次迭代时，关键帧机器人本体全局位姿状态变量初始值为步骤104获取的关键帧机器人本体全局位姿初始化结果，视觉信标全局位置状态变量初始值为步骤104获取的视觉信标全局位置初始化结果；在第一次之后的迭代过程中，则将上一次迭代所求得的状态变量作为当前状态变量，以进行当前迭代时所优化的状态变量的求解。
基于所获得的视觉信标全局位姿，构建信标地图。
在上述步骤中，采用分步式的联合优化策略，先采用M约束联合优化估计信标全局位置，再执行V约束联合优化估计信标的全局位姿，降低了优化算法陷入局部极小点的概率。当然，里程计约束残差与M约束残差的联合优化可以不执行，这样，第二目标函数的进行迭代收敛的初始值可以采用步骤104得到的关键帧机器人本体全局位姿初始化结果、以及视觉信标全局位姿初始化结果，其中，视觉信标全局位置坐标初始化结果由步骤104获得，视觉信标全局姿态初始化结果由步骤107获得。
参见图3所示，图3示出了以上述步骤101-109所涉及的图像数据的关联关系的一种示意图。图中：
采集的原始数据(图像帧)经过筛选，得到至少包括视觉信标的关键帧；
基于包括有视觉信标图像的关键帧，根据相机内参矩阵、信标参数，特征点在关键帧的图像坐标位置，得到各视觉信标相对相机的局部位姿，包括视觉信标相对相机的局部位置
Figure BDA0002119958380000134
视觉信标相对相机的局部姿态
Figure BDA0002119958380000135
即，视觉信标被观测测量值；
基于里程计测量数据，将里程计测量数据结果作为关键帧机器人本体全局位姿初始化结果，包括关键帧机器人本体全局位姿坐标初始化结果
Figure BDA0002119958380000136
关键帧机器人本体全局角度初始化结果
Figure BDA0002119958380000137
从相机外参获得相机外参位姿
Figure BDA0002119958380000138
以及相机外参姿态
Figure BDA0002119958380000139
根据所述
Figure BDA0002119958380000141
(椭圆虚线中)，获得各个视觉信标全局位置初始化结果
Figure BDA0002119958380000142
通过所述
Figure BDA0002119958380000143
获得各视觉信标在其所在关键帧相机坐标系下的局部位置，根据该局部位置与被观测测量值之间的差值，得到各视觉信标的M约束残差，
对所有关键帧里程计约束残差和所有关键帧中视觉信标的约束残差，进行以以所述各个关键帧机器人本体全局位姿初始化结果、及各个视觉信标全局位置初始化结果为初始值(图中未示出)的联合优化，获得所有关键帧机器人本体全局位姿初步优化结果和所有关键帧中所有视觉信标全局位置初步优化结果；其中，所述所有关键帧里程计约束残差基于里程计测量数据获得；
通过视觉信标的全局位姿初始化结果(包括视觉信标全局姿态初始化结果、视觉信标全局位置初始化结果)、相机内参矩阵、信标参数、特征点在相机图像中的局部位置，计算所有关键桢中各个视觉信标的各个特征点的V约束残差，对所有关键帧里程计约束残差和所有关键桢中所有视觉信标的所有特征点的V约束残差，进行以所述各个关键帧机器人本体全局位姿初始化结果、和所述各个视觉信标全局位姿初始化结果为初始值的联合优化，获得所有关键帧机器人本体全局位姿优化结果和所有关键帧中所有视觉信标全局位姿优化结果。
参见图4所示，图4为本发明实施例基于视觉信标建立信标地图的装置的一种示意图。该装置包括：
数据采集模块，获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
初始化模块，基于所述里程计测量数据，获得各个图像帧机器人本体全局位姿初始化结果；基于视觉信标检测数据，获取各个视觉信标全局位姿初始化结果；
里程计约束与视觉信标特征点图像重投影约束联合优化模块，对基于里程计测量数据获取的所有图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的所有特征点在其图像帧上重投影约束残差，进行以所述各个图像帧机器人本体全局位姿为状态变量、和所述各个视觉信标全局位姿为状态变量的联合优化，获得各个视觉信标全局位姿优化结果；其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述视觉信标全局位姿的初始值为所述各个视觉信标全局位姿初始化结果；
建图模块，基于视觉信标全局位姿优化结果，建立信标地图。
其中，所述数据采集模块还包括，采集布置于待建信标地图物理环境中的各个视觉信标的图像至少一次，在采集的同一时刻进行里程计的测量。
所述数据采集模块包括，用于获得里程计测量数据的里程计测量模块，和用于获得视觉信标检测数据的视觉信标检测模块。
所述初始化模块包括，用于基于所述里程计测量数据，获得各个图像帧机器人本体全局位姿初始化结果的图像帧机器人本体全局位姿初始化模块，和，用于基于所述视觉信标检测数据，获取各个视觉信标全局位姿初始化结果的视觉信标全局位姿初始化模块；其中，
所述图像帧机器人本体全局位姿初始化模块包括，将各个所述图像帧机器人本体全局位姿的初始化结果直接采用对应图像帧的里程计测量数据结果；
所述视觉信标全局位姿初始化模块包括，对于图像帧中任一视觉信标，通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，
其中，所述视觉信标检测数据包括视觉信标被观测测量值，从视觉信标相对相机的局部位姿获得；
所述视觉信标相对于相机的局部位姿根据视觉信标中特征点的图像坐标、相机内参、以及信标参数获得。
所述视觉信标全局位姿初始化模块还包括，根据图像帧机器人本体全局位姿向量、相机外参位姿向量、以及视觉信标相对相机的局部位置向量，获得所述视觉信标全局位置初始化结果；
根据图像帧机器人本体坐标系的全局旋转向量、相机外参旋转向量、视觉信标在相机坐标系中的局部旋转向量，获得所述视觉信标全局姿态初始化结果，
其中，
所述图像帧机器人本体坐标系的全局旋转向量从所述图像帧机器人本体全局位姿初始化结果获得，
所述相机外参旋转向量从所述相机外参位姿获得，
所述视觉信标在相机坐标系中的局部旋转向量从所述视觉信标相对相机的局部位姿获得。
该装置还包括，
里程计约束与视觉信标局部位置测量约束联合优化模块，对所述所有图像帧的里程计约束残差、以及所述局部位置测量约束残差，进行以所述各个图像帧机器人本体全局位姿为状态变量、及以各个视觉信标全局位置为状态变量的联合优化，获得各个图像帧机器人本体全局位姿的初步优化结果、和各个视觉信标全局位置的初步优化结果，并输出给所述里程计约束与视觉信标特征点图像重投影约束联合优化模块；其中，所述视觉信标全局位置的初始值为所述各个视觉信标全局位置初始化结果；
所述视觉信标全局位姿初始化模块还包括，将所述各个图像帧机器人本体全局位姿的初步优化结果分别作为所述各个图像帧机器人本体全局位姿初始化结果；将所述各个视觉信标全局位置的初步优化结果、以及各个视觉信标全局姿态初始化结果作为所述各个视觉信标全局位姿初始化结果；
所述里程计约束与视觉信标特征点图像重投影约束联合优化模块还包括，构建融合所述里程计约束残差和所述重投影约束残差的第二目标函数，通过以所述各个图像帧机器人本体全局位姿初始化结果、和所述各个视觉信标全局位姿初始化结果为迭代初始值的迭代，获取使得所述第二目标函数为最小值时的各个视觉信标全局位姿优化结果。
所述里程计约束与视觉信标局部位置测量约束联合优化模块还包括，构建融合所述里程计约束残差和所述视觉信标的局部位置测量约束残差的第一目标函数，通过以各个所述图像帧机器人本体全局位姿初始化结果、和各个所述视觉信标全局位置初始化结果为迭代初始值的迭代，获取使得所述第一目标函数为最小值时的各个图像帧机器人本体全局位姿的初步优化结果、和各个视觉信标全局位置的初步优化结果。
所述里程计约束与视觉信标局部位置测量约束联合优化模块还包括，
对任一所述图像帧，
基于所述里程计测量数据，获取所述图像帧与其相邻图像帧之间的机器人本体相对位姿、和所述图像帧与其相邻图像帧之间的里程计相对位姿；
由所述机器人本体相对位姿和里程计相对位姿的差异计算所述图像帧的里程计约束残差，
累计各个所述图像帧的里程计约束残差，得到所有所述图像帧的里程计约束残差。所述里程计约束与视觉信标局部位置测量约束联合优化模块还包括，
对任一图像帧中的任一视觉信标，获取该视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置与该视觉信标被观测测量值之间的差异，得到该视觉信标的局部位置测量约束残差，
累计各个图像帧中的各个视觉信标的局部位置测量约束残差，得到所有图像帧中所有视觉信标的局部位置测量约束残差，
其中，
所述视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置通过该视觉相机外参位姿、图像帧机器人本体全局位姿当前状态变量、图像帧机器人本体全局位姿当前状态变量、以及视觉信标的全局位置当前状态变量获得；所述图像帧机器人本体全局位姿当前状态变量在第一次迭代时为图像帧机器人本体全局位姿初始化结果；所述视觉信标的全局位置当前状态变量在第一次迭代时为视觉信标的全局位置初始化结果；
所述视觉信标被观测测量值通过该视觉信标相对于相机的局部位姿中的局部位置获得。
所述里程计约束与视觉信标特征点图像重投影约束联合优化模块还包括，对于任一图像帧中的任一视觉信标所具有的任一特征点，根据相机内参矩阵、图像帧机器人本体全局位姿当前状态变量、该视觉信标的全局位姿当前状态变量、以及该特征点在该视觉信标坐标系下的局部位置的投影函数与该特征点的图像坐标的测量值之差异，获得该视觉信标中该特征点在其图像帧相机图像上重投影误差，
累计各个图像帧中的各个视觉信标的各个特征点的重投影误差，得到所有图像帧中所有视觉信标的所有特征点在图像帧上的重投影约束残差。
所述装置进一步包括，关键帧筛选模块，从所述图像帧中至少筛选出包括视觉信标图像的关键帧；
所述关键帧筛选模块还包括，按照如下任一条件或其组合筛选出关键帧：
当前帧里程计测量与已有关键帧里程计测量之间的位置平移偏差大于设定的平移阈值时，则将当前帧作为关键帧；
当前帧里程计测量与已有关键帧里程计测量之间的旋转偏差大于设定的旋转阈值时，则将当前帧作为为关键帧；
第一帧为关键帧。
本发明还提供了一种基于视觉信标建立信标地图的电子设备，该电子设备包括存储器和处理器，其中，
所述存储器用于存放计算机程序；
所述处理器，用于执行所述存储器上所存放的程序，实现上述基于视觉信标建立信标地图的方法。
存储器可以包括随机存取存储器(Random Access Memory，RAM)，也可以包括非易失性存储器(Non-Volatile Memory，NVM)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital SignalProcessing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
本发明实施例还提供了一种计算机可读存储介质，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现如下步骤：
获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
基于所述里程计测量数据，获得各个图像帧机器人本体全局位姿初始化结果；
基于所述视觉信标检测数据，获取各个视觉信标全局位姿初始化结果；
对基于里程计测量数据获取的所有图像帧里程计约束残差、和基于所述各个视觉信标检测数据获取的所有图像帧中所有视觉信标的所有特征点在其图像帧上重投影约束残差，进行以所述各个图像帧机器人本体全局位姿初始化结果、和所述各个视觉信标全局位姿初始化结果为初始值的联合优化，获得各个视觉信标全局位姿优化结果；其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述视觉信标全局位姿的初始值为所述视觉信标全局位姿初始化结果；
基于视觉信标全局位姿优化结果，建立信标地图。
对于装置/网络侧设备/存储介质实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims (15)

1.一种基于视觉信标建立信标地图的方法，其特征在于，
获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；
基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果；
对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、和所述视觉信标全局位姿为状态变量的联合优化，获得视觉信标全局位姿优化结果；其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述视觉信标全局位姿的初始值为所述视觉信标全局位姿初始化结果；
基于视觉信标全局位姿优化结果，建立信标地图。
2.如权利要求1所述的方法，其特征在于，所述基于视觉信标检测数据，获取视觉信标全局位姿初始化结果之后，进一步包括，
获取图像帧中视觉信标的局部位置测量约束残差，
对所述图像帧的里程计约束残差、以及所述局部位置测量约束残差，进行以所述图像帧机器人本体全局位姿为状态变量、及以视觉信标全局位置为状态变量的联合优化，获得图像帧机器人本体全局位姿的初步优化结果、和视觉信标全局位置的初步优化结果；其中，所述视觉信标全局位置的初始值为所述视觉信标全局位姿初始化结果所包括的视觉信标全局位置初始化结果；
将所述图像帧机器人本体全局位姿的初步优化结果作为所述图像帧机器人本体全局位姿初始化结果；
所述基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果，包括，
基于视觉信标首次被观测的图像帧相机初始位姿、及视觉信标检测数据，获取视觉信标全局姿态初始化结果；
将所述视觉信标全局位置的初步优化结果、以及视觉信标全局姿态初始化结果作为所述视觉信标全局位姿初始化结果。
3.如权利要求2所述的方法，其特征在于，所述进行以所述图像帧机器人本体全局位姿为状态变量、及以视觉信标全局位置为状态变量的联合优化，包括：
构建融合所述里程计约束残差和所述视觉信标的局部位置测量约束残差的第一目标函数，通过以各个所述图像帧机器人本体全局位姿初始化结果、和各个所述视觉信标全局位置初始化结果为迭代初始值的迭代，获取使得所述第一目标函数为最小值时的各个图像帧机器人本体全局位姿的初步优化结果、和各个视觉信标全局位置的初步优化结果。
4.如权利要求1至3任一所述的方法，其特征在于，所述进行以所述图像帧机器人本体全局位姿为状态变量、及视觉信标全局位姿为状态变量的联合优化，包括：
构建融合所述里程计约束残差和所述重投影约束残差的第二目标函数，通过以各个所述图像帧机器人本体全局位姿初始化结果、和各个所述视觉信标全局位姿初始化结果为迭代初始值的迭代，获取使得所述第二目标函数为最小值时的各个视觉信标全局位姿优化结果。
5.如权利要求4所述的方法，其特征在于，所述基于所述里程计测量数据获取的图像帧的里程计约束残差，包括：
对任一所述图像帧，
基于所述里程计测量数据，获取所述图像帧与其相邻图像帧之间的机器人本体相对位姿、和所述图像帧与其相邻图像帧之间的里程计相对位姿；
由所述机器人本体相对位姿和里程计相对位姿的差异计算所述图像帧的里程计约束残差，
累计各个所述图像帧的里程计约束残差，得到所有所述图像帧的里程计约束残差。
6.如权利要求4所述的方法，其特征在于，所述获取图像帧中视觉信标的局部位置测量约束残差，包括，
对任一图像帧中的任一视觉信标，获取该视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置与该视觉信标被观测测量值之间的差异，得到该视觉信标的局部位置测量约束残差，
其中，
所述视觉信标在该视觉信标所在图像帧相机坐标系下的局部位置通过该视觉相机外参位姿、图像帧机器人本体全局位姿当前状态变量、以及视觉信标的全局位置当前状态变量获得；所述图像帧机器人本体全局位姿当前状态变量在第一次迭代时为图像帧机器人本体全局位姿初始化结果；所述视觉信标的全局位置当前状态变量在第一次迭代时为视觉信标的全局位置初始化结果；
所述视觉信标被观测测量值通过该视觉信标相对于相机的局部位姿中的局部位置获得；
累计各个图像帧中的各个视觉信标的局部位置测量约束残差，得到所有图像帧中所有视觉信标的局部位置测量约束残差。
7.如权利要求4所述的方法，其特征在于，所述基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上的重投影约束残差，包括：
根据各个所述视觉信标全局位姿当前状态变量、以及各个所述视觉信标检测数据，获取所述图像帧中所有视觉信标的所有特征点在该图像帧上的重投影约束残差。
8.如权利要求7所述的方法，其特征在于，所述根据各个所述视觉信标全局位姿当前状态变量、以及各个所述视觉信标检测数据，获取所述图像帧中所有视觉信标的所有特征点在该图像帧上的重投影约束残差，包括，
对于任一图像帧中的任一视觉信标所具有的任一特征点，根据相机内参矩阵、图像帧机器人本体全局位姿当前状态变量、该视觉信标的全局位姿当前状态变量、以及该特征点在该视觉信标坐标系下的局部位置的投影函数与该特征点的图像坐标的测量值之差异，获得该视觉信标中该特征点在其图像帧上重投影误差，
累计各个图像帧中的各个视觉信标的各个特征点的重投影误差，得到所有图像帧中所有视觉信标的所有特征点在图像帧上的重投影约束残差。
9.如权利要求1所述的方法，其特征在于，所述基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果包括，
将各个所述图像帧机器人本体全局位姿的初始化结果直接采用对应图像帧的里程计测量数据结果；
所述基于所述视觉信标检测数据，获取视觉信标全局位姿初始化结果，包括：
对于图像帧中任一视觉信标，通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，
其中，所述视觉信标检测数据包括视觉信标被观测测量值，从视觉信标相对相机的局部位姿获得；
所述视觉信标相对于相机的局部位姿根据视觉信标中特征点的图像坐标、相机内参、以及信标参数获得。
10.如权利要求9所述的方法，其特征在于，所述通过该视觉信标首次被观测时的图像帧相机初始位姿、以及该视觉信标检测数据，分别获得该视觉信标全局位置初始化结果、以及该视觉信标全局姿态初始化结果，包括：
根据图像帧机器人本体全局位姿向量、相机外参位姿向量、以及视觉信标相对相机的局部位置向量，获得所述视觉信标全局位置初始化结果；
根据图像帧机器人本体坐标系的全局旋转向量、相机外参旋转向量、视觉信标在相机坐标系中的局部旋转向量，获得所述视觉信标全局姿态初始化结果，
其中，
所述图像帧机器人本体坐标系的全局旋转向量从所述图像帧机器人本体全局位姿初始化结果获得，
所述相机外参旋转向量从所述相机外参位姿获得，
所述视觉信标在相机坐标系中的局部旋转向量从所述视觉信标相对相机的局部位姿获得。
11.如权利要求4所述的方法，其特征在于，所述获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧被采集时刻机器人本体的里程计测量数据包括，采集布置于待建信标地图物理环境中的各个视觉信标的图像至少一次，在采集的同一时刻进行里程计的测量。
12.如权利要求1所述的方法，其特征在于，所述获取至少包括视觉信标图像帧的视觉信标检测数据、和至少包括所述视觉信标图像帧采样时刻的里程计测量数据之后，进一步包括，按照如下任一条件或其组合筛选出关键帧，从所述图像帧中至少筛选出关键帧：
当前帧里程计测量与已有关键帧里程计测量之间的位置平移偏差大于设定的平移阈值时，则将当前帧作为关键帧；
当前帧里程计测量与已有关键帧里程计测量之间的旋转偏差大于设定的旋转阈值时，则将当前帧作为关键帧；
第一帧为关键帧；
所述图像帧为筛选出的关键帧。
13.一种基于视觉信标建立信标地图的装置，其特征在于，
数据采集模块，获取至少包括视觉信标图像帧的视觉信标检测数据、和至少一个图像帧采集时刻机器人本体的里程计测量数据；
初始化模块，基于所述里程计测量数据，获得图像帧机器人本体全局位姿初始化结果；基于视觉信标检测数据，获取视觉信标全局位姿初始化结果；
里程计约束与视觉信标特征点图像重投影约束联合优化模块，对基于里程计测量数据获取的图像帧里程计约束残差、和基于所述视觉信标检测数据获取的图像帧中视觉信标的特征点在其图像帧上重投影约束残差，进行以所述图像帧机器人本体全局位姿初始化结果、和所述视觉信标全局位姿初始化结果为初始值的联合优化，获得视觉信标全局位姿优化结果；其中，所述图像帧机器人本体全局位姿的初始值为图像帧机器人本体全局位姿初始化结果，所述视觉信标全局位姿的初始值为所述视觉信标全局位姿初始化结果；
建图模块，基于视觉信标全局位姿优化结果，建立信标地图。
14.一种基于视觉信标建立信标地图的电子设备，其特征在于，该电子设备包括存储器和处理器，其中，
所述存储器用于存放计算机程序；
所述处理器，用于执行所述存储器上所存放的程序，实现权利要求1-12任一所述基于视觉信标建立信标地图的方法。
15.一种计算机可读存储介质，其特征在于，所述存储介质内存储有计算机程序，所述计算机程序被处理器执行权利要求1-12任一所述基于视觉信标建立信标地图的方法。
CN201910603494.5A 2019-07-05 2019-07-05 一种基于视觉信标建立信标地图方法、装置 Pending CN112183171A (zh)

Priority Applications (5)

Application Number Priority Date Filing Date Title
CN201910603494.5A CN112183171A (zh) 2019-07-05 2019-07-05 一种基于视觉信标建立信标地图方法、装置
EP20837645.9A EP3995988A4 (en) 2019-07-05 2020-07-04 METHOD AND APPARATUS FOR ESTABLISHING A BEACON MAP BASED ON VISUAL BEACONS
PCT/CN2020/100306 WO2021004416A1 (zh) 2019-07-05 2020-07-04 一种基于视觉信标建立信标地图的方法、装置
KR1020227002958A KR20220025028A (ko) 2019-07-05 2020-07-04 시각적 비콘 기반의 비콘 맵 구축 방법, 장치
JP2022500643A JP7300550B2 (ja) 2019-07-05 2020-07-04 視覚標識に基づき標識マップを構築する方法、装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910603494.5A CN112183171A (zh) 2019-07-05 2019-07-05 一种基于视觉信标建立信标地图方法、装置

Publications (1)

Publication Number Publication Date
CN112183171A true CN112183171A (zh) 2021-01-05

Family

ID=73915694

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910603494.5A Pending CN112183171A (zh) 2019-07-05 2019-07-05 一种基于视觉信标建立信标地图方法、装置

Country Status (5)

Country Link
EP (1) EP3995988A4 (zh)
JP (1) JP7300550B2 (zh)
KR (1) KR20220025028A (zh)
CN (1) CN112183171A (zh)
WO (1) WO2021004416A1 (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN112880687A (zh) * 2021-01-21 2021-06-01 深圳市普渡科技有限公司 一种室内定位方法、装置、设备和计算机可读存储介质
CN113204039A (zh) * 2021-05-07 2021-08-03 深圳亿嘉和科技研发有限公司 一种应用于机器人的rtk-gnss外参标定方法
CN114295138A (zh) * 2021-12-31 2022-04-08 深圳市普渡科技有限公司 机器人、地图扩建方法、装置和可读存储介质
CN114415698A (zh) * 2022-03-31 2022-04-29 深圳市普渡科技有限公司 机器人、机器人的定位方法、装置和计算机设备

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112902953B (zh) * 2021-01-26 2022-10-04 中国科学院国家空间科学中心 一种基于slam技术的自主位姿测量方法
WO2023280274A1 (en) * 2021-07-07 2023-01-12 The Hong Kong University Of Science And Technology Geometric structure aided visual localization method and system
CN114111803B (zh) * 2022-01-26 2022-04-19 中国人民解放军战略支援部队航天工程大学 一种室内卫星平台的视觉导航方法
CN114463429B (zh) * 2022-04-12 2022-08-16 深圳市普渡科技有限公司 机器人、地图创建方法、定位方法及介质

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104374395A (zh) * 2014-03-31 2015-02-25 南京邮电大学 基于图的视觉slam方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7689321B2 (en) * 2004-02-13 2010-03-30 Evolution Robotics, Inc. Robust sensor fusion for mapping and localization in a simultaneous localization and mapping (SLAM) system
US8108092B2 (en) * 2006-07-14 2012-01-31 Irobot Corporation Autonomous behaviors for a remote vehicle
JP5222971B2 (ja) * 2011-03-31 2013-06-26 富士ソフト株式会社 歩行ロボット装置及びその制御プログラム
CN105487557B (zh) * 2015-12-07 2018-06-19 浙江大学 一种基于日盲区紫外成像的无人机自主着陆引导系统
CN109071015B (zh) * 2016-04-29 2021-11-30 美国联合包裹服务公司 无人机拾取及递送系统
US10838065B2 (en) * 2017-01-26 2020-11-17 The Regents Of The University Of Michigan Localization using 2D maps which capture vertical structures in 3D point data
CN107179080B (zh) * 2017-06-07 2020-07-24 纳恩博（北京）科技有限公司 电子设备的定位方法和装置、电子设备、电子定位系统
CN107516326B (zh) * 2017-07-14 2020-04-03 中国科学院计算技术研究所 融合单目视觉和编码器信息的机器人定位方法和系统
CN109648558B (zh) * 2018-12-26 2020-08-18 清华大学 机器人曲面运动定位方法及其运动定位系统

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104374395A (zh) * 2014-03-31 2015-02-25 南京邮电大学 基于图的视觉slam方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
HENGBO TANG等: "A Fully Automatic Calibration Algorithm for a Camera Odometry System", 《IEEE SENSORS JOURNAL》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112880687A (zh) * 2021-01-21 2021-06-01 深圳市普渡科技有限公司 一种室内定位方法、装置、设备和计算机可读存储介质
CN112862894A (zh) * 2021-04-12 2021-05-28 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN112862894B (zh) * 2021-04-12 2022-09-06 中国科学技术大学 一种机器人三维点云地图构建与扩充方法
CN113204039A (zh) * 2021-05-07 2021-08-03 深圳亿嘉和科技研发有限公司 一种应用于机器人的rtk-gnss外参标定方法
CN114295138A (zh) * 2021-12-31 2022-04-08 深圳市普渡科技有限公司 机器人、地图扩建方法、装置和可读存储介质
CN114295138B (zh) * 2021-12-31 2024-01-12 深圳市普渡科技有限公司 机器人、地图扩建方法、装置和可读存储介质
CN114415698A (zh) * 2022-03-31 2022-04-29 深圳市普渡科技有限公司 机器人、机器人的定位方法、装置和计算机设备
CN114415698B (zh) * 2022-03-31 2022-11-29 深圳市普渡科技有限公司 机器人、机器人的定位方法、装置和计算机设备

Also Published As

Publication number Publication date
EP3995988A1 (en) 2022-05-11
KR20220025028A (ko) 2022-03-03
EP3995988A4 (en) 2022-08-17
JP7300550B2 (ja) 2023-06-29
WO2021004416A1 (zh) 2021-01-14
JP2022539422A (ja) 2022-09-08

Similar Documents

Publication Publication Date Title
CN112183171A (zh) 一种基于视觉信标建立信标地图方法、装置
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
CN110009681B (zh) 一种基于imu辅助的单目视觉里程计位姿处理方法
Jiao et al. Robust odometry and mapping for multi-lidar systems with online extrinsic calibration
CN107516326B (zh) 融合单目视觉和编码器信息的机器人定位方法和系统
CN112184824A (zh) 一种相机外参标定方法、装置
CN111561923A (zh) 基于多传感器融合的slam制图方法、系统
CN107192376B (zh) 基于帧间连续性的无人机多帧图像目标定位校正方法
CN107330927B (zh) 机载可见光图像定位方法
CN112767546B (zh) 移动机器人基于双目图像的视觉地图生成方法
CN110969665A (zh) 一种外参标定方法、装置、系统及机器人
CN114758011B (zh) 融合离线标定结果的变焦相机在线标定方法
CN115147497A (zh) 一种标定方法、装置及电子设备
Sabatta et al. Vision-based path following using the 1D trifocal tensor
CN114842224A (zh) 一种基于地理底图的单目无人机绝对视觉匹配定位方案
JP2007034964A (ja) カメラ視点運動並びに３次元情報の復元及びレンズ歪パラメータの推定方法、装置、カメラ視点運動並びに３次元情報の復元及びレンズ歪パラメータの推定プログラム
CN114111791A (zh) 一种智能机器人室内自主导航方法、系统及存储介质
CN109919998B (zh) 卫星姿态确定方法、装置和终端设备
Wang et al. Slam-based cooperative calibration for optical sensors array with gps/imu aided
Hu et al. Accurate fiducial mapping for pose estimation using manifold optimization
CN113124854A (zh) 一种视觉定位方法、以及地图构建方法、装置
CN115077467B (zh) 清洁机器人的姿态估计方法、装置及清洁机器人
Tahri et al. Efficient decoupled pose estimation from a set of points
Hu et al. Efficient Visual-Inertial navigation with point-plane map
CN114018284B (zh) 一种基于视觉的轮速里程计校正方法

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
CB02 Change of applicant information

Address after: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province

Applicant after: Hangzhou Hikvision Robot Co.,Ltd.

Address before: 310052 5 / F, building 1, building 2, no.700 Dongliu Road, Binjiang District, Hangzhou City, Zhejiang Province

Applicant before: HANGZHOU HIKROBOT TECHNOLOGY Co.,Ltd.

CB02 Change of applicant information