一种采煤机惯性导航/无线传感器网络组合定位方法与流程

本发明属于采煤机定位技术领域，特别涉及一种采煤机惯性/无线传感器网络组合定位方法。

背景技术：

采煤机、液压支架和刮板输送机是井下综采工作面最重要的三种设备，相互配合完成割煤、运煤和支护工作。其中采煤机为主导设备，是综采工作面割煤和装煤的主要设备，是一种高集成度综采设备。采煤机在工作时沿刮板输送机的轨道进行往复式割煤，液压支架支护顶板并推进工作面。为了实现综采工作面自动化、远程自动控制，需要对采煤机精确动态定位，所以采煤机定位是煤矿生产装备自动化的关键技术。煤矿综采工作面工况复杂，同时空间封闭，因此采煤机定位是一个典型的复杂封闭环境下室内定位问题，这意味着常用的卫星导航定位、天文导航定位等需要借助外部环境的导航定位技术无法使用。

目前采煤机定位方法主要包括捷联惯导定位法、红外定位法、超声波定位法、齿轮计数定位法、无线传感器网络定位法等。

所述的捷联惯导定位方法是一种全自主导航定位方法，无需借助外部信息，利用捷联惯导装置的三轴陀螺和三轴加速度计实时测量采煤机的角速度和线加速度，结合初始装订信息，通过姿态更新算法先解算出采煤机的运动姿态，然后将加速度根据姿态信息投影到导航坐标系，通过积分和二次积分获得采煤机的速度和位置等信息。其短时定位精度高，但在长时间工作后，因为累积误差使得定位精度下降，需要利用组合导航方法进行误差修正来保持高精度定位和定姿。

所述的红外定位法由安装在采煤机上红外发射装置发射信号，液压支架上安装的接收装置接收信号，利用红外测距定位采煤机的位置，但其红外信号易受粉尘影响，且存在固有定位盲区，定位精度不高，因此使用有局限性。

所述的超声波定位法将超声波发射装置安装在工作面巷道中，当采煤机经过时，机身发射超声波，根据各位置超声波接收装置接收信号，利用超声波测距定位采煤机的位置，超声波的优点是可以穿透粉尘，但由于工作面较长，信号失信严重，其定位精度不高，因此使用有局限性。

所述的齿轮计数定位法对采煤机行走齿轮的转动圈数进行计数，根据转动圈数和齿轮圆周长计算采煤机沿输送机轨道方向上的位移。但该方法只能用于定位采煤机沿轨道方向的一维位置，且受到齿轮计数误差影响，不能满足三维定位需要。

所述的无线传感器网络定位法在液压支架上布置多个位置已知的无线传感器(称为锚节点)，在采煤机上布置待定位节点(称为移动节点)，移动节点发射无线信号，锚节点接收无线信号监测采煤机与液压支架间的位置关系，解算出采煤机的位置。但由于工作面环境复杂，无线定位数据不稳定，且锚节点随液压支架移动后会导致自身位置变化，需要更新锚节点位置信息，同时不能进行采煤机定姿，不能满足实时定位定姿需求。

针对综采工作面中采煤机高精度定位和定姿需求，捷联惯导定位方法具有定位数据全面、更新率高、短时定位精度较高以及可以提供姿态信息等优点，然而其具有长时间的累积误差会使得定位精度随着时间下降。无线传感器网络定位方法采用无线信号进行测量解算进而得到采煤机位置信息，是针对每一次的测距信号的单独解算，对于相邻时刻的数据之间无信息交换与传递，因此其不会出现类似与捷联惯导的累积误差，但由于节点间是基于测距解算，使得不能直接提供采煤机的姿态信息，而且针对锚节点对于每一次无线信号的检测以及信号调理等引起的时间消耗，使得无线传感器网络的定位数据更新率显著低于捷联惯导定位数据更新率，且存在定位延时误差。

技术实现要素：

为了解决上述现有技术的不足，本发明提供了一种采煤机惯性/无线传感器网络组合定位方法，能够解决移动节点定位延时误差补偿问题、锚节点位置更新问题和捷联惯导误差修正问题，具有解决移动节点定位延时误差补偿、锚节点位置更新、捷联惯导误差修正的问题的优点。

为了实现上述目的，本发明采用了如下的技术方案：

一种采煤机惯性/无线传感器网络组合定位方法，

步骤一、基于rssi的无线传感测距方法，使用公知的对数-常数无线信号传播模型，s＝a-10mlg(d)，将接收到的信号强度转化为移动节点与锚节点之间的距离；根据位置已知的锚节点坐标，利用最小二乘算法计算在发射信号时移动节点的位置坐标p0＝(x0y0z0)^t；其中：其中，s为信号强度，d为距离，a为接收端距离发射端为1m时接收到的无线信号强度，m为路径损耗；a和m为已知参数，根据s计算距离d；

步骤二、利用陀螺和加速度计测量信息，解算捷联惯导装置俯仰角θ、航向角ψ和滚动角γ，计算捷联惯导装置速度v＝[vnvuve]^t，位置pi＝[pnpupe]^t和经度λ、纬度高度h；

步骤三、利用步骤一和步骤二的信息，进行惯性/无线传感器网络组合导航解算；组合导航滤波器状态变量为其中位置误差δpi＝[δpnδpuδpe]^t，速度误差δv＝[δvnδvuδve]^t，姿态误差φ＝[φnφuφe]^t，陀螺漂移加速度计零偏组合导航滤波状态方程由捷联惯导的速度误差方程、位置误差方程和姿态误差方程组成，组合导航滤波量测方程为y＝hx＝[i3×303×303×303×303×3]x；

采用插值方法解决无线传感器网络和捷联惯导定位信息不同步问题，计算在移动节点发射无线信号时捷联惯导位置计算量测信息利用公知的卡尔曼滤波算法，可得组合导航滤波状态量估计值利用修正捷联惯导的位置、速度和姿态误差；

步骤四、根据捷联惯导短期精度高的特点，利用修正后的捷联惯导速度信息v和定位延迟时间τ，修正移动节点定位延迟误差，计算在组合滤波时刻，移动节点的位置p(tk)＝p0+v(tk)τ；

步骤五、移动节点随采煤机沿轨道运行中的实时定位，在下一次组合导航更新前，即在下一次进行移动节点无线测距定位前，利用捷联惯导速度信息v辅助计算移动节点的位置p(t)＝p(tk)+v(tk)(t-tk)；

步骤六、锚节点随液压支架移动而位置变化后，利用移动节点位置和修正后的捷联惯导信息对该锚节点进行定位，未知锚节点移动到新位置，发射无线信号，移动节点随采煤机沿输送机轨道方向移动，在多个位置接收无线信号，利用rssi算法进行测距，m个位置的移动节点位置坐标为未知锚节点的新位置坐标为(xjyjzj)^t，利用最小二乘算法，计算更新后的锚节点位置坐标为pj＝(xjyjzj)^t。

采煤机工作过程中，采煤机惯性/无线传感器网络组合定位方法，具体步骤如下：

1)定义坐标系采煤机体坐标系obxbybzb：坐标系原点ob固连在捷联惯导装置4中心，xb轴正向由采煤机1指向煤壁，yb轴垂直于xb轴向上，zb轴与xb轴、yb轴构成右手坐标系，前上右坐标系；

2)定义导航坐标系onxnynzn：北天东地理坐标系，xn轴指向地理北向，yn轴指向天向，zn轴指向地理东向；

3)导航坐标系经过三次旋转后与采煤机体坐标系重合，三次旋转的角度即为采煤机1的航向角ψ、俯仰角θ和滚动角γ；

4)n个锚节点在初始位置在导航坐标系下坐标为其中上标i＝1，...，n表示锚节点序号；下标j＝0，1，...表示锚节点的移动次数，每移动到一个新位置，次数加1；所有锚节点在初始位置均为预先设定的，因此已知；

捷联惯导装置接收初始装订信息：初始位置pi(0)＝[xi(0)yi(0)zi(0)]^t和初始姿态其中下标/表示惯导，x，y，z分别表示导航坐标系下北向、天向和东向位置；同时由于采煤机初始处于停止状态，初始速度v＝[000]^t；

5)基于rssi的无线传感测距方法，利用位置已知的锚节点，对移动节点进行测距，并计算发射信号时的移动节点位置；

移动节点发射无线信号，液压支架上位置已知的锚节点接收移动节点的无线信号，利用rssi算法进行测距；基于rssi的测距方法，将接收到的信号强度转化为移动节点与锚节点之间的距离，使用公知的对数-常数无线信号传播模型：

s＝a-10mlg(d)(1)

其中，s为信号强度，d为距离，a为接收端距离发射端为1m时接收到的无线信号强度，m为路径损耗；a和m为已知参数，根据s计算距离d；

根据公式(1)，位置已知的n个锚节点接收到的信号强度si，i＝1，...，n，可得di，i＝1，...，n；

6)位置已知的n个锚节点的位置坐标为(xiyizi)^t，在发射信号时移动节点的位置坐标为(x0y0z0)^t，根据几何关系有：

公式(2)前n-1个方程分别与第n个方程相减，整理可得

则利用最小二乘算法计算在发射信号时移动节点的位置坐标p0＝(x0y0z0)^t

其中

7)捷联惯导装置4位置和姿态解算：

捷联惯导装置4包括三只陀螺和三只加速度计；三只陀螺测量采煤机三个轴的角速度向量三只加速度计测量采煤机三个轴向的加速度向量

捷联惯导装置4首先更新采煤机体坐标系相对于导航坐标系的角速度向量

其中，表示纬度值，为姿态矩阵，ωie是地球自转角速度，r为地球半径，ve和vn表示东向速度和北向速度；

更新四元数q＝[q0q1q2q3]^t

其中，t表示姿态计算周期；

更新捷联惯导装置4相对于当地水平地理坐标系的惯性姿态矩阵

令其中i，j＝1，2，3

根据惯性姿态角计算如下：

俯仰角θ＝sin^-1(c12)

航向角

滚动角

利用将加速度计输出的加速度向量投影到当地水平地理坐标系，fⁿ＝[fnfufe]^t，

更新捷联惯导装置4的速度v＝[vnvuve]^t，位置pi＝[pnpupe]^t和经度λ、纬度高度h：

8)惯性/无线传感器网络组合导航解算，实时修正捷联惯导误差：

组合导航滤波器状态变量为其中位置误差δpi＝[δpnδpuδpe]^t，速度误差δv＝[δvnδvuδve]^t，姿态误差φ＝[φnφuφe]^t，陀螺漂移加速度计零偏

组合导航滤波状态方程由捷联惯导的速度误差方程、位置误差方程和姿态误差方程组成：

速度误差方程：

其中，

位置误差方程

姿态误差方程

9)将速度误差方程、位置误差方程和姿态误差方程离散化并改写为组合导航滤波状态方程形式，其中tk表示滤波时间点；

x(tk)＝f(tk-1)x(tk-1)；(14)

10)组合导航滤波的量测方程

y＝hx＝[i3×303×303×303×303×3]x；(15)

11)计算量测量

在移动节点发射无线信号时，捷联惯导记录时间间隔δt，δt＜δt，采用插值方法，计算在移动节点发射无线信号时捷联惯导位置

同时，利用公式(3)解算的移动节点在发射无线信号时的位置p0，计算量测信息：

12)利用公知的卡尔曼滤波算法，可得组合导航滤波状态量估计值

13)在tk，利用对捷联惯导误差进行闭环点修正：

pi(tk)＝pi(tk)-δp(tk)(18)

v(tk)＝v(tk)-δv(tk)(19)

得到修正后的位置、速度和姿态矩阵。对修正误差后的按照12)，重新计算惯性姿态角：俯仰角θ、航向角ψ和滚动角γ；

14)利用修正后的捷联惯导速度信息v和定位延迟时间τ，修正移动节点定位延迟误差，计算在组合滤波时刻，移动节点的位置p如下：

p(tk)＝p0+v(tk)τ(21)

15)移动节点随采煤机沿轨道运行中的实时定位：在下一次组合导航定位更新前，即在下一次进行移动节点无线测距定位前，利用捷联惯导速度信息v辅助计算移动节点的位置，提高移动节点位置数据更新率，

p(t)＝p(tk)+v(tk)(t-tk)(22)

其中tk≤t＜tk+1；

16)利用移动节点位置和修正后的捷联惯导信息对未知锚节点进行定位；

设未知锚节点序号为j，未知锚节点j在垂直于煤壁方向上移动并停止到新位置，发射无线信号，移动节点随采煤机沿输送机轨道方向移动，在多个位置接收无线信号，利用rssi算法进行测距；基于rssi的测距方法，使用公知的对数-常数无线信号传播模型，将接收到的信号强度转化为移动节点与锚节点之间的距离；

移动节点在m个位置接收到的信号强度si，i＝1，...，m，可得移动节点与位未知锚节点的距离为di，i＝1，...，m；

17)m个位置的移动节点位置坐标为未知锚节点的新位置坐标为(xjyjzj)^t，根据几何关系有：

利用最小二乘算法，计算更新后的锚节点位置坐标为pj＝(xjyjzj)^t。

本发明的有益效果是：由于提供了一种采煤机惯性/无线传感器网络组合定位方法，解决了移动节点定位延时误差补偿问题、锚节点位置更新问题和捷联惯导误差修正问题，实现采煤机高精度定位定姿，使其满足综采工作面自动化、远程自动控制的需求。

附图说明

图1为采煤机惯性导航/无线传感器网络组合定位系统示意图。

图2为采煤机坐标系、导航坐标系、采煤机姿态示意图。

图3为采煤机惯性/无线传感器网络组合定位工作流程图。

图4为采煤机惯性/无线传感器网络组合定位信息时序图。

图中：1、采煤机；2、刮板输送机；3、液压支架；4、捷联惯导装置；5、移动节点；6、锚节点。

具体实施方式

采煤机惯性/无线传感器网络组合定位方法，具体实施方式如下：

1)定义坐标系采煤机体坐标系obxbybzb：坐标系原点ob固连在捷联惯导装置4中心，xb轴正向由采煤机1指向煤壁，yb轴垂直于xb轴向上，zb轴与xb轴、yb轴构成右手坐标系，前上右坐标系，如图2所示。

2)定义导航坐标系onxnynzn：北天东地理坐标系，xn轴指向地理北向，yn轴指向天向，zn轴指向地理东向，如图2所示。

3)导航坐标系经过三次旋转后与采煤机体坐标系重合，三次旋转的角度即为采煤机的航向角ψ、俯仰角θ和滚动角γ，如图2所示。

4)n个锚节点在初始位置在导航坐标系下坐标为其中上标i＝1，...，n表示锚节点序号；下标j＝0，1，...表示锚节点的移动次数，每移动到一个新位置，次数加1；所有锚节点在初始位置均为预先设定的，因此已知。

捷联惯导装置接收初始装订信息：初始位置pi(0)＝[xi(0)yi(0)zi(0)]^t和初始姿态其中下标i表示惯导，x，y，z分别表示导航坐标系下北向、天向和东向位置；同时由于采煤机初始处于停止状态，初始速度v＝[000]^t。

5)基于rssi的无线传感测距方法，利用位置已知的锚节点，对移动节点进行测距，并计算发射信号时的移动节点位置；无线传感器网络定位的时间序列见图4。

移动节点发射无线信号，液压支架上位置已知的锚节点接收移动节点的无线信号，利用rssi算法进行测距。基于rssi的测距方法将接收到的信号强度转化为移动节点与锚节点之间的距离，使用公知的对数-常数无线信号传播模型：

s＝a-10mlg(d)(1)

其中，s为信号强度，d为距离，a为接收端距离发射端为1m时接收到的无线信号强度，m为路径损耗。a和m为已知参数，根据s计算距离d。

根据公式(1)，位置已知的n个锚节点接收到的信号强度si，i＝1，...，n，可得di，i＝1，...，n。

6)位置已知的n个锚节点的位置坐标为(xiyizi)^t，在发射信号时移动节点的位置坐标为(x0y0z0)^t，根据几何关系有：

公式(2)前n-1个方程分别与第n个方程相减，整理可得

则利用最小二乘算法计算在发射信号时移动节点的位置坐标p0＝(x0y0z0)^t

其中

7)捷联惯导装置位置和姿态解算，工作时间序列见图4。

捷联惯导装置包括三只陀螺和三只加速度计，三只陀螺测量采煤机三个轴的角速度向量三只加速度计测量采煤机三个轴向的加速度向量

捷联惯导装置首先更新采煤机体坐标系相对于导航坐标系的角速度向量

其中，表示纬度值，为姿态矩阵，ωie是地球自转角速度，r为地球半径，ve和vn表示东向速度和北向速度；

更新四元数q＝[q0q1q2q3]^t

其中，t表示姿态计算周期；

更新捷联惯导装置4相对于当地水平地理坐标系的惯性姿态矩阵

令其中i，j＝1，2，3

根据惯性姿态角计算如下：

俯仰角θ＝sin^-1(c12)

航向角

滚动角

利用将加速度计输出的加速度向量投影到当地水平地理坐标系，fⁿ＝[fnfufe]^t，

更新捷联惯导装置4的速度v＝[vnvuve]^t，位置pi＝[pnpupe]^t和经度λ、纬度高度h：

8)惯性/无线传感器网络组合导航解算，修正捷联惯导误差，组合导航工作时间序列见图4。

组合导航滤波器状态变量为其中位置误差δpi＝[δpnδpuδpe]^t，速度误差δv＝[δvnδvuδve]^t，姿态误差φ＝[φnφuφe]^t，陀螺漂移加速度计零偏

组合导航滤波状态方程由捷联惯导的速度误差方程、位置误差方程和姿态误差方程组成：

速度误差方程：

其中，

位置误差方程

姿态误差方程

9)将速度误差方程、位置误差方程和姿态误差方程离散化并改写为组合导航滤波状态方程形式，其中tk表示滤波时间点。

x(tk)＝f(tk-1)x(tk-1)(14)

10)组合导航滤波的量测方程

y＝hx＝[i3×303×303×303×303×3]x(15)

11)计算量测量

捷联惯导定位数据更新率显著高于无线传感器网络的定位数据更新率，如图4所示，在移动节点发射无线信号时，捷联惯导记录时间间隔δt，δt＜δt。采用插值方法，计算在移动节点发射无线信号时捷联惯导位置

同时，利用公式(3)解算的移动节点在发射无线信号时的位置p0，计算量测信息：

12)利用公知的卡尔曼滤波算法，可得组合导航滤波状态量估计值

13)在tk，利用对捷联惯导误差进行闭环点修正：

pi(tk)＝pi(tk)-δp(tk)(18)

v(tk)＝v(tk)-δv(tk)(19)

得到修正后的位置、速度和姿态矩阵。对修正误差后的按照12)，重新计算惯性姿态角：俯仰角θ、航向角ψ和滚动角γ。

14)因为移动节点随采煤机沿轨道运行，同时无线测距定位需要一定时间，所以移动节点在发射信号时的位置与组合滤波时的位置不同，需要修正移动节点定位延迟误差。根据捷联惯导短期精度高的特点，利用修正后的捷联惯导速度信息v和定位延迟时间τ，修正移动节点定位延迟误差，如图4所示，计算在组合滤波时刻，移动节点的位置p如下：

p(tk)＝p0+v(tk)τ(21)

15)移动节点随采煤机沿轨道运行中的实时定位。如图4所示，在下一次组合导航定位更新前，即在下一次进行移动节点无线测距定位前，利用捷联惯导速度信息v辅助计算移动节点的位置，提高了移动节点位置数据更新率，

p(t)＝p(tk)+v(tk)(t-tk)(22)

其中tk≤t＜tk+1；

16)利用移动节点位置和修正后的捷联惯导信息对未知锚节点进行定位。

设未知锚节点序号为j。未知锚节点j在垂直于煤壁方向上移动并停止到新位置，发射无线信号，移动节点随采煤机沿输送机轨道方向移动，在多个位置接收无线信号，利用rssi算法进行测距；基于rssi的测距方法，使用如公式(1)所示的公知的对数-常数无线信号传播模型，将接收到的信号强度转化为移动节点与锚节点之间的距离；

移动节点在m个位置接收到的信号强度si，i＝1，...，m，可得移动节点与位未知锚节点的距离为di，i＝1，...，m。

17)根据公式(22)计算，m个位置的移动节点位置坐标为未知锚节点的新位置坐标为(xjyjzj)^t，根据几何关系有：

与步骤一类似，利用最小二乘算法，计算更新后的锚节点位置坐标为pj＝(xjyzj)^t。

本发明的有益效果是：提供了一种采煤机惯性/无线传感器网络组合定位方法，解决了移动节点定位延时误差补偿问题、锚节点位置更新问题和捷联惯导误差修正问题，实现采煤机高精度定位定姿，使其满足综采工作面自动化、远程自动控制的需求。

一种采煤机惯性/无线传感器网络组合定位方法，在采煤机上安装捷联惯导装置和无线传感器网络移动节点；在液压支架下安装n个(不少于4个)无线传感器网络锚节点，所有锚节点均为预先部署，其初始位置已知。移动节点和锚节点均可以发射和接收无线信号。

随着采煤机的截割煤壁运动，采煤机上的移动节点实时发射无线信号，同时发送同步信号给捷联惯导装置，液压支架上位置已知的锚节点接收移动节点的无线信号，利用rssi算法进行测距和位置解算。捷联惯导装置实时测量采煤机的角速度和加速度信息，利用捷联导航算法进行导航解算；捷联惯导装置记录同步信号到达时刻前后的位置信息，与锚节点定位结果进行数据融合，修正移动节点定位延迟误差和捷联惯导误差，获得采煤机的精确位置和姿态信息。

同时，随着采煤机的截割煤壁，液压支架沿垂直于煤壁方向进行跟机运动，各个锚节点依次随着液压支架在垂直于煤壁方向上移动并停止到新位置(因为其位置未知，下文称为未知锚节点)。锚节点停到新位置后，需要对该锚节点定位。未知锚节点发射无线信号，采煤机上的移动节点接收未知锚节点的无线信号，利用rssi算法进行测距，随着采煤机运动，移动节点在m个位置(不少于4个)进行未知锚节点测距，结合捷联惯导位置信息完成未知锚节点的定位。