CN109946649B - 低成本室内狭长环境二维uwb系统定位方法 - Google Patents

Abstract

本发明公开了低成本室内狭长环境二维UWB系统定位方法，该方法首先利用两个最远的基站确定长轴，然后结合中点基站的信息解出标签的较准确的位置，之后以此为初值，使用牛顿迭代法解算出高精度的二维定位信息，最后使用Delphi、MySQL、Nodejs在Windows平台上搭建服务器进行显示。当测距误差扩大到数十厘米时，传统定位模型误差扩大到米级，而本定位方法表现出更强的稳健性，在宽轴的定位精度上提高了一到两个数量级。本发明在走廊、过道、矿道、隧道等狭长且信道环境恶劣的条件下相对于传统的基于到达时间测距定位算法都有较大优势，更低的成本在科研和工程中也有巨大的实际意义。

Description

低成本室内狭长环境二维UWB系统定位方法

技术领域

本发明公开了一种基于改进初值的牛顿迭代法的优化定位方法及其系统实现，属于室内定位方法及应用领域。

背景技术

室内实时定位系统(RTLS)可以实时跟踪封闭环境中的物体、车辆和人员，如生产线产品的跟踪、移动机器人的实时导航、步行者的定位等。

全球定位系统(GPS)已被广泛应用于室外环境中的有效定位，但由于信号的衰减和建筑物的阻隔，GPS不能用于室内环境。因此，有必要开发一种在室内环境中替代GPS的新技术。

近年来，随着无线传感器网络的飞速发展，无线定位技术也越来越受到人们的关注。传统的定位如红外线、蓝牙、超声波、RFID等技术，在定位精度、系统功耗等方面不能满足人们的需求。而超宽带(Ultra Wide Band，UWB)技术具有传输速率高、发射功率低、抗干扰和穿透能力强等特点，特别是UWB定位具有非常高的定位精度(厘米级)，因此对UWB定位技术旳研宄具有重要的应用价值。

目前较为常用的UWB测距方法主要有基于接收信号强度(received signalstrength，RSS)测量的方法、基于信号到达角度(angle of arrival，AOA)测量的方法、基于到达时间(time of arrival，TOA)测量的方法、基于到达时间差(time differenceofarrival，TDOA)测量的方法，其中TOA/TDOA方法都是基于到达时间的测量方法，是最可以体现UWB时间分辨率极高这一优势的方法，也是通常情况下测距精度最高的方法。

然而针对到达时间测距的定位方法目前研究的最多的只有两种，一个是三边测量法，另一个是双曲线法，这两种几何定位方法对环境的依赖性强，一旦测距精度下降，基站(anchor)位置对定位结果就有极大影响，而室内狭长环境容易扩大多径效应的影响，不止无线信号的衍射和散射，强反射能力的混泥土墙壁对测距结果影响最为明显，再加上可能的非视距(non line of sight，NLOS)传播，室内狭长环境的测距误差往往是数十厘米而不是数厘米，这种情况下传统到达时间定位模型必须以最接近正三角形的方式布置基站，才可以达到厘米级的精度要求，否则误差将扩大到米级，但在室内走廊的狭长环境中要想实现近三角形的布点方式对成本的要求是极高的。

本发明针对上述问题，提出一种改进的，基于到达时间测量方法的定位模型。首先使用最少的基站数确定未知标签(tag)的大概位置(粗定位)，然后以粗定位的值作为初值，使用牛顿迭代法进行迭代(精定位)，这种定位方法即使测距误差在几十厘米，二维定位的精度也可以达到厘米级，在测距结果较好时也可以取得和三边方法、双曲线法相似的定位精度，并且最后将此算法系统实现。

发明内容

基于时间测距的优化牛顿迭代法是一种利用最少的基站实现狭长环境下高精度室内二维定位的方法，该方法首先利用两个最远的基站确定长轴，然后结合中点基站的信息解出标签的较准确的位置，然后以此为初值，使用牛顿迭代法解算出高精度的二维定位信息，最后使用Delphi、MySQL、Nodejs在Windows平台上搭建服务器进行显示。本发明在走廊、过道、矿道、隧道等信道恶劣的狭长环境中定位，相对于传统的时间测距定位算法都有较大优势，更低的成本在科研和工程中也有巨大的实际意义。

为方便描述，把长轴设为X轴，短轴设为Y轴，二维时间测距定位的最低基站需求数是三个，分别为A1(x₁,y₁)，A2(x₂,y₂)，A3(x₃,y₃)，未知标签为T(x₀,y₀)，本方法根据现有的时间测距定位方法的缺陷，提出了一种新的基于优化的牛顿迭代法的定位方法，能够有效解决超宽带在狭长环境中宽轴定位结果极差的问题。

该优化定位方法通过下述流程实现：

(1)布置基站，数据接收，将两个基站A1、A3分别布置在X轴两端尽头的同一侧，A2布置在X轴中间Y轴的另一侧，如图1，其坐标分别为A1(x₁,y₁)，A2(x₂,y₂)，A3(x₃,y₃)，时间测距方法接收的标签到三个基站的距离分别为r₁、r₂、r₃。

(2)粗定位，确定位置标签的第一个估计位置，首先利用A1和A3求解x₀，相距远且Y轴坐标相同的两个基站在单独确定x₀上更有优势，因为减少了引入r₂带来的误差，在如图2的两种典型有测量误差的模型下都得到较好的X轴定位效果，可以作为牛顿迭代法的X轴初始估计值，初值公式如下：

在有x₀的估计位置后将其代入下式解算出y₀

解得

针对Y轴的初值估计可以分为如图3的三种情况，一种是x₀与以A2为圆心，r₂为半径的圆R2相交，且交点在走廊中的情况，一种是交点全不在走廊的情况，还有一种是不相交的情况，当y₀无解时，设y₀＝y₂，x₀落在圆R2与走廊的交点上。当y₀有解时，若有在走廊中的解，取走廊中的解。若无走廊中的解，取y₂另一侧走廊的值作为估计值。

以这种方法得到的T坐标相对传统的牛顿迭代法直接取初值是更精确的，即使测距结果的误差提高到数十厘米，也可以得到相对准确的定位结果，将其作为牛顿迭代法的初值进行进一步计算就可以得到更精确的估计位置。

(3)精定位时，使用牛顿迭代法对初值进行迭代，以此得到厘米级的定位精度。牛顿迭代法是以泰勒级数展开的经典公式为基础，取其线性部分进行迭代的方法，每次迭代都是一个更接近真实值的估计值，但初值选取不当，雅可比函数矩阵可能奇异或接近奇异矩阵，导致不收敛。为了推导牛顿迭代法，可以先构造函数

其中r_2,1＝r₂-r₁，r_3,1＝r₃-r₁，对(4)在F＝0上进行泰勒展开，只保留二阶以内的线性项则有

其中

J为函数F的雅克比矩阵，

F_(k)为函数在(x₀,y₀)上的值，

K为迭代次数，初始为0，整理可得适用于二维定位的牛顿迭代法公式

当式(5)的

小于一个阈值或不收敛时，停止迭代，得到最终的估计位置。

(4)硬件部分

定位模块由控制运算单元(单片机STM32)、通信单元(SIM808)、UWB单元(UK100)以及供电单元(电源)组成。

运算单元。STM32系列单片机拥有高性能、低成本、低功耗的特点，作为该系统中的运算控制单元十分适合，拥有丰富的对外接口，可以实现对各个单元的数据读取并处理运算以及实现数据的通信功能。

通信单元。SIM8008是四频模块，全球可用。含有TTL电平接口等接口，能够实现发短信、打电话、GPRS传输数据等功能。本系统中主要使用到的功能是利用GPRS传输数据与服务器进行通信。SIM808模块可以和单片机通过串口进行连接，单片机可以通过AT指令来对SIM808进行控制，实现对数据的获取、处理以及和服务器进行通信等功能。

室内无线定位单元。室内超宽带UWB采用的南京沃旭通讯科技公司的UK-100 UWB定位套件，使用爱尔兰DW1000芯片，在硬件级实现了双向测距，标签接收基站信息，通过串口将数据发送到单片机，单片机通过对数据的迭代计算，计算出相对坐标。

电源。供电单元采用3.7V 2000mah的锂电池，电量足，体积适中。

最终的定位模块连接图如图4。

(5)服务器搭建

服务器在一台带有显示器的计算机(Windows系统)上搭建，主要功能是接收定位模块发送的位置信息并进行室内轨迹的绘制显示，将数据存入数据库使得系统拥有查询历史的功能。

服务器监控程序。Delphi是著名的Borland(现在已和Inprise合并)公司开发的可视化软件开发工具。Delphi被称为第四代编程语言，它具有简单、高效、功能强大的特点。采用Delphi编写服务器应用程序，可以很方便地创建socket服务器与定位模块进行通信，可以通过ODBC驱动连接并管理MySQL数据库。

数据库。MySQL是一个关系型数据库管理系统，由瑞典MySQL AB公司开发，目前属于Oracle旗下产品。MySQL是最流行的关系型数据库管理系统之一，由于其体积小、速度快、总体拥有成本低，尤其是开放源码这一特点，一般中小型网站的开发都选择MySQL作为网站数据库。本系统数据关系较为简单，数据量较少，故采用MySQL作为数据库。

利用改进初值的牛顿迭代法可以很好解决在狭长环境的UWB定位中，传统几何定位模型解算后二维定位宽轴误差极大的问题，使狭长环境的高精度二维定位有了低成本的可靠方案，在实际工程应用中有较大价值。

附图说明

图1：三基站布置例图；

图2：确定X轴的两种典型有测量误差图；

图3：确定Y轴的三种典型图；

图4：定位模块连接图；

图5：基站部署图；

图6：壁挂式基站部署图：

图7：监控程序的室内界面图；

图8：定点定位效果图；

图9：连续定位效果图；

图10：本方法实施流程图。

具体实施方式

步骤1：定位准备，通信模块的组成

将SIM808与STM32通过串口1连接起来并配置天线，通过对STM32编写程序，发送AT指令，达到操作SIM808初始化，连接服务器发送定位信息的功能。

步骤2：UWB模块的组成

将UK-100标签与STM32通过串口2连接，通过对STM32编写程序达到接收UK-100数据、对数据进行滤波处理、利用改进初值的牛顿迭代法计算相对位置等功能。UWB定位需要室内环境基站的部署，室内环境为北工大科学楼11层楼道，部署基站如图5，实现方法为如图6的壁挂式，其坐标全通过激光测量已知，坐标点为(x,y)，X轴为楼道长度，Y轴为楼道宽度，单位为厘米。

步骤3：服务器监控程序的开发

实现通信功能。由于没有公网IP的原因，本系统采用了内网穿透的方式来实现与数据采集模块的通信。内网穿透由oray公司提供的花生壳软件完成。花生壳既是内网穿透软件、内网映射软件，也是端口映射软件，为全球1450多万用户提供动态域名解析服务，支持内网动态域名解析。可靠稳定是本系统采用花生壳软件的原因。

利用简单、高效、功能强大的Delphi来开发所需要的功能：利用ServerSocket控件来管理通信消息，利用Image控件来加载室内地图平面图片，利用TControlCanvas类实现在画布上绘制折线和圆点，另外还编写有清除轨迹、截图等功能。监控程序的界面如图7。

步骤4：数据库的设计

本系统涉及的数据关系比较简单，只包含一张表。通过字段isIndoor(0/1)来区分。

步骤5：开始定位，连接服务器

STM32通过串口1，使用AT指令让SIM808与服务器建立连接。

步骤6：粗定位

UK-100更新速率设置为2Hz，在STM32上处理接收到的测距信息r和对应的ID，根据接收的测距信息r分别给最近的三个基站分配固定角色，把距离最远的两个基站设为A1(x₁,y₁)和A3(x₃,y₃)，剩下的一个设为A2(x₂,y₂)，使用式(1)求解估计的x_0(k)值，之后将x_0(k)代入式(3)，求解估计的y_0(k)值，当y_0(k)无解，即

时，

当y_0(k)有解，且有在楼道中的解时，取其在楼道中的解。若无在楼道中的解，y_0(k)＝y₁或y_0(k)＝y₃以此得到第一次估计的初值T(x_0(k),y_0(k))，初始k＝0，设置收敛阈值ε。

步骤7：精定位，求解雅可比矩阵

利用式(4)构建几何数学模型F，求解F的雅可比矩阵

在(x_0(k),y_0(k))上的行列式，若|J|≠0，则对其求逆，计算出J^-1。若|J|＝0，进入步骤11。

步骤8：位置更新

计算F_(k)在(x_0(k),y_0(k))上的值，将已求得的J^-1，F_(k)，和本次估计的X_(k)，代入式(6)求解新的X_(k+1)，记录

的值，作为收敛与否的判断条件。

步骤9：检查收敛标准

计算范数，若

或

(不收敛)转到步骤11

步骤10：设k＝k+1，转到步骤7

步骤11：STM32通过串口1与SIM808通信，将定位点坐标(x_0(k),y_0(k))传送给服务器，完成后转到步骤6，计算新一轮位置。

下面通过真实实验对本发明设计的室内定位系统的定位效果进行验证。实验环境为北工大科学楼11层楼道，属于1.75米宽，64米长的狭长走廊，一共布置7个基站，其中每组三个定位基站的平均长轴距离为15米，将收敛阈值ε设为20，这里我们使用1-范数进行判断，在四个固定点处使用改进的牛顿迭代法与传统时间测距使用的直接算法，三边算法与激光测距的结果进行对比，采集8～10组真实数据，并加上1～2组有10～20厘米随机误差的模拟更恶劣环境数据，固定点的定位效果如图8，在走廊中间连续行走的服务器监控程序效果如图9。

由结果可以看出，改进初值的牛顿迭代法在定位精度上可以取得厘米级的定位精度，特别是测距误差较大时，在Y轴定位上可以取得巨大进步，当测距误差较小时，三边定位和牛顿迭代法可以取得相似的结果，但测距误差增大时，改进初值的牛顿迭代法稳健性更强，在三边定位算法误差达到米级时，牛顿迭代法依然可以保持厘米级的精度。但因为收敛性的问题，牛顿迭代法可以发现几个明显的异常值，实际运用中可以在剔除非视距误差时一同判断剔除。

Claims (2)

1.低成本室内狭长环境二维UWB系统定位方法，本方法把长轴设为X轴，短轴设为Y轴，取最低需求的三个基站，分别为A1(x₁,y₁)，A2(x₂,y₂)，A3(x₃,y₃)，未知标签为T(x₀,y₀)，定位时将两个基站A1、A3分别布置在X轴两端尽头的同一侧，A2布置在X轴中间Y轴的另一侧，时间测距方法接收的标签到三个基站的距离分别为r₁、r₂、r₃，最后在PC上实现可视化监控；

其特征在于：该方法需要经过如下流程，

(1)粗定位，确定位置标签的第一个估计位置，首先利用A1和A3求解x₀，在两种典型有测量误差的模型下都适用，作为牛顿迭代法的X轴初始估计值，初值公式如下：

在有x₀的估计位置后将其代入下式解算出y₀

解得

针对Y轴的初值估计能够分为三种情况，一种是x₀与以A2为圆心，r₂为半径的圆R2相交，且交点在走廊中的情况，一种是交点全不在走廊的情况，还有一种是不相交的情况；当y₀无解时，设y₀＝y₂，x₀落在圆R2与走廊的交点上；当y₀有解时，若有在走廊中的解，取走廊中的解；若无走廊中的解，取y₂另一侧走廊的值作为估计值；

(2)精定位，使用牛顿迭代法对初值进行迭代，得到厘米级的定位精度；为了推导牛顿迭代法，能够先构造函数

其中r_2,1＝r₂-r₁，r_3,1＝r₃-r₁，对式(4)在F＝0上进行泰勒展开，只保留二阶以内的线性项则有

其中

J为函数F的雅克比矩阵，

F_(k)为函数在(x₀,y₀)上的值，

k为迭代次数，初始为0，整理可得适用于二维定位的牛顿迭代法公式

当式(5)的

小于一个阈值，或不收敛时，停止迭代，得到最终的估计位置；

(3)硬件准备

定位模块由控制运算单元即单片机STM32、通信单元即SIM808、UWB单元即UK100以及供电单元即电源组成；

运算单元；STM32系列单片机能够实现对各个单元的数据读取，并处理运算以及实现数据的通信，选用STM32F103；

通信单元；SIM808模块能够和单片机通过串口进行连接，单片机能够通过AT指令来对SIM808进行控制，实现对数据的获取、处理以及和服务器进行通信功能；

室内无线定位单元；室内超宽带UWB采用的UK-100UWB定位套件，在硬件级实现双向测距，标签接收基站信息，通过串口将数据发送到单片机，单片机通过对数据的迭代计算，计算出相对坐标；

电源；供电单元采用3.7V 2000mah的锂电池；

(4)服务器搭建

服务器在一台带有显示器的计算机上搭建，服务器监控程序使用Delphi，采用Delphi编写服务器应用程序，创建socket服务器与定位模块进行通信，通过ODBC驱动连接并管理MySQL数据库；

采用MySQL作为数据库。

2.根据权利要求1所述的低成本室内狭长环境二维UWB系统定位方法，其特征在于：改进初值的牛顿迭代法的系统实现方式如下，

步骤1：定位准备，通信模块的组成

将SIM808与STM32通过串口1连接起来并配置天线，通过对STM32编写程序，发送AT指令，达到操作SIM808初始化，连接服务器发送定位信息的功能；

步骤2：UWB模块的组成

将UWB标签与STM32通过串口2连接，通过对STM32编写程序达到接收UWB数据、对数据进行滤波处理、利用改进初值的牛顿迭代法计算相对位置功能；UWB定位需要室内环境基站的部署，基站坐标全通过激光测量已知，坐标点为(x,y)，X轴为环境长度，Y轴为环境宽度；

步骤3：服务器监控程序的开发

实现通信功能；采用了内网穿透的方式来实现与数据采集模块的通信；利用ServerSocket控件来管理通信消息，利用Image控件来加载室内地图平面图片，利用TControlCanvas类实现在画布上绘制折线和圆点，另外还编写有清除轨迹、截图功能；

步骤4：数据库的设计

本系统涉及的数据关系只包含一张表；通过字段isIndoor(0/1)来区分；

步骤5：开始定位，连接服务器

STM32通过串口1，使用AT指令让SIM808与服务器建立连接；

步骤6：粗定位

UK-100更新速率设置为2Hz，在STM32上处理接收到的测距信息r和对应的ID，根据接收的测距信息r分别给最近的三个基站分配固定角色，把距离最远的两个基站设为A1(x₁,y₁)和A3(x₃,y₃)，剩下的一个设为A2(x₂,y₂)，使用式(1)求解估计的x_0(k)值，之后将x_0(k)代入式(3)，求解估计的y_0(k)值，当y_0(k)无解，即

时，

当y_0(k)有解，且有在楼道中的解时，取其在楼道中的解；若无在楼道中的解，y_0(k)＝y₁或y_0(k)＝y₃以此得到第一次估计的初值T(x_0(k),y_0(k))，初始k＝0，设置收敛阈值ε；

步骤7：精定位，求解雅可比矩阵

利用式(4)构建几何数学模型F，求解F的雅可比矩阵

在(x_0(k),y_0(k))上的行列式，若|J|≠0，则对其求逆，计算出J^-1；若|J|＝0，进入步骤11；

步骤8：位置更新

计算F_(k)在(x_0(k),y_0(k))上的值，将已求得的J^-1，F_(k)，和本次估计的X_(k)，代入式(6)求解新的X_(k+1)，记录

的值，作为收敛与否的判断条件；

步骤9：检查收敛标准

计算范数，若

或

，转到步骤11

步骤10：设k＝k+1，转到步骤7；

步骤11：STM32通过串口1与SIM808通信，将定位点坐标(x_0(k),y_0(k))传送给服务器，完成后转到步骤6，计算新一轮位置。

Images