一种基于智能手机的井下高精度行人导航定位方法

本发明涉及井下行人导航定位加工，具体公开了一种基于智能手机的井下高精度行人导航定位方法。

背景技术：

1、煤矿安全是煤矿绿色精准开采的重要前提。煤炭生产面临严峻的安全挑战(瓦斯、水灾、火灾、运输等)，由于矿井内部结构复杂、人员活动范围狭小，为了保证煤矿安全生产、事故应急救援、运输车辆智能调度，井下需要装备人员定位系统，随着煤炭绿色精准开采向智能化开采方向发展，获取采煤机等设备在工作面空间中的准确位置成为解决开采设备自主运行的基础和关键。一旦发生事故，人员位置信息的快速确认将为紧急救援提供宝贵时间；同时，部分采掘工作设备的精准轨迹记录也可有效预防事故的发生。为此，建立实时的、稳定的、高精度的矿井上下一体化定位系统十分必要。

2、传统基于wifi、蓝牙或uwb的定位方法需要提前对定位区域规划并布置物理设备，且需要进行后续维护，消耗一定的人力物力，且井下环境复杂，存在较多环境噪声干扰。基于智能手机的自主定位方式则不必考虑硬件设施成本且抗干扰能力较强，只需利用手机内置传感器即可实现自主定位，具有一定的便利性。该结果可通过行人航位推算(pedestriandeadreckoning,pdr)方法以及环境地磁信息实现，通过手机内置的加速度计、陀螺仪、磁力计获取相关行人运动参数，经过相应计算后即可获得定位结果。传统pdr方法中由于硬件误差及环境噪声影响，普遍存在随时间或距离增加而定位轨迹发散的现象，因此需要进行必要的改进。

技术实现思路

1、本发明的目的是在低成本、低消耗的基础上实现高精度的井下人员自主定位，在传统的pdr定位方法上进行改进，并结合井下环境地磁信息，以解决传统pdr方法定位精度以及稳定性较差的问题。

2、本发明通过以下技术方案来实现上述目的：本发明提供一种基于智能手机的井下高精度行人导航定位方法该方法，包括以下步骤：

3、s1、数据预处理：通过低通滤波处理智能手机传感器的原始三轴数据，并引入空间坐标系转换技术，减少由于设备姿态变化引起的误差，同时，构建线性合加速度模型；

4、s2、步数检测：通过s1中所构建的线性合加速度模型进行人员行走过程中准确步数估计，通过滑动窗口技术和动态阈值调整来实时分析行走时的加速度波形特性；

5、s3、非线性步长估计：采用非线性加速度模型，根据个人身高、性别和步频，利用行走过程中加速度的极值特征来计算，实现步长估计；

6、s4、航向角计算：结合小波变换和有偏卡尔曼滤波器的级联滤波系统提高航向角估计的精度和稳定性，对陀螺仪和磁力计数据去噪处理，提取信号的局部特征，其次，利用有偏卡尔曼滤波器(bkf)对处理后的数据进行融合，以实现更准确和稳定的航向角计算；

7、s5、改进动态时间规整算法：通过磁力计收集地磁序列建立地磁指纹库，并结合改进的动态时间规整算法划分磁环境约束，以此抑制pdr轨迹发散，可提高行走过程中的匹配定位结果的精度，实现对行人导航定位。

8、1)数据预处理通过低通滤波处理智能手机传感器的原始三轴数据，并引入空间坐标系转换技术，有效减少由于设备姿态变化引起的误差。此外，构建线性合加速度模型，进一步提升计算可靠性，为后续分析打下坚实基础。

9、2)步数检测方法：本发明通过(1)中所构建的线性合加速度模型进行人员行走过程中准确步数估计，采用一种改进的步数检测方法，通过滑动窗口技术和动态阈值调整来实时分析行走时的加速度波形特性。这种方法可更准确地估计不同行走速度下的步数，显著提高步数检测的准确性和可靠性。

10、3)非线性步长估计方法：为了更准确地估计步长，本发明采用了非线性加速度模型。该模型考虑到了个人身高、性别和步频等因素，利用行走过程中加速度的极值特征来计算，可有效减少这些客观因素对步长估计的影响，提高了模型的普适性。

11、4)航向角计算方法：本发明设计了一种结合小波变换和有偏卡尔曼滤波器的级联滤波系统，用于提高航向角估计的精度和稳定性。对陀螺仪和磁力计数据去噪处理，以提取信号的局部特征。其次，利用有偏卡尔曼滤波器(bkf)对处理后的数据进行融合，以实现更准确和稳定的航向角计算。

12、5)改进动态时间规整算法的应用：本发明通过磁力计收集地磁序列建立地磁指纹库。并结合改进的动态时间规整算法划分磁环境约束，以此抑制pdr轨迹发散。通过该方法，可提高行走过程中的匹配定位结果的精度，与传统pdr结果融合，进一步增强定位的准确性和稳定性。

13、井下行人定位中，智能手机采集的磁力计、陀螺仪等数据是基于载体坐标系下的数据，随着采集设备的姿态变化，数据也会发生改变。而最终需要获得的定位结果是基于地理坐标系下的坐标。因此在进行后续计算中，需要利用数学方法进行坐标转换，本发明利用旋转矩阵进行坐标变换。

14、旋转矩阵：由上知载体坐标系与地理坐标系并不重合，这两种坐标系存在相对夹角，该夹角称为姿态角。姿态角又分为俯仰角航向角(γ)以及横滚角(θ)，其中，俯仰角为载体绕其坐标系x轴旋转的角度，航向角为载体绕其坐标系y轴旋转的角度，横滚角为载体绕其坐标系z轴旋转的角度。其原理为载体坐标系下采集的数据利用上述三个角度进行“矫正”，令其对应于导航坐标系。上述三个角度的旋转矩阵分别如式(1)、式(2)、式(3)所示：

15、

16、

17、

18、其中，cx、cy、cz分别表示俯仰角、航向角、横滚角对应的旋转矩阵。最终旋转矩阵的表达式如式(4)所示：

19、

20、将原始的传感器数据构成矩阵后，与旋转矩阵左乘即可获得转换后的数据。以地磁数据为例，如式(5)所示：

21、

22、其中mx、my、mz分别表示智能手机采集的三轴数据，分别表示转换后的地理坐标系地磁三轴数据。加速度数据在pdr定位中极为重要，涉及到步长估计及步数估计的有关计算，因此，需要对已采集的加速度数据进行预处理。按公式(6)计算得到三轴线性加速度的合加速度模值：

23、

24、式中a为合加速度模值；ax、ay、az分别为加速度计x、y、z三个轴的加速度；g为当地的重力加速度大小，本文将智能手机静止放置采集重力加速度数据，并对其取均值获得g值。

25、本发明提出了一种改进的步数检测方法，在行走时，脚步落地造成的加速度变化通常呈现周期性波动，可通过识别加速度峰值来计步。本发明旨在解决行走时加速度变化的周期性波动计步问题。与传统的峰值检测法相比，本发明具有显著的改进。在传统方法中，通过设置一个“非零阈值”来检测步伐，这种方法虽然能够避免静止状态下的误差，但由于加速度信号中存在的高频噪声，往往会导致误判“伪波峰”，进而影响计步的准确性。针对这一问题，本发明引入了一种滑动窗口技术，用于动态调整波峰之间的最小距离。通过这种自适应调整机制，本发明能够有效地降低因短时间内多次穿越阈值而导致的计步误差。

26、本发明的具体实施步骤包括数据预处理和步数检测两个主要环节。在数据预处理阶段，首先对原始加速度信号进行滤波和噪声抑制。随后，在步数检测阶段，应用本发明的核心算法——滑动窗口技术，以自适应的准确估计行走步数。具体步骤如下：

27、3)对合加速度采用低通滤波器去除在行走过程中人体晃动带来的高频噪声；

28、4)设置滑动窗口大小范围，行人行走过程中的步伐时间间隔通常为0.5s-2s，通过滑动窗口实时分析波形特性，对阈值进行动态调整，以适应不同的行走速度。检测合加速度绝对值大于阈值的波峰与波谷，并将其作为“候选者”；

29、5)根据波峰—波谷—波峰或者波谷—波峰—波谷原则进行筛选。若出现相邻两个波峰之间没有波谷，则取绝对值较大者为该单步波峰；波谷筛选同理；

30、6)最后取波峰波谷数量较少的一方作为探测步数。

31、本发明涉及一种非线性的步长估计方法，该方法基于行走过程中步长与加速度幅值之间的相关性。通过利用行走过程中加速度的最大值与最小值，有效地估算步长。该模型能够最大限度地排除身高、性别等客观因素的影响，从而提高步长估计的准确性。本发明包括三种主要的非线性步长算法：weinberg模型、scarlet模型以及kim模型。这些模型分别通过特定的数学表达式实现步长估计，为本发明的核心组成部分。其表达式如式(7)(8)(9)所示：

32、

33、

34、

35、式中k1、k2和k3分别为各步长模型的常数参数；amax和amin是每一步加速度序列中的最大值和最小值；ai为加速度序列的第i个加速度；n为加速度序列的总个数。

36、本发明提出了一种创新的航向角计算方法，用于提高pdr定位的准确性和可靠性。本发明的核心在于结合了两种传统的航向角计算方法，并引入了一种先进的数据融合技术，称为“小波变换-有偏卡尔曼滤波”(bkf-wt)级联滤波器，传统方法使用手机传感器中的陀螺仪数据，通过积分解算得到前进角度。这种方法具有不受外界环境干扰和短时间内稳定的优点，但存在由于陀螺仪限制而引起的偏移误差。或是通过磁力计的xyz数据解算地磁北与手机之间的夹角，得到前进角度。这种方法的优势在于长时间稳定且不存在偏移误差，但它容易受到外部磁场的干扰，产生较大航向偏差。本发明通过结合小波变换的信号处理技术(wt)和卡尔曼滤波的数据融合方式，克服了传统方法的局限性。首先，应用小波变换对磁力计和陀螺仪数据进行去噪处理，以提取信号的局部特征。其次，利用有偏卡尔曼滤波器(bkf)对处理后的数据进行融合，以实现更准确和稳定的航向角计算。

37、计算原理如下：根据卡尔曼滤波原理，本文定义状态转移方程为式(11)：

38、xk＝axk-1+qk-1   (11)

39、式中，为状态向量；分别为k时刻陀螺仪航向角和角速度。状态转移矩阵a＝[1 δtk；0 1]t；δtk＝tk-tk-1；tk为k时刻时间节点；qk-1为过程噪声。

40、系统量测方程为式(12)

41、zk＝bxk+rk-1   (12)

42、式中，为第k时刻观测值；分别为第k时刻磁力计航向角和根据磁力计航向角对时间求导产生的角速度；b为单位矩阵；rk-1为观测噪声。

43、卡尔曼滤波分为预测和更新两步，预测方程为式(13)

44、

45、式中，为先验状态估计矩阵；为先验协方差矩阵；q为过程噪声矩阵。

46、更新方程为式(14)：

47、

48、式中，kk为卡尔曼增益；xk|k为后验状态矩阵；pk|k为后验协方差矩阵；r观测噪声矩阵；i为单位阵。

49、随后，运用有偏估计的思想，通过偏置卡尔曼滤波的结果，得到优于传统kf的有偏卡尔曼滤波。其定义为式(15)：

50、

51、式中，为有偏卡尔曼滤波估计值，γk为偏参数，为卡尔曼滤波估计值。

52、则的偏差和方差可表示为式(16)(17)：

53、

54、

55、那么的均方误差为式(18)：

56、

57、要使最小，等价于求的最小值问题，则对求导并令导数为零，可得式(19)：

58、

59、式中，||·||2表示向量的欧式范数。

60、则有偏卡尔曼滤波的完整公式可表示为：

61、

62、在建筑区域内，由于钢筋混凝土成分及室内固定铁制设备对地磁的影响，反而增强了不同位置的地磁矢量值的差异，这为地磁室内定位提供了基础；因此，地磁定位可视为一种“指纹定位”，即离线阶段将室内区域进行划分，每处区域内的磁场强度为该区域内特有的“指纹”，再将室内场景中所有指纹及其对应的位置信息整合起来建立指纹库。相对于无线信号室内定位，地磁定位具有连续性强等优势；且与pdr定位类似，同样不需要额外的室内物理设施支持。在进行pdr定位时，使用磁力计采集行走路线的地磁序列数据，构建数据库，再利用相应的匹配算法获得地磁序列的匹配坐标。

63、本发明涉及一种用于地磁室内定位的改进型动态时间规整算法(dynamic timewarping,dtw)，该方法旨在提高地磁序列匹配的效率和准确性，用于对实时采集的地磁序列与事先构建的地磁指纹库中的序列进行匹配。考虑两个时间序列x＝{x1,x2,x3,…,xn}和y＝{y1,y2,y3,…,ym}，其中n和m分别是序列x和y的长度。构建一个n×m的代价矩阵d，其中每个元素d[i,j]表示序列x和y在时间点i和j之间的距离。本发明创建了一个代价矩阵，该矩阵的每个元素代表两个地磁序列在特定点上的欧氏距离差异，如式(21)所示。

64、d[i,j]＝||m-ml||   (21)

65、其中，m表示实时采集的地磁序列，ml表示构建的地磁指纹库中第l个磁序列，d[i,j]表示上述两个序列之间的二维欧氏距离。

66、考虑到传统dtw算法在处理较长序列时计算量巨大，本发明引入了改进动态时间规整算法作为dtw的近似解法。相关步骤如下：

67、(1)下采样处理：设定下采样率为k，对原始序列x和y进行下采样处理，得到新的序列′x′和′y′。

68、(2)初步dtw匹配：在下采样后的序列′x′和′y′上应用标准dtw算法，计算出初步的对齐关系。

69、(3)插值处理：将初步的dtw矩阵′d′扩展到原始序列的长度。可以使用线性插值或其他插值技术实现。

70、(4)原始序列上的微调：在扩展后的dtw矩阵上对原始序列x和y进行最终的对齐和匹配。

71、在传统dtw算法中，计算复杂度为o(n2)。而通过改进的dtw方法处理后，由于初始处理是在下采样的序列上进行，假设下采样后序列的长度为′n′，则初步匹配的计算复杂度降为o(n′2)。随着下采样率k的增加，′n′会减小，从而显著降低计算复杂度。进一步采用全局搜索法在限定的滑动窗口内寻找代价矩阵的最短路径，从而确定两个序列之间的最佳匹配关系。从而获得行走路径的地磁匹配位置。

72、地磁匹配在定位场地范围较大时，会由于分辨率低而出现较多错误匹配的情况，精度较低，pdr定位则存在不可避免的误差累积，可将二者定位方式相结合互相弥补。在pdr提供的轨迹信息基础上，可利用地磁数据进行位置校正。因此，在行人移动过程中，一旦检测到地磁特征的显著变化，即可用这些信息来调整pdr估计的轨迹。可将pdr定位结果视为一个圆形区域的中心，而地磁特征匹配则限制在这个区域内。即在实际计算的时候只匹配该圆形区域内的地磁指纹，指纹库中其他指纹不纳入匹配范围，最终得到组合定位结果。

73、本发明的有益效果为：

74、1)本发明的步数检测方法通过采用滑动窗口技术和滤波处理，显著提高了行走步数的检测准确性和可靠性。它有效地减少了高频噪声和误判“伪波峰”的影响，同时通过实时动态调整阈值，增强了对不同行走速度的适应性。这种技术的创新不仅使步数检测变得更加准确和灵活，而且在健康监测、运动跟踪等多个领域展现了广泛的应用潜力。

75、2)本发明通过结合小波变换和偏置卡尔曼滤波(bkf-wt)技术，显著提高了pdr定位的准确性和可靠性。这种方法有效地结合了陀螺仪的短期稳定性和磁力计的长期稳定性，可明显降低由于传感器限制造成的偏移误差，增强了系统对外部磁场干扰的抵抗能力。这一技术创新不仅提升了航向角计算的准确性和稳定性，而且为室内定位、导航等领域提供了更可靠的技术支持。

76、3)本发明的改进型动态时间规整算法(dtw)，显著提高了磁环境匹配的效率和准确性，有效降低了传统dtw算法在处理长序列时的高计算复杂度。此外，本发明还将地磁匹配与pdr定位相结合，利用地磁数据进行位置校正，克服了单一地磁匹配因分辨率低而产生的错误匹配问题，同时也减少了pdr定位中的误差累积。适用于大规模和复杂的井下空间定位需求。

77、4)本发明通过实际应用验证，证明了其在井下高精度行人导航定位领域的有效性和实用价值。该方法显著提升了井下行人导航定位的准确性和稳定性。此方法能够在复杂的井下环境中提供高精度的行人轨迹信息，对保障井下工作人员的安全以及提高工作效率具有重要意义。