一种通用型飞控及自适应串级ADRC控制算法

本发明涉及自主飞行器的应用领域，具体涉及一种通用型飞控及自适应串级adrc控制算法，本发明旨在设计一种可支持多种载具的通用型飞控及自适应串级adrc控制算法，其中该控制算法使用bp神经网络对自抗扰控制器的参数实施在线整定，从而能够实时提高飞行控制系统的精度。

背景技术：

1、近年来，由于飞行器具有体型小、成本低等优势，并伴随着飞控技术、通信技术和电子技术的快速发展，飞行器的性能不断增强、类型不断增多，使其在军用领域和民用领域中的应用需求不断增大，在各类飞行器中，飞控系统是其核心组成部分，目前飞控系统可靠性、稳定性还有待进一步提高，尤其是小型飞行器的飞控系统，还要求精简、稳定与高性价比。同时目前的大多数飞控系统都是专用于特定的机型，无法拓展多种载具的兼容。

2、飞行器控制算法方面，现有的飞行控制方法，经典的pid控制算法仍然处于主导位置，该方法也是目前工业自动化控制中应用最广泛的控制方法之一，具有结构简单，易于实现等优点。但飞控系统是非线性系统，传统的pid控制难以胜任非线性时变控制系统，且具有鲁棒性差，pid参数难以调参，不能达到高精度控制要求等缺点。想要获得更高的控制精度，就需要开拓更为先进的控制方法。

3、自抗扰控制器(英文全称，active disturbance rejection control，adrc)自pid控制器演变过来，采取了pid误差反馈控制的核心理念，把系统的未建模动态和未知外扰作用都归结为对系统的“总扰动”而进行评估并给予补偿。adrc主要是由扩张状态观测器(简称eso)、跟踪微分器(简称td)、非线性状态误差反馈(简称nlsef)三部分组成。eso是整个adrc的关键环节，它的主要功能是对未建模的动态、各种扰动进行实时的估算，以补偿系统中的不确定性。td的实现主要是对转换过程进行规划，并对差分信号进行合理的提取，提升对系统输出跟踪的速度。nlsef通过与干扰估算的补偿量相结合产生控制信号，提升系统的控制精度。其中eso的三个参数β01、β02、β03对整个飞行器飞行控制系统状态估计影响较大，而nlsef的两个参数β1、β2与pid控制中的kp、kd相仿，尽管adrc控制器较经典的pid控制而言有许多优点，但由于adrc存在参数较多，且通常采用人工经验法调参，导致参数整定过程复杂低效。

技术实现思路

1、为解决上述现有技术的不足，本发明提供一种通用型飞控及自适应串级adrc控制算法。

2、根据第一方面，本发明提供一种通用型飞控，包括：姿态传感单元，包括至少一个姿态传感器，所述姿态传感器用于感知飞行器的姿态信息，以获取姿态控制数据，包括俯仰、横滚和偏航信息；位置传感单元，包括至少一个位置传感器，所述位置传感器用于感知飞行器的位置信息，并产生位置控制数据；核心处理器，与所述姿态传感单元，所述位置传感单元分别通过spi/iic接口连接，用于根据所述姿态控制数据及所述位置控制数据解算出当前飞行器的位姿信息，并将所述位姿信息通过串行总线(tx/rx)传输至协处理器，由于采用模块化硬件设计使得飞控具有较好的拓展性；协处理器，与所述核心处理器通过串行总线(tx/rx)连接，用于接收所述核心处理器输出的所述位姿信息，同时综合无线链路中的遥控器接收机控制信号，产生飞行控制pwm信号并作用于飞行器的动力单元提供升力，最终调节飞行器的飞行姿态和位置，同样基于模块化硬件设计使得飞控具有较好的拓展性。

3、所述姿态传感单元包括加速度计模块、陀螺仪模块、电子罗盘模块、气压计模块中的一者或多者；所述加速度计模块、所述陀螺仪模块、所述电子罗盘模块、所述气压计模块感应所产生的姿态控制数据分别为飞行器三轴加速度数据、飞行器三轴陀螺仪角速度数据、磁罗盘方位数据和气压高度数据。

4、所述位置传感单元包括gps模块、激光雷达模块、视觉导航模块中的一者或多者；所述gps模块、所述激光雷达模块、所述视觉导航模块产生的位置控制数据分别为位置数据、测距高度数据和视觉导航位姿数据。

5、所述核心处理器包括spi通信模块、电源模块、led指示模块、蜂鸣器模块、usrat通信模块、姿态解算模块、usb接口模块、日志存储模块、数据存储模块、iic通信模块和mavlink通信模块，所述核心处理器采用模块化设计思想在通信标准与协议方面也使得飞控具有较好的拓展性和通用性；所述spi通信模块用于接收所述姿态传感单元和所述位置传感单元输出的所述姿态控制数据和所述位置控制数据；所述电源模块用于与飞行器动力单元相连，为所述核心处理器提供稳定工作电压，以及为系统提供电量监测功能；所述led指示模块和所述蜂鸣器模块能够组合产生声光指示信息，用于在离线调试过程中对所述核心处理器的运行状态进行指示；所述usart通信模块与使用usart通信协议的外设进行连接；所述姿态解算模块用于将接收的所述姿态控制数据和所述位置控制数据通过姿态解算和数据融合得到飞行器的姿态信息和位置信息；所述usb接口模块与外部地面站连接，用于与地面站进行有线/无线通信与调试；所述日志存储模块用于存储所述核心处理器和所述协处理器在运行过程中产生的各种状态信息与日志，以便对系统进行监测、调试和后期故障排查；所述数据存储模块用于存储飞行器在运行过程中产生的一系列数据，如所述姿态控制数据、所述位置控制数据以及自适应串级adrc控制算法相关参数；所述iic通信模块与使用iic通信协议的外设进行连接；所述mavlink通信模块用于将所述姿态解算模块输出的所述姿态信息和所述位置信息进一步传输至所述协处理器中。

6、所述协处理器包括mavlink通信模块、控制量计算模块、pwm输出模块、usart通信模块、iic通信模块和led指示模块，所述协处理器采用模块化设计思想在通信标准与协议方面也使得飞控具有较好的拓展性和通用性；所述mavlink通信模块用于接收所述核心处理器输出的所述姿态信息和所述位置信息；所述控制量计算模块用于根据当前飞行器所述姿态信息、遥控器输入控制数据和所述位置信息通过特定控制算法(自适应串级adrc控制算法)计算输出飞行器期望的pwm信号；所述pwm输出模块用于接收所述控制量计算模块输出的所述pwm信号，并传输至飞行器电调模块；所述usart通信模块与使用usart通信协议的外设进行连接；所述led指示模块能够指示飞控状态信息，用于在离线调试过程中对所述协处理器的运行状态进行指示；所述iic通信模块与使用iic通信协议的外设进行连接。

7、根据第二方面，本技术提供一种自适应串级adrc控制算法，包括：角度adrc模组，所述角度adrc模组用于接收飞行器期望姿态角和姿态解算模块输出的实时姿态角，并通过bp神经网络和自抗扰控制器相结合的控制算法输出飞行器期望角速度；角速度adrc模组，所述角速度adrc模组用于接收所述角度adrc模组输出的所述飞行器期望角速度和陀螺仪模块输出的实时角速度，并通过bp神经网络和自抗扰控制器相结合的控制算法输出飞行器期望pwm信号；电调模块，所述电调模块用于接收所述角速度adrc模组输出的所述飞行器期望pwm信号，并将该pwm信号转换成电流信号驱动无刷电机转动，为飞行器提供动力；加速度计模块，所述加速度计模块用于感知飞行器的三轴加速度数据；陀螺仪模块，所述陀螺仪模块用于感知飞行器的三轴陀螺仪角速度数据；姿态解算模块，所述姿态解算模块用于综合所述加速度计模块测得的所述三轴加速度数据和所述陀螺仪模块测得的所述三轴陀螺仪角速度数据，通过数据融合与解算，计算出飞行器所述实时姿态角(俯仰、横滚、偏航)；

8、所述角度adrc模组和所述角速度adrc模组均包括跟踪微分器、非线性状态误差反馈、扩张状态观测器、控制对象和bp神经网络，其不同之处在于所述角度ardc模组的控制对象包括所述角速度adrc模组、所述电调模块和所述多旋翼飞行器，而所述角速度adrc模组的控制对象为所述电调模块和所述多旋翼飞行器。两者的具体实现步骤如下：

9、(1)建立飞行器飞行动力学模型：

10、本技术以“x”型四旋翼飞行器为研究对象，要确切的描述废弃的运动状态，首先要建立适当的坐标轴系，这里需要建立地面坐标系e(oxyz)和机体坐标系b(oxyz)。其中地面坐标系e原点固定于地面上飞机的起飞点，ox轴指向飞机指定的飞行方向；oz轴垂直于水平面向上；oy轴垂直于oxz平面。机体坐标系b固定在机体上，原点设在飞机重心。纵轴ox平行于前后旋翼的连线，指向前方为正方向；竖轴oz平行于左右旋翼的连线，指向右方为正方向；轴oy与轴ox、oz所在平面垂直，并与轴ox，oz组成右手坐标系。下面定义三个欧拉角以确定地面坐标系与机体坐标系之间的关系：偏航角ψ——机体轴ox在地面坐标系水平面oxy上的投影x’与x轴的夹角；俯仰角θ——机体轴ox与店面坐标系水平面oxy的夹角；滚转角——过机体轴ox的铅垂面与对称面的夹角。

11、其次，需要明确旋转矩阵的基础功能：旋转矩阵的作用是将机体坐标系下表示的向量转变到地球坐标系下表示，向量的本质并不发生任何改变，只是改变了向量的表达形式。任一向量从机体坐标系b(oxyz)到地面坐标系e(oxyz)的旋转矩阵可表示为：

12、

13、式(1)中，右下方的b表示机体坐标系，右上方的e表示地面坐标系；φ，θ，ψ分别表示绕x轴旋转的滚转角、绕y轴旋转的俯仰角以及绕z轴旋转的偏航角。即欧拉角。

14、接下来，便可以根据牛顿-欧拉方程建立四旋翼的动力学模型了。动力学模型的输入为合外力及合外力矩，输出为速度和角速度。其中，用f1，f2，f3，f4分别为螺旋桨1、2、3、4所产生的拉力，由于假设了四旋翼飞行器是均匀对称的刚体，所以它们的合力-总拉力f过o点，且始终与oz轴的负方向一致。

15、(1.1)位置动力学模型

16、由牛顿第二定律可推到得到：

17、

18、式(2)中，是四旋翼飞行器的三轴方向上的速度；e3是沿着地面坐标系中oz轴的单位向量；m为四旋翼飞行器的质量。将和e3带入上式即可得到如下方程：

19、

20、上式(3)即为动力学模型中关于合外力和速度的方程，即位置动力学模型。

21、(1.2)姿态动力学模型

22、由欧拉公式可得：

23、

24、式(4)中，ωb表示在机体坐标系下的角速度；ga表示陀螺力矩；τ表示螺旋桨在机体轴上产生的力矩，包括绕ox轴的滚转力矩、绕oy轴的俯仰力矩以及绕oz轴的偏航力矩。

25、关于角速度ωb：分别用p，q，r来表示ωb在机体轴上的三个分量：

26、关于陀螺力矩ga，其计算公式如下：

27、

28、式(5)中，jrp表示整个电机转子和螺旋桨绕机体轴转轴的总转动惯量；ω1，ω2，ω3，ω4表示螺旋桨1，2，3，4的转速；

29、关于关系矩阵j，可表示为：

30、

31、带入上述欧拉公式(4)并化简整理可得：

32、

33、式(7)中，ω＝-ω1-ω2+ω3+ω4。

34、到此，完成了四旋翼飞行器的动力学模型建立。

35、(2)根据步骤(1)所示的动力学方程，设计自抗扰控制器：

36、自抗扰控制器主要由跟踪微分器、扩张状态观测器、非线性状态误差反馈三个模块组成，其基本结构、各部分连接关系为：目标指令θi经所述跟踪微分器后，得到θi的跟踪信号θ1和近似微分信号θ2，将θ1和θ2与所述扩张状态观测器的两个观测信号z1和z2形成误差信号e1＝θ1-z1和e2＝θ2-z2，经过所述非线性状态误差反馈得到初级输出信号u0，再经过扰动补偿得到所述自抗扰控制器的输出信号u，并将u输入到所述控制对象得到输出信号θ0。其中，θ0在所述角度adrc模组中代表所述飞行器的偏航角、俯仰角和滚转角中的一者，在所述角速度adrc模组中代表所述飞行器的三轴角速度其中的一个；u在所述角度adrc模组中代表所述飞行器期望角速度，在所述角速度adrc模组中代表所述飞行器期望pwm信号。

37、(2.1)二阶离散形式的跟踪微分器方程设计：

38、所述跟踪微分器用于安排过渡过程和提取近似微分信号，得到指令的跟踪值和指令的近似微分，有效解决了快速性和超调之间的矛盾，使不跳变的值能合理的跟踪跳变的值，所述跟踪微分器的二阶离散形式如公式(8)：

39、

40、其中，x1表示指令的跟踪值，x1＝θ1；x2是指令的近似微分，x2＝θ2；h为采样周期；θi(k)是第k时刻的指令信号；r为决定跟踪速度的速度因子；fst(·)是公式(3)的快速控制最优综合函数，具体算法为公式(9)：

41、

42、其中，公式(9)中的x1对应公式(8)中fst(·)函数的x1(k)-θi(k)参数；公式(9)中的x2对应于公式(8)中fst(·)函数的x2(k)参数；公式(9)中的r对应公式(8)中fst(·)函数的r参数；公式(9)中的h对应公式(8)中fst(·)函数的h参数；

43、(2.2)三阶离散扩张状态观测器的方程设计：

44、所述扩张观测器是所述自抗扰控制器的核心，其主要原理是根据状态观测器测得的系统输入和输出得到系统内部的状态量，在此基础上，引入一种新的状态变量，此状态变量是系统输入与扰动的结合，利用反馈机制对其进行观察，作用在输出上，有效的抑制扰动对系统的影响。其三阶离散形式如公式(10)所示：

45、

46、其中，z1为输出信号的估计，z2为输出信号微分的估计，z3为总扰动的估计，θ0(k)是第k时刻的输出信号，u(k)是所述自抗扰控制器在第k时刻的输出信号，通常α1取0.5，α2取0.25，δ取0.01，β01、β02、β03是所述扩张观测器需要整定的参数，其中fal(·)为公式(11)：

47、

48、其中，公式(11)中的ε对应公式(10)中fal(·)函数的ε1参数；公式(11)中的α对应公式(10)中fal(·)函数的α2参数；公式(11)中的δ对应公式(10)中fal(·)函数的δ参数；

49、(2.3)非线性状态误差反馈的方程设计：

50、所述非线性状态误差反馈通过非线性反馈结构将状态误差组合在一起，不仅提高了系统的控制性能，还通过所述扩张状态观测器的扰动补偿，实现了动态补偿线性化，具有更好的响应速度和精度，所述非线性状态误差反馈设计如公式(12)所示：

51、

52、其中，u(k)为经过扰动补偿得到的所述自抗扰控制器的输出信号，u(k)在所述角度adrc模组中代表所述飞行器期望角速度，在所述角速度adrc模组中代表所述飞行器期望pwm信号，通常α3，α4的取值为0＜α3＜1＜α4，δ2可取5h＜δ2＜10h，β1和β2是所述非线性状态误差反馈需要整定的两个控制参数，类似于pid控制中的kp和kd。

53、(3)设计bp神经网络实现自抗扰控制器在线参数整定：

54、本技术采用三层bp神经网络，所述bp神经网络输入层的节点个数为4个，在后文公式中用i表示；隐含层的节点个数为7个，在后文中用j表示；输出层的节点个数为5个，在后文中用l表示；各层节点仅与相邻节点有连接，各层内节点之间无任何连接，各层节点之间实行全连接；在后文公式中，x代表每层的输入；y代表每层的输出；上标in代表输入层；上标im代表隐含层；上标out代表输出层。

55、神经网络输入层的输入如公式(13)所示：

56、

57、其中，输入层输入节点分别为einteger＝1，其目的是为了使隐含层的个数为整数、e1和e2为所述非线性状态误差反馈的两个输入、error为所述自抗扰控制器的输入与输出的差值；

58、隐含层的输入和输出为公式(14)：

59、

60、其中，为隐含层加权系数，激活函数采用sigmoid函数：

61、

62、输出层的输入和输出为公式(16)：

63、

64、其中，输出层输出节点分别为所述扩张状态观测器的3个控制参数β01、β02、β03和所述非线性状态误差反馈的两个参数β1、β2。al为输出层增益，输出层神经元激活函数同样采用sigmoid函数。

65、定义系统的损失函数为公式(17)：

66、

67、按照梯度下降法不断迭代修改网络的权值系数即按照l(k)对权值系数的负梯度方向搜索调整，同时附加一项使搜索加快收敛全局极小的惯性项：

68、

69、上述公式(18)中，η为学习率，α为惯性系数，使用链式法则对公式(18)进行分解：

70、

71、

72、

73、由于未知，所以用近似替代，并通过调整学习率η来补偿所产生的影响，

74、网络输出层的权值学习算法为公式(22)：

75、

76、同理可以得到隐含层的权值学习算法公式(23)：

77、

78、在所述bp神经网络与所述自抗扰控制器相结合的结构中，einteger＝1、e1、e2和error＝θi-θ0作为所述bp神经网络的输入，神经元的输出对应于所述自抗扰控制器的所述扩张状态观测器三个参数β01、β02、β03和所述非线性状态误差状态反馈的两个参数β1、β2，所述bp神经网络根据飞行器系统的运行状态，不断调节所述自抗扰控制器的参数，通过神经网络的自学习、加权系数调整，使神经网络输出对应于最优控制规律下的所述自抗扰控制器参数，以达到期望的性能指标最优化。

79、本技术的有益效果是：

80、1.依据上述的第一方面，本发明公布了一种通用型飞控，具体包括姿态传感单元、位置传感单元、核心处理器和协处理器四个部分。本发明将处理器拆分成两部分，核心处理器用于接收传感信息和数据解算，协处理器用于接收指令和控制飞行。该设计大幅提高了系统的鲁棒性，同时便于飞控系统的软硬件更迭。

81、2.依据上述的第一方面，本发明公布了一种通用型飞控，该飞控可支持多种载具，具有较好的通用性。

82、3.依据上述的第二方面，本发明公布了一种自适应串级adrc控制算法，采用两级adrc串行结构，使得整个飞行控制系统更加精确，更加稳定。

83、4.依据上述的第二方面，本发明公布了一种自适应串级adrc控制算法，bp神经网络可以根据飞行器系统的运行状态，不断调节自抗扰控制器的参数，通过神经网络的自学习、加权系数调整，使神经网络输出对应于最优控制规律下的自抗扰控制器参数，以达到期望的性能指标最优化，有效解决了往昔人工经验调参费时费力的缺点。