发明内容
本发明要解决的问题是针对现有矿下测量方法和装置无法满足经常性测量要求及存在的上述问题,提供一种矿下惯性测绘方法,惯性测量单元设置于人员身上,人员沿矿山外侧的矿道绕矿山行进,惯性测量单元包括加速度计、陀螺仪,以采样频率f采集k时刻惯性测量单元在载体坐标系下的加速度值
角速度值
所述矿下惯性测绘方法包括如下步骤:
(1)计算k时刻惯性测量单元的姿态变换矩阵
并利用下式对姿态变换矩阵
进行更新
(2)利用下式计算k时刻惯性测量单元在导航坐标系下的加速度值
并将k时刻惯性测量单元在导航坐标系下的加速度值
进行积分得到k时刻惯性测量单元的位置,其中
g为重力加速度值;
(3)计算人员的每个行进区间的静止时间区间,其中,人员行进过程由多个行进区间组成,每个行进区间由惯性测量单元的静止时间区间和位于所述静止时间区间之后的运动时间区间组成;
(4)在每个行进区间的静止时间区间内,利用卡尔曼滤波方法对惯性测量单元的速度、位置、姿态变换矩阵进行修正,并计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1),其中l代表行进区间的个数;
(5)当人员完成绕矿山行进,回到绕矿山行进的起点后,利用各个行进区间的起点坐标、终点坐标绘制人员行进轨迹,根据所述人员行进轨迹对矿山的形状和/或面积进行测量;其中前一个行进区间的终点即为后一个行进区间的起点。
本发明中,将惯性测量单元设置于人员身上,通过惯性测量单元的角速度值、加速度值计算惯性测量单元的速度和位置坐标。同时,利用卡尔曼滤波构建惯性导航定位误差状态修正模型,结合静止检测策略,对惯性测量单元的速度、位置坐标、姿态变换矩阵进行修正,通过对姿态变换矩阵进行修正提高了迭代运算精度,通过对速度、位置坐标进行修正提高了位置坐标的测量精度,从而提高矿山的测量精度。本发明中,只有在静止时间区间才进行卡尔曼滤波修正,其他步骤一直迭代运算,因为只有当惯性测量单元处于静止时,通过系统迭代运算状态量的改变才能表征在误差的累积。当人员完成绕矿山行进,回到绕矿山行进的起点后,利用各个行进区间的起点坐标、终点坐标绘制人员行进轨迹,根据所述人员行进轨迹即可计算矿山形状和/或面积。利用矿产的平面面积和高度相乘,即可得到矿产体积。也可通过各个测量的坐标直接计算矿产的立体体积。本发明中,仅需人员携带惯性测量单元,绕矿行走一周即可计算得到矿产的形状和/或面积,测量方法简单,可在需要时随时进行测量,测量成本低,且受环境的影响小。由于本发明的测量方法简单,因此可以满足经常性测量要求。
上述技术方案中,所述步骤(4)中,求解如下方程组,计算里程补偿后的各个行进区间的终点坐标
其中r为里程损失补偿比例系数;优选r的计算公式为
其中S为矿下任意一段已知的距离,S
c为惯性测量单元对所述已知的距离的测量的结果,
为里程补偿后第l个行进区间的终点坐标。
本发明中,为了解决里程损失,采用积分距离动态补偿的方法,利用里程损失补偿比例系数对惯性测量单元计算得到的距离进行补偿,减小了测量误差,提高了测量精度。在相同环境的矿下,利用惯性测量单元对任意一段已知的距离进行测量,可以得到里程损失补偿比例系数。
上述技术方案中,所述步骤(5)包括如下步骤:
(5.1)判断人员绕矿山行进形成的轨迹的起点坐标和终点坐标是否重合,若所述轨迹的起点坐标和终点坐标未重合,则进行步骤(5.2),否则,跳转到步骤(5.6);
其中,[w
0-Δw
0,w
0+Δw
0]为初始零偏w
0的修正范围,δ
w为搜索步长,m为修正次数,且
(5.3)利用下式计算零偏修正后惯性测量单元的航向角
其中f
l、f
l+1分别为第l个行进区间的起点的帧号、终点的帧号,f为采样频率,其中
(x
l,y
l)、(x
l+1,y
l+1)分别为第l个行进区间的起点坐标、终点坐标;
(5.4)求解如下方程组,计算零偏修正后惯性测量单元在各个行进区间的终点坐标(x′l+1,y′l+1)
(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)中还包括:
若
且
且第m
n次零偏修正后轨迹的起点坐标和终点坐标未重合,则以δ
w′代替δ
w,且将初始零偏w
0的修正范围修改为[min{w
0+(m
n-1)δ
w,w
0+(m
n+1)δ
w},max{w
0+(m
n-1)δ
w,w
0+(m
n+1)δ
w}],并跳转到步骤(5.2),其中m
n∈m;优选δ′
w取值不大于
本发明中,对航向角进行修正的过程中,可能出现所设置的搜索步长过大,仍使得人员行进的轨迹的起点位置和终点位置无法闭合的情况,因此,若
且
且
均不为0,即始终无法找到可以令轨迹闭合的值,则减小搜索步长,再对航向角进行修正。
上述技术方案中,所述惯性测量单元设置于人员手部或足部;若所述矿下惯性测量单元设置于人员足部,则从人员足部离开地面到人员足部再次接触地面为一个行进区间;若所述惯性测量单元设置于人员手部,则人员在行进过程中,不定时地用手部触碰矿山外侧的矿壁,且从人员手部离开矿壁到人员手部再次接触矿壁为一个行进区间。
惯性测量单元设置于手部或足部时,对行进区间的定义不同,因此在处理过程中,以足部离开地面到人员足部再次接触地面为一个行进区间作为足部的行进区间,以手部离开矿壁到人员手部再次接触矿壁为一个行进区间,进行后续处理。
上述技术方案中,所述步骤(3)中,计算惯性测量单元的所述静止时间区间的方法为:计算第k时刻惯性测量单元的角速度值的模值
和第k时刻惯性测量单元的加速度值的模值
若
且
且
且k>k
a,且k时刻和k
a时刻之间的时间差Δt≤0.1s,则判断k时刻为静止时刻;所述运动时间区间为相邻两个静止时间区间之间的时间区间;其中k、k
a均为采样时刻,peak_threshold为加速度峰值阈值,threshold_amin、threshold_amax分别为加速度模值的静止判断的下限阈值、上限阈值,threshold_ω为陀螺模值的静止判断阈值。
上述技术方案中,所述步骤(3)中,第k时刻惯性测量单元的加速度值的模值Fk all为利用下式平滑后的加速度值的模值
本发明中,通过对惯性测量单元的加速度值的模值进行平滑,可以使得数据的趋势更加明显,便于进行处理和判断。
上述技术方案中,所述步骤(4)包括:
(4.1)选取卡尔曼滤波的状态量
选取卡尔曼滤波的量测量
下式进行卡尔曼滤波
其中,上标*=x,y,z,δφ
k、δv
k、δp
k分别为k时刻惯性测量单元的姿态角误差角、速度误差、位置误差,δφ
k=(δγ
k,δθ
k,δψ
k),
为k时刻惯性测量单元的磁传感器输出角度,baro_h
k为k时刻惯性测量单元的气压高度,
γ
k、θ
k、ψ
k分别为k时刻惯性测量单元的横滚角、俯仰角、航向角,(γ
k、θ
k、ψ
k)为k时刻惯性测量单元的姿态角,δγ
k、δθ
k、δψ
k分别为k时刻惯性测量单元的横滚角误差、俯仰角误差、航向角误差,
为k时刻惯性测量单元的磁场强度;
(4.2)利用δφk、δvk、δpk对惯性测量单元的姿态变换矩阵、速度、位置坐标进行修正,得到滤波修正后惯性测量单元的姿态变换矩阵、速度、位置,并返回到步骤(1),利用滤波修正后惯性测量单元的姿态变换矩阵对惯性测量单元的横滚角、俯仰角、航向角进行修正;并利用修正后的位置坐标计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1)。
其中,γk、θk、ψk分别为k时刻惯性测量单元的横滚角、俯仰角、航向角;
并利用下式计算γ0、θ0、ψ0
其中γ
0、θ
0、ψ
0分别为惯性测量单元的初始横滚角、初始俯仰角、初始航向角,
分别利用k=1时刻惯性测量单元在载体坐标系下的加速度值、磁场强度计算得到或利用k=1时刻到k=K
1时刻之间所有数据求均值计算得到;优选K
1≥100。
步骤(4.2)中,利用修正后的姿态变换矩阵、速度、位置坐标、横滚角、俯仰角、航向角进行后续计算,即进行步骤(1)的计算。
本发明中,对k=1时刻到k=K1时刻之间所有数据求均值计算得到加速度值、磁场强度,通过取均值减小了数据中的随机误差,提高了运算精度。
本发明还提供一种利用上述矿下惯性测绘方法的矿下惯性测绘装置,包括信号处理单元,还包括设置于人员身上且均与所述信号处理单元连接的惯性测量单元,所述惯性测量单元包括加速度计、陀螺仪、磁传感器;
所述加速度计、陀螺仪、磁传感器分别用于以采样频率f采集惯性测量单元的角速度值、磁场强度以及在载体坐标系下的加速度值;
所述信号处理单元用于:计算姿态变换矩阵、计算惯性测量单元在导航坐标系下的加速度值、计算惯性测量单元的位置坐标、在每个行进区间的静止时间区间内,利用卡尔曼滤波方法对惯性测量单元的速度、位置、姿态变换矩阵进行修正,并计算每个行进区间的起点坐标、终点坐标,利用各个行进区间的起点坐标、终点坐标绘制人员的行进轨迹,根据所述行进轨迹对矿山的形状和/或面积进行测量。
本发明具有的优点和积极效果是:本发明中,将特殊的惯性定位方法应用到矿产测量中,测量方便,不受空间限制;采用积分距离动态补偿的方法,利用里程损失补偿比例系数对惯性测量单元计算得到的距离进行补偿;解决了因航向漂移导致的曲线不闭合问题,通过逐步调整航向角,使得起点坐标和终点坐标重合,从而形成封闭路线,即能使起点和终点闭合,又能修正行走过程中的航向漂移,提高面积测量的精度,方便对整个矿山形状的计算。本发明测量方法简单,易于操作,大大降低了矿山形状的测量成本。
具体实施方式
下面将结合本申请的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
对于本领域的普通技术人员而言,可以通过具体情况理解本发明中术语在本发明中的具体含义。本发明中的实施例中,定义惯性测量单元1的位置、气压高度测量单元14的位置、人员的位置是同一个位置。
实施例
如图1所示,本发明提供一种矿下惯性测绘方法,气压高度测量单元14、设置于人员身上的惯性测量单元1均与信号处理单元2连接,以采样频率f采集k时刻惯性测量单元1在载体坐标系下的加速度值
角速度值
人员沿矿山外侧的矿道绕矿山行进。人员的行进过程由多个行进区间组成,每个行进区间由惯性测量单元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的加速度值和角速度值可分别表示为如下数组形式
气压高度为Baro_h
k,磁场强度为
采样周期为T,T的范围为10Hz-1000Hz;
(1.2)利用角速度值和加速度值进行捷联惯导解算,获得人员的角度,速度,位置坐标(1.2.1)利用初始数据总长度为K1的加速度值和磁场强度均值获取初始姿态角,完成数据初始化
其中,横滚角γ、航向角ψ、俯仰角θ即为解算出的惯性测量单元1的姿态角;|g|为重力加速度模值;
为惯性测量单元1的磁场强度均值;
为惯性测量单元1的加速度值均值;采样长度K
1取值为100;
(1.2.2)在完成数据初始化后,由γ、θ、ψ确定人员依据如下公式计算惯性测量单元的初始姿态变换矩阵
为0时刻到K
1时刻所有时刻的加速度值的均值
为0时刻至K
1时刻中各个时刻惯性测量单元1的磁场强度的均值,K
1为采样长度,K
1≥50;所述的步骤(2)中,k=K
1+1,K
1+2,...。本发明中的实施例中,由于惯性测量单元1设置于人员身上(手部或足部),因此惯性测量单元1所在的位置指的是人员身上的位置(人员手部或足部)。本发明中,惯性测量单元1所在的位置和人员的位置具有相同含义。
并利用K1+1之后的角速度值对初始姿态变换矩阵更新,并获取后续姿态变换矩阵,计算如下:
其中,上标n表示导航坐标系,下标b表示载体坐标系,(ω
k×)表示角速度值
的反对陈矩阵,
若初始姿态变换矩阵由0时刻到K
1时刻的数据计算,则下标变量的取值为k≥K
1+1;
为k时刻惯性测量单元1的载体坐标系下的角速度值;
的初始值为
(2)利用姿态变换矩阵、载体坐标系下的加速度值计算导航坐标系下的加速度值,并计算k时刻惯性测量单元1的位置。
(2.1)计算惯性测量单元在导航坐标系下的加速度计值
利用姿态变换矩阵将载体坐标系下的加速度计投影到导航坐标系下,如下:
其中,
为惯性测量单元在导航坐标系下的加速度值,
为载体坐标系下惯性测量单元1的加速度值,
g为重力加速度。
(2.2)计算k时刻惯性测量单元1的位置
利用投影后的加速度计算惯性测量单元1的速度
和位置p*
k
T为数据的采样周期,T为采样频率f的倒数,下标变量的取值为k≥K
1+1,初始速度
和初始位置为
分别均取零。
(3)计算每个行进区间的静止时间区间和运动时间区间
每个行进区间由惯性测量单元1的静止时间区间和位于所述静止时间区间之后的运动时间区间组成。行进区间如图3所示,图3中A点代表第l个行进区间的起点;B点代表第l个行进区间的终点;B点也是第l+1个行进区间的起点;C点代表第l+1个行进区间的终点。利用加速度值和角速度值共同构建人员佩戴部位的“停”、“走”、“停”运动模式,并获取静止检测判断。静止时间区间即为惯性测量单元1为静止状态的时间区间。也就是人员足部或手部为静止状态的时间区间。运动时间区间即为惯性测量单元1为运动状态的时间区间。
(3.1)分别计算第k>=K1+1时刻的陀螺和加速度值的模值
(3.2)考虑到实际数据中高频误差较多为了增加检测精度减少误判,通过平滑处理数据的方式剔除数据中的毛刺抖动。对获取的第k个采样点的加速度总和
采用滑动取平均的方式进行数据平滑,选取滑动窗宽大小为W=10,得到第k个采样点平滑后的加速度值
(3.3)利用对平滑后的加速度值
进行峰值检测,如果
则记录对应采样点对应帧号
将作为后续判断静止区间的辅助依据。peak_threshold优选值为20m/s
2。
峰值检测不是用来记录最大的加速度值,而是要记录最大加速度出现的时间,对于一个实时处理的系统,我们这里就是保存了最大加速度出现的帧号,即最大加速度出现的时刻。
因为人员迈步,步子接触地面之前,或手部接触墙壁之前,即“停”的状态之前,会有一个很大的加速度冲击,即足部冲击地面或手部冲击墙面,因此产生峰值,在此峰值之后,即是“停”的状态,因此,判断帧号大于峰值出现的帧号,即可得到静止的时刻。
(3.4)分别对陀螺和加速度计的模值进行判断,获取静止检测依据,利用下式求PD1、PD2
其中,threshold_amin、threshold_amax分别为加速度模值的静止判断的下限阈值、上限阈值,threshold_ω为陀螺模值的静止判断阈值。只有当PD
1和PD
2同时为1,即PD
1=PD
2=1时,并且其对应帧号Frame
k大于相邻最近的加速度峰值出现的帧号
时才判断为静止。这样的判断有效的利用了人运动过程中的“停”、“走”、“停”规律性运动模式。当加速度的最大峰值出现的时候对应着“走”的运动状态;在陀螺和加速度计静止检测判断PD
1和PD
2同时满足时对应着“停”的运动状态。
本实施例中,优选加速度峰值阈值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。
因为不管是佩戴在脚上或者在手部,在整个运动过程中,加速度的最大峰值一定是出现在佩戴部位刚刚接触地面或者矿壁那一个极短的时间内,因此需帧号大于峰值出现的帧号
才是静止。
这里加速度和陀螺的数据都取的是三轴数据的总模值,由于传感器是佩戴在人员身上的,因此在人员静止站立或者手臂接触矿壁的时候,总体的运动状态是静止但是并不能保证绝对的静止,因此在传感器的数据层面陀螺仪测量数据是表示为一个很小幅度的抖动,所以要求陀螺要小于某一个阈值;而对于加速度计,其感受到的加速度包含重力加速度和人运动带来的加速度,因此人员静止时加速度的模值是在重力加速度大小附近,所以采用的是一个区间阈值加以限制。
(4)对静止时间区间内惯性测量单元1的速度、位置坐标、姿态变换矩阵进行修正
在每个行进区间的静止时间区间内,利用卡尔曼滤波方法对惯性测量单元1的速度、位置、姿态变换矩阵进行修正,并计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1),其中l代表行进区间的个数;;利用卡尔曼滤波构建惯性导航定位误差状态修正模型,结合静止检测策略,磁传感器的测量数据,气压高度数据修正人员运动状态的误差;若k时刻不是静止时刻,则不进行卡尔曼滤波的处理。
(4.1)选取卡尔曼滤波的状态量
选取卡尔曼滤波的量测量
下式进行卡尔曼滤波
其中,上标*=x,y,z,δφ
k、δv
k、δp
k分别为惯性测量单元1的姿态角误差角、速度误差、位置误差,δφ
k=(δγ
k,δθ
k,δψ
k),
为惯性测量单元1的磁传感器输出角度,baro_h
k为k时刻惯性测量单元1的气压高度,
γ
k、θ
k、ψ
k分别为k时刻惯性测量单元1的横滚角、俯仰角、航向角,
为k时刻惯性测量单元1的磁场强度。根据实际应用场景,卡尔曼滤波的相关参数可以做相应修改,本领域技术人员可以理解。
选取姿态误差角,速度误差,位置误差作为卡尔曼滤波状态量
结合静止检测策略,依据静止检测结果当将人员运动的过程判断为静止时,选取静止时刻下磁传感器计算角度
气压高度baro_h
k和计算的速度
作为卡尔曼滤波的量测量
只有在静止时段才进行卡尔曼滤波修正,其他步骤一直迭代运算,因为只有在静止阶段,通过系统迭代运算状态量的改变才能表征在误差的累积。
其中,磁传感器输出角度
由当前最新姿态变换矩阵
通过对公式(2)取反变换获得当前水平姿态角(γ
k,θ
k,ψ
k),利用水平姿态角和磁场强度计算磁传感器输出角度:
航向角误差量测量为
高度误差量测量为
速度误差量测量为
其中上标*=x,y,z,下标k≥K
1+1。
(4.2)利用卡尔曼滤波处理得到姿态误差δφ、速度误差δv、位置误差δp
利用姿态误差δφ、速度误差δv、位置误差δp对惯性测量单元1的姿态、速度、位置进行修正,得到滤波修正后惯性测量单元1的姿态变换矩阵
速度
位置
其中上标*=x,y,z,下标k>=K
1+1。利用滤波修正后惯性测量单元1的姿态变换矩阵
速度
位置
代替修正前的
进行后续计算,循环往复。
利用δφk、δvk、δpk对惯性测量单元1的姿态变换矩阵、速度、位置坐标进行修正,并返回到步骤(1),利用修正后的姿态变换矩阵计算得到滤波修正后惯性测量单元的横滚角、俯仰角、航向角,利用滤波修正后惯性测量单元的姿态变换矩阵、速度、位置坐标、横滚角、俯仰角、航向角进行后续计算。
利用滤波修正后惯性测量单元的位置计算每个行进区间的起点坐标(xl,yl)、终点坐标(xl+1,yl+1)。
利用卡尔曼滤波修正后的惯性测量单元1的位置,即可计算行进区间的起点坐标、终点坐标,前一个行进区间的终点坐标即为后一个行进区间的起点坐标。
为了解决里程损失,拟采用积分距离动态补偿的方法,具体步骤如下:
第一步:在人员绕矿山行走之前,在矿下相同环境进行人工测量,即测量一个固定的路径的长度(已知的距离),设为S;
第二步:利用沿直线行进的惯性测量单元1测量该路径,得到测量结果为Sc,即为惯性测量单元1对此已知距离的测量结果;
第四步:动态补偿积分距离,即假设按照常规计算进行位置更新时,积分公式为v×t,按照里程损失补偿比例系数r对其进行补偿,即求解如下方程组,计算里程补偿后的各个行进区间的终点坐标
其中r为里程损失补偿比例系数;且
S为矿下任意一段,S
c为沿直线行进的惯性测量单元1对所述已知的距离的测量的结果,
为里程补偿后第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。
其中,[w
0-Δw
0,w
0+Δw
0]为初始零偏w
0的修正范围,δ
w为搜索步长,m为整数,且
(5.3)计算因静偏值的改变引起的各个行进区间的方向的改变,即利用下式计算零偏修正后惯性测量单元(1)的航向角
其中f
l、f
l+1分别为第l个行进区间的起点的帧号、终点的帧号,f为采样频率,其中ψ
l0根据步子的起点终点计算得到,
其中(x
l,y
l)、(x
l+1,y
l+1)分别为第l步的起点、终点坐标,(x
l+1,y
l+1)也是第l+1个行进区间的起点坐标,ψ
l'为零偏修正后惯性测量单元(1)的航向角,ψ
l0为零偏修正前的惯性测量单元(1)的航向角。
(5.4)求解如下方程组,计算零偏修正后惯性测量单元(1)在各个行进区间的终点坐标(xl′+1,yl′+1)
(5.5)查看整个轨迹的起点和终点之间的距离dm,若当m=1时对应的起点和终点之间的距离相对原始距离d0,如果变小,但没有闭合,则继续增加m值,直到闭合;如果距离变大,取m=-1,按照上述步骤重新计算dm,如果dm<d0,则继续减少m,直到闭合。具体包括:
判断零偏修正后人员绕矿山行进形成的轨迹的起点坐标和终点坐标是否重合,若重合,则跳转到步骤(5.6);否则,判断是否满足d′
m<d
m,若d′
m<d
m,则令m=m+1,且跳转到步骤(5.2),若d′
m>d
m,则令m=m-1,且跳转到步骤(5.2),其中d
m和d′
m分别为对同一δ
w值的第m次零偏修正前、零偏修正后轨迹的起点和终点之间的距离;若
且
且第m
n次零偏修正后轨迹的起点坐标和终点坐标未重合,则以δ
w′代替δ
w,且将初始零偏w
0的修正范围修改为[min{w
0+(m
n-1)δ
w,w
0+(m
n+1)δ
w},max{w
0+(m
n-1)δ
w,w
0+(m
n+1)δ
w}],并跳转到步骤(5.2),其中m
n∈m,m
n+1∈m,m
n-1∈m;优选δ
w′取值不大于
(5.6)利用零偏修正后的各个行进区间的起点和终点形成的轨迹计算矿山形状。
本发明中,可通过人员的轨迹的起点和终点形成的轨迹计算矿产的平面面积,利用矿产的平面面积和高度相乘,得到矿产体积。也可通过各个测量矿山形状的三维坐标构成行进轨迹,直接计算矿产的立体体积。面积计算采用现有的面积算法即可。
人员的行进轨迹围成的区域的面积即为矿山面积。矿山形状即为矿山面积与矿山高度的乘积。
需要说明的是,本说明书中的各个实施例均采用递进的方式描述,每个实施例重点说明的都是与其他实施例的不同之处,各个实施例之间相同相似的部分互相参见即可。
以上对本发明/发明的实施例进行了详细说明,但所述内容仅为本发明/发明的较佳实施例,不能被认为用于限定本发明的实施范围。凡依本发明/发明范围所作的均等变化与改进等,均应仍归属于本专利涵盖范围之内。在阅读了本发明之后,本领域技术人员对本发明的各种等价形式的修改均落入本申请所附权利要求所限定的范围。在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。