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

权利要求书

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)不断进行迭代，就可以完成对状态变量进行最优估计。