专利名称：一种自主水下机器人的同时检测与定位方法
技术领域：
本发明涉及自主水下机器人(Autonomous Underwater Vehicle-AUV)的导航技 术，属于海洋工程技术领域。
背景技术：
自主水下机器人(Autonomous Underwater Vehicle-AUV)是目前海洋工程领域技 术发展的热点，在水下环境监测、近海石油工程作业、水下搜索与测绘、以及水雷对抗和实 时战区警戒等军事领域获得越来越广泛的应用。而导航技术是实现AUV自主航行的关键。 AUV所具有的工作时间长、环境复杂、信息源少、隐蔽性要求高等特点，给稳定、精确的导航 实现带来很大的挑战。首先，由于电磁波在水下的快速衰减，传统的GPS卫星导航技术只适 用于水体表层有限的区域；其次，基于声学导航的长基线和短基线定位技术需要预先布设 或者船体安装外部信标，使得AUV活动范围受到限制；此外对于小型机器人而言，采用低成 本、小体积的惯性导航技术或者航位推算技术，无法满足中、长程航行的导航要求。基于以 上原因，发展适用于中、长航程的新型导航技术，并联合传统的惯性导航技术形成组合导航 已成为目前水下机器人导航技术发展的重要趋势。近年来国际上对于AUV导航技术的研究非常活跃，主要研究单位包括美国海军、 麻省理工学院、Woods Hole海洋研究所、海军研究生院、佛罗里达大西洋大学等，欧洲的 Norwegian Defense Research Estab 1 ishment>KongsbergSimrad AS>NATO SACLANT/jC下石if 究中心，以及澳大利亚悉尼大学的Field机器人研究中心(Australian Centre for Field Robotics)等。水下导航技术通常需要在精度、操作距离和自动化程度等之间进行平衡，概 括起来主要有以下几种类型航位推算与惯性导航、声学信标导航以及地球物理导航。由于水下声传播的透明性，声纳系统是AUV构成的重要组成部分，用于环境测量、 地形测绘、成像及通信与定位。现有AUV航位推算仪中普遍使用的惯性导航系统(INS)、 多普勒计程仪(DVL)存在时间二阶或一阶累积误差，造成的导航误差在一定时间以后变得 难以接受，同时使所承载的海底地形测量、目标定位等产生较大的偏差。常规方法通过定 期上浮在水面进行GPS修正，这在受时间和能量限制的深海操作或者需要隐秘性的操作 中，是不实际的。基于声纳成像地形匹配的方法是目前水下导航技术研究热点之一。其 执行需要航行区域的先验信息，而实际中通常面临的是未知环境，迫切需要AUV在成像的 同时进行自身定位，无需先验信息的同时定位与地图创建(Simu 1 taneousLocal ization And Mapping, SLAM)技术正是在此背景下发展起来的。SLAM方法中利用概率进行估计的 思想最早在1986年IEEE机器人和自动化学术会议上被提出。此后，Smith、Cheesman和 Durrant-Whyte等人建立了描述地标与AUV运动几何轨迹之间相关性的统计学基础。同时， Smith等人提出了在未知环境下，把AUV成像与定位参数作为状态变量进行联合估计的思 想，然而该方法的主要缺点是未知参数多、算法复杂且较难收敛。随着研究的深入，尤其是 对地标之间相关性对AUV定位的影响等认识的加深，1995年的国际机器人研讨会上SLAM方 法首次被明确提出，研究者把SLAM问题建模为单一的估计问题，论证了该估计问题的收敛性，并给出了一些初始结果。近年来，多家研究机构对SLAM导航方法进行了深入研究，并取 得了重要进展，具有代表性的有美国的麻省理工学院，澳大利亚的悉尼大学等，其中麻省理 工学院把基于卡尔曼滤波(KF)的SLAM技术应用于REMUS机器人，已在300m以上水深实际 环境中取得可靠导航结果。我国的水下机器人导航技术研究开始于上个世纪80年代。主要研究单位包括中 科院沈阳自动化所、中科院声学所、哈尔滨工程大学、西北工业大学、中国海洋大学、中船重 工有关研究所等，另外海军舰艇学院、上海交通大学、东南大学、华中科技大学以及浙江大 学等也开展了有关的研究工作。早期研究主要以长基线和短基线定位为主，包括沈阳自动 化所牵头研制的1000米水下机器人、6000米水下机器人等项目。近年来自主导航技术发展 迅速，以航向传感器与多普勒计程仪的组合导航为主，辅以水面GPS修正，浙江大学与中船 重工有关研究所联合研制的自主水下机器人利用组合导航技术在若干次实验中取得了航 行距离0. 3%至0. 5%的导航精度，性能与国际同类系统相仿。然而对于SLAM技术的研究， 在国内尚处于方法研究及硬件准备阶段，大规模实验尚未开展。综上所述，应用于水下AUV的SLAM技术在国外尚未成熟，在国内处于起步阶段。该 技术有待完善之处包括(1)定位数据更新普遍采用卡尔曼滤波或扩展卡尔曼滤波方法， 算法中Jacobian矩阵的计算使其计算复杂性较高，且需建立多个模型，导致的建模误差使 导航精度降低，尤其在非高斯环境下，估计误差较大；(2)在线地图构建对AUV载声纳系统 及信号处理系统实时性处理要求较高，通常难以实时地满足同时定位及地图构建任务；(3) 远距离、非结构化海底地形等情况下导航的稳定性和精确性有待提高。

发明内容
本发明的目的是提供一种自主水下机器人的同时检测与定位方法。本发明利用AUV搭载的声纳对未知海底环境进行海底特征目标检测，同时通过检 测得到的海底特征目标之间的相关性来实现对AUV自身的定位，本发明在实质上是SLAM技 术的一种简化形式。本发明通过迭代的方法使后验概率达到最大化以实现AUV的位置估 计，即在AUV的初始位置先验通过GPS定位系统测量得到的情况下，融合其他测量数据，利 用AUV的状态转移信息及海底特征声纳成像信息，通过代价函数最优，而代价函数最优也 就是后验概率最大，从而实现AUV的同时检测与定位。本发明与INS、DVL导航等相组合，从 而修正目前INS、DVL由于长时间水下航行导致的因时间积累而产生的全局导航误差，从而 实现高精度的水下机器人组合导航，显著提高AUV整体导航的性能。为实现上述目的，本发明所采取的技术方案是该自主水下机器人的同时检测与 定位方法包括如下步骤(1)在自主水下机器人下潜之前，利用GPS定位系统和航向角测量工具获取自主 水下机器人的当前位置信息，所述当前位置信息包括自主水下机器人的水平面直角坐标位 置和自主水下机器人的航向角；(2)在自主水下机器人下潜过程中，利用声纳开始接收声波数据，所述声波数据包 括声信号的幅度，对当前声波数据进行波束形成处理以对海底地形地貌成像；(3)在自主水下机器人下潜后，在对声纳所接收的当前声波数据进行波束形成处 理以对海底地形地貌成像并在成像时对自主水下机器人进行位置估计；根据当前与前一次海底地形地貌成像的像差得到当前测量条件概率；根据当前和前一次的位置估计利用公式 (1)得到自主水下机器人在当前与后一次位置估计之间的转移概率 式(1)中，分别表示当前和前一次的位置估计，C为归一化常数，A表示自 主水下机器人在前一次和当前次位置估计之间的转移矩阵，P(X1IXw)表示自主水下机器 人在前一次和当前次位置估计之间的转移概率；根据所述当前测量条件概率和所述自主水下机器人在当前与后一次位置估计之 间的转移概率得到自主水下机器人当前位置估计的后验概率；(4)重复步骤(3)得到自主水下机器人所有位置估计的后验概率；(5)根据所述自主水下机器人所有位置估计的后验概率得到自主水下机器人的运 动轨迹的估计，根据自主水下机器人的运动轨迹的估计得到自主水下机器人的自主水下机 器人的实际运动轨迹。本发明相比于现有技术的主要优点为(1)利用概率的方法表示海底地形地貌的 特征，因此对于缺少点、线等图像特征的非结构化海底地形以及较低质量的声纳接收数据， 本发明仍然具有较好的宽容性，同时也具有广泛的适用性；(2)较卡尔曼滤波等方法而言， 本发明没有建模误差，其精度也较卡尔曼滤波等方法高；(3)针对AUV导航的目的，只对海 底特征目标进行成像，而不用创建地图，可大幅降低系统硬件资源的需求，有利于在线实 现，同时又不失导航的精确性，同时，本发明无需计算卡尔曼滤波方法中所使用的Jacobian 矩阵，进一步降低了计算复杂性，提高了 AUV导航的效率。


图1为本发明方法的流程图；图2为本发明实施例的仿真结果示意图，其中，AUV做直线运动；图3为本发明实施例的仿真结果示意图，其中，AUV做圆弧运动。
具体实施例方式本发明自主水下机器人的同时检测与定位方法具体如下(1)在自主水下机器人下潜之前，利用GPS定位系统和航向角测量工具获取AUV的 当前位置信息X1, X1 = [i(l)，j(l)，θ⑴]，其中，i(l)和j(l)为AUV的当前水平面直角 坐标位置，θ (1)为AUV的航行的当前航向角。航向角测量工具是指具有测量航行角功能 的测量工具，例如惯性测量单元、电子罗盘等。(2)在自主水下机器人下潜过程中，利用声纳开始接收声波数据，所接收的声波数 据包括声信号的幅度；并对当前声波数据进行波束形成处理以对海底地形地貌成像。所述 波束形成处理就是使用一定的加权，以便检测从一个特定位置到达的信号。因为加权过程 加重一个从特定位置来的信号，而削弱其他位置来的信号，因而可被认为是得到了从这个 特定位置来的信号。波束形成处理包括常规波束形成处理，自适应波束形成处理等。(3)在自主水下机器人下潜后，GPS淹没在水中，不能对自主水下机器人进行定 位。对声纳所接收到的当前声波数据进行波束形成处理，把波束形成的方向指向海底位置 (i，j)，以得到来自海底位置(i，j)的信号Zij(I)的幅度，(i，j)为水平面直角坐标。在AUV
式(2中)，M为优化搜索时可能的路径的网格点数。利用各次像差的负数，由公式⑶得到测量矩阵 式(3)中，1= 1,2,…，L0利用当前次波束形成处理和前一次波束形成处理的像差的负数，由公式(4) 得到测量条件概率 式(4)中ο 2为噪声的均方误差，可通过统计建模得到。 当前次波束形成处理时的AUV的位置估计 式(5)中，i (1)和j (1)为AUV在当前次波束形成处理时的水平面直角坐标位置 估计；u和ν为AUV在当前次波束形成处理时的水平面直角坐标速度估计；θ (1)为AUV在 当前次波束形成处理时的航行的水平面航向角估计；ω为AUV在当前次波束形成处理时的 航行的水平面航向角速度估计。利用状态转换矩阵A以及当前和前一次的位置估计，由公 式(1)得到AUV在当前一次与当前次位置估计之间的转移概率ρ (X1IXw) 式(1)中，分别表示当前和前一次的位置估计；C为归一化常数，可选取任 意的正数；A表示自主水下机器人在前一次和当前次位置估计之间的转移矩阵；P(X1IXw) 表示自主水下机器人在当前与后一次位置估计之间的转移概率。其中， 式(6)中，T表示当前次波束形成和前一次波束形成的时间间隔。AUV的各次测量矩阵按时间的先后顺序组成序列Z1 = Iz1, . . . zj , AUV的状态各
的一次同时检测和定位过程中需要进行多次波束形成的处理，1表示当前次波束形成处理 在所有波束形成处理中的次序。利用当前次波束形成处理和前一次波束形成处理得到在 (i，j)位置上的像差、々(0。利用在(i，j)位置上的像差、>,(/)，ι)，由公式⑵得到当前 次波束形成处理和前一次波束形成处理的像差的负数次状态按时间的先后顺序组成序列X1= Ix1,... Xl}，通过以下式(7)，利用得到的AUV的当 前次和前一次位置估计的状态转移概率P (X11 X1^1)及AUV的当前次的测量概率ρ (Z11X1)，由 公式(7)得到AUV位置估计的后验概率ρ (X11Z1) ρ (X11 Z1) = ρ (Z11 X1) *p (X11 Z1J = ρ (Z11 X1) *p (X11 X1^1) *p (X1^11 Z1^1) (7)(4)重复步骤(3)得到自主水下机器人所有位置估计的后验概率，按时间的先后 顺序排列。(5)根据自主水下机器人所有位置估计的后验概率得到自主水下机器人的运动轨 迹的估计，根据自主水下机器人的运动轨迹的估计得到自主水下机器人的自主水下机器人 的实际运动轨迹。因为选择AUV的轨迹X1的代价函数满足一阶马尔科夫(Markov)随机过程，也就 是说每个代价函数S1U1, X1^1)只与当前次的状态向量和前一次的状态向量有关，所以由公 式⑶得到选择AUV的轨迹X1的代价函数s (xi; X1^1, ... X1)。 所述AUV的轨迹X1为从第1次波束形成处理到第1次波束形成处理时的AUV的 所有1个位置点。由公式(9)得到最优化过程Χι_Λ . . . X1*) 式(9)中，s*表示取s的最大值，而χΛ Χι_Λ . . . X1*代表着能使s取最大值时的 AUV的运动的轨迹。由公式(10-1)或(10-2)得到当前次波束形成时的代价函数S1(XpXw)与之前所 有代价函数之和的最大值I^1(X1)或Ii1 (X2)。
其中，当1大于2时，使用式(10-1)计算得到I^1(X1)；当1等于2时，使用式 (10-2)计算得到 Ill (X2)。所述选择AUV的轨迹X1的代价函数S(X1)可以利用AUV位置估计的后验概率 P (X11Z1),由式(11)得到。 最后，动态过程更新方程的最大代价函数Iv1 (Xk)可以由式(12)的得到Iv1 (Xk)的 迭代形式。 通过逐步递推优化即可输出AUV的轨迹的定位结果。结合图1的流程图，进行了本发明方法的仿真实验分析。图2显示的是利用本发 明方法，在AUV做直线运动时，对其路径进行估计的结果。图3显示的是利用本发明方法， 在AUV做圆弧运动时，对其路径进行估计结果。仿真实验时AUV的实际运动速度为1.5m/s，速度误差小于0.3m/s，角速度误差小于1° /s, AUV载声纳线阵扫描范围为120度开角， 距离20m。图2和图3中三角形表示广义海底特征，声纳检测输出平均信噪比约OdB，曲线 1为AUV作直线运动时的实际运动轨迹；曲线2根据速度及角速度值积分所得的AUV作直 线运动时的轨迹；曲线3为利用本发明方法对AUV作直线运动时估计得到的运动轨迹，AUV 运动时间约60秒；曲线4为AUV作圆弧运动时的实际运动轨迹；曲线5为根据速度及角速 度值积分所得的AUV作圆弧运动时的轨迹；曲线6为利用本发明方法对AUV作圆弧运动时 估计得到的运动轨迹，AUV运动时间约60秒。从图2和图3中可知，本发明算法估计的轨 迹与实际较一致，且基本不受时间累积的影响。
权利要求
一种自主水下机器人的同时检测与定位方法，其特征是包括如下步骤(1)在自主水下机器人下潜之前，利用GPS定位系统和航向角测量工具获取自主水下机器人的当前位置信息，所述当前位置信息包括自主水下机器人的水平面直角坐标位置和自主水下机器人的航向角；(2)在自主水下机器人下潜过程中，利用声纳开始接收声波数据，所述声波数据包括声信号的幅度，对当前声波数据进行波束形成处理以对海底地形地貌成像；(3)在自主水下机器人下潜后，对声纳所接收的当前声波数据进行波束形成处理以对海底地形地貌成像并在成像时对自主水下机器人进行位置估计；根据当前与前一次海底地形地貌成像的像差得到当前测量条件概率；根据当前和前一次的位置估计利用公式(1)得到自主水下机器人在当前与后一次位置估计之间的转移概率 <mrow><mi>p</mi><mrow> <mo>(</mo> <msub><mi>x</mi><mi>l</mi> </msub> <mo>|</mo> <msub><mi>x</mi><mrow> <mi>l</mi> <mo>-</mo> <mn>1</mn></mrow> </msub> <mo>)</mo></mrow><mo>=</mo><mi>C</mi><mo>/</mo><mrow> <mo>(</mo> <msub><mover> <mi>x</mi> <mo>&OverBar;</mo></mover><mi>l</mi> </msub> <mo>-</mo> <mi>A</mi> <msub><mover> <mi>x</mi> <mo>&OverBar;</mo></mover><mrow> <mi>l</mi> <mo>-</mo> <mn>1</mn></mrow> </msub> <mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo></mrow> </mrow>式(1)中，和分别表示当前和前一次的位置估计，C为归一化常数，A表示自主水下机器人在前一次和当前次位置估计之间的转移矩阵，p(xl|xl 1)表示自主水下机器人在前一次和当前次位置估计之间的转移概率；根据所述当前测量条件概率和所述自主水下机器人在当前与后一次位置估计之间的转移概率得到自主水下机器人当前位置估计的后验概率；(4)重复步骤(3)得到自主水下机器人所有位置估计的后验概率；(5)根据所述自主水下机器人所有位置估计的后验概率得到自主水下机器人的运动轨迹的估计，根据自主水下机器人的运动轨迹的估计得到自主水下机器人的自主水下机器人的实际运动轨迹。FSA00000193681900012.tif,FSA00000193681900013.tif
全文摘要
本发明公开一种自主水下机器人的同时检测与定位方法，包括如下步骤(1)在自主水下机器人下潜之前，获取自主水下机器人的当前位置信息；(2)在自主水下机器人下潜过程中，接收声波数据，对当前声波数据进行波束形成处理以对海底地形地貌成像；(3)在自主水下机器人下潜后，对当前声波数据进行波束形成处理以对海底地形地貌成像并在成像时对自主水下机器人进行位置估计，得到当前测量条件概率以及自主水下机器人在当前与后一次位置估计之间的转移概率；(4)重复步骤(3)得到自主水下机器人所有位置估计的后验概率；(5)根据所有位置估计的后验概率得到自主水下机器人的运动轨迹的估计及实际运动轨迹。本发明提高了AUV导航的效率。
文档编号G01S15/06GK101907711SQ20101022861
公开日2010年12月8日 申请日期2010年7月16日 优先权日2010年7月16日
发明者官兴华, 徐文, 李建龙 申请人:浙江大学