本发明涉及的是一种水下无人航行器集群的水平面路径跟踪控制方法。
背景技术：
uuv(水下无人航行器)是一种完成水下资源勘探、物体打捞以及环境监测等水下任务的重要工具，由于其成本低、体积小等特点，uuv通过导航装置测量的数据可以判断自身的位置信息和通过多普勒测速仪和陀螺仪判断速度和姿态等自身信息，来完成对环境的感知并通过控制算法实现水下任务。但是单个uuv可以感知的范围有限和数量的限制，不能完成大面积的勘探和搜索等任务，也不能完成目标围捕等复杂任务。多uuv集群的路径跟踪能力就是实现上述任务的重要技术前提。多uuv集群中的各航行器通过声呐进行信息的交换，获得其他航行器的位置以及速度信息后，利用一致性控制器实现设定的队形和航行轨迹。但是在复杂的海洋环境中，声呐传播的信息会有延时和丢包的情况发生，uuv集群中的航行器不能保证实时通讯的条件，因此需要确保在通讯条件不好的情况下多uuv集群依然保持队形并完成规定任务。
针对uuv集群的路径跟踪控制，较多路径跟踪控制方法仅针对连续时间的条件，如潘大伟在文章《人工势场和虚拟结构相结合的多水下机器人编队控制》(发表于2017，工兵学报，第2期)，以及赵宁宁在文章《基于路径参数一致性的多auv协同路径跟踪控制》发表于2015，火力与指导控制，第10期)中所提出的编队方法均可以使多航行器编队后可以跟踪期望路径。但是深海中的uuv间通讯传播只能利用声纳，且其他传感器包括加速器等数据都是离散的信息数据。
技术实现要素：
本发明的目的在于提供一种能够有效保证在通讯存在丢包的情况下多uuv集群依然保持队形并完成水平面路径跟踪任务的考虑通讯丢包的uuv集群水平面路径跟踪控制方法。
本发明的目的是这样实现的：
(1)利用反馈线性化将uuv的三自由度非线性模型简化为二阶仿射系统；
(2)利用直接离散化的方法将uuv连续动态方程转换为离散形式；
(3)设计考虑通讯丢包的uuv集群水平面路径跟踪控制器；
(4)利用所述控制器求得uuv集群中每个uuv的位置、姿态和速度；
(5)将信息传输到通讯拓扑图中规定的uuv中，把得到位置、姿态和速度带入每个uuv的动态方程中，计算出每个执行机构的驱动力和驱动力矩的数值；
(6)将控制量输入到航行器执行机构所对应的驱动装置，实现多uuv集群保持队形并跟踪期望路径。
本发明还可以包括：
1.步骤(1)具体包括：所建立的三自由度非线性模型为：
其中向量
将uuv的动态方程表示为二阶仿射形式：
μ＝r(ξ)。
2.步骤(2)具体包括：所述离散模型为：
vi[k+1]＝vi[k]+(t-τij(k))ui[k]
变换坐标为：
x＝[r1(ξ)r2(ξ)r3(ξ)]
v＝[lpr1(ξ)lpr2(ξ)lpr3(ξ)]
输入为ui＝τ(ξ)+γ(ξ)uτ，其中τ(ξ)如下：
3.所述的通讯丢包的通讯延时无界情况为τij(k)依概率p满足t-τij(k)＞τ0，假设多uuv系统在nq个周期内的通讯结构图的并图一定有一个生成树。p表示通信成功的概率，存在一个正整数ns，满足1≤ns≤nq，只在周期[k+(ns-1),k+ns)内uuvi成功接收到uuvj。
4.所述的水平面路径跟踪控制器为：
xd(k)为期望路径即虚拟领航者的航行轨迹，vd(k)为虚拟领航者的速度值，si为uuv集群编队的初始位置坐标，sd为虚拟领航者的初始位置坐标，β1(k)＝(2/(t-τij(k))2)α1(k)和β2(k)＝(2/(t-τij(k))2)α2(k)，α1、α2是控制器增益，aij(k)、bij(k)分别是uuv的位置和速度拓扑图所对应邻接矩阵中元素，
5.步骤(5)具体包括：得到位置和速度带入每个uuv的动态方程中，计算出每个执行机构的驱动力和驱动力矩的数值为：
uτi(k)＝γ-1(xi(k))[ui(k)-t(xi(k))]。
为了解决在离散信息条件下uuv集群的水平面路径跟踪控制问题，本发明提出了一种考虑通讯丢包的uuv集群水平面路径跟踪控制方法，尤其是适用于水下无人航行器在离散信息条件下通讯存在丢包情况的uuv集群的水平面路径跟踪控制方法。
本发明的方法可以有效解决水下无人航行器在离散信息条件下的集群水平面空间路径跟踪控制问题。考虑了水下无人航行器传输信息存在环境干扰和通讯距离影响的特点，设计了考虑通讯丢包的uuv集群水平面路径跟踪控制方法，求得每个uuv的位置、姿态和速度后，将得到位置姿态和速度带入每个uuv的控制器中计算出每个执行机构的驱动力和驱动力矩的数值，再将控制量输入到航行器执行机构所对应的驱动装置，可以保证多uuv集群保持队形并跟踪期望路径。
附图说明
图1是离散信息条件下考虑通讯丢包的uuv集群水平面路径跟踪控制方法流程示意图；
图2是uuv集群各航行器的通讯拓扑关系图；
图3是uuv集群在路径跟踪过程中各个成员的东向方向的位置变化情况图；
图4是uuv集群在路径跟踪过程中各个成员的北向方向的位置变化情况图；
图5是uuv集群在路径跟踪过程中各个成员的首摇角的角度变化情况图；
图6是uuv集群在路径跟踪过程中各个成员的纵荡速度变化情况图；
图7是uuv集群在路径跟踪过程中各个成员的横荡速度变化情况图；
图8是uuv集群在路径跟踪过程中各个成员的航向角速度变化情况图；
图9是uuv集群水平面航行轨迹图。
具体实施方式
在实际的海洋环境中，多uuv在水下进行信息交换时一般都会受到环境的干扰，因此控制航行器运动的输入会受到延时的影响。但是uuv的信息采集都是基于离散时间的信息，因此一些针对连续时间的控制算法不能得到很好的应用。因此针对在离散条件下，并考虑存在通讯延时和丢包的多uuv的一致性研究就具有现实意义。本发明提供的一种考虑通讯丢包的uuv集群水平面路径跟踪控制方法，主要步骤包括：首先利用反馈线性化将uuv的三自由度非线性模型进行简化为二阶仿射系统；其次利用直接离散化得方法将uuv连续动态方程表达成离散形式。可以采用位置信息与速度信息独立传输并给出通讯的拓扑结构图，最大限度的减少单次通信信息量。然后设计考虑通讯丢包的uuv集群水平面路径跟踪控制方法，并利用设计的控制器求得uuv集群中每个uuv的位置和速度；再将信息传输到通讯拓扑图中规定的uuv中，把得到位置和速度带入每个uuv的动态方程中，计算出每个执行机构的驱动力和驱动力矩的数值；最后将控制量输入到航行器执行机构所对应的驱动装置，实现多uuv集群保持队形并跟踪期望路径。
优选地，所述建立的三自由度非线性模型为：
其中向量
将uuv的动态方程表示为二阶仿射形式：
μ＝r(ξ)
优选地，所述离散模型为：
vi[k+1]＝vi[k]+(t-τij(k))ui[k]
变换坐标为：
x＝[r1(ξ)r2(ξ)r3(ξ)]
v＝[lpr1(ξ)lpr2(ξ)lpr3(ξ)]
输入为ui＝τ(ξ)+γ(ξ)uτ，其中τ(ξ)如下：
优选地，所述通讯丢包情况为τij(k)依概率p满足t-τij(k)＞τ0，p表示通信成功的概率。存在一个正整数ns，满足1≤ns≤nq，只在周期[k+(ns-1),k+ns)内uuvi成功接收到uuvj。
优选地，所述的考虑通讯丢包的uuv集群水平面路径跟踪控制方法，其特征在于，基于离散时间下的数学模型设计的协调控制协议求得协调后uuv集群中各航行器的姿态和速度均收敛一致，所提出的水平面路径跟踪控制器为：
xd(k)为期望路径，即虚拟领航者的航行轨迹，vd(k)为虚拟领航者的速度值。si为uuv集群编队的初始位置坐标，sd为虚拟领航者的初始位置坐标。β1(k)＝(2/(t-τij(k))2)α1(k)andβ2(k)＝(2/(t-τij(k))2)α2(k)。α1，α2是控制器增益，aij(k)，bij(k)分别是uuv的位置和速度拓扑图所对应邻接矩阵中元素，
优选地，所述得到位置和速度带入每个uuv的动态方程中，计算出每个执行机构的驱动力和驱动力矩的数值为：
uτi(k)＝γ-1(xi(k))[ui(k)-t(xi(k))]
优选地，所述的考虑通讯丢包的uuv集群水平面路径跟踪控制方法，其特征在于，所述考虑通讯丢包的uuv集群水平面路径控制方法求得uuv集群中各航行器的计算出每个执行机构的驱动力和驱动力矩的数值，并将控制量输入到航行器执行机构所对应的驱动装置。
下面举例对本发明做更详细的描述。
(1)将uuv的非线性耦合连续数学模型转化为二阶仿射形式：
uuv的非线性耦合连续数学模型如下：
其中向量
矩阵m,c(v)和d(v)分别代表惯性矩阵，科氏向心矩阵和阻尼矩阵。g(η)是代表力与力矩，其由重力和浮力一同作用所引起的。τ是uuv力与力矩的输入值。
本发明假设uuv模型的结构在水平面和纵平面是对称的。因此惯性矩阵m是对称矩阵，科氏向心矩阵c(v)是反对称矩阵。为简化计算，将重心和浮心设计为重合状态，所以g(η)可以忽略。
其中,ρ为水密度，l为船长，y.，x.，z.，m.，n.为水动力系数。
其中,m为uuv质量。
d(v)＝-diag{xu,yv,nr}(5)
将公式(1)中的动力学方程改写为：
其中，uτ＝[tu,tv,tw,δs,δr]是表示力与方向舵角，γ(ξ)为矩阵，表达如下：
uuv的连续数学模型可以写为：
将uuv的非线性模型表示为二阶仿射模型：
其中，ξ＝[ητ,vτ]τ，
r(ξ)＝η。
(2)利用状态反馈线性化方法将uuv的非线性耦合连续数学模型转化为二阶积分形式。结合公式(9)中的qij(ξ)，并利用李导数的性质，可以求出矩阵γ(ξ)，表示如下：
根据公式(11)可以计算出具体的qij(ξ)，得出γ(ξ)为非奇异矩阵，因此系统的相对阶为：
ρ1＝2,ρ2＝2,ρ3＝2(11)
因此，该系统的相对阶之和为ρ1+ρ2+ρ3＝6，与系统维数相同，可知uuv的非线性系统可以进行反馈线性化。取变换坐标为：
设新系统的输入为ui＝τ(ξ)+γ(ξ)uτ，其中τ(ξ)如下：
结合公式(14)(15)，可以得出uuv的标准二阶积分形式的反馈线性化动态模型：
其中，
(3)由于uuv之间传输的信息数据均为固定采样时间的离散数据，因此在考虑通讯存在延时情况下利用直接离散化的方法将连续的数学模型转化为离散数学模型：
其中，，i＝1,2,...,n，k代表离散时间指数，t代表采样周期，τij(k)为通讯延时时间，且有界，即t-τij(k)＞τ0，τ0为正常数。
(4)通讯延时无界情况为τij(k)依概率p满足t-τij(k)＞τ0，p表示通信成功的概率。存在一个正整数ns，满足1≤ns≤nq，只在周期[k+(ns-1),k+ns)内uuvi成功接收到uuvj。
(5)基于离散时间下的数学模型设计的协调控制协议求得协调后uuv集群中各航行器的姿态和速度均收敛一致，所提出的水平面路径跟踪控制器为：
xd(k)为期望路径，即虚拟领航者的航行轨迹，vd(k)为虚拟领航者的速度值。si为uuv集群编队的初始位置坐标，sd为虚拟领航者的初始位置坐标。β1(k)＝(2/(t-τij(k))2)α1(k)andβ2(k)＝(2/(t-τij(k))2)α2(k)。α1，α2是控制器增益，aij(k)，bij(k)分别是uuv的位置和速度拓扑图所对应邻接矩阵中元素，
6.得到位置和速度带入每个uuv的动态方程中，计算出每个执行机构的驱动力和驱动力矩的大小为：
uτi(k)＝γ-1(xi(k))[ui(k)-t(xi(k))]
7.求得uuv集群中各航行器的计算出每个执行机构的驱动力和驱动力矩的数值，并将控制量输入到航行器执行机构所对应的驱动装置.
下面利用matlab仿真软件以证明本发明的有效性。
在matlab仿真软件中建立水下无人航行器的非线性模型，并将其转化为二阶仿射系统。设定初始条件，以及期望轨迹，进行试验仿真。
matlab仿真条件设置如下：
设uuv集群中航行器初位置xi(0)，yi(0)任意分布在[0,0]区间，ψi(0)＝0，初始速度向量均为0，虚拟领航者的初速度u＝1m/s,v＝0m/s。多uuv集群的每个航行器的位置坐标分别为：δ1＝[0,0]，δ2＝[10,0]，δ3＝[-10,0]，δ4＝[20,0]，δ1＝[-20,0]，位置和速度的控制器增益分别为α1＝0.3，α2＝0.6。选取控制周期为t＝0.5s，τ0＝0.2，通讯成功的概率为0.8，置信系数为0.999。仿真水平面路径跟踪曲线如下：
仿真结果：
基于上述仿真条件的设置，以及通过matlab仿真软件对所发明方法的编写，经过仿真可以得到如图3到图9的仿真实验结果，其中uuvv表示虚拟领航者，uuv1-5为多uuv集群中的5个航行器。
根据图3至图4的仿真图说明，本发明所提出的在离散信息条件下通讯丢包的uuv集群水平面路径跟踪控制方法可以保证各uuv在x,y轴方形运动跟随期望路径并均一致收敛；根据图5的仿真图说明，多uuv集群中航行器的航向角均能收敛于虚拟领航者的航向角。根据图6至图7的仿真图说明，各个uuv的速度状态均一致收敛；根据图8的仿真图说明，多uuv集群中航行器的航向角速度均能收敛于虚拟领航者的航向角速度。根据图9的仿真图说明，多uuv集群可以保持平行的队形并按期望路径航行。可以看出，本发明所提出的在离散采样信息条件下通讯丢包的uuv集群水平面路径跟踪控制方法能够有效的使个uuv保持队形并能很好的跟踪预先设定的期望路径。