CN110362074B - 一种基于航迹重规划的水面无人艇动态避碰方法 - Google Patents

Abstract

本发明公开了一种基于航迹重规划的水面无人艇动态避碰方法，包含以下步骤：通过艇载传感器获取邻域船舶的航行信息和无人艇自身的位姿信息；构造无人艇和邻域船舶之间的碰撞锥；引入对邻域艇舶运动信息观测的不确定度，对碰撞锥施加一层软约束；施加无人艇的速度和艏向限制范围；获得最终的候选速度集合；引入代价函数选择最优避碰速度；以最优避碰速度进行航行模拟的内循环，得到无人艇动态避碰的航迹重规划点。本发明将水面无人艇的动态避碰决策以航迹重规划的形式输出，满足国际海上避碰规则的约束，能够很好的适应无人艇自身的操纵控制，满足无人艇的动态避碰需求。

Description

一种基于航迹重规划的水面无人艇动态避碰方法

技术领域

本发明涉及无人艇自主智能控制的技术领域，特别涉及一种基于航迹重规划的水面无人艇动态避碰方法。

背景技术

水面无人艇在军用方面作为未来海洋战争的先驱是国家维护海洋权益的重要工具，在民用方面可以替代船员执行任务，从而有效减少人工作业量和降低作业危险性，在军事作战、海域巡逻、岛礁补给等领域具有广泛的应用需求。由于海洋环境复杂，存在较大的不确定性，因此水面无人艇具备高度智能化的避碰水平是其在复杂海域安全航行的关键。目前我国虽然在水下无人潜航器上取得了一定的成果，但在水面无人艇自主智能化技术，尤其是水面无人艇的动态避碰技术上面仍未取得显著突破，因此，研究一种基于航迹重规划的水面无人艇动态避碰方法，对于提高水面无人艇自主智能化水平，实现水面无人艇军用、民用的普及具有重大意义。

发明内容

本发明的目的在于克服现有技术的缺点与不足，提供一种基于航迹重规划的水面无人艇动态避碰方法。本发明针对水面无人艇的动态避碰问题，引入基于方向包围盒的速度障碍区域几何模型描述和基于速度不确定度的速度障碍区域运动模型描述进行速度障碍区域的建模。结合水面无人艇本体动力学特性和国际海上避碰规则对水面无人艇可行速度集合进行了进一步约束，引入代价函数从中计算最优避障速度并进行航迹重规划。

本发明的目的至少通过如下技术方案之一实现。

本发明提供了一种基于航迹重规划的水面无人艇动态避碰方法，包括以下步骤：

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

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

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

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

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

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

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

优选地，所述步骤S1具体为：使用AIS***获取邻域航行船舶的几何尺寸信息和运动信息，使用GPS获取水面无人艇的经纬度，使用电子罗经获取水面无人艇的三维航向角。

优选地，所述步骤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集合是一个有界集合且其上界为常数，将碰撞锥向外平移一个不确定度上界值的量，故而可以构建不确定度最大时即最坏情况下的碰撞锥区域WVO_UO：

由于动态障碍物的速度不确定集合上界为一个常数，因此在原碰撞锥的基础上大了一圈，记为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。

优选地，所述步骤S5具体为：引入分割线将水面无人艇的绝对速度集合即绝对速度空间划分为4个区域，分别为WVO_UO、V₁、V₂和V₃，其中分割线与速度障碍区域两条切线的角平分线相垂直，并且与WVO_UO的底端相切；

V₁区域的速度表示水面无人艇从左舷通过邻域航行船舶，V₁区域的数学描述如下：

(9)式中，[]_z用于提取向量的z轴分量；在这里以水面无人艇几何中心为圆点，正前方为x轴正向，正右方为y轴正向，z轴正向指向纸面向内；

V₂区域的速度表示水面无人艇从右舷通过邻域航行船舶，V₂区域的数学描述如下：

V₃区域的速度表示水面无人艇背离邻域航行船舶，V₃区域的数学描述如下：

结合国际海上避碰规则，可以得出满足国际海上避碰规则的最终候选速度集合CRAV，

当水面无人艇与邻域航行船舶处于追越或者左交叉局面时，将水面无人艇的速度约束在V₁和V₃区域，

CRAV＝{v|v∈RAV,v∈V₁∪V₃} (12)，

当水面无人艇与邻域航行船舶处于对遇或者右交叉局面时，将水面无人艇的速度约束在V₂和V₃区域，

CRAV＝{v|v∈RAV,v∈V₂∪V₃} (13)，

(9)至(11)式中P_o表示邻域航行船舶的位置矢量，(9)至(13)式中v为水面无人艇的绝对速度。

优选地，所述步骤S6具体为：对候选速度集合中的速度v和艏向θ进行离散化网格处理，具体可将速度v按等间距网格离散为M个离散速度，将艏向θ按等间距网格离散为个N离散艏向，每一个离散速度和离散艏向组成一个完整的速度矢量，最终将整个集合离散为M×N个速度矢量；其中M和N是自然数，

M＝INT((v_max-v_min)/Δv)+1 (14)，

N＝INT(2π/Δθ)+1 (15)，

(14)和(15)式中INT为取整函数，v_max和v_min为水面无人艇的最大和最小速度，Δv为水面无人艇的最小速度改变量，Δθ为水面无人艇的最小艏向角改变量；

构建代价函数对M×N个速度矢量进行评价，对于每一个离散速度v_i和离散艏向θ_j而言，1≤i≤M，1≤j≤N，其代价可以按如下公式进行计算：

(16)和(17)式中，v_g是水面无人艇到下一个局部子目标点的目标速度矢量，v_c是水面无人艇当前的速度大小，θ_c是水面无人艇当前的艏向角，P_g是下一个局部子目标点的位置矢量，P_u是水面无人艇的位置矢量，ω₁和ω₂是两个权重系数，计算M×N个速度矢量所具有的代价，选取代价最小的速度

作为水面无人艇的最优避碰速度。

优选地，C_ij综合目标导向和速度改变量两个维度的影响，偏离目标点和速度改变量大的速度集合具有较大的代价，考虑到局部避障算法应该具有的目标导向性，两个权重系数应该满足ω₁＞ω₂。

优选地，所述步骤S7具体为：添加一个计算航迹重规划点的内循环，模拟水面无人艇在沿速度

航行的过程中，不断检查水面无人艇到下一个局部子目标点的目标速度矢量v_g是否在CRAV集合内，如果不在CRAV集合内，则进行下一个时间周期的模拟过程，依次循环直到满足v_g∈CRAV；根据水面无人艇模拟初期的位置矢量P_u和模拟所用的总时间即可计算出航迹重规划点的位置矢量P_r：

(18)式中，n表示模拟循环次数，Δt表示模拟的时间周期。

本发明与现有技术相比，具有如下优点和有益效果：

本发明采用方向包围盒对水面无人艇和领域航行船舶进行几何模型描述，能够很好的适应船舶长宽比较大的特点，同时了可行路径的过多丢失，方便得到更优的避碰决策；引入速度障碍区域的速度不确定度对邻域航行船舶运动信息的观测不确定性进行补偿，通过在碰撞锥***添加一层软约束，为水面无人艇动态避碰提供了安全缓冲作用；结合水面无人艇本身的动力学特性和国际海上避碰规则对水面无人艇的速度进行进一步约束，使得水面无人艇的避碰决策在满足自身操纵性的同时能够与邻域航行船舶之间实现可靠的动态避碰。本发明提出的动态避碰算法实现了水面无人艇与领域航行船舶之间的符合国际海上避碰规则的动态避碰，可广泛适用于各种无人艇的自主智能控制。

附图说明

图1为本发明所述一种基于航迹重规划的水面无人艇动态避碰方法流程图；

图2为实施例中绝对速度集下的碰撞锥VO_UO的构建示意图；

图3为实施例中不确定度δ_O最大时碰撞锥区域WVO_UO的构建示意图；

图4为实施例中水面无人艇绝对速度空间区域划分的示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例：

本实施例提供了一种基于航迹重规划的水面无人艇动态避碰方法，如图1所示，包括以下步骤：

第一步、采集艇载传感器的实时数据，获取动态避碰过程中的关键信息；具体为：使用AIS***获取邻域航行船舶的几何尺寸信息和运动信息，使用GPS获取水面无人艇的经纬度，使用电子罗经获取水面无人艇的三维航向角；

第二步、根据邻域航行船舶和水面无人艇的几何信息和运动信息，在构造碰撞锥区域时将水面无人艇本体和邻域航行船舶用方向包围盒进行描述，如图2所示。斜线填充矩形框U表示水面无人艇，无填充矩形框O表示邻域航行船舶，P_u和v_u分别表示水面无人艇的位置矢量和速度矢量，P_o和v_o分别表示邻域航行船舶的位置矢量和速度矢量。在构造碰撞锥区域时会进行两种相对转化过程，一种是尺寸信息的相对转化，具体过程是以邻域航行船舶矩形包围框边线上的点为几何中心，不断叠加水面无人艇矩形包围框，将叠加后的最***区域视为障碍物区域，从而将水面无人艇视为质点。从质点出发，作障碍物区域的两条切线，将所述两条切线包围的区域为速度障碍区域。另一种是运动信息的相对转化，具体过程是计算水面无人艇相对于邻域航行船舶的速度v_r＝v_u-v_o，从而可以将邻域航行船舶视为静止。

定义一条从水面无人艇几何中心P_u沿相对速度v_r出发的射线为：

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

(1)式中t代表时间，因此射线λ(P_u,v_r)的实际物理意义表示在水面无人艇和邻域航行船舶保持其运动状况不变的前提下两船之间相对位置的变化情况。当射线λ(P_u,v_r)与扩大后的邻域航行船舶相交时，也即射线落在速度障碍区域的两条***线之间时，可以认为水面无人艇与邻域航行船舶在未来某一时刻必然发生碰撞。定义导致水面无人艇和邻域航行船舶发生碰撞的所有相对速度v_r集合为相对速度集下的碰撞锥RCC_UO。类似地，定义导致水面无人艇和邻域航行船舶发生碰撞的所有水面无人艇绝对速度v_u集合为绝对速度集下的碰撞锥

VO_UO：

(2)式中，

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

第三步、在构建碰撞锥后引入邻域航行船舶的速度不确定度δ_O，将邻域航行船舶速度参量中的所有不确定部分都归为集合{W_O|δ_O∈W_O}，W_O集合是一个有界集合且其上界为常数，故而可以构建最坏情况下的碰撞锥区域WVO_UO：

WVO_UO示意图如图3所示，由于动态邻域航行船舶的速度不确定集合上界为一个常数，因此可以用网格线填充的圆形区域来表示W_O，此时的WVO_UO集合则相当于在原来的基础上扩大了一圈，W_O在其中的软约束相当于为水面无人艇局部避障提供了安全缓冲作用。将WVO_UO外的速度集合记为无碰速度集。

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

(4)式和(5)式中，ω、ν分别为水面无人艇的角速度、速度；v_c和ω_c分别表示水面无人艇当前的速度和角速度，

和

分别为水面无人艇加速情况下的最大加速度和角加速度，

和

分别为水面无人艇减速情况下的最大加速度和角加速度；其中，

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

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

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

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

第五步、如图4所示，引入分割线将水面无人艇的绝对速度集合即绝对速度空间划分为4个区域，分别为WVO_UO、V₁、V₂和V₃，WVO_UO的区域为扩大后的碰撞锥区域，其中分割线与速度障碍区域两条切线的角平分线相垂直，并且与WVO_UO的底端相切；

V₁区域的速度表示水面无人艇从左舷通过邻域航行船舶，V₁区域的数学描述如下：

(9)式中，[]_z用于提取向量的z轴分量；在这里以水面无人艇几何中心为圆点，正前方为x轴正向，正右方为y轴正向，z轴正向指向纸面向内；

V₂区域的速度表示水面无人艇从右舷通过邻域航行船舶，V₂区域的数学描述如下：

V₃区域的速度表示水面无人艇背离邻域航行船舶，V₃区域的数学描述如下：

结合国际海上避碰规则，可以得出满足国际海上避碰规则的最终候选速度集合CRAV，

当水面无人艇与邻域航行船舶处于追越或者左交叉局面时，将水面无人艇的速度约束在V₁和V₃区域，

CRAV＝{v|v∈RAV,v∈V₁∪V₃} (12)，

当水面无人艇与邻域航行船舶处于对遇或者右交叉局面时，将水面无人艇的速度约束在V₂和V₃区域，

CRAV＝{v|v∈RAV,v∈V₂∪V₃} (13)，

(9)至(11)式中P_o表示邻域航行船舶的位置矢量，v为水面无人艇的绝对速度。

第六步、对候选速度集合中的速度v和艏向θ进行离散化网格处理，具体可将速度v按等间距网格离散为M个离散速度，将艏向θ按等间距网格离散为个N离散艏向，每一个离散速度和离散艏向组成一个完整的速度矢量，最终将整个集合离散为M×N个速度矢量；其中M和N是自然数，

M＝INT((v_max-v_min)/Δv)+1 (14)，

N＝INT(2π/Δθ)+1 (15)，

(14)和(15)式中INT为取整函数，v_max和v_min为水面无人艇的最大和最小速度，Δv为水面无人艇的最小速度改变量，Δθ为水面无人艇的最小艏向角改变量；

构建代价函数对M×N个速度矢量进行评价，对于每一个离散速度v_i和离散艏向θ_j而言，1≤i≤M，1≤j≤N，其代价可以按如下公式进行计算：

(16)和(17)式中，v_g是水面无人艇到下一个局部子目标点的目标速度矢量，v_c是水面无人艇当前的速度大小，θ_c是水面无人艇当前的艏向角，P_g是下一个局部子目标点的位置矢量，P_u是水面无人艇的位置矢量，ω₁和ω₂是两个权重系数，代价评价函数C_ij综合目标导向和速度改变量两个维度的影响，偏离目标点和速度改变量大的速度集合具有较大的代价，考虑到局部避障算法应该具有的目标导向性，因此两个权重系数应该满足ω₁＞ω₂。计算M×N个速度矢量所具有的代价，选取代价最小的速度

作为水面无人艇的最优避碰速度。

第七步、添加一个计算航迹重规划点的内循环，模拟无人艇在沿速度

航行的过程中，不断检查水面无人艇到下一个局部子目标点的目标速度矢量v_g是否在CRAV集合内，如果不在CRAV集合内，则进行下一个时间周期的模拟过程，依次循环直到满足v_g∈CRAV。根据水面无人艇模拟初期的位置矢量P_u和模拟所用的总时间即可计算出航迹重规划点的位置矢量P_r：

(18)式中，n表示模拟循环次数，Δt表示模拟的时间周期。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (6)

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。

2.根据权利要求1所述的基于航迹重规划的水面无人艇动态避碰方法，其特征在于，所述步骤S1具体为：使用AIS***获取邻域航行船舶的几何尺寸信息和运动信息，使用GPS获取水面无人艇的经纬度，使用电子罗经获取水面无人艇的三维航向角。

3.根据权利要求1所述的基于航迹重规划的水面无人艇动态避碰方法，其特征在于，所述步骤S5具体为：引入分割线将水面无人艇的绝对速度集合即绝对速度空间划分为4个区域，分别为WVO_UO、V₁、V₂和V₃，其中分割线与速度障碍区域两条切线的角平分线相垂直，并且与WVO_UO的底端相切；

V₁区域的速度表示水面无人艇从左舷通过邻域航行船舶，V₁区域的数学描述如下：

(9)式中，[]_z用于提取向量的z轴分量；在这里以水面无人艇几何中心为圆点，正前方为x轴正向，正右方为y轴正向，z轴正向指向纸面向内；P₀为航行船舶的位置矢量，位置矢量是在某一时刻，以坐标原点为起点，以运动质点所在位置为终点的有向线段；V₂区域的速度表示水面无人艇从右舷通过邻域航行船舶，V₂区域的数学描述如下：

V₃区域的速度表示水面无人艇背离邻域航行船舶，V₃区域的数学描述如下：

结合国际海上避碰规则，可以得出满足国际海上避碰规则的最终候选速度集合CRAV，

当水面无人艇与邻域航行船舶处于追越或者左交叉局面时，将水面无人艇的速度约束在V₁和V₃区域，

CRAV＝{v|v∈RAV,v∈V₁∪V₃} (12)，

当水面无人艇与邻域航行船舶处于对遇或者右交叉局面时，将水面无人艇的速度约束在V₂和V₃区域，

CRAV＝{v|v∈RAV,v∈V₂∪V₃} (13)，

(9)至(11)式中P_o表示邻域航行船舶的位置矢量，位置矢量是在某一时刻，以坐标原点为起点，以运动质点所在位置为终点的有向线段，(9)至(13)式中v为水面无人艇的绝对速度。

4.根据权利要求3所述的基于航迹重规划的水面无人艇动态避碰方法，其特征在于，所述步骤S6具体为：对候选速度集合CRAV中的速度v和艏向θ进行离散化网格处理，具体可将速度v按等间距网格离散为M个离散速度，将艏向θ按等间距网格离散为个N离散艏向，每一个离散速度和离散艏向组成一个完整的速度矢量，最终将整个集合离散为M×N个速度矢量；其中M和N是自然数，

M＝INT((v_max-v_min)/Δv)+1 (14)，

N＝INT(2π/Δθ)+1 (15)，

(14)和(15)式中INT为取整函数，v_max和v_min为水面无人艇的最大和最小速度，Δv为水面无人艇的最小速度改变量，Δθ为水面无人艇的最小艏向角改变量；构建代价函数对M×N个速度矢量进行评价，对于每一个离散速度v_i和离散艏向θ_j而言，1≤i≤M，1≤j≤N，其代价可以按如下公式进行计算：

(16)和(17)式中，C_ij为代价评价函数，v_g是水面无人艇到下一个局部子目标点的目标速度矢量，v_c是水面无人艇当前的速度大小，θ_c是水面无人艇当前的艏向角，P_g是下一个局部子目标点的位置矢量，P_u是水面无人艇的位置矢量，ω₁和ω₂是两个权重系数，计算M×N个速度矢量所具有的代价，选取代价最小的速度

作为水面无人艇的最优避碰速度。

5.根据权利要求4所述的基于航迹重规划的水面无人艇动态避碰方法，其特征在于，ω₁＞ω₂。

6.根据权利要求4所述的基于航迹重规划的水面无人艇动态避碰方法，其特征在于，所述步骤S7具体为：添加一个计算航迹重规划点的内循环，模拟水面无人艇在沿速度

航行的过程中，检查水面无人艇到下一个局部子目标点的目标速度矢量v_g是否在CRAV集合内，如果不在CRAV集合内，则进行下一个时间周期的模拟过程，依次循环直到满足v_g∈CRAV；根据水面无人艇模拟初期的位置矢量P_u和模拟所用的总时间即可计算出航迹重规划点的位置矢量P_r：

(18)式中，n表示模拟循环次数，Δt表示模拟的时间周期。

Images