CN112628524A - 一种基于拐弯角的小径管道机器人高精度定位方法 - Google Patents

Abstract

本发明提供了一种基于拐弯角的小径管道机器人高精度定位方法。主要包括以下步骤S11、用带有惯性测量单元和若干里程仪的小车对管道进行检测；S12、分别运用惯性测量单元数据和里程仪数据实现对管道拐弯角的解算；S13、结合里程仪检测信息和管道内的非完整性约束对拐弯角检测信息进行位置、速度和姿态角误差计算；S14、根据初始位置计算三维位置信息；并借助长度已知的直管道段长度信息对惯性导航解算位置进行修正；S15、采用Kalman滤波估计方法进行误差补偿；S16、数据平滑处理；S17、确定管道缺陷位置。本发明解决了未从整体的角度考虑误差，进而影响整体定位精度的问题。

Description

一种基于拐弯角的小径管道机器人高精度定位方法

技术领域

本发明涉及管道测绘技术领域，尤其涉及管道机器人测绘定位方法。

背景技术

目前大量在役的油气、水及化学物质运输管道，因腐蚀、地壳运动、人为破坏等导致的泄漏事故及安全问题引起了广泛的社会关注。近年来国内外公开报道的海底管道泄漏事故上百起，陆地管道泄漏事故更是频繁发生，每年可达百余起。这些泄漏不仅会影响正常的油气田生产，造成重大经济损失；还会引起环境污染和生态灾难；甚至爆炸还会威胁人类生命安全。更为严峻的是，国内外对大量分布于城市地下的小径管道的检测及定位技术还存在很多难题。小径管道的检测对现有检测设备体积要求更严苛，密集的城市地下管网对精确定位要求更高，这使得现有设备在进行城市地下管道检测时困难重重。现实情况是，国外对我国在相关关键技术实行长期封锁，相关设备一直处于禁运状态，只提供价格高昂的检测服务，这使得我国油气管道缺陷定位技术的研究特别是工程应用异常艰难。因此，自主掌握管道机器人测绘关键技术是我国能源大战略的重中之重，研究小径检测管道机器人也是大势所趋，机器人早日投入实际生产应用能有效破解国内外城市地下小径管道测绘时遇到的种种难题。

管道内检测机器人高精度定位技术是实现管道缺陷维护及管道完整性管理的核心技术之一，目前国内外通常采用基于捷联惯性导航系统/里程仪/地表(水下)磁标记点定位方案的管道内检测机器人。但是，采用这种方案的管道检测机器人在进行检测前必须人工铺设多个地表(水下)磁标记点，既延长了检测服务时间，又额外增加了巨额的检测成本。现有技术中通常采用多传感器信息融合技术对管道内诸如裂缝、腐蚀、凹陷等缺陷进行检测，这种检测方式对传感器的要求很高，而且在小径管道内由于尺寸受限，电源模块和传感器的选型也因此受到限制；此外，采用低成本的惯性测量单元进行误差修正的方法并未对管道检测机器人的拐弯角误差进行修正，这也使得低成本的惯性测量单元在小径管道的检测定位中无法保证检测定位精度，而且由于目前几乎全部的城市地下小径管道都是钢铁材质的，采用磁传感器测量管道方向角的方法会因为管道内产生的磁场而对检测精度产生很大的影响。此外，对非钢铁材质的管道采用磁传感器测量管道方向角也会受到管道复杂程度的影响而使检测精度大大降低。

公开号为CN107218942A的中国发明专利提出了一种小径管道缺陷定位装置及基于快速正交搜索算法的定位方法，采用捷联惯性导航算法计算管道测量装置运行的轨迹坐标信息。里程仪测量轴向速度，在管道内的非完整性约束为横向和纵向提供速度。跟踪模块能记录被检测管道沿线坐标位置已知的地表磁标记，提供离散位置。基于快速正交搜索算法的管道连接器检测结果为管道测量装置在直管道内提供方位角和俯仰角误差修正。Kalman滤波估计及数据离线平滑处理从正反两个方向利用这些测量信息并修正惯性导航系统的误差，实现小径管道轨迹和方向的精确测量。这种方式须人工铺设多个水下磁标记点。检测时间较长并且增加了额外成本。

公布号为CN111536969A的中国发明专利提出了一种管道拐弯角检测方法，以四轮线缆驱动式小径管道机器人为运动检测平台，微惯性测量单元与里程仪组合的方式实现城市地下小径管道机器人的精确定位。霍尔式里程仪安装在管道机器人后轮上，实现管道机器人运行速度实时测量。结合管道机器人在被检测管道初始段的直线加速运动，可计算出管道机器人的初始姿态角信息，然后结合初始速度和位置信息可实现管道机器人定位系统初始自对准。摆脱了对磁标记的依赖，但是未能对管道拐弯角部分做充分的定位和误差修正考虑。

公布号为CN111220113A的中国发明专利提出了一种管道拐弯角检测方法，通过小径管道检测机器人在管道中的运动，利用三轴加速度计、三轴陀螺仪和多里程仪分别采集小径管道检测机器人的三轴加速度、三轴角速率和轴向速度。并在离线条件下结合三轴加速计、三轴陀螺仪和多里程仪的输出信息，在小径管道机器人检测前的初始姿态、速度和位置已知的条件下，采用接连惯性导航算法计算出小径管道机器人在管道内运动的姿态、速度和位置信息；同时，根据多里程仪测量信息分别对管道机器人在管道内运动时的速度信息进行修正，进而提高定位精度。

后一方案虽然无需引入高精度方位角参考设备，完成了弯管道部分的测量和修正，但是未从整体考虑误差，进而影响了整体的测量精度。

发明内容

针对现有技术中所存在的不足，本发明提供了一种基于拐弯角的小径管道机器人高精度定位方法，其解决了现有技术中存在的未从整体考虑误差，进而影响了整体的定位精度的问题。

根据本发明的实施例，一种基于拐弯角的小径管道机器人高精度定位方法，其包括以下步骤：

S11、用带有惯性测量单元和若干里程仪的小车对管道进行检测；

S12、分别运用惯性测量单元数据和里程仪数据实现对管道拐弯角的解算；

采用捷联惯性导航算法计算出小径管道检测机器人在管道内运动的姿态、速度和位置信息；

S13、结合里程仪检测信息和非完整性约束对拐弯角检测信息进行位置、速度和姿态角误差计算；

S14、根据初始位置计算三维位置信息；并借助长度已知的直管道段长度信息对位置信息进行修正；

S15、采用Kalman滤波估计方法进行误差补偿；

S16、数据平滑处理；

S17、确定管道缺陷位置。

本发明的技术原理为：本发明通过里程仪获取过弯时小径管道检测机器人的拐弯角；在利用惯性测量单元和捷联惯导算法计算出检测机器人在管道内运动的姿态、速度和位置信息；小径管道检测机器人安装的里程仪及其在已知的直管道段运动的非完整性约束对惯性测量单元进行实时连续的速度修正；再利用小径管道检测机器人在直管道段具有方位角和俯仰角不变的特性，用来修正惯性测量单元的方位角发散误差。最后做数据处理进而得出更加精确的定位。

相比于现有技术，本发明具有如下有益效果：

1、本发明惯性单元采用里程仪和已知的管道长度信息对误差进行修正，避免了累积误差，能够适用于长距离的管道检测工作。

2、本发明中管道拐弯角检测方法是对已有里程仪信息的再次运用，无需任何硬件成本。此外，管道拐弯角的检测是在对整个管道检测完成后进行的，无需实时进行，因此对管道检测数据的分析、计算管道地理坐标和管道测绘工作都是离线进行的，所以不会对现有的管道段检测和评估产生影响。

3、本发明的管道拐弯角检测结果可为管道机器人方向角提供误差修正信息，惯性测量单元在直管道段能够进行方位角和俯仰角误差修正，保证小径管道检测机器人在小径管道中进行检测时的测量精度。

4、本发明既无需地表磁标记，也不依赖GPS和管道长度以外的其他外部信息，适用于无GPS信号的城市地下管网系统的检测，而且还在不影响检测精度的前提下降低成本。

附图说明

图1为本发明实施例的小车结构示意图。

图2为本发明实施例利用里程仪得出拐弯角的流程图。

图3为本发明实施例利用惯性测量单元得出拐弯角的流程图。

图4为本发明实施例基于拐弯角进行定位的流程图。

上述附图中：1、电源模块；2、支撑轮；3、密封圈；4、惯性测量单元；5、数据处理单元；6、数据存储单元；7、里程仪；8、图像采集模块。

具体实施方式

下面结合附图及实施例对本发明中的技术方案进一步说明。

如图1所示，为保证本发明的可实施性，本发明采用的小径管道机器人，其包括电源模块1、支撑轮2、密封圈3、惯性测量单元4、数据存储单元6、图像采集模块8和至少三个轮子上设有里程仪7。电源模块1、惯性测量单元4、数据处理单元5、数据存储单元6依次固定安装在小径管道检测机器人主体内，由外侧前后对称的两个密封圈3固定为一个整体。所述电源模块1为多里程仪小径管道检测机器人提供电能，保证其能正常工作遍历全部待测管道。所述惯性测量单元4用于测量小径管道检测机器人在管道内运用的三轴正交角速率和三轴正交线加速度，用于捷联惯性导航算法解算。所述数据处理单元5和数据存储单元6分别用于里程仪7和惯性测量单元4数据并进行简单的处理和有效的存储。由于实际的管道测绘工程中的城市地下管网的管道长度通常可达到几十甚至几百公里，在这种条件下如果采用串行的通信方式会因为距离的限制而导致很难完成数据的传输工作，同时由于城市地下管网恶劣的环境，也无法采用无线的信号收发装置。因此本发明的实施方式中考虑使用大容量的存储器完成实施测量数据的存储工作，采集的数据包括惯性测量单元数据、里程仪数据信息。

所述密封圈3位于所述电源模块1、惯性测量单元4、数据处理单元5、数据存储单元6的前后两侧，用于保证主体内的电源模块1、惯性测量单元4、数据处理单元5、数据存储单元6等电子元器件的正常工作，防止管内物质进入到小径管道检测机器人的主体内腔中。所述里程仪7分别位于四个支撑轮2上，用于测量小径管道检测机器人在管内运动的轴向速度和距离。同时由于小径管道检测机器人在管内运动时与轴向垂直平面内运动速度为零，结合姿态角(横滚角、纵摇角和方位角)信息可计算出小径管道检测机器人在导航坐标系下的三维速度值，通过三维速度值再进行积分可以得到三维位置的变化值，从而得到小径管道检测机器人的三维坐标信息。

管道机器人采用缆绳拖曳的方式前进移动，在不考虑小径管道检测机器人发生侧滑的前提下，我们可以通过控制缆绳拖曳速度进而控制小径管道检测机器人在管内的运动。由于管内情况的复杂性以及考虑到可能发生的管内存在杂物等突发情况，会导致测量的加速度值发生突变，因此对小径管道检测机器人的行进加速度也要加以控制。管内运动姿态测量系统测得的数据包括姿态角和各轴角速度，这些数据是做状态估计所必需的参数，通过对小径管道检测机器人行进过程中陀螺仪和加速度计输出的数据进行采集和检测，再利用捷联惯性导航算法解算得到对应的姿态角和航向信息，同时通过控制支撑轮的轮速用以保证小径管道检测机器人的横滚角不小于三十度。

上述方案中仅仅为实施本发明的方法的实施提供一种可行的装置体现本发明方法的可实施性，本领域技术人员在本发明所提出的方法的思路下，也可以采用其他常规方式测量相关数据，上述装置并不对本发明所提出的方法构成具体限制。

本发明实施例提出了一种基于拐弯角的小径管道机器人高精度定位方法，

S11、用带有惯性测量单元和若干里程仪的小车对管道进行检测；

S12、分别运用惯性测量单元数据和里程仪数据实现对管道拐弯角的解算；

采用捷联惯性导航算法计算出小径管道检测机器人在管道内运动的姿态、速度和位置信息；

S13、结合里程仪检测信息和非完整性约束对拐弯角检测信息进行位置、速度和姿态角误差计算；

S14、根据初始位置计算三维位置信息；并借助长度已知的直管道段长度信息对位置信息进行修正；

S15、采用Kalman滤波估计方法进行误差补偿；

S16、数据平滑处理；

S17、确定管道缺陷位置。

利用惯性测量单元和里程仪检测过程主要包括：对小径管道机器人进行初始化设置；使小径机器人遍历整个管道，获得惯性测量单元的三轴角速率、三轴加速度信息和各个里程仪的行程信息以及时间信息。

对里程仪数据解算则是对各个里程仪过弯时的速度或者行进距离差异，计算拐弯角。

对惯性测量单元检测管道数据解算则主要包括：利用惯性测量单元的输出信息完成小径管道检测机器人在通过弯管时的速度和位置计算；通过计算得到的弯管处的姿态、速度和位置信息以及陀螺仪输出的旋转角速率方向判断出小径管道检测机器人的转弯方向；结合加速度计积分得到的行进距离信息计算小径管道检测机器人的拐弯角。

这个过程中，通过陀螺仪得到的旋转角速率测量值判断小径管道检测机器人是否通过弯管；管道本身的内部形状模型给予了管道机器人三维路径上的限制，构成非完整性约束，可用于各里程仪的误差判断；管道本身的长度和编号是可以根据安装数据查到。

结合里程仪速度测量信息及非完整性约束、管道段长度及编号查阅信息，管道拐弯角检测信息进行位置、速度和姿态角误差计算。由于惯性测量单元本身的测量误差，会导致惯性测量单元输出误差随着管道检测距离的增长而增大。通过小径管道检测机器人安装的里程仪及其在管道内运动的非完整性约束对惯性测量单元进行实时连续的速度修正；在Kalman滤波估计的作用下能够估计出惯性测量单元的误差，这些误差可以通过反馈校正提高小径管道检测机器人的检测定位精度。最后利用小径管道检测机器人在直管道段具有方位角和俯仰角不变的特性，用来修正惯性测量单元的方位角发散误差。

其中，弯管道段方向角、俯仰角和速度误差计算方式为：根据管道拐弯角检测结果，将由里程仪拐弯角检测在每个弯管段计算出来的方向角A_PB和俯仰角P_PB与对应弯管道段由捷联惯性导航算法计算出的方向角A_SINS和俯仰角P_SINS分别作差，可得出整个弯管道段惯性辅助管道检测定位系统的方向角和俯仰角误差；具体公式如下：

其中，δx表示系统状态变量，由三维位置误差

三维速度误差

三维姿态角误差

及3个陀螺仪漂移误差δωⁿ和3个加速度计零偏误差δfⁿ组成。[δη_P δη_A]^T分别表示俯仰角和方位角量测噪声，则系统观测矩阵H₁表示为：

由捷联惯性导航算法计算出来的三维速度V_e,SINS，V_n,SINS和V_u,SINS与管道测量装置尾部里程仪测量的轴向速度v_odo，以及管道测量装置在管道内横向和纵向的非完整性约束提供的零速经过坐标变换再作差可得出速度误差，为三维速度误差修正做准备；具体计算公式如下：

[δη_v,e δη_v,e δη_v,e]^T表示量测噪声，

系统从载体坐标b系到导航坐标n姿态变换矩阵，则系统观测矩阵H₂表示为：

H₂＝[0_3×3 I_3×3 0_3×3 0_3×3]

直管道段三维位置误差修正；由捷联惯性导航算法在初始位置已知的情况下，经过对加速度信息二次积分和坐标变换后，计算得到三维位置信息rⁿ，然而，基于加速度二次积分的位置算法其误差是随时间累积的，因此需要定期对位置误差进行修正，这里借助单管道段长度已知计算出的管道长度定期对惯性导航解算的位置误差做修正，从而使最大位置误差限制在单管道段长度的范围内，解决了位置误差随时间累积的问题；单管道段长度已知的管道长度转换到导航坐标系：

其中，

为测量噪声；

表示第j段管道段长度l_j在导航坐标系n各轴分量，Z为观测量；。

由惯性导航系统解算得出的管道段长度为：

则位置误差表示为：

其中，H₃＝[I_3×3 0_3×3 0_3×3 0_3×3]。

得出背检测管道在弯管段的俯仰角、方向角以及三位连续速度误差和直管道段的三位离散误差后，采用Kalman滤波估计方法，估计并补偿惯性测量单元误差和捷联惯性导航算法误差。同时，存储相应的系统预测状态和更新状态变量，以及对应的预测和更新协方差阵，用于数据平滑处理。

数据平滑处理过程为；以状态变量和协方差阵为基础，通过反向的方式采用数据平滑处理技术再次对惯性测量单元误差和捷联惯性导航算法误差进行离线估计和补偿，再次提高整个惯性辅助管道检测定位系统的定位和定向精度，以期达到小径管道检测定位系统的定位和定向精度需求。

确定管道缺陷位置，完成定位工作：将数据平滑处理后的被检测管道坐标位置与由管道检测传感器检测并分析出的管道缺陷分析结果进行时间同步操作，得出管道缺陷与管道坐标位置的关系。最后根据检测结构和定位信息，给出相应的管道检测维修报告即可。

如上述方案所述，本发明在确定拐弯角的基础上，多次利用里程仪和惯性测量单元数据，结合捷联惯性导航算法、Kalman估计方法和平滑处理，多次计算并对误差进行多次修正，并利用已知的管道长度将定位的最终误差控制在一个已知管道长度内。不在借助其他外部信息便可实现机器人的高精度定位。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (9)

1.一种基于拐弯角的小径管道机器人高精度定位方法，其特征在于，包括如下步骤：

S11、用带有惯性测量单元和若干里程仪的小车对管道进行检测；

S12、分别运用惯性测量单元数据和里程仪数据实现对管道拐弯角的解算；

采用捷联惯性导航算法计算出小径管道检测机器人在管道内运动的姿态、速度和位置信息；

S13、结合里程仪检测信息和管道非完整性约束对拐弯角检测信息进行位置、速度和姿态角误差计算；

S14、根据初始位置计算管道三维位置信息；并借助长度已知的直管道段长度信息对位置信息进行修正；

S15、采用Kalman滤波估计方法进行误差补偿；

S16、数据平滑处理；

S17、确定管道缺陷位置。

2.如权利要求1所述一种基于拐弯角的小径管道机器人高精度定位方法，其特征在于，步骤S13中所述误差计算的方式包括：根据管道拐弯角检测结果，将由里程仪拐弯角检测在每个弯管段解算出来的方向角A_PB和俯仰角P_PB与对应弯管道段由捷联惯性导航算法计算出的方向角A_SINS和俯仰角P_SINS分别作差，得出整个弯管道段定位的方向角和俯仰角误差；

由捷联惯性导航算法计算出来的三维速度V_e,SINS，V_n,SINS和V_u,SINS与里程仪测量的轴向速度v_odo，以及管道测量装置在管道内横向和纵向的非完整性约束提供的零速经过坐标变换再作差可得出速度误差。

3.如权利要求2所述一种基于拐弯角的小径管道机器人高精度定位方法，其特征在于：步骤S13中，方向角和俯仰角误差计算的具体公式为:

其中，δx表示系统状态变量，由三维位置误差

三维速度误差

三维姿态角误差

及3个陀螺仪漂移误差δωⁿ和3个加速度计零偏误差δfⁿ组成；[δη_P δη_A]^T分别表示俯仰角和方位角量测噪声；系统观测矩阵表示为：

捷联惯性导航算法计算出来的三维速度V_e,SINS，V_n,SINS和V_u,SINS与测量的轴向速度v_odo，以及管道测量装置在管道内横向和纵向的非完整性约束提供的零速经过坐标变换再作差可得出速度误差；具体计算公式为

[δη_v,e δη_v,e δη_v,e]^T表示量测噪声，

系统从载体坐标b系到导航坐标n姿态变换矩阵，则系统观测矩阵H₂表示为：

H₂＝[0_3×3 I_3×3 0_3×3 0_3×3]。

4.如权利要求3所述一种基于拐弯角的小径管道机器人高精度定位方法，其特征在于，步骤S14中的修正方式为：已知的直管道段的长度转换到导航坐标系，具体公式为：

其中，

为测量噪声；

表示第j段管道段长度l_j在导航坐标系n各轴分量，Z为观测量；由捷联惯性导航算法解算得出的管道段长度为：

得出位置误差：

系统观测矩阵为：

H₃＝[I_3×3 0_3×3 0_3×3 0_3×3]。

5.如权利要求4所述一种基于拐弯角的小径管道机器人高精度定位方法，其特征在于，步骤15中误差补偿的具体方式为：将得出的被检测管道在弯管道段的俯仰角、方向角误差及三维连续速度误差和直管道段的三维离散位置误差为观测量，采用Kalman滤波估计方法估计并补偿惯性测量单元误差和捷联惯性导航算法误差。

6.如权利要求5所述一种基于拐弯角的小径管道机器人高精度定位方法，其特征在于，以状态变量和协方差阵为基础，通过反向的方式采用数据平滑处理技术再次对惯性测量单元误差和捷联惯性导航算法误差进行离线估计和补偿。

7.如权利要求6所述一种基于拐弯角的小径管道机器人高精度定位方法，其特征在于，将数据平滑处理后的被检测管道坐标位置与由管道检测传感器检测并分析出的管道缺陷分析结果进行时间同步操作，得出管道缺陷与管道坐标位置的关系。

8.如权利要求7所述一种基于拐弯角的小径管道机器人高精度定位方法，其特征在于，通过惯性测量单元中陀螺仪得到的旋转角速率测量值判断小径管道检测机器人是否通过弯管。

9.如权利要求1-8任一项所述一种基于拐弯角的小径管道机器人高精度定位方法，其特征在于，将缺陷获取时间与缺陷定位时间做时间同步。

Images