CN115576334B - 一种欠驱动水下航行器编队控制方法及系统 - Google Patents
一种欠驱动水下航行器编队控制方法及系统 Download PDFInfo
- Publication number
- CN115576334B CN115576334B CN202211098949.0A CN202211098949A CN115576334B CN 115576334 B CN115576334 B CN 115576334B CN 202211098949 A CN202211098949 A CN 202211098949A CN 115576334 B CN115576334 B CN 115576334B
- Authority
- CN
- China
- Prior art keywords
- aircraft
- matrix
- formation
- jth
- underwater vehicle
- 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.)
- Active
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 66
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000004891 communication Methods 0.000 claims abstract description 38
- 238000013461 design Methods 0.000 claims abstract description 18
- 230000008569 process Effects 0.000 claims abstract description 7
- 239000011159 matrix material Substances 0.000 claims description 59
- 239000013598 vector Substances 0.000 claims description 24
- 230000005484 gravity Effects 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000013016 damping Methods 0.000 claims description 7
- 230000000694 effects Effects 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 description 10
- 238000012545 processing Methods 0.000 description 5
- 238000006467 substitution reaction Methods 0.000 description 5
- 230000006872 improvement Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 3
- 238000009795 derivation Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- KLDZYURQCUYZBL-UHFFFAOYSA-N 2-[3-[(2-hydroxyphenyl)methylideneamino]propyliminomethyl]phenol Chemical compound OC1=CC=CC=C1C=NCCCN=CC1=CC=CC=C1O KLDZYURQCUYZBL-UHFFFAOYSA-N 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 201000001098 delayed sleep phase syndrome Diseases 0.000 description 1
- 208000033921 delayed sleep phase type circadian rhythm sleep disease Diseases 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000006855 networking Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种欠驱动水下航行器编队控制方法及系统,所述方法包括:建立水下航行器模型;利用图论知识建立通信拓扑式,引入辅助变量构建编队控制器;计算虚拟控制律,并将结果用于控制器的设计中,计算出控制推力输入与理想姿态指令;计算输入力矩使水下航行器的姿态跟踪理想姿态指令,使得一组航行器可以实现编队航行。本发明的优势在于:利用单位四元数对姿态进行描述,提高运算效率并可以避免奇点;通过一个积分型辅助变量结合双曲正切饱和函数构建编队控制器,增加了编队集群过程中的平稳性。
Description
技术领域
本发明属于水下航行器领域,具体涉及一种欠驱动水下航行器编队控制方法及系统。
背景技术
水下航行器的水下作业往往以单个形式出现,然而随着任务复杂度的增加(如:水下观测、组网、管道检修、探测等),导致单个水下航行器难以完成复杂的水下任务,需要多个水下航行器以编队航行的形式相互协同配合来完成任务。同时,多个水下航行器之间相互合作也能够减少任务时间,提高工作效率和成功率。
水下航行器编队控制目标是实现多个航行器的编队航行,可以执行更高质量的任务,满足特定的需求。现有的研究没有将编队控制量进行限制,当编队成员过多以及各个成员之间的距离过大时,编队控制量非常大,会增加系统的负担,影响速度、姿态的控制品质。同时,对于姿态的描述通常利用欧拉角,计算效率较低。
发明内容
本发明的目的在于克服当编队成员过多以及各个成员之间的距离过大时,现有技术的编队控制计算量大,增加系统的负担,影响速度、姿态控制品质的缺陷。
为了实现上述目的,本发明提出了一种欠驱动水下航行器编队控制方法,所述方法包括:
步骤1:建立水下航行器模型;
步骤2:利用图论知识建立通信拓扑式,引入辅助变量构建编队控制器;
步骤3:计算虚拟控制律,并将结果用于控制器的设计中,计算出控制推力输入与理想姿态指令;
步骤4:计算输入力矩使水下航行器的姿态跟踪理想姿态指令,使得一组航行器实现编队航行。
作为上述方法的一种改进,所述步骤1具体为:
建立欠驱动水下航行器模型编队运动及动力学微分方程为:
其中,符号T为矩阵转置;定义北东地{E}=[e1,e2,e3]T为惯性坐标系,e1=[1,0,0]T,e2=[0,1,0]T,e3=[0,0,1]T为惯性坐标系{E}在三维方向x,y,z上的单位向量;第j个航行器的本体坐标系为{Bj}=[bj1,bj2,bj3]T,其中,bj1=[1,0,0]T,bj2=[0,1,0]T,bj3=[0,0,1]T为{Bj}在航行器三维方向u,v,w上的单位向量;
ηj1=[xj,yj,zj]T为第j个航行器浮心在{E}下的位置坐标;
其中vj1=[uj,vj,wj]T为第j个航行器浮心在{Bj}中的线速度大小,uj,vj,wj分别是第j个航行器浮心在{Bj}中在u、v、w三个方向的线速度大小;vj2=[pj,qj,rj]T为{Bj}相对于{E}的角速度大小在{Bj}中的投影,pj,qj,rj分别为{Bj}相对于{E}在u,v,w三个方向上的角速度大小在{Bj}中的投影;
单位四元数定义为Qj=[qj1,qj2,qj3,λj]T,用于描述{Bj}相对于{E}的姿态;qj1,qj2,qj3,λj四个变量,一起用于描述{Bj}相对于{E}的在u,v,w三个方向上的姿态;
R(Q)j为{Bj}相对于{E}的旋转矩阵:
其中,I3×3为三阶单位对角矩阵;qj=(qj1,qj2,qj3)T为取单位四元数Qj=[qj1,qj2,qj3,λj]T的前三行定义的列向量;
C(v)j定义为:
其中,C12(v1)j、C21(v1)j C22(v2)j为科氏力与向心力矩阵;
D(v)j定义为:
其中,D11(v1)j、D22(v2)j为流体水动力阻尼矩阵;
G(η)j为恢复力与力矩向量,定义为:
τj为第j个航行器的控制输入,定义为τj=(Nj,0,0,Γj1,Γj2,Γj3)T;其中Nj代表第j个航行器沿着{Bj}系中bj1方向的控制推力,Γj=(Γj1,Γj2,Γj3)T为第j个航行器在u、v、w三个方向上的控制力矩。
作为上述方法的一种改进,所述步骤2具体为:
利用航行器安装的水声通讯模块使编队中的各航行器之间都可以相互通信,利用图论方法对航行器之间的通信拓扑进行建模;所述通信拓扑为带权值的无向图,定义为通信拓扑的一条边,N为航行器个数;第j和k个航行器之间如果存在通信,有(j,k)∈ε;加权相邻矩阵描述了第j和k航行器之间的通信强度权值,定义为:
其中,kjj表示第j个航行器自己与自己的通信强度权值;
引入辅助变量φj,用于构建编队控制器,使各水下航行器实现相对位置保持固定;
作为上述方法的一种改进,所述步骤3具体为:
第j个航行器实际控制推力Nj计算公式为:
其中,设计的中间控制力Fj是推力Nj与理想姿态单位四元数Qjd合成的三维控制输入,按行展开定义为Fj=(Fj1,Fj2,Fj3)T;
理想姿态指令计算公式为:
其中,ωjd为理想坐标系中的航行器姿态指令角速度;S(qd)j为对于向量qjd的斜对称矩阵。
作为上述方法的一种改进,所述步骤4具体为:
计算输入控制力矩为:
其中,表示单位四元数描述的{Bj}系相对于理想坐标系{Dj}之间的姿态误差,为列向量的第一行,为列向量的第二行,为列向量的第三行;变量 表示{Bj}系相对于理想坐标系{Dj}之间的角速度误差;为大于零的控制增益;βj为设计虚拟控制律:
其中,为理想坐标系{Dj}到{Bj}系的旋转矩阵,R(Q)j为{Bj}相对于惯性坐标系{E}的旋转矩阵,R(Qd)j为{Dj}相对于惯性坐标系{E}的旋转矩阵;Cj22(v2)j为科氏力与向心力惯量矩阵;Dj22(v2)j为流体水动力阻尼惯量矩阵;
本发明还提供一种欠驱动水下航行器编队控制器,所述系统包括:
水下航行器模型模块,用于建立水下航行器模型;
编队控制器模块,用于利用图论知识建立通信拓扑式,引入辅助变量构建编队控制器;
控制推力输入模块,用于计算虚拟控制律,并将结果用于控制器的设计中,计算出控制推力输入与理想姿态指令;
输入控制力矩模块,用于计算输入控制力矩使水下航行器的姿态跟踪理想姿态指令,使得一组航行器实现编队航行。
本发明还提供一种计算机设备,包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述处理器执行所述计算机程序时实现如上述任一项所述的方法。
本发明还提供一种计算机可读存储介质,所述计算机可读存储介质存储有计算机程序,所述计算机程序当被处理器执行时使所述处理器执行如上述任一项所述的方法。
与现有技术相比,本发明的优势在于:
本发明利用单位四元数对姿态进行描述,提高运算效率并可以避免奇点;通过一个积分型辅助变量结合双曲正切饱和函数构建编队控制器,增加了编队集群过程中的平稳性。
附图说明
图1所示为欠驱动水下航行器编队控制方法流程图;
图2所示为n个水下航行器编队示意图;
图3所示为水下航行器(Autonomous Underwater Vehicle,AUV)编队过程仿真结果。
具体实施方式
下面结合附图对本发明的技术方案进行详细的说明。
本发明所研究的对象是一种6自由度欠驱动水下航行器的模型,利用水声通信技术,在各水下航行器进行信息交互的条件下,设计编队控制器,使得一组航行器可以实现编队航行,并跟踪指定的速度以及姿态信息。
如图1所示,本发明方法实施方法步骤如下:
1.建立水下航行器模型。
本步骤建立了单位四元数欠驱动水下航行器模型,相比于传统建模,具有运算效率高、避免奇异性的优点。以下是推导过程:
定义北东地{E}=[e1,e2,e3]T为惯性坐标系,e1=[1,0,0]T,e2=[0,1,0]T,e3=[0,0,1]T为惯性坐标系{E}在x,y,z方向上的单位向量;设编队中共有n个航行器,定义下标j,j=[1,2,···,n]对第j个航行器作为区分;定义第j个航行器的本体坐标系为{Bj}=[bj1,bj2,bj3]T,其中,bj1=[1,0,0]T,bj2=[0,1,0]T,bj3=[0,0,1]T为{Bj}在航行器u,v,w方向上的单位向量。{Bj}的原点与航行器的浮心CB重合,浮心在本体系中坐标为rjb=(0,0,0)T;为了产生恢复力矩,航行器的重心CM在bj3的正半轴,重心在本体系中的坐标为rjg=(0,0,zjg)T,zjg>0。
定义第j个水下航行器的位置姿态向量为为航行器浮心在{E}下的位置坐标。单位四元数可定义为Qj=[qj1,qj2,qj3,λj]T(qj1,qj2,qj3,λj这四个量,一起用于描述{Bj}相对于{E}的在u、v、w三个方向上的姿态),用于描述{Bj}相对于{E}的姿态,取单位四元数的前三行定义列向量qj=(qj1,qj2,qj3)T。其中vj1=[uj,vj,wj]T为浮心在{Bj}中的线速度大小(uj,vj,wj分别是浮心在{Bj}中在u、v、w三个方向的线速度大小),vj2=[pj,qj,rj]T为{Bj}相对于{E}的角速度大小在{Bj}中的投影(pj,qj,rj分别为{Bj}相对于{E}在u、v、w三个方向上的角速度大小在{Bj}中的投影)。
可知{Bj}相对于{E}的旋转矩阵为
其中I3×3为三阶单位对角矩阵,斜对称矩阵;qj=(qj1,qj2,qj3)T为取单位四元数Qj=[qj1,qj2,qj3,λj]T的前三行定义的列向量。
欠驱动水下航行器编队示意图如图2所示,运动学及动力学微分方程为:
C(v)j定义为:
其中,C12(v1)j、C21(v1)j C22(v2)j为科氏力与向心力矩阵。
D(v)j定义为:
其中,D11(v1)j、D22(v2)j为流体水动力阻尼矩阵。
G(η)i为恢复力与力矩向量,定义为:
水下航行器的控制输入定义为τj=(Nj,0,0,Γj1,Γj2,Γj3)T;其中Nj代表航行器沿着{Bj}系中bj1方向的控制推力,Γj=(Γj1,Γj2,Γj3)T为水下航行器在u、v、w三个方向上的控制力矩。
2.利用图论知识建立通信拓扑式(11),引入辅助变量构建编队控制器。
编队控制器:各航行器之间相互通信,可以进行位置信息交互,利用相对位置信息设计编队控制器,使多航行器能够保持预设的队形协同航行。
定义编队中任意两个航行器j、k之间的相对位置为ηjk=ηj-ηk,j,k=[1,2,···,n];定义航行器线速度跟踪指令为任意两个航行器j、k之间的相对位置指令为δjk=δj-δk;控制目标为:设计控制推力Nj以及控制力矩Γj,使得每个航行器的速度最终为vd1,即在惯性坐标系下的速度误差满足:
同时,各航行器之间的相对位置关系满足:
最终,编队中的航行器会以一个共同的速度vd1航行,同时任意两个航行器j,k=[1,2,···,n]之间会保持指定的相对位置δjk。
利用航行器安装的水声通讯模块使编队中的各航行器之间都可以相互通信,利用图论方法对航行器之间的通信拓扑进行建模。通信拓扑为带权值的无向图,定义为通信拓扑的一条边,第j和k个航行器之间如果存在通信,那么(j,k)∈ε;加权相邻矩阵描述了第j和k航行器之间的通信强度权值,定义为:
kjj表示第j个航行器自己与自己的通信强度权值;
引入辅助变量φj,用于构建编队控制器,使各水下航行器实现相对位置保持固定。
设计虚拟控制律为:
3.计算虚拟控制律,并将结果用于控制器的设计中,计算出控制推力输入与理想姿态指令。
对式(12)进行积分,计算φj的实时结果,并将结果用于控制器的设计中,计算出控制推力式(33)与理想姿态式(18)、(34)、(35)。
根据式(4)-(8),可将式(5)展开为:
可知式(13)与式(14)均为{Bj}系坐标下的航行器动力学方程,而速度指令vd1是相对于{E}给定的。为了方便控制器的设计,需将式(13)转换到{E}系中。
由式(3)可知:
代入式(13):
其中,变换后的矩阵定义为:
变换后的方程式(16)为{E}系中的位置坐标动力学方程,称为位置子系统;式(14)为{Bj}系中的姿态动力学方程,称为姿态子系统。
由航行器的控制输入τj=(Nj,0,0,Γj1,Γj2,Γj3)T可知,位置子系统在横向bj2与升沉bj3方向没有执行器产生控制量,是欠驱动系统;姿态子系统为全驱动系统。为了能够跟踪一个三维的理想速度vd1,理想的姿态信息需Qjd要作为额外的控制量,与控制推力合成三维控制输入。
S(qd)j为对于向量qjd的斜对称矩阵。
在姿态子系统控制律设计的过程中,为了解决欠驱动问题,首要任务是为每个航行器的位置子系统定义一个具有加速度量纲的中间控制量:
由上式可知,Fj是推力Nj与理想姿态Qjd合成的三维控制输入。首先对Fj(即)进行设计,得到一个确定的关于Fj的函数,之后利用已设计出的具有确定函数值的Fj可以求解出位置子系统所需的推力Nj以及理想姿态Qjd。
根据上述思路,将式(19)代入式(16)可得:
由式(9)所定义的速度误差易知:
将式(22)代入位置子系统式(20)可知:
其中:
定义Lyapunov函数:
定义第j和k个航行器之间的相对速度误差为v1jk=vj1-vk1,vk1为第k个航行器的线速度。对上式求导可得:
由于通信拓扑为无向图,因此满足:
即在无向图通信拓扑下,下标jk是可以交换的。
将位置子系统误差方程式(23)代入式(26)可得:
将式(28)展开得:
设计中间控制力为:
将式(30)代入式(29)可得:
将式(12)代入式(31)可得:
由设计的中间控制力式(30)以及虚拟控制律式(12)可确定式(19)等号右侧的函数,将所设计的中间控制力按行展开定义为Fj=(Fj1,Fj2,Fj3)T,可以解得实际控制推力为:
可以解出λjd为:
可以解出qjd为:
4.设计输入力矩式(40)使水下航行器的姿态跟踪理想姿态指令,使得一组航行器可以实现编队航行,并跟踪指定的速度以及姿态信息。
利用所利用中间控制量计算输入推力以及理想姿态,设计输入力矩,使得一组航行器可以实现编队航行,并跟踪指定的速度以及姿态信息。
在获得目标姿态Qjd与角速度ωjd后,设计控制力矩Γj使姿态子系统Qj跟踪Qjd。
将姿态误差方程式(17)代入姿态子系统微分方程式(14)可得
其中,为理想坐标系{Dj}到{Bj}系的旋转矩阵,R(Q)j为{Bj}相对于惯性坐标系{E}的旋转矩阵,R(Qd)j为{Dj}相对于惯性坐标系{E}的旋转矩阵;Cj22(v2)j为科氏力与向心力惯量矩阵;Dj22(v2)j为流体水动力阻尼惯量矩阵;
定义Lyapunov函数:
对式(38)求导可得:
设计控制力矩为:
设计虚拟控制律为:
将式(40)及(41)代入式(39)可得:
5.闭环系统稳定性分析。
证明:
根据式(25)及(38),定义闭环系统Lyapunov函数为:
对式(43)求导,并将位置子系统控制律式(30)、(12)以及姿态子系统控制律式(40)、(41)可得:
又因为
根据式(12)可知:
最终可以证明:当t→∞时,qj→qd,λj→λd,ηjk→δjk;即编队中的航行器均可按指定的速度vd1航行,任意第j个和k个航行器之间的相对位置可以保持指定的距离δjk,同时各个航行器的姿态子系统都可跟踪位置子系统所生成的姿态指令。
6.仿真验证。
令四个欠驱动水下航行器组成矩形编队,各航行器初始状态为:
理想相对坐标为:δ1=(1,1,0)T,δ2=(-1,1,0)T,δ3=(-1,-1,0)T,δ4=(1,-1,0)T;
仿真结果如图3所示。
本发明的优点有两个:
1、设计控制器,实现了多水下航行器的编队协同工作;
2、所提出的控制器的闭环稳定性得到了证明。
本发明还可提供的一种计算机设备,包括:至少一个处理器、存储器、至少一个网络接口和用户接口。该设备中的各个组件通过总线系统耦合在一起。可理解,总线系统用于实现这些组件之间的连接通信。总线系统除包括数据总线之外,还包括电源总线、控制总线和状态信号总线。
其中,用户接口可以包括显示器、键盘或者点击设备(例如,鼠标,轨迹球(trackball)、触感板或者触摸屏等。
可以理解,本申请公开实施例中的存储器可以是易失性存储器或非易失性存储器,或可包括易失性和非易失性存储器两者。其中,非易失性存储器可以是只读存储器(Read-Only Memory,ROM)、可编程只读存储器(Programmable ROM,PROM)、可擦除可编程只读存储器(Erasable PROM,EPROM)、电可擦除可编程只读存储器(Electrically EPROM,EEPROM)或闪存。易失性存储器可以是随机存取存储器(Random Access Memory,RAM),其用作外部高速缓存。通过示例性但不是限制性说明,许多形式的RAM可用,例如静态随机存取存储器(Static RAM,SRAM)、动态随机存取存储器(Dynamic RAM,DRAM)、同步动态随机存取存储器(Synchronous DRAM,SDRAM)、双倍数据速率同步动态随机存取存储器(Double DataRate SDRAM,DDRSDRAM)、增强型同步动态随机存取存储器(Enhanced SDRAM,ESDRAM)、同步连接动态随机存取存储器(Synchlink DRAM,SLDRAM)和直接内存总线随机存取存储器(Direct Rambus RAM,DRRAM)。本文描述的存储器旨在包括但不限于这些和任意其它适合类型的存储器。
在一些实施方式中,存储器存储了如下的元素,可执行模块或者数据结构,或者他们的子集,或者他们的扩展集:操作系统和应用程序。
其中,操作系统,包含各种系统程序,例如框架层、核心库层、驱动层等,用于实现各种基础业务以及处理基于硬件的任务。应用程序,包含各种应用程序,例如媒体播放器(Media Player)、浏览器(Browser)等,用于实现各种应用业务。实现本公开实施例方法的程序可以包含在应用程序中。
在本上述的实施例中,还可通过调用存储器存储的程序或指令,具体的,可以是应用程序中存储的程序或指令,处理器用于:
执行上述方法的步骤。
上述方法可以应用于处理器中,或者由处理器实现。处理器可能是一种集成电路芯片,具有信号的处理能力。在实现过程中,上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器可以是通用处理器、数字信号处理器(Digital Signal Processor,DSP)、专用集成电路(Application Specific IntegratedCircuit,ASIC)、现场可编程门阵列(Field Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行上述公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合上述公开的方法的步骤可以直接体现为硬件译码处理器执行完成,或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器,闪存、只读存储器,可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器,处理器读取存储器中的信息,结合其硬件完成上述方法的步骤。
可以理解的是,本发明描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现,处理单元可以实现在一个或多个专用集成电路(Application Specific Integrated Circuits,ASIC)、数字信号处理器(Digital SignalProcessing,DSP)、数字信号处理设备(DSP Device,DSPD)、可编程逻辑设备(ProgrammableLogic Device,PLD)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)、通用处理器、控制器、微控制器、微处理器、用于执行本申请所述功能的其它电子单元或其组合中。
对于软件实现,可通过执行本发明的功能模块(例如过程、函数等)来实现本发明技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。
本发明还可提供一种非易失性存储介质,用于存储计算机程序。当该计算机程序被处理器执行时可以实现上述方法实施例中的各个步骤。
最后所应说明的是,以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明,本领域的普通技术人员应当理解,对本发明的技术方案进行修改或者等同替换,都不脱离本发明技术方案的精神和范围,其均应涵盖在本发明的权利要求范围当中。
Claims (6)
1.一种欠驱动水下航行器编队控制方法,所述方法包括:
步骤1:建立水下航行器模型;
步骤2:利用图论知识建立通信拓扑式,引入辅助变量构建编队控制器;
步骤3:计算虚拟控制律,并将结果用于控制器的设计中,计算出控制推力输入与理想姿态指令;
步骤4:计算输入力矩使水下航行器的姿态跟踪理想姿态指令,使得一组航行器实现编队航行;
所述步骤1具体为:
建立欠驱动水下航行器模型编队运动及动力学微分方程为:
其中,符号T为矩阵转置;定义北东地{E}=[e1,e2,e3]T为惯性坐标系,e1=[1,0,0]T,e2=[0,1,0]T,e3=[0,0,1]T为惯性坐标系{E}在三维方向x,y,z上的单位向量;第j个航行器的本体坐标系为{Bj}=[bj1,bj2,bj3]T,其中,bj1=[1,0,0]T,bj2=[0,1,0]T,bj3=[0,0,1]T为{Bj}在航行器三维方向u,v,w上的单位向量;
ηj1=[xj,yj,zj]T为第j个航行器浮心在{E}下的位置坐标;
其中vj1=[uj,vj,wj]T为第j个航行器浮心在{Bj}中的线速度大小,uj,vj,wj分别是第j个航行器浮心在{Bj}中在u、v、w三个方向的线速度大小;vj2=[pj,qj,rj]T为{Bj}相对于{E}的角速度大小在{Bj}中的投影,pj,qj,rj分别为{Bj}相对于{E}在u,v,w三个方向上的角速度大小在{Bj}中的投影;
单位四元数定义为Qj=[qj1,qj2,qj3,λj]T,用于描述{Bj}相对于{E}的姿态;qj1,qj2,qj3,λj四个变量,一起用于描述{Bj}相对于{E}的在u,v,w三个方向上的姿态;
R(Q)j为{Bj}相对于{E}的旋转矩阵:
其中,I3×3为三阶单位对角矩阵;qj=(qj1,qj2,qj3)T为取单位四元数Qj=[qj1,qj2,qj3,λj]T的前三行定义的列向量;
C(v)j定义为:
其中,C12(v1)j、C21(v1)jC22(v2)j为科氏力与向心力矩阵;
D(v)j定义为:
其中,D11(v1)j、D22(v2)j为流体水动力阻尼矩阵;
G(η)j为恢复力与力矩向量,定义为:
τj为第j个航行器的控制输入,定义为τj=(Nj,0,0,Γj1,Γj2,Γj3)2;其中Nj代表第j个航行器沿着{Bj}系中bj1方向的控制推力,Γj=(Γj1,Γj2,Γj3)T为第j个航行器在u、v、w三个方向上的控制力矩;
所述步骤2具体为:
利用航行器安装的水声通讯模块使编队中的各航行器之间都可以相互通信,利用图论方法对航行器之间的通信拓扑进行建模;所述通信拓扑为带权值的无向图,定义为通信拓扑的一条边,N为航行器个数;第j和k个航行器之间如果存在通信,有;加权相邻矩阵描述了第j和k航行器之间的通信强度权值,定义为:
其中,kjj表示第j个航行器自己与自己的通信强度权值;
引入辅助变量φj,用于构建编队控制器,使各水下航行器实现相对位置保持固定;
3.权利要求2所述的欠驱动水下航行器编队控制方法,其特征在于,所述步骤4具体为:
计算输入控制力矩为:
其中,表示单位四元数描述的{Bj}系相对于理想坐标系{Dj}之间的姿态误差,为列向量的第一行,为列向量的第二行,为列向量的第三行;变量表示{Bj}系相对于理想坐标系{Dj}之间的角速度误差;为大于零的控制增益;βj为设计虚拟控制律:
其中,为理想坐标系{Dj}到{Bj}系的旋转矩阵,R(Q)j为{Bj}相对于惯性坐标系{E}的旋转矩阵,R(Qd)j为{Dj}相对于惯性坐标系{E}的旋转矩阵;Cj22(v2)j为科氏力与向心力惯量矩阵;Dj22(v2)j为流体水动力阻尼惯量矩阵;
4.一种欠驱动水下航行器编队控制器,所述控制器包括:
水下航行器模型模块,用于建立水下航行器模型;
编队控制器模块,用于利用图论知识建立通信拓扑式,引入辅助变量构建编队控制器;
控制推力输入模块,用于计算虚拟控制律,并将结果用于控制器的设计中,计算出控制推力输入与理想姿态指令;
输入控制力矩模块,用于计算输入控制力矩使水下航行器的姿态跟踪理想姿态指令,使得一组航行器实现编队航行;
所述水下航行器模型模块具体计算过程为:
建立欠驱动水下航行器模型编队运动及动力学微分方程为:
其中,符号T为矩阵转置;定义北东地{E}=[e1,e2,e3]T为惯性坐标系,e1=[1,0,0]T,e2=[0,1,0]T,e3=[0,0,1]T为惯性坐标系{E}在三维方向x,y,z上的单位向量;第j个航行器的本体坐标系为{Bj}=[bj1,bj2,bj3]T,其中,bj1=[1,0,0]T,bj2=[0,1,0]T,bj3=[0,0,1]T为{Bj}在航行器三维方向u,v,w上的单位向量;
ηj1=[xj,yj,zj]T为第j个航行器浮心在{E}下的位置坐标;
其中vj1=[uj,vj,wj]T为第j个航行器浮心在{Bj}中的线速度大小,uj,vj,wj分别是第j个航行器浮心在{Bj}中在u、v、w三个方向的线速度大小;vj2=[pj,qj,rj]T为{Bj}相对于{E}的角速度大小在{Bj}中的投影,pj,qj,rj分别为{Bj}相对于{E}在u,v,w三个方向上的角速度大小在{Bj}中的投影;
单位四元数定义为Qj=[qj1,qj2,qj3,λj]T,用于描述{Bj}相对于{E}的姿态;qj1,qj2,qj3,λj四个变量,一起用于描述{Bj}相对于{E}的在u,v,w三个方向上的姿态;
R(Q)j为{Bj}相对于{E}的旋转矩阵:
其中,I3×3为三阶单位对角矩阵;qj=(qj1,qj2,qj3)T为取单位四元数Qj=[qj1,qj2,qj3,λj]T的前三行定义的列向量;
C(v)j定义为:
其中,C12(v1)j、C21(v1)jC22(v2)j为科氏力与向心力矩阵;
D(v)j定义为:
其中,D11(v1)j、D22(v2)j为流体水动力阻尼矩阵;
G(η)j为恢复力与力矩向量,定义为:
τj为第j个航行器的控制输入,定义为τj=(Nj,0,0,Γj1,Γj2,Γj3)2;;其中Nj代表第j个航行器沿着{Bj}系中bj1方向的控制推力,Γj=(Γj1,Γj2,Γj3)T为第j个航行器在u、v、w三个方向上的控制力矩;
所述编队控制器模块具体计算过程为:
利用航行器安装的水声通讯模块使编队中的各航行器之间都可以相互通信,利用图论方法对航行器之间的通信拓扑进行建模;所述通信拓扑为带权值的无向图,定义为通信拓扑的一条边,N为航行器个数;第j和k个航行器之间如果存在通信,有;加权相邻矩阵描述了第j和k航行器之间的通信强度权值,定义为:
其中,kjj表示第j个航行器自己与自己的通信强度权值;
引入辅助变量φj,用于构建编队控制器,使各水下航行器实现相对位置保持固定;
5.一种计算机设备,包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序,其特征在于,所述处理器执行所述计算机程序时实现如权利要求1至3中任一项所述的方法。
6.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质存储有计算机程序,所述计算机程序当被处理器执行时使所述处理器执行如权利要求1至3任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211098949.0A CN115576334B (zh) | 2022-09-07 | 2022-09-07 | 一种欠驱动水下航行器编队控制方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211098949.0A CN115576334B (zh) | 2022-09-07 | 2022-09-07 | 一种欠驱动水下航行器编队控制方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115576334A CN115576334A (zh) | 2023-01-06 |
CN115576334B true CN115576334B (zh) | 2023-05-02 |
Family
ID=84580937
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211098949.0A Active CN115576334B (zh) | 2022-09-07 | 2022-09-07 | 一种欠驱动水下航行器编队控制方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115576334B (zh) |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104076689B (zh) * | 2014-07-17 | 2018-04-17 | 山东省科学院海洋仪器仪表研究所 | 一种全驱动式自主水下航行器协同控制方法 |
CN107894775B (zh) * | 2017-10-16 | 2020-08-28 | 中国科学院声学研究所 | 一种欠驱动无人水下航行器轨迹生成与控制方法 |
CN108594845A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种通信限制下基于预测控制的多auv编队方法 |
CN112650231B (zh) * | 2020-12-15 | 2022-07-15 | 哈尔滨工程大学 | 一种实现避碰和避障的欠驱船编队控制系统 |
CN112947462B (zh) * | 2021-03-02 | 2022-11-25 | 广东省智能机器人研究院 | 考虑时变漂角与姿态调节的无人艇群编队协同控制方法 |
CN114326756A (zh) * | 2021-09-08 | 2022-04-12 | 哈尔滨工程大学 | 一种考虑输入量化的自适应预设性能轨迹跟踪控制方法 |
CN114564015B (zh) * | 2022-02-24 | 2024-06-11 | 哈尔滨工程大学 | 一种拒止环境下的欠驱动无人艇分布式编队控制方法 |
-
2022
- 2022-09-07 CN CN202211098949.0A patent/CN115576334B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN115576334A (zh) | 2023-01-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112241125B (zh) | 一种基于微分平坦特性的无人机轨迹跟踪方法 | |
CN102073755B (zh) | 近空间高超声速飞行器运动控制仿真方法 | |
Imanberdiyev et al. | A fast learning control strategy for unmanned aerial manipulators | |
CN113859589B (zh) | 一种基于模型预测控制与滑模控制的航天器姿态控制方法 | |
Carrillo et al. | Deep-learning tracking for autonomous flying systems under adversarial inputs | |
CN115122325A (zh) | 一种具有视场约束的拟人化机械手鲁棒视觉伺服控制方法 | |
CN115639830B (zh) | 一种空地智能体协同编队控制系统及其编队控制方法 | |
CN107894775B (zh) | 一种欠驱动无人水下航行器轨迹生成与控制方法 | |
CN115524969A (zh) | 一种提高空间交会对接模型预测控制运算速度的方法 | |
Joukhadar et al. | Advanced UAVs nonlinear control systems and applications | |
CN116277036A (zh) | 一种柔性基、柔性臂空间机器人快速容错抑振控制方法 | |
Bao et al. | A review of underwater vehicle motion stability | |
Zhu et al. | Adaptive neural network fixed-time sliding mode control for trajectory tracking of underwater vehicle | |
CN117631693A (zh) | 一种欠驱动水下机器人轨迹跟踪的分布式预测控制方法 | |
CN115576334B (zh) | 一种欠驱动水下航行器编队控制方法及系统 | |
CN113741493B (zh) | 一种航天器安全接近控制方法 | |
CN116126003A (zh) | 基于Stewart平台的波浪补偿系统建模和位姿控制方法 | |
Liang et al. | Robust nonlinear path-tracking control of vector-propelled AUVs in complex sea conditions | |
Silva et al. | Models for simulation and control of underwater vehicles | |
Lande | Path planning for marine vehicles using bézier curves | |
Zhang et al. | Modeling and dynamic control of a class of semibiomimetic robotic fish | |
Shang et al. | Finite-time tracking for UUV with system uncertainty using fast adaptive nonsingular terminal high-order sliding mode control | |
Jadhav et al. | Mathematical modeling and control of Biomimetic Autonomous Underwater Vehicle (BAUV) based on flapping propulsion | |
CN117369481B (zh) | 一种状态受限下的无人潜航器避障跟踪控制方法 | |
Liu et al. | Research on the hydrodynamic calculation of variable structure underwater vehicle based on cfd |
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 |