CN109751999B - 一种管道测量机器人的标定方法、装置及系统 - Google Patents

Abstract

本发明提供了一种管道测量机器人的标定方法、装置及系统，通过在标定装置底座上设置V型支架，将测量机器人架设到所述标定装置的V型支架上；控制测量机器人定轴旋转，同时保证测量机器人的几何中心保持不变且位于预设坐标系的原点；根据惯性测量单元中惯导敏感轴采集的惯导数据计算测量机器人的姿态偏移。在重力的作用下，V型支架可以保证测量机器人与V型支架在进行定轴旋转时接触为点接触，减小摩擦力，从而使得管道测量机器人进行高精度转动，该标定装置可以适应不同长度、不同旋转轴直径的管道测量机器人标定，为对管道三维曲线的精确测量提供了便利。

Description

一种管道测量机器人的标定方法、装置及系统

技术领域

本发明涉及工程测量和管线测量技术领域，尤其涉及的是一种基于惯导/里程计的管道三维测量机器人的标定方法、装置及系统。

背景技术

管道测量机器人通过在管道内部运动，可以精确测量管道的三维空间曲线，被广泛用于地下管线位置测量。管道测量机器人的关键功能模块包括，车体结构、测量单元模块及辅助模块(包括采集板、电源等)。管道测量机器人在车体结构上具有对中管道中线的设计，如采用可伸缩的弹簧三角轮装置，可适应不同大小的管径，并保证管道测量机器人的轴线与管道的轴线在同一条曲线上。管道测量机器人测量单元模块包括惯性测量单元(IMU，inertial measurement unit)和里程编码器(DMI，distance measurement instrument，也称为里程计)。管道测量机器人利用IMU数据和里程编码器数据进行组合导航，最终可得到管道的三维曲线。

在管道测量机器人设计时，惯性导航装置的测量轴系与车体的参考轴系之间的空间关系具有规则的设计值。但是在实际制造、安装甚至在使用过程中，惯性导航装置的测量轴系会与车体参考系的轴系发生偏移，导致最终测量的三维曲线误差较大。一般地，两个三维轴系的偏差包括三维位置偏差和三维姿态偏差。微小三维位置偏差可使用游标卡尺等精密测量工具进行直接测量。而微小的三维姿态偏差(下文称为偏置角)则一般难以直接观测，需要设计特殊的标定方法进行标定。

因此，现有技术有待于进一步的改进。

发明内容

鉴于上述现有技术中的不足之处，本发明的目的在于提供一种管道测量机器人的标定方法、装置及系统，克服现有技术中测量机器人在进行测量时，微小的三维姿态偏差难以标定的缺陷。

本发明提供的第一实施例为一种管道测量机器人的标定方法，其中，包括：

控制测量机器人定轴旋转，同时安装在所述测量机器人上的惯性测量单元采集惯导数据；根据采集的所述惯导数据计算测量机器人的姿态偏移。

可选的，所述方法还包括：

采集测量机器人旋转前后的测量数据，并根据所述测量数据对惯性测量单元进行初始化处理。

可选的，所述方法还包括：

分别以当地水平指北坐标系为导航坐标系、以测量机器人的几何中心为原点，以测量机器人固连的坐标系统为车体坐标系和以惯性测量单元固连的坐标系为载体坐标系。

可选的，所述根据采集的所述惯导数据计算测量机器人的姿态偏移的步骤包括：对所述惯导数据进行处理,得到惯导敏感轴在导航坐标系中的位置轨迹和载体坐标系在导航坐标系中的三维姿态角序列；

通过惯性解算得到的所述三维姿态角序列得到每个时刻载体坐标系到导航坐标系的转换关系；

根据所述转换关系将测量机器人的旋转轴投影到导航坐标系；

通过最小二乘拟合，估计定轴旋转的运动轨迹圆心在导航坐标系的位置，从而得到车体坐标系的Y轴在导航坐标系中的方向。

可选的，所述定轴旋转的旋转顺序为：先预设第一时间段内的静态数据采集、预设第二时间段内的正向定轴旋转的数据采集、最后预设第三时间段内的反向定轴旋转。

本发明提供的第二实施例为一种管道测量机器人的标定装置，其中，应用于对测量机器人进行测量标定，所述测量机器人包括：内部安装有惯性测量单元的电子仓、对称设置在所述电子仓两侧的滑杆、均匀设置在所述滑杆上的行走轮；

所述标定装置包括：

底座，设置在所述底座上的支架；

所述支架包括：左支架和右支架；

所述左支架和右支架的上端均设置有V型支架，所述测量机器人架设在所述V型支架上做定轴旋转。

可选的，所述左支架和右支架的本体均一端连接所述V型支架，另一端连接在底座上，所述本体的结构为圆柱形。

可选的，所述左支架和右支架与所述底座活动连接。

可选的，所述左支架和右支架与所述底座固定连接。

本发明提供的第三实施例为一种管道测量机器人的标定系统，其中，包括：管道测量机器人和标定装置；

所述测量机器人包括：所述测量机器人包括：内部安装有惯性测量单元的电子仓、对称设置在所述电子仓两侧的滑杆、均匀设置在所述滑杆上的行走轮；

所述标定装置包括：底座，设置在所述底座上的支架；

所述支架包括：左支架和右支架；

所述左支架和右支架的上端均设置有V型支架，所述滑杆的一端连接在电子仓两侧的中心，另一端架设在所述V型支架上。

有益效果，本发明提供了一种管道测量机器人的标定方法、装置及系统，通过在底座上设置V型支架，将测量机器人架设到所述标定装置的V型支架上；控制测量机器人定轴旋转，同时保证测量机器人的几何中心保持不变且位于预设坐标系的原点；根据惯性测量单元中惯导敏感轴采集的惯导数据计算测量机器人的姿态偏移。在重力的作用下，V型支架可以保证测量机器人与V型支架在进行定轴旋转时接触为点接触，减小摩擦力，从而使得管道测量机器人进行高精度转动，该标定装置可以适应不同长度、不同旋转轴直径的管道测量机器人标定，为对管道三维曲线的精确测量提供了便利。

附图说明

图1是本发明所提供的标定方法的步骤流程图；

图2是本发明所提供的标定方法中各个坐标系的示意图；

图3是本发明所提供的测量机器人标定装置立体结构示意图；

图4是本发明所提供的测量机器人标定装置上架设有测量机器人的结构示意图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。

针对现有管道测量机器人中惯性测量装置和车体的轴系姿态偏移，本发明提出一种通过定轴旋转的偏置角标定方法，对姿态偏移进行估算。本发明设计一种可以使得装置进行定轴旋转的支架，将管道测量机器人放在支架上进行定轴旋转，采集相应的测量数据，最后对测量数据进行解算，估计姿态偏移。

实施例1

本发明提供的第一实施例为一种管道测量机器人的标定方法，如图1所示，所述标定方法包括：

步骤S1、控制测量机器人定轴旋转，同时安装在所述测量机器人上的惯性测量单元采集惯导数据。

本步骤中将测量机器人架设在标定装置上，控制测量机器人定轴旋转。本步骤中使用的标定装置可以为一个能够将管道测量机器人架空的装置，该装置可以仅仅具有一个支撑测量机器人的支架，和用于固定所述支架的底座，也可以为具有使用两个固定的支架实现将测量机器人架空的装置。

将测量机器人架空后，控制测量机器人做定轴旋转。由于测量机器人内安装有惯性测量单元，因此可以通过所述惯性测量单元进行相应的惯导数据采集。

步骤S2、根据采集的所述惯导数据计算测量机器人的姿态偏移。

对采集到的惯导数据进行分析，得到测量机器人的姿态偏移。

所述测量机器人包括：电子仓、与所述电子仓两侧中心连接的滑杆、通过直线轴承与所述滑杆连接的车轮支架、设置在所述车轮支架上的均匀分布的至少两组行走轮、设置在一侧所述滑杆上的且安装有独立收缩轮架的至少一组里程轮和安装在各个所述里程轮上的编码器；所述直线轴承两端的滑杆上安装有弹簧；所述电子仓内设置有：安装在采集板上的惯性测量单元、采集控制模块和电源模块；所述电源模块为测量机器人的各个模块及单元进行供电。

各个所述行走轮通过所述弹簧同步收缩使得所述惯性测量单元的几何中心与所述电子仓的中轴线一致；

所述采集控制模块，用于控制惯性测量单元与编码器进行测量数据的采集，并将所述测量数据与管道基准时刻的三维曲线数据相比较，得到管道三维曲线变形量。

所述采集控制模块上还设置有FPGA单元和晶振单元；所述电源模块与惯性测量单元、FPGA单元、晶振单元和上位机单元均相连接。

所述惯性测量单元通过RS-422转LVTTL接口与所述FPGA单元相连接；

所述FPGA单元分别通过UART接口和USB转串口接口与上位机建立通信连接；用于根据所述晶振单元输出的时钟信号触发控制惯性测量单元及所述编码器同步采集测量数据及将采集到的测量数据上传至上位机单元；

所述上位机单元通过UART接口向FPGA单元发出数据采集控制指令，以及通过USB转串口接口接收测量数据。

为了便于测量机器人运动，所述滑杆的两端设置有动力牵引装置；

所述动力牵引装置包括：驱动电机和电动绞盘；所述驱动电机驱动电动绞盘上的拉线牵引所述测量机器人来回运动。

较佳的，所述行走轮的个数为三个，且三个所述行走轮均匀对称设置，里程轮的个数为三个，且三个所述里程轮均匀对称设置。

具体的，为了便于对惯导数据进行分析，所述方法还包括：

分别以当地水平指北坐标系为导航坐标系、以测量机器人的几何中心为原点，以测量机器人固连的坐标系统为车体坐标系和以惯性测量单元固连的坐标系为载体坐标系。

所述导航坐标系(n系)：为当地水平指北坐标系，其中原点位置为当地坐标，X轴指向东方向，Y轴指向正北方向，Z轴与X、Y轴形成右手坐标系。

所述车体坐标系(v系)：与测量机器人固连的坐标系，测量机器人几何中心为车体坐标系原点，小车右向为X轴，前向为Y轴，Y轴与测量机器人的中心轴线重合。Z轴与X、Y轴构成右手系。

所述载体坐标系(b系)：为惯性元器件(陀螺、加速度计)的敏感轴，即惯性测量值所在的坐标系，其与惯导固连，一般地其原点再惯导几何中心，坐标轴方向由惯导厂家定义，本专利中，惯导坐标轴方向与车体坐标系近似平行。

本发明所提供的方法在惯导数据采集时,将测量机器人架设在标定装置上,操作者用手拨动测量机器人，使其进行定轴旋转，并保证管线仪在轴向方向上不发生平移运动.如图2所示,在标定旋转过程中,由于测量机器人做定轴旋转,测量机器人的中心轴线(也即v坐标系Y轴指向，下文记作Y_v)保持不变，惯导敏感轴Y_b绕测量机器人的中心轴线Y_v做圆周运动,圆的半径即是测量机器人的中心轴线与惯导载体坐标系前向坐标轴之间的夹角。

为了实现采集到较为准确的惯导数据，所述方法还包括：

采集测量机器人旋转前后的测量数据，并根据所述测量数据对惯性测量单元进行初始化处理。

在数据采集前，为了对惯性导航系统进行初始化，先进行一段时间长度为T(1分钟<T<10分钟)的静态数据采集，然后将测量机器人顺时针旋转N圈(N>3),然后逆时针旋转M圈(M>3)，最后再静止采集一段时间T.整个标定数据采集的过程，可以根据惯导性能和标定精度，控制在一定时间之内，一般对于战略级惯导，可以将采集时间为10分钟。

为了实现基于采集到的惯导数据得到准确的姿态偏移，所述根据采集的所述惯导数据计算测量机器人的姿态偏移的步骤包括：

对所述惯导数据进行处理,得到惯导敏感轴在导航坐标系中的位置轨迹和载体坐标系在导航坐标系中的三维姿态角序列；

通过惯性解算得到的所述三维姿态角序列得到每个时刻载体坐标系到导航坐标系的转换关系；

根据所述转换关系将测量机器人的旋转轴投影到导航坐标系；

通过最小二乘拟合，估计定轴旋转的运动轨迹圆心在导航坐标系的位置，从而得到车体坐标系的Y轴在导航坐标系中的方向。

结合图2所示，下面对上述步骤进行更加详细的说明：

在标定旋转过程中,由于车体做定轴旋转,车体轴线(也即v坐标系Y轴指向，下文记作Y_v)保持不变，惯导敏感轴Y_b绕测量机器人的中心轴线Y_v做圆周运动,圆的半径即是测量机器人中心轴线与惯导载体坐标系前向坐标轴之间的夹角。通过惯性编排解算，可以得到惯导敏感轴Y_b在导航坐标系中的运动轨迹。进一步根据运动轨迹，可以精确估计小车旋转轴在导航坐标系中的方向。最后，通过最小二乘法，可以估算惯导载体坐标系b系到小车车体坐标系v系的偏置角。

首先：进行惯性数据解算。利用前向卡尔曼滤波(Kalman filter)和后向RTS平滑(Rauch-Tung-Striebel Smoother)对惯导数据进行处理。在处理过程中加入旋转前和旋转后的位置作为测量约束。对定轴旋转采集的惯导数据进行处理,可以得到惯导敏感轴Y_b在导航坐标系中的位置轨迹和IMU载体坐标系在导航坐标系中的三维姿态角序列。

其次，进行轴系偏置角估计。通过惯性解算得到的三维姿态角序列可得到每个时刻惯导载体坐标系到导航坐标系的转换关系

进而可以将惯导载体坐标系中与定轴旋转轴系相近的轴系单位矢量(在本发明中选为Y轴，记为

且

将旋转轴投影到导航坐标系，计算公式如下：

由于轴系偏置角的存在，惯导载体轴系单位矢量(如Y_b轴)在导航坐标系绕车体旋转轴(Y_v)轨迹画出一个圆锥面。单位矢量终点运动轨迹(即

终点的运动轨迹)，为一个规则的圆(如图2所示)。容易知道，载体坐标系Y轴单位矢量Y_b在导航坐标系中的运动轨迹的圆心与导航坐标系原点的连线即测量机器人的旋转轴在导航坐标系的投影，也即是

通过最小二乘拟合，可以精确估计运动轨道圆心在导航坐标系的位置，从而得到车体坐标系Y_v轴在导航坐系中的方向

最后进行轴系偏置角估算。测量机器人的旋转轴可表示为

且满足：

其中，

与

的关系可以表示为：

将b系与v系之间的偏置角记为α，其中，α_y＝0；由于偏置角一般较小(<5°)，并对偏置角进行近似，即：

同时对等式两边乘以

得到：

其中

又α_y＝0；通过对线性方程进行最小二乘求解，可以得到α_x，α_z的最优解。

最终α_x，α_z即为需要标定的惯导载体坐标系b系到车体坐标系v系的转换关系。

较佳的，为了本发明中所采用的所述定轴旋转的旋转顺序为：先预设第一时间段内的静态数据采集、预设第二时间段内的正向定轴旋转的数据采集、最后预设第三时间段内的反向定轴旋转。本方法中采用静止、正传、反转三种定轴旋转运动方式组合，使得偏置角可以被充分观测。通过最小二乘拟合，得到最优偏转角估计方法。

实施例2

本发明提供的第二实施例为一种管道测量机器人的标定装置，如图3所示，应用于对测量机器人进行测量标定，结合图4，所述测量机器人包括：内部安装有惯性测量单元的电子仓301、对称设置在所述电子仓301两侧的滑杆303、均匀设置在所述滑杆303上的行走轮302；

所述标定装置包括：

底座10，设置在所述底座10上的支架20；

所述支架包括：左支架和右支架；

所述左支架和右支架的上端均设置有V型支架201，所述滑杆303的一端连接在电子仓两侧的中心，另一端架设在所述V型支架201上。

可选的，左支架和右支架的结构相同。

可选的，所述左支架和右支架的本体均一端连接所述V型支架，另一端连接在底座上，所述本体的结构为圆柱形。

可选的，所述左支架和右支架与所述底座活动连接。

可选的，所述左支架和右支架与所述底座固定连接。

实施例3

本发明提供的第三实施例为一种管道测量机器人的标定系统，结合图3和图4所示，所述系统包括：管道测量机器人和标定装置；

所述测量机器人包括：所述测量机器人包括：内部安装有惯性测量单元的电子仓、对称设置在所述电子仓两侧的滑杆、均匀设置在所述滑杆上的行走轮；

所述标定装置包括：底座，设置在所述底座上的支架；

所述支架包括：左支架和右支架；

所述左支架和右支架的上端均设置有V型支架，所述滑杆的一端连接在电子仓两侧的中心，另一端架设在所述V型支架上。

较佳的，所述左支架和右支架为对称设置，两者的结构相同，且由于两者均为圆柱形设置，因此左右支架之间有空间供测量机器人在做定轴旋转，而且为使得在测量机器人做定轴旋转时整个系统为稳定状态，可以将重心设置在底座上，从而保证了采集数据的准确性。

本发明提供了一种测量机器人标定装置及其标定方法，通过在底座上设置V型支架，将测量机器人架设到所述标定装置的V型支架上；控制测量机器人定轴旋转，同时保证测量机器人的几何中心保持不变且位于预设坐标系的原点；根据惯性测量单元中惯导敏感轴采集的惯导数据计算测量机器人的姿态偏移。在重力的作用下，V型支架可以保证测量机器人与V型支架在进行定轴旋转时接触为点接触，减小摩擦力，从而使得管道测量机器人进行高精度转动，该标定装置可以适应不同长度、不同旋转轴直径的管道测量机器人标定，为对管道三维曲线的精确测量提供了便利。

可以理解的是，对本领域普通技术人员来说，可以根据本发明的技术方案及其发明构思加以等同替换或改变，而所有这些改变或替换都应属于本发明所附的权利要求的保护范围。

Claims (4)

1.一种管道测量机器人的标定方法，其特征在于，包括：

控制测量机器人定轴旋转，同时安装在所述测量机器人上的惯性测量单元采集惯导数据；

根据采集的所述惯导数据计算测量机器人的姿态偏移；

所述根据采集的所述惯导数据计算测量机器人的姿态偏移的步骤包括：

利用前向卡尔曼滤波和后向RTS平滑对惯导数据进行处理，得到惯导敏感轴在导航坐标系中的运动轨迹；根据运动轨迹，精确估计所述测量机器人旋转轴在导航坐标系中的方向；通过最小二乘法，估算惯导载体坐标系到所述测量机器人车体坐标系的偏置角；

根据运动轨迹，精确估计所述测量机器人旋转轴在导航坐标系中的方向的步骤包括：通过最小二乘拟合，精确估计运动轨迹圆心在导航坐标系的位置，从而得到测量机器人旋转轴在导航坐标系中的方向；

通过最小二乘法，估算惯导载体坐标系到所述测量机器人车体坐标系的偏置角的步骤包括：

将偏置角记为α，对偏置角进行近似，即：

同时对等式

两边乘以

得到线性方程

其中，

其中α_y＝0，通过对所述线性方程进行最小二乘求解，得到α_x和α_z的最优解；

其中，

为测量机器人的旋转轴，

为测量机器人的旋转轴在导航坐标系的投影；

为惯导载体坐标系到导航坐标系的转换关系；

所述定轴旋转的旋转顺序为：先预设第一时间段内的静态数据采集、预设第二时间段内的正向定轴旋转的数据采集、最后预设第三时间段内的反向定轴旋转的数据采集。

2.根据权利要求1所述的标定方法，其特征在于，所述方法还包括：

采集测量机器人旋转前后的测量数据，并根据所述测量数据对惯性测量单元进行初始化处理。

3.根据权利要求1或2所述的标定方法，其特征在于，所述方法还包括：

分别以当地水平指北坐标系为导航坐标系、以测量机器人的几何中心为原点，以测量机器人固连的坐标系统为车体坐标系和以惯性测量单元固连的坐标系为载体坐标系。

4.根据权利要求3所述的标定方法，其特征在于，所述根据采集的所述惯导数据计算测量机器人的姿态偏移的步骤包括：

对所述惯导数据进行处理,得到惯导敏感轴在导航坐标系中的运动轨迹和载体坐标系在导航坐标系中的三维姿态角序列；

通过惯性解算得到的所述三维姿态角序列得到每个时刻载体坐标系到导航坐标系的转换关系；

根据所述转换关系将测量机器人的旋转轴投影到导航坐标系；

通过最小二乘拟合，估计定轴旋转的运动轨迹圆心在导航坐标系的位置，从而得到测量机器人旋转轴在导航坐标系中的方向。

Images