一种基于改进的转弯半径的固定翼编队控制方法与流程

本发明属于定位导航与控制技术技术领域，尤其涉及一种基于改进的转弯半径的固定翼编队控制方法。

背景技术：

随着微电子传感器、嵌入式处理器和通信技术的迅速发展，多无人机编队控制系统已成为论文广泛研究的课题。编队技术在科学研究，交通运输，地质勘测以及未来无人作战的国防领域都具有非常重要的工程应用价值。

固定翼不同于旋翼和机器人，具有最小的失速正速度和最小的转弯半径限制。僚机在编队队形保持阶段需要实时跟踪动态的坐标点，并没有预定的航线。对于固定翼飞机而言，传统的控制方法难以实现对动态目标点的实时跟踪。且当僚机不断靠近期望点时，外界微小的扰动就会造成僚机期望滚转角剧烈的震荡影响固定翼编队飞行的鲁棒性。

技术实现要素：

发明目的：为解决现有技术难以实现对动态目标点的实时跟踪等问题，本发明提供一种基于改进的转弯半径的固定翼编队控制方法。

技术方案：本发明提供一种基于改进的转弯半径的固定翼编队控制方法，该方法具体包括如下步骤：

步骤1：将长机在gps坐标系下的坐标l(l，λ，h)转换为在utm坐标系下的坐标l^u(x1，y1，z1),且令僚机相对于长机的设定偏差为ef→l(x，y，z)；

步骤2：根据僚机与长机的实时距离，确定僚机的期望点的位置；在长机保持直线飞行时，僚机当前位置与期望点的连线构成僚机的期望飞行轨迹，僚机按照期望飞行轨迹飞行；

步骤3：在长机转弯时，连接期望点b和僚机的实际位置a，构成线段ab，ab与当前时刻僚机航向的夹角为以ab为底边，以与互余的角为底角，确定一个等腰三角形；该等腰三角形的顶点为点c，结合正弦定理，求出该等腰三角形的腰长rc；

步骤4：根据编队的飞行模式、腰长rc、设定偏差ef→l(x，y，z)，并将长机的转弯半径rl将作为实时反馈，得到僚机的最优转弯半径和期望翻滚转角，僚机按照最优转弯半径和期望翻滚转角飞行。

进一步的，所述步骤2中确定僚机的期望点的位置的具体方法如下所示：

其中fc(l，λ，h)为期望点在gps坐标系下的坐标；为近距离；为远距离；为僚机与长机的实时距离；fc→l(l，λ，h)为超前跟踪点，fc→l(l，λ，h)＝g(fc1(l，λ，h)，lψ，lc→l),函数g(f，θ，d)为在gps坐标系中以坐标f为基准，向θ的方向前进距离d；lψ为长机的航向角，lc→l为点fc→l(l，λ，h)超前原始期望点fc1(l，λ，h)的距离；原始期望点fc1(l，λ，h)为在gps坐标系下的坐标，fc→tr(l，λ，h)＝g(fc1(l，λh)，lψ，-lc→tr)为滞后跟踪点；lc→tr为点fc→tr(l，λ，h)滞后原始期望点fc1(l，λ，h)的距离。

进一步的，所述求出腰长的具体方法为：

其中为当前时刻僚机与期望点之间的距离。

进一步的，所述步骤4中编队的飞行形式分为竖一字飞行和除竖一字飞行之外的其他飞行形式。

进一步的，当编队进行除竖一字飞行之外的其他飞行形式飞行时，僚机的转弯半径为：

僚机的期望翻滚转角为：

其中为航迹误差控制比例系数，vf为当前时刻长机的速度。

进一步的，当编队竖一字飞行时，僚机的转弯半径为：

ef→l(x)为僚机相对于长机x轴的给定偏差；

僚机的期望翻滚转角为：

其中为航迹误差控制比例系数,vf为当前时刻长机的速度。

有益效果：本发明结构简单，易于实现，有效降低了编队飞行的故障风险，提高了整体的鲁棒性。

附图说明

图1为本发明的整体流程图；

图2为本发明的滞后点跟踪示意图；

图3为进行三角形编队转弯即进行竖一字编队飞行时的示意图。

具体实施方式

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

本实施例提供一种基于改进的转弯半径的固定翼编队控制方法，该方法具体包括如下步骤：

步骤1：将长机在gps坐标系下的坐标l(l，λ，h)转换为在utm坐标系下的坐标l^u(x1，y1，z1),且令僚机相对于长机的设定偏差为ef→l(x，y，z)；在地面站设置队形的时候可以获得设定偏差；在utm坐标系下将l^u(x1，y1，z1)和ef→l(x,y,z)相加得到僚机的原始期望点(实际期望点)在utm坐标系下的坐标将转换到gps坐标系下，得到原始期望点在gps坐标系下的坐标fc1(l，λ，h)。

步骤2：编队跟踪保持过程中一个很大的难点便是转弯过程中的期望路径震荡，表现出来的现象便是滚转角的震荡和航向偏向相反方向。为本实施例在原始期望点附近设计超前跟踪点和滞后跟踪点。超前跟踪点的设计可以有效的解决过点转弯导致的侧向通道震荡问题，滞后跟踪点的设计使僚机在远距依旧能保持跟踪能力。定义超前跟踪点为fc→l(l,λ,h)，滞后跟踪点为fc→tr(l,λ,h)。定义函数g(f，θ，d)表示以gps坐标系下的位置f为基准点，向θ方向前进距离d。设相对于原始期望点超前距离为lc→l，相对于原始期望点滞后距离为lc→tr。则

fc→l(l,λ,h)＝g(fc(l,λ,h),lψ,lc→l)

fc→tr(l,λ,h)＝g(fc(l,λ,h),lψ,-lc→tr)

式中lψ为长机的航向角，超前点和滞后点是期望点附近沿着长机航向方向选取。

步骤3：当长机直线飞行时，定义长机与僚机的实时距离为根据实时距离确定僚机的期望点fc(l，λ，h)：

其中fc(l，λ，h)为期望点在gps坐标系下的坐标；为近距离；为远距离；僚机当前位置与期望点的连线构成僚机的期望飞行轨迹，僚机按照期望飞行轨迹飞行。

步骤4：当长机转弯时，连接期望点b和僚机的实际位置a，构成线段ab，ab与当前时刻僚机航向的夹角为以ab为底边，以与互余的角为底角，确定一个等腰三角形；该等腰三角形的顶点为点c，结合正弦定理，求出该等腰三角形的腰长rc：

步骤5：根据编队的飞行模式、腰长rc、长机的转弯半径rl、设定偏差ef→l(x，y，z)，得到僚机的最优转弯半径和期望翻滚转角。

当编队进行竖一字飞行之外的其他飞行形式飞行时，僚机的转弯半径为：

僚机的期望翻滚转角为：

其中为航迹误差控制比例系数,vf为当前时刻长机的速度。

当编队竖一字飞行时，僚机的转弯半径为：

ef→l(x)为僚机相对于长机x轴的给定偏差；

僚机的期望翻滚转角为：

如图2所示，本实施以为例，图中代表僚机的航向，表示僚机当前位置与滞后点的连线与僚机航向的夹角。和互余。代表僚机与期望点之间的距离。abc三点组成等腰三角形，显然已知僚机当前位置、僚机航向、滞后点位置，可以求解出将作为已知条件看待，结合三角形的正弦定理求出腰长rc：

如果将rc直接作为僚机的期望转弯半径，再根据定坡度转弯条件便可以求解出僚机的期望滚转角但当僚机不断靠近期望点时，rc不断较小，由于外界微小的扰动就会造成僚机期望滚转角剧烈的震荡。因此本实施例加入长机的转弯半径实时反馈。已知长机的速度为vl，滚转角为φl。则长机的实时转弯半径rl为：

将rl加入反馈后，可以得到：

rotr为最优转弯半径。最后可以得到僚机的期望滚转角为：

为航迹误差控制比例系数，由上式可知，当长机转弯时，如果僚机跟踪在期望点附近，僚机会快速跟踪长机行为，使编队队形快速收敛。当长机沿直线段稳定飞行时，的值主要由决定，即在长机保持直线飞行时，僚机横侧向指令主要由航迹误差生成。这样不仅能保证僚机在转弯过程中能快速跟随长机，使队形收敛速度加快，又能保证在直线段的跟踪段不诱发震荡。

如图3所示，当编队进行竖一字队飞行时，图中僚机1与僚机2处于同一同心圆oc；ef1→l(x)、ef2→l(x)代表僚机1和僚机2相对于长机x轴的给定偏差；则僚机1和僚机2的最优转弯半径rotr1、rotr2为：

另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。