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

权利要求书

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