基于多感知方式组合测量的采煤机位姿检测系统研究

张子悦,符世琛,刘 超,李永泽,兰 玉,吴 淼

(中国矿业大学(北京)机电与信息工程学院，北京 100083)

0 引 言

无人采掘装备是当前国际煤炭领域的发展前沿[1]。采煤机是井下综采工作面的核心设备，综采工作面的工况非常复杂，常存在高温高灰尘以及其他的干扰情况，导致驾驶员操控采煤机作业难以保证精度，采煤效率大幅降低，采煤机的位姿检测是实现采煤机无人化、自动化的关键。近年来，已有多位学者对采煤机的位姿检测进行了研究：张博渊[2]提出了捷联惯导系统与里程计相融合的采煤机定位技术，分析了影响采煤机定位精度的因素，建立了最优估计误差补偿模型，但惯导会产生较大的累积误差；方新秋等[3]提出了微机械陀螺和加速度计传感器组合自主定位系统的技术，根据采煤机的运动特征建立了动力学模型，仿真结果表明惯性传感器的误差是导致自主定位系统精度低的主要因素；李振平[4]提出了运用电子地图显示采煤机位置的技术，通过采煤机在巷道中的投影以及简单的数学计算，可消除采煤机定位系统计算过程中的累积误差；郝尚清等[5]提出了以采区坐标系作为参考坐标来确定采煤机坐标的技术，将此技术应用到采煤机自动调高和工作面自动调直中，但并未测得采煤机的姿态角。目前的采煤机位姿检测方法仅能获取采煤机的局部坐标系坐标，无法获取绝对坐标系坐标，所以无法满足采煤机定位的精度要求。

全站仪具有数据处理快速准确、定方位角快捷和快速自动测距的优点，可满足检测采煤机位姿实效性和精度的要求。UWB定位技术通过检测超宽带信号波到达时间来解算距离[6-8]，可以减小井下高温高灰尘工作环境对采煤机定位检测精度的影响。将寻北仪和倾角传感器安装至采煤机机身，寻北仪和倾角传感器是完全独立自主的，可测出采煤机的姿态角[9-10]。

笔者基于多种感知方式提出了一种采煤机的位姿检测方法，运用全站仪与UWB定位系统，对采煤机的空间坐标进行组合测量。运用倾角传感器和寻北仪测量采煤机的姿态角。重点介绍了UWB定位系统基于TOA定位模型的CT算法，推导了UWB固定基站群坐标系与全站仪坐标系间的坐标变换矩阵，介绍了倾角传感器和寻北仪测量采煤机姿态角的工作原理，分析了该系统在实际应用中的误差来源，提出了相应的改善方案。

1 多感知方式组合测量系统结构

为实现采煤机的位姿检测，提出了一种基于多感知方式组合测量的采煤机远程位姿检测系统，采煤机位姿检测系统布置如图1所示。

图1 采煤机位姿检测系统布置Fig.1 Shearer position and attitude detection system layout

本系统根据综采工作面不同的环境，设计了不同的检测方法，一定程度减小了环境对采煤机位姿检测精度的影响。区段运输平巷光照充足、粉尘少，但巷道狭长，不便采用UWB定位系统定位，故采用全站仪对UWB固定基站群定位。综采工作面工况复杂，采煤机工作时液压支架需移动，UWB定位较为灵活，基站之间可实现自主连续标定，故采用UWB定位系统对UWB定位节点定位。采用UWB系统解算采煤机的姿态角需3个定位节点，解算过程会产生累积误差，影响测量精度，故采用倾角传感器和寻北仪，通过仪器内部数据处理直接测得采煤机的姿态角。

采煤机位姿检测系统结构如图2所示，将UWB定位系统固定基站群安装在综采工作面与运输平巷交点处，为实现UWB移动基站移站时的自主标定，液压支架的顶端和底端分别安装UWB模块，安装UWB模块的液压支架和未安装UWB模块的液压支架间隔布置。将全站仪安装在运输上山与区段运输平巷处，以全站仪为采煤机位姿检测系统的定位基点，建立大地绝对坐标系，由地测人员进行坐标标定。将寻北仪、倾角传感器及UWB定位节点安装至采煤机机身，根据采煤机型号的尺寸规格可将定位节点安装在机身上部或侧部，将UWB定位节点安装在采煤机坐标系中便于测量且振动较小的位置，由于UWB信号易受非视距传播的影响，须保证定位节点与液压支架上的UWB基站视距传播。

图2 采煤机位姿检测系统结构Fig.2 Shearer position and attitude detection system structure

采煤机位姿检测系统原理如图3所示，本系统采用UWB定位技术测量UWB定位节点在UWB固定基站群坐标系中的坐标位置，采用全站仪技术测量UWB固定基站群在定位基点坐标系中的坐标位置。当采煤机工作时，截割头载荷的变化会导致采煤机机身振动，从而引起采煤机采煤方向以及自身水平方向的变化，采用倾角传感器测量采煤机的横滚角和俯仰角，采用寻北仪测得航向角。

图3 采煤机位姿检测系统原理Fig.3 Schematic of shearer posture detection system

2 采煤机的位置检测系统

2.1 全站仪检测系统

由于区段运输平巷空间狭窄、距离长、不便用UWB系统定位，笔者采用全站仪对UWB固定基站群进行空间定位，全站仪的三维坐标测量原理如图4所示。根据区段运输平巷的服务年限，由地测人员每隔半年到一年对设站点进行坐标标定，将设站点坐标C(xC,yC,zC)作为测站点的绝对坐标，以此坐标为基准建立空间坐标系，输入后视点B的绝对坐标B(xB,yB,zB)。

图4 全站仪及坐标测量Fig.4 Total station coordinate measurement chart

已知全站仪C到目标点A的距离为SAC，待定边AC的垂直角为α1，则全站仪C到目标点A的平距[11]为

DAC=SACcosα1

已知边BC的方位角为αBC，待定方向与已知方向的水平角为αAB，全站仪C的高程为HC，则目标点A的三维坐标为

根据全站仪的性能，可以快捷地测出目标点(xA,yA,zA)的坐标位置以及角度，从而可推导出全站仪坐标系与UWB固定基站群坐标系之间的坐标变换矩阵。

2.2 UWB定位系统

UWB技术通过检测超宽带信号波到达时间来解算距离，可以减小井下高温高灰尘工作环境对采煤机定位检测精度的影响，UWB基站布置示意如图5所示。UWB定位技术具有传输速率高、穿透力强、实时性好等优点，为实现UWB基站的自主连续标定，顶端和底端分别安装UWB基站的液压支架与未安装基站的液压支架间隔布置，通过各个UWB基站之间的信号传输，将UWB定位节点的坐标位置传输至UWB固定基站群[12-14]。

图5 UWB模块及基站布置示意Fig.5 UWB base station layout diagram

根据采煤机机身定位节点与定位基站群之间的超宽带信号飞行时间参量建立TOA定位模型。通过测量采煤机机身定位节点到个基站的空间直线距离，利用“在测量无误差的条件下，目标一定位于以各个定位基站为核心，以测量距离为半径的球面上”这一几何特征[15-16]，4个球位置面相交的交点就是采煤机机身定位节点的空间位置坐标。

根据TOA定位模型建立了将Caffery算法和Taylor级数法相结合的算法，利用Caffery算法确定一个初始点坐标[17]，进而使用Taylor级数展开法进行迭代消除误差[18-20]，可精确确定采煤机在空间内的坐标位置。

假设采煤机机身定位节点为M点，坐标为(xM,yM,zM)，安装在液压支架的4个基站分别为1,2,3,4，坐标为(xi,yi,zi)(i=1,2,3,4)，则基站i与定位节点M的空间距离为diM。

基站1,2,3,4与定位节点M之间的TOA定位观测方程组为

(1)

面向采煤机的超宽带位姿检测系统的Caffery定位算法如下，使式(1)中的第n个方程减去第n+1个方程，得到关于(xM,yM,xM)线性方程组：

(2)

将式(2)中计算得出的 (xM,yM,xM)作为对M点的初始估计值，其与估计值(xM0,yM0,zM0)的误差设为:δx、δy、δz，则有：

(3)

将式(1)在(xM,yM,zM)点用Taylor级数展开，并忽略二次及以上项，整理可得：

(4)

对式(4)进行重复迭代计算，当|δx|+|δy|+|δz|小于设定的门限时，则停止迭代，采煤机机身定位节点M的最终估计值(xM1,yM1,zM1)为

运用CT算法以同样原理可算出位于液压支架顶端和底端的各个UWB基站的坐标位置，最终结算出采煤机机身定位节点在UWB固定基站群所建立的坐标系中的空间坐标位置。

2.3 坐标系间的变换矩阵

已知采煤机机身定位节点M在UWB固定基站群坐标系中的坐标为(xM,yM,zM)，通过全站仪的测量已知由全站仪坐标系OXYZ到UWB固定基站群坐标系O′X′Y′Z′沿X轴移动了dX，沿Y轴移动了dY，沿Z轴移动了dZ，绕X轴旋转了θ角，绕Y轴旋转了α角，绕Z轴旋转了γ角，坐标系变换如图6所示，则两坐标系之间的旋转矩阵为式(5)。由于两坐标系间沿X轴移动了dX，沿Y轴移动了dY，沿Z轴移动了dZ距离，所以两坐标系之间的变换矩阵为式(6)。

图6 坐标系变换Fig.6 Transformation of coordinate system

(5)

(6)

3 采煤机姿态检测系统

3.1 倾角传感器结构及工作原理

倾角传感器将所有模块集中至小电路板，其中包括微控制单元、加速度计、数模转换电路以及通信单元，可直接输出所要测量的角度。

倾角传感器运用了牛顿第二定律，在一个系统内部可测量出加速度，根据初速度和加速度，可通过积分计算出线速度。当倾角传感器在水平状态时，没有横向和垂直的加速度，只有一个重力加速度，重力垂直轴与灵敏轴之间的夹角为倾斜角。

“固体摆”式精度更高，应用较为广泛，其工作原理如图7所示。系统由支架、摆锤和摆线组成，摆锤受重力G和拉力T的作用，β为摆线与重力方向的夹角，则所受合外力为

图7 “固体摆”式倾角传感器原理Fig.7 “Solid pendulum”tilt sensor principle

F=Gsinβ=mgsinβ

则有

在小角度测量范围测量时，可认为合外力F和夹角β呈线性关系，“固体摆”式传感器便基于此原理。在实际应用中，倾角传感器的测量精度易受振动的影响，故将倾角传感器安装至机身振动相对较小的位置，并在下一阶段研究中设计减震装置。

3.2 寻北仪结构及工作原理

陀螺寻北仪是由双自由度动力调谐陀螺，机械转动装置和信号解算电路组成的，是一种利用陀螺原理测定地球自转角速率在当地真北方位的惯性测量系统。

寻北仪测角如图8所示，在寻北过程中，将陀螺置于动基座上，测出陀螺在位置1处的角速度ω1，假设初始方向与真北方向的夹角为ψ，地球自转速度为ωe，地球自转在陀螺敏感轴的投影速度为ωecosφ，陀螺输出的零点漂移为ε(t1)，则有

图8 寻北仪及其测角示意Fig.8 North finder metering diagram

ω1=ωecosφcosψ+ε(t1)

(7)

转动基座90°到达位置2处，陀螺在位置2处的角速度ω2，陀螺输出的零点漂移为ε(t2)，则有

ω2=ωecosφcos(ψ+90°)+ε(t2)

(8)

在转动基座90°到达位置3处，陀螺在位置3处的角速度ω3，陀螺输出的零点漂移为ε(t3)，则有

ω3=ωecosφcos(ψ+180°)+ε(t3)

(9)

在转动基座90°到达位置4处，陀螺在位置4处的角速度ω4，陀螺输出的零点漂移为ε(t4)，则有

ω4=ωecosφcos(ψ+270°)+ε(t4)

(10)

在短时间内可将陀螺的零点漂移设为常量，即ε(t1)=ε(t2)=ε(t3)=ε(t4)，则由式(7)—式(10)可知

此方法可消除陀螺的零偏，快速地测得采煤机的航向角。在实际应用中，寻北仪的测量精度易受振动的影响，故将倾角传感器安装至机身振动相对较小的位置，并在下一阶段研究中设计减震装置。

4 结 论

1)建立了一种基于多感知方式组合测量的采煤机位姿检测系统，通过全站仪确定UWB固定基站群的位置坐标，通过UWB定位模型确定UWB定位节点相对于UWB固定基站群的位置坐标；通过倾角传感器和寻北仪对采煤机姿态角进行测量标定，从而实现采煤机的位姿检测。

2)完整推导了UWB定位系统基于TOA定位模型的CT算法，完整推导了全站仪坐标系与UWB固定基站群坐标系间的坐标变换矩阵，介绍了全站仪、寻北仪及倾角传感器的工作原理。分析了基于多感知方式组合测量的采煤机位姿检测系统在实际应用中的受限因素，并简要提出了改善方案。本系统在理论上可实现对采煤机综采工作面中的实时位姿检测，为系统仿真分析和实验平台搭建提供了理论参考。

3)基于多感知方式组合测量的采煤机位姿检测系统理论上实现了对采煤机在综采工作面中的实时位姿检测，为系统仿真分析和实验平台搭建提供了理论参考。