CN113916227B - 基于无线传感器网络的管道内检测机器人跟踪定位方法 - Google Patents

基于无线传感器网络的管道内检测机器人跟踪定位方法 Download PDF

Info

Publication number
CN113916227B
CN113916227B CN202111175107.6A CN202111175107A CN113916227B CN 113916227 B CN113916227 B CN 113916227B CN 202111175107 A CN202111175107 A CN 202111175107A CN 113916227 B CN113916227 B CN 113916227B
Authority
CN
China
Prior art keywords
pipeline
representing
detection robot
speed
robot
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.)
Active
Application number
CN202111175107.6A
Other languages
English (en)
Other versions
CN113916227A (zh
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.)
Chongqing University of Post and Telecommunications
Original Assignee
Chongqing University of Post and Telecommunications
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 Chongqing University of Post and Telecommunications filed Critical Chongqing University of Post and Telecommunications
Priority to CN202111175107.6A priority Critical patent/CN113916227B/zh
Publication of CN113916227A publication Critical patent/CN113916227A/zh
Application granted granted Critical
Publication of CN113916227B publication Critical patent/CN113916227B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J11/00—Manipulators not otherwise provided for
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/087—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices for sensing other physical parameters, e.g. electrical or chemical properties
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/02—Systems for determining distance or velocity not using reflection or reradiation using radio waves
    • G01S11/06—Systems for determining distance or velocity not using reflection or reradiation using radio waves using intensity measurements
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Human Computer Interaction (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及一种基于无线传感器网络的管道内检测机器人跟踪定位方法，属于管道内检测技术领域。该方法包括：S1：根据动力学模型得到管道内检测机器人的位置X′和速度V′，并进行离散化处理，作为卡尔曼滤波算法的预测模型；S2：根据测量模型，判断管道内检测机器人的位置；S3：在高置信度区由RSS测得位置X，IMU测得速度V，X′，V′得到管道内检测机器人的位置和速度；在低置信度区由V，X′，V′得到预估的管道内检测机器人位置和速度，然后利用RTS平滑算法优化位置和速度信息。本发明提高了管道内检测机器人跟踪定位的精度。

Description

基于无线传感器网络的管道内检测机器人跟踪定位方法
技术领域
本发明属于管道内检测技术领域，涉及一种基于无线传感器网络的管道内检测机器人跟踪定位方法。
背景技术
管道运输是石油、天然气、供水、排水等流体输送的主要方式，但随着管道使用年限逐渐增加，管道因腐蚀、老化、地质运动、第三方破坏等因素产生缺陷，同时长期运行的管道还会积累大量的杂质，因此需要定期对流体输送管道进行内部缺陷检测和杂质清理，有效避免管道流量减少、泄漏、爆炸等事故的发生。
管道缺陷检测包括管道内检测和外检测两种，管道内检测因具有比外检测更高的精确度且可同时完成管道内部清理作业任务而得到广泛应用。管道内检测是通过移动机器人沿着管道内部运行来实现的，由于管道结构复杂、内部沉淀杂质过多等原因会导致内检测机器人卡堵在管道内部，导致管道堵塞事故的发生。为此，当内检测机器人在管道内部进行缺陷检测和清理作业时，需要对管道内检测机器人进行实时跟踪与定位。
常用的管道内机器人定位方法包括基于普通光电式传感器的里程轮定位，以及在此基础上的多里程轮数据融合定位和多种传感器综合定位法、定点磁标定位法、静磁场定位法、放射性定位法等。王忠巍等人提出了一种多数据融合算法(王忠巍，曹其新，栾楠，张蕾.基于多传感器数据融合的管道机器人精确定位技术[J].机器人,2008(03):238-241.)，该方法对里程轮长距离定位精度损失进行了补偿，但无法实现对管道内检测器的实时跟踪。吴知非等提出了静磁场定位法(吴知非，吴敏生.一种可用于管道爬行器的静磁场型定位系统[J].测控技术,2001(04):1-2+5.)，静磁场定位法极易受环境磁场的影响，限制了使用环境。姜生元等提出了放射性定位法(姜生元，邓宗全，李斌，于卫真，李瑰贤.内置动力源管内X射线探伤机器人的研制[J].机器人,2001(03):211-216.)，放射性定位法通常采用放射性元素作为放射源，对人体和环境造成不利影响。综上所述，目前管道内检测机器人定位方法都存在一定的局限性，无法做到实时跟踪与定位。
发明内容
有鉴于此，本发明的目的在于提供基于无线传感器网络的管道内检测机器人跟踪定位方法，该方法利用管道内检测机器人携带的通信模块和惯性测量单元，在管道外部通信节点信号覆盖和未覆盖的区域均能实现管道内检测机器人精确跟踪定位，该方法适用于油气、供水、排水等流体管道内检测机器人跟踪定位。
为达到上述目的，本发明提供如下技术方案：
一种基于无线传感器网络的管道内检测机器人跟踪定位方法，具体包括以下步骤：
S1：根据动力学模型得到管道内检测机器人的位置X′和速度V′，并进行离散化处理，作为卡尔曼滤波算法的预测模型；
S2：根据测量模型，判断管道内检测机器人的位置，具体为：由中继器节点(Repeater node，RN)向外发射无线电波信号，若管道内检测机器人上的移动传感器节点(Sensor node，SN)接收到信号，则进入高置信度区(High confidence zone，HCZ)，反之则进入低置信度区(Low confidence zone，LCZ)；在高置信度区时，由惯性测量单元(Inertial measurement unit，IMU)测得速度V，利用接收信号强度(Received signalstrength，RSS)测得位置X；在低置信度区时，由惯性测量单元(Inertial measurementunit，IMU)测得速度V；
S3：在高置信度区由X,V,X′,V′得到管道内检测机器人的位置和速度；在低置信度区由V,X′,V′得到预估的管道内检测机器人位置和速度，然后利用RTS(Rauch-Tung-Striebel)平滑算法优化位置和速度信息；
S4：通过预测模型输出管道内检测机器人的整个行程的位置和速度信息。
进一步，该方法中，采用基站、RN和SN三级通信定位，其中SN搭载在地下管道内检测机器人中，基站与RN位于地面，基站与RN通信，RN与SN直接通信，增强了对管道内检测机器人搭载的移动传感器节点的控制；整个管道区域划分成RN信号覆盖的区域和未覆盖的区域；信号覆盖区域SN和RN可以直接通信并定位，仅有电磁信号传递过程中的误差，具有很高的准确度，将信号覆盖区域定为HCZ；未覆盖的区域定为LCZ。
进一步，步骤S1中，构建管道内检测机器人的动力学模型，具体包括：由管道内检测机器人的运动受力得到下式物理模型：
其中，和/>分别表示管道内检测机器人在t时刻的速度和加速度，ξ是粘性摩擦系数，m表示机器人质量，u(t)是施加的动力，w(t)～N(0,Q)代表了高斯随机白噪声；
其中，U(t)＝u(t),W(t)＝[0,w(t)]T；
RSS和速度的测量都是在离散时间点上测量的，假设估计过程和测量过程都有相同的时间间隙Δt，K用于表示时间点t＝KΔt，将公式(2)改成卡尔曼滤波的形式，得到：
XK＝FKXK-1+GKUK-1+LKWK (3)
FK＝exp(AΔt)≈(I+AΔt) (4)
GK＝(exp(AΔt)-I)A-1B≈BΔt (5)
LK＝(exp(AΔt)-I)A-1≈IΔt (6)
其中XK、UK-1、WK分别表示离散后的U(t)、W(t)；I表示单位矩阵。
进一步，步骤S2中，构建的测量模型为：为了应用RSS定位管道内检测机器人，需先确定RSS与传播路径的关系：yr(y)＝t[x(t)]，yr(t)表示t时刻管道内检测机器人SN处的RSS，r[x(t)]是SN和RN的距离；采用对数长程衰减模型，有：
yr[dB]＝alog(ds)+b-ωΩ[dB] (7)
其中，alog(ds)+b表示平均RSS，可由现场测量得出；ωΩ表示均值为零，方差为Ω的噪声；
管道内检测机器人速度测量由IMU的数据得到：
yv(t)＝v(t)+γΓ (8)
其中，yv(t)表示测量速度，v(t)表示实际速度；γΓ是均值为0，方差为Γ的噪声；令
则测量向量变为：
Y(t)＝h[x(t),v(t)]+V(t) (9)
其中，V(t)是均值为零，协方差为的噪声。
进一步，步骤S2中，高置信度区的定位算法具体包括以下步骤：当管道内检测机器人在HCZ1和HCZ2中移动时，其搭载的SN测量RSS及其速度，并基于这些测量值估计其位置和速度；
S201：初始化卡尔曼滤波：
其中，假设检测机器人在HCZ1区域Ka时刻出发，表示假设的初始时刻的位置和速度向量，/>表示假设的初始时刻的协方差矩阵；
S202：对位置和速度矢量进行预测：
其中，表示由上一时刻预测得到的位置和速度向量，/>表示上一时刻迭代更新后的速度和位置向量，PK|K-1表示由上一时刻预测的当前时刻的状态协方差矩阵，PK-1|K-1表示上一时刻迭代更新的协方差矩阵，/>表示FK的转置，/>表示随机噪声；
S203：迭代更新位置和速度：
PK|K＝(I-KKHK)PK|K-1 (18)
其中，是测量残差，YK表示当前测量的RSS和速度向量，h(·)表示是式(9)中关于预测的位置和速度向量的函数值，SK是残余协方差，KK表示是卡尔曼增益，表示了测量残差/>相对于先前估计/>的相对重要性，/>PK|K分别表示/>与YK结合生成精确的位置和速度向量和协方差估计，HK是对/>线性化处理后的矩阵。
进一步，步骤S2中，低置信度区定位算法具体包括以下步骤：在低置信度区，只能测量管道内检测机器人的速度，采用基于RTS的平滑器，即利用低置信度区的全部测量数据以及临近高置信度区的数据实现平滑处理。从HCZ1进入LCZ时采用卡尔曼滤波器算法计算，估计SN通过LCZ的时间间隙(Kb,Kc)内的速度和位置，称为前向通道；后向通道是在得到前向通道处理的结果后计算SN从HCZ1移动到HCZ2的时间间隔(Km,Kn)内的位置和速度；
S211：基于卡尔曼滤波算法计算前向通道：
其中，和/>用于初始化位置和速度估计值，由上述的高置信度区定位算法计算得到；
按照上式初始化卡尔曼滤波器，然后迭代计算到KC；
PK|K＝(i-KKHK)PK|K-1 (27)
其中，表示由上一时刻预测得到的位置和速度向量，/>表示上一时刻迭代更新的位置和速度向量，PK|K-1表示由上一时刻预测的状态协方差矩阵，PK-1|K-1表示上一时刻迭代更新的协方差矩阵，/>表示FK的转置，/>是随机噪声；/>是测量残差，YK表示当前测量的RSS和速度向量，hv是测量的速度，SK是残余协方差，KK是卡尔曼增益，表示了测量残差/>相对于先前估计/>的相对重要性，Γ表示均值为0，方差为Γ的噪声，/>PK|K分别表示/>与YK结合生成精确的位置和速度向量和协方差估计，HK是对/>线性化处理后的矩阵。
S212：基于前向通道结果利用RTS平滑算法计算得到后向通道：
其中，k＝kn-1,kn-2,...,km，和/>作为初始估计值，由高置信度区算法得到，CK表示平滑增益，/>表示CK的转置，PK+1|Kn表示上一时刻迭代更新的协方差矩阵；按照上式初始化并迭代计算时间间隔(Km,Kn)内的位置和速度，Km和Kn的选择依据精度和数据处理能力进行折中。
本发明的有益效果在于：本发明利用接收信号强度的方式来判断中继器节点和传感器节点的距离可以避免电磁信号穿过土壤时的高衰减特性的影响。利用扩展卡尔曼算法对管道内检测机器人的位置、速度物理模型估计值与IMU测量值进行融合可有效提升管道内检测机器人的跟踪定位精度。同时，将管道划分为中继器节点信号覆盖区和未覆盖区进行管道内检测机器人跟踪定位可以显著减少成本。在信号未覆盖区采用RTS平滑器算法能有效进行定位误差补偿，结合有效的中继器节点安放位置选择，可有效保障在未覆盖区的管道内检测机器人的跟踪定位精度。
本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。
附图说明
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：
图1为基于无线传感器网络的管道内检测机器人定位方法的安装示意图；
图2为移动传感器定位模型示意图；
图3为基于无线传感器网络的管道内检测机器人定位算法流程图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
请参阅图1，本实施例采取的技术方案如图1所示，该方案采用基站，中继器节点和移动传感器节点三级通信定位，其中移动传感器节点搭载在地下管道机器人中，基站与中继器节点位于地面，基站与中继器通信，中继器节点(Repeater node，RN)与移动传感器节点(Sensor node，SN)直接通信，增强了对管道内检测机器人搭载的移动传感器节点的控制。整个管道区域划分成中继器节点信号覆盖的区域和未覆盖的区域。信号覆盖区域SN和RN可以直接通信并定位，仅有电磁信号传递过程中的误差，具有很高的准确度，将信号覆盖区域定为高置信度区(High confidence zone，HCZ)，该区域采用基于测量接收信号强度(Received signal strength，RSS)和惯性测量单元(Inertial measurement unit，IMU)测量的位置、速度进行管道内检测机器人的定位；未覆盖的区域定为低置信度区(Lowconfidence zone，LCZ)，此区域通过IMU测量的加速度和角速度来得到SN的速度信息，同时结合高置信度区的位置和速度信息，利用基于Rauch-Tung-Striebel(RTS)的平滑器补偿误差，可以实现准确的管道内检测机器人实时跟踪定位。
该方案首先通过动力学模型确定管道内检测机器人的位置和速度的函数关系，将其离散化为卡尔曼滤波算法的相应形式。再根据电磁波穿透土壤的传播模型，确定RSS与SN和RN之间距离的关系函数，加上由IMU得到的速度模型，即得到了测量数据与实际的位置、速度关系的模型。在高置信度区，利用卡尔曼滤波算法对管道内检测机器人的速度、位置的理论估计值和测量值进行融合实现管道内检测机器人的精确跟踪定位。在低置信度区，利用卡尔曼滤波算法计算整个区域管道内检测机器人的位置和速度，然后利用基于Rauch-Tung-Striebel(RTS)的平滑器进行误差补充从而实现管道内检测机器人的精确跟踪定位。
1、管道内检测机器人动力学模型：
由管道内检测机器人的运动受力可以得到下式物理模型：
其中，和/>分别代表了管道内检测机器人在t时刻的速度和加速度，ξ是粘性摩擦系数，u(t)是施加的动力，w(t)～N(0,Q)代表了高斯随机白噪声。
其中，U(t)＝u(t),W(t)＝[0,w(t)]T。
RSS和速度的测量都是在离散时间点上测量的，假设估计过程和测量过程都有相同的时间间隙Δt，K用于表示时间点t＝kΔt，将之改成卡尔曼滤波的形式，得到：
XK＝FKXK-1+GKUK-1+LKWK (3)
FK＝exp(AΔt)≈(I+AΔt) (4)
GK＝(exp(AΔt)-I)A-1B≈BΔt (5)
LK＝(exp(AΔt)-I)A-1≈IΔt (6)
2、测量模型：
本实施例考虑两种测量：管道内检测机器人搭载的SN只有在高置信度区时可以测量RSS,管道内检测机器人的速度测量可由机载IMU得到。
为了应用RSS定位管道内检测机器人，需先确定RSS与传播路径的关系yr(t)＝r[x(t)]，yr(t)表示t时刻管道内检测机器人SN处的RSS，r[x(t)]是SN和RN的距离。采用对数长程衰减模型，有：
yr[dB]＝alog(ds)+b-ωΩ[dB] (7)
alog(ds)+b指平均RSS，可由现场测量得出，ωΩ表示均值为零，方差为Ω的噪声。
管道内检测机器人速度测量由IMU单元的数据得到：
yv(t)＝v(t)+γΓ (8)
其中，yv(t)是测量速度，v(t)表示实际速度，γΓ是均值为0，方差为Γ的噪声。令
至此，测量向量可写成：
Y(t)＝h[x(t),v(t)]+V(t) (9)
V(t)是均值为零，协方差的噪声。
3、高置信度区定位算法
参照图2，当管道内检测机器人在HCZ1和HCZ2中移动时，其搭载的SN测量RSS及其速度，并基于这些测量值估计其位置和速度。
步骤1：初始化卡尔曼滤波：
步骤2：对位置和速度矢量进行预测：
步骤3：迭代更新位置和速度：
PK|K＝(I-KKHK)PK|K-1 (18)
在预测步骤中滤波器基于上述管道内检测机器人动力学模型产生预测位置和速度和状态协方差矩阵PK|K-1。在更新步骤中，预测的位置和速度向量/>与当前测量的RSS和速度向量YK结合生成精确的位置和速度/>以及协方差估计PK|K。HK是对/>的线性化处理，/>是测量残差，SK是残余协方差。
4、低置信度区定位算法：
在低置信度区，只能测量管道内检测机器人的速度，采用基于RTS的平滑器，即利用低置信度区的全部测量数据以及临近高置信度区的数据实现平滑处理。从HCZ1进入LCZ时采用卡尔曼滤波器算法计算，估计(Kb,Kc)时间间隙内的速度和位置，称为前向通道。后向通道是在得到前向通道处理的结果后计算时间间隔(Km,Kn)内的位置和速度。
步骤1：基于卡尔曼滤波算法计算前向通道：
按照上式初始化卡尔曼滤波器，然后迭代计算到KC。是随机噪声，hv是测量的速度。
PK|K＝(I-KKHK)PK|K-1 (27)
步骤2：基于前向通道结果利用RTS平滑器计算得到后向通道：
按照上式初始化并迭代计算时间间隔(Km,Kn)内的位置和速度，Km和Kn的选择依据精度和数据处理能力进行折中。
对管道内检测机器人的具体定位方案如图3所示，详细步骤如下：
Step1：根据SN的动力学模型得到管道内检测机器人的位置X′和速度V′，并进行离散化处理，作为卡尔曼滤波算法的预测模型。
Step2：判断管道内检测机器人的位置，由中继器节点向外发射无线电波信号，若管道内检测机器人上的移动传感器节点接收到信号，则进入高置信度区，反之则没有。在高置信度区时由IMU测得速度V，RSS方法测得位置X；低置信度区由IMU测得速度V。
Step3：在高置信度区由X,V,X′,V′得到管道内检测机器人的位置和速度；在低置信度区由V,X′,V′得到预估的管道内检测机器人位置和速度，然后利用RTS平滑算法优化位置和速度信息。
Step4：输出管道内检测机器人的整个行程的位置和速度信息。
最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (6)

1.一种基于无线传感器网络的管道内检测机器人跟踪定位方法，其特征在于，该方法具体包括以下步骤：
S1：根据动力学模型得到管道内检测机器人的位置X′和速度V′，并进行离散化处理，作为卡尔曼滤波算法的预测模型；
S2：根据测量模型，判断管道内检测机器人的位置，具体为：由中继器节点(Repeaternode，RN)向外发射无线电波信号，若管道内检测机器人上的移动传感器节点(Sensornode，SN)接收到信号，则进入高置信度区(High confidence zone，HCZ)，反之则进入低置信度区(Low confidence zone，LCZ)；在高置信度区时，由惯性测量单元(Inertialmeasurement unit，IMU)测得速度V，利用接收信号强度(Received signal strength，RSS)测得位置X；在低置信度区时，由惯性测量单元(Inertial measurement unit，IMU)测得速度V；
S3：在高置信度区由X,V,X′,V′得到管道内检测机器人的位置和速度；在低置信度区由V,X′,V′得到预估的管道内检测机器人位置和速度，然后利用RTS平滑算法优化位置和速度信息；
S4：通过预测模型输出管道内检测机器人的整个行程的位置和速度信息。
2.根据权利要求1所述的管道内检测机器人跟踪定位方法，其特征在于，该方法中，采用基站、RN和SN三级通信定位，其中SN搭载在地下管道内检测机器人中，基站与RN位于地面，基站与RN通信，RN与SN直接通信；整个管道区域划分成RN信号覆盖的区域和未覆盖的区域；将信号覆盖区域定为HCZ；未覆盖的区域定为LCZ。
3.根据权利要求1所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S1中，构建管道内检测机器人的动力学模型，具体包括：由管道内检测机器人的运动受力得到下式物理模型：
其中，和/>分别表示管道内检测机器人在t时刻的速度和加速度，ξ是粘性摩擦系数，m表示机器人质量，u(t)是施加的动力，w(t)表示高斯随机白噪声；
其中，U(t)＝u(t)，W(t)＝[0,w(t)]T；
假设估计过程和测量过程都有相同的时间间隙Δt，K用于表示时间点t＝KΔt，将公式(2)改成卡尔曼滤波的形式，得到：
XK＝FKXK-1+GKUK-1+LKWK (3)
FK＝exp(AΔt)≈(I+AΔt) (4)
GK＝(exp(AΔt)-I)A-1B≈BΔt (5)
LK＝(exp(AΔt)-I)A-1≈IΔt (6)
其中，XK、UK-1、WK分别表示离散后的U(t)、W(t)；I表示单位矩阵。
4.根据权利要求1所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S2中，构建的测量模型为：确定RSS与传播路径的关系：yr(t)＝r[x(t)]，yr(t)表示t时刻管道内检测机器人SN处的RSS，r[x(t)]是SN和RN的距离；采用对数长程衰减模型，有：
yr[dB]＝alog(ds)+b-ωΩ[dB] (7)
其中，alog(ds)+b表示平均RSS，由现场测量得出；ωΩ表示均值为零，方差为Ω的噪声；
管道内检测机器人速度测量由IMU的数据得到：
yv(t)＝v(t)+γΓ (8)
其中，yv(t)表示测量速度，v(t)表示实际速度；γΓ是均值为0，方差为Γ的噪声；令
则测量向量变为：
Y(t)＝h[x(t)，v(t)]+V(t) (9)
其中，V(t)是均值为零，协方差为RC的噪声。
5.根据权利要求3所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S2中，高置信度区的定位算法具体包括以下步骤：
S201：初始化卡尔曼滤波：
其中，假设检测机器人在HCZ1区域Ka时刻出发，表示假设的初始时刻的位置和速度向量，/>表示假设的初始时刻的协方差矩阵；
S202：对位置和速度矢量进行预测：
其中，表示由上一时刻预测得到的当前时刻位置和速度向量，/>表示上一时刻的迭代更新的位置和速度向量值，PK|K-1表示由上一时刻预测的当前时刻的状态协方差矩阵，PK-1|K-1表示上一时刻的迭代更新的协方差矩阵，/>表示FK的转置，/>表示随机噪声；
S203：迭代更新位置和速度：
PK|K＝(I-KKHK)PK|K-1 (18)
其中，是测量残差，YK表示当前测量的RSS和速度向量，h(·)是式(9)中关于预测的位置和速度向量的函数值，SK是残余协方差，KK是卡尔曼增益，表示了测量残差/>相对于先前估计/>的相对重要性，/>PK|K分别表示/>与YK结合生成精确的估计位置和速度向量和协方差估计，HK是对/>线性化处理后的矩阵。
6.根据权利要求3所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S2中，低置信度区定位算法具体包括以下步骤：从HCZ1进入LCZ时采用扩展卡尔曼滤波器算法计算，估计SN通过LCZ的时间间隙(Kb，Kc)内的速度和位置，称为前向通道；后向通道是在得到前向通道处理的结果后计算SN从HCZ1移动到HCZ2的时间间隔(Km，Kn)内的位置和速度；
S211：基于卡尔曼滤波算法计算前向通道：
其中，和/>用于初始化位置和速度估计值，由上述的高置信度区定位算法计算得到；
按照上式初始化卡尔曼滤波器，然后迭代计算到KC；
PK|K＝(I-KKHK)PK|K-1 (27)
其中，表示由上一时刻预测的位置和速度向量，/>表示上一时刻迭代更新的位置和速度向量，PK|K-1表示由上一时刻预测的状态协方差矩阵，PK-1|K-1表示上一时刻迭代更新的协方差矩阵，/>表示FK的转置，/>是随机噪声；/>是测量残差，YK表示当前测量的RSS和速度向量，hv是测量的速度，SK是残余协方差，KK是卡尔曼增益，表示了测量残差/>相对于先前估计/>的相对重要性，Γ表示均值为0，方差为Γ的噪声，/>PK|K分别表示与YK结合生成精确的位置和速度向量和协方差估计，HK是对/>线性化处理后的矩阵；
S212：基于前向通道结果利用RTS平滑算法计算得到后向通道：
其中，k＝kn-1,kn-2,...,km，和/>作为初始估计值，由高置信度区算法得到，CK表示平滑增益，/>是CK的转置，PK+1|Kn表示上一时刻迭代更新的协方差矩阵；按照上式初始化并迭代计算时间间隔(Km,Kn)内的位置和速度，Km和Kn的选择依据精度和数据处理能力进行折中。
CN202111175107.6A 2021-10-09 2021-10-09 基于无线传感器网络的管道内检测机器人跟踪定位方法 Active CN113916227B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111175107.6A CN113916227B (zh) 2021-10-09 2021-10-09 基于无线传感器网络的管道内检测机器人跟踪定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111175107.6A CN113916227B (zh) 2021-10-09 2021-10-09 基于无线传感器网络的管道内检测机器人跟踪定位方法

Publications (2)

Publication Number Publication Date
CN113916227A CN113916227A (zh) 2022-01-11
CN113916227B true CN113916227B (zh) 2023-10-13

Family

ID=79238463

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111175107.6A Active CN113916227B (zh) 2021-10-09 2021-10-09 基于无线传感器网络的管道内检测机器人跟踪定位方法

Country Status (1)

Country Link
CN (1) CN113916227B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5272646A (en) * 1991-04-11 1993-12-21 Farmer Edward J Method for locating leaks in a fluid pipeline and apparatus therefore
CN111692456A (zh) * 2020-06-23 2020-09-22 李柏松 一种用于管道检测的slam系统及方法
CN112584306A (zh) * 2020-11-27 2021-03-30 巢湖学院 一种基于卡尔曼滤波的室内机器人定位算法
CN113108870A (zh) * 2021-03-15 2021-07-13 重庆邮电大学 基于低频窄带噪声激振和多传感器融合的油井动液面测量方法
CN113469177A (zh) * 2021-06-30 2021-10-01 河海大学 基于深度学习的排水管道缺陷检测方法及系统

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2573921B (en) * 2017-05-25 2020-05-20 Halliburton Energy Services Inc Systems and methods to use triangulation through one sensor beamforming in downhole leak detection
JP7111956B2 (ja) * 2018-06-13 2022-08-03 日本電信電話株式会社 伝搬環境認識方法及び伝搬環境認識装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5272646A (en) * 1991-04-11 1993-12-21 Farmer Edward J Method for locating leaks in a fluid pipeline and apparatus therefore
CN111692456A (zh) * 2020-06-23 2020-09-22 李柏松 一种用于管道检测的slam系统及方法
CN112584306A (zh) * 2020-11-27 2021-03-30 巢湖学院 一种基于卡尔曼滤波的室内机器人定位算法
CN113108870A (zh) * 2021-03-15 2021-07-13 重庆邮电大学 基于低频窄带噪声激振和多传感器融合的油井动液面测量方法
CN113469177A (zh) * 2021-06-30 2021-10-01 河海大学 基于深度学习的排水管道缺陷检测方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于无线传感器网络的移动机器人定位导航系统;杨秀萍 等;电子器件;第30卷(第6期);全文 *
基于无迹卡尔曼滤波的机器人无线室内定位估计算法;赵中伟 等;机电技术(第5期);全文 *

Also Published As

Publication number Publication date
CN113916227A (zh) 2022-01-11

Similar Documents

Publication Publication Date Title
US10234292B2 (en) Positioning apparatus and global navigation satellite system, method of detecting satellite signals
CN107228662B (zh) 一种基于管道连接器的小径管道定位装置及定位方法
KR100498987B1 (ko) Ｇｐｓ 위치 데이터 수신 불량 지역에서의 이동체 위치예측 방법
CN105509738B (zh) 基于惯导/多普勒雷达组合的车载定位定向方法
CN103487050B (zh) 一种室内移动机器人定位方法
CN107654852B (zh) 一种基于管道段长度及管道连接器检测的管道内定位装置及定位方法
CN105066917A (zh) 一种小型管道地理信息系统测量装置及其测量方法
CN108983271B (zh) 基于rtk-gps/ins列车组合定位方法
CN106840154B (zh) 地下空间惯性测量与无线传感器组合定位系统与方法
JP2009019992A (ja) 位置検出装置及び位置検出方法
CN104237920A (zh) 高反射环境中的交通工具定位
CN103453903A (zh) 一种基于惯性测量组件的管道探伤系统导航定位方法
CN114739425A (zh) 基于rtk-gnss及全站仪的采煤机定位标定系统及应用方法
CN114545472B (zh) 一种gnss/ins组合系统的导航方法和装置
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN112229422A (zh) 基于fpga时间同步的里程计快速标定方法及系统
KR20160143438A (ko) 추측 항법 시스템에서의 밀결합 측위 방법 및 그 장치
CN113916227B (zh) 基于无线传感器网络的管道内检测机器人跟踪定位方法
KR20090008796A (ko) 지하 매설관의 3차원 지리정보 획득장치
CN113063441B (zh) 里程计累计推算误差的数据源纠正方法及装置
Wang et al. An enhanced positioning technique for underground pipeline robot based on inertial sensor/wheel odometer
JP2006119144A (ja) 道路線形自動測量装置
CN113236363A (zh) 开采设备导航定位方法、系统、设备及可读存储介质
CN116659496A (zh) 激光测距仪与加速度计组合的机器人导航定位系统及方法
Chao et al. Location-constrained particle filter human positioning and tracking 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
GR01 Patent grant
GR01 Patent grant