基于模糊AKF地磁辅助导航的采煤机定位方法*

毛 君, 钟 声,3, 马 英

(1.辽宁工程技术大学 机械工程学院，辽宁 阜新 123000;2.天地科技股份有限公司 开采设计事业部，北京 100013;3.北京天地玛珂电液系统控制有限公司,北京 100013)

0 引 言

根据煤矿安全、高效生产的要求，以及机械自动化技术的发展，综采工作面实现少人或无人开采是必然趋势，其较差的自动控制水平将直接影响综采“无人化”进程的推进[1]。而采煤机在工作面的位置姿态的准确定位是目前研究的热点问题之一[2，3]。樊启高、赵静等人采用了惯性导航方法对采煤机的位置姿态进行定位[4，5]。宋宇、翁新武提出了一种基于光流和惯性导航的小型无人机定位方法[6]。姜华、何风行等人提出了一种基于超声和射频融合的无线传感器网络网格定位方法[7]。李振浩、李英娜提出了基于免疫记忆人工鱼群算法的局部放电超声定位方法[8]等。

本文在此基础之上提出了一种基于模糊自适应卡尔曼滤波(adaptive Kalman filtering,AKF)惯性地磁导航的采煤机动态定位方法，可有效避免惯导定位的误差累积。

1 采煤机惯性导航定位

将惯性导航固定于采煤机，通过三轴加速度计和三轴陀螺仪进行实时反馈采煤机的空间加速度信号和姿态信息，根据采煤机初始位置，可实现对采煤机的动态位置信息实时解算。

图1 惯性导航计算流程

为了降低误差累积，采用迭代最近等值点(iterative closest contour point，ICCP)算法将惯导与地磁辅助技术组合[9]。通过地磁辅助惯性导航所指示位置的P(P1,P2,…,PN)，而实际输出的位置为P＇(P＇1，P＇2，…P＇N)，通过地磁传感器对地磁场值进行测量，则得到地磁场等值线HN(H1,H2,…,HN)。通过欧氏距离法在该地磁场等值线上寻找距离P(P1,P2,…PN)点最近距离MN(M1,M2,…,MN)，通过旋转矩阵R和平移矩阵t对其进行修正，反复进行上述过程，直至达到迭代终止条件。

为了对惯性导航实时性校正，采用模糊自适应的卡尔曼滤波方法与地磁辅助惯性导航组合[10]。其效果通过系统状态向量协方差的变换进行评价。

卡尔曼滤波中，测量新息定义为

rk+1=Zk+1-Hψk+1,kXk

(1)

其测量残差方差实际值为[11]

(2)

式中m0=mN-N+1;N为测量方差。

卡尔曼滤波的测量残差方差理论值为[12]

(3)

式(2)与式(3)的比值为

(4)

式中 tr(·)为矩阵的迹。

由于受到测量噪声的影响，Rr值偏离理想值，则通过模糊控制量β进行自适应调节

Rk+1=βRk

(5)

图2 导航系统计算流程

采煤机模糊AKF的地磁辅助惯性导航得到姿态更新

(6)

采煤机速度矢量更新

(7)

式中vx,vy,vz分别为采煤机在x,y,z方向的速度;ωie为地球自转角速率;RN为采煤机所在地经线圈切线方向地球的曲率半径;f=(fxfyfz)T为加速度计测量的比力；gn为采煤机重力加速度的大小。

采煤机位置更新微分方程

(8)

式中L为当地纬度;h为当地高度;RM为采煤机所在地纬线圈切线方向地球的曲率半径。

通过式(6)可得出采煤机实时姿态角，通过对式(8)进行数值积分，可得到采煤机的所在位置。

2 仿真分析

基于本文所推导的采煤机导航方程，对采用模糊AKF惯性地磁导航的采煤机系统方案进行仿真分析。首先模拟加速度计以及陀螺仪的数据输出，对产生的加速度计信号和陀螺仪信号进行实时采样，并通过惯性处理法计算出采煤机的位移及速度信息。在地理坐标系下采煤机起始位置设定为[0 0 0]T，初始方位角设定为0°，初始速度设定为v0=0.5 m/s。

在采煤机仿真平台中，设定X表示采煤机平行工作面方向，运行10 m，Y表示工作面推移方向，行进0.4 m，Z则表示垂直工作面方向，其中采煤机在X方向4～6 m处进行斜切进刀，仿真时间设定20 s，结果如图3所示，分析可知，采用捷联惯导方法定位结果在理想轨迹附近进行小幅波动，极大地减小了随时间积累的定位误差。

图3 定位结果

3 结束语

针对采煤机煤矿井下定位受环境恶劣以及干扰较多等问题，提出了基于模糊AKF地磁辅助惯性导航的采煤机定位方法，推导了采煤机位置及姿态更新微分方程，在此基础上对系统进行了仿真研究，结果表明：本文方法克服了惯导定位误差随时间累积的缺点，且实现了系统高精度实时定位。

[1] 苏秀苹,李 威,樊启高.采煤机滚筒调高滑模变结构控制策略[J].煤炭学报,2012, 37 (12):2107-21,11.

[2] 方新秋，何 杰，张 斌，等.无人工作面采煤机自主定位系统[J].西安科技大学学报，2008,28(2):349-353.

[3] 吴立新，殷作如，邓智毅，等.论21世纪的矿山—数字矿山[J].煤炭学报，2000,25(4):337-342.

[4] 赵 靖.基于卡尔曼滤波算法的采煤机惯导定位方法[J].工况自动化,2014,40(10):29-32.

[5] 樊启高，李 威，王禹桥，等.一种采用捷联惯导的采煤机动态定位方法[J].煤炭学报，2011, 36(10):1758-1761.

[6] 宋 宇，翁新武，郭昕刚.基于光流和惯性导航的小型无人机定位方法[J].传感器与微系统，2015,34(1):13-16.

[7] 姜 华，何风行，陈文权，等.一种基于超声和射频融合的无线传感器网络网格定位方法[J].传感器与微系统,2015,34(3):24-27.

[8] 李振浩，李英娜，张长胜，等.基于免疫记忆人工鱼群算法的局部放电超声定位方法[J].传感器与微系统,2017,36(3):73-76.

[9] 康 崇，张晓峻，樊黎明.基于ICCP算法的地磁匹配辅助导航[J].应用基础与工程科学学报，2014,22(3):598-604.

[10] 符 拯，王书满，刘丙杰.自适应卡尔曼滤波的最新进展[J].战术导弹技术，2009(6)：62-66.

[11] Escamilla-Ambrosio P J,Mort N.Multisensor data fusion architecture based on adaptive Kalman filters and fuzzy logic performance assessment[C]∥Proceedings of the Fifth International Conference on Information Fusion，FUSION 2002,Annapolis，USA，2002:1542-1549.

[12] 徐田来，游文虎，崔平远.基于模糊自适应卡尔曼滤波的INS/GPS组合导航系统算法研究[J].宇航学报，2006,26(5):571-575.