CN108868772B - 一种连采机快速准直控制方法 - Google Patents

Abstract

一种连采机快速准直控制方法，连采机通过安装于其上的陀螺仪、加速度计和里程计测得的连采机的运动轨迹信息，通过光缆传输给上位机，上位机实时的反映出高精度的连采机运动轨迹信息，实时对反馈回来的连采机运动轨迹与既定的运动轨迹进行比较，并通过上位机的运算处理后，并实时修正连采机运动方向；本发明所述的方法实用性强，克服了激光准直方法在实际应用中存在的局限性和GPS导航系统准直方法受矿道中复杂工作环境的影响，实现采煤机直线掘进；该方法的好处在于系统测量结果精确，为技术人员提供高精度的采煤机运动轨迹信息，并且能够根据实时直线度情况指导工作人员修正采煤机的工作方向。

Description

一种连采机快速准直控制方法

技术领域

本发明涉及一种连采机快速准直控制方法，特别涉及一种三轴光纤陀螺仪、三轴加速度计和里程计组合的惯性导航系统应用于露天煤矿连采机的快速准直控制方法。

背景技术

露天煤场在进行挖掘工作时，为保证施工及山体安全，国家法规要求每个矿道间有一定间隔来用于支撑山体。所以在挖进作业时，为防止坑道之间相互挖穿而导致的安全问题，因此最大化保证每条矿道的直线度是实现安全生产的最主要的指标。

连采机坑道直线度检测技术主要分为两种，传统方法一般使用激光准直仪进行矿道的对直校验。此方法对工作环境的要求较高，对于采煤过程中，连采机本身强烈的抖动和矿道中大量的粉尘都对激光准直造成巨大的影响，激光准直法只能在矿道口的四、五米起到比较好的对直效果。因此传统方法在实际应用中存在非常大的局限性。另一种方法是基于导航系统定位，此方法可以克服矿道中复杂工作环境的影响，实现连采机直线掘进。但目前比较流行的GPS在矿道中工作时无法正常接受和发送信号，导致基于导航系统的对直方法存在缺陷。

发明内容

本发明的目的在于克服现有技术中的不足之处，提供一种控制连采机是否沿着既定直线掘进的方法。该系统是基于惯性导航系统和里程计组合而成的连采机快速准直控制系统，连采机从工作面煤层的起始点开始向前掘进时，通过安装于连采机上的惯性导航系统和里程计测量此时连采机的位置和姿态信息以及速度信息，通过光纤通信的方式传输给上位机，经过上位机的运算处理实时的反映出高精度的连采机运动和姿态信息，实时判断连采机运动轨迹的直线度，并实时修正连采机运动方向。惯性导航系统中的三轴光纤陀螺仪和三轴加速度计直接采集连采机在惯性空间的转动角速度和线性加速度信息，通过光纤通信的方式传输给上位机，上位机计算机系统的运算得到连采机的运动速度、航线、姿态和位置信息；利用安装于轨道连采机行走部的里程计测量其行走速度与距离信息，通过光纤通信方式向上位机反馈此时连采机的速度与距离信息，通过上位机的运算处理得到连采机的速度信息；上位机通过改进型卡尔曼快速滤波程序对惯性导航系统测得运动速度、航线、姿态和位置信息和里程计测得的速度与距离信息进行融合，以提高系统测量的精度和稳定性；再以航位角推算原理上位机解析出连采机实时位置和姿态信息，根据实时位置和姿态信息对轨道连采机掘进方向进行实时修正，以保证连采机按照既定直线掘进。

为了实现本发明的目的，我们将采用如下技术方案予以实施。

一种连采机快速准直控制方法，其特征在于：该方法包括如下步骤：

步骤一：为了确定连采机与工作面煤层顶底板的位置关系，连采机定位坐标系与工作面煤层数据库坐标系使用同一坐标系，即以开采起始点为原点的东北天坐标系；获得连采机初始位置和姿态信息后，系统需要解算出连采机在东北天坐标系下的坐标；

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

步骤三：位于行走部的里程计实时测得的在连采机坐标系OX_bY_bZ_b下在采样周期T内连采机的位移增量S₀；

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

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

步骤四：在惯性导航系统测得连采机位于采煤机坐标系下的坐标后，为了利用惯性绝对参考系，将连采机坐标系下的坐标转换为东北天坐标系下的坐标，其过程可分为三次顺序不可交换的基本旋转：

第一次旋转：是将OX_nY_nZ_n绕Z_n轴旋转ψ角，OX_nY_nZ_n旋转到OX₁Y₁Z₁处，这次旋转可以表示为：

第二次旋转：是将OX₁Y₁Z₁绕x₁轴旋转θ角，OX₁Y₁Z₁旋转到OX₂Y₂Z₂处，此次的旋转可以表示为：

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

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

由于

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

也是正交矩阵。

步骤五：连采机的航向角ψ由三轴光纤陀螺仪测得，真实的航向角ψ和测量的航向角

之间存在航向角误差

即

三轴光纤陀螺仪测量航向角误差

包括随机常值的零偏误差ε_ψ和高斯白噪声w_ψ；

连采机的俯仰角θ由三轴光纤陀螺仪测得，真实的俯仰角θ和测量的俯仰角

之间存在俯仰角误差

即

三轴光纤陀螺仪测量俯仰角误差

包括随机常值零偏误差ε_θ和高斯白噪声w_θ；

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

f₀为采煤机运动方向上的真实加速度，加速度计测得的实际加速的与真实加速度之间的关系为：

展开并忽略

和

的高阶小量，可得

记

则

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

假设加速度计测量误差

包括常值随机误差

和随机噪声w_f，其中随机误差▽是一个随机常量：

结合公式(13)，取系统状态变量

可得到状态方程：

其中φ为量测矩阵，w为噪声矩阵；

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

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

其中

系统噪声w＝[ε_χ w_f 0 0]^T，ε_χ＝[ε_θ ε_ψ]^T；

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

V₀是在采样周期T内连采机移动S₀距离的速度，将其投影在东北天坐标系下后表示为：

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

其中，

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

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

展开并忽略

和

的高阶小量，可得

记

则

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

以导航系统推算的速度与里程计测得的速度之差作为观测量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)

令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)展开项在此项目中选择前两项即可达到精度要求，后面所有展开项舍去，算法改进后，大量复杂的卡尔曼滤波增益和系数的计算通过简化，大大降低了运算量，提高了滤波的速度，占用了更少的计算资源；

步骤八：上位机根据航位推算原理建立连采机的方位方程，通过连采机的方位方程获得高精度的当前实时连采机的姿态和位置信息，将实时的连采机的姿态和位置信息和步骤一拟合的连采机既定的运动轨迹进行比较，得到误差信息，通过上位机对误差信息进行运算处理后，给连采机发送控制信号控制连采机运动方向。

进一步，步骤二中所述的初始姿态信息即姿态矩阵初始值

是由连采机的初始姿态角确定的。

进一步，步骤八中所述的连采机的方位方程为：

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是离散系统的采样周期。

进一步，所述的上位机为工控机，其包括编程设备，编程设备包括编程软件，使用编程软件编辑的程序包括：连采机直线度检测程序，该程序包括连采机姿态基准计算、连采机坐标系到导航坐标系方向余弦函数矩阵、导航计算以及姿态角计算，其中：

连采机姿态基准计算利用三轴加速度计测得的沿连采机坐标系下三个坐标轴的加速度和三轴光纤陀螺仪测得的绕连采机坐标系下三个坐标轴的角速度以及导航计算计算出的导航坐标系下位移进行运算，运算结果包括方位余弦函数矩阵和方向余弦函数矩阵；

连采机坐标系到导航坐标系方向余弦函数矩阵利用里程计测得的连采机坐标系下位移和连采机姿态基准计算计算出的方向余弦函数矩阵进行运算，运算结果包括连采机导航坐标系下位移；

导航计算利用连采机坐标系到导航坐标系方向余弦函数矩阵计算出的连采机导航坐标系下位移进行运算，运算结果包括连采机的导航坐标系下的位置坐标和速度信息以及导航坐标系下位移；

姿态角计算利用连采机姿态基准计算计算出的方位余弦函数矩阵进行运算，运算结果包括姿态方位。

更进一步，所述的编程软件编辑的程序还包括：快速卡尔曼滤波程序，所述的快速卡尔曼滤波程序是将来自三轴光纤陀螺仪和三轴加速度计的测得的连采机速度和姿态信息转化为东北天坐标系下连采机的速度信息，然后根据东北天坐标系下的速度信息建立系统状态方程；将里程计测得的连采机速度信息转化为东北天坐标系下连采机的速度信息，然后根据东北天坐标系下的速度信息建立系统测量方程；利用系统状态方程和系统测量方程进行滤波处理，获得更加精确的连采机在东北天坐标系下的速度信息。

进一步，所述的滤波处理具体实施方法：根据K-1时刻的状态预测第K时刻的状态X(K∣K-1)→根据K-1时刻的系统预测误差估计第K时刻的系统预测误差P(K∣K-1)→计算卡尔曼增益K→计算系统最优估计值→计算系统当前时刻的系统预测误差P(K∣K)→根据K-1时刻的状态预测第K时刻的状态X(K∣K-1)。

更进一步，所述的编程软件编辑的程序还包括：连采机三维定位算法，在上位机的系统初始化后载入连采机的初始姿态角、连采机初始位置、三轴光纤陀螺仪测得的初始角速度、里程计测得的连采机位移增量给连采机三维定位算法：计算初始方位余弦函数矩阵→计算地球坐标系相对于惯性坐标系的角速率→计算导航坐标系相对于地球坐标系的角速率→计算导航坐标系相对于惯性坐标系的角速率→计算连采机坐标系相对于导航坐标系的角速率→解方向余弦函数矩阵微分方程/更新方向余弦函数矩阵→将里程计测得的连采机坐标系下的位移转换到导航坐标系下→在导航坐标系下求得连采机的位置和速率→计算初始方位余弦函数矩阵。

有益效果

本发明所述的一种连采机快速准直控制方法实用性强、精度高，克服了激光准直方法在实际应用中存在的局限性；克服了GPS导航系统准直方法受矿道中复杂工作环境的影响，实现连采机直线掘进；本发明带来的好处在于系统测量结果精确，为技术人员提供高精度的连采机运动轨迹信息，并且能够根据实时直线度情况指导工作人员修正连采机的工作方向。

附图说明

图1为连采机直线度检测程序流程图；

图2为快速卡尔曼滤波程序流程图；

图3为连采机三维定位算法程序流程图；

图4为连采机三轴光纤陀螺仪、三轴加速度计和里程计安装示意图。

具体实施方式

下面结合附图和实施例对本发明做进一步地说明。

实施例1，如图1-4所示，一种连采机快速准直控制方法包括如下步骤：

步骤一：为了确定连采机(4)与工作面煤层顶底板的位置关系，连采机(4)定位坐标系与工作面煤层数据库坐标系使用同一坐标系，即以开采起始点为原点的东北天坐标系；获得连采机(4)初始位置和姿态信息后，系统需要解算出连采机(4)在东北天坐标系下的坐标；

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

步骤三：位于行走部的里程计(3)实时测得的在连采机(4)坐标系OX_bY_bZ_b下在采样周期T内连采机(4)的位移增量S₀；

里程计(3)在连采机(4)行走时会发出A,B两路相位差90°的数字脉冲信号，行走部正转时A超前B为90°，当反转时B超前A为90°，脉冲的个数与位移量成比例关系；惯性导航系统先对里程计(3)输出的两路脉冲进行鉴相，判断是正转还是反转；其次进行正转加数，反转减数；位移增量S₀可以表示为

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

步骤四：在惯性导航系统测得连采机(4)位于连采机(4)坐标系下的坐标后，为了利用惯性绝对参考系，将连采机(4)坐标系下的坐标转换为东北天坐标系下的坐标，其过程可分为三次顺序不可交换的基本旋转：

第一次旋转：是将OX_nY_nZ_n绕Z_n轴旋转ψ角，OX_nY_nZ_n旋转到OX₁Y₁Z₁处，这次旋转可以表示为：

第二次旋转：是将OX₁Y₁Z₁绕x₁轴旋转θ角，OX₁Y₁Z₁旋转到OX₂Y₂Z₂处，此次的旋转可以表示为：

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

则从导航坐标系到连采机(4)坐标系的坐标转换矩阵为：

由于

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

也是正交矩阵：

步骤五：连采机(4)的航向角ψ由三轴光纤陀螺仪(1)测得，真实的航向角ψ和测量的航向角

之间存在航向角误差

即

三轴光纤陀螺仪(1)测量航向角误差

包括随机常值的零偏误差ε_ψ和高斯白噪声w_ψ；

连采机(4)的俯仰角θ由三轴光纤陀螺仪(1)测得，真实的俯仰角θ和测量的俯仰角

之间存在俯仰角误差

即

三轴光纤陀螺仪(1)测量俯仰角误差

包括随机常值零偏误差ε_θ和高斯白噪声w_θ；

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

f₀为连采机(4)运动方向上的真实加速度，加速度计(2)测得的实际加速的与真实加速度之间的关系为：

展开并忽略

和

的高阶小量，可得

记

则

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

假设加速度计(2)测量误差

包括常值随机误差▽和随机噪声w_f，其中随机误差▽是一个随机常量：

结合公式(13)，取系统状态变量

可得到状态方程：

其中φ为量测矩阵，w为噪声矩阵；

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

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

其中

系统噪声w＝[ε_χ w_f 0 0]^T，ε_χ＝[ε_θ ε_ψ]^T；

步骤六：里程计(3)在一个采样周期T内，测得真实位移为s＝[0 s₀ 0]^T，则在采样周期T内其速度为V^D＝[0 V₀ 0]^T，其中

V₀是在采样周期T内连采机移动S₀距离的速度，将其投影在东北天坐标系下后表示为：

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

其中，

为里程计(3)刻度误差，其值为一个随机常数；

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

展开并忽略

和

的高阶小量，可得

记

则

里程计(3)的速度测量误差方程为：

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

其中

w_e为测量噪声；

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

对于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)

令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)展开项在此项目中选择前两项即可达到精度要求，后面所有展开项舍去,算法改进后，大量复杂的卡尔曼滤波增益和系数的计算通过简化，大大降低了运算量，提高了滤波的速度，占用了更少的计算资源；

步骤八：根据航位推算原理建立连采机(4)的方位方程，通过连采机(4)的方位方程获得高精度的当前实时连采机的姿态和位置信息，将实时的连采机(4)的姿态和位置信息和步骤一拟合的连采机(4)既定的运动轨迹进行比较，得到误差信息，通过上位机对误差信息进行运算处理后，给连采机(4)发送控制信号控制连采机(4)运动方向,其中，所述的连采机的方位方程为：

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

其中，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是离散系统的采样周期。

实施例2，如图1-4所示，所述的初始姿态信息即姿态矩阵初始值

是由连采机(4)的初始姿态角确定的。

实施例3，如图1所示，所述的上位机为工控机，其包括编程设备，编程设备包括编程软件，使用编程软件编辑的程序包括：连采机直线度检测程序，该程序包括连采机姿态基准计算、连采机坐标系到导航坐标系方向余弦函数矩阵、导航计算以及姿态角计算，其中：

连采机姿态基准计算利用三轴加速度计测得的沿连采机坐标系下三个坐标轴的加速度和三轴光纤陀螺仪测得的绕连采机坐标系下三个坐标轴的角速度以及导航计算计算出的导航坐标系下位移进行运算，运算结果包括方位余弦函数矩阵和方向余弦函数矩阵；

连采机坐标系到导航坐标系方向余弦函数矩阵利用里程计测得的连采机坐标系下位移和连采机姿态基准计算计算出的方向余弦函数矩阵进行运算，运算结果包括连采机导航坐标系下位移；

导航计算利用连采机坐标系到导航坐标系方向余弦函数矩阵计算出的连采机导航坐标系下位移进行运算，运算结果包括连采机的导航坐标系下的位置坐标和速度信息以及导航坐标系下位移；

姿态角计算利用连采机姿态基准计算计算出的方位余弦函数矩阵进行运算，运算结果包括姿态方位。

实施例4，如图2和4所示，所述的编程软件编辑的程序还包括：快速卡尔曼滤波程序，所述的快速卡尔曼滤波程序是将来自三轴光纤陀螺仪(1)和三轴加速度计(2)的测得的连采机(4)速度和姿态信息转化为东北天坐标系下连采机的速度信息，然后根据东北天坐标系下的速度信息建立系统状态方程；将里程计(3)测得的连采机(4)速度信息转化为东北天坐标系下连采机的速度信息，然后根据东北天坐标系下的速度信息建立系统测量方程；利用系统状态方程和系统测量方程进行滤波处理，获得更加精确的连采机在东北天坐标系下的速度信息。

实施例5，如图2所示，所述的滤波处理具体实施方法：根据K-1时刻的状态预测第K时刻的状态X(K∣K-1)→根据K-1时刻的系统预测误差估计第K时刻的系统预测误差P(K∣K-1)→计算卡尔曼增益K→计算系统最优估计值→计算系统当前时刻的系统预测误差P(K∣K)→根据K-1时刻的状态预测第K时刻的状态X(K∣K-1)。

实施例6，如图3所示，所述的编程软件编辑的程序还包括：连采机三维定位算法，在上位机的系统初始化后载入连采机的初始姿态角、连采机初始位置、三轴光纤陀螺仪测得的初始角速度、里程计测得的连采机位移增量给连采机三维定位算法：计算初始方位余弦函数矩阵→计算地球坐标系相对于惯性坐标系的角速率→计算导航坐标系相对于地球坐标系的角速率→计算导航坐标系相对于惯性坐标系的角速率→计算连采机坐标系相对于导航坐标系的角速率→解方向余弦函数矩阵微分方程/更新方向余弦函数矩阵→将里程计测得的连采机坐标系下的位移转换到导航坐标系下→在导航坐标系下求得连采机的位置和速率→计算初始方位余弦函数矩阵。

实施例7，如图4所示，所述的三轴光纤陀螺仪(1)通过数据连接线向上位机传输绕连采机(4)坐标系下三个坐标轴的角速度信号；

所述的加速度计(2)通过数据连接线向上位机传输沿连采机(4)坐标系下三个坐标轴的加速度信号；

所述的里程计(3)通过数据连接线向上位机传输连采机(4)坐标系下位移信号。

实施例8，如图4所示，所述的上位机为PLC控制系统，PLC控制系统包括PID控制器，PLC控制系统采集所述的绕连采机坐标系下三个坐标轴的角速度信号、沿连采机坐标系下三个坐标轴的加速度信号和连采机坐标系下位移信号，经PLC控制系统运算处理转化为PLC控制系统可识别的数字量后，PLC控制系统根据既定的连采机(4)运动轨迹与反馈回来的运动轨迹进行比较，并经过PID控制器运算处理后，给连采机(4)发送控制信号控制连采机(4)运动方向。

实施例9，所述的编程设备是使用安装了编程软件的计算机，在计算机屏幕上直接生成和编辑各种文本程序或图形程序，能实现不同编程语言之间的相互转换；程序被编译后下载到PLC控制系统，也能将PLC控制系统中的程序上传到计算机；程序和数据能存盘或打印，通过网络，能实现远程编程；编程软件还具有对网络和硬件组态、参数设置、监控和故障诊断功能。

实施例10，在上位机上使用编程软件编辑的程序还包括：连采机(4)方位方程，该方程是依据航位推算原理建立的，通过连采机(4)的方位方程的计算，获得高精度的当前实时连采机(4)的姿态和位置信息。

Claims (7)

1.一种连采机快速准直控制方法，其特征在于：该方法包括如下步骤：

步骤一：为了确定连采机与工作面煤层顶底板的位置关系，连采机定位坐标系与工作面煤层数据库坐标系使用同一坐标系，即以开采起始点为原点的东北天坐标系；获得连采机初始位置和姿态信息后，惯性导航系统需要解算出连采机在东北天坐标系下的坐标；

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

步骤三：位于行走部的里程计实时测得在连采机坐标系OX_bY_bZ_b下在采样周期T内连采机的位移增量S₀；

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

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

步骤四：在惯性导航系统测得连采机位于连采机坐标系下的坐标后，为了利用惯性绝对参考系，将连采机坐标系下的坐标转换为东北天坐标系下的坐标，其过程可分为三次顺序不可交换的基本旋转：

第一次旋转：是将OX_nY_nZ_n绕Z_n轴旋转ψ角，OX_nY_nZ_n旋转到OX₁Y₁Z₁处，这次旋转可以表示为：

第二次旋转：是将OX₁Y₁Z₁绕x₁轴旋转θ角，OX₁Y₁Z₁旋转到OX₂Y₂Z₂处，此次的旋转可以表示为：

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

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

由于

都是正交矩阵，所以

也是正交矩阵：

步骤五：连采机的航向角ψ由三轴光纤陀螺仪测得，真实的航向角ψ和测量的航向角

之间存在航向角误差

即

三轴光纤陀螺仪测量航向角误差

包括随机常值的零偏误差ε_ψ和高斯白噪声w_ψ；

连采机的俯仰角θ由三轴光纤陀螺仪测得，真实的俯仰角θ和测量的俯仰角

之间存在俯仰角误差

即

三轴光纤陀螺仪测量俯仰角误差

包括随机常值零偏误差ε_θ和高斯白噪声w_θ；

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

f₀为连采机运动方向上的真实加速度，加速度计测得的实际加速的与真实加速度之间的关系为：

展开并忽略

和

的高阶小量，可得

记

则

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

假设加速度计测量误差

包括常值随机误差

和随机噪声w_f，其中随机误差

是一个随机常量：

结合公式(13)，取系统状态变量

可得到状态方程：

其中φ为量测矩阵，w为噪声矩阵；

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

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

其中

系统噪声w＝[ε_χ w_f 0 0]^T，ε_χ＝[ε_θ ε_ψ]^T；

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

，V₀是在采样周期T内连采机移动S₀距离的速度，将其投影在东北天坐标系下后表示为：

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

其中，

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

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

展开并忽略

和

的高阶小量，可得

记

则

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

以导航系统推算的速度与里程计测得的速度之差作为观测量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)

令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)展开项在此项目中选择前两项即可达到精度要求，后面所有展开项舍去；算法改进后，大量复杂的卡尔曼滤波增益和系数的计算通过简化，大大降低了运算量，提高了滤波的速度，占用了更少的计算资源；

步骤八：根据航位推算原理建立连采机的方位方程，通过连采机的方位方程获得高精度的当前实时连采机的姿态和位置信息，将实时的连采机的姿态和位置信息和步骤一拟合的连采机既定的运动轨迹进行比较，得到误差信息，通过上位机对误差信息进行运算处理后，给连采机发送控制信号控制连采机运动方向。

2.根据权利要求1所述的一种连采机快速准直控制方法，其特征在于：步骤二中所述的初始姿态信息即姿态矩阵初始值

是由连采机的初始姿态角确定的。

3.根据权利要求1所述的一种连采机快速准直控制方法，其特征在于：步骤八中所述的连采机的方位方程为：

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是离散系统的采样周期。

4.根据权利要求1所述的一种连采机快速准直控制方法，其特征在于：所述的上位机为工控机，其包括编程设备，编程设备包括编程软件，使用编程软件编辑的程序包括：连采机直线度检测程序，该程序包括连采机姿态基准计算、连采机坐标系到导航坐标系方向余弦函数矩阵、导航计算以及姿态角计算，其中：

连采机姿态基准计算利用三轴加速度计测得的沿连采机坐标系下三个坐标轴的加速度和三轴光纤陀螺仪测得的绕连采机坐标系下三个坐标轴的角速度以及导航计算出的导航坐标系下位移进行运算，运算结果包括方位余弦函数矩阵和方向余弦函数矩阵；

连采机坐标系到导航坐标系方向余弦函数矩阵利用里程计测得的连采机坐标系下位移和连采机姿态基准计算出的方向余弦函数矩阵进行运算，运算结果包括连采机导航坐标系下位移；

导航计算利用连采机坐标系到导航坐标系方向余弦函数矩阵计算出的连采机导航坐标系下位移进行运算，运算结果包括连采机的导航坐标系下的位置坐标和速度信息以及导航坐标系下位移；

姿态角计算利用连采机姿态基准计算出的方位余弦函数矩阵进行运算，运算结果包括姿态方位。

5.根据权利要求4所述的一种连采机快速准直控制方法，其特征在于：所述的编程软件编辑的程序还包括：快速卡尔曼滤波程序，所述的快速卡尔曼滤波程序是将来自三轴光纤陀螺仪和三轴加速度计的测得的连采机速度和姿态信息转化为东北天坐标系下连采机的速度信息，然后根据东北天坐标系下的速度信息建立系统状态方程；将里程计测得的连采机速度信息转化为东北天坐标系下连采机的速度信息，然后根据东北天坐标系下的速度信息建立系统测量方程；利用系统状态方程和系统测量方程进行滤波处理，获得更加精确的连采机在东北天坐标系下的速度信息。

6.根据权利要求1所述的一种连采机快速准直控制方法，其特征在于：所述的滤波处理具体实施方法：根据K-1时刻的状态预测第K时刻的状态X(K∣K-1)→根据K-1时刻的系统预测误差估计第K时刻的系统预测误差P(K∣K-1)→计算卡尔曼增益K→计算系统最优估计值→计算系统当前时刻的系统预测误差P(K∣K)→根据K-1时刻的状态预测第K时刻的状态X(K∣K-1)。

7.根据权利要求4所述的一种连采机快速准直控制方法，其特征在于：所述的编程软件编辑的程序还包括：连采机三维定位算法，在上位机的系统初始化后载入连采机的初始姿态角、连采机初始位置、三轴光纤陀螺仪测得的初始角速度、里程计测得的连采机位移增量给连采机三维定位算法：计算初始方位余弦函数矩阵→计算地球坐标系相对于惯性坐标系的角速率→计算导航坐标系相对于地球坐标系的角速率→计算导航坐标系相对于惯性坐标系的角速率→计算连采机坐标系相对于导航坐标系的角速率→解方向余弦函数矩阵微分方程/更新方向余弦函数矩阵→将里程计测得的连采机坐标系下的位移转换到导航坐标系下→在导航坐标系下求得连采机的位置和速率→计算初始方位余弦函数矩阵。

Images