CN109782807B - 一种回形障碍物环境下的auv避障方法 - Google Patents

Abstract

一种回形障碍物环境下的AUV避障方法，属于水下无人航行器智能控制技术领域。本发明把声呐探测的障碍物数据通过轮廓凸算法和贝塞尔插值将障碍物边界简化，利用预测导引视域航迹段算法设计出回形障碍物环境下的避障航迹。考虑到回形障碍物的复杂多样性，很容易使得AUV陷入障碍物无法逃出的问题，算法结合避障权值函数得出全局最优预测避障参数，利用历史航迹与当前形成的导引视域航迹段，得出相应的避障预测轨迹方向，然后利用导引方法使得AUV成功实现检测到回形障碍物内部的目标，并能逃离出回形障碍物，实验数据结果可以看出本发明所提算法有更好的环境适应性、避障执行效率高，具有成功逃离障碍物陷阱的能力。

Description

一种回形障碍物环境下的AUV避障方法

技术领域

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

背景技术

近年来，多自主水下航行器(Autonomous underwater vehicles)的使用受到了很大的关注，尤其针对浩瀚的海洋，无人自主水下航行器利用自身装备的传感器可以完成远洋深水作业，水下资源勘探，海洋科学考察，军事敌情侦查以及海岸沿线的武器布防任务等。

要想完成在水下环境中存在结构复杂的障碍物时，尤其是针对海洋环境中的结构形回形障碍物时，要想实现进入内部排除可能存在的危险目标时，有必要让AUV进入类似回形结构的障碍物中寻找目标，然后再从复杂结构障碍物中逃离，来保证任务的安全执行。

在复杂环境中的避障研究中，通常把障碍物分为两种状态：凸障碍物和凹障碍物，回形障碍物属于凹障碍物的一种，但是具有更复杂的结构，如果没有适用的避碰算法，AUV很难逃离出这种环境。有些学者提出一种A-RPSO(自适应机器人PSO)算法，它考虑到机器人在执行任务中的避障问题，同时在避障中还拥有一个逃离局部最优的控制机制。还有针对环境中动态障碍物可能表现出不同的轨迹，分析障碍物的轨迹来确定其未来位置实现躲避障碍物问题展开研究，通过估计可能发生机器人和障碍物碰撞的未来区域，达到移动机器人能够在碰撞之前采取纠正措施。该方法给出比较可靠的预测，因为考虑了障碍物行进的过去位置的许多样本点。该算法在较高的障碍物速度条件下取得了较好的效果，但是这种避障算法真是针对动态的凸障碍物进行避碰实验，没有对复杂凹形障碍物进行算法设计。还有来自科学考察的方法，主要是针对改进的视线导引算法对冰山水下几何形状进行了测量。一旦检测到冰山，AUV将进入拍摄模式。边缘检测算法将确定边缘的位置，并且视线导引方法将用于边缘跟踪。同时在进行水下避障的过程中也有学者利用路劲规划的方法实现最优路径的避障，比如利用一种基于流线功能的自主水下航行器避障路径规划方法实现路径的平滑性。还有通过在路径规划的基础上优化避障功能将粒子群优化(PSO)算法与Legendre伪谱法(LPM)相结合，实现了静态障碍物和具有不同位置不确定性水平的移动障碍物的实时碰撞避免。

但是以上提到的方法都是针对规则的凸障碍物分布在环境中的特定情况进行研究或者是针对障碍物与AUV相对模型进行求取最优解达到在AUV航行中的避障目的，并没有针对复杂结构的回形障碍物避障问题提出解决方法，而且也没有针对AUV在回形障碍物内部的逃离问题提出优化避障策略。所以在本专利中针对回形障碍物的避障问题提出一种方法去解决在面对结构复杂的障碍物时的避障问题，同时本发明除了适用于回型障碍物的避障也适用于其他任意的凹型结构障碍物下的避障。

发明内容

本发明的目的在于提供一种回形障碍物环境下的AUV避障方法，该方法安全、有效、可靠，保证AUV在自主航行中，可以安全有效的搜索到回形障碍物内部的目标，同时有效的解决回形障碍物带来的陷阱问题，安全，可靠的逃离回形或其它任意凹型结构障碍物。

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

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

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

步骤二：根据障碍物信息进行障碍物类型分类；

步骤三：对检测到的回形障碍物信息进行障碍物边界数据处理；

步骤四：利用有效障碍物信息进行预测导引视域航迹段计算；

步骤五：记录历史回形障碍物的避障航迹信息；

步骤六：根据步骤五的历史避障航迹的记录对比，得到回形障碍物环境下的避障导引航迹段。

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

所述步骤二中障碍物类型分类依据前视声呐探测到的障碍物信息，利用其相对于AUV的位置状态信息，将出现在AUV视域中的障碍物分为有界障碍物，左有界障碍物，右有界障碍物和左右边缘无界障碍物四种类型；若障碍物的边界是在前视声呐80条波束范围中，那么当前障碍物被认为是有界障碍物；若障碍物的右边界超出了声呐波束范围而左边界在范围中，那么被探测到的障碍物称为左有界障碍物；若障碍物的左边界超出了声呐的探测范围，而右边界在声呐波束范围中，那么称障碍物为右有界障碍物；若障碍物两边都超出了声呐的探测范围，那么称障碍物为无界障碍物。

步骤三中对检测到的回形障碍物信息进行障碍物边界数据处理：在本发明中采用凸包算法和简化的贝塞尔插值算法把障碍物轮廓转化为规则形状，最后进行数据的光滑处理，具体步骤为

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

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

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

(3.3)步骤(3.3)中解算出的凸包集代表障碍物简化的轮廓点集，利用这些点作为贝塞尔插值的边界点产生光化边界点；

(3.4)最后将处理过的边界点进行按声呐波束顺序从左到右排序。

所述步骤四中利用有效回形障碍物信息进行预测导引视域航迹段计算时首先根据声呐探测到的数据进行处理，然后结合AUV避障规则和避障算法权重函数利用条件约束最优避障参数，对障碍物数据进行预测航迹映射。

所述步骤五中记录历史回形障碍物的避障航迹信息依据声呐检测到的障碍物数据经过滤波后，依据以上步骤得到预测映射避障航迹，通过设定记忆单元对历史航迹信息进行记录其中包括AUV走过的航向和路径位置信息，作为AUV在进入回形障碍物内部发现目标后当出现陷入障碍物内部无法逃离的情况，同构利用与当前预测的轨迹点对比有效信息，作为逃离回形障碍物陷阱的历史依据，最后选择正确的预测导引航迹逃离回形障碍物陷阱。

所述步骤六中根据步骤五的历史避障航迹的记录对比，得到回形障碍物环境下的避障导引航迹段，导引避障路径的设计是使用更少的转向位置和转向舵角矫正来实现无碰撞的最短和最平滑的路径，依据声呐检测到的障碍物数据经过滤波后，通过与步骤四种的预测航迹信息的对比，根据避障规则和避障权重函数得到最终的预测导引航迹，最后利用直线导引和圆弧导引跟随避障航迹以实现安全避障路径。

所述避障规则包括两条，规则一为当存在安全避障距离的时候，若是左有界障碍物而且满足安全避障条件，AUV转向障碍物左侧进行避障；若是右有界障碍物而且满足安全避障条件，AUV转向障碍物右侧进行避障；若是有界障碍物或者为无界障碍物时且满足安全避障距离，转向偏离虚拟目标近的方向进行避障；规则二为当出现紧急避障情况时，为保证AUV的安全性，需要制定紧急避障规则来保证AUV的安全性；若出现紧急避障且出现有界障碍或是无界障碍物时，以当前AUV艏向为分界线，推算障碍物边界两侧的探测点那边距离虚拟目标点近，则同时计算这一侧的所有声呐探测点，计算最大转弯半径，选取符合安全避障距离且转弯半径最小的进行避障

式中D_v是AUV距离虚拟目标的距离，R_min为所有符合最大转弯半径中的最小半径，λ_r,λ_l为避障路径偏离目标的右侧和左侧角度，δ是设定的常数影响因子；若出现紧急避障且视域中是有界障碍物，朝向有界的一侧，同时计算转弯半径进行安全且耗能最少的进行避障。

本发明有益效果在于：

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

(2)考虑到复杂回形障碍物特殊障碍物结构特点，对于AUV自身的机械结构和运动的限制因素，因此本发明专门针对这种特殊结构的回形障碍物的避障问题和局部陷阱逃离问题进行了研究，提出一种带有记忆且相比与传统避障算法在处理这种局部陷阱问题的弊端，并且也通过实验证明了该发明种所提的逃离陷阱的方法在障碍物信息局部已知的情况下的高效性和安全性特点；

(3)通过把在线路径规划的视线导引算法利用到回形障碍物避障问题中，本发明创造性的把该方法与预测避障轨迹进行结合形成了专门针对这种特殊结构障碍物的避障问题，最后实现了在复杂回形障碍物环境下安全避障的目的。

附图说明

图1为本发明方法流程图；

图2为回形障碍物环境类型示例；

图3为前视声呐模型；

图4为障碍物分类；

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

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

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

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

图9为回形障碍物环境避障轨迹设计；

图10为回形障碍物环境避障算法验证。

具体实施方式

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

步骤一：获取AUV运动模型、声呐数据模型和障碍物信息；

步骤二：根据障碍物信息进行障碍物类型分类；

步骤三：对检测到的回形障碍物信息进行障碍物边界数据处理；

步骤四：利用有效障碍物信息进行预测导引视域航迹段计算；

步骤五：记录历史回形障碍物的避障航迹信息；

步骤六：根据步骤五的历史避障航迹的记录对比，得到回形障碍物环境下的避障导引航迹段。

针对步骤六提到的内容，在本发明中具体针对回形障碍物进行避障轨迹设计。

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

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

通过建立目标与前视声呐约束关系的数学模型，得到检测物体的数据信息，具体关系如下所示：

其中(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前视声呐视野的警戒区域时，在本发明中规定警戒区域是80m的扇形范围，根据声呐发出的80条波束探测回来的数据进行分类。本发明针对的回形障碍物属于一种结构复杂的凹形障碍物，因为这种障碍物比一般凹形障碍物更加容易令AUV陷入局部陷阱，因此对这类凹形障碍物进行避障研究，当AUV处于回形障碍物内部的避障过程中时，会在声呐视域中出现不连续障碍物数据信息，因此需要通过障碍物数据分类，使得AUV能够通过回形障碍物的间隔。

对于声呐检测到的障碍物信息，需要进行障碍物类型分类，确定出现在视域中的障碍物相对于AUV的位置关系，如图4所示：

(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的距离。

前视声呐检测到的数据被保存在β∈R^80x3的矩阵中，如图5所示，在本发明中只采用声呐开角的水平面那一层，所以只需要γ∈β的第二列数据，其中每一项表示声呐波束检测到的物体点的距离和角度信息。如果γ中的某些元素等于零表示对应的声呐波束没有探测到任何物体。首先就是对γ中数据进行分组，而且每一组的数据被认为是属于一个障碍物的数据。具体分组方法依据公式(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解算出的凸包集就代表障碍物简化的轮廓点集，利用这些点作为贝塞尔插值的边界点产生光化边界点。具体计算如式5：

式中：P_i为插值点P_i＝(x_i,y_i)，在本专利中采用三次贝塞尔插值函数，如式6所示。

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

步骤四：利用有效障碍物信息进行预测导引视域航迹段计算：

利用声呐检测到视域中的部分回形障碍物信息，筛选满足避障距离的障碍物数据。利用步骤二与步骤三得到处理后的视域内回形障碍物信息，结合AUV的提供的艏向角和位置信息，计算发出障碍物映射向量。同时根据公式(7)得到由当前回形障碍物得到的避障最大转弯半径，如图8所示。

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

式中：α_i是AUV艏向与声呐检测到的障碍物点的夹角，ρ_i是障碍物点与当前AUV的距离。R_i是相对于当前AUV的最大转弯半径，∠dAc＝β_i。

根据障碍物映射向量有公式(8)得到预测导引避障航迹，如图9所示橘黄色表示得到的预测导引避障航迹。

式中：P_ov是映射向量，p_gv,m预测导引避障航迹点和声呐探测到的所有有效的障碍物有效范围。

步骤五：记录历史回形障碍物的避障航迹信息：

在本发明中依据声呐检测到的障碍物数据经过滤波后，依据以上步骤得到预测映射避障航迹，通过设定记录单元对历史航迹信息进行记录为步骤六最终的逃离回形障碍物陷阱做准备。在本发明录中依据声呐检测到的障碍物数据经过滤波后，依据以上步骤得到预测映射避障航迹，通过设定记忆单元D∈R^n×2对历史航迹信息进行记录其中包括AUV走过的航向和路径位置信息，作为AUV在进入回形障碍物内部发现目标后当出现陷入障碍物内部无法逃离的情况是，同构利用D与当前预测的轨迹点对比有效信息，作为逃离回形障碍物陷阱的历史依据，最后选择正确的预测导引航迹逃离回形障碍物陷阱。

步骤六：根据步骤五的历史避障航迹的记录对比，得到回形障碍物环境下的避障导引航迹段：

导引避障路径设计的目标是使用更少的转向位置和转向舵角矫正来实现无碰撞的最短和最平滑的路径。在本发明中依据声呐检测到的障碍物数据经过滤波后，通过与步骤四种的预测航迹信息的对比，根据避障规则得到最终的预测导引航迹，最后利用直线导引和圆弧导引(其半径是转弯半径)跟随避障航迹以实现安全避障路径。导引避障路径设计的目标是使用更少的转向位置和转向舵角矫正来实现无碰撞的最短和最平滑的路径。在本发明中依据声呐检测到的障碍物数据经过滤波后，通过与步骤四种的预测航迹信息的对比，根据避障规则和权重函数得到最终的预测导引航迹，因此现制定避障规则和避障权重函数，具体内容如下：

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

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

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

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

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

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

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

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

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

然后建立避障权重函数，为了达到避障问题全局最优目的，因此在本发明中考虑了6个因素对避障带来的影响，分别为：安全性，AUV转艏角度变化率，转弯半径，路径最短，避障区域模式。

(1)安全性的权重函数确定

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

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

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

(2)艏向转角变化率的权重函数

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

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

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

(3)转弯半径的权重函数

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

α_t+m＝a tan2(A_uv,t+m(x,y)-O(x_o,y_o))

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

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

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

(4)路径最短的权重函数

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

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

(5)避障区域的权重函数

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

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

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

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

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

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

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

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

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

最后利用直线导引和圆弧导引(其半径是转弯半径)跟随避障航迹以实现安全避障路径。通过执行下面的避障导引算法，纠正AUV的位置和航向形成安全的避障路径。

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

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

为了解决复杂回形障碍物在实际水下环境带来的困难的局部陷阱问题，提高躲避复杂螺旋形的规避效率，本发明中提出了回形障碍物避障的实时切换的视线连续更新的视线导引机制，来克服回形障碍物带来的陷阱问题。在图9中，AUV围绕复杂回形障碍物的右边进入障碍物的陷阱。一旦前视声呐检测到墙形障碍物时，判断当达到安全避障的要求时，这时墙形障碍物避障算法被激活。通过结合上文提到的数据处理算法，形成黑色障碍物轮廓，这样就提高了复杂障碍物轮廓给避障带来的干扰。

回形障碍物避障算法执行后生成如图9显示的橘黄色的导引航迹，同时为了提高单一算法计算的可靠性，回形障碍物避障点被存放到一个二维的数组中，如图中所示当前AUV的视域中有1和2两个预测点被存储在数组中，这两个点分别是位于中，当执行完橘黄色的线段导引航迹后，前视声呐又会陆续探测到其他的比避障预测点和导引航迹，然后重复上述步骤就可以实现回形障碍物的规避和逃离。

关于躲避回形障碍物算法的结束条件，在这里，设计的虚拟目标点，利用AUV艏向朝向目标(容忍偏离目标角度：)和与目标的相对距离衰减趋势的的方向运动，直到在到达目标的同时也实现安全的避障作为结束条件。

为了验证所提避障算法对于逃离U形障碍物环境的效果，设计了AUV寻找回形障碍物中的目标点并逃离障碍物的整个实验过程。在本发明中设置AUV的起始位置为(700,1200)，艏向角以二维轴作为真北方向被设置为0°，回形障碍物中设定一个红色的圆目标点，它的圆心为(910,1010)，半径大小为10，同时为AUV逃离U形障碍物后设定一个终止位置为(1400,300)。最终实现AUV进入回形从障碍物并且检测到目标后安全逃离障碍物，最后到达指定终点的目的。如图10所示设置了一个复杂回形障碍物组成的二维环境下的凹障碍物环境，设置的迭代步数为4000次。规定当目标点出现在AUV声呐视域80m的范围内时标记目标点，表示AUV已经检测到回形障碍物中的目标，当AUV距离终止位置10m的范围内时，标志AUV成功完成避障任务到达预定终止位置。

本发明中针对回形障碍物设定的避障算法，图10表示AUV进入回形障碍物并且检测到目标并且没有陷入障碍物内部，一次性的逃离障碍物，最后快速的到达指定的终止位置。橘黄色的位算法根据检测到的障碍物数据形成的预测导引航迹，蓝色轨迹表示AUV真实的避障轨迹，在图中可以看到红色目标点已经被绿色标记，表示已经成功检测到目标点。

本发明最终针对回形障碍物环境的避障问题，提出回形障碍物环境下的AUV避障方法，在面对复杂回形障碍物环境，通过实验数据证明了本发明提出的避障方法在复杂回形障碍物环境下避障算法的执行效率与可行性，更能够胜任这种特殊障碍物结构的水下环境，能够使得AUV成功实现回形障碍物避障的目的。

Claims (5)

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

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

步骤二：根据障碍物信息进行障碍物类型分类；

步骤三：对检测到的回形障碍物信息进行障碍物边界数据处理；

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

其中，ψ为AUV当前的艏向；d_i为第i个声呐波束输出的距离；θ_i为第i个波束输出的角度；a_uvx、a_uvy为当前AUV的位置；

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

步骤3.3：步骤3.2中解算出的凸包集代表障碍物简化的轮廓点集，利用这些点作为贝塞尔插值的边界点产生光化边界点；

步骤3.4：最后将处理过的边界点进行按声呐波束顺序从左到右排序；

步骤四：利用有效障碍物信息进行预测导引视域航迹段计算；

步骤五：记录历史回形障碍物的避障航迹信息；

步骤六：根据步骤五的历史避障航迹的记录对比，得到回形障碍物环境下的避障导引航迹段；导引避障路径的设计是使用更少的转向位置和转向舵角矫正来实现无碰撞的最短和最平滑的路径，依据声呐检测到的障碍物数据经过滤波后，通过与步骤四中的预测航迹信息的对比，根据避障规则和避障权重函数得到最终的预测导引航迹，最后利用直线导引和圆弧导引跟随避障航迹以实现安全避障路径；

所述避障规则包括两条，规则一为当存在安全避障距离的时候，若是左有界障碍物而且满足安全避障条件，AUV转向障碍物左侧进行避障；若是右有界障碍物而且满足安全避障条件，AUV转向障碍物右侧进行避障；若是有界障碍物或者为无界障碍物时且满足安全避障距离，转向偏离虚拟目标近的方向进行避障；规则二为当出现紧急避障情况时，为保证AUV的安全性，需要制定紧急避障规则来保证AUV的安全性；若出现紧急避障且出现有界障碍或是无界障碍物时，以当前AUV艏向为分界线，推算障碍物边界两侧的探测点那边距离虚拟目标点近，则同时计算这一侧的所有声呐探测点，计算最大转弯半径，选取符合安全避障距离且转弯半径最小的进行避障；

式中，D_v是AUV距离虚拟目标的距离；R_min为所有符合最大转弯半径中的最小半径；λ_r,λ_l为避障路径偏离目标的右侧和左侧角度；δ是设定的常数影响因子；若出现紧急避障且视域中是有界障碍物，朝向有界的一侧，同时计算转弯半径进行安全且耗能最少的进行避障。

2.根据权利要求1所述的一种回形障碍物环境下的AUV避障方法，其特征在于：所述步骤一中获取AUV状态信息包括位置信息、运动信息与姿态信息，位置信息在本体坐标系下用距离和角度表示，位置信息在北东地坐标系下用坐标表示；运动信息用速度和运动方向表示，运动方向以正北方向为基准，运动信息包括速度大小、速度方向、加速度、角速度和角加速度，姿态信息为舷向角，以正北方向为基准；期望的速度和方向由上层算法给出,方向以正北方向为基准，所述的角度与方向都是顺时针方向为正值；获取障碍物的位置和角度信息，角度和位置信息都是相对于AUV本***置的有效信息，为了验证算法的有效性模拟真实的多波束主动前视声纳数据，以Seabeat6012型声呐为母型，建立前视声呐的数学模型，通过填入矩阵中的数组元素来判断视线范围内的某位置是否存障碍物。

3.根据权利要求1所述的一种回形障碍物环境下的AUV避障方法，其特征在于：所述步骤二中障碍物类型分类依据前视声呐探测到的障碍物信息，利用其相对于AUV的位置状态信息，将出现在AUV视域中的障碍物分为有界障碍物，左有界障碍物，右有界障碍物和左右边缘无界障碍物四种类型；若障碍物的边界是在前视声呐80条波束范围中，那么当前障碍物被认为是有界障碍物；若障碍物的右边界超出了声呐波束范围而左边界在范围中，那么被探测到的障碍物称为左有界障碍物；若障碍物的左边界超出了声呐的探测范围，而右边界在声呐波束范围中，那么称障碍物为右有界障碍物；若障碍物两边都超出了声呐的探测范围，那么称障碍物为无界障碍物。

4.根据权利要求1所述的一种回形障碍物环境下的AUV避障方法，其特征在于：所述步骤四中利用有效回形障碍物信息进行预测导引视域航迹段计算时首先根据声呐探测到的数据进行处理，然后结合AUV避障规则和避障算法权重函数利用条件约束最优避障参数，对障碍物数据进行预测航迹映射。

5.根据权利要求1所述的一种回形障碍物环境下的AUV避障方法，其特征在于：所述步骤五中记录历史回形障碍物的避障航迹信息依据声呐检测到的障碍物数据经过滤波后，依据以上步骤得到预测映射避障航迹，通过设定记忆单元对历史航迹信息进行记录其中包括AUV走过的航向和路径位置信息，作为AUV在进入回形障碍物内部发现目标后当出现陷入障碍物内部无法逃离的情况，同构利用与当前预测的轨迹点对比有效信息，作为逃离回形障碍物陷阱的历史依据，最后选择正确的预测导引航迹逃离回形障碍物陷阱。

Images