0 引言
单轨吊机车是煤矿井下用来运送材料、设备和人员的先进辅助运输设备。与传统的辅助运输方式相比，单轨吊机车具有运输效率高、坡度大、不受巷道地面影响等特点。尽管这种运输方式优点突出，但其定位方式落后，目前仍以定位精度较低的射频识别(Radio Frequency Identification，RFID)定位和接收信号强度指示(Received Signal Strength Indication，RSSI)定位为主，难以满足工业应用需求。据2013—2017年我国煤矿事故统计，运输事故数占煤矿事故总数的17.48%，运输事故死亡人数占煤矿事故总死亡人数的10.85%[1]，这与矿井辅助运输机车无法精确定位有直接关系。此外，在不确定机车位置的情况下，无法实现机车的统一调度，会出现多辆单轨吊机车拥堵的情况，运输效率较低。因此，开发单轨吊机车高精度定位系统具有重要的现实意义。
目前，应用于煤矿井下的定位技术有RFID、RSSI、超宽带(Ultra Wide Band，UWB)、捷联惯性导航等定位技术。文献[2]设计了一种基于RFID和ZigBee技术的矿井机车精确定位系统，该系统通过RFID技术实现位置的更新，以ZigBee技术实现数据的传输，电子标签密度越高，定位精度越高，但这会导致成本上升。文献[3]设计了一种基于ZigBee的钨矿井下矿车定位监测系统，该系统利用RSSI测距模型和加权质心定位算法实现了矿车定位，与极大似然估计算法相比，定位精度较高，但该系统需要较多的参考节点，定位成本高。文献[4]提出了一种基于RSSI的矿山井下机车定位算法，利用最小二乘法和chan算法实现机车的定位，最小定位误差为0.4 m，但该方法也需要较多参考节点才能实现精确定位，定位成本高。文献[5]设计了一种基于UWB的矿井机车定位系统，该系统利用UWB技术和到达时间差(Time Difference of Arrival，TDOA)定位算法实现机车定位，定位精度达10 cm，但该系统在实现某一区域的定位时，需要1台同步控制器和多台定位基站，成本较高。文献[6]设计了基于惯性导航的煤矿移动车辆定位系统，该系统采用六轴惯性测量单元(Inertial Measurement Unit，IMU)实现对车辆的定位，然而，六轴IMU解算出的航向角误差较大，难以对机车进行长期、准确的定位。
针对以上定位方法存在的问题，本文提出了一种基于捷联惯性导航的矿用单轨吊机车定位算法，该算法不仅提高了单轨吊机车的定位精度，而且降低了定位成本。
1 定位流程
捷联惯性导航系统(Strapdown Inertial Navigation System，SINS)是一个将IMU固定于载体上的独立定位系统[7]，已广泛应用于需要定位的各种工程领域[8]。基于SINS的矿用单轨吊机车定位流程如图1所示。
图1 基于SINS的矿用单轨吊机车定位流程
Fig.1 Positioning process of mine-used monorail crane locomotive based on SINS
利用九轴IMU(包括三轴加速度传感器、三轴陀螺仪、三轴磁力计)采集机车数据，解算并输出姿态角和加速度值。为了滤除加速度数据中的确定性误差和尖峰噪声，需对其进行预处理。由于加速度数据中含有重力分量，当机车在斜坡上行驶时该分量会对测距造成巨大干扰，因而采用方向余弦矩阵法滤除该分量。在积分测距的过程中，通过加速度变化率阈值法和零速修正法修正稳态误差，根据机车测距结果和初始位置实现定位。
2 数据采集与预处理
2.1 数据采集
采用WT931模块采集数据，该模块集成了高精度的微机电系统(Micro-Electro-Mechanical System，MEMS)加速度计、陀螺仪、磁力计，可分别采集机车的三轴加速度、三轴角速度、三轴磁场强度数据。对加速度积分可以得到机车的行驶距离。以加速度、角速度、磁场强度数据为基础，利用卡尔曼滤波算法可以精准地输出机车的实时姿态角。
2.2 数据预处理
尽管MEMS加速度传感器具有体积小、质量小、功耗低、成本低、易集成等优点[9]，但其采集到的加速度数据中仍存在误差和噪声，所以，必须对加速度数据进行预处理。
MEMS加速度传感器采集到的加速度数据中的误差和噪声主要包括确定性误差、随机误差、尖峰噪声等[10]。误差模型为
a=a0+Kar+R+P
(1)
式中：a为加速度测量值；a0为零偏；K为刻度因数；ar为加速度理论值；R为随机噪声；P为尖峰噪声。
WT931模块中集成了高精度的BMI160六轴传感器，该传感器可输出高精度的三轴加速度和三轴角速度数据，加速度中所含的随机噪声很小，因此，可不考虑其影响。零偏、刻度因数、尖峰噪声均会对加速度测量值造成较大干扰。本文分别采用均值法和限幅滤波法滤除零偏和尖峰噪声，采用翻滚法计算刻度因数。
2.2.1 零偏滤除方法
零偏是指加速度传感器输入为0时的输出量，它是由加速度传感器本身的制作工艺引起的误差。对于零偏，可采用均值法滤除。以Y轴为例，首先将WT931模块置于水平台上(Y轴平行于台面)，连续采集2 000个加速度数据并求出均值aav，此后，用采集到的加速度数据减去aav即可滤除零偏[11]。
2.2.2 尖峰噪声滤除方法
尖峰噪声是由传感器本身的技术问题引起的，它具有幅值大、持续时间短等特点。本文根据限幅滤波的思想来滤除尖峰噪声。已知加速度传感器的量程为±2g(g=9.8 m/s2)，假设相邻2次采样值为an-1和an，如果an的绝对值超过了2g，则令an等于前一时刻的值an-1；如果an的绝对值小于或等于2g，则不对an进行处理[12]。
(2)
式中an为n时刻的加速度。
2.2.3 刻度因数计算方法
刻度因数是指加速度传感器输出量与输入量之比。由于MEMS加速度传感器的折叠梁加工尺寸不一致和微机械结构质心偏离平衡位置等原因[13]，刻度因数为未知的常数。本文根据12位置重力翻滚法的思想，采用翻滚法来求取刻度因数。
以Y轴为例，将WT931模块置于水平台上，将Y轴与水平台的夹角依次调整至90°和-90°，待数据稳定后分别采集2 000个Y轴加速度数据并求取均值aY1和aY2，根据均值可以求出刻度因数，计算公式为
KY=(|aY1|+|aY2|)/2g
(3)
式中KY为Y轴的刻度因数。
3 重力分量滤除方法
惯性模块WT931中的加速度传感器既能检测到静态加速度(重力加速度)，又能检测到动态加速度(运动加速度)[14]。当单轨吊机车在斜坡上运动时，重力加速度(即重力分量)的存在会对运动加速度产生干扰，这会严重影响单轨吊机车的定位精度。因此，本文采用方向余弦矩阵法滤除重力分量。
设方向余弦矩阵为C，C为3阶方阵：
(4)
设机车所在的坐标系为载体坐标系OXbYbZb(b系)，用于定位的坐标系为导航坐标系OXnYnZn(n系)，导航坐标系的方向为东北天，分别对应XYZ轴[15-16]。从导航坐标系到载体坐标系可视为3个有序的旋转过程，每个旋转过程用绕单轴旋转的余弦矩阵来表示[17]。
绕X轴旋转α(俯仰角)得到的余弦矩阵为
(5)
绕Y轴旋转γ(横滚角)得到的余弦矩阵为
(6)
绕Z轴旋转θ(航向角)得到的余弦矩阵为
(7)
假定从导航坐标系到载体坐标系依次绕Z,X,Y轴旋转，相应的姿态矩阵为
(8)
由此可得出重力加速度在载体坐标系各轴上的分量为
(9)
用采集到的加速度值减去重力加速度在载体坐标系各轴上的分量gx,gy,gz，即可滤除重力分量。
4 稳态误差修正算法与测距算法
稳态误差修正算法包括稳态加速度误差修正算法和稳态速度误差修正算法，测距算法包括Lagrange插值多项式测距算法和测距结果定时修正算法，这2部分算法对定位精度起决定性作用。
4.1 稳态误差修正算法
4.1.1 稳态加速度误差修正算法
由于MEMS加速度传感器的精度有限，当其静止或匀速运动时的加速度值总在0附近小幅波动，即存在稳态加速度误差。通过分析数据发现，传感器处于静止与变速运动时的加速度变化率幅值有较大差异，因此，本文提出了加速度变化率阈值法，用以修正此误差。
设a1，a2，…，a10为连续10个加速度值，u为这10个数据的均值，T为采样周期。以10个数据为1组，用组内相邻2个数据求出1个加速度变化率并取绝对值，由此可得：b1=|(a2-a1)|/T，b2=|(a3-a2)|/T，…，b9=|(a10-a9)|/T。用d表示9个数据中大于w(w为经验值，此处取0.1 m/s3)的个数，d最大为9。如果d<8且|u|<0.25 m/s2，则将这10个数据置0；否则，对其不做任何处理[18]。
加速度修正前后曲线比较如图2所示。从图2可看出,在15～20 s，加速度修正前,机车几乎是减速运动，而此时机车实际是近似匀速运动；修正后的加速度始终为0，修正了稳态加速度误差。
图2 加速度修正前后曲线比较
Fig.2 Comparison of curves before and after acceleration correction
4.1.2 稳态速度误差修正算法
机车的速度由加速度积分得来，因此，机车在行驶过程中的加速度误差会导致其停车时的速度不为0[19]，即存在稳态速度误差。根据停车时的特点，本文提出零速修正法修正稳态速度误差。
设an为n时刻的加速度，vn为n时刻的速度，n=0,1,…，vco为一个速度常数(此处取0.005 m/s)。如果vn≤vco且an，an-1，an-2均小于0，则令vn=0，否则不对vn做处理[20]。
机车运行时要经历静止、变速运动、静止3个阶段。无零速修正与有零速修正时的速度变化曲线分别如图3、图4所示。
图3 无零速修正时的速度变化曲线
Fig.3 Velocity variation curves without zero speed correction
图4 有零速修正时的速度变化曲线
Fig.4 Velocity variation curves with zero speed correction
由图3和图4可知，无零速修正时，停车时的速度为负值，无法正确反映机车的运动状态；有零速修正时，停车时的速度为0，可正确反映机车的运动状态。
4.2 测距算法
4.2.1 Lagrange插值多项式测距算法
测距算法是实现准确定位的核心，它的优劣将直接导致累计误差的产生，因此，本文基于Lagrange插值多项式测距算法[21]，提出了含有4个插值节点的Lagrange插值多项式测距算法。
设4个插值节点分别为(x0,y0),(x1,y1),(x2,y2),(x3,y3)，对应的插值多项式如下：
f(x)=
y0+
(10)
在计算机车速度时，相应插值节点分别为(0,a0),(T,a1),(2T,a2),(3T,a3)，a0—a3为按时间顺序采集到的4个加速度，a3为当前时刻的加速度。将这4个插值节点代入f(x)中，得到函数A(x)，对A(x)进行积分即可计算出机车速度。
vno=vla+
A(x)dx
(11)
式中：vno为当前时刻机车速度；vla为上一时刻机车速度。
在计算机车行驶距离时，相应的插值节点分别为(0,v0),(T,v1),(2T,v2),(3T,v3)，v0—v3为按时间顺序求得的4个速度，v3为当前时刻的速度。将这4个插值节点代入f(x)中，得到函数V(x)。对V(x)进行积分即可计算出机车的行驶距离。
hno=hla+V(x)dx
(12)
式中：hno为当前时刻的行驶距离；hla为上一时刻的行驶距离。
4.2.2 测距结果定时修正算法
为了提高测距精度，防止测距时出现过大的误差，提出测距结果定时修正法来修正测距误差。因为单轨吊机车的行驶速度通常为1～2 m/s，所以可通过实验数据对其在给定时间内的测距结果进行估计。如果给定时间内的测距结果与其估计值的差的绝对值大于阈值hth(hth为常数，此处取0.8 m)，则可将测距结果修正为估计值,否则保持原测距结果。
(13)
式中：ΔH为给定时间内的测距结果；Δhes为给定时间内的测距结果估计值。
5 测距实验
为了验证定位算法的准确性，以WT931模块和汽车为实验平台，将WT931模块安装于汽车上用于采集运行数据，以汽车在平坦路面的行驶来模拟单轨吊机车在平直轨道上的行驶。
5.1 实验方案
由于单轨吊机车的最大行驶速度约为2 m/s，所以，本实验将汽车的最高速度控制在2 m/s左右；加速度传感器采样频率设为200 Hz；行驶路线为30.8 m的直线路段，汽车经历静止、变速运动、静止3个阶段。实验现场如图5所示。反复实验10次，实时采集加速度数据,利用Matlab软件计算测距结果。
图5 实验现场
Fig.5 Experimental site
5.2 测距结果及分析
将10次实验测距结果进行统计并计算出相应的测距误差，结果见表1。由表1可知，有4次测距误差范围为0.1～1.3 m；4次测距误差范围为12.3～16.7 m；有2次测距误差分别为3.90 m和8.60 m，误差波动范围较大。
表1 测距结果
Table 1 Ranging results
实验次数测距结果/m误差/m实验次数测距结果/m误差/m131.220.42645.1014.30218.0012.80734.703.90347.5016.70818.5012.30439.408.60930.700.10531.540.741029.501.30
5.3 测距结果定时修正
为了提高测距精度，采用测距结果定时修正法修正测距误差。通过分析大量实验数据发现，当给定时间为27.5 s时,机车行驶距离约为30 m,因此设Δhes=30 m,以此为基础，应用测距结果定时修正法对表1中的测距结果进行修正，结果见表2。
表2 修正后的测距结果
Table 2 Corrected ranging results
实验次数测距结果/m误差/m实验次数测距结果/m误差/m1--630.970.172--730.240.56330.190.61830.250.55435.264.46930.700.10530.150.651030.300.50
由于表2中前2次实验测距时间小于27.5 s，所以，不对其进行修正。第4次测距误差较大,为4.46 m，这是由汽车急停导致的。其余7次测距误差范围为0.10～0.65 m。与现有的定位方法相比，该算法不仅有较高的定位精度，而且定位成本较低。
6 结论
(1)利用加速度变化率阈值法对稳态加速度误差进行修正，提高了加速度数据的准确性。
(2)测距结果定时修正法可以补偿大量测距误差，提高测距精度与定位精度。
(3)实验结果表明，当汽车行程为30.8 m时，测距误差基本在0.65 m以内，占总行程的2%，定位精度较高。
(4)算法未对机车急停时带来的加速度误差进行分析,在后续工作中，将着重对其进行分析与研究。
[1] 蒋星星，李春香.2013—2017年全国煤矿事故统计分析及对策[J].煤炭工程，2019，51(1)：101-105.
JIANG Xingxing，LI Chunxiang.Statistical analysis on coal mine accidents in China from 2013 to 2017 and discussion on the countermeasures[J].Coal Engineering，2019，51(1)：101-105.
[2] 杨勇，王方杰，周思维.基于RFID和ZigBee技术的矿井机车定位系统设计[J].煤炭技术，2017，36(1)：245-247.
YANG Yong，WANG Fangjie，ZHOU Siwei.Design of mine locomotive positioning system based on RFID and ZigBee technology[J].Coal Technology，2017，36(1)：245-247.
[3] 何学文,曹清梅,樊宽刚.基于ZigBee的钨矿井下矿车定位监测系统研究与设计[J].金属矿山，2015，44(2)：127-132.
HE Xuewen，CAO Qingmei，FAN Kuangang.Research and design of underground tramcar positioning and monitoring system in tungsten mine based on ZigBee[J].Metal Mine，2015，44(2)：127-132.
[4] 朱泽朋，王凯，岳雨俭.基于RSSI的矿山井下机车定位算法的改进[J].计算机与数字工程，2015，43(4)：554-557.
ZHU Zepeng，WANG Kai，YUE Yujian.Improved location algorithm of underground mine locomotive based on RSSI[J].Computer & Digital Engineering，2015，43(4)：554-557.
[5] 张麦玲，吴延昌.基于超宽带的矿井机车定位系统设计[J].煤矿机械，2015，36(8)：21-23.
ZHANG Mailing，WU Yanchang.Design of mine locomotive positioning system based on ultra-wideband technology[J].Coal Mine Machinery，2015，36(8)：21-23.
[6] 王玮，吕攀，王琪，等.惯性导航下煤矿移动车辆动态定位研究[J].煤炭工程，2016，48(10)：138-141.
WANG Wei，LYU Pan，WANG Qi，et al.Research on dynamic positioning of mobile vehicle in underground coal mine based on inertial navigation[J].Coal Engineering，2016，48(10)：138-141.
[7] CHANG Lubin，LI Jingshu，CHEN Shengyong.Initial alignment by attitude estimation for strapdown inertial navigation systems[J].IEEE Transactions on Instrumentation and Measurement，2015，64(3)：784-794.
[8] 程向红，赵莹，田芸.一种自适应H∞滤波的运动学约束惯性导航方法[J].中国惯性技术学报，2019，27(3)：295-300.
CHENG Xianghong，ZHAO Ying，TIAN Yun.An adaptive H∞ filtering approach for inertial navigation with motion constrained[J].Journal of Chinese Inertial Technology，2019，27(3)：295-300.
[9] 燕斌.MEMS加速度传感器在矿用随钻轨迹测量系统中的误差校正[J].煤田地质与勘探，2017，45(4)：144-148.
YAN Bin.Error correction of MEMS acceleration sensor used for trajectory measurement while drilling system[J].Coal Geology & Exploration，2017，45(4)：144-148.
[10] 于学鑫.基于MPU6050的微小型无人机AHRS设计研究[D].呼和浩特：内蒙古工业大学，2016.
YU Xuexin.Research on design of MUAV's AHRS based on MPU6050[D].Hohhot：Inner Mongolia University of Technology，2016.
[11] 韩盈党，李哲.MEMS加速度传感器的数据采集和预处理[J].仪表技术与传感器，2015(2)：16-19.
HAN Yingdang，LI Zhe.Data acquisition and pre-processing based on MEMS accelerometer[J].Instrument Technique and Sensor，2015(2)：16-19.
[12] 刘琪芳.单片机测控系统的数字滤波算法研究[J].机械工程与自动化，2011(3)：165-166.
LIU Qifang.Algorithms of digital filter in SCM control system[J].Mechanical Engineering & Automation，2011(3)：165-166.
[13] 杨庆江，李国庆.低精度MEMS IMU的标定方法及误差分析[J].自动化与仪器仪表，2018(10)：153-157.
YANG Qingjiang，LI Guoqing.MEMS IMU error analysis and calibration method[J].Automation & Instrumentation，2018(10)：153-157.
[14] 都悦来，史丽萍，李桂英，等.基于加速度计和弯曲传感器的手指运动姿态监测[J].纳米技术与精密工程，2017，15(1)：61-66.
DU Yuelai，SHI Liping，LI Guiying，et al.Finger motion monitoring based on accelerometer and flex sensor[J].Nanotechnology and Precision Engineering，2017，15(1)：61-66.
[15] 李森.基于惯性导航的工作面直线度测控与定位技术[J].煤炭科学技术，2019，47(8)：169-174.
LI Sen.Measurement & control and localisation for fully-mechanized working face alignment based on inertial navigation[J].Coal Science and Technology，2019，47(8)：169-174.
[16] 郝尚清，李昂，王世博，等.采煤机惯性导航安装偏差对定位误差的影响[J].煤炭学报，2015，40(8)：1963-1968.
HAO Shangqing，LI Ang，WANG Shibo，et al.Effects of shearer inertial navigation installation noncoincidence on shearer positioning error[J].Journal of China Coal Society，2015，40(8)：1963-1968.
[17] 王嘉骏，杨录，张元军.基于MPU-6050运动传感器的惯性导航定位系统[J].科学技术与工程，2017，17(18)：253-257.
WANG Jiajun，YANG Lu，ZHANG Yuanjun.Inertial navigation and positioning system based on MPU-6050 motion sensor[J].Science Technology and Engineering，2017，17(18)：253-257.
[18] ZHANG Ruixiao，LIU Xiaodong，QU Zhiyi.The research of distance measurement method based on mobile phone MEMS accelerometer[C]//International Conference on Intelligent Human-machine Systems & Cybernetics，2016.
[19] 张健敏，修春娣，杨威，等.一种多运动模式下自适应阈值零速修正算法[J].北京航空航天大学学报，2018，44(3)：636-644.
ZHANG Jianmin，XIU Chundi，YANG Wei，et al.Adaptive threshold zero-velocity update algorithm under multi-movement patterns[J].Journal of Beijing University of Aeronautics and Astronautics，2018，44(3)：636-644.
[20] 徐旭，何昆鹏，张琳.在水池中精确测量潜器模型运动参数方法[J].哈尔滨工程大学学报，2020，41(7)：1010-1015.
XU Xu，HE Kunpeng，ZHANG Lin.Method for accurately measuring motion parameters of underwater vehicle model in a pool[J].Journal of Harbin Engineering University，2020，41(7)：1010-1015.
[21] 王剑，王璋奇.振动加速度数值积分的Lagrange多项式拟合方法[J].噪声与振动控制，2015，35(6)：191-196.
WANG Jian，WANG Zhangqi.Lagrange polynomial fitting method for numerical integration of the vibration acceleration[J].Noise and Vibration Control，2015，35(6)：191-196.