CN110440796A - 基于旋转磁场和惯导融合的管道机器人定位装置及方法 - Google Patents

Abstract

本发明提供了基于旋转磁场和惯导融合的管道机器人定位装置及方法，属于定位导航领域。本发明的定位方法，具体步骤为：根据磁偶极子定位的原理确定定位对象的坐标x、y；通过管道机器人定位定姿算法得到管道机器人的位置和速度信息；将定位机器人的位置和速度信息与磁偶极子定位得到的位置和速度信息作差得到误差；将得到的速度和位置误差作为量测值，进行组合卡尔曼滤波；经过卡尔曼滤波，利用得到的数据校正管道机器人的位置和速度，并将其反馈到捷联惯导系统中。本发明提出了一种先进的磁偶极子定位算法，并利用此方法对捷联惯导定位系统进行校正补偿，使得新的定位定姿系统可以长时间高精度用于管道机器人的定位和定姿。

Description

基于旋转磁场和惯导融合的管道机器人定位装置及方法

技术领域

本发明涉及一种基于旋转磁场和惯导融合的管道机器人定位装置及方法，属于定位导航领域。

背景技术

随着管道运输业的发展，对管道缺陷检测技术的要求越来越高，尤其是管内探伤检测以及管内清理的需求日益增加。由于管道具有封闭、缺氧、空间狭小等特殊性，在这种恶劣情况下，难于利用人工方法完成管道缺陷的检测。在管道焊缝检测、城市供水管线检测以及石油输送管道检测等领域广泛地使用管道机器人技术。

石油、天然气等资源开发采用管道进行运输，因此，输油、输气管线的缺陷检测以及及时修复对石油、天然气等资源的开发具有重要意义。管道机器人在管道内移动并通过其携带的传感器对管线进行检测。当管道出现缺陷，需要人工进行处理时，操作人员必须知道机器人所处的准确位置，以便采取相应的措施。因此，在管道机器人的使用中，如何快速、准确的定位是管道机器人位置控制的关键问题。本发明介绍的基于旋转磁场和惯导融合的管道机器人定位方法就是一种先进的解决管道机器人定位定姿问题的方法。

《基于磁偶极子模型的管道机器人定位技术研究》(电波科学报，2006年8月，李军远，李盛凤，陈宏钧，张晓华)：分析了低频电磁波发射天线的磁场分布，为了便于工程实现将发射天线简化为磁偶极子模型，给出了磁偶极子磁场分布的数值计算结果。结果表明，垂直于发射天线轴线方向的磁感应强度幅值随位置的改变呈双峰变化的规律，由此提出了一种基于低频电磁波磁场幅值信号双峰变化规律的管道机器人定位方法。低频电磁波发射天线以平行于管道轴线方向安装在管道机器人上；而低频电磁波接收天线垂直于发射天线安装在海上作业的救援装置上。在定位的过程中，通过检测接收天线检测到信号的强度变化，找到符合双峰变化规律的信号位置，从而达到定位效果。《基于磁偶极子模型的管道机器人定位技术研究》一文中提到的定位技术的实现存在很大的困难。在实际应用中，管道机器人要巡查的管道是很长的，文中提到的定位技术只能在距离较近(文中做实验的距离为2m以内)的位置才能有信号，这样很难快速的进行定位。文中的定位技术依靠的是寻找磁场呈双峰变化规律的位置，在实验室环境下，干扰很少，因此比较容易找到正确的位置，在实际操作中，外界干扰强烈，很有可能导致无法找到磁场呈双峰变化规律的位置。实验中，救援装置和发射天线两者水平线的距离恒定为一米，在实际操作中，管道埋在地下、海中，这就导致在操作过程中，很难保证救援装置和发射天线的距离很近，因此很难利用该技术进行实际操作。

《基于捷联惯导的变压器内微缆机器人定位研究》(现代电子技术，2018年10月，赵晓军，田粮川，孙文博，刘阳)：设计了一种基于捷联惯导的大型变压器内微缆机器人定位系统。该系统搭载加速度传感器、陀螺仪传感器、摄像头等惯性测量单元，运用四阶龙格库塔法来解算姿态矩阵，结合卡尔曼滤波算法融合加速度计传感器数据，对姿态矩阵数据进行修正，从而确定机器人运动的速度及位置，同时摄像头可以对机器人的定位进行定时矫正，以保证位置的精确性。实验数据表明，该系统自主性强，短时间内导航精度高，完全符合检测系统在大型变压器内的定位需求。《基于捷联惯导的变压器内微缆机器人定位研究》一文中介绍了利用捷联惯导系统进行姿态解算和定位，但是利用惯组进行定位会有随机误差，而随机误差的累积会使得定位的误差很大，因此该方法只适用于短距离定位。

与上述定位方法相比，本申请提出了一种基于捷联惯导系统和人工磁场的定位方法，利用人工磁场的精确定位对惯组进行补偿，实现长时间长距离的管道内定位。同时，本申请的磁偶极子定位算法与前文提到的方法相比，精度更高，理论更完善，实际应用更方便。

发明内容

本发明的目的是为了解决上述现有技术存在的问题，进而提供一种基于旋转磁场和惯导融合的管道机器人定位装置及方法。

本发明的目的是通过以下技术方案实现的：

基于旋转磁场和惯导融合的管道机器人定位装置，所述基于旋转磁场和惯导融合的管道机器人定位装置包括捷联惯导系统、磁传感器、管道机器人和磁偶极子，管道机器人位于管道内，管道机器人上设置有捷联惯导系统和磁传感器，管道内壁上设置有两个磁偶极子。

基于旋转磁场和惯导融合的管道机器人定位方法，具体步骤为：

步骤一：根据磁偶极子定位的原理确定定位对象的坐标x、y；

步骤二：通过管道机器人定位定姿算法得到管道机器人的位置和速度信息；

步骤三：将定位机器人的位置和速度信息与磁偶极子定位得到的位置和速度信息作差得到误差；

步骤四：将得到的速度和位置误差作为量测值，进行组合卡尔曼滤波；

步骤五：经过卡尔曼滤波，利用得到的数据校正管道机器人的位置和速度，并将其反馈到捷联惯导系统中。

本发明基于旋转磁场和惯导融合的管道机器人定位方法，所述步骤一的具体计算过程为：

假设定位对象仅在x-y平面中移动，因此可以仅使用两个坐标描述其位置：

将式(1)带入毕奥萨伐尔定律中得：

其中，M1为两极子的磁矩，M2同理；

B₂同理；用a₁和a₂定义两个轴向磁感应强度之间的比例

联立上述公式可以得到只与a₁、a₂有关的x、y的表达式，R表示两信标之间的距离,

本发明基于旋转磁场和惯导融合的管道机器人定位方法，所述步骤二具体计算过程为：

惯导系统各参数的初始化

R＝6371000m，g₀＝9.8m/s²，ω_ie＝7.292×10^-5s^-1

λ(1)＝λ₀，h(1)＝h₀

Q(1)＝Q₀，V_r(1)＝[V_E0 V_N0 V_ξ0]

各参数定义分别为：R——地球的半径；g0——重力加速度；ω_ie——地球的自转角速度；λ(1),h(1)——初始位置；Q(1)——初始四元数；V_r(1)——初始速度；

假设陀螺仪的输出是

Δθ＝[Δθ _xΔθ_y Δθ_z] (7)

其中，Δθ_x、Δθ_y、Δθ_z分别表示三个陀螺仪在一段时间内的输出；

更新四元数

此处，为G_N-1→B_N的四元数；

角速度计算

更新地理坐标系

Δγ₀≈||ω_ig||ΔT (11)

其中，n为地理坐标系中各方向角速度的单位矢量，地理坐标系中G_N-1→G_N的四元数；

加速度输出比力

f^b＝[f_x f_y f_z] (14)

比力变换

其中，为地理坐标系的真实加速度。

计算位置速度信息

本发明基于旋转磁场和惯导融合的管道机器人定位方法，所述步骤四具体计算过程为：

组合卡尔曼滤波的参数如下

人工磁场和捷联惯导组合系统的状态方程和量测方程为

X_k＝φ_k|k-1X_k-1+Γ_k-1W_k-1 (21)

Z_k＝[Z₁ Z₂ Z₃ Z₄]＝H_kX_k+V_k (22)

利用惯导无阻尼误差方程，并将系统的陀螺和加速度计的漂移扩充为状态作为系统的状态方程，其状态变量X为18维列矢量：

其中，φ_E,φ_N,φ_ζ分别表示在地理坐标系三个轴方向的误差角；δV_E,δV_N,δV_ζ为东、北、天三个方向的速度误差；δL,δλ,δh为纬度、经度、高度的误差；ε_bx,ε_by,ε_bz为东、北、天三个陀螺漂移中的随机常值；ε_rx,ε_ry,ε_rz为东、北、天三个陀螺漂移中的一阶马尔柯夫过程；▽_x,▽_y,▽_z为东、北、天三个加速度计的误差；

W_k-1为随机过程噪声；V_k为随机观测噪声；

系统的状态转移矩阵为：

其中：

F(2,7)＝-ω_iesinL，

F(4,2)＝-f_ζ

F(4,3)＝-f_ζ，

F(5,1)＝f_ζ，F(5,3)＝-f_E，

F(6,1)＝-f_N

F(6,2)＝-f_E，

F(6,7)＝-2V_Eω_ie sinL，

F(9,6)＝1，

其中，T_r,T_a为相关时间；F_N为对应9个基本导航参数的系统阵，其余元素为0；L为纬度；

上述式中用到的参数值如下：

R_M＝R_e(1-2f+3fsin²L)，R_N＝R_e(1+fsin²L)，R_e＝63718137m，f＝1/298.257

系统的噪声输入矩阵为：

其中：

量测方程的变量Z₁,Z₂,Z₃,Z₄是平面内速度和位置的误差，因此H是一个4*18的矩阵其中，H(1,4)＝1,H(2,5)＝1,H(3,7)＝R_M,H(4,8)＝R_NcosL；

系统的观测噪声为：

V_k＝[V₁ V₂ N_N N_E]^T

其中，V₁～N{0,R₁},V₂～N{0,R₂}，N_N、N_E指磁传感器沿东、北方向的位置误差；

根据以上定义，直接给出随机线性离散卡尔曼滤波的基本方程：

1)状态一步预测：

2)状态估计

3)滤波增益矩阵

4)一步预测协方差矩阵

5)滤波估计协方差矩阵

P_k＝(I-K_kH_k)P_k/k-1 (27)

式中：

——k时刻的状态变量后验估计；

——k-1时刻到k时刻的一步预测估计，即状态变量的先验估计；

P_k——k时刻的滤波估计协方差矩阵；

K_k——k时刻的滤波增益矩阵；

P_k/k-1——一步预测估计的协方差矩阵；

利用式(23)-(27)不断进行迭代，就可以完成对状态变量进行最优估计。

本发明基于旋转磁场和惯导融合的管道机器人定位装置及方法，提出了一种先进的磁偶极子定位算法，并利用此方法对捷联惯导定位系统进行校正补偿，使得新的定位定姿系统可以长时间高精度用于管道机器人的定位和定姿。

附图说明

图1为本发明中管道机器人的定位示意图。

图2为本发明中偶极子模型。

图3为本发明基于旋转磁场和惯导融合的管道机器人定位定姿算法示意图。

图中附图标记有：1为捷联惯导系统；2为磁传感器；3为管道机器人；4为磁偶极子。

具体实施方式

下面将结合附图对本发明做进一步的详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式，但本发明的保护范围不限于下述实施例。

实施例一：如图1-3所示，本实施例所涉及的一种基于旋转磁场和惯导融合的管道机器人定位装置及方法，本申请提出了一种新的管道机器人定位方案，在使用捷联惯导系统进行姿态解算的同时，利用磁偶极子进行定位，从而尽可能的抵消位置和速度上的误差，以达到长时长距高精度定位。定位示意图如下。

将缠有通电线圈的正方形框抽象成磁极子模型，基于磁偶极子的定位算法基础是毕奥-萨伐尔定律。在目标与磁极子近距离条件下，毕奥-萨伐尔定律描述了电流源与磁场之间的关系。

如图2所示，M₁和M₂分别为两极子的磁矩，频率分别为f₁，f₂。

假设定位对象仅在x-y平面中移动，因此可以仅使用两个坐标描述其位置：

将式(1)带入毕奥萨伐尔定律中得：

其中，M1为两极子的磁矩，M2同理；

B₂同理；用a₁和a₂定义两个轴向磁感应强度之间的比例

联立上述公式可以得到只与a₁、a₂有关的x、y的表达式，R表示两信标之间的距离

通过上述推导可以看出，只要测得x、y两个轴的磁感应强度即可通过本算法求得对象的位置。经过实际的实验，该定位方法的定位精度跟磁场的强度有很大关系，在磁场强度能达到稳定的1量级以上时，能准确的确定位置。如果应用到管道里，使用10cm×10cm的线圈，在±6m范围内定位误差能控制在10cm以下。因此，依靠其高精度的定位功能可以有效的补偿惯导系统的累积误差，从而达到管道机器人的高精度定姿定位。

以上就是磁偶极子定位的原理，将其与捷联惯导系统结合使用，可以大大提高系统的定位精度，并使得惯导系统可以用于管道机器人的长时间定位定姿中。具体的管道机器人定位定姿算法如图3所示。

具体算法流程如下

惯导系统各参数的初始化

R＝6371000m，g₀＝9.8m/s²，ω_ie＝7.292×10^-5s^-1

λ(1)＝λ₀，h(1)＝h₀

Q(1)＝Q₀，V_r(1)＝[V_E0 V_N0 V_ξ0]

各参数定义分别为：R——地球的半径；g₀——重力加速度；ω_ie——地球的自转角速度；λ(1),h(1)——初始位置；Q(1)——初始四元数；V_r(1)——初始速度。

假设陀螺仪的输出是

Δθ＝[Δθ_x Δθ_y Δθ_z] (7)

其中，Δθ_x、Δθ_y、Δθ_z分别表示三个陀螺仪在一段时间内的输出。

更新四元数

此处，为G_N-1→B_N的四元数。

角速度计算

更新地理坐标系

Δγ₀≈||ω_ig||ΔT (11)

其中，n为地理坐标系中各方向角速度的单位矢量，地理坐标系中G_N-1→G_N的四元数。

加速度输出比力

f^b＝[f_x f_y f_z] (14)

比力变换

其中，为地理坐标系的真实加速度。

计算位置速度信息

用得到的位置和速度信息与磁偶极子定位得到的位置和速度信息作差得到误差，将得到的速度和位置误差作为量测值，进行组合卡尔曼滤波。从而估计惯导系统的误差，然后利用估计的误差对惯导系统进行校正。

组合卡尔曼滤波的参数如下

人工磁场和捷联惯导组合系统的状态方程和量测方程为

X_k＝φ_k|k-1X_k-1+Γ_k-1W_k-1 (21)

Z_k＝[Z₁ Z₂ Z₃ Z₄]＝H_kX_k+V_k (22)

在此利用惯导无阻尼误差方程，并将系统的陀螺和加速度计的漂移扩充为状态作为系统的状态方程。其状态变量X为18维列矢量：

其中，φ_E,φ_N,φ_ζ分别表示在地理坐标系三个轴方向的误差角；δV_E,δV_N,δV_ζ为东、北、天三个方向的速度误差；δL,δλ,δh为三个方向的位置误差；ε_bx,ε_by,ε_bz为东、北、天三个陀螺漂移中的随机常值；ε_rx,ε_ry,ε_rz为东、北、天三个陀螺漂移中的一阶马尔柯夫过程；▽_x,▽_y,▽_z为东、北、天三个加速度计的误差。

W_k-1为随机过程噪声；V_k为随机观测噪声；

系统的状态转移矩阵为：

其中：

F(2,7)＝-ω_iesinL，

F(4,2)＝-f_ζ

F(4,3)＝-f_ζ，

F(5,1)＝f_ζ，F(5,3)＝-f_E，

F(6,1)＝-f_N

F(6,2)＝-f_E，

F(6,7)＝-2V_Eω_ie sinL，

其中，T_r,T_a为相关时间；F_N为对应9个基本导航参数的系统阵，其余元素为0；L为纬度。

上述式中用到的参数值如下：

R_M＝R_e(1-2f+3fsin²L)，R_N＝R_e(1+fsin²L)，R_e＝63718137m，f＝1/298.257

系统的噪声输入矩阵为：

其中：

量测方程的变量Z₁,Z₂,Z₃,Z₄是平面内速度和位置的误差，因此H是一个4*18的矩阵其中，H(1,4)＝1,H(2,5)＝1,H(3,7)＝R_M,H(4,8)＝R_NcosL；

系统的观测噪声为：

V_k＝[V₁ V₂ N_N N_E]^T

其中，V₁～N{0,R₁},V₂～N{0,R₂}，N_N、N_E指磁传感器沿东、北方向的位置误差。

根据以上定义，直接给出随机线性离散卡尔曼滤波的基本方程：

1)状态一步预测：

2)状态估计

3)滤波增益矩阵

4)一步预测协方差矩阵

5)滤波估计协方差矩阵

P_k＝(I-K_kH_k)P_k/k-1 (27)

式中：

——k时刻的状态变量后验估计；

——k-1时刻到k时刻的一步预测估计，即状态变量的先验估计；

P_k——k时刻的滤波估计协方差矩阵；

K_k——k时刻的滤波增益矩阵；

P_k/k-1——一步预测估计的协方差矩阵。

卡尔曼滤波的核心思想是通过“预测估计+量测修正”的方式，进行对观测量的估计，通过对滤波增益的不断更新，在预测估计值和量测值之间采取折中思想，对状态变量进行最优估计。因此，利用式23-27不断进行迭代，就可以完成对状态变量进行最优估计。

按照上述公式，经过卡尔曼滤波，利用得到的数据校正管道机器人的位置和速度，并将其反馈到捷联惯导系统中，从而实现人工磁场和捷联惯导系统的联合定位系统。

实施例二，关于数据处理，本发明可以采用本地端的处理器处理，同时也可以采用将磁传感器和惯组采集到的数据上传到物联网，在云端进行处理的方法，这种方法和本地端的计算可以互为印证，作为热备份，一旦本地端出现问题立即介入系统，提高系统的冗余度。

以上所述，仅为本发明较佳的具体实施方式，这些具体实施方式都是基于本发明整体构思下的不同实现方式，而且本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (5)

1.基于旋转磁场和惯导融合的管道机器人定位装置，其特征在于，所述基于旋转磁场和惯导融合的管道机器人定位装置包括捷联惯导系统(1)、磁传感器(2)、管道机器人(3)和磁偶极子(4)，管道机器人(3)位于管道内，管道机器人(3)上设置有捷联惯导系统(1)和磁传感器(2)，管道内壁上设置有两个磁偶极子(4)。

2.基于旋转磁场和惯导融合的管道机器人定位方法，其特征在于，所述基于旋转磁场和惯导融合的管道机器人定位方法的具体步骤为：

步骤一：根据磁偶极子(4)定位的原理确定定位对象的坐标x、y；

步骤二：通过管道机器人(3)定位定姿算法得到管道机器人(3)的位置和速度信息；

步骤三：将定位机器人(3)的位置和速度信息与磁偶极子(4)定位得到的位置和速度信息作差得到误差；

步骤四：将得到的速度和位置误差作为量测值，进行组合卡尔曼滤波；

步骤五：经过卡尔曼滤波，利用得到的数据校正管道机器人(3)的位置和速度，并将其反馈到捷联惯导系统中。

3.根据权利要求2所述的基于旋转磁场和惯导融合的管道机器人定位方法，其特征在于，所述步骤一的具体计算过程为：

假设定位对象仅在x-y平面中移动，因此可以仅使用两个坐标描述其位置：

将式(1)带入毕奥萨伐尔定律中得：

其中，M1为两极子的磁矩，M2同理；

B₂同理；用a₁和a₂定义两个轴向磁感应强度之间的比例

联立上述公式可以得到只与a₁、a₂有关的x、y的表达式，R表示两信标之间的距离

4.根据权利要求2所述的基于旋转磁场和惯导融合的管道机器人定位方法，其特征在于，所述步骤二具体计算过程为：

惯导系统各参数的初始化

R＝6371000m，g₀＝9.8m/s²，ω_ie＝7.292×10^-5s^-1

λ(1)＝λ₀，h(1)＝h₀

Q(1)＝Q₀，V_r(1)＝[V_E0 V_N0 V_ξ0]

各参数定义分别为：R——地球的半径；g0——重力加速度；ω_ie——地球的自转角速度；λ(1),h(1)——初始位置；Q(1)——初始四元数；V_r(1)——初始速度；

假设陀螺仪的输出是

Δθ＝[Δθ_x Δθ_y Δθ_z] (7)

其中，Δθ_x、Δθ_y、Δθ_z分别表示三个陀螺仪在一段时间内的输出；

更新四元数

此处，为G_N-1→B_N的四元数；

角速度计算

更新地理坐标系

Δγ₀≈||ω_ig||ΔT (11)

其中，n为地理坐标系中各方向角速度的单位矢量，地理坐标系中G_N-1→G_N的四元数；

加速度输出比力

f^b＝[f_x f_y f_z] (14)

比力变换

其中，为地理坐标系的真实加速度。

计算位置速度信息

5.根据权利要求2所述的基于旋转磁场和惯导融合的管道机器人定位方法，其特征在于，所述步骤四具体计算过程为：

组合卡尔曼滤波的参数如下

人工磁场和捷联惯导组合系统的状态方程和量测方程为

X_k＝φ_k|k-1X_k-1+Γ_k-1W_k-1 (21)

Z_k＝[Z₁ Z₂ Z₃ Z₄]＝H_kX_k+V_k (22)

利用惯导无阻尼误差方程，并将系统的陀螺和加速度计的漂移扩充为状态作为系统的状态方程，其状态变量X为18维列矢量：

其中，φ_E,φ_N,φ_ζ分别表示在地理坐标系三个轴方向的误差角；δV_E,δV_N,δV_ζ为东、北、天三个方向的速度误差；δL,δλ,δh为纬度、经度、高度的误差；ε_bx,ε_by,ε_bz为东、北、天三个陀螺漂移中的随机常值；ε_rx,ε_ry,ε_rz为东、北、天三个陀螺漂移中的一阶马尔柯夫过程；为东、北、天三个加速度计的误差；

W_k-1为随机过程噪声；V_k为随机观测噪声；

系统的状态转移矩阵为：

其中：

F(2,7)＝-ω_iesinL，

F(4,2)＝-f_ζ

F(4,3)＝-f_ζ，

F(5,1)＝f_ζ，F(5,3)＝-f_E，

F(6,1)＝-f_N

F(6,2)＝-f_E，

F(6,7)＝-2V_Eω_ie sinL，

F(9,6)＝1，

其中，T_r,T_a为相关时间；F_N为对应9个基本导航参数的系统阵，其余元素为0；L为纬度；

上述式中用到的参数值如下：

R_M＝R_e(1-2f+3fsin²L)，R_N＝R_e(1+fsin²L)，R_e＝63718137m，f＝1/298.257

系统的噪声输入矩阵为：

其中：

量测方程的变量Z₁,Z₂,Z₃,Z₄是平面内速度和位置的误差，因此H是一个4*18的矩阵其中，H(1,4)＝1,H(2,5)＝1,H(3,7)＝R_M,H(4,8)＝R_NcosL；

系统的观测噪声为：

V_k＝[V₁ V₂ N_N N_E]^T

其中，V₁～N{0,R₁},V₂～N{0,R₂}，N_N、N_E指磁传感器沿东、北方向的位置误差；

根据以上定义，直接给出随机线性离散卡尔曼滤波的基本方程：

1)状态一步预测：

2)状态估计

3)滤波增益矩阵

4)一步预测协方差矩阵

5)滤波估计协方差矩阵

P_k＝(I-K_kH_k)P_k/k-1 (27)

式中：

——k时刻的状态变量后验估计；

——k-1时刻到k时刻的一步预测估计，即状态变量的先验估计；

P_k——k时刻的滤波估计协方差矩阵；

K_k——k时刻的滤波增益矩阵；

P_k/k-1——一步预测估计的协方差矩阵；

利用式(23)-(27)不断进行迭代，就可以完成对状态变量进行最优估计。