CN111708008B - 一种基于imu和tof的水下机器人单信标导航方法 - Google Patents

一种基于imu和tof的水下机器人单信标导航方法 Download PDF

Info

Publication number
CN111708008B
CN111708008B CN202010382088.3A CN202010382088A CN111708008B CN 111708008 B CN111708008 B CN 111708008B CN 202010382088 A CN202010382088 A CN 202010382088A CN 111708008 B CN111708008 B CN 111708008B
Authority
CN
China
Prior art keywords
underwater robot
beacon
data
module
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
CN202010382088.3A
Other languages
English (en)
Other versions
CN111708008A (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.)
Nanjing Institute of Technology
Original Assignee
Nanjing Institute of Technology
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 Nanjing Institute of Technology filed Critical Nanjing Institute of Technology
Priority to CN202010382088.3A priority Critical patent/CN111708008B/zh
Publication of CN111708008A publication Critical patent/CN111708008A/zh
Application granted granted Critical
Publication of CN111708008B publication Critical patent/CN111708008B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/14—Systems for determining distance or velocity not using reflection or reradiation using ultrasonic, sonic, or infrasonic waves
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position

Abstract

本发明公开了一种基于IMU和TOF的水下机器人单信标导航方法，属于机器人技术领域，包括建立单信标测距导航系统、惯性导航系统和数据处理中心，以单信标测距导航和惯性测量系统为研究对象，对水下机器人AUV的里程和姿态进行了测量，通过最大工作范围滤波器和状态接收阈值滤波器防止由于信标和接收器之间的时钟同步不准确、自发射信标的不准确状态估计、可能影响水声信号传输的各种水下环境条件等造成的TOF测距错误数据，解决了由于信标和接收器之间的时钟同步不准确、自发射信标的不准确状态估计和影响水声信号传输的各种水下环境条件所造成的TOF测距错误数据的技术问题，实现低成本、高精度的水下机器人导航功能。

Description

一种基于IMU和TOF的水下机器人单信标导航方法
技术领域
本发明属于机器人技术领域，涉及一种基于IMU和TOF的水下机器人单信标导航方法。
背景技术
水下机器人系统在海洋研究、海底地形勘测、军事领域得到了广泛应用，导航是水下机器人系统的一个重要组成部分，当前水下导航系统主要有以下几个方向。
全球定位系统(GPS)为地面、空中和水上机器人提供高精度和绝对位置测量，从而实现高精度导航解决方案，但是对于在水下机器人来说，因为射频信号在水中会快速衰减，因此全球定位系统(GPS)不能直接应用到水下机器人系统中。
长基线(LBL)声学导航方法是将水下机器人位置信息通过三角定位方法，定位到水下固定的测量声学应答器上，即通过固定位置的测量声学应答器进行定位计算提供绝对位置信息，但是这种导航方法存在工作覆盖范围受限、需要耗时进行测量计算等缺点。
超短基线(USBL)声学导航不需要固定传感器，但需要将位置数据与水下机器人(AUV)机载数据相融合，舰船通过声学包将超短基线(USBL)位置数据传输给水下机器人(AUV)，这通常会导致时间和数据延迟。并且长基线(LBL)和超短基线(USBL)导航系统价格均很昂贵，不能实现普及应用。
单信标测距导航通过接收GPS的水面信标并将其位置以声波包的形式发送给水下机器人，通过TOF方法在信标和水下机器人之间精确测距，从而实现精准定位水下机器人坐标。
当前单信标测距导航研究是声学导航方法的一个新的研究方向，为精准定位水下机器人XY坐标，减少XY坐标误差提供了一种新的导航方法(Z坐标是深度，是由压力深度传感器测量获得，因此不太需要考虑)。
单信标测距导航系统通过接收GPS的水面信标并将其位置以声波包的形式发送给水下机器人，通过TOF方法在信标和水下机器人之间精确测距。根据TOF计算，水面信标和水下机器人之间的距离是通过测量水下声速来确定的，这个距离用来约束水下机器人的位置估计，从而限制其XY位置误差。单信标测距导航的主要优点是，除了精确定位水下XY位置外，还能够部署多个水下机器人，每个水下机器人同时接收来自同一地面信标的位置数据声学包，因此每个水下机器人的位置都可以实时更新。
可观测性是单信标测距中重要组成部分。由于单信标测距仅提供一个约束，即仅提供了一个相对位置测量，而不是绝对位置测量，因此只能观测到水下机器人部分轨迹，这就需要对可观测性方法进行研究，优化水下机器人运动轨迹，增强其可观测性，使水下机器人导航解更为精确。
IMU惯性导航既可以作为导航传感器来确定水下机器人姿态，也可以作为里程计的输入，通过积分线性加速度来获得速度和位置。但是由于加速度计和陀螺仪的噪声水平、测量偏差和漂移误差较高，线性加速度的双重积分随着时间的推移会导致相当大的位置误差。
惯性导航(IMU)通常包含加速度计、陀螺仪和磁强计，分别测量线加速度、角速率和磁场强度。因此，惯性导航(IMU)既可以作为导航传感器来确定水下机器人的姿态，也可以作为里程计的输入，通过积分线性加速度来获得速度和位置。虽然IMU的尺寸、功耗和成本都在降低，但是由于加速度计和陀螺仪的噪声水平、偏差和漂移误差较高，线性加速度的双重积分随着时间的推移会导致相当大的位置误差，并且偏差误差会随时间产生漂移，这样使得惯性导航(IMU)不能满足许多导航应用要求。
声学导航包括长基线(LBL)声学导航和超短基线(USBL)声学导航，长基线(LBL)声学导航中，水下机器人(AUV)需要将其传感器位置固定，限制任务覆盖范围。超短基线(USBL)声学导航不需要固定传感器，但需要将位置数据与水下机器人(AUV)机载数据相融合，舰船通过声学包将超短基线(USBL)位置数据传输给水下机器人(AUV)，这通常会导致时间和数据延迟。并且长基线(LBL)和超短基线(USBL)导航系统价格均很昂贵，不能实现普及应用。
发明内容
本发明的目的是提供一种基于IMU和TOF的水下机器人单信标导航方法，解决了由于信标和接收器之间的时钟同步不准确、自发射信标的不准确状态估计和影响水声信号传输的各种水下环境条件所造成的TOF测距错误数据的技术问题。
为实现上述目的，本发明采用如下技术方案：
一种基于IMU和TOF的水下机器人单信标导航方法，包括如下步骤：
步骤1：建立单信标测距导航系统、惯性导航系统和数据处理中心，在数据处理中心建立单信标测距导航数据采集模块、惯性导航数据采集模块、耦合距离滤波器、最大工作范围滤波器、状态接收阈值滤波器、动态模型模块、累加线性化模块、水下机器人位置模块、初始位置模块和误差计算模块；
单信标测距导航数据采集模块用于接收和存储单信标测距导航系统传输过来的位置数据声学包；
惯性导航数据采集模块用于接收并存储惯性导航系统传送过来的机器人姿态数据；
步骤2：惯性导航数据采集模块在接收到机器人姿态数据后，再将机器人姿态数据发送到耦合距离滤波器中进行滤波；
步骤3：动态模型模块根据耦合距离滤波器滤波后的数据进行水下机器人的动态位置计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新，水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤4：初始位置模块将第一次从动态模型模块中获取到的水下机器人的当前位置作为水下机器人的初始位置，并存储该初始位置；
步骤5：单信标测距导航数据采集模块将位置数据声学包中的数据放入最大工作范围滤波器进行滤波，判断位置数据声学包中的数据是否超出了最大工作范围：是，则执行步骤6；否，则执行步骤9；
步骤6：累加线性化模块将位置数据声学包中的位置数据通过一阶泰勒级数展开线性化，并对观测时间点的状态值处进行评估从而获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤7：状态接收滤波器对步骤6中获取到的水下机器人的当前位置进行滤波，判断是否超出了状态接收阈值：是，则执行步骤8；否，则执行步骤9；
步骤8：误差计算模块进行TOF测距累加误差计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤9：使用预定状态作为水下机器人的当前位置，并将当前位置发送给水下机器人位置模块进行位置更新。
优选的，所述单信标测距导航系统、所述惯性导航系统和所述数据处理中心通过无线网络或光线相互通信。
优选的，在执行步骤5时，最大工作范围滤波器用于防止在故障距离上处理测量更新方程，其判断如下：如果所述位置数据声学包中的数据范围大于设置的最大范围值，则丢弃该范围，并且不处理测量更新方程，通过多种方法确定最大工作范围值，包括基于环境的地理限制或者声学范围限制；
所述位置数据声学包中的数据为单信标测距范围数据。
优选的，在执行步骤7时，状态接收滤波器用于防止信标状态估计不准确而导致接收水下机器人的更新状态估计，在单信标测距导航系统传输过来的位置数据声学包中，从信标的最后一次单信标测距测量开始，跟踪先前状态估计的位置和时间，在确定更新状态之后，从上次距离测量时的水下机器人状态位置到当前水下机器人更新状态位置的距离，然后将此距离除以自上次单信标测距范围更新以来的时间，通过计算速度，与预定速度值进行比较：如果计算速度小于该预定速度值，则处理更新状态并确定加速度偏差测量；如果计算的速度大于预定的速度值，则忽略更新的状态估计，并向前传输水下机器人的预测状态。
优选的，在计算速度时，采用以下公式进行：
Figure GDA0003679096720000051
Δt＝k-t；
其中，x和y是水下机器人在世界坐标系中的估计位置坐标，vp为预定速度值，k和t分别是当前时间和最后一次范围更新时间。
优选的，在所述数据处理中心建立预测模块，预测模块用于建立预测模型，根据水下机器人初始位置信息，结合惯性导航数据采集模块采集到的机器人姿态数据获得环境状态信息和机器人位姿信息，预测出当前机器人的位置信息。
本发明所述的一种基于IMU和TOF的水下机器人单信标导航方法，解决了由于信标和接收器之间的时钟同步不准确、自发射信标的不准确状态估计和影响水声信号传输的各种水下环境条件所造成的TOF测距错误数据的技术问题，本发明提出了一种基于惯性测量单元(IMU)、水下机器人动态模型速度和单向行程时间声程测量(TOF)的单信标测距导航解决方案，实现低成本、高精度的水下机器人导航功能。
附图说明
图1为本发明的流程图；
图2为本发明的单信标测距导航范围的示意图。
具体实施方式
如图1-图2所示的一种基于IMU和TOF的水下机器人单信标导航方法，包括如下步骤：
步骤1：建立单信标测距导航系统、惯性导航系统和数据处理中心，在数据处理中心建立单信标测距导航数据采集模块、惯性导航数据采集模块、耦合距离滤波器、最大工作范围滤波器、状态接收阈值滤波器、动态模型模块、累加线性化模块、水下机器人位置模块、初始位置模块和误差计算模块；
单信标测距导航数据采集模块用于接收和存储单信标测距导航系统传输过来的位置数据声学包；
惯性导航数据采集模块用于接收并存储惯性导航系统传送过来的机器人姿态数据；
步骤2：惯性导航数据采集模块在接收到机器人姿态数据后，再将机器人姿态数据发送到耦合距离滤波器中进行滤波；
惯性导航通过测量水下机器人加速度、角速度、偏航角等信息，通过积分运算获得水下机器人实时速度和实时位置信息。
耦合距离滤波器的作用是滤除导致不稳定状态估计的距离测量数据，即有部分距离测量数据可能受到各类故障影响，产生奇异数据，而这写数据需要滤除掉；
故障范围包括：
1.由于发射机和接收机之间的时钟同步性差或声环境变化造成的。
2.声学环境由于可能存在多路径传播或反射现象的可能导致TOF测量数据出现错误。
本步骤中采用的耦合距离滤波器包括最大工作范围滤波器和状态接收阈值滤波器两部分，而耦合距离滤波器中的最大工作范围滤波器和状态接收阈值滤波器与在数据处理中心中建立的最大工作范围滤波器和状态接收阈值滤波器的原理相同。
步骤3：动态模型模块根据耦合距离滤波器滤波后的数据进行水下机器人的动态位置计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新，水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤4：初始位置模块将第一次从动态模型模块中获取到的水下机器人的当前位置作为水下机器人的初始位置，并存储该初始位置；
步骤5：单信标测距导航数据采集模块将位置数据声学包中的数据放入最大工作范围滤波器进行滤波，判断位置数据声学包中的数据是否超出了最大工作范围：是，则执行步骤6；否，则执行步骤9；
优选的，在执行步骤5时，最大工作范围滤波器用于防止在故障距离上处理测量更新方程，其判断如下：如果所述位置数据声学包中的数据范围大于设置的最大范围值，则丢弃该范围，并且不处理测量更新方程，通过多种方法确定最大工作范围值，包括基于环境的地理限制或者声学范围限制；
所述位置数据声学包中的数据为单信标测距范围数据。
步骤6：累加线性化模块将位置数据声学包中的位置数据通过一阶泰勒级数展开线性化，并对观测时间点的状态值处进行评估从而获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤7：状态接收滤波器对步骤6中获取到的水下机器人的当前位置进行滤波，判断是否超出了状态接收阈值：是，则执行步骤8；否，则执行步骤9；
优选的，在执行步骤7时，状态接收滤波器用于防止信标状态估计不准确而导致接收水下机器人的更新状态估计，在单信标测距导航系统传输过来的位置数据声学包中，从信标的最后一次单信标测距测量开始，跟踪先前状态估计的位置和时间，在确定更新状态之后，从上次距离测量时的水下机器人状态位置到当前水下机器人更新状态位置的距离，然后将此距离除以自上次单信标测距范围更新以来的时间，通过计算速度，与预定速度值进行比较：如果计算速度小于该预定速度值，则处理更新状态并确定加速度偏差测量；如果计算的速度大于预定的速度值，则忽略更新的状态估计，并向前传输水下机器人的预测状态。
优选的，在计算速度时，采用以下公式进行：
Figure GDA0003679096720000081
Δt＝k-t；
其中，x和y是水下机器人在世界坐标系中的估计位置坐标，vp为预定速度值，k和t分别是当前时间和最后一次范围更新时间。
单信标测距导航在运行过程中，由于发射机和接收机之间的时钟同步性差或水声环境发生变化等因素，会造成运行故障和导航错误信息，例如水下机器人和信标系统中任何一个时钟上的少量漂移都会改变声学包的TOL或TOA，从而改变范围计算；水下多路径传输和物体、地形反射改变了声学环境也可能导致发射信标和接收水下机器人之间的距离测量发生错误，从而使得单信标测距测量数据不准确；在处理距离测量时，一旦超出单信标测距范围，发射信标状态估计中的任何不准确数据都很容易导致接收水下机器人状态估计中的产生错误。
为了解决这些问题，本发明通过将数据与距离滤波器进行耦合，防止处理导致不稳定状态估计的距离测量数据，耦合距离滤波器使用两个不同的过程来防止本发明处理错误的距离测量。这两个过程是最大工作范围滤波器和状态接受滤波器。
步骤8：误差计算模块进行TOF测距累加误差计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤9：使用预定状态作为水下机器人的当前位置，并将当前位置发送给水下机器人位置模块进行位置更新。
优选的，所述单信标测距导航系统、所述惯性导航系统和所述数据处理中心通过无线网络或光线相互通信。
优选的，在所述数据处理中心建立预测模块，预测模块用于建立预测模型，根据水下机器人初始位置信息，结合惯性导航数据采集模块采集到的机器人姿态数据获得环境状态信息和机器人位姿信息，预测出当前机器人的位置信息。
预测模块根据以下公式来预测机器人的位置：
xk＝Fxk-1；
Pk＝FPk-1FT+Q；
其中，x为机器人当前位置，Q为过程协方差矩阵，P为信标预测位置，F为离散系统转移矩阵。
如图2所示，其中X轴为时间；Y轴为单信标测距距离，由图2可以看出，没有耦合的距离滤波器，处理所有接收到的原始单信标测距距离，产生不稳定的轨迹，相反，通过耦合距离滤波器，观察到水下机器人XY轨迹有了显著改善。
本发明结合发射时的信标发射时间(TOL)和水下机器人接收时间(TOA)之间的距离以矢量形式建模，同时考虑增加时不变噪声vrng，其建模公式如下所示：
zrng＝(xTMTMx)1/2+vrng；
其中，M＝[Jv-Jb]，vrng～N(0，Rrng)；
JV表示水下机器人和信标之间的TOA数据信息；
Jb表示水下机器人和信标之间的TOL数据信息，
vrng表示时不变噪声；
Rrng表示观测标准差；
T表示转置矩阵；
水下机器人位置方程：
Figure GDA0003679096720000101
其中，X表示水下机器人位置信息；
k表示工作时间序列；
Figure GDA0003679096720000102
表示测量映射矩阵；
本发明所述的一种基于IMU和TOF的水下机器人单信标导航方法，解决了由于信标和接收器之间的时钟同步不准确、自发射信标的不准确状态估计和影响水声信号传输的各种水下环境条件所造成的TOF测距错误数据的技术问题，本发明提出了一种基于惯性测量单元(IMU)、水下机器人动态模型速度和单向行程时间声程测量(TOF)的单信标测距导航解决方案，实现低成本、高精度的水下机器人导航功能。

Claims (6)

1.一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：包括如下步骤：
步骤1：建立单信标测距导航系统、惯性导航系统和数据处理中心，在数据处理中心建立单信标测距导航数据采集模块、惯性导航数据采集模块、耦合距离滤波器、最大工作范围滤波器、状态接收阈值滤波器、动态模型模块、累加线性化模块、水下机器人位置模块、初始位置模块和误差计算模块；
单信标测距导航数据采集模块用于接收和存储单信标测距导航系统传输过来的位置数据声学包；
惯性导航数据采集模块用于接收并存储惯性导航系统传送过来的机器人姿态数据；
步骤2：惯性导航数据采集模块在接收到机器人姿态数据后，再将机器人姿态数据发送到耦合距离滤波器中进行滤波；
步骤3：动态模型模块根据耦合距离滤波器滤波后的数据进行水下机器人的动态位置计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新，水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤4：初始位置模块将第一次从动态模型模块中获取到的水下机器人的当前位置作为水下机器人的初始位置，并存储该初始位置；
步骤5：单信标测距导航数据采集模块将位置数据声学包中的数据放入最大工作范围滤波器进行滤波，判断位置数据声学包中的数据是否超出了最大工作范围：是，则执行步骤6；否，则执行步骤9；
步骤6：累加线性化模块将位置数据声学包中的位置数据通过一阶泰勒级数展开线性化，并对观测时间点的状态值处进行评估从而获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤7：状态接收滤波器对步骤6中获取到的水下机器人的当前位置进行滤波，判断是否超出了状态接收阈值：是，则执行步骤8；否，则执行步骤9；
步骤8：误差计算模块进行TOF测距累加误差计算，获得水下机器人的当前位置，并将水下机器人的当前位置发送给水下机器人位置模块进行位置更新；水下机器人位置模块将更新后的水下机器人的当前位置作为预定状态；
步骤9：使用预定状态作为水下机器人的当前位置，并将当前位置发送给水下机器人位置模块进行位置更新。
2.如权利要求1所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：所述单信标测距导航系统、所述惯性导航系统和所述数据处理中心通过无线网络或光线相互通信。
3.如权利要求1所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：在执行步骤5时，最大工作范围滤波器用于防止在故障距离上处理测量更新方程，其判断如下：如果所述位置数据声学包中的数据范围大于设置的最大范围值，则丢弃该范围，并且不处理测量更新方程，通过多种方法确定最大工作范围值，包括基于环境的地理限制或者声学范围限制；
所述位置数据声学包中的数据为单信标测距范围数据。
4.如权利要求1所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：在执行步骤7时，状态接收滤波器用于防止信标状态估计不准确而导致接收水下机器人的更新状态估计，在单信标测距导航系统传输过来的位置数据声学包中，从信标的最后一次单信标测距测量开始，跟踪先前状态估计的位置和时间，在确定更新状态之后，从上次距离测量时的水下机器人状态位置到当前水下机器人更新状态位置的距离，然后将此距离除以自上次单信标测距范围更新以来的时间，通过计算速度，与预定速度值进行比较：如果计算速度小于该预定速度值，则处理更新状态并确定加速度偏差测量；如果计算的速度大于预定的速度值，则忽略更新的状态估计，并向前传输水下机器人的预测状态。
5.如权利要求4所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：在计算速度时，采用以下公式进行：
Figure FDA0003679096710000031
Δt＝k-t；
其中，x和y是水下机器人在世界坐标系中的估计位置坐标，vp为预定速度值，k和t分别是当前时间和最后一次范围更新时间。
6.如权利要求1所述的一种基于IMU和TOF的水下机器人单信标导航方法，其特征在于：在所述数据处理中心建立预测模块，预测模块用于建立预测模型，根据水下机器人初始位置信息，结合惯性导航数据采集模块采集到的机器人姿态数据获得环境状态信息和机器人位姿信息，预测出当前机器人的位置信息。
CN202010382088.3A 2020-05-08 2020-05-08 一种基于imu和tof的水下机器人单信标导航方法 Active CN111708008B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010382088.3A CN111708008B (zh) 2020-05-08 2020-05-08 一种基于imu和tof的水下机器人单信标导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010382088.3A CN111708008B (zh) 2020-05-08 2020-05-08 一种基于imu和tof的水下机器人单信标导航方法

Publications (2)

Publication Number Publication Date
CN111708008A CN111708008A (zh) 2020-09-25
CN111708008B true CN111708008B (zh) 2022-08-05

Family

ID=72536438

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010382088.3A Active CN111708008B (zh) 2020-05-08 2020-05-08 一种基于imu和tof的水下机器人单信标导航方法

Country Status (1)

Country Link
CN (1) CN111708008B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112923920B (zh) * 2021-01-27 2022-08-26 嘉兴中科声学科技有限公司 故障标记、导航方法、装置、电子设备及存储介质
CN114384939B (zh) * 2022-03-24 2022-07-29 江苏深瑞光学技术有限公司 一种微型水下探测机器人自主导航系统

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11115798B2 (en) * 2015-07-23 2021-09-07 Irobot Corporation Pairing a beacon with a mobile robot
CN107990891B (zh) * 2016-10-26 2021-05-28 中国科学院沈阳自动化研究所 基于长基线和信标在线标定的水下机器人组合导航方法
WO2019239365A1 (en) * 2018-06-13 2019-12-19 Purohit Ankit System and method for position and orientation tracking of multiple mobile devices
CN110196047A (zh) * 2019-06-20 2019-09-03 东北大学 基于tof深度相机与imu的平仓机器人自主定位方法

Also Published As

Publication number Publication date
CN111708008A (zh) 2020-09-25

Similar Documents

Publication Publication Date Title
Kepper et al. A navigation solution using a MEMS IMU, model-based dead-reckoning, and one-way-travel-time acoustic range measurements for autonomous underwater vehicles
JP5673071B2 (ja) 位置推定装置及びプログラム
CN108594272B (zh) 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法
KR100906362B1 (ko) 2개의 기준점에 대한 거리정보와 저정밀도 관성센서를 이용한 무인잠수정 선단의 의사 lbl 수중항법시스템
CN1325932C (zh) 载人潜水器的组合导航定位方法
CN111595348B (zh) 一种自主水下航行器组合导航系统的主从式协同定位方法
CN110554359B (zh) 一种融合长基线与单信标定位的海底飞行节点定位方法
CN111596333B (zh) 一种水下定位导航方法及系统
CN101900558A (zh) 集成声纳微导航的自主式水下机器人组合导航方法
CN110186461B (zh) 一种基于重力梯度信息测距的协同导航方法
KR101789188B1 (ko) 수중 이동체의 위치 추적을 위한 수중 통합 항법 시스템
Ridao et al. USBL/DVL navigation through delayed position fixes
CN108344415A (zh) 一种组合导航信息融合方法
WO2016203744A1 (ja) 測位装置
CN111708008B (zh) 一种基于imu和tof的水下机器人单信标导航方法
CN109855621A (zh) 一种基于uwb与sins的组合室内行人导航系统及方法
CN113048983B (zh) 一种异时序贯量测的改进分层式auv协同导航定位方法
Song et al. Underwater adaptive height-constraint algorithm based on SINS/LBL tightly coupled
Allotta et al. Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements
WO2020030966A1 (en) Methods and systems for estimating the orientation of an object
CN116222578B (zh) 基于自适应滤波和最优平滑的水下组合导航方法及系统
KR20160143438A (ko) 추측 항법 시스템에서의 밀결합 측위 방법 및 그 장치
CN110954097A (zh) 一种用于机器人组合的导航定位方法
CN108828644B (zh) Gnss/mems紧组合导航系统中动态突变识别方法
Quraishi et al. Easily deployable underwater acoustic navigation system for multi-vehicle environmental sampling applications

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