[发明专利]一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法

权利要求书

1.一种基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，其特征在于，包括如下步骤：

S1、构建无人艇运动学模型，并结合复杂海面环境构建运动约束条件；

S2、根据海上会遇规则遵守判断、碰撞可能性判断、意识并采取避碰手段判断一系列参数建立评价函数，并结合权重影响避碰责任划分；

S3、采用动态最优互惠避碰算法选择理想机动速度代价函数，在期望速度的基础上加入当前速度和当前角度代价，减少速度变化带来的抖振及损耗。

2.根据权利要求1所述的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，其特征在于，所述步骤S1的具体实现过程如下：

S11、构建无人船运动学模型，如下：

其中，(x,y,ψ)表示无人船在地球坐标系的位置和航向，(u,v,r)分别表示在船体坐标系下无人船的前向、横向速度和航向角速度；

S12、将无人船膨化为以船的中心为圆心的圆，半径与船长相关形成避障过程中不能与障碍物接触的安全范围D，保证无人船安全行驶，对于障碍物同样做膨化处理，安全范围定义如下：

其中，p_T表示障碍物位置；p(t)表示己船在t时刻位置；d_O为一固定常数，具体参数根据船长变化而变化。

3.根据权利要求1所述的基于改进动态最优互惠避碰机制的无人水面艇实时避碰方法，其特征在于，所述步骤S2的具体实现过程如下：

S21、引入速度障碍法：

设受到控制的己船OS的位置和速度为P_O(t)和v_O，目标船只TS的位置和速度设为P_T(t)和v_T，己船OS相对于目标船只TS的相对速度设为v_OT＝v_OT-v_T；

将目标无人船进行膨化处理，设障碍物为半径为R_T的圆，同时将己船OS的半径R_O加入到目标船中，将己船OS作为质点，通过相对速度v_OT将目标船视为静止，从己船OS的位置P_O(t)出发沿相对速度v_OT方向与目标船只的膨化范围D(P,R)相交时无人船在时间τ内一定会碰撞，此时定义障碍速度空间如下：

D(P,R)＝{Q|||Q-P||＜R} (3)

其中，Q表示障碍物位置；P表示己船位置；R表示位置常数，大小由己船与障碍物半径共同决定；

其中，表示避免碰撞速度集合；v表示己船速度；t表示有限时间；D表示速度集合区域；P_T表示障碍物位置；P_O表示己船位置；R_T表示障碍物半径；R_O表示己船半径；

定义X与Y的Minkowski和如下：

对于任意速度V_T,若v_T∈V_T，则A和B在当前速度下，至少在τ时间内是无碰撞的；因此，定义避碰速度集合如下：

其中，V_B表示目标船速度集合；

S22、基于速度障碍法，引入互惠速度障碍法：

基于速度障碍法再次更新v_O，具体更新机制如下：

其中，表示互惠速度障碍法避免碰撞速度集合；表示己船更新速度；VO_O|T(v_T)表示原避免碰撞速度集合；

S23、基于引入的互惠速度障碍法，进一步引入最优互惠速度障碍法：

在互惠速度障碍法的基础上再次优化因此进一步引入优化速度用于从可行解中选出最优解，具体的ORCA定义区域定义如下：

其中，u是使点v_OT-v_T到速度障碍区边缘最近的向量，n是该点的法向量，表示每人承担一半的避碰责任；是一个被直线分隔的半平面，该直线通过点方向垂直于n；是一个被直线分隔的半平面，该直线通过点方向垂直于n；

S24、基于引入的最优互惠速度障碍法，进一步引入通常互惠速度障碍法，具体公式如下：

其中，η_O|T表示常数，η_O|T|(η_O|T∈[0,1])与避碰责任相关联，σ为接近无穷小的数，用以确保避碰需要的时间；使点v_OT-v_T到速度障碍区边缘最近的向量，n是该点的法向量；

S25、基于引入的通常互惠速度障碍法，进一步引入动态最优互惠速度障碍法，使用动态参数对避碰责任进行评价和细化，即：

其中，表示动态速度障碍法避免碰撞速度集合；β表示常数，β∈[0,1]，主要由三部分组成，分别为目标船只TS是否意识到避碰威胁并采取手段、避碰可能性、是否遵守海上会遇规则。