专利名称：三维空间中多运动体的寻迹编队控制方法
技术领域：
本发明涉及一种三维空间中多运动体的寻迹编队控制方法。
背景技术：
众所周知，三维空间中单个运动体的寻迹控制技术因其控制律比较简单、控制系统 具有好的鲁棒性以及易于工程实践等特点被广泛应用于军事、太空探索以及空间观测领 域中。所谓单个运动体的寻迹控制，是指在控制系统的驱动下，运动体从任意初始位置 驶入预先规划好的轨迹，并沿此轨迹运动或者最终运动到目的地。例如制导导弹按预 定的弹道飞行去精确轰炸目标、航天器驶入规定航线进行太空探索以及卫星沿着椭圆轨 道监测地面信息等。近些年来，随着协调控制的发展，使得很多复杂任务可以由多个简 单运动体通过合作来共同完成，不仅降低了单个运动体制造成本，而且提高了系统的灵 活性和鲁棒性，如战斗机群编队飞行追击敌机、卫星群拍摄高清晰度的地面照片以及
多个直升机联合执行大范围的搜救任务等。因此，很多国际空间机构(如NASA和ESA) 都将三维空间中多运动体的寻迹编队控制作为21世纪一项关键的技术。
当前，已有的寻迹编队控制方法多是针对平面中的运动体，很少有考虑三维空间中 的多运动体寻迹编队。Princeton大学的Leonard教授指导的团队给出了 Serret-Frenet坐 标系下多个平面运动体在封闭轨道上的协调控制方法(F. Zhang et al, "Control of coordinated patterns for ocean sampling," Int. J. Control, vol.80, pp.ll86-1199， 2007)。为了 通过控制运动体的方向角和速率来实现寻迹编队，该方法要求运动体的速率始终不能为
0，因而造成多运动体(如飞艇)不能执行例如固定于轨道某处进行长时间的观测等 任务。由于运动体间信息交互拓扑采用的是双向链式且目标轨迹是简单闭曲线，从而也 在一定程度上限制了该方法在实际中的运用(如某些运动体只有接收转置而另一些只 安装了发送器，目标轨迹是无自交点的非封闭曲线)。与此同时，运动体在实际中很多 时候得到是在某一固定直角坐标系下确定的局部信息(如通过传感器网络或者GPS 的方式)，并且目标轨迹也是在相同直角坐标系下规划的。
因此，设计三维空间直角坐标下的多运动体的寻迹编队控制方法将更加具有现实意 义。但目前还不存在此类控制方法。
发明内容
技术问题本发明的目的是提供一种三维空间中多运动体的寻迹编队控制方法， 该方法简单可靠、精度较高，可用于多运动体协同探测星体等复杂任务。技术方案本发明是一种三维空间中多运动体的寻迹编队控制方法，特别适用于 在空间直角坐标系下描述的运动体的动态和目标轨迹。考虑空间中由n个运动体组成的 寻迹编队控制系统，运动体的运动可以看成刚体的运动，并且符合牛顿第二定律
其中q, =[^,^，&]T en3表示运动体/在空间直角坐标系中的位置坐标，
p, 二[凡,，；^,Pz,]T 3表示运动体Z'的速度，W,为运动体Z'的质量，
F, = [C,F",FzJT e□ 3是运动体/的控制力输入，f = 1,...,"。
多运动体在寻迹编队运动中，运动体间的信息交互是必不可少的，这里我们用有向 图G^V,E)来描述，其中丫 = {\,￥2,...,乂}为节点集，EcVxV为有向边的集合。如果
节点Y存在有向边到达节点X ，这就意味着运动体z'可以得到运动体y的信息，运动体_/ 是运动体/的相邻节点。运动体/的相邻节点集合用N,表示。我们说节点Y。存在一条有
向路径到达节点X，即顺序存在一组有向边(乂《),(乂,Y2)，…,d，乂)从节点X到节
点\，其中(X,X，…,X1是V中A: + 1个不同节点的集合。如果有向图中其他所有节点
都可以通过有向路径到达节点Y，则节点Y被称为全局可达点。有向图的邻接矩阵 J二[ ]可以定义为^》0当且仅当(Y，VjeE时，其他 =0。对应的Laplacian矩阵 可以表示为丄=[/&]，其中/,, = Zp,^ i/y = -ay 。图1为6个运动体间的信息交互拓扑 对应的有向图，其中节点{1,2，6}是全局可达点。设计时，我们一旦规定好多运动体间 信息交互关系，那么以后每一个时刻运动体/的N,都是不变的，且对应的有向图含有全 局可达点。工程应用中，我们可以根据实际情况灵活的采用通信的方式、传感的方式以 及两种方式相结合的方式来实现多运动体间的信息交互。
对于三维空间中运动体/的目标轨迹q。， q。是空间某一平面中的一条光滑的正则
曲线，可以是简单闭曲线或者是无自身交点的非封闭曲线，并且对于C。上每一点，曲
率^一致有界且不为o。 q。在空间直角坐标系可以用方程组表示为
〖'N,。 (， = ^
气0
〖* B,0 (, = &其中n,。(.)和B,。(.)为q。的单位主法向量和单位次法向量并且是光滑的，
q二[&,&,&]TeW为c;。上的点在空间直角坐标系中的位置坐标，q;。为q。上的一点， 《和g是两个常实数。由于q。是空间某一平面中的曲线，6,。三o。令t,。-n,。aB,。表 示q。的切向量，对于q。上所有的点t,。^0，其中"a"表示向量积。本发明的目的就
是根据得到的相邻运动体的信息，设计每个运动体的控制力使其运动在目标轨迹的同 时，运动体间保持一定的队形。
在本发明中，对于沿着目标轨迹运动的各运动体之间的编队位置关系采用如下方式 规定设目标轨迹q。上的固定点q^为弧长计算的起始点，^是从起始点q!。沿着目标 轨迹到运动体位置间的路程(即弧长)，广义弧长《；r,h)是关于s,—个具有二阶连续 导数的函数(该函数应满足对于所有的&满足3《/&,#0)。各运动体之间保持编队位 置关系是指
如果运动体/的目标轨迹是由参考轨迹平移期望的队形向量h,而得到的(如图2所 示)，规定的弧长计算的起始点满足q:。-q;。=h,-、，那么我们可以简单的定义弧长s, 即为广义弧长《，弧长s,和运动体沿着目标轨迹的速度达到一致就能保证多运动体以期 望的队形q,々)-q,(/):h,-、编队运动。对于更复杂的情况，弧长s,和运动体沿着目标 轨迹的速度达到一致并不能保证多运动体以期望的队形编队运动。图3所示的是三个运 动体运动在不同半径及,的同心圆轨迹上并且保持与圆心在同一条直线上的例子，显然 只有当《=&/《及其导数达到一致时才能保证多运动体以期望的队形编队运动。因此， 本方法选择用广义弧长《及其导数 7,来描述多运动体的编队运动。
本发明的设计思想是，先将目标轨迹q。扩展为不同平面中关于轨迹函数A(q)的等值
曲线簇，其中不同平面是关于轨迹函数A(q)的一组等值平面，并且由正则条件确定对
应运动体i的运动范围q,。通过设计运动体沿着轨迹主法n,。(q,) =(q,)和次法
B,。 (q,) = (q,)方向上的控制力部分使得初始位于q,中的运动体始终运动于q,并且
轨迹函数值乂"q,;)和A(q,)与q。对应的期望值《。和&间的误差《w和&w及其对
时间的导数&G)和&(0减少到0。当运动体运动在各自的目标轨迹上，多运动体的编 队运动就退化为运动体沿着轨迹运动的位置与速度达到一致。根据信息交互得到相邻运 动体的信息，设计运动体沿着轨迹切向T,。 (q,)上的控制力部分使得广义弧长《(0及其 导数7, G)达到一致来实现多运动体的编队运动。 具体的讲
本发明是一种三维空间中多运动体的寻迹编队控制方法，包括如下步骤
5a) 对于空间中某一平面目标轨迹，将其扩展为由不同轨迹函数值表示的不同等值 平面中的等值曲线簇，并确定对应运动体的运动范围；
b) 由轨迹函数计算寻迹误差，设计运动体沿着轨迹主法和次法方向上的控制力来 完成寻迹；
c) 利用轨迹函数和目标轨迹对应的弧长参数计算运动体沿着所在轨迹运动的广义 弧长及其导数，根据信息交互得到的相邻运动体的信息，设计运动体沿着轨迹 切向上的控制力来实现编队；
d) 综合步骤b和c得到的运动体沿着轨迹主法、次法以及切方向上的控制力部分， 联列求解出运动体的控制力输入。
其中所述的运动体的动态和目标轨迹都是在空间直角坐标系下描述的。 其中所述步骤a)包括如下步骤
al)沿着目标轨迹的主法和次法方向将其扩展为不同等值平面中的等值曲线簇； a2)根据曲线的正则性，确定对应运动体的运动范围；
a3)在运动范围上构建轨道函数，使得扩展出的曲线簇可以由轨迹函数取不同的值 来表示。
其中所述步骤b)包括如下步骤
bl)由运动体的位置和轨迹函数，计算轨迹函数值与期望值间的位置寻迹误差； b2)由位置寻迹误差对时间的导数，计算速度寻迹误差；
b3)由位置和速度寻迹误差，设计运动体沿着轨迹主法和次法方向上的控制力部分。 其中所述步骤c)包括如下步骤
cl)规定好目标轨迹上弧长计算的起始点和参数，由此确定曲线簇中其他曲线上弧 长计算的起始点；
c2)由轨迹函数和弧长参数，计算沿着运动体所在轨迹从起始点到运动体位置间的 弧长；
c3)根据编队的要求确定广义弧长与弧长间的函数关系，计算广义弧长及其对时间 的导数；
c4)由信息交互得到的相邻运动体的广义弧长及其导数，设计运动体沿着轨迹切向
上的控制力部分。 其中所述步骤d)包括如下步骤
dl)综合步骤b和c得到的运动体沿着轨迹主法方向、次法以及切方向上的控制力
部分，联列求解出运动体的控制力输入。 d2)由上位机将运动体的控制力输入发送给下位机中，通过伺服系统来完成运动控制。
6当然，如果编队要求运动体的沿着目标轨迹的速度达到期望值/TG)，我们只需改 变上述的步骤C4)中运动体沿着轨迹切向上的控制力的设计即可。
有益效果本方法具有简单可靠、精度较高以及便于实际运用的特点，可用于多 运动体协同探测星体等复杂任务。
图1为信息交互拓扑对应的有向图2为多运动体以三角形队形编队运动在目标轨迹上；
图3为多运动体在同心圆轨迹上的编队运动；
图4为不同等值平面中的等值椭圆簇；
图5为单个运动体在等值椭圆簇中的运动；
图6为三维空间中多运动体的寻迹编队控制设计流程图。
以上的图中有1:运动体l; 2:运动体2; 3:运动体3; 4:运动体4; 5:运动 体5; 6:运动体6; JC :三维直角坐标系中的X轴；_y :三维直角坐标系中的^轴；Z: 三维直角坐标系中的Z轴；O:三维直角坐标系中坐标原点；/ :运动体"7':运动体 运动体"G:参考轨迹；q。运动体/的目标轨迹；:运动体_/的目标轨 迹；C;。运动体A:的目标轨迹；h,:运动体/对应的期望队形向量；h,运动体_/对应 的期望队形向量；运动体t对应的期望队形向量；q;。 q。上弧长计算的起始点； q;。 q。上弧长计算的起始点；q〖。Q。上弧长计算的起始点；C^上的弧长；V q。上的弧长；C;。上的弧长；i,:运动体/沿q。的速度；i,运动体^沿q。的速度； 4:运动体^沿C;。的速度；《q。上的广义弧长；q。上的广义弧长；&: Q。上 的广义弧长；化《对时间的导数；6对时间的导数；^对时间的导数；N,。 单位主法向量；B,。单位次法向量；Q和Q: q。上的每一点沿N,。平移不同实数得到 两条Bertrand曲线；Q和Q: q。上的每一点沿B,。平移不同实数得到两条曲线；i3。
q。
所在的平面；A: Q所在的平面；户2 : Q所在的平面；T,。单位切向量；:弧长 参数；q;1:(;上弧长计算的起始点；q;2 : Q上弧长计算的起始点；q:3: Q上弧长计 算的起始点；
具体实施例方式
图6是本发明的设计流程图，由模块尸l、尸2、尸3和户4构成，各模块叙述如下 1)模块户l
由于本发明设计分布式控制律的前提是将每条目标轨迹扩展为由不同轨迹函数值 表示的不同等值平面中的等值曲线簇，模块尸l用于得到曲线簇、轨迹函数以及运动体的运动范围。
如图4所示，目标轨迹q。为空间中平面P。椭圆曲线。在q。附近，将q。上的每一
点沿着其主法N,。(。方向平移不同的实数4，我们可以得到不同的椭圆曲线(如Q和
q2);将q。上的每一点沿着其次法B,。0方向平移不同的实数l，相当于将q。平移到
与P。平行的不同平面中(如S中的Q和A中的Q)。类似的，我们可以将一般的目标轨迹扩展。模块P1具体按下列步骤实现
第一步在q。附近，将q。上的每一点分别沿着其主法N,。0和次法B,。0方向平移实数4和&得到扩展曲线c;。当《,=0而4取不同值，相当于在q。所在平面A中
将q。扩展为一组Bertrand曲线，即每条曲线的主法向量与q。相同。当&为常值而&取
不同值，相当于
将P。平面中一条Bertrand曲线平移到与P。平行的一组平面中。
第二步由扩展曲线的切向量i;:i;。(i-K。)以及正则条件T;^0，我们选择运
动体/的运动范围Q,为空间中所有满足|4|<5的曲线上点的集合，其中s e 。，l/sup{|《。|})。
第三步由于集合Q,中的每一点都属于与P。平行的某个平面中的一条Bertrand曲线，我们可以在Q,上构建轨道函数
当q,q;^ e Q ，其中每一条Bertrand曲线都是某一平面中的关于^ (q)的一条等值曲线，该平面是关于A(q)的一个等值平面。由轨道函数，运动范围可以写成Q, = {q e I, ^ □ 3 |cn < _4 (q) < c,2}其中q和c,2为两个实数。2)模块P2
模块户2根据轨迹函数设计运动体沿着目标轨迹主法和次法方向上的控制力部分用以完成寻迹。设计按下列步骤实现
第一步计算寻迹位置误差《(/)和KO
A (q) = ￡ N,。 (, = 4 ， & (q)=B,。(,=《
第二步
《=A(q,)D，计算寻迹速度误差^ (0和& (0
第三步
设计运动体/沿着轨迹主法和次法方向上的控制力《,+礼
&《,+
糾
祭《+祭《+祭《+2|^ +2^^A +2急凰
W《％ *A 电' &电 z
g 《"&&询/ " 屯^
〖汰)-V汰:一^4 一& '
-tan,-^~1
2(c,广c,1)
、2(c,2-c,i)
控制参数&，&,&>0， K(《)用于保证初始位于Q,的运动体/始终在Q,中运动，并且当《^0时v/,(《)-0。3)模块尸3
如图5所示，运动体z'沿着轨迹切向T,。(q,)上的运动可以改变运动体沿着当前所在轨迹运动的弧长^。与此同时，运动体i投影到轨迹主法方向N,。(q,)上的运动可以改变同一等值平面中运动体所在的曲线，同样也可以改变弧长s,。而运动体z'投影到轨迹次法方向B,。(q,)上的运动只是改变轨迹所在的平面，并不改变弧长^。因此，本方法选
择目标轨迹对应的弧长参数C7,来刻画运动体运动在Q,中所有轨迹上的弧长。
模块根据信息交互得到相邻运动体的广义弧长^ W及其导数W来设计运动体/沿着轨迹切向上的控制力部分用以实现编队，具体的设计步骤如下
第一步规定好目标轨迹上弧长计算的起始点q:。和参数cr,，由此我们可以选择曲线簇中其他曲线上弧长计算的起始点q^使得q^和q:。对应的弧长参数值是一致的。
第二步由轨迹函数和弧长参数cr,，计算沿着运动体当前所在的轨迹从起始点q:到运动体位置q,间的弧长s,
其中为q!。对应的弧长参数值；
第三步由编队的要求设计好广义弧长函数《，即《是关于^一个具有二阶连续导
数的函数，并且满足对于所有的^满足5《/&,*0。由弧长s,计算广义弧长《G,)及其对时间的导数/7,
9<formula>formula see original document page 10</formula>
第四步根据信息交互所得到的相邻运动体的广义弧长(及其导数^，设计运动体沿着轨迹切向上的控制力
、—1
a+^,+7^,=T,0(q,).F,,
其中
<formula>formula see original document page 10</formula>控制参数&>0， A:,满足
<formula>formula see original document page 10</formula>
其中//,， i=l,2,...,"，是-丄的特征值，ReO和ImO分别表示数的实部和虚部。
对于运动体的沿着目标轨迹的速度达到期望值^G)的编队要求，我们只需将上述运动体沿着轨迹切向上的控制力中《t变成
<formula>formula see original document page 10</formula>
其中控制参数&>0， ^满足
<formula>formula see original document page 10</formula>
4)模块P4
模块户4综合模块P2和P3的结果计算运动体的控制力输入，完成运动体的运动控制，具体的按照以下步骤实现
第一步综合模块户2设计出的运动体/沿着轨迹主法和次法方向上的控制力部分和模块尸3设计出沿着轨迹切向上的控制力部分，联列求解出最终的每个运动体的控制<formula>formula see original document page 11</formula>第二步上位机将运动体的控制力输入以命令的形式发送给下位机，由运动体的伺服系统来完成对运动体的运动控制，并返回到模块尸2 。
权利要求
1.一种三维空间中多运动体的寻迹编队控制方法，其特征在于其中的运动体的动态和目标路径都是在空间直角坐标系下描述的，该方法包括如下步骤a)对于空间中某一平面目标轨迹，将其扩展为由不同轨迹函数值表示的不同等值平面中的等值曲线簇，并确定对应运动体的运动范围；b)由轨迹函数计算寻迹误差，设计运动体沿着轨迹主法方向和次法方向上的控制力部分来完成寻迹；c)利用轨迹函数和目标轨迹对应的弧长参数计算运动体沿着所在轨迹运动的广义弧长及其导数，根据信息交互得到的相邻运动体的信息，设计运动体沿着轨迹切向上的控制力部分来实现编队；d)综合步骤b)和步骤c)得到的运动体沿着轨迹主法、次法以及切方向上的控制力部分，联列求解出运动体的控制力输入。
2、 按权利要求l所述的三维空间中多运动体的寻迹编队控制方法，其特征在于-其中所述步骤a)包括如下步骤al)沿着目标轨迹的主法和次法方向将目标轨迹扩展为不同等值平面中的等值曲线簇； a2)根据曲线的正则性，确定对应运动体的运动范围；a3)在运动范围上构建轨道函数，使扩展出的曲线簇可以由轨迹函数取不同的值来表示。
3、 按权利要求l所述的三维空间中多运动体的寻迹编队控制方法，其特征在于 其中所述步骤b)包括如下步骤bl)由运动体的位置和轨迹函数，计算轨迹函数值与期望值间的位置寻迹误差； b2)由位置寻迹误差对时间的导数，计算速度寻迹误差；b3)由位置和速度寻迹误差，设计运动体沿着轨迹主法和次法方向上的控制力部分。
4、 按权利要求l所述的三维空间中多运动体的寻迹编队控制方法，其特征在于 其中所述步骤C)包括如下步骤cl)规定好目标轨迹上弧长计算的起始点和参数，由此确定曲线簇中其他曲线上弧长计 算的起始点；C2)由轨迹函数和弧长参数，计算沿着运动体所在轨迹从起始点到运动体位置间的弧长; C3)根据编队要求确定广义弧长与弧长间的函数关系，计算广义弧长及其对时间的导数; c4)由信息交互得到的相邻运动体的广义弧长及其导数，设计运动体沿着轨迹切向上的 控制力部分。
5、 按权利要求l所述的三维空间中多运动体的寻迹编队控制方法，其特征在于 其中所述步骤d)包括如下步骤dl)综合步骤b和c得到的运动体沿着轨迹主法、次法以及切方向上的控制力部分，联列求解出运动体的控制力输入，d2)由上位机将运动体的控制力输入发送给下位机中，通过伺服系统来完成运动控制。
全文摘要
本发明是一种三维空间中多运动体的寻迹编队控制方法，包括如下步骤a)对于空间中某一平面目标轨迹，将其扩展为由不同轨迹函数值表示的不同等值平面中的等值曲线簇，并确定对应运动体的运动范围；b)由轨迹函数计算寻迹误差，设计运动体沿着轨迹主法和次法方向上的控制力来完成寻迹；c)利用轨迹函数和目标轨迹对应的弧长参数计算运动体沿着所在轨迹运动的广义弧长及其导数，由信息交互得到的相邻运动体的信息，设计运动体沿着轨迹切向上的控制力来实现编队；d)综合步骤b和c计算运动体的控制力输入。本发明对空间直角坐标系下描述的运动体的动态和目标轨迹尤其适用。该方法简单可靠、精度较高，可用于多运动体协同探测星体等复杂任务。
文档编号G05D1/00GK101650569SQ20091018454
公开日2010年2月17日 申请日期2009年8月31日 优先权日2009年8月31日
发明者田玉平, 陈杨杨 申请人:东南大学