CN107797125A - 一种减小深海探测型auv导航定位误差的方法 - Google Patents

一种减小深海探测型auv导航定位误差的方法 Download PDF

Info

Publication number
CN107797125A
CN107797125A CN201710989686.5A CN201710989686A CN107797125A CN 107797125 A CN107797125 A CN 107797125A CN 201710989686 A CN201710989686 A CN 201710989686A CN 107797125 A CN107797125 A CN 107797125A
Authority
CN
China
Prior art keywords
auv
navigation
data
water
positive
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
CN201710989686.5A
Other languages
English (en)
Other versions
CN107797125B (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201710989686.5A priority Critical patent/CN107797125B/zh
Publication of CN107797125A publication Critical patent/CN107797125A/zh
Application granted granted Critical
Publication of CN107797125B publication Critical patent/CN107797125B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/40—Correcting position, velocity or attitude
    • 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
    • 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/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • 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/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/50—Systems of measurement, based on relative movement of the target
    • G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/88—Sonar systems specially adapted for specific applications
    • G01S15/89—Sonar systems specially adapted for specific applications for mapping or imaging
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Acoustics & Sound (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明提出一种减小深海探测型AUV导航定位误差的方法，包括了正向解算和逆向解算两种解算过程，在水下时，采用SINS/DVL组合导航方式，并对航行过程中采集的数据实时正向解算，用卡尔曼滤波得出的导航参数校正SINS后，得到正向解算后AUV水下直线航行过程中的轨迹；出水后，AUV接收卫星导航数据，对AUV出水后航行的数据进行实时正向解算，采用SINS/DVL与SINS/DVL/GPS两种导航方式并对航行过程中采集的数据独立进行卡尔曼滤波，航行结束后对整个航行过程的数据逆向解算，利用实时正向解算过程保存的结果，进行逆向解算，根据滤波得出的导航参数校正后得到逆向解算后AUV水下直线航行过程中的轨迹；结合正向解算和逆向解算后的AUV轨迹，得到AUV的在水下航行过程的轨迹，从而提高AUV的定位性能。

Description

一种减小深海探测型AUV导航定位误差的方法
技术领域
本发明涉及AUV导航数据的后处理技术，特别是涉及一种基于逆向导航解算，减小深海探测型AUV导航定位误差的AUV导航数据后处理方法。
背景技术
在对海底地形地貌的绘制和勘探作业中，AUV是其重要的运载平台，独立的AUV个体可以在无人控制的状态下全天候、多航时的自主完成水下任务。但其突出缺点是易受到自身能力和携带能源的限制，当面对一些特定任务，如探测范围大等其他实际情况，任务完成的可靠性和有效性则很难保证。
多AUV协同导航系统将高精度的领航AUV与低精度的跟随AUV组合起来形成编队，实现独立的AUV难以或无法完成的复杂任务，提升了作业范围。领航AUV与跟随AUV间存在主从式协同，领航AUV间也构成一个协同系统。领航AUV在水下直线航行，领航AUV间的误差协同通过加减速实现，从而使队形稳定。跟随AUV通过水声通信接收领航AUV的航行状态数据与领航AUV间的相对位置关系，并根据定位算法，实时更新自身状态数据。因此领航AUV的定位精度将影响到整个多AUV协同导航系统的定位精度。在对海底地形地貌的绘制和勘探作业中，虽然领航AUV中配有高精度导航设备与水声通信设备，但在没有外部数据对其进行校正的情况下，其自身定位误差仍会不断累积，同时累积的定位误差直接影响AUV队形的整体协调性和稳定性，进而影响海图的绘制结果。海图的定位精度不高对二次作业的效果产生极大影响，海底勘测也就失去了其勘测的意义。
申请人在研究过程中发现，领航AUV在执行水下任务后出水后，可以引入外部卫星导航数据，通过这个外部数据校正水下直线航行过程中的累积定位误差，来提高领航AUV的定位精度。但领航AUV出水后接收到卫星数据含有噪声干扰，此时如果直接利用出水后的卫星数据对领航AUV进行位置校正，这同样会给领航AUV的定位精度带来影响。
发明内容
为提高深海探测型AUV定位精度，尤其是领航AUV的定位精度，本发明提出了一种减小深海探测型AUV导航定位误差的方法，解决卫星导航数据中的噪声干扰问题及在作业中捷联惯导短期高精度、长期误差发散问题，提高AUV水下航行过程所采集数据的定位精度。
本发明的技术方案为：
所述一种减小深海探测型AUV导航定位误差的方法，其特征在于：包括以下步骤：
步骤1：AUV入水并在水下航行时，采用捷联惯导与多普勒组合导航方式，对航行过程中采集的数据实时正向解算，用卡尔曼滤波估计出的导航参数校正捷联惯导后，得到正向解算的AUV水下航行轨迹；
步骤2：AUV出水并在水面航行时，AUV接收卫星导航数据，采用捷联惯导与多普勒组合导航，以及捷联惯导、多普勒和卫星导航组合导航两种方式独立运行，采用两种方式独立对航行过程中采集的数据实时正向解算，用卡尔曼滤波估计出的导航参数校正捷联惯导后，得到正向解算的AUV水面航行轨迹，并与步骤1得到的正向解算的AUV水下航行轨迹组合得到正向解算的AUV航行轨迹；
步骤3：在AUV航行结束后，对AUV航行过程中传感器采集并存储的整个数据按时间反向顺序进行逆向解算，得到逆向解算的AUV航行轨迹；在逆向解算过程中，取初值为正向解算的末值，所述初值包括初始姿态矩阵、初始速度、初始经度、初始纬度、初始高度，并将正向解算中的陀螺输出值和地球自转角速率取反，并通过卡尔曼滤波对导航参数进行估计，用卡尔曼滤波估计出的导航参数校正捷联惯导数据；
步骤4：将步骤2得到的正向解算的AUV航行轨迹与步骤3得到的逆向解算的AUV航行轨迹做差，得到AUV水下航行过程中两个轨迹的空间距离差取到最小值的时刻，以该时刻为分界，将该时刻之前的正向解算的AUV航行轨迹以及该时刻之后的逆向解算的AUV航行轨迹进行组合，得到AUV水下航行轨迹。
进一步的优选方案，所述一种减小深海探测型AUV导航定位误差的方法，其特征在于：步骤1中AUV水下航行阶段采用捷联惯导与多普勒组合导航方式得到正向解算的AUV水下航行轨迹的过程为：
步骤1.1：建立捷联惯导与多普勒组合导航下的卡尔曼滤波模型；
步骤1.2：AUV在水下航行中实时的通过捷联惯导和多普勒速度仪获得导航定位数据，并作实时正向解算，到每个采样时刻下的位置、姿态矩阵、速度数据，并利用步骤1.1建立的卡尔曼滤波模型进行滤波：
对滤波模型先进行离散化得到离散状态方程，再进行离散卡尔曼滤波，当获得量测更新时，通过滤波对导航参数进行估计，将导航参数反馈给捷联惯导，得到校正后的位置更新，并将位置更新数据和其他滤波获得的数据不断记录并保存下来，得到正向解算的AUV水下航行轨迹。
进一步的优选方案，所述一种减小深海探测型AUV导航定位误差的方法，其特征在于：步骤2中AUV水面航行阶段得到正向解算的AUV水面航行轨迹的过程为：
步骤2.1：建立捷联惯导、多普勒和卫星导航组合导航下的卡尔曼滤波模型；
步骤2.2：AUV接收卫星导航数据，同时AUV还根据通过捷联惯导和多普勒速度仪获得导航定位数据，并作实时正向解算，到每个采样时刻下的位置、姿态矩阵、速度数据，并步骤2.1和步骤1.1建立的卡尔曼滤波模型独立进行滤波处理：
对两种滤波模型先离散化，再独立进行离散卡尔曼滤波，当获得量测更新时，通过滤波对导航参数进行估计，将导航参数反馈给捷联惯导，得到校正后的位置更新，并将位置更新数据和其他滤波获得的数据不断记录并保存下来，得到正向解算后AUV水面航行轨迹。
有益效果
1)在AUV出水后引入卫星导航数据，由于卫星导航数据中含有噪声干扰，通过对AUV在水上建立两种滤波方式，并同时独立进行滤波，抑制了出水后引入卫星导航数据的噪声干扰；
2)该发明通过正向解算和逆向解算，得到两种解算后AUV水下直线航行过程中的轨迹，结合这两种解算方式下的轨迹，来得到AUV水下航行的轨迹，解决了捷联惯导短期高精度、长期误差发散问题，提高了AUV水下航行过程勘测数据的定位精度，提高了海图的绘制结果(如海底地形地貌)的可用性，为二次作业提供良好的基础和准确性。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1为本发明的算法示意图。
图2为误差累积示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
本发明提出的减小深海探测型AUV导航定位误差的方法，它包括了正向解算和逆向解算两种解算过程。不考虑如图1所示的入水阶段和出水阶段，只考虑水下航行阶段和水面航行阶段，出水是为了引入卫星导航数据抑制水下航行中的累积定位误差，故水面航行阶段不需要航行距离过长。
在水下时，AUV直线航行，采用SINS/DVL组合导航方式，并对航行过程中采集的数据实时正向解算，用卡尔曼滤波得出的导航参数校正SINS后，从而可以得到正向解算后AUV水下直线航行过程中的轨迹；出水后，AUV接收卫星导航(此处采用GPS)数据，对AUV出水后航行的数据进行实时正向解算，此时采用SINS/DVL与SINS/DVL/GPS两种导航方式并对航行过程中采集的数据独立进行卡尔曼滤波，航行结束后对整个航行过程的数据逆向解算，利用之前实时正向解算中卡尔曼滤波过程保存的结果，进行逆向解算，根据滤波得出的导航参数校正后得到逆向解算后AUV水下直线航行过程中的轨迹；结合正向解算和逆向解算后的AUV轨迹，得到AUV的在水下航行过程的轨迹，从而提高AUV的定位性能。
AUV在水下直线航行过程建立的滤波模式，用于得到AUV水下直线航行中的导航参数，此时AUV的导航方式为SINS/DVL组合导航方式，即AUV在水下直线航行过程建立的滤波模式为SINS/DVL组合导航下的滤波方式。
AUV在水上航行过程建立的滤波模式，用于得到AUV水上航行中导航参数，此时AUV增加了SINS/DVL/GPS组合导航方式，采用两种滤波方式，且这两种滤波方式独立运行，即AUV在水上航行过程建立的滤波模式为SINS/DVL组合导航下的滤波方式与SINS/DVL/GPS组合导航下的滤波方式独立并存，用于消除GPS数据中噪声干扰带来的误差。
所述正向解算，用于AUV整个航行过程中的数据，对这些数据按时间顺序实时正向(即数据记录的方向)处理，得到每个采样时刻下AUV的位置(经度、纬度、深度)、姿态矩阵、速度(东向、北向、天向的速度)数据。正向解算是在每个采样时刻下实时处理整个航行过程中采集的数据，再通过卡尔曼滤波出的导航参数校正后才得到正向解算后的轨迹。
所述逆向解算，用于航行结束后整个航行过程中保存的数据，对这些数据按时间反向顺序逆向处理，得到在每个采样时刻下AUV的位置、姿态矩阵、速度数据。逆向解算是在航行结束后对整个航行过程保存的数据处理，再通过滤波校正后得到逆向解算后的轨迹。
在得到正向解算和逆向解算的AUV航行轨迹后，AUV水下直线航行过程的轨迹是由正向解算所得的轨迹和逆向解算所得的轨迹相结合得到：将正向解算和逆向解算后所得轨迹做差，二者之差在空间有一个最小值点，把这个最小值点作为切换点，在切换点之前用正向解算的轨迹作为AUV水下直线航行过程的轨迹，在切换点之后用逆向解算的轨迹作为AUV水下直线航行过程的轨迹，结合后得到水下直线航行过程的轨迹，从而得到更为准确的AUV出水前的位置数据，解决了捷联惯导短期高精度、长期误差发散问题，提高了AUV水下航行过程勘测数据的定位精度，提高了海图的绘制结果的可用性。
具体包括以下步骤：
步骤1：AUV入水并在水下航行时，采用SINS/DVL组合导航方式，对航行过程中采集的数据实时正向解算，用卡尔曼滤波估计出的导航参数校正捷联惯导后，得到正向解算的AUV水下航行轨迹：
步骤1.1：建立捷联惯导与多普勒组合导航下的卡尔曼滤波模型；
步骤1.2：AUV在水下航行中实时的通过捷联惯导和多普勒速度仪获得导航定位数据，并作实时正向解算，到每个采样时刻下的位置(经度、纬度、深度)、姿态矩阵、速度(东向、北向、天向的速度)数据，并利用步骤1.1建立的卡尔曼滤波模型进行滤波：
对SINS/DVL组合导航下的滤波模型先进行离散化得到离散状态方程，再进行离散卡尔曼滤波，当获得量测更新时，通过滤波对导航参数(状态向量X(t))进行估计，将导航参数反馈给捷联惯导，得到校正后的位置更新，并将位置更新数据和其他滤波获得的数据不断记录并保存下来，得到正向解算的AUV水下航行轨迹(每个采样时刻下的经纬度值)。
步骤2：AUV出水并在水面航行时，AUV接收卫星导航数据，采用捷联惯导与多普勒组合导航，以及捷联惯导、多普勒和卫星导航组合导航两种方式独立运行，采用两种方式独立对航行过程中采集的数据实时正向解算，用卡尔曼滤波估计出的导航参数校正捷联惯导后，得到正向解算的AUV水面航行轨迹，并与步骤1得到的正向解算的AUV水下航行轨迹组合得到正向解算的AUV航行轨迹；
步骤2.1：建立捷联惯导、多普勒和卫星导航组合导航下的卡尔曼滤波模型；
步骤2.2：AUV接收卫星导航数据，此处引入的GPS数据，同时AUV还根据通过捷联惯导和多普勒速度仪获得导航定位数据，并作实时正向解算，到每个采样时刻下的位置、姿态矩阵、速度数据，并步骤2.1和步骤1.1建立的卡尔曼滤波模型独立进行滤波处理：
对两种滤波模型先离散化，再独立进行离散卡尔曼滤波，当获得量测更新时，通过滤波对导航参数进行估计，将导航参数反馈给捷联惯导，得到校正后的位置更新，并将位置更新数据和其他滤波获得的数据不断记录并保存下来，得到正向解算后AUV水面航行轨迹。
步骤3：在AUV航行结束后，对AUV航行过程中传感器采集并存储的整个数据按时间反向顺序进行逆向解算，得到逆向解算的AUV航行轨迹；在逆向解算过程中，取初值为正向解算的末值，所述初值包括初始姿态矩阵、初始速度、初始经度、初始纬度、初始高度，并将正向解算中的陀螺输出值和地球自转角速率取反，并通过卡尔曼滤波对导航参数进行估计，将导航参数反馈给SINS，得到校正后的位置更新，并将位置更新数据和其他滤波获得的数据不断记录并保存下来，即就得到了逆向解算后AUV的航行轨迹。
步骤4：基于直线航行时误差随时间的增加而累积，将步骤2得到的正向解算的AUV航行轨迹与步骤3得到的逆向解算的AUV航行轨迹做差，得到AUV水下航行过程中两个轨迹的空间距离差取到最小值的时刻，以该时刻为分界，将该时刻之前的正向解算的AUV航行轨迹以及该时刻之后的逆向解算的AUV航行轨迹进行组合，得到AUV水下航行轨迹。
下面结合图2对如何结合正向解算和逆向解算的AUV轨迹进行说明。
如图2所示，基于直线航行时，误差随时间的增加而累积(误差不一定是线性增加的形式，但误差一定是随着时间的增加而增加的)，正向解算得到的轨迹和逆向解算得到的轨迹之差，取轨迹之差的最小值点处作为切换点，轨迹之差的最小值点如图中A点所处的圆所示，将这个圆看作切换点，即就是在A之前选择正向解算后的轨迹，在A点之后选择逆向解算后的轨迹，结合这两段的轨迹就得到AUV的轨迹，即：水下直线航行过程中的OA段采用正向解算后的轨迹，水下直线航行的A点到出水点采用逆向解算后的轨迹，得到水下直线航行过程的轨迹。
下面给出相关模型：
SINS/DVL组合导航下AUV的滤波模型：
滤波器的状态变量的选取：取AUV各状态的误差量，包括：AUV的东向、北向、天向速度误差δVE、δVN、δVU，定位误差(纬度误差、经度误差)δL、δλ，姿态角误差在东向、北向、天向的投影φx、φy、φz，陀螺仪随机漂移误差在东向、北向、天向的投影εx、εy、εz，加速度计的偏置误差在东向、北向、天向的投影考虑当失准角为小角度并忽略重力模型误差，可得SINS的近似线性误差模型：
姿态误差方程：
其中：表示失准角在北向、天向、东向的投影，Vn＝[VN VU VE]T表示AUV航行速度在北向、天向、东向的投影，
εb＝[εN εU εE]T表示陀螺仪随机漂移标定误差在北向、天向、东向的投影，
λ、L与h分别表示AUV的经纬度和深度，RM、RN别为AUV(航行器)所处位置的地球子午圈和卯酉圈曲率半径，ωie为地球自转角速率。
速度误差方程：
式中：为加速度计的偏置误差在东向、北向、天向的投影，
f＝[fN fU fE]T为加速度计在东向、北向、天向测得的比力值。
定位误差方程：
其中
并且有
其中：为载体系(b)到导航坐标系(n)的姿态转换矩阵，表示中第i行j列元素。
取系统状态向量为：
得出状态方程的一般表达式为：
其中，W(t)为误差状态矢量，包括
其中：分别为表示AUV速度误差在东向、北向和天向上的投影，为失准角的角度误差在东向、北向、天向上的投影。
SINS测得的速度为：
VE＝VE0+δVE
VN＝VN0+δVN
VU＝VU0+δVU
其中：V0 n＝[VN0 VU0 VE0]T表示AUV航行速度在北向、天向、东向投影的真值。DVL速度量测误差项对应的状态方程为：
地理坐标系下的DVL速度量测量可表示为：
其中，表示地理坐标系上的量测误差。分别表示量测误差在东向、北向和天向上的投影。分别表示量测值随机干扰误差在东向、北向和天向上的投影，分别表示DVL速度量测量在东向、北向和天向上的投影。
将SINS测得的速度与DVL速度仪测得的速度在地理坐标系下的投影作差可得组合AUV的量测方程：
其中，H(t)可分为两部分：H＝[HINS HDVL]
SINS/DVL/GPS组合导航下AUV的滤波模型：
采用SINS/DVL/GPS组合导航方式，AUV的状态方程与SINS/DVL组合导航状态方程相同；
观测方程为：
其中，观测量Z为状态量的直接观测值，因此观测阵H为常值矩阵H＝[I5×5 0]。
正向解算和逆向解算：
1)姿态更新微分方程
其中：
2)速度更新微分方程
gn＝[0 -g 0]T
其中，fb是加速度计测得航行器的线加速度，g为重力加速度。
3)位置更新微分方程
假设Ts为捷联惯导中AUV的采样周期，将微分方程离散化得：
(1)正向解算计算式：
其中：
下标k、k-1表示k时刻、k-1时刻
若AUV在Tm时刻航到Tn时刻，从M点航行至N点，将上述方程稍作变化，可得：
其中：
将以上式子的各量可记为：
其中，j＝0,1,2,…,m，令p＝m-k+1，
故对下标进行转换，逆向递推关系可以写成：
(2)逆向解算计算式：
式中：
其中：所有的上标^表示是逆向解算。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (3)

1.一种减小深海探测型AUV导航定位误差的方法，其特征在于：包括以下步骤：
步骤1：AUV入水并在水下航行时，采用捷联惯导与多普勒组合导航方式，对航行过程中采集的数据实时正向解算，用卡尔曼滤波估计出的导航参数校正捷联惯导后，得到正向解算的AUV水下航行轨迹；
步骤2：AUV出水并在水面航行时，AUV接收卫星导航数据，采用捷联惯导与多普勒组合导航，以及捷联惯导、多普勒和卫星导航组合导航两种方式独立运行，采用两种方式独立对航行过程中采集的数据实时正向解算，用卡尔曼滤波估计出的导航参数校正捷联惯导后，得到正向解算的AUV水面航行轨迹，并与步骤1得到的正向解算的AUV水下航行轨迹组合得到正向解算的AUV航行轨迹；
步骤3：在AUV航行结束后，对AUV航行过程中传感器采集并存储的整个数据按时间反向顺序进行逆向解算，得到逆向解算的AUV航行轨迹；在逆向解算过程中，取初值为正向解算的末值，所述初值包括初始姿态矩阵、初始速度、初始经度、初始纬度、初始高度，并将正向解算中的陀螺输出值和地球自转角速率取反，并通过卡尔曼滤波对导航参数进行估计，用卡尔曼滤波估计出的导航参数校正捷联惯导数据；
步骤4：将步骤2得到的正向解算的AUV航行轨迹与步骤3得到的逆向解算的AUV航行轨迹做差，得到AUV水下航行过程中两个轨迹的空间距离差取到最小值的时刻，以该时刻为分界，将该时刻之前的正向解算的AUV航行轨迹以及该时刻之后的逆向解算的AUV航行轨迹进行组合，得到AUV水下航行轨迹。
2.根据权利要求1所述一种减小深海探测型AUV导航定位误差的方法，其特征在于：
步骤1中AUV水下航行阶段采用捷联惯导与多普勒组合导航方式得到正向解算的AUV水下航行轨迹的过程为：
步骤1.1：建立捷联惯导与多普勒组合导航下的卡尔曼滤波模型；
步骤1.2：AUV在水下航行中实时的通过捷联惯导和多普勒速度仪获得导航定位数据，并作实时正向解算，到每个采样时刻下的位置、姿态矩阵、速度数据，并利用步骤1.1建立的卡尔曼滤波模型进行滤波：
对滤波模型先进行离散化得到离散状态方程，再进行离散卡尔曼滤波，当获得量测更新时，通过滤波对导航参数进行估计，将导航参数反馈给捷联惯导，得到校正后的位置更新，并将位置更新数据和其他滤波获得的数据不断记录并保存下来，得到正向解算的AUV水下航行轨迹。
3.根据权利要求2所述一种减小深海探测型AUV导航定位误差的方法，其特征在于：
步骤2中AUV水面航行阶段得到正向解算的AUV水面航行轨迹的过程为：
步骤2.1：建立捷联惯导、多普勒和卫星导航组合导航下的卡尔曼滤波模型；
步骤2.2：AUV接收卫星导航数据，同时AUV还根据通过捷联惯导和多普勒速度仪获得导航定位数据，并作实时正向解算，到每个采样时刻下的位置、姿态矩阵、速度数据，并步骤2.1和步骤1.1建立的卡尔曼滤波模型独立进行滤波处理：
对两种滤波模型先离散化，再独立进行离散卡尔曼滤波，当获得量测更新时，通过滤波对导航参数进行估计，将导航参数反馈给捷联惯导，得到校正后的位置更新，并将位置更新数据和其他滤波获得的数据不断记录并保存下来，得到正向解算后AUV水面航行轨迹。
CN201710989686.5A 2017-10-23 2017-10-23 一种减小深海探测型auv导航定位误差的方法 Active CN107797125B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710989686.5A CN107797125B (zh) 2017-10-23 2017-10-23 一种减小深海探测型auv导航定位误差的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710989686.5A CN107797125B (zh) 2017-10-23 2017-10-23 一种减小深海探测型auv导航定位误差的方法

Publications (2)

Publication Number Publication Date
CN107797125A true CN107797125A (zh) 2018-03-13
CN107797125B CN107797125B (zh) 2018-10-26

Family

ID=61533771

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710989686.5A Active CN107797125B (zh) 2017-10-23 2017-10-23 一种减小深海探测型auv导航定位误差的方法

Country Status (1)

Country Link
CN (1) CN107797125B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109856659A (zh) * 2019-01-21 2019-06-07 同济大学 海床基定位授时与数据回收系统及方法
CN111141279A (zh) * 2019-12-20 2020-05-12 北京小马慧行科技有限公司 行车轨迹的处理方法及装置
CN111307160A (zh) * 2020-03-20 2020-06-19 中国人民解放军军事科学院国防科技创新研究院 一种自主水下航行器长航时航迹的修正方法及装置
CN111521178A (zh) * 2020-04-28 2020-08-11 中国人民解放军国防科技大学 基于管长约束的钻探用定位定向仪孔内定位方法
CN112558078A (zh) * 2020-12-10 2021-03-26 湖南国天电子科技有限公司 一种基于多站的轨迹被动跟踪方法
CN112747753A (zh) * 2019-10-29 2021-05-04 北京初速度科技有限公司 一种车辆状态确定方法及车载终端中的处理器
CN113108778A (zh) * 2021-03-03 2021-07-13 中国科学院声学研究所 一种具备多条带模式的深水多波束测深方法及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103900568A (zh) * 2014-03-08 2014-07-02 哈尔滨工程大学 一种改进的捷联惯性导航系统快速阻尼方法
CN105783940A (zh) * 2016-01-07 2016-07-20 东南大学 基于信息预评判及补偿修正的sins/dvl/es组合导航方法
CN106123926A (zh) * 2016-08-25 2016-11-16 哈尔滨工程大学 一种基于gps信息修正惯导位置误差的uuv离线标图方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103900568A (zh) * 2014-03-08 2014-07-02 哈尔滨工程大学 一种改进的捷联惯性导航系统快速阻尼方法
CN105783940A (zh) * 2016-01-07 2016-07-20 东南大学 基于信息预评判及补偿修正的sins/dvl/es组合导航方法
CN106123926A (zh) * 2016-08-25 2016-11-16 哈尔滨工程大学 一种基于gps信息修正惯导位置误差的uuv离线标图方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
SHIMEI DUAN ET AL.: "An Improved Federated Filtering Method for Integrated Navigation System of Autonomous Underwater Vehicle", 《2008 ASIA SIMULATION CONFERENCE — 7TH INTL. CONF. ON SYS. SIMULATION AND SCIENTIFIC COMPUTING》 *
张福斌 等: "基于逆向导航算法的捷联惯导/转速计组合对准方法", 《鱼雷技术》 *
段世梅 等: "基于SINS／DVL／GPS的AUV组合导航技术", 《火力与指挥控制》 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109856659A (zh) * 2019-01-21 2019-06-07 同济大学 海床基定位授时与数据回收系统及方法
CN112747753A (zh) * 2019-10-29 2021-05-04 北京初速度科技有限公司 一种车辆状态确定方法及车载终端中的处理器
CN111141279A (zh) * 2019-12-20 2020-05-12 北京小马慧行科技有限公司 行车轨迹的处理方法及装置
CN111141279B (zh) * 2019-12-20 2022-07-01 北京小马慧行科技有限公司 行车轨迹的处理方法及装置
CN111307160A (zh) * 2020-03-20 2020-06-19 中国人民解放军军事科学院国防科技创新研究院 一种自主水下航行器长航时航迹的修正方法及装置
CN111307160B (zh) * 2020-03-20 2021-06-22 中国人民解放军军事科学院国防科技创新研究院 一种自主水下航行器长航时航迹的修正方法及装置
CN111521178A (zh) * 2020-04-28 2020-08-11 中国人民解放军国防科技大学 基于管长约束的钻探用定位定向仪孔内定位方法
CN111521178B (zh) * 2020-04-28 2021-01-15 中国人民解放军国防科技大学 基于管长约束的钻探用定位定向仪孔内定位方法
CN112558078A (zh) * 2020-12-10 2021-03-26 湖南国天电子科技有限公司 一种基于多站的轨迹被动跟踪方法
CN113108778A (zh) * 2021-03-03 2021-07-13 中国科学院声学研究所 一种具备多条带模式的深水多波束测深方法及系统
CN113108778B (zh) * 2021-03-03 2022-06-14 中国科学院声学研究所 一种具备多条带模式的深水多波束测深方法及系统

Also Published As

Publication number Publication date
CN107797125B (zh) 2018-10-26

Similar Documents

Publication Publication Date Title
CN107797125B (zh) 一种减小深海探测型auv导航定位误差的方法
CN104457754B (zh) 一种基于sins/lbl紧组合的auv水下导航定位方法
CN110006433A (zh) 海底油气管检测机器人的组合导航定位系统及方法
CN109443379A (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
Lee et al. Simulation of an inertial acoustic navigation system with range aiding for an autonomous underwater vehicle
CN102829777B (zh) 自主式水下机器人组合导航系统及方法
Grenon et al. Enhancement of the inertial navigation system for the morpheus autonomous underwater vehicles
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航系统
CN106767793A (zh) 一种基于sins/usbl紧组合的auv水下导航定位方法
CN106595715B (zh) 基于捷联惯导与卫星组合导航系统里程计标定方法及装置
CN111596333B (zh) 一种水下定位导航方法及系统
CN102169184A (zh) 组合导航系统中测量双天线gps安装失准角的方法和装置
CN103542851A (zh) 一种基于水下地形高程数据库的水下航行器辅助导航定位方法
CN104359496B (zh) 基于垂线偏差补偿的高精度姿态修正方法
CN107015259A (zh) 采用多普勒测速仪计算伪距/伪距率的紧组合方法
CN110514203A (zh) 一种基于isr-ukf的水下组合导航方法
CN110274591A (zh) 深潜载人潜水器adcp辅助sins导航方法
CN108931791A (zh) 卫惯紧组合钟差修正系统和方法
CN104776847B (zh) 一种适用于水下导航系统单点估计陀螺漂移的方法
CN110345941A (zh) 深潜载人潜水器sins自辅助导航方法
CN107063245A (zh) 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
CN106199668A (zh) 一种级联式gnss/sins深组合导航方法
CN104049269A (zh) 一种基于激光测距和mems/gps组合导航系统的目标导航测绘方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN110285834A (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
GR01 Patent grant
GR01 Patent grant