采用同时定位与地图构建方法的自主式水下机器人

摘要

本发明属于一种水下航行与运载工具，具体地说是一种采用同时定位与地图构建方法的自主式水下机器人。其以扫描成像声纳为主传感器、以同时定位与地图构建为主要方法，能够完成在较复杂的海底环境下的自主导航，本发明包括主计算机子系统、数据采集子系统、电源子系统、惯性导航子系统、舱内参数检测子系统、推进器控制子系统、舱外传感器子系统、冗余自救子系统和求救报警子系统，其中所述的电源子系统包括主电源子系统和备用电源子系统，数据采集子系统向上通过以太网与主计算机子系统通信，向下通过CAN总线与其它子系统通信。其特别适用于深海未知的复杂环境，制造成本和出海作业费用低，且可靠性好。

权利要求

1.一种采用同时定位与地图构建方法的自主式水下机器人，包括主计算机子系统、数据采集子系统、惯性导航子系统、舱内参数检测子系统、推进器控制子系统、舱外传感器子系统和电源子系统，所述的电源子系统包括主电源子系统和备用电源子系统，所述的机器人还包括冗余自救子系统和求救报警子系统，所述的数据采集子系统与主计算机子系统之间通过以太网通信，数据采集子系统、惯性导航子系统、主电源子系统、舱内参数检测子系统、推进器控制子系统、舱外传感器子系统、冗余自救子系统、备用电源子系统和求救报警子系统分别通过CAN适配器接入CAN总线，数据采集子系统通过CAN总线与惯性导航子系统、主电源子系统、舱内参数检测子系统、推进器控制子系统、舱外传感器子系统、备用电源子系统和求救报警子系统通信，所述的舱外传感器子系统包括声纳传感器，其由安装在外框架前方顶部的窄波束扫描成像声纳和安装在外框架前方底部的普通避碰声纳组成，其特征在于：所述的冗余自救子系统为双冗余自救子系统，其中第一套冗余自救子系统即为数据采集子系统，第二套冗余自救子系统与主计算机子系统和数据采集子系统之间通过RS232接口通信，并通过CAN适配器接入CAN总线与其它子系统通信。

2.根据权利要求1所述的采用同时定位与地图构建方法的自主式水下机器人，其特征在于：所述的主计算机子系统、数据采集子系统、惯性导航子系统、电源子系统、舱内参数检测子系统、冗余自救子系统和求救报警子系统设在压力舱内，所述的压力舱为呈上下设置的双压力舱，其固定在外框架上。

3.根据权利要求1所述的采用同时定位与地图构建方法的自主式水下机器人，其特征在于：所述的惯性导航子系统中的传感器包括光纤陀螺仪、加速度计和数字罗盘，其分别通过CAN适配器接入CAN总线，与数据采集子系统通信。

4.根据权利要求1所述的采用同时定位与地图构建方法的自主式水下机器人，其特征在于：所述的电源子系统由锂离子动力电池组和电池监测板组成，其中电池监测板通过CAN适配器接入CAN总线，所述的备用电源子系统通过CAN总线与冗余自救子系统通信。

5.根据权利要求1所述的采用同时定位与地图构建方法的自主式水下机器人，其特征在于：所述的舱内检测子系统由舱内参数检测板和检测电路组成，其中舱内参数检测板通过CAN适配器接入CAN总线。

6.根据权利要求1所述的采用同时定位与地图构建方法的自主式水下机器人，其特征在于：所述的推进器控制子系统包括五个水下推进器和分别控制各推进器的驱动控制板，其中沿压力舱的垂直方向设有三个推进器，水平方向设有两个推进器，垂直方向推进器分别固定于外框架前部的对称两侧和外框架的尾部，水平方向推进器分别固定于外框架中部的对称两侧，所述的驱动控制板通过CAN适配器接入CAN总线。

7.根据权利要求1所述的采用同时定位与地图构建方法的自主式水下机器人，其特征在于：所述的舱外传感器子系统还包括压力传感器和GPS天线，压力传感器固定于框架的尾部且垂直方向推进器的上方，GPS天线安装在外框架的后部上方，其分别通过CAN适配器接入CAN总线。

8.根据权利要求1所述的采用同时定位与地图构建方法的自主式水下机器人，其特征在于：所述的求救报警子系统由声学求救报警系统和无线求救报警系统组成，该子系统通过CAN适配器接入CAN总线与冗余自救子系统通信。

9.一种采用权利要求1所述的采用同时定位与地图构建方法的自主式水下机器人的声纳信号特征提取方法，其特征在于该方法包括以下步骤：

(1)在声纳每个Ping的所有Bin中找出主回波，定义为点特征；

(2)如果回波中较高强度的Bin连成片，形成了点簇，则将聚集在一定范围内的点簇用一个点特征表达；

(3)一周期的扫描结束后，对于距水下机器人相同距离上存在的连续点特征进行合并处理。

说明书

采用同时定位与地图构建方法的自主式水下机器人

技术领域

[0001] 本发明属于一种水下航行与运载工具，具体地说是一种采用同时定位与地图构建方法的自主式水下机器人。

背景技术

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

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

[0004] AUV作为机器人学科的一个重要分支，一直以来都在不断融入陆路机器人的最新技术和研究热点。目前国内对AUV的研究仅局限于陆路机器人的导航定位方面，在AUV上采用SLAM方法的研究很少。国际上，美国伍兹霍尔海洋研究所(WHOI)的研究者采用六自由度视觉系统基于SLAM方法开展AUV探测的研究，对“泰坦尼克”沉船进行勘探，取得很好效果；悉尼大学现场机器人研究中心用水下机器人进行珊瑚礁海底进行勘查，也取得初步效果。但是上述基于SLAM方法的AUV导航定位范围均很小，一般仅在几百米甚至几十米以内。

[0005] 发明内容

[0006] 本发明的目的在于在更大范围内实现基于同时定位与地图构建方法的自主式水下机器人自主导航，提出了一种采用同时定位与地图构建方法的自主式水下机器人，其以扫描成像声纳为主传感器、以同时定位与地图构建为主要方法，能够完成在较复杂的海底环境下的自主导航，特别适用于深海未知的复杂环境，制造成本低，通过该发明的出海作业费用大大降低，且整个装置的可靠性好。

[0007] 本发明是采用以下的技术方案实现的：一种采用同时定位与地图构建方法的自主式水下机器人，包括主计算机子系统、数据采集子系统、惯性导航子系统、舱内参数检测子系统、推进器控制子系统、舱外传感器子系统和电源子系统，其中所述的电源子系统包括主电源子系统和备用电源子系统，主电源子系统提供机器人系统的电力需求，备用电源子系统独立于主电源子系统，提供机器人系统在自救状态下的电力需求，所述的机器人还包括冗余自救子系统和求救报警子系统，当主计算机子系统无法正常工作时冗余自救系统接手对机器人系统的控制权，所述的数据采集子系统包括USB-CAN适配器，数据采集子系统向上与主计算机子系统之间通过以太网通信，数据采集子系统、惯性导航子系统、主电源子系统、舱内参数检测子系统、推进器控制子系统、舱外传感器子系统、冗余自救子系统、备用电源子系统和求救报警子系统分别通过CAN适配器接入CAN总线，数据采集子系统通过CAN总线与惯性导航子系统、主电源子系统、舱内参数检测子系统、推进器控制子系统、舱外传感器子系统、备用电源子系统和求救报警子系统通信，所述的舱外传感器子系统包括声纳传感器，其由安装在外框架前方顶部的窄波束扫描成像声纳和在外框架前方底部的具有较宽波束角的普通避碰声纳组成，其中窄波束扫描成像声纳用于探测周围环境目标，普通避碰声纳用于测量机器人距离水底的高度和探测前下方障碍物以紧急避碰。 [0008] 本发明中，所述的主计算机子系统、数据采集子系统、惯性导航子系统、电源子系统、舱内参数检测子系统、冗余自救子系统和求救报警子系统设在压力舱内，所述的压力舱为呈上下设置的双压力舱结构，压力舱固定在外框架上，即本发明采用开架式机械结构。 [0009] 所述的惯性导航子系统中的传感器包括光纤陀螺仪、加速度计和数字罗盘，上述传感器 采用中等精度仪器即可满足要求，分别通过CAN适配器接入CAN总线，与数据采集子系统交互数据。

[0010] 所述的电源子系统由锂电子动力电池组和电池监测板组成，电池的容量由水下机器人的续航时间以及推进器功率等因素确定，其中电池监测板通过CAN适配器接入CAN总线，所述的备用电源子系统通过CAN总线与冗余自救子系统通信。

[0011] 所述的舱内检测子系统用于时刻检测压力舱内的各种状态参数等，如温度、湿度、电源电压、电源电流、舱密封状态，其中温度、湿度和舱密封状态采用多点检测，该子系统由舱内参数检测板和前端检测电路组成，其中舱内参数检测板通过CAN适配器接入CAN总线。 [0012] 所述的推进器控制子系统包括五个水下推进器和分别控制各推进器的驱动控制板，可实现五自由度运动，其中沿压力舱的垂直方向设有三个推进器，水平方向设有两个推进器，垂直方向推进器分别固定于外框架前部的对称两侧和外框架的尾部，水平方向推进器分别固定于外框架中部的对称两侧，所述的驱动控制板通过CAN适配器接入CAN总线。 [0013] 所述的舱外传感器子系统还包括压力传感器和GPS天线，压力传感器固定于框架的尾部且垂直方向推进器的上方，用于测量水压从而计算该处距离水面的高度，GPS天线安装在外框架的后部上方，其分别通过CAN适配器接入CAN总线。

[0014] 所述的冗余自救子系统为双冗余自救子系统，其中第一套冗余自救子系统由数据采集子系统兼任，在主计算机子系统不能正常工作时接手控制，第二套冗余自救子系统为完全独立的嵌入式单板控制器，可以采用ARM嵌入式控制板，专门用于自救处理，在主计算机子系统和数据采集子系统都不能正常工作时接手控制，其与主计算机子系统和数据采集子系统之间通过RS232接口通信，并通过CAN适配器接入CAN总线与其它子系统通信。 [0015] 所述的求救报警子系统在冗余自救子系统的控制下工作，包括声学求救报警系统和无线求救报警系统，即当水下机器人出现故障时，可以通过两种方式发出求救报警，该子系统通过CAN总线与冗余自救子系统交互数据。

[0016] 本发明还包括一种采用同时定位与地图构建方法的自主式水下机器人的声纳信号特征提取方法，该方法包括以下步骤：

[0017] (1)在声纳每个Ping的所有Bin中找出主回波，定义为点特征；

[0018] (2)如果回波中较高强度的Bin连成片，形成了点簇，则将聚集在一定范围内，比如0.5米的点簇用一个点特征表达；

[0019] (3)一周期的扫描结束后，对于距水下机器人相同距离上存在的连续点特征进行合并处理。

[0020] 本发明的有益效果是：该自主式水下机器人导航方法与传统方法利用各种外部辅助导航手段不同，其不使用声学基阵如长基线LBL、短基线SBL、超短基线USBL等，也不使用昂贵 的高精度惯性导航系统，而主要依赖主动声纳传感器配合中等精度的惯性导航传感器，并采用同时定位与地图构建方法来完成自主导航定位，因此(1)使用和操作的费用低：传统方法每次出海都需要功能齐全的母船，势必会造成出海花费巨大，而本发明脱离了外部导航支持，比如母船支持和布放声学基阵，因此使得出海作业费用大大降低；(2)制造成本低：该装置采用了具有较好成像能力的主动扫描声纳作为主要的环境探测传感器，且配合使用中等精度的惯性导航传感器即可满足要求，无需采用昂贵的高精度惯性系统，因此大大降低了制造成本；(3)可工作的区域更广阔：由于本发明采用同时定位与地图构建方法的导航方法，减小了误差积累，因此在相同导航误差要求下可航行更远，可以实现10公里左右的自主导航；(5)由于本发明采用两套冗余自救子系统，当机器人发生意外或故障时起作用，保障机器人安全摆脱险境并浮出水面，通过声学方式或无线方式报警，因此整个机器人系统具有很好的可靠性；(4)未知性和复杂性是深海的特点，而本发明原理上是利用障碍物目标之间的统计相关性消除或减小航位计算过程中产生的误差积累，并且在深海环境下声纳的成像效果更优，因此本发明非常适用于深海环境。

附图说明

[0021] 图1为本发明的机械结构示意图；

[0022] 图2为本发明的系统结构示意图；

[0023] 图3为本发明的电控系统结构图；

[0024] 图4为本发明的同时定位与地图构建方法处理流程示意图；

[0025] 图5为水平方向控制器工作示意图；

[0026] 图6为垂直方向控制器工作示意图；

[0027] 图7为本发明工作原理图。

具体实施方式

[0028] 如1所示，本发明采用开架式机械结构，外框架1上固定上下双层压力舱2，并设置五个水下推进器，包括三个垂直方向推进器5和两个水平方向推进器6，其中在外框架1前部的左右对称两侧分别设有垂直方向推进器5，同时在外框架1的前部上方设有窄波束扫描成像声纳3，前部下方设有具有较宽波束的普通避碰声纳4；外框架1中部的左右对称两侧分别固定水平方向推进器6；外框架1的后部上方设有GPS天线8，其后部中间位置设有一个垂直方向推进器5，该推进器的上方安装有压力传感器7。

[0029] 如图2所示，本发明的自主式水下机器人系统主要由舱内主计算机子系统、数据采集子系统、惯性导航子系统、主电源子系统、舱内参数监测子系统、推进器控制子系统、舱外传感器子系统、冗余自救子系统、备用电源子系统和求救报警子系统组成，以下结合图2和图 3对各个子系统模块的功能和工作原理进行详细说明。

[0030] 1、主计算机子系统

[0031] 主计算机子系统是水下机器人的计算平台和上层决策中心，可以采用一台高性能工控机，比如配置IEI公司四核CPU-Q6600主板、8G内存和SATA2高速硬盘。主计算机担负机器人的绝大部分计算任务，包括传感器信号处理、姿态估计与导航计算、控制计算、任务决策、路径规划等计算。它发送指令给数据采集子系统，并从数据采集子系统获取各种传感器数据和底层参数数据。另外，它和第二套冗余自救子系统也有数据交互，以相互确认工作状态。

[0032] 2、数据采集子系统

[0033] 数据采集子系统负责采集来自各个子系统的数据，如惯性导航子系统、电源子系统、舱内参数监测子系统、推进器控制子系统、舱外传感器子系统，并和第二套冗余自救子系统有数据交互，以相互确认工作状态。数据采集子系统由PC104工控机和USB-CAN适配器组成。数据采集子系统向上与主计算机子系统通过以太网通信，接收主计算机子系统的指令数据，并将采集处理的各类数据发送给主计算机，高速网络通信方式能够确保大容量数据的可靠传输，向下通过CAN总线与其它子系统通信。另外，数据采集子系统还作为第一套冗余自救子系统，当主计算机子系统不能正常工作时取得整个系统的控制权。 [0034] 3、惯性导航子系统

[0035] 该子系统中的惯性导航传感器包括光纤陀螺仪、加速度计，并配合一个三轴数字罗盘。陀螺仪采用中等精度的产品，比如俄罗斯的VG951D，其作用是获得水下机器人的偏航角速率，数字量输出并积分后即可得到水下机器人的方向角。加速度计也采用中等精度的产品，比如Jewell的LCF2350，分辨率1μG。数字罗盘可选用HMR3000，角度精度可达0.5度，器件自带的校正功能使得该数字罗盘在距离推进器较近的情况下也可以可靠地工作。惯性导航子系统中的上述各个传感器通过CAN适配器接入CAN总线，和数据采集子系统交互数据。

[0036] 4、主电源子系统

[0037] 主电源由锂离子动力电池组和电池监测板组成，提供机器人系统的电力需求，电池的容量与水下机器人的续航时间以及推进器功率等因素密切相关，可按如下公式推算： [0038] 电池容量＝(推进器总功率×70％+工控机功率+高功率传感器的功率)×续航时间。电池监测板通过CAN适配器接入CAN总线，和数据采集子系统交互数据。 [0039] 5、舱内参数监测子系统

[0040] 要保证水下机器人的正常工作，必须时刻监测压力舱内的各种状态参数包括温度、湿度、电源电压、电源电流、舱密封状态等。其中温度、湿度和舱密封监测采用多点检测。舱内参数监测子系统通过CAN适配器接入CAN总线，和数据采集子系统交互数据。 [0041] 6、推进器控制子系统

[0042] 本发明采用五推进器方式，垂直方向有三个推进器，水平方向有两个推进器，可实现除水平移动外的五自由度运动。推进器采用分布式控制方案，每个推进器分别对应地由一个驱动控制板来控制，该控制板以单片机为核心，五个控制板分别通过CAN适配器接入CAN总线，和数据采集子系统交互数据。推进器由直流永磁电动机或直流无刷电动机配合多叶片螺旋桨组成。

[0043] 7、舱外传感器子系统

[0044] 本发明除了在舱内安装惯性导航传感器之外，还在舱外安装声纳传感器、压力传感器和GPS天线。其中声纳传感器有两个：一个安装在机器人的前方顶部，是窄波束的扫描成像声纳，用于探测周围环境目标；另外一个安装在前方底部，是避碰声纳即普通的扫描成像声纳，其具有较宽的波束角，用于测量机器人距离水底的高度和探测前下方障碍物以紧急避碰。压力传感器用来测量水压从而换算出机器人距离水面的距离。由于工作在水下环境，对舱外传感器的水密性和耐压性的要求较高，因此声纳和压力传感器本身需要足够的水密设计，目前市场上出售的GPS天线一般都不具有水密性，需要单独做水密处理，本发明选择聚四氟乙烯材料的密封壳体封闭GPS天线，具有较好的水密性，而且基本不影响GSP在水面接收无线信号。GPS天线和声纳传感器通过CAN适配器接入CAN总线，压力传感器连接AD采集卡并通过CAN适配器接入CAN总线。

[0045] 8、冗余自救子系统

[0046] 尽管机器人系统的设计相对比较可靠，但是不能保证其在工作过程中完全不会出现故障，尤其是当电池和上层计算机系统出现问题后，将导致水下机器人完全失控，因此冗余自救子系统用于在主计算机子系统或数据采集子系统无法正常工作时接手整个系统的控制权。冗余自救子系统有两个，构成双冗余自救子系统。其中第一套冗余自救子系统由数据采集子系统兼任，在主计算机不能正常工作时接手控制，第二套冗余自救子系统是完全独立的一套嵌入式单板控制器，可以采用ARM嵌入式控制板，其专门用于自救处理，当主计算机子系统和数据采集子系统都不能正常工作时，第二套冗余自救子系统接手控制。 [0047] 9、备用电源子系统

[0048] 备用电源独立于主电源子系统，由锂离子动力电池组和电池监测板组成，提供给机器人系统在自救状态下的电力需求。一般备用电池组的容量比主电源电池组容量小得多，设计时一般考虑至少能够使机器人浮上海面所需要的能量。备用电源子系统的电池监测板通过CAN适配器接入CAN总线与冗余自救子系统交互数据。

[0049] 10、求救报警子系统

[0050] 在水下机器人出现故障时，求救报警子系统在冗余自救子系统的控制下工作，通过两种方式发出求救报警，一种是在水下通过声学Modem方式，另一种是浮出水面后通过无线通信方式。求救报警子系统通过CAN适配器接入CAN总线，与第一套冗余自救子系统即数据采集子系统以及第二套冗余自救子系统交互数据。

[0051] 本发明的水下机器人采用同时定位与地图构建方法即SLAM方法进行导航定位，从而实现自主航行。该方法的工作原理描述如下：利用该方法进行导航定位的过程中涉及到两个坐标系，即静坐标系和动坐标系。静坐标系即绝对坐标系，动坐标系是以当前载体的位置为中心，以当前航向方向为X轴。SLAM方法的执行过程大体可以分为三个阶段：预测，观测和更新。首先将载体姿态和地图特征存储在一个独立的状态向量中，然后通过先预测再观测的迭代递推过程来估计系统状态，实现对载体的定位和特征地图的创建。预测通常是利用自身携带的陀螺仪等仪器对当前时刻载体的姿态进行航位推算，由于传统航位推算的精度一般均很低，所以在预测阶段载体姿态的估计误差会有所增加。在观测阶段，利用声纳感知周围环境，对载体周围环境中的特征进行再次观测，得到载体坐标系下表示特征与载体相对位置的量测值，再利用载体当前的估计位置将地图中的特征与量测值转换到同一坐标系下，通过数据关联过程确定量测与特征的一一对应关系，将两者的位置偏差作为观测值，经过扩展卡尔曼滤波处理后，可以同时改善载体和特征的状态估计。本发明的水下机器人同时定位与地图构建处理算法的流程如图4所示。

[0052] 通过对周围环境感知来进行基于SLAM的自主导航主要依赖于安装在AUV上部的扫描成像声纳。而对于地图在线构建和AUV定位方法而言，只有环境特征是唯一可用的，因此特征的提取是至关重要的一环。在特征提取之前先要进行声纳回波信号的滤波，其中阈值的选择对目标检测有重要的影响，如果阈值选择过低，输出背景信号比重加大，不稳定的潜目标较多，目标信号不明显；选择过高可能会保留过多的噪声，并且丢失部分目标信息。水听设备可以接收到强度在0dB～80dB的声纳回波信号，一般把最高采样分贝T设定在
50dB～60dB之间，则采样范围为[0T]，既可以很好地保留目标信息也可以去除部分高强度噪声的干扰。滤波之后即可从声纳数据中提取特征，提取特征主要采取如下三个步骤： [0053] (1)在声纳每个Ping中的所有Bin中找出主回波，把它定义为点特征。主回波是那些相对Ping中强度幅值的平均值而言明显较高的回波Bin，该Bin对应的距离值代表了声纳和目标之间的距离；

[0054] (2)如果回波中较高强度的Bin连成片，形成了点簇，则将聚集在一定距离，如0.5米之内的点簇用一个点特征表达。这样，那些由海底成片礁石形成的成片点簇就被合并标记成等间距的若干点特征，这些点特征间距为0.5米；

[0055] (3)一周期的扫描结束后，对于距水下机器人相同距离，即相同编号的Bin上存在的连续点特征进行合并处理，将0.5米内的所有点特征合并为一个点特征，但是由上述主回波形成的点特征不参与合并处理。

[0056] 按上述方法获得点特征后，很容易可知每个点特征的方位角和距离。稳定的点特征都将被加入到特征地图中。上述步骤(2)和(3)中都有合并处理，这主要是考虑到海底复杂环境很容易使得声纳数据处理产生过于密集的点特征，不利于水下机器人导航定位算法的实时在线计算。上述步骤中以纵向和横向两种方式进行合并处理，类似于一种针对密集点特征的低通滤波效果，并且由于我们规定主回波形成的点特征不参与此滤波，因此既能形成对大片礁岩稳定而有规律的点特征标记，又不会丢失海底中明显突起礁石的可靠的点特征标记。

[0057] 本发明的控制系统采用了两层控制架构，即底层的推进器驱动控制和高层的控制行为决策系统。底层控制系统采用解耦方式将水下机器人的运动分解为水平方向和垂直方向，因此通过如图5所示的水平方向控制器和如图6所示的垂直方向控制器分别完成水平方向控制和垂直方向控制，其中PID参数以及其它比例系数根据实际工作情况进行整定，高层决策系统的输出量成为底层驱动系统的输入期望值，其采用的行为决策结构如图7中所示。综上所述，本发明中的水下机器人实现自主导航的工作过程如下所示：首先，航行过程中底层数据采集系统对所述的未知海底环境进行数据采集，这个过程由惯性导航子系统和舱外传感器子系统的传感器完成，其中加速度计、光纤陀螺配合数字罗盘在SLAM方法的预测阶段中对当前时刻载体的姿态进行航行推算，其中光纤陀螺获得水下机器人的偏航角速率，数字罗盘输出机器人三轴姿态角；窄波束扫描成像声纳用于探测环境目标，可以较精确地获得周围环境中的目标反射波，成像声纳的观测主要用于SLAM方法的观测阶段，对水下机器人周围环境中的特征进行观测进而通过特征提取获得目标的距离和方位角；普通避碰声纳用于探测前方障碍和测量机器人与海底之间的距离；压力传感器用于测量机器人所述位置与海平面之间的距离。上述传感器的原始数据经数据采集子系统采集并将相关数据传输给主计算机子系统。主计算机子系统通过对SLAM处理算法的计算可获得水下机器人姿态最优估计以及环境特征地图的最优估计，这些信息和前述已获得的距离/方位信息、障碍物距离信息、海拔信息和深度信息分别输入到预先定义的各种行为模式中，行为模式是指水下机器人在一定输入条件下经过预先设计的行为计算方法输出机器人下一时刻的期望执行量，比如偏航角、前移量或深度。本发明中共定义了七种行为模式，分别是设定路线航行行为、区域探测行为、避障行为、设定海拔航行行为、最小海拔航行行为、设定深度航行行为和最小深度航行行为。各个行为模式之间是相互独立的，因此都会产生各自的期望执行量，这些执行量被直接送往偏航角/航速模糊逻辑决策器和深度模糊逻辑决策器中，偏航角/航速模糊逻辑决策器和深度模糊逻辑决策器在 工作时也同时接收任务规划与路径规划模块的指令，它们采用一套事先设计整定好的模糊决策算法来最终确定下一时刻的期望执行量，即期望的偏航角/航速和期望的深度，并发送到底层的推进器控制子系统。推进器控制子系统利用陀螺仪数据和压力传感器数据并使用了两个解耦的PID控制器，图5和图6只是给出了解耦的控制器方案，实际工作中还需要根据水下机器人的机械动力学模型计算出两个水平推进器和三个垂直推进器各自的推力分配值。最后，由PID的输出控制功率驱动电路从而驱动水平方向推进器和垂直方向推进器。

[0058] 本发明实现了一种以扫描成像声纳为主传感器、以同时定位与地图构建方法为主要导航定位方法的自主式水下机器人，该机器人的技术指标是：

[0059] 外形尺寸：应按具体使用要求，特别是搭载负荷要求，比如外框架可以是长1.5米、宽1.2米、高1.1米，由304不锈钢管材制成；压力舱直径0.3米，长1米，由硬铝合金材料制成。

[0060] 工作水深：不限，主要取决于压力舱的材质、结构和水密能力，以及外部传感器的水密指标等。

[0061] 航速：对于开架式的水下机器人，航速一般最高4节；

[0062] 水平推力：为匹配上述外形和结构，30～40公斤力；

[0063] 垂直推力：25公斤力；

[0064] 续航能力：配4000Wh电池，一般可续航5～8小时；

[0065] AUV导航定位精度：在比较复杂的海底环境中以巡航10公里计，定位误差可小于100米。

[0066] 在已知起始点绝对坐标的情况下，该机器人能够完全借助声纳与其它机载传感器在较复杂海底环境下进行自主导航，巡航过程中不上浮，完全采用无缆方式；能够实现特征地图闭环，即在环航返回到出发点时能够识别出起始点海域。

[0067] 本发明通过搭载多种传感器和作业设备，可以开展在江河湖泊、水电大坝、海洋等水下环境中的监测任务以及维护、维修等工作。