多输入融合卡尔曼滤波算法在矿井定位中的优化研究*

刘 云, 房飞翔

(昆明理工大学 信息工程与自动化学院，云南 昆明 650500)

0 引 言

矿井定位对于保障矿井安全生产是一个非常重要的研究领域。由于矿井特殊的物理环境，在目前的矿井无线定位系统中，不规则的隧道和粗糙的墙壁通常会造成多径干扰[1]和非线性视距(non-line-of-sight，NLOS)误差[2]。因此，提高矿井无线定位系统的抗干扰能力显得尤为重要。

Colombo A等人[3]提出了将一个由加速度计、陀螺仪、磁力计组成的可穿戴的惯性测量单元(inertial measurement unit,IMU)融合无线定位技术,佩戴于工作人员的腰部跟踪其行动。IMU相比普通矿井无线定位系统的关键优势是其具有高度的可移植性,提供惯性数据的更新率高,以及不受信号阻塞,但仅靠IMU定位估计，由于传感器偏差的不稳定性会导致惯性漂移的积累[4]。Fourati H[5]提出了零速度更新(zero-speed update,ZUPT)通过与地面接触以纠正测速误差，但基于总步行距离，漂移仍将增长。

刘明伟等人[6]提出了低功耗蓝牙(Bluetooth low energy,BLE)技术，其中的接收信号强度指示(received signal strength indication,RSSI)可以被用来估计接收机相对发射机的距离。通过多组到固定信标节点(发射机)的距离测量值，三边测量法可以用来估计目标节点(接收机)的绝对位置，但定位精度会遭受NLOS和多径干扰的影响。Simon D等人[7]提出了IMU和BLE测量可被融合，以在一个高更新率下准确估计目标节点的无漂移位置。卡尔曼滤波是一种广泛使用的算法，用来从带有高斯白噪声的多组传感器测量值中估计一个系统的状态。但基于来自于多径和NLOS的频繁异常值,无线跟踪系统的测量噪声不能作为高斯分布进行建模。Hassani V等人[8]已经解决了该问题，通过引入自适应卡尔曼滤波器(Kalman filter.KF),如多模自适应估计(multimodel adaptive estimation,MMAE)和基于创新的自适应估计(innovation-based adaptive estimation,IAE),容纳动态条件的变化。MMAE基于有着不同过程和测量噪声矩阵的多个KF进行状态估计,但同时运行多个KF的高计算成本使得其不适合用于可穿戴设备中的低成本的微控制器。IAE适应基于新息序列的新的过程和测量噪声协方差,但其估计值会发散。

本文提出了一种多输入融合卡尔曼滤波(multiple input-fusion Kalman filtering,MIFKF)算法,在存在异常值的情况下准确估计传感器的位置。算法中，位置通过融合IMU中的外部加速度和BLE的三边测量进行估计。使用估计的翻滚角，俯仰角和偏航角，加速度测量值从移动传感器旋转到固定导航坐标系中。三边测量估计的权值基于距离测量和三边测量估计之间的余值自适应设置。定位精度将随着Rauch-Tung-Striebel (RTS)平滑器进一步提高。实验结果表明：该余值与三边测量估计误差有着很强的相关性，MIFKF算法相比标准KF在应对异常值时能够更准确地估算出位置。

1 算法模型

1.1 惯性测量预处理

MIFKF算法整体框架如图1所示，其中,预处理是通过将传感器坐标系S中的惯性测量值输入一个倾斜和偏航KF估计α(偏航角),β(俯仰角)和γ(翻滚角),即关于固定导航坐标系N中Z轴，Y轴和X轴的旋转角度。算法见文献[9]。

图1 MIFKF算法流程

1.2 三边测量估计

通过假设接收机与发射机具有全向天线和发射机具有恒定传输能量,接收功率[10,11]Pr(即RSSI)在接收机的值由式(1)决定

Pr=B-algd+χ

(1)

式中B为功率抵消常数;a为一个环境变量;d为发射机和接收机之间的距离；χ为随机噪声。在固定环境中，设a为一个常数，d根据Pr，a和B估计得到

d=10Pr-B/a

(2)

(3)

(4)

距离估计值di和到目标节点位置x的距离之间的余值可以按式(5)计算

(5)

余值ri=[ri,xri,y]T在X轴和Y轴上的距离di依次由式(6)和式(7)决定

ri=[|xi-x||y-yi|]T

(6)

(7)

总体余值r=[rxry]T在X轴和Y轴上为多组ri的平均值

(8)

2 MIFKF算法实现

在融合KF中,目标节点的位置由外部加速度、惯性测量预处理值和三边测量融合估计。该KF由以下方程控制

xt=At-1xt-1+Bt-1ut-1+wt-1

(9)

zt=Ctxt+vt

(10)

式中xt为由目标节点的位置和速度组成的状态向量；At-1和Bt-1为状态转换和输入矩阵；ut-1为输入向量；wt-1为过程噪声向量；zt为测量值向量；Ct为观测矩阵；vt为测量噪声向量。变量定义如下

(11)

(12)

(13)

(14)

Ct-1=[I2×2O2×2]

(15)

(16)

zt=xBLE,t

(17)

(18)

(19)

为了后期处理,RTS平滑器[12]被用来提高前期多融合KF位置估计后的定位精度，如下

(20)

(21)

(22)

(23)

3 仿真与分析

为了说明MIFKF算法的优越性，本文在MATLAB软件中对矿井环境进行模拟分析。如图2所示，为模拟矿井巷道的一段截取部分，假设该段巷道宽为5m，长为30m，巷道中以近似正三角形的方式布有8个信标节点，信标节点两两之间的距离约为8.6m。假设目标节点在巷道中以平均2m/s的速度做往返变速直线运动，每次模拟，目标节点持续运动30s，模拟测试重复10次。其中，信标节点的发射功率设为23dBm。目标节点在不同的时间接收每个信标节点的RSSI采样频率约为80Hz，IMU采样频率为100Hz。每个信标节点在使用三边测量时的RSSIs平均为10Hz。系统采样频率为100Hz。

图2 矿井巷道节点分布

MIFKF算法在本次模拟的仿真结果将分别与理想值、BLE三边测量算法以及标准KF算法进行对比分析，如图3～图7所示。

如图3(a)所示，在Y轴上比较了BLE三边测量算法、MIFKF算法与理想值。图中显示大部分的三边测量估计值在t=78～80s和108～120s时偏离了参考轨迹,导致平均误差为1.091m。表明MIFKF算法相比三边测量估计算法处理异常值效果明显更好。图3(a)与图3(b)比较表明,仿真中余值与三边测量估计Y轴绝对误差保持很强的相关性。且最大的三边测量误差在108.5s时的3.76m,余值为3.55m。

图3 BLE三边测量和MIFKF算法仿真结果

如图4所示为所有10次测试中三边测量误差对应的余值。该图显示了三边测量在X轴和Y轴上的误差值与余值存在着很强的相关性，且对应相关系数分别为0.897和0.952。印证了图3(b)的结论，同时表明余值可以很好地提供三边测量的可靠性估计。

图4 三边测量估计关于余值在X轴和Y轴上的绝对误差

为了表现出MIFKF算法在矿井定位中的优越性,MIFKF算法的性能将在Y轴上与标准KF进行比较。标准KF设定一个常数三边测量的测量噪声。噪声方差(Rt的对角元素)在X轴Y轴上调整为9×10-4m2。

如图5(a)所示，标准KF在t=79～82s和t=108～110s分别产生一个较大的平均误差为1.281m和1.192m，相应时段MIFKF的平均误差分别为0.653m和0.718m。此外，在拒绝异常值后，标准KF定位跟踪在单位时间间隔下的平均误差为0.774m，而MIFKF为0.531m。

图6比较了标准KF算法与MIFKF算法下的卡尔曼增益。从图中可知：MIFKF相对于标准KF有较低的平均卡尔曼增益为0.0013，而标准KF则为0.0451，MIFKF算法下卡尔曼增益越小表明在其算法中IMU测量相比三边测量所占权重越大。

图7(a)给出了标准KF与MIFKF算法在模拟矿井环境下t=78～109.8s时的定位跟踪轨迹。从图中可以发现标准KF的漂移现象更严重，且MIFKF算法相较标准KF的定位精度明显更高。

图5 标准KF和MIFKF算法仿真结果

图6 标准KF与MIFKF算法上的卡尔曼增益

图7 标准KF与MIFKF算法模拟矿井跟踪轨迹

图5(b)和图7(b)表明:RTS平滑器可以进一步提高定位性能。但在t=78～80s时标准KF仍然存在小漂移。

表1比较了每次测试中标准KF算法和MIFKF算法定位跟踪的均方根误差(root mean square error,RMSE)。MIFKF算法的定位精度相比BLE三边测量和标准KF分别提高了44.1%和26.5%。在采用平滑器时，MIFKF算法相比有平滑器的标准KF和没有平滑器的MIFKF算法，其定位精度分别进一步提高了13.6%和29.1%。

4 结 论

针对矿井无线定位异常值的处理，提出了一种优化的用于矿井工作人员和机车准确估计位置轨迹的(MIFKF)算法，算法通过对IMU测量值进行预处理并融合BLE三边测量值进行滤波优化处理，同时滤波器参数可自动调节。实验结果表明：MIFKF算法相对于三边测量和标准KF在矿井复杂环境中有效减小定位误差，并且在使用RTS平滑处理后准确性得到了进一步提高。

表1 定位跟踪的RMSE cm

[1] 霍 羽，房咪咪，刘逢雪.煤矿巷道壁粗糙度对电磁波传播的影响[J].工矿自动化，2013,39(3)：43-45.

[2] 苏应敢，张林山，姚 建.一种考虑非视距传播的UWB定位算法[J].云南电力技术,2015,43(6)：95-99.

[3] Colombo A,Fontanelli D,Macii D.Flexible indoor localization and tracking based on a wearable platform and sensor data fusi-on[J].IEEE Trans on Instrum Meas,2014，63(4)：864-876.

[4] 吴哲明,孙振国,张文增.基于惯性测量单元旋转的陀螺漂移估计和补偿方法[J].清华大学学报：自然科学版,2014,54(9)：1143-1147.

[5] Fourati H.Heterogeneous data fusion algorithm for pedestrian navigation via foot-mounted inertial measurement unit and complementary filter[J].IEEE Trans on Instrum Meas,2015,64(1)：221-229.

[6] 刘明伟,刘太君,叶 焱.基于低功耗蓝牙技术的室内定位应用研究[J].无线通信技术,2015(3)：19-24.

[7] Simon D.Kalman filtering with state constraints:A survey of linear and nonlinear algorithms[J].IET Control Theory & Applications,2010,4(8)：1303-1318.

[8] Hassani V.Multiple model adaptive wave filtering for dynamic positioning of marine vessels[C]∥American Control Conference,2012：6222-6228.

[9] Zihajehzadeh S,Loh D,Lee M.A cascaded two-step Kalman filter for estimation of human body segment orientation using MEMS-IMU[C]∥Proc of IEEE Engineering in Medicine & Biology Society,2014：6270-6273.

[10] 刘 云,房飞翔.基于LO-EKF算法对矿井人员定位跟踪的优化研究[J].云南大学学报：自然科学版,2016,38(3)：392-398.

[11] 熊志广,石为人,许 磊.基于加权处理的三边测量定位算法[J].计算机工程与应用,2010,46(22)：99-102.

[12] 邹 娟.无线传感网络中基于RTS平滑的目标跟踪技术研究[D].湘潭：湘潭大学,2014.