CN117109561A - 融合激光定位的远距二维码地图创建及定位方法、系统 - Google Patents

融合激光定位的远距二维码地图创建及定位方法、系统 Download PDF

Info

Publication number
CN117109561A
CN117109561A CN202311124250.1A CN202311124250A CN117109561A CN 117109561 A CN117109561 A CN 117109561A CN 202311124250 A CN202311124250 A CN 202311124250A CN 117109561 A CN117109561 A CN 117109561A
Authority
CN
China
Prior art keywords
remote
dimensional code
positioning
map
laser
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
CN202311124250.1A
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.)
Shanghai Xiangong Intelligent Technology Co ltd
Original Assignee
Shanghai Xiangong 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 Shanghai Xiangong Intelligent Technology Co ltd filed Critical Shanghai Xiangong Intelligent Technology Co ltd
Priority to CN202311124250.1A priority Critical patent/CN117109561A/zh
Publication of CN117109561A publication Critical patent/CN117109561A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map 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/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • 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/14—Methods or arrangements for sensing record carriers, e.g. for reading patterns by electromagnetic radiation, e.g. optical sensing; by corpuscular radiation using light without selection of wavelength, e.g. sensing reflected white light
    • G06K7/1404—Methods for optical code recognition
    • G06K7/1408—Methods for optical code recognition the method being specifically adapted for the type of code
    • G06K7/1417—2D bar codes
    • 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
    • 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/28—Quantising the image, e.g. histogram thresholding for discrimination between background and foreground patterns
    • 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/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Databases & Information Systems (AREA)
  • Multimedia (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Toxicology (AREA)
  • Artificial Intelligence (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提供了一种融合激光定位的远距二维码地图创建及定位方法、系统，其中该地图创建方法步骤包括：将远距二维码设置在建图区域顶部相对于相机的仰望位；建立建图区域的激光地图，并获取激光定位信息；经相机获取含远距二维码的图像，以提取远距二维码在相机坐标系下的空间位姿；当判断相机观测到的图像中含有首次观测到的远距二维码时，计算首次观测到的远距二维码在激光地图中的空间位置，并将其加入远距二维码地图中。籍此实现融合激光定位的远距二维码地图的创建与定位，提高了定位场景的适应性。

Description

融合激光定位的远距二维码地图创建及定位方法、系统
技术领域
本发明涉及室内空间建图定位技术，尤其涉及一种融合激光定位的远距二维码地图创建及定位方法、系统。
背景技术
目前主流的移动机器人建图定位方案，通常采用激光建图技术，如机器人使用当前激光点云与点云地图匹配实现激光定位。然而实际实施过程中，由于存在场景变化因素，例如，在堆垛，解垛，窄通道，动态场景下等，容易受到环境及堆放物、移动物体的遮挡及干扰影响，阻挡激光雷达的扫描，加上机器人运动机构时常还存在打滑现象，从而使得采用纯激光定位在长时间的实际运行部署中存在一定的局限。
为此，发明人考虑到远距二维码定位的特殊性，可以将远距二维码设置在室内天花板上这种无遮挡、开阔的环境中，此时相机在拍摄远距二维码的过程中，将不易受到地面环境的影响，同时若采用人工标记方式，在场景中容易发生定位丢失的区域内设置远距二维码进行辅助定位，则可减少或避免前述此类问题的发生，以在该些场景下获得定位优势。
发明内容
为此，本发明的主要目的在于提供一种融合激光定位的远距二维码地图创建及定位方法、系统，以实现背景技术中提出的方案，获得在该些场景下的定位优势。
为了实现上述目的，根据本发明的第一个方面，提供了一种融合激光定位的远距二维码地图创建方法，步骤包括：
步骤S100将远距二维码设置在建图区域顶部相对于相机的仰望位；建立建图区域的激光地图，并获取激光定位信息；
步骤S200经相机获取含远距二维码的图像，以提取远距二维码在相机坐标系下的空间位姿；
步骤S300当判断相机观测到的图像中含有首次观测到的远距二维码时，计算首次观测到的远距二维码在激光地图中的空间位置，并将其加入远距二维码地图中。
在可能的优选实施方式中，步骤S300中，计算首次观测到的远距二维码在激光地图中的空间位置的步骤包括：
步骤S310通过因子图形式融合所有时刻的激光定位信息及远距二维码在相机坐标系下的空间位姿，构建最大后验概率问题优化激光地图中每一个远距二维码的空间位置。
在可能的优选实施方式中，步骤S310中，构建最大后验概率问题优化激光地图中每一个远距二维码的空间位置的步骤包括：
步骤S311设所有远距二维码在激光地图中的位置为，使用测量值约束待优化 变量，使用贝叶斯网络建模为，通过最大后验概率求解优化后的状态变量：
其中，测量值为一组测量值的集合：
表示处于移动机器人位于第个位置时相机对的观测；
表示处于移动机器人在激光地图中的第个位置；
表示相机在车体坐标系下的安装位置。
在可能的优选实施方式中，所述远距二维码，包括：定位码，信息码，其中定位码由数个方向点围绕中心点在数个非交叉方向布设而成，并在各个相邻方向点的夹角处划分出数个象限区域，所述信息码根据预设码表布设在对应象限区域中，以与定位码组成二维点阵，其中定位码与信息码表面设有反光层。
在可能的优选实施方式中，步骤S200中，提取远距二维码在相机坐标系下的空间位姿的步骤包括：
步骤S210获取包含远距二维码信息的图像做二值化处理后，进行边缘提取；
步骤S220根据提取的边缘信息进行椭圆拟合检测，获取原始 码及其圆心坐标；
步骤S230根据定位码的几何关系，定位出原始码中对应各远距二维码的定位码分布位置，以筛选出原始码中的信息码，并根据通码表来获取远距二维码的ID信息；
步骤S240当判断该ID下的远距二维码首次出现时，根据定位码在图像中的圆心坐标，计算对应远距二维码在相机坐标系下的空间位姿并与该ID信息绑定。
在可能的优选实施方式中，步骤S240中计算远距二维码在相机坐标系下空间位姿的步骤包括：
步骤S241设定位码的圆心坐标为，建立齐次矩阵
基于SVD法求解单应性矩阵H，其中u，v表示定位码圆心的像素坐标、表示远距 二维码坐标系下点的坐标，s为等效距离比例因子；
步骤S242根据单应性矩阵与远距二维码在相机坐标系下转换矩阵的关系
求得旋转和平移矩阵，其中P为相机投影矩阵、E为截断外参矩阵，, 分别为相 机方向和方向的焦距，，为相机中心点坐标，为旋转矩阵中的前两列，, , 分别表示在相机坐标系下远距二维码中心的位置，为3×3的单应投影矩阵。
在可能的优选实施方式中，步骤S240中还包括：
步骤243通过迭代优化方法求解最小化误差函数
以优化旋转和平移矩阵，其中R表示旋转，t表示平移
表示平移向量中的第i个元素，表示旋转矩阵中的第i列；其中分别表示 图像中第个远距二维码的圆心投影到相机归一化坐标系中的坐标；表示所有定 位码的的圆心坐标；分别表示由远距二维码中第个定位码的圆心空间坐标，按照旋 转平移矩阵转换到相机归一化平面中的点的坐标；表示在远距二维码坐标系中 每一个定位码的圆心空间坐标；
其中在求解过程中以
作为约束条件。
在可能的优选实施方式中，步骤S200中，提取远距二维码在相机坐标系下的空间位姿的步骤还包括：
步骤S250 按照时间过滤和先验位姿约束，对经步骤S240获得的空间位姿进行约束，其中时间过滤步骤包括：当判断空间位姿结果存在突变时，滤除；先验位姿约束步骤包括：当判断空间位姿结果中远距二维码坐标系和相机坐标系处于非平行时，滤除。
为了实现上述目的，对应上述地图创建方法，本发明第二个方面，还提供了一种融合激光定位的远距二维码地图定位方法，其步骤包括：
步骤S400当位处激光地图与远距二维码地图混合定位区域时,使用置信度描述定 位信息的准确程度，分别设置激光定位置信度与远距二维码定位置信度，通过各 自位置的定位置信度大小进行加权融合获取定位信息：
其中为当的位置，表示当前混合定位的置信度，为上一时刻定位 位置，为当前时刻使用相机定位的定位位置，为当前时刻使用激光定位的位置；
步骤S500当位处远距二维码地图定位区域时，通过相机提取当前场景图像中所有远距二维码的ID信息，以与地图进行匹配获取定位信息。
为了实现上述目的，对应上述地图创建方法及定位方法，本发明第三个方面，还提供了一种融合激光定位的远距二维码地图创建及定位系统，其包括：
存储单元，用于存储包括如上任一所述基于远距二维码的地图创建方法步骤的程序及上述融合激光定位的远距二维码地图定位方法步骤的程序，以供控制单元，处理单元，激光定位单元适时调取执行；
控制单元，用于控制激光雷达及相机进行时空同步，及控制红外相机拍摄包含远距二维码的待处理图像，控制激光雷达进行建图扫描；
激光定位单元，用于根据激光雷达的建图扫描数据，建立建图区域的激光地图，并获取激光定位信息；
处理单元，用于以提取远距二维码在相机坐标系下的空间位姿，当判断相机观测 到的图像中含有首次观测到的远距二维码时，计算首次观测到的远距二维码在激光地图中 的空间位置，并将其加入远距二维码地图中；当位处激光地图与远距二维码地图混合定位 区域时,使用置信度描述定位信息的准确程度，分别设置激光定位置信度与远距二维 码定位置信度，通过各自位置的定位置信度大小进行加权融合获取定位信息；当位处 远距二维码地图定位区域时，通过相机提取当前场景图像中所有远距二维码的ID信息，以 与地图进行匹配获取定位信息。
通过本发明提供的该融合激光定位的远距二维码地图创建及定位方法、系统，巧妙的利用了远距二维码的建图和定位优势及特性，以对激光建图和定位的短板进行了补充融合，从而解决了传统单一激光建图定位容易发生定位丢失的问题，以在更多场景下获得定位优势。
附图说明
构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明的远距二维码在室内环境下粘贴于天花板，用于与激光定位相融合的建图定位场景示意图；
图2为本发明的融合激光定位的远距二维码地图创建方法的步骤示意图；
图3为本发明的融合激光定位的远距二维码地图创建方法的流程逻辑示意图；
图4为本发明的融合激光定位的远距二维码地图创建方法中，相机与激光雷达时空同步的概念示意图；
图5为本发明的融合激光定位的远距二维码地图创建方法中，避免远距二维码姿态出现奇异解而设置的二次结果评价机制逻辑示意图；
图6为本发明的远距二维码点阵结构示意图，其中圆环部分示意的是信息码的可布设位置；
图7为本发明的远距二维码通码表示意图；
图8为本发明的远距二维码几何位置关系示意图；
图9为红外相机拍摄的本发明的远距二维码的示意图；
图10为本发明的融合激光定位的远距二维码地图创建方法中，基于远距二维码计算物体空间位姿的方法步骤示意图；
图11为本发明的基于远距二维码的地图创建方法中的二值化图像的示意图；
图12为位姿奇异现象的示意图；
图13为本发明的融合激光定位的远距二维码地图创建及定位系统结构示意图。
具体实施方式
为了使本领域的技术人员能够更好的理解本发明的技术方案，下面将结合实施例来对本发明的具体技术方案进行清楚、完整地描述，以助于本领域的技术人员进一步理解本发明。显然，本案所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思及相互不冲突的前提下，本申请中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的揭露及保护范围。
此外本发明的说明书和权利要求书及附图中的术语“第一”、“第二”、“S100”、“S200”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里描述的那些以外的顺序实施。同时本发明中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。除非另有明确的规定和限定，术语“设置”、“布设”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本案中的具体含义。
为了弥补现有激光定位技术在一些场景适应性方面的短板，请参阅图1至图12所示，本发明提供了一种融合激光定位的远距二维码地图创建方法，其步骤包括：
步骤S100将远距二维码设置在建图区域顶部相对于相机的仰望位；建立建图区域的激光地图，并获取激光定位信息。
具体的，在传统的激光建图定位领域，机器人使用当前激光点云与点云地图匹配实现激光定位。但由于存在场景变化，移动机器人也时常发生打滑，遮挡，移动物体干扰等实际情况的存在，因此激光定位在实际使用中存在一定的局限。为此如图1所示，本示例优选采用人工标记的方式，在场景中容易发生激光定位丢失区域的天花板上设置远距二维码，以为弥补激光定位的不足建立定位识别关系。
步骤S200经相机获取含远距二维码的图像，以提取远距二维码在相机坐标系下的空间位姿。
具体来说，请参阅图6至图11所示，本示例的远距二维码，其包括：定位码，信息码，其中定位码由数个方向点围绕中心点在数个非交叉方向布设而成，并在各个相邻方向点的夹角处划分出数个象限区域，所述信息码根据预设码表布设在对应象限区域中，以与定位码组成二维点阵。
具体来说，发明人发现由于圆形（包括：椭圆形、椭圆环形、圆环形）在视角变化时会产生仿射变换，在相机平面中是以椭圆的形式展现，因此其应对视角变换的能力相对较弱。而传统方块像素的二维码，虽然角点在不同视角的情况下的鲁棒性更强，但其弱点是在远距离视角下容易角点发生偏移，基于该发现，发明人设计了一种点阵形态的二维码。
如图6至图8所示，在本示例中，该远距二维码中，定位码由数个方向点根据东南西北四个方向围绕中心点布设而成，以在各个相邻方向点的夹角处划分出如图8所示的四个象限区域，而所述信息码则如图6所示，可根据预设码表在对应的象限区域中布设，以与定位码组成如图7所示的二维点阵，且本示例下，该方向点、中心点、信息点优选呈圆形、椭圆形、椭圆环形、圆环形中的任一一种。
经此设置，该远距二维码中由于只有一种基础图案（圆点），因此不但降低了加工制作二维码的难度。同时此类点阵的设计中，任意两个二维码的排列方式的差异性很大，从而可降低误检测的几率，以支持不同的高度距离。
此外，为了加强定位码的排布特征，如图8所示在本优选示例中，所述数个方向点中至少存在一个主方向点（如图7中的A点）与中心点B的设置距离与其他副方向点相异，如A与B的间隔距离是C、D、F与B间隔距离的一半。而各副方向点则与中心点的设置距离相等。从而将此作为该远距二维码识别时的约束之一，可以进一步提高识别准确率。
进一步的，考虑到环境光因素，以及室内天花板的底色，悬吊干扰物等多种综合因素的影响，会对相机拍摄的图像形成干扰，以影响远距二维码的识别率，为此本示例中，优选在远距二维码的定位码和信息码表面设置反光层，如采用亮银反光布，而对应的该相机优选设置为红外相机，以在相机镜头和CMOS之间加装了一个红外滤光镜，让一定波段内的红外光通过，吸收或反射可见光和紫外线，并可选择为相机添加红外灯。经此设置后，如图9所示，该红外相机便可轻松捕捉到明显包含远距二维码信息的图像。
另一方面，对应上述示例的该远距二维码，如图10所示，其中步骤S200中，提取远距二维码在相机坐标系下的空间位姿的步骤包括：
步骤S210获取包含远距二维码信息的图像做二值化处理后，进行边缘提取。
具体的，根据上述远距二维码的示例可知，当红外相机拍摄到含有远距二维码的图像时，获取的图片如图8所示，除定位码和信息码圆点为白色外，其余部分为黑色，此时可将图像的像素值进行取反。并将取反后的图像进行二值化处理，将灰度图像转换成黑白图像，如图11所示。
之后，将黑白图像利用Canny边缘检测算法，提取边缘。由于前述红外相机的特殊性，此时导致黑白图像中边缘较少，其能大大加快边缘提取的过程。
步骤S220根据提取的边缘信息进行椭圆拟合检测，获取原始码及其圆心坐标。
具体的，圆是椭圆的一种特殊形状，由于视角的不同，远距二维码中的点在成像平 面中一般为近似椭圆。椭圆拟合常用于特征提取，场景建模，摄像机标定等问题中，给定一 组来自某个椭圆的图像测量数据所有边缘点的集合，其中包含了所有的祖成椭圆的边缘 点。表示每个边缘点，其坐标由像素位置组成，此处N表示组成该边缘的像素点数量。这 里使用的是图像点边缘点信息。
由于测量数据不可避免的带有误差，问题变为从带有误差的数据中恢复其对应的 椭圆信息。其中椭圆的方程可写为：其中表示椭圆的参数集合。椭圆参数共有6个。
因为每个椭圆的边缘点都在椭圆上，因此所有的边缘点都在椭圆上。 椭圆拟合转变为线性最小二乘问题:
为保证最小二乘的解一定为椭圆，必须添加约束条件，，，
其中：
从而构造一个带有约束的非线性优化问题:
对于这种非线性优化问题，所以可使用列温伯格-马夸尔特算法求解其对应的椭圆参数信息。
基于上述椭圆检测方案，待提取所有的边缘信息后，将每一个边缘信息分别进行 基于最小二乘的椭圆拟合。进而得到每一个黑色的椭圆拟合信息，即原始码，其中椭圆的一 般方程如下。其中A，B，C，D，E为椭圆方程的参数，在椭 圆拟合过程中求解得到的。
通过椭圆参数。进而得到椭圆的几何中心的坐标为：
其中椭圆的长轴和短轴b的长度分别为：
进一步的，由于图像中会存在噪点或者因为其他反光材料造成的影响，需要对每一个椭圆信息进行过滤，找到最圆的椭圆方程。
为此其中步骤S220中还包括：根据椭圆方程的长轴、短轴公式，判断各原始码的长轴短轴比例是否符合阈值，当不符时进行滤除的步骤。例如当长轴和短轴之间的比例小于1.3时，认为此椭圆（原始码）不符合圆的形状，进而进行滤除。同时，为排除其他噪点的影响，所有属于同一个远距二维码的圆点的半径，应该在相同的范围内，因此可根据此条件对所有的原始码进行进一步的过滤，以快速滤除不合格的原始码。
步骤S230根据定位码的几何关系，定位出原始码中对应各远距二维码的定位码分布位置。
具体的，当得到所有可能组成远距二维码的原始码后，由于远距二维码如上述示 例所述，是由定位码和信息码组成的。定位码由五个点组成，即是一个固定的模板，如图8所 示，因此在中心的一个圆B与与其最近的圆A之间的距离正好是其他三个圆C、D、F分别与圆B 之间距离的。基于上述示例几何关系的存在，因此每一个远距二维码在图像中的位置信 息，可以被快速的被定位。
因此便得到了每一个远距二维码的分组。此种分类方法可大大加快远距二维码在图像中的定位过程，同时由于类似于模板匹配，如果图像中出现多个远距二维码也会被进行区分，而无需额外的聚类过程。
此外，当对一张图像中所有的远距二维码进行分组后，便得到每一个远距二维码的定位码在图像中的位置。为进一步验证，对所有定位码的圆度进行检查，确保每一个圆都有相同的长轴和短轴的长度。通过定位码的空间位置，可以快速得到远距二维码中信息码的分布情况。
由于信息码搭载了远距二维码的ID信息。因此每一个远距二维码都有不同的信息码。当确定定位码位置后，通过计算各象限中像素块的黑白情况，便可得到远距二维码的所有信息码。之前通码表确定了每一个远距二维码圆点的空间位置信息，因此对照该码表便可确定出每一个远距二维码的ID信息。
以供判断该ID下的远距二维码是否首次出现，如果非首次，意味着存在已经计算好的该远距二维码在相机坐标系下的空间位姿矩阵，可直接调用；若为首次，便可执行下一步，计算该远距二维码在相机坐标系下的空间位姿。从而节省算力，同时若该远距二维码在环境中的位置已经被记录，则该ID的识别，也可被作为移动机器人在地图中的定位。
步骤S240根据定位码在图像中的圆心坐标，计算对应远距二维码在相机坐标系下的空间位姿。
具体的来说，第一步，确定输入的标签（远距二维码）坐标系坐标和像素（图像）坐标。
如：计算 3×3 单应矩阵，将齐次坐标中的 2D圆点从标签坐标系（其中 位 于远距二维码的中心，标签在 和 方向上延伸一个单位）投影到 2D 图像坐标系。使用 直接线性变换 (DLT) 算法计算单应性。请注意，由于单应性投影点在齐次坐标中，因此仅 按尺度定义。
第二步，建立单应性矩阵与转换矩阵的关系。
计算远距二维码的位置和方向，即外参矩阵，需要额外的信息：相机的焦距和标签的物理尺寸。 其中3 × 3 单应矩阵（由 DLT 计算）可以写成 3 × 4 相机投影矩阵 P（假设它是已知的）和 4×3 截断外参矩阵 E 的乘积。
而外参矩阵通常是 4 × 4，但标签上的每个位置都在标签坐标系中的 z = 0处。 因此，可以将每个标签坐标重写为 z 隐式为零的二维齐次点，并删除外参矩阵的第三列，形成截断的外参矩阵。
建立后的单应性矩阵与转换矩阵的关系为：
其中P为相机投影矩阵，其旋转分量表示为，E为截断外参矩阵，将平移分量表 示为 ，s为等效距离比例因子。
第三步，求解单应性矩阵。
首先平面的单应性被定义为，从一个平面到另一个平面的投影映射，通过数学表 达式描述即，一个平面上的点与投影矩阵相乘，结果为另一个平面上对应点，点 的 坐标为检测到的椭圆的中心点，其中u，v分别表示椭圆中心的像素坐标。表示标签坐标 系下的点坐标。用齐次矩阵表达即为：
其中投影矩阵 H为一个3×3方阵
将式(10)展开
其中上式中等式的格式为矩阵 H中各行系数求比，也就说等式右边的分子分母可同时乘以一个缩放因子
而不影响结果的正确性，因此投影矩阵不是唯一的，矩阵元素可以等比例放缩，故在求解矩阵 H时，未知数只有8个而不是9个，只要为矩阵中任意一非零元素的赋一确定值，其他元素根据比例也就得到了确定的值。
通常情况下，使用的矩阵是将的值置为1而得到的，记为。 为了使元素 h3、 h6为 u、v在 x和y都取 0时的值，这在某些应用场合是有特定意义的。从 前面的推导可以看出，每提供两个平面上的一对点，即可联立分别基于和坐标的两个 方程，而求投影矩阵 H需要8个约束条件，故需提供4对点才能求出H。
而本示例中，为保证位姿计算的精度，则额外加入了一个点。 由步骤S200椭圆检测示例可知，每一个远距二维码都可以提供至少五组定位点。五组定位点相对与四对定位点提供了更充分的约束，继而保证了位姿计算的精度。
第四步，基于SVD求解单应性矩阵。
将 (13)式写成如下形式
也就是
其中
当有两个平面上的五对点 时，即可得如下所示 的方程组
其中
为了更直观，将下式展开
接下来解这个方程，这个矩阵的解法一般使用奇异值分解。
第五步，通过单应性矩阵求解转换矩阵。
通过第四步经SVD分解得到H矩阵后，由于相机的内参信息可以通过相机标定得 到，因此通过得到的单应性矩阵，以及摄像机位置关系即式(21)，可以分别求向量，，。由于旋转矩阵的正交性可得。得到每一个 远距二维码相对与相机坐标系的转换矩阵，进而可得到旋转和平移矩阵。
此外，由于噪声的影响，该方法计算的旋转和平移矩阵并不是全局最优解，因此需要通过构造非线性优化问题对该结果，进行结果优化。
第六步，通过迭代优化方法求解最小化误差函数，以优化旋转和平移矩阵。
由于直接法得到的结果受图像噪声影响很大，因此需要根据像素点和空间点的分 布情况通过迭代优化方法对结果进行优化，得到更加准确的远距二维码姿态信息。根据空 间点集以及其对应的图像点集确定相机姿态信息。
摄像机相对与二维码坐标系的旋转表示为和平移矩阵
在理想情况下无噪声等因素影响下，在摄像机已经标定的情况下，空间点和投 射到相机归一化平面坐标系中的点满足如下方程，
其中分别表示由远距二维码中第个定位码的圆心坐标，按照旋转平移矩 阵转换到相机归一化平面中的点的坐标，表示在远距二维码坐标系中每一个定 位码的圆心空间坐标。
由于误差的影响，空间点和像素点实际上并不满足上述问题，该问题转变为寻找 最优参数最小化误差函数，其中R表示旋转，t表示平移。其中表示平移向量中 的第i个元素，表示旋转矩阵中的第i列，其中分别表示图像中第个远距二维码的 圆心投影到相机归一化坐标系中的坐标。表示所有定位码的的圆心坐标。则最小化误差 函数为：
在最小化过程中，为保证R为旋转矩阵，所以其约束条件有：
其优化函数变为
对于这种非线性优化问题，使用列温伯格-马夸尔特算法，即可求解其对应准确的旋转和平移矩阵。
进一步的，如图12所示，由于在实际使用中，如果远距二维码很小或从很远的距离观察，有时会发生位置奇异的情况。位置奇异问题属于问题基本特性。从在几何上解释，这两个位姿大致对应于对象围绕平面的翻转，该平面的法线穿过从相机中心到对象中心的视线。在此类情况下，通常有两种位姿情况可以由上述方案求解得到，这两种解的结果重投影误差类似，因此无法使用重投影误差选择正确的姿势。
当前还没有可靠的算法能够解决位姿歧义问题，因为它是平面目标恢复3D位姿问题的一个自然属性。解决此问题需要更多信息来约束二维码的位姿。
为此本示例的方法中，骤还包括：步骤S250 按照时间过滤和先验位姿约束，对经步骤S240获得的空间位姿进行约束，其中时间过滤是当移动机器人获取到远距二维码的位姿信息计算当前移动机器人的位姿时，判断移动机器人的位姿是否发生突变，一旦发生错误定位时，则位姿会存在跳变情况，一旦发现，则可滤除当前远距二维码的位置信息。而先验位姿约束则是考虑到远距二维码粘贴在天花板上，其坐标系和地面也就是车体处于平行的平面中，因此远距二维码一旦发生错误位姿，可以通过该约束进行滤除。
步骤S300如图3所示，当判断相机观测到的图像中含有首次观测到的远距二维码时，计算首次观测到的远距二维码在激光地图中的空间位置，并将其加入远距二维码地图中。
具体的，因为相机在移动机器人车体上的位置，以及激光雷达在车体上的位置可以预先得知。或通过二维码观测得到二维码在相机中的位置，及根据激光在激光地图中的位置方式来得知。从而通过这个位置关系，便可以得到当前远距二维码在激光地图中的空间位置。所以，只要激光定位时相机实时看到二维码，就可以实时知道远距二维码在激光地图中的位置。
另一方面，为保证融合建图的精度，本示例中，该计算观测到的远距二维码在激光地图中的空间位置的步骤包括：
步骤S310通过因子图形式融合所有时刻的激光定位信息及远距二维码在相机坐标系下的空间位姿，构建最大后验概率问题优化激光地图中每一个远距二维码的空间位置。
举例来说，假设所有远距二维码在激光地图中的位置为，使用测量值约束待优 化变量，使用贝叶斯网络建模为，通过最大后验概率求解优化后的状态变量。要估 计最大后验概率出现的位置，即找到一个状态量使得各个因子相乘的概率值最大，即求解：
其中，测量值为一组测量值的集合包含以下几个方面：
表示处于机器人位于第个位置时相机对的观测。
表示处于机器人在地图中的第个位置。
表示相机在车体坐标系下的安装位置，通常由标定模块获得。
首先，将问题使用贝叶斯网络进行建模，该问题描述的是移动机器人在当前全局地图（静态场景中激光地图）下进行运动，并进行实时定位，并在运动过程中对一系列天花板中的远距二维码进行观测：
其中表示在所有观测到的第个码时，车体在地图位 置条件下第个码在地图中的位姿的概率密度函数，其中，表示处于第个位置时相 机对的观测。 表示相机在车体坐标系下的安装位置，即外参。表示在地图中某个位 置对二维码的观测函数。
表示一幅图像中看到两个二维码的条件下，观测到两个二维码 的相对位置关系的概率密度函数。
，
接下来要估计最大后验概率出现的二维码空间位置，即找到一个状态量使得各个因子相乘的概率值最大，即求解：
因此，将最大后验概率问题转化成了一个最小二乘问题。即采用最小二乘问题的问题进行求解。
籍此，通过激光的定位信息为远距二维码定位提供了一个绝对坐标系的参考，同时也保证了远距二维码的稀疏特性，降低了远距二维码的部署密度，远距二维码的引入避免了激光定位的一些局限性，也使激光地图快速更新变得可能。
进一步的，如图3至图4所示，由于相机和激光分别处于不同的定位框架中，为保证多传感器数据的融合质量，需要采用同步方法保证多个传感器的数据源有一个统一的时间和空间基准，进而保证了各个传感器测量数据的时空同步。其中空间同步可以由外参标定技术获得相对的位置关系。
而时间同步则是要保证多个传感器的采集信息在一定的时间误差范围内采集和计算传感器数据。此外，时间同步分为软件同步和硬件同步，本方案实现了两种同步方案。
硬件同步：相机与激光雷达都接入同一个控制器中，由控制器为两者提供稳定时间服务。其中相机由控制器通过电平信号触发采集。控制器按照一定的频率触发相机数据采集，由于激光雷达帧率受内部旋转机构的影响，不能使用统一的触发信号。在本方案中。激光雷达输出的数据除了每个激光点的坐标之外，还包含每一个激光点对应的硬件时间戳信息。
软件同步：相机相对于雷达，激光雷达是一个慢速扫描设备。在本方案中使用相机 的作为时间同步基准，通过激光数据获得的基于激光地图的定位信息会通过时间戳信息与 二维码信息进行组合。采集当前时刻的相机数据为。激光雷达的时间戳为每一帧激光 信息中第一个点的激光信息。也就是当前定位的时间戳。其时间戳信息如图4所示。
为得到相机时刻对应的准确的定位信息，需要以相机时刻为基准，对定位信息进 行插值得到对应时刻的定位信息，进而完成时间同步。
进一步的，在依据上述示例的方案进行建图的过程中，为获得更加准确的远距二维码的空间坐标信息，优选在每一个待观测的二维码标记下进行不重复的多次观测，从而提高地图中远距二维码姿态的精度。
另一方面，地图是定位的基础，融合地图不同于纯激光地图和纯二维码地图是由多种类型的传感器信息测量得到的。融合地图不仅包含激光测量信息，也包含场景中的二维码的姿态信息。使用该地图不仅可以实现激光定位，也可实现二维码辅助定位。该地图可以有效的应对单一传感器失效情况下定位情况。
因此，对应上述建图方法，本发明还提供了一种融合激光定位的远距二维码地图定位方法，其步骤包括：
步骤S400当位处激光地图与远距二维码地图混合定位区域时,使用置信度描述定 位信息的准确程度，分别设置激光定位置信度与远距二维码定位置信度，通过各 自位置的定位置信度大小进行加权融合获取定位信息。
具体来说，在本示例中，该融合激光定位的远距二维码地图（文中简称融合地图）示例由三个方面组成：2D空间激光栅格地图、3D空间二维码地图、定位区块配置信息，其中定位区块配置信息包含四个方面：混合定位区、二维码定位区、激光定位区。整个融合地图的定位，通过定位区块配置实现机器人在不同的区域使用不同的定位方式，保证在机器人在复杂场景下的定位有效性。
对于激光定位区，由在激光地图上使用基于蒙特卡洛定位算法提供位置信息。
而对于混合定位区，则是多种定位方式共同使用的定位区域，多种定位方式可通过各自的位置置信度大小进行加权融合。
因此，在融合地图的混合定位区定位过程中，为了确定定位位置的准确性，首先使用置信度描述定位信息的准确程度：
其中激光定位的置信度表示为：
其中，表示在当前地图位置中激光的每一次测量信息。上式描述的是激光点云 与占据栅格地图的重和程度，激光和地图重和程度越大置信度越高，激光在地图中没有对 应点，则置信度为0。
而远距二维码定位置信度表示为：
表示空间中定位圆数量，分别表示当前二维码中每一个定位码圆心的像素 坐标，表示相机的中心坐标，通常由相机标定获得。描述了一个二维码在当前图像 中的分布情况，距离图像中心越远置信度越低。在机器人定位过程中，根据采用的定位类型 分别实时计算对应的定位置信度。
当移动机器人处于混合定位区域时，2D激光定位和远距二维码定位会同时工作，2D激光通过基于蒙特卡洛定位算法输出当前机器人在激光地图下的位置信息。顶视相机也会捕获天花板中的二维码信息并输出相应的定位信息。两种定位信息不仅包含了机器人的空间位置，还包含当前定位方式的定位置信度。当某一种定位置信度低于阈值，则丢弃该定位方式。
例如，将上一时刻车体定位位置设为，定位置信度设为，当前时刻的使 用相机定位的车体定位位置设为，定位置信度设为。当前时刻使用激光定位的车 体位置设为，对应的定位置信度设为。
其融合定位的结果为：
其中表示当前机器人的位置，表示当前混合定位的置信度。
步骤S500当位处远距二维码地图定位区域时，通过相机提取当前场景图像中所有远距二维码的ID信息，以与地图进行匹配获取定位信息。
具体来说，二维码定位区域仅使用相机定位，当观测不到二维码时，可选择通过机器人的轮式里程信息更新其当前的位姿信息。
其中，远距二维码地图包含了当前场景中所有二维码的空间位置信息，移动机器人在运动过程中，实时拍摄场景中的图像并提取相应的二维码信息，通过二维码信息与地图进行匹配实现定位。
例如，设当前图像中检测到的所有二维码定位点的像素坐标表示为：
其中表示当前图像中所有检测到的二维码数量，表示每一个二维码中定位点 的数量，在本方案中使用的远距二维码有5个定位码点。
因此通过地图可知每一个二维码在空间中的位置，进而计算每一个二维码中每一 个定位码圆的圆心在地图中的空间坐标，其中表示地图中二维码的空间姿态，表 示在二维码坐标系中位置，其中。
通过上式得到地图中二维码所有定位码圆心在地图坐标系下的空间点。通过二维 码ID信息，找到地图中所有当前图像检测到的二维码，进而可提取所有的圆心在地图坐标 系的空间点集。
由此，该图像点集以及空间点集构成了经典的视觉定位问题。使用前述示 例中，关于通过迭代优化方法求解最小化误差函数的方案提出的姿态精制算法，即可求解 出当前相机在远距二维码地图下的姿态信息。该视觉定位属于优化问题，需要提供相机姿 态的初始值，进而保证精制结果的有效性。
例如，当当前图像中仅检测到一个二维码时，通过下式给定相机的初始姿态：
当当前图像中检测到多个二维码时，为避免多个二维码定位的互相干扰，相机姿态初值通过多个二维码卡尔曼滤波得到。每一个二维码定位结果的协方差由每一个二维码置信度给定。
为避免二维码由于图像噪声，场景光线引起的二维码姿态出现奇异解，本方案采用二次结果评价机制删除错误的二维码定位信息，其流程图5所示，从而通过该二次结果评价机制，可避免异常值对定位的影响，增强了定位的鲁棒性。
另一方面，如图13所示，对应上述建图及定位方法，本发明还提供了一种融合激光定位的远距二维码地图创建及定位系统，其包括：
存储单元，用于存储包括如上任一所述基于远距二维码的地图创建方法步骤的程序及上述的融合激光定位的远距二维码地图定位方法步骤的程序，以供控制单元，处理单元，激光定位单元适时调取执行；
控制单元，用于控制激光雷达及相机进行时空同步，及控制红外相机拍摄包含远距二维码的待处理图像，控制激光雷达进行建图扫描
激光定位单元，用于根据激光雷达的建图扫描数据，建立建图区域的激光地图，并获取激光定位信息；
处理单元，用于以提取远距二维码在相机坐标系下的空间位姿，当判断相机观测 到的图像中含有首次观测到的远距二维码时，计算首次观测到的远距二维码在激光地图中 的空间位置，并将其加入远距二维码地图中；当位处激光地图与远距二维码地图混合定位 区域时,使用置信度描述定位信息的准确程度，分别设置激光定位置信度与远距二维 码定位置信度，通过各自位置的定位置信度大小进行加权融合获取定位信息；当位处 远距二维码地图定位区域时，通过相机提取当前场景图像中所有远距二维码的ID信息，以 与地图进行匹配获取定位信息。
综上所述，通过本发明提供的该融合激光定位的远距二维码地图创建及定位方法、系统，巧妙的利用了圆形几何形状在空间中即便随相机视角变化，产生仿射变换也只会变成椭圆状，且依然存在圆心的特性，以及圆形即便在远距离检测容易模糊的情况下依然呈圆/椭圆状的鲁棒特性，设计出了适于远距离检测的二维码，同时对应该远距二维码的前述特性，通过椭圆检测，能够十分精确的获得椭圆圆心以建立与相机的位姿变换矩阵，从而解决了传统二维码与检测相机在距离较远、角度变换时，拍摄出的检测图像中的二维码容易呈现模糊的块团状，而造成无法进行准确识别的问题。在此基础上，本发明利用了远距二维码的建图和定位优势及特性，以对激光建图和定位的短板进行了补充融合，从而解决了传统单一激光建图定位容易发生定位丢失的问题，以在更多场景下获得定位优势。
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置、单元及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置、单元及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
此外实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，RandomAccess Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (10)

1.一种融合激光定位的远距二维码地图创建方法，步骤包括：
步骤S100将远距二维码设置在建图区域顶部相对于相机的仰望位；建立建图区域的激光地图，并获取激光定位信息；
步骤S200经相机获取含远距二维码的图像，以提取远距二维码在相机坐标系下的空间位姿；
步骤S300当判断相机观测到的图像中含有首次观测到的远距二维码时，计算首次观测到的远距二维码在激光地图中的空间位置，并将其加入远距二维码地图中。
2.与根据权利要求1所述的基于远距二维码的地图创建方法，其中步骤S300中，计算首次观测到的远距二维码在激光地图中的空间位置的步骤包括：
步骤S310通过因子图形式融合所有时刻的激光定位信息及远距二维码在相机坐标系下的空间位姿，构建最大后验概率问题优化激光地图中每一个远距二维码的空间位置。
3.根据权利要求2所述的基于远距二维码的地图创建方法，其中步骤S310中，构建最大后验概率问题优化激光地图中每一个远距二维码的空间位置的步骤包括：
步骤S311设所有远距二维码在激光地图中的位置为，使用测量值/>约束待优化变量/>，使用贝叶斯网络建模为/>，通过最大后验概率求解优化后的状态变量：
;
其中，测量值为一组测量值的集合：
表示处于移动机器人位于第/>个位置时相机对/>的观测；
表示处于移动机器人在激光地图中的第/>个位置；
表示相机在车体坐标系下的安装位置。
4.根据权利要求1所述的融合激光定位的远距二维码地图创建方法，其中所述远距二维码，包括：定位码，信息码，其中定位码由数个方向点围绕中心点在数个非交叉方向布设而成，并在各个相邻方向点的夹角处划分出数个象限区域，所述信息码根据预设码表布设在对应象限区域中，以与定位码组成二维点阵，其中定位码与信息码表面设有反光层。
5.根据权利要求4所述的融合激光定位的远距二维码地图创建方法，其中步骤S200中，提取远距二维码在相机坐标系下的空间位姿的步骤包括：
步骤S210获取包含远距二维码信息的图像做二值化处理后，进行边缘提取；
步骤S220根据提取的边缘信息进行椭圆拟合检测，获取原始 码及其圆心坐标；
步骤S230根据定位码的几何关系，定位出原始码中对应各远距二维码的定位码分布位置，以筛选出原始码中的信息码，并根据通码表来获取远距二维码的ID信息；
步骤S240当判断该ID下的远距二维码首次出现时，根据定位码在图像中的圆心坐标，计算对应远距二维码在相机坐标系下的空间位姿并与该ID信息绑定。
6.根据权利要求5所述的融合激光定位的远距二维码地图创建方法，其中步骤S240中计算远距二维码在相机坐标系下空间位姿的步骤包括：
步骤S241设定位码的圆心坐标为，建立齐次矩阵
;
基于SVD法求解单应性矩阵H，其中u，v表示定位码圆心的像素坐标、表示远距二维码坐标系下点的坐标，s为等效距离比例因子；
步骤S242根据单应性矩阵与远距二维码在相机坐标系下转换矩阵的关系
;
求得旋转和平移矩阵，其中P为相机投影矩阵、E为截断外参矩阵，, />分别为相机/>方向和y方向的焦距，/>，/>为相机中心点坐标，/>为旋转矩阵中的前两列，/>, />,分别表示在相机坐标系下远距二维码中心的位置，/>为3×3的单应投影矩阵。
7.根据权利要求6所述的融合激光定位的远距二维码地图创建方法，其中步骤S240中还包括：
步骤243通过迭代优化方法求解最小化误差函数
;
以优化旋转和平移矩阵，其中R表示旋转，t表示平移
,/>;
表示平移向量中的第i个元素，/>表示旋转矩阵中的第i列；其中/>分别表示图像中第/>个远距二维码的圆心投影到相机归一化坐标系中的坐标/>；/>表示所有定位码的的圆心坐标；/>分别表示由远距二维码中第/>个定位码的圆心空间坐标，按照旋转平移矩阵转换到相机归一化平面中的点的坐标/>；/>表示在远距二维码坐标系中每一个定位码的圆心空间坐标/>；
其中在求解过程中以作为约束条件。
8.根据权利要求5所述的融合激光定位的远距二维码地图创建方法，其中步骤S200中，提取远距二维码在相机坐标系下的空间位姿的步骤还包括：
步骤S250 按照时间过滤和先验位姿约束，对经步骤S240获得的空间位姿进行约束，其中时间过滤步骤包括：当判断空间位姿结果存在突变时，滤除；先验位姿约束步骤包括：当判断空间位姿结果中远距二维码坐标系和相机坐标系处于非平行时，滤除。
9.一种融合激光定位的远距二维码地图定位方法，其步骤包括：
步骤S400当位处激光地图与远距二维码地图混合定位区域时,使用置信度描述定位信息的准确程度，分别设置激光定位置信度与远距二维码定位置信度/>，通过各自位置的定位置信度大小进行加权融合获取定位信息：
;
;
其中为当的位置，/>表示当前混合定位的置信度，/>为上一时刻定位位置，为当前时刻使用相机定位的定位位置，/>为当前时刻使用激光定位的位置；
步骤S500当位处远距二维码地图定位区域时，通过相机提取当前场景图像中所有远距二维码的ID信息，以与地图进行匹配获取定位信息。
10.一种融合激光定位的远距二维码地图创建及定位系统，其包括：
存储单元，用于存储包括如权利要求1至8中任一所述基于远距二维码的地图创建方法步骤的程序及权利要求9所述的融合激光定位的远距二维码地图定位方法步骤的程序，以供控制单元，处理单元，激光定位单元适时调取执行；
控制单元，用于控制激光雷达及相机进行时空同步，及控制红外相机拍摄包含远距二维码的待处理图像，控制激光雷达进行建图扫描；
激光定位单元，用于根据激光雷达的建图扫描数据，建立建图区域的激光地图，并获取激光定位信息；
处理单元，用于以提取远距二维码在相机坐标系下的空间位姿，当判断相机观测到的图像中含有首次观测到的远距二维码时，计算首次观测到的远距二维码在激光地图中的空间位置，并将其加入远距二维码地图中；当位处激光地图与远距二维码地图混合定位区域时,使用置信度描述定位信息的准确程度，分别设置激光定位置信度与远距二维码定位置信度/>，通过各自位置的定位置信度大小进行加权融合获取定位信息；当位处远距二维码地图定位区域时，通过相机提取当前场景图像中所有远距二维码的ID信息，以与地图进行匹配获取定位信息。
CN202311124250.1A 2023-09-01 2023-09-01 融合激光定位的远距二维码地图创建及定位方法、系统 Pending CN117109561A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311124250.1A CN117109561A (zh) 2023-09-01 2023-09-01 融合激光定位的远距二维码地图创建及定位方法、系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311124250.1A CN117109561A (zh) 2023-09-01 2023-09-01 融合激光定位的远距二维码地图创建及定位方法、系统

Publications (1)

Publication Number Publication Date
CN117109561A true CN117109561A (zh) 2023-11-24

Family

ID=88798097

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311124250.1A Pending CN117109561A (zh) 2023-09-01 2023-09-01 融合激光定位的远距二维码地图创建及定位方法、系统

Country Status (1)

Country Link
CN (1) CN117109561A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117830604A (zh) * 2024-03-06 2024-04-05 成都睿芯行科技有限公司 一种定位用二维码异常检测方法及介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117830604A (zh) * 2024-03-06 2024-04-05 成都睿芯行科技有限公司 一种定位用二维码异常检测方法及介质
CN117830604B (zh) * 2024-03-06 2024-05-10 成都睿芯行科技有限公司 一种定位用二维码异常检测方法及介质

Similar Documents

Publication Publication Date Title
CN110568447B (zh) 视觉定位的方法、装置及计算机可读介质
Wöhler 3D computer vision: efficient methods and applications
CN105654464B (zh) 图像处理装置及图像处理方法
CN112444242B (zh) 一种位姿优化方法及装置
Treible et al. Cats: A color and thermal stereo benchmark
JP5872818B2 (ja) 測位処理装置、測位処理方法、および画像処理装置
CN103886107B (zh) 基于天花板图像信息的机器人定位与地图构建系统
US10366501B2 (en) Method and apparatus for performing background image registration
WO2008097738A1 (en) Automatic stereo measurement of a point of interest in a scene
WO2012113732A1 (en) Determining model parameters based on transforming a model of an object
Phuc Truong et al. Registration of RGB and thermal point clouds generated by structure from motion
Fiala et al. Panoramic stereo reconstruction using non-SVP optics
CN108537214B (zh) 一种室内语义地图的自动化构建方法
Yan et al. Joint camera intrinsic and lidar-camera extrinsic calibration
KR20230065978A (ko) 구조화된 광을 사용하여 장면에서 평면 표면들을 직접 복구하기 위한 시스템, 방법 및 매체
CN115376109B (zh) 障碍物检测方法、障碍物检测装置以及存储介质
CN117109561A (zh) 融合激光定位的远距二维码地图创建及定位方法、系统
CN115359130B (zh) 雷达和相机的联合标定方法、装置、电子设备及存储介质
CN112150448B (zh) 图像处理方法、装置及设备、存储介质
CN116843748B (zh) 一种远距二维码及其物体空间位姿获取方法及系统
CN113643380A (zh) 一种基于单目相机视觉标靶定位的机械臂引导方法
CN112802114B (zh) 多视觉传感器融合装置及其方法和电子设备
Nedevschi Online cross-calibration of camera and lidar
Zhang LILO: A novel LiDAR–IMU SLAM system with loop optimization
CN116619358A (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