[发明专利]一种基于组合导航技术的超深矿井罐笼位姿测量系统及方法

权利要求书

1.一种基于组合导航技术的超深矿井罐笼位姿测量系统，其特征在于，包括捷联惯性导航装置、地面处理装置、旋转编码器、接近开关和感应通信电缆；

所述捷联惯性导航装置安装在罐笼顶部；

所述旋转编码器安装在提升机卷筒的卷轴上；

所述感应通信电缆铺设于矿井侧壁；

所述接近开关设置在井筒壁上；

所述旋转编码器和接近开关与安装在矿井地表面的地面处理装置进行通信连接，所述地面处理装置通过感应通信电缆与捷联惯性导航装置进行通信连接；

以捷联惯性导航装置、接近开关和旋转编码器构建成组合导航式位姿测量系统。

2.根据权利要求1所述的一种基于组合导航技术的超深矿井罐笼位姿测量系统，其特征在于，所述捷联惯性导航装置包括惯性单元、数据采集及处理模块、导航解算及信息融合模块、数据存储模块及通信模块；

所述惯性单元通过SPI总线与数据采集及处理模块相连，所述数据采集及处理模块与导航解算及信息融合模块相连，所述数据采集及处理模块与导航解算及信息融合模块均与所述数据存储模块相连，所述数据存储模块与通信模块相连。

3.根据权利要求2所述的一种基于组合导航技术的超深矿井罐笼位姿测量系统，其特征在于，所述数据采集及处理模块与导航解算及信息融合模块均采用TI公司的TMS320系列C6748浮点型DSP处理器。

4.一种基于组合导航技术的超深矿井罐笼位姿测量方法，其特征在于，将数据采集与处理和导航解算与信息融合分配给两个不同的处理器处理；

所述数据采集与处理的过程如下：

首先读取捷联惯性导航装置中的惯性单元的数据，根据惯性单元偏置温度系数进行线性温度误差补偿，然后读取地面处理装置数据，最后存储数据，上传数据至上位机；

所述导航解算与信息融合的过程如下：

通过将捷联惯性导航装置安装在罐笼顶部，利用捷联惯性导航装置的惯性单元测量出罐笼运动加速度和角速度，采用四元数法建立罐笼姿态矩阵，通过SINS解算出罐笼位置和姿态信息；通过地面处理装置采集井筒壁上的接近开关和提升机卷筒轴上的旋转编码器测量罐笼的位置和速度，采用联合卡尔曼滤波算法将捷联惯性导航装置的速度和位置信息与罐笼位置与速度进行信息融合，反馈校正捷联惯性导航装置，实现罐笼位置和姿态的精确测量。

5.根据权利要求4所述的一种基于组合导航技术的超深矿井罐笼位姿测量方法，其特征在于，所述信息融合的步骤如下：

步骤1：建立捷联惯性导航装置误差模型，包括三轴陀螺仪误差模型、加速度测量误差模型、位置误差模型及姿态角误差模型；

步骤2：以步骤1中的捷联惯性导航装置误差模型建立离散化的组合导航式位姿测量系统的状态方程；

所述组合导航式位姿测量系统是指由捷联惯性导航装置、接近开关和旋转编码器组成；

步骤3：构建信息融合卡尔曼滤波器，并依据组合导航式位姿测量系统的状态向量建立信息融合卡尔曼滤波器中各子滤波器的观测方程，求得各子滤波器的状态向量估计值；

所述信息融合卡尔曼滤波器包括第一子滤波器、第二子滤波器、第三子滤波器以及主滤波器，所述第一子滤波器、第二子滤波器及第三子滤波器均与主滤波器相连；

所述第一子滤波器、第二子滤波器、第三子滤波器以及主滤波器均为卡尔曼滤波器；

将捷联惯性导航装置的速度与编码器速度之差作为第一子滤波器的输入信号；

将捷联惯性导航装置的位置信号与接近开关位置信号之差作为第二子滤波器的输入信号；

将捷联惯性导航装置的东、北方向的零速度和零位置信号作为约束条件，构造成虚拟信号作为第三子滤波器的输入信号；

第一子滤波器的观测方程为：Z₁(k)＝H₁(k)X₁(k)+v₁(k)；

第一子滤波器观测向量Z₁(k)＝[v_u-v_b]，v_b为地面处理装置基于旋转编码器计算得到的罐笼速度，v₁(k)为第一子滤波器观测噪声，是均值为零的高斯白噪声序列，由高斯函数可得，X₁(k)为第一子滤波器的状态向量，第一子滤波器观测矩阵H₁(k)＝[0_1×8 1 0_1×6]_1×15；

第二子滤波器的观测方程为：Z₂(k)＝H₂(k)X₂(k)+v₂(k)；

第二子滤波器观测向量Z₂(k)＝[H-H_j]，H为捷联惯性导航装置解算出的罐笼垂直位置，H_j为接近开关位置信号，v₂(k)为第二子滤波器位置观测噪声，是均值为零的高斯白噪声序列；X₂(k)为第二子滤波器的状态向量，H₂(k)＝[0_1×5 1 0_1×9]_1×15为第二子滤波器位置观测矩阵；

第三子滤波器观测方程为：Z₃(k)＝H₃(k)X₃(k)+v₃(k)；

第三子滤波器观测向量Z₃(k)＝[L E v_e v_n]^T，L和E为捷联惯性导航装置东向和西向位置，v_e和v_n为捷联惯性导航装置东向和西向速度，v₃(k)为第三子滤波器观测噪声，是均值为零的高斯白噪声序列；X₃(k)为第三子滤波器的状态向量；

第三子滤波器观测矩阵H₃(k)为：

k表示离散时刻；

步骤4：按照如下规则将组合导航式位姿测量系统整体的过程信息包括Q_i(k)、及P_i(k)平均分配到各子滤波器：

$\left\{\begin{array}{c}{Q}_i(k-1)={\mathrm{\β}}_i^{-1}{Q}_g(k-1)\\ P_i(k-1)={\mathrm{\β}}_i^{-1}{P}_g(k-1)\\ X_i(k-1)=X_g(k-1)\end{array}\right.$

且满足

其中，Q_i(k)、及P_i(k)分别为各滤波器的噪声方差阵、状态估计向量、估计误差协方差，i＝1,2,3,m；

Q_g(k)、P_g(k)分别为组合导航式位姿测量系统的噪声方差阵、状态估计向量、估计误差协方差；为分配参数β_i(k)的倒数；

E[]是期望函数，的初始值为[0]_1×15；

由步骤3的观测方程求得，Q_i＝E[W_i(k)W_i(k)^T]/T，W_i为各滤波器的噪声向量；

步骤5：基于步骤4所得各滤波器的噪声方差矩阵、状态估计向量、估计误差协方差即Q_i(k)、及P_i(k)，按照以下公式将各滤波器状态估计与估计误差协方差按组合导航式位姿测量系统的转移矩阵进行转移，得到各滤波器转移后的状态估计向量和估计误差协方差；

步骤6：利用步骤5所得各滤波器转移后的状态估计向量与估计误差协方差，采用如下测量方程，对K_i(k)、P_i(k)按以下公式进行更新：

${\hat{X}}_i\left(k\right)={\hat{X}}_i(k,k-1)+K_i\left(k\right)\left(Z_i\right(k)-H_i(k\left){\hat{X}}_i\right(k,k-1\left)\right)$

$K_i\left(k\right)=P_i(k,k-1)H_i^T\left(k\right){\left(H_i\right(k\left)P_i\right(k,k-1)+R_i(k\left)\right)}^{-1}$

P_i(k)＝(I-K_i(k)H_i(k))P_i(k,k-1)

其中，i＝1,2,3,m，当i＝m时，P_m(k)＝P_m(k,k-1)；K_i为各滤波器的增益矩阵，R_i为各滤波器观测噪声的协方差矩阵，R_i＝E[v_i(k)v_i(k)^T]/T，v_i为各滤波器观测白噪声序列，I为15×15的单位矩阵；T为惯性单元采样周期，P_i(k,k-1)为转移估计误差协方差阵，

步骤7：将各子滤波器的估计信息按照下式进行融合，得到组合导航式位姿测量系统的状态估计与估计误差协方差全局最优估计P_g，并返回步骤4，依据P_g将组合导航式位姿测量系统的过程信息对各子滤波器进行重新分配，对各滤波器进行反馈修正：

$\begin{array}{c}{P}_g={\[P_1^{-1}+P_2^{-1}+P_3^{-1}+P_m^{-1}\]}^{-1}\\ \widehat{X_g}=P_g\[P_1^{-1}\widehat{X_1}+P_2^{-1}\widehat{X_2}+P_3^{-1}\widehat{X_3}+P_m^{-1}\widehat{X_m}\]\end{array}.$