[发明专利]一种连采机惯性导航定位定向方法

权利要求书

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)展开项在此项目中选择前两项即可达到精度要求，后面所有展开项舍去。