[发明专利]一种基于虚拟膨化的运动障碍与UUV同向航行的规避方法

权利要求书

1.一种基于虚拟膨化的运动障碍与UUV同向航行的规避方法，其特征在于，所述基于虚拟膨化的运动障碍与UUV同向航行的规避方法包括以下步骤：

步骤一：设运动障碍航向与引导航向的夹角为headAngle，当时，UUV与运动障碍为同向航行；引导航向是指从UUV当前位置到下一个非运动障碍航路点的连线所形成的向量，非运动障碍航路点是指不是依托于运动障碍形成的环境中静态环境信息的顶点；

步骤二：当UUV检测UUV与运动障碍为同向航行时，根据运动障碍到UUV当前所在位置的连线与运动障碍前进方向的夹角β，分为UUV追击运动障碍和运动障碍追击UUV两种位置关系；

步骤三：当UUV追击运动障碍时，采用膨化和矩形虚拟障碍调整UUV的艏向，规避运动障碍；

步骤四：当运动障碍追击UUV时，根据UUV与运动障碍相对位置变化，UUV不断调整自身运动速度，并采用膨化和矩形虚拟障碍调整UUV的艏向，规避运动障碍。

2.根据权利要求1所述的一种基于虚拟膨化的运动障碍与UUV同向航行的规避方法，其特征在于所述步骤二中UUV追击运动障碍和运动障碍追击UUV两种位置关系的确定方法为：

当时，为UUV追击运动障碍；当时，为运动障碍追击UUV。

3.根据权利要求2所述的一种基于虚拟膨化的运动障碍与UUV同向航行的规避方法，其特征在于所述步骤三中采用膨化和矩形虚拟障碍调整UUV的艏向，规避运动障碍的过程具体为：

若时，沿引导航向垂直方向生成矩形虚拟障碍，引导UUV从运动障碍后方绕行；若headAngle≤smallAngle时，矩形虚拟障碍沿着运动障碍前进方向生成，引导UUV从运动障碍侧面绕行；|op|为UUV到下一个非运动障碍航路点的直线距离，R为运动障碍进行圆形膨化后的半径，矩形虚拟障碍的长为L，宽为2R。

4.根据权利要求3所述的一种基于虚拟膨化的运动障碍与UUV同向航行的规避方法，其特征在于所述步骤四中规避运动障碍的过程具体为：

步骤四一：UUV和下一个非运动障碍航路点分别在运动障碍航向线的两侧，UUV到运动障碍航向的距离为h,当R≤h≤2R时，且UUV在运动障碍航向上的投影到运动障碍物的距离s满足s＜front_safeL时，矩形虚拟障碍沿运动障碍前进方向生成，触发规划；若s＞front_safeL时，则执行步骤四二；所述front_safeL为安全距离；

front_safeL=2Rvu_max×vb+R]]>

若v_b≤v_u时，使v＝v_{u_max}；v_u为UUV巡航速度，v为UUV航速，v_b为运动障碍速度，v_{u_max}为UUV最大速度；

若v_u＜v_b≤v_{u_max}时，减小UUV航速，使

若v_b＞v_{u_max}时，使v＝v_u；

步骤四二：UUV和下一个非运动障碍航路点分别在运动障碍航向线的两侧，当R≤h≤2R且s＞front_safeL时，矩形虚拟障碍在运动障碍前进方向的长度L为：

L＝R+λ'(s-R)

其中0＜λ'＜1；

步骤四三：当h＜R时，矩形虚拟障碍在运动障碍前进方向的长度为L，调整UUV速度为v＝v_{u_max}；

若v_b＞v_{u_max}，且headAngle＜smallAngle，则在UUV航向前方运动障碍的航线上生成一个与膨化后的运动障碍大小相同的虚拟障碍。