CN112611376A - 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统 - Google Patents

一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统 Download PDF

Info

Publication number
CN112611376A
CN112611376A CN202011373126.5A CN202011373126A CN112611376A CN 112611376 A CN112611376 A CN 112611376A CN 202011373126 A CN202011373126 A CN 202011373126A CN 112611376 A CN112611376 A CN 112611376A
Authority
CN
China
Prior art keywords
pose
auv
image
constraint
time
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
CN202011373126.5A
Other languages
English (en)
Other versions
CN112611376B (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.)
Wuhan Institute of Technology
Original Assignee
Wuhan 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 Wuhan Institute of Technology filed Critical Wuhan Institute of Technology
Priority to CN202011373126.5A priority Critical patent/CN112611376B/zh
Publication of CN112611376A publication Critical patent/CN112611376A/zh
Application granted granted Critical
Publication of CN112611376B publication Critical patent/CN112611376B/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
    • 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/14—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 recording the course traversed by the object
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/483—Details of pulse systems
    • G01S7/486—Receivers
    • G01S7/4861—Circuits for detection, sampling, integration or read-out
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/483—Details of pulse systems
    • G01S7/486—Receivers
    • G01S7/487—Extracting wanted echo signals, e.g. pulse detection

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)
  • Electromagnetism (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种RGI‑Lidar/SINS紧耦合AUV水下导航定位方法，包括以下步骤：通过捷联解算得到相应时刻包含AUV位置信息、姿态信息的导航信息；获取水下远距离、高清晰度的选通图像；对IMU输出信息进行预积分，获取AUV在不同时刻的IMU位姿约束；对当前帧的选通图像进行特征提取，并将上一帧选通图像作为参考帧与之进行特征匹配，获取视觉里程计约束；对当前帧选通图像进行回环检测，若检测到符合条件的回环帧图像，则将当前帧选通图像与相应回环帧图像进行特征匹配，获取回环检测约束；将上述三种约束作为约束关系加入到位姿图中进行联合优化，获取并输出优化后的AUV位姿；优化后的AUV位姿可作为惯性数据输入。本发明能满足广泛、长时间、远距离的水下导航定位的需求。

Description

一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统
技术领域
本发明属于AUV水下导航技术领域，具体涉及一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统。
背景技术
在当今日益竞争的国际形势下，水下导航定位无论是对于打捞搜救、海底管道铺设和检查维护、沉船遗址科考、珊瑚礁及海洋生物保护等民事应用，还是潜艇战略巡航、水下排雷、航道封锁和通信中继等军事用途，都具有很高的研宄价值及意义，目前已成为国内外研究的热点与难点。
在现有的定位技术中，由于捷联式惯性导航系统可以长时间稳定的提供载体的方位信息和距离信息，既不依赖于外界设备又不受外界干扰，因此现有的AUV自主导航方法都是围绕惯性导航为中心。但是惯性器件具有误差积累的性质，需要其他导航定位方法辅助，定时修正惯导的导航参数误差，才能实现对载体长时间的精确定位。
传统的惯导辅助导航方法包括地磁导航、重力导航、水声定位导航、GPS位置信息校正等。重力导航定位技术和地磁导航技术需要重力或地磁数据库等先验图作为辅助导航定位参考，但是在绝大多数的海域都没有这类先验图，所以该两种方法不能满足更广泛的水下导航定位需求；水声定位导航作用范围广，但是需要在海底布设应答器阵列，这种方法限制了AUV的任务区域，即必须在声学信标预置的范围内执行任务；并且声学信标的放置和维护成本极高，花费巨大；GPS位置信息校正需要AUV在执行任务过程中中断任务，上浮到水面进行GPS定位，不利于任务的执行。传统的辅助导航方法不能满足广泛、长时间、远距离的水下导航定位的需求。
发明内容
本发明的目的在于，提供一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法及系统，其切片成像技术大大的抑制了水体的后向散射，提高了图像的对比度，和传统光学成像相比，大大提升了光学图像的对比度，能满足广泛、长时间、远距离的水下导航定位的需求。
为解决上述技术问题，本发明的技术方案为：一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法，包括以下步骤：
通过捷联解算得到相应时刻的AUV位置信息、姿态信息的导航信息；
获取水下远距离、高清晰度的选通图像；
对IMU输出信息进行预积分，获取AUV在不同时刻的IMU位姿约束；
对当前帧的选通图像进行特征提取，并将上一帧的选通图像作为参考帧与之进行特征匹配，获取相邻图像之间的位姿关系，即视觉里程计约束；
对当前帧的选通图像进行回环检测，若检测到符合条件的回环帧图像，则将当前帧的选通图像与相应的回环帧图像进行特征匹配，获取回环检测约束；
将IMU位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，获取AUV位姿；
输出AUV位姿；
将AUV位姿作为惯性数据输入。
进一步地，对IMU进行预积分的具体方法为先捷联解算得到AUV在不同时刻速度、位置和姿态，再根据以下公式进行预积分处理，获取AUV在不同时刻之间的位姿约束因子，获取AUV在不同时刻的IMU位姿约束，
Figure BDA0002806698900000031
式中，
Figure BDA0002806698900000032
表示i时刻旋转矩阵的转置，Rj表示j时刻的旋转矩阵，vi和vj分别表示i时刻和j时刻的速度，g是重力加速度，pi和pj分别表示i时刻和j时刻的位置，Δtij表示i时刻和j时刻的时间差。
进一步地，特征提取的方法为ORB特征提取方法，即利用FAST角点检测子提取关键点的位置，并为FAST角点增加尺度和旋转的描述，采用BRIEF描述子来描述关键点的属性。
进一步地，特征匹配的方法为在相邻帧图像提取的特征点中，找到相对应的特征点，即特征点的描述子特征向量之间的距离越小，表示两个特征点越匹配。
进一步地，回环检测为基于欧氏距离的回环检测方法，即根据捷联惯性导航系统推算出当前时刻的位姿Xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿Xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧的选通图像无相应的回环帧图像；若d不大于阈值d0，则认为当前帧的选通图像有相应的回环帧图像，求出两者的位姿关系，即回环约束；其中阈值d0是根据实际应用中AUV的移动速度和成像系统的帧频来设置的。
进一步地，位姿图根据下式表示：
G＝(X，F)
式中，X是位姿图中的节点，表示的是与选通图像相对应时刻AUV的位姿；F是位姿图中的边，表示的是节点之间或者单个节点的约束条件。
一种用于实现上述的RGI-Lidar/SINS紧耦合AUV水下导航定位方法的系统，其特征在于，包括RGI-Lidar，SINS和数据处理单元；其中，
RGI-Lidar，用于获取水下远距离、高清晰度的选通图像，并将选通图像发送至数据处理单元；
SINS，用于通过捷联惯导算法计算得到相应时刻的AUV位置信息、姿态信息的导航信息并发送至数据处理单元；
数据处理单元，用于根据AUV位置信息，对IMU进行预积分，获取AUV在不同时刻的IMU位姿约束；对当前帧的选通图像进行特征提取，并将上一帧的选通图像作为参考帧与之进行特征匹配，获取相邻图像之间的位姿关系，即视觉里程计约束；对当前帧的选通图像进行回环检测，若检测到符合条件的回环帧图像，则将当前帧的选通图像与相应的回环帧图像进行特征匹配，获取回环检测约束；将IMU位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，获取并输出AUV位姿；将AUV位姿发送至SINS作为惯性数据输入。
进一步地，对IMU进行预积分的具体方法为先捷联解算得到AUV在不同时刻速度、位置和姿态，再根据以下公式进行预积分处理，获取AUV在不同时刻之间的位姿约束因子，获取AUV在不同时刻的IMU位姿约束，
Figure BDA0002806698900000041
式中，
Figure BDA0002806698900000042
表示i时刻旋转矩阵的转置，Rj表示j时刻的旋转矩阵，vi和vj分别表示i时刻和j时刻的速度，g是重力加速度，pi和pj分别表示i时刻和j时刻的位置，Δtij表示i时刻和j时刻的时间差。
进一步地，回环检测为基于欧氏距离的回环检测方法，即根据捷联惯性导航系统推算出当前时刻的位姿Xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿Xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧的选通图像无相应的回环帧图像；若d不大于阈值d0，则认为当前帧的选通图像有相应的回环帧图像，求出它们之间的位姿关系，即回环约束；其中阈值d0是根据实际应用中AUV的移动速度和成像系统的帧频来设置的。
进一步地，该系统中还包括传感器组，用于采集多组观测信息加入到位姿图中进行融合。
与现有技术相比，本发明的有益效果为：
1、首次将距离选通三维激光成像雷达RGI-Lidar引入到水下AUV导航定位中，其切片成像技术大大的抑制了水体的后向散射，提高了图像的对比度，和传统光学成像相比，大大提升了光学图像的对比度；
2、配置了RGI-Lidar，填补了水下slam技术在近距离、高精度导航定位这一应用中的空白；
3、提出了一种基于位姿图的紧耦合SLAM框架，适用于多传感器信息的融合，不仅可以融合SINS、RGI-Lidar的观测信息，还可以将成像声呐、GPS、多普勒测速仪(DVL)、测高仪等多种传感器的观测信息加入到位姿图中进行融合，为更广泛、更长时间、更远距离、更精确的水下AUV导航定位提供更多有效的信息冗余。
附图说明
图1为本发明实施例的流程框图；
图2为本发明实施例的位姿图示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。
在此公开本发明结构实施例和方法的描述。可以了解的是并不意图将本发明限制在特定公开的实施例中，而是本发明可以通过使用其它特征，元件方法和实施例来加以实施。不同实施例中的相似元件通常会标示相似的号码。
本发明由安装在由安装在AUV上的捷联惯性导航系统SINS、距离选通三维激光成像雷达RGI-Lidar和数据处理单元三大部分组成，其中所述的SINS包括惯性测量单元IMU和捷联解算模块，所述的数据处理单元包括IMU预积分模块，图优化模块，特征提取及匹配模块，以及回环检测模块，如图1所示，所述方法通过以下步骤实现：
步骤1、捷联惯性导航系统SINS中的IMU元件可以输出系统的加速度和角加速度等惯性数据，捷联解算模块通过对惯性数据进行捷联解算得到相应时刻的AUV位置信息、姿态信息等导航信息；距离选通三维激光成像雷达获取水下远距离、高清晰度的选通图像，并全都输出给数据处理单元；
所述的距离选通三维激光成像雷达采用脉冲激光器作为照明光源，以选通成像器件ICCD作为探测器，以时序控制器实现照明光源和探测器之间的同步；工作中，脉冲激光器发射激光脉冲，当激光脉冲传至水体悬浮颗粒、目标以及背景时，被反射或者散射，会分别形成向后的水体后向散射、目标回波信号以及背景噪声信号；只有当目标的回波信号传播到选通成像器件时，选通门才开启，接收信号并完成图像的采集和生成，其他时候选通门都关闭，因此可以抑制水体后向散射和背景噪声，提高图像的对比度和系统的作用距离。
步骤2、数据处理单元中的IMU预积分模块对IMU输出进行预积分，从而获取AUV在不同时刻之间的IMU位姿约束；特征提取及匹配模块对当前帧选通图像提取特征，并将上一帧作为参考帧与之进行特征匹配，利用从而获取相邻图像之间的位姿关系，即视觉里程计约束；回环检测模块对当前帧图像进行回环检测，若检测到符合条件的回环帧，则将当前帧图像与相应的回环帧进行特征提取与匹配，从而获取回环检测约束；
所述的IMU预积分可以计算出速度、位置和姿态在不同时刻i和j之间的相对值，分别用Δvij,Δpij,和ΔRij来表示，它们的计算方式如下：
Figure BDA0002806698900000071
其中，
Figure BDA0002806698900000072
表示i时刻旋转矩阵的转置，Rj表示j时刻的旋转矩阵，vi和vj分别表示i时刻和j时刻的速度，g是重力加速度，pi和pj分别表示i时刻和j时刻的位置，Δtij表示i时刻和j时刻的时间差。通过IMU预积分可以获取AUV在不同时刻之间的位姿约束因子，并可以方便的将其加入到位姿图中进行图优化。
所述的对当前帧选通图像提取特征，指的是从图像中提取一些有代表性的特征点来代表整个图像信息，得到特征点和描述特征点信息的特征向量。本发明所选用的特征为ORB(Oriented FAST and Rotated BRIEF)特征，即利用FAST角点检测子提取关键点的位置，并为FAST角点增加尺度和旋转的描述，采用BRIEF描述子来描述关键点的属性。相比较于常用的SIFT特征和SURF特征，ORB特征提取方法速度上有一个数量级的优势，适用于AUV的实时导航定位。需要说明的是，本发明所提供水下AUV导航定位方法适用但不仅限于ORB特征。
所述的特征匹配指的是在相邻帧图像提取的特征点中，找到相对应的特征点，即特征点的描述子特征向量之间的距离越小，表示两个特征点越匹配。通过特征匹配，可以得到一组匹配点，以及它们的像素坐标。利用这些匹配关系，用对极几何约束即求解两帧之间的位姿关系，即视觉里程计约束。
所述的回环检测，采用的是基于欧氏距离的回环检测方法：当新的图像帧到来时，根据捷联惯性导航系统推算出当前时刻的位姿Xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿Xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧没有找到回环；若d不大于阈值d0，则认为当前帧找到了回环，将当前帧与最近位姿对应的选通图像进行特征提取并匹配，进而求出它们之间的位姿关系，即回环约束；其中阈值d0是根据实际应用中AUV的移动速度和成像系统的帧频来设置的。
步骤3、图优化模块将IMU位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，从而获取更准确的AUV在不同时刻的位姿。图优化模块输出的更准确的AUV在不同时刻的位姿不仅可以作为导航信息输出，还可以反馈给捷联解算系统SINS，为下一次迭代提供更准确的惯性数据输入。
所述的位姿图如图2所示，位姿图由下式表示：
G＝(X，F)
式中，X是位姿图中的节点，表示的是与选通图像相对应时刻AUV的位姿；F是位姿图中的边，表示的是节点之间或者单个节点的约束条件。本发明中的约束条件包含IMU位姿约束、视觉里程计约束和回环约束；可选的，GPS约束以及声呐、深度传感器、压力传感器等其他传感器的测量值均可以作为约束条件加入到位姿图中，来进一步提高AUV位姿估计的精度。
所述位姿图的优化求解，即在众多约束条件的联合约束下求解AUV的位姿，可以等效为最大似然估计问题，进一步的，在高斯噪声的假设下，可以等效为非线性最小二乘的求解问题。工程中，可以应用ceres solver、g2o等非线性库很方便的求解此类问题。
以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法，其特征在于，包括以下步骤：
通过捷联解算得到相应时刻包含AUV位置信息、速度、姿态信息的导航信息；
获取水下远距离、高清晰度的选通图像；
对IMU输出信息进行预积分，获取AUV在不同时刻的IMU位姿约束；
对当前帧的选通图像进行特征提取，并将上一帧的选通图像作为参考帧与之进行特征匹配，获取相邻图像之间的位姿关系，即视觉里程计约束；
对当前帧的选通图像进行回环检测，若检测到符合条件的回环帧图像，则将当前帧的选通图像与相应的回环帧图像进行特征匹配，获取回环检测约束；
将IMU位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，获取优化后的AUV位姿；
输出AUV位姿；
将AUV位姿作为惯性数据输入。
2.根据权利要求1所述的方法，其特征在于，对IMU进行预积分的具体方法为先捷联解算得到AUV在不同时刻速度、位置和姿态，再根据以下公式进行预积分处理，获取AUV在不同时刻之间的位姿约束因子，获取AUV在不同时刻的IMU位姿约束，
Figure FDA0002806698890000011
式中，
Figure FDA0002806698890000012
表示i时刻旋转矩阵的转置，Rj表示j时刻的旋转矩阵，vi和vj分别表示i时刻和j时刻的速度，g是重力加速度，pi和pj分别表示i时刻和j时刻的位置，Δtij表示i时刻和j时刻的时间差。
3.根据权利要求1所述的方法，其特征在于，特征提取的方法为ORB特征提取方法，即利用FAST角点检测子提取关键点的位置，并为FAST角点增加尺度和旋转的描述，采用BRIEF描述子来描述关键点的属性。
4.根据权利要求1所述的方法，其特征在于，特征匹配的方法为在相邻帧图像提取的特征点中，找到相对应的特征点，即特征点的描述子特征向量之间的距离越小，表示两个特征点越匹配。
5.根据权利要求1所述的方法，其特征在于，回环检测为基于欧氏距离的回环检测方法，即根据捷联惯性导航系统推算出当前时刻的位姿Xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿Xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧的选通图像无相应的回环帧图像；若d不大于阈值d0，则认为当前帧的选通图像有相应的回环帧图像，求出两者的位姿关系，即回环约束；其中阈值d0是根据实际应用中AUV的移动速度和成像系统的帧频来设置的。
6.根据权利要求1所述的方法，其特征在于，位姿图根据下式表示：
G＝(X，F)
式中，X是位姿图中的节点，表示的是与选通图像相对应时刻AUV的位姿；F是位姿图中的边，表示的是节点之间或者单个节点的约束条件。
7.一种用于实现权利要求1所述的RGI-Lidar/SINS紧耦合AUV水下导航定位方法的系统，其特征在于，包括RGI-Lidar，SINS和数据处理单元；其中，
RGI-Lidar，用于获取水下远距离、高清晰度的选通图像，并将选通图像发送至数据处理单元；
SINS，用于通过捷联惯导算法计算得到相应时刻的AUV位置信息、姿态信息的导航信息并发送至数据处理单元；
数据处理单元，用于根据AUV位置信息，对IMU进行预积分，获取AUV在不同时刻的IMU位姿约束；对当前帧的选通图像进行特征提取，并将上一帧的选通图像作为参考帧与之进行特征匹配，获取相邻图像之间的位姿关系，即视觉里程计约束；对当前帧的选通图像进行回环检测，若检测到符合条件的回环帧图像，则将当前帧的选通图像与相应的回环帧图像进行特征匹配，获取回环检测约束；将IMU位姿约束、视觉里程计约束以及回环检测约束作为约束关系加入到位姿图中进行联合优化，获取并输出优化后的AUV位姿；将AUV位姿发送至SINS作为惯性数据输入。
8.根据权利要求7所述的系统，其特征在于，对IMU进行预积分的具体方法为先计算出AUV在不同时刻速度、位置和姿态的相对值，再根据以下公式进行预积分处理，获取AUV在不同时刻之间的位姿约束因子，获取AUV在不同时刻的IMU位姿约束，
Figure FDA0002806698890000031
式中，
Figure FDA0002806698890000032
表示i时刻旋转矩阵的转置，Rj表示j时刻的旋转矩阵，vi和vj分别表示i时刻和j时刻的速度，g是重力加速度，pi和pj分别表示i时刻和j时刻的位置，Δtij表示i时刻和j时刻的时间差。
9.根据权利要求7所述的系统，其特征在于，回环检测为基于欧氏距离的回环检测方法，即根据捷联惯性导航系统推算出当前时刻的位姿Xi，并对之前时刻的位姿进行搜索，找到欧氏距离最近的位姿Xj，用d来表示最近欧氏距离；若d大于阈值d0，则认为当前帧的选通图像无相应的回环帧图像；若d不大于阈值d0，则认为当前帧的选通图像有相应的回环帧图像，求出它们之间的位姿关系，即回环约束；其中阈值d0是根据实际应用中AUV的移动速度和成像系统的帧频来设置的。
10.根据权利要求7所述的系统，其特征在于，还包括传感器组，用于采集多组观测信息加入到位姿图中进行融合。
CN202011373126.5A 2020-11-30 2020-11-30 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统 Active CN112611376B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011373126.5A CN112611376B (zh) 2020-11-30 2020-11-30 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011373126.5A CN112611376B (zh) 2020-11-30 2020-11-30 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统

Publications (2)

Publication Number Publication Date
CN112611376A true CN112611376A (zh) 2021-04-06
CN112611376B CN112611376B (zh) 2023-08-01

Family

ID=75228113

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011373126.5A Active CN112611376B (zh) 2020-11-30 2020-11-30 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统

Country Status (1)

Country Link
CN (1) CN112611376B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113077515A (zh) * 2021-06-07 2021-07-06 之江实验室 一种水下视觉惯导压力定位的紧耦合初始化方法
CN113256697A (zh) * 2021-04-27 2021-08-13 武汉理工大学 水下场景的三维重建方法、系统、装置和存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1656524A (zh) * 2002-03-22 2005-08-17 易卜拉欣·纳拉 车辆的导航、防撞、和控制系统
US8761439B1 (en) * 2011-08-24 2014-06-24 Sri International Method and apparatus for generating three-dimensional pose using monocular visual sensor and inertial measurement unit
CN104536009A (zh) * 2014-12-30 2015-04-22 华中科技大学 一种激光红外复合的地面建筑物识别及导航方法
US9103671B1 (en) * 2007-11-29 2015-08-11 American Vehicular Sciences, LLC Mapping techniques using probe vehicles
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1656524A (zh) * 2002-03-22 2005-08-17 易卜拉欣·纳拉 车辆的导航、防撞、和控制系统
US9103671B1 (en) * 2007-11-29 2015-08-11 American Vehicular Sciences, LLC Mapping techniques using probe vehicles
US8761439B1 (en) * 2011-08-24 2014-06-24 Sri International Method and apparatus for generating three-dimensional pose using monocular visual sensor and inertial measurement unit
CN104536009A (zh) * 2014-12-30 2015-04-22 华中科技大学 一种激光红外复合的地面建筑物识别及导航方法
US20160363653A1 (en) * 2014-12-30 2016-12-15 Huazhong University Of Science And Technology Above-ground building recognition method and navigation method combining laser and infrared
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
刘征宇;徐利刚;: "基于距离选通技术的水下目标成像系统研究", 光学仪器, no. 01, pages 82 - 85 *
杨述斌;彭复员;: "激光水下距离选通成像门控实验研究", 激光杂志, no. 03, pages 10 - 11 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113256697A (zh) * 2021-04-27 2021-08-13 武汉理工大学 水下场景的三维重建方法、系统、装置和存储介质
CN113077515A (zh) * 2021-06-07 2021-07-06 之江实验室 一种水下视觉惯导压力定位的紧耦合初始化方法
CN113077515B (zh) * 2021-06-07 2021-09-21 之江实验室 一种水下视觉惯导压力定位的紧耦合初始化方法

Also Published As

Publication number Publication date
CN112611376B (zh) 2023-08-01

Similar Documents

Publication Publication Date Title
Wu et al. Survey of underwater robot positioning navigation
Rahman et al. Sonar visual inertial slam of underwater structures
CN108444478B (zh) 一种用于水下航行器的移动目标视觉位姿估计方法
CN113340295B (zh) 一种多个测距传感器的无人艇近岸实时定位与建图方法
Eustice et al. Exactly sparse delayed-state filters for view-based SLAM
Chen et al. Review of AUV underwater terrain matching navigation
CN102042835B (zh) 自主式水下机器人组合导航系统
CN109901205B (zh) 一种水下机器人多传感器融合与运动轨迹预测方法
CN114488164B (zh) 水下航行器同步定位与建图方法及水下航行器
CN112611376B (zh) 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统
Ridao et al. USBL/DVL navigation through delayed position fixes
CN110186461A (zh) 一种基于重力梯度信息测距的协同导航方法
CN113108774B (zh) 水下机器人及水下机器人导航定位方法
Menna et al. Towards real-time underwater photogrammetry for subsea metrology applications
WO2023283987A1 (zh) 无人系统的传感器安全性检测方法、设备及存储介质
Rahman et al. Contour based reconstruction of underwater structures using sonar, visual, inertial, and depth sensor
Wang et al. An overview of key SLAM technologies for underwater scenes
CN116448100A (zh) 一种多传感器融合的近岸无人船slam方法
CN106971395A (zh) 一种基于点云平均背景差的三维声纳图像建模方法
Ma et al. A review of terrain aided navigation for underwater vehicles
CN103090861A (zh) 水下机器人的多线地形匹配导航方法
Yin et al. Study on underwater simultaneous localization and mapping based on different sensors
KR102263037B1 (ko) 수중 운동체와 수중 음향 탐지 장비를 이용한 수중 환경 지도 작성 시스템 및 그 방법
Medagoda et al. Water column current profile aided localisation combined with view-based SLAM for autonomous underwater vehicle navigation
Padial et al. Correlation of imaging sonar acoustic shadows and bathymetry for ROV terrain-relative localization

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