CN105573307B - 基于流速场协作自适应估计的寻迹编队控制方法 - Google Patents

Abstract

本发明是一种基于流速场协作自适应估计的寻迹编队控制方法，包括如下步骤：a)由寻迹误差和流速场强度的估计，设计运动体沿轨道法方向的速度；b)由广义弧长和流速场强度的估计，设计运动体沿轨道切方向的速度；c)计算真实与设计的沿轨道法方向的速度间误差，由邻居信息设计运动体沿轨道法方向的控制力；d)计算真实与设计的沿轨道切方向的速度间误差，设计运动体沿轨道切方向的控制力；e)由步骤c)和d)求解运动体的控制力，利用伺服系统完成运动体的运动控制；f)由步骤a)和b)以及邻居信息，设计流速场强度的协作自适应估计更新率。该方法简单可靠，精度较高，可用于复杂环境中协同信息采集等。

Description

基于流速场协作自适应估计的寻迹编队控制方法

技术领域

本发明涉及一种基于流速场协作自适应估计的寻迹编队控制方法。

背景技术

安装传感器的多运动体协作采集信息因其相对廉价的个体、采集的数据精度高以及鲁棒性好，近些年倍受国内外众多科研机构和著名学者的关注。普林斯顿大学联合了多家著名的科研机构先后多次开展了多水下运动体联合采集海洋生物群体信息的实验，效果十分显著(Princeton University,“Adaptive Sampling and Prediction”.http://www.princeton.edu/dcsl/asap/)。早在1998年，我国就着力于加强海洋污染调查、监测和管理，完善污染监测网，健全卫星、船舶、岸站立体监视和执法体系。为了能够充分地利用有限个运动体来实现大范围的区域的数据采集以及最大限度的保证采集信息的精度，通常需要规划每个运动体的轨道并且要求多运动体在给定轨道上形成一定的队形，即寻迹编队控制技术。

当前，寻迹编队控制方法大都忽略外界流速场对多运动体协同采集系统的影响(陈杨杨，田玉平，基于轨道扩展的多机器人寻迹编队控制设计方法,专利号：ZL201010552508.4；)或者是针对已知时不变流速场的情况(陈杨杨，一种二维定常风速场中多机器人的寻迹编队控制方法，专利申请号：201310318275.5)。然而，野外作业无论是在海里还是在地面、天空都避免不了流速场(如洋流场、风场)的影响而且由于环境的复杂性使得流速场时常改变，进而很难精确得到其强度。与此同时，马里兰大学的Paley教授在他的论文(D.A.Paley and C.K.Peterson,“Stabilization of collective motion in atime-invariant flowfield,”J.Guidance,Contr.,and Dynamics,2009,32(3):771-779)指出外界流速场不仅使得运动体偏离自己的预定轨道而且严重影响数据采集的精度。因此，非常有必要研究未知流速场强度的估计以及基于估计的寻迹编队控制方法。

Peterson和Paley在文献(C.K.Peterson and D.A.Paley,"Distributedestimation for motion coordinated in an unknown spatially varying flowfield,"J.Guidance,Contr.,and Dynamics,2013,36(3):894-898)给出了一种基于一致滤波和信息滤波的流速场强度估计方法，但是该方法需要保证被测目标的群体能观性并且信息滤波器需要在一致滤波器使得各运动体的信息算子达到一致后才能开始使用，实际上信息算子何时达到一致该文没有定性分析和定量时间界线。与此同时，上述方法只用于圆轨道上的均匀分布控制，显然不能满足实际中非圆轨道上的寻迹编队控制的要求。由此可见，对于基于流速场估计的寻迹编队控制设计问题还没有得到很好地解决。

发明内容

技术问题：本发明的目的是提供一种基于流速场协作自适应估计的寻迹编队控制方法，该方法简单可靠、精度较高，可应用于复杂环境中协同信息采集等领域。

技术方案：本发明是一种基于流速场协作自适应估计的寻迹编队控制方法，特别适用于流速场方向已知、强度未知以及轨道函数描述的轨道。考虑θ描述的流速场中运动n个运动体，其中流速场的方向已知并且是关于位置坐标和时间t的一阶连续导数函数，流速场强度θ未知。每个运动体满足牛顿的动力学方程并受到外界流速场的作用，具体形式如下

其中v_i表示运动体i的运动速度，m_i为其质量，F_i为运动体的控制力输入，i＝1,…,n。

运动体i的目标轨道C_i0是平面中一条曲率为正的简单闭曲线。由陈杨杨和田玉平提出的同心压缩曲线扩展方法(陈杨杨和田玉平，“基于轨道扩展的多机器人的寻迹编队控制方法”，专利号ZL201010552508.4，2010)，可以将C_i0扩展为一组关于轨道函数

λ_i(z):Ω_i→(-1,ε_i)

的等值曲线簇，并且满足Ω_i＝{z∈C²|-1＜λ_i(z)＜ε_i＜∞}，ε_i为正常数。λ_i(z)＝c(c为常数)，表示等值曲线簇中的一条等值曲线C_i,c，目标轨道对应的轨道函数值为0，即λ_i(z)＝0(如图1所示)。

多运动体在寻迹编队运动中，运动体间的通信是必不可少的，这里我们用双向图来描述，其中为节点集，为有向边的集合。如果存在一条边连接节点和表明运动体i和k可以交换信息，它们互为相邻节点(即邻居)。运动体i的相邻节点集合用表示。当图中任意两个节点间都存在着一条路径，那么图是连通的。这里的两个节点和间的路径是指由不同节点和边构成的图。图的邻接矩阵A＝[a_ij]可以定义为a_ij＞0当且仅当时，其他a_ij＝0。图2为5个运动体间的通信拓扑对应的连通图。设计时，我们一旦规定好多运动体间信息交互关系，那么以后每一个时刻运动体i的都是不变的，且对应的双向图是连通的。本发明的目的就是根据通信得到的相邻运动体的信息，设计每个运动体的控制力和流速场强度协作自适应估计更新率，使其运动在目标轨道的同时运动体间保持一定的队形。

在本发明中，对于沿着目标轨道运动的各运动体之间的队形位置关系采用如下方式规定：设目标轨道上的固定点为弧长计算的起始点，s_i是从起始点沿着目标轨道到运动体位置间的路程(即弧长)，广义弧长ξ_i＝τ_i(s_i)是关于s_i的一阶连续可导函数，并且对于所有的s_i满足有界且不为0。各运动体之间保持队形位置关系是指：

τ_i(s_i)-τ_j(s_j)＝0。

如图3所示三个运动体运动在不同长短轴的同心椭圆轨道并且保持与椭圆心在同一条直线上，同心椭圆轨道方式如下：

其中R_ia和R_ib分别表示长短轴的长度。为了保证期望队形，我们需要ξ_i＝s_i/R_i达到一致。

本发明设计思想是先根据轨道函数的梯度方向(即轨道法方向)和与它垂直的方向(即轨道切方向)将运动体的运动投影到这两个方向上，为了设计协作自适应流速场强度估计更新率以及基于该估计的寻迹编队控制律(即运动体的控制力)，先根据流速场强度的估计值，通过设计运动体沿轨道法方向的速度使得轨道函数值λ_i(z_i)与期望值间的寻迹误差d_i减少到满足设计的要求，与此同时设计运动体沿轨道切方向的速度使得广义弧长τ_i(s_i)达到一致且运动体沿轨道运动的速度达到期望值接着，分别设计运动体的沿着轨道法方向控制力和沿轨道切方向的控制力使得真实和设计好的沿轨道法方向和切方向的速度间的误差和减少到满足设计的要求，然后联列求解出每个运动体的控制力；最后，根据设计好的沿轨道法方向和切方向的速度以及邻居信息，设计流速场强度的协作自适应估计更新率

具体的讲：

本发明是一种基于流速场协作自适应估计的寻迹编队控制方法，适用于流速场的方向已知而强度未知并且是用轨道函数描述的运动体的运动轨道，其特征在于该方法包括如下步骤：

a)由寻迹误差和流速场强度的估计值，设计运动体沿轨道法方向的速度；

b)由广义弧长和流速场强度的估计值，设计运动体沿着轨道切方向的速度；

c)计算真实与设计的沿轨道法方向的速度间误差，由邻居信息设计运动体沿轨道法方向的控制力部分；

d)计算真实与设计的沿轨道切方向的速度间误差，由邻居信息设计运动体沿轨道切方向的控制力部分；

e)由步骤c)和d)设计出的运动体沿轨道法方向和切方向的控制力部分求解运动体的控制力，利用伺服系统完成运动体的运动控制；

f)由步骤a)和b)设计出的运动体沿轨道法方向和切方向的速度以及邻居信息，设计流速场强度的协作自适应估计率。

其中所述步骤a)包括如下步骤：

a1)由运动体位置、轨道函数以及流速场强度的估计值，计算流场中轨道函数值与期望值间的位置寻迹误差；

a2)由轨道函数计算轨道的单位法向量；

a3)由位置寻迹误差和轨道的单位法向量，设计运动体沿轨道法方向的速度使位置寻迹误差达到设计要求。

其中所述步骤b)包括如下步骤：

b1)规定好目标轨道的弧长参数和弧长计算的起始点，由此计算运动体沿轨道从起始点到当前位置间的弧长；

b2)由队形要求确定广义弧长与弧长间的函数关系，计算广义弧长；

b3)由轨道的单位法向量计算轨道的单位切向量；

b3)由广义弧长、流速场强度的估计值、轨道的单位切向量以及步骤a)得到的沿轨道法方向的速度，设计运动体沿轨道切方向的速度使编队达到设计要求。

其中所述步骤c)包括如下步骤：

c1)由轨道单位法向量计算运动体沿轨道法方向的真实速度；

c2)计算其与步骤a)得到的沿轨道法方向的速度间的法向速度误差；

c2)根据法向速度误差、流速场强度的估计值以及邻居信息，设计运动体沿轨道法方向控制力部分使得法向速度误差减少到满足的设计要求。

其中所述步骤d)包括如下步骤：

d1)由轨道单位切向量，计算运动体沿轨道切方向的真实速度；

d2)计算其与步骤b)得到的沿轨道切方向的速度间的切向速度误差；

d3)根据切向速度误差、流速场强度的估计值以及邻居信息，设计运动体沿轨道切方向控制力部分使得切向速度误差减少到满足的设计要求。

其中所述步骤e)包括如下步骤：

e1)根据步骤c)和d)设计出的运动体沿轨道法方向和切方向控制力部分与运动体控制力的关系求解出运动体的控制力；

e2)由上位机将运动体的控制力输入发送给下位机中，通过伺服系统来完成运动控制。

有益效果：本方法简单可靠，精度较高，可应用于复杂环境中协同信息采集等领域。

附图说明

图1运动体在关于轨道函数的等值曲线上的运动；

图2五个运动体间的信息交互拓扑；

图3三个运动体以一字型队形编队运动在同心椭球轨道上；

图4基于流场协作自适应估计的寻迹编队控制设计流程图。

以上的图中：λ_i：轨道函数；轨道函数值为-1的等值曲线；轨道函数值为0的等值曲线，即目标轨道；轨道函数值为0.5的等值曲线；T_i：轨道的单位切向量；N_i：轨道的单位法向量；运动体i沿T_i的速度；运动体i沿N_i的速度；d_i：寻迹误差；上弧长计算的起始点；上弧长计算的起始点；上弧长计算的起始点；s_i：弧长计算的起始点到运动体当前位置间的距离；运动体1；运动体2；运动体3；运动体4；运动体5；；z₁：机器人1的位置坐标；z₂：机器人2的位置坐标；z₃：机器人3的位置坐标；上弧长计算的起始点；上弧长计算的起始点；上弧长计算的起始点；s₁：上的弧长；s₂：上的弧长；s₃：上的弧长；ξ₁：上的广义弧长；ξ₂：上的广义弧长；ξ₃：上的广义弧长；R₁a：对应的长轴半径；R₂a：对应的长轴半径；R₃a：对应的长轴半径；R₁b：对应的短轴半径；R₂b：对应的短轴半径；R₃b：对应的短轴半径；

具体实施方式

图4是本发明的设计流程图，由步骤P1-P6构成，各步骤叙述如下：

1)步骤P1

本发明是一种基于流速场强度估计的寻迹编队控制方法，因此需要根据流速场强度估计值来先计算运动体与期望轨道的误差，考虑到流速场直接作用于运动体的速度，故先采取设计运动体的速度。由于寻迹的目的是使得运动体位置对应的轨道函数值趋于期望值，对应的设计运动体沿轨道函数负梯度方向(即轨道的法方向)的速度部分，具体实施步骤如下：

第一步：根据运动体的位置z_i和轨道函数λ_i(z_i)，计算寻迹误差d_i

d_i＝λ_i(z_i)-0＝λ_i(z_i)；

第二步：由轨道函数λ_i(z_i)计算轨道的法方向N_i

其中为λ_i的梯度；

第三步：由寻迹误差d_i、轨道的法方向N_i以及流速场的强度估计值设计运动体i沿轨道法方向的速度

其中k₁＞0，

表示流速场方向；

2)步骤P2

步骤P2由广义弧长、流速场强度的估计值以及设计好的沿轨道法方向的速度来设计运动体沿着轨道切方向的速度。具体步骤实施如下：

第一步：规定好弧长参数σ_i，同时设定关于轨道函数的等值曲线簇弧长计算的起始点使得起始点对应的弧长参数值相同，计算沿着运动体i当前所在的轨道从起始点到当前位置z_i间的弧长s_i

第二步：由队形的要求设计好广义弧长函数ξ_i，即ξ_i是关于s_i一个具有连续导数的函数，并且对于所有的s_i满足有界且不为0。由弧长s_i计算广义弧长ξ_i(s_i)。

第三步：由轨道的单位法向量计算轨道的单位切向量T_i

第四步：由广义弧长ξ_i、流速场强度的估计值轨道的单位切向量T_i以及步骤P1设计的沿轨道法方向的速度设计运动体沿轨道切方向的速度

其中在轨期望速度是关于时间t连续函数且对t有界，控制参数k₂＞0。

3)步骤P3

步骤P3根据真实与设计的沿轨道法方向速度间的误差，设计运动体沿着轨道法方向的控制力部分使得该速度误差减少到满足设计的要求。设计按下列步骤实现：

第一步：由轨道的单位法向量N_i计算运动体i沿轨道法方向真实的速度

第二步：计算真实和设计的运动体i沿轨道法方向速度间的误差

第三步：由速度误差和邻居信息，设计运动体i沿轨道法方向的控制力部分

其中，

控制参数k₃和k₄都大于0的常数，是λ_i的海森矩阵。

4)步骤P4

步骤P4根据真实与设计的沿轨道切方向速度间的误差，设计运动体沿着轨道切方向的控制力部分使得该速度误差减少到满足设计的要求。设计按下列步骤实现：

第一步：由轨道的单位切向量T_i计算运动体i沿轨道法方向真实的速度

第二步：计算真实和设计的运动体i沿轨道切方向速度间的误差

第三步：由速度误差和邻居信息，设计运动体i沿轨道切方向的控制力部分

其中，控制参数k₅＞0，

5)步骤P5

步骤P5是根据步骤P3和步骤P4得到的运动体沿轨道法方向和切方向的控制力部分联列求解运动体的控制力，利用伺服系统完成运动体的运动控制。具体按照以下步骤实现：

第一步：根据步骤P3得到的运动体沿轨道法方向控制力部分和步骤P4得到的运动体沿轨道法方向控制力部分联列求解运动体的控制力F_i

第二步：由上位机将运动体的控制力发送给下位机中，通过伺服系统来完成运动体的运动控制。

6)步骤P6

步骤P6是由步骤P1和步骤P2设计的运动体沿轨道法方向和切方向的速度以及邻居信息设计流速场未知强度的协同自适应估计更新率具体形式如下：

并返回到步骤P1，其中，控制参数k₆＞0，

Claims (6)

1.一种基于流速场协作自适应估计的寻迹编队控制方法，适用于流速场的方向已知而强度未知并且是用轨道函数描述的运动体的运动轨道，其特征在于该方法包括如下步骤：

a) 由寻迹误差和流速场强度的估计值，设计运动体沿轨道法方向的速度；

b) 由广义弧长和流速场强度的估计值，设计运动体沿着轨道切方向的速度；

c) 计算真实与设计的沿轨道法方向的速度间误差，由邻居信息设计运动体沿轨道法方向的控制力部分；

d) 计算真实与设计的沿轨道切方向的速度间误差，由邻居信息设计运动体沿轨道切方向的控制力部分；

e) 由步骤c)和d)设计出的运动体沿轨道法方向和切方向的控制力部分求解运动体的控制力，利用伺服系统完成运动体的运动控制;

f) 由步骤a)和b)设计出的运动体沿轨道法方向和切方向的速度以及邻居信息，设计流速场强度的协作自适应估计率。

2.按权利要求1所述的基于流速场协作自适应估计的寻迹编队控制方法，其特征在于：

其中所述步骤a）包括如下步骤：

a1)由运动体位置、轨道函数以及流速场强度的估计值，计算流场中轨道函数值与期望值间的位置寻迹误差；

a2) 由轨道函数计算轨道的单位法向量；

a3) 由位置寻迹误差和轨道的单位法向量，设计运动体沿轨道法方向的速度使位置寻迹误差达到设计要求。

3.按权利要求1所述的基于流速场协作自适应估计的寻迹编队控制方法，其特征在于：

其中所述步骤b）包括如下步骤：

b1) 规定好目标轨道的弧长参数和弧长计算的起始点，由此计算运动体沿轨道从起始点到当前位置间的弧长；

b2) 由队形要求确定广义弧长与弧长间的函数关系，计算广义弧长；

b3) 由轨道的单位法向量计算轨道的单位切向量；

b3）由广义弧长、流速场强度的估计值、轨道的单位切向量以及步骤a)得到的沿轨道法方向的速度，设计运动体沿轨道切方向的速度使编队达到设计要求。

4.按权利要求1所述的基于流速场协作自适应估计的寻迹编队控制方法，其特征在于：

其中所述步骤c）包括如下步骤：

c1) 由轨道单位法向量计算运动体沿轨道法方向的真实速度；

c2) 计算其与步骤a) 得到的沿轨道法方向的速度间的法向速度误差；

c2) 根据法向速度误差、流速场强度的估计值以及邻居信息，设计运动体沿轨道法方向控制力部分使得法向速度误差减少到满足的设计要求。

5.按权利要求1所述的基于流速 场协作自适应估计的寻迹编队控制方法，其特征在于：

其中所述步骤d）包括如下步骤：

d1) 由轨道单位切向量，计算运动体沿轨道切方向的真实速度；

d2) 计算其与步骤b) 得到的沿轨道切方向的速度间的切向速度误差；

d3) 根据切向速度误差、流速场强度的估计值以及邻居信息，设计运动体沿轨道切方向控制力部分使得切向速度误差减少到满足的设计要求。

6.按权利要求1所述的基于流速场协作自适应估计的寻迹编队控制方法，其特征在于：

其中所述步骤e）包括如下步骤：

e1) 根据步骤c)和d)设计出的运动体沿轨道法方向和切方向控制力部分与运动体控制力的关系求解出运动体的控制力；

e2) 由上位机将运动体的控制力输入发送给下位机中，通过伺服系统来完成运动控制。