[发明专利]一种基于避碰准则的多USV群体协同避碰规划方法

权利要求书

1.一种基于避碰准则的多USV群体协同避碰规划方法，其特征在于，该方法包括以下步骤：

步骤1：多USV系统建系；

步骤1.1：取空间内的任意一点O为原点，取空间中与O在同一水平面的正北方向为O-X轴正方向，取空间中与O和O-X轴在同一水平面指向正东方向为O-Y轴正方向，建立大地惯性坐标系；

步骤1.2：对于多USV系统中的每一个USV，取USV重心为原点o，取船体上与o在同一水平面指向船艏的方向为o-x轴正方向，取船体上与o和o-x轴在同一水平面指向右舷-的方向为o-y轴正方向，建立船体随动坐标系o-xy；

步骤1.3：对于多USV系统中的每一个USV，取雷达重心为原点o₀，取雷达上与o₀在同一水平面指向船艏的方向为o₀-x₀轴正方向，取雷达上与o₀和o₀-x₀轴在同一水平面指向右舷的方向为o₀-y₀轴正方向，建立雷达随动坐标系o₀-x₀y₀；

步骤2：对于多USV系统中的每一个USV，取本USV雷达探测到障碍物后的2个连续时刻的探测到的障碍物信息，判断障碍物属于动态障碍物还是静态障碍物；若动态障碍物为多USV系统中其他USV，则计算该USV与本USV的碰撞危险度，判断是否需要采取避碰措施及采取避碰策略的种类和时间；

步骤2.1：取本USV雷达探测到障碍物后的2个连续时刻的探测到的障碍物信息，其中第i时刻雷达探测范围内探测出的障碍物的扫描点集合为：{(X_i1,Y_i1),(X_i2,Y_i2),...,(X_in,Y_in)}，探测出的扫描点集合的坐标为大地惯性坐标系下的坐标值；由障碍物的扫描点集合估算障碍物的中心位置：

其中，为第i时刻障碍物中心估计位置横坐标，为第i时刻障碍物中心估计位置纵坐标，X_ij为第i时刻雷达扫描出的第j障碍物扫描点的横坐标，Y_ij为第i时刻雷达扫描出的第j障碍物扫描点的纵坐标；

步骤2.2：估算相邻时刻障碍物位置之间的距离设置阈值d₃，若则判定障碍物为静态障碍物；否则，判定障碍物为动态障碍物；

步骤2.3：若动态障碍物为多USV系统中其他USV，根据两次障碍物中心估算位置的变化和2个时刻的时间间隔估算出该USV的运动速度和艏向

其中，Δt为两个连续时刻的时间间隔；

步骤2.4：根据已知的本USV的速度v_m和艏向β_m，计算作为动态障碍物的其他USV与本USV之间的相对速度V_r；

步骤2.5：计算本USV与作为动态障碍物的其他USV的最近会遇距离DCPA和到达最近会遇地点的时间TCPA；

步骤2.6：计算本USV与作为动态障碍物的其他USV的空间碰撞危险度u_D；当d₂＜|DCPA|时，本USV航行安全，不需要执行避碰；当|DCPA|＜d₁时，本USV与作为动态障碍物的其他USV存在碰撞危险，需要采取避碰措施来消除，执行步骤2.9；当d₁≤|DCPA|≤d₂时，执行步骤2.7；

其中，d₁＝1.5ρ(θ_T)；d₂＝2d₁；

步骤2.7：计算本USV与作为动态障碍物的其他USV的时间碰撞危险度u_T；

其中，

步骤2.8：计算本USV与作为动态障碍物的其他USV的总碰撞危险度u；

其中，的含义为：

若u_D＝0或u_T＝0，则u＝0；

若u_D≠0且u_T≠0，则u＝max(u_D,u_T)；

步骤2.9：判断本USV与作为动态障碍物的其他USV的会遇局势，包括对遇、右船舷小角度交叉相遇、右船舷大角度交叉相遇、左船舷小角度交叉相遇、左船舷大角度交叉相遇、它船右船舷追越、它船左船舷追越；

(1)对遇；

当其他USV从本USV左船舷5°到右船舷5°的扇形方向驶来时，断定为对遇，各自向右转弯从对方左船舷驶过；

(2)右船舷小角度交叉相遇；

当其他USV从本USV右船舷5°～67.5°的扇形方向驶来时，断定为右船舷小角度交叉相遇，交叉相遇的双方需各自向右转向进行避让；

(3)右船舷大角度交叉相遇；

当其他USV从本USV右船舷67.5°～112.5°的扇形方向驶来时，断定为右船舷大角度交叉相遇，这种情况下本USV或其他USV采取左转避让；

(4)左船舷小角度交叉相遇；

当其他USV从本USV左船舷5°～67.5°的扇形方向驶来时，断定为左船舷小角度交叉相遇；这种情况下，只有在其他USV与本USV形成紧迫局面而其他USV未采取避碰措施时才右转避让；

(5)左船舷大角度交叉相遇；

当其他USV从本USV左船舷67.5°～112.5°的扇形方向驶来时，断定为左船舷大角度交叉相遇；在这种情况下，只有在其他USV与本USV形成紧迫局面而其他USV未采取避碰措施时才右转避让；

(6)它船右船舷追越；

当其他USV从本USV右船舷112.5°～180°的扇形方向驶来存在追赶趋势时，并且驶来的USV能看到本USV的尾，断定为它船右船舷追越；在这种情况下，本USV为被追越船，驶来的USV为追越船，在两个USV没有形成紧迫局面前保持原来航行状态，存在紧迫局面时根据两个USV各自的方位来采取不同的转向避让；

(7)它船左船舷追越；

当其他USV从本USV左船舷112.5°～180°的扇形方向驶来存在追赶趋势时，并且驶来的USV能看到本USV的尾，断定为它船左船舷追越；在这种情况下，本USV为被追越船，驶来的USV为追越船，在两个USV没有形成紧迫局面前保持原来航行状态，存在紧迫局面时根据两个USV各自的方位来采取不同的转向避让；

步骤3：选择遗传算法为核心规划算法，设置种群容量，速度变化量和艏向变化量为个体变量，利用浮点数编码方式对其编码；经迭代后得到每一时刻最优的速度改变量和艏向变化量，由其组成一条USV的最优路径；

第j个USV避碰规划的评价函数取为：

其中，f_i为评价函数值，其值衡为正，f_i值越大被选择成为子代的可能性越大；ω_i+1为本USV下一时刻采取的第i个体艏向角；d为本USV当前位置到终点的距离；d₀为本USV的雷达随动坐标系的原点到静态障碍物的最短距离；μ为本USV的静态障碍物的边界下限角，即本USV的雷达随动坐标系原点与雷达检测到的静态障碍物下边界点的夹角；ν为本USV的静态障碍物的边界上限角，即本USV的雷达随动坐标系原点与雷达检测到的障碍物上边界点的夹角；δ为作为动态障碍物的其他USV的边界下限角，即本USV的雷达坐标系原点与雷达检测到的作为动态障碍物的其他USV的下边界点的夹角；γ为作为动态障碍物的其他USV的边界上限角，即本USV的雷达坐标系原点与雷达检测到的作为动态障碍物的其他USV的上边界点的夹角；

评价函数采取奖惩机制增大区分度，当下一时刻的艏向角在障碍物边界上下限角范围内或者作为动态障碍物的其他USV的边界上下限角范围内，继续航行会接近障碍物或其他USV，此时对这样个体的评价函数值缩小100倍使其被选择的概率减小；当下一时刻的艏向角不在障碍物边界上下限角范围内和作为动态障碍物的其他USV的边界上下限角范围内，继续航行能避开障碍物，此时对这样个体的评价函数值扩大100倍使其被选择的概率增大；

获取多USV系统中各USV在该时刻的评价函数值f₁、f₂…f_j…f_n，再取整体的评价函数为每个USV的评价函数f_j保证每个USV此时刻输出速度改变量和艏向变化量的为最优，而整体的评价函数F保证整体所有USV输出的速度改变量和艏向变化量为最优，以解决单一USV算法应用到多USV出现的某一USV最优而其他USV次优的问题。