[发明专利]一种基于组合导航系统的地下管线三维轨迹测量方法

权利要求书

1.一种基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，步骤包括：

步骤1：建立MEMS陀螺仪和加速度计的误差模型；

步骤2：建立速度、位置、姿态参数的误差方程；

步骤3：根据管线测量仪的运动特征、MEMS陀螺仪和加速度计的误差模型和速度、位置、姿态误差方程确定状态变量以及状态方程；

步骤4：利用MEMS磁强计、里程仪和准零速信息作为外部量测量，建立卡尔曼滤波的量测方程；

步骤5：将卡尔曼滤波连续系统离散化；

步骤6：进行卡尔曼滤波方程解算，根据解算结果对陀螺、加速度计输出数据进行补偿。

2.根据权利要求1所述的基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，所述步骤1中，陀螺仪误差模型为：

加速度计误差模型为：

3.根据权利要求1所述的基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，所述步骤4中，采用准零速信息、磁通门信息和里程仪信息作为外部观测量，对惯性导航解算的结果进行修正。

4.根据权利要求3所述的基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，所述步骤4中，MEMS-IMU解算速度与准零速度约束之差为：

5.根据权利要求3所述的基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，所述步骤4中，里程仪信息更新解算得到的纬度、经度和高度信息为：

6.根据权利要求3所述的基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，所述步骤4中，由三轴加速度计结合三轴磁通门计算俯仰角θ、横滚角γ及方位角的公式为：

7.根据权利要求3所述的基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，所述步骤4中，管道三维轨迹测组合测量方法的量测方程：

8.根据权利要求1所述的基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，所述步骤5中，将卡尔曼滤波连续系统离散化得：

9.根据权利要求1所述的基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，所述步骤6中，对加速度计数据补偿为：

10.根据权利要求1所述的基于组合导航系统的地下管线三维轨迹测量方法，其特征在于，所述步骤6中，对陀螺数据补偿为：