CN114383609A - 基于捷联惯导和里程计的组合导航矿井定位方法和装置 - Google Patents

Abstract

本发明提供基于捷联惯导和里程计的组合导航矿井定位方法，包括获取由捷联惯导系统采集的角速率增量和加速度增量；对角速率增量进行圆锥误差补偿，对加速度增量进行划船误差补偿；构建机身坐标系与地理坐标系的转换矩阵，提取出井下载体在地理坐标系的姿态，并通过姿态矩阵转换获取井下载体在地理坐标系的位置信息；获取里程计采集的里程增量，通过姿态矩阵转换获取里程计在地理坐标系的位置信息；对捷联系统位置信息与里程计位置信息建立量测方程，构建卡尔曼滤波方程来校正捷联惯导系统的位置信息，得到最优的捷联惯导系统的位置信息。本发明还提供了基于捷联惯导和里程计的组合导航矿井定位装置。

Description

基于捷联惯导和里程计的组合导航矿井定位方法和装置

技术领域

本发明涉及煤机定位技术领域，尤其涉及一种基于捷联惯导和里程计的组合导航矿井定位方法。

背景技术

当前，煤矿井下设备运输任务繁重，有出现碰撞翻车等事故的可能。因此，有必要对煤矿井下设备进行导航定位。

相关技术中，有的方案提出利用磁场强度计算当前井下设备的当期位置，也有方案提出利用若干基站沿巷道沿伸方向分布设置，解析数据报文以获得定位位置。

在实现本发明过程中，发明人发现现有技术中至少存在以下问题：磁场定位方式，极容易受到磁场干扰导致定位失败，采用定位基站的方式，需要在行径路线上铺设大量的基站，大大增加了设备成本，而且这两种定位方式数据刷新率低，并且上述方法采用的均为定位方法，只能测试设备的当前位置，无法检测设备的运动姿态与速度。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。

为此，本发明的目的在于提出一种实现对煤机设备的精准定位、定向的基于捷联惯导和里程计的组合导航矿井定位方法和装置。

为达到上述目的，本发明第一方面提供一种基于捷联惯导和里程计的组合导航矿井定位方法，包括：

获取由捷联惯导系统采集的角速率增量和加速度增量，其中捷联惯导系统安装在井下载体上，捷联惯导系统包括陀螺仪和加速度计；

对角速率增量进行圆锥误差补偿，对加速度增量进行划船误差补偿；

构建机身坐标系与地理坐标系的转换矩阵，提取出井下载体在地理坐标系的姿态，并通过姿态矩阵转换获取井下载体在地理坐标系的位置信息；

获取里程计采集的里程增量，通过姿态矩阵转换获取里程计在地理坐标系的位置信息，其中里程计安装在井下载体上；

对捷联系统位置与里程计位置信息构建卡尔曼滤波方程，利用里程计测出的里程增量来校正捷联惯导系统的位置信息，得到最优的捷联惯导系统的位置信息。

本发明第二方面提供一种基于捷联惯导和里程计的组合导航矿井定位装置，包括：

角速率增量和加速度增量采集单元，用于获取由捷联惯导系统采集的角速率增量和加速度增量，其中捷联惯导系统安装在井下载体上，捷联惯导系统包括陀螺仪和加速度计；

误差补偿单元，用于对角速率增量进行圆锥误差补偿，对加速度增量进行划船误差补偿；

井下载体位置信息获取单元，构建机身坐标系与地理坐标系的转换矩阵，提取出井下载体在地理坐标系的姿态，并通过姿态矩阵转换获取井下载体在地理坐标系的位置信息；

里程计位置信息获取单元，用于获取里程计采集的里程增量，通过姿态矩阵转换获取里程计在地理坐标系的位置信息，其中里程计安装在井下载体上；

捷联惯导系统位置信息获取单元，用于对捷联系统位置与里程计位置信息构建卡尔曼滤波方程，利用里程计测出的里程增量来校正捷联惯导系统的位置信息，得到最优的捷联惯导系统的位置信息。

本发明第三方面提供一种基于捷联惯导和里程计的组合导航矿井定位装置，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如上述第一方面所述方法的步骤。

本发明第四方面提供一种计算机可读存储介质，所述计算机程序被处理器执行时实现如上述第一方面所述方法的步骤。

本发明的有益效果是：

本发明用圆锥误差补偿方法补偿煤机设备在振动环境下给捷联惯导带来的不可交换误差，用划船误差补偿方法补偿由于同频不同轴的角振动和线振动给捷联惯导带来的误差，利用捷联惯导和里程计的优势补偿捷联惯导的累积误差，实现对煤机设备的精准定位、定向。本发明可以保证长距离导航参数的准确有效，实时精确监测设备当前的位置点以及运动特征。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1是本发明一实施例提出的基于捷联惯导和里程计的组合导航矿井定位方法的流程示意图。

图2是本发明一实施例提出的基于捷联惯导和里程计的组合导航矿井定位方法的原理示意图。

图3是本发明一实施例提出的机身坐标系、地理坐标系和里程计坐标系的示意图。

图4是本发明一实施例提出的基于捷联惯导和里程计的组合导航矿井定位装置的结构示意图。

图5是本发明又一实施例提出的基于捷联惯导和里程计的组合导航矿井定位装置的结构示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。相反，本发明的实施例包括落入所附加权利要求书的精神和内涵范围内的所有变化、修改和等同物。

图1是本发明一实施例提出的的基于捷联惯导和里程计的组合导航矿井定位方法的流程示意图。

参见图1，图2，流程的执行主体为图1中的基于捷联惯导和里程计的组合导航矿井定位方法。该方法的实现流程详述如下：

步骤S102，获取由捷联惯导系统采集的角速率增量和加速度增量，其中捷联惯导系统安装在井下载体上，捷联惯导系统包括陀螺仪和加速度计。通常而言，陀螺仪是三轴陀螺仪，加速度计是三轴加速度计。

步骤S104，对角速率增量进行圆锥误差补偿，对加速度增量进行划船误差补偿。

本实施例中，步骤S104具体包括：圆锥误差补偿计算公式如下：

式中，ω表示经过补偿后的角速率增量，

表示未补偿前两组角速率增量之和，ω_k为第一组角速率增量值，ω_k+1为第二组角速率增量值。

对加速度增量进行划船误差补偿包括：

划船误差补偿分为速度旋转补偿项

与划船效应补偿项

计算公式如下：

式中，

表示未补偿前两组加速度增量之和，a_k为第一组加速度增量值，a_k+1为第二组加速度增量值，

经过补偿后的加速度增量结果a为：

步骤S106，构建机身坐标系与地理坐标系的转换矩阵，提取出井下载体在地理坐标系的姿态，并通过姿态矩阵转换获取井下载体在地理坐标系的位置信息。

步骤S106具体包括：参照图3，选取东—北—天方向为地理坐标系O-X_nY_nZ_n，选取右-前—上方向为机身坐标系(O′-X_b(m)Y_b(m)Z_b(m))，机身坐标系包括捷联惯导系统坐标系(b系)和里程计坐标系(m系)，构建捷联惯导系统坐标系(b系)与地理坐标系(n系)的转换矩阵

式中，ψ为井下载体的航向角,γ为井下载体的滚转角，θ为井下载体的俯仰角；

通过转换矩阵，得到井下载体在机身坐标系下的姿态。

步骤S108，获取里程计采集的里程增量，通过姿态矩阵转换获取里程计在地理坐标系的位置信息，其中里程计安装在井下载体上。

步骤S108具体包括：里程计使用姿态矩阵

进行坐标变换，获得里程计在地理坐标系的位置信息，对井下载体进行航位推算；

步骤S1081，将惯导误差和航位推算误差合并到一起构建状态矢量X，

式中，X为状态向量，

为捷联惯导系统的数学平台误差角，

分别为捷联惯导系统三个坐标轴的数学平台误差角；δvⁿ为井下载体的速度误差，

分别为井下载体在地理坐标系下的速度误差；δpⁿ为井下载体的位置误差，δL、δλ、δh分别井下载体的为经度、纬度、高度误差；ε^b为陀螺仪的漂移误差，

分别为陀螺仪三轴的漂移误差；

为加速度计的漂移误差，

分别为加速度计三轴的漂移误差；δp_D为里程计的位置误差，δL_D、δλ_D、δh_D分别为里程计在经度、纬度、高度方向上的误差；φ_D为里程计在俯仰δθ_D与航向δψ_D上的安装误差；δk_D为里程计刻度系数误差；dT为里程计的输出延迟时间；

步骤S1082，建立捷联惯导22×22误差转移矩阵F，

其中，F阵前15行15列表示如下：

式中，

为地理坐标系相对于惯性坐标系的角速率误差，

为地理坐标系相对于惯性坐标系的角速率，

为地理坐标系与机身坐标系的坐标转换矩阵，ε_b为地理坐标系角速率误差，fⁿ为地理坐标系下加速度计的输出比力，

为地球坐标系相对于惯性坐标系的角速率误差，

为地理坐标系相对于地球坐标系的角速率误差，

为地球坐标系相对于惯性坐标系的角速率，

为地理坐标系相对于地球坐标系的角速率，δvⁿ为载体的速度误差，δgⁿ为重力加速度误差，

为载体在地理坐标系下y轴的速度，R_m为卯酉圈半径，R_n为子午圈半径，h为当地海拔高度，

载体在地理坐标系下x轴的速度误差，

为载体在地理坐标系下x轴的速度，

载体在地理坐标系下z轴的速度误差，L为当地纬度、λ为当地经度，

为加速度计的漂移误差，w_c为白噪声；

F阵第16行到第18行，第1列到第3列表示如下：

式中，

Rmh＝R_m+h

Rnh＝R_n+h，clRnh＝cos(L)*Rnh

F阵第16行到第18行，第16列到第18列表示如下：

F(16:18,16:18)＝F(7:9,7:9)

F阵第16行到第18行，第19列到第20列表示如下：

F(16:18,19:20)＝Fkk(1:3,[1,3])

式中，

F阵第16行到第18行与第21列表示如下：

误差转移矩阵F未列出项均为0；

步骤S1083，令里程计的姿态矩阵

则在机身坐标系中的里程增量ΔS^m表示为：

ΔS^m＝[0ΔS 0]

式中，ΔS表示为里程计输出；

将机身坐标系中的里程增量ΔS^m投影到地理坐标系下的里程增量ΔSⁿ，

对地理坐标系下的里程增量ΔSⁿ积分的得到里程计三维位置信息：

式中，L_D、λ_D、h_D分别表示里程计在经度、纬度和高度方向上的位置信息，ΔS_N、ΔS_E、ΔS_U分别表示在经度、纬度和高度方向上的里程增量；

里程计的速度信息v_D通过位置信息微分可得：

v_D＝p_D/ΔT，

式中，p_D表示里程计在地理坐标系的三维位置信息，ΔT表示里程计采样间隔。

步骤S110，对捷联系统位置与里程计位置信息构建卡尔曼滤波方程，利用里程计测出的里程增量来校正捷联惯导系统的位置信息，得到最优的捷联惯导系统的位置信息。

步骤S110具体包括：

步骤S1101，建立三维位置量测信息矩阵Z：

Z＝p_INS-p_D

式中，p_INS代表捷联惯导系统的三维位置信息。

步骤S1102，建立量测系数矩阵H：

H＝[0_3×6 I_3×3 0_3×6 -I_3×3 0_3×4]。

步骤S1103，建立卡尔曼滤波方程。具体包括：

状态一步预测

计算如下：

式中，Φ_k,k-1表示一步状态转移矩阵，

表示前一时刻的状态估计；

一步预测方差阵P_k,k-1计算如下：

式中，P_k-1表示前一时刻的方差阵，Γ_k,k-1表示系统噪声分配矩阵，Q_k-1表示系统噪声方差矩阵；

滤波增益矩阵K_k计算如下：

式中，H_k表示k时刻量测矩阵，R_k表示k时刻量测方差矩阵

状态估计

计算如下：

式中，Z_k表示k时刻量测信息矩阵；

估计误差方差阵P_k计算如下：

P_k＝[I-K_kH_k]P_k,k-1

步骤S1104，卡尔曼滤波反馈：将估计得到的速度状态量

反馈给捷联惯导

完成卡尔曼滤波补偿，计算如下：

以上可以看出，本实施例提供的基于捷联惯导和里程计的组合导航矿井定位方法，用圆锥误差补偿方法补偿煤机设备在振动环境下给捷联惯导带来的不可交换误差，用划船误差补偿方法补偿由于同频不同轴的角振动和线振动给捷联惯导带来的误差，利用捷联惯导和里程计的优势补偿捷联惯导的累积误差，实现对煤机设备的精准定位、定向。本实施例方法可以保证长距离导航参数的准确有效，实时精确监测设备当前的位置点以及运动特征。

图4是本发明实施例提出的基于捷联惯导和里程计的组合导航矿井定位装置的结构示意图。为了便于说明，仅仅示出了与本实施例相关的部分。

参照图4，基于捷联惯导和里程计的组合导航矿井定位装置3，包括：

角速率增量和加速度增量采集单元31，用于获取由捷联惯导系统采集的角速率增量和加速度增量，其中捷联惯导系统安装在井下载体上，捷联惯导系统包括陀螺仪和加速度计；

误差补偿单元32，用于对角速率增量进行圆锥误差补偿，对加速度增量进行划船误差补偿；

井下载体位置信息获取单元33，构建机身坐标系与地理坐标系的转换矩阵，提取出井下载体在地理坐标系的姿态，并通过姿态矩阵转换获取井下载体在地理坐标系的位置信息；

里程计位置信息获取单元34，用于获取里程计采集的里程增量，通过姿态矩阵转换获取里程计在地理坐标系的位置信息，其中里程计安装在井下载体上；

捷联惯导系统位置信息获取单元35，用于对捷联系统位置与里程计位置信息构建卡尔曼滤波方程，利用里程计测出的里程增量来校正捷联惯导系统的位置信息，得到最优的捷联惯导系统的位置信息。

需要说明的是，本发明实施例提供的上述装置的各个单元，由于与本发明方法实施例基于同一构思，其带来的技术效果与本发明方法实施例相同，具体内容可参见本发明方法实施例中的叙述，此处不再赘述。

图5是本发明又一实施例提出的基于捷联惯导和里程计的组合导航矿井定位装置的结构示意图。如图5所示，该实施例基于捷联惯导和里程计的组合导航矿井定位装置包括存储器41、处理器40以及存储在所述存储器40中并可在处理器40上运行的计算机程序42。处理器40执行计算机程序42时实现上述各个方法实施例中的步骤，例如图1所示的步骤S102至S110。或者处理器40执行计算机程序42时实现上述各装置实施例中各单元的功能，例如图4中所示单元31至35的功能。所述计算机程序的实施例，可以达到与之对应的前述任意方法实施例相同或者类似的效果。

需要说明的是，在本发明的描述中，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性。此外，在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

此外，应该明白的是，本文所述的计算机可读存储介质(例如，存储器)可以是易失性存储器或非易失性存储器，或者可以包括易失性存储器和非易失性存储器两者。作为例子而非限制性的，非易失性存储器可以包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦写可编程ROM(EEPROM)或快闪存储器。易失性存储器可以包括随机存取存储器(RAM)，该RAM可以充当外部高速缓存存储器。作为例子而非限制性的，RAM可以以多种形式获得，比如同步RAM(DRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据速率SDRAM(DDR SDRAM)、增强SDRAM(ESDRAM)、同步链路DRAM(SLDRAM)以及直接Rambus RAM(DRRAM)。所公开的方面的存储设备意在包括但不限于这些和其它合适类型的存储器。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接或彼此可通讯；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (8)

1.一种基于捷联惯导和里程计的组合导航矿井定位方法，其特征在于，包括：

获取由捷联惯导系统采集的角速率增量和加速度增量，其中捷联惯导系统安装在井下载体上，捷联惯导系统包括陀螺仪和加速度计；

对角速率增量进行圆锥误差补偿，对加速度增量进行划船误差补偿；

构建机身坐标系与地理坐标系的转换矩阵，提取出井下载体在地理坐标系的姿态，并通过姿态矩阵转换获取井下载体在地理坐标系的位置信息；

获取里程计采集的里程增量，通过姿态矩阵转换获取里程计在地理坐标系的位置信息，其中里程计安装在井下载体上；

对捷联系统位置与里程计位置信息构建卡尔曼滤波方程，利用里程计测出的里程增量来校正捷联惯导系统的位置信息，得到最优的捷联惯导系统的位置信息。

2.根据权利要求1所述的基于捷联惯导和里程计的组合导航矿井定位方法，其特征在于，所述对角速率增量进行圆锥误差补偿包括：

圆锥误差补偿计算公式如下：

式中，ω表示经过补偿后的角速率增量，

表示未补偿前两组角速率增量之和，ω_k为第一组角速率增量值，ω_k+1为第二组角速率增量值。

所述对加速度增量进行划船误差补偿包括：

划船误差补偿分为速度旋转补偿项

与划船效应补偿项

计算公式如下：

式中，

表示未补偿前两组加速度增量之和，a_k为第一组加速度增量值，a_k+1为第二组加速度增量值，经过补偿后的加速度增量结果a为：

3.根据权利要求1所述的基于捷联惯导和里程计的组合导航矿井定位方法，其特征在于，

所述构建机身坐标系与地理坐标系的转换矩阵，提取出井下载体在地理坐标系的姿态，并通过姿态矩阵转换获取井下载体在地理坐标系的位置信息包括：

选取东—北—天方向为地理坐标系，选取右-前—上方向为机身坐标系，机身坐标系包括捷联惯导系统坐标系和里程计坐标系，构建捷联惯导系统坐标系与地理坐标系的转换矩阵

式中，ψ为井下载体的航向角,γ为井下载体的滚转角，θ为井下载体的俯仰角；

通过转换矩阵，得到井下载体在机身坐标系下的姿态。

4.根据权利要求3所述的基于捷联惯导和里程计的组合导航矿井定位方法，其特征在于，所述获取里程计采集的里程增量，通过姿态矩阵转换获取里程计在地理坐标系的位置信息，其中里程计安装在井下载体上包括：

所述里程计使用姿态矩阵

进行坐标变换，获得里程计在地理坐标系的位置信息，对所述井下载体进行航位推算；

将惯导误差和航位推算误差合并到一起构建状态矢量X，

式中，X为状态向量，

为捷联惯导系统的数学平台误差角，

分别为捷联惯导系统三个坐标轴的数学平台误差角；δvⁿ为井下载体的速度误差，

分别为井下载体在地理坐标系下的速度误差；δpⁿ为井下载体的位置误差，δL、δλ、δh分别井下载体的为经度、纬度、高度误差；ε^b为陀螺仪的漂移误差，

分别为陀螺仪三轴的漂移误差；

为加速度计的漂移误差，

分别为加速度计三轴的漂移误差；δp_D为里程计的位置误差，δL_D、δλ_D、δh_D分别为里程计在经度、纬度、高度方向上的误差；φ_D为里程计在俯仰δθ_D与航向δψ_D上的安装误差；δk_D为里程计刻度系数误差；dT为里程计的输出延迟时间；

建立捷联惯导22×22误差转移矩阵F，

其中，F阵前15行15列表示如下：

式中，

为地理坐标系相对于惯性坐标系的角速率误差，

为地理坐标系相对于惯性坐标系的角速率，

为地理坐标系与机身坐标系的坐标转换矩阵，ε_b为地理坐标系角速率误差，fⁿ为地理坐标系下加速度计的输出比力，

为地球坐标系相对于惯性坐标系的角速率误差，

为地理坐标系相对于地球坐标系的角速率误差，

为地球坐标系相对于惯性坐标系的角速率，

为地理坐标系相对于地球坐标系的角速率，δvⁿ为载体的速度误差，δgⁿ为重力加速度误差，

为载体在地理坐标系下y轴的速度，R_m为卯酉圈半径，R_n为子午圈半径，h为当地海拔高度，

载体在地理坐标系下x轴的速度误差，

为载体在地理坐标系下x轴的速度，

载体在地理坐标系下z轴的速度误差，L为当地纬度、λ为当地经度，

为加速度计的漂移误差，w_c为白噪声；

F阵第16行到第18行，第1列到第3列表示如下：

式中，

Rmh＝R_m+h

Rnh＝R_n+h，clRnh＝cos(L)*Rnh

F阵第16行到第18行，第16列到第18列表示如下：

F(16:18,16:18)＝F(7:9,7:9)

F阵第16行到第18行，第19列到第20列表示如下：

F(16:18,19:20)＝Fkk(1:3,[1,3])

式中，

F阵第16行到第18行与第21列表示如下：

误差转移矩阵F未列出项均为0；

令里程计的姿态矩阵

则在机身坐标系中的里程增量ΔS^m表示为：

ΔS^m＝[0 ΔS 0]

式中，ΔS表示为里程计输出；

将机身坐标系中的里程增量ΔS^m投影到地理坐标系下的里程增量ΔSⁿ，

对地理坐标系下的里程增量ΔSⁿ积分的得到里程计三维位置信息：

式中，L_D、λ_D、h_D分别表示里程计在经度、纬度和高度方向上的位置信息，ΔS_N、ΔS_E、ΔS_U分别表示在经度、纬度和高度方向上的里程增量；

里程计的速度信息v_D通过位置信息微分可得：

v_D＝p_D/ΔT，

式中，p_D表示里程计在地理坐标系的三维位置信息，ΔT表示里程计采样间隔。

5.根据权利要求4所述的基于捷联惯导和里程计的组合导航矿井定位方法，其特征在于，

所述对捷联系统位置与里程计位置信息构建卡尔曼滤波方程，利用里程计测出的里程增量来校正捷联惯导系统的位置信息，得到最优的捷联惯导系统的位置信息包括：

建立三维位置量测信息矩阵Z：

Z＝p_INS-p_D

式中，p_INS代表捷联惯导系统的三维位置信息；

建立量测系数矩阵H：

H＝[0_3×6 I_3×3 0_3×6 -I_3×3 0_3×4]；

状态一步预测

计算如下：

式中，Φ_k,k-1表示一步状态转移矩阵，

表示前一时刻的状态估计；

一步预测方差阵P_k,k-1计算如下：

式中，P_k-1表示前一时刻的方差阵，Γ_k,k-1表示系统噪声分配矩阵，Q_k-1表示系统噪声方差矩阵；

滤波增益矩阵K_k计算如下：

式中，H_k表示k时刻量测矩阵，R_k表示k时刻量测方差矩阵

状态估计

计算如下：

式中，Z_k表示k时刻量测信息矩阵；

估计误差方差阵P_k计算如下：

P_k＝[I-K_kH_k]P_k,k-1

将估计得到的速度状态量

反馈给捷联惯导

完成卡尔曼滤波补偿，计算如下：

6.一种基于捷联惯导和里程计的组合导航矿井定位装置，其特征在于，包括：

角速率增量和加速度增量采集单元，用于获取由捷联惯导系统采集的角速率增量和加速度增量，其中捷联惯导系统安装在井下载体上，捷联惯导系统包括陀螺仪和加速度计；

误差补偿单元，用于对角速率增量进行圆锥误差补偿，对加速度增量进行划船误差补偿；

井下载体位置信息获取单元，构建机身坐标系与地理坐标系的转换矩阵，提取出井下载体在地理坐标系的姿态，并通过姿态矩阵转换获取井下载体在地理坐标系的位置信息；

里程计位置信息获取单元，用于获取里程计采集的里程增量，通过姿态矩阵转换获取里程计在地理坐标系的位置信息，其中里程计安装在井下载体上；

捷联惯导系统位置信息获取单元，用于对捷联系统位置与里程计位置信息构建卡尔曼滤波方程，利用里程计测出的里程增量来校正捷联惯导系统的位置信息，得到最优的捷联惯导系统的位置信息。

7.一种基于捷联惯导和里程计的组合导航矿井定位装置，其特征在于，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述方法的步骤。

8.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一项所述方法的步骤。

Images