本发明涉及到机器人领域，特别是涉及到机器人室内定位的方法及装置。
背景技术：
同步定位与地图构建(simultaneouslocalizationandmapping，简称slam)是机器人领域众所周知的问题。配有传感器的移动机器人，通过从传感器的测量数据中，估计环境地图以及机器人的位置。传感器可分为机器人外部传感器和内部传感器，外部传感器用来测量机器人相对外界环境的移动量，包括摄像机、激光扫描仪、加速度计或者gps；而内部传感器测量机器人相对自身前一时刻的状态(位置)变化，包括里程计和陀螺仪。在传统的slam方法中，比如ekf-slam(extendedkalmanfilter-simultaneouslocalizationandmapping,基于卡尔曼滤波的同步定位与地图构建)，内部传感器用于测量机器人的状态变化，而外部传感器矫正测量误差。现有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所示，其中节点
令x＝{x1,...,xt}表示拓扑图中节点的位姿向量，其中xi代表第i个节点的位姿，如附图9所示，zij表示节点i和节点j之间的观测约束，ωij表示节点i和节点j观测约束的信息矩阵(误差协方差矩阵的逆)，
s3：将上述拓扑图中的图像坐标转换为机器人坐标的相应位置信息。
机器人的位姿包含了在平面上的坐标(x,y)以及其方向θ，属于se(2)空间，地标可以通过二维空间的坐标表示；机器人位置
通过里程计测量数据得到机器人的位姿。机器人在t时刻的位姿
机器人实时拍摄环境，通过散斑地标的识别，确定散斑地标在图像中的位置，根据摄像机模型，将其转化至摄像机坐标系，得到散斑地标相对于机器人的位置，以作为观测量。本发明实施例中，机器人通过识别红外散斑实现观测。
将机器人通过红外摄像机拍摄到的图像归一化；红外摄像机拍摄到的红外图像是灰色图像，将图像归一化可避免散斑的强弱对散斑地标检测的影响。
对已归一化后的图像进行二值化操作；考虑的散斑投射光照强度的影响，本发明实施例选取自适应阈值方法对灰度图像进行二值化，可通过opencv库中adaptivethreshold函数实现。
在二值化的图像中检测轮廓线，并查找出候选散斑地标对象；考虑到散斑中各个点是分离的，为了提取散斑地标图案的轮廓，本发明首先对图像进行高斯模糊，膨胀以及腐蚀等操作，让散斑中的斑点都连接一起。以便使图像中的散斑变成黑白两种颜色组成的方形。由于本实施例的散斑地标都是长方形，具有闭合的轮廓和特定的大小，因此可以通过检测图像中的轮廓线来查找出目标对象。本发明实施例使用opencv库中的findcontours函数进行图像轮廓的提取，通过存储序列记录一列构成轮廓的连续点来表示轮廓。因为不可能是小的轮廓，并通过contourarea函数计算轮廓面积，剔除面积过小或者过大的非散斑轮廓。本实施例的散斑地标是长方形的，因此轮廓也应该是方形。而经过剔除处理后保留的轮廓中往往有并非四边形轮廓存在。进一步通过多边形近似处理轮廓，记录近似多边形的顶点。如果近似的多边形顶点数大于4，那么这个多边形就不是所要检测的散斑地标，应该舍弃。由于使用的是红外摄像机，在室内时受环境干扰较小，处理之后以便进一步确定是否是散斑地标。同时，摄像机观测物体时，由于距离及方位不同，得到的图像会产生相应的变形。因此要把图像映射到与摄像头垂直的平面上，这样才能得到图像中物体的真实形状。利用opencv函数库中的getperspectivetransform和warpperspective函数，将得到的图像转换到与摄像头垂直的平面上。图像中保留的四边形就能够与环境中的真实形状对应。最后使用minarearect函数计算散斑地标对应的矩形，将矩形的中心点位置(u，v)作为散斑路标的位置。
为了得到散斑地标与机器人之间的位置关系，可利用散斑地标在图像平面上的坐标，确定出其在摄像机坐标系下的三维空间位置，也就是从二维到三维的位姿估计。本实施例根据摄像机投射模型：
将地标的位置从图像坐标系转化至摄像机坐标系，也即机器人对应坐标系。其中，m为摄像机内参和畸变参数组成的映射矩阵，(u，v)为图像坐标系上的点，(x，y)为摄像机坐标系上的点，由于摄像机到天花板上的距离是固定的，因此得到散斑图像相对于摄像机投射到水平面上的坐标位置。
s4：根据上述机器人坐标的相应位置信息定位机器人。
参照图2，进一步地，本发明一实施例的机器人室内定位的方法，步骤s3之后，包括：
s5：确定机器人当前位姿与所述散斑地标之间的闭环条件系约束关系。
本实施例中，机器人在t时刻以及t+1时刻之间的位姿关系通过里程计测量得到，
s6：根据所述闭环条件系约束关系优化所述机器人坐标。
观测值zij的对数似然函数lij表述为：
定义测量误差函数e(xi,xj,zij)表示预期的估计值，
所以，测量误差函数表达式为：
本实施例图的优化目标就是找到使得所有测量误差的对数似然函数f(x)最小的节点配置x*：
在得到一个比较精确的机器人初步位姿
上述jij是eij(x)的雅克比矩阵，代入公式(3)可以得到
通过局部的逼近，重写上述函数：
其中，c＝∑cij，b＝∑bij，和h＝∑hij。
最小化f(x)来解决该线性系统：hδx*＝-b(8)
上述信息矩阵h，是通过雅克比矩阵来映射测量误差到机器人运动轨迹中，所以结构是稀疏的，其中的非零项是位姿之间的观测限制。
根据等式(8)，系统的信息矩阵h和残差矩阵b均是一系列矩阵和向量累加的结构，其中，每一个矩阵和向量分别对应着一个约束，每一个约束均会对系统贡献一个附加项cij，而附加项的结构依赖于系统误差函数的雅克比行列式。由于约束的物产函数仅仅依赖于两个节点之间的值，式(6)雅克比矩阵有如下形式：
其中aij和bij分别是误差函数分别与节点i和节点j相关的导数，从等式(7)可以得到矩阵块hij和系数向量bij的结构如下：
为了简单起见，公式中省略了零项。
参照图3，进一步地，本发明一实施例的机器人室内定位的方法，所述步骤s5，包括：
s50：判断机器人当前位姿下观测的散斑地标与机器人指定位姿下观测的所述散斑地标是否相同。
本实施例中，当机器人移动超过0.5m或者旋转角度超过0.5〇时，就往拓扑图中插入新的节点，随着机器人在室内环境中漫游，整个拓扑图就被构建出来。相邻时刻节点之间的实线箭头代表两节点之间的里程计约束，通过前后两帧图像之间散斑地标匹配来提高里程计测量估计精度，而不相邻时刻的节点之间的虚线箭头代表了相机观测数据的约束。机器人开始在未知区域内探索，通过摄像机观测，与过去的观测进行匹配。
s51：若相同，则判定存在闭环条件系的所述约束关系。
若不同位置观测到同一散斑地标，则构成闭环条件系的约束关系。
参照图4，进一步地，本发明一实施例的机器人室内定位的方法，所述步骤s6，包括：
s60：根据所述闭环条件系约束关系，定义机器人相邻时刻位姿下的观测参量与传感器实测参量之间的第一测量误差函数。
构造机器人位姿
地标在欧拉坐标系下，可以直接计它们的差值来表示误差。地标的误差函数为：
定义机器人相邻测量位姿
同样地，第一测量误差函数为：
s61：选择机器人移动过程中相邻时刻位姿参量的增量参数。
本实施例的地标位置属于欧拉空间内的点，其对应的增量可以直接相加得到。但机器人的位姿参量不属于欧拉空间，在se(2)群中，这个空间允许包含许多参数，例如包括旋转矩阵r(θ)和平移向量(x,y)t或者是旋转角度θ和平移向量(x,y)t。
机器人通过在二维平面上的移动作为参数增量可以选择旋转角度θ和平移向量(x,y)t，通过定义
对应的逆的操作如下：
s62：根据所述增量参数通过迭代的方式求解所述第一测量误差函数，以得到优化的所述机器人坐标。
基于图的slam问题最终归结到求解线性方程(8)的问题。本发明中通过迭代的线性化方式来解决非线性问题。其中式(3)是基于图的slam线性化的表示形式，式(4)是它的解，而使用的增量参数作为其更新步进，在每一次迭代过程中，前一次迭代中的解x*作为下一次迭代过程中的初始位姿。其中系统中绝大多数结构都是稀疏的，可以使用稀疏矩阵去存储海森矩阵h，且由于信息矩阵h的对称性，只需要计算上半部分以提高计算效率。
首先，初始化机器人位姿以及偏移向量b和信息矩阵；
将所有的误差量eij和协方差矩阵ωij，计算器其雅克比矩阵aij和bij；
计算线性系统的h矩阵非零块，根据等式(10)有：
计算系数向量：
最后解线性方程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，用于存储所述内参和畸变参数组成的映射矩阵。
上述实施例中提供的机器人室内定位的装置和机器人室内定位的方法均是基于相同的发明构思。因此，机器人室内定位的装置中各个具体实施例的功能模块/单元的具体的功能可以参见前述方法实施例，在此不再赘述。
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。