CN116027796A - 一种多自治水下机器人编队控制系统及方法 - Google Patents
一种多自治水下机器人编队控制系统及方法 Download PDFInfo
- Publication number
- CN116027796A CN116027796A CN202211675922.3A CN202211675922A CN116027796A CN 116027796 A CN116027796 A CN 116027796A CN 202211675922 A CN202211675922 A CN 202211675922A CN 116027796 A CN116027796 A CN 116027796A
- Authority
- CN
- China
- Prior art keywords
- auv
- formation
- virtual
- control
- autonomous underwater
- 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.)
- Pending
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 146
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000013461 design Methods 0.000 claims description 19
- 238000005381 potential energy Methods 0.000 claims description 12
- 238000012545 processing Methods 0.000 claims description 5
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 5
- 230000009471 action Effects 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 4
- 238000013528 artificial neural network Methods 0.000 claims description 3
- 238000013016 damping Methods 0.000 claims description 3
- 238000012938 design process Methods 0.000 claims description 3
- 238000012634 optical imaging Methods 0.000 claims description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims 1
- 230000004888 barrier function Effects 0.000 abstract description 2
- 230000008569 process Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000003321 amplification Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000003199 nucleic acid amplification method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 229910052500 inorganic mineral Inorganic materials 0.000 description 1
- 239000011707 mineral Substances 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Abstract
本发明公开了一种多自治水下机器人编队控制系统及方法，编队控制系统包括：上位机以及含有若干自治水下机器人AUV的自治水下机器人编队系统；所述上位机设于母船或者岸上，用于在编队未开始时接收每台自治水下机器人发送的信息，并在编队任务开始后根据所接收的信息指定领航AUV和跟随AUV，并在编队任务执行中定时向领航AUV发送编队控制指令；所述自治水下机器人编队系统与上位机通信，用于向上位机发送位置信息，并接收来自上位机的控制指令以完成相应的编队任务。机器人之间有效协作，遇到障碍物多的情况下可保持稳定，不存在局部极小和目标点不可达的问题。
Description
技术领域
本发明属于水下机器人协作控制技术领域，具体涉及一种多自治水下机器人编队控制系统及方法。
背景技术
AUV(Autonomous Underwater Vehicle)的编队控制属于多机器人协作问题，是指多AUV在航行的过程中保持特定的几何形状，同时能够根据环境的变化来改变队形实现如避开障碍物等目的的控制技术。近年来，随着通信技术，计算机科学等的快速发展，使得多AUV编队控制技术也取得了很大的进展。多AUV的编队控制在很多领域都有着广泛的应用，例如多AUV编队航行，对水下矿产资源的勘探，以及海底搜救等。在这些领域多AUV编队控制技术都发挥着无可比拟的作用。多AUV编队控制较单机器人作业有很多优势，包括：可以形成覆盖面积较大的探测区域，快速执行大范围的搜索任务，节约作业时间、提高工作效率；可以组成一个功能强大并且全面的系统；数据的实时共享将使多AUV系统获得更全面的外部环境信息，提高系统本身的鲁棒性和自适应能力等。多AUV编队研究有着很高的实践及战略意义。
目前编队控制方法大致分为以下几种：基于行为法、领航-跟随法、虚拟结构法、模型预测控制方法、图论法、人工势场法和动态窗口法等。但是基于行为法仅是模拟人或动物的行为，机器人之间缺乏有效的协作；虚拟结构法、领航-跟随法将每个机器人限制在一定的范围内保持队形的稳定，但是这些算法局限于简单的工作环境，对环境信息的依赖太大，遇到障碍物多的情况无法保持稳定。人工势场和动态窗口算法能够是机器人在虚拟力的作用下形成编队，但是他们缺乏全局信息，存在局部极小和目标点不可达的问题。模型预测算法对模型准确度要求高，且泛化能力差。因此，多自治水下机器人在复杂海洋环境下的编队控制仍有很大的提升空间。
发明内容
本发明所要解决的技术问题是针对上述现有技术的不足，提供一种多自治水下机器人编队控制系统及方法。
为实现上述技术目的，本发明采取的技术方案为：
一种多自治水下机器人编队控制系统，包括上位机以及含有若干自治水下机器人AUV的自治水下机器人编队系统；
所述上位机设于母船或者岸上，用于在编队未开始时接收每台自治水下机器人发送的信息，并在编队任务开始后根据所接收的信息指定领航AUV和跟随AUV，并在编队任务执行中定时向领航AUV发送编队控制指令；
所述自治水下机器人编队系统与上位机通信，用于向上位机发送位置信息，并接收来自上位机的控制指令以完成相应的编队任务。
为优化上述技术方案，采取的具体措施还包括：
上述的上位机包括基于中央处理器的控制单元、功率放大器和多路发射接收单元；
所述基于中央处理器的控制单元用于运行自组织神经网络算法，产生控制指令；
所述功率放大器与控制单元连接，用于发射或接收信号的放大；
所述多路发射接收单元与功率放大单元连接，用于对经过功率放大器放大的控制信号进行调制，并将其发射给自治水下机器人编队系统，同时，接收自治水下机器人发送的信息，经功率放大单元传输给控制单元。
每台自治水下机器人均包括：
信号接收和发射器，用于接收上位机发送编队控制指令，同时将位置信息发送给上位机；
行为控制器，与信号接收和发射单元连接，用于将接收的上位机控制指令转换为自治水下机器人控制信号；并对传感器系统采集的数据进行分析，判断是否到达控制要求；
舵机控制模块，与行为控制器连接，用于控制转向和抬/沉艏的方向；
电机控制模块，与行为控制器连接，用于控制电机，和舵机控制模块配合完成自治水下机器人的前进、后退、左转、右转、抬艏、沉艏动作；
传感器系统，与行为控制器和信号接收和发射器连接，用于采集数据并将数据传输至行为控制模块以及通过信号接收和发射单元发送给上位机。
上述的传感器系统包括深度传感器、速度传感器、姿态传感器、声呐传感器、光学成像传感器。
一种多自治水下机器人编队控制方法，包含以下步骤：
步骤1、上位机在编队任务开始前，获取每个AUV的位置信息；
步骤2、根据位置信息和任务要求，上位机指定一个AUV为领航者，其余AUV为跟随者；
步骤3、编队任务开始后，为每一个跟随AUV引入一个虚拟AUV；
步骤4、根据领航AUV的位置信息和队形要求，计算出虚拟AUV的轨迹和速度；
步骤5、跟随AUV利用反步的方式对虚拟AUV进行轨迹跟踪，实现编队；
步骤6、多AUV以编队形式向目标航行时，如果遇见障碍物，编队以虚拟弹簧的方式进行避障以及保持队形；
步骤7、上位机定期向领航AUV发送编队信息，领航AUV根据编队信息确定航行位置；
步骤8、如果领航AUV定期收到上位机信息，则重复步骤5～7，如果领航AUV超时未收到上位机信息，则保持现有队形浮出水面，避免出现事故。
上述的步骤4中的虚拟AUV的轨迹和速度，计算步骤包括：
步骤4.1、虚拟AUV参考轨迹量设计；
为得到虚拟AUV的位置量和速度量，设领航AUV的位置信息为ηm＝[xm ym zmψm]T，其中，xm，ym，zm为惯性坐标系下领航AUV的位置信息，ψm为领航AUV绕Z轴逆时针方向转过的角度值，首先根据领航AUV的位置量ηm得到其参考轨迹量：
ηd＝ηm+J(ηm)·l (1)
式中，l＝[dsinαcosβdsinαsinβdcosα0]T为载体坐标系下虚拟AUV与领航AUV的相对位置，d、α、β为队形参数；
代入相应参数，由式(1)得到：
步骤4.2、虚拟AUV位置量设计；
为得到虚拟AUV的位置量ηv和速度量Vv，利用其参考轨迹量ηd，并且引入辅助控制变量及相关设计参数，位置量ηv具体设计过程如下：
设虚拟AUV位置量为ηv＝[xv yv zvψv]T，其中，xv，yv，zv为惯性坐标系下虚拟AUV的位置信息，ψv为虚拟AUV绕Z轴逆时针方向转过的角度值，定义虚拟AUV误差量：
引入辅助误差变量：
re＝ev+φ (4)
其中，φ＝[φ1,φ2,φ3,φ4]T，为辅助控制变量，φi(i＝1,2,3,4)为设计参数；
定义辅助控制变量φ的导数为：
式中，β1(φ)＝[λ1 tanh(φ1/λ1)λ2 tanh(φ2/λ2)λ3 tanh(φ3/λ3)λ4 tanh(φ4/λ4)]T，
为设计参数，K＝diag[k1,k2,k3,k4]，
为设计参数。
将式(3)带入式(4)，然后代入式(5)得到：
将相关参数代入式(6)，得到：
定义虚拟AUV的位置量微分方程为：
式中，
β2(φ)＝[k1 tanh(φ1/k1)k2 tanh(φ2/k2)k3 tanh(φ3/k3)k4 tanh(φ4/k4)]T，φi,ki(i＝1,2,3,4)
为设计参数，与辅助控制变量φ中φi及K中ki设置值相同；
代入相关参数，式(8)展开得到微分方程：
步骤4.3、虚拟AUV速度量计算：
定义虚拟AUV速度量为：
Vv＝[uv vv wv rv]T＝J-1(ηv)(β1(φ)+β2(φ)) (10)
结合式(7)和式(9)，得到微分方程组：
根据式(11)和式(12)，经计算得到虚拟AUV位置量ηv和辅助控制变量φ。根据式(10)得到虚拟AUV速度量Vv，则可实现仅利用领航AUV位置量ηm得到虚拟AUV的位置量ηv和速度量。
上述的步骤5包括以下步骤：
步骤5.1编队误差设计：
虚拟AUV在惯性坐标系下的运行轨迹为：ηv＝[xv yv zvψv]T，速度信息：Vv＝[uv vvwv rv]T，(xv，yv，zv)为惯性坐标系下虚拟AUV的位置信息，ψv为虚拟AUV绕Z轴逆时针方向转过的角度值；
跟随AUV的实际航行状态定义为：ηf＝[xf yf zfψf]T，其中，xf，yf，zf为惯性坐标系下跟随AUV的位置信息，ψf为跟随AUV绕Z轴逆时针方向转过的角度值；
为实现编队控制任务，使AUV按照期望队形进行航行，需要控制跟随AUV的速度和角速度量Vf＝[uf vf wf rf]T，其中，uf vf wf分别为跟随AUV线速度矢量在载体坐标系中的3个分量，rf分别为跟随AUV角速度矢量在载体坐标系Z轴中的分量，使其能够跟踪虚拟AUV的轨迹，最终使惯性坐标系下编队控制误差ee收敛于0；
其中，误差ee为：
步骤5.2、运动学编队控制率设计：
将步骤4得到的虚拟AUV位置信息及速度信息，作为跟随AUV的期望状态，为跟随AUV设计稳定的运动学编队控制律，使跟随AUV跟踪虚拟AUV的轨迹，进而与领航AUV保持期望距离和角度，实现编队任务，具体的：
由反步方法得到运动学编队控制律：
式中，k，kz，kψ为正常系数；
上述的步骤6包括以下步骤：
步骤6.1领航AUV合力分析：
根据障碍物的影响范围ρo和目标点和障碍物距离的关系引入
使AUV处于障碍物的影响范围之内，受到弹簧牵引力大于障碍物的弹性排斥力，从而保证AUV可以到达目标；
领航AUV的合力包含目标弹簧的牵引力和障碍物的排斥力；
目标弹簧牵引力为：
其中，kg为目标虚拟弹簧的正比例因子，q为AUV的位置，qt为目标位置，qo为障碍物的位置，f(qo-qt)是障碍物距目标点的欧式距离；
障碍物的排斥力Freq(q)为：
其中，f(qo-qt)为AUV和障碍物之间的距离，l1是障碍物的最大撞击距离；k0为弹簧的弹性系数；
领航AUV的合力F表示为：
步骤6.2跟随AUV合力分析：
存储在领航AUV虚拟弹簧中的势能为：
其中，Eij(q)是AUV间的虚拟弹簧的势能，Et(q)是领航AUV和目标之间的虚拟弹簧的势能，Eo(q)是AUV和障碍物之间的虚拟弹簧的势能；kf为虚拟弹簧的阻尼系数，lrij为弹簧实际长度；
虚拟弹簧模型考虑到了跟随AUV的避障功能，当跟随AUV运动到障碍物影响范围内时，将受到公式(17)障碍物的斥力Frep，跟随者将在排斥力的驱使下避开障碍物向虚拟领航者位置移动，此时跟随AUV合力Fa表示为：
其中，Frij为虚拟弹簧引力；
步骤6.3局部极小点解决：
当AUV被困在复杂障碍物时，即陷入局部极小状态，AUV切换至避障行为，此时附加旋转力场被激活，fi r将找到一条新的路径逃离局部极小状态；
其中，
γ为常量。
旋转力场被激活后，AUV控制律的总矢量为：
此时，AUV处于躲避障碍物状态的控制律为
其中，θF和θl分别为领航机器人合力的方向角与当前运动的朝向角，
和
为增益系数。vlim和ωlim是领航AUV的速度和角速度上限。
本发明具有以下有益效果：
本发明的编队控制系统包含上位机以及与上位机无线通信连接的多台自治水下机器人。一旦上位机发送编队控制指令，多台自治水下机器人采用基于虚拟弹簧的领航-跟随控制方式进行编队。首先为多AUV系统指定一个领航AUV，其余为跟随AUV；然后为每个跟随AUV引入虚拟AUV，根据编队队形计算出虚拟AUV的轨迹和速度；最后跟随AUV利用反步的方式实现对虚拟AUV的跟踪，进而实现编队。本发明考虑在未知环境下，机器人的初始位姿与终点为已知条件，环境中的障碍物信息未知，编队过程中，领航者进行路径规划，负责驶向目标、避障和引导编队运动，跟随机器人在队形力与障碍物虚拟弹簧力的“合力”作用下，跟随领航者向虚拟领航者位置移动。若没有障碍物，多机器人会保持预定的队形，当遇到障碍物时，机器人切换避障模式，跟随者改变与领航者的期望角度和距离，使用虚拟弹簧法避开障碍物，解决在复杂环境中AUV易碰撞和易脱队的问题。上位机定期发送信息给领航AUV，领航AUV根据上位机指令，带领跟随AUV航行。如果在规定的时间领航AUV没有收到信息，则保持现有队形浮出水面，避免出现事故。机器人之间有效协作，遇到障碍物多的情况下可保持稳定，不存在局部极小和目标点不可达的问题。
附图说明
图1为本发明一种多自治水下机器人编队控制系统框图。
图2为本发明三维领航-跟随式编队控制原理图。
图3为本发明运动学编队控制系统基本结构框图。
图4为本发明基于虚拟弹簧领航-跟随编队方法的流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
本发明中的步骤虽然用标号进行了排列，但并不用于限定步骤的先后次序，除非明确说明了步骤的次序或者某步骤的执行需要其他步骤作为基础，否则步骤的相对次序是可以调整的。可以理解，本文中所使用的术语“和/或”涉及且涵盖相关联的所列项目中的一者或一者以上的任何和所有可能的组合。
如图1所示，本发明一种多自治水下机器人编队控制系统，包含：
1.上位机，在编队未开始时接收每台自治水下机器人发送的信息，并根据这些信息在编队任务开始后指定领航AUV和跟随AUV。并且在编队任务执行中定时向领航AUV发送编队控制指令。上位机放置在母船或者岸上，主要包括
(1)基于中央处理器的控制单元∶对上位机进行控制，并运行自组织神经网络算法，产生控制指令；
(2)功率放大器：与控制单元连接，用于放大发射与接收的信号；
(3)多路发射接收单元：和功率放大单元连接，对经过功率放大器放大的控制信号进行调制，并将其发射给自治水下机器人编队系统。同时，接收自治水下机器人发送的信息，经功率放大单元传输给控制单元。
2.由一台以上的自治水下机器人组成的自治水下机器人编队系统，其与上位机进行通信，发送位置信息，并接收来自上位机的控制指令以完成相应的编队任务。其中，每台自治水下机器人分别包含以下单元：
(1)信号接收和发射器：用于接收上位机发送编队控制指令，同时将位置信息发送给上位机；
(2)行为控制器：与接收和发射单元连接，将接收的上位机控制指令转换为自治水下机器人控制信号；并对传感器系统采集的数据进行分析，是否到达控制要求；
(3)舵机控制模块：与行为控制器连接，用于控制转向和抬/沉艏两个自由度上的方向；
(4)电机控制模块：与行为控制器连接，用于控制电机。和舵机控制模块配合完成自治水下机器人的前进、后退、左转、右转、抬艏、沉艏等动作；
(5)传感器系统：与行为控制器和信号接收和发射器连接，该系统中包含深度传感器、速度传感器、姿态传感器、声呐传感器、光学成像传感器。传感器系统将采集到的数据传输给行为控制模块，还将一部分数据通过接收和发射单元发送给上位机。
如图4所示，本发明的一种多自治水下机器人编队控制方法，用于上述多自治水下机器人编队控制系统，至少包含以下步骤：
步骤1、上位机在编队任务开始前，获取每个AUV的位置信息；
步骤2、根据位置信息和任务要求，上位机指定一个AUV为领航者，其余AUV为跟随者；
步骤3、编队任务开始后，为每一个跟随AUV引入一个虚拟AUV；
步骤4、根据领航AUV的位置信息和队形要求；计算出虚拟AUV的轨迹和速度；
步骤5、跟随AUV利用反步的方式对虚拟AUV进行轨迹跟踪，从而达到编队的目的；
步骤6、多AUV以编队形式向目标航行时，如果遇见障碍物，编队将以虚拟弹簧的方式进行避障以及保持队形；
步骤7、上位机定期向领航AUV发送编队信息，领航AUV根据编队信息确定航行位置；
步骤8、如果领航AUV定期收到上位机信息，则重复步骤5～7；如果领航AUV超时未收到上位机信息，则保持现有队形浮出水面，避免出现事故。
所述步骤4中的虚拟AUV的轨迹和速度，计算步骤包括：
步骤4.1、虚拟AUV参考轨迹量设计；
为得到虚拟AUV的位置量和速度量，设领航AUV的位置信息为ηm＝[xm ym zmψm]T，其中，xm，ym，zm)为惯性坐标系下领航AUV的位置信息，ψm为领航AUV绕Z轴逆时针方向转过的角度值)首先根据领航AUV的位置量ηm得到其参考轨迹量：
ηd＝ηm+J(ηm)·l (1)
式中，l＝[dsinαcosβdsinαsinβdcosα0]T为载体坐标系下虚拟AUV与领航AUV的相对位置，d、α、β为队形参数，具体数值在仿真过程中根据编队任务进行设定，具体意义参见图2。
代入相应参数，由式(1)得到：
步骤4.2、虚拟AUV位置量设计；
为得到虚拟AUV的位置量ηv和速度量Vv，利用其参考轨迹量ηd，并且引入辅助控制变量及相关设计参数，位置量ηv具体设计过程如下：
设虚拟AUV位置量为ηv＝[xv yv zvψv]T，其中，xv，yv，zv为惯性坐标系下虚拟AUV的位置信息，ψv为虚拟AUV绕Z轴逆时针方向转过的角度值，定义虚拟AUV误差量：
虚拟AUV控制算法设计的目的是通过辅助变量的引入得到虚拟AUV的位置量和速度量，最终使得误差量ev为0；
引入辅助误差变量：
re＝ev+φ (4)
其中，φ＝[φ1,φ2,φ3,φ4]T，为辅助控制变量，φi(i＝1,2,3,4)为设计参数。
定义辅助控制变量φ的导数为：
式中，β1(φ)＝[λ1tanh(φ1/λ1)λ2tanh(φ2/λ2)λ3tanh(φ3/λ3)λ4tanh(φ4/λ4)]T，
为设计参数，K＝diag[k1,k2,k3,k4]，
为设计参数。
将式(3)带入式(4)，然后代入式(5)得到：
将相关参数代入式(6)，得到：
定义虚拟AUV的位置量微分方程为：
式中，β2(φ)＝[k1 tanh(φ1/k1)k2 tanh(φ2/k2)k3 tanh(φ3/k3)k4 tanh(φ4/k4)]T，φi,ki(i＝1,2,3,4)为设计参数，与辅助控制变量φ中φi及K中ki设置值相同。
代入相关参数，式(8)展开得到微分方程：
步骤4.3、虚拟AUV速度量计算。
定义虚拟AUV速度量为：
Vv＝[uv vv wv rv]T＝J-1(ηv)(β1(φ)+β2(φ)) (10)
式中，β1(φ)和β2(φ)意义与式(5)和式(8)相应参数意义相同。
结合式(7)和式(9)，得到微分方程组：
根据式(11)和式(12)，经计算可得到虚拟AUV位置量ηv和辅助控制变量φ。据式(10)得到虚拟AUV速度量Vv，则可实现仅利用领航AUV位置量ηm得到虚拟AUV的位置量ηv和速度量Vv。
所述步骤5还包含以下步骤：
步骤5.1编队误差设计；
虚拟AUV在惯性坐标系下的运行轨迹为：ηv＝[xv yv zvψv]T，速度信息：Vv＝[uv vvwv rv]T，(xv，yv，zv)为惯性坐标系下虚拟AUV的位置信息，ψv为虚拟AUV绕Z轴逆时针方向转过的角度值。跟随AUV的实际航行状态定义为：ηf＝[xf yf zfψf]T。其中，xf，yf，zf为惯性坐标系下跟随AUV的位置信息，ψf为跟随AUV绕Z轴逆时针方向转过的角度值；为实现编队控制任务，使AUV按照期望队形进行航行，需要控制跟随AUV的速度和角速度量Vf＝[uf vf wf rf]T，其中，uf vf wf分别为跟随AUV线速度矢量在载体坐标系中的3个分量；rf分别为跟随AUV角速度矢量在载体坐标系Z轴中的分量，使其能够跟踪虚拟AUV的轨迹，最终使惯性坐标系下编队控制误差ee收敛于0。
步骤5.2、运动学编队控制率设计。
将步骤4中得到的虚拟AUV位置信息及速度信息，作为跟随AUV的期望状态，为跟随AUV设计稳定的运动学编队控制律，使跟随AUV跟踪虚拟AUV的轨迹，进而与领航AUV保持期望距离和角度，实现编队任务。
反步控制方法因其控制性能简单实用，广泛应用于移动机器人中，现将其应用在三维水下环境AUV编队控制之中，对应的控制律设计也是基于三维环境的。
由反步方法得到运动学编队控制律：
式中，k，kz，kψ为正常系数。
所述步骤6还包含以下步骤：
步骤6.1领航AUV合力
根据障碍物的影响范围ρo和目标点和障碍物距离的关系引入
使AUV处于障碍物的影响范围之内，受到弹簧牵引力大于障碍物的弹性排斥力，从而保证AUV可以到达目标。领航AUV的合力包含目标弹簧的牵引力和障碍物的排斥力。
目标弹簧牵引力Fatt(q)为：
其中kg为目标虚拟弹簧的正比例因子，q为AUV的位置，qt为目标位置，qo为障碍物的位置，f(qo-qt)是障碍物距目标点的欧式距离。
障碍物的斥力Freq(q)为：
其中，f(qo-qt)为AUV和障碍物之间的距离，l1是障碍物的最大撞击距离，k0为弹簧的弹性系数。
领航AUV的合力F表示为：
步骤6.2跟随AUV合力
存储在领航AUV虚拟弹簧中的势能为：
其中，Eij(q)是AUV间的虚拟弹簧的势能，Et(q)是领航AUV和目标之间的虚拟弹簧的势能，Eo(q)是AUV和障碍物之间的虚拟弹簧的势能，kf为虚拟弹簧的阻尼系数，lrij为弹簧实际长度。
虚拟弹簧模型考虑到了跟随AUV的避障功能，当跟随AUV运动到障碍物影响范围内时，将受到公式(17)障碍物的斥力Frep，跟随者将在排斥力的驱使下避开障碍物向虚拟领航者位置移动，此时跟随者合力Fa表示为：
其中，Frij为虚拟弹簧引力。
步骤6.3局部极小点解决
当AUV被困在复杂障碍物时，即陷入局部极小状态，AUV切换至避障行为，此时附加旋转力场被激活，fi r将找到一条新的路径逃离局部极小状态。
其中
γ表示一常量。
旋转力场被激活后，AUV控制律的总矢量为：
此时，AUV处于躲避障碍物状态的控制律为
其中，θF和θl分别为领航机器人合力的方向角与当前运动的朝向角，
和
为增益系数，vlim和ωlim是领航AUV的速度和角速度上限。
如图2所示，考虑到自治水下机器人在编队过程中上位机只与领航AUV通信。此时，需要先求出期望的跟随AUV的速度量以及位置量，然后引入反步控制策略，为跟随AUV设计编队控制率，让跟随AUV对虚拟AUV进行轨迹跟踪，从而达到预期的编队效果。
图2给出了具体编队过程，将多AUV拆分为两两一组，首先以两个AUV(包括领航AUV和跟随AUV)为例进行编队设计，然后将其扩展到7AUV的编队，进而可以扩展到更多AUV。
如图3所示，运动学编队控制系统中，利用领航AUV位置信息ηm＝[xm ym zmψm]T得到虚拟AUV速度信息Vv＝[uv uv wv rv]T及位置信息ηv＝[xv yv zvψv]T。将跟随AUV实际轨迹ηf＝[xf yf zfψf]T与虚拟AUV轨迹误差量ee＝[ex ey ez eψ]T和虚拟AUV的速度值Vv＝[uv vv wvrv]T共同作为反步控制的输入。
综上所述，整个多自治水下机器人编队控制系统的工作过程是：
编队任务开始之前，每个AUV将自己的位置信息发送给上位机。
一旦编队任务开始，上位机根据当前AUV的位置，指定一个领航AUV，其余为跟随AUV；
然后为每个跟随AUV引入虚拟AUV，根据编队队形计算出虚拟AUV的轨迹和速度；跟随AUV利用反步的方式实现对虚拟AUV的跟踪，进而实现编队。
在航行过程中，编队通过虚拟弹簧方式躲避障碍物。
同时，上位机定期发送信息给领航AUV，领航AUV根据上位机指令，带领跟随AUV航行。
如果在规定的时间领航AUV没有收到信息，则保持现有队形浮出水面，避免出现事故。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。
Claims (8)
1.一种多自治水下机器人编队控制系统，其特征在于，包括上位机以及含有若干自治水下机器人AUV的自治水下机器人编队系统；
所述上位机设于母船或者岸上，用于在编队未开始时接收每台自治水下机器人发送的信息，并在编队任务开始后根据所接收的信息指定领航AUV和跟随AUV，并在编队任务执行中定时向领航AUV发送编队控制指令；
所述自治水下机器人编队系统与上位机通信，用于向上位机发送位置信息，并接收来自上位机的控制指令以完成相应的编队任务。
2.根据权利要求1所述的一种多自治水下机器人编队控制系统，其特征在于，所述上位机包括基于中央处理器的控制单元、功率放大器和多路发射接收单元；
所述基于中央处理器的控制单元用于运行自组织神经网络算法，产生控制指令；
所述功率放大器与控制单元连接，用于发射或接收信号的放大；
所述多路发射接收单元与功率放大单元连接，用于对经过功率放大器放大的控制信号进行调制，并将其发射给自治水下机器人编队系统，同时，接收自治水下机器人发送的信息，经功率放大单元传输给控制单元。
3.根据权利要求1所述的一种多自治水下机器人编队控制系统，其特征在于，每台自治水下机器人均包括：
信号接收和发射器，用于接收上位机发送编队控制指令，同时将位置信息发送给上位机；
行为控制器，与信号接收和发射单元连接，用于将接收的上位机控制指令转换为自治水下机器人控制信号；并对传感器系统采集的数据进行分析，判断是否到达控制要求；
舵机控制模块，与行为控制器连接，用于控制转向和抬/沉艏的方向；
电机控制模块，与行为控制器连接，用于控制电机，和舵机控制模块配合完成自治水下机器人的前进、后退、左转、右转、抬艏、沉艏动作；
传感器系统，与行为控制器和信号接收和发射器连接，用于采集数据并将数据传输至行为控制模块以及通过信号接收和发射单元发送给上位机。
4.根据权利要求1所述的一种多自治水下机器人编队控制系统，其特征在于，所述传感器系统包括深度传感器、速度传感器、姿态传感器、声呐传感器、光学成像传感器。
5.根据权利要求1-4任一所述的系统的一种多自治水下机器人编队控制方法，其特征在于，所述方法包含以下步骤：
步骤1、上位机在编队任务开始前，获取每个AUV的位置信息；
步骤2、根据位置信息和任务要求，上位机指定一个AUV为领航者，其余AUV为跟随者；
步骤3、编队任务开始后，为每一个跟随AUV引入一个虚拟AUV；
步骤4、根据领航AUV的位置信息和队形要求，计算出虚拟AUV的轨迹和速度；
步骤5、跟随AUV利用反步的方式对虚拟AUV进行轨迹跟踪，实现编队；
步骤6、多AUV以编队形式向目标航行时，如果遇见障碍物，编队以虚拟弹簧的方式进行避障以及保持队形；
步骤7、上位机定期向领航AUV发送编队信息，领航AUV根据编队信息确定航行位置；
步骤8、如果领航AUV定期收到上位机信息，则重复步骤5～7，如果领航AUV超时未收到上位机信息，则保持现有队形浮出水面，避免出现事故。
6.根据权利要求5所述的一种多自治水下机器人编队控制方法，其特征在于，
所述步骤4中的虚拟AUV的轨迹和速度，计算步骤包括：
步骤4.1、虚拟AUV参考轨迹量设计；
为得到虚拟AUV的位置量和速度量，设领航AUV的位置信息为ηm＝[xm ym zmψm]T，其中，xm，ym，zm为惯性坐标系下领航AUV的位置信息，ψm为领航AUV绕Z轴逆时针方向转过的角度值，首先根据领航AUV的位置量ηm得到其参考轨迹量：
ηd＝ηm+J(ηm)·l (1)
式中，l＝[dsinαcosβdsinαsinβdcosα0]T为载体坐标系下虚拟AUV与领航AUV的相对位置，d、α、β为队形参数；
代入相应参数，由式(1)得到：
步骤4.2、虚拟AUV位置量设计；
为得到虚拟AUV的位置量ηv和速度量Vv，利用其参考轨迹量ηd，并且引入辅助控制变量及相关设计参数，位置量ηv具体设计过程如下：
设虚拟AUV位置量为ηv＝[xv yv zv ψv]T，其中，xv，yv，zv为惯性坐标系下虚拟AUV的位置信息，ψv为虚拟AUV绕Z轴逆时针方向转过的角度值，定义虚拟AUV误差量：
引入辅助误差变量：
re＝ev+φ (4)
其中，φ＝[φ1,φ2,φ3,φ4]T，为辅助控制变量，φi(i＝1,2,3,4)为设计参数；
定义辅助控制变量φ的导数为：
式中，β1(φ)＝[λ1tanh(φ1/λ1)λ2tanh(φ2/λ2)λ3tanh(φ3/λ3)λ4tanh(φ4/λ4)]T，
为设计参数，K＝diag[k1,k2,k3,k4]，
为设计参数；
将式(3)带入式(4)，然后代入式(5)得到：
将相关参数代入式(6)，得到：
定义虚拟AUV的位置量微分方程为：
式中，
β2(φ)＝[k1tanh(φ1/k1) k2tanh(φ2/k2) k3tanh(φ3/k3) k4tanh(φ4/k4)]T，φi,ki(i＝1,2,3,4)
为设计参数，与辅助控制变量φ中φi及K中ki设置值相同；
代入相关参数，式(8)展开得到微分方程：
步骤4.3、虚拟AUV速度量计算：
定义虚拟AUV速度量为：
Vv＝[uv vv wv rv]T＝J-1(ηv)(β1(φ)+β2(φ)) (10)
结合式(7)和式(9)，得到微分方程组：
根据式(11)和式(12)，经计算得到虚拟AUV位置量ηv和辅助控制变量φ，根据式(10)得到虚拟AUV速度量Vv，则可实现仅利用领航AUV位置量ηm得到虚拟AUV的位置量ηv和速度量Vv。
7.根据权利要求5所述的一种多自治水下机器人编队控制方法，其特征在于，所述步骤5包括以下步骤：
步骤5.1编队误差设计：
虚拟AUV在惯性坐标系下的运行轨迹为：ηv＝[xv yv zv ψv]T，速度信息：Vv＝[uv vv wvrv]T，(xv，yv，zv)为惯性坐标系下虚拟AUV的位置信息，ψv为虚拟AUV绕Z轴逆时针方向转过的角度值；
跟随AUV的实际航行状态定义为：ηf＝[xf yf zf ψf]T，其中，xf，yf，zf为惯性坐标系下跟随AUV的位置信息，ψf为跟随AUV绕Z轴逆时针方向转过的角度值；
为实现编队控制任务，使AUV按照期望队形进行航行，需要控制跟随AUV的速度和角速度量Vf＝[uf vf wf rf]T，其中，uf vf wf分别为跟随AUV线速度矢量在载体坐标系中的3个分量，rf分别为跟随AUV角速度矢量在载体坐标系Z轴中的分量，使其能够跟踪虚拟AUV的轨迹，最终使惯性坐标系下编队控制误差ee收敛于0；
其中，误差ee为：
步骤5.2、运动学编队控制率设计：
将步骤4得到的虚拟AUV位置信息及速度信息，作为跟随AUV的期望状态，为跟随AUV设计稳定的运动学编队控制律，使跟随AUV跟踪虚拟AUV的轨迹，进而与领航AUV保持期望距离和角度，实现编队任务，具体的：
由反步方法得到运动学编队控制律：
式中，k，kz，kψ为正常系数。
8.根据权利要求5所述的一种多自治水下机器人编队控制方法，其特征在于，所述步骤6包括以下步骤：
步骤6.1领航AUV合力分析：
根据障碍物的影响范围ρo和目标点和障碍物距离的关系引入
使AUV处于障碍物的影响范围之内，受到弹簧牵引力大于障碍物的弹性排斥力，从而保证AUV可以到达目标；
领航AUV的合力包含目标弹簧的牵引力和障碍物的排斥力；
目标弹簧牵引力Fatt(q)为：
其中，kg为目标虚拟弹簧的正比例因子，q为AUV的位置，qt为目标位置，qo为障碍物的位置，f(qo-qt)是障碍物距目标点的欧式距离；
障碍物的排斥力Freq(q)为：
其中，f(qo-qt)为AUV和障碍物之间的距离，l1是障碍物的最大撞击距离，k0为弹簧的弹性系数；
领航AUV的合力F表示为：
步骤6.2跟随AUV合力分析：
存储在领航AUV虚拟弹簧中的势能为：
其中，Eij(q)是AUV间的虚拟弹簧的势能，Et(q)是领航AUV和目标之间的虚拟弹簧的势能，Eo(q)是AUV和障碍物之间的虚拟弹簧的势能，kf为虚拟弹簧的阻尼系数，lrij为弹簧实际长度；
虚拟弹簧模型考虑到了跟随AUV的避障功能，当跟随AUV运动到障碍物影响范围内时，将受到公式(17)障碍物的斥力Frep，跟随者将在排斥力的驱使下避开障碍物向虚拟领航者位置移动，此时跟随AUV合力Fa表示为：
其中，Frij为虚拟弹簧引力；
步骤6.3局部极小点解决：
当AUV被困在复杂障碍物时，即陷入局部极小状态，AUV切换至避障行为，此时附加旋转力场被激活，fi r将找到一条新的路径逃离局部极小状态；
其中，
γ为常量；
旋转力场被激活后，AUV控制律的总矢量为：
此时，AUV处于躲避障碍物状态的控制律为
其中，θF和θl分别为领航机器人合力的方向角与当前运动的朝向角，
和
为增益系数，vlim和ωlim是领航AUV的速度和角速度上限。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211675922.3A CN116027796A (zh) | 2022-12-26 | 2022-12-26 | 一种多自治水下机器人编队控制系统及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211675922.3A CN116027796A (zh) | 2022-12-26 | 2022-12-26 | 一种多自治水下机器人编队控制系统及方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116027796A true CN116027796A (zh) | 2023-04-28 |
Family
ID=86078731
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211675922.3A Pending CN116027796A (zh) | 2022-12-26 | 2022-12-26 | 一种多自治水下机器人编队控制系统及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116027796A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117111613A (zh) * | 2023-09-05 | 2023-11-24 | 广东工业大学 | 一种水下机器人的集群协同控制方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105259901A (zh) * | 2015-11-18 | 2016-01-20 | 上海海事大学 | 一种自治水下机器人编队运行控制系统及其运行方法 |
| CN111813121A (zh) * | 2020-07-13 | 2020-10-23 | 陕西理工大学 | 基于距离-角度优先级的多移动机器人编队避障方法 |
| WO2020253316A1 (zh) * | 2019-06-18 | 2020-12-24 | 中国科学院上海微系统与信息技术研究所 | 移动机器人的领航跟随系统和领航跟随控制方法 |
| CN113190020A (zh) * | 2021-05-31 | 2021-07-30 | 山东建筑大学 | 一种移动机器人队列系统及路径规划、跟随方法 |
| CN114859945A (zh) * | 2022-05-26 | 2022-08-05 | 厦门大学 | 一种基于人工势场法的水下编队控制方法、系统及介质 |
-
2022
- 2022-12-26 CN CN202211675922.3A patent/CN116027796A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105259901A (zh) * | 2015-11-18 | 2016-01-20 | 上海海事大学 | 一种自治水下机器人编队运行控制系统及其运行方法 |
| WO2020253316A1 (zh) * | 2019-06-18 | 2020-12-24 | 中国科学院上海微系统与信息技术研究所 | 移动机器人的领航跟随系统和领航跟随控制方法 |
| CN111813121A (zh) * | 2020-07-13 | 2020-10-23 | 陕西理工大学 | 基于距离-角度优先级的多移动机器人编队避障方法 |
| CN113190020A (zh) * | 2021-05-31 | 2021-07-30 | 山东建筑大学 | 一种移动机器人队列系统及路径规划、跟随方法 |
| CN114859945A (zh) * | 2022-05-26 | 2022-08-05 | 厦门大学 | 一种基于人工势场法的水下编队控制方法、系统及介质 |
Non-Patent Citations (2)
| Title |
|---|
| 丁国华;朱大奇;: "多AUV主从式编队及避障控制方法", 高技术通讯, no. 05, pages 538 - 541 * |
| 陈奕梅等: "基于改进虚拟弹簧模型的多机器人编队控制", 《系统仿真学报》, pages 2 - 6 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117111613A (zh) * | 2023-09-05 | 2023-11-24 | 广东工业大学 | 一种水下机器人的集群协同控制方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109241552B (zh) | 一种基于多约束目标的水下机器人运动规划方法 | |
| CN107168312B (zh) | 一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法 | |
| CN108427414B (zh) | 一种自主水下航行器水平面自适应轨迹跟踪控制方法 | |
| CN109521797B (zh) | 无人船引导水下航行器群的优化割草型编队控制方法 | |
| CN109050835B (zh) | 全驱动自主水下机器人结构及回收三维路径跟踪方法 | |
| Cao et al. | Toward optimal rendezvous of multiple underwater gliders: 3D path planning with combined sawtooth and spiral motion | |
| CN107168309B (zh) | 一种基于行为的多水下机器人路径规划方法 | |
| CN109765929B (zh) | 一种基于改进rnn的uuv实时避障规划方法 | |
| CN109784201B (zh) | 基于四维风险评估的auv动态避障方法 | |
| CN109933074B (zh) | 一种有领导者的多无人船群集运动控制器结构的设计方法 | |
| CN114115262B (zh) | 基于方位角信息的多auv执行器饱和协同编队控制系统和方法 | |
| CN112068440B (zh) | 基于模型预测控制的auv回收对接动力定位控制方法 | |
| Wang et al. | A hybrid heading control scheme for a biomimetic underwater vehicle | |
| CN116027796A (zh) | 一种多自治水下机器人编队控制系统及方法 | |
| CN114967714A (zh) | 一种自主式水下机器人抗扰运动控制方法及系统 | |
| Zhang et al. | Multi-UUV path planning based on improved artificial potential field method | |
| CN113671964B (zh) | 一种无人艇高性能轨迹跟踪控制系统和方法 | |
| CN113342016B (zh) | 基于动态事件触发的自主式水下机器人协同控制方法和系统 | |
| Li et al. | Formation control of a group of AUVs using adaptive high order sliding mode controller | |
| CN113296505B (zh) | 一种基于速变los的无人船多模式路径跟踪控制方法 | |
| CN116339355B (zh) | 水下航行器及其编队跟踪控制方法和装置 | |
| CN115657689B (zh) | 一种基于轨迹预测的自主水下航行器目标跟踪控制方法 | |
| CN115657713B (zh) | 一种考虑发射平台沉浮和晃动条件下发射决策控制方法 | |
| de Cerqueira Gava et al. | AUV cruising auto pilot for a long straight confined underwater tunnel | |
| Fan et al. | Two-layer model predictive formation control of unmanned surface vehicle |
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 |