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

1.本发明属于水下航行器领域，具体涉及一种欠驱动水下航行器编队控制方法及系统。

背景技术：

2.水下航行器的水下作业往往以单个形式出现，然而随着任务复杂度的增加(如：水下观测、组网、管道检修、探测等)，导致单个水下航行器难以完成复杂的水下任务，需要多个水下航行器以编队航行的形式相互协同配合来完成任务。同时，多个水下航行器之间相互合作也能够减少任务时间，提高工作效率和成功率。
3.水下航行器编队控制目标是实现多个航行器的编队航行，可以执行更高质量的任务，满足特定的需求。现有的研究没有将编队控制量进行限制，当编队成员过多以及各个成员之间的距离过大时，编队控制量非常大，会增加系统的负担，影响速度、姿态的控制品质。同时，对于姿态的描述通常利用欧拉角，计算效率较低。

技术实现要素：

4.本发明的目的在于克服当编队成员过多以及各个成员之间的距离过大时，现有技术的编队控制计算量大，增加系统的负担，影响速度、姿态控制品质的缺陷。
5.为了实现上述目的，本发明提出了一种欠驱动水下航行器编队控制方法，所述方法包括：
6.步骤1：建立水下航行器模型；
7.步骤2：利用图论知识建立通信拓扑式，引入辅助变量构建编队控制器；
8.步骤3：计算虚拟控制律，并将结果用于控制器的设计中，计算出控制推力输入与理想姿态指令；
9.步骤4：计算输入力矩使水下航行器的姿态跟踪理想姿态指令，使得一组航行器实现编队航行。
10.作为上述方法的一种改进，所述步骤1具体为：
11.建立欠驱动水下航行器模型编队运动及动力学微分方程为：
[0012][0013][0014]
其中，符号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}＝[b
j1
,b
j2
,b
j3
]
t
，其中，b
j1
＝[1,0,0]
t
，b
j2
＝[0,1,0]
t
，b
j3
＝[0,0,1]
t
为{bj}在航行器三维方向u，v，w上的单位向量；
[0015]
η
j1
＝[xj，yj，zj]
t
为第j个航行器浮心在{e}下的位置坐标；
[0016]
其中v
j1
＝[uj,vj,wj]
t
为第j个航行器浮心在{bj}中的线速度大小，uj,vj,wj分别是第j个航行器浮心在{bj}中在u、v、w三个方向的线速度大小；v
j2
＝[pj,qj,rj]
t
为{bj}相对于{e}的角速度大小在{bj}中的投影，pj,qj,rj分别为{bj}相对于{e}在u，v，w三个方向上的角速度大小在{bj}中的投影；
[0017]
单位四元数定义为qj＝[q
j1
,q
j2
,q
j3
,λj]
t
，用于描述{bj}相对于{e}的姿态；q
j1
,q
j2
,q
j3
，λj四个变量，一起用于描述{bj}相对于{e}的在u，v，w三个方向上的姿态；
[0018]
r(q)j为{bj}相对于{e}的旋转矩阵：
[0019][0020]
其中，i3×3为三阶单位对角矩阵；qj＝(q
j1
,q
j2
,q
j3
)
t
为取单位四元数qj＝[q
j1
,q
j2
,q
j3
,λj]
t
的前三行定义的列向量；
[0021][0022][0023]
为惯性矩阵，满足下式：
[0024][0025]
其中，m
j11
＝mji3×3+a
j11
，mj为航行器的质量，为水下航行器附加质量矩阵；m
j22
＝i
jo
+a
j22
，为航行器的转动惯量，为水下航行器附加惯量矩阵；
[0026]
c(v)j定义为：
[0027][0028]
其中，c
12
(v1)j、c
21
(v1)
j c
22
(v2)j为科氏力与向心力矩阵；
[0029]
d(v)j定义为：
[0030][0031]
其中，d
11
(v1)j、d
22
(v2)j为流体水动力阻尼矩阵；
[0032]
g(η)j为恢复力与力矩向量，定义为：
[0033]
[0034]
其中，wj为第j个航行器受到的重力，bj为第j个航行器所产生的浮力；为重心相对浮心的位置坐标；r
jg
为第j个航行器的重心坐标，r
jb
为第j个航行器的浮心坐标；为对于向量的斜对称矩阵；
[0035]
τj为第j个航行器的控制输入，定义为τj＝(nj，0，0，γ
j1
，γ
j2
，γ
j3
)
t
；其中nj代表第j个航行器沿着{bj}系中b
j1
方向的控制推力，γj＝(γ
j1
，γ
j2
，γ
j3
)
t
为第j个航行器在u、v、w三个方向上的控制力矩。
[0036]
作为上述方法的一种改进，所述步骤2具体为：
[0037]
利用航行器安装的水声通讯模块使编队中的各航行器之间都可以相互通信，利用图论方法对航行器之间的通信拓扑进行建模；所述通信拓扑为带权值的无向图，定义为通信拓扑的一条边，n为航行器个数；第j和k个航行器之间如果存在通信，有(j，k)∈ε；加权相邻矩阵描述了第j和k航行器之间的通信强度权值，定义为：
[0038][0039]
其中，k
jj
表示第j个航行器自己与自己的通信强度权值；
[0040]
引入辅助变量φj，用于构建编队控制器，使各水下航行器实现相对位置保持固定；
[0041]
辅助变量φj的一次导数为虚拟控制律：
[0042][0043]
其中，设计参数为常量；η
jk
为航行器j、k之间的相对位置；δ
jk
为航行器j、k之间的相对位置指令。
[0044]
作为上述方法的一种改进，所述步骤3具体为：
[0045]
第j个航行器实际控制推力nj计算公式为：
[0046][0047]
其中，设计的中间控制力fj是推力nj与理想姿态单位四元数q
jd
合成的三维控制输入，按行展开定义为fj＝(f
j1
,f
j2
,f
j3
)
t
；
[0048][0049]
其中，表示在惯性坐标系下的第j个航行器的速度误差，v
d1
为航行器编队航行时的速度指令；其余变量定义如下：
[0050]
[0051][0052][0053][0054][0055]
对第j个航行器，定义理想姿态单位四元数为所生成的理想姿态信息，列向量
[0056]
理想姿态指令计算公式为：
[0057][0058][0059][0060]
其中，ω
jd
为理想坐标系中的航行器姿态指令角速度；s(qd)j为对于向量q
jd
的斜对称矩阵。
[0061]
作为上述方法的一种改进，所述步骤4具体为：
[0062]
计算输入控制力矩为：
[0063][0064]
其中，表示单位四元数描述的{bj}系相对于理想坐标系{dj}之间的姿态误差，为列向量的第一行，为列向量的第二行，为列向量的第三行；变量变量表示{bj}系相对于理想坐标系{dj}之间的角速度误差；为大于零的控制增益；βj为设计虚拟控制律：
[0065][0066]
其中，为大于零的控制增益；
[0067][0068]
其中，为理想坐标系{dj}到{bj}系的旋转矩阵，r(q)j为{bj}相对于惯性坐标系{e}的旋转矩阵，r(qd)j为{dj}相对于惯性坐标系{e}的旋转矩阵；c
j22
(v2)j为科氏力与向心力惯量矩阵；d
j22
(v2)j为流体水动力阻尼惯量矩阵；
[0069]
为系统实际姿态qj与理想姿态q
jd
之间的误差对位置子系统所造成的影响：
[0070][0071]
本发明还提供一种欠驱动水下航行器编队控制器，所述系统包括：
[0072]
水下航行器模型模块，用于建立水下航行器模型；
[0073]
编队控制器模块，用于利用图论知识建立通信拓扑式，引入辅助变量构建编队控制器；
[0074]
控制推力输入模块，用于计算虚拟控制律，并将结果用于控制器的设计中，计算出控制推力输入与理想姿态指令；
[0075]
输入控制力矩模块，用于计算输入控制力矩使水下航行器的姿态跟踪理想姿态指令，使得一组航行器实现编队航行。
[0076]
本发明还提供一种计算机设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述任一项所述的方法。
[0077]
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行如上述任一项所述的方法。
[0078]
与现有技术相比，本发明的优势在于：
[0079]
本发明利用单位四元数对姿态进行描述，提高运算效率并可以避免奇点；通过一个积分型辅助变量结合双曲正切饱和函数构建编队控制器，增加了编队集过程中的平稳性。
附图说明
[0080]
图1所示为欠驱动水下航行器编队控制方法流程图；
[0081]
图2所示为n个水下航行器编队示意图；
[0082]
图3所示为水下航行器(autonomous underwater vehicle,auv)编队过程仿真结果。
具体实施方式
[0083]
下面结合附图对本发明的技术方案进行详细的说明。
[0084]
本发明所研究的对象是一种6自由度欠驱动水下航行器的模型，利用水声通信技术，在各水下航行器进行信息交互的条件下，设计编队控制器，使得一组航行器可以实现编队航行，并跟踪指定的速度以及姿态信息。
[0085]
如图1所示，本发明方法实施方法步骤如下：
[0086]
1.建立水下航行器模型。
[0087]
本步骤建立了单位四元数欠驱动水下航行器模型，相比于传统建模，具有运算效率高、避免奇异性的优点。以下是推导过程：
[0088]
定义北东地{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}＝[b
j1
,b
j2
,b
j3
]
t
，其中，b
j1
＝[1,0,0]
t
，b
j2
＝[0,1,0]
t
，b
j3
＝[0,0,1]
t
为{bj}在航行器u，v，w方向上的单位向量。{bj}的原点与航行器的浮心cb重合，浮心在本体系中坐标为r
jb
＝(0,0,0)
t
；为了产生恢复力矩，航行器的重心cm在b
j3
的正半轴，重心在本体系中的坐标为r
jg
＝(0,0,z
jg
)
t
，z
jg
＞0。
[0089]
定义第j个水下航行器的位置姿态向量为为航行器浮心在{e}下的位置坐标。单位四元数可定义为qj＝[q
j1
,q
j2
,q
j3
,λj]
t
(q
j1
,q
j2
,q
j3
，λj这四个量，一起用于描述{bj}相对于{e}的在u、v、w三个方向上的姿态)，用于描述{bj}相对于{e}的姿态，取单位四元数的前三行定义列向量qj＝(q
j1
,q
j2
,q
j3
)
t
。其中v
j1
＝[uj,vj,wj]
t
为浮心在{bj}中的线速度大小(uj,vj,wj分别是浮心在{bj}中在u、v、w三个方向的线速度大小)，v
j2
＝[pj,qj,rj]
t
为{bj}相对于{e}的角速度大小在{bj}中的投影(pj,qj,rj分别为{bj}相对于{e}在u、v、w三个方向上的角速度大小在{bj}中的投影)。
[0090]
可知{bj}相对于{e}的旋转矩阵为
[0091][0092]
其中i3×3为三阶单位对角矩阵，斜对称矩阵；qj＝(q
j1
,q
j2
,q
j3
)
t
为取单位四元数qj＝[q
j1
,q
j2
,q
j3
,λj]
t
的前三行定义的列向量。
[0093][0094]
欠驱动水下航行器编队示意图如图2所示，运动学及动力学微分方程为：
[0095][0096]
[0097]
其中，为惯性矩阵，可分块为：
[0098][0099]
其中，m
j11
＝mji3×3+a
j11
，mj为航行器的质量，为水下航行器附加质量矩阵；m
j22
＝i
jo
+a
j22
，为航行器的转动惯量，为水下航行器附加惯量矩阵。
[0100]
c(v)j定义为：
[0101][0102]
其中，c
12
(v1)j、c
21
(v1)
j c
22
(v2)j为科氏力与向心力矩阵。
[0103]
d(v)j定义为：
[0104][0105]
其中，d
11
(v1)j、d
22
(v2)j为流体水动力阻尼矩阵。
[0106]
g(η)i为恢复力与力矩向量，定义为：
[0107][0108]
其中，w为航行器受到的重力，b为航行器所产生的浮力；为重心相对浮心的位置坐标。
[0109]
水下航行器的控制输入定义为τj＝(nj，0，0，γ
j1
，γ
j2
，γ
j3
)
t
；其中nj代表航行器沿着{bj}系中b
j1
方向的控制推力，γj＝(γ
j1
，γ
j2
，γ
j3
)
t
为水下航行器在u、v、w三个方向上的控制力矩。
[0110]
2.利用图论知识建立通信拓扑式(11)，引入辅助变量构建编队控制器。
[0111]
编队控制器：各航行器之间相互通信，可以进行位置信息交互，利用相对位置信息设计编队控制器，使多航行器能够保持预设的队形协同航行。
[0112]
定义编队中任意两个航行器j、k之间的相对位置为η
jk
＝η
j-ηk，j,k＝[1,2,
···
,n]；定义航行器线速度跟踪指令为任意两个航行器j、k之间的相对位置指令为δ
jk
＝δ
j-δk；控制目标为：设计控制推力nj以及控制力矩γj，使得每个航行器的速度最终为v
d1
，即在惯性坐标系下的速度误差满足：
[0113][0114]
同时，各航行器之间的相对位置关系满足：
[0115]
[0116]
最终，编队中的航行器会以一个共同的速度v
d1
航行，同时任意两个航行器j,k＝[1,2,
···
,n]之间会保持指定的相对位置δ
jk
。
[0117]
利用航行器安装的水声通讯模块使编队中的各航行器之间都可以相互通信，利用图论方法对航行器之间的通信拓扑进行建模。通信拓扑为带权值的无向图，定义为通信拓扑的一条边，第j和k个航行器之间如果存在通信，那么(j，k)∈ε；加权相邻矩阵描述了第j和k航行器之间的通信强度权值，定义为：
[0118][0119]kjj
表示第j个航行器自己与自己的通信强度权值；
[0120]
引入辅助变量φj，用于构建编队控制器，使各水下航行器实现相对位置保持固定。
[0121]
设计虚拟控制律为：
[0122][0123]
其中，设计参数为常量，j＝[1,2,
···
,n]。
[0124]
3.计算虚拟控制律，并将结果用于控制器的设计中，计算出控制推力输入与理想姿态指令。
[0125]
对式(12)进行积分，计算φj的实时结果，并将结果用于控制器的设计中，计算出控制推力式(33)与理想姿态式(18)、(34)、(35)。
[0126]
根据式(4)-(8)，可将式(5)展开为：
[0127][0128][0129]
可知式(13)与式(14)均为{bj}系坐标下的航行器动力学方程，而速度指令v
d1
是相对于{e}给定的。为了方便控制器的设计，需将式(13)转换到{e}系中。
[0130]
由式(3)可知：
[0131][0132]
代入式(13)：
[0133][0134]
其中，变换后的矩阵定义为：
[0135][0136][0137][0138][0139]
变换后的方程式(16)为{e}系中的位置坐标动力学方程，称为位置子系统；式(14)为{bj}系中的姿态动力学方程，称为姿态子系统。
[0140]
由航行器的控制输入τj＝(nj，0，0，γ
j1
，γ
j2
，γ
j3
)
t
可知，位置子系统在横向b
j2
与升沉b
j3
方向没有执行器产生控制量，是欠驱动系统；姿态子系统为全驱动系统。为了能够跟踪一个三维的理想速度v
d1
，理想的姿态信息需q
jd
要作为额外的控制量，与控制推力合成三维控制输入。
[0141]
对第j个航行器，定义单位四元数为所生成的理想姿态信息，列向量利用单位四元数的运算关系，可得{bj}系与理想坐标系{dj}之间的姿态误差以及角速度误差为：
[0142][0143]
其中为{dj}系到{bj}系的旋转矩阵，其导数关系满足为关于q
jd
的旋转矩阵，为对于向量的斜对称矩阵；ω
jd
为理想坐标系中的航行器姿态指令角速度，与q
jd
之间满足：
[0144][0145]
s(qd)j为对于向量q
jd
的斜对称矩阵。
[0146]
在姿态子系统控制律设计的过程中，为了解决欠驱动问题，首要任务是为每个航行器的位置子系统定义一个具有加速度量纲的中间控制量：
[0147]
[0148]
由上式可知，fj是推力nj与理想姿态q
jd
合成的三维控制输入。首先对fj(即)进行设计，得到一个确定的关于fj的函数，之后利用已设计出的具有确定函数值的fj可以求解出位置子系统所需的推力nj以及理想姿态q
jd
。
[0149]
根据上述思路，将式(19)代入式(16)可得：
[0150][0151]
其中为系统实际姿态qj与理想姿态q
jd
之间的误差对位置子系统所造成的影响，易知：
[0152][0153]
由式(9)所定义的速度误差易知：
[0154][0155]
将式(22)代入位置子系统式(20)可知：
[0156][0157]
其中：
[0158][0159]
定义lyapunov函数：
[0160][0161]
其中，j＝[1,2,
···
,n]为大于零的增益系数。
[0162]
定义第j和k个航行器之间的相对速度误差为v
1jk
＝v
j1-v
k1
，v
k1
为第k个航行器的线速度。对上式求导可得：
[0163][0164]
由于通信拓扑为无向图，因此满足：
[0165][0166]
即在无向图通信拓扑下，下标jk是可以交换的。
[0167]
从而可得
[0168]
将位置子系统误差方程式(23)代入式(26)可得：
[0169][0170]
将式(28)展开得：
[0171][0172]
设计中间控制力为：
[0173][0174]
其中，表示在惯性坐标系下的第j个航行器的速度误差，v
d1
为航行器编队航行时的速度指令；控制增益j＝[1,2,
···
,n]。
[0175]
将式(30)代入式(29)可得：
[0176][0177]
将式(12)代入式(31)可得：
[0178][0179]
由设计的中间控制力式(30)以及虚拟控制律式(12)可确定式(19)等号右侧的函数，将所设计的中间控制力按行展开定义为fj＝(f
j1
,f
j2
,f
j3
)
t
，可以解得实际控制推力为：
[0180][0181]
可以解出λ
jd
为：
[0182][0183]
可以解出q
jd
为：
[0184][0185]
4.设计输入力矩式(40)使水下航行器的姿态跟踪理想姿态指令，使得一组航行器可以实现编队航行，并跟踪指定的速度以及姿态信息。
[0186]
利用所利用中间控制量计算输入推力以及理想姿态，设计输入力矩，使得一组航行器可以实现编队航行，并跟踪指定的速度以及姿态信息。
[0187]
在获得目标姿态q
jd
与角速度ω
jd
后，设计控制力矩γj使姿态子系统qj跟踪q
jd
。
[0188]
将姿态误差方程式(17)代入姿态子系统微分方程式(14)可得
[0189][0190]
其中为变量对时间的一阶导数，
[0191][0192]
其中，为理想坐标系{dj}到{bj}系的旋转矩阵，r(q)j为{bj}相对于惯性坐标系{e}的旋转矩阵，r(qd)j为{dj}相对于惯性坐标系{e}的旋转矩阵；c
j22
(v2)j为科氏力与向心力惯量矩阵；d
j22
(v2)j为流体水动力阻尼惯量矩阵；
[0193]
定义lyapunov函数：
[0194][0195]
其中，根据四元数运算关系易得根据四元数运算关系易得变量βj为待设计的虚拟控制量。
[0196]
对式(38)求导可得：
[0197]
[0198]
设计控制力矩为：
[0199][0200]
其中，表示单位四元数描述的{bj}系相对于理想坐标系{dj}之间的姿态误差，为列向量的第一行，为列向量的第二行，为列向量的第三行；变量量表示{bj}系相对于理想坐标系{dj}之间的角速度误差；
[0201]
设计虚拟控制律为：
[0202][0203]
其中，为大于零的控制增益。
[0204]
将式(40)及(41)代入式(39)可得：
[0205][0206]
5.闭环系统稳定性分析。
[0207]
定理1.由式(3)与(4)所描述的欠驱动水下航行器，利用所设计的控制推力式(33)以及控制力矩式(40)，当t
→
∞时，qj→
qd，λj→
λd，η
jk
→
δ
jk
。
[0208]
证明：
[0209]
根据式(25)及(38)，定义闭环系统lyapunov函数为：
[0210][0211]
对式(43)求导，并将位置子系统控制律式(30)、(12)以及姿态子系统控制律式(40)、(41)可得：
[0212][0213]
可知φj，sj是有界的。
[0214]
又因为
[0215][0216][0217]
因此有界，可知一致连续。根据barbalat引理可知，因此φj，sj→
0，从而可知qj→
qd，λj→
λd，
[0218]
根据式(12)可知：
[0219][0220]
因此有界，可知一致连续。根据barbalat引理可知：
[0221][0222]
又因为已证明φj→
0，因此可知η
jk-δ
jk
→
0，η
jk
→
δ
jk
。
[0223]
最终可以证明：当t
→
∞时，qj→
qd，λj→
λd，η
jk
→
δ
jk
；即编队中的航行器均可按指定的速度v
d1
航行，任意第j个和k个航行器之间的相对位置可以保持指定的距离δ
jk
，同时各个航行器的姿态子系统都可跟踪位置子系统所生成的姿态指令。
[0224]
6.仿真验证。
[0225]
令四个欠驱动水下航行器组成矩形编队，各航行器初始状态为：
[0226][0227][0228][0229][0230]
参考速度为：
[0231]
控制增益为：k
jk
＝5，
[0232]
理想相对坐标为：δ1＝(1,1,0)
t
，δ2＝(-1,1,0)
t
，δ3＝(-1,-1,0)
t
，δ4＝(1,-1,0)
t
；
[0233]
仿真结果如图3所示。
[0234]
本发明的优点有两个：
[0235]
1、设计控制器，实现了多水下航行器的编队协同工作；
[0236]
2、所提出的控制器的闭环稳定性得到了证明。
[0237]
本发明还可提供的一种计算机设备，包括：至少一个处理器、存储器、至少一个网络接口和用户接口。该设备中的各个组件通过总线系统耦合在一起。可理解，总线系统用于实现这些组件之间的连接通信。总线系统除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。
[0238]
其中，用户接口可以包括显示器、键盘或者点击设备(例如，鼠标，轨迹球(track ball)、触感板或者触摸屏等。
[0239]
可以理解，本技术公开实施例中的存储器可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(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 data rate sdram，ddrsdram)、增强型同步动态随机存取存储器(enhanced sdram，esdram)、同步连接动态随机存取存储器(synchlink dram，sldram)和直接内存总线随机存取存储器(direct rambus ram，drram)。本文描述的存储器旨在包括但不限于这些和任意其它适合类型的存储器。
[0240]
在一些实施方式中，存储器存储了如下的元素，可执行模块或者数据结构，或者他们的子集，或者他们的扩展集：操作系统和应用程序。
[0241]
其中，操作系统，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序，包含各种应用程序，例如媒体播放器(media player)、浏览器(browser)等，用于实现各种应用业务。实现本公开实施例方法的程序可以包含在应用程序中。
[0242]
在本上述的实施例中，还可通过调用存储器存储的程序或指令，具体的，可以是应用程序中存储的程序或指令，处理器用于：
[0243]
执行上述方法的步骤。
[0244]
上述方法可以应用于处理器中，或者由处理器实现。处理器可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器可以是通用处理器、数字信号处理器(digital signal processor，dsp)、专用集成电路(application specific integrated circuit，asic)、现场可编程门阵列(field programmable gate array，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行上述公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合上述公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器读取存储器中的信息，结合其硬件完成上述方法的步
骤。
[0245]
可以理解的是，本发明描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现，处理单元可以实现在一个或多个专用集成电路(application specific integrated circuits，asic)、数字信号处理器(digital signal processing，dsp)、数字信号处理设备(dsp device，dspd)、可编程逻辑设备(programmable logic device，pld)、现场可编程门阵列(field-programmable gate array，fpga)、通用处理器、控制器、微控制器、微处理器、用于执行本技术所述功能的其它电子单元或其组合中。
[0246]
对于软件实现，可通过执行本发明的功能模块(例如过程、函数等)来实现本发明技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。
[0247]
本发明还可提供一种非易失性存储介质，用于存储计算机程序。当该计算机程序被处理器执行时可以实现上述方法实施例中的各个步骤。
[0248]
最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

技术特征：

1.一种欠驱动水下航行器编队控制方法，所述方法包括：步骤1：建立水下航行器模型；步骤2：利用图论知识建立通信拓扑式，引入辅助变量构建编队控制器；步骤3：计算虚拟控制律，并将结果用于控制器的设计中，计算出控制推力输入与理想姿态指令；步骤4：计算输入力矩使水下航行器的姿态跟踪理想姿态指令，使得一组航行器实现编队航行。2.根据权利要求1所述的欠驱动水下航行器编队控制方法，其特征在于，所述步骤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个航行器的本体坐标系为{b
j
}＝[b
j1
,b
j2
,b
j3
]
t
，其中，b
j1
＝[1,0,0]
t
，b
j2
＝[0,1,0]
t
，b
j3
＝[0,0,1]
t
为{b
j
}在航行器三维方向u，v，w上的单位向量；η
j1
＝[x
j
，y
j
，z
j
]
t
为第j个航行器浮心在{e}下的位置坐标；其中v
j1
＝[u
j
,v
j
,w
j
]
t
为第j个航行器浮心在{b
j
}中的线速度大小，u
j
,v
j
,w
j
分别是第j个航行器浮心在{b
j
}中在u、v、w三个方向的线速度大小；v
j2
＝[p
j
,q
j
,r
j
]
t
为{b
j
}相对于{e}的角速度大小在{b
j
}中的投影，p
j
,q
j
,r
j
分别为{b
j
}相对于{e}在u，v，w三个方向上的角速度大小在{b
j
}中的投影；单位四元数定义为q
j
＝[q
j1
,q
j2
,q
j3
,λ
j
]
t
，用于描述{b
j
}相对于{e}的姿态；q
j1
,q
j2
,q
j3
，λ
j
四个变量，一起用于描述{b
j
}相对于{e}的在u，v，w三个方向上的姿态；r(q)
j
为{b
j
}相对于{e}的旋转矩阵：其中，i3×3为三阶单位对角矩阵；q
j
＝(q
j1
,q
j2
,q
j3
)
t
为取单位四元数q
j
＝[q
j1
,q
j2
,q
j3
,λ
j
]
t
的前三行定义的列向量；的前三行定义的列向量；
为惯性矩阵，满足下式：其中，m
j11
＝m
j
i3×3+a
j11
，m
j
为航行器的质量，为水下航行器附加质量矩阵；m
j22
＝i
jo
+a
j22
，为航行器的转动惯量，为水下航行器附加惯量矩阵；c(v)
j
定义为：其中，c
12
(v1)
j
、c
21
(v1)
j c
22
(v2)
j
为科氏力与向心力矩阵；d(v)
j
定义为：其中，d
11
(v1)
j
、d
22
(v2)
j
为流体水动力阻尼矩阵；g(η)
j
为恢复力与力矩向量，定义为：其中，w
j
为第j个航行器受到的重力，b
j
为第j个航行器所产生的浮力；为重心相对浮心的位置坐标；r
jg
为第j个航行器的重心坐标，r
jb
为第j个航行器的浮心坐标；为对于向量的斜对称矩阵；τ
j
为第j个航行器的控制输入，定义为τ
j
＝(n
j
，0，0，γ
j1
，γ
j2
，γ
j3
)
t
；其中n
j
代表第j个航行器沿着{b
j
}系中b
j1
方向的控制推力，γ
j
＝(γ
j1
，γ
j2
，γ
j3
)
t
为第j个航行器在u、v、w三个方向上的控制力矩。3.权利要求2所述的欠驱动水下航行器编队控制方法，其特征在于，所述步骤2具体为：利用航行器安装的水声通讯模块使编队中的各航行器之间都可以相互通信，利用图论方法对航行器之间的通信拓扑进行建模；所述通信拓扑为带权值的无向图，定义为通信拓扑的一条边，n为航行器个数；第j和k个航行器之间如果存在通信，有(j，k)∈ε；加权相邻矩阵描述了第j和k航行器之间的通信强度权值，定义为：其中，k
jj
表示第j个航行器自己与自己的通信强度权值；
引入辅助变量φ
j
，用于构建编队控制器，使各水下航行器实现相对位置保持固定；辅助变量φ
j
的一次导数为虚拟控制律：其中，设计参数为常量；η
jk
为航行器j、k之间的相对位置；δ
jk
为航行器j、k之间的相对位置指令。4.权利要求3所述的欠驱动水下航行器编队控制方法，其特征在于，所述步骤3具体为：第j个航行器实际控制推力n
j
计算公式为：其中，设计的中间控制力f
j
是推力n
j
与理想姿态单位四元数q
jd
合成的三维控制输入，按行展开定义为f
j
＝(f
j1
,f
j2
,f
j3
)
t
；其中，表示在惯性坐标系下的第j个航行器的速度误差，v
d1
为航行器编队航行时的速度指令；其余变量定义如下：航行时的速度指令；其余变量定义如下：航行时的速度指令；其余变量定义如下：航行时的速度指令；其余变量定义如下：航行时的速度指令；其余变量定义如下：对第j个航行器，定义理想姿态单位四元数为所生成的理想姿态信息，列向量理想姿态指令计算公式为：理想姿态指令计算公式为：
其中，ω
jd
为理想坐标系中的航行器姿态指令角速度；s(q
d
)
j
为对于向量q
jd
的斜对称矩阵。5.权利要求4所述的欠驱动水下航行器编队控制方法，其特征在于，所述步骤4具体为：计算输入控制力矩为：其中，表示单位四元数描述的{b
j
}系相对于理想坐标系{d
j
}之间的姿态误差，为列向量的第一行，为列向量的第二行，为列向量的第三行；变量的第三行；变量表示{b
j
}系相对于理想坐标系{d
j
}之间的角速度误差；为大于零的控制增益；β
j
为设计虚拟控制律：其中，为大于零的控制增益；其中，为理想坐标系{d
j
}到{b
j
}系的旋转矩阵，r(q)
j
为{b
j
}相对于惯性坐标系{e}的旋转矩阵，r(q
d
)
j
为{d
j
}相对于惯性坐标系{e}的旋转矩阵；c
j22
(v2)
j
为科氏力与向心力惯量矩阵；d
j22
(v2)
j
为流体水动力阻尼惯量矩阵；为系统实际姿态q
j
与理想姿态q
jd
之间的误差对位置子系统所造成的影响：6.一种欠驱动水下航行器编队控制器，所述系统包括：水下航行器模型模块，用于建立水下航行器模型；编队控制器模块，用于利用图论知识建立通信拓扑式，引入辅助变量构建编队控制器；控制推力输入模块，用于计算虚拟控制律，并将结果用于控制器的设计中，计算出控制推力输入与理想姿态指令；输入控制力矩模块，用于计算输入控制力矩使水下航行器的姿态跟踪理想姿态指令，使得一组航行器实现编队航行。7.一种计算机设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上
运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5中任一项所述的方法。8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行如权利要求1至5任一项所述的方法。

技术总结

本发明提供了一种欠驱动水下航行器编队控制方法及系统，所述方法包括：建立水下航行器模型；利用图论知识建立通信拓扑式，引入辅助变量构建编队控制器；计算虚拟控制律，并将结果用于控制器的设计中，计算出控制推力输入与理想姿态指令；计算输入力矩使水下航行器的姿态跟踪理想姿态指令，使得一组航行器可以实现编队航行。本发明的优势在于：利用单位四元数对姿态进行描述，提高运算效率并可以避免奇点；通过一个积分型辅助变量结合双曲正切饱和函数构建编队控制器，增加了编队集过程中的平稳性。平稳性。平稳性。