0 引言
为了提高铁矿井下作业效率及保障人员安全，井下常采用无人驾驶列车实现矿物运输[1-2]。当前井下无人驾驶定位技术主要采用激光发射器绘制车辆周围的3D地形图，在车轮上安装光电传感器来测量其转速，进而计算列车的位移，结合自动电控匝道判断当前列车的行驶方向，实现对无人驾驶列车的位置判断[3-5]。由于井下运输轨道常常积水，列车行驶在潮湿的轨道上时经常发生打滑现象，此时根据光电传感器推算的位移会产生误差[6]。因此，本文在现有井下无人驾驶定位技术基础上，添加惯性导航模块，结合光电传感器数据和惯导数据来识别列车启动打滑、制动抱死等状态，提高了无人驾驶列车的安全系数。由于惯性导航模块采用了扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法[7-9]，存在累积误差，不适合长时间的参数统计。所以，本文在传统的EKF融合算法基础上，引入权值反馈约束算法，避免由线性化导致的累积误差[10]。同时，在井下巷道每个岔路口设置位置信标，对定位信息进行校准，进一步提高定位精度[11-12]。
1 井下无人驾驶系统
井下无人驾驶列车惯性导航定位系统是井下无人驾驶系统的一部分。井下无人驾驶系统整体结构如图1所示。其中，惯性导航定位系统包括惯性导航模块、PNP光电传感器模块、数据处理模块、无线模块和主控服务器，实现对列车的加速度、角度、姿态等参数的采集和解算，并初步判断列车当前的运行状态。后台控制系统主要根据惯性导航系统采集的数据实现位置信息显示、路况决策判断及任务分配调度，从而实现对井下列车的指挥及任务分配。
2 惯性导航定位系统设计
2.1 系统硬件设计
惯性导航模块采用LPMS-NAV2航向传感器，其内嵌了高精度的单轴陀螺仪和三轴加速度计，是一款基于微机电系统(Micro-Electro-Mechanical System,MEMS)传感器的低成本小型航向传感器。LPMS-NAV2通过数据流模式实现与主控服务器的通信，将三轴加速度、三轴角速度及航向角信息以设定好的频率(100 Hz)发送到主控服务器。
图1 井下无人驾驶系统整体结构
Fig.1 Whole structure of underground driverless system
主控服务器采用基于ARM架构的32 bit Cortex-M3内核的处理芯片。为了识别列车启动打滑、制动抱死等状态，在惯性导航定位系统中引入PNP型光电传感器，结合车轮转速和惯导数据，采用双阈值算法实现对列车行驶状态的准确判断。
2.2 系统软件设计
惯性导航定位系统软件包括1个外部中断、2个定时器中断和2个UART通信串口。首先初始化各个模块并配置相关参数，通过2个定时器检测脉冲信号的频率、幅值，实现对车轮转速的脉冲计数；通过串口UART2实现主控服务器与LPMS-NAV2的数据通信；利用UART1将计算合成的位置信息通过井下监控基站发送至后台主控服务器，实现惯性导航模块与主控服务器的双向通信。系统通信流程如图2所示。
针对轨道有雨水时列车易发生打滑或抱死现象的问题，采用双阈值算法判断列车行驶异常状况，双阈值算法流程如图3所示。
3 惯性导航定位技术实现
惯性导航定位系统利用目标载体的加速度数据，通过积分计算出目标载体的速度、位移，再结合惯性导航传感器的航向角求出目标载体的位置坐标；在此基础上，通过权值反馈约束算法来提高系统的测量精度，同时利用井下位置信标(间隔100 m)进行二次校准，减小累积误差。
图2 惯性导航定位系统通信流程
Fig.2 Communication flow of inertial navigation and positioning system
图3 双阈值算法流程
Fig.3 Flow of double threshold algorithm
3.1 惯性导航定位
在实际运动过程中，用惯性导航模块测量加速度时受到重力加速度的影响，本文采用z轴加速度补偿的方法来消除重力加速度引入的误差。惯性导航模块加速度分布如图4所示。目标载体在初始原点S0，ax为沿x轴加速度，az为沿z轴加速度，ag为重力加速度，a为沿目标前进方向的实际运动加速度，α、β分别为z轴、x轴与重力加速度方向的夹角。目标载体的实际运动加速度计算公式为
图4 惯性导航模块加速度分布
Fig.4 Acceleration distribution of inertial navigation module
(1)
已知系统采样频率f为100 Hz，设初始时刻目标载体位于原点S0(0,0)，初始速度v0=0,初始加速度a0=0。系统采集加速度、速度、方位角等数据，并以2个点的数据为1组存储于数组中；对数组中相邻2组数据，根据牛顿运动定律进行积分处理，求出目标载体在第n个点处的惯性运动速度vn和位移sn：
(2)
式中an为目标载体在第n个点处的实际运动加速度。
根据LPMS-NAV2测得的航向角，用式(3)求出目标载体的位置坐标：
(3)
式中：x，y为目标载体的真实位置坐标；xbuf，ybuf为坐标缓冲；s为当前时刻测量的位移；sbuf为位移缓冲；γ为航向角。
3.2 权值反馈约束算法
权值反馈约束算法的基本思想是在传统卡尔曼滤波融合算法上，将已知的信标坐标作为训练集，通过构建平方差损失函数对系统定位点进行权值约束，以降低累积误差，进一步提高系统性能[13-14]。
平方差损失函数Cx,Cy的计算公式为
(4)
式中dx，dy为信标坐标值。
通过比较平方差损失函数与设定阈值的大小来修正权值，初始权值为随机产生。当Cx>0.5且大于上一时刻Cx值时，x轴的权值Wx减小1个步进(设定每个步进为0.1)；当Cx>0.5且小于上一时刻Cx值时，Wx增大1个步进。同理可得y轴的权值Wy。经过反复训练，直至将损失函数值均控制在0.5以内，输出当前权值作为测试轨道上的最优权值参数。通常井下巷道有多条机车运行轨道，每条运行轨道都应该使用不同的权值参数来约束惯性导航模块输出的坐标值。
利用最优权值来修正目标载体的坐标值：
(5)
式中：x′，y′为引入权值后得到的坐标值；σ为系统融合函数；bx，by为正则项常数。
3.3 信标二次校准
利用已知坐标的信标，对系统引入权值后得到的坐标值进行二次校准。无人驾驶系统控制终端根据信标发送过来的实际位置坐标，以不超过500 ms的延迟通过地下专网发送至惯性导航定位系统，并将该位置坐标作为当前时刻的起始坐标值，实现二次校准。
4 测试分析
在带有减震功能的全向移动实验车上进行了系统性能测试，测试环境如图5所示。测试过程中实验车以不同的速度进行直线运动、S型运动及任意角度运动，每次测试总位移约为500 m，记录10组测试数据。
图5 惯性导航定位系统测试环境
Fig.5 Test environment of inertial navigation and positioning system
采集实验车的加速度、角速度、航向角度、脉冲数等惯性原始数据，分别用EKF融合算法、权值反馈约束算法、权值反馈约束+信标二次校准算法得到其运动轨迹，如图6所示。
由图6可看出，传统EKF融合算法因具有累积误差，位置坐标推算不稳定。权值反馈约束算法的误差比传统EKF融合算法有所减小。添加信标二次校准后，实验车的轨迹基本接近实际运动轨迹，在旋转、加速等波动性比较大的情况下，最大误差也不超过0.98 m。对10组数据进行统计分析，计算采用不同算法时的最大误差及平均误差，结果见表1。
图6 实验车运动轨迹
Fig.6 Trajectory of experimental vehicle
表1 不同算法定位误差
Table 1 Positioning error of different algorithms m
由表1可知，权值反馈修正算法有效抑制了惯性导航模块自身的累积误差，添加信标二次校准后，系统平均误差减小到0.52 m，满足井下巷道无人驾驶列车的定位精度要求。
5 结论
(1) 井下无人驾驶列车惯性导航定位系统采用LPMS-NAV2获取目标载体的加速度、航向角，根据航向角计算目标载体的位置坐标，采用z轴加速度补偿方法来消除重力加速度引入的误差。
(2) 结合光电传感器数据和惯导数据，采用双阈值算法判断列车行驶异常状况，提高了无人驾驶列车的安全系数。
(3) 通过权值反馈约束算法对定位信息进行修正，以避免定位累积误差。通过信标二次校准进一步提高定位精度。
(4) 该系统充分利用了当前地理信息、运动载体固有信息、异常情况辅助信息，实现了对井下目标载体位置的准确估算。室内测试结果表明，井下无人驾驶列车惯性导航定位系统的平均定位误差为0.52 m，满足井下巷道无人驾驶列车的定位精度要求。
参考文献(References)：
[1] 端木庆玲,阮界望,马钧.无人驾驶汽车的先进技术与发展[J].农业装备与车辆工程,2014,52(3):30-33.
DUANMU Qingling,RUAN Jiewang,MA Jun.Development and advanced technology of driverless car[J].Agricultural Equipment & Vehicle Engineering,2014,52(3):30-33.
[2] 李论.基于RSSI的煤矿巷道高精度定位算法研究[D].徐州：中国矿业大学,2015.
[3] 乔钢柱,曾建潮.信标节点链式部署的井下无线传感器网络定位算法[J].煤炭学报,2010,35(7):1229-1233.
QIAO Gangzhu,ZENG Jianchao.Localization algorithm of beacon nodes chain deployment based on coal mine underground wireless sensor networks[J].Journal of China Coal Society,2010,35(7):1229-1233.
[4] 裴忠民,邓志东,巫天华,等.矿井无线传感器网络三阶段定位方法[J].中国矿业大学学报,2010,39(1):87-92.
PEI Zhongmin,DENG Zhidong,WU Tianhua,et al.A novel three-phase localization method in coal mine wireless sensor networks[J].Journal of China University of Mining & Technology,2010,39(1):87-92.
[5] 孙继平,王帅.基于信号强度的改进型井下测距算法的研究[J].煤炭学报,2013,38(11):2072-2076.
SUN Jiping,WANG Shuai.Study on the improved ranging algorithm based on signal strength in the mine[J].Journal of China Coal Society,2013,38(11):2072-2076.
[6] 吴静然,崔冉,赵志凯,等.矿井人员融合定位系统[J].工矿自动化,2018,44(4):74-79.
WU Jingran,CUI Ran,ZHAO Zhikai,et al.Mine personnel fusion location system[J].Industry and Mine Automation,2018,44(4):74-79.
[7] 王铨.基于MEMS惯性传感器的井下人员定位系统[D].徐州:中国矿业大学,2017.
[8] 尚俊娜,刘参,施浒立,等.IMU辅助的室内单星定位方法[J].中国惯性技术学报,2017,25(5):636-642.
SHANG Junna,LIU Can,SHI Huli,et al.Indoor single-star positioning method based on IMU assisting[J].Journal of Chinese Inertial Technology,2017,25(5):636-642.
[9] 原野.室内WiFi指纹定位系统中的半监督训练方法研究[D].上海:上海交通大学,2015.
[10] YANG Z,LIU Y.Quality of trilateration:confidence-based iterative localization[J].IEEE Transactions on Parallel & Distributed Systems,2009,21:631-640.
[11] 李博文,姚丹亚.低成本车载MEMS惯导导航定位方法[J].中国惯性技术学报,2014,22(6):719-723.
LI Bowen,YAO Danya.Low-cost MEMS IMU navigation positioning method for land vehicle[J].Journal of Chinese Inertial Technology,2014,22(6):719-723.
[12] ZHONG X,RABIEE R,YAN Y,et al.A particle filter for vehicle tracking with lane level accuracy under GNSS-denied environments[C]//IEEE 20th International Conference on Intelligent Transportation Systems,2017.
[13] 路永乐,刘洪志,郭俊启,等.基于腰部MEMS-IMU的室内行人导航航向反馈修正算法[J].中国惯性技术学报,2017,25(6):725-730.
LU Yongle,LIU Hongzhi,GUO Junqi,et al.Heading feedback correction algorithm for indoor pedestrian navigation based on waist MEMS-IMU[J].Journal of Chinese Inertial Technology,2017,25(6):725-730.
[14] 邓子豪.无人驾驶智能车导航系统的研究与实现[D].西安:西安工业大学,2014.