CN102042835B

CN102042835B - 自主式水下机器人组合导航***

Info

Publication number: CN102042835B
Application number: CN201010534188A
Authority: CN
Inventors: 魏志强; 殷波; 丛艳平; 贾东宁; 刘兵; 潘学松
Original assignee: Ocean University of China
Current assignee: Ocean University of China
Priority date: 2010-11-05
Filing date: 2010-11-05
Publication date: 2012-10-24
Anticipated expiration: 2030-11-05
Also published as: CN102042835A

Abstract

本发明涉及水下机器人导航***，尤其是一种自主式水下机器人组合导航***。该***包括惯性基础导航装置和外部传感器导航装置，其中惯性基础导航装置包括多普勒测速仪、光纤陀螺、压力传感器、电子罗盘和深度计，外部传感器导航装置包括声纳，其中，该组合导航***还包括水下结构光传感器和水下双目视觉平台，其中水下结构光传感器包括位于自主式水下机器人外框架前部的前视结构光传感器和位于外框架底部的下视结构光传感器，水下双目视觉平台包括位于外框架前部的前视双目视觉平台和位于外框架底部的下视双目视觉平台，前视结构光传感器和前视双目视觉平台组成了位于外框架前部的前视结构光与视觉***模块，下视结构光传感器和下视双目视觉平台组成了位于外框架底部的下视结构光与视觉***模块。

Description

自主式水下机器人组合导航***

技术领域

本发明涉及水下机器人导航***，尤其是一种自主式水下机器人组合导航***。

背景技术

惯性导航是自主式水下机器人(Autonomous Underwater Vehicles，简称AUV)在水下自主航行时采用的主要定位手段，但是其主要问题在于精度能满足要求时，往往价格过于昂贵。由于声纳技术的进步，目前较先进的AUV基本不采用加速度计而是采用多普勒测速仪，可以直接测出AUV相对于海底的3D速度而无需积分。采用多普勒测速仪与光纤陀螺相结合的导航方法，仍然不可避免的产生累积误差，其导航精度为航程的0.2％。为消除这种偏差，目前主要采用的方法有：(1)利用GPS修正，即让AUV在其路途中的一系列位置上浮，接受GPS信号以修正当前位置；(2)声学辅助导航，在作业区域布置如长基线、短基线等；(3)地形辅助导航。但是目前上述主要方法均以不同形式对AUV工作范围加以限制，其中GPS多次上浮修正偏差，能源消耗很大，限制了工作时间和工作范围；而声学基线方法限制AUV只能在短距离内有效；地形辅助导航类似于巡航导弹所采用的方法，利用地形信息来定位和导航，对AUV而言，这要求事先必须有精确的海底地形地图。由于海底地形和陆地不同，因此获得精确的海底地图仍然是各国正在探索的问题，目前而言，只有在近海的部分区域才具有比较精确的海底地形地貌数据，因此基于海图的地形辅助导航的应用也有很大局限。

随着AUV技术的不断发展，其作为深海水下运载技术的重要执行载体，将向更长程、更广阔工作海域发展。如果要求AUV在更广阔的区域以更低廉的运行费用工作，一个可行性思路是尽量减少对母船和基阵等外部导航框架的依赖，使AUV实现自主导航定位。“同时定位与地图构建方法”(Simultaneous Localization and mapping，简称SLAM方法)被认为是机器人在未知环境中真正实现自主导航的关键。SLAM方法可描述为：在未知环境下，移动机器人从未知的起点开始，经过一系列位置，利用机载传感器信息构建增量式环境地图同时对自身位置定位。该方法自1990年提出以来得到很快的发展。同时定位与地图创建方法与地图表达形式密切相关，如栅格地图、特征地图、拓扑地图等，基于环境特征的SLAM方法应用最为广泛，特别是对于户外环境，障碍物没有规则几何形状。它将机器人的位姿和环境特征坐标表达在一个状态向量之中，在机器人的运动过程中通过对环境特征的观测作最优准则的估计。分为三个步骤：(1)基于外部感知的环境特征提取；(2)递推形式的预测和更新算法；(3)相应的数据关联技术。

为此，声纳技术应用到AUV中，扫描声纳具有较好的成像能力，扫描声纳配合使用中等精度的惯性导航传感器即可满足自主导航定位，并且无需采用昂贵的高精度惯性***，大大降低了AUV的制造成本，针对大范围的水下环境，扫描成像声纳和内部传感器可以实现大尺度全局自主导航，但是其不适用于在局部海域内精确自主导航，尤其是当局部海域的地形地貌比较复杂时，扫描成像声纳可能无法实现自主导航的功能。

发明内容

本发明的目的在于提出了一种自主式水下机器人组合导航***，其同时实现了在大尺度、中尺度和小尺度精确自主导航。

本发明是采用以下的技术方案实现的：一种自主式水下机器人组合导航***，包括惯性基础导航装置和外部传感器导航装置，其中惯性基础导航装置包括多普勒测速仪、光纤陀螺、压力传感器、电子罗盘和深度计，外部传感器导航装置包括声纳，其中，该组合导航***还包括水下结构光传感器和水下双目视觉平台，其中水下结构光传感器包括位于自主式水下机器人外框架前部的前视结构光传感器和位于外框架底部的下视结构光传感器，水下双目视觉平台包括位于外框架前部的前视双目视觉平台和位于外框架底部的下视双目视觉平台，前视结构光传感器和前视双目视觉平台组成了位于外框架前部的前视结构光与视觉***模块，下视结构光传感器和下视双目视觉平台组成了位于外框架底部的下视结构光与视觉***模块。

本发明中，所述的声纳包括设置在外框架前部上方的窄波束扫描成像声纳和设置在外框架前部下方的具有较宽波束的普通避碰声纳。所述的水下结构光传感器包括摄像机、激光投射器、振镜以及振镜控制板。所述的水下双目视觉平台包括两台水下高清摄像机和对应的图像采集卡。

本发明还涉及一种自主式水下机器人组合导航***的声纳信号斑特征提取方法，其中该方法包括以下步骤：首先，利用融合脉冲耦合神经网络、中值滤波和维纳滤波优势的混合滤波进行图像噪声滤除；然后，利用广义形态学复合运算进行边缘检测和特征提取，获取障碍物信息，并提取障碍物的质心应用于AUV导航。

本发明还涉及一种自主式水下机器人组合导航***中水下结构光传感器的三维测量方法，其中该方法包括以下步骤：首先，对水下摄像机进行标定：标定时可以利用实验室的水槽实现摄像机标定，利用平面网格靶标作为标定用靶标，标定过程在水中进行，由靶标上的网格点和对应的像面坐标建立若干共轭对，利用这些共轭对采用改进的径向约束排列方法来求解摄像机模型；第二步，结构光光平面标定以及测量点坐标的确定：标定时先在水上根据振镜不同转角下在平面网格靶标上的交线确定振镜的初始角，以及振镜在不同角度下对应的光平面在世界坐标系下的方程，从而求出光平面上任一点即水中物体的三维坐标。

本发明还涉及一种自主式水下机器人组合导航***中水下双目视觉平台的三维测量方法，该方法包括以下步骤：

第一步，特征提取：采用基于Harris角点的特征提取方法，实现水下双目视觉图像的特征提取，同时对特征点周围的灰度分布进行了二次曲面拟合，在实际图像中提取角点时，首先确定要提取的特征点数目N，然后找出角点函数R(x，y)值最大的N个角点，寻找局部极值时，先将整个图像平面分为几块小区域，然后在每个小区域内各提取一定数目的特征点以用于匹配；

第二步，立体匹配：采用基于Harris角点匹配和基于准稠密的方法实现特征的立体匹配；

第三步，视差图的计算：通过对同一被测物体的不同图像进行Harris角点检测和零均值归一化互相关最优策略匹配扩散得到更多特征对应点，对各特征点计算视差后得到视差图；

第四步，三维重建：采用分格显示地貌的方法实现由视差图重建三维场景。

其中分格显示地貌的方法主要包括以下步骤：(1)计算视差图里各点在摄像机坐标系下的三维坐标；(2)建立摄像机坐标系与车体坐标系的转换关系，将摄像机坐标系下的三维坐标转换为车体坐标系下各点的三维坐标；(3)先将视差图分成4×4块，共十六个方格，因为当摄像头被固定在机器人上后有一个俯仰角，所以经过第二步坐标变换后，每一个网格对应着一个实际的区域，这块区域与机器人之前有固定的距离和角度，然后遍历整个视差图，选取适当的障碍物高度阈值T，高于T的物体认为是障碍物，机器人不可通行，对含有障碍物的方格区域，选取障碍物的最高点作为该区域的高度；对于不含障碍物的方格区域，取所有落入点高度的平均值作为该区域的高度，至此上述十六个分格均为带有高度信息的方格，而且可以知道它们是否为障碍物，及其位置和距离。

本发明的有益效果是：本发明中的组合导航***包括惯性基础导航装置和外部传感器装置，其中外部传感器装置中的扫描成像声纳作为该机器人的大尺度探测传感器，与惯性基础导航装置实现大尺度全局自主导航；外部传感器装置中的水下结构光传感器可以实现三维测量，主要针对地形地貌变化相对平缓的局部复杂海域，其与扫描成像声纳结合可以实现中尺度的精确自主导航；水下双目视觉平台采用双摄像机从不同角度获取被测物体的两幅数字图像，并基于视差原理恢复出物体的三维几何信息，重建物体的三维轮廓和位置，为水下环境的三维重建和AUV自主导航提供环境信息，针对地形地貌变化剧烈和海况复杂的局部海域，水下双目视觉平台结合扫描成像声纳可以实现小尺度的精确自主导航。因此该组合导航***可以实现自主式水下机器人的精确导航。

附图说明

图1为本发明的结构示意图；

图2为惯性基础导航装置的软件模块；

图3为分布式联邦扩展卡尔曼滤波方法示意图；

图4为基于分布式联邦扩展卡尔曼滤波SLAM方法示意图；

图5为本发明中自主式水下机器人的硬件***结构示意图；

图6为本发明中自主式水下机器人的软件***结构示意图。

具体实施方式

下面结合附图对本发明做进一步说明。

如图1所示为本发明所述的自主式水下机器人。该机器人采用开架式机械结构，外框架1上固定上下双层压力舱2，并设置五个水下推进器，包括三个垂直方向推进器和两个水平方向推进器8，其中在外框架1前部的左右对称两侧分别设有垂直方向推进器6，外框架1中部的左右对称两侧分别固定水平方向推进器8；外框架1的后部上方设有GPS天线，后部的中间位置设有一个垂直方向推进器9，该推进器的上方安装有压力传感器。

该自主式水下机器人的导航***包括惯性基础导航装置和外部传感器导航装置，其中惯性基础导航装置包括多普勒测速仪、光纤陀螺、压力传感器、电子罗盘、深度计等。外部传感器导航装置包括声纳、水下结构光传感器和水下双目视觉平台。声纳包括设置在外框架1前部上方的窄波束扫描成像声纳3和设置在外框架1前部下方的具有较宽波束的普通避碰声纳5，其中扫描成像声纳作为该机器人的大尺度探测传感器，与惯性基础导航装置实现大尺度全局自主导航。水下结构光传感器包括位于外框架1前部的前视结构光传感器和位于外框架1底部的下视结构光传感器，水下双目视觉平台包括位于外框架1前部的前视双目视觉平台和位于外框架1底部的下视双目视觉平台。其中，前视结构光传感器和前视双目视觉平台组成了位于外框架1前部的前视结构光与视觉***模块4，下视结构光传感器和下视双目视觉平台组成了位于外框架1底部的下视结构光与视觉***模块7。

惯性基础导航装置中，多普勒测速仪用于测量AUV的行进速度；光纤陀螺配合电子罗盘对当前时刻载体的姿态进行航行推算，其中光纤陀螺用于获得AUV的偏航角速率，电子罗盘用于输出AUV的三轴姿态角；压力传感器用于测量AUV所处位置与海平面之间的距离。上述传感器均设置在AUV内部，所以又称为内部传感器。惯性基础导航装置内的各传感器通过初始对准、捷连解算和组合导航等步骤对AUV的速度、位置和姿态角等状态信息进行计算。惯性基础导航装置的软件模块如图2所示，结合附图对利用惯性基础导航装置获得AUV状态信息的方法具体介绍：

第一步，初始对准：由于惯导***的初值误差在以后的导航解算中有积累作用，因此必须控制初始对准误差在一定范围内，特别是初始姿态误差。采用卡尔曼滤波算法实现惯导的精对准，首先建立速度误差方程和姿态误差方程，然后采用离散卡尔曼滤波算法对误差作最优估计。

第二步，惯导数据捷联解算：初始对准以后，首先利用惯导***输出的角速率和上一时刻四元数，根据四阶龙格-库塔法更新四元数，进而更新捷联矩阵，再根据捷联矩阵与姿态角的关系更新姿态角；然后，利用惯导***输出的比力加速度和上一时刻的速度，根据四阶龙格-库塔法更新速度；最后，利用上一时刻的位置，根据龙格-库塔法更新位置。

第三步，组合导航：首先建立捷联惯导***的误差方程，包括速度误差方程、位置误差方程和姿态误差方程，并用其作为***状态方程；然后分别将电子罗盘、深度计和多普勒测速仪的观测作为观测方程；最后，利用联邦扩展卡尔曼滤波算法估计速度、位置和姿态角等状态信息。

自主式水下机器人对其周围大尺度环境的感知主要通过扫描成像声纳3。由于机械扫描式声纳的扫描速度很慢，使得AUV在运动中得到的完整声纳扫描图像会出现失真，从这种声纳图像中提取环境特征就会产生错误的结果。因此，在利用声纳束捕捉图像的同时，对航行器的运动轨迹进行预测，利用轨迹预测对运动产生的图像失真进行补偿，降低失真的影响。经过预处理后即可从声纳数据中提取特征，特征提取方法主要有点特征提取方法和斑状特征提取方法。

从声纳数据中提取点特征分为如下三个步骤：(a)对声纳所有的单个Ping找出和主返回之间的距离，这个距离实际上代表了和目标之间的距离；(b)把这些主返回分成若干簇，其中那些规模小的、明显的簇可以认为是点特征，进而做距离/方位的估计处理；(c)将距离/方位信息与地图中已有的特征进行匹配。

在水下环境中，礁石的突出部分和海底凸起的岩礁在声纳数据中都可以归类为点特征，大面积岩礁的返回相对平缓得多，不视为点特征，只作为障碍。从单个Ping找到那些主返回以后，再找出当前扫描数据中与主返回等深的区块，就可以作为点特征。处理过程中，检查扫描数据的连续Pings看是否存在连续的主返回，如果多个主返回形成的簇比较小，也可以视为一个点特征。进一步，再计算点特征的方位角，方法是找到点特征的主返回簇的分布中心，以此点来计算；计算距离时，取簇中的主返回距离的均值。

从声纳数据中提取斑状特征包括以下步骤：首先，利用融合脉冲耦合神经网络、中值滤波和维纳滤波优势的混合滤波进行图像噪声滤除；然后，利用广义形态学复合运算进行边缘检测和特征提取，获取障碍物信息，并提取障碍物的质心应用于AUV导航。

水下结构光传感器包括摄像机、激光投射器、振镜以及振镜控制板，该传感器可以实现三维测量，主要针对地形地貌变化相对平缓的局部复杂海域，其与扫描成像声纳结合，可以实现中尺度的精确自主导航。由于被测目标在水中，摄像机设置在具有玻璃窗口的密封的壳体中，从被测目标所在的世界坐标系到像面坐标系要由水到玻璃，再由玻璃到空气，发生两次折射，常规的摄像机模型不再适用，可以通过以下步骤实现水下结构光传感器的三维测量：

首先，对水下摄像机进行标定：标定时可以利用实验室的水槽实现摄像机标定，利用平面网格靶标作为标定用靶标，标定过程在水中进行，由靶标上的网格点和对应的像面坐标建立若干共轭对，利用这些共轭对采用改进的径向约束排列方法来求解摄像机模型，该方法可直接解出摄像机模型中的未知参数。为了实现利用结构光来对水下大尺度空间进行测量，采用将水上标定结果的参数转换为水下测量结果来实现摄像机的标定，在水上可以利用平面网格靶标进行较大尺度的标定，在实验室内可利用3*3米靶标进行水上标定，由于***的测量景深较大，该标定结果可用于整个测量范围，将水上标定结果代入参数转换模型可用于水下大尺度的测量；

第二步，结构光光平面标定以及测量点坐标的确定：结构光标定是确定结构光光平面和摄像机像面的映射关系。在本***中，激光器投射出的激光平面经振镜反射后穿过空气和玻璃进入水中，发生了两次折射。该部分的标定方法同上一部分的标定过程，先在水上根据振镜不同转角下在平面网格靶标上的交线确定振镜的初始角，以及振镜在不同角度下对应的光平面在世界坐标系下的方程，从而求出光平面上任一点即水下物体点的三维坐标。

本发明利用水下结构光传感器作为一个相对独立的模块，分别用于水下地形地貌的测量和AUV的前视导航，结构光的标定可独立进行，即在安装到AUV上之前进行标定，将结构光的测量过程做成单独的动态库供AUV主机调用，该库中主要包含三个函数。AUV主机通过调用其中的测量函数可返回被测目标的三维点云数据。根据这些数据可绘制海底地形地貌，也可以获得前方障碍物的距离形状、体积等信息，为实现准确导航提供丰富的信息。

水下双目视觉平台包括两台水下高清摄像机和图像采集卡。该平台采用双摄像机从不同角度获取被测物体的两幅数字图像，并基于视差原理恢复出物体的三维几何信息，重建物体的三维轮廓和位置，为水下环境的三维重建和AUV自主导航提供环境信息。针对地形地貌变化剧烈和海况复杂的局部海域，水下双目视觉平台结合扫描成像声纳可以实现小尺度的精确自主导航。水下双目视觉平台采用基于Harris角点的图像特征提取、特征匹配和视差图计算等步骤，并在此基础上利用分格显示地貌实现三维重建。下面对利用水下双目视觉平台实现三维重建的方法详细的说明：

第一步，特征提取：由于水下场景的随意性，同时提取的特征点应满足一致性，即两幅图像中的特征点应该尽量一一对应，并且它的周围点应该包含足够的信息以便于匹配，因此采用基于Harris角点的特征提取方法，实现水下双目视觉图像的特征提取。同时，对特征点周围的灰度分布进行了二次曲面拟合，通过求解该二次曲面的极值可以将精度提高到亚像素级而Harris角点提取方法只能精确到像素级，因此进一步提高了精度；其次，在实际图像中提取角点时，首先确定要提取的特征点数目N，然后找出角点函数R(x，y)值最大的N个角点，克服了阈值选择较为困难的问题；另外，由于角点可能集中在图像的个别区域，而另外的区域可能没有足够的角点用于匹配和重建。因此在寻找局部极值时，先将整个图像平面分为几块小区域，然后在每个小区域内各提取一定数目的特征点以用于匹配。

第二步，立体匹配：主要采用基于Harris角点匹配和基于准稠密的方法实现特征的立体匹配。

第三步，视差图的计算：通过对左右两幅图像进行Harris角点检测和零均值归一化互相关(Zero-mean normalized cross-correlation，简称ZNCC)最优策略匹配扩散得到更多特征对应点，对各特征点计算视差后得到视差图。

第四步，三维重建：根据立体视觉在导航应用中的特点，采用一种分格显示地貌的方法实现由视差图重建三维场景：1)计算视差图里各点在摄像机坐标系下的三维坐标；2)建立摄像机坐标系与车体坐标系的转换关系，将摄像机坐标系下的三维坐标转换为车体坐标系下各点的三维坐标；3)先将视差图分成4×4块，共十六个方格，因为当摄像头被固定在机器人上后有一个俯仰角，所以经过第二步坐标变换后，每一个网格对应着一个实际的区域，这块区域与机器人之前有固定的距离和角度，然后遍历整个视差图，选取适当的障碍物高度阈值T，高于T的物体认为是障碍物，机器人不可通行，对含有障碍物的方格区域，选取障碍物的最高点作为该区域的高度；对于不含障碍物的方格区域，取所有落入点高度的平均值作为该区域的高度，这样就得到了十六个有高度信息的方格，而且可以知道它们是否为障碍物，以及它们的位置和距离等信息。通过分格显示地貌的算法，可以减少数据量。实验结果表明，该算法计算量小、鲁棒性强，完全满足视觉导航中的实时性、准确性要求。

由于本发明中的水下机器人采用包括惯性基础导航装置和外部传感器导航装置的组合导航***，为了有效地融合声纳、水下结构光传感器、水下双目立体视觉平台以及惯性基础导航装置的传感器信息，因此如图3所示，本发明利用分布式联邦扩展卡尔曼滤波方法实现了惯性基础导航装置、扫描成像声纳、水下双目视觉***和结构光传感器的数据融合，从而实现了以扫描成像声纳作为大尺度探测传感器、以结构光为中尺度探测传感器，以双目视觉***为小尺度探测传感器的多传感器AUV精确组合导航装置。如图4所示，SLAM子***的执行过程大体可以分为三个阶段：预测，观测和更新。预测阶段，惯性基础导航装置将检测到的姿态、速度和初始位置等数据输入SLAM子***的AUV运动方程中，通过AUV运动方程计算出的位置预测和误差协方差预测输入观测方程中得到预测的观测值；观测阶段，外部传感器装置对周围环境中的特征再次观测，将实际观测的距离和方位角等数据输入SLAM子***中；更新过程中，通过数据关联过程确定预测的观测值与实际观测值的一一对应关系，并计算残差V_r、S_r、W_r；更新阶段，将两者的残差最为观测值，通过扩展示卡尔曼滤波处理后，对载体和特征的状态估计进行改善和更新。

在深海复杂环境下应用时，传统的扩展示卡尔曼滤波算法时间复杂度较高，随着特征点的个数增加，运算时间急剧增长，最消耗时间的部分是环境特征协方差矩阵的求逆运算，由于本***环境特征协方差矩阵具有正定性和对称性，对该矩阵进行LDL分解，这样只需对其中一部分进行求逆就能得到原来矩阵的逆矩阵，提高了矩阵求逆计算的效率，时间大约为原来的三分之一；另外在实际应用中，AUV自身的更新和环境特征的更新一般是不同步的，因此采取更新AUV位置多次，更新一次环境特征的方法，使AUV的运动轨迹更加精确。在数据关联方面，采用基于最近邻数据关联算法的数据关联算法，并辅以缓冲技术即开辟一个临时特征数组，将新的特征先放入临时数组中，只有当满足一定条件时再加入到实际的地图中去，有效的避免了杂波以及多重匹配的错误，提高了***的稳定性。通过在算法复杂度和数据关联两方面进行改进，使得在环境特征点较少的情况下能够达到很好的精度和实时性。

如图5所示，本发明所述的自主式水下机器人主要包括信息采集***、嵌入式核心控制***、应急状态检测***、运动驱动***和水面计算机工作站，***各硬件之间主要采用PCI总线、RS-232串行通信端口和USB接口进行通信。其中嵌入式核心控制***采用的是研华UNO-3074嵌入式工控主机，集成有Pentium M1.8GHZ处理器，1GB DDR SDRRAM，160G硬盘，通信端口包括四个RS-232接口，四个USB接口，四个PCI接口和八个DIO数据输入输出接口，大大提高了嵌入式***的数据处理和存储能力以及***稳定性。信息采集***中包含组合导航***，其中水下结构光传感器通过PCI总线与嵌入式核心控制***连接，实现了测量数据的采集；水下双目视觉平台通过USB接口与嵌入式核心控制***连接，实现了图像信息的采集；声纳、电子罗盘和多普勒测速仪等惯性基础导航传感器通过RS-232串行通信端口与嵌入式核心控制***连接，实现数据采集。电源管理模块可以提供5V，12V和24V多种电源接口。应急状态检测***中，通过A/D数据采集卡获取AUV深度信息、电池电压和漏水等模拟信息，A/D数据采集卡通过PCI总线与嵌入式工控机连接。运动驱动***中，通过D/A数据采集卡向AUV的无刷电机推进器发送模拟电压信号，使其发出AUV运动所需的推力，从而实现位置或速度跟踪控制，D/A数据采集卡通过PCI总线与嵌入式工控机连接。同时，嵌入式工控机采用TCP协议与水面计算机工作站通信以获取任务和目标指令。

如图6所示，AUV的控制***软件需要有较好的实时性和可靠性，本发明的AUV控制***采用采用基于优先级的抢占式多任务调度的嵌入式实时操作***vxWorks，如图11所示，***任务按功能主要分为四类：

(1)数据采集子***：该部分根据核心控制***的目标指令，获取导航***的导航信息以及获取基础运动控制***底层传感器数据，包括结构光传感器、双目视觉、声纳、惯导等提供的导航信息以及深度、漏水和电池电压等信息，其数据处理主要是对获取的数据进行预处理和数据融合，根据感知结果生成下一时刻的目标信息。

(2)核心控制子***：完成核心控制算法的运行，根据感知层提供的观测数据，确定AUV当前的运动状态和下一步的运动目标，并利用控制算法获取各种运动控制参数，计算得到AUV为了达到目标状态所需的各自由度的推力。

(3)运动驱动子***：根据核心控制***计算的推力结果，将推力分配到各推进器上。

(4)应急情况处理子***：实现传感器、推进器故障、漏电、漏水和电压检测，并根据故障的等级对AUV的作业情况进行处理。为了保证任务的快速响应，同时考虑AUV的智能要求，建立了专门的任务来处理紧急情况，同时在某些紧急情况下控制***的信息将不通过核心控制***，直接由数据采集***传递给运动驱动***，以提高响应时间。

本发明采用两组48V/300AH的锂电池组为AUV提供能源，其参数为电压范围：43.2V-53.2V，正常工作电流：70A，工作温度：-20℃-50℃。同时开发了电源管理模块，可以为AUV提供5V、12V和24V的电源接口。

Claims

1.一种自主式水下机器人组合导航***，包括惯性基础导航装置和外部传感器导航装置，其中惯性基础导航装置包括多普勒测速仪、光纤陀螺、压力传感器、电子罗盘和深度计，外部传感器导航装置包括声纳，其特征在于：该组合导航***还包括水下结构光传感器和水下双目视觉平台，其中水下结构光传感器包括位于自主式水下机器人外框架（1）前部的前视结构光传感器和位于外框架（1）底部的下视结构光传感器，水下双目视觉平台包括位于外框架（1）前部的前视双目视觉平台和位于外框架（1）底部的下视双目视觉平台，前视结构光传感器和前视双目视觉平台组成了位于外框架（1）前部的前视结构光与视觉***模块（4），下视结构光传感器和下视双目视觉平台组成了位于外框架（1）底部的下视结构光与视觉***模块（7）；所述的声纳包括设置在外框架（1）前部上方的窄波束扫描成像声纳（3）和设置在外框架（1）前部下方的具有较宽波束的普通避碰声纳（5）；所述的水下结构光传感器包括摄像机、激光投射器、振镜以及振镜控制板；水下双目视觉平台包括两台水下高清摄像机和对应的图像采集卡。

2.一种权利要求1所述自主式水下机器人组合导航***的声纳信号斑特征提取方法，其特征在于该方法包括以下步骤：首先，利用融合脉冲耦合神经网络、中值滤波和维纳滤波优势的混合滤波进行图像噪声滤除；然后，利用广义形态学复合运算进行边缘检测和特征提取，获取障碍物信息，并提取障碍物的质心应用于AUV导航。

3.一种权利要求1所述自主式水下机器人组合导航***中水下结构光传感器的三维测量方法，其特征在于该方法包括以下步骤：

第一步，对水下摄像机进行标定：标定时利用实验室的水槽实现摄像机标定，利用平面网格靶标作为标定用靶标，标定过程在水中进行，由靶标上的网格点和对应的像面坐标建立若干共轭对，利用这些共轭对采用改进的径向约束排列方法来求解摄像机模型；

第二步，结构光光平面标定以及测量点坐标的确定：标定时先在水上根据振镜不同转角下在平面网格靶标上的交线确定振镜的初始角，以及振镜在不同角度下对应的光平面在世界坐标系下的方程，从而求出光平面上任一点即水中物体的三维坐标。