基于Decawave DWM1001的井下精确定位系统设计

2020-08-20孙泽明

山西电子技术 2020年4期

孙泽明

(山西省电子工业科学研究所有限公司，山西太原 030006)

0 引言

我国煤炭资源非常丰富，并且煤炭在我国能源结构中占有绝对主导地位。据统计，我国每年的煤炭发电量仍占所有发电量的65%以上。《中国可持续能源发展战略》报告指出，到21世纪中期，煤炭仍是我国主要使用的能源。由此看来，在相当长时间段内，煤炭资源的开发使用仍占有非常重要的地位。由于矿井中环境复杂，监管难度大，工作人员对煤矿生产中存在的安全隐患如瓦斯、透水等情况难以及时发现，从而经常导致事故发生；事故发生以后，又由于井下定位技术相对落后，被困在矿井下的人员很难进行精确定位，给事故救援工作造成很大的困难，因此实现井下人员实时精确定位显得非常重要，也越来越受到国家监管部门的高度重视。现有的煤矿人员定位系统只实现区域定位，对事故后的救援、安全生产提供的支持有限。UWB是一种超宽带技术的无线通信技术，其定位精度可达厘米级，可综合应用于煤矿井下人员、车辆的定位管理。

1 系统介绍

DWM1001是Decawave公司提供的专注于实时定位系统(RTLS)的一体化解决方案。该方案集成了DW1000 UWB收发器、nRF52832片上系统、一个3-axis加速度计和UWB天线和蓝牙天线。nRF52832片上系统除了包含蓝牙模块，还内嵌32位ARMCortex-M4F CPU，该CPU具有512kBFlash及64kRAM。DW1000UWB 收发器符合IEEE802.15.4-2011标准，3-axis加速度计可以获取运动状态下各设备的速度、位移数据，所以该方案就是一个完整的UWB实时定位模块，既可以作为定位标签(定位卡)，也可以作为定位分站，也可以作为网关实现与外网(internet、wifi)的连接，将数据传输到任何网络。

2 测距算法

2.1 UWB基本测距原理

生活中，为了获取一个物体在二维或三维空间的绝对位置，需要确定它与一些已知的物体之间的距离。UWB测距就是利用已知锚点(分站)的坐标，通过计算各个锚点与未知站点(人员或设备，定位卡)的距离而计算出未知站点的坐标。计算单个分站与定位卡的距离主要有几类算法:信号强度算法、时间算法、相位算法等，而基于时间的算法由于对设备要求相对简单，精度相对较高，成本更低，所以应用更广泛。

基于时间的算法主要有飞行时间(TOF)算法、到达时间差算法(TDOA)等，由于时间算法对分站之间、分站与定位卡之间的时间同步要求非常高，每1μs的差距在测距上就会产生30 cm的误差，所以大都采用双向测距(TWR:TWO WAY RANGING)，通过高速处理器，消耗相对多的信号传输次数和时间来回避设备之间由于时间不同步而产生的误差。

2.2 基于时间的测距原理

TOF(Time Of Flight飞行时间测距法)主要利用信号在两个设备(分站与定位卡)之间飞行时间来测量设备间的距离。双向飞行时间法(TW-TOF,two way-time of flight)每个模块从启动开始即会生成一条携带ID和时间戳的信息，设备A在S1时刻发射请求定位性质的脉冲信号，设备B在S2收到设备A的请求信号，并在S3时刻发射一个应答信号，被设备A在自己的S4时刻接收。由此可以计算出脉冲信号在两个设备之间的飞行时间，从而确定飞行距离L。

图1 TOF算法原理

距离：L=C*[(S2-S1)+(S4-S3)]/2(C为光速)[2]。

单独的TOF算法有一个非常严格的约束，即发送设备和接收设备必须时间上完全同步，每1μs的误差就会造成30cm的误差。这是一个目前很难解决的问题。为了去除时间同步问题，产生了一种新型算法就是Double-sided Two-way Ranging的算法巧妙地避开了这个问题，它既利用了TOF测距的优点，又去除了TOF的同步问题，从而为TOF的实用化提供了支持。

2.3 TWR双向测距原理(如图2)

图2 TWR测距原理

如图2所示，设备A即定位卡，设备B即定位分站，分站具有处理信息、计算能力，定位卡只发送数据而不计算。

●<分站>向<定位卡>发送第一次测距请求，发送信息包括设备ID和时间戳1

●<分站>收到<定位卡>请求定位信息后再向定位卡发送确认信息，发送信息包括设备ID和时间戳2

●第一次交互之间的时间差Tprop就是两者之间的传输时间

●<分站>收到<定位卡>第一次信息后到向<定位卡>发送确认信息是<分站>处理信息的时间，即Treply1，这在软件上是可以获取的

●同理，<定位卡>在收到<分站>的确认信息后再次向分站发送ID和时间戳

●从图2可以得出Tprop=(Tround1+Tround2-Treply1-Treply2)/2，即分站与定位卡之间信号的传输时间，由此可以算出两者之间的距离

●Treply1、Treply2、Tround1、Tround2只与自身时钟有关，而与其他设备无关，所以避开了设备之间时钟的误差。

2.4 系统设计

本系统主要对分站与定位卡之间的距离进行计算，不对多个分站之间协同计算定位卡的2维或者3维坐标进行计算，所以软件分为定位卡和分站两种。

2.4.1 硬件初始化(如图3)

图3 硬件初始化流程

2.4.2 定位卡软件流程

定位卡只负责搜索定位分站，搜索到后发送定位请求，收到定位分站的应答后再发送一次ID和时间戳即可，软件流程如图4。

图4 定位卡软件流程

2.4.3 定位分站软件流程

定位分站负责对收到的定位请求进行应答，在收到第二次请求后将两次的数据进行计算，发给上位机或者进行2维、3维位置计算的主定位分站，软件流程如图5。

图5 定位分站软件流程

3 实际应用

以上程序在室内进行试验时精度能达到30 cm以下，但是在井下进行实验时测距数据就很不稳定，数据精度比较差，分析原因是由于煤矿井下环境相对复杂，巷道内设备、金属构件比较多，UWB波的吸收较多，多径效应比较大，所以为了提高测量精度必须解决多径效应问题，而卡尔曼滤波算法是解决多径效应的不错的方法。

3.1 卡尔曼滤波介绍

卡尔曼滤波(Kalman filtering)是一种最优化自回归数据处理算法，不要求信号和噪声都是平稳过程,对于每个时刻的系统扰动和观测误差(即噪声)，只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均意义上求得误差为最小的真实信号的估计值。观测数据中包括系统扰动和外界噪声，所以这种估值算法也可看作是滤波过程。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，所以Kalman滤波是目前应用最为广泛的滤波方法，在通信和控制等很多领域得到了较好的应用。

3.2 建立卡尔曼滤波模型

现以井下人员做匀速直线运动且速度不超过1m/s为前提，以最小方差估计为算法进行建模。

Lm+1=Lm+Vmt

(1)

Lm=Lm-1+Vm-1t.

(2)

其中，Lm为m时刻井下人员移动，t为测量间隔，V为移动速度

在TWR系统模型中，假设井下人员作匀速直线移动，在时间小于1 s的前提下，井下人员当前的位置是跟前一时刻的位置相同的。由于井下人员的位置是标量时间函数，而且没有外部因素控制，所以采用标量差分方程表示系统状态：

状态方程：

L(m|m-1)=L(m-1|m-1)+λ(m).

(3)

观测方程：

d(m)=L(m)+ε(m).

(4)

过程噪声λk)可以认为主要是由井下人员具有一定速度引起的，观测噪声ε(m)主要是定位过程中由于晶振的频率漂移所引入的量化噪声，λ(m)和ε(m)都是均值为零的正态白噪声。在建立该状态方程和观测方程情况下，卡尔曼滤波基本能得到“最佳”估计。

Kalman滤波的状态更新方程为:

L(m|m-1)=L(m-1|m-1).

(5)

P(m|m-1)=P(m-1|m-1)+Q

(6)

Kalman滤波的测量更新方程为:

γ(m)=d(m)-L(m|m-1)

(7)

Kg(m)=P(m|m-1)/[P(m|m-1)+R]

(8)

L(m|m)=L(m|m-1)+Kg(m)*γ(m)

(9)

P(m|m)=[1-Kg(m)]*P(m|m-1).

(10)

从式(9)可以看出，m时刻的状态估计值是由m时刻的测量值和上一时刻的估计值计算得到的。

当TWR测量值存在较大误差时，后续的TWR最优估计值也受到较大影响，当TWR测量值存在较大偏差时，不仅使当前的TWR滤波值发生较大偏差，而且由于卡尔曼滤波器的记忆性，后续的TWR滤波值也会受到影响，造成滤波估计值与真实值发生偏离。对于这些偏差较大的TWR测量值，会使TWR估计值严重失真，故需要对卡尔曼滤波中出现的异常测量值进行修正。

因此本文引入阈值δ来判断当前TWR测量值误差是否偏大。将计算所得的信息γ(m)与阈值δ进行比较，如果信息值γ(m)>阈值δ，表明当前TWR测量值严重失真，不应该带入卡尔曼滤波进行迭代计算，并用状态预测值来代替TWR估计值。如果信息值γ(m)不大于阈值δ，则表明当前的测量值在误差容忍范围之内，可带入卡尔曼滤波迭代过程进行计算。针对该点，将卡尔曼滤波算法迭代过程中的式(8)修改为当γ(m)<δ时Kg(m)=P(m|m-1)/[P(m|m-1)+R]，当γ(m)>=δ时Kg(m)=0

阈值的获取必须通过在井下实际使用环境内通过实验获得。在实际应用中，通过大量的现场实验数据来设置不同的阈值，考察TWR估计值的误差来确定井下特殊环境中的阈值[1]。