CN110440796A - 基于旋转磁场和惯导融合的管道机器人定位装置及方法 - Google Patents
基于旋转磁场和惯导融合的管道机器人定位装置及方法 Download PDFInfo
- Publication number
- CN110440796A CN110440796A CN201910764500.5A CN201910764500A CN110440796A CN 110440796 A CN110440796 A CN 110440796A CN 201910764500 A CN201910764500 A CN 201910764500A CN 110440796 A CN110440796 A CN 110440796A
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- positioning
- pipe robot
- error
- follows
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 230000005284 excitation Effects 0.000 title claims abstract description 22
- 230000004927 fusion Effects 0.000 title claims abstract description 22
- 238000001914 filtration Methods 0.000 claims abstract description 23
- 230000004807 localization Effects 0.000 claims abstract description 15
- 238000012937 correction Methods 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 27
- 230000001133 acceleration Effects 0.000 claims description 10
- 238000005259 measurement Methods 0.000 claims description 8
- 239000013598 vector Substances 0.000 claims description 6
- 230000006698 induction Effects 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000001514 detection method Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 6
- 230000002902 bimodal effect Effects 0.000 description 4
- 230000007547 defect Effects 0.000 description 4
- VNWKTOKETHGBQD-UHFFFAOYSA-N methane Chemical compound C VNWKTOKETHGBQD-UHFFFAOYSA-N 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 239000003208 petroleum Substances 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 230000018109 developmental process Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 239000003345 natural gas Substances 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000007789 gas Substances 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
- 238000003466 welding Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明提供了基于旋转磁场和惯导融合的管道机器人定位装置及方法,属于定位导航领域。本发明的定位方法,具体步骤为:根据磁偶极子定位的原理确定定位对象的坐标x、y;通过管道机器人定位定姿算法得到管道机器人的位置和速度信息;将定位机器人的位置和速度信息与磁偶极子定位得到的位置和速度信息作差得到误差;将得到的速度和位置误差作为量测值,进行组合卡尔曼滤波;经过卡尔曼滤波,利用得到的数据校正管道机器人的位置和速度,并将其反馈到捷联惯导***中。本发明提出了一种先进的磁偶极子定位算法,并利用此方法对捷联惯导定位***进行校正补偿,使得新的定位定姿***可以长时间高精度用于管道机器人的定位和定姿。
Description
技术领域
本发明涉及一种基于旋转磁场和惯导融合的管道机器人定位装置及方法,属于定位导航领域。
背景技术
随着管道运输业的发展,对管道缺陷检测技术的要求越来越高,尤其是管内探伤检测以及管内清理的需求日益增加。由于管道具有封闭、缺氧、空间狭小等特殊性,在这种恶劣情况下,难于利用人工方法完成管道缺陷的检测。在管道焊缝检测、城市供水管线检测以及石油输送管道检测等领域广泛地使用管道机器人技术。
石油、天然气等资源开发采用管道进行运输,因此,输油、输气管线的缺陷检测以及及时修复对石油、天然气等资源的开发具有重要意义。管道机器人在管道内移动并通过其携带的传感器对管线进行检测。当管道出现缺陷,需要人工进行处理时,操作人员必须知道机器人所处的准确位置,以便采取相应的措施。因此,在管道机器人的使用中,如何快速、准确的定位是管道机器人位置控制的关键问题。本发明介绍的基于旋转磁场和惯导融合的管道机器人定位方法就是一种先进的解决管道机器人定位定姿问题的方法。
《基于磁偶极子模型的管道机器人定位技术研究》(电波科学报,2006年8月,李军远,李盛凤,陈宏钧,张晓华):分析了低频电磁波发射天线的磁场分布,为了便于工程实现将发射天线简化为磁偶极子模型,给出了磁偶极子磁场分布的数值计算结果。结果表明,垂直于发射天线轴线方向的磁感应强度幅值随位置的改变呈双峰变化的规律,由此提出了一种基于低频电磁波磁场幅值信号双峰变化规律的管道机器人定位方法。低频电磁波发射天线以平行于管道轴线方向安装在管道机器人上;而低频电磁波接收天线垂直于发射天线安装在海上作业的救援装置上。在定位的过程中,通过检测接收天线检测到信号的强度变化,找到符合双峰变化规律的信号位置,从而达到定位效果。《基于磁偶极子模型的管道机器人定位技术研究》一文中提到的定位技术的实现存在很大的困难。在实际应用中,管道机器人要巡查的管道是很长的,文中提到的定位技术只能在距离较近(文中做实验的距离为2m以内)的位置才能有信号,这样很难快速的进行定位。文中的定位技术依靠的是寻找磁场呈双峰变化规律的位置,在实验室环境下,干扰很少,因此比较容易找到正确的位置,在实际操作中,外界干扰强烈,很有可能导致无法找到磁场呈双峰变化规律的位置。实验中,救援装置和发射天线两者水平线的距离恒定为一米,在实际操作中,管道埋在地下、海中,这就导致在操作过程中,很难保证救援装置和发射天线的距离很近,因此很难利用该技术进行实际操作。
《基于捷联惯导的变压器内微缆机器人定位研究》(现代电子技术,2018年10月,赵晓军,田粮川,孙文博,刘阳):设计了一种基于捷联惯导的大型变压器内微缆机器人定位***。该***搭载加速度传感器、陀螺仪传感器、摄像头等惯性测量单元,运用四阶龙格库塔法来解算姿态矩阵,结合卡尔曼滤波算法融合加速度计传感器数据,对姿态矩阵数据进行修正,从而确定机器人运动的速度及位置,同时摄像头可以对机器人的定位进行定时矫正,以保证位置的精确性。实验数据表明,该***自主性强,短时间内导航精度高,完全符合检测***在大型变压器内的定位需求。《基于捷联惯导的变压器内微缆机器人定位研究》一文中介绍了利用捷联惯导***进行姿态解算和定位,但是利用惯组进行定位会有随机误差,而随机误差的累积会使得定位的误差很大,因此该方法只适用于短距离定位。
与上述定位方法相比,本申请提出了一种基于捷联惯导***和人工磁场的定位方法,利用人工磁场的精确定位对惯组进行补偿,实现长时间长距离的管道内定位。同时,本申请的磁偶极子定位算法与前文提到的方法相比,精度更高,理论更完善,实际应用更方便。
发明内容
本发明的目的是为了解决上述现有技术存在的问题,进而提供一种基于旋转磁场和惯导融合的管道机器人定位装置及方法。
本发明的目的是通过以下技术方案实现的:
基于旋转磁场和惯导融合的管道机器人定位装置,所述基于旋转磁场和惯导融合的管道机器人定位装置包括捷联惯导***、磁传感器、管道机器人和磁偶极子,管道机器人位于管道内,管道机器人上设置有捷联惯导***和磁传感器,管道内壁上设置有两个磁偶极子。
基于旋转磁场和惯导融合的管道机器人定位方法,具体步骤为:
步骤一:根据磁偶极子定位的原理确定定位对象的坐标x、y;
步骤二:通过管道机器人定位定姿算法得到管道机器人的位置和速度信息;
步骤三:将定位机器人的位置和速度信息与磁偶极子定位得到的位置和速度信息作差得到误差;
步骤四:将得到的速度和位置误差作为量测值,进行组合卡尔曼滤波;
步骤五:经过卡尔曼滤波,利用得到的数据校正管道机器人的位置和速度,并将其反馈到捷联惯导***中。
本发明基于旋转磁场和惯导融合的管道机器人定位方法,所述步骤一的具体计算过程为:
假设定位对象仅在x-y平面中移动,因此可以仅使用两个坐标描述其位置:
将式(1)带入毕奥萨伐尔定律中得:
其中,M1为两极子的磁矩,M2同理;
B2同理;用a1和a2定义两个轴向磁感应强度之间的比例
联立上述公式可以得到只与a1、a2有关的x、y的表达式,R表示两信标之间的距离,
本发明基于旋转磁场和惯导融合的管道机器人定位方法,所述步骤二具体计算过程为:
惯导***各参数的初始化
R=6371000m,g0=9.8m/s2,ωie=7.292×10-5s-1
λ(1)=λ0,h(1)=h0
Q(1)=Q0,Vr(1)=[VE0 VN0 Vξ0]
各参数定义分别为:R——地球的半径;g0——重力加速度;ωie——地球的自转角速度;λ(1),h(1)——初始位置;Q(1)——初始四元数;Vr(1)——初始速度;
假设陀螺仪的输出是
Δθ=[Δθ xΔθy Δθz] (7)
其中,Δθx、Δθy、Δθz分别表示三个陀螺仪在一段时间内的输出;
更新四元数
此处,为GN-1→BN的四元数;
角速度计算
更新地理坐标系
Δγ0≈||ωig||ΔT (11)
其中,n为地理坐标系中各方向角速度的单位矢量,地理坐标系中GN-1→GN的四元数;
加速度输出比力
fb=[fx fy fz] (14)
比力变换
其中,为地理坐标系的真实加速度。
计算位置速度信息
本发明基于旋转磁场和惯导融合的管道机器人定位方法,所述步骤四具体计算过程为:
组合卡尔曼滤波的参数如下
人工磁场和捷联惯导组合***的状态方程和量测方程为
Xk=φk|k-1Xk-1+Γk-1Wk-1 (21)
Zk=[Z1 Z2 Z3 Z4]=HkXk+Vk (22)
利用惯导无阻尼误差方程,并将***的陀螺和加速度计的漂移扩充为状态作为***的状态方程,其状态变量X为18维列矢量:
其中,φE,φN,φζ分别表示在地理坐标系三个轴方向的误差角;δVE,δVN,δVζ为东、北、天三个方向的速度误差;δL,δλ,δh为纬度、经度、高度的误差;εbx,εby,εbz为东、北、天三个陀螺漂移中的随机常值;εrx,εry,εrz为东、北、天三个陀螺漂移中的一阶马尔柯夫过程;▽x,▽y,▽z为东、北、天三个加速度计的误差;
Wk-1为随机过程噪声;Vk为随机观测噪声;
***的状态转移矩阵为:
其中:
F(2,7)=-ωiesinL,
F(4,2)=-fζ
F(4,3)=-fζ,
F(5,1)=fζ,F(5,3)=-fE,
F(6,1)=-fN
F(6,2)=-fE,
F(6,7)=-2VEωie sinL,
F(9,6)=1,
其中,Tr,Ta为相关时间;FN为对应9个基本导航参数的***阵,其余元素为0;L为纬度;
上述式中用到的参数值如下:
RM=Re(1-2f+3fsin2L),RN=Re(1+fsin2L),Re=63718137m,f=1/298.257
***的噪声输入矩阵为:
其中:
量测方程的变量Z1,Z2,Z3,Z4是平面内速度和位置的误差,因此H是一个4*18的矩阵其中,H(1,4)=1,H(2,5)=1,H(3,7)=RM,H(4,8)=RNcosL;
***的观测噪声为:
Vk=[V1 V2 NN NE]T
其中,V1~N{0,R1},V2~N{0,R2},NN、NE指磁传感器沿东、北方向的位置误差;
根据以上定义,直接给出随机线性离散卡尔曼滤波的基本方程:
1)状态一步预测:
2)状态估计
3)滤波增益矩阵
4)一步预测协方差矩阵
5)滤波估计协方差矩阵
Pk=(I-KkHk)Pk/k-1 (27)
式中:
——k时刻的状态变量后验估计;
——k-1时刻到k时刻的一步预测估计,即状态变量的先验估计;
Pk——k时刻的滤波估计协方差矩阵;
Kk——k时刻的滤波增益矩阵;
Pk/k-1——一步预测估计的协方差矩阵;
利用式(23)-(27)不断进行迭代,就可以完成对状态变量进行最优估计。
本发明基于旋转磁场和惯导融合的管道机器人定位装置及方法,提出了一种先进的磁偶极子定位算法,并利用此方法对捷联惯导定位***进行校正补偿,使得新的定位定姿***可以长时间高精度用于管道机器人的定位和定姿。
附图说明
图1为本发明中管道机器人的定位示意图。
图2为本发明中偶极子模型。
图3为本发明基于旋转磁场和惯导融合的管道机器人定位定姿算法示意图。
图中附图标记有:1为捷联惯导***;2为磁传感器;3为管道机器人;4为磁偶极子。
具体实施方式
下面将结合附图对本发明做进一步的详细说明:本实施例在以本发明技术方案为前提下进行实施,给出了详细的实施方式,但本发明的保护范围不限于下述实施例。
实施例一:如图1-3所示,本实施例所涉及的一种基于旋转磁场和惯导融合的管道机器人定位装置及方法,本申请提出了一种新的管道机器人定位方案,在使用捷联惯导***进行姿态解算的同时,利用磁偶极子进行定位,从而尽可能的抵消位置和速度上的误差,以达到长时长距高精度定位。定位示意图如下。
将缠有通电线圈的正方形框抽象成磁极子模型,基于磁偶极子的定位算法基础是毕奥-萨伐尔定律。在目标与磁极子近距离条件下,毕奥-萨伐尔定律描述了电流源与磁场之间的关系。
如图2所示,M1和M2分别为两极子的磁矩,频率分别为f1,f2。
假设定位对象仅在x-y平面中移动,因此可以仅使用两个坐标描述其位置:
将式(1)带入毕奥萨伐尔定律中得:
其中,M1为两极子的磁矩,M2同理;
B2同理;用a1和a2定义两个轴向磁感应强度之间的比例
联立上述公式可以得到只与a1、a2有关的x、y的表达式,R表示两信标之间的距离
通过上述推导可以看出,只要测得x、y两个轴的磁感应强度即可通过本算法求得对象的位置。经过实际的实验,该定位方法的定位精度跟磁场的强度有很大关系,在磁场强度能达到稳定的1量级以上时,能准确的确定位置。如果应用到管道里,使用10cm×10cm的线圈,在±6m范围内定位误差能控制在10cm以下。因此,依靠其高精度的定位功能可以有效的补偿惯导***的累积误差,从而达到管道机器人的高精度定姿定位。
以上就是磁偶极子定位的原理,将其与捷联惯导***结合使用,可以大大提高***的定位精度,并使得惯导***可以用于管道机器人的长时间定位定姿中。具体的管道机器人定位定姿算法如图3所示。
具体算法流程如下
惯导***各参数的初始化
R=6371000m,g0=9.8m/s2,ωie=7.292×10-5s-1
λ(1)=λ0,h(1)=h0
Q(1)=Q0,Vr(1)=[VE0 VN0 Vξ0]
各参数定义分别为:R——地球的半径;g0——重力加速度;ωie——地球的自转角速度;λ(1),h(1)——初始位置;Q(1)——初始四元数;Vr(1)——初始速度。
假设陀螺仪的输出是
Δθ=[Δθx Δθy Δθz] (7)
其中,Δθx、Δθy、Δθz分别表示三个陀螺仪在一段时间内的输出。
更新四元数
此处,为GN-1→BN的四元数。
角速度计算
更新地理坐标系
Δγ0≈||ωig||ΔT (11)
其中,n为地理坐标系中各方向角速度的单位矢量,地理坐标系中GN-1→GN的四元数。
加速度输出比力
fb=[fx fy fz] (14)
比力变换
其中,为地理坐标系的真实加速度。
计算位置速度信息
用得到的位置和速度信息与磁偶极子定位得到的位置和速度信息作差得到误差,将得到的速度和位置误差作为量测值,进行组合卡尔曼滤波。从而估计惯导***的误差,然后利用估计的误差对惯导***进行校正。
组合卡尔曼滤波的参数如下
人工磁场和捷联惯导组合***的状态方程和量测方程为
Xk=φk|k-1Xk-1+Γk-1Wk-1 (21)
Zk=[Z1 Z2 Z3 Z4]=HkXk+Vk (22)
在此利用惯导无阻尼误差方程,并将***的陀螺和加速度计的漂移扩充为状态作为***的状态方程。其状态变量X为18维列矢量:
其中,φE,φN,φζ分别表示在地理坐标系三个轴方向的误差角;δVE,δVN,δVζ为东、北、天三个方向的速度误差;δL,δλ,δh为三个方向的位置误差;εbx,εby,εbz为东、北、天三个陀螺漂移中的随机常值;εrx,εry,εrz为东、北、天三个陀螺漂移中的一阶马尔柯夫过程;▽x,▽y,▽z为东、北、天三个加速度计的误差。
Wk-1为随机过程噪声;Vk为随机观测噪声;
***的状态转移矩阵为:
其中:
F(2,7)=-ωiesinL,
F(4,2)=-fζ
F(4,3)=-fζ,
F(5,1)=fζ,F(5,3)=-fE,
F(6,1)=-fN
F(6,2)=-fE,
F(6,7)=-2VEωie sinL,
其中,Tr,Ta为相关时间;FN为对应9个基本导航参数的***阵,其余元素为0;L为纬度。
上述式中用到的参数值如下:
RM=Re(1-2f+3fsin2L),RN=Re(1+fsin2L),Re=63718137m,f=1/298.257
***的噪声输入矩阵为:
其中:
量测方程的变量Z1,Z2,Z3,Z4是平面内速度和位置的误差,因此H是一个4*18的矩阵其中,H(1,4)=1,H(2,5)=1,H(3,7)=RM,H(4,8)=RNcosL;
***的观测噪声为:
Vk=[V1 V2 NN NE]T
其中,V1~N{0,R1},V2~N{0,R2},NN、NE指磁传感器沿东、北方向的位置误差。
根据以上定义,直接给出随机线性离散卡尔曼滤波的基本方程:
1)状态一步预测:
2)状态估计
3)滤波增益矩阵
4)一步预测协方差矩阵
5)滤波估计协方差矩阵
Pk=(I-KkHk)Pk/k-1 (27)
式中:
——k时刻的状态变量后验估计;
——k-1时刻到k时刻的一步预测估计,即状态变量的先验估计;
Pk——k时刻的滤波估计协方差矩阵;
Kk——k时刻的滤波增益矩阵;
Pk/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同理;
B2同理;用a1和a2定义两个轴向磁感应强度之间的比例
联立上述公式可以得到只与a1、a2有关的x、y的表达式,R表示两信标之间的距离
4.根据权利要求2所述的基于旋转磁场和惯导融合的管道机器人定位方法,其特征在于,所述步骤二具体计算过程为:
惯导***各参数的初始化
R=6371000m,g0=9.8m/s2,ωie=7.292×10-5s-1
λ(1)=λ0,h(1)=h0
Q(1)=Q0,Vr(1)=[VE0 VN0 Vξ0]
各参数定义分别为:R——地球的半径;g0——重力加速度;ωie——地球的自转角速度;λ(1),h(1)——初始位置;Q(1)——初始四元数;Vr(1)——初始速度;
假设陀螺仪的输出是
Δθ=[Δθx Δθy Δθz] (7)
其中,Δθx、Δθy、Δθz分别表示三个陀螺仪在一段时间内的输出;
更新四元数
此处,为GN-1→BN的四元数;
角速度计算
更新地理坐标系
Δγ0≈||ωig||ΔT (11)
其中,n为地理坐标系中各方向角速度的单位矢量,地理坐标系中GN-1→GN的四元数;
加速度输出比力
fb=[fx fy fz] (14)
比力变换
其中,为地理坐标系的真实加速度。
计算位置速度信息
5.根据权利要求2所述的基于旋转磁场和惯导融合的管道机器人定位方法,其特征在于,所述步骤四具体计算过程为:
组合卡尔曼滤波的参数如下
人工磁场和捷联惯导组合***的状态方程和量测方程为
Xk=φk|k-1Xk-1+Γk-1Wk-1 (21)
Zk=[Z1 Z2 Z3 Z4]=HkXk+Vk (22)
利用惯导无阻尼误差方程,并将***的陀螺和加速度计的漂移扩充为状态作为***的状态方程,其状态变量X为18维列矢量:
其中,φE,φN,φζ分别表示在地理坐标系三个轴方向的误差角;δVE,δVN,δVζ为东、北、天三个方向的速度误差;δL,δλ,δh为纬度、经度、高度的误差;εbx,εby,εbz为东、北、天三个陀螺漂移中的随机常值;εrx,εry,εrz为东、北、天三个陀螺漂移中的一阶马尔柯夫过程;为东、北、天三个加速度计的误差;
Wk-1为随机过程噪声;Vk为随机观测噪声;
***的状态转移矩阵为:
其中:
F(2,7)=-ωiesinL,
F(4,2)=-fζ
F(4,3)=-fζ,
F(5,1)=fζ,F(5,3)=-fE,
F(6,1)=-fN
F(6,2)=-fE,
F(6,7)=-2VEωie sinL,
F(9,6)=1,
其中,Tr,Ta为相关时间;FN为对应9个基本导航参数的***阵,其余元素为0;L为纬度;
上述式中用到的参数值如下:
RM=Re(1-2f+3fsin2L),RN=Re(1+fsin2L),Re=63718137m,f=1/298.257
***的噪声输入矩阵为:
其中:
量测方程的变量Z1,Z2,Z3,Z4是平面内速度和位置的误差,因此H是一个4*18的矩阵其中,H(1,4)=1,H(2,5)=1,H(3,7)=RM,H(4,8)=RNcosL;
***的观测噪声为:
Vk=[V1 V2 NN NE]T
其中,V1~N{0,R1},V2~N{0,R2},NN、NE指磁传感器沿东、北方向的位置误差;
根据以上定义,直接给出随机线性离散卡尔曼滤波的基本方程:
1)状态一步预测:
2)状态估计
3)滤波增益矩阵
4)一步预测协方差矩阵
5)滤波估计协方差矩阵
Pk=(I-KkHk)Pk/k-1 (27)
式中:
——k时刻的状态变量后验估计;
——k-1时刻到k时刻的一步预测估计,即状态变量的先验估计;
Pk——k时刻的滤波估计协方差矩阵;
Kk——k时刻的滤波增益矩阵;
Pk/k-1——一步预测估计的协方差矩阵;
利用式(23)-(27)不断进行迭代,就可以完成对状态变量进行最优估计。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910764500.5A CN110440796A (zh) | 2019-08-19 | 2019-08-19 | 基于旋转磁场和惯导融合的管道机器人定位装置及方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910764500.5A CN110440796A (zh) | 2019-08-19 | 2019-08-19 | 基于旋转磁场和惯导融合的管道机器人定位装置及方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN110440796A true CN110440796A (zh) | 2019-11-12 |
Family
ID=68436378
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910764500.5A Pending CN110440796A (zh) | 2019-08-19 | 2019-08-19 | 基于旋转磁场和惯导融合的管道机器人定位装置及方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110440796A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111504306A (zh) * | 2020-06-17 | 2020-08-07 | 哈尔滨工业大学 | 一种基于惯性导航的定位方法、装置及*** |
CN116429096A (zh) * | 2023-02-22 | 2023-07-14 | 中国科学院国家空间科学中心 | 一种地磁惯性融合导航方法及*** |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104181572A (zh) * | 2014-05-22 | 2014-12-03 | 南京理工大学 | 一种弹载惯性/卫星紧组合导航方法 |
CN104567871A (zh) * | 2015-01-12 | 2015-04-29 | 哈尔滨工程大学 | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 |
CN109073388A (zh) * | 2016-04-11 | 2018-12-21 | Hrl实验室有限责任公司 | 旋磁地理定位*** |
CN109579836A (zh) * | 2018-11-21 | 2019-04-05 | 阳光凯讯(北京)科技有限公司 | 一种基于mems惯性导航的室内行人方位校准方法 |
CN109633491A (zh) * | 2019-01-23 | 2019-04-16 | 中国科学院上海微***与信息技术研究所 | 全张量磁梯度测量***安装误差的标定装置及标定方法 |
US20190170516A1 (en) * | 2016-04-11 | 2019-06-06 | The Regents Of The University Of Michigan | Magnetic beacon and inertial sensor localization technology |
CN109883415A (zh) * | 2019-03-01 | 2019-06-14 | 哈尔滨工业大学 | 一种基于三角函数拟合的旋转磁场定位方法 |
-
2019
- 2019-08-19 CN CN201910764500.5A patent/CN110440796A/zh active Pending
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104181572A (zh) * | 2014-05-22 | 2014-12-03 | 南京理工大学 | 一种弹载惯性/卫星紧组合导航方法 |
CN104567871A (zh) * | 2015-01-12 | 2015-04-29 | 哈尔滨工程大学 | 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法 |
CN109073388A (zh) * | 2016-04-11 | 2018-12-21 | Hrl实验室有限责任公司 | 旋磁地理定位*** |
US20190170516A1 (en) * | 2016-04-11 | 2019-06-06 | The Regents Of The University Of Michigan | Magnetic beacon and inertial sensor localization technology |
CN109579836A (zh) * | 2018-11-21 | 2019-04-05 | 阳光凯讯(北京)科技有限公司 | 一种基于mems惯性导航的室内行人方位校准方法 |
CN109633491A (zh) * | 2019-01-23 | 2019-04-16 | 中国科学院上海微***与信息技术研究所 | 全张量磁梯度测量***安装误差的标定装置及标定方法 |
CN109883415A (zh) * | 2019-03-01 | 2019-06-14 | 哈尔滨工业大学 | 一种基于三角函数拟合的旋转磁场定位方法 |
Non-Patent Citations (3)
Title |
---|
朱托: "GPS/SINS组合导航与姿态测量的设计", 《中国优秀硕士学位论文全文数据库 基础科学辑》 * |
杨永刚: "亚轨道飞行器导航***方案研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 * |
王冠 等: "基于低频磁信标的全天时自主定位方法", 《导航定位与授时》 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111504306A (zh) * | 2020-06-17 | 2020-08-07 | 哈尔滨工业大学 | 一种基于惯性导航的定位方法、装置及*** |
CN116429096A (zh) * | 2023-02-22 | 2023-07-14 | 中国科学院国家空间科学中心 | 一种地磁惯性融合导航方法及*** |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US9285222B2 (en) | Autonomous vehicle power line position and load parameter estimation | |
CN106225784B (zh) | 基于低成本多传感器融合行人航位推算方法 | |
CN105066917B (zh) | 一种小型管道地理信息***测量装置及其测量方法 | |
US8005635B2 (en) | Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS) | |
Renaudin et al. | New method for magnetometers based orientation estimation | |
CN101476860B (zh) | 一种高背景磁场下的磁定位方法及装置 | |
Li et al. | Magnetic sensors for navigation applications: an overview | |
CN102288170B (zh) | 一种水下航行器内电子罗盘的校正方法 | |
CN109883415B (zh) | 一种基于三角函数拟合的旋转磁场定位方法 | |
CN103712622B (zh) | 基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置 | |
Wahdan et al. | Three-dimensional magnetometer calibration with small space coverage for pedestrians | |
WO2015188396A1 (zh) | 一种空地一体的地磁场联合观测方法及*** | |
CN109073388B (zh) | 旋磁地理定位*** | |
CN103712621B (zh) | 偏振光及红外传感器辅助惯导***定姿方法 | |
CN110440796A (zh) | 基于旋转磁场和惯导融合的管道机器人定位装置及方法 | |
CN104049269A (zh) | 一种基于激光测距和mems/gps组合导航***的目标导航测绘方法 | |
CN109073380A (zh) | 自校准且自主的地磁观测台 | |
Healey et al. | Online compensation of heading sensor bias for low cost AUVs | |
Küchler et al. | Real-time estimation of a ship's attitude | |
Chen et al. | An adaptive calibration approach for a 2-axis digital compass in a low-cost pedestrian navigation system | |
Huang et al. | Study on INS/DR integration navigation system using EKF/RK4 algorithm for underwater gliders | |
WO2013139486A1 (en) | True north seeking and attitude system | |
Včelák et al. | Long-range magnetic tracking system | |
Ye et al. | Experiment evaluation of rapid error compensation for magnetic compass in underwater vehicle | |
Kluga et al. | Magnetometer error models of low-cost land vehicle navigation system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20191112 |
|
RJ01 | Rejection of invention patent application after publication |