CN109725650B - 一种密集障碍物环境下的auv避障方法 - Google Patents

Abstract

一种密集障碍物环境下的AUV避障方法，属于水下无人航行器智能控制技术领域。发明把声呐探测的障碍物数据通过轮廓凸算法和贝塞尔插值将障碍物边界简化，利用预测导引避障算法设计出密集障碍物环境下的避障航迹。考虑到密集障碍物的复杂多样性，因此该方法结合避障参数和避障规则制定出避障权值函数得出全局最优预测避障参数，最后制定出相应的避障导引航迹点使得AUV实现避障的目的。实验数据可以看出本发明所提算法针对复杂密集障碍物环境有更好的适应性和避障执行效率，能够更好的利用到水下无人潜航器AUV的水下避障过程中。

Description

一种密集障碍物环境下的AUV避障方法

技术领域

本发明属于水下无人航行器智能控制技术领域，具体涉及一种密集障碍物环境下的AUV避障方法。

背景技术

AUV通常适用于动态、杂乱的海洋环境中，尤其是在近海岸存在结构复杂的密集障碍物时，为了能够穿过密集障碍物群进入到海岸探测信息，因此就需要提升水下无人航行器(Autonomous underwater vehicles)的先进性在面对密集障碍物的海洋环境时能够有能力去寻找一条安全和高效率导航轨迹实现海底的水下作业任务，能够使得AUV从出发位置安全的到达指定的最终的目标位置。显然这种避障算法需要通过各种优化策略去实现避障过程而且还能最大的体现出AUV自身的性能。

近年来，解决以上问题的各种方法得到了很大的发展并将其应用到AUV的水下自主避碰环境当中。这些问题可以根据算法适应的环境分为全局避碰路径规划问题和局部已知而全局未知的避碰方法设计。关于全局避碰路径规划问题，当包含各种障碍物的全局环境信息已知，这种全局避碰路径规划问题就变成已知全局变量信息去寻找全局最优解的非线性最优规划问题。例如非线性规划和启发式算法，另外还有这类优化问题的另一类算法是图搜索方法，包括A*算法和D*算法。另一方面，如果AUV航行的全局未知海洋环境但是对于AUV自身是局部已知的情况，对于这种存在各种类型障碍物的环境需要一种非线性的方法来规划出AUV的避障航迹以确保执行任务过程中的安全，为了解决这个问题比如有人工势场法，还有进化算法例如遗传算法(GA)和粒子群优化算法(PSO)，相对于传统的优化方法，这些进化算法通常可以得到全局最优解或者是接近全局最优解的方法。但是这种进化算法在解决非线性最有问题时会导致数值精度差和和难以处理，同时在算法的迭代周期比较长，不进行优化会很容易陷入局部极小值而无法得到全局最优解。

发明内容

本发明的目的在于提供一种密集障碍物环境下的AUV避障方法，解决在动态、杂乱的海洋环境中，为了提升AUV的先进性和安全性在面对复杂密集障碍物环境时能够有能力去寻找一条安全、高效的航迹来完成海底安全的水下作业任务的技术难题。

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

一种密集障碍物环境下的AUV避障方法，包括如下步骤：

步骤1：获取AUV状态信息和障碍物信息、建立声呐数据模型；

步骤2：通过出现在声呐视域中的障碍物类型对当前障碍物环境分类；

步骤3：把声呐传感器探测到的障碍物信息进行数据处理；

步骤4：预测导引AUV避障算法设计；

步骤5：单凸障碍物进行避障设计原理；

步骤6：根据步骤5针对单障碍物的避障原理，进行密集障碍物环境下的避障航迹的设计。

所述步骤1中获取AUV状态信息包括位置信息、运动信息和姿态信息，所述位置信息在本体坐标系下用距离和角度表示，位置信息在北东地坐标系下用坐标表示；所述运动信息用速度和运动方向表示，运动方向以正北方向为基准，运动信息包括速度大小、速度方向、加速度、角速度和角加速度；所述姿态信息为舷向角，以正北方向为基准，期望的速度和方向由上层算法给出,方向以正北方向为基准；通过建立目标与前视声呐约束关系的数学模型，得到检测物体的数据信息，具体关系为

其中(x_t,y_t,z_t)表示为

式中(x,y,z)为目标在艇体坐标系(Ox₀y₀z₀)下的坐标，(x₀,y₀,z₀)为AUV配置声呐的本体坐标，(x_t,y_t,z_t)表示目标与此AUV的相对位置关系。

所述步骤2中障碍物类型主要分类依据是当障碍物进入到AUV前视声呐视野的警戒区域时，规定警戒区域是80m的扇形范围，通过声呐发出的80条波束探测回来的数据进行分类；共分为有界障碍物，左有界障碍物，右有界障碍物和左右边缘无界障碍物四种类型；若障碍物的边界在前视声呐模型的80条波束范围中，当前障碍物为有界障碍物；若障碍物的右边界超出了声呐波束范围而左边界在范围中，被探测到的障碍物为左有界障碍物；若障碍物的左边界超出了声呐的探测范围，而右边界在声呐波束范围中，障碍物为右有界障碍物；若障碍物两边都超出了声呐的探测范围，障碍物为无界障碍物。

所述步骤3中声呐传感器探测到的障碍物信息的数据每一项表示声呐波束检测到的物体点的距离和角度信息，首先对数据进行分组，每一组的数据被认为是属于一个障碍物的数据，具体分组方法为

式中d_b是波束间距，S_i检测到障碍物点，λ_t是筛选因子，l_e为声呐的探测范围；然后进行障碍物轮廓凸算法数据处理，采用凸包算法和简化的贝塞尔插值算法把障碍物轮廓转化为规则形状，最后进行数据的光滑处理。

所述步骤4中预测导引AUV避障算法设计具体步骤为首先根据障碍物未知信息计算AUV最大避障转弯半径；然后制定AUV避障规则；再根据避障需要满足的条件进行避障算法权重函数的构建；最后根据AUV物理结构以及避障过程条件约束利用预测控制的二次寻优问题计算密集障碍物环境下的最优避障参数，然后进行下一步避障航迹点的设计。

所述步骤5中单凸障碍物进行避障设计原理为导引避障路径设计的目标是使用更少的转向位置和转向舵角矫正来实现无碰撞的最短和最平滑的路径；依据声呐检测到的障碍物设计避障点，并通过直线导引和圆弧导引跟随避障航迹点以实现安全路径。

所述步骤6中密集障碍物环境下的避障航迹的设计具体方法为首先总是朝向虚拟目标点且AUV收益最大的方向运动，同时在AUV运动的过程中保持安全距离以确保AUV的安全性；当AUV的前视声呐检测到大于两个以上的障碍物时，且被检测到的障碍物满足避障范围的条件，那么此刻的AUV面临的环境就被认为是密集障碍物复杂环境；根据上文形成最优避障参数，然后针对密集障碍物的特点，结合算法选择出最优的航迹点，最后形成AUV安全避障航迹。

本发明有益效果在于：

(1)针对复杂密集障碍物环境，提出一种密集障碍物环境下基于预测导引的AUV避障算法，相比于传统的避障方法，不仅兼顾了安全避障距离，而且对AUV的避障航迹的平滑性和整体避障的代价消耗进行了提高，实验数据证明了本发明提出的算法具有明显的低代价和高效率的特点；

(2)考虑到未知水下复杂环境，对于AUV自身而言已知的范围只有声呐探测到的局部已知小范围，因此本发明专门针对这种全局未知而局部已知的情况进行障碍物避障设计，相比与传统避障算法，该发明研究的是在局部已知的情况下的避障决策问题；

(3)考虑到视线导引算法是针对在线路径规划的问题，本发明创造性的把该方法与局部已知且全局未知的预测避障进行结合，实现了在未知密集障碍物环境下安全避障的目的。

附图说明

图1为本发明避障算法流程图；

图2为前视声纳模型；

图3为密集障碍物环境类型示例；

图4为障碍物分类；

图5为前视声呐探测障碍物；

图6为声呐探测数据分组；

图7为凸算法和贝塞尔插值后的障碍物表面；

图8为避障点的期望最大转弯半径；

图9为避障规则流程图；

图10为权重函数松弛前后结果对比；

图11为右有界障碍物避障算法设计的轨迹；

图12为密集障碍物环境避障；

图13为密集障碍物环境避障轨迹。

具体实施方式

下面结合附图对本发明做更进一步描述。

步骤一：获取AUV信息和声呐探测的障碍物信息；

步骤二：通过对出现在声呐视域中的障碍物进行类型分类；

步骤三：把声呐传感器探测到的障碍物信息进行数据处理；

步骤四：预测导引AUV避障算法设计；

步骤五：单凸障碍物进行避障设计原理；

步骤六：根据步骤五针对单障碍物的避障原理，进行密集障碍物环境下的避障航迹的设计。

本发明要解决的技术难题是：在动态、杂乱的海洋环境中，为了提升AUV的先进性和安全性在面对复杂密集障碍物环境时能够有能力去寻找一条安全、高效的航迹来完成海底安全的水下作业任务。因此针对提出的问题为了实现成功躲避密集障碍物的目的，本发明所采用的技术方案是：一种密集障碍物环境下基于预测导引的AUV避障算法。

本发明的目的是通过以下步骤实现的：

步骤一：获取AUV状态信息和障碍物信息、建立声呐数据模型：

在本发明中，所利用的AUV属于欠驱动类型的航行器。本专利中AUV的位置信息在本体坐标系下用距离和角度表示，运动信息用速度和运动方向表示，运动方向以正北方向为基准，获取的AUV信息包括位置信息、运动与姿态信息，位置信息在北东地坐标系下用，坐标表示，运动信息包括速度大小、速度方向、加速度、角速度和角加速度,姿态信息为舷向角，以正北方向为基准，期望的速度和方向由上层算法给出,方向以正北方向为基准，以上所述的角度与方向都是顺时针方向为正值。获取障碍物的位置和角度信息，角度和位置信息都是相对于AUV本体位置的有效信息，为了验证算法的有效性，本发明中模拟真实的多波束主动前视声纳数据，以Seabeat6012型声呐为母型，建立前视声呐的数学模型，通过填入矩阵中的数组元素来判断可视范围内的某位置是否存障碍物。前视声呐模型如图2所示：通过建立目标与前视声呐约束关系的数学模型，得到检测物体的数据信息，具体关系如下所示：

其中(x_t,y_t,z_t)可表示为：

式中：(x,y,z)为目标在艇体坐标系(Ox₀y₀z₀)下的坐标。(x₀,y₀,z₀)为AUV配置声呐的本体坐标，(x_t,y_t,z_t)则表示目标与此AUV的相对位置关系，通过判断(x_t,y_t,z_t)的位置和角度关系来确认目标是否在声呐视域内。

步骤二：通过出现在声呐视域中的障碍物类型对当前障碍物环境分类：

为了提高AUV在执行水下作业任务时的安全性，给AUV的避障带来方便在本发明中通过对障碍物相对于AUV的位置状态，把出现在AUV视域中的障碍物分为四种类型，分别是：有界障碍物，左有界障碍物，右有界障碍物和左右边缘无界障碍物。例如图3列出了未知水域中存在的障碍物分布情况：密集凸障碍物环境。主要分类依据是当障碍物进入到AUV前视声呐视野的警戒区域是，在本文中规定警戒区域是80m的扇形范围，通过声呐发出的80条波束探测回来的数据进行分类如图4所示：图4障碍物分类(a)有界障碍物；(b)左有界障碍物；(c)右有界障碍物；(d)无界障碍物

(1)如果障碍物的边界是在前视声呐模型FLS(Forward looking sonar)的80条波束范围中，那么当前障碍物被认为是有界障碍物简称BO(bounded obstacle)。

(2)如果障碍物的右边界超出了声呐波束范围而左边界在范围中，那么被探测到的障碍物称为左有界障碍物简称LBO(left bounded obstacle)。

(3)如果障碍物的左边界超出了声呐的探测范围，而右边界在声呐波束范围中，那么称障碍物为右有界障碍物简称RBO(right bounded obstacle)。

(4)如果障碍物两边都超出了声呐的探测范围，那么称障碍物为无界障碍物简称为UBO(unbounded obstacle)。

其中k,l为声呐波束探测到的障碍物边界的右侧和左侧边界的波束，δ,

为实常数，S_i,S_j表示探测到障碍物的第i,j个障碍物点距离AUV的距离。

步骤三：把声呐传感器探测到的障碍物信息进行数据处理：前视声呐检测到的数据每一项包含声呐波束检测到的物体点的距离和角度信息。首先就是对数据进行分组，而且每一组的数据被认为是属于一个障碍物的数据。然后进行障碍物轮廓凸算法数据处理：在本发明中采用凸包算法和简化的贝塞尔插值算法把障碍物轮廓转化为规则形状，最后进行数据的光滑处理。

本发明是基于前视声呐(forward-looking sonar)的实时避障策略，环境中的所有障碍物都被认为是未知的，它们的形状是随机生成的，障碍物的轮廓是由FLS的检测数据产生的，考虑到AUV的俯仰角在同一个水平面很少改变的事实，在本专利中采用了本体坐标中的地平线上的多波束扇面。如图5所示紫色线是AUV本体坐标中水平面上的声纳光束，灰色物体是障碍物，蓝色是障碍物轮廓曲线。

前视声呐检测到的数据被保存在β∈R^80x3的矩阵中，采用声呐开角的水平面那一层，所以只需要γ∈β的第二列数据，其中每一项表示声呐波束检测到的物体点的距离和角度信息。如果γ中的某些元素等于零表示对应的声呐波束没有探测到任何物体。首先就是对γ中数据进行分组，而且每一组的数据被认为是属于一个障碍物的数据，具体分组方法依据公式(1)和(2)。在图6中，声呐探测到的数据被分成两组。

式中d_b是波束间距，S_i检测到障碍物点，λ_t是筛选因子，l_e为声呐的探测范围。

然后进行障碍物轮廓凸算法数据处理，在本发明中采用凸包算法和简化的贝塞尔插值算法把障碍物轮廓转化为规则形状，最后进行数据的光滑处理。具体执行步骤如下：

step1：把声呐波束线输出的数据解算成障碍物边界点

其中ψ为AUV当前的艏向，d_i为第i个声呐波束输出的距离，θ_i为第i个波束输出的角度，a_ix,a_iy为当前AUV的位置。

step2：对解算出的障碍物边界点组成的集合X，寻找一个凸包集代替X集合进行，这个凸包集包括所有的X集合中的点。选取集合X中的最左边且是最下边的一个点作为极坐标的原点，对集合中所有的点进行与极坐标原点距离由小到大且满足极角由小到大的原则排序，然后选取排序后的第二个点与极坐标产生的射线，然后，利用向量叉积寻找角度由小到大，若角度相同按距离由近到远的顺序寻找凸包集合。

step3：step2解算出的凸包集就代表障碍物简化的轮廓点集，利用这些点作为贝塞尔插值的边界点产生光化边界点。具体计算如下：

其中P_i为插值点P_i＝(x_i,y_i)，在本文中采用三次贝塞尔插值函数。

step4：最后的处理过的边界点进行按声呐波束顺序从左到右排序，最后结果如图7所示：其中红色线为凸算法处理后的轮廓连线，蓝色的线为最后的处理结果。

步骤四：预测导引AUV避障算法设计：

(1)AUV最大避障转弯半径

当AUV在没有洋流的水下环境中以2m/s的速度航行时，当舵角设定在35°的最大转向角时，最小转弯半径约为AUV长度的五倍，大约需要3.5s的舵角从0°到35°不等。如果考虑转向角过渡的时间延迟，则轨迹偏差距离为1-1.5m，与转弯半径相比较小，以简化偏差距离被忽略的问题，换句话说，轨迹偏差转动被具有一定半径的弧取代。

在图8中，D_i是前视声呐检测障碍物中的最佳避障点，结合当前AUV的状态信息，以及前视声呐采集到的障碍物数据，可以知道前的障碍物点相对于AUV角度与距离信息(α_i，ρ_i)，其中α_i是AUV艏向与声呐检测到的障碍物点的夹角，ρ_i是障碍物点与当前AUV的距离。R_i是相对于当前AUV的最大转弯半径，O是最大转换半径所在圆的圆心，ob是Ac的中垂线，D_s是安全距离，具体定义如下：

∠dAD_i＝α_i，∠dAc＝β_i，

最后得出最大转弯半径如下：

R_i＝ρ_i[cosα_i-sinα_i/tan(2β_i)] (9)

如果AUV是通过围绕障碍物右边缘的进行避障，那么R_i代表第i个声呐波束探测到的障碍物点的最大转弯半径，期望的最大转弯半径表示如下：

R_max＝min{R_i|i＝41,42,...,80} (10)

否则，当AUV围绕障碍物的左边缘进行避障，那么期望的最大转弯半径表示如下：

R_max＝min{R_i|i＝1,2，...，40} (11)

(2)AUV避障规则

避障规则流程图如图9所示，障碍物存在有界障碍物，无界障碍物的安全避障距离，或者存在紧急避障距离，所以在本发明中制定如下规则对存在的两种情况进行处理：

规则1首先是当存在安全避障距离的时候

a.如果是左有界障碍物而且满足安全避障条件，AUV转向障碍物左侧进行避障

b.如果是右有界障碍物而且满足安全避障条件，AUV转向障碍物右侧进行避障

c.如果是有界障碍物或者为无界障碍物时且满足安全避障距离，转向偏离虚拟目标近的方向进行避障

规则2当出现紧急避障情况是，为保证AUV的安全性，需要制定紧急避障规则来保证AUV的安全性。

a.如果出现紧急避障且出现有界障碍或者是无界障碍物时，以当前AUV艏向为分界线，推算障碍物边界两侧的探测点那边距离虚拟目标点近，则同时计算这一侧的所有声呐探测点，根据前面提到的方法计算最大转弯半径，选取符合安全避障距离且转弯半径最小的进行避障

b.

其中D_v是AUV距离虚拟目标的距离，R_min所有符合最大转弯半径中的最小半径，λ_r,λ_l避障路径偏离目标的右侧和左侧角度，δ是设定的常数影响因子。

c.如果出现紧急避障且视域中是有界障碍物，朝向有界的一侧，同时计算转弯半径进行安全且耗能最少的进行避障。

(3)避障算法权重函数的构建

依据预测控制模型的原理，本文中规定预测步长为M，为了确保AUV安全性，同时取得理想的避障航迹，达到全局最优的目的，因此在本中考虑了6个因素对避障带来的影响，分别为：安全性，AUV转艏角度变化率，转弯半径，路径最短，避障区域模式。

a.安全性的权重函数确定

为了提高AUV避障的安全性，因此必须考虑AUV选择则的参考障碍物点与当前AUV距离和安全距离的关系，因此有如下关系：

其中d_s为AUV与声呐探测到的障碍物边界点的相对距离，D_s为安全距离，γ_s表示安全阈值，k₁为权重系数。

显然引入这个权重函数可以优化这个问题对避障算法带来的影响，可以逃离因单一因素引起的局部避障最优的现象。

b.艏向转角变化率的权重函数

为防止控制过程中,艏向转角的抖动,此处引入转艏变化率的影响,以实现平顺的AUV转艏控制效果,并定义如下权重函数：

f_z,value＝k₂(ψ_t+m+1-ψ_t+m) (15)

其中k₂为权值系数,ψ_t+m为某时刻的AUV艏向。

c.转弯半径的权重函数

本发明中利用预测航迹点进行导引控制实现局部避障，根据上文提出的转弯半径的求取方法，不能够直接利用得到的最大转弯半径作为导引的参数，因为转弯半径只是影响避障性能的一个因素，因此这里引入转弯半径权重函数，作为全局最优评价函数的一个参数，具体权重函数如下：

f_l,value＝k₃||A_uv(x_t+m,y_t+m)-T_arget(x,y)||₂ (17)

其中γ为AUV所在最大转弯半径的位置与以圆心为极坐标原点的夹角，R转弯半径，

表示最大转弯半径所在的障碍物点的坐标，A_uv,t+m(x,y)为AUV避障航迹的实时位置，T_arget(x,y)为虚拟目标点，k₃为权重系数。

d.路径最短的权重函数

为了保证在可预测范围内，AUV能够到达期望的虚拟目标点并保持行驶路径最短，可以定义路径最短权重函数。

其中T表示到达期望目标所花费的时间，A_uv,t+m(x,y)为m时刻的AUV位置信息，k₄为权重系数。

e.避障区域的权重函数

为了提高避障的可靠性，在本文中通常把声呐视域的扇形范围分为三类，分别为非避障区域，一般安全避障区域和紧急避障区域，因此得判断当前检测到的障碍物属于哪个区域中的障碍物，然后采取不同的避障策略，因此定义避障区域的权重函数。

其中d_i为障碍物边界点距离AUV当前位置的距离，l_e,l_s,l_d,D_s分别为声呐可探测最长距离，声呐视域中的安全避障区域最大距离，紧急避障区域最大范围和安全距离，k₅权重系数。

(4)AUV物理结构以及避障过程条件约束

受AUV自身的物理结构特点和在避障环境中带来的影响，在进行全局优化时需要满足几点约束条件：

a.根据AUV物理设计的结构特性，其最大转艏角度满足:-35°≤ω_t≤35°；

b.为了保证AUV的安全性，设置安全距离满足条件：5l_o≤D_s≤6l_o，其中l_o为AUV的长度；

c.最大转弯半径满足条件：R∈[R_max,R_min]。

(5)预测控制的二次寻优问题：

结合权重函数，模型参数和避障原理，得到基于预测控制的二次寻优问题，

由于公式(17)由于其引入了非连续的布尔变量作为权重函数,会导致目标函数(18)为非凸。相应的问题就变成混合整数的非线性规划(Mixed integer nonlinearprogramming,MINLP)问题,但目前为止这类问题并没有找到一种统一、成熟的求解方法。但对式(17)进行观察可见,其引入的布尔变量仅用于权重函数的评价,而并没有出现在需要优化的状态变量或者控制变量,因此本质上仍属于带约束的非线性规划(Nonlinearprogramming,NLP)问题。显然,求解NLP问题有很多优秀算法。但是,由于权重函数(17)为非凸,因此这类NLP问题只能得到局部最优解。为了削弱布尔变量影响,本文中利用Sigmod型函数中的双曲线正切函数将权重函数(17)的布尔变量松弛到连续空间,，如下：

其中μ为S型函数的斜率，d_i为可行域的连续变量。

松弛前后的权重函数如图8所示，显然具有较好的近似度,因此保留了原有权重函数的性质。而且,布尔变量进行松弛后得到了连续的凸问题,显然凸NLP问题可以求得全局最优解。由此,我们可使用现有的序列二次规划(Sequential quadraticprogramming,SQP)方法进行求解。

步骤五：单凸障碍物避障设计原理：

导引避障路径设计的目标是使用更少的转向位置和转向舵角矫正来实现无碰撞的最短和最平滑的路径。在本文中依据声呐检测到的障碍物设计了几个避障点，并通过直线导引和圆弧导引(其半径是转弯半径)跟随避障航迹点以实现安全路径。

在图9中前视声呐探测到障碍物为右有界情况，利用前面提到的避障规则推算出合适的避障点使得AUV沿着最佳避障点绕过障碍物，具体执行步骤如下：

step1利用声呐检测到的物体数据结构，依据障碍物数据分组，确定障碍物的数量和类型。

step2依据权重函数得出的最优参数，计算声呐波束检测的障碍物点与当前AUV的关系，并利用上文提到的方法计算所有点的坐标信息：

P_j,i,min＝min(D＝[(O_ix,O_iy),i＝1,...,m]),m≤n (22)

P_j,i,max＝max(D＝[(O_ix,O_iy),i＝1,...,m]),m≤n (23)

Q_minj,i＝{(P_{j,i,min_x},P_{j,i,min_y}),d_j,i,min,θ_j,i},θ_j,i∈[ψ-π/3,ψ+π/3] (24)

Q_maxj,i＝{(P_{j,i,max_x},P_{j,i,max_y}),d_j,i,max,α_j,i,},θ_j,i∈[ψ-π/3,ψ+π/3] (25)

其中P_j,i,min是所有检测到障碍物波束点中距离AUV距离最小的波束点，D是所有波束点的集合，Q_minj,i是最小点的所有信息集合，Q_maxj,i是距离最大点的所有信息集合，ψ为当前AUV的艏向。

step3计算当前点是在以AUV艏向为分界的声呐中线的左边还是右边，如果是在左边并且利用步骤1分类的结果，例如j＝1，说明是连续的单一障碍物，同时判断d_j,i,min是与一般避障距离l_s和警戒避障距离l_d的关系:

其中η_obs是障碍物占据的声呐视域的占有率，SOA是安全避障范围，EOA是紧急避障范围，Hobs是初级声呐视域占有率，OHobs中级声呐视域占有率，Eobs是紧急声呐视域占有率。

step4如果P_j,i,min∈[S_i,i＝1,...,40]，同时满足SOA和Hobs条件，那么预测的避障导引点推导如下，计算后，转到步骤6执行。

如果P_j,i,min∈[S_i,i＝41,...,80]，同时满足SOA和OHobs条件，那么预测的避障导引点如下，计算后，转到步骤6执行。

其中δ，ε是障碍物视域占有剩余率。

step5如果P_j,i,min∈[S_i,i＝1,...,40]，同时满足SOA和Eobs条件，那么预测的避障导引点为P_j,i,guide＝S₈₀(x,y)，转到步骤7执行。

如果P_j,i,min∈[S_i,i＝1,...,40]，同时满足SOA和OHobs条件，那么预测的避障导引点为P_j,i,guide＝S₈₀(x,y)，转到步骤7执行。

如果P_j,i,min∈[S_i,i＝41,...,80]，同时满足SOA和Eobs条件，那么预测的避障导引点为P_j,i,guide＝S₁(x,y)，转到步骤7执行。

如果P_j,i,min∈[S_i,i＝41,...,80]，同时满足SOA和OHobs条件，那么预测的避障导引点为P_j,i,guide＝S₁(x,y)，转到步骤7执行。

step6依据上面步骤得到的避障航迹点，执行下面的避障导引算法，纠正AUV的位置和航向形成安全的避障路径。

其中α(t)的选取有一定的规则：当AUV的当前位置距离期望路径比较远时，及ε(t)＞Δ，此时前视向量与路径没有交点，选取α(t)为垂直于路径方向，大小为π/2；当AUV的当前位置距离期望路径比较近时，选取α(t)＝asin(ε(t)/Δ)，即

其中A_uv(x,y)是AU当期那的位置，A_uv(x(t),y(t))是AUV的实时避障位置，δ(t)为AUV当前位置到避障路径端点连线与路径的夹角。避障航迹点最大转弯半径R_i,max所在圆的圆心，ψ_d是期望的艏向，β_i是避障航迹点与正北方向的角度，ε(t)是圆心与AUV的距离，α(t)为前视向量与避障路径的夹角。

step7依据上面步骤得到的避障航迹点，执行下面的避障导引算法，纠正AUV的位置和航向形成安全的避障路径。

此处α(t)的选取如下：

其中P(x_o,y_o)是避障航迹点最大转弯半径R_i,max所在圆的圆心，ψ_d是期望的艏向，β_i是避障航迹点与正北方向的角度，ε(t)是圆心与AUV的距离，α(t)为前视向量与AUV到圆心的夹角。

step8结束。

步骤六：密集障碍物环境下的避障航迹的设计：

密集障碍物环境下的避障算法是实时的躲避障碍物的算法，可以在AUV执行水下作业任务时确保AUV的安全性，穿越障碍物水域，其中包括条件有：

(1)总是朝向虚拟目标点且AUV收益最大的方向运动，AUV配置的前视声呐没有检测到障碍物或者是检测到的障碍物不满足避障范围时，AUV要朝向获得收益最大的目标点方向运动。

(2)保持安全距离：当声呐检测到的障碍物满足避障范围，而且已经获取当前视域中的障碍物类型数量，在AUV躲避障碍物的同时也要考虑到最小安全距离，在本文中AUV的长度是5m，安全距离一般选取4倍的AUV长度，所以在选取避障导引点的时候要判断是否满足安全距离，以确保AUV的安全性。

(3)密集障碍物环境情况：AUV的前视声呐检测到大于两个以上的障碍物时，以上两种提到的避障方法是不可以完全胜任的，需要结合导引避障算法、规则相结合和多障碍物分类的不同情况综合进行评价，来实现多障碍物环境的安全行驶。

密集障碍物环境避障航迹设计：当出现在AUV前视声呐视域中的障碍物数量大于等于两个的时候，且被检测到的障碍物满足避障范围的条件，那么此刻的AUV面临的环境就被认为是密集障碍物复杂环境。

任何进入到前视声呐的80m避障区域内都被认为是威胁AUV安全性的障碍物，根据对出现在声呐视域中的障碍物进行分组，每一组代表一个障碍物，并且基于障碍物轮廓处理算法，分别对每一个障碍物轮廓进行简化，如图10中所示是经过处理后的出现在AUV视域中的障碍物群。在本文中，单纯延用上文中提出的单障碍物避障算法和墙形障碍物避障算法是不能解决当前视域中的群障碍物环境带来的问题，因此如下面为算法公式所示，可以选择出最优的航迹点，然后依据上文的避障算法形成安全路径。

G_uide＝{P_j,i,guide(x,y)|j＝1,...,m+1；i∈[1,80]} (40)

G_ap＝{l_i,i＝1,...,m+1} (41)

P_guide＝f(G_ap,λ_c,i) (44)

其中G_uide表示所有障碍物间隙的可能存在的导引航迹点，G_ap是所有障碍物间距，λ_s,i,λ_c,i分别代表间距宽度影响因子和航迹点选择因子，γ_c为衰减因子，ρ_d,i,l_o代表航迹点与AUV的距离和AUV的长度，f最优导引航迹点的评价函数，m为检测到的障碍物数量，在图10中m＝4。

考虑到转弯半径的大小对AUV躲避障碍物的安全性的的考虑，所以不能够当发现最大的障碍物间隙就选择当前的间隙中的某一个位置作为到导引航迹点，这样会带来能耗上的损失，而且也会偏离目标点。所以在选择衰减因子和评价函数的时候都要综合考虑各种因素，在这里选择γ_c＝0.5，f选择分段线性函数，符合航迹点选择中权值带来的影响。在图10中障碍物蓝色的部分在现实中声呐是无法探测到的区域，所以对AUV而言是无法区分1号和2号障碍物，在声呐视域中是属于一块区域。但是真实的情况中障碍物室友1和2好障碍物拼接而成的，因此需要利用相应分割算法进行不同障碍物的分类，这无疑在现实中对于避障是十分有利的。在本文中利用步骤三中数据处理方法可以实现图10中的障碍物1和2号进行分割，从而得到障碍物间距l₂和预测导引点P_2,25,guide，因此增加可选择的AUB导引航迹点，提高避障的安全性和选择性。

为了验证本专利方法有的有效性，设计了仿真案例，设计了AUV为到达虚拟目标点需要进行密集凸障碍物环境避障。如图11所示，设置AUV的起始位置为(180,180)，艏向角以二维轴作为真北方向被设置为0°，虚拟目标点为红色的圆，它的圆心为(1800,1800)，半径大小为40，同时在二维环境中设置了15个凸障碍物组成密集凸障碍物环境，设置的迭代步数为4000次，规定当AUV与虚拟目标点的距离为10m的时候，结束避障过程，同时结束到达目标的任务，标志AUV已经到达目标成功完成避障任务。

图11表示PGOA算法在密集障碍物环境下形成的避障航迹。图中粉色的圆圈表示形成的预测航迹点，从图中可以看出AUV选择很合理的避障路径到达虚拟目标点没有带来避障风险，同时保持安全避障距离的要求，花费的整个避障代价控制在1500以内，使得整个避障过程消耗的代价值很小。通过实验结果可以得到本文所提避障算法在密集障碍物障碍物环境有很大的优势。

本发明最终针对密集障碍物环境的避障问题，提出密集环境下基于预测导引的AUV避障方法，在面对复杂密集障碍物环境，通过实验数据证明了本发明提出的避障方法在复杂密集障碍物环境下避障算法的效率与可行性，更能胜任这种随机障碍物的水下复杂环境，能够使得AUV灵活应对不同的外界环境。

Claims (5)

1.一种密集障碍物环境下的AUV避障方法，其特征在于，包括：

步骤一、获取AUV状态信息和障碍物信息、建立声呐数据模型；

步骤二、通过出现在声呐视域中的障碍物类型对当前障碍物环境分类；

步骤三、把声呐传感器探测到的障碍物信息进行数据处理；

所述的声呐传感器探测到的障碍物信息的数据每一项表示声呐波束检测到的物体点的距离和角度信息，首先对数据进行分组，每一组的数据被认为是属于一个障碍物的数据，具体分组方法为：

式中，d_b是波束间距；S_i检测到障碍物点；λ_t是筛选因子；l_e为声呐的探测范围；然后进行障碍物轮廓凸算法数据处理，采用凸包算法和简化的贝塞尔插值算法把障碍物轮廓转化为规则形状，最后进行数据的光滑处理；

步骤四、预测导引AUV避障算法设计；

首先根据障碍物未知信息计算AUV最大避障转弯半径；然后制定AUV避障规则；再根据避障需要满足的条件进行避障算法权重函数的构建；最后根据AUV物理结构以及避障过程条件约束利用预测控制的二次寻优问题计算密集障碍物环境下的最优避障参数，然后进行下一步避障航迹点的设计；

步骤五、单凸障碍物进行避障设计原理；

步骤六、根据步骤五针对单障碍物的避障原理，进行密集障碍物环境下的避障航迹的设计。

2.根据权利要求1所述的一种密集障碍物环境下的AUV避障方法，其特征在于：所述步骤一中获取AUV状态信息包括位置信息、运动信息和姿态信息，所述位置信息在本体坐标系下用距离和角度表示，位置信息在北东地坐标系下用坐标表示；所述运动信息用速度和运动方向表示，运动方向以正北方向为基准，运动信息包括速度大小、速度方向、加速度、角速度和角加速度；所述姿态信息为舷向角，以正北方向为基准，期望的速度和方向由上层方法给出,方向以正北方向为基准；通过建立目标与前视声呐约束关系的数学模型，得到检测物体的数据信息，具体关系为

其中(x_t,y_t,z_t)表示为

式中(x,y,z)为目标在艇体坐标系(Ox₀y₀z₀)下的坐标，(x₀,y₀,z₀)为AUV配置声呐的本体坐标，(x_t,y_t,z_t)表示目标与此AUV的相对位置关系。

3.根据权利要求1所述的一种密集障碍物环境下的AUV避障方法，其特征在于：所述步骤二中障碍物类型主要分类依据是当障碍物进入到AUV前视声呐视野的警戒区域时，规定警戒区域是80m的扇形范围，通过声呐发出的80条波束探测回来的数据进行分类；共分为有界障碍物，左有界障碍物，右有界障碍物和左右边缘无界障碍物四种类型；若障碍物的边界在前视声呐模型的80条波束范围中，当前障碍物为有界障碍物；若障碍物的右边界超出了声呐波束范围而左边界在范围中，被探测到的障碍物为左有界障碍物；若障碍物的左边界超出了声呐的探测范围，而右边界在声呐波束范围中，障碍物为右有界障碍物；若障碍物两边都超出了声呐的探测范围，障碍物为无界障碍物。

4.根据权利要求1所述的一种密集障碍物环境下的AUV避障方法，其特征在于：所述步骤五中单凸障碍物进行避障设计原理为导引避障路径设计的目标是使用更少的转向位置和转向舵角矫正来实现无碰撞的最短和最平滑的路径；依据声呐检测到的障碍物设计避障点，并通过直线导引和圆弧导引跟随避障航迹点以实现安全路径。

5.根据权利要求1所述的一种密集障碍物环境下的AUV避障方法，其特征在于：所述步骤六中密集障碍物环境下的避障航迹的设计具体方法为首先总是朝向虚拟目标点且AUV收益最大的方向运动，同时在AUV运动的过程中保持安全距离以确保AUV的安全性；当AUV的前视声呐检测到大于两个以上的障碍物时，且被检测到的障碍物满足避障范围的条件，那么此刻的AUV面临的环境就被认为是密集障碍物复杂环境；根据上文形成最优避障参数，然后针对密集障碍物的特点，选择出最优的航迹点，最后形成AUV安全避障航迹。

Images