一种基于积分法和反步法改进的非线性轨迹跟踪控制方法

本发明属于智能体轨迹跟踪领域，涉及一种基于积分法和反步法改进的非线性轨迹跟踪控制方法

背景技术：

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

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

3、(1)在追逃模型下编队对其期望轨迹的轨迹跟踪控制难以应用；

4、(2)该模型下控制器设计主要集中线性控制方法上，虽然能够使设计的控制器在局部工作点附近具有稳定性，但并不能使其全局稳定。

5、因此，采用非线性方法并加强其抗干扰能力是解决这一类问题的关键。

技术实现思路

1、要解决的技术问题

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

3、技术方案

4、一种基于积分法和反步法改进的非线性轨迹跟踪控制方法，其特征在于步骤如下：

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、其中：xeλx ye zeλzλy xe，ye，ze分别表示逃避者的x轴、y轴和z轴坐标；xp，yp，zp分别表示追逐者的x轴、y轴和z轴坐标；λ表示一个追逐者到一个逃避者之间的距离，将其进行三角分解成x轴、y轴和z轴分量；表示逃避者速度与x轴的夹角，如下：

12、

13、

14、λz＝zp-ze；

15、步骤3、根据步骤1和步骤2的模型设计轨迹跟踪控制器：

16、所述轨迹跟踪控制器包括高度跟踪控制器u1，偏航控制器u4和滚转u2、俯仰控制器u3，构成无人机系统的控制输入矩阵为：

17、

18、所述轨迹跟踪控制器还包括横、纵向位置跟踪控制器，

19、ux＝cosφsinθcosψ+sinφsinψ

20、uy＝cosφsinθcosψ-sinφsinψ

21、其中：kt表示无人机的升力系数，fi表示第i个电机产生的升力，l表示旋翼中心到交叉头的距离，kd表示系统的扭矩系数；

22、进一步逃避者系统中高度跟踪控制器u1的控制输入为：

23、

24、其中：自适应选择c1,c2,λ1>0，

25、进一步选择逃避者系统中滚转、俯仰的控制输入为：

26、

27、

28、其中，自适应选择c9,c10,c11,c12,λ5,λ6>0；因此，四个子系统共同构成逃避者的轨迹跟踪控制器

29、进一步逃避者系统中偏航控制输入为：

30、

31、其中，自适应选择c7,c8,λ4>0；

32、式中：e1、e2为逃避者在跟踪期望轨迹时的高度误差；e3、e4、e5、e6为逃避者在跟踪期望轨迹时的横、纵向位置误差；e7、e8为逃避者在跟踪期望轨迹时的偏航误差；e9、e10、e11、e12为在跟踪期望轨迹时的滚转、俯仰姿态误差。

33、所述四旋翼无人机的动力学模型的建立过程为：

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

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

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

37、其中：x,y,z为无人机在空间中的飞行状态的坐标位置，φ,θ,ψ为无人机在空间中的飞行状态的姿态角度，t表示矩阵转置；

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

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

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

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

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

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

44、其中：p q r分别表示翻滚、俯仰和偏航角速度；

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

46、

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

48、

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

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

51、

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

53、

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

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

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

57、位置运动子系统：

58、

59、姿态运动子系统：

60、

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

62、

63、其中：φ,θ,ψ分别表示翻滚角、俯仰角和偏航角；

64、得到无人机动力学总模型，表示为：

65、

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

67、

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

69、所述步骤2建立编队逃避者跟踪其期望轨迹的运动学模型的过程为：

70、设计无人机垂向子系统，表述为：

71、

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

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

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

75、无人机的横、纵向位置子系统表示为：

76、

77、将λ沿坐标系的x、y轴分解为λx,λy为：

78、

79、

80、将三维空间中编队的轨迹跟踪问题转化为逃避者的轨迹跟踪问题，逃避者的飞行轨迹就是编队期望的飞行轨迹，则逃避者的期望跟踪应满足：

81、

82、对于追逐者，其最终轨迹取决于λd的大小，当编队以三角形队形运动时，追逐者需要满足的条件即为编队逃避者跟踪其期望轨迹的运动学模型：

83、

84、其中：表示分别表示追逐者的x轴、y轴和z轴的期望坐标。

85、所述三维空间中，设vp,ωp分别为追逐者在x-o-y平面内的前飞速度和航向角速度，追逐者需要的速度满足：

86、

87、其中，为追逐者惯性系统中的期望速度，并将其分解为x轴，y轴和z轴分量和

88、和为追逐者惯性系下延x，y轴方向上的期望速度，表示为：

89、

90、追逐者为了保持编队队形，其航向角和偏航角之间的关系为：

91、

92、其中，vpxb和vpyb分别为vp分解到无人机体坐标系下延x，y轴方向的速度。

93、所述逃避者在跟踪期望轨迹时的高度误差e1、e2为：

94、

95、其中：zd和z分别为逃避者在跟踪期望轨迹时的期望高度和高度，系数c1,λ1>0。

96、所述逃避者在跟踪期望轨迹时的横、纵向位置误差e3、e4、e5、e6为：

97、

98、

99、其中：xd和x分别为逃避者在跟踪期望轨迹时的期望x坐标和x坐标；yd和y分别为逃避者在跟踪期望轨迹时的期望y坐标和y坐标；系数c3,c5,λ2,λ3>0。

100、所述逃避者在跟踪期望轨迹时的偏航误差e7、e8为：

101、

102、其中：ψd和ψ分别为逃避者在跟踪期望轨迹时的期望偏航角和偏航角，系数也全大于0。

103、所述逃避者在跟踪期望轨迹时的滚转、俯仰姿态误差e9、e10、e11、e12为：

104、

105、

106、其中：φd、θd为逃避者在对期望轨迹进行跟踪时其期望的滚转、俯仰角。

107、所述逃避者在对期望轨迹进行跟踪时其期望的滚转、俯仰角φd、θd为：

108、φd＝arcsin(ux sinψd-uy cosψd)

109、

110、有益效果

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

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

113、1、本发明可以应用于智能体轨迹跟踪领域中，将积分法与反步法进行结合，利用非线性控制，使得所设计的控制器有效且收敛。

114、2、本发明方法所形成的一种基于积分法和反步法改进的非线性轨迹跟踪控制方法有效使追逐者能够驱赶逃避者沿着期望轨迹运动。与以往轨迹跟踪方法相比，本发明方法验证了其收敛性和有效性。

115、3、本发明方法所形成的一种基于积分法和反步法改进的非线性轨迹跟踪控制方法有效将追逐者与逃避者构成的编队作为一个整体的飞行控制问题简化为逃避者的飞行控制问题。