[发明专利]一种螺旋前进设备导航误差补偿方法

权利要求书

1.一种螺旋前进设备导航误差补偿方法，其特征在于，该方法包括如下步骤：

步骤1)读取惯性/里程计数据和路标点数据；

步骤2)进行惯性导航系统初始对准，得到惯导系统的初始姿态；

步骤3)对滤波器进行初始化，并转入惯导/里程计定位定向滤波状态，进行滤波计算；

步骤4)根据惯性测量单元和里程计的测量数据判断检测设备当前是否处于静止状态；

步骤5)判断设备当前的运动状态以及是否到达路标点，进行以下判断：

5.1)如果当前既不是静止状态，也没有到达路标点，则继续保持惯导/里程计定位定向滤波状态；

5.2)如果当前不是静止状态，但到达了路标点，采用路标点位置作为观测，开始执行路标点间的后处理算法模块；

5.3)如果当前是静止状态，但没有到达路标点，则采用该数据点的速度信息作为观测，开始执行虚拟路标点间的后处理算法模块；

5.4)如果当前是静止状态，同时也到达了路标点，则同时采用路标点位置和数据点的速度信息作为观测量，开始执行融合的路标点间后处理算法模块；

步骤6)达到最后一个路标点并处理完成；

所述的步骤5.3)中的后处理算法模块的算法具体为：

将静止状态点设定为虚拟路标点，在起点采用里程计输出的速度信息作为观测量，对惯性解算进行组合导航滤波算法，到达虚拟路标点后，以该虚拟路标点为起点，进行滤波器反向滤波计算，同样采用闭环修正的方式修正估计误差，之后再进行一次正向滤波计算。

2.如权利要求1所述的一种螺旋前进设备导航误差补偿方法，其特征在于：对步骤1)得出的惯性/里程计数据和路标点数据进行正确性进行检查，具体为

路标点数据中应至少包含两个路标点的详细位置信息及检测设备到达每一路标点的时间；惯性/里程计数据的信息开始数据点测量时间要至少早于检测设备到达某个路标点的时间，结束点测量时间要至少晚于检测设备到达该路标点的下一个路标点的时间，否则数据正确性 检查失败，流程终止。

3.如权利要求1所述的一种螺旋前进设备导航误差补偿方法，其特征在于，

惯性/里程计数据包括：实时存储的惯性器件测量信息、里程计测量信息及其测量时间；

路标点数据包括：各路标点的位置信息和检测设备达到路标点的时间。

4.如权利要求1所述的一种螺旋前进设备导航误差补偿方法，其特征在于：所述的步骤2)惯性导航系统的方式为自对准、辅助设备进行装订对准或者传递对准。

5.如权利要求1所述的一种螺旋前进设备导航误差补偿方法，其特征在于：所述的步骤3)的滤波计算的过程中，采用闭环修正的方式修正估计误差。

6.如权利要求1所述的一种螺旋前进设备导航误差补偿方法，其特征在于，所述的步骤4)所述的静止状态需要同时满足以下三个条件：

陀螺仪输出数据的平方和处于自身阈值范围内；

加速度计输出数据的平方和处于自身阈值范围内；

以及里程计的输出数据为零。

7.如权利要求1所述的一种螺旋前进设备导航误差补偿方法，其特征在于，所述的步骤5.2)中的后处理算法模块中采用的是正反滤波算法。

8.如权利要求1所述的一种螺旋前进设备导航误差补偿方法，其特征在于，所述的步骤5.4)中后处理算法模块的算法具体为：

在起点采用里程计输出的速度信息作为观测量，对惯性解算进行组合导航滤波算法，到达路标点后，将路标点位置信息作为观测量，进行滤波计算，之后再以该路标点为起点，进行滤波器反向滤波计算，同样采用闭环修正的方式修正估计误差，之后再进行一次正向滤波计算。