[发明专利]一种长输地下管道定位系统的误差补偿方法及实施系统

权利要求书

1.一种长输地下管道定位的误差补偿方法，其特征在于，所述方法的具体步骤如下：

步骤1、以管道入口或管道入口附近的标志建筑设置为坐标原点，建立三维地面坐标系O-XYZ；

步骤2、根据预先掌握的管道位置信息，垂直于O-XY平面向下钻孔至管道表面，设置管道外的坐标修正电路并测量该点处管道中心位置的垂直深度，再利用工程测距仪确定出坐标修正点和管道首末端的三维坐标，并将所测的数据作为坐标修正值提前写入数据处理器中；

步骤3、将定位装置固定在管道机器人上，结合MEMS惯性测量模块的态角数据和里程采集模块的数据完成对管道机器人的定位；

步骤4、在所述管道机器人行进过程中，当接收线圈第k(k＝1,2,3…)次产生感应电流并达到最大值时，将第k个坐标修正值写入数据存储模块；

步骤5、完成管道定位后，上位机对存储模块中的数据进行读取、修正以及三维显示。

2.如权利要求1所述的一种长输地下管道定位的误差补偿方法，其特征在于，所述三维地面坐标系O-XYZ被设置为东北天坐标系，X轴与东西向重合，东向为正，Y轴与南北向重合，北向为正，Z轴与O-XY平面的法线重合，向上为正。

3.如权利要求1所述的一种长输地下管道定位的误差补偿方法，其特征在于，所述MEMS惯性测量模块采用四元数法进行初始对准和姿态角解算。

4.如权利要求3所述的一种长输地下管道定位的误差补偿方法，其特征在于，所述姿态角在无滤波器作用时存在零点漂移和大量噪声信号，对所述姿态角进行降噪处理被设置为采用卡尔曼滤波原理，具体操作如下：

首先根据卡尔曼滤波算法建立系统的状态方程和测量方程：

其中，式中X_k为陀螺仪k时刻的系统状态，Y_k是k时刻的测量值，A_k|k-1和H为状态转移矩阵，W_k和V_k分别表示过程和测量的噪声，设定噪声为高斯白噪声，所述噪声不随系统状态变化而变化，协方差大小分别设置为Q和R，即：

W_k～N(0,Q) 公式(2)

V_k～N(0,R) 公式(3)

验估计预测值为：

验估计误差的协方差矩阵为：

P_k|k-1＝AP_k-1|k-1A^T+Q 公式(5)

根据卡尔曼原理逐步递推系统状态量，在第k步之前的状态已知的情况下，可以得到第k步最优化估计值X_k|k，构造出卡尔曼滤波器的表达式：

式中为Kg_k卡尔曼增益系数，表达式：

后验估计误差协方差矩阵为：

P_k|k＝(I-Kg_kH)P_k|k-1 公式(8)

式中I为单位矩阵，当系统进入k+1状态时，P_k|k成为式(5)的P_k-1|k-1，如此自回归地运算下去。

5.如权利要求1所述的一种长输地下管道定位的误差补偿方法，其特征在于，根据惯性导航累计误差的特点，每间隔800到1000米设置一个坐标修正点。

6.如权利要求1所述的一种长输地下管道定位的误差补偿方法，其特征在于，在所述数据处理器中运行FATFS文件系统，使管道坐标数据在存储模块内以文件的格式存储，方便对管道数据进行管理。

7.如权利要求1所述的一种长输地下管道定位的误差补偿方法，其特征在于，所述管道机器人速度可控，每间隔固定时间停止1s，使惯性测量元件可以输出稳定的俯仰角和航向角。