CN110133597A - 一种基于超声波信标系统的融合定位方法及系统 - Google Patents

Abstract

本发明公开一种基于超声波信标系统的融合定位方法及系统。方法包括：获取固定信标和移动信标的原始坐标信息；根据固定信标和移动信标的原始坐标信息，得到固定信标和移动信标之间的原始距离信息；采用卡尔曼滤波方法对距离信息进行修正，得到修正距离信息；根据固定信标的原始坐标信息和修正距离信息，得到移动信标的真实坐标信息；将真实坐标信息通过调制解调器上传至客户端。本发明针对实际应用中由非视距传播引起的测量误差，采用卡尔曼滤波算法对测距进行优化，能够提高定位精度，同时通过采用最小二乘法将数据信息化，也能进一步提高定位精度。

Description

一种基于超声波信标系统的融合定位方法及系统

技术领域

本发明涉及无线传感网络领域，特别是涉及一种基于超声波信标系统的融合定位方法及系统。

背景技术

伴随移动通信、数据和多媒体等技术的高速发展，定位服务已然成为了一项拥有巨大市场的应用需求，尤其在复杂的室内环境，如餐馆、超市、停车场、仓库等，常常需要通过移动终端确定目标的当前位置信息。例如，在户外旅游时，移动终端定位系统可以为我们提供当前的位置信息，给出附近的景点信息，推荐附近的餐厅和酒店等服务；车载导航系统也被广泛应用于获取车辆的实时位置，提供交通信息，帮助驾驶员选择正确的道路，减轻交通负荷等应用场景。可见，定位服务系统正逐步应用于我们生活的各个方面，给我们的生活带来了更多的便利。根据应用范围的不同，定位服务可被分为户外定位和室内定位。

户外定位：指代的是已然实现了全球覆盖的无线GPS定位，主要用于车载导航系统、智能手机内置导航系统等。它的优势在于利用GPS卫星定位有效覆盖范围广，且定位和导航信号都是免费使用的。然而，在实际应用中，GPS卫星发射的无线电信号无法穿透大部分建筑，这导致GPS系统在室内定位场景中无法应用。

室内定位：目前市面上还没有统一的室内定位系统(Indoor PositioningSystem,IPS)标准。为了解决GPS失效区域的定位问题，有人提出使用距离传感器和光流技术与同步定位和映射(Simultaneous LocalizationAndMapping,SLAM)技术相结合。然而，SLAM和光流的性能取决于环境的优劣。例如，如果视觉传感器无法获得足够的特征来识别无人机的位置，就会导致较高的计算成本，并积累定位误差。另一种方法是基于运动捕捉的室内定位，它能够应用于复杂和高精度的导航，不需要任何卫星信号。然而，它的实现需要一个复杂和昂贵的运动捕捉系统。这之后，科学家开发了超宽带信标系统(Ultra-wideBandwidth,UWB)，提供高精度的定位。然而，一些实验表明，毫米级精度的超宽带信标定位系统，由于成本高和缺乏集成度，直接应用于移动机器人系统目前来看并不现实。

发明内容

本发明的目的是提供一种基于超声波信标系统的融合定位方法及系统，能够得到高精度的室内定位信息。

为实现上述目的，本发明提供了如下方案：

一种基于超声波信标系统的融合定位方法，所述方法基于一种超声波信标系统，所述超声波信标系统包括固定信标、移动信标和调制解调器，所述移动信标搭载在可移动机器人上，所述方法包括：

获取所述固定信标和所述移动信标的坐标信息；

根据所述固定信标和所述移动信标的原始坐标信息，得到所述固定信标和所述移动信标之间的原始距离信息；

采用卡尔曼滤波方法对所述原始距离信息进行修正，得到修正距离信息；

根据所述固定信标的原始坐标信息和所述修正距离信息，得到所述移动信标的真实坐标信息；

将所述真实坐标信息通过所述调制解调器上传至客户端。

可选的，所述根据所述固定信标和所述移动信标的原始坐标信息，得到所述固定信标和所述移动信标之间的原始距离信息，具体包括：

根据所述固定信标的原始坐标信息和所述移动信标的原始坐标信息通过飞行时间测距法，得到所述固定信标和所述移动信标之间的原始距离信息。

可选的，所述采用卡尔曼滤波方法对所述原始距离信息进行修正，得到修正距离信息，具体包括：

采用卡尔曼滤波方法滤除所述原始距离信息的非视距误差，得到修正距离信息。

可选的，所述根据所述固定信标的原始坐标信息和所述修正距离信息，得到所述移动信标的真实坐标信息，具体包括：

根据所述固定信标的原始坐标信息和所述修正距离信息采用最小二乘法，得到真实坐标信息。

一种基于超声波信标系统的融合定位系统，包括：

获取模块，用于获取固定信标和移动信标的原始坐标信息；

距离信息确定模块，用于根据所述固定信标和所述移动信标的原始坐标信息，得到所述固定信标和所述移动信标之间的原始距离信息；

卡尔曼滤波修正模块，用于采用卡尔曼滤波方法对所述原始距离信息进行修正，得到修正距离信息；

真实坐标信息确定模块，用于根据所述固定信标的原始坐标信息和所述修正距离信息，得到所述移动信标的真实坐标信息；

数据上传模块，用于将所述真实坐标信息通过所述调制解调器上传至客户端。

可选的，所述距离信息确定模块，具体包括：

距离信息确定单元，用于根据所述固定信标的原始坐标信息和所述移动信标的原始坐标信息通过飞行时间测距法，得到所述固定信标和所述移动信标之间的原始距离信息。

可选的，所述卡尔曼滤波修正模块，具体包括：

卡尔曼滤波修正单元，用于采用卡尔曼滤波方法滤除所述原始距离信息的非视距误差，得到修正距离信息。

可选的，所述真实坐标信息确定模块，具体包括：

真实坐标信息确定单元，用于根据所述固定信标的原始坐标信息和所述修正距离信息采用最小二乘法，得到真实坐标信息。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供一种基于超声波信标系统的融合定位方法，所述方法基于一种超声波信标系统，所述超声波信标系统包括固定信标、移动信标和调制解调器，所述移动信标搭载在可移动机器人上，方法包括：获取固定信标和移动信标的原始坐标信息；根据固定信标的坐标信息和移动信标的原始坐标信息，得到固定信标和移动信标之间的原始距离信息；采用卡尔曼滤波方法对原始距离信息进行修正，得到修正距离信息；根据固定信标的原始坐标信息和修正距离信息，得到移动信标的真实坐标信息；将真实坐标信息通过调制解调器上传至客户端。本发明针对实际应用中由非视距传播引起的测量误差，采用卡尔曼滤波算法对测距进行优化，能够提高定位精度，同时通过采用最小二乘法将数据信息化，也能进一步提高定位精度。

附图说明

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

图1为本发明基于超声波信标系统的融合定位方法流程图；

图2为本发明基于超声波信标系统的融合定位系统结构图。

具体实施方式

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

本发明的目的是提供一种基于超声波信标系统的融合定位方法及系统，能够得到高精度的定位信息。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

针对目前室内定位系统存在的各种问题，超声波信标系统(Ultrasonic BeaconSystem,UBS)可以作为使用低成本硬件的实际测绘和定位系统的替代方案。UBS拥有与UWB类似的概念和机制，但它比UWB更便宜，更容易集成到移动机器人中。因此，选择UBS作为移动机器人的定位传感器和地图生成装置。UBS的硬件组成包含固定信标、移动信标与调制解调器三种器件。其中，固定信标辅助确定机器人的工作空间与生成虚拟地图，移动信标搭载在机器人本体上负责室内定位，调制解调器用以初始化系统和负责信标与上位机之间的通信。信标之间的距离信息通过飞行时间测距法(Time ofFlight,TOF)得到。这些距离信息经卡尔曼滤波(KalmanFilter)后进行融合定位，最终可以得到高精度的机器人室内静态与动态位置信息。

实施例1：

图1为本发明基于超声波信标系统的融合定位方法流程图。如图1所示，一种基于超声波信标系统的融合定位方法，所述方法基于一种超声波信标系统，所述超声波信标系统包括固定信标、移动信标和调制解调器，所述移动信标搭载在可移动机器人上，所述方法包括：

步骤101：获取固定信标和移动信标的原始坐标信息；

步骤102：根据固定信标和移动信标的原始坐标信息，得到固定信标和移动信标之间的原始距离信息，具体包括：

根据固定信标的原始坐标信息和移动信标的原始坐标信息通过飞行时间测距法，得到固定信标和移动信标之间的原始距离信息。

步骤103：采用卡尔曼滤波方法对原始距离信息进行修正，得到修正距离信息，具体包括：

采用卡尔曼滤波法滤除原始距离信息的非视距误差，得到修正距离信息。

步骤104：根据固定信标的原始坐标信息和修正距离信息，得到移动信标的真实坐标信息，具体包括：

根据固定信标的原始坐标信息和修正距离信息采用最小二乘法，得到真实坐标信息。

在根据已知的固定信标的位置和修正距离信息解算出移动信标的坐标信息时，首先根据几何关系可以得到以下方程组：

(x-x_i)²+(y-y_i)²+(z-z_i)²＝r_i ²

其中，(x_i,y_i,z_i)为固定信标i的坐标，r_i为经过卡尔曼滤波KF优化后移动信标到固定信标i的修正距离。

用第i个式子减去第j个式子得：

2x(x_i-x_j)+2y(y_i-y_j)+2z(z_i-z_j)

＝r_i ²-r_j ²+x_j ²-x_i ²+y_j ²-y_i ²+z_j ²-z_i ²

从而有：

其中，令：

X＝[x y z]^T

可以得到：

AX＝b

使用最小二乘法就能得到它的解的表达式为：

这样就解算出了由UBS使用TOF得到的原始距离和坐标信息经KF算法融合后的修正距离与真实坐标信息。至此，UBS得到了所需的全部数据，接下来可以与机器人进行双工通信，进而实现室内定位，甚至可以进行路径规划与自动导航。

步骤105：将真实坐标信息通过调制解调器上传至客户端。

本发明针对实际应用中由非视距传播引起的测量误差，采用卡尔曼滤波算法对测距进行优化，能够提高定位精度，同时通过采用最小二乘法将数据信息化，也能进一步提高定位精度。

实施例2：

本发明基于超声波信标系统(Ultrasonic Beacon System,UBS)与Turtlebot移动机器人平台完成。其中，移动机器人负责搭载UBS组成部分之一的移动信标运动，作为追踪对象；UBS负责建立地图和室内定位，其采集到的距离信息与位置信息可以通过上位机软件Dashboard呈现，并进行人机交互。随后，UBS提供的原始距离信息将通过卡尔曼滤波(Kalman filter,KF)算法进行融合定位，从而得到比较精确的位置信息，实现精确室内定位。

超声波信标系统UBS的硬件组成包含固定信标、移动信标与调制解调器三种器件。其中，固定信标辅助确定机器人的工作空间与生成虚拟地图，移动信标搭载在机器人本体上负责室内定位，调制解调器用以初始化系统和负责信标与上位机之间的通信，固定信标与移动信标的外观一致，且可以通过上位机Dashboard对其功能进行选择选择、配置。在实际工作中，为实现系统的大部分功能，推荐的基本硬件配置为四个固定信标、一个移动信标与一个调制解调器。在实际工作中，固定信标可竖直安装在墙面上，天线垂直于墙面，四个固定信标即可围成一个立方体的工作空间；移动信标安装在移动机器人平台上，天线竖直向上，追踪移动机器人的实际位置。

分别获取各个固定信标与移动信标之间的直线距离信息。这些距离信息是通过飞行时间测距法(Time ofFlight,TOF)得到的。

得到飞行时间测距法TOF采集的原始距离信息后，可以通过融合定位的方式得到更精确的距离信息和位置信息。

针对超声波信标、UWB等室内定位方案中最经常遇到的非视距传播(Not LineofSight，NLOS)问题，为提高定位精度，需要采取一些方法对定位误差进行修正。本发明采用卡尔曼滤波(KalmanFilter，KF)算法对系统使用TOF采集到的原始距离信息进行优化，对NLOS误差值进行迭代处理，以此来减小定位误差。首先利用状态向量方程对原始距离信号进行滤波，估计出NLOS误差值N_i(t)，然后从最初的测距值d_i(t)中除去非视距误差，得到精确的距离值。具体执行步骤如下：

现假设t时刻移动信标到固定信标的实际距离为r_i(t)，观测时产生的非视距误差设为N_i(t)，则可用下式表示它们之间的关系：

d_i(t)＝r_i(t)+N_i(t)

式中d_i(t)为使用TOF得到的测量值。接下来，考虑把非视距误差从测量值中分离出来，加入到待估计的状态变量中，直接估计出非视距误差。然后用d_i(t)减去非视距误差的估计值，从而得到实际距离r_i(t)。

系统的状态方程和观测方程如下：

式中，Δt为采样间隔；ω_d(t)和ω_N(t)分别为测量过程中的噪声误差分量；β为实验参数；v_i(t)为观测误差。可以看到在状态方程中，NLOS误差分量被加入到状态变量中作为状态变量的第三个分量，其目的就是为了得到对NLOS误差的估计。所以，只需再给出t时刻的状态向量和估计误差的协方差初始值，就可以通过迭代运算对不同时刻的状态向量做出估计。需要注意的是由于非视距误差具有非负性，因此在迭代过程中如N_i(t)出现负值就强制置零。

这一步之后，将观测值d_i(t)减去估计出的N_i(t)，即可得到修正距离r_i(t)。

下一步的目标是根据已知的固定信标的位置和修正距离r_i解算出移动信标的坐标信息。根据几何关系可以得到以下方程组：

(x-x_i)²+(y-y_i)²+(z-z_i)²＝r_i ²

其中(x_i,y_i,z_i)为固定信标i的坐标，r_i为经过KF优化后移动信标到固定信标i的距离。

用第i个式子减去第j个式子得：

2x(x_i-x_j)+2y(y_i-y_j)+2z(z_i-z_j)

＝r_i ²-r_j ²+x_j ²-x_i ²+y_j ²-y_i ²+z_j ²-z_i ²

从而有：

其中，令：

X＝[x y z]^T

可以得到：

AX＝b

使用最小二乘法就能得到它的解的表达式为：

这样就解算出了由UBS使用TOF得到的原始距离和坐标信息经KF算法融合后的修正距离与真实坐标信息。至此，UBS得到了所需的全部数据，接下来可以与机器人进行双工通信，进而实现室内定位，甚至可以进行路径规划与自动导航。

实施例3：

图2为本发明基于超声波信标系统的融合定位系统结构图。如图2所示，一种基于超声波信标系统的融合定位系统，包括：

获取模块201，用于获取固定信标和移动信标的坐标信息；

距离信息确定模块202，用于根据所述固定信标和所述移动信标的原始坐标信息，得到所述固定信标和所述移动信标之间的原始距离信息；

卡尔曼滤波修正模块203，用于采用卡尔曼滤波方法对所述原始距离信息进行修正，得到修正距离信息；

真实坐标信息确定模块204，用于根据所述固定信标的原始坐标信息和所述修正距离信息，得到所述移动信标的真实坐标信息；

数据上传模块205，用于将所述真实坐标信息通过所述调制解调器上传至客户端。

所述距离信息确定模块202，具体包括：

距离信息确定单元，用于根据所述固定信标的原始坐标信息和所述移动信标的原始坐标信息通过飞行时间测距法，得到所述固定信标和所述移动信标之间的原始距离信息。

所述卡尔曼滤波修正模块203，具体包括：

卡尔曼滤波修正单元，用于采用卡尔曼滤波方法滤除所述原始距离信息的非视距误差，得到修正距离信息。

所述真实坐标信息确定模块204，具体包括：

真实坐标信息确定单元，用于根据所述固定信标的原始坐标信息和所述修正距离信息采用最小二乘法，得到真实坐标信息。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种基于超声波信标系统的融合定位方法，所述方法基于一种超声波信标系统，所述超声波信标系统包括固定信标、移动信标和调制解调器，所述移动信标搭载在可移动机器人上，其特征在于包括：

获取所述固定信标和所述移动信标的原始坐标信息；

根据所述固定信标和所述移动信标的原始坐标信息，得到所述固定信标和所述移动信标之间的原始距离信息；

采用卡尔曼滤波方法对所述原始距离信息进行修正，得到修正距离信息；

根据所述固定信标的原始坐标信息和所述修正距离信息，得到所述移动信标的真实坐标信息；

将所述真实坐标信息通过所述调制解调器上传至客户端。

2.根据权利要求1所述的基于超声波信标系统的融合定位方法，其特征在于，所述根据所述固定信标和所述移动信标的原始坐标信息，得到所述固定信标和所述移动信标之间的原始距离信息，具体包括：

根据所述固定信标的原始坐标信息和所述移动信标的原始坐标信息通过飞行时间测距法，得到所述固定信标和所述移动信标之间的原始距离信息。

3.根据权利要求1所述的基于超声波信标系统的融合定位方法，其特征在于，所述采用卡尔曼滤波方法对所述原始距离信息进行修正，得到修正距离信息，具体包括：

采用卡尔曼滤波方法滤除所述原始距离信息的非视距误差，得到修正距离信息。

4.根据权利要求1所述的基于超声波信标系统的融合定位方法，其特征在于，所述根据所述固定信标的原始坐标信息和所述修正距离信息，得到所述移动信标的真实坐标信息，具体包括：

根据所述固定信标的原始坐标信息和所述修正距离信息采用最小二乘法，得到真实坐标信息。

5.一种基于超声波信标系统的融合定位系统，其特征在于，包括：

获取模块，用于获取固定信标和移动信标的原始坐标信息；

距离信息确定模块，用于根据所述固定信标和所述移动信标的原始坐标信息，得到所述固定信标和所述移动信标之间的原始距离信息；

卡尔曼滤波修正模块，用于采用卡尔曼滤波方法对所述距离信息进行修正，得到修正距离信息；

真实坐标信息确定模块，用于根据所述固定信标的原始坐标信息和所述修正距离信息，得到所述移动信标的真实坐标信息；

数据上传模块，用于将所述真实坐标信息通过所述调制解调器上传至客户端。

6.根据权利要求5所述的基于超声波信标系统的融合定位系统，其特征在于，所述距离信息确定模块，具体包括：

距离信息确定单元，用于根据所述固定信标的原始坐标信息和所述移动信标的原始坐标信息通过飞行时间测距法，得到所述固定信标和所述移动信标之间的原始距离信息。

7.根据权利要求5所述的基于超声波信标系统的融合定位系统，其特征在于，所述卡尔曼滤波修正模块，具体包括：

卡尔曼滤波修正单元，用于采用卡尔曼滤波方法滤除所述原始距离信息的非视距误差，得到修正距离信息。

8.根据权利要求5所述的基于超声波信标系统的融合定位系统，其特征在于，所述真实坐标信息确定模块，具体包括：

真实坐标信息确定单元，用于根据所述固定信标的原始坐标信息和所述修正距离信息采用最小二乘法，得到真实坐标信息。