CN113670297A - 一种基于mems和轮式里程计的离线定位方法 - Google Patents

一种基于mems和轮式里程计的离线定位方法 Download PDF

Info

Publication number
CN113670297A
CN113670297A CN202110967908.XA CN202110967908A CN113670297A CN 113670297 A CN113670297 A CN 113670297A CN 202110967908 A CN202110967908 A CN 202110967908A CN 113670297 A CN113670297 A CN 113670297A
Authority
CN
China
Prior art keywords
pipeline
coordinate system
navigation
speed
starting point
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
Application number
CN202110967908.XA
Other languages
English (en)
Inventor
闫雪娇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Aerospace System Engineering Institute
Original Assignee
Shanghai Aerospace System Engineering Institute
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shanghai Aerospace System Engineering Institute filed Critical Shanghai Aerospace System Engineering Institute
Priority to CN202110967908.XA priority Critical patent/CN113670297A/zh
Publication of CN113670297A publication Critical patent/CN113670297A/zh
Pending legal-status Critical Current

Links

Images

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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于MEMS和轮式里程计的离线定位方法S1：在待测管道起点静置20s，得到起点位置信息；S2：沿管道运动，实时采集并分别得到三轴加速度和三轴角速度，基于三轴加速度和三轴角速度解算得到姿态信息；S3：沿管道运动，实时采集并进行速度解算得到载体坐标系下的前向速度；S4：将姿态信息和前向速度进行数据融合，得到导航坐标系下的导航速度，基于导航速度得到组合导航管道轨迹；S5：当运动停止，测量并得到终点位置信息；S6：基于起点位置信息和终点位置信息和组合导航管道轨迹对组合导航管道轨迹进行外置矫正，得到最终管道轨迹。本发明不需要外部信息辅助，通过管道首尾点坐标对定位轨迹校正，定位方法简单，精度高，成本低，适用范围广。

Description

一种基于MEMS和轮式里程计的离线定位方法
技术领域
本发明属于电子仪器测量导航领域，尤其涉及一种基于MEMS和轮式里程计的离线定位方法。
背景技术
惯导/里程计组合导航管道定位方法与纯惯导和视觉同步定位与画图等管道定位方法相比，有着精度高、成本低和适用范围广等优点，因此惯导/轮式里程计组合导航管道定位方法已经越来越多的应用于各种地下管道检测任务。
惯性导航系统(inertial navigation system，INS，以下简称惯导)因其不易受到外界干扰、实时性好的优点，是管道测量机器人中最主要的导航系统。惯导系统的主要传感器为陀螺仪和加速度计，陀螺仪采集载体角速度信息，加速度计采集载体加速度信息，通过积分运算得到载体的速度、位置、姿态信息。
惯导/里程计组合导航管道定位方法可以有效的降低管道机器人成本，广泛应用于地下管道检测任务。惯导/里程计组合导航管道定位方法是指通过轮式里程计信息来抑制惯导随时间累积的误差，从而获得较高精度的导航方法。
《传感技术学报》2016年1月第29卷第1期《采用MEMS惯导的小口径管道内检测定位方案可行性研究》设计了一种融合了MEMS惯导、管道里程计、地面标记器以及管道运动约束的组合定位方案及其导航定位算法，定位方法复杂，且成本高。《石油学报》2014年9月第35卷第5期《基于多传感器数据融合的长输埋地管道中心测量》针对长距离油气管道的中心测量问题，提出一种利用多传感器数据融合的管道中心线计算方法，以惯导、里程计和地面GPS标记盒组合进行管道机器人定位。《导航与控制》2015年10月第14卷第5期《惯性/里程计组合导航技术在管道定位中的应用》利用里程计解算出的速度作为观测量与惯导数据进行卡尔曼滤波融合。《IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY》2020年3月第28卷第2期《Inertial Navigation System of Pipeline Inspection Gauge》提出一种基于高精度的三维简化惯性传感器系统(3-D reduced inertial sensor system，RISS)/里程计组合导航管道定位方法。公开号为CN201910316184.5的申请专利《一种基于MEMS和里程计组合的地下管线探测方法》中，需要往返测试三组管道机器人运动数据平均值来计算管道轨迹。
以上文献均没有充分利用易于获得的管道首尾两点坐标，文献中或是仅惯导/里程计组合进行管道轨迹测量，或是设计复杂且成本高的辅助信息对惯导误差进行校正，其定位方法难实现且适用范围小。
发明内容
本发明的技术目的是提供一种基于MEMS和轮式里程计的离线定位方法，以实现简单且快速地实现定位。
为解决上述问题，本发明的技术方案为：
一种基于MEMS和轮式里程计的离线定位方法，包括如下步骤：
S1：将管道机器人在待测管道起点静置20s，测量起点位置，得到起点位置信息。S2：沿管道运动，实时采集三轴加速度计数据和三轴陀螺仪数据，分别得到三轴加速度和三轴角速度，基于三轴加速度和三轴角速度解算得到姿态信息。S3：沿管道运动，实时采集里程计数据，并进行速度解算得到载体坐标系下的前向速度。S4：将姿态信息和前向速度进行数据融合，得到导航坐标系下的导航速度，基于导航速度得到组合导航管道轨迹。S5：当管道机器人运动停止，测量终点位置，得到终点位置信息。S6：基于起点位置信息和终点位置信息进行计算得到终点相对于起点的实际方位角和实际俯仰角。
基于实际方位角、实际俯仰角和组合导航管道轨迹计算得到方位角误差和俯仰角误差，基于方位角误差和俯仰角误差得到旋转矩阵，根据旋转矩阵对组合导航管道轨迹进行外置矫正，得到最终管道轨迹。
具体地，在步骤S1中，获得起点位置信息为通过GPS采集得到起点的经度、纬度和高度。
其中，步骤S2中具体包括如下步骤
S21：实时采集三轴加速度计数据得到三轴加速度
Figure BDA0003224591930000031
其中，
Figure BDA0003224591930000032
分别为三轴加速度计测量的加速度在载体坐标系三轴上的分量。实时采集三轴陀螺仪数据得到三轴角速度
Figure BDA0003224591930000033
其中，
Figure BDA0003224591930000034
分别为三轴陀螺仪测量的载体坐标系相对惯性坐标系的角速度在载体坐标系三轴上的分量。
载体坐标系以管道机器人前进方向为y轴，以前进方向顺时针旋转90度为x轴,以向上方向为z轴；
S22：基于地球自转角速率ωie以及当前的地理纬度L，得到地球自转角速率在地理坐标系下的投影
Figure BDA0003224591930000035
公式如下
Figure BDA0003224591930000036
S23：基于管道机器人的东向速度ve、北向速度vn、地球半径Re以及当前的地理纬度L，得到管道机器人的运动引起的地理坐标系相对地球坐标系得角速度
Figure BDA0003224591930000037
公式如下
Figure BDA0003224591930000038
S24：根据投影
Figure BDA0003224591930000039
三轴陀螺仪的角速率
Figure BDA00032245919300000310
以及第k-1次采样时刻的捷联矩阵T,第k次为当前采样时刻，计算得到地理坐标系相对于载体坐标系的角速率
Figure BDA00032245919300000311
公式如下
Figure BDA00032245919300000312
S25：根据角速率
Figure BDA00032245919300000313
采样周期t以及第k-1次采样时刻的四元数Qk-1＝[q0k-1q1k-1 q2k-1 q3k-1]T,得到第k次采样时刻的四元数Qk＝[q0k q1k q2k q3k]T，计算公式如下
Figure BDA00032245919300000314
其中，四元数
Figure BDA0003224591930000041
表示一个坐标系相对另一个坐标系的旋转，标量部分表示转角的一半余弦值，而矢量部分表示瞬时转轴的方向、瞬时转轴于与参考坐标系之间的方向余弦值。
S26：基于步骤S25中的第k次采样时刻的四元数，得到第k次采样时刻的矩阵T，计算公式如下
Figure BDA0003224591930000042
S27：基于第k次采样时刻的捷联矩阵T得到对水平姿态角及方位角实现更新，计算公式如下：
θ＝arcsin(T32)
Figure BDA0003224591930000043
Figure BDA0003224591930000044
其中θ、φ和
Figure BDA0003224591930000045
分别为俯仰角、翻滚角和方位角。Tij(i,j＝1,2,3)表示捷联矩阵T的第i行第j列元素。
其中，步骤S3中具体包括如下步骤
S31：通过安装一对转速测量单元于管道机器人的左、右轮上，分别实时获取左、右转速al、ar。
S32：获取管道机器人的左、右轮的周长为S,得到左轮速度、右轮速度vl＝alS,vr＝arS。
S33：基于左轮速度和右轮速度，得到管道机器人的前向速度
Figure BDA0003224591930000046
具体地，在步骤S4中，得到导航速度的计算公式为
Figure BDA0003224591930000047
其中，ve为导航坐标系下的东向速度、vn为导航坐标系下的北向速度、vu为导航坐标系下的天向速度。
基于导航速度以及管道机器人k-1时刻的位置，得到k时刻的管道机器人位置，公式如下
Xk＝Xk-1+Vnt
其中，Vn＝[ve vn vu]T，t为采样周期，基于k时刻的管道机器人位置，以此得到组合导航管道轨迹。
具体地，在步骤S5中，获得终点位置信息为通过GPS采集得到终点的经度、纬度和高度。
其中，步骤S6具体包括如下步骤
S61：基于起点位置信息和终点位置信息进行计算得到终点相对于起点的实际方位角
实际方位角计算公式为
Figure BDA0003224591930000051
其中，xe，ye分别为实际管道的终点东向和北向位置，xs，ys分别为实际管道起点的东向和北向位置。
基于组合导航管道轨迹计算得到解算方位角，计算公式如下
Figure BDA0003224591930000052
其中，xes，yes为组合导航管道轨迹的终点东向和北向位置，xss，yss为组合导航管道轨迹的起始点东向和北向位置。
S62：基于实际方位角和解算方位角，得到补偿方位角，计算公式如下
Figure BDA0003224591930000053
S63：基于起点位置信息和终点位置信息进行计算得到终点相对于起点的实际俯仰角，实际俯仰角计算公式为
θt＝arctan[(ze-zs)/(xe-xs)]
其中，xe，ze为实际管道的终点东向和天向位置，xs，zs为实际管道起始点的东向和天向位置。
基于组合导航管道轨迹计算得到解算俯仰角，计算公式如下
θs＝arctan[(zes-zss)/(xes-xss)]。
其中，xes，zes为组合导航管道轨迹的终点东向和北向位置，xss，zss为组合导航管道轨迹的起始点东向和北向位置。
S64：基于实际俯仰角和解算俯仰角，得到补偿俯仰角，计算公式如下Δθ＝θt-θs。
S65：基于补偿方位角和补偿俯仰角，得到旋转矩阵，计算公式如下
Figure BDA0003224591930000061
S66：基于旋转矩阵对组合导航管道轨迹进行矫正，得到最终管道轨迹。
本发明由于采用以上技术方案，使其与现有技术相比具有以下的优点和积极效果：
本发明设置提出一种基于MEMS和轮式里程计的离线定位方法，通过实际管道首尾点坐标对MEMS和轮式里程计解算出的轨迹进行校正，从而获得最终管道轨迹。其优点在于：(1)不需要外部信息辅助。(2)通过管道首尾点坐标对惯导/里程计定位轨迹校正。(3)定位方法简单，精度高，成本低，适用范围广。(4)一方面传感器静态数据采集时间较短(约为20s)，另一方面只需要采集一组单程运动过程就可以计算出管道轨迹，数据采集简单易操作。
附图说明
通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。
图1为本发明的一种基于MEMS和轮式里程计的离线定位方法的流程示意图；
图2为本发明的一种基于MEMS和轮式里程计的离线定位方法的原理框图；
图3为本发明第一段搭建管道解算后的俯视图和剖面图；
图4为本发明第二段搭建管道第一组数据解算后的俯视图和剖面图；
图5为本发明第二段搭建管道第二组数据解算后的俯视图和剖面图；
图6为本发明第二段搭建管道第三组数据解算后的俯视图和剖面图。
具体实施方式
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施方式。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施方式。
为使图面简洁，各图中只示意性地表示出了与本发明相关的部分，它们并不代表其作为产品的实际结构。另外，以使图面简洁便于理解，在有些图中具有相同结构或功能的部件，仅示意性地绘示了其中的一个，或仅标出了其中的一个。在本文中，“一个”不仅表示“仅此一个”，也可以表示“多于一个”的情形。
以下结合附图和具体实施例对本发明提出的一种基于MEMS和轮式里程计的离线定位方法作进一步详细说明。根据下面说明和权利要求书，本发明的优点和特征将更清楚。
实施例
参看图1和图2，本实施例提供一种基于MEMS和轮式里程计的离线定位方法，首先需要说明的是，本实施例实施需要通过管道机器人进行测量，对管道机器人的结构不进行限定，管道机器人上搭载三轴加速度计和三轴陀螺仪(MEMS)以及轮式里程计对相应参数进行采集，并进行数据处理得到相对应的轨迹，该轨迹即为待测管道的排布。采集并数据处理具体包括如下步骤。
首先在步骤S1中，将管道机器人放置在待测管道起点即管口静置20s，以保证初始对准准确度。通过GPS采集得到得到起点位置信息，包括起点的经度、纬度和高度等信息。
接着，在步骤S2中，让管道机器人沿着管道进行运动，同时，实时采集三轴加速度计数据和三轴陀螺仪数据，分别得到三轴加速度和三轴角速度，基于三轴加速度和三轴角速度解算得到姿态信息。
步骤S2可具体拆分为如下步骤
在步骤S21中，实时采集三轴加速度计的数据得到三轴加速度
Figure BDA0003224591930000081
其中，
Figure BDA0003224591930000082
分别为三轴加速度计测量的加速度在载体坐标系三轴上的分量(单位为m/s2)。
实时采集三轴陀螺仪的数据得到三轴角速度
Figure BDA0003224591930000083
其中，
Figure BDA0003224591930000084
分别为三轴陀螺仪测量的载体坐标系相对惯性坐标系的角速度在载体坐标系三轴上的分量(单位为rad/s)。
在本实施例中，载体坐标系以管道机器人前进方向为y轴，以前进方向顺时针旋转90度为x轴,以向上方向为z轴，y轴和x轴所构成的平面与水平面相平行。
接着，在步骤S22中对角速度进行修正。基于地球自转角速率ωie以及当前的地理纬度L，得到地球自转角速率在地理坐标系下的投影
Figure BDA0003224591930000085
公式如下
Figure BDA0003224591930000086
然后进入步骤S23，将管道机器人的东向速度设为ve、北向速度设为vn、地球半径Re以及当前的地理纬度L，得到管道机器人的运动引起的地理坐标系相对地球坐标系得角速度
Figure BDA0003224591930000087
公式如下
Figure BDA0003224591930000088
接着，在步骤S24中，根据投影
Figure BDA0003224591930000089
步骤23得到的角速度
Figure BDA00032245919300000810
和三轴陀螺仪的角速率
Figure BDA00032245919300000811
以及第k-1次采样时刻的捷联矩阵T,计算得到地理坐标系相对于载体坐标系的角速率
Figure BDA00032245919300000812
公式如下
Figure BDA00032245919300000813
然后，在步骤S25则根据角速率
Figure BDA00032245919300000814
和k-1时刻的四元数Qk-1＝[q0k-1 q1k-1 q2k-1q3k-1]T,得到第k次采样时刻的四元数Qk＝[q0k q1k q2k q3k]T，计算公式如下
Figure BDA0003224591930000091
其中，t为采样周期，四元数
Figure BDA0003224591930000092
表示一个坐标系相对另一个坐标系的旋转，标量部分表示转角的一半余弦值，而矢量部分表示瞬时转轴的方向、瞬时转轴于与参考坐标系之间的方向余弦值。
后续基于步骤S25中的第k次采样时刻的四元数，得到捷联矩阵T，计算公式如下
Figure BDA0003224591930000093
最后，在步骤S27中，基于第k次采样时刻的捷联矩阵T得到对水平姿态角及方位角实现更新，计算公式如下：
θ＝arcsin(T32)
Figure BDA0003224591930000094
Figure BDA0003224591930000095
其中θ、φ和
Figure BDA0003224591930000096
分别为俯仰角、翻滚角和方位角。Tij(i,j＝1,2,3)表示捷联矩阵T的第i行第j列元素
参看图2，解算得到姿态信息的同时还会对前向速度进行获取，具体为步骤S3中，转速测量单元分别安装于管道机器人的左、右轮上，可分别测量并获取管道机器人的左、右轮两轮转速al、ar。具体地，
在步骤S31中，通过转速测量单元实时获取管道机器人的左、右轮两轮转速al、ar。接着在步骤S32中，事先获取管道机器人的左、右轮的周长为S,计算得到左轮速度、右轮速度分别为vl＝alS,vr＝arS。最后在步骤S33中：基于左轮速度和右轮速度，得到管道机器人的前向速度
Figure BDA0003224591930000101
参看图2，然后在步骤S4中将步骤S2姿态信息和步骤S3前向速度进行数据融合，得到导航坐标系下的导航速度，基于导航速度得到组合导航管道轨迹。
具体地，首先通过姿态信息和前向速度
Figure BDA0003224591930000102
得到的导航速度的计算公式为
Figure BDA0003224591930000103
其中，ve为管道机器人在导航坐标系下的东向速度、vn为管道机器人在导航坐标系下的北向速度、vu为管道机器人在导航坐标系下的天向速度。
基于导航速度以及管道机器人k-1时刻的位置，即可得到k时刻的管道机器人位置，公式如下
Xk＝Xk-1+Vnt
其中，Vn＝[ve vn vu]T，t为采样周期，基于各个时刻的管道机器人位置进行组合，便可得到组合导航管道轨迹，直至管道机器人在终点处停止运行。
在步骤S5中，当管道机器人运动停止即到达终点管口，通过GPS采集测量终点位置，得到终点的经度、纬度和高度。
在步骤S6中，由于管道机器人的初始对准的航向角不准，导致解算出来的机器人轨迹的方位角将与真实轨迹方位角和俯仰角有个恒定角度偏差，此时需要根据已知的惯导起始点和终止点对其进行校正。
首先，在步骤S61中，利用GPS获得的起点位置信息和终点位置信息进行计算得到终点相对于起点的实际方位角。
实际方位角计算公式为
Figure BDA0003224591930000104
其中，xe，ye分别为实际管道的终点东向和北向位置，xs，ys分别为实际管道起点的东向和北向位置。
另外，相对应的根据组合导航管道轨迹计算得到解算方位角，计算公式如下
Figure BDA0003224591930000111
其中，xes，yes为组合导航管道轨迹的终点东向和北向位置，xss，yss为组合导航管道轨迹的起始点东向和北向位置。
因此，在步骤S62中，基于实际方位角和解算方位角，得到补偿方位角，计算公式如下
Figure BDA0003224591930000112
即为方位角误差。
然后进入步骤S63，相类似地，基于起点位置信息和终点位置信息进行计算得到终点相对于起点的实际俯仰角，实际俯仰角计算公式为
θt＝arctan[(ze-zs)/(xe-xs)]
其中，xe，ze为实际管道的终点东向和天向位置，xs，zs为实际管道起始点的东向和天向位置。
同样，基于组合导航管道轨迹计算得到解算俯仰角，计算公式如下
θs＝arctan[(zes-zss)/(xes-xss)]。
其中，xes，zes为组合导航管道轨迹的终点东向和北向位置，xss，zss为组合导航管道轨迹的起始点东向和北向位置。
因此，在步骤S64中，基于实际俯仰角和解算俯仰角，得到补偿俯仰角，计算公式如下Δθ＝θt-θs，即为俯仰角误差。
然后在步骤S65中，将上述得到的补偿方位角和补偿俯仰角，得到组合导航管道轨迹与实际轨迹旋转矩阵，计算公式如下
Figure BDA0003224591930000113
最后基于旋转矩阵对组合导航管道轨迹进行矫正，得到最终管道轨迹，公式如下X＝CX-
其中X-表示校正前的组合导航轨迹，X表示校正后的轨迹。
现进行性能分析，让管道机器人采用上述方法，对其定位精度进行了测试。选取的测试数据分别为不同长度和形状的两组管道传感器数据，定位基准为管道中GPS单元的测量数据。
参看图3，搭建第一段管道进行检验
第一段管道中的管道长度为524m，管道直径为240mm，管道形状俯视图大致为直线形。
第一段管道起点坐标为东向：-27325.8345m；北向：-13798.3645m；高度：4.69m
第一段管道终点坐标：东向：-27451.7711m；北向：-13290.3226m；高度：3.62m
参看图4，搭建第二段管道进行检验
第二段管道中的管道长度为94m，管道直径为240mm，管道形状俯视图大致为“S”形。在不同的时间段获得第二段管道的测量数据。
第二段管道起点坐标：东向：-11.676800m；北向：0.888800m；高度：0.207700m
第二段管道终点坐标：东向：73.878400m；北向：17.591200m；高度：0.348400m
利用本实施例，得到陀螺仪误差标定结果。其中，图3的(a)、(b)图分别表示第一段管道经过本实施例获得轨迹与通过GPS采集的基准俯视图和剖面图，由图中可知第一段管道该组传感器数据的最大水平定位误差为0.8m，最大高程定位误差为0.4m。水平定位定位精度为0.8/524＝0.15％,高程定位精度为0.4/524＝0.07％；图4的(a)、(b)图分别表示第二段管道的第一组数据经过本实施例获得轨迹与GPS采集的基准俯视图和剖面图，由图中可知第二段管道的第一组传感器数据的最大水平定位误差为0.4m，其中直线水平定位误差为0.1m，最大高程定位误差为0.17m。直线水平定位定位精度为0.1/94＝0.1％,弯道水平定位精度为0.4/94＝0.42％,高程定位精度为0.17/94＝0.18％。图5的(a)、(b)图分别表示第二段管道的第二组数据通过本实施例方法获得轨迹与GPS采集的基准俯视图和剖面图，由图中可知第二段管道的第二组传感器数据的最大水平定位误差为0.5m，其中直线水平定位误差为0.12m，最大高程定位误差为0.16m。直线水平定位定位精度为0.12/94＝0.12％,弯道水平定位精度为0.5/94＝0.53％,高程定位精度为0.16/94＝0.17％。图6的(a)、(b)图分别表示第二段管道的第三组数据经过本实施例获得轨迹与GPS采集的基准俯视图和剖面图，由图中可知第二段管道的第三组传感器数据的最大水平定位误差为0.56m，其中直线水平定位误差为0.14m，最大高程定位误差为0.21m。直线水平定位定位精度为0.14/94＝0.14％,弯道水平定位精度为0.56/94＝0.59％,高程定位精度为0.18/94＝0.19％。试验结果表明：本实施例的算法导航稳定性好，管道直线水平定位精度小于0.14％，高程定位精度小于0.2％。实际应用中的油气管道多为耐腐蚀材质，常规管道(如水泥、金属、聚乙烯等材质)在浇筑时多为直线形，因此针对实际应用中的管道，本实施例提出的导航方法满足高精度管道定位的需求。
上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式。即使对本发明作出各种变化，倘若这些变化属于本发明权利要求及其等同技术的范围之内，则仍落入在本发明的保护范围之中。

Claims (7)

1.一种基于MEMS和轮式里程计的离线定位方法，其特征在于，包括如下步骤：
S1：将管道机器人在待测管道起点静置20s，测量起点位置，得到起点位置信息；
S2：沿管道运动，实时采集三轴加速度计数据和三轴陀螺仪数据，分别得到三轴加速度和三轴角速度，基于所述三轴加速度和所述三轴角速度解算得到姿态信息；
S3：沿管道运动，实时采集里程计数据，并进行速度解算得到载体坐标系下的前向速度；
S4：将所述姿态信息和所述前向速度进行数据融合，得到导航坐标系下的导航速度，基于所述导航速度得到组合导航管道轨迹；
S5：当管道机器人运动停止，测量终点位置，得到终点位置信息；
S6：基于所述起点位置信息和所述终点位置信息进行计算得到终点相对于起点的实际方位角和实际俯仰角；
基于所述实际方位角、所述实际俯仰角和所述组合导航管道轨迹计算得到方位角误差和俯仰角误差，基于所述方位角误差和所述俯仰角误差得到旋转矩阵，根据所述旋转矩阵对所述组合导航管道轨迹进行外置矫正，得到最终管道轨迹。
2.根据权利要求1所述的基于MEMS和轮式里程计的离线定位方法，其特征在于，在所述步骤S1中，获得所述起点位置信息为通过GPS采集得到起点的经度、纬度和高度。
3.根据权利要求1所述的基于MEMS和轮式里程计的离线定位方法，其特征在于，所述步骤S2中具体包括如下步骤
S21：实时采集三轴加速度计数据得到三轴加速度
Figure FDA0003224591920000011
其中，
Figure FDA0003224591920000012
分别为三轴加速度计测量的加速度在载体坐标系三轴上的分量；
实时采集三轴陀螺仪数据得到三轴角速度
Figure FDA0003224591920000013
其中，
Figure FDA0003224591920000014
分别为三轴陀螺仪测量的载体坐标系相对惯性坐标系的角速度在所述载体坐标系三轴上的分量；
所述载体坐标系以管道机器人前进方向为y轴，以前进方向顺时针旋转90度为x轴,以向上方向为z轴；
S22：基于地球自转角速率ωie以及当前的地理纬度L，得到地球自转角速率在地理坐标系下的投影
Figure FDA0003224591920000021
公式如下
Figure FDA0003224591920000022
S23：基于管道机器人的东向速度ve、北向速度vn、地球半径Re以及当前的地理纬度L，得到管道机器人的运动引起的地理坐标系相对地球坐标系得角速度
Figure FDA0003224591920000023
公式如下
Figure FDA0003224591920000024
S24：根据所述投影
Figure FDA0003224591920000025
三轴陀螺仪的角速率
Figure FDA0003224591920000026
以及第k-1次采样时刻的捷联矩阵T,第k次为当前采样时刻，计算得到地理坐标系相对于所述载体坐标系的角速率
Figure FDA0003224591920000027
公式如下
Figure FDA0003224591920000028
S25：根据所述角速率
Figure FDA0003224591920000029
采样周期t以及第k-1次采样时刻的四元数Qk-1＝[q0k-1 q1k-1q2k-1 q3k-1]T,得到第k次采样时刻的四元数Qk＝[q0k q1k q2k q3k]T，计算公式如下
Figure FDA00032245919200000210
其中，四元数
Figure FDA00032245919200000211
表示一个坐标系相对另一个坐标系的旋转，标量部分表示转角的一半余弦值，而矢量部分表示瞬时转轴的方向、瞬时转轴于与参考坐标系之间的方向余弦值；
S26：基于所述步骤S25中的第k次采样时刻的四元数，得到第k次采样时刻的矩阵T，计算公式如下
Figure FDA0003224591920000031
S27：基于所述第k次采样时刻的捷联矩阵T得到对水平姿态角及方位角实现更新，计算公式如下：
θ＝arcsin(T32)
Figure FDA0003224591920000032
Figure FDA0003224591920000033
其中θ、φ和
Figure FDA0003224591920000034
分别为俯仰角、翻滚角和方位角；Tij(i,j＝1,2,3)表示捷联矩阵T的第i行第j列元素。
4.根据权利要求1所述的基于MEMS和轮式里程计的离线定位方法，其特征在于，所述步骤S3中具体包括如下步骤
S31：通过安装一对转速测量单元于管道机器人的左、右轮上，分别实时获取左、右转速al、ar；
S32：获取管道机器人的左、右轮的周长为S,得到左轮速度、右轮速度vl＝alS,vr＝arS；
S33：基于所述左轮速度和所述右轮速度，得到管道机器人的所述前向速度
Figure FDA0003224591920000035
5.根据权利要求1所述的基于MEMS和轮式里程计的离线定位方法，其特征在于，在所述步骤S4中，得到所述导航速度的计算公式为
Figure FDA0003224591920000036
其中，ve为所述导航坐标系下的东向速度、vn为所述导航坐标系下的北向速度、vu为所述导航坐标系下的天向速度；
基于所述所述导航速度以及管道机器人k-1时刻的位置，得到k时刻的管道机器人位置，公式如下
Xk＝Xk-1+Vnt
其中，Vn＝[ve vn vu]T，t为采样周期，基于k时刻的管道机器人位置，以此得到所述组合导航管道轨迹。
6.根据权利要求1所述的基于MEMS和轮式里程计的离线定位方法，其特征在于，在所述步骤S5中，获得所述终点位置信息为通过GPS采集得到终点的经度、纬度和高度。
7.根据权利要求1所述的基于MEMS和轮式里程计的离线定位方法，其特征在于，所述步骤S6具体包括如下步骤
S61：基于所述起点位置信息和所述终点位置信息进行计算得到终点相对于起点的所述实际方位角
所述实际方位角计算公式为
Figure FDA0003224591920000041
其中，xe，ye分别为实际管道的终点东向和北向位置，xs，ys分别为实际管道起点的东向和北向位置；
基于所述组合导航管道轨迹计算得到解算方位角，计算公式如下
Figure FDA0003224591920000042
其中，xes，yes为所述组合导航管道轨迹的终点东向和北向位置，xss，yss为所述组合导航管道轨迹的起始点东向和北向位置；
S62：基于所述实际方位角和所述解算方位角，得到补偿方位角，计算公式如下
Figure FDA0003224591920000043
S63：基于所述起点位置信息和所述终点位置信息进行计算得到终点相对于起点的所述实际俯仰角，所述实际俯仰角计算公式为
θt＝arctan[(ze-zs)/(xe-xs)]
其中，xe，ze为实际管道的终点东向和天向位置，xs，zs为实际管道起始点的东向和天向位置；
基于所述组合导航管道轨迹计算得到解算俯仰角，计算公式如下
θs＝arctan[(zes-zss)/(xes-xss)]。
其中，xes，zes为所述组合导航管道轨迹的终点东向和北向位置，xss，zss为所述组合导航管道轨迹的起始点东向和北向位置；
S64：基于所述实际俯仰角和所述解算俯仰角，得到补偿俯仰角，计算公式如下Δθ＝θt-θs；
S65：基于所述补偿方位角和所述补偿俯仰角，得到旋转矩阵，计算公式如下
Figure FDA0003224591920000051
S66：基于所述旋转矩阵对所述组合导航管道轨迹进行矫正，得到所述最终管道轨迹。
CN202110967908.XA 2021-08-23 2021-08-23 一种基于mems和轮式里程计的离线定位方法 Pending CN113670297A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110967908.XA CN113670297A (zh) 2021-08-23 2021-08-23 一种基于mems和轮式里程计的离线定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110967908.XA CN113670297A (zh) 2021-08-23 2021-08-23 一种基于mems和轮式里程计的离线定位方法

Publications (1)

Publication Number Publication Date
CN113670297A true CN113670297A (zh) 2021-11-19

Family

ID=78545037

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110967908.XA Pending CN113670297A (zh) 2021-08-23 2021-08-23 一种基于mems和轮式里程计的离线定位方法

Country Status (1)

Country Link
CN (1) CN113670297A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115019537A (zh) * 2022-06-20 2022-09-06 芜湖宏景电子股份有限公司 一种应用于低质量网络条件下的智能导航系统

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050022402A1 (en) * 2003-08-01 2005-02-03 Ash Michael E. Compact navigation system and method
US20070168119A1 (en) * 2006-01-17 2007-07-19 Daishi Mori Navigation Device And Current-Position-Calculation Method
CN102003967A (zh) * 2010-09-03 2011-04-06 哈尔滨工程大学 基于罗经原理的旋转式船用捷联惯导方位对准方法
CN107228662A (zh) * 2017-06-05 2017-10-03 哈尔滨工程大学 一种基于管道连接器的小径管道定位装置及定位方法
CN109870173A (zh) * 2019-04-11 2019-06-11 中国石油化工股份有限公司 一种基于校验点的海底管道惯性导航系统的轨迹修正方法
CN109974697A (zh) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 一种基于惯性系统的高精度测绘方法
CN110109191A (zh) * 2019-04-19 2019-08-09 哈尔滨工业大学 一种基于mems和里程计组合的地下管线探测方法
CN110411444A (zh) * 2019-08-22 2019-11-05 深圳赛奥航空科技有限公司 一种地面下采掘移动设备用惯性导航定位系统与定位方法
CN110673608A (zh) * 2019-09-26 2020-01-10 福建首松智能科技有限公司 一种机器人导航方法
CN112697153A (zh) * 2020-12-31 2021-04-23 广东美的白色家电技术创新中心有限公司 自主移动设备的定位方法、电子设备及存储介质
CN113175933A (zh) * 2021-04-28 2021-07-27 南京航空航天大学 一种基于高精度惯性预积分的因子图组合导航方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050022402A1 (en) * 2003-08-01 2005-02-03 Ash Michael E. Compact navigation system and method
US20070168119A1 (en) * 2006-01-17 2007-07-19 Daishi Mori Navigation Device And Current-Position-Calculation Method
CN102003967A (zh) * 2010-09-03 2011-04-06 哈尔滨工程大学 基于罗经原理的旋转式船用捷联惯导方位对准方法
CN107228662A (zh) * 2017-06-05 2017-10-03 哈尔滨工程大学 一种基于管道连接器的小径管道定位装置及定位方法
CN109974697A (zh) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 一种基于惯性系统的高精度测绘方法
CN109870173A (zh) * 2019-04-11 2019-06-11 中国石油化工股份有限公司 一种基于校验点的海底管道惯性导航系统的轨迹修正方法
CN110109191A (zh) * 2019-04-19 2019-08-09 哈尔滨工业大学 一种基于mems和里程计组合的地下管线探测方法
CN110411444A (zh) * 2019-08-22 2019-11-05 深圳赛奥航空科技有限公司 一种地面下采掘移动设备用惯性导航定位系统与定位方法
CN110673608A (zh) * 2019-09-26 2020-01-10 福建首松智能科技有限公司 一种机器人导航方法
CN112697153A (zh) * 2020-12-31 2021-04-23 广东美的白色家电技术创新中心有限公司 自主移动设备的定位方法、电子设备及存储介质
CN113175933A (zh) * 2021-04-28 2021-07-27 南京航空航天大学 一种基于高精度惯性预积分的因子图组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
崔莅杭、陈贵金、王勇刚: "基于位置修正的轨迹测量方法研究", vol. 4, no. 6, pages 42 - 48 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115019537A (zh) * 2022-06-20 2022-09-06 芜湖宏景电子股份有限公司 一种应用于低质量网络条件下的智能导航系统
CN115019537B (zh) * 2022-06-20 2023-11-14 芜湖宏景电子股份有限公司 一种应用于低质量网络条件下的智能导航系统

Similar Documents

Publication Publication Date Title
CN109974697B (zh) 一种基于惯性系统的高精度测绘方法
CN106289246B (zh) 一种基于位置和姿态测量系统的柔性杆臂测量方法
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN107655493B (zh) 一种光纤陀螺sins六位置系统级标定方法
CN106842271B (zh) 导航定位方法及装置
CN110631574B (zh) 一种惯性/里程计/rtk多信息融合方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN105547288A (zh) 一种煤矿井下移动设备自主定位的方法及系统
CN109870173A (zh) 一种基于校验点的海底管道惯性导航系统的轨迹修正方法
CN101173858A (zh) 一种月面巡视探测器的三维定姿与局部定位方法
CN114739425A (zh) 基于rtk-gnss及全站仪的采煤机定位标定系统及应用方法
CN111024070A (zh) 一种基于航向自观测的惯性足绑式行人定位方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN107202578B (zh) 一种基于mems技术的捷联式垂直陀螺仪解算方法
CN103712622A (zh) 基于惯性测量单元旋转的陀螺漂移估计补偿方法及装置
CN102937450A (zh) 一种基于陀螺测量信息的相对姿态确定方法
CN115615430B (zh) 基于捷联惯导的定位数据修正方法及系统
CN114061619A (zh) 一种基于在线标定的惯导系统姿态补偿方法
CN112880669A (zh) 一种航天器星光折射和单轴旋转调制惯性组合导航方法
CN105953797B (zh) 利用单轴陀螺仪、倾角仪和里程计的组合导航装置及方法
CN109084755B (zh) 一种基于重力视速度与参数辨识的加速度计零偏估计方法
CN113670297A (zh) 一种基于mems和轮式里程计的离线定位方法
CN101943585B (zh) 一种基于ccd星敏感器的标定方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法

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