0 引言
目前，井下人员定位系统主要采用RFID、ZigBee、WiFi等技术实现定位[1-2]。RFID技术传输距离较短，信号在基站之间传播时容易相互干扰，造成定位精度下降。基于ZigBee技术的定位系统能够定位的区域较小，必须大规模布置传感器才能获得满意的定位效果。基于WiFi技术的定位系统则要求布置数量较多的传感器，通过采集大量的定位数据计算目标位置，经济和时间成本过高。
近年来，微机械电子系统(Micro Electro Mechanical System，MEMS)传感器逐渐被用于人员定位中[3]。MEMS传感器由加速度计、磁力计和陀螺仪3种功能不同的小运动传感器组成，它能够实时捕获并输出行人的各种姿态信息，通过步态估计、步长估计和方向角估计计算出行人的坐标，可靠性强。为此，笔者利用MEMS传感器设计了煤矿井下人员定位系统，该系统通过井下已有的WiFi 基站，辅以行人航迹推算(Pedestrian Dead Reckoning，PDR)[4]算法实现井下人员的精确定位。该系统利用融合了行走频率和加速度方差的表达式来确定步长，并根据四元数法计算行人方向角，同时利用扩展卡尔曼滤波(Extended Kalman Filter，EKF)[5]修正加速度计、磁力计漂移带来的数据测量误差，以提高定位精度。
1 系统设计
基于MEMS传感器的煤矿井下人员定位系统由地下信息采集系统和地面显示系统2个部分组成。在地下信息采集系统中，传感器定位终端将与人体姿态相关的数据信息传送到地面显示系统，通过计算分析，就能够在地面显示系统界面上显示出井下人员的位置及运动轨迹。定位终端模块主要由CC2530主控芯片、MPU9150惯性传感器、WiFi通信模块和电源模块等部分构成,如图1所示。MPU9150惯性传感器用于获取携带者的步长、步态和运动方向角等信息；CC2530主控芯片用于实现对MPU9150惯性传感器数据的采集、传输控制。WiFi通信模块采用EMW3162模块，系统以WiFi通信方式传输惯性传感器的数据。
系统定位流程主要包括获取初始位置、数据采集、位置估计、结果显示等，如图2所示。
2 PDR定位算法
PDR定位算法主要由步态估计、步长估计、方向角估计以及位置计算等部分组成。
图1 定位终端模块结构
Fig.1 Modular structure of positioning terminal
图2 系统定位流程
Fig.2 Positioning flow of the system
2.1 步态估计
常用的步态估计算法有过零点检测法、峰值检测法、平滑区域检测法等，本文将峰值检测法和零点检测法相结合来估计步态。在计算过程中，忽略加速度计的方向对估计结果的影响，采用三轴总加速度对步态进行估计，三轴总加速度限定在[0，2g](g为重力加速度)范围内[7]。步态估计具体步骤：① 初始化，获取三轴加速度数据。② 根据滑动窗口消除重力加速度的干扰。③ 进行峰值检测和零点检测。④ 若检测到的峰值大于给定的峰值阈值，并且2个邻居峰值间相隔的时间大于给定的时间阈值，则认为该峰值有效。⑤ 检测跨步结束点并计算步态。
2.2 步长估计
经过测试发现，步长可以用行走频率和加速度方差间的融合表达式来确定[7]。行走频率可表示为
式中：fW为行走频率；tk，tk-1分别为现在和前一步的时间点。
加速度方差可以表示为
式中：AV为加速度方差；p为行走1步的过程包含的样本数；ak为行走第k步时的加速度；
为行走1步的过程加速度平均值。
当行人的速度加快时，行走频率就会增大，则步长也会随之增大；同理，如果行人的速度加快，则加速度的方差也会随之增大。在实际测量中，难免会出现误差，此时，步长估计的表达式为
Swf=awffW+bwf=S+vwf
(3)
Sav=aavAV+bav=S+vav
(4)
式中：Swf为根据行走频率fW估算出的步长值；Sav为根据加速度方差AV估算出的步长值；S为实际步长值；vwf，vav分别为fW和AV的测量误差；awf，bwf，aav，bav均为估计参数。
根据以上分析，可以用式(5)所示的拟合表达式来表示步长估计值：
S′=kwfSwf+kavSav=kwf(awffW+bwf)+
kav(aavAV+bav)=(kwfawf)fW+(kavaav)AV+
(kwfbwf+kavbav)=αfW+βAV+γ
(5)
式中：kwf为Swf的拟合系数；kav为Sav的拟合系数；α，β，γ为步长估计参数。
2.3 方向角估计
2.3.1 基于四元数的方向角估计
本文利用四元数来计算行人运动的姿态矩阵及方向角[8]。定义姿态四元数的运动方程为
式中：
为姿态矩阵;Q为四元数向量，Q=q0+q1i+q2j+q3l，q0,q1,q2,q3为实数,i,j,l为虚数单位；W为陀螺仪测量的角速度初始值,W=w0+w1i+w2j+w3l;t0为行人开始运动的时间点；Q0为四元数向量初始值。
基于四元数的矩阵表示形式为
式中：w1为滚转角速度；w2为俯仰角速度;w3>为航向角速度;Ω(w)为测量坐标系中的角速度。
假设角速度在单个采样周期内为常量，利用求解差分算法对式(7)进行处理后，可得在离散域上的四元数公式为
(8)
式中:I为单位矩阵;ΔT为采样时间间隔;Δθ为ΔT内的角位移,
2.3.2 基于扩展卡尔曼滤波的方向角修正
由于井下环境复杂，在方向角的测量过程中，陀螺仪容易出现漂移现象，从而导致行人在转弯时方向角出现较大偏差。为了减小方向角偏差对定位精度的影响，利用扩展卡尔曼滤波对传感器数据进行处理，以提高方向角的测量精度。扩展卡尔曼滤波模型[9]为
式中:Xk+1为k+1时的状态向量；ok+1为状态转移函数；Uk为k时的平稳噪声；Zk+1为k+1时的测量向量；hk+1为观测函数；Vk+1为k+1时的观测噪声。
根据离散时间模型更新姿态角，利用旋转四元数表示状态向量，建立式(10)所示的状态方程：
Qk+1=FQk+ωk
(10)
式中：F为状态转移矩阵，
声。
观测方程由加速度计、磁力计的测量数据共同组成，表达式为
式中：ak+1,mk+1分别为加速度计、磁力计的测量数据；Tbn(Qk+1)为四元数旋转矩阵；g和h分别为归一化重力向量和磁场强度向量；avk+1和mvk+1分别为加速度计和磁力计的数据测量噪声，它们是均值为零且不相关的白噪声。
由观测方程可知，状态向量和测量向量存在非线性关系，可以将式(11)线性化，从而得到关系矩阵[10]。
2.4 位置计算
由以上计算得到了行人行走的步长和方向角，再结合初始位置坐标，则可得到任意时刻的地理位置坐标：
(12)
式中：(xk，yk)为第k步时的行人坐标；φ为第k-1步时的方向角。
3 实验及分析
为了测试定位系统的可靠性，在某煤矿巷道进行实验测试。实验员运动路线如图3所示。实验开始时，在实验员的腰部绑定定位终端，实验员从点A处开始行走，沿着巷道，依次经过点B、点C，直到到达终点D，其中，A，B间的距离为51 m，B，C间的距离为30 m，C，D间的距离为19 m，整个行走距离为100 m，图中虚线为设定路线。
3.1 步态测试
记录不同实验员按照任意方向实际行走的步数以及与之对应的测试数据，并计算出估计行走步数。步数测试结果见表1，由表1可知，4组测试的步数准确率均在97.1%以上。
图3 实验员运动路线
Fig.3 Tester moving route
表1 步数测试结果
Table 1 Results of step number test
3.2 步长测试
实验员沿着某一方向行走40步，记录下每一步的实际行走距离以及与之对应的测试数据，并计算出估计步长。由于实验员行走时的迈步时间和加速度各不相同，则每步的估计步长也不同。步长测试结果见表2。经过多组数据对比可知，计算步长的准确率可达97.3%。
表2 步长测试结果
Table 2 Results of step length test
3.3 方向角测试
经EKF算法校正后得到的行人行走方向角如图4所示，其中，真实方向角是提前确定的。从图4可知，计算得出的方向角和真实方向角基本重合，这表明利用PDR算法计算出的方向角精度较高。
3.4 定位效果分析
系统的定位效果如图5所示。由图5可知，当实验员在A，B之间行走时，实际行走路线和测试路线重合度较好。由于在B处,实验员的行走方向发生了改变，此时陀螺仪的角度测量数据也会发生漂移误差，导致测试路线和实际行走路线之间出现偏差，并且每次行走方向改变时，都会发生偏差，但由于本文使用EKF算法对方向角的测量值进行了校正，所以，偏差会被降低到最小，以满足井下巷道定位的需求。通过多次测量实验得出，定位系统在100 m行走距离内的定位误差小于2.2 m，能够实现煤矿井下人员的高精度定位。
图4 方向角测试结果
Fig.4 Testing results of direction angle
图5 系统定位效果
Fig.5 Positioning results of the system
4 结语
基于MEMS传感器的煤矿井下人员定位系统以MPU9150传感器、CC2530控制器为主要构成模块，采用峰值检测法和零点检测法相结合的方式实现步态估计，采用融合了行走频率和加速度方差的表达式计算步长，利用四元数法和扩展卡尔曼滤波得到较为精确的方向角估计，最终获得井下人员的具体位置。实验结果表明，该系统定位精度高，100 m距离内定位误差小于2.2 m，为矿难发生时救援工作的开展提供了有力保障。
参考文献(References)：
[1] 蒋磊,杨柳铭,武方达,等.基于GMapping算法与指纹地图构建的井下定位方法[J].工矿自动化,2017,43(9):96-101.
JIANG Lei,YANG Liuming,WU Fangda,et al.Underground positioning method based on GMapping algorithm and fingerprint map construction [J].Industry and Mine Automation,2017,43(9):96-101.
[2] 谢晓佳,程丽君,王勇.基于ZigBee网络平台的井下人员跟踪定位系统[J].煤炭学报,2007,32(8):884-888.
XIE Xiaojia,CHENG Lijun,WANG Yong.The tracking and locating system of the mining personnel based on the ZigBee wireless network platform [J].Journal of China Coal Society,2007,32(8):884-888.
[3] PARK C G,JEON H C,KIM H J,et al.Design of a base station for MEMS CCR localization in an optical sensor network.[J].Sensors,2014,14(5):8313-8329.
[4] 周瑞,李志强,罗磊.基于粒子滤波的WiFi行人航位推算融合室内定位[J].计算机应用,2016,36(5):1188-1191.
ZHOU Rui,LI Zhiqiang,LUO Lei.WiFi-pedestrian dead reckoning fused indoor positioning based on particle filtering [J].Journal of Computer Applications,2016,36(5):1188-1191.
[5] 陈旭光,杨平,陈意.MEMS陀螺仪零位误差分析与处理[J].传感技术学报,2012,25(5):628-632.
CHEN Xuguang,YANG Ping,CHEN Yi.Analysis and processing on zero position error of MEMS gyroscope [J].Chinese Journal of Sensors and Actuators,2012,25(5):628-632.
[6] KANG W,HAN Y.Smart PDR: smartphone-based pedestrian dead reckoning for indoor localization[J].IEEE Sensors Journal,2015,15(5):2906-2916.
[7] RENAUDIN V,SUSI M,LACHAPELLE G.Step length estimation using handheld inertial sensors[J].Sensors,2012,12(7):8507.
[8] 陈孟元,谢义建,陈跃东.基于四元数改进型互补滤波的MEMS姿态解算[J].电子测量与仪器学报,2015,29(9):1391-1397.
CHEN Mengyuan,XIE Yijian,CHEN Yuedong.Attitude estimation of MEMS based on improved quaternion complementary filter[J].Journal of Electronic Measurement and Instrumentation,2015,29(9):1391-1397.
[9] 周亮,付永涛,李广军.无线定位与惯性导航结合的室内定位系统设计[J].电子技术应用,2014,40(4):73-76.
ZHOU Liang,FU Yongtao,LI Guangjun.Design of indoor positioning system based on inertial navigation and wireless location [J].Application of Electronic Technique,2014,40(4):73-76.
[10] ZHENG X,FU J.Study on PDR and RSSI based indoor localization algorithm[J].Chinese Journal of Scientific Instrument,2015,36(5):1177-1185.