CN108710372A - 一种用于群水面无人艇的自主避碰系统及方法 - Google Patents

Abstract

本发明提出一种用于群水面无人艇的自主避碰系统及方法，包括艇载避碰系统包括运动操纵模块、无线通信模块、数据收集处理模块和避碰决策模块，完成从周边无人艇航行姿态信息获取、分析处理到制定出避碰决策所有过程，应用运动操纵模块、无线通信模块、数据收集处理模块和避碰决策模块各自的功能，实现无人艇在开阔水域避碰群无人艇的功能，同时，使用动态分组算法和同组跟随算法完成避碰过程中无人艇交通流的生成与群组无人艇避让，最终使得每一艘无人艇都能够安全驶过。本发明能够实现水面无人艇在复杂态势下的避碰，并根据航行态势变化调整避让决策，实现无人艇的自主避碰。

Description

一种用于群水面无人艇的自主避碰系统及方法

技术领域

本发明属于海上巡航与搜救的技术领域，尤其涉及一种用于群水面无人艇的自主避碰系统及方法。

背景技术

由于水面无人艇(Unmanned Surface Vehicle)巨大的应用潜力，研究者对其无人化的研究从未中断，特别是随着智能控制、运动规划等新兴技术的发展，具备一定自主航行能力的无人艇在海洋科考、海事管理、水上搜救等领域发挥了重要作用，同时在军事领域的侦察和攻击任务中亦得到应用。自主避碰对于保障水面无人艇安全具有重要意义，多年来以单个无人艇为中心的自主避碰已经取得了一定的研究成果，但是在群无人艇避碰方面的研究尚未引起广泛的关注。

传统的避碰方法一般使用路径规划算法计算避碰路径，或者使用最优化方法迭代求出当前态势下的最优避碰方案，但是传统的避碰方法都存在明显的弊端：运行速度慢、不能适应多变的避碰环境、不能充分考虑无人艇的主观能动性。借鉴矢量障碍物算法(ORCA)、行为预测方法、无线通信技术以及交通流的思想，提出一种在复杂态势下仍能自主避碰的艇载避碰系统，具有广阔的应用前景。

发明内容

本发明所要解决的技术问题在于针对上述存在的问题，提供一种用于群水面无人艇的自主避碰系统及方法，解决传统避碰方法中存在的计算缓慢、不能够动态更新避碰决策、复杂会遇态势下空间有效利用等避碰问题，实现群无人艇避碰。

本发明解决上述技术问题所采用的技术方案是：一种用于群水面无人艇的自主避碰系统，其特征在于，包括运动操纵模块、无线通信模块、数据收集处理模块和避碰决策模块，所述运动操纵模块包含螺旋桨和舵叶设备，所述无线通信模块为ZigBee模块，与所述数据收集处理模块相连，数据收集处理模块包含自身传感数据处理单元和周边环境数据处理单元，所述自身传感单元包括位置传感器GPS、航向传感器电子罗盘、舵角传感器，所述周边环境处理单元是将无线通信模块的环境数据整合后传递给避碰决策模块，所述避碰决策模块的输入端与数据收集处理模块相连，输出端与运动操纵模块相连，控制所述螺旋桨和舵叶运行。

一种用于群水面无人艇的自主避碰方法，其特征在于，包括如下步骤：

S1)所述无线通信模块在基于Zigbee协议栈的基础上，设计了一种应用层上的协商协议，通过在Zigbee通信网络中互相发送消息达到协商的目的，并将协商结果通过数据收集处理模块送到避碰决策模块，同时，在航行过程中周期性的广播自身航行姿态信息和获取周边无人艇航行姿态信息，并将其送达数据收集处理模块进一步处理；

S2)所述数据收集处理模块从传感器获取相应的原始数据，并将这些数据结构化后传递给运动操纵模块和避碰决策模块，所述周边环境处理单元是将无线通信模块的环境数据整合后传递给避碰决策模块；

S3)所述避碰决策模块能够从数据收集处理模块获取自身的姿态信息和周边无人艇航行姿态信息，根据动态分组算法和同组跟随算法得出避碰决策，向运动操纵模块发出控制指令，通过控制螺旋桨和舵叶角度来调整无人艇航行姿态，从数据收集处理模块主动获取航向C₀和舵角δ₀数据，使用PID航向控制算法调整航向，调节螺旋桨转速调整航速，保证无人艇能够根据决策指令迅速调整到目标航行姿态。

按上述方案，所述步骤S3)中动态分组算法包括如下内容：

S31)在数据收集处理模块中，获取的位置信息是经纬度，首先按照墨卡托投影的方式将经纬度转换到全局平面坐标系中，在全局坐标系中，无人艇USV_i位置表示为(P_xi，P_yi)，航向为C_i，航速为V_i，舵角为δ_i；

S32)避碰决策模块从数据收集处理模块获取数据，将周边无人艇位置、航向、速度和舵角映射到艇体坐标系XOY中，所述艇体坐标系是以本无人艇为中心，以本艇航向为OY轴，与其垂直的右正横方向作为OX轴，从航向开始顺时针旋转的极角作为相对方位角θ；映射公式如下：

ci＝C_i-C₀

v_i＝V_i

γ_i＝δ_i

θ_i＝arctan(Y_i/X_i)

式中：X_i,Y_i表示USV_i在艇体坐标系中位置，P_x0,P_y0表示本艇USV₀的全局坐标位置，C₀表示本无人艇的航向，c_i表示USV_i在艇体坐标系中的航向，v_i表示USV_i在艇体坐标系中的航速，γ_i表示USV_i在艇体坐标系中的舵角，θ_i表示USV_i相对于本艇的相对方位角；

S33)在艇体坐标系中，计算每一艘无人艇与临近无人艇的运动姿态相似度，如果小于μ_max则分到同一分组中，所述μ_max根据分组效果调整，相似度μ计算公式如下：

式中：

X_i,X_j—无人艇USV_i、USV_j的OX坐标

Y_i,Y_j—无人艇USV_i、USV_j的OY坐标

c_i,c_j—无人艇USV_i、USV_j的航向

v_i,v_j—无人艇USV_i、USV_j的航速；

S34)如果本无人艇与周边无人艇被分到同一组中，则使用同组跟随算法计算跟随对象；

S35)在无人艇运动过程中周期性地更新分组，完成避碰过程中无人艇交通流的生成与群组无人艇避让，最终使得每一艘无人艇都能够安全驶过。

按上述方案，所述步骤S34)中同组跟随算法包括如下内容：

S341)在艇体坐标系中，以本艇为坐标原点，顺时针依次扫描相对方位角330°-360°、0°-90°、270°-330°范围内的无人艇；

S342)将第一个扫描到的无人艇作为跟随艇，并调整航行姿态与这个无人艇相同；

S343)若没有扫描到无人艇，则认为本艇是本组的领队无人艇，根据避碰决策调整航向姿态。

按上述方案，所述步骤S3)中PID航向控制算法包括如下内容：利用数据收集处理模块数据执行避碰决策模块的控制指令，根据无人艇的当前航向和目标航向调整舵角，控制无人艇迅速调整到目标航向，具体的控制公式如下所示：

e(k)＝r(k)-C(k)

δ(k)＝δ(k-1)+Δu(k)

式中：e(k)表示第k次航向偏差值，r(k)表示目标航向，C(k)表示第k次航向角，δ₀是初始舵角，K_p表示比例系数，K_i表示积分系数，K_d表示微分系数，T表示采样周期，Δu(k)表示第k次舵角增量，δ(k)表示第k次舵角。

本发明的有益效果是：提供一种用于群水面无人艇的自主避碰系统及方法，考虑了无人艇的操纵性能，根据操纵性能调整避碰危险距离；充分利用通信技术，实现环境信息获取和多艘无人艇之间的协调避碰；借鉴了传统的交通流特性，将无人艇的机动性体现了出来，动态分组算法使得无人艇在避碰过程中自主的形成多股交通流，简化会遇态势；综合利用了无人艇的操纵性、通信互联技术以及避碰算法，并且同组跟随算法实现群无人艇避让动作的一致性，使得无人艇避碰过程顺畅自然。

附图说明

图1为本发明一个实施例的无人艇硬件布局图。

图2为本发明一个实施例的各模块之间的导向关系以及数据传递流程图。

图3为本发明一个实施例的无线通信模块协商状态转换图。

图4为本发明一个实施例的全局坐标系中群无人艇关系示意图。

图5为本发明一个实施例的艇体坐标系示意图。

图6为本发明一个实施例的艇体坐标系中群无人艇关系示意图。

图7为本发明一个实施例的周边无人艇分组后的聚类效果图。

图8为本发明一个实施例的无人艇分组后跟随领域划分图。

图9为本发明一个实施例的最终的跟随链图。

图10为本发明一个实施例的时段1-55s群无人艇避让轨迹图。

图11为本发明一个实施例的时段1-125s群无人艇避让轨迹图。

图12为本发明一个实施例的群无人艇航向历时曲线图。

图13为本发明一个实施例的群无人艇航角历时曲线图。

图14为本发明一个实施例的群无人艇速度历时曲线图。

图15为本发明一个实施例的群无人艇相互之间间距变化历时曲线图。

具体实施方式

为更好地理解本发明，下面结合附图和实施例对本发明进一步的描述。

如图1-图2所示，一种用于群水面无人艇的自主避碰系统，包括运动操纵模块、无线通信模块、数据收集处理模块和避碰决策模块，运动操纵模块包含螺旋桨和舵叶设备，无线通信模块为ZigBee模块，与数据收集处理模块相连，数据收集处理模块包含自身传感数据处理单元和周边环境数据处理单元，自身传感单元包括位置传感器GPS、航向传感器电子罗盘、舵角传感器，周边环境处理单元是将无线通信模块的环境数据整合后传递给避碰决策模块，避碰决策模块的输入端与数据收集处理模块相连，输出端与运动操纵模块相连，控制螺旋桨和舵叶运行。

运动操纵模块通过控制螺旋桨和舵叶角度(舵角)来调整无人艇航行姿态，同时，从数据收集处理模块主动获取航向C₀和舵角δ₀数据，使用PID航向控制算法调整航向，调节螺旋桨转速调整航速，保证无人艇能够根据决策指令迅速调整到目标航行姿态；

所述PID航向控制算法，其特征在于能够利用数据收集处理模块数据执行避碰决策模块的控制指令，根据无人艇的当前航向和目标航向调整舵角，控制无人艇迅速调整到目标航向，具体的控制公式如下所示：

e(k)＝r(k)-C(k)

δ(k)＝δ(k-1)+Δu(k)

式中：e(k)表示第k次航向偏差值，r(k)表示目标航向，C(k)表示第k次航向角，δ₀是初始舵角，K_p表示比例系数，K_i表示积分系数，K_d表示微分系数，T表示采样周期，Δu(k)表示第k次舵角增量，δ(k)表示第k次舵角。

无线通信模块在基于Zigbee协议栈的基础上，设计了一种应用层上的协商协议，通过在Zigbee通信网络中互相发送消息达到协商的目的(见图3)，并将协商结果通过数据收集处理模块送到避碰决策模块，同时，在航行过程中周期性的广播自身航行姿态信息和获取周边无人艇航行姿态信息，并将其送达数据收集处理模块进一步处理。

如图2所示，数据收集处理模块从传感器获取相应的原始数据，并将这些数据结构化后传递给运动操纵模块和避碰决策模块，周边环境处理单元是将无线通信模块的环境数据整合后传递给避碰决策模块。避碰决策模块能够从数据收集处理模块获取自身的姿态信息和周边无人艇航行姿态信息，根据动态分组算法和同组跟随算法得出避碰决策，向运动操纵模块发出控制指令，进而改变无人艇的航行姿态。

所述的动态分组算法包含以下步骤：

S1在数据收集处理模块中，获取的位置信息是经纬度，首先按照墨卡托投影的方式将经纬度转换到全局平面坐标系中，在全局坐标系中(见图4)，无人艇USV_i位置表示为(P_xi，P_yi)，航向为C_i，航速为V_i，舵角为δ_i。

S2避碰决策模块从数据收集处理模块获取数据，将周边无人艇位置、航向、速度和舵角映射到艇体坐标系XOY中，转换到艇体坐标系后群无人艇关系如图6所示，艇体坐标系是以本无人艇为中心，以本艇航向为OY轴，与其垂直的右正横方向作为OX轴(见图5)，从航向开始顺时针旋转的极角作为相对方位角θ；映射公式如下：

ci＝C_i-C₀

v_i＝V_i

γ_i＝δ_i

θ_i＝arctan(Y_i/X_i)

式中：X_i,Y_i表示USV_i在艇体坐标系中位置，P_x0,P_y0表示本艇USV₀的全局坐标位置，C₀表示本无人艇的航向，c_i表示USV_i在艇体坐标系中的航向，v_i表示USV_i在艇体坐标系中的航速，γ_i表示USV_i在艇体坐标系中的舵角，θ_i表示USV_i相对于本艇的相对方位角。

S3在艇体坐标系中，计算每一艘无人艇与临近无人艇的运动姿态相似度，如果小于μ_max则分到同一分组中(见图7)，所述μ_max根据分组效果调整。相似度μ计算公式如下：

式中：

X_i,X_j—无人艇USV_i、USV_j的OX坐标

Y_i,Y_j—无人艇USV_i、USV_j的OY坐标

c_i,c_j—无人艇USV_i、USV_j的航向

v_i,v_j—无人艇USV_i、USV_j的航速

S4如果本无人艇与周边无人艇被分到同一组中，则使用同组跟随算法计算跟随对象(见图8)，顺时针依次扫描划分的跟随领域得到最终的跟随链(见图9)；

S5在无人艇运动过程中周期性地更新分组；

所述的同组跟随算法包含以下步骤：

S1在艇体坐标系中，以本艇为坐标原点，顺时针依次扫描相对方位角330°-360°、0°-90°、270°-330°范围内的无人艇；

S2将第一个扫描到的无人艇作为跟随艇，并调整航行姿态与这个无人艇相同；

S3若没有扫描到无人艇，则认为本艇是本组的领队无人艇，根据避碰决策调整航向姿态。

艇载避碰系统能够完成从周边无人艇航行姿态信息获取、分析处理到制定出避碰决策所有过程，应用运动操纵模块、无线通信模块、数据收集处理模块和避碰决策模块各自的功能，实现无人艇在开阔水域避碰群无人艇的功能，同时，使用动态分组算法和同组跟随算法完成避碰过程中无人艇交通流的生成与群组无人艇避让，最终使得每一艘无人艇都能够安全驶过。

为验证本发明的有效性，进行水面无人艇仿真实验，仿真实验中艇长设置为5m，艇宽2m，操纵性KT指数K₀＝2，T₀＝5，PID的三个参数K_p＝0.8，K_i＝5，K_d＝10。

使用五艘相同规格的USV进行仿真实验，初始化参数如表1所示。

表1仿真中五艘无人艇初始参数

仿真中无人艇各参数历时曲线如图10-图15所示。在图10中，起初五艘无人艇处于复杂的会遇态势之中，但是经过40s后，本文的避碰算法使得整体的态势发生了转变，USV₁与USV₂由于分组算法成为一组，计算后由于右舷有无人艇USV₅并存在碰撞危险，向左转向避让，USV₃和USV₄则采取向右转向的小动作避让，使得态势迅速简化。本发明不仅考虑无人艇之间会遇态势的变化，同时兼顾航速，如图14中速度变化历时曲线，由于分组算法的影响，最终速度趋于一致，同时航向也有接近的趋势，总体的避碰决策随着态势的变化而调整。相比于传统的避碰方法，本发明采用的避碰系统应对复杂态势更加自然，使复杂态势下的群无人艇自主地形成多股交通流，达到简化态势的目的，并且在避碰过程中能动态的调整避碰决策，最终使得群无人艇都能够安全驶过。

以上仅为本发明的较佳实施例，不能以此来限定本发明之权利范围，因此依本发明申请专利范围所做的等效变化，仍属本发明的保护范围。

Claims (5)

1.一种用于群水面无人艇的自主避碰系统，其特征在于，包括运动操纵模块、无线通信模块、数据收集处理模块和避碰决策模块，所述运动操纵模块包含螺旋桨和舵叶设备，所述无线通信模块为ZigBee模块，与所述数据收集处理模块相连，数据收集处理模块包含自身传感数据处理单元和周边环境数据处理单元，所述自身传感单元包括位置传感器GPS、航向传感器电子罗盘、舵角传感器，所述周边环境处理单元是将无线通信模块的环境数据整合后传递给避碰决策模块，所述避碰决策模块的输入端与数据收集处理模块相连，输出端与运动操纵模块相连，控制所述螺旋桨和舵叶运行。

2.一种用于群水面无人艇的自主避碰方法，其特征在于，包括如下步骤：

S1)所述无线通信模块在基于Zigbee协议栈的基础上，设计了一种应用层上的协商协议，通过在Zigbee通信网络中互相发送消息达到协商的目的，并将协商结果通过数据收集处理模块送到避碰决策模块，同时，在航行过程中周期性的广播自身航行姿态信息和获取周边无人艇航行姿态信息，并将其送达数据收集处理模块进一步处理；

S2)所述数据收集处理模块从传感器获取相应的原始数据，并将这些数据结构化后传递给运动操纵模块和避碰决策模块，所述周边环境处理单元是将无线通信模块的环境数据整合后传递给避碰决策模块；

S3)所述避碰决策模块能够从数据收集处理模块获取自身的姿态信息和周边无人艇航行姿态信息，根据动态分组算法和同组跟随算法得出避碰决策，向运动操纵模块发出控制指令，通过控制螺旋桨和舵叶角度来调整无人艇航行姿态，从数据收集处理模块主动获取航向C₀和舵角δ₀数据，使用PID航向控制算法调整航向，调节螺旋桨转速调整航速，保证无人艇能够根据决策指令迅速调整到目标航行姿态。

3.根据权利要求2所述的一种用于群水面无人艇的自主避碰方法，其特征在于，所述步骤S3)中动态分组算法包括如下内容：

S31)在数据收集处理模块中，获取的位置信息是经纬度，首先按照墨卡托投影的方式将经纬度转换到全局平面坐标系中，在全局坐标系中，无人艇USV_i位置表示为(P_xi，P_yi)，航向为C_i，航速为V_i，舵角为δ_i；

S32)避碰决策模块从数据收集处理模块获取数据，将周边无人艇位置、航向、速度和舵角映射到艇体坐标系XOY中，所述艇体坐标系是以本无人艇为中心，以本艇航向为OY轴，与其垂直的右正横方向作为OX轴，从航向开始顺时针旋转的极角作为相对方位角θ；映射公式如下：

ci＝C_i-C₀

v_i＝V_i

γ_i＝δ_i

θ_i＝arctan(Y_i/X_i)

式中：X_i,Y_i表示USV_i在艇体坐标系中位置，P_x0,P_y0表示本艇USV₀的全局坐标位置，C₀表示本无人艇的航向，c_i表示USV_i在艇体坐标系中的航向，v_i表示USV_i在艇体坐标系中的航速，γ_i表示USV_i在艇体坐标系中的舵角，θ_i表示USV_i相对于本艇的相对方位角；

S33)在艇体坐标系中，计算每一艘无人艇与临近无人艇的运动姿态相似度，如果小于μ_max则分到同一分组中，所述μ_max根据分组效果调整，相似度μ计算公式如下：

式中：

X_i,X_j—无人艇USV_i、USV_j的OX坐标

Y_i,Y_j—无人艇USV_i、USV_j的OY坐标

c_i,c_j—无人艇USV_i、USV_j的航向

v_i,v_j—无人艇USV_i、USV_j的航速；

S34)如果本无人艇与周边无人艇被分到同一组中，则使用同组跟随算法计算跟随对象；

S35)在无人艇运动过程中周期性地更新分组，完成避碰过程中无人艇交通流的生成与群组无人艇避让，最终使得每一艘无人艇都能够安全驶过。

4.根据权利要求2所述的一种用于群水面无人艇的自主避碰方法，其特征在于，所述步骤S34)中同组跟随算法包括如下内容：

S341)在艇体坐标系中，以本艇为坐标原点，顺时针依次扫描相对方位角330°-360°、0°-90°、270°-330°范围内的无人艇；

S342)将第一个扫描到的无人艇作为跟随艇，并调整航行姿态与这个无人艇相同；

S343)若没有扫描到无人艇，则认为本艇是本组的领队无人艇，根据避碰决策调整航向姿态。

5.根据权利要求2所述的一种用于群水面无人艇的自主避碰方法，其特征在于，所述步骤S3)中PID航向控制算法包括如下内容：利用数据收集处理模块数据执行避碰决策模块的控制指令，根据无人艇的当前航向和目标航向调整舵角，控制无人艇迅速调整到目标航向，具体的控制公式如下所示：

e(k)＝r(k)-C(k)

δ(k)=δ(k-1)+Δu(k)

式中：e(k)表示第k次航向偏差值，r(k)表示目标航向，C(k)表示第k次航向角，δ₀是初始舵角，K_p表示比例系数，K_i表示积分系数，K_d表示微分系数，T表示采样周期，Δu(k)表示第k次舵角增量，δ(k)表示第k次舵角。