CN102231082B - 基于迷你声纳的水下目标探测与auv自动避碰方法及其系统 - Google Patents

Abstract

本发明涉及一种基于迷你声纳的水下目标探测与AUV自动避碰系统，包括迷你声纳、声纳计算机和主控计算机；所述迷你声纳和声纳计算机之间通过串口RS485通讯，所述声纳计算机与主控计算机之间通过网络连接，所述主控计算机通过串口RS232分别连接光纤陀螺、多普勒测速仪、高度计、倾角传感器以及磁罗经，主控计算机同时通过DA通道连接推进器和舵翼。本发明同时提供了一种基于迷你声纳的水下目标探测与AUV自动避碰方法。本发明使用迷你声纳检测水下目标，确定障碍物位置，给出避碰指令，使AUV做出正确的避碰决策。

Description

基于迷你声纳的水下目标探测与AUV自动避碰方法及其系统

技术领域

本发明涉及水下目标的声探测系统以及AUV自动避碰技术，具体涉及一种使用迷你声纳检测水下目标、确定障碍物位置、给出避碰指令，实现AUV自动避碰的方法以及实现该方法的系统。

背景技术

由于在军用和民用两方面都具有极为广阔的应用前景，作为水下运载平台和作业工具的小型水下机器人技术得到了迅猛的发展。许多国家尤其是发达国家都致力于相关的技术研究和产品开发。作为完全自治的水下机器人，探测系统就是它的眼睛和耳目，具有极其重要的地位和作用。通过探测系统，机器人能够快速获取水下周围环境信息，为其运动和水下作业提供引导。而在特殊的水下环境下，声波是迄今为止唯一可以进行远程信息传输的载体。这就决定了小型水下机器人装备的探测系统必须以声纳为主。

声纳安装在AUV前部，实时地获取障碍物的位置信息，为AUV避碰提供前提条件。由于存在环境干扰，声纳常常会提供伪信息。这种伪信息的存在会导致AUV的误判，使AUV不能向正确的方向前进。这就需要通过对噪声的滤除提高障碍物探测的准确度。同时由于障碍物的多样性和未知性，基于传感器信息，采用怎样的避碰算法如何进行避碰规避，确保AUV能够及时快速地对障碍物做出反应，也是决定系统性能水平的一个重点。

发明内容

本发明进行了声探测方面的研究，其的目的在于提供一种基于迷你声纳的水下目标探测与AUV自动避碰方法及其系统。实现障碍物定位置，使AUV做出正确的避碰决策。

本发明的技术方案如下：

一种基于迷你声纳的水下目标探测与AUV自动避碰系统，包括迷你声纳、声纳计算机和主控计算机；所述迷你声纳和声纳计算机之间通过串口RS485通讯，所述声纳计算机与主控计算机之间通过网络连接，所述主控计算机通过串口RS232分别连接光纤陀螺、多普勒测速仪、高度计、倾角传感器以及磁罗经，主控计算机同时通过DA通道连接推进器和舵翼。

本发明同时提供了一种基于迷你声纳的水下目标探测与AUV自动避碰方法，包括如下步骤：

步骤1）：光纤陀螺、多普勒测速仪、高度计、倾角传感器及磁罗经等传感器采集AUV的状态信息，通过串口传给主控计算机，主控计算机进行导航信息处理；

步骤2）：迷你声纳发射波束并接收反射回波，并对反射回波进行数字化，定时向声纳计算机发送串行通讯数据包；

步骤3）：声纳计算机接收到声纳数据后根据协议格式进行解码，提取回波数据，通过自动检测算法生成水下目标的方位距离与特征信息，通过网络传给主控计算机；

步骤4）：主控计算机根据步骤1）获取的AUV当前状态和步骤3）获取的水下目标所在的位置，通过自动避碰算法生成AUV的航向、速度和深度指令；

步骤5）：执行机构按控制指令调整AUV的航向、速度和深度，高速时采用舵翼控制航向，低速时采用推进器控制航向，实现AUV避碰。

其进一步的技术方案为：所述步骤3）所采用的水下目标自动检测算法包括如下步骤：

1）波束内插

通过相邻波束的输出幅度内插出中间多个波束的输出值，采用两点间内插四点的六点内插公式：

$\left\{\begin{array}{c}P\left(x_{-\frac{4}{5}}\right)=({18y}_{-1}+9y_0-2y_1)/25\\ P\left(x_{-\frac{3}{5}}\right)=({12y}_{-1}+16y_0-3y_1)/25\\ P\left(x_{-\frac{2}{5}}\right)=({7y}_{-1}+21y_0-3y_1)/25\\ P\left(x_{-\frac{1}{5}}\right)=({3y}_{-1}+24y_0-2y_1)/25\\ P\left(x_{\frac{1}{5}}\right)=(-2y_{-1}+24y_0+3y_1)/25\\ P\left(x_{\frac{2}{5}}\right)=(-3y_{-1}+21y_0+7y_1)/25\\ P\left(x_{\frac{3}{5}}\right)=(-3y_{-1}+16y_0+12y_1)/25\\ P\left(x_{\frac{4}{5}}\right)=(-2y_{-1}+9y_0+18y_1)/25\end{array}\right.$

2）坐标变换

当声波束触发到来时，锁定方位角度θ，距离计数r由零依次增加，回波数据在显示屏上地址(x，y)由下式决定：

$\left\{\begin{array}{c}x=r\sin \mathrm{\θ}\\ y=r\cos \mathrm{\θ}\end{array}\right.$

在坐标变换时，通过映射预先存储于地址表内的rsinθ和rcosθ的值，直接查找出计算值；

3）伪彩色图像增强

用伪彩色代替灰度，采用调色板索引，突出图像中的细节，提高声呐图像分辨能力；

4）中值滤波

对每个3×3窗口内的像素进行排序以求中值：首先将像素(I，j)周围3*3方形窗口的像素值读入数组array[9]；然后用冒泡法对array进行顺序，array[4]为该邻域中值；最后用array[4]代替f(I，j)；

5）两层图像分割

首先通过高阈值t₂选择目标的亮区，然后选择高于低阈值t₁和与亮区相邻的次亮区，亮区和次亮区整个区域即为目标区域；应选择t₂使每个目标有一些像素灰度级高于t₂，而背景不含有这样的像素；同时，应选择t₁使每个目标像素点具有高于t₁的灰度级；经过分割模块处理后，声纳图像变成以0、1表示的二值图像；

6）图像缺损的拟合

若f(x，y)是目标点，而f(x，y+1)是背景点，则u₁＝y+1；

若f(x，y)是背景点，而f(x，y+1)是目标点，则u2＝y+1；

若0<u₂-u₁<30，则f(x，i)置为目标点，其中u₁≤i≤u₂；

7）图像的细化

以上、下、左、右的顺序反复迭代消除相应方向上的简单边界点，直至形成弧和曲线。

8）边界跟踪

将二值图像区域点的灰度值置为1，背景点灰度值置为0；以c表示前处理的点，以b表示当前点c的一个背景邻点；若某一点p的灰度值为t，则表示为p＝t；

从边界的任一点开始，此点如第一个当前点c，将其灰度值置为3，作为跟踪结束的标志，且将c点坐标放入表列中记录下来；

找到当前点c的一个背景邻点b，将其灰度值从0变为2；

从b点开始沿顺时针方向访问当前点c的8个邻点，直到找到灰度值为1或3或4的点，称这些点为d1，d2，…，dk。此时有两种情况：若dk=4，c=3，如果c的一个邻近点dn=2，n<k，则将dn灰度值变为0，c点灰度值变为4，然后终止算法；否则，记录此点坐标，若c=1，则将其灰度值变为4；然后令dk为新的当前点c，dk-1为其背景邻点b；

返回3；

9）目标位置估计

以形心作为目标位置进行输出；由于二值化的结果，目标图像R以内的信号幅度为“1”，目标图像R以外的信号幅度为“0”；把形心解算式改写成：

$\left\{\begin{array}{c}\stackrel{\&OverBar;}{x}=(1/M){\&Integral;}_c^d{\&Integral;}_a^{b}V(x,y)\mathrm{xdxdy}\\ \stackrel{\&OverBar;}{y}=(1/M){\&Integral;}_c^d{\&Integral;}_a^{b}V(x,y)\mathrm{ydxdy}\\ M={\&Integral;}_c^d{\&Integral;}_a^{b}V(x,y)\mathrm{xdxdy}\end{array}\right.$

其中，当(x，y)属于R区域内时V(x，y)＝1；当(x，y)不属于R区域时V(x，y)＝0；a，b，c，d为跟踪窗口边界坐标。

所述步骤4）所采用的AUV自动避碰算法基于人工势场避碰模型，具体步骤如下：

构造目标位姿引力场和障碍物周围斥力场共同作用的人工势力场，搜索势函数的下降方向来寻找无碰撞路径；

斥力为斥力函数的负梯度，即：

$F_{\mathrm{re}}\left(x\right)=-\mathrm{grad}\left[U_{\mathrm{re}}\left(x\right)\right]=\left\{\begin{array}{cc}\mathrm{\&eta;}(\frac{1}{\mathrm{\&rho;}}-\frac{1}{{\mathrm{\&rho;}}_0})\frac{1}{{\mathrm{\&rho;}}^2}\frac{\&PartialD;\mathrm{\&rho;}}{\&PartialD;x}& \mathrm{\&rho;}\&le;{\mathrm{\&rho;}}_0\\ 0& \mathrm{\&rho;}>{\mathrm{\&rho;}}_0\end{array}\right.$

其中η为位置增益函数，ρ为AUV与障碍物之间的最短距离，ρ₀代表障碍物的影像距离；

引力为目标势场的负梯度，即：

F_at(x)=-grad[U_at(x)]=h(x-x_goal)

当AUV到达目标x_goal的过程中，这个力收敛于0，斥力与引力的合力为：

F=F_re+F_at

AUV下一步的运动方向由合力的方向决定，定义AUV当前的位置(xk,yk)，则根据目标点位置和障碍物信息得到AUV下一步的路径点坐标(xk+1，yk+1)为：

$\left[\begin{array}{c}{x}_{k+1}\\ y_{k+1}\end{array}\right]=\left[\begin{array}{c}{x}_k\cos (\&PartialD;\stackrel{\&RightArrow;}{n})\\ y_k\sin (\&PartialD;\stackrel{\&RightArrow;}{n})\end{array}\right]\left[\begin{array}{c}{\mathrm{\&lambda;}}_1\\ {\mathrm{\&lambda;}}_2\end{array}\right]$

其中λ₁、λ₂为常量，是AUV每次移动的绝对唯一参数，求得路径点的地球坐标，转化为AUV的航向、速度、深度，反馈给AUV运动模型。

本发明的有益技术效果是：

本发明使用迷你声纳检测水下目标、确定障碍物位置、给出避碰指令，实现AUV自动避碰。在水下环境不是十分复杂的条件下，本发明可以取得良好的测试效果，并能在短时间内给出物体的径向距离和方位角度的信息。本发明采用自适应算法，从判断到输出结果都为自主完成，不需要人为干预。

附图说明

图1是本发明的系统组成图。

图2是伪彩色增强的声纳图像。

图3是二层图像分割示意图（图中黑色区域表示选择区域）。

图4是图像细化流程图。

图5是经二值化处理后的目标图像。

具体实施方式

下面结合附图对本发明的具体实施方式做进一步说明。

如图1所示，本发明的水下目标探测与AUV自动避碰系统包括迷你声纳1、声纳计算机2、主控计算机3。迷你声纳1和声纳计算机2之间通过串口RS485通讯，迷你声纳1上电后持续发送回波数据到声纳计算机2，声纳计算机2装载声纳处理程序，同时可以控制迷你声纳1。声纳计算机2与主控计算机3之间通过以太网络连接，声纳计算机2将处理后的目标位置信息发送给主控计算机3。主控计算机3通过串口RS232分别连接光纤陀螺4、多普勒测速仪5、高度计6、倾角传感器7、磁罗经8等导航传感器，并通过DA通道驱动推进器9和舵翼10。

综合考虑微小型水下机器人的实际需求和安装限制，本实施例采用英国Tritech公司的Micron DST声纳作为水声探测设备进行了声探测方面的研究。与其它类型声纳相比，Micron DST声纳的突出优点是高性能、低价格、重量轻、体积小、低能耗和拆卸方便，非常适于微小型水下机器人上使用。Micron DST声纳系统的技术参数如下：

探测跟踪区域：以声纳为起点，扫描距离的1/5处到4/5。

目标点的大小：至少有一个形状参数>=2米。

方位判断误差：探测距离误差<=1m，探测角度误差<=4.5度。

本发明基于上述系统的水下目标探测与AUV自动避碰方法为：

1）光纤陀螺4、多普勒测速仪5、高度计6、倾角传感器7、磁罗经8等传感器采集AUV的状态信息，通过串口传给主控计算机3，主控计算机3进行导航信息处理；

2）Micron DST声呐1发射波束并接收反射回波；反射回波通过声纳接收后进行数字化，定时向声纳计算机2发送串行通讯数据包；

3）声纳计算机2接收到声纳数据后根据协议格式进行解码，提取回波数据，通过目标检测算法生成目标点的方位距离与特征信息，传给主控计算机3；

4）主控计算机3根据AUV当前状态和障碍物所在的位置，通过避碰算法计算出AUV的航向、速度和深度指令；

5）由基础运动控制模块通过运动控制解算，利用控制电压驱动执行机构，按控制指令调整AUV的航向、速度和深度，高速时采用舵翼10控制航向，低速时采用推进器9控制航向，实现AUV避碰。

上述步骤3）所采用的水下目标自动检测算法包括如下步骤：

1、波束内插

由于声呐系统中相邻多个波束间有一定的覆盖宽度，因此当目标方位对准某一波束时，相邻波束仍有一定的输出。在这种情况下，通过相邻波束的输出幅度就可以内插出中间多个波束的输出值。该算法易于软件实现，对硬件的额外要求很低，而其结果却可以改善由于基阵的波束过少造成的角度分辨率不够的问题，改善图像显示，提高目标的定位精度。

本实施例采用两点间内插四点的六点内插公式：

$\left\{\begin{array}{c}P\left(x_{-\frac{4}{5}}\right)=({18y}_{-1}+9y_0-2y_1)/25\\ P\left(x_{-\frac{3}{5}}\right)=({12y}_{-1}+16y_0-3y_1)/25\\ P\left(x_{-\frac{2}{5}}\right)=({7y}_{-1}+21y_0-3y_1)/25\\ P\left(x_{-\frac{1}{5}}\right)=({3y}_{-1}+24y_0-2y_1)/25\\ P\left(x_{\frac{1}{5}}\right)=(-2y_{-1}+24y_0+3y_1)/25\\ P\left(x_{\frac{2}{5}}\right)=(-3y_{-1}+21y_0+7y_1)/25\\ P\left(x_{\frac{3}{5}}\right)=(-3y_{-1}+16y_0+12y_1)/25\\ P\left(x_{\frac{4}{5}}\right)=(-2y_{-1}+9y_0+18y_1)/25\end{array}\right.$

2、坐标变换

声呐回波信号为距离—角度二维极坐标模式，需要转换成直角坐标系下进行后续的信号处理。针对传统坐标变换存在的坐标变换后极坐标与直角坐标之间不能一一对应以及运算速度过慢的问题，本发明采用快速的查表映射方法。

实时声呐信号采集要求高效、高速的地址变换。当声波束触发到来时，锁定一个方位角度θ，然后距离计数r由零依次增加，物理意义上表明该方位由声呐扫描天线的中心逐步到显示量程边缘。由于采集到的回波数据径向分布，而扫描显示各像素按直角坐标显示，故回波数据在显示屏上地址(x，y)由下式决定：

$\left\{\begin{array}{c}x=r\sin \mathrm{\&theta;}\\ y=r\cos \mathrm{\&theta;}\end{array}\right.$

如果对应每一个r和θ都进行一次计算，整个计算量会非常巨大。由于r为0到显示量程的等差序列，而θ也是在扫描宽度内的等差序列，为了减小运算量，我们可以实现一个地址表，将rsinθ和rcosθ的值预先存储。在坐标变换时可直接查找计算值，通过增加地址空间的代价来换取减小运算时间。

3、伪彩色图像增强

用伪彩色代替灰度，采用调色板索引，可以更好地突出图像中的某些细节，提高声呐图像的分辨能力。伪彩色增强声纳图像如图2所示。

4、中值滤波

中值滤波算法对每个3×3窗口内的像素进行排序以求中值，其算法如下：

将像素(I，j)周围3*3方形窗口的像素值读入数组array[9]；

用冒泡法对array进行顺序，array[4]为该邻域中值；

用array[4]代替f(I，j)；

5、两层图像分割

首先通过高阈值t₂选择目标的亮区，然后选择高于低阈值t₁和与亮区相邻的次亮区，亮区和次亮区整个区域即为目标区域。t₂的选择要使每个目标有一些像素灰度级高于t₂，而背景不含有这样的像素。同时，应选择t₁使每个目标像素点具有高于t₁的灰度级。如果只使用t₂，则目标总是分割的不完整；如果只使用t₁则会有许多背景像素被错分为目标像素。而同时用两个阈值就能把背景与目标很好的分割开来。经过分割模块处理后，声纳图像就变成以0、1表示的二值图像。二层图像分割示意图如图2所示。

6、图像缺损的拟合

二值化后图像的目标区域可能是不连贯的，为此需要进行图像缺损的拟合。具体方法如下：

若f(x，y)是目标点，而f(x，y+1)是背景点，则u₁＝y+1；

若f(x，y)是背景点，而f(x，y+1)是目标点，则u2＝y+1；

若0<u₂-u₁<30，则f(x，i)置为目标点，其中u₁≤i≤u₂。

7、图像的细化

本发明采用多次迭代的图像细化算法，以上、下、左、右的顺序反复消除相应方向上的简单边界点。为保证细化的结果尽量处于中心线位置，交替处理相反方向的边界，即以上、下、左、右、上、下、左、右的次序反复进行，直至形成弧和曲线。该细化算法保留了原图像的连通性，这就保证了下一步处理边界跟踪的实现；且细化后的曲线基本位于原图轮廓的中心线位置，保证了原图的形状特征。

8、边界跟踪

将二值图像区域点的灰度值置为1，背景点灰度值置为0。以c表示前处理的点，以b表示当前点c的一个背景邻点。若某一点p的灰度值为t，则表示为p=t，具体流程如下：

边界跟踪从边界的任一点开始，此点如第一个当前点c，将其灰度值置为3，作为跟踪结束的标志，且将c点坐标放入表列中记录下来；

找到当前点c的一个背景邻点b，将其灰度值从0变为2；

从b点开始沿顺时针方向访问当前点c的8个邻点，直到找到灰度值为1或3或4的点，称这些点为d1，d2，…，dk。此时有两种情况：若dk=4，c=3，如果c的一个邻近点dn=2，n<k，则将dn灰度值变为0，c点灰度值变为4，然后终止算法；否则，记录此点坐标，若c=1，则将其灰度值变为4。然后令dk为新的当前点c，dk-1为其背景邻点b；

返回3。

9、目标位置估计

形心的位置是目标图形上的一个确定的点，当目标姿态变化时.这个点的位置变动一般较小，而且抗杂波干扰的能力强。本发明以形心作为目标位置进行输出。图5表示一个经二值化处理后的目标图像R。

由于二值化的结果，目标图像R以内的信号幅度为“1”，目标图像R以外的信号幅度为“0”。可以把形心解算式改写成：

$\left\{\begin{array}{c}\stackrel{\&OverBar;}{x}=(1/M){\&Integral;}_c^d{\&Integral;}_a^{b}V(x,y)\mathrm{xdxdy}\\ \stackrel{\&OverBar;}{y}=(1/M){\&Integral;}_c^d{\&Integral;}_a^{b}V(x,y)\mathrm{ydxdy}\\ M={\&Integral;}_c^d{\&Integral;}_a^{b}V(x,y)\mathrm{xdxdy}\end{array}\right.$

其中，当(x，y)属于R区域内时V(x，y)＝1；当(x，y)不属于R区域时V(x，y)＝0；a，b，c，d为跟踪窗口边界坐标。

上述步骤4）所采用的AUV自动避碰算法基于人工势场避碰模型，具体步骤如下：

构造目标位姿引力场和障碍物周围斥力场共同作用的人工势力场，搜索势函数的下降方向来寻找无碰撞路径。

斥力为斥力函数的负梯度，即：

$F_{\mathrm{re}}\left(x\right)=-\mathrm{grad}\left[U_{\mathrm{re}}\left(x\right)\right]=\left\{\begin{array}{cc}\mathrm{\&eta;}(\frac{1}{\mathrm{\&rho;}}-\frac{1}{{\mathrm{\&rho;}}_0})\frac{1}{{\mathrm{\&rho;}}^2}\frac{\&PartialD;\mathrm{\&rho;}}{\&PartialD;x}& \mathrm{\&rho;}\&le;{\mathrm{\&rho;}}_0\\ 0& \mathrm{\&rho;}>{\mathrm{\&rho;}}_0\end{array}\right.$

其中η为位置增益函数，ρ为AUV与障碍物之间的最短距离，ρ₀代表障碍物的影像距离。

引力为目标势场的负梯度，即：

F_at(x)=-grad[U_at(x)]=h(x-x_goal)

当AUV到达目标x_goal的过程中，这个力收敛于0，斥力与引力的合力为：

F=F_re+F_at

AUV下一步的运动方向由合力的方向

决定，定义AUV当前的位置(xk,yk)，则根据目标点位置和障碍物信息得到AUV下一步的路径点坐标(xk+1，yk+1)为：

$\left[\begin{array}{c}{x}_{k+1}\\ y_{k+1}\end{array}\right]=\left[\begin{array}{c}{x}_k\cos (\&PartialD;\stackrel{\&RightArrow;}{n})\\ y_k\sin (\&PartialD;\stackrel{\&RightArrow;}{n})\end{array}\right]\left[\begin{array}{c}{\mathrm{\&lambda;}}_1\\ {\mathrm{\&lambda;}}_2\end{array}\right]$

其中λ₁、λ₂为常量，是AUV每次移动的绝对唯一参数，求得路径点的地球坐标，进而转化为AUV的航向、速度、深度，反馈给AUV运动模型，从而构成一个闭环。

本发明采用自适应算法，从判断到输出结果都为自主完成，不需要人为干预。在水下环境不是十分复杂的条件下，本发明可以取得良好的测试效果，并能在短时间内给出物体的径向距离和方位角度的信息。

以上所述的仅是本发明的优选实施方式，本发明不限于以上实施例。可以理解，本领域技术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应认为包含在本发明的保护范围之内。

Claims (1)

1.一种基于迷你声纳的水下目标探测与AUV自动避碰方法，其特征在于包括如下步骤：

步骤1）：光纤陀螺（4）、多普勒测速仪（5）、高度计（6）、倾角传感器（7）及磁罗经（8）采集水下机器人的状态信息，通过串口传给主控计算机（3），主控计算机（3）进行导航信息处理；

步骤2）：迷你声纳（1）发射波束并接收反射回波，并对反射回波进行数字化，定时向声纳计算机（2）发送串行通讯数据包；

步骤3）：声纳计算机（2）接收到声纳数据后根据协议格式进行解码，提取回波数据，通过自动检测算法生成水下目标的方位距离与特征信息，通过网络传给主控计算机（3）；

步骤4）：主控计算机（3）根据步骤1）获取的水下机器人当前状态和步骤3）获取的水下目标所在的位置，通过自动避碰算法生成水下机器人的航向、速度和深度指令；

步骤5）：执行机构按控制指令调整水下机器人的航向、速度和深度，高速时采用舵翼（10）控制航向，低速时采用推进器（9）控制航向，实现水下机器人避碰；

所述步骤3）所采用的水下目标自动检测算法包括如下步骤：

1）波束内插

通过相邻波束的输出幅度内插出中间多个波束的输出值；

2）坐标变换

当声波束触发到来时，锁定方位角度θ，距离计数r由零依次增加，回波数据在显示屏上地址(x，y)由下式决定：

$\left\{\begin{array}{c}x=r\sin \mathrm{\&theta;}\\ y=r\cos \mathrm{\&theta;}\end{array}\right.$

在坐标变换时，通过映射预先存储于地址表内的r sinθ和r cosθ的值，直接查找出计算值；

3）伪彩色图像增强

用伪彩色代替灰度，采用调色板索引，突出图像中的细节，提高声纳图像分辨能力；

4）中值滤波

对每个3×3窗口内的像素进行排序以求中值：首先将像素(I，j)周围3*3方形窗口的像素值读入数组array[9]；然后用冒泡法对array进行顺序，array[4]为邻域中值；最后用array[4]代替f(I，j)；

5）两层图像分割

首先通过高阈值t₂选择目标的亮区，然后选择高于低阈值t₁和与亮区相邻的次亮区，亮区和次亮区整个区域即为目标区域；应选择t₂使每个目标有一些像素灰度级高于t₂，而背景不含有这样的像素；同时，应选择t₁使每个目标像素点具有高于t₁的灰度级；经过分割模块处理后，声纳图像变成以0、1表示的二值图像；

6）图像缺损的拟合

若f(x，y)是目标点，而f(x，y+1)是背景点，则u₁＝y+1；

若f(x，y)是背景点，而f(x，y+1)是目标点，则u2＝y+1；

若0<u₂-u₁<30，则f(x，i)置为目标点，其中u₁≤i≤u₂；

7）图像的细化

以上、下、左、右的顺序反复迭代消除相应方向上的简单边界点，直至形成弧和曲线；

8）边界跟踪

①将二值图像区域点的灰度值置为1，背景点灰度值置为0；以c表示前处理的点，以b表示当前点c的一个背景邻点；若某一点p的灰度值为t，则表示为p=t；

②从边界的任一点开始，此点如第一个当前点c，将其灰度值置为3，作为跟踪结束的标志，且将c点坐标放入表列中记录下来；

③找到当前点c的一个背景邻点b，将其灰度值从0变为2；

④从b点开始沿顺时针方向访问当前点c的8个邻点，直到找到灰度值为1或3或4的点，称这些点为d1，d2，…，dk；此时有两种情况：若dk=4，c=3，如果c的一个邻近点dn=2，n<k，则将dn灰度值变为0，c点灰度值变为4，然后终止算法；否则，记录此点坐标，若c=1，则将其灰度值变为4；然后令dk为新的当前点c，dk-1为其背景邻点b；

返回③；

9）目标位置估计

以形心作为目标位置进行输出；由于二值化的结果，目标图像R以内的信号幅度为“1”，目标图像R以外的信号幅度为“0”；把形心解算式改写成：

$\left\{\begin{array}{c}\stackrel{-}{x}=(1/M){\&Integral;}_c^d{\&Integral;}_a^{b}V(x,y)\mathrm{xdxdy}\\ \stackrel{-}{y}=(1/M){\&Integral;}_c^d{\&Integral;}_a^{b}V(x,y)\mathrm{ydxdy}\\ M={\&Integral;}_c^d{\&Integral;}_a^{b}V(x,y)\mathrm{dxdy}\end{array}\right.$

其中，当(x，y)属于R区域内时V(x，y)＝1；当(x，y)不属于R区域时V(x，y)＝0；a，b，c，d为跟踪窗口边界坐标；

所述步骤4）所采用的水下机器人自动避碰算法基于人工势场避碰模型，具体步骤如下：

构造目标位姿引力场和障碍物周围斥力场共同作用的人工势力场，搜索势函数的下降方向来寻找无碰撞路径；

斥力为斥力函数的负梯度，即：

$F_{\mathrm{re}}\left(x\right)=-\mathrm{grad}\left[U_{\mathrm{re}}\left(x\right)\right]=\left\{\begin{array}{cc}\mathrm{\&eta;}(\frac{1}{\mathrm{\&rho;}}-\frac{1}{{\mathrm{\&rho;}}_0})\frac{1}{{\mathrm{\&rho;}}^2}\frac{\&PartialD;\mathrm{\&rho;}}{\&PartialD;x}& \mathrm{\&rho;}\&le;{\mathrm{\&rho;}}_0\\ 0& \mathrm{\&rho;}>{\mathrm{\&rho;}}_0\end{array}\right.$

其中η为位置增益函数，ρ为AUV与障碍物之间的最短距离，ρ₀代表障碍物的影像距离；

引力为目标势场的负梯度，即：

F_at(x)=-grad[U_at(x)]=h(x-x_goal)

Ure是斥力场强度函数，模拟电场同类电荷的斥力强度；Uat()是引力场强度函数，模拟电场异类电荷的引力强度；x代表水下机器人距离障碍物的距离；h代表引力大小；Xgoat代表当引力与斥力相等时水下机器人距离障碍物的距离；

当水下机器人到达目标x_goal的过程中，这个力收敛于0，斥力与引力的合力为：

F=F_re+F_at

水下机器人下一步的运动方向由合力F的方向

决定，定义水下机器人当前的位置(X_k,Y_k)，则根据目标点位置和障碍物信息得到水下机器人下一步的路径点坐标(X_k+1,Y_k+1)为：

$\left[\begin{array}{c}{X}_{k+1}\\ Y_{k+1}\end{array}\right]=\left[\begin{array}{c}{X}_k\cos (\&PartialD;\stackrel{\&RightArrow;}{n})\\ Y_k\sin (\&PartialD;\stackrel{\&RightArrow;}{n})\end{array}\right]\left[\begin{array}{c}{\mathrm{\&lambda;}}_1\\ {\mathrm{\&lambda;}}_2\end{array}\right]$

其中λ₁、λ₂为常量，是水下机器人每次移动的绝对唯一参数，求得路径点的地球坐标，转化为水下机器人的航向、速度、深度，反馈给水下机器人运动模型。

Images