CN111536969B - 一种基于初始姿态角自对准的小径管道机器人定位方法 - Google Patents

Abstract

本发明提供一种基于初始姿态角自对准的小径管道机器人定位方法，属于管道测绘技术领域。以四轮线缆驱动式小径管道机器人为运动检测平台，微惯性传感器与里程仪组合的方式实现城市地下小径管道机器人的精确定位。霍尔式里程仪安装在管道机器人后轮上，实现管道机器人运行速度实时测量。结合管道机器人在被检测管道初始段的直线加速运动，可计算出管道机器人的初始姿态角信息，然后结合初始速度和位置信息可实现管道机器人定位系统初始自对准。本发明的小径管道机器人在进行城市地下等复杂场合管道检测时，无需引入高精度方位角参考设备，成本低、使用方便。

Description

一种基于初始姿态角自对准的小径管道机器人定位方法

技术领域

本发明涉及一种基于初始姿态角自对准的小径管道机器人定位方法，属于管道测绘技术领域。

背景技术

目前，城市因油气管道口径及分布复杂等因素只能进行外检测，近30％管道甚至无法检测。然而，城市地下小径管道在我们日渐发展成熟的城市中扮演着越来越重要的角色，管道的泄漏不仅会造成巨大的经济损失，还会污染周边环境，引发生态灾难，甚至其爆炸会严重威胁人民生命财产安全。因此，定期采用管道机器人对城市地下管道进行缺陷检测，能够有效预防管道泄漏等事故发生。

管道机器人微惯性定位系统是管道机器人的重要组成部分，在管道机器人进入被检测管道时，精确的初始姿态(俯仰角、横滚角及方位角)信息是保证管道机器人微惯性定位系统精确定位的重要前提。管道机器人微惯性定位系统初始自对准技术是在无任何外界参考输入的前提下，结合管道机器人在直管道内的运动特性，以水平加速度计测量值计算管道机器人微惯性定位系统初始俯仰角和横滚角信息，以管道机器人轴向速度测量信息为观测值，推导方位角测量值。

对于管道的缺陷检测首先是要准确地判断管道机器人所在位置，对机器人进行精确定位。本发明采用微机电系统(Micro-Electro-Mechanical Systems，MEMS)惯性测量器件进行管道机器人的数据采集，以MEMS惯性测量器件为核心搭建城市地下小径管道定位系统。但是，由于MEMS惯性器件的陀螺噪声大于地球自转角速度，因此无法使用陀螺数据直接计算得到的初始方位角。然而在初始阶段，可采用里程仪作为辅助手段结合微惯性测量系统进行小径管道内初始方位角的解算，里程仪可以测量管道机器人在管道内的测量行程，小巧、便于安装且成本低，除此之外，里程仪还可以在以后的检测中提供速度信息，使得测量结果更加精确。

目前在核心期刊与专利查询中均未发现与此发明类似的方法介绍。

发明内容

本发明的目的是为了为了解决上述问题而提供一种基于初始姿态角自对准的小径管道机器人定位方法。

本发明的目的是这样实现的：步骤如下：

步骤一，数据采集与分析处理：

对管道机器人进行数据采集，由惯性测量单元以及里程仪采集得到陀螺仪、加速度计以及里程仪的原始数据，采用复连续小波变换检测管道连接器；

步骤二，初始姿态角自对准：

在初始运行时，管道机器人在水平方向上做短时直线加速度机动，通过里程仪检测得到两个不同时刻管道机器人的速度，得到管道机器人导航加速度平均变化量，将求得的平均加速度与步骤一惯性测量器件中加速度计测量的比力加速度以及方向余弦矩阵结合得到管道机器人纵轴相对于地理北向的方位角，实现初始姿态角自对准；

步骤三，捷联惯性导航系统解算：

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

步骤四，误差计算：

采用已知管道长度段信息计算管道机器人定位系统在直管道段位置误差，以里程仪速度信息计算管道机器人定位系统速度误差，以管道连接器检测和管道非完整性约束可计算管道机器人定位系统姿态角误差；

步骤五，误差补偿及数据平滑处理：

根据步骤四计算出的误差，对导航定位系统采集的导航信息由于噪声产生的误差进行正向和反向修正，得出定位结果；

步骤六，确定管道缺陷位置，完成定位工作。

本发明还包括这样一些结构特征：

1.所述步骤二具体包括：

(1)初始时刻，小径管道机器人在水平方向上做短时直线加速度机动，同时保存微惯性传感器及里程仪测量数据；

(2)通过里程仪检测得到两个不同时刻管道机器人的速度V_mile^b(t_(k-1))与V_mile^b(t_(k))得到管道机器人行进速度平均变化量，即管道机器人在时间段[t_(k-1)，t_(k)]内的水平加速度；同时，根据水平加速度测量信息求得水平姿态矩阵；

(3)结合惯性测量器件加速度计测得的比力加速度与已知的重力加速度结合列方程求管道机器人加速度；

(4)根据方位相关矩阵求得管道机器人纵轴相对于地理北向的方位角。

2.步骤六具体为：将数据平滑处理后的被检测管道坐标位置与由管道检测传感器检测并分析出的管道缺陷分析结果进行时间同步操作，得出管道缺陷与管道坐标位置的关系，指导管道缺陷维护。

与现有技术相比，本发明的有益效果是：第一，本发明将里程仪与MEMS微惯性测量系统组合进行方位角测量，无需借助高精度姿态测量系统，可完全自主进行小径管道机器人在管道内运动时的姿态、速度以及位置信息的解算。第二，由于城市的日益发展，国内外目前对于管道机器人研究十分重视，特别是针对城市地下小口径管道内缺陷部位的检测，而对于管道机器人在小径管道内的定位是首先需要解决的关键问题，本发明着重于小口径城市地下管道定位，有着重要的意义。第三，整个小径管道机器人开发成本低，使用时无需连接外部设备，使用方便。

附图说明

图1是一种基于初始姿态角自对准的小径管道机器人定位方法的整体流程图。

图2是小径管道机器人定位系统初始姿态角自对准流程图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

结合图1和图2，本发明的目的就是针对上述使用MEMS惯性测量器件进行城市地下管道内定位技术存在的问题，设计提供一种里程仪/MEMS微惯性检测系统组合方式进行管道内方位角测量，达到该组合检测系统在无外部设备辅助情况下进行管道机器人全姿态的测量。

所述一种基于里程仪/MEMS微惯性检测系统的方位角测量方法在管道内主要装置包括MEMS惯性测量单元(Inertial Measurement Unit，IMU)、电源模块、管道机器人搭载平台、里程仪、数据存储单元以及数据处理单元。

MEMS IMU、电源模块、数据存储单元以及数据处理单元均安装于管道机器人搭载平台上，采用STM32F4作为主控芯片进行控制，将里程仪安装于轮子旁侧与微惯性测量平台运行无障碍连接，用于里程仪检测的磁铁块嵌入到轮子内部以便于检测，磁铁块需用胶粘住以防机器人在运动过程中磁铁块被甩出。

一种基于里程仪与微惯性组合的方位角测量方法在管道内主要装置包括MEMS惯性测量单元、电源模块、管道机器人搭载平台、里程仪、数据存储单元以及数据处理单元。以STM32处理器搭载微惯性测量平台，将微惯性测量平台固定于管道机器人上，将里程仪安装于轮子旁侧与微惯性测量平台运行无障碍连接，用于里程仪检测的磁铁块嵌入到轮子内部以便于检测。

如图1所示，是基于初始姿态角自对准的小径管道机器人定位方法的整体流程图，包括以下步骤：

步骤一，数据采集与分析处理。首先对系统进行上电，检查一切正常等待几十秒使系统进入稳定状态，开始对管道机器人进行数据采集，由惯性测量单元以及里程仪采集得到陀螺仪、加速度计以及里程仪的原始数据。同时，采用复连续小波变换检测管道连接器。

步骤二，初始姿态角自对准。在初始运行时，使管道机器人在水平方向上做短时直线加速度机动，通过里程仪检测得到两个不同时刻管道机器人的速度，可近似求得管道机器人导航加速度平均变化量，将求得的平均加速度与步骤一惯性测量器件中加速度计测量的比力加速度以及方向余弦矩阵结合列方程，容易求得管道机器人纵轴相对于地理北向的方位角，实现初始姿态角自对准。

步骤三，捷联惯性导航系统解算。以步骤一采集的数据和步骤二得到的初始姿态角为基础，采用捷联惯性导航解算算法，计算出管道机器人在管道内运动的姿态角、速度和位置信息。

步骤四，误差计算。采用已知管道长度段信息可计算管道机器人定位系统在直管道段位置误差，以里程仪速度信息计算管道机器人定位系统速度误差，以管道连接器检测和管道非完整性约束可计算管道机器人定位系统姿态角误差。

步骤五，误差补偿及数据平滑处理。根据步骤四计算出的误差，以Kalman滤波为理论及数据平滑处理理论为基础，对导航定位系统采集的导航信息由于噪声产生的误差进行正向和反向修正，得出更加精确地、可信度高的定位结果。

步骤六，确定管道缺陷位置，完成定位工作。将数据平滑处理后的被检测管道坐标位置与由管道检测传感器检测并分析出的管道缺陷分析结果进行时间同步操作，得出管道缺陷与管道坐标位置的关系，指导管道缺陷维护。

所述里程计辅助小径管道机器人解算方位角方法包括以下步骤：

步骤一，初始时刻，使小径管道机器人在水平方向上做短时直线加速度机动，同时保存微惯性传感器及里程仪测量数据。

步骤二，通过里程仪检测得到两个不同时刻管道机器人的速度V_mile^b(t_(k-1))与V_mile^b(t_(k))可近似求得管道机器人行进速度平均变化量，即管道机器人在时间段[t_(k-1)，t_(k)]内的水平加速度；同时，根据水平加速度测量信息求得水平姿态矩阵；

步骤三，根据步骤二，并结合惯性测量器件加速度计测得的比力加速度与已知的重力加速度结合列方程求管道机器人加速度。

步骤四，两种方法求得的加速度近似相等，根据方位相关矩阵求得管道机器人纵轴相对于地理北向的方位角。

如图2所示，采用里程仪速度信息进行小径管道机器人定位系统方位角解算流程图，具体步骤如下：

步骤一，对系统上电，等待几十秒使系统进入稳定状态，使用微惯性测量单元进行数据采集并存储于存储卡中。进入步骤二；

步骤二，使用捷联惯性导航算法将惯性测量单元采集三轴陀螺仪与三轴加速度计的原始数据进行解算，得到初始水平姿态阵

进入步骤三；

步骤三，结合里程仪采集得到的速度信息、三轴加速度计测得的比力加速度以及重力加速度列方程，解算管道机器人方位角。

为解算实际方位角，可对比例方程作如下近似：

记上式为公式1，其中，h系为当地水平坐标系，

为管道机器人在导航坐标系中的加速度，f^b为载体坐标系下的比力投影，f^h为水平坐标系下的比力投影，gⁿ为导航坐标系下的重力加速度投影，

为载体坐标系相对于导航坐标系的姿态转换矩阵，

为载体坐标系相对于水平坐标系的姿态阵，

为水平坐标系相对于导航坐标系的姿态阵。

其中，

则公式(1)等于

在初始运行时，使管道机器人在水平方向上做短时直线加速度机动，通过里程仪检测得到两个不同时刻管道机器人的速度

与

近似求得管道机器人导航加速度在载体坐标系内的平均变化量为

近似计算为：

将上述公式转换至导航坐标系中可得：

式中：

为里程仪测得管道机器人在导航坐标系中的近似水平加速度，

为在导航坐标系下里程仪检测得到两个不同时刻的速度。

假设短时间内[t_k-1，t_k]内载体方位近似保持为常值，

亦为常值，将公式1展开，仅取水平分量可得：

其中，记：

和

由公式(5)可解得

由上述公式分母表达式

显示，较大的水平加速度有利于可靠地求得

和

至此，实现了通过在水平方向上做短时直线加速机动来实现管道机器人纵轴相对于地理北向的方位角。进入步骤四；

步骤四，求得

之后，带入

即可求得完整的姿态阵初始化。结合以上所求，就可以得到管道机器人的全姿态、速度以及位置信息。

综上，一种基于初始姿态角自对准的小径管道机器人定位方法，属于管道测绘技术领域。以四轮线缆驱动式小径管道机器人为运动检测平台，微惯性传感器与里程仪组合的方式实现城市地下小径管道机器人的精确定位。其中，霍尔式里程仪安装在管道机器人后轮上，实现管道机器人运行速度实时测量。同时，结合管道机器人在被检测管道初始段的直线加速运动，可计算出管道机器人的初始姿态角(方位角、俯仰角和横滚角)信息，然后结合初始速度和位置信息可实现管道机器人定位系统初始自对准。而且，该小径管道机器人定位系统以管道非完整性约束、管道段长度及管道连接器检测为基础采用Kalman滤波估计和数据平滑处理方法提高系统定位精度。此外，本发明的小径管道机器人在进行城市地下等复杂场合管道检测时，无需引入高精度方位角参考设备，成本低、使用方便。

Claims (3)

1.一种基于初始姿态角自对准的小径管道机器人定位方法，其特征在于：步骤如下：

步骤一，数据采集与分析处理：

对管道机器人进行数据采集，由惯性测量单元以及里程仪采集得到陀螺仪、加速度计以及里程仪的原始数据，采用复连续小波变换检测管道连接器；

步骤二，初始姿态角自对准：

在初始运行时，管道机器人在水平方向上做短时直线加速度机动，通过里程仪检测得到两个不同时刻管道机器人的速度，得到管道机器人导航速度平均变化量，将求得的平均加速度与步骤一惯性测量器件中加速度计测量的比力加速度以及方向余弦矩阵结合得到管道机器人纵轴相对于地理北向的方位角，实现初始姿态角自对准；

步骤三，捷联惯性导航系统解算：

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

步骤四，误差计算：

采用已知管道长度段信息计算管道机器人定位系统在直管道段位置误差，以里程仪速度信息计算管道机器人定位系统速度误差，以管道连接器检测和管道非完整性约束可计算管道机器人定位系统姿态角误差；

步骤五，误差补偿及数据平滑处理：

根据步骤四计算出的误差，对导航定位系统采集的导航信息由于噪声产生的误差进行正向和反向修正，得出定位结果；

步骤六，确定管道缺陷位置，完成定位工作。

2.根据权利要求1所述的一种基于初始姿态角自对准的小径管道机器人定位方法，其特征在于：所述步骤二具体包括：

(1)初始时刻，小径管道机器人在水平方向上做短时直线加速度机动，同时保存微惯性传感器及里程仪测量数据；

(2)通过里程仪检测得到两个不同时刻管道机器人的速度V_mile^b(t_(k-1))与V_mile^b(t_(k))得到管道机器人行进速度平均变化量，即管道机器人在时间段[t_(k-1)，t_(k)]内的水平加速度；同时，根据水平加速度测量信息求得水平姿态矩阵；

(3)结合惯性测量器件加速度计测得的比力加速度与已知的重力加速度结合列方程求管道机器人加速度；

为解算实际方位角，可对比例方程作如下近似：

记上式为公式(1)，其中，h系为当地水平坐标系，

为管道机器人在导航坐标系中的加速度，f^b为载体坐标系下的比力投影，f^h为水平坐标系下的比力投影，gⁿ为导航坐标系下的重力加速度投影，

为载体坐标系相对于导航坐标系的姿态转换矩阵，

为载体坐标系相对于水平坐标系的姿态阵，

为水平坐标系相对于导航坐标系的姿态阵；

(4)根据方位矩阵求得管道机器人纵轴相对于地理北向的方位角。

3.根据权利要求1或2所述的一种基于初始姿态角自对准的小径管道机器人定位方法，其特征在于：步骤六具体为：将数据平滑处理后的被检测管道坐标位置与由管道检测传感器检测并分析出的管道缺陷分析结果进行时间同步操作，得出管道缺陷与管道坐标位置的关系，指导管道缺陷维护。

Images