技术领域
[0001] 本
发明涉及本发明涉及一种时空可变流场下飞行器的球面轨道编队跟踪控制方法。
背景技术
[0002] 近年来，随着通信技术、网络技术和
传感器技术的发展，为了能够有效地利用有限个运动体全方面地实现三维空间目标的信息采集，通常需要运动体各自运动在目标球面上的期望轨道并且保持一定的队形，即球面轨道编队跟踪控制问题。美国海洋局曾联合多个国家和科研机构先后开展了Argo计划以及多
水下
机器人协作海洋监测项目。NASA早在1993年就宣布开展火星探索计划，最近一次公告显示美国打算构建基于探测卫星和火星车空地一体的火星探测系统。由此可见，球面轨道编队控制技术在三维空间信息采集、监测方面具有重要的价值。
[0003] 当前，球面轨道编队控制方法大多数都忽略外部流场的影响并且飞行器的运动采用最简单的
牛顿质点(陈杨杨，王凯旋，“球面环绕编队控制的几何设计方法”，
专利号：CN201510582120.1)。然而在实际应用中，无论是海洋中鱼群、
微生物群的信息采集，还是太空星球探测，多运动体不可避免地都会受到外界环境中流场的影响(如洋流、
风等)。实际上流场不仅会使得运动体偏离自己的期望轨道而且会破坏运动体间期望的队形，进而造成采集数据
精度下降等不利的影响。当前对于有流场影响的轨道编队跟踪控制的设计几乎都集中在二维平面并且流场是时不变的(陈杨杨，“一种二维定常风速场中多机器人的寻迹编队控制方法”，专利号：ZL201310318275.5)，然而现实中无论是洋流场还是风场都是随着时间和空间变化的，显然，已有的设计方法不再适应，需要发展一种新的控制方法来解决时空可变流场下飞行器的球面轨道编队跟踪控制问题，本案由此产生。
发明内容
[0004] 本发明的目的，在于提供一种时空可变流场下飞行器的球面轨道编队跟踪控制方法，其简单可靠、精度较高，适应于任意已知时空可变流场中的协作监测等复杂任务。
[0005] 为了达成上述目的，本发明的解决方案是：
[0006] 一种时空可变流场下飞行器的球面轨道编队跟踪控制方法，飞行器的动态是球
坐标系中表示的非完整动
力学方程并且已知流场是随着时间和空间变化的，所述方法包括如下步骤：
[0007] a)用弗莱纳公式表示以球坐标系下的飞行器动力学方程；
[0008] b)计算球面跟踪误差、轨道跟踪误差以及横向编队误差；
[0009] c)设计期望的横摆
角速度、
倾侧角速度以及线
加速度，使得误差达到设计要求的同时保障飞行器不运动到南北极的连线；
[0010] d)设计横摆角和倾侧
角加速度使得实际的横摆和倾侧角速度达到期望值。
[0011] 上述步骤a)包括如下步骤：
[0012] a1)合成球坐标系中飞行器运动学方程和流场流速，计算弗莱纳方程中飞行器合成速度对应的横摆角；
[0013] a2)由横摆角计算弗莱纳方程中飞行器合成速度对应的主法方向；
[0014] a3)由合成速度对应的横摆角和主法方向，计算飞行器合成速度对应的
俯仰角和单位速度方向；
[0015] a4)计算与合成速度方向和主法方向都垂直的次法方向；
[0016] a5)由合成速度对应的主法方向、计算飞行器速度以及流场流速，计算合成速度投影到轨道所在平面中的大小；
[0017] a6)由合成速度对应的主法方向、单位速度方向、次法方向以及合成速度投影到轨道所在平面中的大小得到球坐标系下的飞行器弗莱纳方程。
[0018] 上述步骤b)包括如下步骤：
[0019] b1)由飞行器当前
位置计算其到球心的距离，进而计算球面跟踪误差；
[0020] b2)由飞行器当前位置计算纬度角，进而计算轨道跟踪纬度角误差；
[0021] b3)计算轨道所在平面内飞行器合成运动方向与轨道切方向间的轨道角度误差；
[0022] b4)由飞行器合成速度对应的俯仰角确定合成速度与轨道所在平面间的轨面角度误差；
[0023] b5)由飞行器当前位置计算经度角，再根据通信得到的邻居的经度角计算横向编队误差以及在轨速度。
[0024] 上述步骤c)包括如下步骤：
[0025] c1)由球面跟踪误差、轨道跟踪纬度角误差以及轨面角度误差设计球面跟踪控制律，使得球面跟踪误差减少到满足的设计要求；
[0026] c2)由球面跟踪误差、轨道跟踪纬度角误差、轨道角度误差、轨面角度误差以及编队误差设计轨道跟踪控制律，使得轨道跟踪误差减少到满足的设计要求，同时保障飞行器不运动到南北极的连线；
[0027] c3)由轨道角度误差，横向编队误差以及在轨速度设计横向编队控制律，使得编队误差减少到满足的设计要求；
[0028] c4)由球面跟踪控制律、轨道跟踪控制律以及横向编队控制律联列，求解出飞行器期望的
横摆角速度、倾侧角速度以及线加速度。
[0029] 上述步骤d)包括如下步骤：
[0030] d1)计算飞行器真实横摆角速度与期望的横摆角速度间的横摆角速度误差，设计横摆角加速度使得横摆角速度误差减少到满足的设计要求；
[0031] d2)计算飞行器真实倾侧角速度与期望的倾侧角速度间的倾侧角速度误差，设计倾侧角加速度使得倾侧角速度误差减少到满足的设计要求；
[0032] d3)由上位机将飞行器的控制输入发送给下位机中，通过伺服系统来完成运动控制。
[0033] 采用上述方案后，本发明简单可靠，精度较高，适用任意已知时空可变流场中的协作监测等。
附图说明
[0034] 图1是弗莱纳公式表示下的飞行器；
[0035] 图2是球面轨道跟踪和横向编队示意图；
[0037] 附图中参数说明：o：球坐标系的原点；x：球坐标系的x轴；y：球坐标系的y轴；z：球坐标系的z轴；vi：三维运动体的速度； 三维运动体速度单位方向向量；f(pi,t)：已知时空流场的流速；xi：合成速度的单位方向向量；yi：单位主法方向向量；zi：单位次法方向向量； 投影到平面的三维运动体的速度；W:旋转矩阵 第i个飞行器； 第j个飞行器； ：第k个飞行器；ψi：第i个飞行器位置对应的经度角；ψj：第j个飞行器位置对应的经度角；ψk：第k个飞行器位置对应的经度角；φi：第i个飞行器位置对应的纬度角；δi：
第i个飞行器对应的轨道角度误差；Ti：第i个飞行器期望轨道的单位切向量；R：对角矩阵diag(1,1,0)；λi：量化的距离误差；ρi：目标球的半径；γei：第i个飞行器对应的轨面角度误差。
具体实施方式
[0038] 以下将结合附图，对本发明的技术方案进行详细说明。
[0039] 本发明提供一种时空可变流场下飞行器的球面轨道编队跟踪控制方法，特别适用于球坐标系中表示的非完整飞行器动态并且已知流场是随着时间和空间变化的。令球坐标系s＝{o,x,y,z}的原点位于目标球面的球心，其z轴垂直与球面圆轨道所在的平面。当然对于目标球面的球心不在球坐标系的原点以及球面圆轨道所在的平面不垂直与z轴的情况，我们可以通过坐标系的平移和旋转来实现。在球坐标系中，已知的时空可变流场用关于位置pi和时间t的二阶连续可导函数流速f(pi,t)＝[fx(pi,t),fy(pi,t),fz(pi,t)]T表示，其中fx(pi,t)、fy(pi,t)和 fz(pi,t)分别表示流速在x、y和z方向上的强度。在已知时空可变流场作用下，第i个飞行器非完整飞行器的动力学方程如下：
[0040]
[0041] 其中， 为位置坐标，vi为线速度，为速度方向，αi和θi分别表示倾侧角和横摆角， 和 分别为倾侧角速度和横摆角速度，和 分别表示线加速度，倾侧角加速度和横摆角加速度。
[0042] 球坐标系中期望跟踪的目标球面k可以表示为：
[0043]
[0044] 其中，ρ为目标球面的半径。由于球坐标系中球面上的圆轨道上的每一点对应着相同的纬度角，这里用纬度角 来表示期望的轨道。横向编队用相邻飞行器位置对应的相对经度角来表示。各运动体之间保持期望横向队形是指：
[0045]
[0046] 其中，ψi∈(-π,π)为第i个飞行器位置对应的经度角， 为其期望的队形经度值，ψj∈(-π,π)为第j个飞行器位置对应的经度角， 为其对应的期望的队形经度值。
[0047] 在本发明中，横向编队是通过双向通信得到邻居的经度角和期望的队形经度值来实现的。这里用双向图 来描述，其中 为
节点集， 为有向边的集合。如果存在一条边连接节点 和 表明飞行器i和k可以交换信息，它们互为相邻节点(即邻居)。第i个飞行器的相邻节点集合用 表示。当图中任意两个节点间都存在着一条路径，那么图是连通的。这里的两个节点 和 间的路径是指由不同节点
和边 构成的图。图的邻接矩阵A＝[aij]可以定义为aij＞0当且仅
当 时，其他 aij＝0。设计时，我们一旦规定好飞行器群间信息交互关系，那么以后每一个时刻第i个飞行器的 都是不变的，且对应的双向图是连通的。
[0048] 图3是本发明的设计流程图，由步骤P1-P4构成，各步骤叙述如下：
[0049] 1)步骤P1
[0050] 本发明是一种时空可变流场下飞行器的球面轨道编队跟踪控制设计，已知时空可变流场可以改变非完整飞行器原来的运动速度和方向，因此要在球坐标系下考虑飞行器合成后的速度；由设计中需要完成横向编队，飞行器合成速度投影到轨道所在平面的速度需要给出；为了实现球面轨道，与飞行器合成速度方向垂直的主法方向和次法方向也需要表示；合成速度方向、主法方向和次法方向构建出时空可变流场下飞行器的弗莱纳方程，如图1所示。具体的步骤如下：
[0051] 第一步：合成球坐标系中飞行器速度 和流场流速fi(pi,t)，计算弗莱纳方程中飞行器合成速度对应的横摆角βi：
[0052]
[0053] 第二步：由横摆角βi，计算弗莱纳方程中飞行器合成速度对应的主法方向yi：
[0054] yi＝[-sinβi,cosβi,0]T；
[0055] 第三步：根据横摆角βi和主法方向yi，计算飞行器合成速度对应的俯仰角γi和单位速度方向xi：
[0056]
[0057] xi＝[cosγi cosβi,cosγi sinβi,sinγi]T，
[0058] 其中，
[0059] 第四步：计算与xi和y2i垂直的次法方向zi：
[0060] zi＝[-sinγi cosβi,-sinγi sinβi,cosγi]T；
[0061] 第五步：由合成速度对应的主法方向yi、飞行器速度vi以及流场流速f(pi,t)，计算合成速度投影到轨道所在平面中的大小
[0062]
[0063] 其中，R＝diag(1,1,0)；
[0064] 第六步：由xi、yi、zi和 得到球坐标系下的飞行器弗莱纳方程：
[0065]
[0066] 其中，
[0067]
[0068]
[0069]
[0070]
[0071]
[0072]
[0073]
[0074] 。
[0075] 2)步骤P2
[0076] 图2反映了球面跟踪误差、轨道跟踪误差以及横向编队误差。步骤P2就是根据飞行器弗莱纳方程以及通信得到的邻居信息来计算球面跟踪误差、轨道跟踪误差以及横向编队误差。具体的步骤如下：
[0077] 第一步：由飞行器当前位置pi计算其到球心的距离，进而计算与期望距离ρ间的球面跟踪误差λi：
[0078]
[0079] 第二步：由飞行器当前位置pi计算纬度角φi：
[0080]
[0081] 进而计算与期望纬度角φi*间的轨道跟踪纬度角误差φei：
[0082] φei＝φi-φi*；
[0083] 第三步：计算轨道所在平面内飞行器合成运动方向yi与轨道切方向Ti间的轨道角度误差δi：
[0084]
[0085] 其中，Ti＝[sinψi,-cosψi,0]T；
[0086] 第四步：由飞行器合成速度对应的俯仰角γi确定合成速度与轨道所在平面间的轨面角度误差γei：
[0087] γei＝γi-0；
[0088] 第五步：由飞行器当前位置pi计算经度角ψi：
[0089]
[0090] 再根据通信得到的邻居的经度角ψj计算横向编队误差 以及在轨速度ηi：
[0091]
[0092]
[0093] 3)步骤P3
[0094] 步骤P3根据球面跟踪误差、轨道跟踪纬度角误差、横向编队误差以及在轨速度，设计期望的横摆角速度、倾侧角速度以及线加速度，使得误差达到设计要求的同时保障飞行器不运动到南北极的连线。设计按下列步骤实现：
[0095] 第一步：由球面跟踪误差λi、轨道跟踪纬度角误差φei以及轨面角度误差γei，设计球面跟踪控制律
[0096]
[0097] 其中，控制增益k1＞0，
[0098]
[0099]
[0100]
[0101] 第二步：由球面跟踪误差λi、轨道跟踪纬度角误差φei、轨道角度误差δi、轨面角度误差γei、横向编队误差 以及在轨速度ηi，设计轨道跟踪控制律
[0102]
[0103] 其中，控制增益k0,k2＞0，
[0104]
[0105] 第三步：由轨道角度误差δi，横向编队误差 以及在轨速度ηi，设计横向编队控制律
[0106]
[0107] 其中，控制增益k3＞0，ηm＝2max|f(pi,t)|，期望的在轨速度η*＜ηm，[0108]
[0109] 第四步：由球面跟踪控制律 轨道跟踪控制律以及横向编队控制律 联列求解出飞行器期望的横摆角速度 倾侧
角速度 以及线加速度
[0110]
[0111]
[0112]
[0113] 其中， 。
[0114] 4)步骤P4
[0115] 步骤P4根据真实与期望的横摆角速度、倾侧角速度间的误差，设计飞行器横摆角加速度和倾侧加速度使得误差减少到满足设计的要求。设计按下列步骤实现：
[0116] 第一步：计算飞行器真实横摆角速度 与期望的横摆角速度 间的横摆角速度误差
[0117]
[0118] 进而设计横摆角加速度
[0119]
[0120] 其中，控制增益k4＞0；
[0121] 第二步：计算飞行器真实倾侧角速度 与期望的倾侧角速度 间的倾侧角速度误差
[0122]
[0123] 进而设计倾侧角加速度
[0124]
[0125] 其中，控制增益k5＞0；
[0126] 第三步：由上位机将飞行器的控制输入 和 发送给下位机中，通过伺服系统来完成运动控制。
[0127] 以上
实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案
基础上所做的任何改动，均落入本发明保护范围之内。
