CN110989639B - 一种基于应力矩阵的水下航行器编队控制方法 - Google Patents

Abstract

一种基于应力矩阵的水下航行器编队控制方法，包括若干水下航行器；根据编队中水下航行器的数量设计标称队形，使得标称队形具有一般性，设置1‑4号水下航行器作为领航者，其余作为跟随者；对标称队形中各水下航行器的位置坐标进行重组，设计矩阵Q；通过求解半正定规划问题，得到标称队形的应力矩阵Ω；在编队控制开始后，利用单体轨迹跟踪算法设计1号领航者轨迹跟踪控制器，其余水下航行器利用相邻水下航行器的状态信息计算各自编队控制律，并通过调整2‑4号领航者编队控制器中仿射变换矩阵A取值，实现水下三维环境中的编队跟踪、编队朝向控制和队形变换控制；控制方法仅需少量个体利用仿射变换矩阵控制队形结构变换方式，降低了群集内部的数据通信量。

Description

一种基于应力矩阵的水下航行器编队控制方法

技术领域

本发明属于机器人智能控制领域，具体涉及一种水下机器人编队控制方法。

背景技术

仿生机器鱼是一种基于仿生学原理模拟海洋鱼类游动的新型水下仿生机器人。与传统的水下潜航器利用螺旋桨推进的方式不同，仿生机器鱼通过模拟鱼类尾和鳍的协调配合实现了水下高效运动。这种运动方式将推进器和舵巧妙融合起来，具有高机动性和低扰动性等特点，非常适合在水下复杂和狭窄的环境中进行海洋勘探、环境监测、管道巡检和海洋生物调查等工作。受载荷和续航的限制，单条机器鱼的感知能力有限，所以在水下协同搜索过程中，需要利用多条机器鱼自主协调形成编队实现感知区域的扩展。在编队游动过程中，由于水下环境的复杂性和不确定性，需根据环境约束和任务需求变化动态调整机器鱼群的编队构型。因此，可实现队形动态变换的编队控制方法是机器鱼群协同执行任务的重要保障。

现有的编队控制方法中队形调整的策略，主要是将缩放因子引入协调变量中，通过预先指定缩放因子的变化来调整期望队形的结构。虽然这种方法可以直接对现有编队控制方案进行扩展，简单易行，但其中缩放因子的变化率需提前指定，不利于队形的动态调整。另一种通过保持相邻智能体方位角来实现编队控制的方法，可以通过调整相邻智能体的距离来实现队形的缩放，但这种方法难以实现编队朝向的控制，无法满足任务执行过程中队形朝向控制的需求。近些年还出现的基于复拉普拉斯矩阵(complex Laplacian)的队形变换控制方法，可以利用复拉普拉斯矩阵对于编队线性变换的不变性，实现队形的灵活变换。但由于复拉普拉斯矩阵的限制，这种方法只能实现二维平面内的队形变换，无法应用于水下三维环境的任务场景。

发明内容

本发明的目的在于提供一种基于应力矩阵的水下航行器编队控制方法，能够解决水下三维环境中机器鱼群编队跟踪、编队朝向和队形动态调整的问题。

为实现上述目的，本发明提供如下技术方案：

一种基于应力矩阵的水下航行器编队控制方法，包括若干水下航行器；其特征在于：

步骤1)，根据编队中水下航行器的数量设计标称队形，使得标称队形具有一般性，设置部分水下航行器作为领航者，其余作为跟随者；

步骤2)，对标称队形中各水下航行器的位置坐标进行重组，设计矩阵Q；

步骤3)，通过求解半正定规划问题，得到标称队形的应力矩阵Ω；

步骤4)，在编队控制开始后，利用PID算法设计1号领航者的轨迹跟踪控制器，其余水下航行器利用相邻水下航行器的状态信息计算出各自的编队控制律；水下航行器任务结束前，循环执行对各水下航行器的编队控制。

本发明控制方法的优点：

1)、可以实现水下三维环境中的编队跟踪、编队朝向控制和队形变换控制，以往的编队控制技术要么只能实现二维平面内的队形变换控制，要么无法在三维空间中同时实现编队跟踪、编队朝向控制和队形变换控制；2)、以往大部分队形变换控制方法中需要群体中的所有个体都获取队形变换参数，而本发明所提出的队形变换控制方法中仅需少量(三维空间中为4个)个体利用仿射变换矩阵控制队形结构变换方式，其余个体无需获取队形变换参数，降低了群集内部的数据通信量。

附图说明

图1为编队控制算法流程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明主要提出了一种分布式的编队控制方法，通过设计编队标称队形，将应力矩阵引入编队控制器中，利用应力矩阵对于标称队形仿射变换的不变性，来实现水下三维环境中仿生机器鱼群的动态队形变换。当编队框架具有普遍刚性且标称队形具有一般性时，只需控制鱼群中少量个体作为领航者确定队形变换方式，其余个体作为跟随者，与相邻机器鱼保持平衡应力关系，即可实现编队跟踪、编队朝向控制和队形的灵活动态变换。

本发明的控制算法流程如图1所示。

步骤1)，根据鱼群中机器鱼的数量设计标称队形，使得标称队形具有一般性，其中领航者设置为1-4号。

首先假设鱼群中机器鱼数量为n(n…5)，各机器鱼的运动学模型为其中X_i＝[x_i,y_i,z_i]^T表示机器鱼i在水下三维环境中的位置；G_i＝[V_i,1cosθ_i,V_i,1sinθ_i,V_i,2]^T，其中V_i,1表示机器鱼i的水平运动速度，V_i,2表示机器鱼i的上浮或下潜速度，θ_i表示机器鱼i在水平面内的方向角；机器鱼i的控制量用U_i＝[V_i,1,θ_i,V_i,2]^T来表示。

由于机器鱼群的编队控制需要个体之间进行通信，所以可将机器鱼之间的通信拓扑表示为一个无向图G＝{V,E}。其中V＝{1,2,...,n}表示点集，表示边集。点集中令V_l＝{1,2,...,n_l}表示领航者集合，而V_f＝{n_l+1,...,n}表示跟随者的集合。由于通信拓扑为无向图，所以图G中的边(i,j)表示机器鱼i和机器鱼j可以各自收到对方传来的信息。机器鱼编队的框架可以表示为(G,p)，/>表示编队的队形，p_i表示队形中各机器鱼的位置，其中领航者群体和跟随者群体的位置可以分别表示为/>和若队形p中的各机器鱼位置坐标线性无关，则称此队形具有一般性。

队形p的仿射变换定义如下

从定义方式可以看出，仿射变换对加法和标量乘法都是封闭的，所以它是一种线性变换。仿射变换后，队形中节点之间共线或共面的特性不变，且相互平行的线段在仿射变换后依然平行。

设机器鱼群的编队框架(G,p)具有普遍刚性，且标称队形p具有一般性。在这些前提条件下，可以证明当标称队形中存在4条机器鱼的位置不共面时，无论标称队形进行何种仿射变换，鱼群中其他机器鱼的位置都可以通过这4条机器鱼的位置唯一确定。基于此结论，可以选取标称队形中位置不共面的4条机器鱼作为领航者，来确定整个鱼群的队形变换方式。为表示方便，这里将1-4号机器鱼作为领航者，其余作为跟随者。

其中，标称队形(nominal configuration)：表示鱼群进行队形变换的基准编队构型。

普遍刚性(universally rigid)：对于编队框架(G,p)，如果队形p在所有维空间中均为图G关于刚性运动(平移和旋转)的唯一实现，则称此编队框架具有普遍刚性。

步骤2)，对标称队形中各机器鱼的位置坐标进行重组，设计满足条件(8)的矩阵Q。

其中，将标称队形位置坐标p₁,…,p_n中每一维的坐标值提取出来重组为三个向量构造矩阵/>使其满足以下条件

Q1_n＝0,Qq₁＝0,Qq₂＝0,Qq₃＝0,QQ^T＝I₃ (8)

步骤3)，在上述步骤基础上求解半正定规划问题(10)，得到标称队形的应力矩阵Ω。

其中，令ω＝[ω₁,...,ω_m]^T表示图中包含所有m条边应力值的向量，那么应力矩阵还可以表示为以下形式

Ω＝Bdiag(ω)B^T (9)

其中，为图G的关联矩阵，diag(ω)表示对角线元素为应力值的m维对角阵。

其中，关联矩阵(incidence matrix)：将图G中每条边都进行编号并任意指定方向，然后关联矩阵可以定义为其中，|E|表示图中所有边的数量。

通过求解以下半正定规划问题，即可求得各边的应力值ω_ij。

其中，QΩQ^T＞λI₃表示QΩQ^T-λI₃正定，可以取任意正实数。

可以证明当机器鱼群的编队框架(G,p)具有普遍刚性时，此优化问题总有可行解。此半正定规划问题可以通过MOSEK、SEDUMI或SDPT3等求解器进行求解，由于各边的应力值在标称队形确定后固定不变，半正定规划问题只需在编队控制开始前计算一次即可，无需考虑求解此优化问题的实时性。

步骤4)，在编队控制开始后，利用PID等算法设计1号机器鱼的轨迹跟踪控制器，其余机器鱼利用相邻机器鱼的状态信息通过(3)(6)(7)式可计算各自的编队控制律。在需要进行队形变换时，只需鱼群中2-4号领航者调整其编队控制器中仿射变换矩阵A的取值，即可实现鱼群整体的编队队形变换。任务结束前，循环执行对各机器鱼的编队控制。

步骤4-1)，对于领航者1号机器鱼，可以利用PID等控制方法进行轨迹跟踪控制，其轨迹可以作为参考轨迹。

步骤4-2)，对于领航者2-4号机器鱼，先计算仿射变换矩阵A及其变化率。

步骤4-3)，对于1号机器鱼外的其他各机器鱼计算各自的编队控制律。

机器鱼i的期望位置可以表示为

其中，表示标称队形的旋转和形变等仿射变换操作，而/>表示队形的平移操作。根据机器鱼i期望位置可以定义如下形式的编队控制误差

其中，E_f＝{(i,j)|i∈V_f,j∈V}表示拓扑中与跟随者相连的边的集合；

E_l＝{(i,j)|i∈V_l,j∈V_l}表示领航者之间相连的边的集合；(i,j)∈E_l时，a_ij＝1，否则a_ij＝0；ω_ij表示与(i,j)边相关的应力值，对于一个稳定的编队构型，拓扑中各边对应的应力值应满足平衡应力关系

其中N_i表示机器鱼i相邻的机器鱼集合。以上平衡应力关系表示成矩阵形式为其中/>为应力矩阵，矩阵中各元素的定义为

对于式(3)，根据应力矩阵关于标称队形仿射变换的不变性，当鱼群的编队构型处于标称队形的仿射变换空间时，等号右边第一部分的值为0，进一步，当领航者处于期望编队位置时，等号右边第二部分的值为0，这时，鱼群中所有机器鱼都处于期望编队位置，即达到了编队控制的目标。根据式(3)，利用反演控制(backstepping control)思路可设计机器鱼i的控制量U_i满足以下等式

其中，

K_i为正定实对角阵，表示可调节的控制器参数。接着，根据式(6)得到的矩阵G_i(U_i)，通过和θ_i＝arctan(g_i,2/g_i,1)可以直接反解出实现编队控制和队形变换的控制量U_i。

结合(3)(6)(7)式可以看出，跟随者机器鱼的控制器中与矩阵A的有关的部分始终为0。所以在队形变换过程中各跟随者无需知道队形变换的参数，降低了机器鱼群体内部所需的通信量，在水下通信质量难以保证的客观前提下，提高了算法实施的可靠性。

当用户需要利用多水下机器鱼进行水下三维环境的协同搜索时，可以利用本发明提出的编队控制算法，以特定的编队构型实现对水下一定范围内的搜索区域覆盖，通过搜索路径的优化实现水下高效协同搜索。当通过狭窄水道时，可以通过领航者机器鱼调整仿射变换矩阵，将队形压缩为相对较窄的形态，跟随者机器鱼只需保持与邻居的平衡应力关系，即可实现整体队形结构的变化，使鱼群安全通过此狭窄区域。

本发明可以直接应用于水下仿生机器鱼的协同编队控制中，通过对算法模型的细微调整，也可以拓展应用于其他类型的水下航行器编队控制中。

最后应说明的是：以上所述仅为本发明的解释，并不用于限制本发明，尽管对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于应力矩阵的水下航行器编队控制方法，包括若干水下航行器；其特征在于：

步骤1)，根据编队中水下航行器的数量设计标称队形，使得标称队形具有一般性，设置部分水下航行器作为领航者，其余作为跟随者；

步骤2)，对标称队形中各水下航行器的位置坐标进行重组，设计矩阵Q；

步骤3)，通过求解半正定规划问题，得到标称队形的应力矩阵Ω；

步骤4)，在编队控制开始后，利用PID方法设计1号领航者水下航行器的轨迹跟踪控制器，其余水下航行器利用相邻水下航行器的状态信息计算出各自的编队控制律；水下航行器任务结束前，循环执行对各水下航行器的编队控制；其中，所述水下航行器为仿生机器鱼；

步骤4)中，

步骤4-1)，对于领航者1号机器鱼，利用轨迹规划算法设计1号机器鱼的轨迹跟踪控制器，进行单体轨迹跟踪控制，其轨迹作为参考轨迹；

步骤4-2)，对于领航者2-4号机器鱼，先计算仿射变换矩阵A及其变化率；

步骤4-3)，对于1号机器鱼外的其他各机器鱼计算各自的编队控制律；

步骤4-3)中，

机器鱼i的期望位置可以表示为

其中，表示标称队形的旋转和形变等仿射变换操作，而/>表示队形的平移操作；根据机器鱼i期望位置可以定义如下形式的编队控制误差

其中，E_f＝{(i,j)|i∈V_f,j∈V}表示拓扑中与跟随者相连的边的集合；E_l＝{(i,j)|i∈V_l,j∈V_l}表示领航者之间相连的边的集合；(i,j)∈E_l时，a_ij＝1，否则a_ij＝0；ω_ij表示与(i,j)边相关的应力值，对于一个稳定的编队构型，拓扑中各边对应的应力值应满足平衡应力关系

其中N_i表示机器鱼i相邻的机器鱼集合；以上平衡应力关系表示成矩阵形式为其中/>为应力矩阵，矩阵中各元素的定义为

对于式(3)，根据应力矩阵关于标称队形仿射变换的不变性，当鱼群的编队构型处于标称队形的仿射变换空间时，等号右边第一部分的值为0，进一步，当领航者处于期望编队位置时，等号右边第二部分的值为0，这时，鱼群中所有机器鱼都处于期望编队位置，即达到了编队控制的目标；根据式(3)，利用反演控制思路可设计机器鱼i的控制量U_i满足以下等式

其中，

K_i为正定实对角阵，表示可调节的控制器参数；接着，根据式(6)得到的矩阵G_i(U_i)，通过和θ_i＝arctan(g_i,2/g_i,1)可以直接反解出实现编队控制和队形变换的控制量U_i。

2.根据权利要求1所述的基于应力矩阵的水下航行器编队控制方法，其特征在于，步骤4)中，在需要进行队形变换时，只需领航者调整其编队控制器中仿射变换矩阵A的取值，即可实现整体的编队队形变换。

3.根据权利要求1所述的基于应力矩阵的水下航行器编队控制方法，其特征在于，作为领航者的水下航行器采用4个，编号设为1-4号。

4.根据权利要求1所述的基于应力矩阵的水下航行器编队控制方法，其特征在于，步骤1)中，首先假设鱼群中机器鱼数量为n(n…5)，各机器鱼的运动学模型为其中X_i＝[x_i,y_i,z_i]^T表示机器鱼i在水下三维环境中的位置；

G_i＝[V_i,1cosθ_i,V_i,1sinθ_i,V_i,2]^T，其中V_i,1表示机器鱼i的水平运动速度，V_i,2表示机器鱼i的上浮或下潜速度，θ_i表示机器鱼i在水平面内的方向角；机器鱼i的控制量用U_i＝[V_i,1,θ_i,V_i,2]^T来表示；

由于机器鱼群的编队控制需要个体之间进行通信，所以可将机器鱼之间的通信拓扑表示为一个无向图G＝{V,E}；其中V＝{1,2,…,n}表示点集,表示边集；点集中令Vl＝{1,2,…,n_l}表示领航者集合，而V_f＝{n_l+1,…,n}表示跟随者的集合；由于通信拓扑为无向图，所以图G中的边(i,j)表示机器鱼i和机器鱼j可以各自收到对方传来的信息；机器鱼编队的框架可以表示为(G,p)，/>表示编队的队形，pi表示队形中各机器鱼的位置，其中领航者群体和跟随者群体的位置可以分别表示为/>和/>若队形p中的各机器鱼位置坐标线性无关，则称此队形具有一般性；队形p的仿射变换定义如下

5.根据权利要求1所述的基于应力矩阵的水下航行器编队控制方法，其特征在于，

步骤2)中，矩阵Q满足条件(8)；

其中，将标称队形位置坐标p₁,…,p_n中每一维的坐标值提取出来重组为三个向量构造矩阵/>使其满足以下条件

Q1_n＝0,Qq₁＝0,Qq₂＝0,Qq₃＝0,QQ^T＝I₃ (8)。

6.根据权利要求1所述的基于应力矩阵的水下航行器编队控制方法，其特征在于，

在步骤3)中，令ω＝[ω₁,…,ω_m]^T表示图中包含所有m条边应力值的向量，那么应力矩阵还可以表示为以下形式

Ω＝Bdiag(ω)B^T (9)

其中,图G的关联矩阵，diag(ω)表示对角线元素为应力值的m维对角阵；通过求解以下半正定规划问题，即可求得各边的应力值ω_ij

其中，QΩQ^T＞λI₃表示QΩQ^T-λI₃正定，可以取任意正实数。

7.根据权利要求1所述的基于应力矩阵的水下航行器编队控制方法，其特征在于，步骤3)中，半正定规划问题通过MOSEK、SEDUMI或SDPT3求解器进行求解。