CN104235618A - 一种基于mems惯性测量单元的管道测绘及缺陷定位装置及其管道测绘及缺陷定位方法 - Google Patents

Abstract

本发明属于管道测绘技术领域，具体涉及一种基于MEMS惯性测量单元的管道测绘及缺陷定位装置及其管道测绘及缺陷定位方法。基于MEMS惯性测量单元的管道测绘及缺陷定位装置,包括测量单元，修正单元，缺陷探测单元，供电单元，数据处理及存储单元。与之前的发明、论文等相比，MEMS惯性测量单元成本较低，除自主性外，管径适用范围更加广，最小可至60mm。MEMS惯性测量单元与里程计、磁通门磁力计、超声检测装置相组合，解决了没有铺设定点磁标的管道测绘问题，同时检测标记出缺陷位置信息，为管道缺陷的维修、加固提供了便利。里程轮同时连接发电装置，避免了外部供电所引发的问题。

Description

一种基于MEMS惯性测量单元的管道测绘及缺陷定位装置及其管道测绘及缺陷定位方法

技术领域

本发明属于管道测绘技术领域，具体涉及一种基于MEMS惯性测量单元的管道测绘及缺陷定位装置及其管道测绘及缺陷定位方法。

背景技术

根据美国中央情报局《世界各国纪实年鉴》的统计，2013年全球运行的管道有3559186公里，如果取赤道周长为40075.04公里，那么油气管道可绕地球88圈。截至2012上半年，全国油气管道总长度达9.3万公里，而2004年我国油气管道总里程还不到3万公里。预计到2015年我国油气管道总长度达15万公里左右。油气管网是能源输送的大动脉，但是随着管道运行时间的增长,油气田改造和管道人员变更的加剧,使得管道的信息大量丢失,给管道的运营管理、调度控制以及系统的改造等工作带来极大不便,同时油气管道服役时间也在增加,我国有大量管道接近设计寿命或己经超过了设计的年限,需要及时对这些管道进行检测、评价和维护,进行有效的管理,以避免由管道的破裂引起人员伤亡和财产的损失。

2013年11月22日上午9时许发生在青岛黄岛的中石化黄潍输油管的爆炸事故，爆炸现场浓烟冲天，直接经济损失7.5亿元。事故主因是输油管路与排水暗渠交汇处管道腐蚀变薄破裂，原油泄漏，流入排水暗渠，挥发的油气与暗渠中的空气混合形成易燃易爆气体，在相对封闭的空间内集聚。石油管道的安全问题应当引起我们的重视。

管道内部测绘及缺陷定位主要解决两方面问题,一是管道的具体线路，看看其附近是否有河流等对其腐蚀或其上是否有建筑物造成挤压变形来重点关注；二是检测管道内部缺陷的精确位置,也就是缺陷的精确定位"在进行管道内部缺陷检测的同时,需要精确确定其位置,为管道缺陷的维修、加固提供便利,这样不仅可以提高缺陷维修与加固的效率,大幅缩短维修加固的周期,提高管道正常运行率；同时还可以节约维修、加固工程的成本。

在搜索相关资料中，发现07年有一个《基于惯性技术的地下管线测量系统及其测算方法》的专利与本发明所述最为相似，其发明所表述的方法也是绝大多数管道测绘所用的方法，但其还有一定的缺陷，并局限于必须铺设了定点磁标的管道，而对于年限长的管道在铺设时并没有设置定点磁标，本发明主要是解决了此类管道测绘及缺陷定位的问题。

发明内容

本发明的目的在于提供一种体积小，精度高，以惯性技术为基础，多种定位方式相组合的基于MEMS惯性测量单元的管道测绘及缺陷定位装置。本发明的目的还在于提供一种基于MEMS惯性测量单元的管道测绘及缺陷定位方法。

本发明的目的是这样实现的：

基于MEMS惯性测量单元的管道测绘及缺陷定位装置,包括测量单元，修正单元，缺陷探测单元，供电单元，数据处理及存储单元：

测量单元：MEMS惯性测量单元采用ADIS16405,包含三轴加速度计、三轴陀螺，陀螺和加速度计提供管道的航向和姿态角，在载体系X-Y-Z三个轴上，沿着轴的方向放有一个加速度计与一个陀螺，分别采集三个方向的加速度和角速度信号，通过SPI通信方式传递给ARM处理器进行处理；

修正单元包括：光电编码器里程计，里程轮在装置的最外侧紧贴管壁，随着系统在管道内运动产生脉冲信号，通过UART接口与ARM处理器的定时器引脚连接，ARM接受脉冲转换成速度作为已知准确量进行速度修正；三轴磁力计，测得航向值与陀螺解算出来的航向在一定误差范围内进行修正，磁力计放在载体系X-Y-Z三个轴线上，用来测三轴方向的地磁分量，信号由SPI接口输入到ARM处理器中，ARM处理器对信号进行处理；全球卫星定位系统GPS，搭载在装置上，测得载体的实时位置，GPS输出信号通过UART接口输入到ARM中；

缺陷探测单元：超声检测装置，为相对单独的个体，搭载在装置上，发出的信号直接给ARM来进行存储；

供电单元：双向锂电池，装发电装置于里程轮上，电池正负极经相应电源模块转换后与所有单元的正负极相连接；

数据处理及存储单元：将ARM采集的信号通过SPI接口将ARM的输出信息输入到SD卡中，输出信息包括载体的航向和姿态角、位置、速度、时间及超声检测道德管壁厚度数据，根据SD卡上存储的数据绘制管道位置走向并标记出缺陷所在位置的三维图。

基于MEMS惯性测量单元的管道测绘及缺陷定位方法，包括：

(1)采集陀螺仪的角速度、加速度计的加速度；GPS的经纬度、高度信息；磁力计测量的当前载体坐标系下的三轴地磁分量以及里程计测得的脉冲转化的速度值；

(2)进行信号处理，初始对准：

(2.1)水平对准时载体静止，重力加速度在载体坐标系OX_bY_bZ_b各个轴向分量为 $f^b={\left[\begin{array}{ccc}{f}_X^b& f_Y^b& f_Z^b\end{array}\right]}^T,$ 在水平坐标系OX_hY_hZ_h各轴分量为f^h＝[0 0 g]^T，已知载体坐标系到水平坐标系的变换矩阵

$C_b^h=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]$

其中γ—横摇角，θ—纵摇角，则根据坐标变换方程

$\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=C_b^h\left[\begin{array}{c}{f}_X^b\\ f_Y^b\\ f_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{f}_X^b\\ f_Y^b\\ f_Z^b\end{array}\right];$

$\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\γ}{f}_X^b+\sin \mathrm{\γ}{f}_Z^{\text{b}}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}{f}_X^b+\cos \mathrm{\θ}{f}_Y^b-\sin \mathrm{\θ}\cos \mathrm{\γ}{f}_Z^b\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}{f}_X^b+\sin \mathrm{\θ}{f}_Y^b+\cos \mathrm{\θ}\cos \mathrm{\γ}{f}_Z^b\end{array}\right]$

$\mathrm{\γ}=\arctan (-\frac{f_X^b}{f_Z^b});$

$\mathrm{\θ}=\arctan (f_Y^{\text{b}}/\sqrt{{\left(f_X^b\right)}^2+{\left(f_Z^b\right)}^2});$

$\mathrm{\θ}=\arcsin (f_Y^{\text{b}}/g);$

获得载体的横摇角γ和纵摇角θ,在静止状态下判断加速度计的采样系统输出；

(2.2)方位对准时磁阻传感器沿载体坐标系安装，载体静止时，地磁场强度在载体坐标系OX_bY_bZ_b各轴向分量为 $H^b={\left[\begin{array}{ccc}{H}_X^b& H_Y^b& H_Z^b\end{array}\right]}^T,$ 在水平坐标系OX_hY_hZ_h各轴分量为 $H^h={\left[\begin{array}{ccc}{H}_X^h& H_Y^h& H_Z^h\end{array}\right]}^T,$ 根据坐标变换方程得到地磁场强度在水平坐标系内的值：

$\left[\begin{array}{c}{H}_X^h\\ H_Y^h\\ H_Z^h\end{array}\right]=C_b^h\left[\begin{array}{c}{H}_X^b\\ H_Y^b\\ H_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{H}_X^b\\ H_Y^b\\ H_Z^b\end{array}\right]$

当地地磁场强度的水平分量为：

$H_X^h=H_X^b\cos \mathrm{\γ}+H_Z^b\sin \mathrm{\γ}$

$H_Y^h=H_X^b\sin \mathrm{\θ}\sin \mathrm{\γ}+H_Y^b\cos \mathrm{\θ}-H_Z^b\sin \mathrm{\θ}\cos \mathrm{\γ}$

求得

由地磁场强度的水平磁力H_H总是指向磁北这一原理，便可得到磁航向ψ，即磁北到Y_h的角度，用地磁强度在水平坐标系下的值表示为：

$\mathrm{\ψ}\left(\mathrm{rad}\right)=\arctan (H_X^m/H_Y^m)=\arctan (-H_X^h/H_Y^h)$

确定出载体磁航向，通过当地的磁偏角确定载体的真实航向角

(2.3)利用GPS得到的经纬度、高度信息给出载体当前的位置信息；

(3)对准后把测绘装置放入管道，重新执行步骤(1)，采集陀螺仪、加速度计和地磁数据，利用里程计得到载体当前的线速度；对应缺陷探测单元，超声检测装置用以检测管道实时的缺陷信息；

(4)导航计算：纯惯导解算利用的是四阶龙格库塔更新方法，解算出来的航向和姿态会随着时间的推移而发散；

(5)卡尔曼滤波：卡尔曼滤波的观测量选取为里程计的速度和地磁量，观测量作为真实值用来修正当前解算出来的带有误差的状态量，状态量包括：速度、位置、四元数、陀螺仪零偏，修正后四元数通过姿态更新矩阵得到修正后的载体航向和姿态角：

(5.1)选取地理坐标系下的位置速度姿态四元数和陀螺零偏为状态量即 $\stackrel{\‾}{X}={\left[\begin{array}{ccccccccccccc}{P}_x^t& P_y^t& P_z^t& V_x^t& V_y^t& V_z^t& q_0& q_1& q_2& q_3& b_{\mathrm{\ωx}}^b& b_{\mathrm{\ωy}}^b& b_{\mathrm{\ωz}}^b\end{array}\right]}^T,$ 建立状态方程：

非线性系统的状态方程为：

$\stackrel{\·}{\stackrel{\‾}{X}}=\left[\begin{array}{c}\stackrel{\·}{{\stackrel{\‾}{P}}_{\mathrm{et}}^t}\\ \stackrel{\·}{{\stackrel{\‾}{V}}_{\mathrm{et}}^t}\\ \stackrel{\·}{\stackrel{\‾}{q}}\\ \stackrel{\·}{{\stackrel{\‾}{b}}_{\mathrm{\ω}}^b}\end{array}\right]=\left[\begin{array}{c}{\stackrel{\‾}{V}}_{\mathrm{et}}^t\\ C_b^t{\stackrel{\‾}{f}}^b+{\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^T+C_b^t{\stackrel{\‾}{W}}_a^b\\ \frac{1}{2}\mathrm{\Ω}\left(\stackrel{\‾}{q}\right)[{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ib}}^b-{\stackrel{\‾}{b}}_{\mathrm{\ω}}^b+{\stackrel{\‾}{W}}_{\mathrm{\ω}}^b]\\ {\stackrel{\‾}{W}}_b^b\end{array}\right]$

其中为载体系下加速度计比力测量值，为载体下加速度计的量测噪声，为载体系下陀螺角速度测量值，为载体系下陀螺的量测噪声，为载体系下陀螺零偏，为载体系下的陀螺零偏噪声；

(5.2)通过里程计测得的速度通过地磁传感器量测出当地磁场强度在载体系下的值得到观测量 $\stackrel{\‾}{Z}{=\left[\begin{array}{cc}{{\stackrel{\‾}{V}}_{\mathrm{et}}^t}^T& {\stackrel{\‾}{B}}^{\mathrm{bT}}\end{array}\right]}^T,$ 令观测噪声为 $\stackrel{\‾}{v}={\left[\begin{array}{cc}{{\stackrel{\‾}{v}}_V}^T& {{\stackrel{\‾}{v}}_B}^T\end{array}\right]}^T,$ 其中表示里程计速度观测误差，表示地磁传感器地磁测量误差，得到系统的量测方程为：

$\stackrel{\‾}{Z}=\stackrel{\‾}{y}+\stackrel{\‾}{v}$

$\stackrel{\‾}{y}=h\left(\stackrel{\‾}{X}\right)=\left[\begin{array}{c}{\stackrel{\‾}{V}}_{\mathrm{et}}^t\\ C_t^b{\stackrel{\‾}{B}}^t\end{array}\right]$

(6)将卡尔曼滤波修正后得到的载体姿态角、航向角、速度、位置、时间信息以及通过超声检测装置检测到的管道缺陷信息存储到SD卡；

(7)测绘仪走完全程后，从管道中拿出，关闭系统，读取其SD卡，根据所存储的航向角、姿态角和缺陷的位置、时间信息，用航迹推算原理进行管道三维信息记录并标明缺陷位置所在，绘制出一张有管道位置走向并标记出缺陷大概位置的三维图。

本发明的有益效果在于：

与之前的发明、论文等相比，MEMS惯性测量单元成本较低，除自主性外，管径适用范围更加广，最小可至60mm。MEMS惯性测量单元与里程计、磁通门磁力计、超声检测装置相组合，解决了没有铺设定点磁标的管道测绘问题，同时检测标记出缺陷位置信息，为管道缺陷的维修、加固提供了便利。里程轮同时连接发电装置，避免了外部供电所引发的问题。

附图说明

图1是本发明的工作流程图；

图2是本发明的硬件搭建结构图。

具体实施方式

下面结合附图对本发明做更详细地描述：

本发明是一种用于管道内测绘及缺陷定位的技术，具体地说是基于一个由MEMS惯性测量单元及里程计、磁力计、超声波检测装置等组成的多功能自供电地下探测装置来进行测绘和缺陷定位的方法，此发明具有自主测量，适用管径范围广，不限管道深度、精度高等优点，在当今管道检测大多数借助于定点磁标的情况下，尤其适用于早期铺设的未设定点磁标年限长的管道。

本发明结合超声检测装置同时检测出管道内部缺陷，实时存储且可在运行完全程后利用所存数据，绘制出管道三维走向坐标图并附带缺陷位置。

本发明是这样实现的：管道测绘及缺陷定位装置包括测量单元，修正单元，缺陷探测单元，供电单元，数据处理及存储单元。其中具体为：

核心处理器以ARM为例来进行各单元的连接，完成各单元的输入输出。

1、测量单元：MEMS惯性测量单元,包含三轴加速度计、三轴陀螺，陀螺和加速度计提供管道的航向和姿态角，此惯性测量单元可采用ADIS16405，其优点是体积小且自带地磁传感器，可适用于管道口径范围广，最小可达60mm。在载体系X-Y-Z三个轴上，沿着轴的方向都放有一个加速度计与一个陀螺，分别用来采集三个方向的加速度和角速度信号，此信号通过SPI通信方式传递给ARM处理器进行处理。

2、修正单元包括：(1)光电编码器里程计，里程轮在装置的最外侧紧贴管壁，随着系统在管道内运动产生脉冲信号，通过UART接口与ARM处理器的定时器引脚连接，ARM接受脉冲就可以转换成速度(具体转换方式为：码盘的遮光条码数就等于轮子转动一圈的脉冲数，用单位时间内的脉冲数除以条码数就是转动的圈数，乘以轮子的周长与时间相除就得到了速度)。此速度可以作为已知准确量进行速度修正；(2)三轴磁力计，来提供航向，测得航向值与陀螺解算出来的航向在一定误差范围内进行修正，磁力计同加速度计和陀螺一样，放在载体系X-Y-Z三个轴线上，用来测三轴方向的地磁分量，产生的信号由SPI接口输入到ARM处理器中，ARM对其信号进行处理；(3)全球卫星定位系统(GPS)，搭载在装置上，在有信号的前提下，可以测得载体的实时位置，GPS输出信号通过UART接口输入到ARM中。

以上两个单元组成了一个组合导航系统，搭载在此装置上，主要完成管道测绘的工作。

3、缺陷探测单元：超声检测装置，利用超声波投射技术，即短脉冲之间的渡越时间被转换为管壁的壁厚，当有泄漏发生时，钢管壁内的渡越时间减少为零，据此可判断泄漏的发生。其检测精度高，能提供定量、绝对数据，并且很精确。此单元为相对单独的个体，搭载在装置上，主要完成缺陷定位的工作，其发出的信号直接给ARM来进行存储。

4、供电单元：双向锂电池，由于此装置无需行进动力，所需电量并不大，装发电装置于里程轮上，在运动的同时产生电能，避免外部供电带来的麻烦，此供电方式已有车轮发电的研究成果，被证明是可实现的；或采用有足够电量的电池来供电，电池正负极经相应电源模块转换后与所有单元的正负极相连接。

5、数据存储单元：将以上所有ARM采集的信号在内部处理后，通过SPI接口将ARM的输出信息输入到SD卡中，这些输出信息包括载体的航向和姿态角、位置、速度、时间及超声检测道德管壁厚度数据，装置完成工作后可以根据SD卡上存储的这些数据绘制管道位置走向并标记出缺陷所在大概位置的三维图。

根据以上的装置具体实现方法的流程如下：

1、对应测量单元及修正单元，要采集的数有陀螺仪的角速度、加速度计的加速度；GPS的经纬度、高度信息；磁力计测量的当前载体坐标系下的三轴地磁分量以及里程计测得的脉冲转化的速度值。

2、进行信号处理，初始对准：初始对准的目的在于得到载体的初始位置和为导航解算提供载体初始的航向和姿态角。分为水平对准和方位对准：

(1)水平对准时载体静止，设重力加速度在载体坐标系OX_bY_bZ_b各个轴向分量为 $f^b={\left[\begin{array}{ccc}{f}_X^b& f_Y^b& f_Z^b\end{array}\right]}^T,$ 在水平坐标系OX_hY_hZ_h各轴分量为f^h＝[0 0 g]^T，已知载体坐标系到水平坐标系的变换矩阵

$C_b^h=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(1\right)$

其中γ—横摇角，θ—纵摇角，则根据坐标变换方程便有

$\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=C_b^h\left[\begin{array}{c}{f}_X^b\\ f_Y^b\\ f_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{f}_X^b\\ f_Y^b\\ f_Z^b\end{array}\right]---\left(2\right)$

即： $\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\γ}{f}_X^b+\sin \mathrm{\γ}{f}_Z^{\text{b}}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}{f}_X^b+\cos \mathrm{\θ}{f}_Y^b-\sin \mathrm{\θ}\cos \mathrm{\γ}{f}_Z^b\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}{f}_X^b+\sin \mathrm{\θ}{f}_Y^b+\cos \mathrm{\θ}\cos \mathrm{\γ}{f}_Z^b\end{array}\right]---\left(3\right)$

最终推得： $\mathrm{\γ}=\arctan (-\frac{f_X^b}{f_Z^b})---\left(4\right)$

$\mathrm{\θ}=\arctan (f_Y^{\text{b}}/\sqrt{{\left(f_X^b\right)}^2+{\left(f_Z^b\right)}^2})---\left(5\right)$

或 $\mathrm{\θ}=\arcsin (f_Y^{\text{b}}/g)---\left(6\right)$

通过上面的推导可知，利用(4)和(5)(6)便可获得载体的横摇角γ和纵摇角θ。而(6)可用于在静止状态下判断加速度计的采样系统输出是否有问题。纵摇角θ的计算可根据需要选择(5)或(6)中的一种方法。

(2)方位对准时磁阻传感器沿载体坐标系安装，载体静止时，设地磁场强度在载体坐标系OX_bY_bZ_b各轴向分量为 $H^b={\left[\begin{array}{ccc}{H}_X^b& H_Y^b& H_Z^b\end{array}\right]}^T,$ 在水平坐标系OX_hY_hZ_h各轴分量为 $H^h={\left[\begin{array}{ccc}{H}_X^h& H_Y^h& H_Z^h\end{array}\right]}^T,$ 根据坐标变换方程可得到地磁场强度在水平坐标系内的值：

$\left[\begin{array}{c}{H}_X^h\\ H_Y^h\\ H_Z^h\end{array}\right]=C_b^h\left[\begin{array}{c}{H}_X^b\\ H_Y^b\\ H_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{H}_X^b\\ H_Y^b\\ H_Z^b\end{array}\right]---\left(7\right)$

则当地地磁场强度的水平分量为：

$H_X^h=H_X^b\cos \mathrm{\γ}+H_Z^b\sin \mathrm{\γ}$

$\left(8\right)$

$H_Y^h=H_X^b\sin \mathrm{\θ}\sin \mathrm{\γ}+H_Y^b\cos \mathrm{\θ}-H_Z^b\sin \mathrm{\θ}\cos \mathrm{\γ}$

将已知的横摇角γ、纵摇角θ代入(8)，即可求得

由地磁场强度的水平磁力H_H总是指向磁北这一原理，便可得到磁航向ψ，即磁北到Y_h的角度，用地磁强度在水平坐标系下的值表示为：

$\mathrm{\ψ}\left(\mathrm{rad}\right)=\arctan (H_X^m/H_Y^m)=\arctan (-H_X^h/H_Y^h)---\left(9\right)$

为了提供0°～360°间的连续磁航向，将式(9)分解为下面的一组方程。

利用式(10)确定出载体磁航向，通过加上或减去当地的磁偏角便能确定载体的真实航向角

(3)利用GPS得到的经纬度、高度信息给出载体当前的位置信息。

3、对准后把测绘装置放入管道，仍然进行过程1，实时采集陀螺仪、加速度计和地磁数据，利用里程计得到载体当前的线速度；对应缺陷探测单元，超声检测装置用以检测管道实时的缺陷信息。

4、导航计算：纯惯导解算利用的是四阶龙格库塔更新方法，此方法过于普遍，在此不赘述，由于龙格库塔为数值积分过程，解算出来的航向和姿态会随着时间的推移而发散。

5、卡尔曼滤波：卡尔曼滤波的观测量选取为里程计的速度和地磁。观测量作为真实值用来修正当前解算出来的带有误差的状态量。状态量包括：速度、位置、四元数、陀螺仪零偏。修正后四元数通过姿态更新矩阵得到修正后的载体航向和姿态角。具体如下：

(1)选取地理坐标系下的位置速度姿态四元数和陀螺零偏为状态量即 $\stackrel{\→}{X}={\left[\begin{array}{ccccccccccccc}{P}_x^t& P_y^t& P_z^t& V_x^t& V_y^t& V_z^t& q_0& q_1& q_2& q_3& b_{\mathrm{\ωx}}^b& b_{\mathrm{\ωy}}^b& b_{\mathrm{\ωz}}^b\end{array}\right]}^T,$ 来建立状态方程。

非线性系统的状态方程为：

$\stackrel{\·}{\stackrel{\→}{X}}=\left[\begin{array}{c}{\stackrel{\·}{\stackrel{\→}{P}}}_{\mathrm{et}}^t\\ {\stackrel{\·}{\stackrel{\→}{V}}}_{\mathrm{et}}^t\\ \stackrel{\·}{\stackrel{\→}{q}}\\ {\stackrel{\·}{\stackrel{\→}{b}}}_{\mathrm{\ω}}^b\end{array}\right]=\left[\begin{array}{c}{\stackrel{\→}{V}}_{\mathrm{et}}^t\\ C_b^t{\stackrel{\→}{f}}^b+{\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^T+C_b^t{\stackrel{\→}{W}}_a^b\\ \frac{1}{2}\mathrm{\Ω}\left(\stackrel{\→}{q}\right)[{\stackrel{\→}{\mathrm{\ω}}}_{\mathrm{ib}}^b-{\stackrel{\→}{b}}_{\mathrm{\ω}}^b+{\stackrel{\→}{W}}_{\mathrm{\ω}}^b]\\ {\stackrel{\→}{W}}_b^b\end{array}\right]---\left(11\right)$

其中为载体系下加速度计比力测量值，为载体下加速度计的量测噪声，为载体系下陀螺角速度测量值，为载体系下陀螺的量测噪声，为载体系下陀螺零偏，为载体系下的陀螺零偏噪声，也即陀螺的随机游走噪声。

(2)通过里程计测得的速度通过地磁传感器量测出当地磁场强度在载体系下的值即得到观测量 $\stackrel{\→}{Z}={\left[\begin{array}{cc}{{\stackrel{\→}{V}}_{\mathrm{et}}^t}^T& {\stackrel{\→}{B}}^{\mathrm{bT}}\end{array}\right]}^T,$ 且令观测噪声为 $\stackrel{\→}{v}={\left[\begin{array}{cc}{{\stackrel{\→}{v}}_V}^T& {{\stackrel{\→}{v}}_B}^T\end{array}\right]}^T,$ 其中表示里程计速度观测误差，表示地磁传感器地磁测量误差，得到系统的量测方程为：

$\stackrel{\→}{Z}=\stackrel{\→}{y}+\stackrel{\→}{v}---\left(12\right)$

$\stackrel{\→}{y}=h\left(\stackrel{\→}{X}\right)=\left[\begin{array}{c}{\stackrel{\→}{V}}_{\mathrm{et}}^t\\ C_t^b{\stackrel{\→}{B}}^t\end{array}\right]---\left(13\right)$

6、将卡尔曼滤波修正后得到的载体姿态角、航向角、速度、位置、时间信息以及通过超声检测装置检测到的管道缺陷信息存储到SD卡。

7、测绘仪走完全程后，从管道中拿出，关闭系统，读取其SD卡，根据所存储的航向角、姿态角和缺陷的位置、时间等信息，用航迹推算原理进行管道三维信息记录并标明缺陷位置所在。最终绘制出一张有管道位置走向并标记出缺陷大概位置的三维图。

试验时所制作的导航电路板主要包括以下芯片：arm采用STM32系列，连接蓄电池供电接口，电压转换芯片，GPS芯片，SD卡芯片，MEMS惯性测量单元采用ADIS16405(包含三轴陀螺、三轴加速度计和三轴地磁传感器)，在系统进入管道之前进行初始对准，用加速度计测量的载体当前加速度和重力加速度的关系得到载体的横滚角和俯仰角。三轴地磁分量通过横滚角和俯仰角转化到水平方向，利用地磁在水平面总是指北这一原理测量当前载体航向值。利用GPS得到的经纬度、高度信息给出载体当前的位置信息。而后把测绘仪放入管道，测绘仪制作时要求两头密封，在石油的推力下行进，无需提供动力，MEMS惯性测量单元搭载到测绘仪上，在管道的行进过程中，其轨迹可以完全表达管道的三维信息。

在行进过程中不断采集陀螺和加速度计数据利用四阶龙格库塔更新方法进行导航解算得到其姿态角和航向信息；里程轮紧贴管壁，并有钢刷对管壁内部清理，一定程度上防止里程轮打滑，提高里程计精度，用其提供的速度信息与解算出的速度做差值对系统做卡尔曼滤波速度修正，里程轮转动的同时带动发电，将电量存储在蓄电池当中；地磁传感器测地磁，确定航向，目前有许多补偿方法对磁力计测航向的精确性进行了补偿校正，当其所测值与陀螺解算出来的数据在一定误差范围内时进行航向修正，有效的提高导航定位精度。

超声检测器在管道内运行过程中,利用超声波投射技术，即短脉冲之间的渡越时间被转换为管壁的壁厚，当有泄漏发生时，钢管壁内的渡越时间减少为零，据此可判断泄漏的发生。其检测精度高，能提供定量、绝对数据，并且很精确。

对于以上各传感器采集的载体姿态角、航向角、速度、位置、时间信息，管道缺陷信息均通过处理器存储到SD卡中，等系统运行完全程后，读取SD卡中的数据对其进行分析处理，系统在管道内的姿态、航向即是管道的走向，通过航迹推算原理，最终绘制出管道三维走向坐标图，根据其走向，能够有效识别由于环境因素等诱发的管道变形和管道位移，评估管道的曲率以及与曲率变化相关的弯曲应变。同时将测绘获得的位置参数、变形和超声波等检测数据结合起来，能够将缺陷位置、变形参数等在三维图中标注出来从而极大地方便管道维修方案的制定与开挖定位，提高维修效率，节省维修费用。

Claims (2)

1.一种基于MEMS惯性测量单元的管道测绘及缺陷定位装置,包括测量单元，修正单元，缺陷探测单元，供电单元，数据处理及存储单元,其特征在于：

测量单元：MEMS惯性测量单元采用ADIS16405,包含三轴加速度计、三轴陀螺，陀螺和加速度计提供管道的航向和姿态角，在载体系X-Y-Z三个轴上，沿着轴的方向放有一个加速度计与一个陀螺，分别采集三个方向的加速度和角速度信号，通过SPI通信方式传递给ARM处理器进行处理；

修正单元包括：光电编码器里程计，里程轮在装置的最外侧紧贴管壁，随着系统在管道内运动产生脉冲信号，通过UART接口与ARM处理器的定时器引脚连接，ARM接受脉冲转换成速度作为已知准确量进行速度修正；三轴磁力计，测得航向值与陀螺解算出来的航向在一定误差范围内进行修正，磁力计放在载体系X-Y-Z三个轴线上，用来测三轴方向的地磁分量，信号由SPI接口输入到ARM处理器中，ARM处理器对信号进行处理；全球卫星定位系统GPS，搭载在装置上，测得载体的实时位置，GPS输出信号通过UART接口输入到ARM中；

缺陷探测单元：超声检测装置，为相对单独的个体，搭载在装置上，发出的信号直接给ARM来进行存储；

供电单元：双向锂电池，装发电装置于里程轮上，电池正负极经相应电源模块转换后与所有单元的正负极相连接；

数据处理及存储单元：将ARM采集的信号通过SPI接口将ARM的输出信息输入到SD卡中，输出信息包括载体的航向和姿态角、位置、速度、时间及超声检测道德管壁厚度数据，根据SD卡上存储的数据绘制管道位置走向并标记出缺陷所在位置的三维图。

2.一种基于MEMS惯性测量单元的管道测绘及缺陷定位方法，其特征在于：

(1)采集陀螺仪的角速度、加速度计的加速度；GPS的经纬度、高度信息；磁力计测量的当前载体坐标系下的三轴地磁分量以及里程计测得的脉冲转化的速度值；

(2)进行信号处理，初始对准：

(2.1)水平对准时载体静止，重力加速度在载体坐标系OX_bY_bZ_b各个轴向分量为 $f^b={\left[\begin{array}{ccc}{f}_X^b& f_Y^b& f_Z^b\end{array}\right]}^T,$ 在水平坐标系OX_hY_hZ_h各轴分量为f^h＝[0 0 g]^T，已知载体坐标系到水平坐标系的变换矩阵

$C_b^h=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]$

其中γ—横摇角，θ—纵摇角，则根据坐标变换方程

$\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=C_b^h\left[\begin{array}{c}{f}_X^b\\ f_Y^b\\ f_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{f}_X^b\\ f_Y^b\\ f_Z^b\end{array}\right];$

$\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]=\left[\begin{array}{c}\cos \mathrm{\γ}{f}_X^b+\sin \mathrm{\γ}{f}_Z^{\text{b}}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}{f}_X^b+\cos \mathrm{\θ}{f}_Y^b-\sin \mathrm{\θ}\cos \mathrm{\γ}{f}_Z^b\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}{f}_X^b+\sin \mathrm{\θ}{f}_Y^b+\cos \mathrm{\θ}\cos \mathrm{\γ}{f}_Z^b\end{array}\right]$

$\mathrm{\γ}=\arctan (-\frac{f_X^b}{f_Z^b});$

$\mathrm{\θ}=\arctan (f_Y^{\text{b}}/\sqrt{{\left(f_X^b\right)}^2+{\left(f_Z^b\right)}^2});$

$\mathrm{\θ}=\arcsin (f_Y^{\text{b}}/g);$

获得载体的横摇角γ和纵摇角θ,在静止状态下判断加速度计的采样系统输出；

(2.2)方位对准时磁阻传感器沿载体坐标系安装，载体静止时，地磁场强度在载体坐标系OX_bY_bZ_b各轴向分量为 $H^b={\left[\begin{array}{ccc}{H}_X^b& H_Y^b& H_Z^b\end{array}\right]}^T,$ 在水平坐标系OX_hY_hZ_h各轴分量为 $H^h={\left[\begin{array}{ccc}{H}_X^h& H_Y^h& H_Z^h\end{array}\right]}^T,$ 根据坐标变换方程得到地磁场强度在水平坐标系内的值：

$\left[\begin{array}{c}{H}_X^h\\ H_Y^h\\ H_Z^h\end{array}\right]=C_b^h\left[\begin{array}{c}{H}_X^b\\ H_Y^b\\ H_Z^b\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\\ \sin \mathrm{\γ}\sin \mathrm{\θ}& \cos \mathrm{\θ}& -\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\sin \mathrm{\γ}\cos \mathrm{\θ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{H}_X^b\\ H_Y^b\\ H_Z^b\end{array}\right]$

当地地磁场强度的水平分量为：

$H_X^h=H_X^b\cos \mathrm{\γ}+H_Z^b\sin \mathrm{\γ}$

$H_Y^h=H_X^b\sin \mathrm{\θ}\sin \mathrm{\γ}+H_Y^b\cos \mathrm{\θ}-H_Z^b\sin \mathrm{\θ}\cos \mathrm{\γ}$

求得

由地磁场强度的水平磁力H_H总是指向磁北这一原理，便可得到磁航向ψ，即磁北到Y_h的角度，用地磁强度在水平坐标系下的值表示为：

$\mathrm{\ψ}\left(\mathrm{rad}\right)=\arctan (H_X^m/H_Y^m)=\arctan (-H_X^h/H_Y^h)$

确定出载体磁航向，通过当地的磁偏角确定载体的真实航向角

(2.3)利用GPS得到的经纬度、高度信息给出载体当前的位置信息；

(3)对准后把测绘装置放入管道，重新执行步骤(1)，采集陀螺仪、加速度计和地磁数据，利用里程计得到载体当前的线速度；对应缺陷探测单元，超声检测装置用以检测管道实时的缺陷信息；

(4)导航计算：纯惯导解算利用的是四阶龙格库塔更新方法，解算出来的航向和姿态会随着时间的推移而发散；

(5)卡尔曼滤波：卡尔曼滤波的观测量选取为里程计的速度和地磁量，观测量作为真实值用来修正当前解算出来的带有误差的状态量，状态量包括：速度、位置、四元数、陀螺仪零偏，修正后四元数通过姿态更新矩阵得到修正后的载体航向和姿态角：

(5.1)选取地理坐标系下的位置速度姿态四元数和陀螺零偏为状态量即 $\stackrel{\‾}{X}={\left[\begin{array}{ccccccccccccc}{P}_x^t& P_y^t& P_z^t& V_x^t& V_y^t& V_z^t& q_0& q_1& q_2& q_3& b_{\mathrm{\ωx}}^b& b_{\mathrm{\ωy}}^b& b_{\mathrm{\ωz}}^b\end{array}\right]}^T,$ 建立状态方程：

非线性系统的状态方程为：

$\stackrel{\·}{\stackrel{\‾}{X}}=\left[\begin{array}{c}\stackrel{\·}{{\stackrel{\‾}{P}}_{\mathrm{et}}^t}\\ \stackrel{\·}{{\stackrel{\‾}{V}}_{\mathrm{et}}^t}\\ \stackrel{\·}{\stackrel{\‾}{q}}\\ \stackrel{\·}{{\stackrel{\‾}{b}}_{\mathrm{\ω}}^b}\end{array}\right]=\left[\begin{array}{c}{\stackrel{\‾}{V}}_{\mathrm{et}}^t\\ C_b^t{\stackrel{\‾}{f}}^b+{\left[\begin{array}{ccc}0& 0& -g\end{array}\right]}^T+C_b^t{\stackrel{\‾}{W}}_a^b\\ \frac{1}{2}\mathrm{\Ω}\left(\stackrel{\‾}{q}\right)[{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ib}}^b-{\stackrel{\‾}{b}}_{\mathrm{\ω}}^b+{\stackrel{\‾}{W}}_{\mathrm{\ω}}^b]\\ {\stackrel{\‾}{W}}_b^b\end{array}\right]$

其中为载体系下加速度计比力测量值，为载体下加速度计的量测噪声，为载体系下陀螺角速度测量值，为载体系下陀螺的量测噪声，为载体系下陀螺零偏，为载体系下的陀螺零偏噪声；

(5.2)通过里程计测得的速度通过地磁传感器量测出当地磁场强度在载体系下的值得到观测量 $\stackrel{\‾}{Z}{=\left[\begin{array}{cc}{{\stackrel{\‾}{V}}_{\mathrm{et}}^t}^T& {\stackrel{\‾}{B}}^{\mathrm{bT}}\end{array}\right]}^T,$ 令观测噪声为 $\stackrel{\‾}{v}={\left[\begin{array}{cc}{{\stackrel{\‾}{v}}_V}^T& {{\stackrel{\‾}{v}}_B}^T\end{array}\right]}^T,$ 其中表示里程计速度观测误差，表示地磁传感器地磁测量误差，得到系统的量测方程为：

$\stackrel{\‾}{Z}=\stackrel{\‾}{y}+\stackrel{\‾}{v}$

$\stackrel{\‾}{y}=h\left(\stackrel{\‾}{X}\right)=\left[\begin{array}{c}{\stackrel{\‾}{V}}_{\mathrm{et}}^t\\ C_t^b{\stackrel{\‾}{B}}^t\end{array}\right]$

(6)将卡尔曼滤波修正后得到的载体姿态角、航向角、速度、位置、时间信息以及通过超声检测装置检测到的管道缺陷信息存储到SD卡；

(7)测绘仪走完全程后，从管道中拿出，关闭系统，读取其SD卡，根据所存储的航向角、姿态角和缺陷的位置、时间信息，用航迹推算原理进行管道三维信息记录并标明缺陷位置所在，绘制出一张有管道位置走向并标记出缺陷大概位置的三维图。