一种基于多传感信息融合的定位算法研究

梁高丽 邓仕军 雷浩

摘  要：随着社会的不断发展，人们对室内定位精度的要求越来越高。针对单传感信息定位精度不高、抗干扰能力不强、算法复杂度较大等问题，提出一种融合多传感信息的定位算法。该算法通过对RFID标签径向速度与移动目标径向速度的匹配，快速识别并定位移动目标。已在服务机器人身上针对该算法展开实验测试，实验结果表明，该算法能够快速准确地定位到动态目标。

关键词：多传感信息；信息融合；径向速度匹配

中图分类号：TP301.6  文献标识码：A    文章编号：2096-4706（2023）08-0033-04

Abstract： With the continuous development of society， people require higher and higher accuracy of indoor positioning. Aiming at the problems of low positioning accuracy of single-sensor information， weak anti-interference ability， and large algorithm complexity， this paper proposes a positioning algorithm that integrates multi-sensor information. The algorithm matches the radial velocity of the RFID tag with the radial velocity of the moving target to quickly identify and locate the moving target. The algorithm has been tested on the service robot， and the experimental results show that the algorithm can locate the dynamic target quickly and accurately.

Keywords： multi-sensor information; information fusion; radial velocity matching

0  引  言

随着物联网技术的飞速发展，人们对定位服务的要求越来越高。目前，室外环境主要利用全球定位系统（GPS）来进行定位，该系统通过卫星信号来对移动物体进行定位，定位精度基本能满足人们的需求[1-3]。由于建筑物的遮挡，卫星信号被遮挡甚至阻断，导致GPS不再适用于室内环境。但是，人们长期处在室内环境当中，因此提升室内移动物体的定位精度尤为重要[4]。室内定位系统的精度不仅与传感器采集数据的精确度有关，还与数据处理的算法息息相关。使用单个传感器对移动物体进行定位，容易产生较大的定位误差，因此许多研究者融合多传感器信息来对移动物体进行定位。赵妍等人提出了一种基于同步定位与地图创建技术（SLAM）的智能激光定位技术。该技术采用最短路径优化算法对机器人进行路径规划；采用SLAM算法构建避障规则；利用激光传感技术来跟踪机器人的移动路径[5]。刘琴等人提出一种多传感器数据融合的定位系统，该系统融合GPS传感数据与惯性导航单元数据来定位动态目标[6]。王铭艺等人为了降低单一传感器的定位误差，提出了一种融合激光传感器与双目視觉的定位方法，该方法能精确地定位移动机器人的位置[7]。

为了进一步提高定位精度，许多研究开始对融合算法进行研究。刘金等人利用BP神经网络来对卡尔曼滤波算法进行优化，并利用优化后的算法来对多传感器信息进行融合，最后再对移动机器人进行定位[8]。毛文勇等人同样采用卡尔曼滤波算法融合激光信息与里程计信息，来对移动机器人进行定位，该算法有效提高了机器人的定位精度[9]。Sun等人提出了多源信息融合算法，该算法对扩展卡尔曼滤波器进行扩展，并且利用扩展后的滤波器来融合超宽带定位技术、里程计信息、陀螺加速计信息，实现了无轨送餐机器人的定位与导航[10]。Gu等人提出了基于多传感器融合的运动航向检测算法，首先利用系统本身的信息来判别物体的运动状态，然后利用扩展卡尔曼滤波算法，将加速度计的输出信息作为观测量，来更新测量值，进而得到物体运动方向[11]。

综上所述，多传感器具有互补特性，因此利用多传感器信息来进行定位能有效地提高定位精度，但算法的复杂度较高[12，13]。一个好的定位系统不仅需要提高定位精度，还需要降低算法复杂度。由于RFID能对物体进行识别，但定位物体需要复杂的算法；激光扫描仪能对物体的位置进行精确估算，但识别物体也许复杂的算法。为了提高传感器的定位精度，同时降低算法复杂度，本文提出一种融合RFID信息与激光信息的定位算法。

1  系统描述

该定位系统由RFID系统与二维激光扫描仪构成，系统框图如图1所示。RFID系统的读写器可以采集标签的相位信息以及标签的ID信息。信号强度信息可以对移动目标进行定位，但是该信息受环境影响较大，导致定位精度低；相位信息可以对移动目标的径向速度进行估算；标签的ID信息可以对移动目标进行识别。二维激光扫描仪可以采集物体的位置信息，但是需要复杂的算法才能对物体进行识别。因此该系统通过标签的ID信息对定位进行识别，并提取定位目标的相位信息，通过相位信息来估算标签的径向速度，然后找出与该径向速度最相似的激光簇的径向速度，最后获取该激光簇的中心坐标，即为定位目标的位置。

2  信息融合算法

2.1  激光聚类

二维激光扫描仪能向周围环境发射激光波束，可通过激光波束的发射时间与接收时间来估算物体与激光扫描仪之间的距离[14]。本文采用SICK公司，型号为S30B-2011BA的二维激光传感器。该传感器的扫描角度为270°；最远扫描距离为29.9 m；角度分辨率为0.5°。使用该扫描仪可向周围-45°至225°的范围内发射激光波束。当这些波束打在某物体上面，该物体的极坐标信息将被返回至激光传感器。因此通过该激光扫描仪采集的物体位置信息是以极坐标形式存在，如：（rn， φn），其中rn为第n束激光的长度信息；φn为第n束激光与激光正前方形成的夹角信息。该极坐标可通过下式转换为笛卡尔坐标形式：

其中，（xn， yn）为第n束激光形成的激光点，xn为该点距扫描仪垂直方向的距离；yn为该点距扫描仪水平方向的距离。通过转换之后，采集的物体信息变为二维坐标下许多离散的点。这些离散的点用pk（1，2，3，…，k）表示，两点之间的欧氏距离用d（ pn， pm）表示为：

由于打在同一物体上的激光不止一束，因此这些离散的激光点，不利于对物体进行区分。为了准确定位目标对象，本文将这些离散的激光点进行聚类处理，聚类之后的每一个激光簇表示一个物体。聚类的整个过程为：首先对原始数据进行分组，然后将组分割为簇，最后，合并过滤具有大半径的簇，即可得到聚类结果。

2.1.1  分组处理

通过欧式距离来对扫描点进行分组，如果相邻两点之间的欧式距离满足式（3），则分为一组：

d（ pi， pi-1）＜dg+Ridp                           （3）

其中，dg为分组阈值，Ri为第i个扫描点与激光扫描仪之间的距离，dp为距离参数。如果pi满足分组条件则被分进Sj当中，如果不满足分组条件，则创建一个新的分组Sj+1。当所有激光点都被分组之后，得到分组集合 ，Ng为分组数目。

2.1.2  分组处理

在分组处理当中，只关注了相邻两点之间的直线距离，不同物体可能被分为同一组。为了提高聚类结果的准确性，本文采用迭代拟合算法对每个分组再进行分割。首先在分组Sj中连接距离最远的两个扫描点，然后搜索离连接线最远的点，最后求得该点与连接线之间的距离 ?di。若 ?di满足分割标准，则在该点处对Sj进行分割。分割标准为：

?di＜ds+Rjdp                               （4）

其中，ds为分割阈值，Rj为连接线的长度。若 ?di满足分割条件，Sj在该点处被分割为两组。然后递归循环分割每一个新的分组，直到不能再分割为止。这时所得到的分组可以用圆来表示，该圆的圆心为连接线的中点，直径为连接线的长度。因此t时刻的聚类结果为：

其中  为t时刻第i个簇的半径， 为t时刻第i个簇的中心点，Nt为t时刻簇的个数。

2.1.3  合并过滤

分割结束后，若两个簇之间是包含或相交的关系，则认为这两个簇为同一个物体。因此需要对这两个簇进行合并，当圆  与圆  满足式（6），说明两圆相交，将这两圆合并为一个簇：

本文需要识别的动态目标为人体，因此可对半径较大的圆进行过滤。当某个圆的半径 （rm为物体半径阈值），将该圆过滤。合并过滤结束之后，原始扫描点已经被分成许多不同的簇，每一个簇表示一个物体。

2.1.4  激光传感信息估算径向速度

同一物体的径向移动速度，可通过相邻时刻簇的移动情况来进行估算。对于t时刻激光聚类完成之后得到的簇 ，可寻找在t-1时刻与其最相近的簇 ，这两个簇可被认为是同一物体。 为：

2.2  相位信息估算径向速度

RFID阅读器通过发射无线电波激活标签中的电路，当标签收获足够的能量后，反向散射无线电信号。标签的旋转相位为：

其中λ为波长，dt为RFID标签与天线之间的距离。φT为读写器传输电路的相位旋转；φR为阅读器接收回路的相位旋转；φTag为标签的反射特征的相位旋转。通过相邻时刻同一标签的相位差可以估算标签的径速度，假设读写器在t-1与t时刻获得的相位分别为θt-1与θt。标签的径向速度为：

2.3  速度匹配

同一物体通过不同传感器估算的径向速度应该极为相似。因此在t时刻寻找与相位差估算的速度  最接近的径向速度 ， 所对应的簇  的中心位置  可认为是定位目标的位置：

3  实验验证

本文将二维激光扫描仪与RFID系统安装在机器人身上，并将机器人固定在室內环境某一位置处，佩戴RFID标签的行人作为动态定位目标在机器人前方移动，移动轨迹为一个4×2 m的矩形，实验环如图2所示。

本定位算法当中涉及的参数有：分组参数dg，距离参数dp，分割参数ds，半径阈值rm，这些参数对激光聚类的结果存在一定影响，从而影响系统的定位精度。图3给出不同ds与dg的误差图，由表1可以看出，激光分组参数与分割参数，对定位误差存在较大的影响，当分组参数过大，同一组数据可能不止包含一个物体；当分组参数过小，同一物体可能被分为两组数据，这两种情况都会导致激光簇不准确，从而影响定位精度。当分割参数过小时，同一物体被分割为两个组；当分割参数过大时，不能将不同物体分割成两组，从而影响定位精度。当dg=0.2，ds=0.2，激光聚类结果较佳，定位精度较为理想。

表2给出了不同dp与rm对定位精度的影响，由表2可以看出，激光测距误差的修补参数dp过小时，激光测距误差不能得到较好的补偿，导致定位精度较低；当dp过大时，激光测距误差补偿过度，也会导致定位精度较差。最大半径rm过小时，将目标圆排除在外；rm过大时，不能很好地过滤掉无效圆，也会导致系统误差增加。当rm=0.4，dp=0.1时，系统定位效果较佳。

将该算法的各参数设置如下：dg=0.2、ds=0.2、rm=0.4、dp=0.1来对目标进行定位。图3给出了该算法的定位图，由图可知，定位点分布在目标移动的真实轨迹附近，少数定位点离真实轨迹较远，其原因是激光簇估算径向速度产生错误，导致系统定位到错误的移动物体，从而使定位点远离真实轨迹。当系统无法找到与标签径向速度较为接近的激光簇时，可根据移动目标前一时刻的位置，与当前时刻的位置来估计移动物体的移动路径，从而实现移动目标准确快速的定位。

4  结  论

本文提出了一种融合多传感信息的定位算法，其通过相位信息与二维激光扫描信息来对室内动态目标进行定位。实验表明，该系统不需要复杂的分类算法，能快速识别目标，并进行定位。但当激光簇的速度与标签径向速度相差较大时，系统获得的定位误差也较大，因此后期需要加入视觉信息或信号强度信息来降低系统定位误差。

参考文献：

[1] 张胜军，林若琳.浅谈室内定位技术现状 [J].测绘与空间地理信息，2018，41（7）：128-131.

[2] LIU X L，XIAO B，LI K Q，et al. RFID Estimation With Blocker Tags [J].IEEE/ACM Transactions on Networking，2017，25（1）：224-237.

[3] HE S N，CHAN S H G. Wi-Fi Fingerprint-Based Indoor Positioning：Recent Advances and Comparisons [J].IEEE Communications Surveys & Tutorials，2016，18（1）：466-490.

[4] YASSIN A，NASSER Y，AWAD M，et al. Recent Advances in Indoor Localization：A Survey on Theoretical Approaches and Applications [J].IEEE Communications Surveys & Tutorials，2017，19（2）：1327-1346.

[5] 趙妍，解迎刚，陈莉莉.基于SLAM算法的机器人智能激光定位技术的研究 [J].激光杂志，2019，40（7）：169-173.

[6] 刘琴，明振，李金波.多传感器数据融合定位方法及其安防机器人：CN109612462A [P].2019-04-12.

[7] 王铭艺，贺利乐，李育，等.多传感器信息融合的移动机器人定位算法研究 [J].测试科学与仪器：英文版，2020，13（4）：152-160.

[8] 刘金.基于传感器融合的室内定位技术研究 [D].北京：北京邮电大学，2019.

[9] 毛文勇，张文安，仇翔.基于多传感器融合的机器人轨迹跟踪控制 [J].控制工程，2020，27（7）：1125-1130.

[10] SUN Y L，GUAN L W，CHANG Z Y，et al. Design of a Low-Cost Indoor Navigation System for Food Delivery Robot Based on Multi-Sensor Information Fusion [J/OL].Sensors（Basel，Switzerland），2019，19（22）：[2022-10-28].https：//www.mdpi.com/1424-8220/19/22/4980.

[11] GU H Y，JIN C C，YUAN H Y，et al. Design and Implementation of Attitude and Heading Reference System with Extended Kalman Filter Based on MEMS Multi-Sensor Fusion [J].International Journal of Uncertainty，Fuzziness and Knowledge-Based Systems，2021，29（Supp01）：157-180.

[12] 吴佳怡，冯锋.基于多传感信息融合的仓储定位追踪系统研究 [J].计算机技术与发展，2019，29（6）：134-137.

[13] 周亚琼.一种基于多传感器信息融合定位技术的机器人：CN110370245A [P].2022-08-12.

[14] 王广奇.基于RFID和激光传感器的语义地图更新及机器人定位方法研究 [D].济南：山东大学，2013.

作者简介：梁高丽（1992—），女，汉族，四川成都人，助教，硕士，研究方向：机器人定位与导航；邓仕军（1989—），男，汉族，四川成都人，工程师，学士，研究方向：室内定位技术。