CN101907711A - 一种自主水下机器人的同时检测与定位方法 - Google Patents

Abstract

本发明公开一种自主水下机器人的同时检测与定位方法，包括如下步骤：(1)在自主水下机器人下潜之前，获取自主水下机器人的当前位置信息；(2)在自主水下机器人下潜过程中，接收声波数据，对当前声波数据进行波束形成处理以对海底地形地貌成像；(3)在自主水下机器人下潜后，对当前声波数据进行波束形成处理以对海底地形地貌成像并在成像时对自主水下机器人进行位置估计，得到当前测量条件概率以及自主水下机器人在当前与后一次位置估计之间的转移概率；(4)重复步骤(3)得到自主水下机器人所有位置估计的后验概率；(5)根据所有位置估计的后验概率得到自主水下机器人的运动轨迹的估计及实际运动轨迹。本发明提高了AUV导航的效率。

Description

一种自主水下机器人的同时检测与定位方法

技术领域

本发明涉及自主水下机器人(Autonomous Underwater Vehicle-AUV)的导航技术，属于海洋工程技术领域。

背景技术

自主水下机器人(Autonomous Underwater Vehicle-AUV)是目前海洋工程领域技术发展的热点，在水下环境监测、近海石油工程作业、水下搜索与测绘、以及水雷对抗和实时战区警戒等军事领域获得越来越广泛的应用。而导航技术是实现AUV自主航行的关键。AUV所具有的工作时间长、环境复杂、信息源少、隐蔽性要求高等特点，给稳定、精确的导航实现带来很大的挑战。首先，由于电磁波在水下的快速衰减，传统的GPS卫星导航技术只适用于水体表层有限的区域；其次，基于声学导航的长基线和短基线定位技术需要预先布设或者船体安装外部信标，使得AUV活动范围受到限制；此外对于小型机器人而言，采用低成本、小体积的惯性导航技术或者航位推算技术，无法满足中、长程航行的导航要求。基于以上原因，发展适用于中、长航程的新型导航技术，并联合传统的惯性导航技术形成组合导航已成为目前水下机器人导航技术发展的重要趋势。

近年来国际上对于AUV导航技术的研究非常活跃，主要研究单位包括美国海军、麻省理工学院、Woods Hole海洋研究所、海军研究生院、佛罗里达大西洋大学等，欧洲的Norwegian Defense Research Establishment、KongsbergSimrad AS、NATO SACLANT水下研究中心，以及澳大利亚悉尼大学的Field机器人研究中心(Australian Centre for Field Robotics)等。水下导航技术通常需要在精度、操作距离和自动化程度等之间进行平衡，概括起来主要有以下几种类型：航位推算与惯性导航、声学信标导航以及地球物理导航。

由于水下声传播的透明性，声纳***是AUV构成的重要组成部分，用于环境测量、地形测绘、成像及通信与定位。现有AUV航位推算仪中普遍使用的惯性导航***(INS)、多普勒计程仪(DVL)存在时间二阶或一阶累积误差，造成的导航误差在一定时间以后变得难以接受，同时使所承载的海底地形测量、目标定位等产生较大的偏差。常规方法通过定期上浮在水面进行GPS修正，这在受时间和能量限制的深海操作或者需要隐秘性的操作中，是不实际的。基于声纳成像地形匹配的方法是目前水下导航技术研究热点之一。其执行需要航行区域的先验信息，而实际中通常面临的是未知环境，迫切需要AUV在成像的同时进行自身定位，无需先验信息的同时定位与地图创建(SimultaneousLocalization 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)得到自主水下机器人在当前与后一次位置估计之间的转移概率

$p\left(x_l\right|x_{l-1})=C/({\stackrel{\‾}{x}}_l+A{\stackrel{\‾}{x}}_{l-1})---\left(1\right)$

式(1)中，和

分别表示当前和前一次的位置估计，C为归一化常数，A表示自主水下机器人在前一次和当前次位置估计之间的转移矩阵，p(x_l|x_l-1)表示自主水下机器人在前一次和当前次位置估计之间的转移概率；

根据所述当前测量条件概率和所述自主水下机器人在当前与后一次位置估计之间的转移概率得到自主水下机器人当前位置估计的后验概率；

(4)重复步骤(3)得到自主水下机器人所有位置估计的后验概率；

(5)根据所述自主水下机器人所有位置估计的后验概率得到自主水下机器人的运动轨迹的估计，根据自主水下机器人的运动轨迹的估计得到自主水下机器人的自主水下机器人的实际运动轨迹。

本发明相比于现有技术的主要优点为：(1)利用概率的方法表示海底地形地貌的特征，因此对于缺少点、线等图像特征的非结构化海底地形以及较低质量的声纳接收数据，本发明仍然具有较好的宽容性，同时也具有广泛的适用性；(2)较卡尔曼滤波等方法而言，本发明没有建模误差，其精度也较卡尔曼滤波等方法高；(3)针对AUV导航的目的，只对海底特征目标进行成像，而不用创建地图，可大幅降低***硬件资源的需求，有利于在线实现，同时又不失导航的精确性，同时，本发明无需计算卡尔曼滤波方法中所使用的Jacobian矩阵，进一步降低了计算复杂性，提高了AUV导航的效率。

附图说明

图1为本发明方法的流程图；

图2为本发明实施例的仿真结果示意图，其中，AUV做直线运动；

图3为本发明实施例的仿真结果示意图，其中，AUV做圆弧运动。

具体实施方式

本发明自主水下机器人的同时检测与定位方法具体如下：

(1)在自主水下机器人下潜之前，利用GPS定位***和航向角测量工具获取AUV的当前位置信息x₁，x₁＝[i(1)，j(1)，θ(1)]，其中，i(1)和j(1)为AUV的当前水平面直角坐标位置，θ(1)为AUV的航行的当前航向角。航向角测量工具是指具有测量航行角功能的测量工具，例如惯性测量单元、电子罗盘等。

(2)在自主水下机器人下潜过程中，利用声纳开始接收声波数据，所接收的声波数据包括声信号的幅度；并对当前声波数据进行波束形成处理以对海底地形地貌成像。所述波束形成处理就是使用一定的加权，以便检测从一个特定位置到达的信号。因为加权过程加重一个从特定位置来的信号，而削弱其他位置来的信号，因而可被认为是得到了从这个特定位置来的信号。波束形成处理包括常规波束形成处理，自适应波束形成处理等。

(3)在自主水下机器人下潜后，GPS淹没在水中，不能对自主水下机器人进行定位。对声纳所接收到的当前声波数据进行波束形成处理，把波束形成的方向指向海底位置(i，j)，以得到来自海底位置(i，j)的信号z_ij(l)的幅度，(i，j)为水平面直角坐标。在AUV的一次同时检测和定位过程中需要进行多次波束形成的处理，l表示当前次波束形成处理在所有波束形成处理中的次序。利用当前次波束形成处理和前一次波束形成处理得到在(i，j)位置上的像差

利用在(i，j)位置上的像差

l)，由公式(2)得到当前次波束形成处理和前一次波束形成处理的像差的负数

${\tilde{z}}_{\mathrm{ij\θ}}\left(l\right)=-\frac{1}{M^2}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}\underset{j=1}{\overset{M}{\mathrm{\Σ}}}{z}_{\mathrm{ij},x_l}{\left(l\right)}^2---\left(2\right)$

式(2中)，M为优化搜索时可能的路径的网格点数。

利用各次像差的负数

由公式(3)得到测量矩阵

$z_l=\left\{{\tilde{z}}_{\mathrm{ij\θ}}\left(l\right)\right\}---\left(3\right)$

式(3)中，l＝1，2，…，L。

利用当前次波束形成处理和前一次波束形成处理的像差的负数由公式(4)得到测量条件概率：

$\log p\left(z_l\right|x_l)\≈\frac{M^2}{{4\mathrm{\σ}}^2}{\tilde{z}}_{\mathrm{ij\θ}}\left(l\right)---\left(4\right)$

式(4)中σ₂为噪声的均方误差，可通过统计建模得到。

当前次波束形成处理时的AUV的位置估计 ${\stackrel{\‾}{x}}_j=\left\{\begin{array}{c}i\left(l\right)\\ u\\ j\left(l\right)\\ v\\ \mathrm{\θ}\left(l\right)\\ \mathrm{\ω}\end{array}\right\}---\left(5\right)$

式(5)中，i(l)和j(l)为AUV在当前次波束形成处理时的水平面直角坐标位置估计；u和v为AUV在当前次波束形成处理时的水平面直角坐标速度估计；θ(l)为AUV在当前次波束形成处理时的航行的水平面航向角估计；ω为AUV在当前次波束形成处理时的航行的水平面航向角速度估计。利用状态转换矩阵A以及当前和前一次的位置估计，由公式(1)得到AUV在当前一次与当前次位置估计之间的转移概率p(x_l|x_l-1)：

$\left(x_l\right|x_{l-1})=C/({\stackrel{\‾}{x}}_l-A{\stackrel{\‾}{x}}_{l-1})---\left(1\right)$

式(1)中，

和

分别表示当前和前一次的位置估计；C为归一化常数，可选取任意的正数；A表示自主水下机器人在前一次和当前次位置估计之间的转移矩阵；p(x_l|x_l-1)表示自主水下机器人在当前与后一次位置估计之间的转移概率。其中，

$A=\left[\begin{array}{cccccc}1& T& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& T& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right]---\left(6\right)$

式(6)中，T表示当前次波束形成和前一次波束形成的时间间隔。

AUV的各次测量矩阵按时间的先后顺序组成序列Z_l＝{z₁，...z_l}，AUV的状态各次状态按时间的先后顺序组成序列X_l＝{x₁，...x_l}，通过以下式(7)，利用得到的AUV的当前次和前一次位置估计的状态转移概率p(x_l|x_l-1)及AUV的当前次的测量概率p(z_l|x_l)，由公式(7)得到AUV位置估计的后验概率p(X_l|Z_l)：

p(X_l|Z_l)＝p(z_l|x_l)^*p(X_l|Z_l-1)＝p(z_l|x_l)^*p(x_l|x_l-1)^*p(X_l-1|Z_l-1)    (7)

(4)重复步骤(3)得到自主水下机器人所有位置估计的后验概率，按时间的先后顺序排列。

(5)根据自主水下机器人所有位置估计的后验概率得到自主水下机器人的运动轨迹的估计，根据自主水下机器人的运动轨迹的估计得到自主水下机器人的自主水下机器人的实际运动轨迹。

因为选择AUV的轨迹X_l的代价函数满足一阶马尔科夫(Markov)随机过程，也就是说每个代价函数s_l(x_l，x_l-1)只与当前次的状态向量和前一次的状态向量有关，所以由公式(8)得到选择AUV的轨迹X_l的代价函数s(x_l，x_l-1，...x₁)。

s(x_l，x_l-1，...x₁)＝s_l(x_l，x_l-1)+s_l-1(x_l-1，x_l-2)...s₁(x₂，x₁)    (8)

所述AUV的轨迹X_l为从第1次波束形成处理到第l次波束形成处理时的AUV的所有l个位置点。

由公式(9)得到最优化过程s^*(x_l ^*，x_l-1 ^*，...x₁ ^*)：

$s^{*}({x_l}^{*},{x_{l-1}}^{*},...{x_1}^{*})=\underset{x_l}{\max }\left[\underset{x_{l-1}}{\max }\right[s_l(x_l,x_{l-1})+\underset{x_{l-2}}{\max }[s_{l-1}(x_{l-1},x_{l-2})+.....+\underset{x_1}{\max }[s_1(x_2,x_1)\left]\right]\left]\right]---\left(9\right)$

$=\underset{x_l}{\max \left[h_{l-1}\left(x_l\right)\right]}$

式(9)中，s^*表示取s的最大值，而x_l ^*，x_l-1 ^*，...x₁ ^*代表着能使s取最大值时的AUV的运动的轨迹。

由公式(10-1)或(10-2)得到当前次波束形成时的代价函数s_l(x_l，x_l-1)与之前所有代价函数之和的最大值h_l-1(x_l)或h₁(x₂)。

$h_{l-1}\left(x_1\right)=\underset{x_{l-1}}{\max }[h_{l-2}\left(x_{l-1}\right)+s_1(x_l,x_{l-1})]---(10-1)$

$h_1\left(x_2\right)=\underset{x_1}{\max }\left[s_1(x_2,x_1)\right]---(10-2)$

其中，当l大于2时，使用式(10-1)计算得到h_l-1(x_l)；当l等于2时，使用式(10-2)计算得到h₁(x₂)。

所述选择AUV的轨迹X_l的代价函数s(X_l)可以利用AUV位置估计的后验概率p(X_l|Z_l)，由式(11)得到。

s(X_l)＝log[p(X_l|Z_l)]    (11)

最后，动态过程更新方程的最大代价函数h_k-1(X_k)可以由式(12)的得到h_k-1(X_k)的迭代形式。

$h_{k-1}\left(X_k\right)=\log \left(P\left(z_k\right|x_k)\right)+\underset{x_{k-1}}{\max }[\log P\left(x_k\right|x_{k-1})+h_{k-2}\left(X_{k-1}\right)]---\left(12\right)$

通过逐步递推优化即可输出AUV的轨迹的定位结果。

结合图1的流程图，进行了本发明方法的仿真实验分析。图2显示的是利用本发明方法，在AUV做直线运动时，对其路径进行估计的结果。图3显示的是利用本发明方法，在AUV做圆弧运动时，对其路径进行估计结果。仿真实验时AUV的实际运动速度为1.5m/s，速度误差小于0.3m/s，角速度误差小于1°/s，AUV载声纳线阵扫描范围为120度开角，距离20m。图2和图3中三角形表示广义海底特征，声纳检测输出平均信噪比约0dB，曲线1为AUV作直线运动时的实际运动轨迹；曲线2根据速度及角速度值积分所得的AUV作直线运动时的轨迹；曲线3为利用本发明方法对AUV作直线运动时估计得到的运动轨迹，AUV运动时间约60秒；曲线4为AUV作圆弧运动时的实际运动轨迹；曲线5为根据速度及角速度值积分所得的AUV作圆弧运动时的轨迹；曲线6为利用本发明方法对AUV作圆弧运动时估计得到的运动轨迹，AUV运动时间约60秒。从图2和图3中可知，本发明算法估计的轨迹与实际较一致，且基本不受时间累积的影响。

Claims (1)

1.一种自主水下机器人的同时检测与定位方法，其特征是包括如下步骤：

(1)在自主水下机器人下潜之前，利用GPS定位***和航向角测量工具获取自主水下机器人的当前位置信息，所述当前位置信息包括自主水下机器人的水平面直角坐标位置和自主水下机器人的航向角；

(2)在自主水下机器人下潜过程中，利用声纳开始接收声波数据，所述声波数据包括声信号的幅度，对当前声波数据进行波束形成处理以对海底地形地貌成像；

(3)在自主水下机器人下潜后，对声纳所接收的当前声波数据进行波束形成处理以对海底地形地貌成像并在成像时对自主水下机器人进行位置估计；根据当前与前一次海底地形地貌成像的像差得到当前测量条件概率；根据当前和前一次的位置估计利用公式(1)得到自主水下机器人在当前与后一次位置估计之间的转移概率

$p\left(x_l\right|x_{l-1})=C/({\stackrel{\‾}{x}}_l-A{\stackrel{\‾}{x}}_{l-1})---\left(1\right)$

式(1)中，

和

分别表示当前和前一次的位置估计，C为归一化常数，A表示自主水下机器人在前一次和当前次位置估计之间的转移矩阵，p(x_l|x_l-1)表示自主水下机器人在前一次和当前次位置估计之间的转移概率；

根据所述当前测量条件概率和所述自主水下机器人在当前与后一次位置估计之间的转移概率得到自主水下机器人当前位置估计的后验概率；

(4)重复步骤(3)得到自主水下机器人所有位置估计的后验概率；

(5)根据所述自主水下机器人所有位置估计的后验概率得到自主水下机器人的运动轨迹的估计，根据自主水下机器人的运动轨迹的估计得到自主水下机器人的自主水下机器人的实际运动轨迹。

Images