CN108332752B - 机器人室内定位的方法及装置 - Google Patents

机器人室内定位的方法及装置 Download PDF

Info

Publication number
CN108332752B
CN108332752B CN201810020362.5A CN201810020362A CN108332752B CN 108332752 B CN108332752 B CN 108332752B CN 201810020362 A CN201810020362 A CN 201810020362A CN 108332752 B CN108332752 B CN 108332752B
Authority
CN
China
Prior art keywords
robot
speckle
pose
landmarks
representing
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.)
Active
Application number
CN201810020362.5A
Other languages
English (en)
Other versions
CN108332752A (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.)
Shenzhen Infinite Power Development Co., Ltd.
Original Assignee
Shenzhen Water World 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 Shenzhen Water World Co Ltd filed Critical Shenzhen Water World Co Ltd
Priority to CN201810020362.5A priority Critical patent/CN108332752B/zh
Publication of CN108332752A publication Critical patent/CN108332752A/zh
Application granted granted Critical
Publication of CN108332752B publication Critical patent/CN108332752B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • 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
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明揭示了机器人室内定位的方法及装置，其中，本发明提供的机器人室内定位的方法，包括：识别预设于室内天花板上的散斑地标；根据所述散斑地标以及机器人的位置变化形成图像位置的拓扑图；将所述拓扑图中的图像坐标转换为机器人坐标的相应位置；根据所述机器人坐标的相应位置定位机器人。本发明预设于天花板的散斑地标作为位置识别标识，便于图像识别，降低前端图像处理算法的复杂度以及计算量，提高定位***的实时性，进而提高机器人室内定位的精准度。

Description

机器人室内定位的方法及装置
技术领域
本发明涉及到机器人领域，特别是涉及到机器人室内定位的方法及装置。
背景技术
同步定位与地图构建(Simultaneous Localization and Mapping，简称SLAM)是机器人领域众所周知的问题。配有传感器的移动机器人，通过从传感器的测量数据中，估计环境地图以及机器人的位置。传感器可分为机器人外部传感器和内部传感器，外部传感器用来测量机器人相对外界环境的移动量，包括摄像机、激光扫描仪、加速度计或者GPS；而内部传感器测量机器人相对自身前一时刻的状态(位置)变化，包括里程计和陀螺仪。在传统的SLAM方法中，比如EKF-SLAM(Extended Kalman Filter-Simultaneous Localizationand Mapping,基于卡尔曼滤波的同步定位与地图构建)，内部传感器用于测量机器人的状态变化，而外部传感器矫正测量误差。现有SLAM应用有两种典型情境：追踪，通常机器人的初始位置已知；全局定位，通常只给出很少甚至不给出有关于机器人起始位置或者环境特征的先验信息。因此，SLAM的核心问题是关于机器人运动路径和所处环境特征的后验估计。要解决上述核心问题，就必须建立适当的模型，以解出后验概率。
机器人学术界涌现出了各种各样的针对SLAM核心问题的解决方案。其中，基于扩展卡尔曼滤波器的EKF-SLAM方法获得了广泛的认可并被普遍应用。EKF-SLAM方法首先估计运动过程中某一时刻的状态，然后以(含噪声的)测量变量的方式获得反馈，最后根据反馈修正估计值。这样，EKF-SLAM方法就能在无需了解机器人详细性质的前提下，高效地对运动的过去、当前、甚至将来的状态进行估计。但上述方法还存在以下缺点：路标常常放置于地面上，容易被周围过往的其它机器人所干扰；简单的路标虽然图像处理简单，但是不具备纠错性，复杂路标虽然具有完整数据纠错性，但是图案过于复杂，实时性差；而且由于测量误差的积累以及计算复杂度随节点的数量增加而急速增大，计算量过大，不容易扩展到大环境下的室内定位。
因此，现有技术还有待改进。
发明内容
本发明的主要目的为提供一种室内定位的方法及装置，旨在解决现有室内定位中SLAM方法中路标数据处理中易引起测量误差，定位不精准的技术问题。
本发明提供了的机器人室内定位的方法，包括：
识别预设于室内天花板上的散斑地标；
根据所述散斑地标以及机器人的位姿变化形成图像位置的拓扑图；
将所述拓扑图中的图像坐标转换为机器人坐标的相应位置信息；
根据所述机器人坐标的相应位置信息定位机器人。
本发明还提供一种机器人室内定位的装置，包括：
识别模块，用于识别预设于室内天花板上的散斑地标；
形成模块，用于根据所述散斑地标以及机器人的位姿变化形成图像位置的拓扑图；
转换模块，用于将所述拓扑图中的图像坐标转换为机器人坐标的相应位置信息；
定位模块，用于根据所述机器人坐标的相应位置信息定位机器人。
本发明有益技术效果：本发明预设于天花板的散斑地标作为位置识别标识，便于图像识别，降低前端图像处理算法的复杂度以及计算量，提高定位***的实时性，进而提高机器人室内定位的精准度。而且本发明采用红外激光投射器主动在天花板上投射红外散斑，不受外界光照的影响，在黑暗环境下也适用，适用于大尺度室内环境的定位以及地图构建。
附图说明
图1本发明一实施例的机器人室内定位的方法流程示意图；
图2本发明一实施例的机器人室内定位的优化方法流程示意图；
图3本发明一实施例的步骤S5的流程示意图；
图4本发明一实施例的步骤S6的流程示意图；
图5本发明一实施例的步骤S62的流程示意图；
图6本发明一实施例的步骤S1的流程示意图；
图7本发明另一实施例的机器人室内定位的方法流程示意图；
图8本发明一实施例的拓扑图示意图；
图9本发明一实施例的拓扑图中的闭环条件系约束关系示意图；
图10本发明一实施例的机器人室内定位***示意图；
图11本发明一实施例的机器人室内定位装置的结构示意图；
图12本发明一实施例的机器人室内定位装置的优化结构示意图；
图13本发明一实施例的确定模块的结构示意图；
图14本发明一实施例的优化模块的结构示意图；
图15本发明一实施例的求解单元的结构示意图；
图16本发明一实施例的识别模块的结构示意图；
图17本发明另一实施例的机器人室内定位装置的结构示意图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
参照图1，本发明一实施例的机器人室内定位的方法，包括：
S1：识别预设于室内天花板上的散斑地标。
本步骤的散斑地标包括预先贴敷于天花板的图片散斑和预先投射的光投影散斑。本实施例优选由红外激光散斑投射器投射的红外散斑，肉眼不可见且对人体无害，且不受外界光照的影响，在黑暗环境下也适用，适用于大尺度室内环境的定位以及地图构建。如附图10所示，本实施例提供了一种机器人室内定位***示意图，该***包括红外激光散斑投射器1、固定在机器人上的广角红外摄像机3以及搭载有里程计的机器人底盘2。本实施例优选在室内均匀放置多个红外激光散斑投射器，将散斑地标投射至天花板上，散斑地标随机分布，每个散斑地标图案各异，以便于识别。根据散斑投射器的投射范围确定红外激光散斑投射器的数量以及位置布局，尽量减少红外散斑的区域重叠，以简化识别，进一步简化计算，同时结合对应的红外摄像机的视野大小以及红外摄像机距离天花板的高度综合考虑，以使得红外摄像机不存在观测死角为准，即摄像机在场景中任何地方至少能拍摄到一个散斑地标。本实施例优选波长为830nm红外激光投射器，使单个红外激光投射器覆盖的定位范围尽可能广。本实施例优选垂直于天花板方向设置于机器人上的红外广角摄像机进行散斑识别，以便红外摄像机有足够大的视野范围。本实施例优选焦距为2.8mm或者2.5mm的广角镜头，镜头前方安装中心波长为830nm的窄带滤波片，只对红外散斑的光波进行感光成像。
S2：根据上述散斑地标以及机器人的位姿变化形成图像位置的拓扑图。
本实施例的基于图的SLAM方法将抽象原始的传感器测量数据转化为构建简单的图优化问题。原始的测量数据被图中的边取代，拓扑图中两个节点之间的边通过机器人位姿的相对位置的概率分布来标记，受到传感器测量数据的约束，如附图8所示，其中节点
Figure GDA0002949339130000041
表示机器人的位姿，
Figure GDA0002949339130000042
表示地标的位置，
Figure GDA0002949339130000043
表示地标与机器人之间的约束关系，
Figure GDA0002949339130000044
表示在t-1时刻与t时刻机器人位姿的约束关系，其中
Figure GDA0002949339130000045
可以通过地标探测传感器测得，而
Figure GDA0002949339130000046
可以通过里程计测量得到。本实施例通过地标与机器人位姿之间的约束关系构建模型，通过图优化解决机器人定位与地图构建问题。
令x＝{x1,...,xT}表示拓扑图中节点的位姿向量，其中xi代表第i个节点的位姿，如附图9所示，zij表示节点i和节点j之间的观测约束，Ωij表示节点i和节点j观测约束的信息矩阵(误差协方差矩阵的逆)，
Figure GDA0002949339130000047
表示预测节点i和节点j之间的位姿转换关系即里程计测量值。
S3：将上述拓扑图中的图像坐标转换为机器人坐标的相应位置信息。
机器人的位姿包含了在平面上的坐标(x,y)以及其方向θ，属于SE(2)空间，地标可以通过二维空间的坐标表示；机器人位置
Figure GDA0002949339130000048
地标位置
Figure GDA0002949339130000049
机器人每移动一定距离或者旋转一定角度，可以通过里程计测量可得到机器人当前时刻的位姿，根据摄像机拍摄的图像，检测当前机器人探测到的路标。
通过里程计测量数据得到机器人的位姿。机器人在t时刻的位姿
Figure GDA00029493391300000410
设里程计测得t到t+1之间的运动距离为Δd，此时机器人在水平面上的航向变化率为ω，则t+1时刻机器人的位姿
Figure GDA0002949339130000051
按照航位推算公式计算为：
Figure GDA0002949339130000052
机器人实时拍摄环境，通过散斑地标的识别，确定散斑地标在图像中的位置，根据摄像机模型，将其转化至摄像机坐标系，得到散斑地标相对于机器人的位置，以作为观测量。本发明实施例中，机器人通过识别红外散斑实现观测。
将机器人通过红外摄像机拍摄到的图像归一化；红外摄像机拍摄到的红外图像是灰色图像，将图像归一化可避免散斑的强弱对散斑地标检测的影响。
对已归一化后的图像进行二值化操作；考虑的散斑投射光照强度的影响，本发明实施例选取自适应阈值方法对灰度图像进行二值化，可通过OpenCV库中adaptiveThreshold函数实现。
在二值化的图像中检测轮廓线，并查找出候选散斑地标对象；考虑到散斑中各个点是分离的，为了提取散斑地标图案的轮廓，本发明首先对图像进行高斯模糊，膨胀以及腐蚀等操作，让散斑中的斑点都连接一起。以便使图像中的散斑变成黑白两种颜色组成的方形。由于本实施例的散斑地标都是长方形，具有闭合的轮廓和特定的大小，因此可以通过检测图像中的轮廓线来查找出目标对象。本发明实施例使用OpenCV库中的FindContours函数进行图像轮廓的提取，通过存储序列记录一列构成轮廓的连续点来表示轮廓。因为不可能是小的轮廓，并通过ContourArea函数计算轮廓面积，剔除面积过小或者过大的非散斑轮廓。本实施例的散斑地标是长方形的，因此轮廓也应该是方形。而经过剔除处理后保留的轮廓中往往有并非四边形轮廓存在。进一步通过多边形近似处理轮廓，记录近似多边形的顶点。如果近似的多边形顶点数大于4，那么这个多边形就不是所要检测的散斑地标，应该舍弃。由于使用的是红外摄像机，在室内时受环境干扰较小，处理之后以便进一步确定是否是散斑地标。同时，摄像机观测物体时，由于距离及方位不同，得到的图像会产生相应的变形。因此要把图像映射到与摄像头垂直的平面上，这样才能得到图像中物体的真实形状。利用OpenCV函数库中的GetPerspectiveTransform和WarpPerspective函数，将得到的图像转换到与摄像头垂直的平面上。图像中保留的四边形就能够与环境中的真实形状对应。最后使用minAreaRect函数计算散斑地标对应的矩形，将矩形的中心点位置(u，v)作为散斑路标的位置。
为了得到散斑地标与机器人之间的位置关系，可利用散斑地标在图像平面上的坐标，确定出其在摄像机坐标系下的三维空间位置，也就是从二维到三维的位姿估计。本实施例根据摄像机投射模型：
Figure GDA0002949339130000061
将地标的位置从图像坐标系转化至摄像机坐标系，也即机器人对应坐标系。其中，M为摄像机内参和畸变参数组成的映射矩阵，(u，v)为图像坐标系上的点，(x，y)为摄像机坐标系上的点，由于摄像机到天花板上的距离是固定的，因此得到散斑图像相对于摄像机投射到水平面上的坐标位置。
S4：根据上述机器人坐标的相应位置信息定位机器人。
参照图2，进一步地，本发明一实施例的机器人室内定位的方法，步骤S3之后，包括：
S5：确定机器人当前位姿与所述散斑地标之间的闭环条件系约束关系。
本实施例中，机器人在t时刻以及t+1时刻之间的位姿关系通过里程计测量得到，
Figure GDA0002949339130000062
到
Figure GDA0002949339130000063
表示机器人在固定时间间隔内的相对运动。由于里程计测量受到噪声的影响，这种测量结果通常与真实是略微有点偏差。里程计测量是在SE(2)群中欧拉变换，假设测量噪声是高斯白噪声，通过3*3的对称矩阵来表示其测量误差，即为信息矩阵，该矩阵取决于机器人的运动。举例地，当机器人移动越大时，其不确定性也随之越大。拓扑图中节点
Figure GDA0002949339130000064
到
Figure GDA0002949339130000065
之间的边由下面两个量来约束：
Figure GDA0002949339130000066
表示节点之间的运动；
Figure GDA0002949339130000067
表示测量的协方差矩阵，为对称的正定矩阵。当机器人检测在位置
Figure GDA0002949339130000068
时探测到地标
Figure GDA0002949339130000069
其在图中对应的边可以通过机器人当前的位姿以及地标的位置来建模。地标的测量有平面上二维坐标(x，y)表示。假设在高斯白噪声下，噪声可以通过其协方差矩阵的逆来建模。机器人位姿到地标之间边的模型通过以下参数表示：
Figure GDA0002949339130000071
表示机器人在
Figure GDA0002949339130000072
所探测到地标的位置；
Figure GDA0002949339130000073
表示地标测量的协方差矩阵。
S6：根据所述闭环条件系约束关系优化所述机器人坐标。
观测值zij的对数似然函数lij，表述为：
Figure GDA0002949339130000074
Figure GDA0002949339130000075
定义测量误差函数e(xi,xj,zij)表示里程计测量值
Figure GDA0002949339130000076
与实际的观测值zij之间的误差，表示为：
Figure GDA0002949339130000077
所以，测量误差函数的对数似然函数的表达式为：
Figure GDA0002949339130000078
Fij表示系数。
本实施例图的优化目标就是找到使得所有测量误差函数的对数似然函数F(x)最小的节点配置x*：
Figure GDA0002949339130000079
在得到一个比较精确的机器人初步位姿
Figure GDA00029493391300000710
的情况下，利用牛顿-高斯优化数值算法求的式(4)的解，在初步位姿
Figure GDA00029493391300000711
附近利用一阶泰勒展开式来逼近误差函数：
Figure GDA00029493391300000712
上述Jij是eij的雅克比矩阵，代入公式(3)可以得到
Figure GDA00029493391300000713
的近似表示：
Figure GDA00029493391300000714
通过局部的逼近，重写上述函数：
Figure GDA00029493391300000715
其中，c＝∑cij，b＝∑bij，和H＝∑Hij。
最小化F(x)来解决该线性***：HΔx*＝-b (8)
上述信息矩阵H，是通过雅克比矩阵来映射测量误差到机器人运动轨迹中，所以结构是稀疏的，其中的非零项是位姿之间的观测限制。
根据等式(8)，***的信息矩阵H和残差矩阵b均是一系列矩阵和向量累加的结构，其中，每一个矩阵和向量分别对应着一个约束，每一个约束均会对***贡献一个附加项Cij，而附加项的结构依赖于***误差函数的雅克比行列式。由于约束的物产函数仅仅依赖于两个节点之间的值，式(6)雅克比矩阵有如下形式：
Figure GDA0002949339130000081
其中Aij和Bij分别是误差函数分别与节点i和节点j相关的导数，从等式(7)可以得到矩阵块Hij和系数向量bij的结构如下：
Figure GDA0002949339130000082
为了简单起见，公式中省略了零项。
参照图3，进一步地，本发明一实施例的机器人室内定位的方法，所述步骤S5，包括：
S50：判断机器人当前位姿下观测的散斑地标与机器人指定位姿下观测的所述散斑地标是否相同。
本实施例中，当机器人移动超过0.5m或者旋转角度超过0.5〇时，就往拓扑图中***新的节点，随着机器人在室内环境中漫游，整个拓扑图就被构建出来。相邻时刻节点之间的实线箭头代表两节点之间的里程计约束，通过前后两帧图像之间散斑地标匹配来提高里程计测量估计精度，而不相邻时刻的节点之间的虚线箭头代表了相机观测数据的约束。机器人开始在未知区域内探索，通过摄像机观测，与过去的观测进行匹配。
S51：若相同，则判定存在闭环条件系的所述约束关系。
若不同位置观测到同一散斑地标，则构成闭环条件系的约束关系。
参照图4，进一步地，本发明一实施例的机器人室内定位的方法，所述步骤S6，包括：
S60：根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第一测量误差函数。
构造机器人位姿
Figure GDA0002949339130000091
到地标
Figure GDA0002949339130000092
之间的误差函数，
Figure GDA0002949339130000093
表示虚拟测量，虚拟测量是机器人从
Figure GDA0002949339130000094
观测到地标
Figure GDA0002949339130000095
的位置，用等式1表示
Figure GDA0002949339130000096
地标在欧拉坐标系下，可以直接计它们的差值来表示误差。地标的误差函数为：
Figure GDA0002949339130000097
定义机器人相邻测量位姿
Figure GDA0002949339130000098
到
Figure GDA0002949339130000099
之间的测量误差函数，里程计测量在SE(2)空间内，通过
Figure GDA00029493391300000910
操作可以写出测量函数为：
Figure GDA00029493391300000911
同样地，第一测量误差函数为：
Figure GDA00029493391300000912
S61：选择机器人移动过程中相邻时刻位姿参量的增量参数。
本实施例的地标位置属于欧拉空间内的点，其对应的增量可以直接相加得到。但机器人的位姿参量不属于欧拉空间，在SE(2)群中，这个空间允许包含许多参数，例如包括旋转矩阵R(θ)和平移向量(x,y)T或者是旋转角度θ和平移向量(x,y)T。
机器人通过在二维平面上的移动作为参数增量，可以选择旋转角度θ和平移向量(x,y)T，通过定义
Figure GDA00029493391300000916
操作来表示机器人相邻时刻位姿的对应增量关系。由于角度不属于欧拉空间，无法直接相加表示增量，每一次累加后必须从新归一化。在本专利中，给出机器人的位姿
Figure GDA00029493391300000913
和增量
Figure GDA00029493391300000914
定义如下操作：
Figure GDA00029493391300000915
Figure GDA0002949339130000101
对应的逆的操作如下：
Figure GDA0002949339130000102
S62：根据所述增量参数通过迭代的方式求解所述第一测量误差函数，以得到优化的所述机器人坐标。
基于图的SLAM问题最终归结到求解线性方程(8)的问题。本发明中通过迭代的线性化方式来解决非线性问题。其中式(3)是基于图的SLAM线性化的表示形式，式(4)是它的解，而使用的增量参数作为其更新步进，在每一次迭代过程中，前一次迭代中的解x*作为下一次迭代过程中的初始位姿。其中***中绝大多数结构都是稀疏的，可以使用稀疏矩阵去存储海森矩阵H，且由于信息矩阵H的对称性，只需要计算上半部分以提高计算效率。
首先，初始化机器人位姿以及偏移向量b和信息矩阵；
将所有的误差量eij和协方差矩阵Ωij，计算器其雅克比矩阵Aij和Bij；
Figure GDA0002949339130000103
计算线性***的H矩阵非零块，根据等式(10)有：
Figure GDA0002949339130000104
Figure GDA0002949339130000105
计算系数向量：
Figure GDA0002949339130000106
最后解线性方程HΔx*＝-b即可实现优化机器人坐标。
参照图5，进一步地，本发明一实施例的机器人室内定位的方法，步骤S62，包括：
S620：在机器人更新位姿时，判断迭代求解所述第一测量误差函数的过程中所述函数值是否具有收敛性。
本步骤的收敛性即是否有趋近于某一指定值的趋势，若有，则具有收敛性。
S621：若是，则判定对应的函数值为所述第一测量误差函数的解。
本步骤中的最趋近的指定值为第一测量误差函数的解，即找到最优化的值，退出循环，计算在起点空间内的海森矩阵H，此时x*为SLAM***的最终结果。
进一步地，步骤S620之后，包括：
S622：若否，则根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第二测量误差函数。
本实施例在更新机器人位姿以及地标位置时，判断其没有收敛，则根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第二测量误差函数，继续循环判断。
参照图6，进一步地，本发明一实施例的机器人室内定位的方法，步骤S1，包括：
S10：通过散斑信息判断拍摄到的所述散斑地标是否为已知散斑地标。
本步骤的散斑信息包括散斑位置、散斑区域、散斑形状等，将新拍摄的散斑地标与之前拍摄的散斑地标通过模板匹配，判断是否是新的地标，如果之前拍摄的散斑地标有与之匹配的模板，则不是新的散斑地标，反之是新的散斑地标。
S11：若是，则调取所述散斑地标的ID信息。
已知散斑地标都存在与之匹配的ID信息，通过识别ID信息区别不同的散斑地标。
S12：通过所述ID信息标识所述散斑地标。
本实施例中对于每一个不同的散斑地标，为之分配一个唯一的ID信息，方便识别散斑地标，以便估计标志物相对于机器人的位置和位姿。
进一步地，步骤S10之后，包括：
S13：若否，则为所述散斑地标匹配相应的ID信息。
若为新的散斑地标，则为之分配新ID信息并保存至本地，以便观测使用。
参照图7，本发明另一实施例的机器人室内定位的方法，步骤S1之前，包括：
S101：标定摄像机的内参和畸变参数。
S102：存储所述内参和畸变参数组成的映射矩阵。
标定摄像机内参和畸变参数组成的映射矩阵M，以便将地标的位置从图像坐标系转化至摄像机坐标系，也即机器人对应坐标系。
参照图11，本发明一实施例的机器人室内定位的装置，包括：
识别模块1，用于识别预设于室内天花板上的散斑地标；
形成模块2，用于根据所述散斑地标以及机器人的位姿变化形成图像位置拓扑图；
转换模块3，用于将所述拓扑图中的图像坐标转换为机器人坐标的相应位置信息；
定位模块4，用于根据所述机器人坐标的相应位置信息定位机器人。
参照图12，进一步地，本发明一实施例的机器人室内定位的装置，包括：
确定模块5，用于确定机器人当前位姿与所述散斑地标之间的闭环条件系约束关系；
优化模块6，用于根据所述闭环条件系约束关系优化所述机器人坐标。
参照图13，进一步地，本发明一实施例的确定模块5，包括：
第一判断单元50，用于判断机器人当前位姿下观测的散斑地标与机器人指定位姿下观测的所述散斑地标是否相同；
判定单元51，用于若相同，则判定存在闭环条件系的所述约束关系。
参照图14，进一步地，本发明一实施例的优化模块6，包括：
定义单元60，用于根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第一测量误差函数；
选择单元61，用于选择机器人移动过程中相邻时刻位姿参量的增量参数；
求解单元62，用于根据所述增量参数通过迭代的方式求解所述第一测量误差函数，以得到优化的所述机器人坐标。
参照图15，进一步地，本发明一实施例的求解单元62，包括：
判断子单元620，用于在机器人更新位姿时，判断迭代求解所述第一测量误差函数的过程中所述函数值是否具有收敛性；
判定子单元621，用于若是，则判定对应的函数值为所述第一测量误差函数的解。
优选地，所述求解单元62，包括：
重新定义子单元622，用于若否，则根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第二测量误差函数。
参照图16，进一步地，本发明一实施例的识别模块1，包括：
第二判断单元10，用于通过散斑信息判断拍摄到的所述散斑地标是否为已知散斑地标；
调取单元11，用于若是，则调取所述散斑地标的ID信息；
标识单元12，用于通过所述ID信息标识所述散斑地标。
进一步地，所述识别模块1，包括：
匹配单元13，用于若否，则为所述散斑地标匹配相应的ID信息。
参照图17，本发明另一实施例的机器人室内定位的装置，包括：
标定模块101，用于标定摄像机的内参和畸变参数。
存储模块102，用于存储所述内参和畸变参数组成的映射矩阵。
上述实施例中提供的机器人室内定位的装置和机器人室内定位的方法均是基于相同的发明构思。因此，机器人室内定位的装置中各个具体实施例的功能模块/单元的具体的功能可以参见前述方法实施例，在此不再赘述。
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (8)

1.一种机器人室内定位的方法，其特征在于，包括：
识别预设于室内天花板上的散斑地标；
根据所述散斑地标以及机器人的位姿变化形成图像位置的拓扑图，拓扑图中两个节点之间的边通过机器人位姿的相对位置的概率分布来标记，受到传感器测量数据的约束，其中节点包括表示机器人的位姿的节点，表示地标的位置的节点，机器人的位姿的节点与地标的位置的节点之间的连线，表示地标与机器人之间的约束关系，两个相邻时刻的所述机器人的位姿的节点之间的连线，表示两个相邻时刻机器人位姿的约束关系，其中地标与机器人之间的约束关系，可以通过地标探测传感器测得，而两个相邻时刻机器人位姿的约束关系，可以通过里程计测量得到；
将所述拓扑图中的图像坐标转换为机器人坐标的相应位置信息；
根据所述机器人坐标的相应位置信息定位所述机器人；
所述将所述拓扑图中的图像坐标转换为机器人坐标的相应位置信息的步骤之后，包括：
确定所述机器人当前位姿与所述散斑地标之间的闭环条件系约束关系；
根据所述闭环条件系约束关系优化所述机器人坐标；
观测值zij的对数似然函数lij,表述为：
Figure FDA0002949339120000011
其中，Ωij表示节点i和节点j观测约束的信息矩阵，
Figure FDA0002949339120000012
表示预测节点i和节点j之间的位姿转换关系的里程计测量值；
测量误差函数e(xi,xj,zij)表示里程计测量值
Figure FDA0002949339120000013
与实际的观测值zij之间的误差，表示为：
Figure FDA0002949339120000014
测量误差函数的对数似然函数的表达式为：
Figure FDA0002949339120000015
Fij表示系数；
找到使得所有测量误差函数的对数似然函数F(x)最小的节点配置x*：
Figure FDA0002949339120000016
利用牛顿-高斯优化数值算法求解
Figure FDA0002949339120000017
在初步位姿
Figure FDA0002949339120000018
附近利用一阶泰勒展开式来逼近误差函数：
Figure FDA0002949339120000021
其中，Jij是eij的雅克比矩阵，得到
Figure FDA0002949339120000022
的近似表示：
Figure FDA0002949339120000023
Δx表示x轴方向的增量，cij为附加项，Hij表示矩阵块，bij表示系数向量；
通过局部的逼近，重写函数：
Figure FDA0002949339120000024
其中，c＝∑cij，b＝∑bij，和H＝∑Hij，最小化F(x)来解决线性***：HΔx*＝-b。
2.根据权利要求1所述的机器人室内定位的方法，其特征在于，所述确定所述机器人当前位姿与所述散斑地标之间的闭环条件系约束关系的步骤，包括：
判断所述机器人当前位姿下观测的散斑地标与所述机器人指定位姿下观测的所述散斑地标是否相同；
若相同，则判定存在所述闭环条件系约束关系。
3.根据权利要求1所述的机器人室内定位的方法，其特征在于，所述根据所述闭环条件系约束关系优化所述机器人坐标的步骤，包括：
根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第一测量误差函数；
选择机器人移动过程中相邻时刻位姿参量的增量参数；
根据所述增量参数通过迭代的方式求解所述第一测量误差函数，以得到优化的所述机器人坐标。
4.根据权利要求3所述的机器人室内定位的方法，其特征在于，所述根据所述增量参数通过迭代的方式求解所述第一测量误差函数的步骤，包括：
在机器人更新位姿时，判断迭代求解所述第一测量误差函数的过程中所述函数值是否具有收敛性；
若是，则判定对应的函数值为所述第一测量误差函数的解；
若否，则根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第二测量误差函数。
5.一种机器人室内定位的装置，其特征在于，包括：
识别模块，用于识别预设于室内天花板上的散斑地标；
形成模块，用于根据所述散斑地标以及机器人的位姿变化形成图像位置的拓扑图，拓扑图中两个节点之间的边通过机器人位姿的相对位置的概率分布来标记，受到传感器测量数据的约束，其中节点包括表示机器人的位姿的节点，表示地标的位置的节点，机器人的位姿的节点与地标的位置的节点之间的连线，表示地标与机器人之间的约束关系，两个相邻时刻的所述机器人的位姿的节点之间的连线，表示两个相邻时刻机器人位姿的约束关系，其中地标与机器人之间的约束关系，可以通过地标探测传感器测得，而两个相邻时刻机器人位姿的约束关系，可以通过里程计测量得到；
转换模块，用于将所述拓扑图中的图像坐标转换为机器人坐标的相应位置信息；
定位模块，用于根据所述机器人坐标的相应位置信息定位机器人；
确定模块，用于确定机器人当前位姿与所述散斑地标之间的闭环条件系约束关系；
优化模块，用于根据所述闭环条件系约束关系优化所述机器人坐标；
观测值zij的对数似然函数lij,表述为：
Figure FDA0002949339120000031
其中，Ωij表示节点i和节点j观测约束的信息矩阵，
Figure FDA0002949339120000032
表示预测节点i和节点j之间的位姿转换关系的里程计测量值；测量误差函数e(xi,xj,zij)表示里程计测量值
Figure FDA0002949339120000033
与实际的观测值zij之间的误差，表示为：
Figure FDA0002949339120000034
测量误差函数的对数似然函数的表达式为：
Figure FDA0002949339120000035
Fij表示系数；找到使得所有测量误差函数的对数似然函数F(x)最小的节点配置x*：
Figure FDA0002949339120000036
利用牛顿-高斯优化数值算法求解
Figure FDA0002949339120000041
在初步位姿
Figure FDA0002949339120000042
附近利用一阶泰勒展开式来逼近误差函数：
Figure FDA0002949339120000043
其中，Jij是eij的雅克比矩阵，得到
Figure FDA0002949339120000044
的近似表示：
Figure FDA0002949339120000045
Δx表示x轴方向的增量，cij为附加项，Hij表示矩阵块，bij表示系数向量；
通过局部的逼近，重写函数：
Figure FDA0002949339120000046
其中，c＝∑cij，b＝∑bij，和H＝∑Hij，最小化F(x)来解决线性***：HΔx*＝-b。
6.根据权利要求5所述的机器人室内定位的装置，其特征在于，所述确定模块，包括：
第一判断单元，用于判断机器人当前位姿下观测的散斑地标与机器人指定位姿下观测的所述散斑地标是否相同；
判定单元，用于若相同，则判定存在闭环条件系的所述约束关系。
7.根据权利要求5所述的机器人室内定位的装置，其特征在于，所述优化模块，包括：
定义单元，用于根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第一测量误差函数；
选择单元，用于选择机器人移动过程中相邻时刻位姿参量的增量参数；
求解单元，用于根据所述增量参数通过迭代的方式求解所述第一测量误差函数，以得到优化的所述机器人坐标。
8.根据权利要求7所述的机器人室内定位的装置，其特征在于，所述求解单元，包括：
判断子单元，用于在机器人更新位姿时，判断迭代求解所述第一测量误差函数的过程中所述函数值是否具有收敛性；
判定子单元，用于若是，则判定对应的函数值为所述第一测量误差函数的解；
重新定义子单元，用于若否，则根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第二测量误差函数。
CN201810020362.5A 2018-01-09 2018-01-09 机器人室内定位的方法及装置 Active CN108332752B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810020362.5A CN108332752B (zh) 2018-01-09 2018-01-09 机器人室内定位的方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810020362.5A CN108332752B (zh) 2018-01-09 2018-01-09 机器人室内定位的方法及装置

Publications (2)

Publication Number Publication Date
CN108332752A CN108332752A (zh) 2018-07-27
CN108332752B true CN108332752B (zh) 2021-04-20

Family

ID=62924393

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810020362.5A Active CN108332752B (zh) 2018-01-09 2018-01-09 机器人室内定位的方法及装置

Country Status (1)

Country Link
CN (1) CN108332752B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109523595B (zh) * 2018-11-21 2023-07-18 南京链和科技有限公司 一种建筑工程直线棱角间距视觉测量方法
CN109613472B (zh) * 2018-12-26 2023-04-28 芜湖哈特机器人产业技术研究院有限公司 一种用于室内无轨导航的红外顶标及其识别方法
CN110874101B (zh) * 2019-11-29 2023-04-18 合肥哈工澳汀智能科技有限公司 一种机器人清扫路径的生成方法及装置
CN111256689B (zh) * 2020-01-15 2022-01-21 北京智华机器人科技有限公司 一种机器人定位方法、机器人和存储介质
CN111274343B (zh) * 2020-01-20 2023-11-24 阿波罗智能技术(北京)有限公司 一种车辆定位方法、装置、电子设备及存储介质
CN112099509B (zh) * 2020-09-24 2024-05-28 杭州海康机器人股份有限公司 地图优化方法、装置及机器人
CN111932675B (zh) * 2020-10-16 2020-12-29 北京猎户星空科技有限公司 建立地图的方法、装置、自移动设备和存储介质

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106153048A (zh) * 2016-08-11 2016-11-23 广东技术师范学院 一种基于多传感器的机器人室内定位及制图***

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9286810B2 (en) * 2010-09-24 2016-03-15 Irobot Corporation Systems and methods for VSLAM optimization
CN102135429B (zh) * 2010-12-29 2012-06-13 东南大学 一种基于视觉的机器人室内定位导航方法
KR101295959B1 (ko) * 2011-10-20 2013-08-13 충북대학교 산학협력단 실내 천장 영상을 이용한 이동 로봇 위치 측정 장치 및 방법
GB2511096A (en) * 2013-02-22 2014-08-27 Fox Murphy Ltd A Mobile Indoor Navigation System
CN104374395A (zh) * 2014-03-31 2015-02-25 南京邮电大学 基于图的视觉slam方法
CN106802658B (zh) * 2017-03-21 2019-09-20 厦门大学 一种全自动高精度室内快速定位方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106153048A (zh) * 2016-08-11 2016-11-23 广东技术师范学院 一种基于多传感器的机器人室内定位及制图***

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
"基于相关系数AR模型的陀螺随机漂移分析方法";刘成瑞 等;《空间控制技术与应用》;20110831;第37卷(第4期);全文 *

Also Published As

Publication number Publication date
CN108332752A (zh) 2018-07-27

Similar Documents

Publication Publication Date Title
CN108332752B (zh) 机器人室内定位的方法及装置
JP6760114B2 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
CN107160395B (zh) 地图构建方法及机器人控制***
CN111076733B (zh) 一种基于视觉与激光slam的机器人室内建图方法及***
WO2019136613A1 (zh) 机器人室内定位的方法及装置
CN112985416B (zh) 激光与视觉信息融合的鲁棒定位和建图方法及***
US9208395B2 (en) Position and orientation measurement apparatus, position and orientation measurement method, and storage medium
Mei et al. Calibration between a central catadioptric camera and a laser range finder for robotic applications
Acharya et al. BIM-Tracker: A model-based visual tracking approach for indoor localisation using a 3D building model
JP5480667B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、プログラム
Muñoz-Bañón et al. Targetless camera-LiDAR calibration in unstructured environments
CN112444246B (zh) 高精度的数字孪生场景中的激光融合定位方法
CN112184824A (zh) 一种相机外参标定方法、装置
Yan et al. Joint camera intrinsic and lidar-camera extrinsic calibration
KR102490521B1 (ko) 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법
Zhang LILO: A novel LiDAR–IMU SLAM system with loop optimization
JP6922348B2 (ja) 情報処理装置、方法、及びプログラム
CN112762929B (zh) 一种智能导航方法、装置和设备
CN111571561B (zh) 移动机器人
CN212044739U (zh) 一种基于惯性数据和视觉特征的定位装置及机器人
Cupec et al. Global localization based on 3d planar surface segments
JP2018116147A (ja) 地図作成装置、地図作成方法及び地図作成用コンピュータプログラム
CN117576200B (zh) 一种长周期移动机器人定位方法、***、设备及介质
CN114983302B (zh) 姿态的确定方法、装置、清洁设备、存储介质及电子装置
Li et al. CAD-vision-range-based self-localization for mobile robot using one landmark

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
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20190906

Address after: Room 402, 4th floor, Kanghe Sheng Building, New Energy Innovation Industrial Park, No. 1 Chuangsheng Road, Nanshan District, Shenzhen City, Guangdong Province, 518000

Applicant after: Shenzhen Infinite Power Development Co., Ltd.

Address before: 518000 Block 503,602, Garden City Digital Building B, 1079 Nanhai Avenue, Shekou, Nanshan District, Shenzhen City, Guangdong Province

Applicant before: SHENZHEN WOTE WODE CO., LTD.

GR01 Patent grant
GR01 Patent grant