CN111596333B - 一种水下定位导航方法及系统 - Google Patents

一种水下定位导航方法及系统 Download PDF

Info

Publication number
CN111596333B
CN111596333B CN202010506455.6A CN202010506455A CN111596333B CN 111596333 B CN111596333 B CN 111596333B CN 202010506455 A CN202010506455 A CN 202010506455A CN 111596333 B CN111596333 B CN 111596333B
Authority
CN
China
Prior art keywords
underwater
navigation equipment
position information
underwater navigation
information
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
CN202010506455.6A
Other languages
English (en)
Other versions
CN111596333A (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.)
Peng Cheng Laboratory
Original Assignee
Peng Cheng Laboratory
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 Peng Cheng Laboratory filed Critical Peng Cheng Laboratory
Priority to CN202010506455.6A priority Critical patent/CN111596333B/zh
Publication of CN111596333A publication Critical patent/CN111596333A/zh
Application granted granted Critical
Publication of CN111596333B publication Critical patent/CN111596333B/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
    • 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
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection
    • 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
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

本发明实施例提供了一种水下定位导航方法及系统，用于拓展水下航行设备的导航范围，提高对水下航行设备的定位精度。本发明实施例方法包括：可移动的水面导航设备以一定时间间隔不断获取水下航行设备的相对位置信息；所述水面导航设备将所述相对位置信息、GPS位置信息及姿态信息作为第一辅助信息发送给所述水下航行设备；所述水下航行设备根据所述第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息。水下航行设备的实时精确位置信息用于自身导航，并将精确位置信息发送水面导航设备，用于精确跟踪，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。

Description

一种水下定位导航方法及系统
技术领域
本发明涉及水下定位导航技术领域，尤其涉及一种水下定位导航方法及系统。
背景技术
海洋中蕴含着大量的矿产资源、海水化学资源、海洋生物资源、海洋能和海洋空间资源。自主水下机器人(Autonomous Underwater Vehicle，水下航行设备)以其自主性好、灵活性强、体积小、质量轻、活动范围广及隐蔽性好等优点，在海洋探索与开发中发挥着重要作用。水下航行设备要在复杂的水下环境下完成各种任务，就必须要有一个性能好、可靠性高的定位导航系统，它就像一双眼睛指引着水下航行设备沿着规划路径完成任务。
现有的水下航行设备主要依靠捷联惯导系统进行姿态和位置的自主推算，从而实现定位导航。捷联惯导系统根据加速度计的比力数据和陀螺仪的角加速度数据，进行速度更新、姿态更新、位置更新，这是一种自主导航的方式，有着实现简单、隐蔽性能好的优点。然而捷联惯导系统采用了积分的方式计算位置，随着时间的增加，速度、姿态、位置都会出现越来越大的误差。
有鉴于此，有必要提出一种新的水下定位导航方法。
发明内容
本发明实施例提供了一种水下定位导航方法及系统，用于拓展水下航行设备的导航范围，提高对水下航行设备的定位精度。
本发明实施例第一方面提供了一种水下定位导航方法，运用于水下航行设备的定位导航，可包括：
可移动的水面导航设备以一定时间间隔不断获取水下航行设备的相对位置信息；
所述水面导航设备将所述相对位置信息、GPS位置信息及姿态信息作为第一辅助信息发送给所述水下航行设备；
所述水下航行设备根据所述第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航方法，还可以包括：
所述水面导航设备根据所述相对位置信息、GPS位置信息及姿态信息计算所述水下航行设备的粗略GPS位置信息；
所述水面导航设备根据所述水下航行设备的粗略GPS位置信息跟踪所述水下航行设备移动，以使得所述水面导航设备与所述水下航行设备的距离小于预设阈值。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航方法中，所述水下航行设备自身的多种传感器信息包括罗盘测得的航向角、惯性测量单元测得的线速度和角速度、深度计测量的深度参数以及声学多普勒效应的测速设备测得载体相对于海底或海水层的速度，所述水下航行设备根据所述第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息包括：
水下航行设备采用扩展卡尔曼滤波算法对所述第一辅助信息中的相对位置信息及所述多种传感器信息进行融合计算，得到水下航行设备的实时状态估计值；
水下航行设备根据所述第一辅助信息中的GPS位置信息及姿态信息以及自身的实时状态估计值计算自身的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航方法中，所述水面导航设备以一定时间间隔不断获取水下航行设备的相对位置信息，包括：
水面导航设备以一定时间间隔不断采用超短基线获取水下航行设备的相对位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航方法还可以包括：所述水下航行设备将自身的实时精确位置信息发送给所述水面导航设备，使得所述水面导航设备精确跟踪所述水下航行设备，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。
本发明实施例第二方面提供了一种水下定位导航方法，运用于多个水下航行设备的定位导航，可包括：
可移动的水面导航设备以一定时间间隔不断获取各个水下航行设备的相对位置信息，并发送给对应的水下航行设备；
水面导航设备将GPS位置信息及姿态信息作为第二辅助信息发送给各个水下航行设备；
各个水下航行设备根据自身的多种传感器信息、各自相对位置信息及所述第二辅助信息分别计算各自的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航方法，还可以包括：
水面导航设备根据各个水下航行设备的相对位置信息及第二辅助信息计算各个水下航行设备的粗略GPS位置信息，并发送给对应的水下航行设备；
水下航行设备根据所述粗略GPS位置信息跟踪所述水面导航设备移动，以使得所述水面导航设备与各个水下航行设备的距离小于预设阈值。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航方法中，所述各个水下航行设备根据自身的多种传感器信息、各自初始相对位置信息及所述第二辅助信息分别计算各自的实时精确位置信息，包括：
各个水下航行设备采用扩展卡尔曼滤波算法对自身的相对位置信息及所述多种传感器信息进行融合计算，得到各自的实时状态估计值；
各个水下航行设备根据所述第二辅助信息及各自的实时状态估计值计算各自的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航方法中，所述水面导航设备以一定时间间隔不断获取各个水下航行设备的相对位置信息，包括：
水面导航设备以一定时间间隔不断采用超短基线获取各个水下航行设备的初始相对位置信息。
本发明实施例第三方面提供了一种水下定位导航系统，可包括单个水下航行设备及可移动的水面导航设备，其中，所述水面导航设备用于获取水下航行设备的相对位置信息；
所述水面导航设备还用于将所述相对位置信息、GPS位置信息及姿态信息作为第一辅助信息发送给所述水下航行设备；
所述水下航行设备根据所述第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，所述水面导航设备还用于根据所述相对位置信息、GPS位置信息及姿态信息计算所述水下航行设备的粗略GPS位置信息；
所述水面导航设备还用于根据所述水下航行设备的粗略GPS位置信息跟踪所述水下航行设备移动，以使得所述水面导航设备与所述水下航行设备的距离小于预设阈值。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，所述水下航行设备自身的多种传感器信息包括罗盘测得的航向角、惯性测量单元测得的线速度和角速度、深度计测量的深度参数以及声学多普勒效应的测速设备测得载体相对于海底或海水层的速度，所述水下航行设备根据所述第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息可以包括：
水下航行设备采用扩展卡尔曼滤波算法对所述第一辅助信息中的相对位置信息及所述多种传感器信息进行融合计算，得到水下航行设备的实时状态估计值；
水下航行设备根据所述第一辅助信息中的GPS位置信息及姿态信息以及自身的实时状态估计值计算自身的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，所述水面导航设备以一定时间间隔不断获取水下航行设备的相对位置信息，可以包括：
水面导航设备以一定时间间隔不断采用超短基线获取水下航行设备的相对位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，所述水下航行设备将自身的实时精确位置信息发送给所述水面导航设备，使得所述水面导航设备精确跟踪所述水下航行设备，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。
本发明实施例第四方面提供了一种水下定位导航系统，可包括多个水下航行设备及可移动的水面导航设备，其中，
所述水面导航设备用于以一定时间间隔不断获取各个水下航行设备的相对位置信息，并发送给对应的水下航行设备；
所述水面导航设备还用于将GPS位置信息及姿态信息作为第二辅助信息发送给各个水下航行设备；
各个所述水下航行设备用于根据自身的多种传感器信息、各自相对位置信息及所述第二辅助信息分别计算各自的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，水面导航设备还用于根据各个水下航行设备的相对位置信息及第二辅助信息计算各个水下航行设备的粗略GPS位置信息，并发送给对应的水下航行设备；
水下航行设备还用于根据所述粗略GPS位置信息跟踪所述水面导航设备移动，以使得所述水面导航设备与各个水下航行设备的距离小于预设阈值。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，所述各个水下航行设备根据自身的多种传感器信息、各自相对位置信息及所述第二辅助信息分别计算各自的实时精确位置信息，具体可包括：
各个水下航行设备采用扩展卡尔曼滤波算法对自身的相对位置信息及所述多种传感器信息进行融合计算，得到各自的实时状态估计值；
各个水下航行设备根据所述第二辅助信息及各自的实时状态估计值计算各自的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，所述水面导航设备以一定时间间隔不断获取各个水下航行设备的相对位置信息，具体可包括：
水面导航设备以一定时间间隔不断采用超短基线获取各个水下航行设备的初始相对位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，所述水下航行设备根据自身的实时精确位置信息发送给所述水面导航设备，并精确跟踪所述水面导航设备，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。
从以上技术方案可以看出，本发明实施例具有以下优点：
本发明实施例中设置了可移动的水面导航设备获取水下航行设备的相对位置信息，并将相对位置信息、GPS位置信息及姿态信息作为第一辅助信息解算出水下航行设备的粗略位置，并对其进行粗跟踪，同时发送给所述水下航行设备。水下航行设备根据第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息，用于自身导航，并将精确位置信息发送水面导航设备，用于精确跟踪，确保通信和定位链路的稳定，如此往复。相对于现有的水下导航方法，本发明实施例中采用可移动的水面导航设备和水下航行设备进行粗跟踪和精确跟踪，保持通信和定位链路的稳定，可以拓展水下航行设备的导航范围，而且采用了多种传感器信息进行融合计算，可以提高对水下航行设备的定位精度。
附图说明
图1为本发明实施例中一种水下定位导航方法的一个实施例示意图；
图2为本发明实施例中一种水下定位导航方法的另一个实施例示意图；
图3为本发明实施例中一种水下定位导航方法的一个具体应用实施例示意图；
图4为本发明实施例中一种水下定位导航方法的另一个具体应用实施例示意图；
图5为本发明实施例中一种水下定位导航系统的一个实施例示意图；
图6为本发明实施例中一种水下定位导航系统的另一个实施例示意图。
具体实施方式
本发明实施例提供了一种水下定位导航方法及系统，用于拓展水下航行设备的导航范围，提高对水下航行设备的定位精度。
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
水下导航技术能为水下航行设备提供准确的速度、姿态和位置信息，是决定水下航行设备能否准确抵达预定地点，顺利完成任务并安全返回的关键。本发明提出的水下定位导航方法，引入可移动的水面导航设备进行辅助定位，在运动过程中能够不断获取到水下航行设备的准确GPS坐标，克服捷联惯性导航系统误差累计的问题，提高水下航行设备动态移动过程中的导航准确性。本发明实施例具体可以运用于两种工况：一、单个水下航行设备的定位导航；二、多个水下航行设备的定位导航。
为了便于理解，下面对本发明实施例中的具体流程对第一种工况进行描述，请参阅图1，本发明实施例中一种水下定位导航方法的一个实施例可包括：
101、可移动的水面导航设备以一定时间间隔不断获取水下航行设备的相对位置信息；
由于水下航行设备在水下作业时，受水介质对无线电波强烈吸收效应的影响，以卫星导航为代表的无线电导航系统无法在水下为水下航行设备导航。有鉴于此，本发明实施例中采用水面导航设备辅助水下航行设备进行定位。
水面导航设备首先需要获取水下航行设备相对于水面导航设备的相对位置信息，具体的相对位置信息获取方式此处不做限定，示例性的，水面导航设备可以基于超短基线(Ultra-Short Base Line,USBL)方式获取水下航行设备的相对位置信息。
102、水面导航设备将相对位置信息、GPS位置信息及姿态信息作为第一辅助信息发送给水下航行设备；
为了实现定位，本发明实施例中的水面导航设备中还可以设置差分全球定位系统(Differential Global Position System，DGPS)及惯性测量单元(Inertial measurementunit，简称IMU)，分别用于获取水面导航设备GPS位置信息、测量水面导航设备三轴姿态角(或角速率)以及加速度等姿态信息。
水面导航设备可以将相对位置信息、GPS位置信息及姿态信息作为第一辅助信息周期性的发送给水下航行设备。
103、水下航行设备根据第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息。
水下航行设备在动态运动过程中，在下一个周期接收到新的第一辅助信息之前，可以根据当前的第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息。
具体的，以采用超短基线(USBL)获取水下航行设备的相对位置信息为例，水下航行设备自身的多种传感器信息可以包括罗盘测得的航向角Ψ，惯性测量单元(IMU)测得的线速度和角速度u、v、ω、r，深度计测量的深度参数z。水下航行设备可以采用扩展卡尔曼滤波算法对第一辅助信息及自身的多种传感器信息进行计算，得到自身的实时精确位置信息，具体实现过程可以参照如下过程：
1、水下航行设备在水下运动时，可忽略其横滚和纵倾两个自由度，使用四自由度对水下航行设备进行如下建模：
x＝[ηT vT]T＝[x y z ψ u v ω r]
其中，x为水下航行设备的状态向量，η为水下航行设备的位置向量，v为水下航行设备的速度向量，x、y、z、ψ为水下航行设备的位置坐标和航向角，u、v、ω、r为水下航行设备对应的线速度和角速度。
2、确定水下航行设备的初始状态向量，初始化时，假定水下航行设备处于静止状态，根据第一辅助信息和罗盘测得的航向角ψC可以确定初始状态向量：
Figure BDA0002526722550000091
其中，xU、yU、zU为第一辅助信息中的相对位置信息。初始估计误差协方差矩阵如下：
Figure BDA0002526722550000092
其中，下标U表示USBL，下表C表示罗盘，参考坐标系为北东地坐标系，
Figure BDA0002526722550000093
表示使用USBL测量的x的误差方差，σUxy表示使用USBL测量的x与y的误差协方差，
Figure BDA0002526722550000094
为航向角的方差，
Figure BDA0002526722550000095
分别为u、v、ω、r的方差。
若水下航行设备使用匀速运动学模型，则其状态转移方程为：
xk＝g(xk-1,nk-1)，展开如下：
Figure BDA0002526722550000096
其中，n＝[nu nv nω nr]T为零均值高斯白噪声，nu、nv、nω、nr对应u、v、ω、r的零均值高斯噪声。
向量n的协方差由系统噪声矩阵Qk表示：
Figure BDA0002526722550000101
其中，
Figure BDA0002526722550000102
表示nu的方差，矩阵中其他参数含义依此类推。
4、各种传感器获取到不同的信息，系统的观测矩阵为：
zk＝Hxk+mk，展开如下：
Figure BDA0002526722550000103
其中，U表示USBL，C表示罗盘，D表示DVL(Doppler Velocity Log，基于声学多普勒效应的测速设备)，DVL可以测得载体相对于海底或海水层的速度，能提供较高精度的载体速度信息,且其误差不随时间积累。P表示深度计，m为测量零均值高斯白噪声，
Figure BDA0002526722550000104
表示DVL测量的u零均值高斯白噪声，向量中其他参数含义依此类推。
向量m的协方差由测量噪声协方差矩阵Rk表示：
Figure BDA0002526722550000111
其中，σDuv为DVL测量u与v的噪声协方差，矩阵中其他参数含义依此类推，矩阵中各个元素的值根据不同传感器的规格确定。
5、水下航行设备的初始状态、状态转移方程及观测方程确定后，可以得到水下航行设备的状态估计值
Figure BDA0002526722550000112
在获得水下航行设备的状态估计值之后，可对惯性导航产生的累计误差进行修正，结合DGPS设备获得的GPS位置信息，可以获取到水下航行设备精确GPS位置。
具体的，可以根据水面导航设备的位姿、DGPS位置信息及USBL收发器及DGPS的安装位置，将获取到的AUV的相对位置转换成粗略的GPS位置信息，其转换公式如下：
[BAUV LAUV HAUV]T＝f(Γ[xT yT zT]T)
其中，[BAUV LAUV HAUV]T是水下航行设备的精确GPS位置信息；Γ是转换矩阵，将USBL收发器安装位置转换到DGPS安装位置，该矩阵由USBL的位姿、USBL收发器安装位置及DGPS安装位置决定；f是直角坐标系到大地坐标系的映射，根据不同的地球模型选择不同的映射。
本发明实施例中设置了可移动的水面导航设备获取水下航行设备的相对位置信息，并将相对位置信息、GPS位置信息及姿态信息作为第一辅助信息发送给水下航行设备。水下航行设备根据第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息。相对于现有的水下导航方法，本发明实施例中采用可移动的水面导航设备可以拓展水下航行设备的导航范围，而且采用了多种传感器信息进行融合计算，可以提高对水下航行设备的定位精度。
在上述图1所示的实施例的基础上，为了提高水下航行设备与水面导航设备之间通信和定位的精度，需要维持水下航行设备与水面导航设备的相对距离在预设阈值范围之内。为此，本发明实施例中，水面导航设备可以根据相对位置信息、GPS位置信息及姿态信息计算出水下航行设备的粗略GPS位置信息；水面导航设备根据该粗略GPS位置信息跟踪水下航行设备移动，以使得水面导航设备与水下航行设备的距离小于预设阈值。
具体的，水面导航设备通过USBL的收发器主动发出水声定位信号，水下航行设备端的应答器收到水声定位信号后发送应答信号，水面导航设备上的USBL收发器根据收到的应答信号，解算出AUV的相对位置xU、yU、zU。根据水面导航设备的位姿、DGPS位置信息及USBL收发器及DGPS的安装位置，将获取到的水下航行设备的相对位置转换成粗略的GPS位置信息，其转换公式如下：
[BAUV LAUV HAUV]T＝f(Γ[xU yU zU]T)
其中，[BAUV LAUV HAUV]T是水下航行设备的粗略GPS位置信息；Γ是转换矩阵，将USBL收发器安装位置转换到DGPS安装位置，该矩阵由USBL的位姿、USBL收发器安装位置及DGPS安装位置决定；f是直角坐标系到大地坐标系的映射，根据不同的地球模型选择不同的映射。可移动的水面导航设备根据水下航行设备的粗略GPS位置信息对水下航行设备进行粗跟踪，使得水面导航设备一直处于水下航行设备的一定范围内。
下面将结合图2，对本发明实施例中的第二种工况进行描述，本发明实施例中一种水下定位导航方法的另一个实施例可包括：
201、可移动的水面导航设备以一定时间间隔不断获取各个水下航行设备的相对位置信息，并发送给对应的水下航行设备；
当可移动的水面导航设备需要对多个水下航行设备组成的集群进行导航时，首先需要以一定时间间隔不断获取各个水下航行设备的相对位置信息，并发送给对应的水下航行设备，使得每个水下航行设备可以得到各自的相对位置信息。
202、水面导航设备将GPS位置信息及姿态信息作为第二辅助信息发送给各个水下航行设备；
水面导航设备将GPS位置信息及姿态信息作为第二辅助信息周期性的发送给各个水下航行设备。
203、各个水下航行设备根据自身的多种传感器信息、各自相对位置信息及第二辅助信息分别计算各自的实时精确位置信息。
水下航行设备在动态运动过程中，在下一个周期接收到新的第二辅助信息及新的相对位置信息之前，可以根据当前的第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息。具体的单个水下航行设备可以采用扩展卡尔曼滤波算法对第一辅助信息及自身的多种传感器信息进行计算，得到自身的实时精确位置信息，具体的实现过程可以参照图1所示的实施例中的步骤103中的示例，具体此处不再赘述。
本发明实施例中设置了可移动的水面导航设备获取水下航行设备的相对位置信息，并将相对位置信息、GPS位置信息及姿态信息作为第二辅助信息发送给水下航行设备。水下航行设备根据第二辅助信息及自身的多种传感器信息计算自身的实时精确位置信息。相对于现有的水下导航方法，本发明实施例中采用可移动的水面导航设备可以拓展水下航行设备的导航范围，而且采用了多种传感器信息进行融合计算，可以提高对水下航行设备的定位精度。
在上述图2所示的实施例的基础上，为了提高水下航行设备与水面导航设备之间通信和定位的精度，需要维持水下航行设备与水面导航设备的相对距离在预设阈值范围之内。为此，本发明实施例中，水面导航设备可以根据各个水下航行设备的相对位置信息及第二辅助信息计算各个水下航行设备的粗略GPS位置信息，并将此GPS信息及水面导航设备的位置及航行信息发送给对应的水下航行设备，水下航行设备根据粗略GPS位置信息跟踪水面导航设备移动，以使得水面导航设备与各个水下航行设备的距离小于预设阈值。
为了便于理解，下面将结合具体的运用实施例对本发明实施例中的水下定位导航方法进行描述。具体运用实施例中的水下航行设备对应为AUV(水下机器人，AutonomousUnderwater Vehicle)，水面导航设备对应为USV(无人水面艇，Unmanned SurfaceVehicle)。该AUV中搭载有DVL(Doppler Velocity Log，基于声学多普勒效应的测速设备)、IMU(惯性测量单元，Inertial measurement unit)、罗盘、深度计及水声通信定位一体机(含USBL应答器)。该USV中也设置有IMU、罗盘及水声通信定位一体机(USBL收发器)。具体的工况也分为两种：第一种工况下AUV领航，USV对单个AUV进行跟踪从而保持动态条件下的水声定位和通信链路的相对稳定。第二种工况与第一种工况的不同之处在于USV领航，AUV集群对USV进行跟踪，在将AUV的粗略位置发送给对应AUV时也一并将USV的位置、速度及航向等航行信息发出，AUV接收到USV航行信息后进行粗略跟踪，解算出自身精确位置信息后进行精确跟踪。
请参阅图3，下面将对第一种工况进行描述。第一种工况下，USV上的USBL收发器能够获取到AUV的相对位置，结合DGPS、IMU及罗盘的信息能够解算出AUV粗略的GPS位置，USV根据解算出的粗略位置对AUV进行粗略跟踪，同时将AUV的粗略位置发送给AUV本体，该粗略位置信息与AUV上搭载的DVL、IMU、罗盘及深度计的信息进行融合，能够计算出AUV精确的GPS位置，并将该信息发送回USV，USV对AUV进行精确跟踪。这样既能减小捷联惯性导航系统的累计误差，减轻DVL对地失锁的影响，又能使AUV获取到精确的GPS位置信息，提高AUV的定位导航精度，提高USBL的定位精度，扩展USBL的作用范围。
请参阅图3，第一种工况下的具体实现流程如下：
第一步，初始化，包括下水前的一系列准备工作，如AUV与USV时间同步、AUV初始位置确定等。
第二步，USBL定位，USV上USBL的收发器主动发出水声定位信号，AUV端的应答器收到水声定位信号后发送应答信号，USV上的USBL收发器根据收到的应答信号，解算出AUV的相对位置xU、yU、zU。
第三步，根据USV的位姿、DGPS位置信息及USBL收发器及DGPS的安装位置，将获取到的AUV的相对位置转换成粗略的GPS位置信息，其转换公式如下：
[BAUV LAUV HAUV]T＝f(Γ[xU yU zU]T)
其中，[BAUV LAUV HAUV]T是AUV的粗略GPS位置信息；Γ是转换矩阵，将USBL收发器安装位置转换到DGPS安装位置，该矩阵由USBL的位姿、USBL收发器安装位置及DGPS安装位置决定；f是直角坐标系到大地坐标系的映射，根据不同的地球模型选择不同的映射。
第四步，USV根据AUV的粗略GPS位置信息对AUV进行粗跟踪，使得USV一直处于AUV的一定范围内，同时将USV上的USBL定位信息，DGPS定位信息等通过水声通信发送给AUV。
第五步，AUV将接收到的USBL定位信息、DGPS定位信息与搭载的DVL、IMU、罗盘及深度计的信息进行融合，获得AUV的精确GPS位置信息。
接下来阐述使用扩展卡尔曼滤波(Extended Kalman Filter，EKF)对传感器信息进行融合的过程。
1、AUV在水下运动时，可忽略其横滚和纵倾两个自由度，故使用四自由度对AUV进行如下建模：
x＝[ηT vT]T＝[x y z ψ u v ω r]
其中，x为AUV的状态向量，η为AUV的位置向量，v为AUV的速度向量，x、y、z、ψ为AUV的位置坐标和航向角，u、v、ω、r为AUV对应的线速度和角速度。
2、确定AUV的初始状态向量，初始化时，假定AUV处于静止状态，根据USBL和罗盘的数据可以确定初始状态向量：
Figure BDA0002526722550000151
初始估计误差协方差矩阵为：
Figure BDA0002526722550000161
其中，下标U表示USBL，下表C表示罗盘，参考坐标系为北东地坐标系，
Figure BDA0002526722550000162
表示使用USBL测量的x的误差方差，σUxy表示使用USBL测量的x与y的误差协方差，
Figure BDA0002526722550000163
为航向角的方差，
Figure BDA0002526722550000164
分别为u、v、ω、r的方差。
3、假设AUV使用匀速运动学模型，则其状态转移方程为：
xk＝g(xk-1,nk-1)，扩展如下：
Figure BDA0002526722550000165
其中，n＝[nu nv nωnr]T为零均值高斯白噪声，nu、nv、nω、nr对应u、v、ω、r的零均值高斯噪声。
向量n的协方差由系统噪声矩阵Qk表示：
Figure BDA0002526722550000171
其中，
Figure BDA0002526722550000172
表示nu的方差，矩阵中其他参数含义依此类推。
4、各种传感器获取到不同的信息，系统的观测矩阵为：
zk＝Hxk+mk，扩展如下：
Figure BDA0002526722550000173
其中，U表示USBL，C表示罗盘，D表示DVL，P表示深度计，m为测量零均值高斯白噪声，
Figure BDA0002526722550000174
表示DVL测量的u零均值高斯白噪声，向量中其他参数含义依此类推。
向量m的协方差由测量噪声协方差矩阵Rk表示：
Figure BDA0002526722550000175
其中，σDuv为DVL测量u与v的噪声协方差，矩阵中其他参数含义依此类推，矩阵中各个元素的值根据不同传感器的规格确定。
5、AUV的初始状态、状态转移方程及观测方程确定后，可以使用EKF对传感器信息进行处理，可以得到更优的状态估计值
Figure BDA0002526722550000181
并能对惯性导航产生的累计误差进行修正。
6、根据得到的更优的状态估计值
Figure BDA0002526722550000182
及DGPS的信息，可以获取到更优的AUV精确GPS位置(使用更优的状体估计值代入即可得到更优的AUV精确GPS位置)，将该位置通过水声通信发送到USV，USV收到后对AUV进行精确跟踪，在一定程度上达到USV与AUV相对位置不变，为水声定位和通信提供更好的条件。
请参阅图4，具体运用实施例中的第二种工况进行描述。
第二种工况的工作流程如图4所示，与第一种工况的主要区别为USV是对AUV集群中每个个体进行定位，然后将AUV集群的位置信息和USV本体的航行信息发送信息给AUV集群，AUV智能决策与系统模块根据这些信息对USV进行粗跟踪，AUV集群结合自身传感器信息解算出自身精确位置信息后，除要将精确位置信息等发送回USV本体供USV本体上的智能决策与协同模块使用外，也要将精确位置交由AUV智能决策与协同模块进行处理，以便对USV进行精跟踪，其他步骤与第一种工况类似，此处不再赘述。
本发明中提出的方法与现有方法对比具有如下优点：
(1)将USV的USBL定位信息与DGPS的信息发送到水下AUV，其能够获取到自身精确的GPS位置信息，实现了AUV自主导航；
(2)USV与AUV先进行粗略跟踪，再进行精确跟踪，如此往复，能够保证USV与AUV相对位置的稳定，从而使得动态水声通信和定位链路相对稳定，有利于水声通信与定位，为AUV执行复杂任务提供基本条件；
(3)USBL一般搭载于母船或者浮标上，这严重制约了USBL的有效范围，搭载USBL的USV具有极大的机动性，能够极大的扩展USBL的定位范围；
(4)本发明中的方法不仅使用于单个AUV，也适用于AUV集群，既能保证AUV集群的定位与导航，又能有效的共享AUV集群的信息，为AUV集群的智能决策和协同提供基本保障；
(5)本发明的方法能够克服传统水下机器人定位导航的缺点，利用多种传感器的信息，大大提高AUV定位精度和自主导航能力。
请参阅图5，本发明实施例还提供了一种水下定位导航系统，可包括水下航行设备(AUV)及可移动的水面导航设备(USV)，其中，水面导航设备用于获取水下航行设备的相对位置信息；
水面导航设备还用于将相对位置信息、GPS位置信息及姿态信息作为第一辅助信息发送给水下航行设备；
水下航行设备根据第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，水面导航设备还用于根据相对位置信息、GPS位置信息及姿态信息计算水下航行设备的粗略GPS位置信息；
水面导航设备还用于根据GPS位置信息跟踪水下航行设备移动，以使得水面导航设备与水下航行设备的距离小于预设阈值。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，水下航行设备自身的传感器信息包括罗盘测得的航向角、惯性测量单元测得的线速度和角速度、深度计测量的深度参数以及声学多普勒效应的测速设备测得载体相对于海底或海水层的速度，水下航行设备根据第一辅助信息及自身的多种传感器信息计算自身的实时精确位置信息可以包括：
水下航行设备采用扩展卡尔曼滤波算法对第一辅助信息中的相对位置信息及传感器信息进行融合计算，得到水下航行设备的实时状态估计值；
水下航行设备根据第一辅助信息中的GPS位置信息及姿态信息以及自身的实时状态估计值计算自身的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，水面导航设备以一定时间间隔不断获取水下航行设备的相对位置信息，可以包括：
水面导航设备以一定时间间隔不断采用超短基线获取水下航行设备的初始相对位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，水下航行设备将自身的实时精确位置信息发送给水面导航设备，使得所述水面导航设备精确跟踪水下航行设备，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。
请参阅图6，本发明实施例还提供了一种水下定位导航系统，可包括多个水下航行设备(AUV)及可移动的水面导航设备(USV)，其中，
水面导航设备用于以一定时间间隔不断获取各个水下航行设备的相对位置信息，并发送给对应的水下航行设备；
水面导航设备还用于将GPS位置信息及姿态信息作为第二辅助信息发送给各个水下航行设备；
各个水下航行设备用于根据自身的多种传感器信息、各自相对位置信息及第二辅助信息分别计算各自的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，水面导航设备还用于根据各个水下航行设备的相对位置信息及第二辅助信息计算各个水下航行设备的GPS位置信息，并发送给对应的水下航行设备；
水下航行设备还用于根据GPS位置信息跟踪水面导航设备移动，以使得水面导航设备与各个水下航行设备的距离小于预设阈值。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，各个水下航行设备根据自身的多种传感器信息、各自相对位置信息及第二辅助信息分别计算各自的实时精确位置信息，具体可包括：
各个水下航行设备采用扩展卡尔曼滤波算法对自身的相对位置信息及传感器信息进行融合计算，得到各自的实时状态估计值；
各个水下航行设备根据第二辅助信息及各自的实时状态估计值计算各自的实时精确位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，水面导航设备以一定时间间隔不断获取各个水下航行设备的相对位置信息，具体可包括：
水面导航设备以一定时间间隔不断采用超短基线获取各个水下航行设备的相对位置信息。
可选的，作为一种可能的实施方式，本发明实施例中的水下定位导航系统中，水下航行设备根据自身的实时精确位置信息发送给水面导航设备，并精确跟踪水面导航设备，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (8)

1.一种水下定位导航方法，其特征在于，运用于水下航行设备的定位导航，所述方法包括：
可移动的水面导航设备以一定时间间隔不断获取水下航行设备的相对位置信息；
所述水面导航设备将所述相对位置信息、GPS位置信息及姿态信息作为第一辅助信息发送给所述水下航行设备；
所述水下航行设备根据所述第一辅助信息及自身的多种传感器信息，确定所述水下航行设备的状态向量模型、初始状态向量以及状态转移方程，并根据所述水下航行设备自身的多种传感器信息确定观测矩阵；
所述水下航行设备根据所述状态向量模型、所述初始状态向量、所述状态转移方程、所述观测矩阵以及所述第一辅助信息计算自身的实时精确位置信息；
所述方法还包括：
所述水面导航设备根据所述相对位置信息、GPS位置信息及姿态信息计算所述水下航行设备的粗略GPS位置信息；
所述水面导航设备根据所述水下航行设备的粗略GPS位置信息跟踪所述水下航行设备移动，以使得所述水面导航设备与所述水下航行设备的距离小于预设阈值；
所述水下航行设备将自身的实时精确位置信息发送给所述水面导航设备，使得所述水面导航设备精确跟踪所述水下航行设备，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。
2.根据权利要求1所述的方法，其特征在于，所述水下航行设备自身的多种传感器信息包括罗盘测得的航向角、惯性测量单元测得的线速度和角速度、深度计测量的深度参数以及声学多普勒效应的测速设备测得载体相对于海底或海水层的速度，所述水下航行设备根据所述状态向量模型、所述初始状态向量、所述状态转移方程、所述观测矩阵以及所述第一辅助信息计算自身的实时精确位置信息包括：
水下航行设备采用扩展卡尔曼滤波算法对所述状态向量模型、所述初始状态向量、所述状态转移方程以及所述观测矩阵进行融合计算，得到水下航行设备的实时状态估计值；
水下航行设备根据所述第一辅助信息中的GPS位置信息及姿态信息以及自身的实时状态估计值计算自身的实时精确位置信息。
3.根据权利要求1至2中任一项所述的方法，其特征在于，所述水面导航设备以一定时间间隔不断获取水下航行设备的相对位置信息，包括：
水面导航设备以一定时间间隔不断采用超短基线获取水下航行设备的初始相对位置信息。
4.一种水下定位导航方法，其特征在于，运用于多个水下航行设备的定位导航，所述方法包括：
可移动的水面导航设备以一定时间间隔不断获取各个水下航行设备的相对位置信息，并发送给对应的水下航行设备；
所述水面导航设备将GPS位置信息及姿态信息作为第二辅助信息发送给各个水下航行设备；
各个水下航行设备根据自身的多种传感器信息、各自相对位置信息及所述第二辅助信息分别确定各自的状态向量模型、初始状态向量以及状态转移方程，并根据各自的多种传感器信息确定各自的观测矩阵；
各个水下航行设备根据各自的所述状态向量模型、所述初始状态向量、所述状态转移方程、所述观测矩阵以及所述第二辅助信息计算各自的实时精确位置信息；
所述方法还包括：
所述水面导航设备根据各个水下航行设备的相对位置信息及第二辅助信息计算各个水下航行设备的粗略GPS位置信息，并发送给对应的水下航行设备；
各个水下航行设备根据所述粗略GPS位置信息跟踪所述水面导航设备移动，以使得所述水面导航设备与各个水下航行设备的距离小于预设阈值；
所述水下航行设备根据自身的实时精确位置信息发送给所述水面导航设备，并精确跟踪所述水面导航设备，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。
5.根据权利要求4所述的方法，其特征在于，所述各个水下航行设备根据各自的所述状态向量模型、所述初始状态向量、所述状态转移方程、所述观测矩阵以及所述第二辅助信息计算各自的实时精确位置信息，包括：
各个水下航行设备采用扩展卡尔曼滤波算法对各自的所述状态向量模型、所述初始状态向量、所述状态转移方程、所述观测矩阵进行融合计算，得到各自的实时状态估计值；
各个水下航行设备根据所述第二辅助信息及各自的实时状态估计值计算各自的实时精确位置信息。
6.根据权利要求4至5中任一项所述的方法，其特征在于，所述水面导航设备以一定时间间隔不断获取各个水下航行设备的相对位置信息，包括：
水面导航设备以一定时间间隔不断采用超短基线获取各个水下航行设备的相对位置信息。
7.一种水下定位导航系统，其特征在于，包括单个水下航行设备及可移动的水面导航设备，其中，所述水面导航设备用于以一定时间间隔不断获取水下航行设备的相对位置信息；
所述水面导航设备还用于将所述相对位置信息、GPS位置信息及姿态信息作为第一辅助信息发送给所述水下航行设备；
所述水下航行设备根据所述第一辅助信息及自身的多种传感器信息，确定所述水下航行设备的状态向量模型、初始状态向量以及状态转移方程，并根据所述水下航行设备自身的多种传感器信息确定观测矩阵；
所述水下航行设备根据所述状态向量模型、所述初始状态向量、所述状态转移方程、所述观测矩阵以及所述第一辅助信息计算自身的实时精确位置信息；
所述水面导航设备还用于根据所述相对位置信息、GPS位置信息及姿态信息计算所述水下航行设备的粗略GPS位置信息；
所述水面导航设备还用于根据所述水下航行设备的粗略GPS位置信息跟踪所述水下航行设备移动，以使得所述水面导航设备与所述水下航行设备的距离小于预设阈值；
所述水下航行设备还用于将自身的实时精确位置信息发送给所述水面导航设备，使得所述水面导航设备精确跟踪所述水下航行设备，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。
8.一种水下定位导航系统，其特征在于，包括多个水下航行设备及可移动的水面导航设备，其中，所述水面导航设备用于以一定时间间隔不断获取各个水下航行设备的相对位置信息，并发送给对应的水下航行设备；
所述水面导航设备还用于将GPS位置信息及姿态信息作为第二辅助信息发送给各个水下航行设备；
各个所述水下航行设备用于根据自身的多种传感器信息、各自相对位置信息及所述第二辅助信息分别确定各自的状态向量模型、初始状态向量以及状态转移方程，并根据各自的多种传感器信息确定各自的观测矩阵；
各个水下航行设备根据各自的所述状态向量模型、所述初始状态向量、所述状态转移方程、所述观测矩阵以及所述第二辅助信息计算各自的实时精确位置信息；
所述水面导航设备还用于根据各个水下航行设备的相对位置信息及第二辅助信息计算各个水下航行设备的粗略GPS位置信息，并发送给对应的水下航行设备；
各个水下航行设备还用于根据所述粗略GPS位置信息跟踪所述水面导航设备移动，以使得所述水面导航设备与各个水下航行设备的距离小于预设阈值；
各个水下航行设备还用于根据自身的实时精确位置信息发送给所述水面导航设备，并精确跟踪所述水面导航设备，确保通信和定位链路的稳定，如此往复，以拓展水下航行设备的导航范围和定位精度。
CN202010506455.6A 2020-06-05 2020-06-05 一种水下定位导航方法及系统 Active CN111596333B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010506455.6A CN111596333B (zh) 2020-06-05 2020-06-05 一种水下定位导航方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010506455.6A CN111596333B (zh) 2020-06-05 2020-06-05 一种水下定位导航方法及系统

Publications (2)

Publication Number Publication Date
CN111596333A CN111596333A (zh) 2020-08-28
CN111596333B true CN111596333B (zh) 2023-02-21

Family

ID=72186050

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010506455.6A Active CN111596333B (zh) 2020-06-05 2020-06-05 一种水下定位导航方法及系统

Country Status (1)

Country Link
CN (1) CN111596333B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11435199B2 (en) * 2018-06-22 2022-09-06 Massachusetts Institute Of Technology Route determination in dynamic and uncertain environments
CN112433538B (zh) * 2020-11-25 2023-06-09 中国航天空气动力技术研究院 一种auv编队方法、系统及存储介质
CN113075665B (zh) * 2021-03-24 2023-06-20 鹏城实验室 水下定位方法、水下载体航行器及计算机可读存储介质
CN113311388B (zh) * 2021-05-27 2023-08-08 湖南国天电子科技有限公司 一种水下机器人的超短基线定位系统
CN114610021A (zh) * 2022-02-07 2022-06-10 哈尔滨工程大学 一种自主式水下航行器归航路径规划方法和装置
CN115092359A (zh) * 2022-08-26 2022-09-23 浙江九段智能科技有限公司 一种浮标式自主水下航行器

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1779485A (zh) * 2004-11-17 2006-05-31 中国科学院沈阳自动化研究所 载人潜水器的组合导航定位方法
CN103376802A (zh) * 2012-04-18 2013-10-30 中国科学院沈阳自动化研究所 一种利用水面机器人跟踪水下机器人的方法
CN103543459A (zh) * 2013-09-16 2014-01-29 中国船舶重工集团公司第七一〇研究所 一种实现水下拖体定位的组合导航装置及定位方法
CN107505597A (zh) * 2017-08-03 2017-12-22 浙江大学 一种基于深度信息的超短基线定位系统及方法
CN107576328A (zh) * 2017-08-22 2018-01-12 西北工业大学 一种用于水下航行器的水面自动跟踪监控系统
CN109765594A (zh) * 2017-11-09 2019-05-17 中国科学院沈阳自动化研究所 一种深海潜水器导航后处理方法
CN110006433A (zh) * 2019-04-22 2019-07-12 哈尔滨工程大学 海底油气管检测机器人的组合导航定位系统及方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08249060A (ja) * 1995-03-15 1996-09-27 Yokogawa Denshi Kiki Kk 自律型水中航走装置
JP2006313087A (ja) * 2005-05-06 2006-11-16 Mitsui Eng & Shipbuild Co Ltd 水中航走体の検出位置修正方法およびシステム
KR20190125643A (ko) * 2018-04-30 2019-11-07 한국전력공사 수중시설 감시용 무인잠수정
CN110749339A (zh) * 2019-10-25 2020-02-04 嘉兴中科声学科技有限公司 水下航行器的定位校准方法、装置、船载设备和水下设备

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1779485A (zh) * 2004-11-17 2006-05-31 中国科学院沈阳自动化研究所 载人潜水器的组合导航定位方法
CN103376802A (zh) * 2012-04-18 2013-10-30 中国科学院沈阳自动化研究所 一种利用水面机器人跟踪水下机器人的方法
CN103543459A (zh) * 2013-09-16 2014-01-29 中国船舶重工集团公司第七一〇研究所 一种实现水下拖体定位的组合导航装置及定位方法
CN107505597A (zh) * 2017-08-03 2017-12-22 浙江大学 一种基于深度信息的超短基线定位系统及方法
CN107576328A (zh) * 2017-08-22 2018-01-12 西北工业大学 一种用于水下航行器的水面自动跟踪监控系统
CN109765594A (zh) * 2017-11-09 2019-05-17 中国科学院沈阳自动化研究所 一种深海潜水器导航后处理方法
CN110006433A (zh) * 2019-04-22 2019-07-12 哈尔滨工程大学 海底油气管检测机器人的组合导航定位系统及方法

Also Published As

Publication number Publication date
CN111596333A (zh) 2020-08-28

Similar Documents

Publication Publication Date Title
CN111596333B (zh) 一种水下定位导航方法及系统
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
Kinsey et al. A survey of underwater vehicle navigation: Recent advances and new challenges
CN104316045B (zh) 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航系统
CN1325932C (zh) 载人潜水器的组合导航定位方法
CN102829777A (zh) 自主式水下机器人组合导航系统及方法
Hegrenaes et al. Doppler water-track aided inertial navigation for autonomous underwater vehicle
Xu et al. A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles
LaPointe Virtual long baseline (VLBL) autonomous underwater vehicle navigation using a single transponder
CN110906933B (zh) 一种基于深度神经网络的auv辅助导航方法
CN107797125A (zh) 一种减小深海探测型auv导航定位误差的方法
Troni et al. Preliminary experimental evaluation of a Doppler-aided attitude estimator for improved Doppler navigation of underwater vehicles
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN104061930A (zh) 基于捷联惯性制导和多普勒计程仪的导航方法
Lee et al. An integrated navigation system for autonomous underwater vehicles with two range sonars, inertial sensors and Doppler velocity log
Xu et al. Accurate two-step filtering for AUV navigation in large deep-sea environment
Allotta et al. Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements
CN111708008B (zh) 一种基于imu和tof的水下机器人单信标导航方法
CN115390012B (zh) 用于hov精准定位的多应答器坐标测量方法、装置及系统
CN116839574A (zh) 一种基于mcckf的管道潜航机器人组合导航方法、装置、存储介质及设备
CN103616026A (zh) 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法
Medagoda et al. Water column current profile aided localisation combined with view-based SLAM for autonomous underwater vehicle navigation
CN103697887B (zh) 一种基于捷联惯导系统和多普勒计程仪的优化导航方法
Harris et al. Cooperative acoustic navigation of underwater vehicles without a DVL utilizing a dynamic process model: Theory and field evaluation

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