AUV智能避碰装置及避碰方法

摘要

本发明提供的是一种AUV智能避碰装置及避碰方法。传感器采集AUV的状态信息，通过串口传给动态控制机，动态控制机在将这些信息通过网络发送给使命管理机；多波束前视声纳采集障碍物信息，转换为描述局部环境的数字信号；使命管理机接收局部环境的数字信号，用AUV障碍物判定系统确定障碍物位置，建立局部环境模型并传递给避碰规划系统；避碰规划系统通过避碰算法计算出AUV航向、速度、深度，并把这三个指令通过网络传递给动态控制机；动态控制机通过运动控制解算，利用控制电压驱动执行机构，按控制指令调整AUV的航向、速度和深度，实现AUV避碰。此发明的优点在于无需先验知识，能够实时实现避碰，且通过湖试验证了该发明方案的可靠性和有效性。

说明书

(一)技术领域

本发明涉及一种使用多波束前视声纳检测障碍物，确定障碍物位置，给出避碰指令，实现AUV智能避碰的避碰装置和避碰方法。

(二)背景技术

在复杂和未知的水下环境中航行的AUV，确保其安全的一个基本要求是AUV要具有避碰能力，这也是AUV自主能力和智能化的体现。在避碰过程中，传感器作为底层控制的一部分起着举足轻重的作用。目前广泛使用的主要有超声波传感器、红外线传感器、激光测距仪、摄像机、声纳等等。在大范围海域中航行的AUV大都采用声纳作为避碰的主要设备。我们所采用的这种多波束前视声纳安装在AUV前部，实时地获取障碍物的位置信息，为AUV避碰提供前提条件。由于存在环境干扰，声纳常常会提供“伪信息”。这种“伪信息”的存在会导致AUV的误判，使AUV不能向正确的方向前进。这就需要我们通过对噪声的滤除提高障碍物探测的准确度。同时，由于障碍物的多样性和未知性，基于传感器信息，采用怎样的避碰算法，如何进行避碰规划，确保AUV能够及时快速地对障碍物做出反应也是一个研究重点。

专利申请号为200720007317.3的专利申请文件中公开了一种“船用智能避碰导航仪”。该导航仪集成了来自雷达、AIS、GPS、计程仪和测深仪的动态信息与来自电子海图的静态信息，当船舶接近危险物或汇遇来船时，该导航仪可通过声光、图形、符号等方式发出报警信号，并自动给出安全避让决策，辅助船舶驾驶员操纵船舶。该导航仪用雷达作为障碍物探测设备，最终的避碰决策支持系统通过机器学习识别和利用障碍物的信息，采用正向推理控制策略、启发式搜索方法来实现避碰。该避碰决策支持系统建立在《国际海上避碰规则》及一定的航海知识和航海经验的基础上，当不具备先验知识或者先验知识比较少时，该避碰系统就无法给出正确的决策。

(三)发明内容

本发明的目的在于提供一种应用多波束前视声纳及其它相关传感器信息，使AUV做出正确的避碰决策的AUV智能避碰装置和避碰方法。

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

本发明的AUV智能避碰装置包括水面工作站1、使命管理机2和动态控制机3，水面工作站1、使命管理机2和动态控制机3中嵌装有软件系统，水面工作站1、使命管理机2和动态控制机3之间通过网络连接，动态控制机3通过串口连接光纤陀螺仪7、多普勒测速仪8、测高声纳9、姿态传感器10，多波束前视声纳系统6与使命管理机2通过串口连接，使命管理机2中加载有基于神经网络的障碍物判定系统软件4和避碰规划系统软件5，动态控制机3通过电信号连接舵11和推进器12。

基于本发明的AUV智能避碰装置的避碰方法为：

(1)传感器采集AUV的状态信息，通过串口传给动态控制机，动态控制机将这些信息通过网络以2Hz的频率发送给使命管理机；

(2)多波束前视声纳采集障碍物信息，将探测到的模拟信号转换为描述局部环境的数字信号；

(3)使命管理机通过串口接收局部环境的数字信号，用AUV障碍物判定系统确定障碍物位置，滤除环境噪声，建立局部环境模型并发送给避碰规划系统；

(4)避碰规划系统根据AUV当前状态和障碍物所在的位置，通过避碰算法计算出AUV航向、速度、深度，并把这三个指令通过网络传递给动态控制机；

(5)动态控制机通过运动控制解算，利用控制电压驱动执行机构，按控制指令调整AUV的航向、速度和深度，高航速时采用舵控制航向，低航速时采用推进器控制航向，实现AUV避碰。

本发明的方法还可以包括：

1、所述的AUV状态信息包括：AUV经纬度、航向、速度、深度、纵倾、横倾，测量状态信息的传感器设备为：光纤陀螺仪、多普勒测速仪、测高声纳、姿态传感器。

2、所述的把模拟信号转为数字信号的设备为多波束前视声纳的嵌入式计算机。

3、所述的AUV障碍物判定系统为一个基于神经网络的障碍物判定系统，由量化模块、数据库、神经网络模块、反量化模块和解释器5个部分组成。

量化模块：以每个栅格grid[i][j]周围的若干个相邻栅格为一组，被占用的栅格量化为0.8，未被占用的栅格量化为0.3；

数据库：存放初始事实，推理的中间结论以及推理出的最终结果的数据；

神经网络模块：采用BP网络，网络输入取8个或者更多，BP网络离线训练，在线使用；八个输入为量化后的相邻栅格数据，输出为中心栅格的估计值grid[i][j]′；当越多的相邻栅格被占用时，grid[i][j]被占用的可能性越大；如果相邻栅格分值比较低，则grid[i][j]的分值不应该增加太多；同时，对于中心栅格，它自身占用信息应优先考虑，让其自身所占权重为1，而通过相邻栅格得到的分值所占权重为0.5；得到中心栅格的分值为：

grid[i][j]＝grid[i][j]′×0.5+grid[i][j]×1

反量化模块：根据多次试验取定一个阈值ξ，grid[i][j]＞ξ时栅格赋1，grid[i][j]＜ξ时栅格赋0；

解释器：输出最终推理结果，即障碍物信息的两种情况：1为有障碍物，0为无障碍物。

第一次量化后的信息数据送入神经网络模块，经神经网络推理后把判定结果直接传给解释器，同时存入数据库；下一次数据传来时，检查数据库中是否有相匹配的类型，若有，不再用神经网络模块作重复判定，直接在数据库中通过匹配得出结论，否则送到神经网络模块重新判定。

4、所述的避碰算法为改进的人工势场法

(1)人工势场避碰模型

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

斥力为斥力势场的负梯度，即：

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

其中η为位置增益系数，ρ为AUV与障碍物之间的最短距离，ρ₀是一个常数，代表障碍物的影响距离。

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

F_at(X)＝-grad[U_at(X)]＝h(X-X_goal)

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

F＝F_re+F_at

AUV下一步的运动方向由合力的方向决定，定义AUV当前的位置(x_k，y_k)，则根据目标点位置和障碍物信息得到AUV下一步的路径点坐标(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 (\partial n)\\ y_k\sin (\partial n)\end{array}\right)\left(\begin{array}{c}{\lambda }_1\\ {\lambda }_2\end{array}\right)$

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

(2)人工势场法的改进算法

传统的人工势场法的缺陷在于把所有信息合并为单个合力，并以该合力方向控制AUV下一步的运动，不考虑局部障碍物分布的其它有价值的信息，使得AUV在避碰过程中可能会出现陷入势场的局部极小点或者在局部极小点的周围位置徘徊，以及AUV在局部势场环境中徘徊抖动的现象，这两种情形统一称为“势场陷阱”。

AUV陷入局部极小是因为会有AUV在环境中某一点受到的吸引力和排斥力的合力为零的情况。当$\sqrt{{(x_{k+m}-x_k)}^2+{(y_{k+m}-y_k)}^2}\le \sigma$(σ为一个无穷小的量)，其中m＝2，3，4...，表明AUV在第k步和k+m-1步的m个点上依次周期性地徘徊，无法达到目标。针对这一现象，给出了“随机扰动法”。其基本思路是当AUV陷入了这种“势场陷阱”后，通过随机产生一个-180～+180度之间的扰动角度Δθ来提供下一步的运动方向，经过安全处理，确保AUV沿着该方向航行时，不会出现与障碍物相撞的情形。此时AUV的下一目标点为：

$\left(\begin{array}{c}{x}_{k+1}=x_k+{\lambda }_1\cos ({\theta }_k+\Delta {\theta }_k)\\ x_{k+1}=x_k+{\lambda }_2\cos ({\theta }_k+\Delta {\theta }_k)\end{array}\right)$

AUV航迹出现抖动是因为在障碍物附近合力方向会发生突然变化，在合力控制下，AUV下一步运动方向会有大角度突变，于是就产生了徘徊抖动现象。这种情况的产生严重影响了AUV避碰效果和到达目标的效率。为了解决这一问题，本发明采用一种优化位移参数的方法，给出一个参数调节因子ε，通过试验，找到最佳的位移参数调整因子ε_b。此时有：

$\left(\begin{array}{c}{x}_{k+1}=x_k+{ϵ}_{b1}{·\lambda }_1\cos ({\theta }_k+\Delta {\theta }_k)\\ x_{k+1}=x_k+{{ϵ}_{b2}·\lambda }_2\cos ({\theta }_k+\Delta {\theta }_k)\end{array}\right)$

这样就可以得到AUV从当前点到下一目标点的最佳路径。

本发明的AUV智能避碰系统，采用多波束前视声纳作为障碍物探测设备，将声纳、光纤陀螺仪、多普勒测速仪、测高声纳、姿态传感器、舵、推进器等设备组合在一起，将软件嵌入水面工作站、使命管理机和动态控制机，使AUV具备实时避让水面或者水下障碍物的自主决策功能。该发明的核心部分是设计了一个基于神经网络的障碍物判定系统，采用改进的人工势场法作为避碰规划系统的核心算法计算航向，并求出相应的速度、深度，最后由动态控制机执行控制指令，实现避碰。

此发明的优点在于无需先验知识，能够实时实现避碰，且通过湖试证明了该发明方案的可靠性和有效性。

(四)附图说明

图1为AUV智能避碰系统总体结构图；

图2为基于神经网络的障碍物判定系统结构图；

图3为BP神经网络结构图；

图4a和图4b为障碍物判定前后效果对比图，其中图4a为障碍物判定前的原始声纳图像，图4b为障碍物判定后的图像；

图5a和图5b描述了AUV陷入“势场陷阱”的现象，其中图5a为陷入局部极小的情形，图5b为存在“抖动”的情形；

图6为应用改进后的人工势场法作避碰规划的仿真结果；

图7是实际避碰效果图。

(五)具体实施方式

下面结合附图对本发明做更详细地描述：

1、AUV智能避碰系统总体结构

如图1所示，发明的AUV智能避碰装置采用多波束前视声纳作为障碍物探测设备，将声纳6、光纤陀螺仪7、多普勒测速仪8、测高声纳9、姿态传感器10、舵11、推进器12等仪器设备组合在一起，将软件系统嵌入水面工作站1、使命管理机2和动态控制机3，使AUV具备实时避让水面或者水下障碍物的自主决策功能。水面工作站1、使命管理机2和动态控制机3之间通过网络连接；传感器7、8、9、10通过串口与动态控制机3连接；多波束前视声纳系统6与使命管理机2通过串口连接；基于神经网络的障碍物判定系统软件4和避碰规划系统软件5加载到使命管理机2。

本发明是一种基于多波束前视声纳的具备智能化和可靠性的避碰系统，主要应用于AUV。它由水面工作站1、使命管理机2和动态控制机3以及各种传感器6、7、8、9、10组成。水面工作站1和动态控制机3是PC机，使命管理机2是PC104总线的计算机，三台机器通过局域网相连。除了上述硬件设备，该发明还包括安装在使命管理机上的基于神经网络的障碍物判定系统软件5和避碰规划系统软件4，这是该发明的核心部分。

水面工作站1将声纳工作参数和声纳开机指令通过网络传递给使命管理机2，由使命管理机2通过串口发送给声纳6。声纳6接收指令后，开启探测功能，通过串口把局部环境信息发送到使命管理机2，当发现障碍物时，声纳6把障碍物信息传递到使命管理机2上的障碍物判定系统5做数据处理，确定障碍物的真实位置，建立局部环境模型。与此同时动态控制机2把传感器7、8、9、10采集到的AUV姿态和速度信息通过网络传递给使命管理机2上的避碰规划系统4。避碰规划系统4根据障碍物位置结合AUV姿态、速度和环境信息，规划出恰当的避碰方向，把计算出的航向、速度、深度发送到动态控制机3，由动态控制机3控制舵11或推进器12来执行控制指令。

2、基于神经网络的障碍物判定系统

采用一个56×40的栅格数组描述声纳探测到的信息，每个栅格代表实际探测到5×5m²正方形区域。由于水面波浪、水底粗糙不平、水介质起伏、内波以及声纳载体的运动，使得这些多途径信号随机时变，而海洋噪声场或混响干扰场均不能满足均匀各向同性的条件，会使前视声纳返回的障碍物信息不够准确，使得测量结果达不到避碰要求，导致AUV无法确定下一步的航向。

针对这个问题，本发明设计了一个基于神经网络的障碍物判定系统来滤除环境噪声。该方法的依据是：对某个栅格，如果它相邻单元中被占用的栅格越多，它自身被占用的可能性就越大，相应取值也就应该增加。在栅格中，考虑越多的相邻单元，判定结果越准确，但同时也加大了计算量，为了加快计算速度，此处采用神经网络来作栅格数据的处理。

基于神经网络的障碍物判定系统结构如图2所示，由量化模块、数据库、神经网络模块、反量化模块和解释器5个部分组成。

量化模块：以每个栅格grid[i][j]周围的若干个相邻栅格为一组，具体个数根据AUV航速和滤波所要求的精确度为依据来选择。被占用的栅格量化为0.8，未被占用的栅格量化为0.3。

数据库：存放初始事实，推理的中间结论以及推理出的最终结果的数据。

神经网络模块：本发明采用BP网络，网络输入可以取8个或者更多，根据量化模块给出的数据来确定，BP网络离线训练，在线使用。以8-6-1结构为例，见图3，八个输入为量化后的相邻栅格数据，输出为中心栅格的估计值grid[i][j]′。当越多的相邻栅格被占用时，grid[i][j]被占用的可能性越大。但如果相邻栅格分值比较低，则grid[i][j]的分值不应该增加太多。同时，对于中心栅格，它自身占用信息应该优先考虑，让其自身所占权重为1，而通过相邻栅格得到的分值所占权重为0.5。这样得到中心栅格的分值为：

grid[i][j]＝grid[i][j]′×0.5+grid[i][j]×1

反量化模块：根据多次试验取定一个阈值ξ，grid[i][j]＞ξ时栅格赋1，grid[i][j]＜ξ时栅格赋0。

解释器：输出最终推理结果，即障碍物信息的两种情况：1为有障碍物，0为无障碍物。

第一次量化后的信息数据送入神经网络模块，经神经网络推理后把判定结果直接传给解释器，同时存入数据库。下一次数据传来时，检查数据库中是否有相匹配的类型，若有，不再用神经网络模块作重复判定，直接在数据库中通过匹配得出结论，否则送到神经网络模块重新判定。

在湖试中，AUV试验航速通常在1～3节，假定前视声纳在2.5m之内对于同一障碍物的观测结果相同，根据相对应的AUV航速确定每一组感兴趣的栅格区域的个数。前视声纳有数据返回时，采用基于神经网络的障碍物判定系统对声纳信息作滤波处理，得到滤波结果如图4所示。由图4可见，经过该判定系统，环境噪声被有效滤除，准确地得到了障碍物的轮廓图，其中图4a为障碍物判定前的原始声纳图像，图4b为障碍物判定后的图像。

3.避碰算法

(1)人工势场避碰模型

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

斥力为斥力势场的负梯度，即：

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

其中η为位置增益系数，ρ为AUV与障碍物之间的最短距离，ρ₀是一个常数，代表障碍物的影响距离。

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

F_at(X)＝-grad[U_at(X)]＝h(X-X_goal)

当AUV到达目标X_goal的过程中，这个力收敛于零。由此得到斥力与引力的合力为：

F＝F_re+F_at

AUV下一步的运动方向由合力的方向决定。定义AUV当前的位置(x_k，y_k)，则根据目标点位置和障碍物信息得到AUV下一步的路径点坐标(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 (\partial n)\\ y_k\sin (\partial n)\end{array}\right)\left(\begin{array}{c}{\lambda }_1\\ {\lambda }_2\end{array}\right)$

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

(2)人工势场法的改进算法

传统的人工势场法的缺陷在于把所有信息合并为单个合力，并以该合力方向控制AUV下一步的运动，不考虑局部障碍物分布的其它有价值的信息，使得AUV在避碰过程中可能会出现陷入势场的局部极小点或者在局部极小点的周围位置徘徊，如图5(a)，以及AUV在局部势场环境中徘徊抖动的现象，如图5(b)，这两种情形统一称为“势场陷阱”。

AUV陷入局部极小是因为会有AUV在环境中某一点受到的吸引力和排斥力的合力为零的情况。当$\sqrt{{(x_{k+m}-x_k)}^2+{(y_{k+m}-y_k)}^2}\le \sigma$(σ为一个无穷小的量)，其中m＝2，3，4...，表明AUV在第k步和k+m-1步的m个点上依次周期性地徘徊，无法达到目标。针对这一现象，给出了“随机扰动法”。其基本思路是当AUV陷入了这种“势场陷阱”后，通过随机产生一个-180～+180度之间的扰动角度Δθ来提供下一步的运动方向，经过安全处理，确保AUV沿着该方向航行时，不会出现与障碍物相撞的情形。此时AUV的下一目标点为：

$\left(\begin{array}{c}{x}_{k+1}=x_k+{\lambda }_1\cos ({\theta }_k+\Delta {\theta }_k)\\ x_{k+1}=x_k+{\lambda }_2\cos ({\theta }_k+\Delta {\theta }_k)\end{array}\right)$

AUV航迹出现抖动是因为在障碍物附近合力方向会发生突然变化，在合力控制下，AUV下一步运动方向会有大角度突变，于是就产生了徘徊抖动现象。这种情况的产生严重影响了AUV避碰效果和到达目标的效率。为了解决这一问题，本发明采用一种优化位移参数的方法，给出一个参数调节因子ε，通过试验，找到最佳的位移参数调整因子ε_b。此时有：

$\left(\begin{array}{c}{x}_{k+1}=x_k+{ϵ}_{b1}{·\lambda }_1\cos ({\theta }_k+\Delta {\theta }_k)\\ x_{k+1}=x_k+{{ϵ}_{b2}·\lambda }_2\cos ({\theta }_k+\Delta {\theta }_k)\end{array}\right)$

这样就可以得到AUV从当前点到下一目标点的最佳路径，如图6所示。

湖试中采用了本发明所述的避碰规划系统。障碍物是位于湖中心的一个独立小岛。AUV起点A，目标点B。AUV从A点出发，前视声纳探测到小岛时，根据传感器信息以及安全半径，由AUV智能避碰系统规划出局部路径。每一个规划好的点包含AUV躲避障碍物的航向、速度、深度及航向控制模式信息，低速下由推进器控制航向，高速时用舵控制航向，使AUV转艏避开障碍物。图7是湖试时的避碰结果截图，单位是米。