CN101900558B - 集成声纳微导航的自主式水下机器人组合导航方法 - Google Patents

Abstract

本发明公开一种集成声纳微导航的自主式水下机器人组合导航方法，主要包括如下步骤：(1)利用全球定位系统获取自主式水下机器人的初始绝对位置信息，利用传感器采集得到自主式水下机器人的速度信息和姿态信息，并利用声纳系统估计自主式水下机器人的速度信息和/或姿态信息；(2)利用数据融合方法将传感器采集到的所述速度信息和姿态信息以及声纳系统估计的速度信息和/或姿态信息进行融合，并结合全球定位系统所获取自主式水下机器人的初始绝对位置信息，得到自主式水下机器人的绝对位置信息和姿态信息。本发明的有益效果是：通过融合AUV载声纳估计的速度和/或姿态信息，以及传感器采集的速度和姿态信息，估计AUV绝对位置，提高AUV的导航精度。

Description

集成声纳微导航的自主式水下机器人组合导航方法

技术领域

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

背景技术

自主式水下机器人是目前海洋工程领域技术发展的热点，在水下环境监测、近海石油工程作业、水下搜索与测绘、以及水雷对抗和实时战区警戒等军事领域获得越来越广泛的应用。导航技术是实现AUV自主航行的关键，而AUV所具有的工作时间长、环境复杂、信息源少、隐蔽性要求高等特点，给稳定、精确的导航实现带来很大的挑战。

近年来国际上对于AUV导航技术的研究一直非常活跃。水下导航技术通常需要在精度、操作距离和自动化程度等之间进行平衡，概括起来主要有以下几种类型：航位推算与惯性导航、声学信标导航、及地球物理导航等。

充分兑现AUV移动性带来的好处，需要获取精确的AUV位置和姿态信息。而在这些位置信息不具备的条件下，可通过自身测量数据之间的关联特性来修正载体运动引起的误差，如形成位移相位中心阵(Displaced Phase-CenterAntenna-DPCA)，利用相邻发射重合相位中心的相关特性进行运动补偿。其处理过程反过来即可用于获取载体位置信息，基于相邻位置声相干处理的微导航技术因此发展起来。

现有的水下机器人通常利用惯性导航系统(Inertial Navigation System，INS)采集的姿态及加速度信息或磁罗经(Magnetic Compass，MC)采集姿态信息，以及多普勒记程仪(Doppler Velocity Log，DVL)采集的速度信息进行组合导航。但是由于上述姿态和速度数据存在一定误差，这些误差会在组合导航的过程中随时间积累，在AUV航行一定距离之后，AUV位置误差将积累到不可接受的程度。而研究结果表明，基于声纳处理的微导航技术对于某些运动参数如横向速度、前向速度可提供比常规惯性导航技术高一到两个数量级的测量精度，而对另一些参数如航偏角的估计又不如常规惯性导航设备精确。因此，将声纳微导航获取的高精度速度测量信息与现有的导航设备记录的速度和姿态信息进行融合，可显著提高AUV整体导航的精度。

发明内容

本发明的目的是提供一种集成声纳微导航的自主式水下机器人组合导航方法，以提高对装载声纳的自主式水下机器人的导航精确度。

为实现上述目的，本发明所采取的技术方案是：该集成声纳微导航的自主式水下机器人组合导航方法主要包括如下步骤：

(1)利用全球定位系统获取自主式水下机器人的初始绝对位置信息，利用传感器采集得到自主式水下机器人的速度信息和姿态信息，并利用声纳系统估计自主式水下机器人的速度信息和/或姿态信息；

(2)利用数据融合方法将传感器采集到的所述速度信息和姿态信息以及声纳系统估计的所述速度信息和/或姿态信息进行融合，并结合全球定位系统所获取自主式水下机器人的初始绝对位置信息，得到自主式水下机器人的绝对位置信息和姿态信息。

进一步地，本发明传感器采集到的所述速度信息为横偏速度信息、前进速度信息、垂直速度信息中的任一种或任几种。

进一步地，本发明传感器采集到的所述姿态信息包括航偏角信息和航偏角速度信息。

进一步地，本发明传感器采集到的所述姿态信息还包括横滚信息和/或纵倾信息。

进一步地，本发明声纳系统估计的所述速度信息包括横偏速度信息。

进一步地，本发明声纳系统估计的所述速度信息还包括前进速度信息和/或垂直速度信息。

进一步地，本发明声纳系统估计的所述姿态信息包括航偏角信息。

进一步地，本发明声纳系统估计的所述姿态信息还包括横滚信息和/或纵倾信息。

进一步地，本发明全球定位系统所获取自主式水下机器人的初始绝对位置信息为经度信息和纬度信息。

与现有技术相比，本发明的有益效果是：通过数据融合方法，融合AUV载声纳估计的速度和/或姿态信息，以及多种传感器采集的速度和姿态信息，估计AUV绝对位置，提高自主式水下机器人的导航精度。

附图说明

图1是本发明组合导航方法的工作示意图。

图2是相位中心近似的几何示意图。

图3是等效相位中心阵示意图。其中，1为发射阵元，2为接收阵，3为经过相位中心等效后的DPCA阵。

图4是利用DPCA估计AUV相邻脉冲间横向位移及航偏角示意图。其中4为某次脉冲发射时刻DPCA阵，5为相邻次脉冲发射时刻DPCA阵，6表示相邻脉冲间形成的重合相位中心对。

图5(a)是存在四对重合相位中心对时，利用DPCA微导航估计AUV航行时的横偏位移的仿真结果。其中星号线7表示利用DPCA估计出的AUV横偏位移，实线8表示AUV真实横偏位移。

图5(b)是存在四对重合相位中心对时，利用DPCA微导航估计AUV航行时的航偏角的仿真结果。其中星号线9表示利用DPCA估计出的AUV航偏角，实线10表示AUV真实航偏角。

图6是利用图5中的横偏位移和航偏角推算AUV轨迹仿真结果

其中，a：DPCA估计值与实际值比较，星号线11表示利用DPCA估计出的AUV运动轨迹，实线12表示AUV真实运动轨迹；b：AUV的估计轨迹相对于真实轨迹的误差随AUV前距离的变化曲线(实线13)。

图7是本发明中AUV的一种载声纳系统重合等效相位中心示意图(空心圆与实心圆分别表示相邻脉冲的等效相位中心)。

图8是DPCA+IGC+partial DVL组合导航轨迹推算与DVL+IGC轨迹推算的比较，实验数据处理结果。其中虚线14表示没有结合微导航技术(DVL+IGC)的AUV绝对位置估计结果，虚线15表示结合了微导航技术后(DPCA+IGC+partialDVL)AUV绝对位置估计结果，点16表示AUV上浮时刻其上安装的GPS记录位置。

图9是DPCA+IGC+partial DVL组合导航轨迹推算与DVL+IGC轨迹推算的比较，实验数据处理结果(不同批次的数据)。其中虚线17表示没有结合微导航技术(DVL+IGC)的AUV绝对位置估计结果，虚线18表示结合了微导航技术后(DPCA+IGC+partial DVL)AUV绝对位置估计结果，点19表示AUV上浮时刻其上安装的GPS记录的AUV绝对位置，星号线20表示AUV下潜之前其上安装的GPS记录的AUV轨迹。

具体实施方式

本发明的组合导航方法的工作过程如图1所示。导航数据来源于三种途径，包括：(1)利用声纳接收数据，通过DPCA方法估计得到的AUV速度及姿态角信息；(2)INS记录的姿态信息，或MC记录的姿态信息；(3)DVL记录的AUV速度信息。利用数据融合方法对上述三类信息进行融合，得到水下AUV组合导航运动轨迹，结合AUV下潜之前水面GPS接收机位置信息，可最终估计AUV任一时刻的水下绝对位置。

声纳微导航的工作原理如下：如图2所示，采用相位中心近似(Phase CenterApproximation，PCA)，将声纳基阵每个发射阵元T点和接收阵元R点声路径的时延用位于它们几何中心(相位中心)C点的双程传播近似，即由该声纳发射阵元发射经水底反射/散射后接收阵元接收信号的过程可以等价近似为发射阵和接收阵都位于它们相位中心时的情况。在AUV航行过程中，其上搭载的侧视声纳向水底周期性发射脉冲声信号。每个发射脉冲对应构成发射信号时刻AUV空间位置处的PCA相位中心阵。如图3所示，设阵元数为8，应用PCA就可以将由一个发射阵元和8个接收阵元组成的均匀线阵，等价近似为8个PCA组成的均匀线阵。通过设定AUV航行速度或调整声纳发射的发射周期，就可实现相邻发射脉冲周期内相位中心阵的部分重叠。图4中相邻脉冲间存在4对重合相位中心对。设相邻脉冲间AUV横向位移为γ，航偏角为θ，则在相邻脉冲间AUV航偏角变化很小的条件下，图4中各重合相位中心的相对位移为

d_i＝γ+θx_i，i＝1，2，3，4                        (1)

其中PCA阵的阵元横坐标x_i为

$x_i=(i-1-\frac{N-1}{2})\frac{\mathrm{\Δ}}{2},---\left(2\right)$

式中Δ为接收阵阵元间距，N为阵元个数。由于DPCA阵元接收的信号相干性强，因此可通过相关器估计相邻脉冲间重合相位中心对接收信号的时延差τ_i。根据几何关系，该时延差与该重合相位中心对的相对位移之间满足

${\mathrm{\τ}}_i=\frac{{2d}_i}{c}.---\left(3\right)$

式(3)与式(1)相结合，当相邻脉冲间存在至少一对重合相位中心对时，即可估计AUV相邻脉冲间的横向位移γ；当相邻脉冲间存在至少两对重合相位中心对时，即可估计AUV相邻脉冲间的横向位移γ和航偏角θ。横向位移γ除以脉冲间隔即为AUV在此脉冲间隔内平均速度。当相邻脉冲间存在至少两对重合相位中心对时，还可以利用水平方向波束形成的方法将所有重合相位中心对变成一对虚拟的重合相位中心对，进而获得精度较单对重合相位中心估计结果更高的AUV横向速度。对重合相位中心经插值处理，可以估计AUV在航向上的前进速度。如果采用多排水平线阵进行垂直维更多相位中心的重叠，还可实现对AUV垂直速度、纵倾角和横滚角的估计。

本发明集成声纳微导航的自主式水下机器人组合导航方法具体如下：

(1)AUV下潜之前，通过其上安装的全球定位系统(GPS)获取AUV的初始绝对位置。全球定位系统所获取自主式水下机器人的初始绝对位置信息一般为经度信息和纬度信息。

(2)在AUV航行过程中，AUV上安装的传感器采集AUV的速度和姿态信息。例如可使用惯性导航系统或多普勒记程仪采集AUV的速度信息，使用磁罗经采集AUV的姿态信息。

(3)利用声纳系统估计自主式水下机器人的速度信息和/或姿态信息。

如果声纳系统仅估计自主式水下机器人的速度信息，可采用数据融合方法融合将该速度信息与传感器所采集的AUV的速度和姿态信息，并结合全球定位系统所获取自主式水下机器人的初始绝对位置信息，得到自主式水下机器人的绝对位置信息和姿态信息。

如果声纳系统仅估计自主式水下机器人的姿态信息，可采用数据融合方法融合将该姿态信息与传感器所采集的AUV的速度和姿态信息，并结合全球定位系统所获取自主式水下机器人的初始绝对位置信息，得到自主式水下机器人的绝对位置信息和姿态信息。

如果声纳系统同时估计自主式水下机器人的速度信息和姿态信息，则采用数据融合方法融合将该速度信息和姿态信息与传感器所采集的AUV的速度和姿态信息，并结合全球定位系统所获取自主式水下机器人的初始绝对位置信息，得到自主式水下机器人的绝对位置信息和姿态信息。此方案比声纳系统仅估计自主式水下机器人的姿态信息或速度信息能够得到更为精确的AUV的绝对位置信息和姿态信息。

在本发明中，传感器采集到的速度信息为横偏速度信息、前进速度信息、垂直速度信息中的任一种或任几种。横偏速度信息、前进速度信息、垂直速度信息分别是三维空间中三个正交方向的速度信息。

若传感器采集到的姿态信息为AUV的航偏角信息和航偏角速度信息，则可用于进行AUV的二维空间的导航。若进一步采集AUV的横滚信息和/或纵倾信息，则可用于实现AUV的三维空间的导航，并有更佳的导航精度。

若声纳系统估计到AUV的横偏速度信息，即可用于进行AUV的二维空间的导航。若声纳系统估计的速度信息还包括前进速度信息，则可提高AUV的导航精度。若声纳系统估计的速度信息还包括垂直速度信息，则可用于实现AUV的三维空间的导航，并有更佳的导航精度。

若声纳系统估计的姿态信息为航偏角信息，即可用于进行AUV的二维空间的导航。若声纳系统估计的姿态信息还包括横滚信息和/或纵倾信息，则可用于实现AUV的三维空间的导航，并有更佳的导航精度。

在本发明方法，一般所融合的速度信息和姿态信息越多，则AUV的导航精度越高。

以下举例说明本发明所采用的数据融合方法。

采用数据融合方法融合声纳系统和AUV载传感器获取的AUV速度和/或姿态信息，得到AUV在水下运动轨迹，结合AUV上安装的GPS记录的下潜时AUV初始绝对位置，得到AUV在水下的绝对位置信息和姿态信息。由于AUV在水下通常定深航行，且深度维信息由压力传感器获取，而高度信息可通过DVL获得，下文仅给出水平面内两维组合导航例子。

AUV的状态向量如公式(4)所示

x(n)＝[p_x(n) p_y(n) θ(n) w(n) v_f(n) v_l(n)]^T           (4)

其中p_x(n)和p_y(n)分别是在采样点n时刻AUV在x和y方向的水平位移。v_f(n)和v_l(n)分别是AUV在采样点n时刻的真实前进速度和横向速度。θ(n)是AUV在n时刻航偏角(与x轴的夹角)；ω(n)是n时刻θ(n)变化的角速度。

AUV的观测向量如公式(5)所示

y(n)＝[v_f(n) v_l(n) θ(n) w(n)]^T                       (5)

其中前进速度v_f(n)可由多普勒计程仪或者声纳系统提供，横向速度v_l(n)可以由多普勒计程仪或者声纳系统提供。航偏角θ(n)可由惯性测量单元、声纳系统或磁罗经提供，航偏角速度ω(n)由惯性测量单元提供。

根据公式(4)和(5)，可以得到如公式(6)所示的状态更新方程

以及如公式(7)所示的观测方程

$y\left(n\right)=\left[\begin{array}{cccccc}0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\end{array}\right]x\left(n\right)+v---\left(7\right)$

其中Δt为速度采样间隔，w和v分别为状态更新噪声和观测噪声。

在完成状态向量、观测向量、状态更新方程和观测方程定义之后，即可以利用卡尔曼(Kalman)滤波器估计AUV的运动轨迹，结合AUV上安装的GPS记录的AUV下潜时刻绝对位置，就可以得到AUV在水下的绝对位置。

在该例子中只考虑水平面内两维组合导航，本发明可以实现AUV的三维导航。但该例子中的状态向量、观测向量、状态更新方程和观测方程定义因视实际应用而做相应改变。

本发明可利用目前组合导航中应用最普遍的卡尔曼滤波器，一方面因为它数值计算上的优越性，另一方面因为其具有相对简单和稳健的特性。其他数据融合方法如例子滤波器、遗传算法等也可以作为本导航技术中的数据融合方法。

实施例

设AUV沿直线航行，速度为1m/s。AUV上装有一排水平接收阵，阵元数为8，阵元间距为0.1m。在航行过程中向海底发射声信号并接收回波，信号(ping)间隔为0.1s，则相邻脉冲间有6个重合的等效相位中心。根据相邻脉冲之间的相关性利用DPCA算法估计AUV横向速度结果如图5所示，可以看出DPCA能够准确地估计出航行体的横向速度。利用AUV的前进速度和DPCA估计出的横向速度进行轨迹推算，结果如图6所示，在400个脉冲时间内，AUV运动了40米，而估计误差在厘米级别，仿真环境较理想，下面给出实际实施例。

现场实验中，AUV载声纳系统为多线阵，由三排水听器组成，从上到下每排的水听器数分别是6-4-6，间距为0.1m。根据DPCA导航理论，形成的等效相位中心间距为0.05m。在试验中AUV水下速度约为1.5m/s，脉冲间隔为0.1s，则相邻脉冲间可以有7个重合等效相位中心，如图7所示。利用上述7个重合等效相位中心可以估计相邻脉冲间AUV的横向位移，除以脉冲间隔就可以得到横向速度。利用DPCA导航估计得到的AUV横向速度，结合由智能罗经仪(Intelligent Gyro Compass，IGC)测量得到的航偏角数据以及DVL的前进速度，可推算出AUV的运动轨迹。实验中，AUV水下航行速度为1.5m/s，定深15m航行，航行区域水深大于25m。图8和图9给出了利用本发明集成声纳微导航的自主式水下机器人组合导航方法进行AUV导航的两批结果，并同没有融合声纳微导航的AUV导航结果做比较。其中虚线15、18表示横偏速度由声纳系统提供，前进速度由DVL提供，航偏角和航偏角速度由IGC提供，数据融合方法为卡尔曼滤波时的AUV导航结果(DPCA+IGC+partial DVL)；而虚线14、17表示前进速度及横向速度都由DVL提供，航偏角和航偏角速度由IGC提供，数据融合方法为卡尔曼滤波时的AUV导航结果(DVL+IGC)。星号线20为GPS接收机记录的位置信息(AUV处于水面)。图8中AUV从(50m，0m)附近下潜，图9中AUV从(10m，250m)附近下潜，两图中路径中间部分处于水下，因此没有GPS位置信息。从图中明显可以看出，采用DPCA+IGC+partial DVL估计出的AUV轨迹比DVL+IGC估计出的AUV轨迹在AUV上浮时刻的绝对位置更接近其上的GPS获取的绝对位置(DVL长时精度±0.4％±0.2cm/s)。上述实施例表明利用本发明集成声纳微导航的自主式水下机器人组合导航方法可显著提高组合导航系统精度。

Claims (9)

1.一种集成声纳微导航的自主式水下机器人组合导航方法，其特征是包括如下步骤：

（1）利用全球定位系统获取自主式水下机器人的初始绝对位置信息，利用传感器采集得到自主式水下机器人的速度信息和姿态信息，并利用声纳系统采用如下相位中心近似方法估计自主式水下机器人的速度信息和/或姿态信息：

a)在自主式水下机器人航行过程中，由其上搭载的侧视声纳向水底周期性发射脉冲声信号，每个发射脉冲对应构成发射信号时刻自主式水下机器人的空间位置处的相位中心阵，并通过设定自主式水下机器人的航行速度或调整声纳发射的发射周期，使相邻发射脉冲周期内的相位中心阵部分重合；

b）利用以下公式（1）-（3）估计自主式水下机器人的速度信息和/或姿态信息：

d_i=γ+θx_i,i=1,2,3,4             （1）

$x_i=(i-1-\frac{N-1}{2})\frac{\mathrm{\Δ}}{2},---\left(2\right)$

${\mathrm{\τ}}_i=\frac{2d_i}{c}.---\left(3\right)$

式（1）-（3）中，d_i表示各重合相位中心的相对位移，γ表示自主式水下机器人相邻发射脉冲间的横向位移，θ表示自主式水下机器人相邻发射脉冲间的航偏角，x_i表示相位中心阵的阵元横坐标，Δ表示接收阵阵元的间距，N表示阵元个数，τ_i表示相邻脉冲间重合相位中心对接收信号的时延差，c表示声速；

并且，当相邻发射脉冲间存在至少一对重合相位中心对时，能够估计得到自主式水下机器人相邻脉冲间的横向位移γ；当相邻发射脉冲间存在至少两对重合相位中心对时，能够估计得到自主式水下机器人相邻发射脉冲间的横向位移γ和航偏角θ；将横向位移γ除以脉冲间隔能够得到自主式水下机器人在此脉冲间隔内的平均速度；当相邻发射脉冲间存在至少两对重合相位中心对时，通过利用水平方向波束形成的方法将所有重合相位中心对变成一对虚拟的重合相位中心对，能够获得自主式水下机器人横向速度；对重合相位中心经插值处理，能够估计得到自主式水下机器人在航向上的前进速度；通过采用多排水平线阵进行垂直维更多相位中心的重叠，能够对自主式水下机器人的垂直速度、纵倾角和横滚角进行估计；

（2）利用数据融合方法将传感器采集到的所述速度信息和姿态信息以及声纳系统估计的所述速度信息和/或姿态信息进行融合，并结合全球定位系统所获取自主式水下机器人的初始绝对位置信息，得到自主式水下机器人的绝对位置信息和姿态信息。

2.根据权利要求1所述的集成声纳微导航的自主式水下机器人组合导航方法，其特征是：传感器采集到的所述速度信息为横偏速度信息、前进速度信息、垂直速度信息中的任一种或任几种。

3.根据权利要求1所述的集成声纳微导航的自主式水下机器人组合导航方法，其特征是：传感器采集到的所述姿态信息包括航偏角信息和航偏角速度信息。

4.根据权利要求3所述的集成声纳微导航的自主式水下机器人组合导航方法，其特征是：传感器采集到的所述姿态信息还包括横滚信息和/或纵倾信息。

5.根据权利要求1所述的集成声纳微导航的自主式水下机器人组合导航方法，其特征是：声纳系统估计的所述速度信息包括横偏速度信息。

6.根据权利要求5所述的集成声纳微导航的自主式水下机器人组合导航方法，其特征是：声纳系统估计的所述速度信息还包括前进速度信息和/或垂直速度信息。

7.根据权利要求1所述的集成声纳微导航的自主式水下机器人组合导航方法，其特征是：声纳系统估计的所述姿态信息包括航偏角信息。

8.根据权利要求7所述的集成声纳微导航的自主式水下机器人组合导航方法，其特征是：声纳系统估计的所述姿态信息还包括横滚信息和/或纵倾信息。

9.根据权利要求1所述的集成声纳微导航的自主式水下机器人组合导航方法，其特征是：全球定位系统所获取自主式水下机器人的初始绝对位置信息为经度信息和纬度信息。

Images