[发明专利]一种回形障碍物环境下的AUV避障方法

权利要求书

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型声呐为母型，建立前视声呐的数学模型，通过填入矩阵中的数组元素来判断视线范围内的某位置是否存障碍物。