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

摘要

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

权利要求

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任一者所述的方法。

说明书

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

技术领域

[0001] 本申请涉及水下导航领域，尤其涉及机器人在液体下建筑内高精度导航的方法和装置。

背景技术

[0002] 需要说明的是，本申请的高精度导航方案适于各种液体，例如，水、油、酒精等，为了方便起见，在如下的描述中将液体具体化为水。

[0003] 目前，基于水下机器人的水利水电工程检测和维修是当前水利水电工程运维领域的一大趋势。水利水电工程检测和维修中经常需要水下机器人进入水利水电工程建筑内部
检查设施状态，寻找缺陷和隐患并对缺陷部位进行修复。水利水电领域常见的水下建筑包
含隧洞、倒虹吸、涵箱、水电站内部等。水下机器人的检测和修复作业需要水下机器人具备
在水下建筑内高精度导航的能力。

[0004] 由于电磁波等介质无法在水下长距离传播，陆上和空中大量使用的GNSS（Global Navigation Satellite System，全球导航卫星系统）无法在水下使用，水下高精度导航一
直是导航领域内的难题。

[0005] 陆上建筑内导航采用的技术手段包含wifi、蓝牙、视觉、激光等，在室内清扫机器人、室内设备、室内车辆和人员定位等领域存在大量应用。其中wifi和蓝牙的通信介质依然
是电磁波，无法在水下长距离传播和有效应用。光线在水利水电工程中浑浊的水中传播距
离有限，基于视觉或激光的导航方法难以使用。

[0006] 水下导航的常用技术手段包括INS（Inertial Navigation System, 惯性导航系统）、水声DVL（Doppler Velocity Log，多普勒测速仪）、水声LBL/SBL/USBL导航（Long 
Baseline / Short Baseline / Ultra Short Baseline，长/短/超短基线）等。惯性导航存
在误差随时间累积的问题。多普勒测速仪仅能够测速，定位误差也会随着时间累积，从而导
致定位不准。LBL/SBL/USBL等水声导航需要外部水声基阵支持，且范围有限。

[0007] 因此水下导航中一般将上述两种或多种导航方法综合应用，形成组合导航。然而，由于水声在水下建筑内存在遮挡、反射、多径等多种因素影响，LBL/SBL/USBL水声导航在水
下建筑内应用效果不佳或根本无法使用；惯性导航、多普勒测速仪都存在定位误差，无法实
现对水下机器人的高精度定位，在空间狭小的水下建筑内更加难以实现高精度导航，而且，
惯性导航和多普勒测速仪的定位误差会随着时间累计，对于水利水电工程建筑内检测和作
业的水下机器人，工作时间一般在2小时以上，“惯性导航+多普勒测速仪”组合导航的定位
精度无法满足应用需求。

[0008] 从而，由于水声在水下建筑内存在遮挡、反射、多径等多种因素影响，基于水声的导航方法在水下建筑内应用效果不佳甚至无法使用，水下建筑内的高精度导航尤其困难，
相关的方案很少。

[0009] 图像声纳（例如，多波束图像声纳、三维声纳、侧扫声纳）可以实时获取水下图像或点云（声纳图像），且具有测距能力，也可以认为是一种视觉手段。当前图像声纳多用于避
障、目标探测、结构测量。也有研究利用SLAM（Simultaneous Localization and Mapping，
同步定位与建图）等方法开展了图像声纳导航的探索。由于声纳图像与光学图像存在较大
差别，信噪比低，无色彩信息，干扰多，基于图像声纳的导航应用并不成熟。

发明内容

[0010] 基于此，本申请提供了一种实现机器人在水下建筑内高精度导航的方案，该方案将第一导航设备（例如，惯性导航设备）、第二导航设备（例如，多普勒测速仪）和图像声纳所
提供的信息融合，形成组合导航，实现机器人在水下建筑内高精度导航。

[0011] 具体来说，通过第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）组合，实现水下大范围、连续的导航定位，基于该定位信息获取在水下建筑结构
内的初步位置；利用图像声纳拍摄机器人周围水下建筑的声纳图像，本方案中要求已知水
下建筑的布局、结构等信息，已知的水下建筑中易识别的特征部位（如比较明显的建筑内设
备、明显的突起、明显的拐角、管道的接缝、铆钉槽、线槽等）的具体位置信息，在声纳图像中
识别这些已知位置的建筑部位，并根据声纳图像得到这些部位相对机器人的方位、距离等
信息，可根据这些信息计算机器人的位置信息，也可将这些信息直接融合到组合导航中，优
化计算水下机器人的位置信息，然后通过融合的方法修正第一导航设备（例如，惯性导航设
备）和第二导航设备（例如，多普勒测速仪）的导航误差，实现水下建筑内高精度导航。

[0012] 根据本发明的第一个方面，提供一种机器人在液体下建筑内高精度导航方法，包括：

[0013] 通过第一导航设备获取所述机器人的第一导航相关信息；

[0014] 通过第二导航设备获取所述机器人的第二导航相关信息；

[0015] 基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息；

[0016] 响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，通过图像声纳获取所述机器人的位置信息；以及

[0017] 融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息。

[0018] 根据本发明的第二个方面，提供一种机器人在液体下建筑内高精度导航装置，包括：

[0019] 第一获取单元，用于通过第一导航设备获取所述机器人的第一导航相关信息；

[0020] 第二获取单元，用于通过第二导航设备获取所述机器人的第二导航相关信息；

[0021] 第三获取单元，基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息；

[0022] 第四获取单元，响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，用于通过图像声纳获取所述机器人的位置信息；以及

[0023] 融合单元，用于融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息。

[0024] 根据本发明的第三个方面，提供一种电子设备，包括：

[0025] 处理器；以及

[0026] 存储器，存储有计算机指令，当所述计算机指令被所述处理器执行时，使得所述处理器执行第一方面所述的方法。

[0027] 根据本发明的第四个方面，提供一种非瞬时性计算机存储介质，存储有计算机程序，当所述计算机程序被多个处理器执行时，使得所述处理器执行第一方面所述的方法。

[0028] 根据本发明提供了机器人在水下建筑内高精度导航的方法、装置、电子识别和非瞬时性计算机存储介质，依靠图像声纳识别水下建筑的特征部位，根据特征部位的先验位
置信息以及声纳图像获取的机器人与特征部位的相对位置信息，可以获知机器人的当前位
置信息，将机器人的当前位置信息与第一导航设备（例如，惯性导航设备）和第二导航设备
（例如，多普勒测速仪）提供的相关信息进行融合计算，可以得到机器人的导航信息，从而实
现机器人在水下建筑内高精度导航。本发明的方案考虑了水下建筑内导航的特点，依靠水
下建筑的特征部位提供的位置信息获取机器人当前精确位置信息，从而实现机器人的高精
度导航。本发明的方案可用于水利水电、水下搜救、海油等所有需进入水下工程、沉船、大型
水下设备等已知构造内部导航的场景。

附图说明

[0029] 为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于
本领域普通技术人员来讲，还可以根据这些附图获取其他的附图，而并不超出本申请要求
保护的范围。

[0030] 图1为根据本申请实施例的机器人在水下建筑内高精度导航的逻辑示意图。

[0031] 图2为根据申请实施例的基于图像声纳获取水下机器人导航信息进行组合导航的流程图。

[0032] 图3为根据本申请实施例的机器人在水下建筑内高精度导航方法的流程图。

[0033] 图4为声纳图像中获取特征部位相对水下机器人的距离和方位信息的示意图。

[0034] 图5为根据本申请实施例的机器人在水下建筑内高精度导航装置的示意图。

[0035] 图6是本发明提供的一种电子设备的结构图。

具体实施方式

[0036] 下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申
请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，
都属于本申请保护的范围。

[0037] 图1为根据本申请实施例的机器人在水下建筑内高精度导航的逻辑示意图，大体上显示了本申请方案的逻辑。如图1所示，惯性导航提供了机器人的第一导航相关信息，该
第一导航相关信息包括机器人的姿态、速度、位置、加速度；多普勒测速仪提供了第二导航
相关信息，该第二导航相关信息包括机器人的速度；图像声纳提供了机器人的位置信息；然
后，根据第一导航相关信息、第二导航相关信息和机器人的位置信息进行融合计算，得到机
器人的导航信息，该导航信息包括机器人的位置、速度和姿态等。

[0038] 图2为根据申请实施例的基于图像声纳获取水下机器人导航信息进行组合导航的流程图。在图2中，第一导航设备和第二导航设备分别为惯性导航系统和多普勒测速仪
（DVL），二者提供连续的导航信息，包括提供机器人的初步位置信息。

[0039] 在水下建筑内，可以存在一个或多个特征部位，且这些特征部位的位置是已知的，称为建筑内特征部位的先验位置信息。根据惯性导航和DVL提供的机器人的初步位置信息，
判断机器人目前是否已经接近某一个特征部位，在接近特征部位时，机器人通过自动的方
式或者在人工操纵下，用图像声纳对周围成像，在图像声纳中识别已知水下建筑内的特征
部位，在识别到特征部位后，声纳图像测量特征部位相对机器人的相对位置信息，包括距离
和方位信息，然后根据声纳图像获得机器人的位置信息，最后，将通过声纳图像获得的机器
人的位置信息参与组合导航，进行数据融合，修正惯性导航和DVL的导航误差。

[0040] 根据本发明的一个方面，提供一种机器人在水下建筑内高精度导航方法。图3为根据本申请实施例的机器人在水下建筑内高精度导航方法的流程图。如图3所示，该方法包括
如下步骤。

[0041] 步骤S301，通过第一导航设备获取所述机器人的第一导航相关信息。

[0042] 在一个具体实施例中，第一导航设备包括惯性导航设备。通过惯性导航结果获取机器人的第一导航相关信息，该第一导航相关信息包括机器人的姿态、速度、位置、加速度
和角速度信息等。

[0043] 步骤S302，通过第二导航设备获取所述机器人的第二导航相关信息。

[0044] 在一个具体实施例中，第二导航设备包括多普勒测速仪。通过多普勒测速仪获取机器人的第二导航相关信息，该第二导航相关信息包括机器人的速度。

[0045] 步骤S303，基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息。

[0046] 通过步骤S301和步骤S302，基于第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）的组合导航结果获取机器人的初步位置信息。

[0047] 在步骤S301和步骤S302中，以惯性导航设备和多普勒测速仪为例描述第一导航设备和第二导航设备，它们的导航结果作为水下大范围、连续的导航定位，本领域技术人员在
上述方案的启发下，能够想到的其他导航设备的组合都属于本申请覆盖的范围。

[0048] 步骤S304，响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，通过图像声纳获取所述机器人的位置信息。

[0049] 在本申请的方案中，要求已知水下建筑的布局、结构等信息，已知的水下建筑中易识别的特征部位（如比较明显的建筑内设备、明显的突起、明显的拐角、管道的接缝、铆钉
槽、线槽等）的具体位置信息。在通过第一导航设备和第二导航设备获得机器人的初步位置
信息后，判断该初步位置与建筑内特征部位的先验位置是否接近，通常可以通过一个预定
阈值来确定，如果建筑内特征部位的先验位置信息与机器人的初步位置信息之差在预定阈
值内，该预定阈值可以根据所在建筑的尺寸、建筑布局和图像声纳性能等具体情形进行设
定，则通过图像声纳获取机器人的位置信息。具体来说，在声纳图像中识别这些已知位置的
水下建筑的特征部位，并根据声纳图像得到这些特征部位相对水下机器人的方位、距离等
信息，可根据这些信息获取机器人的位置信息。从而，步骤S304具体包括：

[0050] 子步骤S1：根据建筑内特征部位的成像特点识别所述特征部位。

[0051] 图像声纳根据先验信息已知水下建筑中一些特征部位的位置（根据导航需要，该位置坐标可以在大地地理坐标系内表示，也可以在其他定义的局部坐标系内表示），并且根
据先验信息已知这些特征部位在图像声纳上的成像特点，以便于进行识别。当水下机器人
的图像声纳对特征部位成像后，可以识别出这些特征部位。在声纳图像中识别已知水下建
筑的特征部位，可以采用计算机视觉中特征提取和匹配的方式，也可以采用深度学习方式
进行训练和识别。

[0052] 子步骤S2：获取所述机器人相对所述特征部位的相对位置信息。

[0053] 当图像声纳识别出特征部位后，就可以根据先验信息获取特征部位的位置信息。图4为声纳图像中获取特征部位相对水下机器人的距离和方位信息的示意图。如图4所示的
多波束声纳图像中，根据声纳图像上识别到的水下建筑的特征部位，得到特征部位在图像
上的坐标，可以得到水下机器人相对特征部位的相对位置信息，该相对位置信息包括机器
人相对特征部位中心点的距离和方位信息。

[0054] 在获取机器人的相对位置信息后，就可以获取机器人的绝对位置信息。步骤S304还包括：

[0055] 子步骤S3：根据所述特征部位的先验位置信息以及所述相对位置信息获取所述机器人的绝对位置信息。

[0056] 在一个具体实施例中，假设根据声纳图像得到的机器人相对特征部位中心点的距离和方位角分别为L和 ，若此时水下机器人声纳的姿态用姿态矩阵 表示，该矩阵可
以是一3*3的矩阵，可以将声纳坐标系（记作s系）表示的矢量转换到导航坐标系（记作n系），
即某矢量在s系的表示 为，则其在n系的表示 为

[0057] （1）

[0058] 姿态矩阵 可以根据水下机器人导航的姿态、声纳在水下机器人上的安装关系、和成像时刻声纳的指向角等信息计算得到。

[0059] 若已知特征部位中心点的位置矢量为Pref，假设水下机器人的位置为P（该位置矢量为与Pref相同的导航坐标系内表示），则相对距离L和相对方位角 满足

[0060]  （2）

[0061] 其中符号| |表示对其中的矢量取大小，[]x和[]y分别表示取其中矢量的x和y分量， 表示姿态矩阵 的转置。

[0062] 若定义声纳坐标系为前‑左‑上坐标系，即声纳成像探测的正前方为x轴，左侧90度为y轴，垂直xy平面向上为z轴，则水下建筑的特征部位和水下机器人位置差在声纳坐标系
（s系）的表示为

[0063] （3）

[0064] 根据上述关系式可以计算得到依据声纳测量的水下机器人位置P：

[0065] （4）

[0066] 由于图像声纳测量的水下机器人位置一般存在误差，为便于理解，在组合导航中，将水下机器人的真实位置用P表示，将图像声纳测量的水下机器人位置用Psonar表示。

[0067] 这样，就获取机器人的绝对位置信息。需要说明的是，上述计算机器人绝对位置信息的方法只是一个具体的方式，本领域技术人员能够获知根据不同的应用场景和设备，还
可以采用其他方式计算机器人的绝对位置信息，这都属于本申请涵盖的范围。

[0068] 步骤S305，融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息。

[0069] 根据本发明的一个方面，在获取所述机器人相对所述特征部位的相对位置信息后，可以将该相对位置信息直接与第一导航相关信息和第二导航相关信息进行融合，得到
机器人的导航信息。这样，步骤S305包括：融合所述第一导航相关信息、所述第二导航相关
信息以及所述相对位置信息获取所述机器人的导航信息。

[0070] 根据本发明的另一个方面，还可以将机器人的绝对位置信息与第一导航相关信息和第二导航相关信息进行融合，得到机器人的导航信息。这样，步骤S305包括：融合所述第
一导航相关信息、所述第二导航相关信息以及所述机器人的绝对位置信息获取所述机器人
的导航信息。

[0071] 在一个具体实施例中，执行步骤S305的方式包括Kalman滤波法、粒子滤波法和最优估计法。

[0072] 下面，以惯性导航、多普勒测速仪和图像声纳的组合按照Kalman滤波法为例，来简单说明本发明的融合过程。

[0073] Kalman滤波需要首先建立状态方程和观测方程：

[0074] 状态方程：                            （5）

[0075] 观测方程：                           （6）

[0076] 其中X表示滤波器状态矢量， 表示状态对时间的导数，f(X)表示状态的函数，W表示状态模型噪声；Z表示观测矢量，h(X)表示表达观测矢量的状态函数，V表示观测噪声。

[0077] 在此基础上就可以利用Kalman滤波的框架进行信息融合，优化估计状态矢量X。Kalman滤波是目前公知的技术，本发明不做详细介绍。

[0078] 本发明的信息融合若采用Kalman滤波，则需首先建立滤波器状态，本发明将水下机器人的姿态、速度和位置误差作为滤波器状态矢量（也可包含惯性器件误差、DVL器件误
差等）：

[0079]                     （7）

[0080] 其中，为水下机器人姿态误差角， 为水下机器人速度误差， 为水下机器人位置误差。若水下机器人的姿态用姿态矩阵 表示，与上文介绍的 类似， 为水下机
器人本体系(记为b系)到导航系(n系)的转换矩阵，该矩阵为3*3矩阵，可将b系表示的矢量
转换到n系表示，则导航系统计算的姿态矩阵结果 （有误差）与真值 （无误差）、姿态误
差角 间的关系满足 ，其中I3表示3*3单位阵， 表示姿态误差角矢量的叉
乘矩阵；记水下机器人速度为 ，则导航系统速度计算结果 （有误差）与真值 （无误差）
间满足 ；记水下机器人的位置为P，则导航系统位置计算结果含误差的位置导航
结果 （有误差）与真值P（无误差）间满足 ；上标T表示矢量的转置。

[0081] Kalman滤波的状态方程可根据惯性导航系统误差传播规律建立。

[0082] 根据惯性导航理论，惯性导航中姿态误差角、速度误差和位置误差各状态矢量满足：

[0083]       （8）

[0084] 其中 表示导航系(n系)相对惯性系(记为i系)的旋转角速度在导航系的表示，b
表示 的计算误差， 为陀螺测量的角速度误差，f 是加速度计测量的比力， 表示
加速度计测量的比力误差， 表示地球自转角速度， 表示水下机器人运动导致的系相对
地球系(记为e系)的旋转角速度， 、 表示 和 的计算误差， 表示重力加速度计
算误差。

[0085] 依据公式（8）可以建立Kalman滤波状态方程，建立方法可参考公开文献，本发明不做详细推导。

[0086] Kalman滤波还需要建立观测方程，本发明中的观测包含DVL的速度测量和图像声纳的位置测量。

[0087] DVL的速度观测vDVL是在水下机器人本体系(b系)的表示，需要借助导航的姿态矩阵结果 转换到导航坐标系：

[0088] （9）

[0089] 图像声纳的位置观测前文的公式（2）（4）已做说明。~

[0090] 由于状态矢量为误差形式，Kalman滤波的观测矢量也写成误差形式。当声纳图像中无特征部位时，仅采用惯性导航和DVL组合，观测矢量为：   （10）

[0091] 当图像声纳识别到特征部位时，依据特征部位信息计算得到的水下机器人位置参与信息融合，观测矢量为：

[0092]                  （11）

[0093] 依据公式（10）、（11）可以建立Kalman滤波器观测方程，具体建立方法可参考公开文献，本发明不做详细推导。

[0094] 利用Kalman滤波数据融合优化估计得到状态矢量 、 和 后，进而可反馈修正导航结果，得到滤波后的导航姿态、速度和位置：

[0095] （12）

[0096] 其中矢量上的“”符号表示该矢量有误差，“^”符号表示矢量的优化估计值。~

[0097] 需要说明的是，在上述第一导航设备、第二导航设备和图像声纳的导航组合方案中，第一导航设备和第二导航设备实现水下大范围、连续的导航定位，提供初步的导航信
息，而图像声纳是间歇性参与导航组合方案，在识别到建筑内特征部位时，才参与到第一导
航设备和第二导航设备中，对第一导航设备和第二导航设备的导航误差进行修正。

[0098] 根据本发明提供了机器人在水下建筑内高精度导航的方法，依靠图像声纳识别水下建筑的特征部位，根据特征部位的先验位置信息以及声纳图像获取的机器人与特征部位
的相对位置信息，可以获知机器人的当前位置信息，将机器人的当前位置信息与第一导航
设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）提供的相关信息进行融
合计算，可以得到机器人的导航信息，从而实现机器人在水下建筑内高精度导航。本发明的
方案考虑了水下建筑内导航的特点，依靠水下建筑的特征部位提供的位置信息获取机器人
当前精确位置信息，从而实现机器人的高精度导航。

[0099] 根据本发明的另一个方面，提供一种机器人在水下建筑内高精度导航装置。图5为根据本申请实施例的机器人在水下建筑内高精度导航装置的示意图。如图5所示，该装置包
括如下单元。

[0100] 第一获取单元501，用于通过第一导航设备获取所述机器人的第一导航相关信息。

[0101] 在一个具体实施例中，第一导航设备包括惯性导航设备。通过惯性导航结果获取机器人的第一导航相关信息，该第一导航相关信息包括机器人的姿态、速度、位置、加速度
和角速度信息等。

[0102] 第二获取单元502，用于通过第二导航设备获取所述机器人的第二导航相关信息。

[0103] 在一个具体实施例中，第二导航设备包括多普勒测速仪。通过多普勒测速仪获取机器人的第二导航相关信息，该第二导航相关信息包括机器人的速度。

[0104] 第三获取单元503，用于基于所述第一导航相关信息和所述第二导航相关信息获取所述机器人的初步位置信息。

[0105] 通过第一获取单元501和第二获取单元502，基于第一导航设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）的组合导航结果获取机器人的初步位置信息。

[0106] 在第一获取单元501和第二获取单元502中，以惯性导航设备和多普勒测速仪为例描述第一导航设备和第二导航设备，它们的导航结果作为水下大范围、连续的导航定位，本
领域技术人员在上述方案的启发下，能够想到的其他导航设备的组合都属于本申请覆盖的
范围。

[0107] 第四获取单元504，用于响应于建筑内特征部位的先验位置信息与所述机器人的初步位置信息之差在预定阈值内，通过图像声纳获取所述机器人的位置信息。

[0108] 在本申请的方案中，要求已知水下建筑的布局、结构等信息，已知的水下建筑中易识别的特征部位（如比较明显的建筑内设备、明显的突起、明显的拐角、管道的接缝、铆钉
槽、线槽等）的具体位置信息。在通过第一导航设备和第二导航设备获得机器人的初步位置
信息后，判断该初步位置与建筑内特征部位的先验位置是否接近，通常可以通过一个预定
阈值来确定，如果建筑内特征部位的先验位置信息与机器人的初步位置信息之差在预定阈
值内，该预定阈值可以根据所在建筑的尺寸、建筑布局和图像声纳性能等具体情形进行设
定，则通过图像声纳获取机器人的位置信息。具体来说，在声纳图像中识别这些已知位置的
水下建筑的特征部位，并根据声纳图像得到这些特征部位相对水下机器人的方位、距离等
信息，可根据这些信息获取机器人的位置信息。从而，第四获取单元504具体包括：

[0109] 识别子单元，用于根据建筑内特征部位的成像特点识别所述特征部位。

[0110] 图像声纳根据先验信息已知水下建筑中一些特征部位的位置（根据导航需要，该位置坐标可以在大地地理坐标系内表示，也可以在其他定义的局部坐标系内表示），并且根
据先验信息已知这些特征部位在图像声纳上的成像特点，以便于进行识别。当水下机器人
的图像声纳对特征部位成像后，可以识别出这些特征部位。在声纳图像中识别已知水下建
筑的特征部位，可以采用计算机视觉中特征提取和匹配的方式，也可以采用深度学习方式
进行训练和识别。

[0111] 第一获取子单元，用于获取所述机器人相对所述特征部位的相对位置信息。

[0112] 当图像声纳识别出特征部位后，就可以根据先验信息获取特征部位的位置信息。图5为声纳图像中获取特征部位相对水下机器人的距离和方位信息的示意图。如图5所示的
多波束声纳图像中，根据声纳图像上识别到的水下建筑的特征部位，得到特征部位在图像
上的坐标，可以得到水下机器人相对特征部位的相对位置信息，该相对位置信息包括机器
人相对特征部位中心点的距离和方位信息。

[0113] 在获取机器人的相对位置信息后，就可以获取机器人的绝对位置信息。第四获取单元504还包括：

[0114] 第二获取子单元，用于根据所述特征部位的先验位置信息以及所述相对位置信息获取所述机器人的绝对位置信息。

[0115] 融合单元505，用于融合所述第一导航相关信息、所述第二导航相关信息以及所述机器人的位置信息获取所述机器人的导航信息。

[0116] 根据本发明的一个方面，在获取所述机器人相对所述特征部位的相对位置信息后，可以将该相对位置信息直接与第一导航相关信息和第二导航相关信息进行融合，得到
机器人的导航信息。这样， 融合单元505用于：融合所述第一导航相关信息、所述第二导航
相关信息以及所述相对位置信息获取所述机器人的导航信息。

[0117] 根据本发明的另一个方面，还可以将机器人的绝对位置信息与第一导航相关信息和第二导航相关信息进行融合，得到机器人的导航信息。这样，融合单元505用于：融合所述
第一导航相关信息、所述第二导航相关信息以及所述机器人的绝对位置信息获取所述机器
人的导航信息。

[0118] 在一个具体实施例中，融合单元505的实施方式包括Kalman滤波法、粒子滤波法和最优估计法。

[0119] 需要说明的是，在上述第一导航设备、第二导航设备和图像声纳的导航组合方案中，第一导航设备和第二导航设备实现水下大范围、连续的导航定位，提供初步的导航信
息，而图像声纳是间歇性参与导航组合方案，在识别到建筑内特征部位时，才参与到第一导
航设备和第二导航设备中，对第一导航设备和第二导航设备的导航误差进行修正。

[0120] 根据本发明提供了机器人在水下建筑内高精度导航的装置，依靠图像声纳识别水下建筑的特征部位，根据特征部位的先验位置信息以及声纳图像获取的机器人与特征部位
的相对位置信息，可以获知机器人的当前位置信息，将机器人的当前位置信息与第一导航
设备（例如，惯性导航设备）和第二导航设备（例如，多普勒测速仪）提供的相关信息进行融
合计算，可以得到机器人的导航信息，从而实现机器人在水下建筑内高精度导航。本发明的
方案考虑了水下建筑内导航的特点，依靠水下建筑的特征部位提供的位置信息获取机器人
当前精确位置信息，从而实现机器人的高精度导航。

[0121] 参阅图6，图6提供一种电子设备，包括处理器；以及存储器，所述存储器存储有计算机指令，当所述计算机指令被所述处理器执行时，使得所述处理器执行所述计算机指令
时实现如图3所示的方法以及细化方案。

[0122] 应该理解，上述的装置实施例仅是示意性的，本发明披露的装置还可通过其它的方式实现。例如，上述实施例中所述单元/模块的划分，仅仅为一种逻辑功能划分，实际实现
时可以有另外的划分方式。例如，多个单元、模块或组件可以结合，或者可以集成到另一个
系统，或一些特征可以忽略或不执行。

[0123] 另外，若无特别说明，在本发明各个实施例中的各功能单元/模块可以集成在一个单元/模块中，也可以是各个单元/模块单独物理存在，也可以两个以上单元/模块集成在一
起。上述集成的单元/模块既可以采用硬件的形式实现，也可以采用软件程序模块的形式实
现。

[0124] 所述集成的单元/模块如果以硬件的形式实现时，该硬件可以是数字电路，模拟电路等等。硬件结构的物理实现包括但不局限于晶体管，忆阻器等等。若无特别说明，所述处
理器或芯片可以是任何适当的硬件处理器，比如CPU、GPU、FPGA、DSP和ASIC等等。若无特别
说明，所述片上缓存、片外内存、存储器可以是任何适当的磁存储介质或者磁光存储介质，
比如，阻变式存储器RRAM（Resistive Random Access Memory）、动态随机存取存储器DRAM
（Dynamic Random Access Memory）、静态随机存取存储器SRAM（Static Random‑Access 
Memory）、增强动态随机存取存储器EDRAM（Enhanced Dynamic Random Access Memory）、 
高带宽内存HBM（High‑Bandwidth Memory）、混合存储立方 HMC（Hybrid Memory Cube）等
等。

[0125] 所述集成的单元/模块如果以软件程序模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储器中。基于这样的理解，本发明的技术方案本质
上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形
式体现出来，该计算机软件产品存储在一个存储器中，包括若干指令用以使得一台计算机
设备（可为个人计算机、服务器或者网络设备等）执行本披露各个实施例所述方法的全部或
部分步骤。而前述的存储器包括：U盘、只读存储器（ROM，Read‑Only Memory）、随机存取存储
器（RAM，Random Access Memory）、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介
质。

[0126] 本申请实施例还提供一种非瞬时性计算机存储介质，存储有计算机程序，当所述计算机程序被多个处理器执行时，使得所述处理器执行如图3所示的方法以及细化方案。

[0127] 以上对本申请实施例进行了详细介绍，本文中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明仅用于帮助理解本申请的方法及其核心思想。同
时，本领域技术人员依据本申请的思想，基于本申请的具体实施方式及应用范围上做出的
改变或变形之处，都属于本申请保护的范围。综上所述，本说明书内容不应理解为对本申请
的限制。