CN113486445A - 一种多个自主式水下航行器协同作业防碰撞方法及系统 - Google Patents

一种多个自主式水下航行器协同作业防碰撞方法及系统 Download PDF

Info

Publication number
CN113486445A
CN113486445A CN202110755754.8A CN202110755754A CN113486445A CN 113486445 A CN113486445 A CN 113486445A CN 202110755754 A CN202110755754 A CN 202110755754A CN 113486445 A CN113486445 A CN 113486445A
Authority
CN
China
Prior art keywords
underwater vehicle
point
underwater
distance
tail
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110755754.8A
Other languages
English (en)
Other versions
CN113486445B (zh
Inventor
陈路
肖志伟
朱小龙
杨睿
张金
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan Guotian Electronic Technology Co ltd
Original Assignee
Hunan Guotian Electronic Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan Guotian Electronic Technology Co ltd filed Critical Hunan Guotian Electronic Technology Co ltd
Priority to CN202110755754.8A priority Critical patent/CN113486445B/zh
Publication of CN113486445A publication Critical patent/CN113486445A/zh
Application granted granted Critical
Publication of CN113486445B publication Critical patent/CN113486445B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/15—Vehicle, aircraft or watercraft design
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63B—SHIPS OR OTHER WATERBORNE VESSELS; EQUIPMENT FOR SHIPPING 
    • B63B43/00—Improving safety of vessels, e.g. damage control, not otherwise provided for
    • B63B43/18—Improving safety of vessels, e.g. damage control, not otherwise provided for preventing collision or grounding; reducing collision damage
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/10—Numerical modelling

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Geometry (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Health & Medical Sciences (AREA)
  • Public Health (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Mechanical Engineering (AREA)
  • Ocean & Marine Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明提供一种多个自主式水下航行器协同作业防碰撞方法及系统，方法包括以下步骤：选取任意两个自主式水下航行器，简化自主式水下航行器的形状，计算两个水下航行器首尾部球心的距离，进而得到两个航行器首尾部间的距离，将得到的两个航行器首尾部间的距离与防碰撞安全系数值比较，并确定是否调整两个水下航行器的航向，若需要调整航向则进一步计算两个航行器躯体间轴线间最小距离dmin，进而得到两个航行器胶囊体之间的最小距离，并再次与防碰撞安全系数值相比较，确定是否需要调整航向；继续选择剩余其他的水下航行器中的任意两个重复上述步骤，避免多个自主式水下航行器协同作业时所有的自主式水下航行器在航行中碰撞情况的发生。

Description

一种多个自主式水下航行器协同作业防碰撞方法及系统
技术领域
本发明属于水下航行避撞技术领域，具体涉及一种多个自主式水下航行器协同作业防碰撞方法及系统。
背景技术
水下自主式水下航行器(又称AUV)，能够在某一单独海域完成水底勘察、水文要素获取、打捞救援等人类无法亲自实现的特殊任务。随着AUV的广泛使用，它极大地拓展了人类探索的空间。同时，单AUV水下作业时，工作效率和工作局限性逐渐暴露。水下多AUV系统组群应用，越来越成为一种选择趋势，多AUV协同作业具有高度的并行性、冗余性，通过水下合作协调，可完成更为复杂的水下作业任务，大大提高了工作效率和探测目标的几率。随着多AUV协同作业的发展，水下AUV之间存在相互碰撞的风险，如发生碰撞事故，直接导致工作任务无法完成，以及因AUV本身价格高昂造成的巨大经济损失。因此，急需一种多自主式水下航行器协同作业防碰撞系统及方法，以解决上述不足。
发明内容
针对当前多水下AUV集群作业时，存在碰撞的风险。本发明提出的一种多个自主式水下航行器协同作业防碰撞方法及系统，可防止多AUV集群系统航行工作中，避免发生相互碰撞，本发明提供一种多个自主式水下航行器协同作业防碰撞方法和系统。
本发明提供如下技术方案：一种多个自主式水下航行器协同作业防碰撞方法，包括以下步骤：
S1：选取多个自主式水下航行器中的任意两个自主式水下航行器，作为第一水下航行器和第二水下航行器；
S2：简化所述自主式水下航行器的形状，将所述自主式水下航行器的首部和尾部简化为半球体，；确定第一水下航行器首部球心的坐标OA为(xA,yA,zA)、第一水下航行器尾部球心的坐标OC为(xC,yC,zC)、第二水下航行器首部球心的坐标OB为(xB,yB,zB)、第二水下航行器尾部球心的坐标OD为(xD,yD,zD)；令第一水下航行器首部和尾部的半球体半径均为r1，第二水下航行器首部和尾部的半球体半径均为r2；
S3：计算第一水下航行器的首部球心与第二水下航行器的首部球心和尾部球心的距离：DAB、DAD；并计算第一水下航行器的尾部球心与第二水下航行器的首部球心和尾部球心的距离：DCB、DCD；
S4：根据所述步骤S3得到的DAB、DAD、DCD、DCB，计算第一水下航行器首部球体与第二水下航行器首部和尾部间的距离：ΔAB、ΔAD，并计算第一水下航行器尾部球体与第二水下航行器首部和尾部间的距离：ΔCB、ΔCD；
S5：将所述S4步骤得到的ΔAB、ΔAD、ΔCB、ΔCD与防碰撞安全系数值比较，若小于所述防碰撞安全系数值，则水面控制站通过水声通信发射器向第一水下航行器和第二水下航行器发出信号，调整两个水下航行器的航向，避免碰撞，若大于所述防碰撞安全系数值，则进一步计算第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin；所述防碰撞安全系数值为10米；
S6：根据所述S5步骤计算得到的第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin，计算得到第一水下航行器和第二水下航行器胶囊体之间的最小距离ΔCC，ΔCC＝dmin-r1-r2，将所述ΔCC与所述防碰撞安全系数值相比较，若小于所述防碰撞安全系数值，则水面控制站继续通过水声通信发射器向第一水下航行器和第二水下航行器发出信号，调整两个水下航行器的航向；若大于所述防碰撞安全系数值，则水面控制站继续通过水声通信发射器向第一水下航行器和第二水下航行器继续保持现有航向航行；
S7：继续选择多个自主式水下航行器剩余其他的水下航行器中的任意两个重复所述步骤S1-S6，避免所有的自主式水下航行器在航行中碰撞情况的发生。
进一步地，所述S3步骤计算DAB、DAD、DCD、DCB的计算公式为：
Figure BDA0003147252660000031
Figure BDA0003147252660000032
Figure BDA0003147252660000033
Figure BDA0003147252660000034
进一步地，所述S4步骤计算ΔAB、ΔAD、ΔCB、ΔCD的计算公式为：
ΔAB＝DAB-r1-r2；ΔAD＝DAD-r1-r2；ΔCB＝DCB-r1-r2；ΔCD＝DCD-r1-r2。
进一步地，所述S5步骤包括以下步骤：
S51：将所述第一水下航行器和第二水下航行器的躯体中心线段长分别设为L1、L2，确定第一水下航行器的躯体中心线段首端点M的坐标为(x1,y1,z1)、尾端点N的坐标为(x2,y2,z2)，确定第二水下航行器的躯体中心线段首端点P的坐标为(x3,y3,z3)、尾端点M的坐标为(x4,y4,z4)；
S52：取第一水下航行器躯体中心线段任一点R、第二水下航行器躯体中心线段任一点K，构建R点和K点坐标计算模型：
Figure BDA0003147252660000035
其中，s为R点坐标计算参数，t为K点坐标计算参数；
S53：计算第一下航行器躯体中心线段任一点R与第二水下航行器躯体中心线段任一点K的向量RK：RK＝(P-M)-s(N-M)+t(Q-P)，并代入所述S51步骤确定的各个端点坐标，得到所述向量RK：
RK＝[x3-x1,y3-y1,z3-z1]-s[x2-x1,y2-y1,z2-z1]+t[x4-x3,y4-y3,z4-z3]；
S54：利用S52步骤构建的R点和K点坐标计算模型构建带有R点坐标计算参数s和K点坐标计算参数t的所述向量RK的计算模型；
并构建有关于参数s和t的第一水下航行器躯体中心线段任一点R和第二水下航行器躯体中心线段任一点K之间距离‖RK‖的计算模型f(s,t)，求解两个水下航行器的躯体中心线段长L1和L2之间的最小距离，进而求解得到R点坐标计算参数s和K点坐标计算参数t；
再分别求点M、N到线段PQ，点P、Q到线段MN的最小距离，以及线段MN和线段PQ之间的公垂线距离dCOMM，将空间中两线段MN和PQ之间的最短距离转为点到线段间距离计算，进而构建第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin的计算模型。
进一步地，所述S54步骤中构建的所述向量RK的计算模型为RK＝[ax-scx+tbx，ay-scy+tby,az-scz+tbz]，其中：
Figure BDA0003147252660000041
进一步地，所述S54步骤中构建的‖RK‖的计算模型f(s,t)如下：
f(s,t)＝(‖RK‖)2＝(ax-scx+tbx)2+(ay-scy+tby)2+(az-scz+tbz)2；
进一步地，所述S54步骤中求解两个水下航行器的躯体中心线段长L1和L2之间的最小距离转化为求解所述‖RK‖的计算模型f(s,t)的最小值问题，包括以下步骤：
S541：令所述‖RK‖的计算模型f(s,t)分别关于所述s和所述t的二阶导为0：
Figure BDA0003147252660000042
S542：进而得到求解所述s和所述t的计算公式：
Figure BDA0003147252660000043
进一步地，所述S54步骤中构建的第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin的计算模型为：
Figure BDA0003147252660000051
其中，dM、dP、dN、dQ分别为点M到线段PQ的最小距离、点P到线段MN的最小距离、点N到线段PQ的最小距离、点Q到线段MN的最小距离，dCOMM为两线段公垂线距离。
进一步地，所述点M到PQ的最小距离的计算方法包括以下步骤：
B1：所述点M在不同位置时，分别过点M做PQ所在直线垂线，垂足为点C；
B2：构建点M到PQ的最小距离dM的计算模型：
Figure BDA0003147252660000052
其中，u为求解参数，|·|为两点之间的距离；采用点到线段距离求解的矢量法，定义参数u：
Figure BDA0003147252660000053
其中PM为点P和点M之间的向量，PQ为点P和点Q之间的向量；
B3：通过三角形面积相等条件，求解所述B2中的两点之间的距离|MC|、|MP|、|MQ|，以|MC|为例：
Figure BDA0003147252660000061
其中，MP为点M和点P之间的向量，MQ为点M和点Q之间的向量。
本发明还提供采用上述方法的多个自主式水下航行器协同作业防碰撞系统，所述系统包括作为控制中心的水面控制站和多个自主式水下航行器湿端，所述水面控制站与多个自主式水下航行器湿端双相通信连接。所述水面控制站包括控制工作站、高精度GPS定位模块和水声通信发射器；所述自主式水下航行器湿端包括安装在所述自主式水下航行器本体的首部声学应答器和尾部声学应答器，以及安装在所述自主式水下航行器本体中间的航行计程仪和高精度惯导模块。
本发明的有益效果为：
1、本发明提供的多个自主式水下航行器协同作业防碰撞方法为基于空间几何模型方法，计算量小、检测效率高，结果稳定，适合于实际应用。
2、本发明提供的多个自主式水下航行器协同作业防碰撞方法采取简化AUV模型，再预留安全余量，将空间问题转化为两线段的最短距离和点到线段间距离计算，简单可靠。
3)本发明提供的多个自主式水下航行器协同作业防碰撞系统基于现有成熟的高精度GPS、水声声学应答器、航行计程仪传感器，AUV设计和改造极为方便易实现。
4)本发明提供的多个自主式水下航行器协同作业防碰撞方法和系统基于水声通信，相对于其他防碰撞设计方式，成本低、后期维护便利等优点。
附图说明
在下文中将基于实施例并参考附图来对本发明进行更详细的描述。其中：
图1为本发明提供的方法流程示意图；
图2为本发明提供的方法中第一水下航行器与第二水下航行器首尾部端点坐标及连线示意图；
图3为本发明实施例2提供的方法中第一水下航行器与第二水下航行器躯体间示意图；
图4为本发明实施例2提供的方法中点M到PQ的最小距离dM的计算方法示意图；
图5为本发明提供的系统的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1
如图1所示，为本实施例提供的一种多个自主式水下航行器协同作业防碰撞方法，包括以下步骤：
S1：选取多个自主式水下航行器中的任意两个自主式水下航行器，作为第一水下航行器和第二水下航行器；
S2：简化所述自主式水下航行器的形状，将所述自主式水下航行器的首部和尾部简化为半球体，；确定第一水下航行器首部球心A的坐标OA为(xA,yA,zA)、第一水下航行器尾部球心C的坐标OC为(xC,yC,zC)、第二水下航行器首部球心B的坐标OB为(xB,yB,zB)、第二水下航行器尾部球心D的坐标OD为(xD,yD,zD)；令第一水下航行器首部和尾部的半球体半径均为r1，第二水下航行器首部和尾部的半球体半径均为r2；
S3：计算第一水下航行器的首部球心A与第二水下航行器的首部球心B和尾部球心D的距离：DAB、DAD；并计算第一水下航行器的尾部球心C与第二水下航行器的首部球心B和尾部球心D的距离：DCB、DCD；
S4：根据所述步骤S3得到的DAB、DAD、DCD、DCB，计算第一水下航行器首部球体与第二水下航行器首部和尾部间的距离：ΔAB、ΔAD，并计算第一水下航行器尾部球体与第二水下航行器首部和尾部间的距离：ΔCB、ΔCD；
S5：将所述S4步骤得到的ΔAB、ΔAD、ΔCB、ΔCD与防碰撞安全系数值比较，若小于所述防碰撞安全系数值，则水面控制站通过水声通信发射器向第一水下航行器和第二水下航行器发出信号，调整两个水下航行器的航向，避免碰撞，若大于所述防碰撞安全系数值，则进一步计算第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin；所述防碰撞安全系数值为10米；
S6：根据所述S5步骤计算得到的第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin，计算得到第一水下航行器和第二水下航行器胶囊体之间的最小距离ΔCC，ΔCC＝dmin-r1-r2，将所述ΔCC与所述防碰撞安全系数值相比较，若小于所述防碰撞安全系数值，则水面控制站继续通过水声通信发射器向第一水下航行器和第二水下航行器发出信号，调整两个水下航行器的航向；若大于所述防碰撞安全系数值，则水面控制站继续通过水声通信发射器向第一水下航行器和第二水下航行器继续保持现有航向航行；
S7：继续选择多个自主式水下航行器剩余其他的水下航行器中的任意两个重复所述步骤S1-S6，避免所有的自主式水下航行器在航行中碰撞情况的发生。
实施例2
如图1所示，为本实施例提供的一种多个自主式水下航行器协同作业防碰撞方法，包括以下步骤：
S1：选取多个自主式水下航行器中的任意两个自主式水下航行器，作为第一水下航行器和第二水下航行器；
S2：简化所述自主式水下航行器的形状，将所述自主式水下航行器的首部和尾部简化为半球体，；确定第一水下航行器首部球心A的坐标OA为(xA,yA,zA)、第一水下航行器尾部球心C的坐标OC为(xC,yC,zC)、第二水下航行器首部球心B的坐标OB为(xB,yB,zB)、第二水下航行器尾部球心D的坐标OD为(xD,yD,zD)；令第一水下航行器首部和尾部的半球体半径均为r1，第二水下航行器首部和尾部的半球体半径均为r2；
S3：如图2所示，计算第一水下航行器(即AUV1)的首部球心A与第二水下航行器(即AUV2)的首部球心B和尾部球心D的距离：DAB、DAD；并计算第一水下航行器的尾部球心C与第二水下航行器的首部球心B和尾部球心D的距离：DCB、DCD；
计算DAB、DAD、DCD、DCB的计算公式为：
Figure BDA0003147252660000091
Figure BDA0003147252660000092
Figure BDA0003147252660000093
Figure BDA0003147252660000094
S4：根据所述步骤S3得到的DAB、DAD、DCD、DCB，计算第一水下航行器首部球体与第二水下航行器首部和尾部间的距离：ΔAB、ΔAD，并计算第一水下航行器尾部球体与第二水下航行器首部和尾部间的距离：ΔCB、ΔCD；
计算ΔAB、ΔAD、ΔCB、ΔCD的计算公式为：
ΔAB＝DAB-r1-r2；ΔAD＝DAD-r1-r2；ΔCB＝DCB-r1-r2；ΔCD＝DCD-r1-r2；
S5：将所述S4步骤得到的ΔAB、ΔAD、ΔCB、ΔCD与防碰撞安全系数值比较，若小于所述防碰撞安全系数值，则水面控制站通过水声通信发射器向第一水下航行器和第二水下航行器发出信号，调整两个水下航行器的航向，避免碰撞，若大于所述防碰撞安全系数值，则进一步计算第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin；所述防碰撞安全系数值为10米；
具体包括以下步骤：
S51：如图3所示，将所述第一水下航行器和第二水下航行器的躯体中心线段长分别设为L1、L2，确定第一水下航行器的躯体中心线段首端点M的坐标为(x1,y1,z1)、尾端点N的坐标为(x2,y2,z2)，确定第二水下航行器的躯体中心线段首端点P的坐标为(x3,y3,z3)、尾端点M的坐标为(x4,y4,z4)；
S52：取第一水下航行器躯体中心线段任一点R、第二水下航行器躯体中心线段任一点K，构建R点和K点坐标计算模型：
Figure BDA0003147252660000101
其中，s为R点坐标计算参数，t为K点坐标计算参数；
S53：计算第一下航行器躯体中心线段任一点R与第二水下航行器躯体中心线段任一点K的向量RK：RK＝(P-M)-s(N-M)+t(Q-P)，并代入所述S51步骤确定的各个端点坐标，得到所述向量RK：
RK＝[x3-x1,y3-y1,z3-z1]-s[x2-x1,y2-y1,z2-z1]+t[x4-x3,y4-y3,z4-z3]；
S54：利用S52步骤构建的R点和K点坐标计算模型构建带有R点坐标计算参数s和K点坐标计算参数t的所述向量RK的计算模型；
RK＝[ax-scx+tbx，ay-scy+tby,az-scz+tbz]，其中：
Figure BDA0003147252660000102
并构建有关于参数s和t的第一水下航行器躯体中心线段任一点R和第二水下航行器躯体中心线段任一点K之间距离‖RK‖的计算模型f(s,t)：
f(s,t)＝(‖RK‖)2＝(ax-scx+tbx)2+(ay-scy+tby)2+(az-scz+tbz)2；
然后求解两个水下航行器的躯体中心线段长L1和L2之间的最小距离，进而求解得到R点坐标计算参数s和K点坐标计算参数t；
将求解两个水下航行器的躯体中心线段长L1和L2之间的最小距离转化为求解所述‖RK‖的计算模型f(s,t)的最小值问题，包括以下步骤：
S541：令所述‖RK‖的计算模型f(s,t)分别关于所述s和所述t的二阶导为0：
Figure BDA0003147252660000111
S542：进而得到求解所述s和所述t的计算公式：
Figure BDA0003147252660000112
再分别求点M、N到线段PQ，点P、Q到线段MN的最小距离，以及线段MN和线段PQ之间的公垂线距离dCOMM，将空间中两线段MN和PQ之间的最短距离转为点到线段间距离计算，进而构建第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin的计算模型：
Figure BDA0003147252660000113
其中，dM、dP、dN、dQ分别为点M到线段PQ的最小距离、点P到线段MN的最小距离、点N到线段PQ的最小距离、点Q到线段MN的最小距离，dCOMM为两线段公垂线距离；
如图4所示，点M到PQ的最小距离dM的计算方法包括以下步骤：
B1：所述点M在不同位置时，分别过点M做PQ所在直线垂线，垂足为点C；
B2：构建点M到PQ的最小距离dM的计算模型：
Figure BDA0003147252660000121
其中，u为求解参数，|·|为两点之间的距离，即|MC|为点M和点C之间的距离，|MC|为点M和点P之间的距离，|MQ|为点M和点Q之间的距离；采用点到线段距离求解的矢量法，定义参数u：
Figure BDA0003147252660000122
其中PM为点P和点M之间的向量，PQ为点P和点Q之间的向量；其中，图4中的u≤0为图(a)所示的情况，0＜u＜1为图(b)所示情况，u≥1为图c所示情况；
B3：通过三角形面积相等条件，求解所述B2中的两点之间的距离|MC|、|MP|、|MQ|，以|MC|为例：
Figure BDA0003147252660000123
其中，MP为点M和点P之间的向量，MQ为点M和点Q之间的向量；
S6：根据所述S5步骤计算得到的第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin，计算得到第一水下航行器和第二水下航行器胶囊体之间的最小距离ΔCC，ΔCC＝dmin-r1-r2，将所述ΔCC与所述防碰撞安全系数值相比较，若小于所述防碰撞安全系数值，则水面控制站继续通过水声通信发射器向第一水下航行器和第二水下航行器发出信号，调整两个水下航行器的航向；若大于所述防碰撞安全系数值，则水面控制站继续通过水声通信发射器向第一水下航行器和第二水下航行器继续保持现有航向航行；
S7：继续选择多个自主式水下航行器剩余其他的水下航行器中的任意两个重复所述步骤S1-S6，避免所有的自主式水下航行器在航行中碰撞情况的发生。
实施例3
如图5所示，为本实施例提供的采用实施例2提供的方法的多个自主式水下航行器协同作业防碰撞系统，所述系统包括作为控制中心的水面控制站和多个自主式水下航行器湿端，所述水面控制站与多个自主式水下航行器湿端双相通信连接。所述水面控制站包括控制工作站、高精度GPS定位模块和水声通信发射器；所述自主式水下航行器湿端包括安装在所述自主式水下航行器本体的首部声学应答器和尾部声学应答器，以及安装在所述自主式水下航行器本体中间的航行计程仪和高精度惯导模块。
虽然已经参考优选实施例对本发明进行了描述，但在不脱离本发明的范围的情况下，可以对其进行各种改进并且可以用等效物替换其中的部件。尤其是，只要不存在结构冲突，各个实施例中所提到的各项技术特征均可以任意方式组合起来。本发明并不局限于文中公开的特定实施例，而是包括落入权利要求的范围内的所有技术方案。

Claims (10)

1.一种多个自主式水下航行器协同作业防碰撞方法，其特征在于，包括以下步骤：
S1：选取多个自主式水下航行器中的任意两个自主式水下航行器，作为第一水下航行器和第二水下航行器；
S2：简化所述自主式水下航行器的形状，将所述自主式水下航行器的首部和尾部简化为半球体，；确定第一水下航行器首部球心的坐标OA为(xA,yA,zA)、第一水下航行器尾部球心的坐标OC为(xC,yC,zC)、第二水下航行器首部球心的坐标OB为(xB,yB,zB)、第二水下航行器尾部球心的坐标OD为(xD,yD,zD)；令第一水下航行器首部和尾部的半球体半径均为r1，第二水下航行器首部和尾部的半球体半径均为r2；
S3：计算第一水下航行器的首部球心与第二水下航行器的首部球心和尾部球心的距离：DAB、DAD；并计算第一水下航行器的尾部球心与第二水下航行器的首部球心和尾部球心的距离：DCB、DCD；
S4：根据所述步骤S3得到的DAB、DAD、DCD、DCB，计算第一水下航行器首部球体与第二水下航行器首部和尾部间的距离：ΔAB、ΔAD，并计算第一水下航行器尾部球体与第二水下航行器首部和尾部间的距离：ΔCB、ΔCD；
S5：将所述S4步骤得到的ΔAB、ΔAD、ΔCB、ΔCD与防碰撞安全系数值比较，若小于所述防碰撞安全系数值，则水面控制站通过水声通信发射器向第一水下航行器和第二水下航行器发出信号，调整两个水下航行器的航向，避免碰撞，若大于所述防碰撞安全系数值，则进一步计算第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin；所述防碰撞安全系数值为10米；
S6：根据所述S5步骤计算得到的第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin，计算得到第一水下航行器和第二水下航行器胶囊体之间的最小距离ΔCC，ΔCC＝dmin-r1-r2，将所述ΔCC与所述防碰撞安全系数值相比较，若小于所述防碰撞安全系数值，则水面控制站继续通过水声通信发射器向第一水下航行器和第二水下航行器发出信号，调整两个水下航行器的航向；若大于所述防碰撞安全系数值，则水面控制站继续通过水声通信发射器向第一水下航行器和第二水下航行器继续保持现有航向航行；
S7：继续选择多个自主式水下航行器剩余其他的水下航行器中的任意两个重复所述步骤S1-S6，避免所有的自主式水下航行器在航行中碰撞情况的发生。
2.根据权利要求1所述的一种多个自主式水下航行器协同作业防碰撞方法，其特征在于，所述S3步骤计算DAB、DAD、DCD、DCB的计算公式为：
Figure FDA0003147252650000021
Figure FDA0003147252650000022
Figure FDA0003147252650000023
Figure FDA0003147252650000024
3.根据权利要求1所述的一种多个自主式水下航行器协同作业防碰撞方法，其特征在于，所述S4步骤计算ΔAB、ΔAD、ΔCB、ΔCD的计算公式为：
ΔAB＝DAB-r1-r2；ΔAD＝DAD-r1-r2；ΔCB＝DCB-r1-r2；ΔCD＝DCD-r1-r2。
4.根据权利要求1所述的一种多个自主式水下航行器协同作业防碰撞方法，其特征在于，所述S5步骤包括以下步骤：
S51：将所述第一水下航行器和第二水下航行器的躯体中心线段长分别设为L1、L2，确定第一水下航行器的躯体中心线段首端点M的坐标为(x1,y1,z1)、尾端点N的坐标为(x2,y2,z2)，确定第二水下航行器的躯体中心线段首端点P的坐标为(x3,y3,z3)、尾端点M的坐标为(x4,y4,z4)；
S52：取第一水下航行器躯体中心线段任一点R、第二水下航行器躯体中心线段任一点K，构建R点和K点坐标计算模型：
Figure FDA0003147252650000031
其中，s为R点坐标计算参数，t为K点坐标计算参数；
S53：计算第一下航行器躯体中心线段任一点R与第二水下航行器躯体中心线段任一点K的向量RK：RK＝(P-M)-s(N-M)+t(Q-P)，并代入所述S51步骤确定的各个端点坐标，得到所述向量RK：
RK＝[x3-x1,y3-y1,z3-z1]-s[x2-x1,y2-y1,z2-z1]+t[x4-x3,y4-y3,z4-z3]；
S54：利用S52步骤构建的R点和K点坐标计算模型构建带有R点坐标计算参数s和K点坐标计算参数t的所述向量RK的计算模型；
并构建有关于参数s和t的第一水下航行器躯体中心线段任一点R和第二水下航行器躯体中心线段任一点K之间距离‖RK‖的计算模型f(s,t)，求解两个水下航行器的躯体中心线段长L1和L2之间的最小距离，进而求解得到R点坐标计算参数s和K点坐标计算参数t；
再分别求点M、N到线段PQ，点P、Q到线段MN的最小距离，以及线段MN和线段PQ之间的公垂线距离dCOMM，将空间中两线段MN和PQ之间的最短距离转为点到线段间距离计算，进而构建第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin的计算模型。
5.根据权利要求4所述的一种多个自主式水下航行器协同作业防碰撞方法，其特征在于，所述S54步骤中构建的所述向量RK的计算模型为RK＝[ax-scx+tbx，ay-scy+tby,az-scz+tbz]，其中：
Figure FDA0003147252650000032
6.根据权利要求5所述的一种多个自主式水下航行器协同作业防碰撞方法，其特征在于，所述S54步骤中构建的‖RK‖的计算模型f(s,t)如下：
f(s,t)＝(‖RK‖)2＝(ax-scx+tbx)2+(ay-scy+tby)2+(az-scz+tbz)2。
7.根据权利要求5所述的一种多个自主式水下航行器协同作业防碰撞方法，其特征在于，所述S54步骤中求解两个水下航行器的躯体中心线段长L1和L2之间的最小距离转化为求解所述‖RK‖的计算模型f(s,t)的最小值问题，包括以下步骤：
S541：令所述‖RK‖的计算模型f(s,t)分别关于所述s和所述t的二阶导为0：
Figure FDA0003147252650000041
S542：进而得到求解所述s和所述t的计算公式：
Figure FDA0003147252650000042
8.根据权利要求4所述的一种多个自主式水下航行器协同作业防碰撞方法，其特征在于，所述S54步骤中构建的第一水下航行器和第二水下航行器躯体间轴线间最小距离dmin的计算模型为：
Figure FDA0003147252650000043
其中，dM、dP、dN、dQ分别为点M到线段PQ的最小距离、点P到线段MN的最小距离、点N到线段PQ的最小距离、点Q到线段MN的最小距离，dCOMM为两线段公垂线距离。
9.根据权利要求4所述的一种多个自主式水下航行器协同作业防碰撞方法，其特征在于，所述点M到PQ的最小距离的计算方法包括以下步骤：
B1：所述点M在不同位置时，分别过点M做PQ所在直线垂线，垂足为点C；
B2：构建点M到PQ的最小距离dM的计算模型：
Figure FDA0003147252650000051
其中，u为求解参数，|·|为两点之间的距离；采用点到线段距离求解的矢量法，定义参数u：
Figure FDA0003147252650000052
其中PM为点P和点M之间的向量，PQ为点P和点Q之间的向量；
B3：通过三角形面积相等条件，求解所述B2中的两点之间的距离|MC|、|MP|、|MQ|，以|MC|为例：
Figure FDA0003147252650000053
其中，MP为点M和点P之间的向量，MQ为点M和点Q之间的向量。
10.采用根据权利要求1-9任一所述方法的多个自主式水下航行器协同作业防碰撞系统，其特征在于，所述系统包括作为控制中心的水面控制站和多个自主式水下航行器湿端，所述水面控制站与多个自主式水下航行器湿端双相通信连接。所述水面控制站包括控制工作站、高精度GPS定位模块和水声通信发射器；所述自主式水下航行器湿端包括安装在所述自主式水下航行器本体的首部声学应答器和尾部声学应答器，以及安装在所述自主式水下航行器本体中间的航行计程仪和高精度惯导模块。
CN202110755754.8A 2021-07-05 2021-07-05 一种多个自主式水下航行器协同作业防碰撞方法及系统 Active CN113486445B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110755754.8A CN113486445B (zh) 2021-07-05 2021-07-05 一种多个自主式水下航行器协同作业防碰撞方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110755754.8A CN113486445B (zh) 2021-07-05 2021-07-05 一种多个自主式水下航行器协同作业防碰撞方法及系统

Publications (2)

Publication Number Publication Date
CN113486445A true CN113486445A (zh) 2021-10-08
CN113486445B CN113486445B (zh) 2022-04-15

Family

ID=77939916

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110755754.8A Active CN113486445B (zh) 2021-07-05 2021-07-05 一种多个自主式水下航行器协同作业防碰撞方法及系统

Country Status (1)

Country Link
CN (1) CN113486445B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109470249A (zh) * 2018-11-07 2019-03-15 河海大学 一种水下航行器的最优路径规划与避障设计方法
CN109769222A (zh) * 2019-02-27 2019-05-17 天津城建大学 基于多水下自主航行器的水下传感器网络路由方法
JP2019179474A (ja) * 2018-03-30 2019-10-17 国立研究開発法人 海上・港湾・航空技術研究所 水中航走体の衝突後対応方法、衝突後対応装置、及び衝突後対応プログラム
CN111483578A (zh) * 2020-04-30 2020-08-04 山东省科学院海洋仪器仪表研究所 自主式水下航行器与海洋平台的柔性对接装置
CN113044172A (zh) * 2021-03-09 2021-06-29 大连海事大学 一种蜂窝状的auv集群布放回收系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2019179474A (ja) * 2018-03-30 2019-10-17 国立研究開発法人 海上・港湾・航空技術研究所 水中航走体の衝突後対応方法、衝突後対応装置、及び衝突後対応プログラム
CN109470249A (zh) * 2018-11-07 2019-03-15 河海大学 一种水下航行器的最优路径规划与避障设计方法
CN109769222A (zh) * 2019-02-27 2019-05-17 天津城建大学 基于多水下自主航行器的水下传感器网络路由方法
CN111483578A (zh) * 2020-04-30 2020-08-04 山东省科学院海洋仪器仪表研究所 自主式水下航行器与海洋平台的柔性对接装置
CN113044172A (zh) * 2021-03-09 2021-06-29 大连海事大学 一种蜂窝状的auv集群布放回收系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
袁健等: "多自主水下航行器的有限时间无碰撞协调控制方法", 《山东科学》 *

Also Published As

Publication number Publication date
CN113486445B (zh) 2022-04-15

Similar Documents

Publication Publication Date Title
CA3067576A1 (en) Cooperative autonomous navigation systems and methods for multiple unmanned surface vehicles considering communication
CN110196599B (zh) 一种避碰与连接保持约束下的无人艇分布式编队控制方法
WO2023045357A1 (zh) 一种基于统计相似度量的组合导航鲁棒滤波方法
CN111366962A (zh) 一种深远海低成本长航时协同导航定位系统
CN111273298B (zh) 基于波浪滑翔器组网技术的水下声学目标定位与跟踪方法
CN108062108A (zh) 一种基于机载计算机的智能多旋翼无人机及其实现方法
CN101788295A (zh) 一种小型水下航行器用组合导航系统及方法
CN109375646A (zh) 基于fmsrupf算法的auv对接回收自主导航方法
CN109855649B (zh) 一种水下滑翔机平台运动轨迹被动确定方法
CN101957204B (zh) 一种基于相互测距信息的机群惯性导航数据融合方法
CN107664096B (zh) 偏航对风控制方法、装置及系统
CN110737742A (zh) 一种地图平台建模与人员轨迹显示的方法和系统
CN113486445B (zh) 一种多个自主式水下航行器协同作业防碰撞方法及系统
US12091137B1 (en) Intelligent assistance system and method for berthing and unberthing based on multi-tugboat collaboration
CN103400416A (zh) 一种基于概率多层地形的城市环境机器人导航方法
CN114994600B (zh) 基于高度辅助的大规模矿井下用户三维实时定位方法
CN111398905A (zh) 一种基于多声学信标引导的auv水下回收对接系统
CN112051845A (zh) 一种面向无人拖航作业的分布式集群控制系统及其方法
CN104602191B (zh) 一种海上搜救无线传感网节点定位方法
CN103699007B (zh) 一种船舶动力定位系统的设计方法
CN110389318A (zh) 一种基于立体六元阵的水下移动平台定位系统及方法
CN103108281A (zh) 一种基于无线传感器网络的海上搜救目标实时定位方法
CN111830461A (zh) 一种无人机机载uwb定位方法
CN108012328A (zh) 一种基于无线传感器网络的水下搜救区域预测方法
CN115793687A (zh) 一种基于碰撞风险的旋翼无人机运行安全间隔标定方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
PE01 Entry into force of the registration of the contract for pledge of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: A Collision Prevention Method and System for Cooperative Operation of Multiple Autonomous Underwater Vehicles

Effective date of registration: 20221027

Granted publication date: 20220415

Pledgee: Hunan Xiangjiang Zhongying Investment Management Co.,Ltd.

Pledgor: HUNAN GUOTIAN ELECTRONIC TECHNOLOGY CO.,LTD.

Registration number: Y2022980019937