CN111142533B - 多个无人水面艇的多势场避障方法及复杂环境下多usv多模式编队避障控制方法 - Google Patents

Abstract

多个无人水面艇的多势场避障方法及复杂环境下多USV多模式编队避障控制方法，属于多USV多模式编队避障领域，用于解决规划避障路径更为全面，且能够对多USV合理避障的问题，步骤是利用各USV传感器探测，得到局部环境信息，建立局部环境模型；利用局部环境模型，以领航者正前方为参考，划出三个区域，设立不同区域权值，统计出障碍物及可通行区域信息；建立避障模式决策器，以可通行区域信息为输入量，避障模式为输出传递给领航者；领航者根据决策器输出为指示，进行编队避障，效果是实现多USV协同编队避障。

Description

多个无人水面艇的多势场避障方法及复杂环境下多USV多模 式编队避障控制方法

技术领域

本发明属于多USV(无人水面艇)多模式编队避障领域，涉及一种在未知环境下建立多种势场函数利用势场函数进行路径规划，并在多种避障模式中选择适合的避障模式实现编队避障控制的方法。

背景技术

势场函数由局部路径规划方法中的人工势场法而来。人工势场法将USV运动的环境假设为一个虚拟的势场，USV在受到势场的作用进行运动。传统的人工势场主要由目标点产生的吸引势场与障碍物产生的斥力势场组成。引力势场吸引USV朝着目标点运动，使得USV能够准确达到目标点；斥力势场排斥USV靠近障碍物，使得USV能够避开障碍物，从而达到避障的目的。USV在这两个合力的作用下实现局部路径规划。但在现实海洋环境中存在较多的环境干扰因素，为了提高USV的控制稳定性，避免USV在外界的环境干扰下产生偏航与漂移，本发明参考人工势场法的思想，将现实存在的干扰力也量化为一个现实势场，与虚拟势场相结合共同作用在USV上，使得USV在这个多势场的作用下进行运动。

人工势场法在单个USV路径规划中适应性较强，常应用于未知的海洋环境中进行路径规划。然而在多USV编队控制中，由于队伍阵容大，不同于单个USV，需要考虑全体路径。在传统情况下，一般使用保持固定队形或者变换为一字队形的方式实现避障，避障方法比较单一。本发明将多种编队避障模型相结合使用，可在避障前选择避障模式进行编队避障。

发明内容

为了解决规划避障路径更为全面，且能够对多USV合理避障的问题，本发明提出如下技术方案：一种多个无人水面艇的多势场避障方法，包括如下步骤：

1)初始状态下的各无人水面艇处于安全区域，各无人水面艇在目标点的引力势场、现实势场与各成员间的弹性势场的作用下朝着目标点航行，并逐步形成预期队形；

2)航行中各个无人水面艇通过自身姿态与外部传感器探测信息，得出局部环境信息，并通过通信拓扑结构将信息传送给领航者，由领航者无人水面艇融合各个无人水面艇传递的局部环境信息，得到探测的总体局部环境信息，建立局部环境模型；

3)通过局部环境模型获得当前无人水面艇在航行过程中受到的合力，将合力在平面坐标系分解，得到当前无人水面艇的避障航速与航向角。

进一步的，建立局部环境模型的方法是：

1)计算目标点对USV的引力势场：USVi在大地坐标系中的坐标为P_i(x_i,y_i),目标点位于大地坐标系的坐标为P_g(x_g,y_g)，则目标点对USV产生的引力势场为：

其中，ρ_ig＝||P_i-P_g||代表第i条USV与目标点相对于大地坐标系上的距离，||·||为范数；

2)计算障碍物对USVi的斥力势场：

m为势场函数的变化幅值参数，c为势场函数的变化速度参数，ρ_io为第i条USV与障碍物之间的距离，R_safe为安全距离即为需要进行避障行为的参考距离，表示USV与障碍物之间的最小安全距离；

3)计算USVi受到的总的弹性势场：

K_ij表示各个成员之间的弹簧势场系数，n表示除自身外的成员个数；

为USVi与USVj之间的弹性势场，/>为USVi与USVj的弹性势场系数，r_ij为实际距离与预期距离的差，P_ij为USVi与USVj之间的实际距离，ρ_ij为USVi与USVj之间的预期距离；

4)USV受到的外界干扰的总势场为：

U_r＝U_w+U_s

U_w＝k_wu_w ²

U_s＝k_su_s ²

U_r、U_w、U_s分别表示实际势场、海风势场和海流势场，k_w、k_s分别表示海风势场系数、海流势场系数，u_w、u_s分别表示海风风速与海流流速；

5)将势场相加得USVi在环境中受到的总势场，得到局部环境数学模型：

与/>分别表示USVi受到的总势场、引力势场、斥力势场、实际势场与各成员之间的弹簧势场，k_rep、k_att、k_real分别表示引力势场系数、斥力势场系数、实际势场系数；

USV_i受到的总势场的负梯度为USV_i在航行过程中受到的合力：

为USV_i受到的合力，/>与/>分别为USV受到的引力、斥力、外部干扰力与各成员之间弹簧势场力，K_att、K_rep、K_real与K_h分别为USV受到的引力、斥力、外部干扰力与各成员之间弹簧势场力的调节系数；

6)通过得到的合力向量，将合力分解为由x轴与y轴向量力组成的合力，得到USV_i分别在x轴与y轴上的速度，求出USV_i的避障航速υ_i与航向角θ_i。

本发明还涉及一种复杂环境下多USV多模式编队避障控制方法：

1)各个USV间利用无向图的通信方式进行通信，融合各个USV的局部环境信息，得到障碍物信息；

2)将领导者USV正前方分为三个区域，分别为左边区域、右边区域、中心区域，H_l、H_r、H_c分别表示各区域最大可通行宽度，计算由各个障碍物分割出的可通行宽度，并标记其所在的区域，并得出各区域最大可通行宽度；

3)计算多USV队伍的宽度；

4)将当前队伍的阵型宽度、可通行宽度与区域权值作为决策器的输入量，通过决策器计算得出编队避障模式，并将其作为输出量传递给领航者，由领航者发布控制命令，根据避障模式，进行避障。

进一步的，所述的避障的方法是上述方法。

进一步的，计算多USV队伍的宽度的方法如下：

P为多USV队伍最小通行区域宽度，是多USV以当前队形通过时，在海洋环境中运动的队形宽度S为各USV可调节活动范围；D_ij为多USV队伍横向两端USV的预期坐标差，即USV的队形宽度，p_i与p_j分别为USV队形中最左边与最右边USV的坐标，是多USV队伍的编队横向预期宽度；n为横向队伍的USV个数；Do为单个USV可通行最小宽度；radius为USV的船体宽度。

进一步的，通过对比USV编队队形宽度D_ij与安全通行区域宽度，形成避障规则如下：

a)当H_c>P时，可直接朝着中心区域行驶，且安全距离能够满足USV队伍以当前队形通过，使用保持队形避障模式；

b)当D_o<H_c<P时，设n＝2，可得出横向为两条USV的最小通行区域宽度Pt，若P_t<H_c<P,且H_l、H_r均大于P_t时，因为中心区域H_c<P，而无法保证USV当前队形通过，使用分组避障模式，队伍分成多个小组，依据各区域可通行距离宽度，实现各小组USV数量划分，并设置各个小组的编队队形，选择各小组领航者，以设置的编队队形，由各小组领航者带领小组成员进行避障运动，到达安全区域；

c)当D_o<H_c<P_t，D_o<H_r<P_t，D_o<H_l<P_t时，使用个体避障的方式，各USV自成一组，各自为领航者，通过避障算法，自主规划避障路径，达到安全区域；

d)当D_o<H_c<P_t，H_r<D_o，H_l<D_o时，采用队形变换的方式，编队队形变换为竖一字型，模拟领航者运动避障路径，驶出障碍区域。

进一步的，若输出为保持队形编队避障，则保持原来队形不变、领航者不变，领航者根据局部环境信息，利用改进的人工势场函数，计算USV在势场环境中受到的合力方向，以此作为USV的航向，做完当前操作后，进入判断是否达到目标点，若达到则停止航行；反之，则进入判断是否驶出障碍区域，若为驶出则保持队形朝着目标点航行，反之则进入决策器，由决策器输出避障模式，以此循环直至达到目标点停止；

若输出为变换队形避障，则领航者发出变换队形命令，使得队形变为竖一字型，领航者利用改进的人工势场法进行路径规划，其余跟随USV跟随领航者的航行轨迹航行，操作完成后，进入目标点达到，若达到则停止，反之进入是否驶出障碍区判断，若驶出领航者发出队形恢复命令，队伍恢复预期队形；反之则进入决策器，由决策器输出避障模式，以此循环直至达到目标点停止；

若输出为分组避障，则根据中心区域为判断依据，以四条USV队伍菱形队形航行为例，若中心区域无法保证横向宽度为2条USV的队形宽度要求时，则分为三个小组，以左右各为一组，领航者与后面的USV为一组，保持竖一字型队形航行，各小组领航者以人工势场法进行路径规划，并将其余小组USV也作为障碍物处理，避免发生碰撞；下一步进入目标点判断，若小组达到目标点则停止运动，若为达到则判断是否驶出障碍物区域，若驶出则由领航者发出队形恢复命令，各小组以领航者为目标点，朝着领航者靠近，当接近领航者后，各小组进行队形形成运动，以形成预期队形；若未驶出障碍区域，则继续进行避障航行，若个别小组率先驶出障碍区域，而未收到领航者的队形恢复命令，则降低速度朝着目标点缓慢行驶，避免远离队伍，等待领航者命令；在小组朝着领航者航行过程中，均以分组避障模式执行避障处理，若队伍靠近领航者，则执行队形恢复操作，若遇见障碍物，则进入决策器，由决策器选择避障模式，以此循环，直至到达目标点；

4)若输出为个体避障，则各USV独立航行，自主规划路径，并将其余USV作为障碍物处理，当各USV通过障碍物区域后，若未收到领航者队形恢复命令，则朝着目标点缓慢行驶，等待领航者指令；当收到领航者队形命令后，以领航者为目标点，朝着领航者航行，当靠近领航者时，执行队形恢复操作，形成预期队形，进入障碍物判断程序，循环执行，直至到达目标点。

有益效果：本发明的目的在于提供一种复杂环境下多USV多模式编队避障控制方法，能够在避障的时候考虑更为全面的势场，避障更为准确，且通过障碍物信息能够对于多个USV的避障模式进行选择，能够适合多个USV配合避障。

附图说明

图1是本发明的总体协同编队避障流程图。

具体实施方法

下面结合附图举例说明对本发明做详细的描述：

实施例1：假设USV在运动过程中存在海风、海流的干扰，将海风、海流进行量化，得到一个作用于USV上的势场函数。即为现实势场函数；由于是多USV编队控制，因此在运动中还需要防止多USV在运动过程中成员间发生碰撞，因此在各成员间增加一个“弹簧-阻尼”模型，使得成员间存在一个弹性势场，将现实势场、弹性势场与传统人工势场相结合得到一个新的势场，USV在总势场的作用下完成运动，以此建立环境数学模型。

7)计算目标点对USV的引力：

USVi在大地坐标系中的坐标为P_i(x_i,y_i),目标点位于大地坐标系的坐标为P_g(x_g,y_g)，则目标点对USV产生的引力势场为：

其中，ρ_ig＝||P_i-P_g||代表第i条USV与目标点相对于大地坐标系上的距离，||·||为范数。

8)障碍物对USVi(即第i个USV)的斥力势场为：

m为势场函数的变化幅值参数，c为势场函数的变化速度参数。ρ_io为第i条USV与障碍物之间的距离，表示USV与障碍物之间的最小安全距离，超过这个距离则会发生碰撞事故。

9)USVi受到的弹性势场：

其中，为USVi与USVj之间的弹性势场，/>为USVi与USVj的弹性势场系数，r_ij为实际距离与预期距离的差，P_ij为USVi与USVj之间的实际距离，ρ_ij为USVi与USVj之间的预期距离；则USVi受到的总弹性势场为：

其中，K_ij表示各个成员之间的弹簧势场系数。

10)假设USV受到的外界干扰的总势场为：

U_r＝U_w+U_s

U_w＝k_wu_w ²

U_s＝k_su_s ²

其中U_r、U_w、U_s分别表示实际势场、海风势场和海流势场，k_w、k_s分别表示海风势场系数、海流势场系数，u_w、u_s分别表示海风风速与海流流速。

11)将上述势场相加可得USVi在环境中受到的总势场为：

其中，与/>分别表示USVi受到的总势场、引力势场、斥力势场、实际势场与各成员之间的弹簧势场，k_rep、k_att、k_real分别表示引力势场系数、斥力势场系数、实际势场系数；USV_i受到的总势场的负梯度即为USV_i在航行过程中受到的合力，即为：

其中，K_att、K_rep、K_wave与K_h分别为USV受到的引力、斥力、外部干扰力与各成员之间弹簧势场力的调节系数；为USV_i受到的合力。

12)通过得到的合力向量，将合力分解为由x与y轴向量力组成的合力，因此得到USV_i分别在x与y轴上的速度，可求出USV_i的航速：υ_i与航向角θ_i：

13)利用无向图的通信方式进行通信，融合各个USV的局部环境信息，得到障碍物信息，以此建立局部环境数学模型。

14)以领导者USV为参考点，将其正前方分为三个区域。对三个区域设立不同的权值。

15)建立决策器，将各区域中的可通行区域与权值作为决策器的输入量，避障模式作为输出量，以此模式进行编队避障控制。

16)计算多USV队伍的宽度：

其中P为多USV队伍最小通行区域宽度，即多USV以当前队形通过时，在海洋环境中运动的队形宽度。S为各USV可调节活动范围，由于受外部因素影响，会使得USV产生偏移，为防止USV邻居之间产生碰撞，为每个USV均提供一个活动范围，在该范围内为该USV的私人空间。n为横向队伍的USV个数，Dij为多USV队伍横向两端USV的预期坐标差，即多USV队伍的编队横向预期宽度。Do为单个USV可通行最小宽度，radius为USV的船体宽度。

17)通过对比USV编队队形宽度D_ij与安全通行区域宽度，形成避障规则如下：

e)当H_c>P时，可直接朝着中心区域行驶，且安全距离能够满足USV队伍以当前队形通过，因此使用保持队形避障模式。

f)当D<H_c<P时，设n＝2，得出Pt，若P_t<H_c<P,且H_l、H_r均大于P_t时，因为中心区域H_c<P，而无法保证USV当前队形通过，因此使用分组避障模式，队伍分成多个小组，依据各区域可通行距离宽度，实现各小组USV数量划分，并设置各个小组的编队队形，选择各小组领航者，以设置的编队队形，由各小组领航者带领小组成员进行避障运动，到达安全区域。在本发明中，预设各小组保持竖一字型队形，跟随小组领航进行避障航行。

g)当D<H_c<P_t，D<H_r<P_t，D<H_l<P_t时，使用个体避障的方式，各USV自成一组，各自为领航者。通过避障算法，自主规划避障路径，达到安全区域。

h)当D<H_c<P_t，H_r<D，H_l<D时，采用队形变换的方式，编队队形变换为竖一字型，模拟领航者运动避障路径，驶出障碍区域。

18)将各区域中的障碍物信息与权值带入人工势场法中，计算出各个势场力，相结合得出势场合力，以此对USV进行实时路径规划。

实施例2：作为一个独立的方案或者上述实施例的补充方案，本实施例继续公开一种多USV多模式协同编队避障控制方法，其实施例流程图如图1所示，先根据图1对本方法方法做详细说明如下：

5)在初始状态下，各USV处于安全区域则各USV在目标点的引力势场、现实势场与各成员间的弹性势场且没有障碍物的斥力势场的作用下，形成编队队形，并在形成过程中朝着目标点运动，逐步形成预期队形。

6)在航行中，各个USV通过自身姿态与外部传感器探测信息，得出局部环境信息，并通过通信拓扑结构将信息传送给领航者，由领航者融合各个USV传递的局部环境信息，得到探测的总体局部环境信息，建立局部环境模型。

7)通过局部环境模型，判断是否有障碍物，若存在障碍物，计算由各个障碍物分割出的可通行宽度，并标记其所在的区域。

8)计算多USV队伍的宽度：

9)将当前队伍的阵型宽度、可通行宽度与区域权值作为决策器的输入量，通过决策器计算得出编队避障模式，并将其作为输出量传递给领航者，由领航者发布控制命令。

10)根据避障模式，进行避障。

11)若输出为保持队形编队避障，则保持原来队形不变、领航者不变，领航者根据局部环境信息，利用改进的人工势场函数，计算USV在势场环境中受到的合力方向，以此作为USV的航向，做完当前操作后，进入判断是否达到目标点，若达到则停止航行；反之，则进入判断是否驶出障碍区域，若为驶出则保持队形朝着目标点航行，反之则进入决策器，由决策器输出避障模式，以此循环直至达到目标点停止。

12)若输出为变换队形避障，则领航者发出变换队形命令，使得队形变为竖一字型，领航者利用改进的人工势场法进行路径规划，其余跟随USV跟随领航者的航行轨迹航行，操作完成后，进入目标点达到，若达到则停止，反之进入是否驶出障碍区判断，若驶出领航者发出队形恢复命令，队伍恢复预期队形。反之则进入决策器，由决策器输出避障模式，以此循环直至达到目标点停止。

13)若输出为分组避障。则根据中心区域为判断依据，以四条USV队伍菱形队形航行为例，若中心区域无法保证横向宽度为2条USV的队形宽度要求时，则分为三个小组，以左右各为一组，领航者与后面的USV为一组，保持竖一字型队形航行，各小组领航者以人工势场法进行路径规划，并将其余小组USV也作为障碍物处理，避免发生碰撞。下一步进入目标点判断，若小组达到目标点则停止运动，若为达到则判断是否驶出障碍物区域，若驶出则由领航者发出队形恢复命令，各小组以领航者为目标点，朝着领航者靠近，当接近领航者后，各小组进行队形形成运动，以形成预期队形；若未驶出障碍区域，则继续进行避障航行，若个别小组率先驶出障碍区域，而未收到领航者的队形恢复命令，则降低速度朝着目标点缓慢行驶，避免远离队伍，等待领航者命令。在小组朝着领航者航行过程中，均以分组避障模式执行避障处理，若队伍靠近领航者，则执行队形恢复操作，若遇见障碍物，则进入决策器，由决策器选择避障模式，以此循环，直至到达目标点。

14)若输出为个体避障，则各USV独立航行，自主规划路径，并将其余USV作为障碍物处理，当各USV通过障碍物区域后，若未收到领航者队形恢复命令，则朝着目标点缓慢行驶，等待领航者指令。当收到领航者队形命令后，以领航者为目标点，朝着领航者航行，当靠近领航者时，执行队形恢复操作，形成预期队形，进入障碍物判断程序，循环执行，直至到达目标点。

以上所述，仅为本发明创造较佳的具体实施方式，但本发明创造的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明创造披露的技术范围内，根据本发明创造的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明创造的保护范围之内。

Claims (1)

1.一种多个无人水面艇的多势场避障方法，其特征在于，包括如下步骤：

1)初始状态下的各无人水面艇处于安全区域，各无人水面艇在目标点的引力势场、现实势场与各成员间的弹性势场的作用下朝着目标点航行，并逐步形成预期队形；

2)航行中各个无人水面艇通过自身姿态与外部传感器探测信息，得出局部环境信息，并通过通信拓扑结构将信息传送给领航者，由领航者无人水面艇融合各个无人水面艇传递的局部环境信息，得到探测的总体局部环境信息，建立局部环境模型；

3)通过局部环境模型获得当前无人水面艇在航行过程中受到的合力，将合力在平面坐标系分解，得到当前无人水面艇的避障航速与航向角；

其中，所述建立局部环境模型的方法是：

1)计算目标点对USV的引力势场：USVi在大地坐标系中的坐标为P_i(x_i,y_i),目标点位于大地坐标系的坐标为P_g(x_g,y_g)，则目标点对USV产生的引力势场为：

其中，ρ_ig＝||P_i-P_g||代表第i条USV与目标点相对于大地坐标系上的距离，||·||为范数；

2)计算障碍物对USVi的斥力势场：

m为势场函数的变化幅值参数，c为势场函数的变化速度参数，ρ_io为第i条USV与障碍物之间的距离，R_safe为安全距离即为需要进行避障行为的参考距离，表示USV与障碍物之间的最小安全距离；

3)计算USVi受到的总的弹性势场：

K_ij表示各个成员之间的弹性势场系数，n表示除自身外的成员个数；

为USVi与USVj之间的弹性势场，/>为USVi与USVj的弹性势场系数，r_ij为实际距离与预期距离的差，P_ij为USVi与USVj之间的实际距离，ρ_ij为USVi与USVj之间的预期距离；

4)USV受到的外界干扰的总势场为：

U_r＝U_w+U_s

U_w＝k_wu_w ²

U_s＝k_su_s ²

U_r、U_w、U_s分别表示现实势场、海风势场和海流势场，k_w、k_s分别表示海风势场系数、海流势场系数，u_w、u_s分别表示海风风速与海流流速；

5)将势场相加得USVi在环境中受到的总势场，得到局部环境数学模型：

与/>分别表示USVi受到的总势场、引力势场、斥力势场、现实势场与各成员之间的弹性势场，k_att、k_rep、k_real分别表示引力势场系数、斥力势场系数、现实势场系数；

USV_i受到的总势场的负梯度为USV_i在航行过程中受到的合力：

为USV_i受到的合力，/>与/>分别为USV受到的引力、斥力、外部干扰力与各成员之间弹性势场力，K_att、K_rep、K_real与K_h分别为USV受到的引力、斥力、外部干扰力与各成员之间弹性势场力的调节系数；

6)通过得到的合力向量，将合力分解为由x轴与y轴向量力组成的合力，得到USV_i分别在x轴与y轴上的速度，求出USV_i的避障航速υ_i与航向角θ_i。