CN113218386B - 机器人在液体下建筑内高精度导航的方法和装置 - Google Patents

机器人在液体下建筑内高精度导航的方法和装置 Download PDF

Info

Publication number
CN113218386B
CN113218386B CN202110770136.0A CN202110770136A CN113218386B CN 113218386 B CN113218386 B CN 113218386B CN 202110770136 A CN202110770136 A CN 202110770136A CN 113218386 B CN113218386 B CN 113218386B
Authority
CN
China
Prior art keywords
navigation
robot
information
position information
related 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
CN202110770136.0A
Other languages
English (en)
Other versions
CN113218386A (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.)
Deepinfar Ocean Technology Inc
Original Assignee
Deepinfar Ocean Technology Inc
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 Deepinfar Ocean Technology Inc filed Critical Deepinfar Ocean Technology Inc
Priority to CN202110770136.0A priority Critical patent/CN113218386B/zh
Publication of CN113218386A publication Critical patent/CN113218386A/zh
Application granted granted Critical
Publication of CN113218386B publication Critical patent/CN113218386B/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/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
    • 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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本申请提供一种机器人在液体下建筑内高精度导航方法，包括：通过第一导航设备获取机器人的第一导航相关信息；通过第二导航设备获取机器人的第二导航相关信息；基于第一导航相关信息和第二导航相关信息获取机器人的初步位置信息；响应于建筑内特征部位的先验位置信息与机器人的初步位置信息之差在预定阈值内，通过图像声纳获取机器人的位置信息；以及融合第一导航相关信息、第二导航相关信息以及机器人的位置信息获取机器人的导航信息。本申请还提供机器人在液体下建筑内高精度导航的装置，依靠图像声纳识别液体下建筑的特征部位，实现机器人在液体下建筑内高精度导航。

Description

机器人在液体下建筑内高精度导航的方法和装置
技术领域
本申请涉及水下导航领域，尤其涉及机器人在液体下建筑内高精度导航的方法和装置。
背景技术
需要说明的是，本申请的高精度导航方案适于各种液体，例如，水、油、酒精等，为了方便起见，在如下的描述中将液体具体化为水。
目前，基于水下机器人的水利水电工程检测和维修是当前水利水电工程运维领域的一大趋势。水利水电工程检测和维修中经常需要水下机器人进入水利水电工程建筑内部检查设施状态，寻找缺陷和隐患并对缺陷部位进行修复。水利水电领域常见的水下建筑包含隧洞、倒虹吸、涵箱、水电站内部等。水下机器人的检测和修复作业需要水下机器人具备在水下建筑内高精度导航的能力。
由于电磁波等介质无法在水下长距离传播，陆上和空中大量使用的GNSS（GlobalNavigation Satellite System，全球导航卫星系统）无法在水下使用，水下高精度导航一直是导航领域内的难题。
陆上建筑内导航采用的技术手段包含wifi、蓝牙、视觉、激光等，在室内清扫机器人、室内设备、室内车辆和人员定位等领域存在大量应用。其中wifi和蓝牙的通信介质依然是电磁波，无法在水下长距离传播和有效应用。光线在水利水电工程中浑浊的水中传播距离有限，基于视觉或激光的导航方法难以使用。
水下导航的常用技术手段包括INS（Inertial Navigation System, 惯性导航系统）、水声DVL（Doppler Velocity Log，多普勒测速仪）、水声LBL/SBL/USBL导航（LongBaseline / Short Baseline / Ultra Short Baseline，长/短/超短基线）等。惯性导航存在误差随时间累积的问题。多普勒测速仪仅能够测速，定位误差也会随着时间累积，从而导致定位不准。LBL/SBL/USBL等水声导航需要外部水声基阵支持，且范围有限。
因此水下导航中一般将上述两种或多种导航方法综合应用，形成组合导航。然而，由于水声在水下建筑内存在遮挡、反射、多径等多种因素影响，LBL/SBL/USBL水声导航在水下建筑内应用效果不佳或根本无法使用；惯性导航、多普勒测速仪都存在定位误差，无法实现对水下机器人的高精度定位，在空间狭小的水下建筑内更加难以实现高精度导航，而且，惯性导航和多普勒测速仪的定位误差会随着时间累计，对于水利水电工程建筑内检测和作业的水下机器人，工作时间一般在2小时以上，“惯性导航+多普勒测速仪”组合导航的定位精度无法满足应用需求。
从而，由于水声在水下建筑内存在遮挡、反射、多径等多种因素影响，基于水声的导航方法在水下建筑内应用效果不佳甚至无法使用，水下建筑内的高精度导航尤其困难，相关的方案很少。
图像声纳（例如，多波束图像声纳、三维声纳、侧扫声纳）可以实时获取水下图像或点云（声纳图像），且具有测距能力，也可以认为是一种视觉手段。当前图像声纳多用于避障、目标探测、结构测量。也有研究利用SLAM（Simultaneous Localization and Mapping，同步定位与建图）等方法开展了图像声纳导航的探索。由于声纳图像与光学图像存在较大差别，信噪比低，无色彩信息，干扰多，基于图像声纳的导航应用并不成熟。
发明内容
基于此，本申请提供了一种实现机器人在水下建筑内高精度导航的方案，该方案将第一导航设备（例如，惯性导航设备）、第二导航设备（例如，多普勒测速仪）和图像声纳所提供的信息融合，形成组合导航，实现机器人在水下建筑内高精度导航。
具体来说，通过第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）组合，实现水下大范围、连续的导航定位，基于该定位信息获取在水下建筑结构内的初步位置；利用图像声纳拍摄机器人周围水下建筑的声纳图像，本方案中要求已知水下建筑的布局、结构等信息，已知的水下建筑中易识别的特征部位（如比较明显的建筑内设备、明显的突起、明显的拐角、管道的接缝、铆钉槽、线槽等）的具体位置信息，在声纳图像中识别这些已知位置的建筑部位，并根据声纳图像得到这些部位相对机器人的方位、距离等信息，可根据这些信息计算机器人的位置信息，也可将这些信息直接融合到组合导航中，优化计算水下机器人的位置信息，然后通过融合的方法修正第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）的导航误差，实现水下建筑内高精度导航。
根据本发明的第一个方面，提供一种机器人在液体下建筑内高精度导航方法，包括：
通过第一导航设备获取所述机器人的第一导航相关信息；
通过第二导航设备获取所述机器人的第二导航相关信息；
基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息；
响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，通过图像声纳获取所述机器人的位置信息；以及
融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息。
根据本发明的第二个方面，提供一种机器人在液体下建筑内高精度导航装置，包括：
第一获取单元，用于通过第一导航设备获取所述机器人的第一导航相关信息；
第二获取单元，用于通过第二导航设备获取所述机器人的第二导航相关信息；
第三获取单元，基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息；
第四获取单元，响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，用于通过图像声纳获取所述机器人的位置信息；以及
融合单元，用于融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息。
根据本发明的第三个方面，提供一种电子设备，包括：
处理器；以及
存储器，存储有计算机指令，当所述计算机指令被所述处理器执行时，使得所述处理器执行第一方面所述的方法。
根据本发明的第四个方面，提供一种非瞬时性计算机存储介质，存储有计算机程序，当所述计算机程序被多个处理器执行时，使得所述处理器执行第一方面所述的方法。
根据本发明提供了机器人在水下建筑内高精度导航的方法、装置、电子识别和非瞬时性计算机存储介质，依靠图像声纳识别水下建筑的特征部位，根据特征部位的先验位置信息以及声纳图像获取的机器人与特征部位的相对位置信息，可以获知机器人的当前位置信息，将机器人的当前位置信息与第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）提供的相关信息进行融合计算，可以得到机器人的导航信息，从而实现机器人在水下建筑内高精度导航。本发明的方案考虑了水下建筑内导航的特点，依靠水下建筑的特征部位提供的位置信息获取机器人当前精确位置信息，从而实现机器人的高精度导航。本发明的方案可用于水利水电、水下搜救、海油等所有需进入水下工程、沉船、大型水下设备等已知构造内部导航的场景。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获取其他的附图，而并不超出本申请要求保护的范围。
图1为根据本申请实施例的机器人在水下建筑内高精度导航的逻辑示意图。
图2为根据申请实施例的基于图像声纳获取水下机器人导航信息进行组合导航的流程图。
图3为根据本申请实施例的机器人在水下建筑内高精度导航方法的流程图。
图4为声纳图像中获取特征部位相对水下机器人的距离和方位信息的示意图。
图5为根据本申请实施例的机器人在水下建筑内高精度导航装置的示意图。
图6是本发明提供的一种电子设备的结构图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
图1为根据本申请实施例的机器人在水下建筑内高精度导航的逻辑示意图，大体上显示了本申请方案的逻辑。如图1所示，惯性导航提供了机器人的第一导航相关信息，该第一导航相关信息包括机器人的姿态、速度、位置、加速度；多普勒测速仪提供了第二导航相关信息，该第二导航相关信息包括机器人的速度；图像声纳提供了机器人的位置信息；然后，根据第一导航相关信息、第二导航相关信息和机器人的位置信息进行融合计算，得到机器人的导航信息，该导航信息包括机器人的位置、速度和姿态等。
图2为根据申请实施例的基于图像声纳获取水下机器人导航信息进行组合导航的流程图。在图2中，第一导航设备和第二导航设备分别为惯性导航系统和多普勒测速仪（DVL），二者提供连续的导航信息，包括提供机器人的初步位置信息。
在水下建筑内，可以存在一个或多个特征部位，且这些特征部位的位置是已知的，称为建筑内特征部位的先验位置信息。根据惯性导航和DVL提供的机器人的初步位置信息，判断机器人目前是否已经接近某一个特征部位，在接近特征部位时，机器人通过自动的方式或者在人工操纵下，用图像声纳对周围成像，在图像声纳中识别已知水下建筑内的特征部位，在识别到特征部位后，声纳图像测量特征部位相对机器人的相对位置信息，包括距离和方位信息，然后根据声纳图像获得机器人的位置信息，最后，将通过声纳图像获得的机器人的位置信息参与组合导航，进行数据融合，修正惯性导航和DVL的导航误差。
根据本发明的一个方面，提供一种机器人在水下建筑内高精度导航方法。图3为根据本申请实施例的机器人在水下建筑内高精度导航方法的流程图。如图3所示，该方法包括如下步骤。
步骤S301，通过第一导航设备获取所述机器人的第一导航相关信息。
在一个具体实施例中，第一导航设备包括惯性导航设备。通过惯性导航结果获取机器人的第一导航相关信息，该第一导航相关信息包括机器人的姿态、速度、位置、加速度和角速度信息等。
步骤S302，通过第二导航设备获取所述机器人的第二导航相关信息。
在一个具体实施例中，第二导航设备包括多普勒测速仪。通过多普勒测速仪获取机器人的第二导航相关信息，该第二导航相关信息包括机器人的速度。
步骤S303，基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息。
通过步骤S301和步骤S302，基于第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）的组合导航结果获取机器人的初步位置信息。
在步骤S301和步骤S302中，以惯性导航设备和多普勒测速仪为例描述第一导航设备和第二导航设备，它们的导航结果作为水下大范围、连续的导航定位，本领域技术人员在上述方案的启发下，能够想到的其他导航设备的组合都属于本申请覆盖的范围。
步骤S304，响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，通过图像声纳获取所述机器人的位置信息。
在本申请的方案中，要求已知水下建筑的布局、结构等信息，已知的水下建筑中易识别的特征部位（如比较明显的建筑内设备、明显的突起、明显的拐角、管道的接缝、铆钉槽、线槽等）的具体位置信息。在通过第一导航设备和第二导航设备获得机器人的初步位置信息后，判断该初步位置与建筑内特征部位的先验位置是否接近，通常可以通过一个预定阈值来确定，如果建筑内特征部位的先验位置信息与机器人的初步位置信息之差在预定阈值内，该预定阈值可以根据所在建筑的尺寸、建筑布局和图像声纳性能等具体情形进行设定，则通过图像声纳获取机器人的位置信息。具体来说，在声纳图像中识别这些已知位置的水下建筑的特征部位，并根据声纳图像得到这些特征部位相对水下机器人的方位、距离等信息，可根据这些信息获取机器人的位置信息。从而，步骤S304具体包括：
子步骤S1：根据建筑内特征部位的成像特点识别所述特征部位。
图像声纳根据先验信息已知水下建筑中一些特征部位的位置（根据导航需要，该位置坐标可以在大地地理坐标系内表示，也可以在其他定义的局部坐标系内表示），并且根据先验信息已知这些特征部位在图像声纳上的成像特点，以便于进行识别。当水下机器人的图像声纳对特征部位成像后，可以识别出这些特征部位。在声纳图像中识别已知水下建筑的特征部位，可以采用计算机视觉中特征提取和匹配的方式，也可以采用深度学习方式进行训练和识别。
子步骤S2：获取所述机器人相对所述特征部位的相对位置信息。
当图像声纳识别出特征部位后，就可以根据先验信息获取特征部位的位置信息。图4为声纳图像中获取特征部位相对水下机器人的距离和方位信息的示意图。如图4所示的多波束声纳图像中，根据声纳图像上识别到的水下建筑的特征部位，得到特征部位在图像上的坐标，可以得到水下机器人相对特征部位的相对位置信息，该相对位置信息包括机器人相对特征部位中心点的距离和方位信息。
在获取机器人的相对位置信息后，就可以获取机器人的绝对位置信息。步骤S304还包括：
子步骤S3：根据所述特征部位的先验位置信息以及所述相对位置信息获取所述机器人的绝对位置信息。
在一个具体实施例中，假设根据声纳图像得到的机器人相对特征部位中心点的距 离和方位角分别为L和
Figure 531831DEST_PATH_IMAGE001
，若此时水下机器人声纳的姿态用姿态矩阵
Figure 906181DEST_PATH_IMAGE002
表示，该矩阵可 以是一3*3的矩阵，可以将声纳坐标系（记作s系）表示的矢量转换到导航坐标系（记作n系）， 即某矢量在s系的表示
Figure 517553DEST_PATH_IMAGE003
为，则其在n系的表示
Figure 483104DEST_PATH_IMAGE004
为
Figure 960353DEST_PATH_IMAGE005
（1）
姿态矩阵
Figure 905437DEST_PATH_IMAGE002
可以根据水下机器人导航的姿态、声纳在水下机器人上的安装关 系、和成像时刻声纳的指向角等信息计算得到。
若已知特征部位中心点的位置矢量为P ref ，假设水下机器人的位置为P（该位置矢 量为与P ref 相同的导航坐标系内表示），则相对距离L和相对方位角
Figure 932168DEST_PATH_IMAGE001
满足
Figure 616090DEST_PATH_IMAGE006
（2）
其中符号| |表示对其中的矢量取大小，[]x和[]y分别表示取其中矢量的x和y分 量，
Figure 596947DEST_PATH_IMAGE007
表示姿态矩阵
Figure 391727DEST_PATH_IMAGE002
的转置。
若定义声纳坐标系为前-左-上坐标系，即声纳成像探测的正前方为x轴，左侧90度为y轴，垂直xy平面向上为z轴，则水下建筑的特征部位和水下机器人位置差在声纳坐标系（s系）的表示为
Figure 272965DEST_PATH_IMAGE008
（3）
根据上述关系式可以计算得到依据声纳测量的水下机器人位置P：
Figure 596630DEST_PATH_IMAGE009
（4）
由于图像声纳测量的水下机器人位置一般存在误差，为便于理解，在组合导航中，将水下机器人的真实位置用P表示，将图像声纳测量的水下机器人位置用P sonar 表示。
这样，就获取机器人的绝对位置信息。需要说明的是，上述计算机器人绝对位置信息的方法只是一个具体的方式，本领域技术人员能够获知根据不同的应用场景和设备，还可以采用其他方式计算机器人的绝对位置信息，这都属于本申请涵盖的范围。
步骤S305，融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息。
根据本发明的一个方面，在获取所述机器人相对所述特征部位的相对位置信息后，可以将该相对位置信息直接与第一导航相关信息和第二导航相关信息进行融合，得到机器人的导航信息。这样，步骤S305包括：融合所述第一导航相关信息、所述第二导航相关信息以及所述相对位置信息获取所述机器人的导航信息。
根据本发明的另一个方面，还可以将机器人的绝对位置信息与第一导航相关信息和第二导航相关信息进行融合，得到机器人的导航信息。这样，步骤S305包括：融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的绝对位置信息获取所述机器人的导航信息。
在一个具体实施例中，执行步骤S305的方式包括Kalman滤波法、粒子滤波法和最优估计法。
下面，以惯性导航、多普勒测速仪和图像声纳的组合按照Kalman滤波法为例，来简单说明本发明的融合过程。
Kalman滤波需要首先建立状态方程和观测方程：
状态方程：
Figure 376367DEST_PATH_IMAGE010
（5）
观测方程：
Figure 459992DEST_PATH_IMAGE011
（6）
其中X表示滤波器状态矢量，
Figure 946468DEST_PATH_IMAGE012
表示状态对时间的导数，f(X)表示状态的函数，W表 示状态模型噪声；Z表示观测矢量，h(X)表示表达观测矢量的状态函数，V表示观测噪声。
在此基础上就可以利用Kalman滤波的框架进行信息融合，优化估计状态矢量X。Kalman滤波是目前公知的技术，本发明不做详细介绍。
本发明的信息融合若采用Kalman滤波，则需首先建立滤波器状态，本发明将水下机器人的姿态、速度和位置误差作为滤波器状态矢量（也可包含惯性器件误差、DVL器件误差等）：
Figure 955881DEST_PATH_IMAGE013
（7）
其中，
Figure 160597DEST_PATH_IMAGE014
为水下机器人姿态误差角，
Figure 47913DEST_PATH_IMAGE015
为水下机器人速度误差，
Figure 185633DEST_PATH_IMAGE016
为水下机器人 位置误差。若水下机器人的姿态用姿态矩阵
Figure 365948DEST_PATH_IMAGE017
表示，与上文介绍的
Figure 57960DEST_PATH_IMAGE002
类似，
Figure 483388DEST_PATH_IMAGE017
为水下机 器人本体系(记为b系)到导航系(n系)的转换矩阵，该矩阵为3*3矩阵，可将b系表示的矢量 转换到n系表示，则导航系统计算的姿态矩阵结果
Figure 741194DEST_PATH_IMAGE018
（有误差）与真值
Figure 108721DEST_PATH_IMAGE017
（无误差）、姿态误 差角
Figure 6139DEST_PATH_IMAGE019
间的关系满足
Figure 484525DEST_PATH_IMAGE020
，其中I 3 表示3*3单位阵，
Figure 601430DEST_PATH_IMAGE021
表示姿态误差角矢量的叉 乘矩阵；记水下机器人速度为
Figure 874280DEST_PATH_IMAGE022
，则导航系统速度计算结果
Figure 524573DEST_PATH_IMAGE023
（有误差）与真值
Figure 541070DEST_PATH_IMAGE024
（无误差） 间满足
Figure 773468DEST_PATH_IMAGE025
；记水下机器人的位置为P，则导航系统位置计算结果含误差的位置导航 结果
Figure 967952DEST_PATH_IMAGE026
（有误差）与真值P（无误差）间满足
Figure 121852DEST_PATH_IMAGE027
；上标T表示矢量的转置。
Kalman滤波的状态方程可根据惯性导航系统误差传播规律建立。
根据惯性导航理论，惯性导航中姿态误差角、速度误差和位置误差各状态矢量满足：
Figure 925729DEST_PATH_IMAGE028
（8）
其中
Figure 215896DEST_PATH_IMAGE029
表示导航系(n系)相对惯性系(记为i系)的旋转角速度在导航系的表示，
Figure 846860DEST_PATH_IMAGE030
表示
Figure 19215DEST_PATH_IMAGE031
的计算误差，
Figure 626783DEST_PATH_IMAGE032
为陀螺测量的角速度误差，f b 是加速度计测量的比力，
Figure 37036DEST_PATH_IMAGE033
表示 加速度计测量的比力误差，
Figure 307742DEST_PATH_IMAGE034
表示地球自转角速度，
Figure 170656DEST_PATH_IMAGE035
表示水下机器人运动导致的系相对 地球系(记为e系)的旋转角速度，
Figure 581915DEST_PATH_IMAGE036
、
Figure 643412DEST_PATH_IMAGE037
表示
Figure 865446DEST_PATH_IMAGE034
和
Figure 966388DEST_PATH_IMAGE035
的计算误差，
Figure 666491DEST_PATH_IMAGE038
表示重力加速度计 算误差。
依据公式（8）可以建立Kalman滤波状态方程，建立方法可参考公开文献，本发明不做详细推导。
Kalman滤波还需要建立观测方程，本发明中的观测包含DVL的速度测量和图像声纳的位置测量。
DVL的速度观测v DVL 是在水下机器人本体系(b系)的表示，需要借助导航的姿态矩 阵结果
Figure 769445DEST_PATH_IMAGE039
转换到导航坐标系：
Figure 162380DEST_PATH_IMAGE040
（9）
图像声纳的位置观测前文的公式（2）~（4）已做说明。
由于状态矢量为误差形式，Kalman滤波的观测矢量也写成误差形式。当声纳图像 中无特征部位时，仅采用惯性导航和DVL组合，观测矢量为：
Figure 750619DEST_PATH_IMAGE041
（10）
当图像声纳识别到特征部位时，依据特征部位信息计算得到的水下机器人位置参与信息融合，观测矢量为：
Figure 254412DEST_PATH_IMAGE042
（11）
依据公式（10）、（11）可以建立Kalman滤波器观测方程，具体建立方法可参考公开文献，本发明不做详细推导。
利用Kalman滤波数据融合优化估计得到状态矢量
Figure 274190DEST_PATH_IMAGE043
、
Figure 838026DEST_PATH_IMAGE044
和
Figure 913561DEST_PATH_IMAGE016
后，进而可反馈修正 导航结果，得到滤波后的导航姿态、速度和位置：
Figure 955466DEST_PATH_IMAGE045
（12）
其中矢量上的“~”符号表示该矢量有误差，“^”符号表示矢量的优化估计值。
需要说明的是，在上述第一导航设备、第二导航设备和图像声纳的导航组合方案中，第一导航设备和第二导航设备实现水下大范围、连续的导航定位，提供初步的导航信息，而图像声纳是间歇性参与导航组合方案，在识别到建筑内特征部位时，才参与到第一导航设备和第二导航设备中，对第一导航设备和第二导航设备的导航误差进行修正。
根据本发明提供了机器人在水下建筑内高精度导航的方法，依靠图像声纳识别水下建筑的特征部位，根据特征部位的先验位置信息以及声纳图像获取的机器人与特征部位的相对位置信息，可以获知机器人的当前位置信息，将机器人的当前位置信息与第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）提供的相关信息进行融合计算，可以得到机器人的导航信息，从而实现机器人在水下建筑内高精度导航。本发明的方案考虑了水下建筑内导航的特点，依靠水下建筑的特征部位提供的位置信息获取机器人当前精确位置信息，从而实现机器人的高精度导航。
根据本发明的另一个方面，提供一种机器人在水下建筑内高精度导航装置。图5为根据本申请实施例的机器人在水下建筑内高精度导航装置的示意图。如图5所示，该装置包括如下单元。
第一获取单元501，用于通过第一导航设备获取所述机器人的第一导航相关信息。
在一个具体实施例中，第一导航设备包括惯性导航设备。通过惯性导航结果获取机器人的第一导航相关信息，该第一导航相关信息包括机器人的姿态、速度、位置、加速度和角速度信息等。
第二获取单元502，用于通过第二导航设备获取所述机器人的第二导航相关信息。
在一个具体实施例中，第二导航设备包括多普勒测速仪。通过多普勒测速仪获取机器人的第二导航相关信息，该第二导航相关信息包括机器人的速度。
第三获取单元503，用于基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息。
通过第一获取单元501和第二获取单元502，基于第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）的组合导航结果获取机器人的初步位置信息。
在第一获取单元501和第二获取单元502中，以惯性导航设备和多普勒测速仪为例描述第一导航设备和第二导航设备，它们的导航结果作为水下大范围、连续的导航定位，本领域技术人员在上述方案的启发下，能够想到的其他导航设备的组合都属于本申请覆盖的范围。
第四获取单元504，用于响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，通过图像声纳获取所述机器人的位置信息。
在本申请的方案中，要求已知水下建筑的布局、结构等信息，已知的水下建筑中易识别的特征部位（如比较明显的建筑内设备、明显的突起、明显的拐角、管道的接缝、铆钉槽、线槽等）的具体位置信息。在通过第一导航设备和第二导航设备获得机器人的初步位置信息后，判断该初步位置与建筑内特征部位的先验位置是否接近，通常可以通过一个预定阈值来确定，如果建筑内特征部位的先验位置信息与机器人的初步位置信息之差在预定阈值内，该预定阈值可以根据所在建筑的尺寸、建筑布局和图像声纳性能等具体情形进行设定，则通过图像声纳获取机器人的位置信息。具体来说，在声纳图像中识别这些已知位置的水下建筑的特征部位，并根据声纳图像得到这些特征部位相对水下机器人的方位、距离等信息，可根据这些信息获取机器人的位置信息。从而，第四获取单元504具体包括：
识别子单元，用于根据建筑内特征部位的成像特点识别所述特征部位。
图像声纳根据先验信息已知水下建筑中一些特征部位的位置（根据导航需要，该位置坐标可以在大地地理坐标系内表示，也可以在其他定义的局部坐标系内表示），并且根据先验信息已知这些特征部位在图像声纳上的成像特点，以便于进行识别。当水下机器人的图像声纳对特征部位成像后，可以识别出这些特征部位。在声纳图像中识别已知水下建筑的特征部位，可以采用计算机视觉中特征提取和匹配的方式，也可以采用深度学习方式进行训练和识别。
第一获取子单元，用于获取所述机器人相对所述特征部位的相对位置信息。
当图像声纳识别出特征部位后，就可以根据先验信息获取特征部位的位置信息。图5为声纳图像中获取特征部位相对水下机器人的距离和方位信息的示意图。如图5所示的多波束声纳图像中，根据声纳图像上识别到的水下建筑的特征部位，得到特征部位在图像上的坐标，可以得到水下机器人相对特征部位的相对位置信息，该相对位置信息包括机器人相对特征部位中心点的距离和方位信息。
在获取机器人的相对位置信息后，就可以获取机器人的绝对位置信息。第四获取单元504还包括：
第二获取子单元，用于根据所述特征部位的先验位置信息以及所述相对位置信息获取所述机器人的绝对位置信息。
融合单元505，用于融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息。
根据本发明的一个方面，在获取所述机器人相对所述特征部位的相对位置信息后，可以将该相对位置信息直接与第一导航相关信息和第二导航相关信息进行融合，得到机器人的导航信息。这样， 融合单元505用于：融合所述第一导航相关信息、所述第二导航相关信息以及所述相对位置信息获取所述机器人的导航信息。
根据本发明的另一个方面，还可以将机器人的绝对位置信息与第一导航相关信息和第二导航相关信息进行融合，得到机器人的导航信息。这样，融合单元505用于：融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的绝对位置信息获取所述机器人的导航信息。
在一个具体实施例中，融合单元505的实施方式包括Kalman滤波法、粒子滤波法和最优估计法。
需要说明的是，在上述第一导航设备、第二导航设备和图像声纳的导航组合方案中，第一导航设备和第二导航设备实现水下大范围、连续的导航定位，提供初步的导航信息，而图像声纳是间歇性参与导航组合方案，在识别到建筑内特征部位时，才参与到第一导航设备和第二导航设备中，对第一导航设备和第二导航设备的导航误差进行修正。
根据本发明提供了机器人在水下建筑内高精度导航的装置，依靠图像声纳识别水下建筑的特征部位，根据特征部位的先验位置信息以及声纳图像获取的机器人与特征部位的相对位置信息，可以获知机器人的当前位置信息，将机器人的当前位置信息与第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）提供的相关信息进行融合计算，可以得到机器人的导航信息，从而实现机器人在水下建筑内高精度导航。本发明的方案考虑了水下建筑内导航的特点，依靠水下建筑的特征部位提供的位置信息获取机器人当前精确位置信息，从而实现机器人的高精度导航。
参阅图6，图6提供一种电子设备，包括处理器；以及存储器，所述存储器存储有计算机指令，当所述计算机指令被所述处理器执行时，使得所述处理器执行所述计算机指令时实现如图3所示的方法以及细化方案。
应该理解，上述的装置实施例仅是示意性的，本发明披露的装置还可通过其它的方式实现。例如，上述实施例中所述单元/模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。例如，多个单元、模块或组件可以结合，或者可以集成到另一个系统，或一些特征可以忽略或不执行。
另外，若无特别说明，在本发明各个实施例中的各功能单元/模块可以集成在一个单元/模块中，也可以是各个单元/模块单独物理存在，也可以两个以上单元/模块集成在一起。上述集成的单元/模块既可以采用硬件的形式实现，也可以采用软件程序模块的形式实现。
所述集成的单元/模块如果以硬件的形式实现时，该硬件可以是数字电路，模拟电路等等。硬件结构的物理实现包括但不局限于晶体管，忆阻器等等。若无特别说明，所述处理器或芯片可以是任何适当的硬件处理器，比如CPU、GPU、FPGA、DSP和ASIC等等。若无特别说明，所述片上缓存、片外内存、存储器可以是任何适当的磁存储介质或者磁光存储介质，比如，阻变式存储器RRAM（Resistive Random Access Memory）、动态随机存取存储器DRAM（Dynamic Random Access Memory）、静态随机存取存储器SRAM（Static Random-AccessMemory）、增强动态随机存取存储器EDRAM（Enhanced Dynamic Random Access Memory）、高带宽内存HBM（High-Bandwidth Memory）、混合存储立方 HMC（Hybrid Memory Cube）等等。
所述集成的单元/模块如果以软件程序模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储器中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干指令用以使得一台计算机设备（可为个人计算机、服务器或者网络设备等）执行本披露各个实施例所述方法的全部或部分步骤。而前述的存储器包括：U盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
本申请实施例还提供一种非瞬时性计算机存储介质，存储有计算机程序，当所述计算机程序被多个处理器执行时，使得所述处理器执行如图3所示的方法以及细化方案。
以上对本申请实施例进行了详细介绍，本文中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明仅用于帮助理解本申请的方法及其核心思想。同时，本领域技术人员依据本申请的思想，基于本申请的具体实施方式及应用范围上做出的改变或变形之处，都属于本申请保护的范围。综上所述，本说明书内容不应理解为对本申请的限制。

Claims (14)

1.一种机器人在液体下建筑内高精度导航方法，其特征是，所述方法包括：
通过第一导航设备获取所述机器人的第一导航相关信息，其中，所述第一导航设备包括惯性导航设备，所述第一导航相关信息包括姿态、速度、位置、加速度和角速度；
通过第二导航设备获取所述机器人的第二导航相关信息，其中，所述第二导航设备包括多普勒测速仪，所述第二导航相关信息包括速度；
基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息；
响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，通过图像声纳获取所述机器人的位置信息；以及
融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息，其中，所述导航信息包括所述机器人的导航姿态、速度和位置信息；
其中，所述通过图像声纳获取所述机器人的位置信息包括：
根据建筑内特征部位的成像特点识别所述特征部位；以及
获取所述机器人相对所述特征部位的相对位置信息；其中，所述相对位置信息包括所述机器人相对所述特征部位中心点的距离和方位信息。
2.如权利要求1所述的方法，其特征是，所述通过图像声纳获取所述机器人的位置信息还包括：
根据所述特征部位的先验位置信息以及所述相对位置信息获取所述机器人的绝对位置信息。
3.如权利要求1所述的方法，其特征是，所述融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息包括：
融合所述第一导航相关信息、所述第二导航相关信息以及所述相对位置信息获取所述机器人的导航信息。
4.如权利要求2所述的方法，其特征是，所述融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息包括：
融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的绝对位置信息获取所述机器人的导航信息。
5.如权利要求1所述的方法，其特征是，所述根据特征部位的成像特点识别所述特征部位的方式包括计算机视觉中特征提取和匹配的方式以及深度学习方式。
6.如权利要求1所述的方法，其特征是，所述融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息的方式包括Kalman滤波法、粒子滤波法和最优估计法。
7.一种机器人在液体下建筑内高精度导航装置，其特征是，所述装置包括：
第一获取单元，用于通过第一导航设备获取所述机器人的第一导航相关信息，其中，所述第一导航设备包括惯性导航设备，所述第一导航相关信息包括姿态、速度、位置、加速度和角速度；
第二获取单元，用于通过第二导航设备获取所述机器人的第二导航相关信息，其中，所述第二导航设备包括多普勒测速仪，所述第二导航相关信息包括速度；
第三获取单元，基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息；
第四获取单元，响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，用于通过图像声纳获取所述机器人的位置信息；以及
融合单元，用于融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息，其中，所述导航信息包括所述机器人的导航姿态、速度和位置信息；
其中，所述第四获取单元包括：
识别子单元，用于根据建筑内特征部位的成像特点识别所述特征部位；以及
第一获取子单元，用于获取所述机器人相对所述特征部位的相对位置信息；其中，所述相对位置信息包括所述机器人相对所述特征部位中心点的距离和方位信息。
8.如权利要求7所述的装置，其特征是，所述第四获取单元还包括：
第二获取子单元，用于根据所述特征部位的先验位置信息以及所述相对位置信息获取所述机器人的绝对位置信息。
9.如权利要求7所述的装置，其特征是，所述融合单元用于融合所述第一导航相关信息、所述第二导航相关信息以及所述相对位置信息获取所述机器人的导航信息。
10.如权利要求8所述的装置，其特征是，所述融合单元用于融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的绝对位置信息获取所述机器人的导航信息。
11.如权利要求7所述的装置，其特征是，所述根据特征部位的成像特点识别所述特征部位的方式包括计算机视觉中特征提取和匹配的方式以及深度学习方式。
12.如权利要求7所述的装置，其特征是，所述融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息的方式包括Kalman滤波法、粒子滤波法和最优估计法。
13.一种电子设备，其特征是，所述电子设备包括：
处理器；以及
存储器，存储有计算机指令，当所述计算机指令被所述处理器执行时，使得所述处理器执行权利要求1-6任一者所述的方法。
14.一种非瞬时性计算机存储介质，存储有计算机程序，其特征是，当所述计算机程序被多个处理器执行时，使得所述处理器执行权利要求1-6任一者所述的方法。
CN202110770136.0A 2021-07-08 2021-07-08 机器人在液体下建筑内高精度导航的方法和装置 Active CN113218386B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110770136.0A CN113218386B (zh) 2021-07-08 2021-07-08 机器人在液体下建筑内高精度导航的方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110770136.0A CN113218386B (zh) 2021-07-08 2021-07-08 机器人在液体下建筑内高精度导航的方法和装置

Publications (2)

Publication Number Publication Date
CN113218386A CN113218386A (zh) 2021-08-06
CN113218386B true CN113218386B (zh) 2021-10-08

Family

ID=77081186

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110770136.0A Active CN113218386B (zh) 2021-07-08 2021-07-08 机器人在液体下建筑内高精度导航的方法和装置

Country Status (1)

Country Link
CN (1) CN113218386B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113776514A (zh) * 2021-08-10 2021-12-10 深之蓝海洋科技股份有限公司 液体下检测竖直表面的机器人的相对导航方法和装置
WO2023132752A1 (en) * 2022-01-05 2023-07-13 Norwegian Defence Research Establishment Navigation aiding method and apparatus
CN115599126A (zh) * 2022-12-15 2023-01-13 深之蓝海洋科技股份有限公司（Cn） 自动避碰的无线遥控无人潜水器和自动避碰方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105256847A (zh) * 2015-10-20 2016-01-20 中国水利水电第七工程局有限公司 一种水下建筑物定位转移测量装置与方法
CN108344999A (zh) * 2018-01-09 2018-07-31 浙江大学 一种用于水下机器人导航的声呐地图构建与重定位方法
CN111307136A (zh) * 2020-02-12 2020-06-19 哈尔滨工程大学 一种双智能水下机器人水下航行地形匹配导航方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108663051A (zh) * 2018-04-28 2018-10-16 南京信息工程大学 一种水下无源组合导航系统建模及信息融合方法
CN110221278B (zh) * 2019-06-17 2021-07-30 中国科学院声学研究所 一种基于多传感器组合的合成孔径声呐运动补偿方法
CN110907936B (zh) * 2019-11-22 2021-11-16 哈尔滨工程大学 一种水下三维地形匹配定位导航声呐及导航方法
CN112347070B (zh) * 2020-11-06 2023-05-23 北京石油化工学院 自主潜航器容迟探测系统及方法
CN112946660A (zh) * 2021-01-28 2021-06-11 西北工业大学 一种基于多波束前视声呐的同时定位与建图方法
CN113075665B (zh) * 2021-03-24 2023-06-20 鹏城实验室 水下定位方法、水下载体航行器及计算机可读存储介质

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105256847A (zh) * 2015-10-20 2016-01-20 中国水利水电第七工程局有限公司 一种水下建筑物定位转移测量装置与方法
CN108344999A (zh) * 2018-01-09 2018-07-31 浙江大学 一种用于水下机器人导航的声呐地图构建与重定位方法
CN111307136A (zh) * 2020-02-12 2020-06-19 哈尔滨工程大学 一种双智能水下机器人水下航行地形匹配导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种结合地形和环境特征的水下导航定位方法;徐晓苏等;《中国惯性技术学报》;20151031;第2卷(第5期);第590-596页 *

Also Published As

Publication number Publication date
CN113218386A (zh) 2021-08-06

Similar Documents

Publication Publication Date Title
CN113218386B (zh) 机器人在液体下建筑内高精度导航的方法和装置
Wu et al. Survey of underwater robot positioning navigation
US20190033867A1 (en) Systems and methods for determining a vehicle position
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
Menna et al. Towards real-time underwater photogrammetry for subsea metrology applications
CN115307646B (zh) 一种多传感器融合的机器人定位方法、系统及装置
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能体位姿估计方法
Cahyadi et al. Performance of GPS and IMU sensor fusion using unscented Kalman filter for precise i-Boat navigation in infinite wide waters
CN114355956A (zh) 一种基于三维点云的水下机器人路径规划方法
Ding et al. RD-VIO: Relative-depth-aided visual-inertial odometry for autonomous underwater vehicles
US11561553B1 (en) System and method of providing a multi-modal localization for an object
Song et al. Acoustic-VINS: Tightly coupled acoustic-visual-inertial navigation system for autonomous underwater vehicles
Ho et al. Smartphone level indoor/outdoor ubiquitous pedestrian positioning 3DMA GNSS/VINS integration using FGO
Lager et al. Underwater terrain navigation using standard sea charts and magnetic field maps
Ma et al. A Vision-Integrated Navigation Method in AUV Terminal Mobile Docking Based on Factor Graph Optimization
Menna et al. Particle filter based autonomous underwater vehicle navigation system aided thru acoustic communication ranging
CN113470342B (zh) 一种自运动估计的方法及装置
CN109737950B (zh) 基于vio和动力学模型的水下机器人定位系统及方法
Hsu et al. New integrated navigation scheme for the level 4 autonomous vehicles in dense urban areas
KR20220084910A (ko) 벡터 맵을 이용한 이동체의 맵 매칭 위치 추정 방법
CN107990896B (zh) 一种基于xyz格式地图的路径生成方法
Li et al. Sonar image processing based underwater localization method and its experimental studies
Đuraš et al. Recovering the 3d uuv position using UAV imagery in shallow-water environments
CN113776514A (zh) 液体下检测竖直表面的机器人的相对导航方法和装置
Wen 3D LiDAR aided GNSS positioning and its application in sensor fusion for autonomous vehicles in urban canyons

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