CN112325883B - 一种WiFi和视觉多源融合的移动机器人室内定位方法 - Google Patents

Abstract

本发明提供一种WiFi和视觉多源融合的移动机器人室内定位方法，包括如下步骤：采集室内定位空间中的WiFi信息和图像信息，进行预处理后，建立WiFi地图和视觉SLAM地图；对多个地标分别计算在所述WiFi地图和所述视觉SLAM地图中的位置信息，并采用指纹地标法建立所述WiFi地图和所述视觉SLAM地图的映射关系，使所述WiFi地图和所述视觉SLAM地图融合；移动机器人处于定位空间内的任一位置时，采用基于信息增益的KNN指纹定位算法利用所述WiFi地图进行WiFi定位；采用图像的特征匹配算法利用所述视觉SLAM地图进行视觉SLAM定位；将WiFi定位与视觉SLAM定位中的视觉词袋技术融合实现粗定位；以粗定位的范围作为约束条件采用视觉SLAM定位实现精确定位。本发明提供的定位方法定位精度高。

Description

一种WiFi和视觉多源融合的移动机器人室内定位方法

技术领域

本发明涉及室内定位技术领域，尤其涉及一种WiFi和视觉多源融合的 移动机器人室内定位方法。

背景技术

移动机器人在完成接待、搬运、巡逻等多种任务时必然离不开自主导 航系统，然而自主导航系统中最关键的技术是精准定位技术。近年来，移 动机器人的室内定位技术得到了飞速的发展，以相机、超声波、地磁场、 蓝牙等为主的室内定位技术不断涌出，但是目前没有任何一种室内定位技 术被广泛应用于市场中，仅被局部使用。由于室内环境复杂多变，存在光 照强度，人为活动，室内温度等多重干扰，最终影响室内定位的精度。同 时不同的传感器存在各自的局限性，获取的特征信息单一，被应用在不同 场景中。目前还没有设计出具备高精度、低成本、高实时性的室内定位系 统应用于多种大型室内环境中。

相关技术中，单一的WiFi定位和视觉SLAM定位被广泛应用于室内 定位技术之中。在大型室内环境中，WiFi定位虽然实时性高、部署简单、 成本低，但是定位精度较低；视觉SLAM虽然具有较高的定位精度，但是 存在实时性低、鲁棒性差的问题。

因此，有必要提供一种WiFi和视觉多源融合的移动机器人室内定位方 法及系统来解决上述问题。

发明内容

本发明针对上述要解决的技术问题，提供一种在大型室内定位空间中 定位精度高、实时性高的一种WiFi和视觉多源融合的移动机器人室内定位 方法。

本发明提供一种WiFi和视觉多源融合的移动机器人室内定位方法，包 括如下步骤：

S1：采集室内定位空间中的WiFi信息和图像信息，进行预处理后，建 立WiFi地图和视觉SLAM地图；

S2：对多个地标分别计算在所述WiFi地图和所述视觉SLAM地图中 的位置信息，并采用指纹地标法建立所述WiFi地图和所述视觉SLAM地 图的映射关系，使所述WiFi地图和所述视觉SLAM地图融合；

S3：移动机器人处于定位空间内的任一位置时，采用基于信息增益的 KNN指纹定位算法利用所述WiFi地图进行WiFi定位；采用图像的特征匹 配算法利用所述视觉SLAM地图进行视觉SLAM定位；

S4：将WiFi定位与视觉SLAM定位中的视觉词袋技术融合实现粗定 位；

S5：以粗定位的范围作为约束条件采用视觉SLAM定位实现精确定位。

优选的，所述步骤S1包括如下步骤：

S11：在室内定位空间中布置M个网指纹网格点，每个所述指纹网格 点获取N个无线接入点所发出的WiFi信息，每个所述无线接入点采集n个 样本数据；同时获取每个所述指纹网格点周围的图像信息；

S12：对所述样本数据采用格拉布斯准则进行异常值检测，若检测为异 常值则剔除，反之则保留；

S13：对室内定位空间中的所有无线接入点计算其信息增益，并根据信 息增益的大小对所述无线接入点进行排序，以信息增益从大到小的顺序筛 选出前N_AP个无线接入点，依次表示为：

S14：每个所述指纹网格点的WiFi信息表示为L_i＝{RSSI_i,o_i},i＝1,2,...,M， 其中，

表示在第i个指纹网格点上所采集的N_AP个无线 接入点信号强度的信息向量，

是第i个指纹网格点上测得第N_AP个无线 接入点的信号强度，o_i＝{x_i,y_i}表示第i个指纹网格点的位置信息，其中x_i和y_i分别是第i个指纹网格点在世界坐标系下的x轴和y轴坐标；

S15：通过所述指纹网格点的WiFi信息构建WiFi地图，通过所述指纹 网格点的图像信息构建视觉SLAM地图。

优选的，所述图像信息包括彩色图像信息和深度图像信息。

优选的，所述步骤S12包括如下步骤：

S121：通过n个所述数据样本选定明显水平a，查找格拉布斯表获取临 界值T(n,a)；

S122：计算样本数据的平均值X_avg和标准差σ，计算过程如下：

S123：对每个数据样本计算统计量T，计算过程如下：

S124：针对任意数据样本，当T≥T(n,a)时，则该数据为异常值需要被 剔除，当T＜T(n,a)时为正常数据进行保留。

优选的，所述步骤S2具体为：

在所述指纹网格点中随机选择若干个作为地标，得到所述地标在世界 坐标系下的位置信息，通过视觉图像计算得到相应地标在机器人坐标系下 的位置信息，世界坐标系和机器人坐标系的位置转化表示如下：

其中，Loc和Loc′分别表示世界坐标系和机器人坐标系下的位置，R_T表 示平移矩阵。

优选的，所述步骤S3包括如下步骤：

在WiFi定位中，计算移动机器人所处的位置与指纹网格点的欧式距 离：

定义距离信息为

且满足d₁＜d₂＜…＜d_M，式 中

表示相应指纹网格点的位置信息，取前K个距离信息

采用基于信息增益的KNN指纹定位算法，将前K个距离值进行权重值 计算，得到权重值信息：

最终依据权重值估计移动机 器人所处的位置：

优选的，所述步骤S3包括如下步骤：

在视觉SLAM定位中，先筛选步骤S1中采集到的图像信息得到关键 帧图像信息，然后采用ORB特征提取算法得到相似度值信息，以特征点对 为中心建立3*3的核，并对这9个灰度值信息取平均值得到领域灰度信息， 依据该信息得到新相似度值信息：

其中，N1表示视觉词袋中关键帧总数，S_i，i＝1,2,...N1表示当前移动机器 人所处的位置的图像信息与关键帧图像信息的相似度值，s₁＞s₂＞…＞s_N1，通 过相似度来估计移动机器人所处的位置。

优选的，所述步骤S4包括如下步骤：

建立一个得分机制score(i)(j)＝score1(i)+score2(i)+score3(i)(j)，其中score1(i),i＝1,2,…,K为K个WiFi指纹距离得分；score2(j),j＝1,2,...,NK为NK个图 像相似度得分；

将K个WiFi指纹位置与NK个图像位置采用欧氏距离进行两两组合， 得到K*NK个组合位置距离

score3(i)(j),i＝1,2,...,K；j＝1,2,...,NK为K*NK个组合位置距离得分；

通过得分机制得到任意点与各个指纹网格点的得分值score(i)(j)，从中 选出得分值最高所对应的指纹网格点的相关信息，作为定位的粗定位位置， 粗定位位置表示为：

和

分别表示从中选出得分值最高所对应的WiFi指纹位置信息和 图像位置信息。

优选的，所述步骤S5包括如下步骤：

通过WiFi定位与视觉SLAM定位融合得到的粗定位位置o_m，以o_m为 圆心，以距离阈值d_t为半径，所形成的圆形区域为粗定位范围，以粗定位 范围为限制条件，采用自适应蒙特卡洛定位算法，进行视觉SLAM精定位， 实现最终定位结果。

与相关技术相比，本发明中应用WiFi和视觉SLAM中的视觉词袋技 术实现粗定位，最后在粗定位的约束下采用视觉SLAM定位技术从而实现 最终定位结果。本发明有效的将WiFi和视觉SLAM进行融合，并在定位 准确率、定位精度、定位时间等性能上得到显著提升。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述 中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅 是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性 劳动的前提下，还可以根据这些附图获得其它的附图，其中：

图1为本发明提供的一种WiFi和视觉多源融合的移动机器人室内定位 方的步骤流程图；

图2为图1所示的一种WiFi和视觉多源融合的移动机器人室内定位方 的流程简图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进 行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例， 而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没 有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的 范围。

请结合参阅图1及图2，本发明提供了本发明提供一种WiFi和视觉多 源融合的移动机器人室内定位方法，包括如下步骤：

S1：采集室内定位空间中的WiFi信息和图像信息，进行预处理后，建 立WiFi地图和视觉SLAM地图。

所述步骤S1具体为：

S11：在室内定位空间中布置M个指纹网格点，每个所述指纹网格点获 取N个无线接入点所发出的WiFi信息，每个所述无线接入点采集n个样本 数据；同时获取每个所述指纹网格点位置周围的图像信息。所述图像信息 包括彩色图像信息和深度图像信息。

S12：对所述样本数据采用格拉布斯准则进行异常值检测，若检测为异 常值则剔除，反之则保留。

所述步骤S12具体为：

S121：通过n个所述数据样本选定明显水平a，查找格拉布斯表获取临 界值T(n,a)；

S122：计算样本数据的平均值X_avg和标准差σ，计算过程如下：

S123：对每个数据样本计算统计量T，计算过程如下：

S124：针对任意数据样本，当T≥T(n,a)时，则该数据为异常值需要被 剔除，当T＜T(n,a)时为正常数据进行保留。

格拉布斯准则置信概率为95％，可以有效的剔除偶然误差，提高自适 应速度及样本数据精度。

S13：对室内定位空间中的所有无线接入点计算其信息增益，并根据信 息增益的大小对所述无线接入点进行排序，以信息增益从大到小的顺序筛 选出前N_AP个无线接入点，依次表示为：

S14：每个所述指纹网格点的WiFi信息表示为L_i＝{RSSI_i,o_i},i＝1,2,…,M， 其中，

表示在第i个指纹网格点上所采集的N_AP个无线 接入点信号强度的信息向量，

是第i个指纹网格点上测得第N_AP个无线 接入点的信号强度，o_i＝{x_i,y_i}表示第i个指纹网格点的位置信息，其中x_i和y_i分别是第i个指纹网格点在世界坐标系下的x轴和y轴坐标；

S15：通过所述指纹网格点的WiFi信息构建WiFi地图，通过所述指纹 网格点的图像信息构建视觉SLAM地图。

S2：对多个地标分别计算在所述WiFi地图和所述视觉SLAM地图中 的位置信息，并采用指纹地标法建立所述WiFi地图和所述视觉SLAM地 图的映射关系，使所述WiFi地图和所述视觉SLAM地图融合。

所述步骤S2具体为：

在所述指纹网格点中随机选择若干个作为地标，得到所述地标在世界 坐标系下的位置信息，通过视觉图像计算得到相应地标在机器人坐标系下 的位置信息，世界坐标系和机器人坐标系的位置转化表示如下：

其中，Loc和Loc′分别表示世界坐标系和机器人坐标系下的位置，R_T表 示平移矩阵。

将机器人坐标系与世界坐标系设定在同一原点，此时平移矩阵R_T为0， 从表达式可知两地图的映射关系取决于角度θ，选择任一已知的地标，将 其在世界坐标系和机器人坐标系中的位置分别代入代入表达式可以计算得 到角度θ。其中所述WiFi地图对应的是世界坐标系，所述视觉SLAM地图 对应的是机器人坐标系。

S3：移动机器人处于定位空间内的任一位置时，采用基于信息增益的KNN指纹定位算法利用所述WiFi地图进行WiFi定位；采用图像的特征匹 配算法利用所述视觉SLAM地图进行视觉SLAM定位。

在WiFi定位中，对所筛选的N_AP个无线接入点依据不同的信息增益分 别赋予不同的权重值，通过不同权重值的无线接入点来估算所述移动机器 人所处的位置。具体的，计算移动机器人所处的位置与指纹网格点的欧式 距离：

定义距离信息为

且满足d₁＜d₂＜…＜d_M，式 中

表示相应指纹网格点的位置信息，取前K个距离信息

采用基于信息增益的KNN指纹定位算法，将 前K个距离值进行权重值计算，得到相应权重值信息：

最终依据权重值估计移动机器人所处的位置：

在视觉SLAM定位中，先筛选步骤S1中采集到的图像信息得到关键 帧图像信息，然后采用ORB特征提取算法从而得到相似度值信息，同时为 提高特征点的匹配率，一方面加入图像预处理，采用小波变换对图像进行 降噪处理，另一方面在BRIEF算法中提出一种基于领域灰度信息的特征描 述子作为特征点匹配的评估标准，以特征点对为中心建立3*3的核，并对 这9个灰度值信息取平均值得到领域灰度信息，依据该信息得到新相似度 值信息：

N1表示视觉词袋中关键帧总数， S_i，i＝1,2,...N1表示当前移动机器人所处的位置的图像信息与关键帧图像信 息的相似度值，其中s₁＞s₂＞…＞s_N1。通过相似度来估计移动机器人所处的位 置。

S4：将WiFi定位与视觉SLAM定位中的视觉词袋技术融合实现粗定 位。

改进的KNN指纹定位算法，在定位准确率上有了显著提升，但是仍 存在一定的定位失败。同时人员流动会导致WiFi信号变化大，一天之内， 白天和黑夜也有一定的信号差异，这都将导致定位精度的降低。为进一步 提高粗定位的正确率，采用WiFi定位与视觉SLAM定位融合实现定位的 方式，其具体的步骤如下：

建立一个得分机制score(i)(j)＝score1(i)+score2(i)+score3(i)(j)，其中score1(i),i＝1,2,...,K为K个WiFi指纹距离得分；score2(j),j＝1,2,...,NK为NK个图像相似度得分；将K个WiFi指纹位置与NK个图像位置采用欧氏距离进行 两两组合，得到K*NK个组合位置距离

score3(i)(j),i＝1,2,...,K；j＝1,2,...,NK为K*NK个组合位置距离得分。该机制分为三 个部分，在WiFi定位中，距离越近表示当前估计位置与该指纹网格点位置 越近，因此前K个WiFi指纹距离得分score1(i),i＝1,2,...,K就越高；在视觉 SLAM定位中，两图像相似度越高，则当前图像信息的估计位置与该关键 帧图像信息位置越近，因此前NK个图像相似度得分score2(j),j＝1,2,...,NK就 越高。将K个WiFi指纹位置信息与NK个图像位置采用欧氏距离进行两两 组合，得到K*NK个位置距离

WiFi指纹 位置与关键帧位置之间距离越近，则移动机器人所在位置的概率越大，得 分score3(i)(j)越高；通过得分机制得到任意点与各个指纹网格点的得分值， 从中选出得分值最高所对应的指纹网格点的相关信息，作为定位的粗定位 位置，粗定位位置表示为：

和

分别表示从中选出得 分值最高所对应的WiFi指纹位置信息和图像位置信息。

S5：以粗定位的范围作为约束条件采用视觉SLAM最终实现精确定位。

通过WiFi与视觉SLAM图像数据融合得到融合定位位置o_m，以o_m为 圆心，以距离阈值d_t为半径，所形成的圆形区域为粗定位范围。以粗定位 范围为限制条件，进行视觉SLAM精定位，即采用自适应蒙特卡洛定位算 法，最终实现定位结果。

与相关技术相比，本发明中应用WiFi和视觉SLAM中的视觉词袋技 术实现粗定位，最后在粗定位的约束下采用视觉SLAM定位技术从而实现 最终定位结果。本发明有效的将WiFi和视觉SLAM进行融合，并在定位 准确率、定位精度、定位时间等性能上得到显著提升。

以上所述的仅是本发明的实施方式，在此应当指出，对于本领域的普 通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出改进， 但这些均属于本发明的保护范围。

Claims (8)

1.一种WiFi和视觉多源融合的移动机器人室内定位方法，其特征在于，包括如下步骤：

S1：采集室内定位空间中的WiFi信息和图像信息，进行预处理后，建立WiFi地图和视觉SLAM地图；

S2：对多个地标分别计算在所述WiFi地图和所述视觉SLAM地图中的位置信息，并采用指纹地标法建立所述WiFi地图和所述视觉SLAM地图的映射关系，使所述WiFi地图和所述视觉SLAM地图融合；

S3：移动机器人处于定位空间内的任一位置时，采用基于信息增益的KNN指纹定位算法利用所述WiFi地图进行WiFi定位；采用图像的特征匹配算法利用所述视觉SLAM地图进行视觉SLAM定位；

S4：将WiFi定位与视觉SLAM定位中的视觉词袋技术融合实现粗定位，具体包括如下步骤：

建立一个得分机制score(i)(j)＝score1(i)+score2(j)+score3(i)(j)，其中score1(i),i＝1,2,K,K为K个WiFi指纹距离得分；score2(j),j＝1,2,K,NK为NK个图像相似度得分；

将K个WiFi指纹位置与NK个图像位置采用欧氏距离进行两两组合，得到K*NK个组合位置距离

score3(i)(j),i＝1,2,...,K；j＝1,2,...,NK为K*NK个组合位置距离得分；

通过得分机制得到任意点与各个指纹网格点的得分值score(i)(j)，从中选出得分值最高所对应的指纹网格点的相关信息，作为定位的粗定位位置，粗定位位置表示为：

和

分别表示从中选出得分值最高所对应的WiFi指纹位置信息和图像位置信息；

S5：以粗定位的范围作为约束条件采用视觉SLAM定位实现精确定位。

2.根据权利要求1所述的WiFi和视觉多源融合的移动机器人室内定位方法，其特征在于，所述步骤S1包括如下步骤：

S11：在室内定位空间中布置M个指纹网格点，每个所述指纹网格点获取N个无线接入点所发出的WiFi信息，每个所述无线接入点采集n个样本数据；同时获取每个所述指纹网格点周围的图像信息；

S12：对所述样本数据采用格拉布斯准则进行异常值检测，若检测为异常值则剔除，反之则保留；

S13：对室内定位空间中的所有无线接入点计算其信息增益，并根据信息增益的大小对所述无线接入点进行排序，以信息增益从大到小的顺序筛选出前N_AP个无线接入点，依次表示为：

S14：每个所述指纹网格点的WiFi信息表示为L_i＝{RSSI_i,o_i},i＝1,2,K,M，其中，

表示在第i个指纹网格点上所采集的N_AP个无线接入点信号强度的信息向量，

是第i个指纹网格点上测得第N_AP个无线接入点的信号强度，o_i＝{x_i,y_i}表示第i个指纹网格点的位置信息，其中x_i和y_i分别是第i个指纹网格点在世界坐标系下的x轴和y轴坐标；

S15：通过所述指纹网格点的WiFi信息构建WiFi地图，通过所述指纹网格点的图像信息构建视觉SLAM地图。

3.根据权利要求2所述的WiFi和视觉多源融合的移动机器人室内定位方法，其特征在于，所述图像信息包括彩色图像信息和深度图像信息。

4.根据权利要求2所述的WiFi和视觉多源融合的移动机器人室内定位方法，其特征在于，所述步骤S12包括如下步骤：

S121：通过n个所述样本数据选定明显水平a，查找格拉布斯表获取临界值T(n,a)；

S122：计算样本数据的平均值X_avg和标准差σ，计算过程如下：

S123：对每个数据样本计算统计量T，计算过程如下：

S124：针对任意数据样本，当T≥T(n,a)时，则该数据为异常值需要被剔除，当T<T(n,a)时为正常数据进行保留。

5.根据权利要求2所述的WiFi和视觉多源融合的移动机器人室内定位方法，其特征在于，所述步骤S2具体为：

在所述指纹网格点中随机选择若干个作为地标，得到所述地标在世界坐标系下的位置信息，通过视觉图像计算得到相应地标在机器人坐标系下的位置信息，世界坐标系和机器人坐标系的位置转化表示如下：

其中，Loc和Loc′分别表示世界坐标系和机器人坐标系下的位置，R_T表示平移矩阵。

6.根据权利要求2所述的WiFi和视觉多源融合的移动机器人室内定位方法，其特征在于，所述步骤S3包括如下步骤：

在WiFi定位中，计算移动机器人所处的位置与指纹网格点的欧式距离：

定义距离信息为

且满足d₁<d₂<L<d_M，式中

表示相应指纹网格点的位置信息，取前K个距离信息

采用基于信息增益的KNN指纹定位算法，将前K个距离值进行权重值计算，得到权重值信息：

最终依据权重值估计移动机器人所处的位置：

7.根据权利要求6所述的WiFi和视觉多源融合的移动机器人室内定位方法，其特征在于，所述步骤S3包括如下步骤：

在视觉SLAM定位中，先筛选步骤S1中采集到的图像信息得到关键帧图像信息，然后采用ORB特征提取算法得到相似度值信息，以特征点对为中心建立3*3的核，并对这9个灰度值信息取平均值得到领域灰度信息，依据该信息得到新相似度值信息：

其中，N1表示视觉词袋中关键帧总数，S_i，i＝1,2,...N1表示当前移动机器人所处的位置的图像信息与关键帧图像信息的相似度值，s₁>s₂>L>s_N1，通过相似度来估计移动机器人所处的位置。

8.根据权利要求1所述的WiFi和视觉多源融合的移动机器人室内定位方法，其特征在于，所述步骤S5包括如下步骤：

通过WiFi定位与视觉SLAM定位融合得到的粗定位位置o_m，以o_m为圆心，以距离阈值d_t为半径，所形成的圆形区域为粗定位范围，以粗定位范围为限制条件，采用自适应蒙特卡洛定位算法，进行视觉SLAM精定位，实现最终定位结果。

Images