CN110456786B - 基于地形特征提取的uuv海底地形跟踪系统及跟踪方法 - Google Patents

Abstract

本发明提供的是一种基于地形特征提取的UUV海底地形跟踪系统及跟踪方法。海底地形特征提取模块根据三个测距声呐的安装角度及海底距离、姿态传感器测量的纵倾角和艏向角、深度计测量的实际航行深度、惯性导航仪测量的经度和纬度，提取当前所处的海底地形的特征，并解算出距海底的实际高度；高度指令生成模块根据提海底地形特征、多普勒测量的航行速度，结合期望航行高度，生成指令航行高度；高度指令转深度指令模块将指令航行高度转换成指令航行深度；深度控制器根据PID控制算法产生水平舵角控制指令。本发明可以使UUV具备对局部海底地形的在线构建能力和变化趋势的预判能力，在保证航行安全性的前提下，实现对未知海底地形的良好跟踪。

Description

基于地形特征提取的UUV海底地形跟踪系统及跟踪方法

技术领域

本发明涉及的是一种UUV(水下无人航行器，Unmanned Underwater Vehicle)航行运动控制装置及方法。

背景技术

UUV是一种具有自主能力的水下作业工具，在科学和经济等领域都有广泛应用，能够完成海底管线巡检、海底地形地貌勘测、海底失事飞机与船只搜索等任务。在执行这些任务的时候，为了获得良好的搜探效果通常需要UUV采取海底地形跟踪的航行方式。面对崎岖起伏的海底地形，UUV海底地形跟踪的要求是在保证航行安全的前提下，使UUV相对于海底以恒定的高度进行航行。根据对海底地形的了解情况，可以把海底地形跟踪分为已知海底地形跟踪和未知海底地形跟踪。对于已知海底的地形跟踪，通常采用预先规划好可行的航路，然后UUV跟踪航路即可获得良好的地形跟踪效果。然而在实际应用中，海底地形通常是未知的，无法预先规划跟踪航路，那么就需要根据自身配置的探测传感器在线的对海底地形进行探测，获取海底地形的局部信息，同时产生控制运动实现对地形的有效跟踪。然而，由于海底地形崎岖起伏、十分复杂，如何保证UUV不与地形发生碰撞，又能保证UUV以恒定的高度保持对地形的良好跟踪效果，而且能在不同的地形间进行有效的跟踪切换，是非常困难的。本发明利用UUV自身配置的多个单波束测距声呐对海底地形进行联合探测，并根据探测信息提取出海底地形特征信息，然后根据地形特征采取相应的控制方法实现地形跟踪，优点是可以使UUV具备对局部海底地形结构的在线构建能力和变化趋势的预判能力，因而可以安全、有效的实现UUV对多种海底地形的跟踪。

申请号为201410619398.7的专利文件中公开了“一种UUV抵近海底作业的定高航行系统及航行方法”，实现UUV近海底作业时对海底地形兼顾避碰的定高航行。首先，该发明专利的所构建的定高航行系统与本发明所构建的地形跟踪系统不同，特别是单波束测距声呐在UUV上的安装配置与本发明不同，且相关系统模块也不一样。其次，该发明专利对于单波束测距声呐探测信息的利用方式与本发明不同，其主要是利用单波束测距声呐的探测信息产生对海底地形避碰的最近障碍距离，而本发明是利用单波束测距声呐的探测信息提取海底地形的特征。最后，该发明专利实现UUV对海底定高航行的方法与本发明不同，其主要是通过解算出对海底地形的避碰高度实现定高航行，而本发明是通过多个单波束测距声呐对海底地形进行联合探测，提取海底地形特征后，根据地形特征实现海底地形跟踪的定高航行。

申请号为201610312367.6的专利文件中公开了“一种UUV海底地形跟踪过程中的航行切换系统及切换方法”，实现UUV在海底地形跟踪过程中，遇到海底悬崖时可能会发生航行安全问题，采用定高和定深航行切换的方式，保证UUV能够安全的通过悬崖地形。首先，该发明专利解决的问题与本发明不同，其解决的是UUV跟踪海底地形过程中遇到悬崖时的安全切换运动控制问题，而本发明解决的是UUV对各种海底地形的有效跟踪运动控制问题，因此其所构建的航行切换系统与本发明构建的地形跟踪系统也不同。其次，虽然该发明专利提出了UUV判断海底地形是否为悬崖的方法，一定程度上也可以认为是对地形特征的提取，但是其提取和判断方法与本发明完全不同，而且其只是针对海底悬崖地形特征，而本发明可以针对多种海底地形特征。最后，由于该发明瞄准解决的是UUV安全通过海底悬崖的问题，因而其提出的是UUV安全切换运动控制方法，与本发明提出的UUV基于地形特征提取的海底地形跟踪方法不同。

发明内容

本发明的目的在于提供一种能够实现安全、有效的海底地形跟踪航行的基于地形特征提取的UUV海底地形跟踪系统。本发明的目的还在于提供一种利用本发明的基于地形特征提取的UUV海底地形跟踪系统的跟踪方法。

本发明的目的是这样实现的：

本发明的基于地形特征提取的UUV海底地形跟踪系统包括前下单波束测距声呐、正下单波束测距声呐、后下单波束测距声呐、姿态传感器、多普勒测速仪、深度计、惯性导航仪、海底地形特征提取模块、高度指令生成模块、高度指令转深度指令模块、深度控制器和水平舵；

所述的前下单波束测距声呐测量UUV航行方向前下方的海底距离d₁，其声呐波束与海底地形的探测交点为A，将探测到的海底距离d₁传送到海底地形特征提取模块；

所述的正下单波束测距声呐测量UUV航行方向正下方的海底距离d₂，其声呐波束与海底地形的探测交点为B，并将探测到的海底距离d₂传送到海底地形特征提取模块；

所述的后下单波束测距声呐测量UUV航行方向后下方的海底距离d₃，其声呐波束与海底地形的探测交点为C，并将探测到的海底距离d₃传送到海底地形特征提取模块；

所述的姿态传感器测量UUV的纵倾角θ和艏向角ψ，并将纵倾角θ和艏向角ψ信息传送到海底地形特征提取模块；

所述的多普勒测速仪测量UUV的航行速度u，并将航行速度u传送到高度指令生成模块；

所述的深度计测量UUV的实际航行深度z，并将深度z传送到海底地形特征提取模块、高度指令转深度指令模块和深度控制器；

所述的惯性导航仪测量UUV的经度lon及纬度lat，并将经度lon和纬度lat传送到海底地形特征提取模块；

所述的海底地形特征提取模块根据三个单波束测距声呐的安装角度α₁、α₂和α₃，接收到的三个单波束测距声呐探测到的距离信息d₁、d₂、d₃，姿态传感器测量的UUV纵倾角θ、艏向角ψ，深度计测量的UUV实际航行深度z，以及惯性导航仪测量UUV的经度lon、纬度lat，提取UUV当前所处的局部海底地形的特征T，并解算出UUV距海底的实际高度h，把海底地形特征T传送到高度指令生成模块，把实际高度h传送到高度指令转深度指令模块；

所述的高度指令生成模块根据接收到的海底地形特征提取模块提取的海底地形特征T、多普勒测速仪测量的UUV航行速度u，结合期望航行高度h_p，生成指令航行高度h_c；把指令航行高度h_c传送到高度指令转深度指令模块；

所述的高度指令转深度指令模块，根据接收到的深度计测量的UUV实际航行深度z以及海底地形特征提取模块解算的实际高度h，将指令航行高度h_c转换成指令航行深度z_c，并把指令航行深度z_c传送到深度控制器；

所述的深度控制器根据接收到的高度指令转深度指令模块生成的指令航行深度z_c和深度计测量的UUV实际航行深度z，根据PID控制算法产生水平舵角控制指令δ_c，控制水平舵实现UUV的航行深度控制；

水平舵为控制UUV航行深度的执行机构。

本发明的基于地形特征提取的UUV海底地形跟踪系统还可以包括：

1.前下单波束测距声呐、正下单波束测距声呐、后下单波束测距声呐均安装于UUV的腹部且在UUV的纵中剖面位置。

2.前下单波束测距声呐、正下单波束测距声呐、后下单波束测距声呐的安装角度与UUV纵向方向顺时针夹角分别为α₁度、α₂度、α₃度，α₁的范围为0～90度，α₂固定为90度，α₃的范围为90～180度，三个单波束测距声呐的探测距离范围为0～200米。

利用本发明的基于地形特征提取的UUV海底地形跟踪系统的跟踪方法包括以下几个步骤：

步骤一、设置UUV海底地形跟踪的期望航行高度h_p，大地坐标原点经度lon₀和纬度lat₀，任务终点经度lon_E和纬度lat_E；

步骤二、初始化声纳安装角度α₁、α₂和α₃；

步骤三、根据声纳探测信息，提取地形特征T；

步骤四、判断地形是否太陡，影响UUV航行安全，如果是转步骤十二，任务结束；否则转步骤五；

步骤五、解算出UUV距海底的实际高度h；

步骤六、断UUV距海底实际高度h是否太近，影响UUV航行安全，如果是转步骤十二，任务结束；否则转步骤七；

步骤七、根据地形特征T，解算高度偏差Δh；

步骤八、解算指令航行高度h_c；

步骤九、将指令航行高度h_c转换为指令航行深度z_c；

步骤十、深度控制器根据指令航行深度z_c和实际航行深度z得到深度偏差，利用PID控制算法产生水平舵舵角指令δ_c，控制水平舵实现UUV的深度控制；

步骤十一、判断任务是否完成，若完成，转步骤十二，任务结束；否则转步骤三；

步骤一十二、任务结束。

利用本发明的基于地形特征提取的UUV海底地形跟踪系统的跟踪方法可以具体描述为：

步骤一：设置UUV海底地形跟踪的期望航行高度h_p的值，设置大地坐标原点经纬度lon₀和lat₀，设置任务终点经纬度lon_E和lat_E；

步骤二：根据三个声呐在UUV上的实际安装角度，初始化声呐安装角度α₁、α₂和α₃；

步骤三：根据声呐探测信息，提取地形特征T，提取方法为：

1.解算UUV在大地坐标系下的坐标(x,y,z)，解算方法为：

x＝(lon-lon₀)·1852·60，y＝(lat-lat₀)·1852·60，z就是深度计测量出的UUV深度；

2.根据三个单波束测距声呐探测到的距离d₁、d₂、d₃，以及三个单波束测距声呐波束中心与UUV纵向方向顺时针夹角α₁、α₂、α₃，计算出三个单波束测距声呐与海底地形探测交点A、B、C在UUV大地坐标系下的坐标(x_A,y_A,z_A)、(x_B,y_B,z_B)、(x_C,y_C,z_C)，计算方法为：

3.计算出AB、AC、BC三条线段的斜率k_AB、k_AC、k_BC，计算方法为：

4.提取地形特征T，提取方法为：

当k_AB＞k_BC＞0，表示UUV所处的局部海底地形为陡上坡，提取地形特征T＝T1；

当k_AB＞0＞k_BC，表示UUV所处的局部海底地形为凹形，提取地形特征T＝T2；

当0＞k_BC＞k_AB，表示UUV所处的局部海底地形为陡下坡，提取地形特征T＝T3；

当k_BC＞0＞k_AB，表示UUV所处的局部海底地形为凸形，提取地形特征T＝T4；

当k_BC＞k_AB＞0，表示UUV所处的局部海底地形为缓上坡，提取地形特征T＝T5；

当0＞k_AB＞k_BC，表示UUV所处的局部海底地形为缓下坡，提取地形特征T＝T6；

步骤四：判断地形是否太陡，如果是转步骤十二，任务结束；否则转步骤五，判断方法为：当T＝T1时，如果

判定地形是太陡影响UUV航行安全；当T＝T3时，如果

判定地形是太陡影响UUV航行安全；其他地形时不影响UUV航行安全；

步骤五：解算出UUV距海底的实际高度h，解算方法为：

h＝dsin(α+θ)

当

时，d＝d₁，α＝α₁；

当

时，d＝d₃，α＝α₃；

其余情况，d＝d₂，α＝α₂；

步骤六：判断UUV距海底实际高度h是否太近，影响UUV航行安全，如果是转步骤十二，任务结束；否则转步骤七，判断方法为：当h＜2米时，判定UUV距海底实际高度h太近，影响UUV航行安全；

步骤七：根据地形特征T，解算高度偏差Δh，解算方法为：

σ为采样时间，取值为0.5秒；

步骤八：解算指令航行高度h_c，解算方法为：h_c＝h_p+Δh

步骤九：将指令航行高度h_c转换为指令航行深度z_c，转换方法为：

z_c＝h+z-h_c；

步骤十：深度控制器根据指令航行深度z_c和实际航行深度z得到深度偏差，利用PID控制算法产生水平舵舵角指令δ_c，控制水平舵实现UUV的深度控制；

步骤十一：判断任务是否完成，若完成，转步骤十二，任务结束；否则转步骤三，判断方法为如果

则判定任务结束；

步骤十二：任务结束。

本发明涉及UUV对未知海底地形的探测与特征提取，以及根据提取的地形特征控制UUV进行海底地形跟踪的系统及方法。本发明提供了一种能实现UUV对未知海底地形进行探测、提取地形特征，以及对未知海底地形进行跟踪的跟踪系统。本发明还提供了一种能够实现安全、有效的海底地形跟踪航行的，基于基于地形特征提取的UUV海底地形跟踪方法。

本发明的有益效果：

通过地形特征提取，可以使UUV具备对局部海底地形结构的在线构建能力和变化趋势的预判能力，从而可以提高UUV地形跟踪的效果；

针对不同的地形不需要频繁的切换控制指令，可以保证UUV航行安全性的前提下，实现对未知复杂海底地形的良好跟踪；

系统及方法实现简单，计算量小、实时性好，便于工程实践应用。

附图说明

图1UUV海底地形跟踪示意图；

图2UUV的三个单波束测距声呐安装角度示意图；

图3UUV单波束测距声呐探测海底地形示意图；

图4基于地形特征提取的UUV海底地形跟踪系统图；

图5UUV提取的海底地形特征示意图；

图6基于地形特征提取的UUV海底地形跟踪方法流程图；

图7本发明的UUV海底地形跟踪的效果图。

具体实施方式

下面举例对本发明做更详细的描述。

本发明的基于地形特征提取的UUV海底地形跟踪系统包括前下单波束测距声呐、正下单波束测距声呐、后下单波束测距声呐、姿态传感器、多普勒测速仪、深度计、惯性导航仪、海底地形特征提取模块、高度指令生成模块、高度指令转深度指令模块、深度控制器、水平舵。

所述的前下单波束测距声呐用于测量UUV航行方向前下方的海底距离d₁(单位：米)，声呐波束与海底地形的探测交点为A，并将探测到的海底距离信息d₁传送到海底地形特征提取模块。

所述的正下单波束测距声呐用于测量UUV航行方向正下方的海底距离d₂(单位：米)，声呐波束与海底地形的探测交点为B，并将探测到的海底距离信息d₂传送到海底地形特征提取模块。

所述的后下单波束测距声呐用于测量UUV航行方向后下方的海底距离d₃(单位：米)，声呐波束与海底地形的探测交点为C，并将探测到的海底距离信息d₃传送到海底地形特征提取模块。

所述的姿态传感器用于测量UUV的纵倾角θ(单位：度)和艏向角ψ(单位：度)，并将纵倾角θ和艏向角ψ信息传送到海底地形特征提取模块。

所述的多普勒测速仪用于测量UUV的航行速度u(单位：米/秒)，并将航行速度信息u传送到高度指令生成模块。

所述的深度计用于测量UUV的实际航行深度z(单位：米)，并将深度信息z传送到海底地形特征提取模块、高度指令转深度指令模块、深度控制器。

所述的惯性导航仪用于测量UUV的经度lon(单位：度)及纬度lat(单位：度)，并将经纬度信息lon和lat传送到海底地形特征提取模块。

所述的海底地形特征提取模块根据三个单波束测距声呐的安装角度α₁、α₂和α₃，接收到的三个单波束测距声呐探测到的距离信息d₁、d₂、d₃，姿态传感器测量的UUV纵倾角θ、艏向角ψ，深度计测量的UUV实际航行深度z，以及惯性导航仪测量UUV的经度lon、纬度lat，提取UUV当前所处的局部海底地形的特征T，并解算出UUV距海底的实际高度h，把海底地形特征T传送到高度指令生成模块，把实际高度h传送到高度指令转深度指令模块。

所述的高度指令生成模块根据接收到的海底地形特征提取模块提取的海底地形特征T、多普勒测速仪测量的UUV航行速度u，结合期望航行高度h_p(单位：米)，生成指令航行高度h_c(单位：米)；把指令航行高度h_c传送到高度指令转深度指令模块。

所述的高度指令转深度指令模块，根据接收到的深度计测量的UUV实际航行深度z以及海底地形特征提取模块解算的实际高度h，将指令航行高度h_c转换成指令航行深度z_c(单位：米)，并把指令航行深度z_c传送到深度控制器。

所述的深度控制器根据接收到的高度指令转深度指令模块生成的指令航行深度z_c和深度计测量的UUV实际航行深度z，根据PID控制算法产生水平舵角控制指令δ_c(单位：度)，控制水平舵实现UUV的航行深度控制。

水平舵为控制UUV航行深度的执行机构。

结合图1，UUV海底地形跟踪描述如下。

如图1所示，海底地形崎岖起伏，z为UUV相对于海平面的距离，表示UUV的航行深度，h为UUV相对于海底的距离，表示UUV的航行高度，z+h即为海深。UUV海底地形跟踪就是要求UUV在保证航行安全性，不与海底地形碰撞的前提下，使其相对于海底的航行高度h保持恒定，即要求实现h始终等于期望的航行高度h_p。此外，因为UUV的航行深度和航行高度属于同属于垂直面内的运动参数，并且UUV通常是通过深度控制实现在垂直面内的机动，因此可以把UUV海底地形跟踪的保持恒定高度航行转换为深度控制来实现。

结合图2，介绍UUV所配置的用于探测海底地形的三个单波束测距声呐的安装角度。

图2所示为UUV的纵剖面图，图中所示x方向为UUV的纵向方向，z方向为UUV的垂向方向。UUV所配置的用于探测海底地形的前下单波束测距声呐1、正下单波束测距声呐2、后下单波束测距声呐3均安装于UUV的腹部且在UUV的纵中剖面位置。声呐的安装角度是指声呐波束中心线与UUV纵向方向顺时针的夹角。

如图2，前下单波束测距声呐、正下单波束测距声呐、后下单波束测距声呐的安装角度与分别为α₁度、α₂度、α₃度，α₁的范围为(0，90)度，α₂固定为90度，α₃的范围为(90，180)度，三个单波束测距声呐的探测距离范围为[0，200]米。

结合图3，介绍UUV单波束测距声呐对海底地形的探测。

如图3所示，前下单波束测距声呐用于测量UUV航行方向前下方的海底距离d₁，声呐波束与海底地形的探测交点为A。正下单波束测距声呐用于测量UUV航行方向正下方的海底距离d₂，声呐波束与海底地形的探测交点为B。后下单波束测距声呐用于测量UUV航行方向后下方的海底距离d₃，声呐波束与海底地形的探测交点为C。设三个探测交点连接形成的三条线段AB、BC、AC的斜率分别为k_AB、k_BC、k_AC。

结合图4介绍UUV基于地形特征提取的UUV海底地形跟踪系统。

包括前下单波束测距声呐、正下单波束测距声呐、后下单波束测距声呐、姿态传感器、多普勒测速仪、深度计、惯性导航仪、海底地形特征提取模块、高度指令生成模块、高度指令转深度指令模块、深度控制器、水平舵。

姿态传感器用于测量UUV的纵倾角θ和艏向角ψ，多普勒测速仪用于测量UUV的航行速度u，深度计用于测量UUV的实际航行深度z，惯性导航仪用于测量UUV的经度lon及纬度lat。

海底地形特征提取模块根据接收到的三个单波束测距声呐探测到的距离信息d₁、d₂、d₃，姿态传感器测量的UUV纵倾角θ和艏向角ψ，深度计测量的UUV实际航行深度z，惯性导航仪测量UUV的经度lon和纬度lat，提取UUV当前所处的局部海底地形的特征T:

当k_AB＞k_BC＞0，表示UUV所处的局部海底地形为陡上坡，提取地形特征T＝T1；

当k_AB＞0＞k_BC，表示UUV所处的局部海底地形为凹形，提取地形特征T＝T2；

当0＞k_BC＞k_AB，表示UUV所处的局部海底地形为陡下坡，提取地形特征T＝T3；

当k_BC＞0＞k_AB，表示UUV所处的局部海底地形为凸形，提取地形特征T＝T4；

当k_BC＞k_AB＞0，表示UUV所处的局部海底地形为缓上坡，提取地形特征T＝T5；

当0＞k_AB＞k_BC，表示UUV所处的局部海底地形为缓下坡，提取地形特征T＝T6。

AB、BC、AC三条线段的斜率为k_AB、k_BC、k_AC，且有：

式中，

x＝(lon-lon₀)·1852·60；

y＝(lat-lat₀)·1852·60。

同时，海底地形特征提取模块根据三个单波束测距声呐的安装角度α₁、α₂和α₃，三个单波束测距声呐探测到的距离信息d₁、d₂、d₃，姿态传感器测量的UUV纵倾角θ，解算出UUV距海底实际高度h：

h＝dsin(α+θ)

当

时，d＝d₁，α＝α₁；

当

时，d＝d₃，α＝α₃；

其余情况，d＝d₂，α＝α₂。

所述的高度指令生成模块根据接收到的海底地形特征提取模块提取的海底地形特征T、多普勒测速仪测量的UUV航行速度u，结合期望航行高度h_p，生成指令航行高度h_c：

h_c＝h_p+Δh

式中，Δh为高度偏差，且有

σ为采样时间，取值为0.5秒。

高度指令转深度指令模块，根据接收到的深度计测量的UUV实际航行深度z以及海底地形特征提取模块解算的实际高度h，将指令航行高度h_c转换成指令航行深度z_c：

z_c＝h+z-h_c

深度控制器根据接收到的高度指令转深度指令模块生成指令航行深度z_c和深度计测量的UUV实际航行深度z，根据PID控制算法产生水平舵角控制指令δ_c，控制水平舵实现UUV的航行深度控制。

水平舵为控制UUV航行深度的执行机构。

结合图5，介绍UUV提取的海底地形特征。

如图5所示，(a)中k_AB＞k_BC＞0，提取该地形特征为陡上坡。对于该种地形，航行器有与海底地形发生相撞的危险，所以希望航行器快速的越过陡上坡地形。故选择斜率最大的线段AB的斜率产生高度偏差指令，所以Δh＝u·σ·k_AB。

(b)中k_AB＞0＞k_BC，提取该地形特征为凹形。对于该种地形，航行器在跟踪这种地形时容易陷入地形内，影响航行安全。因此对跟踪地形进行平滑处理，以线段AC的斜率产生高度偏差指令，所以Δh＝u·σ·k_AC。

(c)中0＞k_BC＞k_AB，提取该地形特征为陡下坡。对于该种地形，航行器由于惯性和延迟产生的航行超调，有与坡底相撞的风险。所以采用缓下坡的策略，以较平缓的线段BC的斜率产生高度偏差指令，即Δh＝u·σ·k_BC。

(d)中k_BC＞0＞k_AB，提取该地形特征为凸形。对于该种地形，航行器在跟踪这种地形时容易凸起的地形相撞，影响航行安全。因此对跟踪地形进行平滑处理，以线段AC的斜率产生高度偏差指令，所以Δh＝u·σ·k_AC。

(e)中k_BC＞k_AB＞0，提取该地形特征为缓上坡。对于该种地形，希望航行器快速的越过上坡地形。所以，以斜率最大的线段BC的斜率产生高度偏差指令，所以Δh＝u·σ·k_BC。

(f)中0＞k_AB＞k_BC，提取该地形特征缓下坡。对于该种地形，考虑到航行器的航行安全以及跟踪地形的精度，所以采用缓下坡的策略，以较平缓的线段AB的斜率产生高度偏差指令，即Δh＝u·σ·k_AB。

结合图6，UUV的基于单波束声纳提取海底地形特征的地形跟踪方法为：

1、任务开始，设置UUV海底地形跟踪的期望航行高度h_p，大地坐标原点经纬度lon₀和lat₀，任务终点经纬度lon_E和lat_E；

2、初始化声纳安装角度α₁、α₂和α₃；

3、根据声纳探测信息，提取地形特征T；

4、判断地形是否太陡，影响UUV航行安全，如果是转步骤12，任务结束；否则转步骤5；

5、解算出UUV距海底的实际高度h；

6、断UUV距海底实际高度h是否太近，影响UUV航行安全，如果是转步骤12，任务结束；否则转步骤7；

7、根据地形特征T，解算高度偏差Δh；

8、解算指令航行高度h_c；

9、将指令航行高度h_c转换为指令航行深度z_c；

10、深度控制器根据指令航行深度z_c和实际航行深度z得到深度偏差，利用PID控制算法产生水平舵舵角指令δ_c，控制水平舵实现UUV的深度控制；

11、判断任务是否完成，若完成，转步骤12，任务结束；否则转步骤3；

12、任务结束。

结合图7，呈现利用本发明的UUV海底地形跟踪的效果图。

图7中，设最大海深30米，设置如图所示的海底地形。UUV按保持期望航行高度5米进行海底地形跟踪。从图中可以看出，UUV能够在不与起伏的海底地形发生碰撞的前提下，可实现对海底地形的良好跟踪，且与海底的高度基本保持在期望航行高度。

Claims (5)

1.一种基于地形特征提取的UUV海底地形跟踪系统，其特征是：包括前下单波束测距声呐、正下单波束测距声呐、后下单波束测距声呐、姿态传感器、多普勒测速仪、深度计、惯性导航仪、海底地形特征提取模块、高度指令生成模块、高度指令转深度指令模块、深度控制器和水平舵；

所述的前下单波束测距声呐测量UUV航行方向前下方的海底距离d₁，其声呐波束与海底地形的探测交点为A，将探测到的海底距离d₁传送到海底地形特征提取模块；

所述的正下单波束测距声呐测量UUV航行方向正下方的海底距离d₂，其声呐波束与海底地形的探测交点为B，并将探测到的海底距离d₂传送到海底地形特征提取模块；

所述的后下单波束测距声呐测量UUV航行方向后下方的海底距离d₃，其声呐波束与海底地形的探测交点为C，并将探测到的海底距离d₃传送到海底地形特征提取模块；

所述的姿态传感器测量UUV的纵倾角θ和艏向角ψ，并将纵倾角θ和艏向角ψ信息传送到海底地形特征提取模块；

所述的多普勒测速仪测量UUV的航行速度u，并将航行速度u传送到高度指令生成模块；

所述的深度计测量UUV的实际航行深度z，并将深度z传送到海底地形特征提取模块、高度指令转深度指令模块和深度控制器；

所述的惯性导航仪测量UUV的经度lon及纬度lat，并将经度lon和纬度lat传送到海底地形特征提取模块；

所述的海底地形特征提取模块根据三个单波束测距声呐的安装角度α₁、α₂和α₃，接收到的三个单波束测距声呐探测到的距离信息d₁、d₂、d₃，姿态传感器测量的UUV纵倾角θ、艏向角ψ，深度计测量的UUV实际航行深度z，以及惯性导航仪测量UUV的经度lon、纬度lat，提取UUV当前所处的局部海底地形的特征T，并解算出UUV距海底的实际高度h，把海底地形特征T传送到高度指令生成模块，把实际高度h传送到高度指令转深度指令模块；

提取地形特征T，提取方法为：

当k_AB＞k_BC＞0，表示UUV所处的局部海底地形为陡上坡，提取地形特征T＝T1；

当k_AB＞0＞k_BC，表示UUV所处的局部海底地形为凹形，提取地形特征T＝T2；

当0＞k_BC＞k_AB，表示UUV所处的局部海底地形为陡下坡，提取地形特征T＝T3；

当k_BC＞0＞k_AB，表示UUV所处的局部海底地形为凸形，提取地形特征T＝T4；

当k_BC＞k_AB＞0，表示UUV所处的局部海底地形为缓上坡，提取地形特征T＝T5；

当0＞k_AB＞k_BC，表示UUV所处的局部海底地形为缓下坡，提取地形特征T＝T6；

h＝d sin(α+θ)

当

时，d＝d₁，α＝α₁；

当

时，d＝d₃，α＝α₃；

其余情况，d＝d₂，α＝α₂；

所述的高度指令生成模块根据接收到的海底地形特征提取模块提取的海底地形特征T、多普勒测速仪测量的UUV航行速度u，结合期望航行高度h_p，生成指令航行高度h_c；把指令航行高度h_c传送到高度指令转深度指令模块；

所述的高度指令转深度指令模块，根据接收到的深度计测量的UUV实际航行深度z以及海底地形特征提取模块解算的实际高度h，将指令航行高度h_c转换成指令航行深度z_c，并把指令航行深度z_c传送到深度控制器；

h_c＝h_p+Δh

式中，Δh为高度偏差，且有

σ为采样时间，

z_c＝h+z-h_c；

所述的深度控制器根据接收到的高度指令转深度指令模块生成的指令航行深度z_c和深度计测量的UUV实际航行深度z，根据PID控制算法产生水平舵角控制指令δ_c，控制水平舵实现UUV的航行深度控制；

水平舵为控制UUV航行深度的执行机构。

2.根据权利要求1所述的基于地形特征提取的UUV海底地形跟踪系统，其特征是：前下单波束测距声呐、正下单波束测距声呐、后下单波束测距声呐均安装于UUV的腹部且在UUV的纵中剖面位置。

3.根据权利要求1或2所述的基于地形特征提取的UUV海底地形跟踪系统，其特征是：前下单波束测距声呐、正下单波束测距声呐、后下单波束测距声呐的安装角度与UUV纵向方向顺时针夹角分别为α₁度、α₂度、α₃度，α₁的范围为0～90度，α₂固定为90度，α₃的范围为90～180度，三个单波束测距声呐的探测距离范围为0～200米。

4.一种利用权利要求1所述的基于地形特征提取的UUV海底地形跟踪系统的跟踪方法，其特征是：

步骤一、设置UUV海底地形跟踪的期望航行高度h_p，大地坐标原点经度lon₀和纬度lat₀，任务终点经度lon_E和纬度lat_E；

步骤二、初始化声纳安装角度α₁、α₂和α₃；

步骤三、根据声纳探测信息，提取地形特征T；

(1)解算UUV在大地坐标系下的坐标(x,y,z)，解算方法为：

x＝(lon-lon₀)·1852·60，y＝(lat-lat₀)·1852·60，z就是深度计测量出的UUV深度；

(2)根据三个单波束测距声呐探测到的距离d₁、d₂、d₃，以及三个单波束测距声呐波束中心与UUV纵向方向顺时针夹角α₁、α₂、α₃，计算出三个单波束测距声呐与海底地形探测交点A、B、C在UUV大地坐标系下的坐标(x_A,y_A,z_A)、(x_B,y_B,z_B)、(x_C,y_C,z_C)，计算方法为：

(3)计算出AB、AC、BC三条线段的斜率k_AB、k_AC、k_BC，计算方法为：

(4)提取地形特征T，提取方法为：

当k_AB＞k_BC＞0，表示UUV所处的局部海底地形为陡上坡，提取地形特征T＝T1；

当k_AB＞0＞k_BC，表示UUV所处的局部海底地形为凹形，提取地形特征T＝T2；

当0＞k_BC＞k_AB，表示UUV所处的局部海底地形为陡下坡，提取地形特征T＝T3；

当k_BC＞0＞k_AB，表示UUV所处的局部海底地形为凸形，提取地形特征T＝T4；

当k_BC＞k_AB＞0，表示UUV所处的局部海底地形为缓上坡，提取地形特征T＝T5；

当0＞k_AB＞k_BC，表示UUV所处的局部海底地形为缓下坡，提取地形特征T＝T6；步骤四、判断地形是否太陡，影响UUV航行安全，如果是转步骤十二，任务结束；否则转步骤五；

步骤五、解算出UUV距海底的实际高度h；

h＝d sin(α+θ)

当

时，d＝d₁，α＝α₁；

当

时，d＝d₃，α＝α₃；

其余情况，d＝d₂，α＝α₂；步骤六、断UUV距海底实际高度h是否太近，影响UUV航行安全，如果是转步骤十二，任务结束；否则转步骤七；

步骤七、根据地形特征T，解算高度偏差Δh；

σ为采样时间，取值为0.5秒；步骤八、解算指令航行高度h_c；

步骤九、将指令航行高度h_c转换为指令航行深度z_c；

步骤十、深度控制器根据指令航行深度z_c和实际航行深度z得到深度偏差，利用PID控制算法产生水平舵舵角指令δ_c，控制水平舵实现UUV的深度控制；

步骤十一、判断任务是否完成，若完成，转步骤十二，任务结束；否则转步骤三；

步骤一十二、任务结束。

5.根据权利要求4所述的利用基于地形特征提取的UUV海底地形跟踪系统的跟踪方法，其特征是所述的判断地形是否太陡的方法为：当T＝T1时，如果

判定地形是太陡影响UUV航行安全；当T＝T3时，如果

判定地形是太陡影响UUV航行安全；其他地形时不影响UUV航行安全。

Images