[发明专利]煤矿井下自动驾驶车辆精确定位系统及其定位方法

权利要求书

1.一种煤矿井下自动驾驶车辆精确定位系统，其特征在于：具有UWB定位系统、超声波雷达、矿井GIS地理信息系统、UWB定位卡以及IMU，将UWB定位系统、超声波雷达以及矿井GIS地理信息系统三者进行融合形成类GNSS定位算法，得到自动驾驶车辆在煤矿井下的三维坐标，将UWB定位卡、超声波雷达以及IMU均安装在自动驾驶车辆上，将类GNSS定位算法与IMU相互结合，IMU得到的数据作为卡尔曼滤波的先验信息，类GNSS定位算法得到的数据作为矫正信息。

2.如权利要求1所述的煤矿井下自动驾驶车辆精确定位系统，其特征在于：所述UWB定位卡位于自动驾驶车辆的车顶。

3.一种如权利要求1所述的煤矿井下自动驾驶车辆精确定位系统的定位方法，其特征在于，具体步骤如下：

第1步骤、计算出自动驾驶车辆相对于UWB基站的坐标(X、Y、Z)；

第2步骤、计算出自动驾驶车辆的坐标(Xm、Ym、Zm)；

第3步骤、将IMU安装在自动驾驶车辆上，利用IMU直接测得自动驾驶车辆的三轴加速度与三轴角速度，并以此对目标进行定位，通过类GNSS定位算法得到的坐标信息(Xm、Ym、Zm)与IMU通过卡尔曼滤波融合，得到包括自动驾驶车辆位姿信息的定位信息。

4.如权利要求3所述的煤矿井下自动驾驶车辆精确定位系统的定位方法，其特征在于：所述第1步骤的具体步骤如下：

UWB基站与UWB定位卡的直线距离L的计算公式如下：

L²＝X²+Y²+Z² (1)

其中，L表示UWB基站与UWB定位卡的直线距离；

X表示自动驾驶车辆UWB基站在x轴方向的距离；

Y表示自动驾驶车辆UWB基站在y轴方向的距离；

Z表示自动驾驶车辆UWB基站在z轴方向的距离；

利用UWB定位系统直接测量出UWB基站与自动驾驶车辆的直线距离L；

利用超声波雷达直接测量出自动驾驶车辆与巷道壁的距离X；

测量UWB基站与UWB定位卡位置的垂直巷道距离Z；

由公式(1)可以推算出Y的计算公式，具体如下：

进而得到自动驾驶车辆相对于UWB基站的坐标(X、Y、Z)。

5.如权利要求4所述的煤矿井下自动驾驶车辆精确定位系统的定位方法，其特征在于：所述距离Z是由尺子量出来的结果。

6.如权利要求3所述的煤矿井下自动驾驶车辆精确定位系统的定位方法，其特征在于：所述第2步骤的具体步骤如下：

利用矿井GIS地理信息系统得到UWB基站的坐标(X₁、Y₁、Z₁)；

设定垂直于巷道壁的轴为巷道x轴，直接利用矿井GIS地理信息系统得到巷道x轴的单位向量为

设定平行于巷道的轴为巷道y轴，直接利用矿井GIS地理信息系统得到巷道y轴的单位向量为

设定垂直于巷道面的轴为巷道z轴，直接利用矿井GIS地理信息系统得到巷道z轴的单位向量为

进而得到自动驾驶车辆的坐标为(Xm、Ym、Zm)。

7.如权利要求6所述的煤矿井下自动驾驶车辆精确定位系统的定位方法，其特征在于：利用不同坐标系下的坐标转换方式得到自动驾驶车辆的坐标(Xm、Ym、Zm)。

8.如权利要求7所述的煤矿井下自动驾驶车辆精确定位系统的定位方法，其特征在于：自动驾驶车辆坐标(Xm、Ym、Zm)中Xm计算公式如下：

其中，是基于巷道坐标系下的x轴的方向向量；是基于GIS坐标系下的x轴方向向量。

9.如权利要求7所述的煤矿井下自动驾驶车辆精确定位系统的定位方法，其特征在于：自动驾驶车辆坐标(Xm、Ym、Zm)中Ym计算公式如下：

其中，是基于巷道坐标系下的y轴的方向向量；是基于GIS坐标系下的y轴方向向量。

10.如权利要求7所述的煤矿井下自动驾驶车辆精确定位系统的定位方法，其特征在于：自动驾驶车辆坐标(Xm、Ym、Zm)中Zm计算公式如下：

其中，是基于巷道坐标系下的z轴的方向向量；是基于GIS坐标系下的z轴方向向量。