[发明专利]一种考虑无通信的多USV群体协同避碰规划方法

权利要求书

1.一种考虑无通信的多USV群体协同避碰规划方法，其特征在于，该方法包括以下步骤：

步骤1：选取雷达传感器参数构建雷达探测模型，利用雷达模块探测静态障碍物位置和动态USV位置、速度、艏向信息，计算无通信情况下的多USV运动参数及碰撞危险度，判断是否需要采取避碰措施及采取的时间；

在无通信情况下，USV之间只能通过导航雷达测量静态障碍物位置和其他USV位置、速度、艏向参数；取雷达探测到障碍物后的两个连续时刻检测到的障碍物信息，其中第i时刻雷达探测范围内探测出有障碍物的扫描点集合为：

{(X_i1，Y_i1)，(X_i2，Y_i2)，…，(X_in，Y_in)}

其中，探测出的扫描点集合的坐标为大地环境坐标系下的坐标值，且每一时刻探测出的扫描点集合点数不一定相等；

由障碍物的扫描点集合可估算出障碍物的中心位置：

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

相邻时刻障碍物估算位置之间的距离：

这里设置一个阈值，如果则判定障碍物为静态障碍物；如果则判定障碍物为动态USV；d₀为雷达坐标系的原点到障碍物的最短距离；

当障碍物为动态USV时，根据两次障碍物中心估算位置的变化和两个连续时刻的时间间隔Δt估算出动态目标的运动速度和航向：

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

另一USV速度在横纵方向上的分量为：

同理已知本USV的速度和艏向β可计算出横纵方向上的分量那么两USV的相对速度为：

USV之间的相对速度大小：

由估算的中心位置和此时本USV位置可估算出USV之间的相对距离和相对方位：

其中，(X₀,Y₀)为本USV的位置；

USV之间的最近会遇距离DCPA：

其中，DCPA的符号由USV之间的相对位置决定，当另一USV位于本USV的船艏方向时，DCPA为正；当另一USV位于本USV的船尾方向时，DCPA为负；

到达最近会遇地点的时间TCPA：

其中，TCPA的符号由USV之间的相对位置决定，当USV未到达最近相遇地点时，TCPA为正；当USV超过最近相遇地点时，TCPA为负；

空间碰撞危险度是衡量USV之间发生碰撞可能性大小的度量之一，影响空间碰撞危险度大小的因素主要包括USV之间的最近会遇距离DCPA、最近会遇地点相对本USV的距离和方向、另一USV所在方向和距离以及无人航行器之间的最低安全会遇距离；

其中，DCPA为USV之间的最近会遇距离；u为DCPA的隶属函数值；d₁为本USV最晚进行避碰时到另一USV的距离；d₂为恰好构成碰撞紧迫局面时本USV与另一USV的临界距离；当d₂＜|DCPA|时，本USV航行安全；当|DCPA|＜d₁时，本USV与另一USV存在碰撞危险，需要采取避碰措施来消除；当d₁＜|DCPA|＜d₂时，不确定是否存在碰撞危险，需要计算碰撞危险度来判定；

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

时间碰撞危险度是衡量USV之间发生碰撞可能性大小的度量之一，影响时间碰撞危险度大小的因素主要包括两个USV的航行速度、本USV与另一USV的距离、两个USV的长度；

其中，

综合空间碰撞危险度和时间碰撞危险度得出USV系统的碰撞危险度：

其中，的含义为：若u_D＝0或u_T＝0，则u＝0；若u_D≠0且u_T≠0，则u＝max(u_D,u_T)；

步骤2：利用雷达模块探测进入雷达探测范围内的静态障碍物位置和动态USV位置、速度、艏向信息构建无通信情况下遗传方法评价函数；

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

式中，d表示USV当前位置到相对应终点的距离；μ为障碍物边界下限角，即雷达坐标系原点与雷达检测到的障碍物下边界点的夹角；ν为障碍物边界上限角，即雷达坐标系原点与雷达检测到的障碍物上边界点的夹角；δ为另一USV边界下限角，即雷达坐标系原点与雷达检测到的另一USV下边界点的夹角；γ为另一USV边界上限角，即雷达坐标系原点与雷达检测到的另一USV上边界点的夹角；f为评价函数值，其值衡为正，f值越大被选择成为子代的可能性越大；ω_i+1为USV下一时刻取第i个体的艏向角；

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