一种基于PID控制和反步法改进的编队保持控制方法

本发明属于智能体编队控制领域，涉及一种基于pid控制和反步法改进的编队保持控制方法。

背景技术：

1、近年来，无人机在编队运动时包括各无人机之间的状态控制、协调控制，都需要考虑编队保持等技术问题。尤其在面对实际应用时，需要考虑许多问题如算法可行性、控制器的收敛性等问题。

2、通过对目前已有控制律的比较，发现以下两个关键问题：

3、(1)在追逃模型下追逐者与逃避者的编队保持控制难以应用；

4、(2)该模型下控制器设计主要集中在以逃避者为主体的结构上，追逐者难以有效对逃避者进行捕获。

5、因此，设计一种以追逐者为主体且飞行稳定的控制器是解决这一类问题的关键。

技术实现思路

1、要解决的技术问题

2、为了避免现有技术的不足之处，本发明提出一种基于pid控制和反步法改进的编队保持控制方法，首先建立四旋翼无人机的动力学模型；接下来根据三维空间中追逐者与逃避者的编队保持问题，建立编队追逐者与逃避者的相对运动学模型；然后根据已建立的模型，设计编队保持控制器。所述该方法能够根据设计的四旋翼无人机在追逃模型下追逐者与逃避者的编队保持控制律，得到稳定的飞行轨迹。

3、技术方案

4、一种基于pid控制和反步法改进的编队保持控制方法，其特征在于步骤如下：

5、步骤1：建立四旋翼无人机模型，其惯性坐标系下位置向量和姿态向量分别表示为：

6、

7、其中：m表示无人机的质量，jr无人机各旋翼转子转动惯量和，jx，jy，jz分别是转动惯量jr沿x、y、z轴的分量，g为重力加速度，φψθ为无人机飞行中的姿态角，τ1τ2τ3为无人机飞行中的姿的态角力矩，t表示运行时间；

8、所述ωg＝ω1-ω2+ω3-ω4ωi表示第i个电机的转速；

9、步骤2：建立编队追逐者和逃避者的相对运动学模型

10、

11、其中：

12、

13、

14、将λd设定为追逐者与逃避者之间的期望相对距离并设定方位角追逐者与逃避者之间的相对距离λd为预设的常值；ve，vp分别表示逃避者和追逐者的速度；vze，vzp分别表示逃避者和追逐者的速度在z轴上的分量；ωe和ωp分别表示逃避者和追逐者的航向角速度；航向角误差和分别表示追逐者和逃避者速度与x轴的夹角；

15、在x-o-y平面内追逐者与逃避者之间的相对位置误差，表示为：

16、

17、

18、

19、其中：分别表示在x轴方向和y轴方向上的期望距离；

20、步骤3：根据步骤1和步骤2的模型建立，设计编队保持控制器

21、

22、其中：为系数，t为一个时间步长；

23、以编队保持控制器作为反步法对追逐者期望的vp和ωp设计的编队保持控制律。

24、所述步骤1建立四旋翼无人机模型的过程为：

25、四旋翼无人机模型的惯性坐标系下位置向量和姿态向量分别表示为：

26、p＝[x,y,z]t

27、θ＝[φ,θ,ψ]t

28、其中，无人机在空间中的飞行状态是由位置、姿态向量描述的；

29、无人机飞行中的升力及其姿态角方向上的三个力矩均由旋翼上的电机提供，表示为：

30、τ＝[τ1,τ2,τ3]t

31、其中，各力矩可以对无人机运动时的位置和姿态进行单独控制，无人机的总升力表示为：

32、f＝[0,0,t]t

33、将u＝[t,τ1,τ2,τ3]t设为无人机的控制输入，其在惯性系下的角速度设为：

34、ω＝[p,q,r]t

35、进一步地得到无人机在惯性系与机体坐标系之间关于其角速度的旋转矩阵为：

36、

37、根据牛顿第二定律有：

38、

39、其中，m表示无人机的质量，g为重力加速度，ξ1＝[0,0,1]t，u1表示四个电机产生的升力之和；

40、根据动量矩定理可知，无人机的动力学方程表示为：

41、

42、其中，j表示系统的正定惯性矩阵，-ω×jω表示其陀螺效应，τ为控制输入矩阵，具体表示为：

43、

44、其中，l表示旋翼中心到交叉头的距离，kd表示系统的扭矩系数；

45、系统动力学模型可以分为位置运动子系统和姿态运动子系统：

46、位置运动子系统：

47、

48、姿态运动子系统：

49、

50、其中，ωg＝ω1-ω2+ω3-ω4，无人机各旋翼转子转动惯量和为jr且jx、jy、jz分别是转动惯量jr沿x、y、z轴的分量；

51、当四旋翼以较小的速度在低空飞行时，机体系下的角速度与欧拉角速度近似相等，有：

52、

53、得到无人机动力学总模型：

54、

55、所述步骤2中预设常值‖λd‖2＝λ0，

56、所述系数

57、所述四个电机产生的升力之和u1：

58、

59、其中，kt表示无人机的升力系数，ωi表示第i个电机的转速。

60、所述建立编队追逐者和逃避者的相对运动学模型的过程：

61、设计无人机垂向子系统，将其表述为以下形式：

62、

63、其中，viz表示第i个无人机在惯性系中的垂向速度；

64、建立编队在x-o-y平面的相对运动学坐标：

65、λ表示一个追逐者到一个逃避者之间的距离，表示逃避者速度与x轴的夹角，表示追逐者速度与x轴的夹角；

66、无人机的三个方向的子系统表示为：

67、

68、进一步地，将λ沿坐标系的x、y轴分解为λx,λy，具体表示为：

69、

70、

71、其中，为了保证编队能够始终朝着相同的方向运动，确保

72、得出编队在飞行过程中三个方向的子系统；

73、在三维空间中追逐者的编队保持问题描述为：

74、

75、其中，λz＝zp-ze，和表示在三个方向上的期望距离；设定航向角误差

76、

77、设vp,ωp分别为追逐者在x-o-y平面内的前飞速度和航向角速度，与前飞速度垂直的方向上存在速度约束，具体为：

78、

79、进一步地，对λx求导得：

80、

81、其中，

82、进一步地，对λy求导得：

83、

84、将λd设定为追逐者与逃避者之间的期望相对距离并设定方位角

85、追逐者与逃避者之间的相对位置关系表示为：

86、

87、当编队稳定后，追逐者与逃避者之间的相对距离λd为预设的常值；

88、所以‖λd‖2＝λ0，对相对位置关系求导并化简：

89、

90、在x-o-y平面内追逐者与逃避者之间的相对位置误差为：

91、

92、进一步地求导后得到：

93、

94、

95、为了简化运算，上述的部分项记作为：

96、

97、

98、追逐者与逃避者的相对运动学模型表示为：

99、

100、所述设计编队保持控制器的设计过程：

101、追逐者延z轴方向的相对位置误差函数设定为：

102、

103、对于垂向通道，因其与横、纵向通道完全解耦，采用pid控制方法对其进行设计；其垂向通道所需要的期望速度信号为：

104、

105、其中，kzp,kzi,kzd为系数；

106、逃避者的具体位置以及追逐者与逃避者之间的相对距离是已知的，将追逐者期望的vp和ωp作为编队整体的参考速度；

107、当ωe≠0时，逃避者在三维空间的期望飞行轨迹是弯曲的，通过误差系统进行分析可知：

108、

109、当且控制参数k1>0时，动态误差ey→0；

110、设定vp和ωp分别为：

111、

112、

113、其中：k1,k2为系数。

114、所述kzp＝2.5,kzi＝0.2,kzd＝0.3。

115、所述k1＝2.7,k2＝5.6。

116、一种计算机程序产品，其特征在于包括计算机可执行指令，所述指令在被执行时用于实现权利要求1～9所述的方法。

117、有益效果

118、本发明提出的一种基于pid控制和反步法改进的编队保持控制方法，属于智能体编队控制领域；首先建立四旋翼无人机的动力学模型；接下来根据三维空间中追逐者与逃避者的编队保持问题，建立编队追逐者与逃避者的相对运动学模型；然后根据已建立的模型，设计编队保持控制器。本发明可以应用于智能体编队控制领域中，根据设计的四旋翼无人机在追逃模型下追逐者与逃避者的编队保持控制律，得到稳定的飞行轨迹。

119、本发明的有益效果具体如下：

120、1、本发明可以应用于智能体编队控制领域中，将pid与反步法进行结合，以追逐者为主体，使得所设计的控制器有效且收敛。

121、2、本发明方法建立了一种新颖的相对运动学框架，以确保追逐者能够与逃避者保持一种稳定状态，驱赶逃避者向目标移动。与以往的方法相比，本发明方法简化了控制器的设计，提高了编队控制的可行性。

122、3、本发明方法使追逐者能够保持一个预设的编队，同时引导逃避者沿着所需的轨迹移动，相比原有方法，本发明还通过理论验证了其收敛性和有效性。