CN104076689B - 一种全驱动式自主水下航行器协同控制方法 - Google Patents

一种全驱动式自主水下航行器协同控制方法 Download PDF

Info

Publication number
CN104076689B
CN104076689B CN201410339282.8A CN201410339282A CN104076689B CN 104076689 B CN104076689 B CN 104076689B CN 201410339282 A CN201410339282 A CN 201410339282A CN 104076689 B CN104076689 B CN 104076689B
Authority
CN
China
Prior art keywords
faauv
information
control method
vector
speed
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.)
Expired - Fee Related
Application number
CN201410339282.8A
Other languages
English (en)
Other versions
CN104076689A (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.)
Oceanographic Instrumentation Research Institute Shandong Academy of Sciences
Original Assignee
Oceanographic Instrumentation Research Institute Shandong Academy of Sciences
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 Oceanographic Instrumentation Research Institute Shandong Academy of Sciences filed Critical Oceanographic Instrumentation Research Institute Shandong Academy of Sciences
Priority to CN201410339282.8A priority Critical patent/CN104076689B/zh
Publication of CN104076689A publication Critical patent/CN104076689A/zh
Application granted granted Critical
Publication of CN104076689B publication Critical patent/CN104076689B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种全驱动式自主水下航行器(FAAUV)协同控制方法，包括以下步骤：首先建立FAAUV的二阶离散运动模型和动力学模型；其次由于每个水下机器人导航定位设备的定位误差不同，导致其拥有的航路信息具有不一致性；将航路信息视为虚拟领航者信息，其由一系列连续的航路点构成，提供一种将有限时间控制方法和一致性结合起来的有限时间一致性控制器，对FAAUV的速度和位置进行有限时间内的协同控制；每个水下机器人利用测速仪和陀螺仪测定自身的航速和运动姿态，并定时广播其自身的虚拟领航者信息和自身的速度信息。本发明计算出多个FAAUV的不同轴的驱动控制力，实现多个水下航行器在有限的时间内达到预定的编队形状。

Description

一种全驱动式自主水下航行器协同控制方法
技术领域
本发明涉及一种控制方法，特别是涉及一种全驱动式自主水下航行器协同控制方法。
背景技术
多自主式水下航行器(FAAUVs)的协同控制对于海洋科学考察和海洋开发等具有重要意义。多FAAUVs协协同编队控制可以显著提高FAAUV的包括海洋采样、成像、监视和通信在内众多应用方面的能力。与陆地多机器人(或多智能体)协同控制相比，多FAAUVs协同控制特别困难。虽然近年来多FAAUVs协同控制问题受到了广泛的重视，但多FAAUVs协同控制研究不像陆地多机器人(或多智能体)协同控制那样成熟。当前关于自主水下航行器的编队控制的研究，多采用分散控制器的设计思路，而忽略了自主水下航行器间的耦合关系；或者将多FAAUVs建模为多智能体，而忽略了其姿态特性(横滚角、俯仰角和偏航角)。文献<Multi-AUV control and adaptive sampling in Monterey Bay>中Fiorelli等在Monterey海湾进行了多AUVs的协同控制和自适应采样研究的实验，其使用了基于电缆的模块化多AUVs的快速行驶及避障控制，受限于电缆的长度并非完全自主控制。文献<Highprecision formation control of mobile robots using virtual structure approach>中采用了集中式的虚拟结构协同控制方法，采用了虚拟结构编队框架实现了机器人的协同控制，但是虚拟结构为假想存在，现实中并不存在，限制了实际工程应用。Do在文献<Formation tracking control of unicycle-type mobile robots with limitedsensing ranges>中研究了通讯受限情况下陆地机器人的协同控制问题，但其并没有考虑水下洋流对系统的扰动影响。
当前的控制方法多是采用无限时间上的轨迹跟踪控制方法，不能保证轨迹在有限的时间内形成，而多机协调控制要求多个机器人能在有限的时间内形成预定的轨迹形状。
发明内容
本发明所要解决的技术问题是提供一种全驱动式自主水下航行器协同控制方法，其通过对FAAUV运动学和动力学的建模，实现多个FAAUV在有限的时间内形成预定的编队轨迹形状。
本发明是通过下述技术方案来解决上述技术问题的：一种全驱动式自主水下航行器协同控制方法，其特征在于，所述全驱动式自主水下航行器协同控制方法包括以下步骤：首先建立FAAUV的二阶离散运动模型和动力学模型；其次由于每个水下机器人导航定位设备的定位误差不同，导致其拥有的航路信息具有不一致性；将航路信息视为虚拟领航者信息，其由一系列连续的航路点构成，提供一种将有限时间控制方法和一致性结合起来的有限时间一致性控制器，对FAAUV的航速和运动姿态进行有限时间内的一致性协同控制；然后每个水下机器人利用测速仪和陀螺仪测定自身的航速和运动姿态，并且定时向外广播其自身的虚拟领航者信息和自身的航速信息，接收并获得的其他机器人的水声通信广播信息，得到其他机器人的虚拟领航者信息和航速信息，利用协同控制方法求得协同后的航速和运动姿态姿态；每个机器人都执行上述工作，于是最终形成一致的航速和运动姿态；最后得到一致性协同控制后的航速和运动姿态，并将航速和运动姿态代入到每个FAAUV的动力学方程中，计算出每个机器人的驱动力和力矩，根据推进器的布局，将其分配到对应轴的推进电机上，从而驱动相应的电动机并带动螺旋桨按照给定的驱动力和力矩转动。
优选地，所述建立FAAUV的二阶离散运动模型FAAUVi在惯性坐标系下的t时刻运动模型表示为下列二阶系统的形式：
其中xi(t)表示FAAUVi重心在惯性系下的位置和运动姿态，vi(t)表示FAAUVi在惯性坐标系下的航速矢量，ui(t)为需要设计的FAAUVi的一致性控制矢量，其中，i表示第i个FAAUV，i的数量根据需要协同编队的机器人的数量而定，对上式进行前向差分离散化得：
xi(k+1)＝ΔTvi(k)+xi(k)
vi(k+1)＝ΔTui(k)+vi(k)
其中k表示设备的采样时刻，k＝0，1，2…，ΔT表示采样周期。
优选地，所述虚拟领航者信息表示每个FAAUV存储在控制器中的期望领航者位置信息，每个FAAUV利用其自身的位置矢量与其到自身虚拟领航者的距离信息之差来进行一致性协同，可以实现所有FAAUV在惯性坐标系下的线速度、角速度和角位移矢量在有限时间内达到一致，并且使每个FAAUV所有用的虚拟领航者的位置矢量信息有限时间达到一致，所提出的有限时间一致性控制方法为：
其中Nj为与FAAUVi有通信联系的FAAUV的集合，hi为FAAUVi到虚拟领航者的距离矢量，表示k时刻FAAUVi所拥有的虚拟领航者的位置矢量信息，vi(k)表示k时刻FAAUVi在惯性坐标系下的航速矢量；0＜a，b≤1为与收敛性速度相关的系数；0＜χ＜1为与收敛性速度相关的系数；||表示矢量每个元素的绝对值，sig()为矢量函数。
优选地，所述动力学模型的建立如下：FAAUVi要实现六自由度运动，安装三对具有正反推力的推进器，沿着三个坐标轴的方向进行布局。沿着E-ξ、E-η和E-ζ轴方向上的合力分别为和作用于E-ξ、E-η和E-ζ轴上的合力矩分别为和分别表示如下：
其中为FAAUVi第ε个推进器和与之平行的坐标轴之间的垂直距离，FAAUVi的第ε个推进器产生的推力为ε＝1，2…6。
优选地，所述全驱动式自主水下航行器协同控制方法求出各个FAAUV的推进器的驱动合力和合力矩，将协同控制方法求得协同后的航速和运动姿态代入到每个FAAUV的动力学方程中，计算出每个机器人的驱动力和力矩，根据推进器的布局，将其分配到对应轴的推进电机上，从而驱动相应的电动机并带动螺旋桨按照给定的驱动力和力矩转动。
本发明的积极进步效果在于：本发明首先建立FAAUV的三维运动学和动力学模型，提供一种将有限时间控制方法和一致性结合起来的有限时间一致性控制器，对FAAUV的航速(线速度和角速度)和运动姿态(平移和角度)进行一致性协同控制。得到协同后的航速和运动姿态，带入每个FAAUV的动力学方程中，求解各个轴向的推进力。该协调控制方法能够控制多个FAAUV的每个轴向的推进力，实现多个水下航行器在有限的时间内达到预定的编队形状。本发明能够计算出多个FAAUV的不同轴的驱动控制力，实现多个水下航行器在有限的时间内达到预定的编队形状。
附图说明
图1为FAAUVi系统在惯性系下的坐标示意图；
图2为FAAUVi推力和推力矩示意图；
图3为基于虚拟领航者位置信息的七个FAAUV编队控制矢量关系示意图；
图4为七个FAAUV协同控制队形；
图5为FAAUV 1的推力矩和推力；
图6为七个FAAUV的x方向的位协同控制效果曲线图；
图7为七个FAAUV的y方向的位协同控制效果曲线图；
图8为七个FAAUV的z方向的位协同控制效果曲线图；
图9为七个FAAUV的α角度的位协同控制效果曲线图；
图10为七个FAAUV的β角度的位协同控制效果曲线图；
图11为七个FAAUV的γ角度的位协同控制效果曲线图；
图12为本发明全驱动式自主水下航行器协同控制方法的流程图。
具体实施方式
下面结合附图给出本发明较佳实施例，以详细说明本发明的技术方案。
本发明包括以下步骤：首先建立FAAUV的二阶离散运动模型和动力学模型；其次由于每个水下机器人导航定位设备的定位误差不同，导致其拥有的航路信息具有不一致性；将航路信息视为虚拟领航者信息，其由一系列连续的航路点构成，提供一种将有限时间控制方法和一致性结合起来的有限时间一致性控制器，对FAAUV的航速(线速度和角速度)和运动姿态(平移和角度)进行有限时间内的一致性协同控制；然后每个水下机器人利用测速仪和陀螺仪测定自身的航速和运动姿态，并且定时向外广播其自身的虚拟领航者信息和自身的航速信息，接收并获得的其他机器人的水声通信广播信息，得到其他机器人的虚拟领航者信息和航速信息，利用协同控制方法求得协同后的航速和运动姿态；每个机器人都执行上述工作，于是最终形成一致的航速和运动姿态；最后得到一致性协同控制后的航速和运动姿态，并将航速和运动姿态代入到每个FAAUV的动力学方程中，计算出每个机器人的驱动力和力矩，根据推进器的布局，将其分配到对应轴的推进电机上，从而驱动相应的电动机并带动螺旋桨按照给定的驱动力和力矩转动。
如图1至图12所示，本发明全驱动式自主水下航行器协同控制方法的过程如下：
首先进行FAAUV系统的初始化，初始化k＝0，利用定位传感器、加速度传感器和陀螺仪测定其自身当前的位置、速度和航行角；对于每个机器人首先建立两个直角坐标系：惯性坐标系O-xyz和FAAUV本体坐标系E-ξηζ。惯性坐标的原点O定义在海平面内的下水点，O-x和O-y轴置于水平面内，O-x与经线平行且指向北，O-y与纬线平行且指向东，O-z轴垂直于水平面且指向地心；三者成右手螺旋关系。本体坐标系的原点E定义在FAAUV的重心，E-ξ定义在纵向中剖面内且指向线速度方向，E-η与纵向中剖面垂直且指向右手边，E-ζ在纵向中剖面内且指向下；三者成右手螺旋关系。E-ξ、E-η和E-ζ是FAAUV的惯性主轴。
惯性坐标系到本体坐标系的转换矩阵如下式(1)：
其中如下式(2)和(3)：
其中，αi为FAAUVi的横滚角，βi为FAAUVi的俯仰角，γi为FAAUVi的偏航角。
单个FAAUV的二阶离散运动模型建模如下：建立每个机器人的运动状态矢量为xi(t)＝[xsi(t) ysi(t) zsi(t) γi(t) αi(t) βi(t)]T(上标T表示向量的转置)，表示t时刻FAAUVi重心在惯性系下的位置和姿态，表示t时刻FAAUVi在本体坐标系下的线速度矢量和角速度矢量，所以令为t时刻FAAUVi在惯性坐标系下的速度矢量，所以令为FAAUVi在惯性坐标系下的加速度矢量。所以AUVi在惯性坐标系下的运动模型表示为下列二阶系统的形式如下式(4)：
对上式进行前向差分离散化得如下式(5)：
xi(k+1)＝ΔTvi(k)+xi(k)
vi(k+1)＝ΔTui(k)+vi(k)…………………(5)
其中k表示设备的采样时刻，k＝0，1，2…，ΔT表示采样周期。
建立单个FAAUV在本体坐标系下的动力学模型，在FAAUVi本体坐标系下，建立其力和力矩矢量为如下式(6)：
其中为FAAUVi的浮心在本体坐标系下的坐标，即相对于重心的偏移量。通过对其所受的合力和力矩的分析，FAAUVi在本体坐标系下的动力学模型表示为下列形式：
对其离散化得下列形式：
其中，如下式(7)：
这里∑Fi(k)和∑Mi(k)、和和和和分别为k时刻FAAUVi受到的合力和合力矩、重力和重力矩、浮力和浮力矩、推力和推力矩以及水流阻力和阻力矩；mi为FAAUVi的质量，I6X6为单位矩阵。
FAAUVi受到地球引力作用，由此产生重力和重力矩，但由于本体坐标原点定义在重心，所以不产生重力矩。其在本体坐标系下的表达式为如下式(8)：
其中，Gi为FAAUVi所受的重力。
FAAUVi受到浮力作用，由此产生浮力和浮力矩，反映到本体坐标系下可以表示如下式(9)：
其中，Bi为FAAUVi受到的浮力。
FAAUVi所受的推力和推力矩与推进器的布局有关。要求FAAUVi实现6自由度运动，需要安装3对具有正反推力的推进器。沿着三个坐标轴的方向进行布局，如图2所示。其中推进器T1和T2位于E-ξη平面并且相对于ξ轴对称，其可以实现沿ξ轴的以及绕ζ轴的转动；推进器T3和T4位于E-ηζ平面并且相对于η轴对称，其可以实现沿η轴的平动以及绕ξ轴的转动；推进器T5和T6位于E-ξζ平面并且相对于ζ轴对称，其可以实现沿ζ轴的平动以及绕η轴的转动。
FAAUVi第ε个推进器的螺旋桨转速为螺旋桨直径为第ε个推进器的推力系数为水密度为ρi，则第ε个推进器产生的推力为如下式(10)：
k时刻沿着E-ξ、E-η和E-ζ轴方向上的合力分别为 和作用于E-ξ、E-η和E-ζ轴上的合力矩分别为 和分别表示如下式(11)：
其中，为FAAUVi第ε个推进器和与之平行的坐标轴之间的垂直距离。
FAAUVi受到水动力作用，由此产生的力和力矩要反映到本体坐标系中。水流在惯性坐标系下的速度为则水流速度在FAAUVi本体坐标系下的速度可以表示为：则水流相对于FAAUVi的速度可以表示为如下式(12)：
水流阻力与FAAUVi相对水流速度的平方成正比。沿E-ξ、E-η和E-ζ轴方向的水流阻力分别为和可以表示为如下式(13)：
其中，ρi为水密度，Cdi为轴向阻力系数，Sxi，Syi，Szi为FAAUV i分别垂直于E-ξ、E-η和E-ζ轴的横截面积。
水流产生的阻力矩和FAAUVi的角速度平方成正比。阻力矩系数Kxi，Kyi，Kzi由试验测得，可以求出流体阻力矩在E-ξ、E-η和E-ζ轴方向的分量分别为如下式(14)：
以上是对单个机器人系统的动力学建模过程，为实现协同控制需要得到其协同一致的速度和姿态。虚拟领航者信息为每个FAAUV存储在控制器中的航路点位置信息。本发明提出基于虚拟领航者位置信息的编队控制方法，每个FAAUV利用其自身的位置矢量与其到虚拟领航者的距离信息hi之差来进行一致性协同，就是使每个FAAUV所拥有的虚拟领航者的位置矢量信息(这里其中xi(1：3)表示FAAUVi的位置信息分量)达到一致：i≠j，从而所有FAAUV在空间位置上形成一定的编队形状。因为有限时间编队控制还要求所有FAAUV在惯性坐标系下的线速度、角速度和角位移矢量在有限时间内达到一致，并且使每个FAAUV所有用的虚拟领航者的位置矢量信息有限时间达到一致，这就要求存在一个时刻ts，使得k≥ts时，如下式(15)：
其中Nj为与FAAUVi有通信联系的FAAUV的集合，||||表示矢量的范数，hi为FAAUVi到虚拟领航者的距离矢量(以七个FAAUV的三角形编队队形控制为例)，其表达式为如下式(16)：
这里r为FAAUV到虚拟领航者的选定的单位距离，其根据每个机器人之间的距离进行设置。
对于具有形如式(5)的二阶系统，提出以下有限时间二阶一致性控制方法，能够实现在某一个确定的时间内使系统的运动轨迹与预定航路点信息重合，如下式(17)：
其中0＜a，b≤1为与收敛性速度相关的系数，a，b的值越小，其形成编队的时间越长但机器人的运动轨迹越平滑；0＜χ＜1为与收敛性速度相关的系数，其值越小，其形成编队的时间越长但机器人的运动轨迹越平滑；||表示矢量每个元素的绝对值，sgn()为矢量符号函数，
其表达式为如下式(18)：
sgn(σ)＝[sign(σ1) … sign(σi) … sign(σn)]T…………………(18)
这里σ＝[σ1 … σi … σn]∈Rn表示矢量，如下式(19)：
为表述方便，将符号函数写成以下形式，如下式(20)：sig(σ)ρ＝sgn(σ)|σ|ρ＝[sign(σ1)|σ1|ρ … sign(σi)|σi|ρ … sign(σn)|σn|ρ]T……(20)
其中0＜ρ＜1为与收敛性速度相关的系数，其值越小，其形成编队的时间越长但机器人的运动轨迹越平滑；所以控制方法(17)可以进一步表示为如下式(21)：
求出各个FAAUV的推进器的驱动力和力矩：利用控制方法(21)带入式(5)求出vi(k)，再带入动力学模型，可以求出合力和合力矩∑Fi(k)和∑Mi(k)，由于和和和均为已知或可以实验测定，所以利用式(7)求出编队情形下各个AUV的推进器的驱动力和其表达式为，如下式(22)：
再利用式(11)的6个方程，从而可以求出沿着E-ξ、E-η和E-ζ轴方向上的合力和和合力矩和根据推进器的布局，将推进器的推力和力矩分配到对应轴的推进电机上，从而驱动相应的电动机并带动螺旋桨按照给定的驱动力和力矩转动。该协调控制方法能够计算出多个FAAUV的不同轴的驱动控制力，实现多个水下航行器在有限的时间内达到预定的编队形状。
本发明的具体实例如下：
以七个FAAUV的三角形编队队形控制为例说明协同控制方法的有效性。所有FAAUV为同型机器人，FAAUVi质量mi＝16(kg)，转动惯量惯性积浮心在本体坐标系下的坐标重力加速度g＝9.8(N/kg)。初始时刻测定水流相对于FAAUVi的速度水密度ρi＝1000(kg/m3)，轴向阻力系数Cdi＝0.94，截面积Sxi＝0.16(m2)，Syi＝0.16(m2)，Szi＝0.16(m2)；阻力矩系数Kxi＝0.58，Kyi＝0.58，Kzi＝0.58。综合考虑轨迹跟踪的快速性和运动轨迹的平滑性，选取轨迹跟踪收敛性系数a＝1，b＝1，χ＝0.3。选择单位间距r＝12(m)，FAAUVi的通信半径为SetDis＝30(m)，采样间隔ΔT＝0.1(s)。图4为七个机器人协同控制效果图：初始时位置和姿态的杂乱无章，最终在有限的时间内实现了多个机器入的协同一致编队。图5为FAAUV1的推力和推力矩。图6到图11为七个机器人位置和姿态随时间变化曲线。
上述说明并非是对本发明的限制，本发明并不仅限于上述举例，本技术领域的普通技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (4)

1.一种全驱动式自主水下航行器协同控制方法，其特征在于，所述全驱动式自主水下航行器协同控制方法包括以下步骤：
首先建立FAAUV的二阶离散运动模型和动力学模型，FAAUV的二阶离散运动模型FAAUVi在惯性坐标系下的t时刻运动模型表示为二阶系统的形式；动力学模型FAAUV i要实现六自由度运动，安装三对具有正反推力的推进器，沿着三个坐标轴的方向进行布局；沿着E-ξ、E-η和E-ζ轴方向上的合力分别为和作用于E-ξ、E-η和E-ζ轴上的合力矩分别为和其中，
沿着E-ξ、E-η和E-ζ轴方向上的合力分别为和作用于E-ξ、E-η和E-ζ轴上的合力矩分别为和分别表示如下：
<mrow> <msubsup> <mi>F</mi> <mrow> <mi>x</mi> <mi>i</mi> </mrow> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>2</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow>
<mrow> <msubsup> <mi>F</mi> <mrow> <mi>y</mi> <mi>i</mi> </mrow> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>3</mn> </mrow> <mi>j</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>4</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow>
<mrow> <msubsup> <mi>F</mi> <mrow> <mi>z</mi> <mi>i</mi> </mrow> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>5</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>6</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow>
<mrow> <msubsup> <mi>M</mi> <mrow> <mi>x</mi> <mi>i</mi> </mrow> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>R</mi> <mrow> <mi>T</mi> <mn>3</mn> </mrow> <mi>i</mi> </msubsup> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>3</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>R</mi> <mrow> <mi>T</mi> <mn>4</mn> </mrow> <mi>i</mi> </msubsup> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>4</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow>
<mrow> <msubsup> <mi>M</mi> <mrow> <mi>y</mi> <mi>i</mi> </mrow> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>R</mi> <mrow> <mi>T</mi> <mn>5</mn> </mrow> <mi>i</mi> </msubsup> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>5</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>R</mi> <mrow> <mi>T</mi> <mn>6</mn> </mrow> <mi>i</mi> </msubsup> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>6</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow>
<mrow> <msubsup> <mi>M</mi> <mrow> <mi>z</mi> <mi>i</mi> </mrow> <mi>T</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>R</mi> <mrow> <mi>T</mi> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>R</mi> <mrow> <mi>T</mi> <mn>2</mn> </mrow> <mi>i</mi> </msubsup> <msubsup> <mi>F</mi> <mrow> <mi>T</mi> <mn>2</mn> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow>
其中为FAAUV i第ε个推进器和与之平行的坐标轴之间的垂直距离，FAAUV i的第ε个推进器产生的推力为
其次由于每个水下机器人导航定位设备的定位误差不同，导致其拥有的航路信息具有不一致性；将航路信息视为虚拟领航者信息，其由一系列连续的航路点构成，提供一种将有限时间控制方法和一致性结合起来的有限时间一致性控制器，虚拟领航者信息表示每个FAAUV存储在控制器中的期望领航者位置信息，每个FAAUV利用其自身的位置矢量与其到自身虚拟领航者的距离信息之差来进行一致性协同，可以实现所有FAAUV在惯性坐标系下的线速度、角速度和角位移矢量在有限时间内达到一致，并且使每个FAAUV所有用的虚拟领航者的位置矢量信息有限时间达到一致，所提出的有限时间一致性控制方法，对FAAUV的速度和位置进行有限时间内的一致性协同控制；
然后每个水下机器人利用测速仪和陀螺仪测定自身的航速和运动姿态，并且定时向外广播其自身的虚拟领航者信息和自身的速度信息，接收并获得的其他机器人的水声通信广播信息，得到其他机器人的虚拟领航者信息和速度信息，利用协同控制方法求得协同后的航速和运动姿态；每个机器人都执行上述工作，于是最终形成一致的航速和运动姿态；
最后得到一致性协同控制后的航速和运动姿态，并将速度和位置代入到每个FAAUV的动力学方程中，计算出每个机器人的驱动力和力矩，根据推进器的布局，将其分配到对应轴的推进电机上，从而驱动相应的电动机并带动螺旋桨按照给定的驱动力和力矩转动。
2.如权利要求1所述的全驱动式自主水下航行器协同控制方法，其特征在于，所述建立FAAUV的二阶离散运动模型FAAUV i在惯性坐标系下的t时刻运动模型表示为下列二阶系统的形式：
<mrow> <msub> <mover> <mi>x</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>v</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>u</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
其中xi(t)表示FAAUV i重心在惯性系下的位置和姿态，vi(t)表示FAAUV i在惯性坐标系下的速度矢量，ui(t)为需要设计的FAAUV i的一致性控制矢量，其中，i表示第i个FAAUV，i的数量根据需要协同编队的机器人的数量而定，对上式进行前向差分离散化得：
xi(k+1)＝ΔTvi(k)+xi(k)
vi(k+1)＝ΔTui(k)+vi(k)
其中k表示设备的采样时刻，k＝0，1，2…，ΔT表示采样周期。
3.如权利要求1所述的全驱动式自主水下航行器协同控制方法，其特征在于，所述虚拟领航者信息表示每个FAAUV存储在控制器中的期望领航者位置信息，每个FAAUV利用其自身的位置矢量与其到自身虚拟领航者的距离信息之差来进行一致性协同，可以实现所有FAAUV在惯性坐标系下的线速度、角速度和角位移矢量在有限时间内达到一致，并且使每个FAAUV所有用的虚拟领航者的位置矢量信息有限时间达到一致，所提出的有限时间一致性控制方法为：
<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>u</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <munder> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>&amp;Element;</mo> <msub> <mi>N</mi> <mi>j</mi> </msub> </mrow> </munder> <mo>(</mo> <mi>a</mi> <mo>(</mo> <mrow> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>x</mi> <mrow> <mi>v</mi> <mi>l</mi> </mrow> <mi>j</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>x</mi> <mrow> <mi>v</mi> <mi>l</mi> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <mo>|</mo> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>x</mi> <mrow> <mi>v</mi> <mi>l</mi> </mrow> <mi>j</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>x</mi> <mrow> <mi>v</mi> <mi>l</mi> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <msup> <mo>|</mo> <mi>&amp;chi;</mi> </msup> <mo>+</mo> <mi>s</mi> <mi>i</mi> <mi>g</mi> <msup> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>x</mi> <mrow> <mi>v</mi> <mi>l</mi> </mrow> <mi>j</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>x</mi> <mrow> <mi>v</mi> <mi>l</mi> </mrow> <mi>i</mi> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <mi>&amp;chi;</mi> </msup> </mrow> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mi>b</mi> <mrow> <mo>(</mo> <mo>(</mo> <mrow> <msub> <mi>v</mi> <mi>j</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>v</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> <mo>)</mo> <mo>|</mo> <msub> <mi>v</mi> <mi>j</mi> </msub> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>v</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mo>|</mo> <mi>&amp;chi;</mi> </msup> <mo>+</mo> <mi>b</mi> <mi>s</mi> <mi>i</mi> <mi>g</mi> <mo>(</mo> <mrow> <msub> <mi>v</mi> <mi>j</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>v</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msup> <mo>)</mo> <mi>&amp;chi;</mi> </msup> </mrow> <mo>)</mo> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced>
其中Nj为与FAAUV i有通信联系的FAAUVs的集合，hi为FAAUV i到虚拟领航者的距离矢量，表示k时刻FAAUV i所拥有的虚拟领航者的位置矢量信息，vi(k)表示k时刻FAAUV i在惯性坐标系下的速度矢量；0＜a，b≤1为与收敛性速度相关的系数；0＜χ＜1为与收敛性速度相关的系数；||表示矢量每个元素的绝对值，sig0为矢量函数。
4.如权利要求1所述的全驱动式自主水下航行器协同控制方法，其特征在于，所述全驱动式自主水下航行器协同控制方法求出各个FAAUV的推进器的驱动合力和合力矩，将协同控制方法求得协同后的航速和运动姿态代入到每个FAAUV的动力学方程中，计算出每个机器人的驱动力和力矩，根据推进器的布局，将其分配到对应轴的推进电机上，从而驱动相应的电动机并带动螺旋桨按照给定的驱动力和力矩转动。
CN201410339282.8A 2014-07-17 2014-07-17 一种全驱动式自主水下航行器协同控制方法 Expired - Fee Related CN104076689B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410339282.8A CN104076689B (zh) 2014-07-17 2014-07-17 一种全驱动式自主水下航行器协同控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410339282.8A CN104076689B (zh) 2014-07-17 2014-07-17 一种全驱动式自主水下航行器协同控制方法

Publications (2)

Publication Number Publication Date
CN104076689A CN104076689A (zh) 2014-10-01
CN104076689B true CN104076689B (zh) 2018-04-17

Family

ID=51598025

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410339282.8A Expired - Fee Related CN104076689B (zh) 2014-07-17 2014-07-17 一种全驱动式自主水下航行器协同控制方法

Country Status (1)

Country Link
CN (1) CN104076689B (zh)

Families Citing this family (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105425610B (zh) * 2015-11-09 2018-07-24 哈尔滨工程大学 一种作业型水下机器人液压推进器的推力控制仿真系统
CN107168329B (zh) * 2017-06-20 2018-03-27 南京长峰航天电子科技有限公司 基于跟随领航者法的航速航向协同控制编队航行控制方法
CN108459504B (zh) * 2018-03-08 2020-12-22 上海阜有海洋科技有限公司 多点系泊协同自适应迭代学习控制方法
CN108663938A (zh) * 2018-05-25 2018-10-16 哈尔滨工程大学 一种考虑通讯拓扑变换的uuv集群协调控制方法
CN108829126A (zh) * 2018-05-25 2018-11-16 哈尔滨工程大学 一种考虑通讯延时的auv集群协调控制方法
CN108663939A (zh) * 2018-06-12 2018-10-16 哈尔滨工程大学 考虑通讯丢包的uuv集群水平面路径跟踪控制方法
CN108829132A (zh) * 2018-06-12 2018-11-16 哈尔滨工程大学 一种考虑大时滞的多uuv空间机动控制方法
CN109144047B (zh) * 2018-06-12 2021-07-06 南京理工大学 一种基于多智能体系统的有限时间编队控制方法
CN109032138B (zh) * 2018-07-25 2021-10-12 南京理工大学 基于一致性算法的多机器人编队控制系统及方法
CN108983612A (zh) * 2018-08-08 2018-12-11 华南理工大学 一种具有预设性能和连接保持的水下机器人编队控制方法
CN108958294B (zh) * 2018-09-25 2020-11-27 北京航空航天大学 无人水下潜航器编队控制方法及装置
CN109240317B (zh) * 2018-11-21 2021-06-04 哈尔滨工程大学 考虑螺旋桨故障的海底地震检波飞行节点有限时间构型包含控制方法
CN109634307B (zh) * 2019-01-15 2021-08-03 大连海事大学 一种无人水下航行器复合航迹跟踪控制方法
CN111259546B (zh) * 2020-01-15 2022-05-13 中国科学技术大学 一种自驱动交通工具圆周编队控制方法、系统及存储介质
CN114942646B (zh) * 2022-07-27 2022-10-25 山东省科学院海洋仪器仪表研究所 异构无人系统三维空间编队控制方法
CN115576334B (zh) * 2022-09-07 2023-05-02 中国科学院声学研究所 一种欠驱动水下航行器编队控制方法及系统
CN117657402B (zh) * 2024-01-31 2024-04-19 青岛哈尔滨工程大学创新发展中心 一种水下巡航器以及控制方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080300742A1 (en) * 2007-05-30 2008-12-04 Oceaneering International, Inc. Hybrid remotely/autonomously operated underwater vehicle
US8548656B1 (en) * 2009-06-30 2013-10-01 Zytek Communications Corporation Underwater vehicles with improved efficiency over a range of velocities
CN102004447A (zh) * 2010-11-11 2011-04-06 西北工业大学 水下航行器组合导航与控制半实物仿真试验系统
CN102679982B (zh) * 2012-04-06 2014-09-10 西北工业大学 一种面向不确定任务时间的自主水下航行器航路规划方法
CN102914786B (zh) * 2012-09-25 2014-05-14 北京理工大学 水下自主航行器auv自主定位方法
CN103616821A (zh) * 2013-12-09 2014-03-05 天津工业大学 一种六自由度航行器的鲁棒控制器的设计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
网络环境下多自主水下航行器编队控制研究;袁健;《中国博士学位论文全文数据库 信息科技辑》;20120615;82-85、89-99、113-129 *

Also Published As

Publication number Publication date
CN104076689A (zh) 2014-10-01

Similar Documents

Publication Publication Date Title
CN104076689B (zh) 一种全驱动式自主水下航行器协同控制方法
CN103942383B (zh) 一种深海作业型水下机器人的动力学和运动学估计方法
Sarkar et al. Coordinated motion planning and control of autonomous underwater vehicle-manipulator systems subject to drag optimization
Lin et al. Development of a spherical underwater robot equipped with multiple vectored water-jet-based thrusters
Liu et al. Investigation of the vectored thruster AUVs based on 3SPS-S parallel manipulator
Carreras et al. Sparus II, design of a lightweight hovering AUV
CN106737659A (zh) 一种水下无人航行器和机械手系统的手艇协调控制方法
CN108663938A (zh) 一种考虑通讯拓扑变换的uuv集群协调控制方法
US11774987B2 (en) Control of vehicle movement by application of geometric algebra and state and error estimation
Pazmiño et al. Experiences and results from designing and developing a 6 DoF underwater parallel robot
Lin et al. Underwater experiments of a water-jet-based spherical underwater robot
KR101643193B1 (ko) 질량 이동을 이용한 수중 운동체
CN111857165A (zh) 一种水下航行器的轨迹跟踪控制方法
CN108363400B (zh) 一种基于虚拟锚泊的欠驱动auv三维控制区域镇定控制方法
Zavari et al. Early stage design of a spherical underwater robotic vehicle
Fernandez et al. Design, modeling and control of a spherical autonomous underwater vehicle for mine exploration
CN106840143B (zh) 一种判别水下机器人姿态稳定的方法
Barbera et al. Attitude control for a pectoral fin actuated bio-inspired robotic fish
Lyu et al. A Snake Eel Inspired Multi-joint Underwater Inspection Robot for Undersea Infrastructure Intelligent Maintenance
Liu et al. Dynamics modeling and typical motion performance analysis for a multi-joint autonomous underwater vehicle
Ye et al. Design and simulation of a bio-inspired rigid-soft hybrid robotic blimp
Lee et al. A New Complex Marine Unmanned Platform and Field Test
An et al. Underwater motion control of a bio-inspired father-son robot based on hydrodynamics analysis
Torres et al. A new AUV configuration with four tilting thrusters: Navigation and hover tasks
Hirano et al. Hardware-in-the-Loop Simulator with Low-Thrust Actuator for Free-Flying Robot's Omni-Directional Control

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20180417

Termination date: 20190717