CN109540130A - 一种连采机惯性导航定位定向方法 - Google Patents

Abstract

本发明一种连采机惯性导航定位定向方法，通过惯导系统的测量，向上位机返回此时采煤机的位置信息和姿态，实时反映出采煤机工作时直线度的信息；捷联式惯性导航系统的光纤陀螺仪和加速度计直接采集采煤机在惯性空间的转动角速度的和线性加速度，经过积分运算得到轨道采煤机的运动速度、航线、姿态和位置信息。利用安装与采煤机行走部的里程计测量其行走速度与距离。且通过快速卡尔曼快速滤波算法对导航系统测得信息和里程计测得的信息进行融合。再以航位角推算原理解析出采煤机实时位置和姿态信息，根据实时位置和姿态信息对采煤机掘进方向进行实时修正，保证采煤机按照既定直线掘进。该方法测量结果精确，结构简单。

Description

一种连采机惯性导航定位定向方法

技术领域

本发明涉及一种连采机惯性导航定位定向方法，特别涉及一种惯性导航系 统和里程计组合的系统应用于露天煤矿连采机的定位定向算法。

背景技术

历史上由于我国煤矿安全事故频发，每年造成大量人员伤亡和巨额财产损 失。因此我国为解决煤矿事故发生，近年来提出发展智慧化矿山发展规划。其 中无人自动化开采称为其最重要的一个环节，而连采机及其他设备在开采中的 定位导航技术则是目前研究的热点。目前导航设备技术已经非常成熟，例如： 无线电导航、卫星定位、地理信息辅助定位、惯性导航等。因为矿井下复杂的 电磁工作环境，信号无法穿透岩层，上述被动式导航定位技术无法在井下工作。 惯性导航系统中的传感器—加速度计和陀螺仪利用牛顿第二定律和萨格纳克效 应等原理均对惯性力有反应，从而不利用任何外界信息的情况下确定载体加速 度和姿态的变化，进而进行导航定位。这种主动式导航定位方案，非常适合在 电磁环境复杂的矿井下工作。

发明内容

本发明是为了解决上述问题提出一种连采机惯性导航定位定向方法，尤其 是一种基于捷联惯导和里程计组合导航系统的连采机惯性导航定位定向方法。 采煤机从矿道口向前掘进时，通过惯导系统的测量，向上位机返回此时采煤机 的位置信息和姿态，从而实时的反映出采煤机工作时直线度的信息；捷联式惯 性导航系统的光纤陀螺仪和加速度计直接采集采煤机在惯性空间的转动角速度 的和线性加速度，经过积分运算得到轨道采煤机的运动速度、航线、姿态和位 置等信息。利用安装与采煤机行走部的里程计测量其行走速度与距离(标量， 行走方向由惯性导航装置确定)。并且通过改进型快速卡尔曼快速滤波算法对 导航系统测得信息和里程计测得的信息进行融合，以提高系统测量的精度和稳定性。再以航位角推算原理解析出采煤机实时位置和姿态信息，根据实时位置 和姿态信息对采煤机掘进方向进行实时修正，以保证采煤机按照既定直线掘进。

本发明一种连采机惯性导航定位定向方法，步骤如下：

步骤一：将捷联惯导系统和里程计分别安装在采煤机的顶部和运动部，并 确定采煤机的初始信息，包括初始东北天坐标系下位置坐标以及采煤机姿态信 息。

步骤二：晃动基座下，采用惯性系对准方法，确定采煤机的初始姿态信息， 即姿态矩阵初始值

步骤三：通过安装在采煤机运动部的里程计测量出采煤机的位移信息，并 在以一个采样周期T内的平均速度作为采煤机里程计输出的速度信息。

步骤四：惯导系统中光纤陀螺及加速度计实时测量采煤机运动角速度和加 速度信息，通过时间积分更新姿态矩阵及速度信息。

步骤五：根据惯导系统中光纤陀螺和加速度计测量的采煤机运动信息即煤 机运动角速度和加速度信息，得到系统状态方程。

步骤六：根据里程计输出的速度信息，得到系统量测方程。

步骤七：因为惯性导航测得速度信息是随时间发散的，里程计测得的速度 与时间无关系。使用快速卡尔曼滤波算法，对惯导系统测得速度信息和里程计 测得速度信息进行数据融合，得到更精确的采煤机速度信息。实时估计并修正 传感器(包括光纤陀螺和加速度计)误差、姿态误差及速度误差等。从而得到更 精确的采煤机运动信息即采煤机工作过程中的速度和姿态信息。

步骤八：通过步骤七得到的高精度的采煤机运动信息，使用航位推算原理， 得到采煤机位于东北天坐标系下的实时位置信息。

步骤九：根据步骤八得到实时位置信息和步骤七得到的高精度的采煤机运 动信息，与既定的目标运动轨迹进行比较，并将误差返回给系统，系统根据误 差信息对采煤机的姿态进行控制，使其按既定轨迹运动。

其中，步骤七所述的快速卡尔曼滤波算法，具体如下：在获得了惯导系统和 里程计测得速度信息以后，采用快速卡尔曼滤波算法进行滤波处理可以很大程 度上减少系统滤波过程使用时间，保证系统实时输出当前采煤机的运动轨迹信 息：

对于X(k-1)协方差阵：

增益：K(k)＝P(k-1)C^T(k)[C(k)P(k-1)C^T(k)+R(k)]^-1 (23)

式中，Q(k)系统噪声w的协方差阵；R(k)测量噪声w_e的协方差阵；

可得到k时刻，最优X(k)＝X(k-1)+K(k)[Z(k)-C(k)X(k-1)] (24)

协方差阵P(k)的更新：P(k)＝[I-K(k)C(k)]P(k-1) (25)

在式(23)中，假设

M(k)＝C(k)P(k-1)C^T(k)+R(k) (26)

令N(k)＝M(k)-I，则当N(k)的普半径ρ(n)＜1时，

M^-1(k)＝I-N(k)+N(k)·N(k)-N(k)·N(k)·N(k)+... (27)

当ρ(M)≥1时，式(9)不再收敛；此时假设M(k)绝对值最大特征值为λ_m，若 λ_m为正，选择N₁(k)＝(M(k)-η·I)/η则可以按照下式计算：

M^-1(k)＝{I-N₁(k)+N₁(k)·N₁(k)-N₁(k)·N₁(k)·N₁(k)+...}/η (28)

其中η＞λ_m为任意正数，一般可以选择一个便于计算且与λ_m相差不大的正整 数；

若λ_m为负，选择N₂(k)＝(M(k)-ξ·I)/ξ则可以按照下式计算：

M^-1(k)＝{I-N₂(k)+N₂(k)·N₂(k)-N₂(k)·N₂(k)·N₂(k)+...}/ξ (29)

其中ξ＜λ_m为任意负数，一般可以选择一个便于计算且与λ_m相差不大的负整 数；式(27)-(29)展开项在此项目中选择前两项即可达到精度要求，后面所 有展开项舍去。

本发明一种连采机惯性导航定位定向方法，其优点及功效在于该方法测量结 果精确，结构简单，为技术人员提供高精度的采煤机运动轨迹信息，并且能够 根据实时直线度情况指导工作人员修正采煤机的工作方向。

附图说明

图1为本发明方法的原理示意图；

图2为快速卡尔曼滤波原理示意图；

图3为采煤机三维定位算法示意图；

图4为采煤机惯导系统和里程计安装示意图；

具体实施方式

下面结合具体实施方式，对本发明的技术方案做进一步等说明。

一种连采机惯性导航定位定向方法，如图1所示，具体包括如下步骤：

步骤一：将捷联惯导系统和里程计分别安装在采煤机的顶部和运动部，(如 图4所示，1为惯导系统；2为采煤机；3为里程计；)并确定采煤机的初始信息， 包括初始东北天坐标系下位置坐标。

本发明是基于惯性导航系统的采煤机准直快速检测装置，为了确定采煤机与 工作面煤层顶底板的位置关系，采煤机定位坐标系与工作面煤层数据库坐标系 使用同一坐标系，即以开采起始点为原点的东北天坐标系。获得采煤机初始位 置和姿态信息后，系统需要解算出采煤机在东北天坐标系下的坐标。

步骤二：晃动基座下，采用惯性系对准方法，确定采煤机的初始姿态信息， 即姿态矩阵初始值其中，晃动基座是指惯导系统与安装载体捷联，惯导 系统随着载体晃动而晃动的情况；

步骤三：通过安装在采煤机运动部的里程计测量出采煤机的位移信息，并 在以一个采样周期T内的平均速度作为采煤机里程计输出的速度信息。

位于运动部的里程计实时测得的在采煤机坐标系OX_bY_bZ_b下单位时间内轨道 检测车位移增量

里程计在采煤机车运动时会发出A,B两路相位差90°的数字脉冲信号。运动部 正转时A超前B为90°，当反转时B超前A为90°。脉冲的个数与位移量成比例关系。 系统先对里程计输出的两路脉冲进行鉴相，判断是正转还是反转；其次进行正 转加数，反转减数。位移增量可以表示为

其中A为采煤机运动部在运动时，每旋转1°所对应运动的位移；Z为里程计 的分辨率(脉冲数/转)；M₀为里程计输出的脉冲数；K_D为里程计输出脉冲的 倍频系数。

里程计在一个采样周期T内，测得真实位移为则在时间T内 其速度为V^D＝[0 V₀ 0]^T，其中

步骤四：惯导系统中光纤陀螺及加速度计实时测量采煤机运动角速度和加 速度信息，通过时间积分更新姿态矩阵及速度信息。

在系统测得轨道检测车位于采煤机坐标系下的坐标后，为了利用惯性绝对参 考系，将采煤机坐标系下的坐标转换为东北天坐标系下的坐标，其过程可分为 上次顺序不可交换的基本旋转：

1、首先将OX_nY_nZ_n绕Z_n轴旋转ψ角，OX_nY_nZ_n旋转到OX₁Y₁Z₁处，这次变化可以 表示为：

2、然后将OX₁Y₁Z₁绕x₁轴旋转转θ角，OX₁Y₁Z₁旋转到OX₂Y₂Z₂处，此次的变换 关系可以表示为：

3、最后一次旋转是将OX₂Y₂Z₂绕y₂旋转φ角，此时OX₂Y₂Z₂旋转到OX_bY_bZ_b处， 此次变换关系可表示为：

则从导航坐标系到机体坐标系的坐标转换矩阵为：

由于都是正交矩阵，他们相乘的结果也是正交的，即也是正交矩阵。

步骤五：根据惯导系统中光纤陀螺和加速度计测量的采煤机运动信息即煤 机运动角速度和加速度信息，得到系统状态方程。

采煤机的航向角ψ由陀螺测得，真实的航向角ψ和测量的航向角之间存在 航向误差角即

陀螺测量航向角误差包括随机常值的零偏误差ε_ψ和高斯白噪声w_ψ。

采煤机的俯仰角θ由陀螺测得，真实的俯仰角θ和测量的俯仰角之间存在 航向误差角即

陀螺测量俯仰角误差包括随机常值零偏误差ε_θ和高斯白噪声w_θ。

通过安装在采煤机内惯导系统中的加速度计测量的比力f_G，去除比力f_G中 地球重力等影响项，得到采煤机运动加速度f＝[0 f₀ 0]^T并转换到东北天坐标 系下得到

加速度计测得的实际加速的与真实加速度之间的关系为

展开并忽略和的高阶小量，可得

记

则

惯导系统的速度测量误差方程：

假设加速度计测量误差包括常值随机误差和随机噪声w_f，其中随机误 差是一个随机常量。

结合公式(13)，取系统状态变量可得到状态方程

对系统以步长T＝t(k)-t(k-1)进行离散化后，得到系统的状态方程

X(k)＝φ(k-1)X(k-1)+w (15)

其中系统噪声w＝[ε_χ w_f 0 0]^T，

ε_χ＝[ε_θ ε_ψ]^T；

步骤六：根据里程计输出的速度信息，得到系统量测方程。

里程计在一个采样周期T内，测得真实位移为s＝[0 s₀ 0]^T，则在时间T内 其速度为V^D＝[0 V₀ 0]^T，其中将其投影在东北天坐标系下后表示为

里程计测得的速度的测量值和真实的速度值间的关系为

其中，为里程计刻度误差，其值为一个随机常数。

实际测得里程计输出速度在东北天坐标系下的投影为

展开并忽略和的高阶小量，可得

记

则

里程计的速度测量误差方程为

以导航系统推算的速度与里程计测得的速度之差作为观测量Z(k)，则可得到 测量方程

其中w_e为测量噪声。

步骤七：在获得了惯导系统和里程计测得速度信息以后，采用一种快速卡 尔曼滤波算法进行滤波处理可以很大程度上减少系统滤波过程使用时间。保证 系统实时输出当前采煤机的运动轨迹信息即采煤机工作过程中的速度和姿态信 息。其具体实施方法如下。

对于X(k-1)协方差阵：

增益：K(k)＝P(k-1)C^T(k)[C(k)P(k-1)C^T(k)+R(k)]^-1 (23)

式中，Q(k)系统噪声w的协方差阵；R(k)测量噪声w_e的协方差阵。

可得到k时刻，最优X(k)＝X(k-1)+K(k)[Z(k)-C(k)X(k-1)] (24)

协方差阵P(k)的更新：P(k)＝[I-K(k)C(k)]P(k-1) (25)

在式(23)中，假设

M(k)＝C(k)P(k-1)C^T(k)+R(k) (26)

令N(k)＝M(k)-I，则当N(k)的普半径ρ(n)＜1时，

M^-1(k)＝I-N(k)+N(k)·N(k)-N(k)·N(k)·N(k)+... (27)

当ρ(M)≥1时，式(9)不再收敛。此时假设M(k)绝对值最大特征值为λ_m，若 λ_m为正，选择N₁(k)＝(M(k)-η·I)/η则可以按照下式计算：

M^-1(k)＝{I-N₁(k)+N₁(k)·N₁(k)-N₁(k)·N₁(k)·N₁(k)+...}/η (28)

其中η＞λ_m为任意正数，一般可以选择一个便于计算且与λ_m相差不大的正整 数。

若λ_m为负，选择N₂(k)＝(M(k)-ξ·I)/ξ则可以按照下式计算：

M^-1(k)＝{I-N₂(k)+N₂(k)·N₂(k)-N₂(k)·N₂(k)·N₂(k)+...}/ξ (29)

其中ξ＜λ_m为任意负数，一般可以选择一个便于计算且与λ_m相差不大的负整 数。式(27)-(29)展开项在此项目中选择前两项即可达到精度要求，后面所 有展开项舍去。

算法改进后，大量复杂的卡尔曼滤波增益和系数的计算通过简化，大大降 低了运算量，提高了滤波的速度，占用了更少的计算资源。

步骤八：通过快速卡尔曼滤波算法，将惯导系统和里程计测得数据进行融 合，得到了更加准确的在东北天坐标系下的速度信息V_n(t)。然后根据航位推算 原理建立采煤机的方位方程为：

P_n(t)＝P_n(t-1)+V_n(t)×T (29)

其中，P_n(t),P(t-1)分别为在东北天坐标系下采煤机t和t-1采样时刻的位置。 P_n(t)＝[P_E(t),P_N(t),P_U(t)]^T，P_N(t)为t时刻北向位置，P_E(t)为t时刻东向位置，P_U(t)为 t时刻天向位置，V_n(t)是经过快速卡尔曼滤波处理的东北天坐标系下的t时刻速度 信息，T是离散系统的采样周期。

步骤九：在实际工程中，通过矿道洞口的坐标和目标位置的坐标拟合出一 条采煤机在东北天坐标系的运动轨迹。根据采煤机方位方程，得到实时当前采 煤机的姿态和位置，将实时的采煤机位置信息和预先得到的运动轨迹进行比较， 得到误差信息。系统通过误差信息对采煤机的运动姿态进行控制，使其按照既 定的运动轨迹运动。

Claims (2)

1.一种连采机惯性导航定位定向方法，其特征在于：该方法步骤如下：

步骤一：将捷联惯导系统和里程计分别安装在采煤机的顶部和运动部，并确定采煤机的初始信息，包括初始东北天坐标系下位置坐标以及采煤机姿态信息；

步骤二：晃动基座下，采用惯性系对准方法，确定采煤机的初始姿态信息，即姿态矩阵初始值

步骤三：通过安装在采煤机运动部的里程计测量出采煤机的位移信息，并在以一个采样周期T内的平均速度作为采煤机里程计输出的速度信息；

步骤四：惯导系统中光纤陀螺及加速度计实时测量采煤机运动角速度和加速度信息，通过时间积分更新姿态矩阵及速度信息；

步骤五：根据惯导系统中光纤陀螺和加速度计测量的采煤机运动信息即煤机运动角速度和加速度信息，得到系统状态方程；

步骤六：根据里程计输出的速度信息，得到系统量测方程；

步骤七：使用快速卡尔曼滤波算法，对惯导系统测得速度信息和里程计测得速度信息进行数据融合，得到更精确的采煤机速度信息；实时估计并修正传感器误差、姿态误差及速度误差，从而得到更精确的采煤机运动信息即采煤机工作过程中的速度和姿态信息；

步骤八：通过步骤七得到的高精度的采煤机运动信息，使用航位推算原理，得到采煤机位于东北天坐标系下的实时位置信息；

步骤九：根据步骤八得到实时位置信息和步骤七得到的高精度的采煤机运动信息，与既定的目标运动轨迹进行比较，并将误差返回给系统，系统根据误差信息对采煤机的姿态进行控制，使其按既定轨迹运动。

2.根据权利要求1所述的一种连采机惯性导航定位定向方法，其特征在于：步骤七所述的快速卡尔曼滤波算法，具体如下：在获得了惯导系统和里程计测得速度信息以后，采用快速卡尔曼滤波算法进行滤波处理可以很大程度上减少系统滤波过程使用时间，保证系统实时输出当前采煤机的运动轨迹信息：

对于X(k-1)协方差阵：

增益：K(k)＝P(k-1)C^T(k)[C(k)P(k-1)C^T(k)+R(k)]^-1 (23)

式中，Q(k)系统噪声w的协方差阵；R(k)测量噪声w_e的协方差阵；

可得到k时刻，最优X(k)＝X(k-1)+K(k)[Z(k)-C(k)X(k-1)] (24)

协方差阵P(k)的更新：P(k)＝[I-K(k)C(k)]P(k-1) (25)

在式(23)中，假设

M(k)＝C(k)P(k-1)C^T(k)+R(k) (26)

令N(k)＝M(k)-I，则当N(k)的普半径ρ(n)＜1时，

M^-1(k)＝I-N(k)+N(k)·N(k)-N(k)·N(k)·N(k)+... (27)

当ρ(M)≥1时，式(9)不再收敛；此时假设M(k)绝对值最大特征值为λ_m，若λ_m为正，选择N₁(k)＝(M(k)-η·I)/η则可以按照下式计算：

M^-1(k)＝{I-N₁(k)+N₁(k)·N₁(k)-N₁(k)·N₁(k)·N₁(k)+...}/η (28)

其中η＞λ_m为任意正数，一般可以选择一个便于计算且与λ_m相差不大的正整数；

若λ_m为负，选择N₂(k)＝(M(k)-ξ·I)/ξ则可以按照下式计算：

M^-1(k)＝{I-N₂(k)+N₂(k)·N₂(k)-N₂(k)·N₂(k)·N₂(k)+...}/ξ (29)

其中ξ＜λ_m为任意负数，一般可以选择一个便于计算且与λ_m相差不大的负整数；式(27)-(29)展开项在此项目中选择前两项即可达到精度要求，后面所有展开项舍去。