CN111506068B - 一种用于多波束声呐扫描作业的水面无人艇局部路径规划方法 - Google Patents

Abstract

一种用于多波束声呐扫描作业的水面无人艇局部路径规划方法，本发明涉及水面无人艇局部路径规划方法。本发明的目的是为了解决现有局部路径规划方法避障准确率低的问题。过程为：一、得到水面无人艇航所受引力的大小和方向；二、得到水面无人艇所受的斥力大小；三、将水面无人艇所受引力的大小和所受斥力的大小进行矢量和计算，判断矢量和是否为0，若为0，则执行四；若不为0，则将矢量和叠加在无人艇上，完成无人艇的局部路径规划；四、得包含逃逸势场的改进引力场函数，对改进引力场函数取负梯度得改进引力函数；五、将所受引力和斥力进行矢量和计算后叠加在无人艇上，完成无人艇的局部路径规划。本发明用于无人艇局部路径规划领域。

Description

一种用于多波束声呐扫描作业的水面无人艇局部路径规划 方法

技术领域

本发明涉及水面无人艇局部路径规划方法。

背景技术

水面无人艇(Unmanned Surface Vehicle,USV)是几大无人载体中的其中一类，但是较无人机，无人车和无人水下航行器发展的较晚。同有人舰艇相比，水面无人艇凭借其无人、体积小、高速等典型优点，可以更高效的完成一些民用和科考任务，如海洋资源勘探，海洋地理信息收集，海图绘制和海上搜救等，同时也可执行多种军事任务，如海上情报收集，重点海域监测，排雷作业，反潜探测，反恐突击等^[1-3]([1]Li M.,He Y.,Ma Y.,etal.Design and implementation of a new jet-boat based unmanned surface vehicle[C].Proceedings of International Conference on Automatic Control&ArtificialIntelligence,Xiamen,China,2012:768-771.)([2]Villa J L.,Paez J.,Quintero C.,etal.Design and control ofan unmanned surface vehicle for environmentalmonitoring applications[C].Proceedings ofRobotics&Automation,Bogota,Colombia,2017:1-5.)([3]Specht C.,Switalski E.,Specht M.Application of an autonomous/unmanned survey vessel(ASV/USV)and bathymetric measurements[J].Polish MarineResearch,2017,24(3):36-44.)，如图1所示。

局部路径规划则是由所携带的传感器，实时获取周围环境主要是未知障碍物的信息，然后根据事先设置好的避碰准则，实时重新规划一条局部路径完成避障。但由于水面无人艇高度非线性、交叉耦合的系统动力学特性和不可预测的复杂水面环境，进而增大了局部路径规划控制器设计的难度。

因此，本发明专利针对无人艇的局部路径规划问题，提出一种基于改进人工势场法的局部路径规划方法。

发明内容

本发明的目的是为了解决现有局部路径规划方法避障准确率低的问题，而提出一种用于多波束声呐扫描作业的水面无人艇局部路径规划方法。

一种用于多波束声呐扫描作业的水面无人艇局部路径规划方法具体过程为：

步骤一、基于传统的人工势场法的引力场设计改进引力场，得到水面无人艇航行过程中所受引力的大小和方向；

步骤二、基于传统的斥力场设计改进斥力场，得到水面无人艇航行过程中所受的斥力大小；

步骤三、将步骤一得到的水面无人艇所受引力的大小和步骤二得到的水面无人艇航行过程中所受斥力的大小进行矢量和计算，判断矢量和是否为0，若为0，则执行步骤四；若不为0，则将矢量和叠加在无人艇上，以此完成无人艇的局部路径规划；

步骤四、基于步骤一和步骤二设计逃逸势场，将逃逸势场带入改进引力场，得包含逃逸势场的改进引力场函数，对改进引力场函数取负梯度得改进引力函数；

步骤五、将所受引力和斥力进行矢量和计算后叠加在无人艇上，以此完成无人艇的局部路径规划。

本发明的有益效果为：

本发明由传统人工势场法的原理分析其主要缺点并提出改进方案。传统人工势场法只考虑相对位置的影响不足以充分描述出USV与目标点和障碍物之间的状态，本发明在引力场中加入速度场，在斥力场周静加入加速度场。同时由于多波束声呐的成像特性要求USV能按照全局规划出的既定路径航行，这就要求USV在完成局部避障后能迅速回到预设路径上，因此本发明在引力场中加入回归势场。同时，针对传统人工势场法的易陷入局部极小值问题，本发明加入逃逸势场，使USV在陷入局部极小值时迅速脱离。将传统人工势场法和本发明的改进人工势场法通过搭建的仿真平台分别进行仿真，通过对比实验说明了本发明提出的改进人工势场法的有效性和优越性，解决了现有局部路径规划方法避障准确率低的问题，提高了现有局部路径规划方法避障准确率。

附图说明

图1为本发明水面无人艇示意图；

图2为引力场模型示意图；

图3为斥力场模型示意图；

图4为合力场模型示意图；

图5为引力矢量运算图；

图6为斥力力矢量运算图；

图7为无人艇陷入局部极小值示意图；

图8为逃逸力示意图；

图9为仿真平台示意图；

图10为传统人工势场法仿真结果图；

图11为调整系数后仿真结果图；

图12为T₁时刻仿真结果图；

图13为T₂时刻仿真结果图；

图14为T₃时刻仿真结果图；

图15为T₄时刻仿真结果图。

具体实施方式

具体实施方式一：本实施方式一种用于多波束声呐扫描作业的水面无人艇局部路径规划方法具体过程为：

人工势场法由Khatib在1986年首次提出，其借鉴了电子在电场中的运动时将会受到引力和斥力的共同影响的思想，将机器人当做质点处理，在运动中也会受到引力和斥力的影响，其中目标点将对机器人产生引力，障碍物将对机器人产生斥力，力的大小与他们间的距离正相关。在计算出引力和斥力的矢量和后，便可驱动机器人运动。

传统引力场函数为：

公式中的ε是引力场的权重系数，ρ(q,q_goal)表示物体当前位置与目标位置的距离。那么对引力场函数求负梯度可得引力函数：

其中F_attr为矢量，方向由机器人实时位置指向目标位置。

引力场模型如图2；

传统斥力场函数为：

公式中的η是增益系数，ρ(q,q_obs)为机器人和障碍物之间的距离，p₀为障碍物的最小安全半径。那么同样的，对斥力场函数求负梯度可得斥力函数：

斥力场模型如图3；

机器人所受的总势场，就是引力势场和斥力势场的矢量和叠加：

U(q)＝U_attr+U_rep(q)

那么将机器人所受的引力和斥力叠加即为引导机器人运动的合力：

F(q)＝F_attr+F_rep(q)

为了使用基于人工势场法的无人艇局部路径规划控制器，实现对水面无人艇的局部路径规划控制，本发明的目标，即设计局部路径规划控制器，使水面无人艇在水面航行时遇到各种水面障碍物时能规划出理想的路径，使水面无人艇完成既定目标。

传统的人工势场法中主要以相对位置产生的引力场和斥力场所产生的合场来驱动机器人运动^[4-6]([4]Khatib O.Real-Time Obstacle Avoidance for Manipulators andMobile Robots[J].International Journal of Robotics Research,1986,5(1):90-98)([5]Akishita S,Kawamura S,Hisanobu T.Velocity Potential Approach to PathPlanning for Avoiding Moving Obstacles[J].Advanced Robotics,1992,7(5):463-478.)([6]Khosla P,Volpe R.Superquadric Artificial Potentials for ObstacleAvoidance and Approach[C].Proceedings of IEEE International Conference onRobotics and Automation.Philadelphia,PA:IEEE,1988:1778-1784.)。在机器人运动过程中，相对速度也可以反应出位置改变的趋势，但是传统人工势场法并没有考虑到速度因素带来的影响，所以本发明增加了相对速度场。由于全局规划出的路径是根据多波束声呐成像特性等工程特殊要求决定的，所以水面无人艇是否能长时间的处于全局规划路径中对水面无人艇的巡检效率至关重要，所以本发明在引力场中增加了回归势场，可以保证水面无人艇在躲避过障碍物之后用最短的时间回到全局规划路径中。在斥力场中，只考虑相对位置带来的影响不足以充分描述出水面无人艇与障碍物之间的关系，因为相对加速度可以反应出相对位置的变化趋势，所以本文在斥力场中增加相对加速度场。同时，针对人工势场法存在的易陷入局部极小值的问题^[7,8]([7]于振中,闫继宏,赵杰,et al.改进人工势场法的移动机器人路径规划[J].哈尔滨工业大学学报,2011,43(1):50-55.)([8]罗强,王海宝,崔小劲,et al.改进人工势场法自主移动机器人路径规划[J].控制工程,2019(6):1091-1098.)，增加了逃逸势场，确保当无人艇陷入局部极小值时能尽快逃逸。

步骤一、基于传统的人工势场法的引力场设计改进引力场，得到水面无人艇航行过程中所受引力的大小和方向；

步骤二、基于传统的斥力场设计改进斥力场，得到水面无人艇航行过程中所受的斥力大小；

步骤三、将步骤一得到的水面无人艇所受引力的大小和步骤二得到的水面无人艇航行过程中所受斥力的大小进行矢量和计算，判断矢量和是否为0，若为0，则执行步骤四；若不为0，则将矢量和叠加在无人艇上，以此完成无人艇的局部路径规划；

步骤四、基于步骤一和步骤二设计逃逸势场，使无人艇在陷入局部极小值时逃逸出，将逃逸势场带入改进引力场，得包含逃逸势场的改进引力场函数，对改进引力场函数取负梯度得改进引力函数；

步骤五、将所受引力和斥力进行矢量和计算后叠加在无人艇上，以此完成无人艇的局部路径规划。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中基于传统的人工势场法的引力场设计改进引力场，得到水面无人艇航行过程中所受引力的大小和方向；具体过程为：

基于上文分析，改进的引力场在传统引力场中的位置场的基础上，加入相对速度场和回归势场。局部路径规划可以转化为在每两个路径点形成的直线的局部避障规划。

步骤一一、传统的人工势场法的引力场函数为式(1)：

U_attr(s)＝α₁||s-s_g||^j (1)

式中α₁表示在引力场中相对位置对水面无人艇产生引力的权重，j表示可调整的增益系数，s-s_g为USV与障碍物的相对位置，s为USV位置，s_g为引力场中的障碍物位置；

步骤一二、基于传统的人工势场法的引力场设计改进引力场，得到最终水面无人艇航行过程中所受引力F_attr(s,v,q)的大小和方向；具体过程为：

在传统人工势场法的基础上增加相对速度场和回归势场，那么改进引力场就为相对位置场，相对速度场和回归势场组成的合场，改进引力场函数如式(2)：

U_attr(s,v,q)＝α₁||s-s_g||^j+α₂||v||^k+α₃||q-q_line||^l (2)

式中α₁表示在引力场中相对位置对水面无人艇产生引力的权重，α₂表示在引力场中相对速度对水面无人艇产生引力的权重，α₃表示在引力场中回归势场对水面无人艇产生引力的权重，j,k,l表示可调整的增益系数，s-s_g为USV与障碍物的相对位置，q-q_line为水面无人艇的实时位置与全局规划路径的相对距离，q为USV实时的路径位置，q_line为全局规划出的路径位置，其中s＝q；v为相对速度；

所述相对距离取全局规划路径上距水面无人艇的实时位置最近的点的距离；

对改进引力场函数取负梯度得改进引力函数：

式中

式中

为水面无人艇实时位置指向目标点的单位向量，

为USV和障碍物的相对速度的单位向量，

为水面无人艇实时位置指向全局路径最近目标点的单位向量；

为负梯度的符号，F_attr(s)为相对位置产生的引力，F_attr(v)为相对速度产生的引力，F_attr(q)为USV实时位置与全局规划路径位置的相对位置产生的引力；

所以得改进引力函数为：

F_attr(s,v,q)＝F_attr(s)+F_attr(v)+F_attr(q) (7)

然后通过三个分力的矢量和运算，即可得到最终水面无人艇所受引力F_attr(s,v,q)的大小和方向，引力矢量运算如图5所示；

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤二中基于传统的斥力场设计改进斥力场，得到水面无人艇航行过程中所受的斥力大小；具体过程为：

在传统的斥力场中仅包含了水面无人艇与障碍物的相对位置信息，但当无人艇在海上航行时，仅考虑与障碍物的相对位置信息是不能够充足的反映出与障碍物的全部状态。因为相对加速度可以反应出二者相对位置变化的趋势，所以本发明在传统的斥力场中增加相对加速度场。

步骤二一、传统斥力场函数如下：

式中α₄为相对位置场在斥力场中的权重系数，s-s_obs为无人艇到障碍物的相对距离，s为USV位置，s_obs为斥力场中的障碍物位置，d₀为障碍物最小影响半径，即当距离障碍物的距离小于障碍物最小影响半径时，就会受到障碍物的斥力作用；

步骤二二、为了方便起见，令s-s_obs＝p_obs，p_obs为中间变量，对斥力场函数取负梯度，得斥力函数如下：

式中

式中

为负梯度的符号；

将相对加速度对斥力场的影响加入传统斥力场，得到改进斥力场函数：

式中α₅为相对加速度场在斥力场中的权重系数，m为增益系数，a为相对加速度；

改进的斥力场函数将无人艇在航行时所受斥力的状态分为三种：

第一种情况是当无人艇与障碍物的距离小于最小影响半径，且相对加速度大于零时，说明与障碍物有加速靠近的趋势，那么无人艇将受到相对位置场和相对加速度场的合场作用；

第二种情况是当无人艇与障碍物的距离小于最小影响半径，且相对加速度小于等于零时，说明与障碍物有加速远离的趋势，那么无人艇将不会受到加速度场的影响，仅受到相对位置场的影响；

第三种情况是当无人艇与障碍物的距离大于最小影响半径时，那么无人艇将不会受到障碍物的斥力影响。

步骤二三、由改进斥力场函数求负梯度得改进斥力函数：

式中

式中

为无人艇实时位置指向障碍物的单位向量，

为USV与障碍物的相对加速度的单位向量；m、n为相应的增益系数，F_rep(p)为相对位置给USV的斥力，U_rep(p)为相对位置给USV的斥力场，U_rep(a)为相对加速度给USV的斥力场，F_rep(a)为相对加速度给USV的斥力；

得到水面无人艇航行过程中所受的斥力大小就为相对位置形成的斥力和相对加速度形成的斥力的矢量和，斥力矢量运算如图6所示。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述步骤四中基于步骤一和步骤二设计逃逸势场，使无人艇在陷入局部极小值时逃逸出，将逃逸势场带入改进引力场，得包含逃逸势场的改进引力场函数，对改进引力场函数取负梯度得改进引力函数；具体过程为：

传统人工势场法的一个显著的缺点就是容易陷入局部极小值。当使用人工势场法进行局部路径规划时，无人艇受到以目标点为主的引力场和以障碍物为主的斥力场的共同作用。由于所受的合力是由引力和斥力的矢量叠加得到的，那么在运动中就有可能存在某一时刻，所受的引力和斥力大小相等方向相反，导致所受合力为0。那么就会陷于局部极小值，使无人艇在极值点的运动产生震荡或是停止不前，导致无法到达目标点。

如图7所示，当无人艇、障碍物和目标点处于共线，并当F_attr＝F_rep，所受引力和斥力的合力为0，就无法规划下一步路径从而导致避障失败。本发明对提出的改进引力场(式9)再次进行修改，增加逃逸势场，使无人艇在陷入局部极小值时可以快速逃逸出。

步骤四一、设计逃逸势场；

当所受引力和斥力大小相等，方向相反时，逃逸势场便作用于引力场，方向为当前前进方向顺时针或逆时针旋转90度。同时，当陷入局部极小值区域时，往往是因为进入了障碍物的最小影响半径，斥力的增大并与引力相等。为了航行安全，需要尽快逃逸出极小值区域。所以将无人艇距障碍物最小安全半径的距离考虑到逃逸势场内，可得逃逸势场函数如下：

U_esc(s)＝α₆||s-s_sec||^r (13)

对逃逸势场函数求负梯度得逃逸力函数：

式中s_sec为无人艇距障碍物最小安全半径，当距障碍物越近时，逃逸力越大；α₆为逃逸力的权重系数，r为逃逸力的增益系数；逃逸力示意图如图8；

步骤四二、逃逸势场只在无人艇陷入局部极小值时才起作用。将逃逸势场带入改进引力场，得包含逃逸势场的改进引力场函数为：

式中当无人艇陷入局部极小值区时g＝1，未陷入极小值区时g＝0；

同样的，对引力场函数取负梯度得改进引力函数：

其它步骤及参数与具体实施方式一至三之一相同。

采用以下实施例验证本发明的有益效果：

仿真准备

为了可以充分的体现仿真效果，以全局规划路径点中的两个路径点为例，并放大两点之间的间距搭建仿真平台。路径点1的坐标为(10,10)，路径点2的坐标为(100,100),全局规划出的预设路径即为两个路径点之间的连线。在matlab中搭建仿真平台如图9；

传统人工势场法仿真试验

首先使用传统人工势场法进行仿真，无人艇出发位置坐标为(10,10)，终点坐标为(100,100)，静态障碍物1坐标为(21,21)，动态障碍物起始坐标为(60,65)，起始速度为(1,-1)，加速度为(0.2,-0.2)。权重系数和加速系数的值首先全部取1，d₀＝2。仿真结果如图10；

图10中可以看出，无人艇出发后在T₁时刻遇到障碍物1，由于系数没有设置好，产生的引力远远大于斥力，导致无人艇的路径直接从障碍物中穿过。而当在时刻T₂遇到动态障碍物时，由于接近了目标点没有一开始那么大，引力和斥力的作用方向相反，陷入局部极小值，无人艇在障碍物附近震荡，无法前进。

基于以上分析，在仿真平台上进行多次仿真实验，以确定基本的各项系数α₁,α₄,j,d₀。出发坐标为(10,10)，终点坐标为(100,100)，静态障碍物1的坐标为(19,18)，静态障碍物2的坐标为(45,44)，静态障碍物3的坐标为(81,79)，动态障碍物的起始坐标为(56,68)，起始速度为(1,-1)加速度为(0.08,-0.08)。经过多次实验取得合适的各项系数后，仿真结果如图11；

经过多次调整后的系数为α₁＝0.8,α₄＝1,j＝2,d₀＝2。由仿真结果可以看出，当T₁,T₂,T₃,T₄时刻遇到各个障碍物时，无人艇都能及时的躲避障碍物并持续朝目标运动，并最终到达目标区域。但是在每个目标点附近存在着运动轨迹震荡现象，这是因为陷入局部极小值区导致的。并且当躲避障碍物以后，是沿着与目标点连线的方向运动的，并没有及时回到预设的路径上，在风电场巡检工程中，这将会影响多波束声呐的成像效果。

改进人工势场法仿真试验

基于以上分析，使用改进的人工势场法进行仿真实验，加入速度势场，回归势场和逃逸势场。

α₁＝0.8,α₂＝1.2,α₃＝1.5,α₄＝1,α₅＝2,j＝2,k＝2,l＝2,m＝2,n＝2,r＝2,d₀＝2。出发位置坐标为(10,10)，终点坐标为(100,100)，静态障碍物1的坐标为(18,16)，静态障碍物2的坐标为(38,39)，静态障碍物3的坐标为(85,83)，动态障碍物的起始坐标为(56,68)，起始速度为(1,-1)加速度为(0.08,-0.08)。

由图12可以看出，无人艇在T₁时刻进入障碍物1的影响范围，在障碍物1的斥力场作用下，向左转向，开始局部避障过程，并且在避障过程中欧冠并没有产生震荡现象。在快要脱离障碍物1的影响范围时，所受的引力场逐渐占据主导地位，引导无人艇重新向着目标点开始运动。

由图13可以看出，无人艇在经过T₁时刻后，由于回归势场的作用，迅速的回到预设路径上，并在遇到障碍物2之前都能沿着预设路径前进。在进入障碍物2的影响范围后。受到障碍物2的斥力作用下，向右转向，开始局部避障过程。在脱离障碍物1的影响范围后，立即向着预设路径运动。

由图14可以看出，在T₃时刻时，与动态障碍物相遇，在改进引力场和斥力场的合场的作用下，右转采取避障规划，在躲避过动态障碍物后，依旧可以立即向着预设路径转向，在预设后继续沿着预设路径向这目标点运动。

由图15可以看出，T₄时刻时，无人艇与障碍物4相遇，同样在引力场合场的作用下成功避开障碍物4。并在绕过障碍物4后，迅速回到预设路径并最终到达终点。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (1)

1.一种用于多波束声呐扫描作业的水面无人艇局部路径规划方法，其特征在于：所述方法具体过程为：

步骤一、基于传统的人工势场法的引力场设计改进引力场，得到水面无人艇航行过程中所受引力的大小和方向；

步骤二、基于传统的斥力场设计改进斥力场，得到水面无人艇航行过程中所受的斥力大小；

步骤三、将步骤一得到的水面无人艇所受引力的大小和步骤二得到的水面无人艇航行过程中所受斥力的大小进行矢量和计算，判断矢量和是否为0，若为0，则执行步骤四；若不为0，则将矢量和叠加在无人艇上，以此完成无人艇的局部路径规划；

步骤四、基于步骤一和步骤二设计逃逸势场，将逃逸势场带入改进引力场，得包含逃逸势场的改进引力场函数，对改进引力场函数取负梯度得改进引力函数；

步骤五、将所受引力和斥力进行矢量和计算后叠加在无人艇上，以此完成无人艇的局部路径规划；

所述步骤一中基于传统的人工势场法的引力场设计改进引力场，得到水面无人艇航行过程中所受引力的大小和方向；具体过程为：

步骤一一、传统的人工势场法的引力场函数为式(1)：

U_attr(s)＝α₁||s-s_g||^j (1)

式中α₁表示在引力场中相对位置对水面无人艇产生引力的权重，j表示可调整的增益系数，s-s_g为USV与障碍物的相对位置，s为USV位置，s_g为引力场中的障碍物位置；

步骤一二、基于传统的人工势场法的引力场设计改进引力场，得到水面无人艇航行过程中所受引力F_attr(s,v,q)的大小和方向；具体过程为：

在传统人工势场法的基础上增加相对速度场和回归势场，那么改进引力场就为相对位置场，相对速度场和回归势场组成的合场，改进引力场函数如式(2)：

U_attr(s,v,q)＝α₁||s-s_g||^j+α₂||v||^k+α₃||q-q_line||^l (2)

式中α₁表示在引力场中相对位置对水面无人艇产生引力的权重，α₂表示在引力场中相对速度对水面无人艇产生引力的权重，α₃表示在引力场中回归势场对水面无人艇产生引力的权重，j,k,l表示可调整的增益系数，q-q_line为水面无人艇的实时位置与全局规划路径的相对距离，q为USV实时的路径位置，q_line为全局规划出的路径位置，其中s＝q；v为相对速度；

所述相对距离取全局规划路径上距水面无人艇的实时位置最近的点的距离；

对改进引力场函数取负梯度得改进引力函数：

式中

式中

为水面无人艇实时位置指向目标点的单位向量，

为USV和障碍物的相对速度的单位向量，

为水面无人艇实时位置指向全局路径最近目标点的单位向量；▽_s、▽_v、▽_q为负梯度的符号，F_attr(s)为相对位置产生的引力，F_attr(v)为相对速度产生的引力，F_attr(q)为USV实时位置与全局规划路径位置的相对位置产生的引力；

所以得改进引力函数为：

F_attr(s,v,q)＝F_attr(s)+F_attr(v)+F_attr(q) (7)

得到最终水面无人艇所受引力F_attr(s,v,q)的大小和方向；

所述步骤二中基于传统的斥力场设计改进斥力场，得到水面无人艇航行过程中所受的斥力大小；具体过程为：

步骤二一、传统斥力场函数如下：

式中α₄为相对位置场在斥力场中的权重系数，s-s_obs为无人艇到障碍物的相对距离，s为USV位置，s_obs为斥力场中的障碍物位置，d₀为障碍物最小影响半径；

步骤二二、令s-s_obs＝p_obs，p_obs为中间变量；

将相对加速度对斥力场的影响加入传统斥力场，得到改进斥力场函数：

式中α₅为相对加速度场在斥力场中的权重系数，m为增益系数，a为相对加速度；

步骤二三、由改进斥力场函数求负梯度得改进斥力函数：

式中

式中

为无人艇实时位置指向障碍物的单位向量，

为USV与障碍物的相对加速度的单位向量；m、n为相应的增益系数，F_rep(p)为相对位置给USV的斥力，U_rep(p)为相对位置给USV的斥力场，U_rep(a)为相对加速度给USV的斥力场，F_rep(a)为相对加速度给USV的斥力；

得到水面无人艇航行过程中所受的斥力大小；

所述步骤四中基于步骤一和步骤二设计逃逸势场，将逃逸势场带入改进引力场，得包含逃逸势场的改进引力场函数，对改进引力场函数取负梯度得改进引力函数；具体过程为：

步骤四一、设计逃逸势场；

逃逸势场函数如下：

U_esc(s)＝α₆||s-s_sec||^r (13)

对逃逸势场函数求负梯度得逃逸力函数：

式中s_sec为无人艇距障碍物最小安全半径；α₆为逃逸力的权重系数，r为逃逸力的增益系数；

步骤四二、将逃逸势场带入改进引力场，得包含逃逸势场的改进引力场函数为：

当无人艇陷入局部极小值区时g＝1，未陷入极小值区时g＝0；

同样的，对引力场函数取负梯度得改进引力函数：

Images