CN115576334B - 一种欠驱动水下航行器编队控制方法及系统 - Google Patents

一种欠驱动水下航行器编队控制方法及系统 Download PDF

Info

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
Application number
CN202211098949.0A
Other languages
English (en)
Other versions
CN115576334A (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.)
Institute of Acoustics CAS
Original Assignee
Institute of Acoustics CAS
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 Institute of Acoustics CAS filed Critical Institute of Acoustics CAS
Priority to CN202211098949.0A priority Critical patent/CN115576334B/zh
Publication of CN115576334A publication Critical patent/CN115576334A/zh
Application granted granted Critical
Publication of CN115576334B publication Critical patent/CN115576334B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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具体为：
建立欠驱动水下航行器模型编队运动及动力学微分方程为：
Figure BDA0003836455910000025
Figure BDA0003836455910000021
其中，符号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}下的位置坐标；
Figure BDA0003836455910000026
其中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}的旋转矩阵：
Figure BDA0003836455910000022
其中，I3×3为三阶单位对角矩阵；qj＝(qj1,qj2,qj3)T为取单位四元数Qj＝[qj1,qj2,qj3,λj]T的前三行定义的列向量；
Figure BDA0003836455910000023
Figure BDA0003836455910000024
Figure BDA0003836455910000031
为惯性矩阵，满足下式：
Figure BDA0003836455910000032
其中，Mj11＝mjI3×3+Aj11，mj为航行器的质量，
Figure BDA0003836455910000033
为水下航行器附加质量矩阵；Mj22＝Ijo+Aj22，
Figure BDA0003836455910000034
为航行器的转动惯量，
Figure BDA0003836455910000035
为水下航行器附加惯量矩阵；
C(v)j定义为：
Figure BDA0003836455910000036
其中，C12(v1)j、C21(v1)j C22(v2)j为科氏力与向心力矩阵；
D(v)j定义为：
Figure BDA0003836455910000037
其中，D11(v1)j、D22(v2)j为流体水动力阻尼矩阵；
G(η)j为恢复力与力矩向量，定义为：
Figure BDA0003836455910000038
其中，Wj为第j个航行器受到的重力，Bj为第j个航行器所产生的浮力；
Figure BDA0003836455910000039
为重心相对浮心的位置坐标；rjg为第j个航行器的重心坐标，rjb为第j个航行器的浮心坐标；
Figure BDA00038364559100000310
为对于向量
Figure BDA00038364559100000311
的斜对称矩阵；
τj为第j个航行器的控制输入，定义为τj＝(Nj，0，0，Γj1，Γj2，Γj3)T；其中Nj代表第j个航行器沿着{Bj}系中bj1方向的控制推力，Γj＝(Γj1，Γj2，Γj3)T为第j个航行器在u、v、w三个方向上的控制力矩。
作为上述方法的一种改进，所述步骤2具体为：
利用航行器安装的水声通讯模块使编队中的各航行器之间都可以相互通信，利用图论方法对航行器之间的通信拓扑进行建模；所述通信拓扑为带权值的无向图，定义
Figure BDA0003836455910000041
为通信拓扑的一条边，N为航行器个数；第j和k个航行器之间如果存在通信，有(j，k)∈ε；加权相邻矩阵
Figure BDA0003836455910000042
描述了第j和k航行器之间的通信强度权值，定义为：
Figure BDA0003836455910000043
其中，kjj表示第j个航行器自己与自己的通信强度权值；
引入辅助变量φj，用于构建编队控制器，使各水下航行器实现相对位置保持固定；
辅助变量φj的一次导数
Figure BDA0003836455910000044
为虚拟控制律：
Figure BDA0003836455910000045
其中，设计参数
Figure BDA0003836455910000046
为常量；ηjk为航行器j、k之间的相对位置；δjk为航行器j、k之间的相对位置指令。
作为上述方法的一种改进，所述步骤3具体为：
第j个航行器实际控制推力Nj计算公式为：
Figure BDA0003836455910000047
其中，设计的中间控制力Fj是推力Nj与理想姿态单位四元数Qjd合成的三维控制输入，按行展开定义为Fj＝(Fj1,Fj2,Fj3)T；
Figure BDA0003836455910000048
其中，
Figure BDA0003836455910000049
表示在惯性坐标系下的第j个航行器的速度误差，vd1为航行器编队航行时的速度指令；其余变量定义如下：
Figure BDA00038364559100000410
Figure BDA00038364559100000411
Figure BDA00038364559100000412
Figure BDA0003836455910000051
Figure BDA0003836455910000052
对第j个航行器，定义理想姿态单位四元数
Figure BDA0003836455910000053
为所生成的理想姿态信息，列向量
Figure BDA0003836455910000054
理想姿态指令计算公式为：
Figure BDA0003836455910000055
Figure BDA0003836455910000056
Figure BDA0003836455910000057
其中，ωjd为理想坐标系中的航行器姿态指令角速度；S(qd)j为对于向量qjd的斜对称矩阵。
作为上述方法的一种改进，所述步骤4具体为：
计算输入控制力矩为：
Figure BDA0003836455910000058
其中，
Figure BDA0003836455910000059
表示单位四元数描述的{Bj}系相对于理想坐标系{Dj}之间的姿态误差，
Figure BDA00038364559100000510
为列向量
Figure BDA00038364559100000511
的第一行，
Figure BDA00038364559100000512
为列向量
Figure BDA00038364559100000513
的第二行，
Figure BDA00038364559100000514
为列向量
Figure BDA00038364559100000515
的第三行；变量
Figure BDA00038364559100000516
Figure BDA00038364559100000517
表示{Bj}系相对于理想坐标系{Dj}之间的角速度误差；
Figure BDA00038364559100000518
为大于零的控制增益；βj为设计虚拟控制律：
Figure BDA00038364559100000519
其中，
Figure BDA00038364559100000520
为大于零的控制增益；
Figure BDA00038364559100000521
其中，
Figure BDA0003836455910000061
为理想坐标系{Dj}到{Bj}系的旋转矩阵，R(Q)j为{Bj}相对于惯性坐标系{E}的旋转矩阵，R(Qd)j为{Dj}相对于惯性坐标系{E}的旋转矩阵；Cj22(v2)j为科氏力与向心力惯量矩阵；Dj22(v2)j为流体水动力阻尼惯量矩阵；
Figure BDA0003836455910000062
为系统实际姿态Qj与理想姿态Qjd之间的误差对位置子系统所造成的影响：
Figure BDA0003836455910000063
本发明还提供一种欠驱动水下航行器编队控制器，所述系统包括：
水下航行器模型模块，用于建立水下航行器模型；
编队控制器模块，用于利用图论知识建立通信拓扑式，引入辅助变量构建编队控制器；
控制推力输入模块，用于计算虚拟控制律，并将结果用于控制器的设计中，计算出控制推力输入与理想姿态指令；
输入控制力矩模块，用于计算输入控制力矩使水下航行器的姿态跟踪理想姿态指令，使得一组航行器实现编队航行。
本发明还提供一种计算机设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述任一项所述的方法。
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行如上述任一项所述的方法。
与现有技术相比，本发明的优势在于：
本发明利用单位四元数对姿态进行描述，提高运算效率并可以避免奇点；通过一个积分型辅助变量结合双曲正切饱和函数构建编队控制器，增加了编队集群过程中的平稳性。
附图说明
图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个水下航行器的位置姿态向量为
Figure BDA0003836455910000071
为航行器浮心在{E}下的位置坐标。单位四元数可定义为Qj＝[qj1,qj2,qj3,λj]T(qj1,qj2,qj3，λj这四个量，一起用于描述{Bj}相对于{E}的在u、v、w三个方向上的姿态)，用于描述{Bj}相对于{E}的姿态，取单位四元数的前三行定义列向量qj＝(qj1,qj2,qj3)T。
Figure BDA0003836455910000072
其中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}的旋转矩阵为
Figure BDA0003836455910000081
其中I3×3为三阶单位对角矩阵，斜对称矩阵；qj＝(qj1,qj2,qj3)T为取单位四元数Qj＝[qj1,qj2,qj3,λj]T的前三行定义的列向量。
Figure BDA0003836455910000082
欠驱动水下航行器编队示意图如图2所示，运动学及动力学微分方程为：
Figure BDA0003836455910000083
Figure BDA0003836455910000084
其中，
Figure BDA0003836455910000085
为惯性矩阵，可分块为：
Figure BDA0003836455910000086
其中，Mj11＝mjI3×3+Aj11，mj为航行器的质量，
Figure BDA0003836455910000087
为水下航行器附加质量矩阵；Mj22＝Ijo+Aj22，
Figure BDA0003836455910000088
为航行器的转动惯量，
Figure BDA0003836455910000089
为水下航行器附加惯量矩阵。
C(v)j定义为：
Figure BDA00038364559100000810
其中，C12(v1)j、C21(v1)j C22(v2)j为科氏力与向心力矩阵。
D(v)j定义为：
Figure BDA00038364559100000811
其中，D11(v1)j、D22(v2)j为流体水动力阻尼矩阵。
G(η)i为恢复力与力矩向量，定义为：
Figure BDA0003836455910000091
其中，W为航行器受到的重力，B为航行器所产生的浮力；
Figure BDA0003836455910000092
为重心相对浮心的位置坐标。
水下航行器的控制输入定义为τ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]；定义航行器线速度跟踪指令为
Figure BDA0003836455910000093
任意两个航行器j、k之间的相对位置指令为
Figure BDA0003836455910000094
δjk＝δj-δk；控制目标为：设计控制推力Nj以及控制力矩Γj，使得每个航行器的速度最终为vd1，即在惯性坐标系下的速度误差满足：
Figure BDA0003836455910000095
同时，各航行器之间的相对位置关系满足：
Figure BDA0003836455910000096
最终，编队中的航行器会以一个共同的速度vd1航行，同时任意两个航行器j,k＝[1,2,···,n]之间会保持指定的相对位置δjk。
利用航行器安装的水声通讯模块使编队中的各航行器之间都可以相互通信，利用图论方法对航行器之间的通信拓扑进行建模。通信拓扑为带权值的无向图，定义
Figure BDA0003836455910000097
为通信拓扑的一条边，第j和k个航行器之间如果存在通信，那么(j，k)∈ε；加权相邻矩阵
Figure BDA0003836455910000098
描述了第j和k航行器之间的通信强度权值，定义为：
Figure BDA0003836455910000101
kjj表示第j个航行器自己与自己的通信强度权值；
引入辅助变量φj，用于构建编队控制器，使各水下航行器实现相对位置保持固定。
设计虚拟控制律为：
Figure BDA0003836455910000102
其中，设计参数
Figure BDA0003836455910000103
为常量，j＝[1,2,···,n]。
3.计算虚拟控制律，并将结果用于控制器的设计中，计算出控制推力输入与理想姿态指令。
对式(12)进行积分，计算φj的实时结果，并将结果用于控制器的设计中，计算出控制推力式(33)与理想姿态式(18)、(34)、(35)。
根据式(4)-(8)，可将式(5)展开为：
Figure BDA0003836455910000104
Figure BDA0003836455910000105
可知式(13)与式(14)均为{Bj}系坐标下的航行器动力学方程，而速度指令vd1是相对于{E}给定的。为了方便控制器的设计，需将式(13)转换到{E}系中。
由式(3)可知：
Figure BDA0003836455910000106
代入式(13)：
Figure BDA0003836455910000107
其中，变换后的矩阵定义为：
Figure BDA0003836455910000111
Figure BDA0003836455910000112
Figure BDA0003836455910000113
Figure BDA0003836455910000114
变换后的方程式(16)为{E}系中的位置坐标动力学方程，称为位置子系统；式(14)为{Bj}系中的姿态动力学方程，称为姿态子系统。
由航行器的控制输入τj＝(Nj，0，0，Γj1，Γj2，Γj3)T可知，位置子系统在横向bj2与升沉bj3方向没有执行器产生控制量，是欠驱动系统；姿态子系统为全驱动系统。为了能够跟踪一个三维的理想速度vd1，理想的姿态信息需Qjd要作为额外的控制量，与控制推力合成三维控制输入。
对第j个航行器，定义单位四元数
Figure BDA0003836455910000115
为所生成的理想姿态信息，列向量
Figure BDA0003836455910000116
利用单位四元数的运算关系，可得{Bj}系与理想坐标系{Dj}之间的姿态误差以及角速度误差为：
Figure BDA0003836455910000117
其中
Figure BDA0003836455910000118
为{Dj}系到{Bj}系的旋转矩阵，其导数关系满足
Figure BDA0003836455910000119
为关于Qjd的旋转矩阵，
Figure BDA00038364559100001110
为对于向量
Figure BDA00038364559100001111
的斜对称矩阵；ωjd为理想坐标系中的航行器姿态指令角速度，与Qjd之间满足：
Figure BDA00038364559100001112
S(qd)j为对于向量qjd的斜对称矩阵。
在姿态子系统控制律设计的过程中，为了解决欠驱动问题，首要任务是为每个航行器的位置子系统定义一个具有加速度量纲的中间控制量：
Figure BDA0003836455910000121
由上式可知，Fj是推力Nj与理想姿态Qjd合成的三维控制输入。首先对Fj(即
Figure BDA0003836455910000122
)进行设计，得到一个确定的关于Fj的函数，之后利用已设计出的具有确定函数值的Fj可以求解出位置子系统所需的推力Nj以及理想姿态Qjd。
根据上述思路，将式(19)代入式(16)可得：
Figure BDA0003836455910000123
其中
Figure BDA0003836455910000124
为系统实际姿态Qj与理想姿态Qjd之间的误差对位置子系统所造成的影响，易知：
Figure BDA0003836455910000125
由式(9)所定义的速度误差易知：
Figure BDA0003836455910000126
将式(22)代入位置子系统式(20)可知：
Figure BDA0003836455910000127
其中：
Figure BDA0003836455910000128
定义Lyapunov函数：
Figure BDA0003836455910000129
其中，
Figure BDA0003836455910000131
j＝[1,2,···,n]为大于零的增益系数。
定义第j和k个航行器之间的相对速度误差为v1jk＝vj1-vk1，vk1为第k个航行器的线速度。对上式求导可得：
Figure BDA0003836455910000132
由于通信拓扑为无向图，因此满足：
Figure BDA0003836455910000133
即在无向图通信拓扑下，下标jk是可以交换的。
从而可得
Figure BDA0003836455910000134
将位置子系统误差方程式(23)代入式(26)可得：
Figure BDA0003836455910000135
将式(28)展开得：
Figure BDA0003836455910000136
设计中间控制力为：
Figure BDA0003836455910000137
其中，
Figure BDA0003836455910000138
表示在惯性坐标系下的第j个航行器的速度误差，vd1为航行器编队航行时的速度指令；控制增益
Figure BDA0003836455910000139
j＝[1,2,···,n]。
将式(30)代入式(29)可得：
Figure BDA0003836455910000141
将式(12)代入式(31)可得：
Figure BDA0003836455910000142
由设计的中间控制力式(30)以及虚拟控制律式(12)可确定式(19)等号右侧的函数，将所设计的中间控制力按行展开定义为Fj＝(Fj1,Fj2,Fj3)T，可以解得实际控制推力为：
Figure BDA0003836455910000143
可以解出λjd为：
Figure BDA0003836455910000144
可以解出qjd为：
Figure BDA0003836455910000145
4.设计输入力矩式(40)使水下航行器的姿态跟踪理想姿态指令，使得一组航行器可以实现编队航行，并跟踪指定的速度以及姿态信息。
利用所利用中间控制量计算输入推力以及理想姿态，设计输入力矩，使得一组航行器可以实现编队航行，并跟踪指定的速度以及姿态信息。
在获得目标姿态Qjd与角速度ωjd后，设计控制力矩Γj使姿态子系统Qj跟踪Qjd。
将姿态误差方程式(17)代入姿态子系统微分方程式(14)可得
Figure BDA0003836455910000146
其中
Figure BDA0003836455910000147
为变量
Figure BDA0003836455910000148
对时间的一阶导数，
Figure BDA0003836455910000149
其中，
Figure BDA0003836455910000151
为理想坐标系{Dj}到{Bj}系的旋转矩阵，R(Q)j为{Bj}相对于惯性坐标系{E}的旋转矩阵，R(Qd)j为{Dj}相对于惯性坐标系{E}的旋转矩阵；Cj22(v2)j为科氏力与向心力惯量矩阵；Dj22(v2)j为流体水动力阻尼惯量矩阵；
定义Lyapunov函数：
Figure BDA0003836455910000152
其中，
Figure BDA0003836455910000153
根据四元数运算关系易得
Figure BDA0003836455910000154
Figure BDA0003836455910000155
变量
Figure BDA0003836455910000156
βj为待设计的虚拟控制量。
对式(38)求导可得：
Figure BDA0003836455910000157
设计控制力矩为：
Figure BDA0003836455910000158
其中，
Figure BDA0003836455910000159
表示单位四元数描述的{Bj}系相对于理想坐标系{Dj}之间的姿态误差，
Figure BDA00038364559100001510
为列向量
Figure BDA00038364559100001511
的第一行，
Figure BDA00038364559100001512
为列向量
Figure BDA00038364559100001513
的第二行，
Figure BDA00038364559100001514
为列向量
Figure BDA00038364559100001515
的第三行；变量
Figure BDA00038364559100001516
Figure BDA00038364559100001517
表示{Bj}系相对于理想坐标系{Dj}之间的角速度误差；
设计虚拟控制律为：
Figure BDA00038364559100001518
其中，
Figure BDA00038364559100001519
为大于零的控制增益。
将式(40)及(41)代入式(39)可得：
Figure BDA0003836455910000161
5.闭环系统稳定性分析。
定理1.由式(3)与(4)所描述的欠驱动水下航行器，利用所设计的控制推力式(33)以及控制力矩式(40)，当t→∞时，
Figure BDA0003836455910000162
qj→qd，λj→λd，
Figure BDA0003836455910000163
ηjk→δjk。
证明：
根据式(25)及(38)，定义闭环系统Lyapunov函数为：
Figure BDA0003836455910000164
对式(43)求导，并将位置子系统控制律式(30)、(12)以及姿态子系统控制律式(40)、(41)可得：
Figure BDA0003836455910000165
可知
Figure BDA0003836455910000166
φj，
Figure BDA0003836455910000167
sj是有界的。
又因为
Figure BDA0003836455910000168
Figure BDA0003836455910000169
因此
Figure BDA0003836455910000171
有界，可知
Figure BDA0003836455910000172
一致连续。根据Barbalat引理可知，
Figure BDA0003836455910000173
因此
Figure BDA0003836455910000174
φj，
Figure BDA0003836455910000175
sj→0，从而可知
Figure BDA0003836455910000176
qj→qd，λj→λd，
Figure BDA0003836455910000177
根据式(12)可知：
Figure BDA0003836455910000178
因此
Figure BDA0003836455910000179
有界，可知
Figure BDA00038364559100001710
一致连续。根据Barbalat引理可知：
Figure BDA00038364559100001711
又因为已证明
Figure BDA00038364559100001712
φj→0，因此可知ηjk-δjk→0，ηjk→δjk。
最终可以证明：当t→∞时，
Figure BDA00038364559100001713
qj→qd，λj→λd，
Figure BDA00038364559100001714
ηjk→δjk；即编队中的航行器均可按指定的速度vd1航行，任意第j个和k个航行器之间的相对位置可以保持指定的距离δjk，同时各个航行器的姿态子系统都可跟踪位置子系统所生成的姿态指令。
6.仿真验证。
令四个欠驱动水下航行器组成矩形编队，各航行器初始状态为：
Figure BDA00038364559100001715
Figure BDA00038364559100001716
Figure BDA00038364559100001717
Figure BDA00038364559100001718
参考速度为：
Figure BDA00038364559100001719
控制增益为：kjk＝5，
Figure BDA00038364559100001720
理想相对坐标为：δ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的前三行定义的列向量；
为惯性矩阵，满足下式：
其中，Mj11＝mjI3×3+Aj11，mj为航行器的质量，为水下航行器附加质量矩阵；Mj22＝Ijo+Aj22，为航行器的转动惯量，为水下航行器附加惯量矩阵；
C(v)j定义为：
其中，C12(v1)j、C21(v1)jC22(v2)j为科氏力与向心力矩阵；
D(v)j定义为：
其中，D11(v1)j、D22(v2)j为流体水动力阻尼矩阵；
G(η)j为恢复力与力矩向量，定义为：
其中，Wj为第j个航行器受到的重力，Bj为第j个航行器所产生的浮力；为重心相对浮心的位置坐标；rjg为第j个航行器的重心坐标，rjb为第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，用于构建编队控制器，使各水下航行器实现相对位置保持固定；
辅助变量φj的一次导数为虚拟控制律：
其中，设计参数为常量；ηjk为航行器j、k之间的相对位置；δjk为航行器j、k之间的相对位置指令。
2.权利要求1所述的欠驱动水下航行器编队控制方法，其特征在于，所述步骤3具体为：
第j个航行器实际控制推力Nj计算公式为：
其中，设计的中间控制力Fj是推力Nj与理想姿态单位四元数Qjd合成的三维控制输入，按行展开定义为Fj＝(Fj1,Fj2,Fj3)T；
其中，表示在惯性坐标系下的第j个航行器的速度误差，vd1为航行器编队航行时的速度指令；其余变量定义如下：
对第j个航行器，定义理想姿态单位四元数为所生成的理想姿态信息，列向量
理想姿态指令计算公式为：
其中，ωjd为理想坐标系中的航行器姿态指令角速度；S(qd)j为对于向量qjd的斜对称矩阵。
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为流体水动力阻尼惯量矩阵；
为系统实际姿态Qj与理想姿态Qjd之间的误差对位置子系统所造成的影响：
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的前三行定义的列向量；
为惯性矩阵，满足下式：
其中，Mj11＝mjI3×3+Aj11，mj为航行器的质量，为水下航行器附加质量矩阵；Mj22＝Ijo+Aj22，为航行器的转动惯量，为水下航行器附加惯量矩阵；
C(v)j定义为：
其中，C12(v1)j、C21(v1)jC22(v2)j为科氏力与向心力矩阵；
D(v)j定义为：
其中，D11(v1)j、D22(v2)j为流体水动力阻尼矩阵；
G(η)j为恢复力与力矩向量，定义为：
其中，Wj为第j个航行器受到的重力，Bj为第j个航行器所产生的浮力；为重心相对浮心的位置坐标；rjg为第j个航行器的重心坐标，rjb为第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，用于构建编队控制器，使各水下航行器实现相对位置保持固定；
辅助变量φj的一次导数为虚拟控制律：
其中，设计参数为常量；ηjk为航行器j、k之间的相对位置；δjk为航行器j、k之间的相对位置指令。
5.一种计算机设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至3中任一项所述的方法。
6.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行如权利要求1至3任一项所述的方法。
CN202211098949.0A 2022-09-07 2022-09-07 一种欠驱动水下航行器编队控制方法及系统 Active CN115576334B (zh)

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)

* Cited by examiner, † Cited by third party
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 哈尔滨工程大学 一种考虑输入量化的自适应预设性能轨迹跟踪控制方法
CN114564015A (zh) * 2022-02-24 2022-05-31 哈尔滨工程大学 一种拒止环境下的欠驱动无人艇分布式编队控制方法

Also Published As

Publication number Publication date
CN115576334A (zh) 2023-01-06

Similar Documents

Publication Publication Date Title
CN112241125B (zh) 一种基于微分平坦特性的无人机轨迹跟踪方法
CN106444806A (zh) 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法
Imanberdiyev et al. A fast learning control strategy for unmanned aerial manipulators
Carrillo et al. Deep-learning tracking for autonomous flying systems under adversarial inputs
CN115639830B (zh) 一种空地智能体协同编队控制系统及其编队控制方法
CN113859589B (zh) 一种基于模型预测控制与滑模控制的航天器姿态控制方法
CN108638068A (zh) 一种携带冗余度机械臂的飞行机器人控制系统设计方法
CN113296507A (zh) 一种基于时空解耦的多动力定位船舶协同编队控制方法
CN107894775B (zh) 一种欠驱动无人水下航行器轨迹生成与控制方法
Joukhadar et al. Advanced UAVs nonlinear control systems and applications
CN115122325A (zh) 一种具有视场约束的拟人化机械手鲁棒视觉伺服控制方法
CN107247464B (zh) 一种四旋翼无人飞行器的状态受限控制方法和系统
CN115576334B (zh) 一种欠驱动水下航行器编队控制方法及系统
CN116277036A (zh) 一种柔性基、柔性臂空间机器人快速容错抑振控制方法
Wadi et al. Modeling and system identification of an autonomous underwater vehicle
CN115524969A (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
CN113741493B (zh) 一种航天器安全接近控制方法
CN116126003A (zh) 基于Stewart平台的波浪补偿系统建模和位姿控制方法
Silva et al. Models for simulation and control of underwater vehicles
Shang et al. Finite-time tracking for UUV with system uncertainty using fast adaptive nonsingular terminal high-order sliding mode control
Zhang et al. Modeling and dynamic control of a class of semibiomimetic robotic fish
Liang et al. Robust nonlinear path-tracking control of vector-propelled AUVs in complex sea conditions
CN117369481B (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