CN106959434A - 一种基于uwb的矿井无人机车高精度定位系统及方法 - Google Patents

Abstract

本发明公开了一种基于UWB的矿井无人机车高精度定位系统及方法，包括若干UWB标签节点Tag、UWB锚节点Anchor，井下通信AP设备及信标器四部分通过无线或者有线信号进行连接；本发明并不是直接定位机车位置，而是将定位分成三步，第一步测距，在测距方面节点采用的是窄脉冲信号传输具有很强抗干扰的特性，避免了井下灰尘过多及磁场紊乱等原因而造成的定位偏差，第二步是计算在进路里的坐标，在第一步测的多点间精确距离的基础上利用最小二乘算法求解节点的坐标，第三步监控中心根据进路中信标器的位置与机车坐标的相对位置，从而最终确定定位机车在矿井的位置，最高定位精度可达0.1m。

Description

一种基于UWB的矿井无人机车高精度定位系统及方法

技术领域

本发明涉及矿井无人机车定位系统领域，具体是一种基于UWB的矿井无人机车高精度定位系统及方法。

背景技术

井下大巷运输的主要方式是采用矿井轨道机车运输，现阶段，我国矿井机车均为人工驾驶，但矿井下复杂多变的生产环境给井下作业人员的生命安全造成了极大的威胁，井下作业的智能化和无人化成为今后矿业作业的必然趋势，矿井机车无人驾驶系统为井下机车为解决上述问题提供了一种有效的解决方案。

但在矿井机车无人驾驶系统的安全可靠运行需要准确获取井下的各类信息，其中井下机车实时位置的获取对机车的运行调度及安全驾驶具有非常重要的意义，但由于井下存在灰尘较多、磁场不稳定等原因，使得目前的定位方法如基于RFID的定位方法和基于Zigbee的定位方法等误差较大，其定位精度都至少在1米以上完全不符合矿井无人驾驶系统的安全运行需求严重影响了机车的精确调度和可靠运行。

发明内容

本发明的目的是提供一种基于UWB的矿井无人机车高精度定位系统及方法，以解决现有技术定位方法信道传输不稳定、定位不精确的问题。

为了达到上述目的，本发明所采用的技术方案为：

一种基于UWB的矿井无人机车高精度定位系统，其特征在于：包括：

若干UWB标签节点Tag，分别设置于矿井中作为UWB信号发送器，各个UWB标签节点Tag的坐标已知并分配有相应的ID号；

UWB锚节点Anchor，其设置在矿井中机车上作为UWB信号接收器，所述UWB锚节点Anchor实时接收各个UWB标签节点Tag发送的脉冲信号，并记录脉冲信号的到达时间以及脉冲信号对应的UWB标签节点Tag的坐标数据；

AP设备，其设置在矿井中，所述UWB标签节点Tag、UWB锚节点Anchor分别与AP设备无线通信连接；

每个进路至少一个信号机，所述信号机就是井下的路红灯，只有绿灯时机车才能驶入该条进路，信号机与井下交换机直接相连，信标器与信号机成对存在，所述信标器位置已知直接与井下交换机连接，可将其位置信息直接通过井下光纤环网传输至地面监控中心。

所述的一种基于UWB的矿井无人机车高精度定位系统，其特征在于：UWB标签节点Tag、UWB锚节点Anchor均由主处理器以及连接在主处理器上的UWB模块、调试接口、LCD显示器及各类接口组成。

所述的一种基于UWB的矿井无人机车高精度定位系统，其特征在于：UWB标签节点Tag等间距布设在矿井大巷上壁，相邻UWB标签节点Tag的间距小于其通信距离的三分之一。

所述的一种基于UWB的矿井无人机车高精度定位系统，其特征在于：UWB锚节点Anchor布设在矿井中机车顶部。

一种基于UWB的矿井无人机车高精度定位方法，其特征在于：包括以下步骤：

(1)、将所有的UWB标签节点Tag和UWB锚节点Anchor维持一个相同的高精度同步时钟；

(2)、使各个UWB标签节点Tag定期向UWB锚节点Anchor发送UWB脉冲信号及自身坐标；

(3)、UWB锚节点Anchor将接收到的数据提供给自身主处理器，主处理器通过TOA测距算法测得各个UWB标签节点Tag至锚UWB锚节点Anchor的距离；

(4)、根据已知的各个UWB标签节点Tag的坐标及与UWB锚节点Anchor的距离，使用最小二乘法计算给出UWB锚节点Anchor的坐标；

(5)、UWB锚节点Anchor通过无线方式将数据传输到AP设备，AP设备再将数据经过井下光纤环网传输到地面监控中心，地面面监控中心结合矿井机车所在信标器位置给出机车井下的最终位置。

所述的一种基于UWB的矿井无人机车高精度定位方法，其特征在于：所述步骤(3)的具体过程为：

(3.1)、UWB锚节点Anchor收集到的信息为UWB标签节点Tag_i传输到的时间t_i及其坐标(x_i,y_i)，其中i代表标签节点Tag在井下的编号，由于各个UWB标签节点Tag具有等间距分布且间距小于其通信距离的三分之一的分布特点，UWB锚节点Anchor接收到的UWB标签节点Tag坐标一般为3个；

(3.2)、通过TOA算法，测得UWB标签节点Tag_i到UWB锚节点Anchor的传输时间Δt_i，然后用传输时间Δt_i乘以光速c得到UWB标签节点Tag_i到UWB锚节点Anchor的距离d_i。

所述的一种基于UWB的矿井无人机车高精度定位方法，其特征在于：所述步骤(4)的具体过程为：

(4.1)、根据上述步骤(3.2)中的距离及坐标数据，列出方程组(1)；

(4.2)、将方程组(1)的前(n-1)个方程分别减去第n个方程，从而达到二次方程线性化的目的，得到下列方程组(2)：

(4.3)、定义：

将方程组(2)改写成θ＝Q^-1b，用最小二乘法LS求解可得θ＝(Q^TQ)^-1Q^Tb。

本发明是一种基于UWB的矿井无人机车高精度定位方法，采用UWB技术，节点之间通过记录窄脉冲的到达时间来计算节点距离并结合最小二乘算法计算出机车最终的坐标位置。

与已有技术相比，本发明的有益效果体现在：

本发明的UWB定位方法并不是直接定位机车位置，而是将定位分成三步，第一步测距，在测距方面节点采用的是窄脉冲信号传输具有很强抗干扰的特性，避免了井下灰尘过多及磁场紊乱等原因而造成的定位偏差，第二步是计算在进路里的坐标，在第一步测的多点间精确距离的基础上利用最小二乘算法求解节点的坐标，第三步监控中心根据进路中信标器的位置与机车坐标的相对位置，从而最终确定定位机车在矿井的位置，最高定位精度可达0.1m。

附图说明

图1基于UWB的无人机车定位系统结构框图。

图2井下UWB的无人机车定位方法示意图。

图3 UWB标签节点tag与锚节点Anchor的结构框图。

图4 UWB测距算法示意图。

具体实施方式

如图1所示，一种基于UWB的矿井无人机车高精度定位系统，包括：

若干UWB标签节点Tag，分别设置于矿井中作为UWB信号发送器，各个UWB标签节点Tag的坐标已知并分配有相应的ID号；

UWB锚节点Anchor，其设置在矿井中机车上作为UWB信号接收器，所述UWB锚节点Anchor实时接收各个UWB标签节点Tag发送的脉冲信号，并记录脉冲信号的到达时间以及脉冲信号对应的UWB标签节点Tag的坐标数据；

AP设备，其设置在矿井中，所述UWB标签节点Tag、UWB锚节点Anchor分别与AP设备无线通信连接；

每个进路至少一个信号机，所述信号机主要是显示当前进路是否开放允许级车进入，信号机与井下交换机直接相连，信标器与信号机成对存在，所述信标器位置已知直接与井下交换机连接，可将其位置信息直接通过井下光纤环网传输至地面监控中心。

UWB标签节点Tag、UWB锚节点Anchor均由主处理器以及连接在主处理器上的UWB模块、调试接口、LCD显示器及各类接口组成。

UWB标签节点Tag等间距布设在矿井大巷上壁，相邻UWB标签节点Tag的间距小于其通信距离的三分之一。

UWB锚节点Anchor布设在矿井中机车顶部。

一种基于UWB的矿井无人机车高精度定位方法，包括以下步骤：

(1)、将所有的UWB标签节点Tag和UWB锚节点Anchor维持一个相同的高精度同步时钟；

(2)、使各个UWB标签节点Tag定期向UWB锚节点Anchor发送UWB脉冲信号及自身坐标；

(3)、UWB锚节点Anchor将接收到的数据提供给自身主处理器，主处理器通过TOA测距算法测得各个UWB标签节点Tag至锚UWB锚节点Anchor的距离；

(4)、根据已知的各个UWB标签节点Tag的坐标及与UWB锚节点Anchor的距离，使用最小二乘法计算给出UWB锚节点Anchor的坐标；

(5)、UWB锚节点Anchor通过无线方式将数据传输到AP设备，AP设备再将数据经过井下光纤环网传输到地面监控中心，地面面监控中心结合矿井机车所在信标器位置给出机车井下的最终位置。

步骤(3)的具体过程为：

(3.1)、UWB锚节点Anchor收集到的信息为UWB标签节点Tag_i传输到的时间t_i及其坐标(x_i,y_i)，其中i代表标签节点Tag在井下的编号，由于各个UWB标签节点Tag具有等间距分布且间距小于其通信距离的三分之一的分布特点，UWB锚节点Anchor接收到的UWB标签节点Tag坐标一般为3个；

(3.2)、通过TOA算法，测得UWB标签节点Tag_i到UWB锚节点Anchor的传输时间Δt_i，然后用传输时间Δt_i乘以光速c得到UWB标签节点Tag_i到UWB锚节点Anchor的距离d_i。

步骤(4)的具体过程为：

(4.1)、根据上述步骤(3.2)中的距离及坐标数据，列出方程组(1)；

(4.2)、将方程组(1)的前(n-1)个方程分别减去第n个方程，从而达到二次方程线性化的目的，得到下列方程组(2)：

(4.3)、定义：

将方程组(2)改写成θ＝Q^-1b，用最小二乘法LS求解可得θ＝(Q^TQ)^-1Q^Tb。UWB测距方法的距离节点如图2所示具备如下装置：若干标签节点Tag、锚节点Anchor、通信AP、信标器等及部分组成。

本发明中，各类装置如图2所示进行布设，UWB标签节点Tag均匀布设在井下大巷顶部并对其一一编号，UWB标签节点Tag的最大通信距离为80米，每个UWB标签节点Tag相隔20米，UWB锚节点Anchor布置在机车顶部。

如图3所示，UWB标签节点Tag、UWB锚节点Anchor都是由DW1000 UWB模块、RAM/STM处理器、调试接口、LCD显示器及各类接口组成。

所有UWB标签节点Tag及UWB锚节点Anchor都维持一个共同的同步时钟，每隔一个时钟周期T，标签节点Tag发射一次脉冲信号。

如图4所示：根据UWB标签节点Tag的布设特点，UWB锚节点Anchor隔一个周期都能收到三个UWB标签节点Tag发来的占脉冲信号，并记录其到达时间分别为t₁，t₂，t₃及相应坐标如(20,15)、(30,15)、(40,15)。

UWB锚节点Anchor通过UWB标签节点Tag的发送时间t₀计算节点传输的时间差Δt₁，Δt₂，Δt₃，并乘以光速获得UWB锚节点Anchor与Tag1，Tag2，Tag3节点的距离分别为25米、15米、25米。

将上述UWB锚节坐标及距离数据带入得到方程组(1)：

将方程组(1)的前(n-1)个方程分别减去第n个方程，从而达到把二次方程线性化的目的，得到下列方程组(2)：

定义：

解得最终利用最小二乘法θ＝(Q^TQ)^-1Q^Tb，最终解得得到锚节点在进路里的坐标位置为(40,0)。

UWB锚节点Anchor将坐标数据经过AP上传至地面监控中心，地面监控中心结合信标器i的坐标位置最终确定机车在井下巷道的最终位置。

Claims (7)

1.一种基于UWB的矿井无人机车高精度定位系统，其特征在于：包括：

若干UWB标签节点Tag，分别设置于矿井中作为UWB信号发送器，各个UWB标签节点Tag的坐标已知并分配有相应的ID号；

UWB锚节点Anchor，其设置在矿井中机车上作为UWB信号接收器，所述UWB锚节点Anchor实时接收各个UWB标签节点Tag发送的脉冲信号，并记录脉冲信号的到达时间以及脉冲信号对应的UWB标签节点Tag的坐标数据；

AP设备，其设置在矿井中，所述UWB标签节点Tag、UWB锚节点Anchor分别与AP设备无线通信连接；

每个进路至少一个信号机，所述信号机就是井下的路红灯，只有绿灯时机车才能驶入该条进路，信号机与井下交换机直接相连，信标器与信号机成对存在，所述信标器位置已知直接与井下交换机连接，可将其位置信息直接通过井下光纤环网传输至地面监控中心。

2.根据权利要求1所述的一种基于UWB的矿井无人机车高精度定位系统，其特征在于：UWB标签节点Tag、UWB锚节点Anchor均由主处理器以及连接在主处理器上的UWB模块、调试接口、LCD显示器及各类接口组成。

3.根据权利要求1所述的一种基于UWB的矿井无人机车高精度定位系统，其特征在于：UWB标签节点Tag等间距布设在矿井大巷上壁，相邻UWB标签节点Tag的间距小于其通信距离的三分之一。

4.根据权利要求1所述的一种基于UWB的矿井无人机车高精度定位系统，其特征在于：UWB锚节点Anchor布设在矿井中机车顶部。

5.一种基于UWB的矿井无人机车高精度定位方法，其特征在于：包括以下步骤：

(1)、将所有的UWB标签节点Tag和UWB锚节点Anchor维持一个相同的高精度同步时钟；

(2)、使各个UWB标签节点Tag定期向UWB锚节点Anchor发送UWB脉冲信号及自身坐标；

(3)、UWB锚节点Anchor将接收到的数据提供给自身主处理器，主处理器通过TOA测距算法测得各个UWB标签节点Tag至锚UWB锚节点Anchor的距离；

(4)、根据已知的各个UWB标签节点Tag的坐标及与UWB锚节点Anchor的距离，使用最小二乘法计算给出UWB锚节点Anchor的坐标；

(5)、UWB锚节点Anchor通过无线方式将数据传输到AP设备，AP设备再将数据经过井下光纤环网传输到地面监控中心，地面面监控中心结合矿井机车所在信标器位置给出机车井下的最终位置。

6.根据权利要求5所述的一种基于UWB的矿井无人机车高精度定位方法，其特征在于：所述步骤(3)的具体过程为：

(3.1)、UWB锚节点Anchor收集到的信息为UWB标签节点Tag_i传输到的时间t_i及其坐标(x_i,y_i)，其中i代表标签节点Tag在井下的编号，由于各个UWB标签节点Tag具有等间距分布且间距小于其通信距离的三分之一的分布特点，UWB锚节点Anchor接收到的UWB标签节点Tag坐标一般为3个；

(3.2)、通过TOA算法，测得UWB标签节点Tag_i到UWB锚节点Anchor的传输时间Δt_i，然后用传输时间Δt_i乘以光速c得到UWB标签节点Tag_i到UWB锚节点Anchor的距离d_i。

7.根据权利要求5或6所述的一种基于UWB的矿井无人机车高精度定位方法，其特征在于：所述步骤(4)的具体过程为：

(4.1)、根据上述步骤(3.2)中的距离及坐标数据，列出方程组(1)；

$\left\{\begin{array}{c}{(x-x_1)}^2+{(y-y_1)}^2={d_1}^2\\ {(x-x_2)}^2+{(y-y_2)}^2={d_2}^2\\ {(x-x_3)}^2+{(y-y_3)}^2={d_3}^2\\ ...\\ {(x-x_n)}^2+{(y-y_n)}^2={d_n}^2\end{array}\right.---\left(1\right),$

(4.2)、将方程组(1)的前(n-1)个方程分别减去第n个方程，从而达到二次方程线性化的目的，得到下列方程组(2)：

$\left\{\begin{array}{c}2(x_n-x_1)x+2(y_n-y_1)y={d_1}^2-{d_n}^2-({x_1}^2+{y_1}^2)+({x_n}^2+{y_n}^2)\\ 2(x_n-x_2)x+2(y_n-y_2)y={d_2}^2-{d_n}^2-({x_2}^2+{y_2}^2)+({x_n}^2+{y_n}^2)\\ 2(x_n-x_3)x+2(y_n-y_3)y={d_3}^2-{d_n}^2-({x_3}^2+{y_3}^2)+({x_n}^2+{y_n}^2)\\ ...\\ 2(x_n-x_{n-1})x+2(y_n-y_{n-1})y={d_{n-1}}^2-{d_n}^2-({x_{n-1}}^2+{y_{n-1}}^2)+({x_n}^2+{y_n}^2)\end{array}\right.---\left(2\right),$

(4.3)、定义：

$\mathrm{\θ}=\left[\begin{array}{c}x\\ y\end{array}\right]---\left(3\right),$

$Q=\left[\begin{array}{cc}2(x_n-x_1)& 2(y_n-y_1)\\ 2(x_n-x_2)& 2(y_n-y_2)\\ ...& ...\\ 2(x_n-x_{n-1})& 2(y_n-y_{n-1})\end{array}\right]---\left(4\right),$

$b=\left[\begin{array}{c}{d_1}^2-{d_n}^2-({x_1}^2+{y_1}^2)+({x_n}^2+{y_n}^2)\\ {d_2}^2-{d_n}^2-({x_2}^2+{y_2}^2)+({x_n}^2+{y_n}^2)\\ ...\\ {d_{n-1}}^2-{d_n}^2-({x_{n-1}}^2+{y_{n-1}}^2)+({x_n}^2+{y_n}^2)\end{array}\right]---\left(5\right),$

将方程组(2)改写成θ＝Q^-1b，用最小二乘法LS求解可得θ＝(Q^TQ)^-1Q^Tb。