一种采用姿态协同的多无人机集群控制方法

本发明涉及多无人机编队的高度协同集群控制领域，具体而言，涉及一种采用姿态协同的多无人机集群控制方法。

背景技术：

1、速度控制与高度控制是多无人机编队飞行面料的两个主要问题，速度控制主要是接近防避撞、防脱队与队形整齐度问题。而高度控制的精度也将影响无人机编队飞行能够保持队形完整一致不变形。由于领机、僚机、从机之间的通信存在易被干扰、延迟等特点，因此在高度控制中应当加强领机、僚机与从机之间的强耦合联系，尤其是要加强最后一级延迟后的从机的高度快速响应能力，这样才能使得无人机集群飞行具有较好的整体动态特性与抗干扰能力。同时由于无人机俯仰姿态对高度变化有着密切的关系，因此基于姿态一致与协同来实现纵向质心位置的无人机集群整体控制是非常合适的一种选择。基于上述背景原因，本发明提出了基于多级俯仰角期望信号叠加的姿态协同实现无人机高度通道的整体集群控制，以及采用姿态位置误差叠加姿态滑模控制实现解决最后一级从机的大延迟问题，具有很高的理论价值与工程实用价值。

2、需要说明的是，在上述背景技术部分发明的信息仅用于加强对本发明的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

技术实现思路

1、本发明的目的在于提供一种采用姿态协同的多无人机集群控制方法，进而克服了由于相关技术限制而导致的无人机集群编高度控制队形精度不高的问题。

2、根据本发明的一个方面，提供一种采用姿态协同的多无人机集群控制方法，包括以下五个步骤：

3、步骤s10，在领机上安装惯性导航设备，测量领机垂向速度信号与领机高度信号与领机俯仰角速度信号；然后根据飞行任务，设置领机期望高度信号；再根据领机高度信号与期望高度信号进行比较作差，得到领机高度误差信号；再针对领机高度误差信号进行积分，得到领机高度误差积分信号；再根据领机俯仰角速度信号进行积分，得到领机俯仰角信号；然后根据领机高度误差信号与领机垂向速度信号组合，得到领机俯仰角期望信号；并在领机上安装通信设备，将领机俯仰角期望信号与领机高度信号发送给僚机；并将领机俯仰角期望信号与领机俯仰角信号进行对比得到领机俯仰角误差信号；然后再叠加领机俯仰角速度信号、领机垂向速度信号，得到领机升降舵机偏转角信号。

4、步骤s20，在僚机上安装惯性导航设备，测量僚机垂向速度信号与僚机高度信号与僚机俯仰角速度信号；根据僚机高度信号与领机高度信号进行对比，得到僚机高度误差信号；再对僚机高度误差信号进行积分，得到僚机高度误差积分信号；然后根据僚机高度误差信号与僚机垂直速度信号进行叠加，得到僚机俯仰角期望信号；同时在僚机上安装通信设备，接收领机高度信号与领机俯仰角期望信号；并将僚机高度信号与领机俯仰角期望信号、僚机俯仰角期望信号发送给两个从机；然后根据僚机俯仰角速度信号进行积分得到僚机俯仰角信号；然后与领机俯仰角期望信号以及僚机俯仰角期望信号进行对比，得到僚机俯仰角误差信号；然后根据所述的僚机高度误差信号、僚机高度误差积分信号、僚机俯仰角误差信号、僚机俯仰角速率信号进行叠加，得到僚机升降舵机偏转角信号。

5、步骤s30，在一号从机上安装惯性导航设备，分别测量一号从机垂向速度信号、一号从机高度信号、一号从机俯仰角速度信号；在一号从机上安装通信设备，接收僚机高度信号与领机俯仰角期望信号、僚机俯仰角期望信号；然后根据僚机高度信号与一号从机高度信号进行对比，得到一号从机高度误差信号；然后进行积分得到一号从机高度误差积分信号；然后根据一号从机俯仰角速度信号进行积分得到一号从机俯仰角信号；然后根据一号从机供电误差信号与一号从机垂向速度信号进行叠加得到一号从机俯仰角期望信号；然后根据一号从机俯仰角信号与领机俯仰角期望信号、僚机俯仰角期望信号以及一号从机俯仰角期望信号进行对比，得到一号从机俯仰角误差信号。

6、步骤s40，根据一号从机俯仰角误差信号进行积分得到一号从机俯仰角误差积分信号；然后设计一阶滤波微分器，得到一号从机俯仰角误差滤波微分信号；再根据一号从机俯仰角误差信号、一号从机俯仰角误差积分信号、一号从机俯仰角误差滤波微分信号进行组合得到一号从机俯仰角误差滑模信号；然后进行柔化变换与指数变换，得到一号从机俯仰滑模控制；再叠加一号从机俯仰角误差滤波微分信号、一号从机俯仰角速度信号、一号从机俯仰角误差信号、一号从机高度误差信号、一号从机高度误差积分信号得到一号从机升降舵机偏转角信号。

7、步骤s50，在二号从机上安装惯性导航设备，分别测量二号从机垂向速度信号、二号从机高度信号、二号从机俯仰角速度信号；在二号从机上安装通信设备，接收僚机高度信号与领机俯仰角期望信号、僚机俯仰角期望信号；然后根据僚机高度信号与二号从机高度信号进行对比，得到二号从机高度误差信号；然后进行积分得到二号从机高度误差积分信号；然后根据二号从机俯仰角速度信号进行积分得到二号从机俯仰角信号；然后根据二号从机供电误差信号与二号从机垂向速度信号进行叠加得到二号从机俯仰角期望信号；然后根据二号从机俯仰角信号与领机俯仰角期望信号、僚机俯仰角期望信号以及二号从机俯仰角期望信号进行对比，得到二号从机俯仰角误差信号。

8、步骤s60，根据二号从机俯仰角误差信号进行积分得到二号从机俯仰角误差积分信号；然后设计二阶滤波微分器，得到二号从机俯仰角误差滤波微分信号；再根据二号从机俯仰角误差信号、二号从机俯仰角误差积分信号、二号从机俯仰角误差滤波微分信号进行组合得到二号从机俯仰角误差滑模信号；然后进行柔化变换与指数变换，得到二号从机俯仰滑模控制；再叠加二号从机俯仰角误差滤波微分信号、二号从机俯仰角速度信号、二号从机俯仰角误差信号、二号从机高度误差信号、二号从机高度误差积分信号得到二号从机升降舵机偏转角信号。

9、根据所述的主机升降舵机偏转角信号、僚机升降舵机偏转角信号、一号从机升降舵机偏转角信号、二号从机升降舵机偏转角信号操作各个无人机的高度通道实现多无人机集群的高度协同集群控制。

10、在本发明的一种示例实施例中，根据飞行任务，设置领机期望高度信号；再根据领机高度信号与期望高度信号进行比较作差，得到领机高度误差信号；再针对领机高度误差信号进行积分，得到领机高度误差积分信号；再根据领机俯仰角速度信号进行积分，得到领机俯仰角信号；然后根据领机高度误差信号与领机垂向速度信号组合，得到领机俯仰角期望信号；并在领机上安装通信设备，将领机俯仰角期望信号与领机高度信号发送给僚机；并将领机俯仰角期望信号与领机俯仰角信号进行对比得到领机俯仰角误差信号；然后再叠加领机俯仰角速度信号、领机垂向速度信号，得到领机升降舵机偏转角信号包括：

11、

12、s1＝∫e1dt；

13、

14、θ1＝∫q1dt；

15、

16、δt1＝k11e1+k12s1+k13e1a+k14q1；

17、其中e1为领机高度误差信号；h1为领机高度信号，为领机高度期望信号；s1为领机高度误差积分信号；为领机俯仰角期望信号；vh1为领机垂向速度信号；l1、l2为常值控制参数；θ1为领机俯仰角信号；q1为领机俯仰角速度信号；e1a为俯仰角误差信号；δt1为领机升降舵机偏转角信号；k11、k12、k13、k14为常值控制参数。

18、在本发明的一种示例实施例中，根据僚机高度信号与领机高度信号进行对比，得到僚机高度误差信号；再对僚机高度误差信号进行积分，得到僚机高度误差积分信号；然后根据僚机高度误差信号与僚机垂直速度信号进行叠加，得到僚机俯仰角期望信号；同时在僚机上安装通信设备，接收领机高度信号与领机俯仰角期望信号；并将僚机高度信号与领机俯仰角期望信号、僚机俯仰角期望信号发送给两个从机；然后根据僚机俯仰角速度信号进行积分得到僚机俯仰角信号；然后与领机俯仰角期望信号以及僚机俯仰角期望信号进行对比，得到僚机俯仰角误差信号；然后根据所述的僚机高度误差信号、僚机高度误差积分信号、僚机俯仰角误差信号、僚机俯仰角速率信号进行叠加，得到僚机升降舵机偏转角信号包括：

19、e2＝h2-h1；

20、s2＝∫e2dt；

21、θ2＝∫q2dt；

22、

23、

24、δt2＝k21e2+k22s2+k23e2a+k24q2；

25、其中e2为僚机高度误差信号；h2为僚机高度信号；s2为僚机高度误差积分信号；θ2为僚机俯仰角信号；为僚机俯仰角期望信号；l3、l4为常值控制参数；vh2为僚机垂向速度信号；e2a为僚机俯仰角误差信号；q2为僚机俯仰角速度信号；δt2为僚机升降舵机偏转角信号；k21、k22、k23、k24为常值控制参数。

26、在本发明的一种示例实施例中，根据僚机高度信号与一号从机高度信号进行对比，得到一号从机高度误差信号；然后进行积分得到一号从机高度误差积分信号；然后根据一号从机俯仰角速度信号进行积分得到一号从机俯仰角信号；然后根据一号从机供电误差信号与一号从机垂向速度信号进行叠加得到一号从机俯仰角期望信号；然后根据一号从机俯仰角信号与领机俯仰角期望信号、僚机俯仰角期望信号以及一号从机俯仰角期望信号进行对比，得到一号从机俯仰角误差信号包括：

27、e3＝h3-h2；

28、s3＝∫e3dt；

29、θ3＝∫q3dt；

30、

31、

32、其中e3为一号从机高度误差信号；h3为一号从机高度信号；s3为一号从机高度误差积分信号；q3为一号从机俯仰角速度信号；θ3为一号从机俯仰角信号；vh3为一号从机垂向速度信号；为一号从机俯仰角期望信号；l5、l6为为常值控制参数；e3a为一号从机俯仰角误差信号。

33、在本发明的一种示例实施例中，根据一号从机俯仰角误差信号进行积分得到一号从机俯仰角误差积分信号；然后设计一阶滤波微分器，得到一号从机俯仰角误差滤波微分信号；再根据一号从机俯仰角误差信号、一号从机俯仰角误差积分信号、一号从机俯仰角误差滤波微分信号进行组合得到一号从机俯仰角误差滑模信号；然后进行柔化变换与指数变换，得到一号从机俯仰滑模控制；再叠加一号从机俯仰角误差滤波微分信号、一号从机俯仰角速度信号、一号从机俯仰角误差信号、一号从机高度误差信号、一号从机高度误差积分信号得到一号从机升降舵机偏转角信号包括：

34、

35、s3a＝∫e3adt；

36、s3b＝c1e3a+c2s3a+c3e3ad；

37、

38、δt3＝k31e3+k32s3+k33e3a+k34q3+k35e3ad+k36δt3a；

39、其中s为一阶滤波微分器传递函数的微分算子，t1为一阶滤波微分器的常值时间参数；e3ad为一号从机俯仰角误差滤波微分信号；s3a为一号从机俯仰角误差积分信号；s3b为一号从机俯仰角误差滑模信号；c1、c2、c3为常值滑模参数；d1、ε1为变换的常值参数；δt3a为一号从机俯仰滑模控制信号；δt3为一号从机升降舵机偏转角信号；k31、k32、k33、k34、k35、k36为常值控制参数。

40、在本发明的一种示例实施例中，根据僚机高度信号与二号从机高度信号进行对比，得到二号从机高度误差信号；然后进行积分得到二号从机高度误差积分信号；然后根据二号从机俯仰角速度信号进行积分得到二号从机俯仰角信号；然后根据二号从机供电误差信号与二号从机垂向速度信号进行叠加得到二号从机俯仰角期望信号；然后根据二号从机俯仰角信号与领机俯仰角期望信号、僚机俯仰角期望信号以及二号从机俯仰角期望信号进行对比，得到二号从机俯仰角误差信号包括：

41、e4＝h4-h2；

42、s4＝∫e4dt；

43、θ4＝∫q4dt；

44、

45、

46、其中e4为二号从机高度误差信号；h4为二号从机高度信号；s4为二号从机高度误差积分信号；q4为二号从机俯仰角速度信号；θ4为二号从机俯仰角信号；vh4为二号从机垂向速度信号；为二号从机俯仰角期望信号；l7、l8为为常值控制参数；e4a为二号从机俯仰角误差信号。

47、在本发明的一种示例实施例中，根据二号从机俯仰角误差信号进行积分得到二号从机俯仰角误差积分信号；然后设计二阶滤波微分器，得到二号从机俯仰角误差滤波微分信号；再根据二号从机俯仰角误差信号、二号从机俯仰角误差积分信号、二号从机俯仰角误差滤波微分信号进行组合得到二号从机俯仰角误差滑模信号；然后进行柔化变换与指数变换，得到二号从机俯仰滑模控制；再叠加二号从机俯仰角误差滤波微分信号、二号从机俯仰角速度信号、二号从机俯仰角误差信号、二号从机高度误差信号、二号从机高度误差积分信号得到二号从机升降舵机偏转角信号包括：

48、

49、s4a＝∫e4adt；

50、s4b＝c4e4a+c5s4a+c6e4ad；

51、

52、δt4＝k41e4+k42s4+k43e4a+k44q4+k45e4ad+k46δt4a；

53、其中e4ad为二号从机俯仰角误差滤波微分信号；s4a为二号从机俯仰角误差积分信号；s4b为二号从机俯仰角误差滑模信号；c4、c5、c6为常值滑模参数；δt4a为二号从机俯仰滑模控制信号；δt4为二号从机升降舵机偏转角信号；k41、k42、k43、k44、k45、k46为常值控制参数。

54、有益效果

55、本发明提供了一种采用姿态协同的多无人机集群控制方法，其主要创新点有如下三点。第一是提出了基于姿态协同实现无人机高度集群控制的方法，充分利用了姿态与高度之间的快速反应耦合关系，使得高度集群的精度与响应速度得到有效提高。第二是提出了主机期望角速度、僚机期望角速度、与从机期望角速度层层叠加形成各级的姿态误差信号的方法，能有效地加强主机、僚机、从机之间的动态耦合与关联，能够大大提前机群在干扰作用下的紧密一致性。第三是提出了采用高度误差、姿态误差叠加姿态误差滑模控制信号以及俯仰角速率信号的方式实现从机的高度集群控制，能有效的提高从机高度响应的快速性，从而接近领机与僚机的二次延迟问题。

56、应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本发明。