CN101650569A - 三维空间中多运动体的寻迹编队控制方法 - Google Patents

Abstract

本发明是一种三维空间中多运动体的寻迹编队控制方法，包括如下步骤：a)对于空间中某一平面目标轨迹，将其扩展为由不同轨迹函数值表示的不同等值平面中的等值曲线簇，并确定对应运动体的运动范围；b)由轨迹函数计算寻迹误差，设计运动体沿着轨迹主法和次法方向上的控制力来完成寻迹；c)利用轨迹函数和目标轨迹对应的弧长参数计算运动体沿着所在轨迹运动的广义弧长及其导数，由信息交互得到的相邻运动体的信息，设计运动体沿着轨迹切向上的控制力来实现编队；d)综合步骤b和c计算运动体的控制力输入。本发明对空间直角坐标系下描述的运动体的动态和目标轨迹尤其适用。该方法简单可靠、精度较高，可用于多运动体协同探测星体等复杂任务。

Description

三维空间中多运动体的寻迹编队控制方法

技术领域

本发明涉及一种三维空间中多运动体的寻迹编队控制方法。

背景技术

众所周知，三维空间中单个运动体的寻迹控制技术因其控制律比较简单、控制系统具有好的鲁棒性以及易于工程实践等特点被广泛应用于军事、太空探索以及空间观测领域中。所谓单个运动体的寻迹控制，是指在控制系统的驱动下，运动体从任意初始位置驶入预先规划好的轨迹，并沿此轨迹运动或者最终运动到目的地。例如：制导导弹按预定的弹道飞行去精确轰炸目标、航天器驶入规定航线进行太空探索以及卫星沿着椭圆轨道监测地面信息等。近些年来，随着协调控制的发展，使得很多复杂任务可以由多个简单运动体通过合作来共同完成，不仅降低了单个运动体制造成本，而且提高了系统的灵活性和鲁棒性，如：战斗机群编队飞行追击敌机、卫星群拍摄高清晰度的地面照片以及多个直升机联合执行大范围的搜救任务等。因此，很多国际空间机构(如NASA和ESA)都将三维空间中多运动体的寻迹编队控制作为21世纪一项关键的技术。

当前，已有的寻迹编队控制方法多是针对平面中的运动体，很少有考虑三维空间中的多运动体寻迹编队。Princeton大学的Leonard教授指导的团队给出了Serret-Frenet坐标系下多个平面运动体在封闭轨道上的协调控制方法(F.Zhang et al，″Control ofcoordinated patterns for ocean sampling，″Int.J.Control，vol.80，pp.1186-1199，2007)。为了通过控制运动体的方向角和速率来实现寻迹编队，该方法要求运动体的速率始终不能为0，因而造成多运动体(如：飞艇)不能执行例如固定于轨道某处进行长时间的观测等任务。由于运动体间信息交互拓扑采用的是双向链式且目标轨迹是简单闭曲线，从而也在一定程度上限制了该方法在实际中的运用(如：某些运动体只有接收转置而另一些只安装了发送器，目标轨迹是无自交点的非封闭曲线)。与此同时，运动体在实际中很多时候得到是在某一固定直角坐标系下确定的局部信息(如：通过传感器网络或者GPS的方式)，并且目标轨迹也是在相同直角坐标系下规划的。

因此，设计三维空间直角坐标下的多运动体的寻迹编队控制方法将更加具有现实意义。但目前还不存在此类控制方法。

发明内容

技术问题：本发明的目的是提供一种三维空间中多运动体的寻迹编队控制方法，该方法简单可靠、精度较高，可用于多运动体协同探测星体等复杂任务。

技术方案：本发明是一种三维空间中多运动体的寻迹编队控制方法，特别适用于在空间直角坐标系下描述的运动体的动态和目标轨迹。考虑空间中由n个运动体组成的寻迹编队控制系统，运动体的运动可以看成刚体的运动，并且符合牛顿第二定律：

${\stackrel{\·}{q}}_i=p_i$

$m_i{\stackrel{\·}{p}}_i=F_i$

其中q_i＝[q_xi，q_yi，q_zi]^T∈□³表示运动体i在空间直角坐标系中的位置坐标，p_i＝[p_xi，p_yi，p_zi]^T∈□³表示运动体i的速度，m_i为运动体i的质量，F_i＝[F_xi，F_yi，F_zi]^T∈□³是运动体i的控制力输入，i＝1，...，n。

多运动体在寻迹编队运动中，运动体间的信息交互是必不可少的，这里我们用有向图G＝(V，E)来描述，其中V＝{V₁，V₂，...，V_n}为节点集， $E\⋐V\×V$ 为有向边的集合。如果节点V_i存在有向边到达节点V_j，这就意味着运动体i可以得到运动体j的信息，运动体j是运动体i的相邻节点。运动体i的相邻节点集合用N_i表示。我们说节点

存在一条有向路径到达节点

即顺序存在一组有向边

从节点

到节点

其中是V中k+1个不同节点的集合。如果有向图中其他所有节点都可以通过有向路径到达节点V_i，则节点V_i被称为全局可达点。有向图的邻接矩阵A＝[a_ij]可以定义为a_ij＞0当且仅当(V_i，V_j)∈E时，其他a_ij＝0。对应的Laplacian矩阵可以表示为L＝[l_ij]，其中l_ii＝∑_j≠i且l_ij＝-a_ij。图1为6个运动体间的信息交互拓扑对应的有向图，其中节点{1，2，6}是全局可达点。设计时，我们一旦规定好多运动体间信息交互关系，那么以后每一个时刻运动体i的N_i都是不变的，且对应的有向图含有全局可达点。工程应用中，我们可以根据实际情况灵活的采用通信的方式、传感的方式以及两种方式相结合的方式来实现多运动体间的信息交互。

对于三维空间中运动体i的目标轨迹C_i0，C_i0是空间某一平面中的一条光滑的正则曲线，可以是简单闭曲线或者是无自身交点的非封闭曲线，并且对于C₀上每一点，曲率k_i0一致有界且不为0。C_i0在空间直角坐标系可以用方程组表示为

${\∫}_{q_{i0}^{*}}^q{N}_{i0}\left(\mathrm{\τ}\right)\mathrm{d\τ}={\mathrm{\λ}}_{i0}^{*}$

其中N_i0(·)和B_i0(·)为C_i0的单位主法向量和单位次法向量并且是光滑的，q＝[q_x，q_y，q_z]^T∈□³为C_i0上的点在空间直角坐标系中的位置坐标，q_i0 ^*为C_i0上的一点，λ_i0 ^*和

是两个常实数。由于C_i0是空间某一平面中的曲线， ${\stackrel{\·}{B}}_{i0}\≡0.$ 令T_i0＝N_i0^B_i0表示C_i0的切向量，对于C_i0上所有的点T_i0≠0，其中“^”表示向量积。本发明的目的就是根据得到的相邻运动体的信息，设计每个运动体的控制力使其运动在目标轨迹的同时，运动体间保持一定的队形。

在本发明中，对于沿着目标轨迹运动的各运动体之间的编队位置关系采用如下方式规定：设目标轨迹C_i0上的固定点q_i0 ^*为弧长计算的起始点，s_i是从起始点q_i0 ^*沿着目标轨迹到运动体位置间的路程(即弧长)，广义弧长ξ_i＝τ_i(s_i)是关于s_i一个具有二阶连续导数的函数(该函数应满足对于所有的s_i满足 ${\∂\mathrm{\ξ}}_i/{\∂s}_i\≠0$ )。各运动体之间保持编队位置关系是指：

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

如果运动体i的目标轨迹是由参考轨迹平移期望的队形向量h_i而得到的(如图2所示)，规定的弧长计算的起始点满足 $q_{i0}^{*}-q_{j0}^{*}=h_i-h_j,$ 那么我们可以简单的定义弧长s_i即为广义弧长ξ_i，弧长s_i和运动体沿着目标轨迹的速度达到一致就能保证多运动体以期望的队形q_i(t)-q_j(t)＝h_i-h_j编队运动。对于更复杂的情况，弧长s_i和运动体沿着目标轨迹的速度达到一致并不能保证多运动体以期望的队形编队运动。图3所示的是三个运动体运动在不同半径R_i的同心圆轨迹上并且保持与圆心在同一条直线上的例子，显然只有当ξ_i＝s_i/R_i及其导数达到一致时才能保证多运动体以期望的队形编队运动。因此，本方法选择用广义弧长ξ_i及其导数η_i来描述多运动体的编队运动。本发明的设计思想是，先将目标轨迹C_i0扩展为不同平面中关于轨迹函数f_ik(q)的等值曲线簇，其中不同平面是关于轨迹函数

的一组等值平面，并且由正则条件确定对应运动体i的运动范围Ω_i。通过设计运动体沿着轨迹主法 $N_{i0}\left(q_i\right)=\▿f_{\mathrm{ik}}\left(q_i\right)$ 和次法方向上的控制力部分使得初始位于Ω_i中的运动体始终运动于Ω_i并且轨迹函数值f_ik(q_i)和

与C_i0对应的期望值λ_i0 ^*和

间的误差d_ik(t)和及其对时间的导数r_ik(t)和

减少到0。当运动体运动在各自的目标轨迹上，多运动体的编队运动就退化为运动体沿着轨迹运动的位置与速度达到一致。根据信息交互得到相邻运动体的信息，设计运动体沿着轨迹切向T_i0(q_i)上的控制力部分使得广义弧长ξ_i(t)及其导数η_i(t)达到一致来实现多运动体的编队运动。

具体的讲：

本发明是一种三维空间中多运动体的寻迹编队控制方法，包括如下步骤：

a)对于空间中某一平面目标轨迹，将其扩展为由不同轨迹函数值表示的不同等值平面中的等值曲线簇，并确定对应运动体的运动范围；

b)由轨迹函数计算寻迹误差，设计运动体沿着轨迹主法和次法方向上的控制力来完成寻迹；

c)利用轨迹函数和目标轨迹对应的弧长参数计算运动体沿着所在轨迹运动的广义弧长及其导数，根据信息交互得到的相邻运动体的信息，设计运动体沿着轨迹切向上的控制力来实现编队；

d)综合步骤b和c得到的运动体沿着轨迹主法、次法以及切方向上的控制力部分，联列求解出运动体的控制力输入。

其中所述的运动体的动态和目标轨迹都是在空间直角坐标系下描述的。

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

a1)沿着目标轨迹的主法和次法方向将其扩展为不同等值平面中的等值曲线簇；

a2)根据曲线的正则性，确定对应运动体的运动范围；

a3)在运动范围上构建轨道函数，使得扩展出的曲线簇可以由轨迹函数取不同的值来表示。

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

b1)由运动体的位置和轨迹函数，计算轨迹函数值与期望值间的位置寻迹误差；

b2)由位置寻迹误差对时间的导数，计算速度寻迹误差；

b3)由位置和速度寻迹误差，设计运动体沿着轨迹主法和次法方向上的控制力部分。

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

c1)规定好目标轨迹上弧长计算的起始点和参数，由此确定曲线簇中其他曲线上弧长计算的起始点；

c2)由轨迹函数和弧长参数，计算沿着运动体所在轨迹从起始点到运动体位置间的弧长；

c3)根据编队的要求确定广义弧长与弧长间的函数关系，计算广义弧长及其对时间的导数；

c4)由信息交互得到的相邻运动体的广义弧长及其导数，设计运动体沿着轨迹切向上的控制力部分。

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

d1)综合步骤b和c得到的运动体沿着轨迹主法方向、次法以及切方向上的控制力部分，联列求解出运动体的控制力输入。

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

当然，如果编队要求运动体的沿着目标轨迹的速度达到期望值η^*(t)，我们只需改变上述的步骤c4)中运动体沿着轨迹切向上的控制力的设计即可。

有益效果：本方法具有简单可靠、精度较高以及便于实际运用的特点，可用于多运动体协同探测星体等复杂任务。

附图说明

图1为信息交互拓扑对应的有向图；

图2为多运动体以三角形队形编队运动在目标轨迹上；

图3为多运动体在同心圆轨迹上的编队运动；

图4为不同等值平面中的等值椭圆簇；

图5为单个运动体在等值椭圆簇中的运动；

图6为三维空间中多运动体的寻迹编队控制设计流程图。

以上的图中有：1：运动体1；2：运动体2；3：运动体3；4：运动体4；5：运动体5；6：运动体6；x：三维直角坐标系中的x轴；y：三维直角坐标系中的y轴；z：三维直角坐标系中的z轴；o：三维直角坐标系中坐标原点；i：运动体i；j：运动体j；k：运动体k；C_T：参考轨迹；C_i0：运动体i的目标轨迹；C_j0：运动体j的目标轨迹；C_k0：运动体k的目标轨迹；h_i：运动体i对应的期望队形向量；h_j：运动体j对应的期望队形向量；h_k：运动体k对应的期望队形向量；q_i0 ^*：C_i0上弧长计算的起始点；q_j0 ^*：C_j0上弧长计算的起始点；q_k0 ^*：C_k0上弧长计算的起始点；s_i：C_j0上的弧长；s_j：C_j0上的弧长；s_k：C_k0上的弧长；

运动体i沿C_i0的速度；

运动体j沿C_j0的速度；运动体k沿C_k0的速度；ξ_i：C_i0上的广义弧长；ξ_j：C_j0上的广义弧长；ξ_k：C_k0上的广义弧长；η_i：ξ_i对时间的导数；η_j：ξ_j对时间的导数；η_k：ξ_k对时间的导数；N_i0：单位主法向量；B_i0：单位次法向量；C_i1和C_i2：C_i0上的每一点沿N_i0平移不同实数得到两条Bertrand曲线；C_i3和C_i4：C_i0上的每一点沿B_i0平移不同实数得到两条曲线；P₀：C_u0所在的平面；P₁：C_i3所在的平面；P₂：C_i4所在的平面；T_i0：单位切向量；σ_i：弧长参数；q_i1 ^*：C_i1上弧长计算的起始点；q_i2 ^*：C_i2上弧长计算的起始点；q_i3 ^*：C_i3上弧长计算的起始点；

具体实施方式

图6是本发明的设计流程图，由模块P1、P2、P3和P4构成，各模块叙述如下：

1)模块P1

由于本发明设计分布式控制律的前提是将每条目标轨迹扩展为由不同轨迹函数值表示的不同等值平面中的等值曲线簇，模块P1用于得到曲线簇、轨迹函数以及运动体的运动范围。

如图4所示，目标轨迹C_i0为空间中平面P₀椭圆曲线。在C_i0附近，将C_i0上的每一点沿着其主法N_i0(·)方向平移不同的实数λ_ik，我们可以得到不同的椭圆曲线(如：C_i1和C_i2)；将C_i0上的每一点沿着其次法B_i0(·)方向平移不同的实数

相当于将C_i0平移到与P₀平行的不同平面中(如：P₁中的C_i3和P₂中的C_i4)。类似的，我们可以将一般的目标轨迹扩展。模块P1具体按下列步骤实现：

第一步：在C_i0附近，将C_i0上的每一点分别沿着其主法N_i0(·)和次法B_i0(·)方向平移实数λ_ik和

得到扩展曲线C_ik。当

而λ_ik取不同值，相当于在C_i0所在平面P₀中将C_i0扩展为一组Bertrand曲线，即每条曲线的主法向量与C_i0相同。当λ_ik为常值而

取不同值，相当于将P₀平面中一条Bertrand曲线平移到与P₀平行的一组平面中。

第二步：由扩展曲线的切向量T_ik＝T_i0(1-λ_ikk_i0)以及正则条件T_ik≠0，我们选择运动体i的运动范围Ω_i为空间中所有满足|λ_ik|＜ε的曲线上点的集合，其中ε∈(0，1/sup{|k_i0|})。

第三步：由于集合Ω_i中的每一点都属于与P₀平行的某个平面中的一条Bertrand曲线，我们可以在Ω_i上构建轨道函数

$f_{\mathrm{ik}}\left(q\right)={\∫}_{q_{\mathrm{ik}}^{*}}^q{N}_{i0}\left(\mathrm{\τ}\right)\mathrm{d\τ}={\mathrm{\λ}}_{\mathrm{ik}},$

当q， $q_{\mathrm{ik}}^{*}\∈C_{\mathrm{ik}},$ 其中每一条Bertrand曲线都是某一平面中的关于f_ik(q)的一条等值曲线，该平面是关于

的一个等值平面。由轨道函数，运动范围可以写成

其中c_i1和c_i2为两个实数。

2)模块P2

模块P2根据轨迹函数设计运动体沿着目标轨迹主法和次法方向上的控制力部分用以完成寻迹。设计按下列步骤实现：

第一步：计算寻迹位置误差d_ik(t)和

$d_{\mathrm{ik}}=f_{\mathrm{ik}}\left(q_i\right)-{\mathrm{\λ}}_{i0}^{*},$

第二步：计算寻迹速度误差r_ik(t)和

$r_{\mathrm{ik}}={\stackrel{\·}{d}}_{\mathrm{ik}}=\frac{\∂f_{\mathrm{ik}}}{\∂q_{\mathrm{xi}}}{p}_{\mathrm{xi}}+\frac{\∂f_{\mathrm{ik}}}{\∂q_{\mathrm{yi}}}{p}_{\mathrm{yi}}+\frac{\∂f_{\mathrm{ik}}}{{\∂q}_{\mathrm{zi}}}{p}_{\mathrm{zi}},$

第三步：设计运动体i沿着轨迹主法和次法方向上的控制力

$\frac{{\∂f}_{\mathrm{ik}}}{\∂q_{\mathrm{xi}}}{F}_{\mathrm{xi}}+\frac{\∂f_{\mathrm{ik}}}{{\∂q}_{\mathrm{yi}}}{F}_{\mathrm{yi}}+\frac{\∂f_{\mathrm{ik}}}{{\∂q}_{\mathrm{zi}}}{F}_{\mathrm{zi}}=N_{i0}\left(q_i\right){\·F}_i=m_i{G}_{\mathrm{ik}},$

其中

$G_{\mathrm{ik}}=-(\frac{{\∂}^2{f}_{\mathrm{ik}}}{\∂q_{\mathrm{xi}}^2}{p}_{\mathrm{xi}}^2+\frac{{\∂}^2{f}_{\mathrm{ik}}}{\∂q_{\mathrm{yi}}^2}{p}_{\mathrm{yi}}^2+\frac{{\∂}^2{f}_{\mathrm{ik}}}{\∂q_{\mathrm{zi}}^2}{p}_{\mathrm{zi}}^2+2\frac{{\∂}^2{f}_{\mathrm{ik}}}{{\∂q}_{\mathrm{xi}}\∂q_{\mathrm{yi}}}{p}_{\mathrm{xi}}{p}_{\mathrm{yi}}+2\frac{{\∂}^2{f}_{1i}}{\∂q_{\mathrm{yi}}\∂q_{\mathrm{zi}}}{p}_{\mathrm{yi}}{p}_{\mathrm{zi}}+2\frac{{\∂}^2{f}_{\mathrm{ik}}}{\∂q_{\mathrm{xi}}\∂q_{\mathrm{zi}}}{p}_{\mathrm{xi}}{p}_{\mathrm{zi}})-{\mathrm{\ψ}}_i\left(d_{\mathrm{ik}}\right)-k_1{r}_{\mathrm{ik}},$

${\mathrm{\ψ}}_i\left(d_{\mathrm{ik}}\right)=\tan \left(\frac{\mathrm{\π}(2d_{\mathrm{ik}}+2{\mathrm{\λ}}_{i0}^{*}-c_{i2}-c_{i1})}{2(c_{i2}-c_{i1})}\right)-\tan \left(\frac{\mathrm{\π}(2{\mathrm{\λ}}_{i0}^{*}-c_{i2}-c_{i1})}{2(c_{i2}-c_{i1})}\right),$

控制参数k₁，k₂，k₃＞0，ψ_i(d_ik)用于保证初始位于Ω_i的运动体i始终在Ω_i中运动，并且当d_ik＝0时ψ_i(d_ik)＝0。

3)模块P3

如图5所示，运动体i沿着轨迹切向T_i0(q_i)上的运动可以改变运动体沿着当前所在轨迹运动的弧长s_i。与此同时，运动体i投影到轨迹主法方向N_i0(q_i)上的运动可以改变同一等值平面中运动体所在的曲线，同样也可以改变弧长s_i。而运动体i投影到轨迹次法方向B_i0(q_i)上的运动只是改变轨迹所在的平面，并不改变弧长s_i。因此，本方法选择目标轨迹对应的弧长参数σ_i来刻画运动体运动在Ω_i中所有轨迹上的弧长。

模块P3根据信息交互得到相邻运动体的广义弧长ξ_j(t)及其导数η_j(t)来设计运动体i沿着轨迹切向上的控制力部分用以实现编队，具体的设计步骤如下：

第一步：规定好目标轨迹上弧长计算的起始点q_i0 ^*和参数σ_i，由此我们可以选择曲线簇中其他曲线上弧长计算的起始点q_ik ^*，使得q_ik ^*和q_i0 ^*对应的弧长参数值是一致的。

第二步：由轨迹函数和弧长参数σ_i，计算沿着运动体当前所在的轨迹从起始点q_ik ^*到运动体位置q_i间的弧长s_i

$s_i(f_{\mathrm{ik}},{\mathrm{\σ}}_i)={\∫}_{{\mathrm{\σ}}_{i0}^{*}}^{{\mathrm{\σ}}_i}\frac{{\∂s}_i(f_{\mathrm{ik}},\mathrm{\τ})}{\∂\mathrm{\τ}}\mathrm{d\τ}$

其中σ_i0 ^*为q_i0 ^*对应的弧长参数值；

第三步：由编队的要求设计好广义弧长函数ξ_i，即ξ_i是关于s_i一个具有二阶连续导数的函数，并且满足对于所有的s_i满足 ${\∂\mathrm{\ξ}}_i/{\∂s}_i\≠0.$ 由弧长s_i计算广义弧长ξ_i(s_i)及其对时间的导数η_i

${\mathrm{\η}}_i=\frac{{\∂\mathrm{\ξ}}_i}{\∂s_i}{\stackrel{\·}{s}}_i=\frac{\∂{\mathrm{\ξ}}_i}{{\∂s}_i}(T_{i0}^x{p}_{\mathrm{xi}}+T_{i0}^y{p}_{\mathrm{yi}}+T_{i0}^z{p}_{\mathrm{zi}}+\frac{{\∂s}_i(f_{\mathrm{ik}},{\mathrm{\σ}}_i)}{{\∂f}_{\mathrm{ik}}}{r}_{\mathrm{ik}})$

其中 $T_{i0}\left(q_i\right)={[T_{i0}^x,T_{i0}^y,T_{i0}^z]}^T.$

第四步：根据信息交互所得到的相邻运动体的广义弧长ξ_j及其导数η_j，设计运动体沿着轨迹切向上的控制力

$T_{i0}^x{F}_{\mathrm{xi}}+T_{i0}^y{F}_{\mathrm{yi}}+T_{i0}^z{F}_{\mathrm{zi}}=T_{i0}\left(q_i\right)\·F_i=m_i{\left(\frac{{\∂\mathrm{\ξ}}_i}{{\∂s}_i}\right)}^{-1}{\tilde{G}}_{\mathrm{ik}}$

其中

${\tilde{G}}_{\mathrm{ik}}=-g_{\mathrm{ik}}-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}[k_4({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_j)-k_5({\mathrm{\η}}_i-{\mathrm{\η}}_j)],$

$g_{\mathrm{ik}}=\frac{{\∂\mathrm{\ξ}}_i}{\∂s_i}[({\stackrel{\·}{T}}_{i0}^x{p}_{\mathrm{xi}}+{\stackrel{\·}{T}}_{i0}^y{p}_{\mathrm{yi}}+{\stackrel{\·}{T}}_{i0}^z{p}_{\mathrm{zi}})+\frac{\∂s_i(f_{\mathrm{ik}},{\mathrm{\σ}}_i)}{\∂f_{\mathrm{ik}}}(-{\mathrm{\ψ}}_i\left(d_{\mathrm{ik}}\right)-k_1{r}_{\mathrm{ik}})$

$+\frac{{\∂}^2{s}_i(f_{\mathrm{ik}},{\mathrm{\σ}}_i)}{\∂f_{\mathrm{ik}}{\∂\mathrm{\σ}}_i}{r}_{\mathrm{ik}}{\stackrel{\·}{\mathrm{\σ}}}_i+\frac{{\∂}^2{s}_i(f_{\mathrm{ik}},{\mathrm{\σ}}_i)}{\∂f_{\mathrm{ik}}^2}{r}_{\mathrm{ik}}^2]+\frac{{\∂}^2{\mathrm{\ξ}}_i}{{\∂s}_i^2}{\stackrel{\·}{s}}_i^2,$

控制参数k₄＞0，k₅满足

$k_5>\underset{{\mathrm{\μ}}_i\≠0}{\max }\frac{\sqrt{k_4}\left|\mathrm{Im}\left({\mathrm{\μ}}_i\right)\right|}{\left|{\mathrm{\μ}}_i\right|\sqrt{-\mathrm{Re}\left({\mathrm{\μ}}_i\right)}}$

其中μ_i，i＝1，2，...，n，是-L的特征值，Re(·)和Im(·)分别表示数的实部和虚部。

对于运动体的沿着目标轨迹的速度达到期望值η^*(t)的编队要求，我们只需将上述运动体沿着轨迹切向上的控制力中

变成

${\tilde{G}}_{\mathrm{ik}}=-g_{\mathrm{ik}}+{\stackrel{\·}{\mathrm{\η}}}^{*}-k_6({\mathrm{\η}}_i-{\mathrm{\η}}^{*})-\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}[k_4({\mathrm{\ξ}}_i-{\mathrm{\ξ}}_j)-k_5({\mathrm{\η}}_i-{\mathrm{\η}}_j)]$

其中控制参数k₆＞0，k₅满足

$k_5>\underset{{\mathrm{\μ}}_i\≠0}{\max }(\frac{k_6}{\mathrm{Re}\left({\mathrm{\μ}}_i\right)}+\frac{k_6{\left(\mathrm{Im}\left({\mathrm{\μ}}_i\right)\right)}^2+\left|\mathrm{Im}\left({\mathrm{\μ}}_i\right)\right|\sqrt{{\left(k_6\mathrm{Im}\left({\mathrm{\μ}}_i\right)\right)}^2-4k_4\mathrm{Re}\left({\mathrm{\μ}}_i\right)({\left(\mathrm{Re}\left({\mathrm{\μ}}_i\right)\right)}^2+{\left(\mathrm{Im}\left({\mathrm{\μ}}_i\right)\right)}^2)}}{-2\mathrm{Re}\left({\mathrm{\μ}}_i\right)({\left(\mathrm{Re}\left({\mathrm{\μ}}_i\right)\right)}^2+{\left(\mathrm{Im}\left({\mathrm{\μ}}_i\right)\right)}^2)}).$

4)模块P4

模块P4综合模块P2和P3的结果计算运动体的控制力输入，完成运动体的运动控制，具体的按照以下步骤实现：

第一步：综合模块P2设计出的运动体i沿着轨迹主法和次法方向上的控制力部分和模块P3设计出沿着轨迹切向上的控制力部分，联列求解出最终的每个运动体的控制力输入

第二步：上位机将运动体的控制力输入以命令的形式发送给下位机，由运动体的伺服系统来完成对运动体的运动控制，并返回到模块P2。

Claims (5)

1.一种三维空间中多运动体的寻迹编队控制方法，其特征在于其中的运动体的动态和目标路径都是在空间直角坐标系下描述的，该方法包括如下步骤：

a)对于空间中某一平面目标轨迹，将其扩展为由不同轨迹函数值表示的不同等值平面中的等值曲线簇，并确定对应运动体的运动范围；

b)由轨迹函数计算寻迹误差，设计运动体沿着轨迹主法方向和次法方向上的控制力部分来完成寻迹；

c)利用轨迹函数和目标轨迹对应的弧长参数计算运动体沿着所在轨迹运动的广义弧长及其导数，根据信息交互得到的相邻运动体的信息，设计运动体沿着轨迹切向上的控制力部分来实现编队；

d)综合步骤b)和步骤c)得到的运动体沿着轨迹主法、次法以及切方向上的控制力部分，联列求解出运动体的控制力输入。

2、按权利要求1所述的三维空间中多运动体的寻迹编队控制方法，其特征在于：其中所述步骤a)包括如下步骤：

a1)沿着目标轨迹的主法和次法方向将目标轨迹扩展为不同等值平面中的等值曲线簇；

a2)根据曲线的正则性，确定对应运动体的运动范围；

a3)在运动范围上构建轨道函数，使扩展出的曲线簇可以由轨迹函数取不同的值来表示。

3、按权利要求1所述的三维空间中多运动体的寻迹编队控制方法，其特征在于：其中所述步骤b)包括如下步骤：

b1)由运动体的位置和轨迹函数，计算轨迹函数值与期望值间的位置寻迹误差；

b2)由位置寻迹误差对时间的导数，计算速度寻迹误差；

b3)由位置和速度寻迹误差，设计运动体沿着轨迹主法和次法方向上的控制力部分。

4、按权利要求1所述的三维空间中多运动体的寻迹编队控制方法，其特征在于：其中所述步骤c)包括如下步骤：

c1)规定好目标轨迹上弧长计算的起始点和参数，由此确定曲线簇中其他曲线上弧长计算的起始点；

c2)由轨迹函数和弧长参数，计算沿着运动体所在轨迹从起始点到运动体位置间的弧长；

c3)根据编队要求确定广义弧长与弧长间的函数关系，计算广义弧长及其对时间的导数；

c4)由信息交互得到的相邻运动体的广义弧长及其导数，设计运动体沿着轨迹切向上的控制力部分。

5、按权利要求1所述的三维空间中多运动体的寻迹编队控制方法，其特征在于：其中所述步骤d)包括如下步骤：

d1)综合步骤b和c得到的运动体沿着轨迹主法、次法以及切方向上的控制力部分，联列求解出运动体的控制力输入，

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

Images