[发明专利]一种采煤机多惯导定位装置及方法

权利要求书

1.一种采煤机多惯导定位装置的定位方法，采煤机多惯导定位装置包括：主惯导、从惯导、轴编码器、解算系统、滤波器、数据采集显示系统和防爆外壳；

在采煤机前端和后端分别固定防爆外壳，其中前端防爆外壳内安装主惯导和解算系统，后端防爆外壳内安装从惯导和解算系统；轴编码器安装在采煤机行走部；滤波器安装在采煤机电气控制柜中；数据采集显示系统安装在顺槽的工控机上；

主惯导和从惯导分别安装在采煤机前端和后端，两个惯导的相对距离为固定值，以此作为约束条件，提高双惯导定位精度；

包括如下步骤：

步骤1)以两个惯导为例，解算系统采集惯导姿态角和轴编码器速度数据，采用航位推算法解算位置坐标；解算系统将主惯导和从惯导的位置坐标传输到滤波器；

步骤2)滤波器状态方程由航位推算法推导；根据航位推算法，定位方程可简写为其中下标k-1、k分别表示k-1时刻和k时刻；状态量X＝[N₁ E₁ U₁N₂E₂ U₂]^T，N₁、E₁、U₁分别为主惯导北、东、天方向位置坐标，N₂、E₂、U₂分别为从惯导北、东、天方向位置坐标；v为轴编码器输出的速度值；T为采样周期；θ分别为惯导输出的航向角和俯仰角；W为状态噪声；以此定位方程作为扩展卡尔曼滤波器的状态方程；

步骤3)以两个惯导安装后相对距离为固定值的约束条件推导滤波器量测方程；记固定值为r，其满足如下关系式：

式(1)可简写为Z_k＝h(X_k)+V_k，其中下标k表示k时刻；量测量Z＝[r]；V为量测噪声；以此方程作为扩展卡尔曼滤波器的量测方程；

步骤4)滤波器输出为主惯导和从惯导北、东、天的位置坐标，将主惯导北、东、天的位置坐标传输到数据采集显示系统作为采煤机的定位结果；

所述步骤1)中，航位推算法如下所示：

其中下标k-1、k分别表示k-1时刻和k时刻；N、E、U分别为惯导北、东、天方向位置坐标；v为轴编码器输出的速度值；T为采样周期；θ分别为惯导输出的航向角和俯仰角。