CN104330090B - 机器人分布式表征智能语义地图创建方法 - Google Patents

Abstract

机器人分布式表征智能语义地图创建方法。首先，机器人遍历室内环境，采用基于拓展卡尔曼滤波算法的视觉定位方法和基于边界虚拟标签算法的射频识别系统分别对机器人和带有快速识别码的人工路标进行定位，建立度量层；然后，利用最小二乘法优化采样点坐标，并且使用自适应谱聚类方法对定位结果进行分类，建立拓扑层；最后，根据摄像机快速识别的QR code语义信息，更新地图的语义属性，建立语义层。本发明在对室内环境中物体状态进行检测时，采用带有QR code的人工路标，大大提高了语义地图创建的效率，降低了创建的难度；同时采用QR code与RFID技术相结合的方法，提高了机器人定位的精确度和地图创建的可靠度。

Description

机器人分布式表征智能语义地图创建方法

技术领域

本发明属于移动机器人导航技术领域，涉及一种室内环境下面向移动机器人同步定位与地图构建(Simultaneous Localization and Mapping,SLAM)问题的机器人分布式表征智能语义地图创建方法。

背景技术

近年来，随着移动机器人在服务、探测、物流等领域的广泛应用，SLAM问题受到了学术界及工程界的密切关注，被视为移动机器人实现完全智能化的关键。

然而，大部分的SLAM研究仍停留在机器人自身的定位和物理环境的重建上，没有涉及到机器人与人的交互，这使得机器人无法完成人类语义命令，大大限制了机器人的工作职能，降低了工作效率。事实上，室内办公环境存在多种带有语义信息的大物体，当移动机器人在室内办公环境中工作时，机器人需要获得复杂工作环境的区域信息和全局信息，创建具有语义信息的全局地图，实现与人类的语义交互，协助人类完成多种复杂工作。因此，一个有效的室内环境下移动机器人精确定位和智能语义地图构建方法具有重要的理论意义及应用价值。

C.Nieto-Granda等人利用高斯区域分析方法，将环境特征按照概率进行划分，从而实现了基于语义的地图划分；而H.Wu等人提出利用快速识别码(Quick Response Code，QR code)存储环境信息，机器人通过视觉传感器来捕获信息，进而确定空间语义关系。然而，前者只考虑了自然路标，在室内工作环境中，一般缺少易于捕获的自然环境特征，故无法解决自然路标稀疏带来的问题；而后者虽然考虑到了自然路标稀疏，辅以QR code来表述环境，但视觉传感器容易受到视角和距离的限制，且在大物体阻挡情况下会失效，进而导致定位精度下降，影响语义地图构建的可靠性。

发明内容

针对移动机器人在室内办公环境下难以获取语义信息、完成语义命令的问题，提供一种面向移动机器人SLAM问题的分布式表征智能语义地图创建方法，机器人在环境中进行遍历式扫描时，利用视觉传感器和射频识别系统(Radio Frequency Identification，RFID)联合定位，同时获取大物体上的QR code信息，将室内环境按照功能进行划分与语义更新，既提高机器人定位的准确性，又实现了语义地图的构建。

室内环境下机器人分布式表征语义地图创建方法，包括以下步骤：

步骤1：采集环境图像，实现机器人视觉定位。

步骤1.1，移动机器人通过双目视觉传感器采集环境图像，获取人工路标集合。

步骤1.2，考虑到极几何约束，基于三角测量原理，利用视差法获取图像中人工路标的深度信息，并计算该路标的全局坐标。

步骤1.3，通过拓展卡尔曼滤波(Extended Kalman Filter，EKF)算法，对机器人位姿及人工路标的位置进行定位。

步骤2：利用RFID系统进行机器人接收信号强度值(Received Signal Strength，RSS)定位。

步骤2.1，根据实际参考标签的坐标和阅读器读取的RSS值，求取虚拟参考标签的坐标值和RSS值。

选用对数距离路径损耗模型表述虚拟参考标签与实际参考标签上的能量差值随两者距离的变化，获取虚拟标签的RSS值。

步骤2.2，基于边界虚拟标签(Boundary Virtual Labels，BVIRE)算法确定目标标签(机器人和人工路标)的坐标值。

根据目标标签和参考标签RSS值的差来选取最近邻标签。设定阈值，构建模糊地图。对于目标标签来说，由于每个阅读器都有自己对应的一个模糊地图，将多个模糊地图求取交集可以求得最近邻参考标签，从而确定目标标签的坐标。

步骤3：修正机器人的全局坐标值，并利用自适应谱聚类算法进行地图划分。

步骤3.1：修正机器人及人工路标的坐标值。

根据视觉传感器和RFID系统获得的机器人坐标值，利用最小二乘法(LeastSquare，LS)对两者进行修正，得到更加准确的机器人全局坐标值。

步骤3.2：利用自适应谱聚类算法，将修正后的机器人坐标集合进行拓扑层划分。

步骤4：语义信息更新。

步骤4.1：机器人判断视界中的红色圆形区域，并利用椭圆拟合方法快速识别和读取QR code中的语义信息。

步骤4.2：根据QR code中的语义信息，将机器人观测点和相应物品之间进行关联，每个子集的语义拓扑地图可以实时更新。

步骤4.3：根据更新的语义信息实时修正拓扑层子地图分类，直至机器人完成遍历性扫描。

与现有技术相比，本发明具有以下有益效果：

一方面，本发明在室内地图进行构建时，采用视觉定位与RFID定位相结合的方法，解决了摄像机视界局限和障碍物阻挡等问题，提高了机器人定位的精度；另一方面，考虑了室内环境下可辨识环境路标缺失带来的语义地图构建困难，采用QR code技术表述环境语义特征，提高了语义地图构建的准确度，为实现人机交互和机器人完成语义命令打下良好的基础。

附图说明

图1是本发明所涉及方法的框架结构图；

图2是本发明机载双目视觉传感器机器人模型示意图；

图3是本发明RFID系统示意图；

图4是基于视觉和RFID联合定位的算法流程图；

图5是本发明中采用QR code人工路标的示意图；

图6是机器人分布式表征智能语义地图创建算法图；

图7是机器人定位结果图；(a)为视觉定位结果，(b)为RFID定位结果；

图8是LS法修正结果图；

图9是本发明中基于语义信息的移动机器人的语义地图划分仿真结果图；(a)为期望结果，(b)为仿真聚类结果。

具体实施方式

以下结合附图对本发明作进一步详细说明。

如图1-9所示，本发明所涉及方法的框架结构如图1所示，包括以下步骤：

步骤1：采集环境图像，实现机器人视觉定位。

步骤1.1，移动机器人通过双目视觉传感器采集环境图像，获取人工路标集合。

步骤1.2，考虑到极几何约束，基于三角测量原理，利用视差法获取图像中人工路标的深度信息，并计算该路标的全局坐标。

(1)移动机器人具有两个独立的驱动车轮，并在机身上装备双目摄像机，机器人模型示意图如图2所示。基于几何学，可以获得：

其中，d_t表示目标的深度信息，f为焦距，两个摄像头之间的基准宽度是L，目标与光学中心线的距离为D₁和D₂，目标与投影距离为d₁和d₂。

(2)在机器人坐标系中，目标的坐标m_l-vision’可以描述为：

(3)显然，全局坐标系中，其坐标m_l-vision可以表示如下：

其中，m_r-vision表示机器人在全球坐标系中的位置，θ为机器人坐标系相对全局坐标系的偏转角度。

步骤1.3，通过EKF算法，对机器人位姿及人工路标的位置进行定位。

(1)初始化机器人的位置状态和误差协方差矩阵，根据当前时间k的位置，可以估计下一时刻k+1的位置：

P(k+1/k)＝F(k)P(k/k)F(k)^T+Q(k)

其中，表示机器人位置估计值，f为状态传递函数，P表示误差协方差矩阵，Q为噪声协方差矩阵，F是f的雅可比矩阵。

(2)利用机载双目视觉摄像机观测人工路标的位置信息，并利用机器人位置估计值和地标的观测值m_l-vision来预测观测估计。

其中，表示观测估计值，h为观测传递函数。

(3)新息及其协方差矩阵可以表示为：

S(k+1)＝H(k)P(k+1/k)H(k)^T+R(k)

其中，γ表示新息值，z为实际观测值，S表示新息协方差矩阵，R为噪声协方差矩阵，H是h的雅可比矩阵。

(4)增益矩阵、机器人位置状态和它的协方差矩阵可以更新如下：

W(k+1)＝P(k+1/k)H^TS^-1(k+1)

P(k+1/k+1)＝(I-W(k+1)H(k))P(k+1/k)

其中，W表示增益矩阵，w表示各采样点的增益值，I表示单位矩阵。

在全局扫描结束后，基于视觉传感器的机器人同步定位与度量层地图构建即完成。

步骤2：利用RFID系统进行机器人RSS定位。

步骤2.1，根据实际参考标签的坐标和阅读器读取的RSS值，求取虚拟参考标签的坐标值和RSS值。

将每4个参考标签作为顶点形成一个单元网格，对每个网格做N*N的划分，添加网格虚拟参考标签，根据实际参考标签位置坐标可求得网络虚拟参考标签的坐标。

(1)选用对数距离路径损耗模型获取虚拟标签的RSS值：

其中，P(d_ij)表示虚拟标签的RSS值，P₀表示实际参考标签的RSS值，d_ij、d₀分别表示虚拟标签和实际参考标签与阅读器的距离，ξ_ij表示噪声，n为路径损耗指数。室内各特定位置的路径损耗指数n是不同的，需要进行实时的测量，影响室内定位的及时性。

(2)采用参考标签来实现动态测量路径损耗指数n，提高室内定位的准确度。即在得到两条靠近待定位标签路径的路径损耗后，可以根据待定位标签与阅读器构成的直线和参考标签与阅读器构成的直线之间的角度求取权值进行加权：

其中，n₁、n₂分别为两条靠近待定位标签路径的路径损耗指数，θ₁、θ₂为两者与带定位路标的夹角。

步骤2.2，基于BVIRE算法确定目标标签(机器人和人工路标)的坐标值。

(1)根据目标标签和参考标签RSS值的差来选取最近邻标签。设定阈值，构建模糊地图。对于目标标签来说，由于每个阅读器都有自己对应的一个模糊地图，将多个模糊地图求取交集可以求得最近邻参考标签，从而确定目标标签的坐标：

其中，m_lRFID、m_rRFID为通过RFID系统获取的人工路标和机器人坐标值，n_a为各阅读器获得的坐标信息总数。w_i为权值，可通过下式计算：

其中，S_k(T_i)表示第K个阅读器读到的(i,j)位置上的场强值，n_a为整个区域内选出的最近邻参考标签的数量，n_ci与邻近参考标签i连接在一起的区域的个数。

通过上述算法，可以实现基于RFID的机器人室内定位和度量层地图创建。

步骤3：修正机器人的全局坐标值，并利用自适应谱聚类算法进行地图划分。

步骤3.1：修正机器人及人工路标的坐标值。

根据视觉传感器和RFID系统获得的机器人坐标值，利用LS方法对两者进行修正，得到更加准确的机器人全局坐标值。

(1)以人工路标为例，视觉定位和RFID定位的的线性校正为：

m_l＝αm_lRFID+(1-α)m_l-vision

其中，ml为修正的人工路标坐标值，α为修正参数。

人工路标的定位误差可表示为：

其中，e为定位误差，(x₀,y₀)为初始坐标值。

(2)基于LS方法，要使得定位误差为极小值，则e'＝0。参数α可以计算为：

由上式可以得到纠正后人工路标的坐标。同理可以获得机器人观测点坐标的校正结果。

步骤3.2：利用自适应谱聚类算法，将修正后的机器人坐标集合进行拓扑层划分。

(1)设将无向加权图G分成两个子图A和B，且A∪B＝V,A∩B＝f。根据谱图理论建立规范割目标函数：

其中，Ncut(A,B)为规范割目标函数，cut(A,B)、cut(B,A)表示图G分成两个子图A和B的代价函数，asso(A,V)、asso(B,V)分别为A、B中各节点与所有节点连接权值总和。

其中：

其中，w(u,v)、w(u,t)表示各节点的连接权值。

(2)最小化Ncut值即获得最理想的组划分：

这种分组的本质是使得大集合V下的两个小集合A，B的元素对对方的影响最小化，即实现了基于谱聚类的区分算法。

(3)基于BVIRE算法的RFID信息和QR code识别信息进行不同位姿下的环境特征匹配。根据传感空间交叠情况设置节点间连线的权值w，两个观测节点i、j对应的匹配程度超过某一阈值K，则认为两节点相似，否则判定两节点无关。

(4)确定所有节点间的权值，形成对称相似矩阵W。将相似矩阵的每行元素相加，即得到该顶点的度d_i，以所有度值为对角元素构成度矩阵(对角矩阵)，用D表示。其中D_ii＝d_i。

其中，x表示谱聚类样本。

(5)将x松弛到连续域[-1.1]，求解minNcut(A,B)的问题转化为：

上式的优化问题等于求解下列等式的第二小特征值问题：(D-W)x＝λDx。其中，λ表示其特征值。与第二小特征值对应的特征向量就包含图的划分信息。基于此特征向量对节点进行组划分，最终获得全局拓扑地图。

步骤4：语义信息更新。

步骤4.1：机器人判断视界中的红色圆形区域，并利用椭圆拟合方法快速识别和读取QR code中的语义信息。

(1)在其外围设定红色圆形识别圈，方便机器人在距离较远时准确找到QR code，进而靠近它并扫描。

(2)机器人到达距离阀值后，机器人实时检测机载摄像机采集的图像信息中是否存在红色区域。检测到红色区域后利用椭圆拟合确定图像中是否存在椭圆，即可确定视野中存在人工地标。

(3)依据视野中人工地标的外层椭圆轮廓控制机器人运动，调整机器人与人工地标之间的距离，使之到达QR Code的识读距离阈值之内。期间须时刻保证检测到的椭圆轮廓处于摄像机视野的中间区域。调整策略为将机载摄像机视野做3*3分割，从左到右从上到下依次为1到9区。当椭圆的中心处于不同区域时，采取相应的运动控制方式。例如，当椭圆中心在1区(左上方)时，机器人应向左转，同时向前行进；当椭圆中心在6区(右侧)时，机器人应向右转。

步骤4.2：根据QR code中的语义信息，将机器人观测点和相应物品之间进行关联，每个子集的语义拓扑地图可以实时更新。

步骤4.3：根据更新的语义信息实时修正拓扑层子地图分类，直至机器人完成遍历性扫描。QR code人工路标中写入了编号、名字、功能和所属区域等信息。机器人在观测图像中读取QR code，并立即向数据库添加语义信息。当机器人进行全局扫描时，将机器人观测点和相应物品之间进行关联，从而每个子集的语义拓扑地图可以实时更新。数据库中存储着所有大物体的语义信息，以及根据QR code中的信息实现的拓扑地图的语义分类。机器人分布式表征智能语义地图创建算法图如图6所示。

为了证明本发明的有效性，进行了仿真实验。以某8*8室内办公环境为例，办公室中包含沙发、办公桌等大物体；在办公室天花板上安装RFID定位系统。视觉定位结果与RFID定位结果如图7(a)、(b)所示。将两者定位结果通过LS方法进行修正，其结果如图8所示，可见联合定位准确度明显高于独立定位结果。基于语义信息的谱聚类结果如图9所示，仿真结果显示该方法所构建的语义地图具有很高的准确性和有效性，使得机器人在实现准确定位的同时，完成智能语义地图的构建。

Claims (2)

1.室内环境下机器人分布式表征语义地图创建方法，其特征在于：该方法包括以下步骤，

步骤1：采集环境图像，实现机器人视觉定位；

步骤1.1，移动机器人通过双目视觉传感器采集环境图像，获取人工路标集合；

步骤1.2，考虑到极几何约束，基于三角测量原理，利用视差法获取图像中人工路标的深度信息，并计算该路标的全局坐标；

步骤1.3，通过拓展卡尔曼滤波算法，对机器人位姿及人工路标的位置进行定位；

步骤2：利用RFID系统进行机器人接收信号强度值定位；

步骤2.1，根据实际参考标签的坐标和阅读器读取的RSS值，求取虚拟参考标签的坐标值和RSS值；

选用对数距离路径损耗模型表述虚拟参考标签与实际参考标签上的能量差值随两者距离的变化，获取虚拟标签的RSS值；

步骤2.2，基于边界虚拟标签算法确定目标标签的坐标值；

根据目标标签和参考标签RSS值的差来选取最近邻标签；设定阈值，构建模糊地图；对于目标标签来说，由于每个阅读器都有自己对应的一个模糊地图，将多个模糊地图求取交集可以求得最近邻参考标签，从而确定目标标签的坐标；

步骤3：修正机器人的全局坐标值，并利用自适应谱聚类算法进行地图划分；

步骤3.1：修正机器人及人工路标的坐标值；

根据视觉传感器和RFID系统获得的机器人坐标值，利用最小二乘法(Least Square，LS)对两者进行修正，得到更加准确的机器人全局坐标值；

步骤3.2：利用自适应谱聚类算法，将修正后的机器人坐标集合进行拓扑层划分；

步骤4：语义信息更新；

步骤4.1：机器人判断视界中的红色圆形区域，并利用椭圆拟合方法快速识别和读取QRcode中的语义信息；

步骤4.2：根据QR code中的语义信息，将机器人观测点和相应物品之间进行关联，每个子集的语义拓扑地图可以实时更新；

步骤4.3：根据更新的语义信息实时修正拓扑层子地图分类，直至机器人完成遍历性扫描。

2.根据权利要求1所述的室内环境下机器人分布式表征语义地图创建方法，其特征在于：该方法的具体实施流程如下步骤，

步骤1：采集环境图像，实现机器人视觉定位；

步骤1.1，移动机器人通过双目视觉传感器采集环境图像，获取人工路标集合；

步骤1.2，考虑到极几何约束，基于三角测量原理，利用视差法获取图像中人工路标的深度信息，并计算该路标的全局坐标；

(1)移动机器人具有两个独立的驱动车轮，并在机身上装备双目摄像机；基于几何学，可以获得：

$L=D_1-D_2=\frac{d_t}{f}\left(d_1-d_2\right)=\frac{d_t}{f}\mathrm{\Δ}d$

$d_t=\frac{L\×f}{\mathrm{\Δ}d}$

其中，d_t表示目标的深度信息，f为焦距，两个摄像头之间的基准宽度是L，目标与光学中心线的距离为D₁和D₂，目标与投影距离为d₁和d₂；

(2)在机器人坐标系中，目标的坐标m_l-vision’可以描述为：

${m_{l-vision}}^{\′}={(d_t,-\frac{d_t}{f}(\frac{d_1+d_2}{2}\left)\right)}^T$

(3)显然，全局坐标系中，其坐标m_l-vision可以表示如下：

$m_{l-vision}=m_{r-vision}+\left[\begin{array}{cc}cos\mathrm{\θ}& sin\mathrm{\θ}\\ -sin\mathrm{\θ}& cos\mathrm{\θ}\end{array}\right]\·{m_{l-vision}}^{\′}$

其中，m_r-vision表示机器人在全球坐标系中的位置，θ为机器人坐标系相对全局坐标系的偏转角度；

步骤1.3，通过EKF算法，对机器人位姿及人工路标的位置进行定位；

(1)初始化机器人的位置状态和误差协方差矩阵，根据当前时间k的位置，可以估计下一时刻k+1的位置：

${\hat{m}}_{r-vision}(k+1/k)=f\left({\hat{m}}_{r-vision}\right(k/k\left)\right)$

P(k+1/k)＝F(k)P(k/k)F(k)^T+Q(k)

其中，表示机器人位置估计值，f为状态传递函数，P表示误差协方差矩阵，Q为噪声协方差矩阵，F是f的雅可比矩阵；

(2)利用机载双目视觉摄像机观测人工路标的位置信息，并利用机器人位置估计值和地标的观测值m_l-vision来预测观测估计；

$\hat{z}(k+1)=h\left({\hat{m}}_{r-vision}\right(k+1/k),m_{l-vision})$

其中，表示观测估计值，h为观测传递函数；

(3)新息及其协方差矩阵可以表示为：

$\mathrm{\γ}(k+1)=z(k+1)-\hat{z}(k+1)$

S(k+1)＝H(k)P(k+1/k)H(k)^T+R(k)

其中，γ表示新息值，z为实际观测值，S表示新息协方差矩阵，R为噪声协方差矩阵，H是h的雅可比矩阵；

(4)增益矩阵、机器人位置状态和它的协方差矩阵可以更新如下：

W(k+1)＝P(k+1/k)H^TS^-1(k+1)

${\hat{m}}_{r-vision}(k+1/k+1)={\hat{m}}_{r-vision}(k+1/k)+w(k+1)\mathrm{\γ}(k+1)$

P(k+1/k+1)＝(I-W(k+1)H(k))P(k+1/k)

其中，W表示增益矩阵，w表示各采样点的增益值，I表示单位矩阵；

在全局扫描结束后，基于视觉传感器的机器人同步定位与度量层地图构建即完成；

步骤2：利用RFID系统进行机器人RSS定位；

步骤2.1，根据实际参考标签的坐标和阅读器读取的RSS值，求取虚拟参考标签的坐标值和RSS值；

将每4个参考标签作为顶点形成一个单元网格，对每个网格做N*N的划分，添加网格虚拟参考标签，根据实际参考标签位置坐标可求得网络虚拟参考标签的坐标；

(1)选用对数距离路径损耗模型获取虚拟标签的RSS值：

$P\left(d_{ij}\right)=P_0-10n\lg \left(\frac{d_{ij}}{d_o}\right)+{\mathrm{\ξ}}_{ij}$

其中，P(d_ij)表示虚拟标签的RSS值，P₀表示实际参考标签的RSS值，d_ij、d₀分别表示虚拟标签和实际参考标签与阅读器的距离，ξ_ij表示噪声，n为路径损耗指数；室内各特定位置的路径损耗指数n是不同的，需要进行实时的测量，影响室内定位的及时性；

(2)采用参考标签来实现动态测量路径损耗指数n，提高室内定位的准确度；即在得到两条靠近待定位标签路径的路径损耗后，可以根据待定位标签与阅读器构成的直线和参考标签与阅读器构成的直线之间的角度求取权值进行加权：

$n=n_1\frac{{\mathrm{\θ}}_1}{{\mathrm{\θ}}_1+{\mathrm{\θ}}_2}+n_2\frac{{\mathrm{\θ}}_2}{{\mathrm{\θ}}_1+{\mathrm{\θ}}_2}$

其中，n₁、n₂分别为两条靠近待定位标签路径的路径损耗指数，θ₁、θ₂为两者与带定位路标的夹角；

步骤2.2，基于BVIRE算法确定目标标签的坐标值；

(1)根据目标标签和参考标签RSS值的差来选取最近邻标签；设定阈值，构建模糊地图；对于目标标签来说，由于每个阅读器都有自己对应的一个模糊地图，将多个模糊地图求取交集可以求得最近邻参考标签，从而确定目标标签的坐标：

$m_{lRFID}=(x_{lRFID},y_{lRFID})={\mathrm{\Σ}}_{i=1}^{n_a}{w}_i(x_{li},y_{li})$

$m_{rRFID}=(x_{rRFID},y_{rRFID})={\mathrm{\Σ}}_{i=1}^{n_a}{w}_i(x_{ri},y_{ri})$

其中，m_lRFID、m_rRFID为通过RFID系统获取的人工路标和机器人坐标值，n_a为各阅读器获得的坐标信息总数；w_i为权值，可通过下式计算：

$w_i=\frac{1}{{\mathrm{\Σ}}_{k=1}^K\frac{|S_k\left(T_i\right)-S_k\left(R\right)|}{K\×S_k\left(T_i\right)}}\×\frac{n_{ci}}{{\mathrm{\Σ}}_{i=1}^{n_a}{n}_{ci}}$

其中，S_k(T_i)表示第K个阅读器读到的(i,j)位置上的场强值，n_a为整个区域内选出的最近邻参考标签的数量，n_ci与邻近参考标签i连接在一起的区域的个数；

通过上述算法，可以实现基于RFID的机器人室内定位和度量层地图创建；

步骤3：修正机器人的全局坐标值，并利用自适应谱聚类算法进行地图划分；

步骤3.1：修正机器人及人工路标的坐标值；

根据视觉传感器和RFID系统获得的机器人坐标值，利用LS方法对两者进行修正，得到更加准确的机器人全局坐标值；

(1)以人工路标为例，视觉定位和RFID定位的的线性校正为：

m_l＝αm_lRFID+(1-α)m_l-vision

其中，ml为修正的人工路标坐标值，α为修正参数；

人工路标的定位误差可表示为：

$e={\{{\[\mathrm{\α}(x_{lRFID}-x_{l-vision})+(x_{l-vision}-x_0)\]}^2+{\[\mathrm{\α}(y_{lRFID}-y_{l-vision})+(y_{l-vision}-y_0)\]}^2\}}^{\frac{1}{2}}$

其中，e为定位误差，(x₀,y₀)为初始坐标值；

(2)基于LS方法，要使得定位误差为极小值，则e'＝0；参数α可以计算为：

$\mathrm{\α}=-\frac{\left(x_{lRFID}-x_{l-vision}\right)\left(x_{l-vision}-x_0\right)}{{\left(x_{lRFID}-x_{l-vision}\right)}^2+{\left(y_{lRFID}-y_{l-vision}\right)}^2}-\frac{\left(y_{lRFID}-y_{l-vision}\right)\left(y_{l-vision}-y_0\right)}{{\left(x_{lRFID}-x_{l-vision}\right)}^2+{\left(y_{lRFID}-y_{l-vision}\right)}^2}$

由上式可以得到纠正后人工路标的坐标；同理可以获得机器人观测点坐标的校正结果；

步骤3.2：利用自适应谱聚类算法，将修正后的机器人坐标集合进行拓扑层划分；

(1)设将无向加权图G分成两个子图A和B，且A∪B＝V,A∩B＝f；根据谱图理论建立规范割目标函数：

$Ncut(A,B)=\frac{cut(A,B)}{asso(A,V)}+\frac{cut(B,A)}{asso(B,V)}$

其中，Ncut(A,B)为规范割目标函数，cut(A,B)、cut(B,A)表示图G分成两个子图A和B的代价函数，asso(A,V)、asso(B,V)分别为A、B中各节点与所有节点连接权值总和；

其中：

$cut(A,B)=\underset{u\∈A,v\∈B}{\Σ}w(u,v)$

$asso(A,V)=\underset{u\∈A,t\∈V}{\Σ}w(u,t)$

其中，w(u,v),w(u,t)表示各节点的连接权值；

(2)最小化Ncut值即获得最理想的组划分：

$\{A,B\}=\arg \underset{A,B}{\min }Ncut(A,B)$

这种分组的本质是使得大集合V下的两个小集合A，B的元素对对方的影响最小化，即实现了基于谱聚类的区分算法；

(3)基于BVIRE算法的RFID信息和QR code识别信息进行不同位姿下的环境特征匹配；根据传感空间交叠情况设置节点间连线的权值w，两个观测节点i、j对应的匹配程度超过某一阈值K，则认为两节点相似，否则判定两节点无关；

(4)确定所有节点间的权值，形成对称相似矩阵W；将相似矩阵的每行元素相加，即得到该顶点的度d_i，以所有度值为对角元素构成度矩阵，用D表示；其中D_ii＝d_i；

$\min Ncut(A,B)=min\frac{x^T(D-W)x}{x^{T}Dx}$

其中，x表示谱聚类样本；

(5)将x松弛到连续域[-1，1]，求解minNcut(A,B)的问题转化为：

$\arg \underset{x^{T}DI=0}{min}\frac{x^T(D-W)x}{x^{T}Dx}$

上式的优化问题等于求解下列等式的第二小特征值问题：(D-W)x＝λDx；其中，λ表示其特征值；与第二小特征值对应的特征向量就包含图的划分信息；基于此特征向量对节点进行组划分，最终获得全局拓扑地图；

步骤4：语义信息更新；

步骤4.1：机器人判断视界中的红色圆形区域，并利用椭圆拟合方法快速识别和读取QRcode中的语义信息；

(1)在其外围设定红色圆形识别圈，方便机器人在距离较远时准确找到QR code，进而靠近它并扫描；

(2)机器人到达距离阀值后，机器人实时检测机载摄像机采集的图像信息中是否存在红色区域；检测到红色区域后利用椭圆拟合确定图像中是否存在椭圆，即可确定视野中存在人工地标；

(3)依据视野中人工地标的外层椭圆轮廓控制机器人运动，调整机器人与人工地标之间的距离，使之到达QR Code的识读距离阈值之内；期间须时刻保证检测到的椭圆轮廓处于摄像机视野的中间区域；调整策略为将机载摄像机视野做3*3分割，从左到右从上到下依次为1到9区；当椭圆的中心处于不同区域时，采取相应的运动控制方式；例如，当椭圆中心在1区即左上方时，机器人应向左转，同时向前行进；当椭圆中心在6区即右侧时，机器人应向右转；

步骤4.2：根据QR code中的语义信息，将机器人观测点和相应物品之间进行关联，每个子集的语义拓扑地图可以实时更新；

步骤4.3：根据更新的语义信息实时修正拓扑层子地图分类，直至机器人完成遍历性扫描；QR code人工路标中写入了编号、名字、功能和所属区域等信息；机器人在观测图像中读取QR code，并立即向数据库添加语义信息；当机器人进行全局扫描时，将机器人观测点和相应物品之间进行关联，从而每个子集的语义拓扑地图可以实时更新；数据库中存储着所有大物体的语义信息，以及根据QR code中的信息实现的拓扑地图的语义分类。