[发明专利]一种基于位置不变约束及零速校正的采煤机定姿定向方法

说明书

本发明公开了一种基于位置不变约束及零速校正的采煤机定姿定向方法。本发明针对采煤机中的捷联惯导系统长时间工作后，由于惯性器件误差累积导致姿态航向误差发散的问题，发明了一种在惯导系统静止状态时以速度为观测量以及在系统动态工作时以位置不变为观测量两种组合模态下的航向姿态误差抑制方法，对惯导系统的平台偏角以及惯性器件误差进行估计，利用估计出的平台偏角对惯导系统的姿态航向进行修正，从而提高惯导系统在长时间工作时的姿态、航向精度。本发明为采煤机中捷联惯导系长时间工作后的姿态航向保持能力问题提供了解决方案，具有很强的实用价值。

技术领域

本发明涉及一种基于位置不变约束及零速校正的采煤机定姿定向方法，主要适用于提高惯导系统长时间工作后姿态航向的保持能力，属于采煤机用惯性导航技术领域。

背景技术

煤炭是我国重要的能源来源之一，由于开采煤矿环境的恶劣，自动化、无人化、智能化成为我国当前开采煤矿的重要发展趋势。因此，提高采煤机的定位定向精度具有重要的实用意义。

目前，针对采煤机的研究大多为利用无线传感器网络、超声波反射等方法提高其定位精度，但由于煤矿内信号弱，且随着工作长度的增加，信号会衰减等原因会导致定位出现偏差，且无法获取采煤机的姿态航向信息。惯性导航技术得到初始信息后，可无需借助其他导航方式即可进行自主导航，并且既可以提供系统实时位置信息，也可以提供姿态航向信息，因此成为采煤机定位定向的有效解决方案。

然而，在实际应用中，采煤机的工作时间较长，惯导系统在长时间工作后，由于惯性器件误差累积会导致惯导系统测定姿态航向不准确。为提高惯导系统的长航时姿态航向保持能力，本发明提出了一种结合位置和速度两种量测信息的平台偏角估计方法，进而提高惯导系统长时间导航后的姿态航向精度。

发明内容

本发明解决的技术问题是：针对采煤机长时间工作后，惯导系统难以保持高精度姿态航向的问题，提供一种基于位置不变约束及零速校正的采煤机定姿定向方法，在采煤机运动和静止两种模态下，分别选取位置不变约束和零速度作为量测，设计卡尔曼滤波器进行滤波，对系统的平台偏角进行估计并进行补偿，可有效提高惯导系统长航时的姿态航向精度。

本发明的技术解决方案：一种基于位置不变约束及零速校正的采煤机定姿定向方法，其特点在于：利用采煤机时走时停的特点，设计了在运动和静止两种模态下分别利用位置与速度进行观测的卡尔曼滤波模型，通过该方法，可实现对惯导系统平台偏角的估计，补偿后可减小惯导系统的姿态航向误差。该种基于位置不变约束以及零速校正的采煤机定姿定向方法包括如下步骤：

步骤一：对捷联惯导系统进行导航解算，得到惯导系统的实时位置、速度以及姿态航向信息；

步骤二：在执行步骤一的过程中，当惯导系统处于动态工作状态时，由于采煤机总体行进距离较短，为200米左右，可认为其位置基本没有变化，因此，以初始位置信息作为基准，利用惯导系统实时位置信息与初始位置信息之差为观测的卡尔曼滤波模型对系统运动过程中的平台偏角进行估计，得到运动过程中惯导系统平台偏角的估计结果；当惯导系统处于静止状态时，以零速为基准，利用惯导系统静止时刻的速度与零速之差为观测量，进行卡尔曼滤波对系统静止状态下的平台偏角进行估计，得到静止状态下惯导系统平台偏角的估计结果；

步骤三：将两种模态下得到的平台偏角的估计结果补偿进惯导系统中，得到修正后的惯导系统姿态航向信息。

所述步骤二中，结合惯导系统位置和速度信息为观测量的卡尔曼滤波模型对系统运动过程中的平台偏角进行估计过程如下：

卡尔曼滤波模型以误差传播方程和惯性器件误差特性为基础建立状态方程，状态量包括位置误差、速度误差，东向、北向、天向平台偏角以及机体系陀螺漂移，共10维；

卡尔曼滤波模型为：