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

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

Info

Publication number
CN113916227A
CN113916227A CN202111175107.6A CN202111175107A CN113916227A CN 113916227 A CN113916227 A CN 113916227A CN 202111175107 A CN202111175107 A CN 202111175107A CN 113916227 A CN113916227 A CN 113916227A
Authority
CN
China
Prior art keywords
pipeline
detection robot
velocity
representing
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.)
Granted
Application number
CN202111175107.6A
Other languages
English (en)
Other versions
CN113916227B (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

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/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中，构建管道内检测机器人的动力学模型，具体包括：由管道内检测机器人的运动受力得到下式物理模型：
Figure BDA0003295128780000021
其中，
Figure BDA0003295128780000022
和
Figure BDA0003295128780000023
分别表示管道内检测机器人在t时刻的速度和加速度，ξ是粘性摩擦系数，m表示机器人质量，u(t)是施加的动力，w(t)～N(0，Q)代表了高斯随机白噪声；
Figure BDA0003295128780000024
其中，
Figure BDA0003295128780000025
U(t)＝[0，u(t)]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分别表示离散后的
Figure BDA0003295128780000031
U(t)、W(t)；I表示单位矩阵。
进一步，步骤S2中，构建的测量模型为：为了应用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，方差为Γ的噪声；令
Figure BDA0003295128780000032
则测量向量变为：
Y(t)＝h[x(t)，v(t)]+V(t) (9)
其中，V(t)是均值为零，协方差为
Figure BDA0003295128780000033
的噪声。
进一步，步骤S2中，高置信度区的定位算法具体包括以下步骤：当管道内检测机器人在HCZ1和HCZ2中移动时，其搭载的SN测量RSS及其速度，并基于这些测量值估计其位置和速度；
S201：初始化卡尔曼滤波：
Figure BDA0003295128780000034
Figure BDA0003295128780000035
其中，假设检测机器人在HCZ1区域Ka时刻出发，
Figure BDA0003295128780000036
表示假设的初始时刻的位置和速度向量，
Figure BDA0003295128780000037
表示假设的初始时刻的协方差矩阵；
S202：对位置和速度矢量进行预测：
Figure BDA0003295128780000038
Figure BDA0003295128780000041
其中，
Figure BDA0003295128780000042
定示由上一时刻预测得到的位置和速度向量，
Figure BDA0003295128780000043
表示上一时刻迭代更新后的速度和位置向量，PK|K-1表示由上一时刻预测的当前时刻的状态协方差矩阵，PK-1|K-1表示上一时刻迭代更新的协方差矩阵，
Figure BDA0003295128780000044
表示FK的转置，
Figure BDA0003295128780000045
表示随机噪声；
S203：迭代更新位置和速度：
Figure BDA0003295128780000046
Figure BDA0003295128780000047
Figure BDA0003295128780000048
Figure BDA0003295128780000049
PK|K＝(I-KKHK)PK|K-1 (18)
其中，
Figure BDA00032951287800000410
是测量残差，YK表示当前测量的RSS和速度向量，h(·)表示是式(9)中关于预测的位置和速度向量的函数值，SK是残余协方差，KK表示是卡尔曼增益，表示了测量残差
Figure BDA00032951287800000411
相对于先前估计
Figure BDA00032951287800000412
的相对重要性，
Figure BDA00032951287800000413
分别表示
Figure BDA00032951287800000414
与YK结合生成精确的位置和速度向量和协方差估计，HK是对
Figure BDA00032951287800000415
线性化处理后的矩阵。
进一步，步骤S2中，低置信度区定位算法具体包括以下步骤：在低置信度区，只能测量管道内检测机器人的速度，采用基于RTS的平滑器，即利用低置信度区的全部测量数据以及临近高置信度区的数据实现平滑处理。从HCZ1进入LCZ时采用卡尔曼滤波器算法计算，估计SN通过LCZ的时间间隙(Kb，Kc)内的速度和位置，称为前向通道；后向通道是在得到前向通道处理的结果后计算SN从HCZ1移动到HCZ2的时间间隔(Km，Kn)内的位置和速度；
S211：基于卡尔曼滤波算法计算前向通道：
Figure BDA00032951287800000416
Figure BDA00032951287800000417
其中，
Figure BDA00032951287800000418
和
Figure BDA00032951287800000419
用于初始化位置和速度估计值，由上述的高置信度区定位算法计算得到；
按照上式初始化卡尔曼滤波器，然后迭代计算到KC；
Figure BDA00032951287800000420
Figure BDA00032951287800000421
Figure BDA00032951287800000422
Figure BDA00032951287800000423
Figure BDA0003295128780000051
Figure BDA0003295128780000052
PK|K＝(I-KKHK)PK|K-1 (27)
其中，
Figure BDA0003295128780000053
表示由上一时刻预测得到的位置和速度向量，
Figure BDA0003295128780000054
表示上一时刻迭代更新的位置和速度向量，PK|K-1表示由上一时刻预测的状态协方差矩阵，PK-1|K-1表示上一时刻迭代更新的协方差矩阵，
Figure BDA0003295128780000055
表示FK的转置，
Figure BDA0003295128780000056
是随机噪声；
Figure BDA0003295128780000057
是测量残差，YK表示当前测量的RSS和速度向量，hv是测量的速度，SK是残余协方差，KK是卡尔曼增益，表示了测量残差
Figure BDA0003295128780000058
相对于先前估计
Figure BDA0003295128780000059
的相对重要性，Γ表示均值为0，方差为Γ的噪声，
Figure BDA00032951287800000510
分别表示
Figure BDA00032951287800000511
与YK结合生成精确的位置和速度向量和协方差估计，HK是对
Figure BDA00032951287800000512
线性化处理后的矩阵。
S212：基于前向通道结果利用RTS平滑算法计算得到后向通道：
Figure BDA00032951287800000513
Figure BDA00032951287800000514
Figure BDA00032951287800000515
Figure BDA00032951287800000516
Figure BDA00032951287800000517
其中，k＝kn-1，kn-2，...，km，
Figure BDA00032951287800000518
和
Figure BDA00032951287800000519
作为初始估计值，由高置信度区算法得到，CK表示平滑增益，
Figure BDA00032951287800000520
表示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、管道内检测机器人动力学模型：
由管道内检测机器人的运动受力可以得到下式物理模型：
Figure BDA0003295128780000071
其中，
Figure BDA0003295128780000072
和
Figure BDA0003295128780000073
分别代表了管道内检测机器人在t时刻的速度和加速度，ξ是粘性摩擦系数，u(t)是施加的动力，w(t)～N(0，Q)代表了高斯随机白噪声。
Figure BDA0003295128780000074
其中，
Figure BDA0003295128780000075
U(t)＝[0，u(t)]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，方差为Γ的噪声。令
Figure BDA0003295128780000076
至此，测量向量可写成：
Y(t)＝h[x(t)，v(t)]+V(t) (9)
V(t)是均值为零，协方差
Figure BDA0003295128780000081
的噪声。
3、高置信度区定位算法
参照图2，当管道内检测机器人在HCZ1和HCZ2中移动时，其搭载的SN测量RSS及其速度，并基于这些测量值估计其位置和速度。
步骤1：初始化卡尔曼滤波：
Figure BDA0003295128780000082
Figure BDA0003295128780000083
步骤2：对位置和速度矢量进行预测：
Figure BDA0003295128780000084
Figure BDA0003295128780000085
步骤3：迭代更新位置和速度：
Figure BDA0003295128780000086
Figure BDA0003295128780000087
Figure BDA0003295128780000088
Figure BDA0003295128780000089
PK|K＝(I-KKHK)PK|K-1 (18)
在预测步骤中滤波器基于上述管道内检测机器人动力学模型产生预测位置和速度
Figure BDA00032951287800000810
和状态协方差矩阵PK|K-1。在更新步骤中，预测的位置和速度向量
Figure BDA00032951287800000811
与当前测量的RSS和速度向量YK结合生成精确的位置和速度
Figure BDA00032951287800000812
以及协方差估计PK|K。HK是对
Figure BDA00032951287800000813
的线性化处理，
Figure BDA00032951287800000814
是测量残差，SK是残余协方差。
4、低置信度区定位算法：
在低置信度区，只能测量管道内检测机器人的速度，采用基于RTS的平滑器，即利用低置信度区的全部测量数据以及临近高置信度区的数据实现平滑处理。从HCZ1进入LCZ时采用卡尔曼滤波器算法计算，估计(Kb，kc)时间间隙内的速度和位置，称为前向通道。后向通道是在得到前向通道处理的结果后计算时间间隔(Km，Kn)内的位置和速度。
步骤1：基于卡尔曼滤波算法计算前向通道：
Figure BDA00032951287800000815
Figure BDA00032951287800000816
按照上式初始化卡尔曼滤波器，然后迭代计算到KC。
Figure BDA0003295128780000091
是随机噪声，hv是测量的速度。
Figure BDA0003295128780000092
Figure BDA0003295128780000093
Figure BDA0003295128780000094
Figure BDA0003295128780000095
Figure BDA0003295128780000096
Figure BDA0003295128780000097
PK|K＝(I-KKHK)PK|K-1 (27)
步骤2：基于前向通道结果利用RTS平滑器计算得到后向通道：
Figure BDA0003295128780000098
Figure BDA0003295128780000099
Figure BDA00032951287800000910
Figure BDA00032951287800000911
Figure BDA00032951287800000912
按照上式初始化并迭代计算时间间隔(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中，构建管道内检测机器人的动力学模型，具体包括：由管道内检测机器人的运动受力得到下式物理模型：
Figure FDA0003295128770000011
其中，
Figure FDA0003295128770000012
和
Figure FDA0003295128770000013
分别表示管道内检测机器人在t时刻的速度和加速度，ξ是粘性摩擦系数，m表示机器人质量，u(t)是施加的动力，w(t)表示高斯随机白噪声；
Figure FDA0003295128770000014
其中，
Figure FDA0003295128770000015
U(t)＝[0，u(t)]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分别表示离散后的
Figure FDA00032951287700000210
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，方差为Γ的噪声；令
Figure FDA0003295128770000021
则测量向量变为：
Y(t)＝h[x(t)，v(t)]+V(t) (9)
其中，V(t)是均值为零，协方差为RC的噪声。
5.根据权利要求3所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S2中，高置信度区的定位算法具体包括以下步骤：
S201：初始化卡尔曼滤波：
Figure FDA0003295128770000022
Figure FDA0003295128770000023
其中，假设检测机器人在HCZ1区域Ka时刻出发，
Figure FDA0003295128770000024
表示假设的初始时刻的位置和速度向量，
Figure FDA0003295128770000025
表示假设的初始时刻的协方差矩阵；
S202：对位置和速度矢量进行预测：
Figure FDA0003295128770000026
Figure FDA0003295128770000027
其中，
Figure FDA0003295128770000028
表示由上一时刻预测得到的当前时刻位置和速度向量，
Figure FDA0003295128770000029
表示上一时刻的迭代更新的位置和速度向量值，PK|K-1表示由上一时刻预测的当前时刻的状态协方差矩阵，PK-1|K-1表示上一时刻的迭代更新的协方差矩阵，
Figure FDA0003295128770000031
表示FK的转置，
Figure FDA0003295128770000032
表示随机噪声；
S203：迭代更新位置和速度：
Figure FDA0003295128770000033
Figure FDA0003295128770000034
Figure FDA0003295128770000035
Figure FDA0003295128770000036
PK|K＝(I-KKHK)PK|K-1 (18)
其中，
Figure FDA0003295128770000037
是测量残差，YK表示当前测量的RSS和速度向量，h(·)是式(9)中关于预测的位置和速度向量的函数值，SK是残余协方差，KK是卡尔曼增益，表示了测量残差
Figure FDA0003295128770000038
阳对于先前估计
Figure FDA0003295128770000039
的相对重要性，
Figure FDA00032951287700000310
PK|K分别表示
Figure FDA00032951287700000311
与YK结合生成精确的估计位置和速度向量和协方差估计，HK是对
Figure FDA00032951287700000312
线性化处理后的矩阵。
6.根据权利要求3所述的管道内检测机器人跟踪定位方法，其特征在于，步骤S2中，低置信度区定位算法具体包括以下步骤：从HCZ1进入LCZ时采用扩展卡尔曼滤波器算法计算，估计SN通过LCZ的时间间隙(Kb，kc)内的速度和位置，称为前向通道；后向通道是在得到前向通道处理的结果后计算SN从HCZ1移动到HCZ2的时间间隔(Km，Kn)内的位置和速度；
S211：基于卡尔曼滤波算法计算前向通道：
Figure FDA00032951287700000313
Figure FDA00032951287700000314
其中，
Figure FDA00032951287700000315
和
Figure FDA00032951287700000316
用于初始化位置和速度估计值，由上述的高置信度区定位算法计算得到；
按照上式初始化卡尔曼滤波器，然后迭代计算到KC；
Figure FDA00032951287700000317
Figure FDA00032951287700000318
Figure FDA00032951287700000319
Figure FDA00032951287700000320
Figure FDA00032951287700000321
Figure FDA00032951287700000322
PK|K＝(I-KKHK)PK|K-1 (27)
其中，
Figure FDA0003295128770000041
表示由上一时刻预测的位置和速度向量，
Figure FDA0003295128770000042
表示上一时刻迭代更新的位置和速度向量，PK|K-1表示由上一时刻预测的状态协方差矩阵，PK-1|K-1表示上一时刻迭代更新的协方差矩阵，
Figure FDA0003295128770000043
表示FK的转置，
Figure FDA0003295128770000044
是随机噪声；
Figure FDA0003295128770000045
是测量残差，YK表示当前测量的RSS和速度向量，hv是测量的速度，SK是残余协方差，KK是卡尔曼增益，表示了测量残差
Figure FDA0003295128770000046
相对于先前估计
Figure FDA0003295128770000047
的相对重要性，Γ表示均值为0，方差为Γ的噪声，
Figure FDA0003295128770000048
PK|K分别表示
Figure FDA0003295128770000049
与YK结合生成精确的位置和速度向量和协方差估计，HK是对
Figure FDA00032951287700000410
线性化处理后的矩阵；
S212：基于前向通道结果利用RTS平滑算法计算得到后向通道：
Figure FDA00032951287700000411
Figure FDA00032951287700000412
Figure FDA00032951287700000413
Figure FDA00032951287700000414
Figure FDA00032951287700000415
其中，k＝kn-1，kn-2，…，km，
Figure FDA00032951287700000416
和
Figure FDA00032951287700000417
作为初始估计值，由高置信度区算法得到，CK表示平滑增益，
Figure FDA00032951287700000418
是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 true CN113916227A (zh) 2022-01-11
CN113916227B 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 (7)

* 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
US20180371897A1 (en) * 2017-05-25 2018-12-27 Halliburton Energy Services, Inc. Systems and methods to use triangulation through one sensor beamforming in downhole leak detection
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 重庆邮电大学 基于低频窄带噪声激振和多传感器融合的油井动液面测量方法
US20210250110A1 (en) * 2018-06-13 2021-08-12 Nippon Telegraph And Telephone Corporation Propogation environment recognition method and propagation environment recognition apparatus
CN113469177A (zh) * 2021-06-30 2021-10-01 河海大学 基于深度学习的排水管道缺陷检测方法及系统

Patent Citations (7)

* 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
US20180371897A1 (en) * 2017-05-25 2018-12-27 Halliburton Energy Services, Inc. Systems and methods to use triangulation through one sensor beamforming in downhole leak detection
US20210250110A1 (en) * 2018-06-13 2021-08-12 Nippon Telegraph And Telephone Corporation Propogation environment recognition method and propagation environment recognition apparatus
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
杨秀萍 等: "基于无线传感器网络的移动机器人定位导航系统", 电子器件, vol. 30, no. 6 *
赵中伟 等: "基于无迹卡尔曼滤波的机器人无线室内定位估计算法", 机电技术, no. 5 *

Also Published As

Publication number Publication date
CN113916227B (zh) 2023-10-13

Similar Documents

Publication Publication Date Title
CN103487050B (zh) 一种室内移动机器人定位方法
Wu et al. Node localization in robotic sensor networks for pipeline inspection
AU2010304861B2 (en) Improvements in or relating to radio navigation
JP6525325B2 (ja) デバイスのロケーションを求める方法およびデバイス
CN108983271B (zh) 基于rtk-gps/ins列车组合定位方法
Tian et al. An INS and UWB fusion approach with adaptive ranging error mitigation for pedestrian tracking
EP0870174A1 (en) Improved vehicle navigation system and method using gps velocities
KR102288771B1 (ko) 반송파 위상 시간 차분 측정치 기반 항법 시스템 및 위치측정 방법
US20150308831A1 (en) Apparatus for inferring pedestrian position based on pedestrian movement detection, and method therefor
CN104237920A (zh) 高反射环境中的交通工具定位
CN103616036A (zh) 一种基于合作目标的机载传感器系统误差估计与补偿方法
JP2016114603A (ja) Ｇｐｓドリフトの処理方法、およびそのシステム
US20150031387A1 (en) Compensation of the signal attenuation by human body in indoor wi-fi positioning
US20060058954A1 (en) Constrained tracking of ground objects using regional measurements
Kuusniemi et al. Reliability considerations of multi-sensor multi-network pedestrian navigation
KR20160143438A (ko) 추측 항법 시스템에서의 밀결합 측위 방법 및 그 장치
KR100882590B1 (ko) 위치 판단 장치 및 방법
JP2004069536A (ja) データ検定装置および方法
KR20150132165A (ko) 외부 지원 정보를 사용하여 고정, 타이밍-기반 무선 위치 결정 네트워크의 성능을 개선하기 위한 기술
CN113419265B (zh) 一种基于多传感器融合的定位方法、装置与电子设备
CN113063441B (zh) 里程计累计推算误差的数据源纠正方法及装置
CN114217339A (zh) 定位方法、装置、电子设备及存储介质
CN113916227A (zh) 基于无线传感器网络的管道内检测机器人跟踪定位方法
Chao et al. Location-constrained particle filter human positioning and tracking system
Kausar et al. A novel Kalman filter based trilateration approach for indoor localization problem

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