CN108549208A - 一种基于因子自适应模糊pid的四旋翼飞行器姿态控制方法 - Google Patents

Abstract

本发明公开了一种四旋翼飞行器姿态控制方法，首先获取四旋翼飞行器当前的姿态角信息，所述姿态角包括俯仰角、翻滚角和偏航角，并计算给定的期望姿态角与当前姿态角的误差量，然后确定量化因子、比例因子与误差、误差变化率之间的模糊逻辑关系并制定模糊规则表，最后以误差和误差变化率作为输入，以量化因子和比例因子作为输出建立基于因子自适应的模糊PID控制器，实现其量化因子、比例因子在线实时调整。该控制方法具有更好的动态跟踪效果以及系统鲁棒性，为四旋翼飞行器更灵活的完成飞行任务提供可靠保障。

Description

一种基于因子自适应模糊PID的四旋翼飞行器姿态控制方法

技术领域

本发明属于四旋翼飞行器控制技术领域，涉及一种能够对四旋翼飞行器的姿态实现稳定控制的算法。

背景技术

近年来，四旋翼飞行器因其具有垂直起降、可携带负载、方便控制、低成本、结构简单的特点而获得研究人员越来越多的关注。由于这些特点，四旋翼飞行器被广泛应用于众多领域比如：监控安保、搜寻营救、航拍、军事侦查等领域。但是四旋翼飞行器系统非常复杂，具有多变量、非线性、强耦合、欠驱动等特点，飞行过程中易受到系统不确定性和外界干扰，影响稳定性，这使得四旋翼飞行器的飞行控制系统设计难度较大。四旋翼飞行器控制系统中，姿态控制是整个飞行控制的关键，因为四旋翼飞行器的姿态与位置存在直接耦合关系，精确的姿态控制是实现位置控制的基础。因此，亟需对四旋翼行器姿态的稳定控制进行更加深入的研究，为四旋翼飞行器更灵活的完成飞行任务提供可靠保障。

目前，模糊PID(Proportion Integration Differentiation)控制因其不依赖于被控对象的精确数学模型，并且有较好的鲁棒性，被广泛应用于四旋翼飞行器的控制系统中。但是在使用模糊控制时，量化因子和比例因子的选取通常具有一定的主观性和盲目性，往往需要做大量实验才能取得合适的量化因子和比例因子，而且采用固定的量化因子和比例因子往往达不到预期的控制效果。因此实现量化因子和比例因子的在线调整，进一步提高模糊控制的性能是非常必要的。

发明内容

有鉴于此，本发明的目的在于针对现有技术的上述量化因子和比例因子不能实时优化问题，提出一种因子自适应模糊PID控制方法，该控制方法能实现量化因子和比例因子的在线调整，使得模糊PID控制器性能提高，从而使四旋翼飞行器的抗干扰能力加强，实现飞行姿态的实时调整。

为达到上述目的，本发明提出的一种基于因子自适应模糊PID的四旋翼飞行器姿态控制方法，包括如下步骤：为实现上述目的，本发明的技术方案如下：

一种基于因子自适应模糊PID的四旋翼飞行器姿态控制方法，包括以下步骤：

S1：四旋翼飞行器通过机身的姿态传感器，实时采集四旋翼飞行器的加速度和角速度信息，经过姿态解算得到四旋翼飞行器当前的姿态角信息，主要包括四旋翼飞行器当前的俯仰角信息，当前的翻滚角信息，当前的偏航角信息。

S2：给定给定期望姿态角，包括给定期望俯仰角，期望翻滚角，期望偏航角。由步骤1中实时采集的当前姿态角信息，计算三个期望姿态角与当前姿态角之间的差值，计算方法如式(1)所示：

e＝[θ_e-θ，φ_e-φ，ψ_e-ψ]^T (1)

其中θ、φ、ψ分别为实际的俯仰角、翻滚角、偏航角；θ_e、φ_e、ψ_e分别为给定期望俯仰角、期望翻滚角、期望偏航角；

S3：整定量化因子和比例因子：在系统控制的作用下，误差逐渐缩小，误差的模糊划分更多地落在ZE(零)附近，此时的初始论域[-E，E]相对于缩小后的误差偏大，模糊推理仅仅用到ZE附近的规则，系统的控制精度不高。变论域的控制方法就是不改变控制器的模糊规则和隶属度函数，但其论域随着误差变小而伸缩，本发明引入量化因子实现论域的伸缩变换，通过在线调整量化因子，实现论域的伸缩变换。量化因子的整定方法为：根据误差e和误差变化率ec进行在线调整，量化因子与e和ec成单调递减趋势，设计公式为：

其中a为函数曲线的伸缩因子，其大小能决定函数曲线在u＝0与u＝1之间的形状，c控制函数曲线中心对称点的横坐标，b和λ控制将k₀进行缩放的范围，n表示误差的量化等级数的个数，k₀表示量化因子初始值，同时选取比例因子为整定后的量化因子的倒数；量化因子和比例因子的整定，使得步骤S4PID参数整定中，当误差逐渐减小时，误差量的附近始终都有较多的模糊划分，依然可以运用较多的模糊规则，消除了模糊规则数量与控制精度的矛盾，增加了控制灵敏度。

S4：整定PID参数：PID参数整定过程中，输入为k_e*e和k_ec*ec，输出为PID控制参数ΔK_p、ΔK_i、ΔK_d；根据输入的k_e*e和k_ec*ec，查询模糊控制规则表得到PID控制参数的模糊值；最后结合Mamdani推理算法和重心法将PID控制参数的模糊值清晰化，得到清晰化的PID控制参数，再乘以比例因子k_u，得到最终的ΔK_p、ΔK_i、ΔK_d，则微调之后PID参数如式(3)所示：

式中K_p0、K_i0、K_d0是初始设定值。

该方法所用位置式PID算法如式(4)所示：

u(k)＝[K_p0+ΔK_p(k)]*e(k)+[K_i0+ΔK_i(k)]*∑e(k)+[K_d0+ΔK_d(k)]*[e(k)-e(k-1)] (4)

式中u(k)为第k个控制周期的期望PWM占空比值，即电机控制量。

本发明的有益效果在于：本发明所提出的一种基于因子自适应模糊PID的四旋翼飞行器姿态控制方法，在模糊PID控制的基础上引入变论域思想，通过找出量化因子、比例因子与误差和误差变化率之间的关系，建立带修正因子的模糊PID控制器，实现量化因子和比例因子的在线自整定，在系统运行的不同阶段采用不同的量化因子和比例因子来调整控制效果。解决了量化因子、比例因子的实时优化问题，无需再通过大量实验获得最优的量化因子、比例因子。

在实际应用过程中，可以根据实际四旋翼飞行器情况给定任意期望姿态角，并将由该方法得到的控制量传输至执行机构实现四旋翼飞行器的姿态控制。

附图说明

为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：

图1为本发明四旋翼飞行器系统整体框图；

图2为本发明提出的因子自适应模糊PID控制方法的控制原理图；

图3为本发明四旋翼飞行器姿态运动示意图；

图4为本发明提出的姿态控制方法具体流程图；

图5为本发明提出的量化因子整定函数曲线图。

具体实施方式

以下结合附图对本发明作进一步说明。

1.四旋翼飞行器包括电源模块，处理器模块，传感器模块包括姿态传感器模块和气压传感器模块，执行机构电机，系统整体框图如图1；其中姿态传感器用来实时采集四旋翼飞行器的加速度和角速度信息，经过姿态解算得到四旋翼飞行器当前的姿态角信息。

2.图2是本发明所提出的的一种因子自适应模糊PID控制方法的控制原理图，图中包括输入信号IN，量化因子比例因子模糊控制器模块，PID自适应模糊控制器模块，常规PID控制器模块，被控对象四旋翼飞行器，姿态传感器模块，系统输出量OUT；输入信号经过最终经过常规PID控制器模块，作用于被控对象四旋翼飞行器上，得到的系统输出量经过传感器，转换为与输入信号形式相同的量，并与输入信号进行比较，将信号的误差量作为输入变量，输入到量化因子比例因子模糊控制器模块，实现量化因子比例因子的在线调整，再经过PID自适应模糊控制器模块，作用到常规PID控制器模块上，实现PID参数的在线整定以及系统闭环控制。

3.本发明所提出的因子自适应模糊PID控制方法，具体步骤如下：

S1：四旋翼飞行器通过机身的姿态角传感器，实时采集四旋翼飞行器的加速度和角速度信息，经过姿态解算得到四旋翼飞行器当前的姿态角信息，主要包括四旋翼飞行器当前的俯仰角信息，当前的翻滚角信息，当前的偏航角信息。

S2：给定给定期望姿态角，包括给定期望俯仰角，期望翻滚角，期望偏航角。由步骤1中实时采集的当前姿态角信息，计算三个期望姿态角与当前姿态角之间的差值，计算方法如公式(1)所示：

e＝[θ_e-θ，φ_e-θ，ψ_e-ψ]^T 公式(1)

其中θ、φ、ψ分别为实际的俯仰角、翻滚角、偏航角，如图3所示；θ_e、φ_e、ψ_e分别为给定期望俯仰角、期望翻滚角、期望偏航角；

S3：整定量化因子和比例因子：在系统控制的作用下，误差逐渐缩小，误差的模糊划分更多地落在ZE(零)附近，此时的初始论域[-E，E]相对于缩小后的误差偏大，模糊推理仅仅用到ZE附近的规则，系统的控制精度不高。变论域的控制方法就是不改变控制器的模糊规则和隶属度函数，但其论域随着误差变小而伸缩，本发明引入量化因子实现论域的伸缩变换，通过在线调整量化因子，实现论域的伸缩变换。误差的量化因子的定义为：(n为误差变化率的量化级数，e为误差实际值)，误差变化率的量化因子的定义为(n为误差变化率的量化级数，ec为误差变化率的实际值)。

所采用量化因子比例因子的整定规则如下：

(1)在误差e和误差变化率ec大时，取小的k_e和k_ec、大的k_u；

(2)在误差e和误差变化率ec较大时，取较小的k_e和k_ec、较大的k_u；

(3)在误差e和误差变化率ec较小时，取较大的k_e和k_ec、较大的k_u；

(4)在误差e和误差变化率ec小时，取大的k_e和k_ec、小的k_u；

对以上规则的说明：

1)当误差e和误差变化率ec较大时，应当取较小的k_e和k_ec以降低对输人量e和ec的灵敏度，同时取较大的k_u以增大控制量的变化，加快系统的过渡过程。

2)当误差e和误差变化率ec较小时，应当取较大的k_e和k_ec以提高对输人量e和ec的灵敏度，同时取较小的k_u减小控制量的变化，抑制系统响应超调量的增加，使系统尽快达到稳态。

基于上述规则，本发明提出如公式(2)所示函数关系式，以满足所述整定规则：

其中λ≥b＞0，5＜a＜13，a为函数曲线的伸缩因子，其大小能决定函数曲线在u＝0与u＝1之间的形状，c控制函数曲线中心对称点的横坐标，b和λ控制将k₀进行缩放的范围，n表示误差的量化等级数的个数，k₀表示量化因子初始值。

本发明结合控制经验，对a、b、c、λ、k₀取值，绘制出函数曲线示意图如图5所示：

由图5可以看出，函数曲线整体呈单调递减趋势，满足所述整定规则的(1)～(4)条。更进一步，从图中可以看出，在误差和误差的变化率从大到小的变化过程中，函数曲线斜率的绝对值是先增大，后减小。斜率绝对值增大阶段，使得在误差较大时，量化因子能更加快速的变化，使得PID控制器能更加快速的收敛；斜率绝对值减小阶段，使得在误差较小时，量化因子能更加稳定的变化，使得PID控制器能更加稳定的收敛于期望值，减小波动。

S4：整定PID参数：PID参数整定过程中，输入为k_e*e和k_ec*ec，即将姿态角的误差量和误差变化率乘以量化因子后作为模糊控制器的输入，输出控制参数ΔK_p、ΔK_i、ΔK_d；输入量和输出量的的模糊集合定义为{NB，NM，NS，ZE，PS，PM，PB}其中，NB为负大、NM为负中、NS为负小、ZE为零、PS为正小、PM为正中、PB为正大；输入量和输出量的隶属度函数均选取三角形函数；采用7×7的模糊推理规则，由k_e*e和k_ec*ec通过模糊控制规则表查询推理得到PID参数的模糊语言变量，可供参考的模糊规则表如表1；最后进行去模糊化，采用重心法去模糊化，得到清晰化的PID参数增量，再乘以比例因子k_u，得到最终的ΔK_p、ΔK_i、ΔK_d，则微调之后PID参数如式(3)所示：

式中K_p0、K_i0、K_d0是初始设定值。

表1

该方法所用位置式PID算法如公式(4)所示：

u(k)＝[K_p0+ΔK_p(k)]*e(k)+[K_i0+ΔK_i(k)]*∑e(k)+[K_d0+ΔK_d(k)]*[e(k)-e(k-1)] 公式(4)

式中u(k)为第k个控制周期的期望PWM占空比值，即电机控制量。

经过上述因子自适应模糊PID控制之后，可以得到如如公式(4)所示的电机控制量，执行机构820空心杯电机接收此控制量，做出调整，实现姿态的调整。

在本实施例中，所述四旋翼飞行器姿态控制方法具体流程图如图4所示。下面进一步的举一个例子来描述：

1.系统启动后，姿态传感器采集四旋翼飞行器的加速度和角速度信息，处理器进行姿态解算后得到四旋翼飞行器的三个当前姿态角(俯仰角、偏航角、翻滚角)信息，与给定的期望姿态角相减后得到姿态角误差量e，进一步，对误差进行求导得到误差变化率ec。

2.量化因整模块由输入的姿态角误差量e和误差变化率ec，根据公式(2)确定量化因子k_e和k_ec的数值，实现量化因子的在线调整。比例因子k_u取值为k_e的倒数，那么也即是实现了比例因子的在线调整。

3.模糊PID控制模块以k_e*e和k_ec*ec为输入，选取三角形函数作为隶属度函数，通过表1的模糊规则表查询确定ΔK_p、ΔK_i、ΔK_d的模糊值，使用Mamdani推理法结合重心法确定ΔK_p、ΔK_i、ΔK_d的清晰值，并经过比例因子k_u的变换后，得到最终的PID参数增量ΔK_p、ΔK_i、ΔK_d，再根据公式(3)得到控制参数K_p、K_i、K_d。

4.由位置式PID算法，根据公式(4)计算出PWM波占空比增量，通过PWM控制电机，进而实现姿态控制。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (3)

1.一种基于因子自适应模糊PID的四旋翼飞行器姿态控制方法，其特征在于：具体包含如下步骤：

S1：四旋翼飞行器通过姿态传感器，实时采集四旋翼飞行器的加速度和角速度信息，经过姿态解算得到四旋翼飞行器当的姿态角信息；

S2：给定期望姿态角，包括给定期望俯仰角，期望翻滚角，期望偏航角。由步骤S1中实时采集的当前姿态角信息，计算三个期望姿态角与当前姿态角之间的差值；

S3：整定量化因子和比例因子：以期望姿态角与当前姿态角的误差和误差变化率作为量化因子整定模块的输入，应用变论域思想，采用函数型表达式，得到量化因子和比例因子的数值；

S4：整定PID参数：整定后的量化因子k_e和k_ec相应的分别乘以误差e和误差变化率ec作为模糊控制器的输入，通过模糊算法整定PID参数，经过PID控制，得到电机控制量，进而调整四旋翼飞行器姿态达到期望的姿态量。

2.根据权利要求1中所述的一种基于因子自适应模糊PID的四旋翼飞行器姿态控制方法，其特征在于：所述步骤S3中量化因子的整定方法为：根据误差e和误差变化率ec进行在线调整，量化因子与e和ec成单调递减趋势，设计公式为：

其中a为函数曲线的伸缩因子，其大小能决定函数曲线在u＝0与u＝1之间的形状，c控制函数曲线中心对称点的横坐标，b和λ控制将k₀进行缩放的范围，n表示误差的量化等级数的个数，k₀表示量化因子初始值；同时选取比例因子为整定后的量化因子的倒数；量化因子和比例因子的整定，使得步骤S4 PID参数整定中，当误差逐渐减小时，误差量的附近始终可以运用较多的模糊规则，消除了模糊规则数量与控制精度的矛盾，增加了控制灵敏度。

3.根据权利要求1中所述的一种基于因子自适应模糊PID的四旋翼飞行器姿态控制方法，其特征在于：所述步骤S4中，PID参数整定过程：输入为k_e*e和k_ec*ec，输出为PID控制参数ΔK_p、ΔK_i、ΔK_d；根据输入的k_e*e和k_ec*ec，查询模糊控制规则表得到PID控制参数的模糊值；最后结合Mamdani推理算法和重心法将PID控制参数的模糊值清晰化，得到清晰化的PID控制参数，再乘以比例因子k_u，得到最终的ΔK_p、ΔK_i、ΔK_d。