基于捷联惯性导航的非开挖管道位置检测系统

，，，

(中国计量大学 机电工程学院，杭州 310018)

0 引言

城市地下管线是运输水、油气、热力等流体的重要载体，管道输运是否正常运行是关乎民生安全和国民经济发展的重大问题。地下管道泄漏不仅带来巨大经济损失，甚至会造成重大环境污染和人员伤亡事故[1]。

非开挖技术因其经济、环保、安全、高效等特点得到广泛应用，加速了管道运输业的发展[2]。非开挖技术敷设的管道往往掩埋深度较大，而且常出现穿过河流、湖泊等情况，加大了管道探测难度，给桥梁施工和管道并行敷设带来极大不便。目前常用的地下管道位置探测方法有电磁感应法、地质雷达法、高密度电法和高精度磁测法[3]。但对于埋深在5米以下或穿过河流、湖泊的管道，应用以上方法探测难以达到精度要求。为此，研究有效的非开挖管道位置检测系统有很大的现实意义。

为解决非开挖管道探测深度大和环境干扰的问题，本文基于捷联惯性导航原理设计一种非开挖管道位置检测系统，结合姿态角和里程轮数据，检测管道机器人的位置，从而实现对管道的定位。重点阐述系统组成、惯性导航原理、以及利用卡尔曼滤波算法对姿态角进行最优估计。最后，在模拟非开挖管道内完成检测实验，计算系统的检测精度，并验证系统的正确性与可行性。

1 系统概述

惯性导航系统(inertial navigation system, INS)依靠惯性敏感元件，不与外界进行信息交互，不易受到电磁干扰的影响，是一种自主式导航系统[4]。惯性导航定位不会因探测深度大而不工作，工作效率高，设备体积小，对人员技术要求低。

非开挖管道位置检测系统总体构成如图1所示。该系统主要由主控制模块、MEMS惯性测量单元、里程采集单元、存储卡以及PC机界面构成。MEMS惯性传感器具有功耗低、重量轻、体积小、成本低等优点，在众多领域得到应用[5]。系统中主控制模块、惯性测量单元、里程采集单元、存储卡构成检测器固定在管道机器人的车体上，随机器人在管道中行进，完成机器人地理坐标的检测。设备在非开挖管道内难以与地面实时通信，因此选择将定位数据存入存储卡，待机器人走完管道全程再读取存储卡数据，从而完成对管道定位的整个流程。

图1 系统总体构成

2 系统组成

2.1 硬件组成

检测器主要由三部分组成，分别为主控制模块、管道小车姿态距离传感器模块以及存储卡模块。

系统微控制器主要完成对传感器数据的采集、处理与存储。主控制模块以STM32F103RET6为核心，该芯片是基于Coretex-M3内核的32位ARM处理器，具有512 KB的Flash以及64 KB的SRAM，最高工作频率可达到72 MHz。并且芯片的DMA控制器和SPI接口符合设计要求，并包含睡眠、停机和待机3种模式，具有功耗低、运行速度快的优点。惯性模块采用MPU9250传感器，该传感器内部集成有三轴加速度计、三轴陀螺仪和三轴磁力计；可以通过集成电路总线(IIC)或串行外设接口(SPI)方式与单片机进行数据交换。

里程轮上使用磁铁和霍尔片提供里程信息，结构如图2所示，霍尔片选用SS495A磁敏器件。里程轮在杂质较多的管道内易出现打滑现象，从而影响数据准确性，因此要采用多个里程轮并根据实际改进里程轮的设计[6]。本设计根据管径大小选取合适减震筒，使车轮一直紧贴管壁行驶；采用高强度材料作为轮毂，并对车轮外表面做滚花处理[7]，增加车轮与管壁的摩擦力。

图2 里程轮结构示意图

SD卡选用8 GB的microSD卡(TF卡)，microSD卡是一种极细小的快闪存储器卡，广泛应用于智能设备中。

2.2 系统主要流程

检测器工作流程如图3所示。开机时检测器对准东方向，使航向角在东向大小为零度。开机完成各模块初始化后，将机器人放入管道入口，待机器人平稳时，按键开始定位并将原点坐标存入存储卡。机器人在管道内由拖拽装置牵引行驶，速度为0.2～0.3 m/s，每间隔2.5 s进行一次数据的采集、处理与存储。机器人向前行驶直至管道出口处，按停止键结束定位操作。

图3 检测器工作流程

FatFS文件系统为底层硬件驱动提供了标准的接口函数，是由标准C语言编写的开源文件系统，适用于小型嵌入式系统。为方便管理SD卡中的数据，选择将FatFS文件系统嵌入到STM32控制器中。SD卡具有两种通信模式：SD模式与SPI模式。SD模式相比于SPI模式对硬件要求更高，因此SPI模式被广泛应用。SPI是一种高速的、同步的全双工通信总线。设计本系统SD卡电路时，主从设备间只需要连接三根线，即SCK、SDI和SDO。SPI通信是以字节为单位，在SCK时钟脉冲的控制下，完成主机与从机的通信。管道地理坐标数据在SD卡内以.xml文件格式存储，方便对坐标数据进行管理。检测工作结束后，利用MATLAB读取SD卡中的文件数据，完成管道坐标的三维显示?

3 系统工作原理

3.1 坐标系选择

选用东北天坐标系作为地面坐标系(o1-x1y1z1)，以定位起始位置或起始位置附近的标志性建筑为坐标原点，x轴与东西向重合，东向为正，y轴与南北向重合，北向为正，z轴与xy平面的法线重合，向上为正[8]。载体绕z轴旋转的角度定义为航向角(Yaw)，绕y轴旋转的角度定义为俯仰角(Pitch)。图4为惯性导航坐标变换示意图。

图4 坐标变换示意图

3.2 惯性导航基本方程

惯性导航(Inertial Navigation)分为平台式和捷联式两种。捷联方式是惯性测量器件直接固连在载体上，并随载体做同样的运动，从而实现对载体速度和姿态角的解算。在无磁场干扰的情况下，MPU9250传感器的航向角可以通过磁力计校准，但现实环境中磁场无处不在，尤其是铁磁性金属管道内磁场干扰较大，会给航向角带来更大偏差。因此，本系统不采用磁力计的算法对航向角进行较准。

如图4所示，惯性导航将载体坐标系(o2-x2y2z2)的旋转体现在地面坐标系(o1-x1y1z1)上，用姿态角的形式直观地表现出来。姿态角解算常用的算法有方向余弦法、欧拉角求解法和四元数法。方向余弦法存在计算量大的缺点，难以实现载体姿态实时解算。欧拉角法的解算方程存在奇点，不能对载体的所有姿态进行解算。由于四元数法计算量小，计算方程无奇点的优点，因此四元数法被广泛应用于姿态解算[9]。1843年爱尔兰数学家哈密顿首先提出了四元数的数学概念。四元数是由一个实数和3个虚数单位i、j、k组成，其基本数学表达式为[10]：

(1)

式中，λ、p1、p2、p3为实数，i、j、k为两两正交的单位向量。以Ψ、θ、γ分别表示载体的航向角、俯仰角和横滚角。

首先由初始姿态角确定初始四元数[11]：

(2)

四元数微分方程可以实现四元数的更新，MPU9250传感器首先检测出角速度值，角速度与四元数的微分方程为：

(3)

式中，ω为MPU9250传感器角速度矩阵：

(4)

四元数微分方程通常采用四阶龙格库塔法和四元数归一化求解?龙格库塔法是拉格朗日中值定理的改进，在给定的区间内预估计几个点的斜率，再将他们的加权平均数作为平均斜率，构成高精度计算，预估计区间内4个点的斜率则为四阶龙格库塔公式，也就是工程上应用广泛的经典龙格库塔公式。四阶龙格库塔法求解四元数微分方程的具体过程为：

(5)

则:

(6)

其中：T为更新周期，q(t)为t时刻的初始四元数，q(t+T)为经过T时间后的四元数的值。K1是初始时刻的斜率，K2、K3都是中间点的斜率，K4是时间终点的斜率。

通过实时更新的四元数与旋转矩阵，可以解算出各姿态角的值[12]:

(7)

姿态角表达式为：

俯仰角：θ=-sin-1(T13)

(8)

(9)

式中,T表示旋转矩阵中的元素。

3.3 卡尔曼滤波器设计

以航向角为例，将MPU9250传感器固定，在无滤波器作用时对传感器进行静态测试，采集分析150个航向角数据，结果如图5所示。结果表明，惯性传感器静止时姿态角波动较大，存在零点漂移和大量的噪声信号。如果直接将未滤波的姿态角应用于位置检测，结果必然难以达到精度要求。

图5 无滤波器的静态结果

卡尔曼滤波器可以解决离散的线性系统滤波问题，是一种高效、快速的自回归滤波器。卡尔曼滤波算法通过递归方式减小系统的噪声与误差，具有实时性好和精度高等特点。针对MPU9250传感器测量过程中的漂移问题，本系统提出利用卡尔曼滤波算法对姿态角进行降噪处理。首先根据卡尔曼滤波算法建立系统的状态方程和测量方程[13]：

(10)

式中,Xk为陀螺仪k时刻的系统状态，Yk是k时刻的测量值，Ak|k-1和H为状态转移矩阵，Wk和Vk分别表示过程和测量的噪声。将噪声近似为均值为零的独立高斯白噪声(White Gaussian Noise)，它们不随系统状态变化而变化，协方差矩阵是非负定常值对角矩阵，大小设为Q和R，即：

Wk～N(0,Q)

(11)

Vk～N(0,R)

(12)

卡尔曼滤波是利用反馈控制来估计的过程，包含两个部分：时间更新方程和状态更新方程。时间更新方程负责估计当前状态和计算先验估计协方差。先验估计状态值为：

(13)

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

Pk|k-1=APk-1|k-1AT+Q

(14)

根据卡尔曼原理逐步递推系统状态量，在第k步之前的状态已知的情况下，可以得到第k步最优化估计值Xk|k，构造出卡尔曼滤波器的状态更新方程[14]：

(15)

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

(16)

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

Pk|k=(I-KgkH)Pk|k-1

(17)

式中,I为单位矩阵。状态更新方程首先要计算卡尔曼增益Kgk，再根据测量值Yk和先验估计值确定后验估计。最后计算后验估计误差协方差。当系统进入k+1状态时，Pk|k代替式(11)的Pk-1|k-1，如此自回归地运算下去。

滤波参数包括过程噪声协方差Q和测量噪声协方差R，将先前的150个姿态角作为滤波器输入，运用实验的方法计算出矩阵Q和R。根据以上算法流程得到稳定的最优估计角度。同样对惯性传感器进行静态测试，采集150个航向角数据，结果如图6所示。

图6 卡尔曼滤波静态结果

为了更直观地观测本文卡尔曼滤波的效果，给出卡尔曼滤波前后姿态角的方差值，如表1所示。

表1 静态姿态角方差对比

对比姿态曲线图和方差结果，证明本文的卡尔曼滤波方法可以减小随机误差，具有良好的漂移抑制能力。

3.4 里程计算原理

检测器固定在管道机器人车体上，并随管道机器人在管道内平稳行驶。检测工作开始后，里程轮紧贴管壁行驶。由于在轮毂和减震筒分别安装磁铁和霍尔片，车轮每旋转一周霍尔片就会发出一个电脉冲信号，使主控模块计数器的值n加一。结合里程轮外周长l，则管道机器人的行驶距离为：

S=n×l

(18)

4 实验结果与分析

4.1 实验结果

本实验有两个目的：1)对比施加强磁干扰前后的检测结果，验证磁场对定位精度是否产生影响；2)通过管道位置检测实验，计算系统的定位精度。具体操作如下：搭建实验平台模拟非开挖管道，实验管道与管道机器人成比例缩小，管道直径为20 cm，长度为15 m。管道机器人由拖拽装置牵引行驶，行驶速度为0.2～0.3 m/s，采样时间为2.5 s。实验利用指南针确定坐标系，20 m卷尺进行实际测量。

在实验管道内完成两组实验：第一组管道外壁不放置强力磁铁，第二组在管道外壁每间隔0.7 m放置两个强力磁铁。以定位起始点为坐标原点，以东北天方向分别作为三轴建立坐标系。检测结果如图7所示。

图7 管道位置检测结果

4.2 实验分析

1)对比前后两组实验的结果，发现两组数据基本重合，证明外界磁场干扰对检测精度不产生影响。

2)由检测结果的三维曲线图可知，计算轨迹与实际轨迹基本相符。实际终点坐标为(12.78, 3.86,0)，测量的终点坐标为(12.14,3.62,-0.15)。终点处x轴的测量偏差为-0.64 m，y轴的测量偏差为-0.24 m，z轴高度方向的偏差为-0.15 m。如果不使用卡尔曼滤波，定位结果会在开始之后很快发散。

5 结语

本文基于捷联惯性导航原理设计非开挖管道位置检测系统。检测器工作时不需要与外界进行信息交互，解决了油气管道探测深度大和环境干扰的难题。

首先设计系统的硬件组成，阐述管道定位的主要流程；重点介绍了惯性导航基本原理和卡尔曼滤波算法，并对比卡尔曼滤波前后姿态角的漂移情况，解决姿态角的随机误差问题，从而显著提高了检测精度。

最后在模拟非开挖管道内完成位置检测实验，对比证明了磁场干扰对定位精度不产生影响，计算了系统的定位精度，验证了该系统的有效性与可靠性。

[1] 孟令雅,李玉星,宋立群等.输气管道泄漏音波传播特性及监测定位[J].天然气工业，2010,30(11)：74-79.

[2] 沈林勇.非开挖地下信息管线的三维曲线探测新技术研究[D].上海:上海大学,2011.

[3] 王 勇,王 永.综合物探方法在非开挖工艺敷设地下管线探测中的应用[J].测绘通报,2011,4:58-61.

[4] 任春华,陈 灿,王满喜，等.一种惯性测量与距离组合的定位修正方法研究[J],传感仪表学报,2013,32(11):2513-2519.

[5] Rantakokko J,Rydell Joakim,Stroback Peter.Accurate and reliable soldier and first responder indoor positioning:mulitsensor systems and cooperative localization[J].IEEE Wireless Communications,2011,18(2):10-18.

[6] 郭静波,蔡 雄,胡铁华,等.油气管道中智能机器人跟踪定位关键技术综述[J].仪器仪表学报,2015,36(3):481-498.

[7] 臧延旭,邱 城,胡铁华,等.管道内检测器里程轮打滑原因[J].油气储运,2016，35(3)：306-310.

[8] 黄 昆,胡小毛,李士心,等.SINS/GPS组合导航系统选星算法[J].中国惯性技术学报,2009,17(6):718-722.

[9] 杜海龙,张荣辉,刘 平,等.捷联惯导系统姿态解算模块的实现[J].光学精密工程,2008,16(10):1956-1962.

[10] Wei Chunling,Zhang Hongyue.SIN S in-Flight Alignment Using Quaternion Error Models[J].Chihese Journal of Aeronautics.2001,14(3):166-170.

[11] 张荣辉,贾宏光,陈 涛,等.基于四元数法的捷联式惯性导航系统的姿态解算[J],光学精密工程,2008，16(10):1963-1970.

[12] 李连仲,王小虎,蔡述江.捷联惯性导航、制导系统中方向余弦矩阵的递推算法[J].宇航学报,2006,27(3):349-353.

[13] 牛小骥,旷 俭,陈起金.采用MEMS惯导的小口径管道内检测定位方案可行性研究[J].传感技术学报,2016,29(1)：40-44.

[14] 彭丁聪.卡尔曼滤波的基本原理及应用[J].软件导刊，2009,8(11):33-34.