[发明专利]一种基于蓝牙AOA的综采工作面采煤机定位方法及系统

权利要求书

1.一种基于蓝牙AOA的综采工作面采煤机定位方法，其特征在于，包括以下步骤：

数据获取步骤：获取惯性导航装置(1)的姿态角和加速度，以及蓝牙AOA定位系统(2)在采煤机(3)切割过程中测得的所述采煤机(3)在空间三轴方向上的第一位置坐标；

数据处理步骤：所述姿态角和所述加速度通过计算得到所述采煤机(3)在空间三轴方向上的第二位置坐标，并将所述采煤机(3)在空间三轴方向上的第一位置坐标和所述采煤机(3)在空间三轴方向上的第二位置坐标输入卡尔曼滤波器中；

实时坐标计算步骤：所述采煤机(3)在空间三轴方向上的所述第一位置坐标作为所述卡曼尔滤波器的观测量，所述采煤机(3)在空间三轴方向上的所述第二位置坐标作为所述卡曼尔滤波器的状态量，将所述观测量和所述状态量输入所述卡曼尔滤波器，得到所述采煤机(3)的实时坐标。

2.根据权利要求1所述的一种基于蓝牙AOA的综采工作面采煤机定位方法，其特征在于，

在所述数据获取步骤中，所述蓝牙AOA定位系统(2)包括：蓝牙AOA主基站(21)、蓝牙AOA从基站(22)和蓝牙AOA标签(23)；其中，所述蓝牙AOA主基站(21)安装在进风巷道和回风巷道的超前液压支架上，所述蓝牙AOA从基站(22)安装在工作面液压支架(4)上，所述蓝牙AOA标签(23)安装在所述采煤机(3)的机身上；

所述惯性导航装置(1)安装在所述采煤机(3)机身上。

3.根据权利要求2所述的一种基于蓝牙AOA的综采工作面采煤机定位方法，其特征在于，

在所述实时坐标输出步骤之后还包括数据更新步骤，具体内容为：按照所述蓝牙AOA从基站(22)的工作面液压支架(4)在拉架动作完成后，实时更新移动后所述蓝牙AOA从基站(22)的三维坐标，并将更新后的坐标数据传输至所述数据获取步骤。

4.根据权利要求3所述的一种基于蓝牙AOA的综采工作面采煤机定位方法，其特征在于，

在所述采煤机(3)割一刀煤的过程中，重复所述数据获取步骤～所述数据更新步骤。

5.根据权利要求1所述的一种基于蓝牙AOA的综采工作面采煤机定位方法，其特征在于，

所述实时坐标计算步骤中，所述卡尔曼滤波器的状态方程建立方法如下：

所述采煤机(3)在空间三轴方向上的第二位置坐标为(X_INS,Y_INS,Z_INS)，以S_k＝[X_INS Y_INSZ_INS]^T为状态量建立状态方程，状态方程如下：

其中k代表当前时刻；A(k)为三阶单位矩阵；为三轴姿态角ψ、θ、构成的姿态转换矩阵；v_x、v_y、v_z分别为上一时刻的三轴速度；a_x、a_y、a_z分别为当前时刻的三轴加速度；T为惯性导航装置的采样周期；w_k为过程噪声。

6.根据权利要求5所述的一种基于蓝牙AOA的综采工作面采煤机定位方法，其特征在于，

所述实时坐标计算步骤中，所述卡尔曼滤波器的观测方程建立方法如下：

所述采煤机(3)在空间三轴方向上的第一位置坐标为(X_AOA,Y_AOA,Z_AOA)，以Z_k＝[X_AOA Y_AOAZ_AOA]^T为观测量建立观测方程，观测方程如下：

Z_k＝HS_k+r_k

其中H为单位矩阵，r_k为过程噪声。