[发明专利]一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法

权利要求书

1.一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法，其特征在于，包括以下步骤：

步骤一：实现本发明方法的相关系统设备组成；

步骤二：实施本发明方法的设备部署方案；

步骤三：使用基于Kalman滤波无线时钟同步法计算得到POLL消息TDOA时间差值；

步骤四：使用DS-TWR方法得到识别卡的TOA测距值；

步骤五：根据TDOA与DS-TWR联合定位结果，使用Chan方法计算识别卡坐标值。

2.根据权利要求1所述一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法，其特征在于，由UWB定位器M、UWB定位器S1、UWB定位器S2、集控器共同组成，UWB定位器与集控器采用RS-485或CAN总线连接，并实时上报接收到的定位数据和时钟同步数据，并在集控器内实现初步的位置信息解算。

3.根据权利要求2所述一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法，其特征在于，沿着井下巷道两侧依次布设所述UWB定位器M、集控器、UWB定位器S1、UWB定位器S2，其中三个UWB定位器不能同时共线，间距大于1米，小于20米。

4.根据权利要求1所述的一种基于UWB技术的TDOA与DS_TWR结合的井下二维定位方法，其特征在于，使用基于Kalman滤波方法实现无线时钟同步法，并计算POLL消息到达各UWB定位器的到达时间差值(TDOA)；

T为时钟周期，U_θ,k和U_γ,k分别是第K周期的时钟偏移和时钟偏移的修正值，θ_k和γ_k分别是时钟偏移和时钟漂移；

在(K+1)T时刻，时钟迭代关系：

(θ_k+1)＝θ_k-U_θ,k+(γ_k-U_γ,k)T+ω_θ,k

(γ_k+1)＝γ_k-U_γ,k+ω_γ

ω_θ,k是时钟偏差方差，ω_γ,k是时钟漂移方差；

假设ω_k＝[ω_θ,k ω_γ,k]^T，协方差矩阵为Q，定义向量矩阵：

x_k＝[θ_k γ_k]^T,u_k＝[U_θ,k U_γ,k]^T.

使用Kalman滤波迭代：

建立预测方程：(X_k+1)＝Ax_k+Bu_k

其中：

Kalman增益矩阵：

K_k+1＝P_k+1|k[H_k+1]^T(R_k+1+H_k+1P_k+1|k[H_k+1]^T)^-1

R_k+1是观测噪声的协方差矩阵，H_k+1是测量矩阵，单位方阵；

修正：

最小均方差误差矩阵：

P_k+1＝(1-K_k+1)P_k+1|k

经过Kalman滤波，第K+1时刻时钟周期的修正值另是补偿时钟偏移和时钟漂移，则有UWB定位器S1和UWB定位器S2的时钟偏移分别为S1U_K+1和S2U_K+1。