[发明专利]一种基于航迹重规划的水面无人艇动态避碰方法

权利要求书

1.一种基于航迹重规划的水面无人艇动态避碰方法，其特征在于，包括以下步骤：

S1、采集艇载传感器的实时数据，获取邻域航行船舶的几何尺寸信息、运动信息和水面无人艇本体的位姿信息；

S2、结合邻域航行船舶的几何尺寸信息、运动信息和水面无人艇本体的位姿信息，采用方向包围盒描述邻域航行船舶和水面无人艇的几何信息，建立水面无人艇和邻域航行船舶发生碰撞时的速度集合，即构造水面无人艇和邻域航行船舶之间的碰撞锥；

S3、引入对邻域航行船舶速度信息观测的不确定度，将碰撞锥向外平移一个不确定度上界值的量，即对碰撞锥施加一层软约束，以约束外的速度集合为无碰速度集；

S4、结合水面无人艇本身的动力学特性，求解水面无人艇的速度和艏向限制范围，与无碰速度集进行交集运算，获得水面无人艇的可行无碰速度集；

S5、结合国际海上避碰规则的约束，根据不同的会遇局面对可行无碰速度集进行划分，获得最终的候选速度集合；

S6、对候选速度集合进行离散化处理，引入代价函数对其中的每个速度进行评价，选择代价最小的速度为最优避碰速度；

S7、以最优避碰速度进行航行模拟的内循环，得到水面无人艇动态避碰的航迹重规划点；

其中，所述步骤S2具体为：沿着水面无人艇和邻域航行船舶的主成分方向生成最小的矩形包围框来分别统一相应描述水面无人艇和邻域航行船舶的形状，将水面无人艇自身的几何尺寸沿着航行方向叠加到邻域航行船舶上，具体过程为以邻域航行船舶的矩形包围框边线上的点为几何中心，叠加水面无人艇的矩形包围框，将叠加后的最外围区域视为障碍物区域，从而将水面无人艇视为质点；从质点出发，作障碍物区域的两条切线，将所述两条切线包围的区域为速度障碍区域，所述切线为一条从水面无人艇几何中心P_u沿相对速度v_r出发的射线：

λ(P_u,v_r)＝{P_u+v_rt|t≥0} (1)，

(1)式中t代表时间，v_r为水面无人艇相对于邻域航行船舶的速度v_r＝v_u-v_o，P_u和v_u分别表示水面无人艇的位置矢量和速度矢量，v_o表示邻域航行船舶的速度矢量；当射线λ(P_u,v_r)与速度障碍区域相交时，也即射线落在速度障碍区域的两条切线之间时，认为水面无人艇与邻域航行船舶在某一时刻必然发生碰撞；定义导致水面无人艇和邻域航行船舶发生碰撞的相对速度v_r集合为相对速度集下的碰撞锥RCC_UO；定义导致水面无人艇和邻域航行船舶发生碰撞的所有水面无人艇绝对速度v_u集合为绝对速度集下的碰撞锥VO_UO：

(2)式中，为闵可夫斯基矢量和运算，实际物理意义是将相对碰撞区域RCC_UO沿着邻域航行船舶速度v_o进行平移；

所述步骤S3具体为：在构建碰撞锥后引入邻域航行船舶的速度不确定度δ_O，将邻域航行船舶速度参量中的所有不确定部分都归为集合{W_O|δ_O∈W_O}，W_O集合是一个有界集合且其上界为常数，将碰撞锥向外平移一个不确定度上界值的量，故而可以构建不确定度δ_O最大时的碰撞锥区域WVO_UO：

将WVO_UO外的速度集合记为无碰速度集；

所述步骤S4具体为：结合水面无人艇本身的动力学特性，引入动态窗口的概念对水面无人艇的可行速度空间做进一步的约束，给定一个时间窗口Δt，计算水面无人艇在Δt时间内可以达到的速度窗口v_d和角速度窗口ω_d，

(4)式和(5)式中，ν和ω分别为水面无人艇的速度和角速度；v_c和ω_c分别表示水面无人艇当前的速度和角速度，和分别为水面无人艇加速情况下的最大加速度和角加速度，和分别为水面无人艇减速情况下的最大加速度和角加速度；其中，

(6)式和(7)式中，R_T为水面无人艇和邻域船舶之间的相对距离，以使水面无人艇在碰到会遇船舶之前能够停下来；

水面无人艇在Δt时间内可以达到的艏向窗口θ_d：

(8)式中，θ表示水面无人艇的艏向角；θ_c表示水面无人艇当前的艏向角；

通过速度窗口v_d和艏向窗口θ_d即可构建水面无人艇的可行速度集，与步骤S3获得的无碰速度集进行交集运算，获得无人艇的可行无碰速度集RAV。