CN109211234B - 一种矿下惯性测绘方法及装置 - Google Patents

一种矿下惯性测绘方法及装置 Download PDF

Info

Publication number
CN109211234B
CN109211234B CN201710526261.0A CN201710526261A CN109211234B CN 109211234 B CN109211234 B CN 109211234B CN 201710526261 A CN201710526261 A CN 201710526261A CN 109211234 B CN109211234 B CN 109211234B
Authority
CN
China
Prior art keywords
measurement unit
mine
inertial
interval
calculating
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710526261.0A
Other languages
English (en)
Other versions
CN109211234A (zh
Inventor
杨丛昊
曾昆
胡双
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan yunjiangna micro Information Technology Co.,Ltd.
Original Assignee
Hunan Glonavin Information Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan Glonavin Information Technology Co ltd filed Critical Hunan Glonavin Information Technology Co ltd
Priority to CN201710526261.0A priority Critical patent/CN109211234B/zh
Publication of CN109211234A publication Critical patent/CN109211234A/zh
Application granted granted Critical
Publication of CN109211234B publication Critical patent/CN109211234B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种矿下惯性测绘的方法及装置，人员沿矿山外侧的矿道绕矿山行进，所述矿下惯性测绘方法包括如下步骤：(1)进行姿态变换矩阵的计算与更新；(2)计算导航坐标系下的加速度计测量数据，并计算k时刻惯性测量单元的位置；(3)计算每个行进区间的静止时间区间和运动时间区间；(4)对速度、位置坐标、姿态变换矩阵进行修正；(5)计算矿山形状。本发明将惯性定位方法应用到矿产测量中，测量方便，不受空间限制；对惯性测量单元计算得到的距离进行补偿；且解决了因航向漂移导致的曲线不闭合问题，提高矿山面积、形状的测量精度，方便对整个矿山形状的计算。本发明测量方法简单，易于操作，大大降低了矿山的测量成本。

Description

一种矿下惯性测绘方法及装置
技术领域
本发明涉及一种矿下测量方法及装置，属于矿山测量领域。
背景技术
随着我国矿山事业的发展，对矿山地形测量也提出了更高要求，而且在矿山开采过程中需要经常性的测量开采进度。由于矿山的形状不规则，给测量带来了许多困难。现有的矿山地形测量一般采用全站仪、激光装置或GPS装置进行测量；全站仪测量的设备复杂、重量大，价格昂贵，不易进行测量，浪费人力、物力，测量效率不高；利用激光装置测量的成本较高，测量不便，而且激光装置测量受限于矿道空间，影响测量的进行；由于矿下GPS信号较弱，因此利用GPS装置测量的信号影响大，无法进行测量，而且GPS测量的精度不高。因此，现有的测量装置和方法无法满足经常性的矿山地形要求。
由于惯性器件本身特性，无法准确的补偿惯性器件的相关参数，且零偏也无法准确的获取。之外，由于算法原理本身的限制，提取的静止区间也不可能完成正确。惯性器件的误差是累积的，当使用较长时间时，误差会积累的越来越大，这导致惯性器件解算出来的轨迹和真实的轨迹存在两个方面的差别：
(1)里程损失
里程损失即当直线行进距离L时，惯导计算出来的距离小于L。表现在测量中就是惯导轨迹形成的整个闭合区域变小，导致计算处理的闭合区域的面积也变小。导致里程损失的原因比较多，比较重要的原因包括：(A)器件精度及标定不准确造成。这个问题受工艺、标定模型等技术的影响，难以在短期内改进；(B)惯导解算问题，主要包括姿态解算、静止检测等。如果检测出来的静止区间包括了少量的运动部分，则导致积分区间变短，造成里程的损失。
(2)航向漂移
航向漂移即真实的行进方向和解算出来的方向存在差异，这会造成虽然真实行进路线为一个封闭的路线，但是惯导解算出来的路线不是封闭的，而是有个缺口，即曲线的起始位置和结束位置有个距离。这导致无法进行惯导行进轨迹围成的面积的计算。
综上所述，现有惯性测量技术及装置无法满足矿山地形测量的需求。
发明内容
本发明要解决的问题是针对现有矿下测量方法和装置无法满足经常性测量要求及存在的上述问题，提供一种矿下惯性测绘方法，惯性测量单元设置于人员身上，人员沿矿山外侧的矿道绕矿山行进，惯性测量单元包括加速度计、陀螺仪，以采样频率f采集k时刻惯性测量单元在载体坐标系下的加速度值
Figure GDA0003465753480000021
角速度值
Figure GDA0003465753480000022
所述矿下惯性测绘方法包括如下步骤：
(1)计算k时刻惯性测量单元的姿态变换矩阵
Figure GDA0003465753480000023
并利用下式对姿态变换矩阵
Figure GDA0003465753480000024
进行更新
Figure GDA0003465753480000025
其中
Figure GDA0003465753480000026
(2)利用下式计算k时刻惯性测量单元在导航坐标系下的加速度值
Figure GDA0003465753480000027
并将k时刻惯性测量单元在导航坐标系下的加速度值
Figure GDA0003465753480000028
进行积分得到k时刻惯性测量单元的位置，其中
Figure GDA0003465753480000029
g为重力加速度值；
(3)计算人员的每个行进区间的静止时间区间，其中，人员行进过程由多个行进区间组成，每个行进区间由惯性测量单元的静止时间区间和位于所述静止时间区间之后的运动时间区间组成；
(4)在每个行进区间的静止时间区间内，利用卡尔曼滤波方法对惯性测量单元的速度、位置、姿态变换矩阵进行修正，并计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1)，其中l代表行进区间的个数；
(5)当人员完成绕矿山行进，回到绕矿山行进的起点后，利用各个行进区间的起点坐标、终点坐标绘制人员行进轨迹，根据所述人员行进轨迹对矿山的形状和/或面积进行测量；其中前一个行进区间的终点即为后一个行进区间的起点。
本发明中，将惯性测量单元设置于人员身上，通过惯性测量单元的角速度值、加速度值计算惯性测量单元的速度和位置坐标。同时，利用卡尔曼滤波构建惯性导航定位误差状态修正模型，结合静止检测策略，对惯性测量单元的速度、位置坐标、姿态变换矩阵进行修正，通过对姿态变换矩阵进行修正提高了迭代运算精度，通过对速度、位置坐标进行修正提高了位置坐标的测量精度，从而提高矿山的测量精度。本发明中，只有在静止时间区间才进行卡尔曼滤波修正，其他步骤一直迭代运算，因为只有当惯性测量单元处于静止时，通过系统迭代运算状态量的改变才能表征在误差的累积。当人员完成绕矿山行进，回到绕矿山行进的起点后，利用各个行进区间的起点坐标、终点坐标绘制人员行进轨迹，根据所述人员行进轨迹即可计算矿山形状和/或面积。利用矿产的平面面积和高度相乘，即可得到矿产体积。也可通过各个测量的坐标直接计算矿产的立体体积。本发明中，仅需人员携带惯性测量单元，绕矿行走一周即可计算得到矿产的形状和/或面积，测量方法简单，可在需要时随时进行测量，测量成本低，且受环境的影响小。由于本发明的测量方法简单，因此可以满足经常性测量要求。
上述技术方案中，所述步骤(4)中，求解如下方程组，计算里程补偿后的各个行进区间的终点坐标
Figure GDA0003465753480000031
其中r为里程损失补偿比例系数；优选r的计算公式为
Figure GDA0003465753480000032
其中S为矿下任意一段已知的距离，Sc为惯性测量单元对所述已知的距离的测量的结果，
Figure GDA0003465753480000033
为里程补偿后第l个行进区间的终点坐标。
本发明中，为了解决里程损失，采用积分距离动态补偿的方法，利用里程损失补偿比例系数对惯性测量单元计算得到的距离进行补偿，减小了测量误差，提高了测量精度。在相同环境的矿下，利用惯性测量单元对任意一段已知的距离进行测量，可以得到里程损失补偿比例系数。
上述技术方案中，所述步骤(5)包括如下步骤：
(5.1)判断人员绕矿山行进形成的轨迹的起点坐标和终点坐标是否重合，若所述轨迹的起点坐标和终点坐标未重合，则进行步骤(5.2)，否则，跳转到步骤(5.6)；
(5.2)利用下式计算修正后的初始零偏值
Figure GDA0003465753480000034
Figure GDA0003465753480000035
其中，[w0-Δw0，w0+Δw0]为初始零偏w0的修正范围，δw为搜索步长，m为修正次数，且
Figure GDA0003465753480000036
(5.3)利用下式计算零偏修正后惯性测量单元的航向角
Figure GDA0003465753480000037
其中fl、fl+1分别为第l个行进区间的起点的帧号、终点的帧号，f为采样频率，其中
Figure GDA0003465753480000041
(xl,yl)、(xl+1,yl+1)分别为第l个行进区间的起点坐标、终点坐标；
(5.4)求解如下方程组，计算零偏修正后惯性测量单元在各个行进区间的终点坐标(x′l+1,y′l+1)
Figure GDA0003465753480000042
(5.5)判断零偏修正后人员绕矿山行进形成的轨迹的起点坐标和终点坐标是否重合，若二者重合，则跳转到步骤(5.6)；
否则，判断是否满足d′m＜dm，若d′m＜dm，则令m＝m+1，且跳转到步骤(5.2)，若d′m＞dm，则令m＝m-1，且跳转到步骤(5.2)，其中dm和d′m分别为第m次零偏修正前、零偏修正后轨迹的起点和终点之间的距离；
(5.6)利用零偏修正后的各个行进区间的起点和终点形成的轨迹对矿山的形状和/或面积进行测量。
本发明中，为了解决因航向角漂移而造成的真实行走路线无法形成封闭路线的问题，根据保存的轨迹文件解决因航向漂移导致的曲线不闭合问题。在处理过程中，设置初始零偏的修正范围和搜索步长，利用搜索步长对每一个行进区间的航向角漂移进行补偿，使得每个行进区间的终点坐标得到修正，通过逐步调整航向角，使得起点坐标和终点坐标重合，从而形成封闭路线，减小了测量误差，提高了面积测量的精度，方便对整个矿山的测量。
上述技术方案中，所述步骤(5.5)中还包括：
若
Figure GDA0003465753480000043
且
Figure GDA0003465753480000044
且第mn次零偏修正后轨迹的起点坐标和终点坐标未重合，则以δw′代替δw，且将初始零偏w0的修正范围修改为[min{w0+(mn-1)δw，w0+(mn+1)δw}，max{w0+(mn-1)δw，w0+(mn+1)δw}]，并跳转到步骤(5.2)，其中mn∈m；优选δ′w取值不大于
Figure GDA0003465753480000045
本发明中，对航向角进行修正的过程中，可能出现所设置的搜索步长过大，仍使得人员行进的轨迹的起点位置和终点位置无法闭合的情况，因此，若
Figure GDA0003465753480000046
且
Figure GDA0003465753480000047
且
Figure GDA0003465753480000048
均不为0，即始终无法找到可以令轨迹闭合的值，则减小搜索步长，再对航向角进行修正。
上述技术方案中，所述惯性测量单元设置于人员手部或足部；若所述矿下惯性测量单元设置于人员足部，则从人员足部离开地面到人员足部再次接触地面为一个行进区间；若所述惯性测量单元设置于人员手部，则人员在行进过程中，不定时地用手部触碰矿山外侧的矿壁，且从人员手部离开矿壁到人员手部再次接触矿壁为一个行进区间。
惯性测量单元设置于手部或足部时，对行进区间的定义不同，因此在处理过程中，以足部离开地面到人员足部再次接触地面为一个行进区间作为足部的行进区间，以手部离开矿壁到人员手部再次接触矿壁为一个行进区间，进行后续处理。
上述技术方案中，所述步骤(3)中，计算惯性测量单元的所述静止时间区间的方法为：计算第k时刻惯性测量单元的角速度值的模值
Figure GDA0003465753480000051
和第k时刻惯性测量单元的加速度值的模值
Figure GDA0003465753480000052
若
Figure GDA0003465753480000053
且
Figure GDA0003465753480000054
且
Figure GDA0003465753480000055
且k＞ka，且k时刻和ka时刻之间的时间差Δt≤0.1s，则判断k时刻为静止时刻；所述运动时间区间为相邻两个静止时间区间之间的时间区间；其中k、ka均为采样时刻，peak_threshold为加速度峰值阈值，threshold_amin、threshold_amax分别为加速度模值的静止判断的下限阈值、上限阈值，threshold_ω为陀螺模值的静止判断阈值。
上述技术方案中，所述步骤(3)中，第k时刻惯性测量单元的加速度值的模值Fk all为利用下式平滑后的加速度值的模值
Figure GDA0003465753480000056
其中，W为滑动窗宽，W≥5，
Figure GDA0003465753480000057
其中优选W＝10。
本发明中，通过对惯性测量单元的加速度值的模值进行平滑，可以使得数据的趋势更加明显，便于进行处理和判断。
上述技术方案中，所述步骤(4)包括：
(4.1)选取卡尔曼滤波的状态量
Figure GDA0003465753480000058
选取卡尔曼滤波的量测量
Figure GDA0003465753480000059
下式进行卡尔曼滤波
Figure GDA00034657534800000510
其中，上标*＝x,y,z，δφk、δvk、δpk分别为k时刻惯性测量单元的姿态角误差角、速度误差、位置误差，δφk＝(δγk,δθk,δψk)，
Figure GDA00034657534800000511
为k时刻惯性测量单元的磁传感器输出角度，baro_hk为k时刻惯性测量单元的气压高度，
Figure GDA0003465753480000061
γk、θk、ψk分别为k时刻惯性测量单元的横滚角、俯仰角、航向角，(γk、θk、ψk)为k时刻惯性测量单元的姿态角，δγk、δθk、δψk分别为k时刻惯性测量单元的横滚角误差、俯仰角误差、航向角误差，
Figure GDA0003465753480000062
为k时刻惯性测量单元的磁场强度；
(4.2)利用δφk、δvk、δpk对惯性测量单元的姿态变换矩阵、速度、位置坐标进行修正，得到滤波修正后惯性测量单元的姿态变换矩阵、速度、位置，并返回到步骤(1)，利用滤波修正后惯性测量单元的姿态变换矩阵对惯性测量单元的横滚角、俯仰角、航向角进行修正；并利用修正后的位置坐标计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1)。
上述技术方案中，所述的步骤(1)中，利用下式计算
Figure GDA0003465753480000063
Figure GDA0003465753480000064
其中，γk、θk、ψk分别为k时刻惯性测量单元的横滚角、俯仰角、航向角；
并利用下式计算γ0、θ0、ψ0
Figure GDA0003465753480000065
其中γ0、θ0、ψ0分别为惯性测量单元的初始横滚角、初始俯仰角、初始航向角，
Figure GDA0003465753480000066
分别利用k＝1时刻惯性测量单元在载体坐标系下的加速度值、磁场强度计算得到或利用k＝1时刻到k＝K1时刻之间所有数据求均值计算得到；优选K1≥100。
步骤(4.2)中，利用修正后的姿态变换矩阵、速度、位置坐标、横滚角、俯仰角、航向角进行后续计算，即进行步骤(1)的计算。
本发明中，对k＝1时刻到k＝K1时刻之间所有数据求均值计算得到加速度值、磁场强度，通过取均值减小了数据中的随机误差，提高了运算精度。
本发明还提供一种利用上述矿下惯性测绘方法的矿下惯性测绘装置，包括信号处理单元，还包括设置于人员身上且均与所述信号处理单元连接的惯性测量单元，所述惯性测量单元包括加速度计、陀螺仪、磁传感器；
所述加速度计、陀螺仪、磁传感器分别用于以采样频率f采集惯性测量单元的角速度值、磁场强度以及在载体坐标系下的加速度值；
所述信号处理单元用于：计算姿态变换矩阵、计算惯性测量单元在导航坐标系下的加速度值、计算惯性测量单元的位置坐标、在每个行进区间的静止时间区间内，利用卡尔曼滤波方法对惯性测量单元的速度、位置、姿态变换矩阵进行修正，并计算每个行进区间的起点坐标、终点坐标，利用各个行进区间的起点坐标、终点坐标绘制人员的行进轨迹，根据所述行进轨迹对矿山的形状和/或面积进行测量。
本发明具有的优点和积极效果是：本发明中，将特殊的惯性定位方法应用到矿产测量中，测量方便，不受空间限制；采用积分距离动态补偿的方法，利用里程损失补偿比例系数对惯性测量单元计算得到的距离进行补偿；解决了因航向漂移导致的曲线不闭合问题，通过逐步调整航向角，使得起点坐标和终点坐标重合，从而形成封闭路线，即能使起点和终点闭合，又能修正行走过程中的航向漂移，提高面积测量的精度，方便对整个矿山形状的计算。本发明测量方法简单，易于操作，大大降低了矿山形状的测量成本。
附图说明
图1是本发明的一个实施例的矿下惯性测量方法步骤示意图。
图2是本发明的一个实施例的矿下惯性测量单元的结构示意图。
图3是本发明的一个实施例的行进区间的组成示意图；
图中，1、惯性测量单元，11、加速度计，12、陀螺仪，13、磁传感器，14、气压高度测量单元，2、信号处理单元，3、数据传输模块。
具体实施方式
下面将结合本申请的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
对于本领域的普通技术人员而言，可以通过具体情况理解本发明中术语在本发明中的具体含义。本发明中的实施例中，定义惯性测量单元1的位置、气压高度测量单元14的位置、人员的位置是同一个位置。
实施例
如图1所示，本发明提供一种矿下惯性测绘方法，气压高度测量单元14、设置于人员身上的惯性测量单元1均与信号处理单元2连接，以采样频率f采集k时刻惯性测量单元1在载体坐标系下的加速度值
Figure GDA0003465753480000081
角速度值
Figure GDA0003465753480000082
人员沿矿山外侧的矿道绕矿山行进。人员的行进过程由多个行进区间组成，每个行进区间由惯性测量单元1的静止时间区间和位于所述静止时间区间之后的运动时间区间组成；本发明所需要的数据包括角速度值、加速度值、气压高度值、磁场强度。
如图2所示，本发明还提供一种利用上述矿下惯性测绘方法的矿下惯性测绘装置，包括信号处理单元2，还包括设置于人员身上且均与所述信号处理单元2连接的气压高度测量单元14、惯性测量单元1，所述惯性测量单元1包括加速度计11、陀螺仪12、磁传感器13；所述加速度计11、陀螺仪12、磁传感器13分别用于以采样频率f采集惯性测量单元1在载体坐标系下的加速度值、角速度值、磁场强度；所述信号处理单元2用于：计算姿态变换矩阵、计算导航坐标系下的加速度值、计算惯性测量单元1的位置坐标、在每个行进区间的静止时间区间内，利用卡尔曼滤波方法对惯性测量单元1的速度、位置、姿态变换矩阵进行修正，并计算每个行进区间的起点位置、终点坐标，利用各个行进区间的起点位置、终点位置绘制人员的行进轨迹，根据所述人员行进轨迹计算矿山形状。数据传输模块3与信号处理单元2连接。本发明中，可利用数据传输模块3，利用无线或有线的方式，将信号处理单元2的处理结果传递给中央控制单元或相关人员，有助于对数据进行监控或进行后续处理。
所述矿下惯性测绘装置设置于人员手部或足部。在行进过程中，人员手部触碰矿壁等同于人员足部与地面接触，人员手部未触碰矿壁等同于人员足部未与地面接触。
若所述矿下惯性测绘装置设置于人员足部，则从人员足部离开地面到人员足部再次接触地面为人员的一个行进区间(一步)；若所述矿下惯性测绘装置设置于人员手部，则人员在行进过程中，不定时地用手部触碰矿山外侧的矿壁，且从人员手部离开矿壁到人员手部再次接触矿壁为人员的一个行进区间(一步)。本发明中，一个行进区间即为一步。
以足部安装惯性测量单元1为例，在行进过程中，人员足部离开地面的位置为本步的起点坐标，人员足部再次接触地面的位置为本步的终点坐标；以手部安装惯性测量单元1为例，在行进过程中，人员手部离开矿壁的位置为本步的起点坐标，人员手部再次接触矿壁的位置为本步的终点坐标。本发明中，假定从人员足部接触地面到足部离开地面的静止时间区间内，惯性测量装置1为静止状态，假定从人员手部接触矿壁到手部离开矿壁的静止时间区间内，惯性测量装置1为静止状态。
对于设置于手部的惯性测量单元1。通过手部接触矿壁，结合惯性算法，得出惯性测绘装置的数据，来进行简单可靠方便快捷的测量。因为使用环境是在用戴在手上的传感器进行测量，手部在贴合矿壁运动时是做了限制的，即手部不能随意的甩动，需要类似足部行进，进行盖章式的触壁，因此不会存在长时期的无规则运动，偶尔的无规则运动不会对位置造成大影响。
当人员完成绕矿山行进后，对于手部设置惯性测量单元1的情况，需要在最后一个行进区间令手部触碰绕矿山行进的矿壁的起点位置(即第一个行进区间接触矿壁的起始位置)；对于足部设置惯性测量单元1的情况，需要在最后一个行进区间令足部接触绕矿山行进的地面的起点位置(即第一个行进区间足部接触地面的起始位置)。
依据实际使用矿下环境，根据需要将惯性测绘装置佩戴在测量人员的手部或者足部，使用依据简要如下：在矿道相对平整而矿壁粗糙的情况下优选将惯性测绘装置佩戴在足部，在矿道相对粗糙而矿壁平整的情况下优选将惯性测绘装置佩戴在手部，如有需要两个部位均可佩戴。两个部位都佩戴的情况下，可以利用卡尔曼滤波等方法，通过数据后端融合处理得到相对更高的定位精度。选择惯性测绘装置的佩戴位置。若佩戴在手部，则要求使用过程中人员手部要接触矿壁。手部的运动方式和在足部是相似的，都是抬起(动)，放下(静)，再抬起(动)，重复这样的运动方式。通过人员规律性的“停”，“走”，“停”运动模式，结合捷联惯性导航算法，依据数据解算出人员在矿道的运动距离和角度变化，进行矿的形状的测量。人员靠近矿壁沿矿山外侧的矿壁规律性行进。
本发明中，利用穿戴在人员身上的惯性测量单元1实现一个便捷有效的矿下惯性测绘方法。将惯性测量单元1佩戴在测绘人员身上(手臂或者足部)，通过人员靠近矿壁行进，由佩戴在人员的身上的惯性测量单元1获取人员规律性运动数据，结合惯性导航算法，实现简单可靠的矿下测量；惯性测量单元1所用到的数据包括加速度计11、陀螺仪12、气压高度测量单元、磁传感器13的数据。利用加速度计和陀螺这类惯性数据获取佩戴人员运动过程中的规律性运动信息，利用捷联惯性算法计算人员在矿道中的运动轨迹，结合人员运动过程中周期性的“停”，“走”，“停”的运动模式，检测惯性测量单元1的静止时间区间进行惯性误差修正并由气压高度值和磁传感器的测量数据作为外界辅助修正人员运动高度和方向。
惯性测量单元1在实时显示轨迹的同时，把轨迹文件保存在存储器中。保存的内容主要包括各步子的起点Al(xl,yl)、终点Al+1(xl+1,yl+1)和对应的原始数据帧号fl和fl+1(根据帧号、采样频域f可以换算出时间)等。
如图1—图2所示，一种矿下惯性测绘装置，包括相互连接的惯性测量单元1、信号处理单元2，所述惯性测量单元1包括加速度计11、陀螺仪12、磁传感器13、气压高度测量单元14，所述加速度计11、陀螺仪12、气压高度测量单元14、磁传感器13均与信号处理单元连接。通过矿下惯性测绘装置测量矿产外边沿水平面的多个点，得到当前矿产形状。气压高度测量单元14可采用气压计。气压高度测量单元14测量的数据为k时刻惯性测量单元1的气压高度。磁传感器13测量的数据为k时刻惯性测量单元1的磁场强度。
惯性测量单元1在实时显示轨迹的同时，把轨迹文件保存在存储器中。保存的内容主要包括各步子的起点Al(xl,yl)、终点Al+1(xl+1,yl+1)和对应的原始数据帧号fl和fl+1(根据帧号、采样频域f可以换算出时间)等。
本发明的矿下惯性测绘方法具体包括如下步骤：
(1)采集角速度值、加速度值、磁场强度，并进行姿态变换矩阵的计算与更新
利用惯性测绘装置中的加速度计和陀螺实时获取人员所佩戴位置运动的惯性测量数据，其中k时刻惯性测量单元1的加速度值和角速度值可分别表示为如下数组形式
Figure GDA0003465753480000101
Figure GDA0003465753480000102
气压高度为Baro_hk，磁场强度为
Figure GDA0003465753480000103
采样周期为T，T的范围为10Hz-1000Hz；
(1.2)利用角速度值和加速度值进行捷联惯导解算，获得人员的角度，速度，位置坐标(1.2.1)利用初始数据总长度为K1的加速度值和磁场强度均值获取初始姿态角，完成数据初始化
Figure GDA0003465753480000104
其中，横滚角γ、航向角ψ、俯仰角θ即为解算出的惯性测量单元1的姿态角；|g|为重力加速度模值；
Figure GDA0003465753480000105
为惯性测量单元1的磁场强度均值；
Figure GDA0003465753480000106
为惯性测量单元1的加速度值均值；采样长度K1取值为100；
(1.2.2)在完成数据初始化后，由γ、θ、ψ确定人员依据如下公式计算惯性测量单元的初始姿态变换矩阵
Figure GDA0003465753480000111
Figure GDA0003465753480000112
为0时刻到K1时刻所有时刻的加速度值的均值
Figure GDA0003465753480000113
为0时刻至K1时刻中各个时刻惯性测量单元1的磁场强度的均值，K1为采样长度，K1≥50；所述的步骤(2)中，k＝K1+1,K1+2,...。本发明中的实施例中，由于惯性测量单元1设置于人员身上(手部或足部)，因此惯性测量单元1所在的位置指的是人员身上的位置(人员手部或足部)。本发明中，惯性测量单元1所在的位置和人员的位置具有相同含义。
并利用K1+1之后的角速度值对初始姿态变换矩阵更新，并获取后续姿态变换矩阵，计算如下：
Figure GDA0003465753480000114
其中，上标n表示导航坐标系，下标b表示载体坐标系，(ωk×)表示角速度值
Figure GDA0003465753480000115
的反对陈矩阵，
Figure GDA0003465753480000116
若初始姿态变换矩阵由0时刻到K1时刻的数据计算，则下标变量的取值为k≥K1+1；
Figure GDA0003465753480000117
为k时刻惯性测量单元1的载体坐标系下的角速度值；
Figure GDA0003465753480000118
的初始值为
Figure GDA0003465753480000119
(2)利用姿态变换矩阵、载体坐标系下的加速度值计算导航坐标系下的加速度值，并计算k时刻惯性测量单元1的位置。
(2.1)计算惯性测量单元在导航坐标系下的加速度计值
利用姿态变换矩阵将载体坐标系下的加速度计投影到导航坐标系下，如下：
Figure GDA00034657534800001110
其中，
Figure GDA00034657534800001111
为惯性测量单元在导航坐标系下的加速度值，
Figure GDA00034657534800001112
为载体坐标系下惯性测量单元1的加速度值，
Figure GDA00034657534800001113
g为重力加速度。
(2.2)计算k时刻惯性测量单元1的位置
利用投影后的加速度计算惯性测量单元1的速度
Figure GDA00034657534800001114
和位置p*k
Figure GDA0003465753480000121
T为数据的采样周期，T为采样频率f的倒数，下标变量的取值为k≥K1+1，初始速度
Figure GDA0003465753480000122
和初始位置为
Figure GDA0003465753480000123
分别均取零。
(3)计算每个行进区间的静止时间区间和运动时间区间
每个行进区间由惯性测量单元1的静止时间区间和位于所述静止时间区间之后的运动时间区间组成。行进区间如图3所示，图3中A点代表第l个行进区间的起点；B点代表第l个行进区间的终点；B点也是第l+1个行进区间的起点；C点代表第l+1个行进区间的终点。利用加速度值和角速度值共同构建人员佩戴部位的“停”、“走”、“停”运动模式，并获取静止检测判断。静止时间区间即为惯性测量单元1为静止状态的时间区间。也就是人员足部或手部为静止状态的时间区间。运动时间区间即为惯性测量单元1为运动状态的时间区间。
(3.1)分别计算第k＞＝K1+1时刻的陀螺和加速度值的模值
Figure GDA0003465753480000124
(3.2)考虑到实际数据中高频误差较多为了增加检测精度减少误判，通过平滑处理数据的方式剔除数据中的毛刺抖动。对获取的第k个采样点的加速度总和
Figure GDA0003465753480000125
采用滑动取平均的方式进行数据平滑，选取滑动窗宽大小为W＝10，得到第k个采样点平滑后的加速度值
Figure GDA0003465753480000126
Figure GDA0003465753480000127
(3.3)利用对平滑后的加速度值
Figure GDA0003465753480000128
进行峰值检测，如果
Figure GDA0003465753480000129
则记录对应采样点对应帧号
Figure GDA00034657534800001210
将作为后续判断静止区间的辅助依据。peak_threshold优选值为20m/s2。
峰值检测不是用来记录最大的加速度值，而是要记录最大加速度出现的时间，对于一个实时处理的系统，我们这里就是保存了最大加速度出现的帧号，即最大加速度出现的时刻。
因为人员迈步，步子接触地面之前，或手部接触墙壁之前，即“停”的状态之前，会有一个很大的加速度冲击，即足部冲击地面或手部冲击墙面，因此产生峰值，在此峰值之后，即是“停”的状态，因此，判断帧号大于峰值出现的帧号，即可得到静止的时刻。
(3.4)分别对陀螺和加速度计的模值进行判断，获取静止检测依据，利用下式求PD1、PD2
Figure GDA0003465753480000131
Figure GDA0003465753480000132
其中，threshold_amin、threshold_amax分别为加速度模值的静止判断的下限阈值、上限阈值，threshold_ω为陀螺模值的静止判断阈值。只有当PD1和PD2同时为1，即PD1＝PD2＝1时，并且其对应帧号Framek大于相邻最近的加速度峰值出现的帧号
Figure GDA0003465753480000133
时才判断为静止。这样的判断有效的利用了人运动过程中的“停”、“走”、“停”规律性运动模式。当加速度的最大峰值出现的时候对应着“走”的运动状态；在陀螺和加速度计静止检测判断PD1和PD2同时满足时对应着“停”的运动状态。
本实施例中，优选加速度峰值阈值peak_threshold≥20m/s2，更优选peak_threshold＝20m/s2，陀螺模值的静止判断阈值threshold_ω的优选取值范围为0.4°/s～0.5°/s，加速度模值的静止判断的下限阈值threshold_amin的优选取值范围为9.5m/s2-9.6m/s2，加速度模值的静止判断的上限阈值threshold_amax的优选取值范围为10m/s2-10.1m/s2。
因为不管是佩戴在脚上或者在手部，在整个运动过程中，加速度的最大峰值一定是出现在佩戴部位刚刚接触地面或者矿壁那一个极短的时间内，因此需帧号大于峰值出现的帧号
Figure GDA0003465753480000134
才是静止。
这里加速度和陀螺的数据都取的是三轴数据的总模值，由于传感器是佩戴在人员身上的，因此在人员静止站立或者手臂接触矿壁的时候，总体的运动状态是静止但是并不能保证绝对的静止，因此在传感器的数据层面陀螺仪测量数据是表示为一个很小幅度的抖动，所以要求陀螺要小于某一个阈值；而对于加速度计，其感受到的加速度包含重力加速度和人运动带来的加速度，因此人员静止时加速度的模值是在重力加速度大小附近，所以采用的是一个区间阈值加以限制。
(4)对静止时间区间内惯性测量单元1的速度、位置坐标、姿态变换矩阵进行修正
在每个行进区间的静止时间区间内，利用卡尔曼滤波方法对惯性测量单元1的速度、位置、姿态变换矩阵进行修正，并计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1)，其中l代表行进区间的个数；；利用卡尔曼滤波构建惯性导航定位误差状态修正模型，结合静止检测策略，磁传感器的测量数据，气压高度数据修正人员运动状态的误差；若k时刻不是静止时刻，则不进行卡尔曼滤波的处理。
(4.1)选取卡尔曼滤波的状态量
Figure GDA0003465753480000141
选取卡尔曼滤波的量测量
Figure GDA0003465753480000142
下式进行卡尔曼滤波
Figure GDA0003465753480000143
其中，上标*＝x,y,z，δφk、δvk、δpk分别为惯性测量单元1的姿态角误差角、速度误差、位置误差，δφk＝(δγk,δθk,δψk)，
Figure GDA0003465753480000144
为惯性测量单元1的磁传感器输出角度，baro_hk为k时刻惯性测量单元1的气压高度，
Figure GDA0003465753480000145
γk、θk、ψk分别为k时刻惯性测量单元1的横滚角、俯仰角、航向角，
Figure GDA0003465753480000146
为k时刻惯性测量单元1的磁场强度。根据实际应用场景，卡尔曼滤波的相关参数可以做相应修改，本领域技术人员可以理解。
选取姿态误差角，速度误差，位置误差作为卡尔曼滤波状态量
Figure GDA0003465753480000147
结合静止检测策略，依据静止检测结果当将人员运动的过程判断为静止时，选取静止时刻下磁传感器计算角度
Figure GDA0003465753480000148
气压高度baro_hk和计算的速度
Figure GDA0003465753480000149
作为卡尔曼滤波的量测量
Figure GDA00034657534800001410
只有在静止时段才进行卡尔曼滤波修正，其他步骤一直迭代运算，因为只有在静止阶段，通过系统迭代运算状态量的改变才能表征在误差的累积。
其中，磁传感器输出角度
Figure GDA00034657534800001411
由当前最新姿态变换矩阵
Figure GDA00034657534800001412
通过对公式(2)取反变换获得当前水平姿态角(γk,θk,ψk)，利用水平姿态角和磁场强度计算磁传感器输出角度：
Figure GDA00034657534800001413
航向角误差量测量为
Figure GDA00034657534800001414
高度误差量测量为
Figure GDA00034657534800001415
速度误差量测量为
Figure GDA00034657534800001416
其中上标*＝x,y,z，下标k≥K1+1。
(4.2)利用卡尔曼滤波处理得到姿态误差δφ、速度误差δv、位置误差δp
利用姿态误差δφ、速度误差δv、位置误差δp对惯性测量单元1的姿态、速度、位置进行修正，得到滤波修正后惯性测量单元1的姿态变换矩阵
Figure GDA0003465753480000151
速度
Figure GDA0003465753480000152
位置
Figure GDA0003465753480000153
Figure GDA0003465753480000154
其中上标*＝x,y,z，下标k＞＝K1+1。利用滤波修正后惯性测量单元1的姿态变换矩阵
Figure GDA0003465753480000155
速度
Figure GDA0003465753480000156
位置
Figure GDA0003465753480000157
代替修正前的
Figure GDA0003465753480000158
进行后续计算，循环往复。
利用δφk、δvk、δpk对惯性测量单元1的姿态变换矩阵、速度、位置坐标进行修正，并返回到步骤(1)，利用修正后的姿态变换矩阵计算得到滤波修正后惯性测量单元的横滚角、俯仰角、航向角，利用滤波修正后惯性测量单元的姿态变换矩阵、速度、位置坐标、横滚角、俯仰角、航向角进行后续计算。
利用滤波修正后惯性测量单元的位置计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1)。
利用卡尔曼滤波修正后的惯性测量单元1的位置，即可计算行进区间的起点坐标、终点坐标，前一个行进区间的终点坐标即为后一个行进区间的起点坐标。
为了解决里程损失，拟采用积分距离动态补偿的方法，具体步骤如下：
第一步：在人员绕矿山行走之前，在矿下相同环境进行人工测量，即测量一个固定的路径的长度(已知的距离)，设为S；
第二步：利用沿直线行进的惯性测量单元1测量该路径，得到测量结果为Sc，即为惯性测量单元1对此已知距离的测量结果；
第三步：计算里程损失补偿比例系数r，
Figure GDA0003465753480000159
第四步：动态补偿积分距离，即假设按照常规计算进行位置更新时，积分公式为v×t，按照里程损失补偿比例系数r对其进行补偿，即求解如下方程组，计算里程补偿后的各个行进区间的终点坐标
Figure GDA00034657534800001510
其中r为里程损失补偿比例系数；且
Figure GDA0003465753480000161
S为矿下任意一段，Sc为沿直线行进的惯性测量单元1对所述已知的距离的测量的结果，
Figure GDA0003465753480000162
为里程补偿后第l个行进区间的终点坐标。
本发明中，里程损失补偿在Kalman滤波之后进行。
(5)矿山测量
当人员完成绕矿山行进，回到绕矿山行进的起点后，利用各个行进区间的起点坐标、终点坐标绘制人员行进轨迹，根据所述人员行进轨迹计算矿山形状；其中前一个行进区间的终点即为后一个行进区间的起点。本发明中，提出如下策略，即能使起点和终点闭合，又能修正行进过程中的航向漂移，提高面积测量的精度。根据保存的轨迹文件解决因航向漂移导致的曲线不闭合问题，具体步骤如下：
(5.1)判断人员绕矿山行进形成的轨迹的起点坐标和终点坐标是否重合，若所述轨迹的起点坐标和终点坐标未重合，则进行步骤(5.2)，否则，跳转到步骤(5.6)；
在实际使用中，轨迹的起点坐标和终点坐标的重合指的是起点坐标和终点坐标的相互关系满足一定条件，例如小于一定的距离值，即认为轨迹的起点和终点重合，本领域人员可以在实际使用中设置。人员绕矿山行进形成的轨迹的起点即为本次行进过程中人员第一个行进区间的起点，人员绕矿山行进形成的轨迹的终点即为本次行进过程中最后一个行进区间的终点。
(5.2)根据初始化时，惯性器件陀螺z轴取得的初始零偏wo，给出一个可能范围[wo-Δw0，wo+Δw0]和搜索步长δw，δw优选为0.0001rad/s，Δw0优选为0.005rad/s。
利用下式计算修正后的零偏值
Figure GDA0003465753480000163
Figure GDA0003465753480000164
其中，[w0-Δw0，w0+Δw0]为初始零偏w0的修正范围，δw为搜索步长，m为整数，且
Figure GDA0003465753480000165
(5.3)计算因静偏值的改变引起的各个行进区间的方向的改变，即利用下式计算零偏修正后惯性测量单元(1)的航向角
Figure GDA0003465753480000166
其中fl、fl+1分别为第l个行进区间的起点的帧号、终点的帧号，f为采样频率，其中ψl0根据步子的起点终点计算得到，
Figure GDA0003465753480000167
其中(xl,yl)、(xl+1,yl+1)分别为第l步的起点、终点坐标，(xl+1,yl+1)也是第l+1个行进区间的起点坐标，ψl'为零偏修正后惯性测量单元(1)的航向角，ψl0为零偏修正前的惯性测量单元(1)的航向角。
(5.4)求解如下方程组，计算零偏修正后惯性测量单元(1)在各个行进区间的终点坐标(xl′+1,yl′+1)
Figure GDA0003465753480000171
(5.5)查看整个轨迹的起点和终点之间的距离dm，若当m＝1时对应的起点和终点之间的距离相对原始距离d0，如果变小，但没有闭合，则继续增加m值，直到闭合；如果距离变大，取m＝-1，按照上述步骤重新计算dm，如果dm＜d0,则继续减少m，直到闭合。具体包括：
判断零偏修正后人员绕矿山行进形成的轨迹的起点坐标和终点坐标是否重合，若重合，则跳转到步骤(5.6)；否则，判断是否满足d′m＜dm，若d′m＜dm，则令m＝m+1，且跳转到步骤(5.2)，若d′m＞dm，则令m＝m-1，且跳转到步骤(5.2)，其中dm和d′m分别为对同一δw值的第m次零偏修正前、零偏修正后轨迹的起点和终点之间的距离；若
Figure GDA0003465753480000172
且
Figure GDA0003465753480000173
且第mn次零偏修正后轨迹的起点坐标和终点坐标未重合，则以δw′代替δw，且将初始零偏w0的修正范围修改为[min{w0+(mn-1)δw，w0+(mn+1)δw}，max{w0+(mn-1)δw，w0+(mn+1)δw}]，并跳转到步骤(5.2)，其中mn∈m，mn+1∈m，mn-1∈m；优选δw′取值不大于
Figure GDA0003465753480000174
(5.6)利用零偏修正后的各个行进区间的起点和终点形成的轨迹计算矿山形状。
本发明中，可通过人员的轨迹的起点和终点形成的轨迹计算矿产的平面面积，利用矿产的平面面积和高度相乘，得到矿产体积。也可通过各个测量矿山形状的三维坐标构成行进轨迹，直接计算矿产的立体体积。面积计算采用现有的面积算法即可。
人员的行进轨迹围成的区域的面积即为矿山面积。矿山形状即为矿山面积与矿山高度的乘积。
需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
以上对本发明/发明的实施例进行了详细说明，但所述内容仅为本发明/发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明/发明范围所作的均等变化与改进等，均应仍归属于本专利涵盖范围之内。在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落入本申请所附权利要求所限定的范围。在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

Claims (11)

1.一种矿下惯性测绘方法，其特征在于：惯性测量单元设置于人员身上，人员沿矿山外侧的矿道绕矿山行进，惯性测量单元包括加速度计(11)、陀螺仪(12)，以采样频率f采集k时刻惯性测量单元在载体坐标系下的加速度值
Figure FDA0003465753470000011
角速度值
Figure FDA0003465753470000012
所述矿下惯性测绘方法包括如下步骤：
(1)计算k时刻惯性测量单元的姿态变换矩阵
Figure FDA0003465753470000013
并利用下式对姿态变换矩阵
Figure FDA0003465753470000014
进行更新
Figure FDA0003465753470000015
其中
Figure FDA0003465753470000016
(2)利用下式计算k时刻惯性测量单元在导航坐标系下的加速度值
Figure FDA0003465753470000017
并将k时刻惯性测量单元在导航坐标系下的加速度值
Figure FDA0003465753470000018
进行积分得到k时刻惯性测量单元的速度和位置，其中
Figure FDA0003465753470000019
g为重力加速度值；
(3)计算人员的每个行进区间的静止时间区间，其中，人员行进过程由多个行进区间组成，每个行进区间由惯性测量单元的静止时间区间和位于所述静止时间区间之后的运动时间区间构成；
(4)在每个行进区间的静止时间区间内，利用卡尔曼滤波方法对惯性测量单元的速度、位置、姿态变换矩阵进行修正，并利用修正后的位置坐标计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1)，其中l代表行进区间的个数；
(5)当人员完成绕矿山行进，回到绕矿山行进的起点后，利用各个行进区间的起点坐标、终点坐标绘制人员行进轨迹，根据所述人员行进轨迹对矿山的形状和/或面积进行测量；其中前一个行进区间的终点即为后一个行进区间的起点。
2.根据权利要求1所述的矿下惯性测绘方法，其特征在于：所述步骤(4)中，求解如下方程组，计算里程补偿后的各个行进区间的终点坐标
Figure FDA00034657534700000110
其中r为里程损失补偿比例系数；r的计算公式为
Figure FDA0003465753470000021
其中S为矿下任意一段已知的距离，Sc为惯性测量单元对所述已知的距离的测量的结果，
Figure FDA0003465753470000022
为里程补偿后第l个行进区间的终点坐标。
3.根据权利要求1所述的矿下惯性测绘方法，其特征在于：所述步骤(5)包括如下步骤：
(5.1)判断人员绕矿山行进形成的轨迹的起点坐标和终点坐标是否重合，若所述轨迹的起点坐标和终点坐标未重合，则进行步骤(5.2)，否则，跳转到步骤(5.6)；
(5.2)利用下式计算修正后的初始零偏值
Figure FDA0003465753470000023
Figure FDA0003465753470000024
其中，[w0-Δw0，w0+Δw0]为初始零偏w0的修正范围，δw为搜索步长，m为修正次数，且
Figure FDA0003465753470000025
(5.3)利用下式计算零偏修正后惯性测量单元的航向角
Figure FDA0003465753470000026
其中fl、fl+1分别为第l个行进区间的起点的帧号、终点的帧号，f为采样频率，其中
Figure FDA0003465753470000027
(5.4)求解如下方程组，计算零偏修正后惯性测量单元在各个行进区间的终点坐标(x′l+1,y′l+1)
Figure FDA0003465753470000028
(5.5)判断零偏修正后人员绕矿山行进形成的轨迹的起点坐标和终点坐标是否重合，若重合，则跳转到步骤(5.6)；
否则，判断是否满足d′m＜dm，若d′m＜dm，则令m＝m+1，且跳转到步骤(5.2)，若d′m＞dm，则令m＝m-1，且跳转到步骤(5.2)，其中dm和d′m分别为第m次零偏修正前、零偏修正后轨迹的起点和终点之间的距离；
(5.6)利用零偏修正后的各个行进区间的起点和终点形成的轨迹对矿山的形状和/或面积进行测量。
4.根据权利要求3所述的矿下惯性测绘方法，其特征在于：所述步骤(5.5)中还包括：
若
Figure FDA0003465753470000031
且
Figure FDA0003465753470000032
且第mn次零偏修正后轨迹的起点坐标和终点坐标未重合，则以δ′w代替δw，且将初始零偏w0的修正范围修改为[min{w0+(mn-1)δw，w0+(mn+1)δw}，max{w0+(mn-1)δw，w0+(mn+1)δw}]，并跳转到步骤(5.2)，其中mn∈m；δ′w取值不大于
Figure FDA0003465753470000033
5.根据权利要求1所述的矿下惯性测绘方法，其特征在于：所述惯性测量单元设置于人员手部或足部；若所述惯性测量单元设置于人员足部，则从人员足部离开地面到人员足部再次接触地面为一个行进区间；若所述惯性测量单元设置于人员手部，则人员在行进过程中，不定时地用手部触碰矿山外侧的矿壁，且从人员手部离开矿壁到人员手部再次接触矿壁为一个行进区间。
6.根据权利要求1-5中任一项所述的矿下惯性测绘方法，其特征在于：所述步骤(3)中，计算惯性测量单元的所述静止时间区间的方法为：
计算第k时刻惯性测量单元的角速度值的模值
Figure FDA0003465753470000034
和第k时刻惯性测量单元的加速度值的模值
Figure FDA0003465753470000035
若
Figure FDA0003465753470000036
且
Figure FDA0003465753470000037
且
Figure FDA0003465753470000038
且k＞ka，且k时刻和ka时刻之间的时间差Δt≤0.1s，则判断k时刻为静止时刻；所述运动时间区间为相邻两个静止时间区间之间的时间区间；其中k、ka均为采样时刻，peak_threshold为加速度峰值阈值，threshold_amin、threshold_amax分别为加速度模值的静止判断的下限阈值、上限阈值，threshold_ω为陀螺模值的静止判断阈值。
7.根据权利要求6所述的矿下惯性测绘方法，其特征在于：所述步骤(3)中，第k时刻的加速度值的模值
Figure FDA0003465753470000039
为利用下式平滑后的加速度值的模值
Figure FDA00034657534700000310
其中，W为滑动窗宽，W≥5，
Figure FDA00034657534700000311
8.根据权利要求7所述的矿下惯性测绘方法，其特征在于：W＝10。
9.根据权利要求1-5中任一项所述的矿下惯性测绘方法，其特征在于：所述步骤(4)包括：
(4.1)选取卡尔曼滤波的状态量
Figure FDA0003465753470000041
选取卡尔曼滤波的量测量
Figure FDA0003465753470000042
下式进行卡尔曼滤波
Figure FDA0003465753470000043
其中，上标*＝x,y,z，δφk、δvk、δpk分别为k时刻惯性测量单元的姿态角误差角、速度误差、位置误差，δφk＝(δγk,δθk,δψk)，
Figure FDA0003465753470000044
为k时刻惯性测量单元的磁传感器输出角度，baro_hk为k时刻惯性测量单元的气压高度，
Figure FDA0003465753470000045
γk、θk、ψk分别为k时刻惯性测量单元的横滚角、俯仰角、航向角，(γk、θk、ψk)为k时刻惯性测量单元的姿态角，δγk、δθk、δψk分别为k时刻惯性测量单元的横滚角误差、俯仰角误差、航向角误差，
Figure FDA0003465753470000046
为k时刻惯性测量单元的磁场强度；
(4.2)利用δφk、δvk、δpk对惯性测量单元的姿态变换矩阵、速度、位置坐标进行修正，得到滤波修正后惯性测量单元的姿态变换矩阵、速度、位置，并返回到步骤(1)，利用滤波修正后惯性测量单元的姿态变换矩阵对惯性测量单元的横滚角、俯仰角、航向角进行修正；并利用滤波修正后惯性测量单元的位置计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1)。
10.根据权利要求1-5中任一项所述的矿下惯性测绘方法，其特征在于：所述的步骤(1)中，利用下式计算
Figure FDA0003465753470000047
Figure FDA0003465753470000048
其中，γk、θk、ψk分别为k时刻惯性测量单元的横滚角、俯仰角、航向角；
并利用下式计算γ0、θ0、ψ0
Figure FDA0003465753470000051
其中γ0、θ0、ψ0分别为惯性测量单元的初始横滚角、初始俯仰角、初始航向角，
Figure FDA0003465753470000052
分别利用k＝1时刻惯性测量单元在载体坐标系下的加速度值和磁场强度计算得到或利用k＝1时刻到k＝K1时刻之间所有加速度值的均值和磁场强度的均值计算得到；K1≥100。
11.一种利用权利要求1-10中任一项所述的矿下惯性测绘方法的矿下惯性测绘装置，其特征在于：包括信号处理单元，还包括设置于人员身上且均与所述信号处理单元连接的气压高度测量单元(14)、惯性测量单元，所述惯性测量单元包括加速度计(11)、陀螺仪(12)、磁传感器(13)；
所述加速度计(11)、陀螺仪(12)、磁传感器(13)分别用于以采样频率f采集惯性测量单元的角速度值、磁场强度以及在载体坐标系下的加速度值；
所述信号处理单元用于：计算惯性测量单元的姿态变换矩阵、计算惯性测量单元在导航坐标系下的加速度值、计算惯性测量单元的位置坐标、在每个行进区间的静止时间区间内，利用卡尔曼滤波方法对惯性测量单元的速度、位置、姿态变换矩阵进行修正，并计算每个行进区间的起点坐标、终点坐标，利用各个行进区间的起点位置、终点位置绘制人员的行进轨迹，根据所述行进轨迹对矿山的形状和/或面积进行测量。
CN201710526261.0A 2017-06-30 2017-06-30 一种矿下惯性测绘方法及装置 Active CN109211234B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710526261.0A CN109211234B (zh) 2017-06-30 2017-06-30 一种矿下惯性测绘方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710526261.0A CN109211234B (zh) 2017-06-30 2017-06-30 一种矿下惯性测绘方法及装置

Publications (2)

Publication Number Publication Date
CN109211234A CN109211234A (zh) 2019-01-15
CN109211234B true CN109211234B (zh) 2022-03-29

Family

ID=64977112

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710526261.0A Active CN109211234B (zh) 2017-06-30 2017-06-30 一种矿下惯性测绘方法及装置

Country Status (1)

Country Link
CN (1) CN109211234B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110579786A (zh) * 2019-07-19 2019-12-17 北京理工新源信息科技有限公司 定位方法及系统、导航方法及系统和一种车辆管理终端
CN112077881B (zh) * 2020-11-13 2021-01-29 浙江欣奕华智能科技有限公司 一种机器人移动性能的评估方法及其评估装置
CN113932791B (zh) * 2021-09-15 2023-11-07 江苏徐工工程机械研究院有限公司 一种露天矿区装卸作业区地图采集方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104406566A (zh) * 2014-12-11 2015-03-11 安徽惠洲地质安全研究院股份有限公司 一种矿井智能综合地质测量仪及其测量方法
CN104613966A (zh) * 2015-01-30 2015-05-13 东南大学 一种地籍测量事后数据处理方法
CN106441292A (zh) * 2016-09-28 2017-02-22 哈尔滨工业大学 一种基于众包imu惯导数据的楼宇室内平面图建立方法
CN106679649A (zh) * 2016-12-12 2017-05-17 浙江大学 一种手部运动追踪系统及追踪方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9792739B2 (en) * 2015-12-10 2017-10-17 Caterpillar Inc. Operation monitoring system for machine and method thereof

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104406566A (zh) * 2014-12-11 2015-03-11 安徽惠洲地质安全研究院股份有限公司 一种矿井智能综合地质测量仪及其测量方法
CN104613966A (zh) * 2015-01-30 2015-05-13 东南大学 一种地籍测量事后数据处理方法
CN106441292A (zh) * 2016-09-28 2017-02-22 哈尔滨工业大学 一种基于众包imu惯导数据的楼宇室内平面图建立方法
CN106679649A (zh) * 2016-12-12 2017-05-17 浙江大学 一种手部运动追踪系统及追踪方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
An improved PDR/magnetometer/floor map integration algorithm for ubiquitous positioning using the adaptive unscented Kalman filter;Wang J等;《ISPRS International Journal of Geo-Information》;20151125;第4卷(第4期);第2638-2659页 *
SINS/WSN下采煤机位姿监测系统及实验研究;应葆华;《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅰ辑》;20160215;第B021-189页 *
基于地理信息系统(GIS)的采煤机定位定姿技术研究;葛世荣 等;《煤炭学报》;20151130;第40卷(第01期);第2503-2508页 *

Also Published As

Publication number Publication date
CN109211234A (zh) 2019-01-15

Similar Documents

Publication Publication Date Title
CN109827577B (zh) 基于运动状态检测的高精度惯性导航定位算法
EP3819592B1 (en) Positioning device, positioning method, and program
CN110146077A (zh) 移动机器人姿态角解算方法
EP3418692B1 (en) Method and device for detecting pedestrian stride length and walking path
CN103335647B (zh) 一种盾构机姿态测量系统及其测量方法
CN109211234B (zh) 一种矿下惯性测绘方法及装置
CN107063305B (zh) 用惯导、压力传感器修正下坡悬空后轮里程计误差的方法
US20150362330A1 (en) Method and System for Varying Step Length Estimation Using Nonlinear System Identification
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN107490378B (zh) 一种基于mpu6050与智能手机的室内定位与导航的方法
CN105074382B (zh) 自卸卡车
CN104613965B (zh) 一种基于双向滤波平滑技术的步进式行人导航方法
CN105371846B (zh) 载体姿态检测方法及其系统
CN111024126A (zh) 一种行人导航定位中的自适应零速修正方法
CN106153069A (zh) 自主导航系统中的姿态修正装置和方法
CN113236363A (zh) 开采设备导航定位方法、系统、设备及可读存储介质
CN108592907A (zh) 一种基于双向滤波平滑技术的准实时步进式行人导航方法
CN107782307A (zh) 一种sins/dr组合导航系统里程计异常数据后处理方法
CN113029148A (zh) 一种基于航向角准确修正的惯性导航室内定位方法
CN106500649A (zh) 一种基于惯性测量与轨道信标组合技术的矿井下车辆轨道变形监测方法
CN115597535A (zh) 基于惯性导航的高速磁悬浮轨道不平顺检测系统及方法
KR101226767B1 (ko) 주행 장치를 위한 위치측정 시스템 및 방법
KR101356347B1 (ko) 보행용 관성 항법 시스템 및 이를 이용한 경사각 및 높이 성분의 추정 방법
CN114719858B (zh) 一种基于imu和楼层高度目标补偿的3维定位方法
KR101208717B1 (ko) 차량의 경사각 보정방법 및 그 보정장치

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information

Address after: 410205 No. 5, 4th Floor, Building F4, Yuyuan, Lugu, 27 Wenxuan Road, Changsha High-tech Development Zone, Hunan Province

Applicant after: Hunan Wagner micro Mdt InfoTech Ltd

Address before: 410205 Room 1610, 16th Floor, Headquarters Building, Zhongdian Software Park, 39 Jianshan Road, Changsha High-tech Development Zone, Hunan Province

Applicant before: Hunan Wagner micro Mdt InfoTech Ltd

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address

Address after: 410205 1705, building 1b, Hunan military civilian integration science and Technology Innovation Industrial Park, No. 699, Qingshan Road, Changsha high tech Development Zone, Changsha, Hunan

Patentee after: Hunan yunjiangna micro Information Technology Co.,Ltd.

Address before: 410205 No. 5, 4th Floor, Building F4, Yuyuan, Lugu, 27 Wenxuan Road, Changsha High-tech Development Zone, Hunan Province

Patentee before: HUNAN GLONAVIN INFORMATION TECHNOLOGY CO.,LTD.

CP03 Change of name, title or address