有向切换拓扑下多AUV编队队形一致性控制方法与流程

有向切换拓扑下多auv编队队形一致性控制方法
技术领域
1.本发明属于多自主式水下航行器的协调控制技术领域，具体涉及有向切换拓扑下多auv(autonomous underwater vehicle，自主式水下航行器)编队队形一致性控制方法。


背景技术：

2.自主水下航行器利用其搭载的各种传感器模块和其他任务模块，在水下资源勘测，水下扫雷，情报搜集等军事和民用领域起着重要的作用。随着执行海洋任务的难度增加，执行任务的环境变得更加复杂，单水下航行器体现出了执行任务效率低、工作范围小、系统冗余性差等不可避免的局限性。为了解决上述问题，多航行器协同执行任务成了发展的必然方向。多水下航行器协同执行任务可以增加系统的冗余性和鲁棒性，降低完成任务的成本，提高工作效率。
3.近些年来，许多科研人员提出了适用于不同模型的先进编队控制方法，其中更多的是将动力学模型抽象为一阶积分器，实际中，动力学模型抽象为二阶积分器更加真实。文献(cui rongxin,ge sam shuzhi,bernardvoon ee how,et al.leader
–
follower formation control of underactuated autonomous underwater vehicles[j])提出了一种基于李雅普诺夫反步法的多自主水下航行器编队协同运动控制方法。文献(qi,xue,cai zhi
‑
jun.three
‑
dimensional formation control based on nonlinear small gain method for multiple underactuated underwater vehicles[j])针对在三维空间运动的欠驱动水下航行器，提出了一种分布式编队跟踪控制器。文献(r.raja,s.bidyadhar.a backstepping approach for the formation control of multiple autonomous underwater vehicles using a leader
–
follower strategy[j])建立了多艘由于通信故障而导致编队不完善的自主水下航行器的一致控制问题。文献(chen shun,daniel w.c.ho.consensus control for multiple auvs under imperfect information caused by communication faults[j])设计了一种基于反馈线性化的姿态控制器，使每个水下机器人的姿态收敛到其期望值。文献(yang hui,wang chuanfeng,zhang fumin.robust geometric formation control of multiple autonomous[c])利用一致性理论和速度通信拓扑控制多水下航行器编队保持，实现速度和航向一致性。任伟等人提出了一个二阶积分编队控制协议，为多智能体编队的固定拓扑和变换拓扑的情况提供了充分条件。
[0004]
基于有向通信拓扑结构，文献(xia guoqing,zhang yu,yang ying.control method of multi
‑
auv circular formation combining consensus theory and artificial potential field method[c])考虑了二阶积分器动力学模型的一致性算法，证明了与速度控制增益大于某个界限时，具有有向生成树的编队一致性是渐进达成的。文献(ren wei.on consensus algorithms for double
‑
integrator dynamics[c])提出了一种采样周期性间歇采样一致性控制协议，将多智能体间的误差控制在一定范围内，降低了控制器的采样时间。文献(yu zhiyong,jiang haijun,hu cheng.second
‑
order consensus for multiagent systems via intermittent sampled data control[j])研究了高阶线
性时变编队控制，提出一种通用的编队控制协议，数值仿真表明切换拓扑的群系统可以实现一致。文献(dong xiwang,shi zongying,geng lu,et al.time
‑
varying formation control for high
‑
order linear swarm systems with switching interaction topologies[j]以及yang daopeng,ren wei,liu xiangdong,et al.decentralized event
‑
triggered consensus for linear multi
‑
agent systems under general directed graphs[j])研究了一般线性模型的多智能体系统的一致性控制协议以及事件触发一致性，通过在离散的采样时间，实现了多智能体编队的一致性。文献(二阶时滞多智能体系统分组一致性分析[j])研究了强连通图结构下，二阶多智能体系统的分组一致性。文献(nian xiaohong,su saijun,pan huan.consensus tracking protocol and formation control of multi
‑
agent systems with switching topology[j])在已知拓扑集合下，设计了一阶编队控制协议，解决了已知拓扑集合下的一阶多智能体编队控制问题。文献(有向切换通信拓扑下多无人机分布式编队控制[j])的有向拓扑图条件为强连通且平衡条件，显然结果不具有一般性。
[0005]
综上所述，虽然现有方法在多自主水下航行器协调控制方面已经取得了一定的成就，但是在随机切换拓扑和编队队形变换时，现有控制方法的鲁棒性和稳定性仍然较差，因此，提高现有控制方法的鲁棒性和稳定性是目前亟需解决的问题。


技术实现要素：

[0006]
本发明的目的是为解决在随机切换拓扑和编队队形变换时，现有控制方法的鲁棒性和稳定性差的问题，而提出一种在有向切换拓扑下多auv编队队形一致性控制方法。
[0007]
本发明为解决上述技术问题所采取的技术方案是：
[0008]
有向切换拓扑下多auv编队队形一致性控制方法，所述方法具体包括以下步骤：
[0009]
步骤一、定义用于描述多auv之间通信拓扑关系的有向图；
[0010]
步骤二、基于反馈线性化方法，将auv的运动学方程和动力学方程线性化为二阶积分形式的反馈线性化动态模型；
[0011]
步骤三、利用有向图表示的auv之间通信拓扑关系和二阶积分形式的反馈线性化动态模型，设计有向切换拓扑下多auv编队队形一致性控制器；
[0012]
步骤四、基于步骤三中的控制器，得到每个auv的闭环控制形式。
[0013]
本发明的有益效果是：
[0014]
本发明在有向切换拓扑和变换编队队形情况下，通过反馈线性化方法将auv运动学模型和动力学模型转化为二阶积分器形式，并在控制算法中加入水下航行器之间的相对位置信息和速度阻尼器，形成编队队形控制律。仿真结果表明，多auv编队收敛速度提高，可以形成稳定的编队队形的速度更快，在切换拓扑和队形变换的情况下，控制器可以保持很好的稳定性和鲁棒性，多auv系统可以收敛到期望位置。
附图说明
[0015]
图1为auv固定坐标系及运动坐标系的示意图；
[0016]
图中，i为固定坐标系，e为海平面或海里某一点，即固定坐标系的原点，ξ,η,ζ为固定坐标系下的三个坐标轴，ξ轴的坐标表示auv北向位置，η轴的坐标表示auv东向位置，ζ轴
的坐标表示auv纵向位置，ψ为艏向角，θ为纵倾角，为横倾角，b为运动坐标系，o为auv重心，即运动坐标系的原点，x,y,z为运动坐标系下的三个坐标轴，x轴的正方向为由原点o指向auv艏部的方向，y轴的正方向为由原点o指向auv右舷的方向，z轴的正方向为auv垂直向下方向，w为纵向速度，p为横倾角速度，u为北向速度，v为东向速度；
[0017]
图2a为多auv系统通信拓扑图一；
[0018]
图2b为多auv系统通信拓扑图二；
[0019]
图2c为多auv系统通信拓扑图三；
[0020]
图2d为多auv系统通信拓扑图四；
[0021]
图3a为位姿切换拓扑时序图；
[0022]
图3b为速度切换拓扑时序图；
[0023]
图4为多auv编队队形变换图；
[0024]
图5为切换拓扑下多auv的纵倾角曲线图；
[0025]
图6为切换拓扑下多auv的艏向角曲线图；
[0026]
图7为切换拓扑下多auv的北向速度曲线图；
[0027]
图8为切换拓扑下多auv的东向速度曲线图；
[0028]
图9为切换拓扑下多auv的纵向速度曲线图；
[0029]
图10为切换拓扑下多auv的纵倾角速度曲线图；
[0030]
图11为切换拓扑下多auv的艏向角速度曲线图。
具体实施方式
[0031]
具体实施方式一、本实施方式所述的有向切换拓扑下多auv编队队形一致性控制方法，所述方法具体包括以下步骤：
[0032]
步骤一、定义用于描述多auv之间通信拓扑关系的有向图；
[0033]
步骤二、基于反馈线性化方法，将auv的运动学方程和动力学方程线性化为二阶积分形式的反馈线性化动态模型；
[0034]
步骤三、利用有向图表示的auv之间通信拓扑关系和二阶积分形式的反馈线性化动态模型，设计有向切换拓扑下多auv编队队形一致性控制器；
[0035]
步骤四、基于步骤三中的控制器，得到每个auv的闭环控制形式。
[0036]
具体实施方式二、本实施方式与具体实施方式一不同的是：所述步骤一的具体过程为：
[0037]
将每个auv作为有向图的一个顶点，有向图记为g＝(v,ε,c)，其中，v代表全部顶点的集合，v＝{v1,v2,
…
,v
n
}，n为编队内auv的个数，(v
i
,v
j
)∈ε，ε代表顶点之间边的集合，也就是各auv成员通信链的集合，(v
i
,v
j
)表示由顶点v
i
和v
j
构成的有向边，顶点v
j
接收顶点v
i
的信息，v
i
称为边的父顶点，v
j
称为边的子顶点，c为邻接矩阵；
[0038]
邻接矩阵c满足：对于若i＝j，则c
ij
＝0，若i
≠j且集合ε中存在有向边(v
i
,v
j
)，则c
ij
＝1，否则c
ij
＝0。
[0039]
有向图的入度矩阵d＝diag(deg(v1),deg(v2),
…
,deg(v
n
))，如果矩阵l满足l＝d
‑
c，那么称矩阵l为有向图g＝(v,ε,a)的laplacian矩阵(拉普拉斯矩阵)。
[0040]
其它步骤及参数与具体实施方式一相同。
[0041]
具体实施方式三、本实施方式与具体实施方式一或二之一不同的是：所述auv的运动学方程为：
[0042][0043]
式中，为η的一阶导数，表示位置状态和欧拉角状态，ξ0为auv的北向坐标，η0为auv的东向坐标，ζ0为auv的纵向坐标，θ为auv的纵倾角，ψ为auv的艏向角，代表实数域，表示速度的状态，u0为auv的北向速度，v0为auv的东向速度，w0为auv的纵向速度，q为auv的纵倾角速度，r为auv的艏向角速度，j(η)为auv从运动坐标系到固定坐标系下的雅可比矩阵。
[0044]
其它步骤及参数与具体实施方式一或二相同。
[0045]
具体实施方式四、本实施方式与具体实施方式一至三之一不同的是：所述auv的动力学方程为：
[0046][0047]
式中，为υ的一阶导数，非奇异矩阵m＝m
r
m
a
，m
r
为惯性矩阵，m
a
为附加惯性矩阵，τ0表示控制输入力和力矩，c(υ)＝c
r
(v) c
a
(v)，c
r
(v)表示刚体哥氏力和向心力矩阵，c
a
(v)是由于流体的附加质量所产生的力矩，d(υ)为总的水动力阻尼，g(η)为重力和浮力所产生的恢复力和力矩。
[0048]
其它步骤及参数与具体实施方式一至三之一相同。
[0049]
具体实施方式五、本实施方式与具体实施方式一至四之一不同的是：所述二阶积分形式的反馈线性化动态模型为：
[0050][0051]
其中，x
i
(t)和v
i
(t)分别表示第i个auv在t时刻的位置和速度，(t)分别表示第i个auv在t时刻的位置和速度，为v
i
(t)的一阶导数，是第i个auv在t时刻的控制输入，i＝1,2,
…
n，为x
i
(t)的一阶导数。
[0052]
其它步骤及参数与具体实施方式一至四之一相同。
[0053]
将auv的运动学方程和动力学方程线性化为二阶积分形式的反馈线性化动态模型时采用的是现有方法，其具体过程如下：
[0054]
由于横摇对平动的影响不大，本发明忽略横摇速度。从而得到了运动学和动力学的非线性耦合方程：
[0055][0056]
其中，m＝m
r
m
a
包括了惯性矩阵和附加惯性矩阵，并且m一定是非奇异矩阵。τ表示
控制输入力和力矩。将模型中c(υ)υ,d(υ)υ,g(η)三项合并成一个列向量w(η,υ)υ表示惯性质量及附加惯性质量之外的部分。
[0057][0058]
为方便反馈线性化过程，将上式写成下面的形式，
[0059][0060]
将航行器的位姿状态信息和速度状态信息构成向量ξ＝[η
t
,υ
t
]
t
。
[0061]
将式(25)表示成
[0062][0063]
其中，其中，其中，表示控制输入向量,
[0064]
其中：
[0065][0066]
为了简化表示取σ1和σ2，
[0067][0068][0069][0070]
那么，定义一个输出函数ζ＝h(ξ)后，与式(26)一起，将auv系统动力学模型表示为一般非线性系统。
[0071][0072]
为了更清楚地理解反馈线性化过程，现将反馈线性化中需要的数学基础进行简要
介绍。
[0073]
【向量场】：考虑单输入—单输出系统
[0074][0075]
其中，f,g和h在定义域上足够光滑。映射和称为d上的向量场。导数
[0076]
【lie导数】：导数为
[0077][0078]
其中称为h关于f或沿f的lie导数，这种表示方法类似于h沿系统轨迹的导数。当重复计算关于同一向量场或一新向量场的导数时，这种新表示法较为方便。例如，要用到以下表示：
[0079][0080][0081]
如果l
g
h(x)＝0，则与u无关。则继续计算y的二阶导数，那么对于有y的二阶导数有：
[0082][0083]
【相对阶】如果对于所有有并且也就是可表示为如下形式：
[0084][0085]
时称非线性系统在区域上具有相对阶ρ，1≤ρ≤n。
[0086]
【mimo相对阶】给出如下的多输入多输出系统：
[0087][0088]
其中：为auv编队的状态向量，为auv编队的输出向量，为auv编队的控制输入向量，f(x
i
),g(x
i
)为n维向量场，且g(x)＝[g1(x),
…
,g
m
(x)]，h(x)＝[h1(x),
…
,h
m
(x)]
t
。
[0089]
若上述mimo系统存在相对阶，则须满足下述条件：
[0090]
[0091]
对于任意1≤i≤m，至少存在一个j满足且存在m
×
m维矩阵
[0092][0093]
γ(x)是非奇异矩阵，多输入多输出系统的相对阶向量为其中每一个ρ
i
代表相对于h
i
(x)的子相对阶。
[0094]
那么，针对非线性耦合的潜航器数学模型，精确反馈线性化的方法思想是：寻找一个反馈控制率u以及坐标变换。对于式(31)的一般非线性表示的潜航器数学模型，可取
[0095]
h(x)＝[h1(x),h2(x),h3(x),h4(x),h5(x)]
t
＝[x,y,z,θ,ψ]
t
ꢀꢀꢀꢀꢀꢀꢀꢀ
(41)
[0096]
从auv的运动学模型和动力学模型中可获取f(x),g(x)，控制输入仍是三个轴方向的推进器的推力和两个舵的舵角。
[0097][0098]
其中f(x)的表示形式如下：
[0099][0100]
根据lie导数的定义，可得式(31)的一般非线性系统形式auv模型的lie导数，
[0101][0102]
由h(x)的定义可以得到偏微分方程如下：
[0103][0104]
根据g
i
(x)可知，它的前五行元素都为0，所以对于任意1≤i≤5，1≤j≤5，
[0105]
l
gi
h
j
(x)＝0
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(46)
[0106]
求得h(x)沿f(x)求取李导数有如下结果：
[0107][0108]
同理，根据二阶lie导数的定义(36)可导出，
[0109][0110]
设由γ
ij
(x)＝l
gj
l
f
h
i
(x)，1≤i≤5，1≤j≤5构成的矩阵可表示为
[0111][0112]
其中，m
*
g
*
＝m
33
g
′
34
m
33
g
′
44
，m
′
g
′
＝m
22
g
′
24
m
25
g
′
55
。
[0113]
因此auv系统存在相对阶：ρ1＝2,ρ2＝2,ρ3＝2,ρ4＝2,ρ5＝2，且相对阶ρ1 ρ2 ρ3 ρ4 ρ5＝10和系统阶数相同。
[0114]
取坐标变换：
[0115][0116]
由lie导数的定义可知，
[0117][0118]
[0119]
在新的坐标变换下，控制输入u可以表示为
[0120][0121]
可得
[0122][0123]
由此可得该坐标系统下的线性二阶积分模型，即在坐标变换(50)以及反馈输入(55)下可得
[0124][0125][0126]
上面的精确反馈线性化过程是针对潜航器单体。由于本发明研究的潜航器编队是建立在同构的基础上，所以编队中全部潜航器具有相同的水动力方程及运动学方程。所以编队中其他成员可以进行相同的反馈线性化。那么针对编队第i个潜航器个体，可以有
[0127][0128][0129]
其中，
[0130]
具体实施方式六、本实施方式与具体实施方式一至五之一不同的是：所述有向切换拓扑下多auv编队队形一致性控制器的形式为：
[0131][0132]
其中：u(t)为控制器，v
d
(t)是t时刻的阻尼速度，即航行器形成编队后的行驶速度，k0，k1，k2是控制增益，n
i
为父顶点v
i
的子顶点的集合，τ是auv之间的通信延迟，x
j
(t
‑
τ)表示第j个auv在t
‑
τ时刻的位置，v
j
(t
‑
τ)表示第j个auv在t
‑
τ时刻的速度，表示第i个auv相对于第j个auv的期望位置，表示第j个auv相对于第i个auv的期望位置，a
ij
表示位置耦合项的邻接矩阵，b
ij
表示速度耦合项的邻接矩阵。
[0133]
本实施方式设计一种分布式多航行器系统，只需知道局部航行器之间的相对位置和速度信息，就可以实现全局的协调一致。控制律由速度阻尼项，航行器位置耦合项和速度耦合项组成，当v
d
(t)＝0时，水下航行器形成静态编队；当v
d
(t)≠0时，整个编队以速度v
d
(t)前进。为了实现稳定的编队，水下航行器达到相同的速度，为了使其保持一定的队形，要求与相邻的水下航行器保持预先设定的位置。
[0134]
位置耦合项的邻接矩阵a
ij
和速度耦合项的邻接矩阵b
ij
均根据邻接矩阵c的定义方式获得。
[0135]
其它步骤及参数与具体实施方式一至五之一相同。
[0136]
具体实施方式七、本实施方式与具体实施方式一至六之一不同的是：所述步骤四的具体过程为：
[0137][0138]
本实施方式表明，水下航行器能否形成期望队形、收敛至期望速度，不仅与多auv之间通信拓扑相关，还取决于反馈增益系数和时延大小。
[0139]
定义：对于多auv系统的任意初始状态，给定闭环控制形式在一个有限时间的区间内t0∈[0, ∞)，使得
[0140][0141]
当t＞t0时，有x
j
(t)
‑
h
j
＝x
i
(t)
‑
h
i
，v
j
(t)＝v
i
(t)，i,j＝1,2,
…
n,则称多auv系统可以在有限时间里达成一致。
[0142]
其它步骤及参数与具体实施方式一至六之一相同。
[0143]
1、控制算法稳定性分析
[0144]
1.1模型简化
[0145]
对一个时延系统进行稳定性分析，需要采用泛函微分方程，本发明通过构造合适的lyapunov
‑
krasovskii函数沿着其求导，再通过适当放缩，得出使得含时延系统实现一致性的充分条件。
[0146]
证明：
[0147]
令则式(1)有
[0148][0149]
a
ij
为系统邻接矩阵，令将式(3)写成紧凑形式
[0150][0151]
存在正交矩阵u，使u
t
au＝i
n
‑
λ，u
t
bu＝i
n
‑
λ，其中u是laplacian矩阵l的特征向量组成的矩阵，λ＝diag(λ1,λ2,
…
,λ
n
)，考虑坐标变换则式(8)可以写成
[0152][0153]
上式经过线性变换表示为下式，
[0154][0155]
令，
[0156][0157][0158]
将上式简写为：
[0159][0160]
1.2稳定性分析
[0161]
定义lyapunov
‑
krasovskii函数为：
[0162][0163]
对v(t)沿着系统求导得：
[0164][0165]
将系统方程代入上式得
[0166][0167]
由于且对于任意的x,y∈r
n
和正定对称矩阵∑∈r
n
×
n
，有：
[0168]
±
2x
t
y≤x
t
∑
‑1x y
t
∑y
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0169]
所以：
[0170][0171]
式(18)可以写成：
[0172][0173]
令
[0174][0175]
可将改写为矩阵形式：
[0176][0177]
令λ1＝(e f)
t
p p(e f) τe
t
re q，并由引理1可得：
[0178][0179]
如果m＜0，那么根据lyapunov稳定性判据可知，如果那么误差系统全局渐近稳定，时延编队控制算法可以保持队形并使速度达到一致。
[0180]
引理1：(schur补)给定对称矩阵其中a∈r
r
×
r
，则下面条件等价：
[0181]
1).s是正定的；
[0182]
2).当且仅当c和a
‑
bc
‑1b
t
都是正定的或者a和c
‑
b
t
a
‑1b均是正定的。
[0183]
2、仿真结果
[0184]
在本发明中，给出了数值仿真例子来验证编队队形控制算法的有效性，本发明考虑4个auv组成的多auv系统，其有向通信拓扑结构如图2a，图2b，图2c，图2d所示。
[0185]
多auv编队所示通信拓扑结构的邻接矩阵c可表示为：
[0186][0187][0188]
下面对水下航行器编队过程进行仿真分析，一致性控制算法中参数设置为k0＝0.4,k1＝0.1,k2＝0.2,水下航行器的期望速度设为v
d
＝0.2，常数时延τ＝0.1s，编队队形矩阵h设置为
[0189][0190]
将一致性控制算法中位置和速度的通信拓扑在图2a至图2d中4个通信拓扑状态下进行随机变化，位置和速度的通信拓扑变化时序图如图3a和图3b所示，多auv系统编队队形变化如图4所示，多auv系统的纵倾角和艏向角变化如图5和图6所示，多auv系统的北向速度变化如图7所示，东向速度变化如图8所示，纵向速度如图9所示，纵倾角速度如图10所示，艏向角速度如图11所示。
[0191]
随机布放4个水下航行器，多auv编队在不同位置开始形成编队，并在300s时形成稳定编队队形，保持编队一段时间后，在1000s时进行编队队形变换。通过仿真验证多auv编队在切换拓扑的情况下可以快速形成稳定的编队，并在编队队形变化时，能够快速改变编队队形，并保持稳定队形，证明了控制算法的稳定性和鲁棒性。
[0192]
在图7、图8和图9的仿真结果中可以大体看出，auv1，auv2，auv3，auv4组成的多水下航行器系统，在切换拓扑条件下和编队进行变换时，水下航行器的北向速度，东向速度和纵向速度在动态变化的情况下可以快速达成一致，并保持和变化编队队形。
[0193]
在图10和图11的仿真结果中可以大体看出，auv1，auv2，auv3，auv4组成的水下航行器系统，在通信拓扑动态变化的情况下，多auv系统形成稳定编队后，纵倾角速度和艏向角速度保持稳定，在编队队形变化时也可保持稳定。
[0194]
本发明提出了一种带有速度阻尼器的一致性编队队形控制算法，利用lyapunov
‑
krasovskii函数，以及积分不等式和矩阵不等式数学方法证明，得出了使多auv编队系统可以实现一致的充分条件，并验证了在实际环境中可能出现的有向通信拓扑发生变化和编队队形发生变化下本发明控制方法的有效性和稳定性。
[0195]
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明
的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。