CN105843235A - 一种以姿态角为控制对象验证控制理论的实验系统及方法 - Google Patents

Abstract

本发明公开了一种以姿态角为控制对象验证控制理论的实验系统及方法，其中，所述系统包括参数设定及观测单元、微处理器控制单元、执行单元和运动姿态角检测单元。本发明的结构简单、控制过程较短、成本低；响应速度快、能耗小，可以在短时间内多次重复进行实验，并且可以针对不同的自动控制理论进行验证，可广泛应用于高校的教学实验以及科研机构的实验研究，具有较好的实际应用价值。

Description

一种以姿态角为控制对象验证控制理论的实验系统及方法

技术领域

本发明涉及自动化控制技术领域，涉及结合传感器检测技术、微处理器控制技术、通讯传输技术，进行自动控制理论验证的实验系统及方法，特别涉及一种以姿态角为控制对象验证控制理论的实验系统及方法。

背景技术

自动控制理论实验装置是一种在高校实验室中供用户实际验证自动控制理论的教学科研实验装置，广泛应用于高校测量和控制类专业的实验教学以及科研领域。现有的自动控制理论验证装置主要存在以下缺点：

(1)经典的自动控制理论验证实验装置有如温度自动控制实验装置，由于以电热丝为执行机构，以温度为控制对象，结构复杂、控制过程较为漫长、响应速度慢、能耗大、不便于需要多次调整控制参数和需要重复实验的控制理论的验证；

(2)大多数的实验装置不具有自动控制理论可选择性，即对于同一种实验装置的控制理论单一，若要验证不同的控制理论需要不同的实验装置，从而造成了资源上的浪费。

因此，迫切需要一种能够克服上述缺点的验证控制理论的实验系统及方法，以应用于高效或者科研机构的实验。

发明内容

本发明的目的在于提供一种控制过程较短、控制理论可选择、成本低、响应速度快、能耗小并且可以在短时间内多次重复进行实验的以四旋翼飞行器的姿态角为控制对象验证多种自动控制理论的实验系统及方法。

为此，本发明提出一种姿态角为控制对象验证控制理论的实验系统及方法，可以充分地消除由于现有技术的限制和缺陷导致的一个或多个问题。

本发明另外的优点、目的和特性，一部分将在下面的说明书中得到阐明，而另一部分对于本领域的普通技术人员通过对下面的说明的考察将是明显的或从本发明的实施中学到。通过在文字的说明书和权利要求书及附图中特别地指出的结构可实现和获得本发明目的和优点。

本发明提供了一种以姿态角为控制对象验证控制理论的实验系统，其特征在于，所述系统包括参数设定及观测单元、微处理器控制单元、执行单元和运动姿态角检测单元，其中：

所述参数设定及观测单元包括人机交互模块和存储模块，所述存储模块用于存储自动控制算法数据，所述人机交互模块用于向用户提供人机交互界面，用户通过所述人机交互界面选择欲验证的自动控制算法，所述人机交互模块从所述存储模块中提取用户选择的自动控制算法，并且在人机交互界面上显示所述自动控制算法的名称，用户根据显示的所述自动控制算法的名称，通过所述人机交互界面设置执行单元的工作参数以及所述自动控制算法所对应的参数，所述参数设定及观测单元将设置好的上述参数通过有线或无线的方式传送给微处理器控制单元，其中，用户所设置的执行单元的工作参数可以包括：四旋翼飞行器模型的期望姿态角(包括期望横滚角，期望俯仰角，期望偏航角)、无刷电机的基础转速；

所述微处理器控制单元包括信号处理模块，所述信号处理模块将所述执行单元的工作参数传送给所述执行单元以使所述执行单元按照所述工作参数工作；

所述执行单元包括四旋翼飞行器模型，所述四旋翼飞行器模型采用十字交叉的结构，共有四个机臂，每一机臂上都有一个无刷电机，每一个无刷电机的转轴上电连接一个旋翼，且每一个无刷电机电连接一个电子调速器；

所述运动姿态角检测单元用于测量所述执行单元中的四旋翼飞行器模型在X、Y、Z三个方向的加速度和横滚角速度、俯仰角速度、偏航角速度；

其中，所述微处理控制单元中的信号处理模块对所述运动姿态角检测单元采集的所述四旋翼飞行器模型在X、Y、Z三个方向的加速度和横滚角速度、俯仰角速度、偏航角速度进行计算，得到所述四旋翼飞行器模型的姿态角。

优选的，所述执行单元还包括稳定底座和球关节轴承，所述稳定底座和所述球关节轴承的一端固定，所述球关节轴承的另一端固定连接在所述四旋翼飞行器模型的底端。

优选的，所述运动姿态角检测单元被设计为整合性6轴运动处理组件MPU6050和3轴电子罗盘HMC5883L，其中6轴运动处理组件MPU6050集成了3轴MEMS陀螺仪、3轴MEMS加速度计和一个可扩展的数字运动处理器DMP(DigitalMotion Processor)，并且可通过I2C接口连接一个第三方的数字传感器。

优选的，所述第三方的数字传感器为磁力计，其用于测量四旋翼飞行器的偏航角度。

优选的，存储模块还通过人机交互模块向用户提供了二次开发的接口，用户可以自行编写自动控制算法，然后通过所述接口向存储模块添加、优化自己的自动控制算法。

优选的，所述信号处理模块还根据用户所选择的自动控制算法计算得到控制量，将所述控制量发送给执行单元，进而控制四旋翼无人飞行器的姿态。

优选的，在人机交互模块上显示四旋翼飞行器的期望姿态角与实际姿态角的响应变化曲线，以供用户观测实时数据信息和控制效果，其中，用户可以根据该响应曲线观测控制效果，反复调整该自动控制算法的参数和执行单元的工作参数，直到观测到四旋翼飞行器的姿态角达到期望姿态角。

本发明还提出了一种以姿态角为控制对象验证控制理论的实验方法，所述方法包括以下步骤：

(1)用户通过参数设定及观测单元的人机交互界面选择欲验证的自动控制算法，并设置执行单元的工作参数以及所述自动控制算法所对应的参数，所述参数设定及观测单元将设置好的上述参数通过有线或无线的方式传送给微处理器控制单元，其中，用户所设置的执行单元的工作参数可以包括：四旋翼飞行器模型的期望姿态角(包括期望横滚角，期望俯仰角，期望偏航角)、无刷电机的基础转速；

(2)微处理器控制单元通过通信模块接收所述执行单元的工作参数和所述自动控制算法所对应的参数并将上述参数发送给信号处理模块。

(3)微处理器控制单元的信号处理模块根据执行单元的工作参数控制执行单元的四旋翼飞行器模型的转动，同时控制运动姿态角检测单元采集四旋翼飞行器在X、Y、Z三个方向的加速度以及横滚角速度、俯仰角速度、偏航角速度，并且运动姿态角检测单元将采集的数据传送给所述信号处理模块。

(4)所述信号处理模块对从运动姿态角检测单元接收的信息进行计算，得到四旋翼飞行器模型的姿态角；

(5)所述信号处理模块根据用户所选择的自动控制算法计算得到控制量，将所述控制量发送给执行单元，进而控制四旋翼无人飞行器的姿态；

(6)将四旋翼飞行器的期望姿态角与实际姿态角的响应变化曲线通过通信模块传送给参数设定及观测单元中的人机交互模块上，供用户观测实时数据信息和控制效果，其中，用户可以根据该响应曲线观测控制效果，反复调整该自动控制算法的参数和执行单元的工作参数，直到观测到四旋翼飞行器的姿态角达到期望姿态角。

优选的，所述步骤(4)具体包括以下子步骤：

(a)根据公式 ${\mathrm{\θ}}_1=\arctan \frac{G_x}{G_z}$ 和公式 ${\mathrm{\φ}}_1=\arctan \frac{G_y}{\sqrt{{G_x}^2+{G_z}^2}},$ 计算得到估计的横摇角度φ₁和纵摇角度φ₁，其中，G_x，G_y，G_z分别为所述运动姿态角检测单元检测到的四旋翼飞行器在X、Y、Z三个方向的加速度；

(b)将接收到的陀螺仪测量的横摇角速度φ′，纵摇角速度θ′，偏航角速度ψ′进行积分得到角度增量Vθ、Vφ、Vψ。

(c)将通过步骤(a)的计算所得到的估计的横摇角度φ和纵摇角度θ以及由磁力计测量得到的偏航角度ψ与通过步骤(b)的计算所得到的角度增量Vθ、Vφ、Vψ进行融合，得到所述四旋翼飞行器模型的纵摇姿态角，其计算公式为g_n＝g_n1*p+g_n2*(1-p){p∈(0，1)}

其中，g_n(n＝x，y，z)＝[θ φ ψ]为最终计算得到的四旋翼飞行器姿态角，g_n1(n＝x，y，z)＝[θ₁ φ₁ ψ₁]为加速度计估计和磁力计测量得到的姿态角，g_n2(n＝x，y，z)＝[Vθ Vφ Vψ]为陀螺仪积分得到的姿态角度增量，其中，p为互补滤波权重因子。

优选的，p一般取0.95。

本发明的结构简单、控制过程较短、成本低；响应速度快、能耗小，可以在短时间内多次重复进行实验，并且可以针对不同的自动控制理论进行验证，可广泛应用于高校的教学实验以及科研机构的实验研究，具有较好的实际应用价值。

附图说明

图1为根据本发明实施例的、以姿态角为控制对象验证控制理论的实验系统的结构示意图；

图2A为根据本发明实施例的、执行单元和运动姿态角检测单元、微处理器控制单元组合的结构示意图；

图2B为根据本发明实施例的、执行单元的结构示意图；

图3为根据本发明实施例的、以姿态角为控制对象验证控制理论的实验系统的总体工作状态原理示意图；

图4为根据本发明实施例的、PID控制的原理框图。

图5为根据本发明实施例的、模糊控制的原理框图。

图6为根据本发明实施例的、模糊PID控制的原理框图。

图7为根据本发明实施例的、以姿态角为控制对象验证控制理论的实验方法的流程图。

具体实施方式

下面参照附图对本发明进行更全面的描述，其中说明本发明的示例性实施例。

如图1所示，本发明所提出的以姿态角为控制对象验证控制理论的实验系统包括参数设定及观测单元1、微处理器控制单元2、执行单元3和运动姿态角检测单元4，其中，参数设定及观测单元1、执行单元3和运动姿态角检测单元4都与微处理器控制单元2连接。

所述参数设定及观测单元1包括人机交互模块11和存储模块12，其中，所述存储模块12用于存储自动控制算法数据，例如，PID控制、模糊控制、模糊PID控制等算法数据。所述人机交互模块11用于向用户提供人机交互界面，用户通过所述人机交互界面选择欲验证的自动控制算法，所述人机交互模块11从所述存储模块12中提取用户选择的自动控制算法，并且在人机交互界面上显示所述自动控制算法的名称，用户根据显示的所述自动控制算法的名称，通过所述人机交互界面设置执行单元3的工作参数以及所述自动控制算法所对应的参数，所述参数设定及观测单元1将设置好的上述参数通过有线或无线的方式传送给微处理器控制单元2。

在上述实施例中，用户所设置的执行单元3的工作参数可以包括：四旋翼飞行器模型的期望姿态角(包括期望横滚角，期望俯仰角，期望偏航角)、无刷电机331的基础转速(使得旋翼332能够转动的初始条件)；用户设置的自动控制算法所对应的参数可以包括：如选择PID控制算法的话，需要设定比例P，积分I和微分D这3个基本参数；如选择模糊控制算法，需要设定论域，模糊集合，隶属度函数，模糊规则库，解模糊法这些基本参数；如选择模糊PID控制算法，需设定的参数与模糊控制算法相同。以上参数仅为举例说明，对于不同的控制算法显然可以选择不同的参数，因此，本发明并不限于上述特定实施例。

在上述实施例中，所述参数设定及观测单元1可以采用微型计算机来实现，所述人机交互模块可以11采用触摸显示屏来实现。

需要进一步指出的是，存储模块12中存储的自动控制算法数据是对执行单元3中四旋翼飞行器模型33的期望姿态角进行控制的自动控制理论中的常用自动控制算法数据，譬如PID控制、模糊控制、模糊PID控制等。对于与执行单元3有关的内容，下面将结合图2A，2B作进一步的详细说明。

同时存储模块12还通过人机交互模块11向用户提供了二次开发的接口，即用户可以自行编写自动控制算法然后通过这个接口向存储模块12添加、优化自己的自动控制算法，方便了用户根据自己实际需要进行二次算法开发，验证自己的控制算法。

继续参考图1，微处理器控制单元2包括通信模块21和信号处理模块22。

所述通信模块21用于实现微处理器控制单元2和参数设定及观测单元1之间的通信。

所述信号处理模块22用于将参数设定及观测单元1中设置的执行单元3的工作参数转换为执行单元3能够识别的信号并传送给执行单元3，使执行单元3按照预设定的工作参数工作。如上所述，执行单元3的工作参数可以包括：四旋翼飞行器模型的期望姿态角(包括期望横滚角，期望俯仰角，期望偏航角)、无刷电机331的基础转速(使得旋翼332能够转动的初始条件)。

所述信号处理模块22用于还用于对运动姿态角检测单元4采集的执行单元3中的四旋翼飞行器模型33在X、Y、Z三个方向的加速度和横滚角速度、俯仰角速度、偏航角速度进行计算，得到执行单元3中四旋翼飞行器模型33的姿态角，并将其通过通信模块21传送给参数设定及观测单元1。

在上述实施例中，信号处理模块22还可以计算出执行单元3中飞行器模型33的角速度和角加速度。这是由于每种自动控制算法需要的数据不同，角速度和角加速度是有的自动控制算法所需要的，所以根据不同的需要可以在人机交互界面11上选择需要返回的信息数据。

在上述实施例中，微处理器控制单元2可以采用具有Cortex-M3内核的微处理器STM32；通信模块21可以采用MAX232电平转换芯片，用于实现微处理器控制单元2的TTL电平和参数设定及观测单元1的RS232电平之间的转换，从而达到通过串口单元进行通信的目的。

在上述实施例中，运动姿态角检测单元4与微处理器控制单元2中的信号处理模块22连接，其用于测量执行单元3中的四旋翼飞行器模型33在X、Y、Z三个方向的加速度和横滚角速度、俯仰角速度、偏航角速度。本发明创造性地将运动姿态角检测单元4设计为整合性6轴运动处理组件，其集成了3轴MEMS陀螺仪、3轴MEMS加速度计和一个可扩展的数字运动处理器DMP(Digital MotionProcessor)，并且可通过I2C接口连接一个第三方的数字传感器，比如磁力计。拓展之后就可以通过其I2C或SPI接口输出一个9轴的信号。相较于现有的多组件方案，本发明提出的运动姿态角检测单元4同时测量到X、Y、Z三个方向的产生加速度和姿态角速度，不但免除了组合陀螺仪与加速器时之轴间差的问题，提高了运动姿态角的读取速度，而且减少了大量的封装空间，使整个系统显得更加简洁，并且节约了成本。这也是本发明的主要改进点之一。

作为一款物理传感器，所述运动姿态角检测单元4的工作原理是利用物理效应，诸如压电效应，磁致伸缩现象，离化、极化、热电、光电、磁电等效应，将被测信号量的微小变化转换成电信号。数字运动处理器DMP引擎可减少复杂的融合演算数据，以数字输出6轴的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据，移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的漂移。用户可以利用微处理器控制单元2获得所述运动姿态角检测单元4输出的四元数，进行融合计算之后，便可得到传感器的角度。

下面结合图2A，2B介绍执行单元3。如图2A，2B所示，执行单元3根据微处理器控制单元2传送的工作参数进行工作，执行单元3包括稳定底座31、球关节轴承32和四旋翼飞行器模型33，稳定底座31和球关节轴承32的一端固定，球关节轴承32的另一端固定连接在四旋翼飞行器模型33的底端；四旋翼飞行器模型33采用十字交叉的结构，共有四个机臂，每一机臂上都有一个无刷电机331，每一个无刷电机331的转轴上电连接一个旋翼332，且每一个无刷电机331电连接一个电子调速器333。

根据本发明的一个实施例，所述执行单元3还包括电源功能系统334，其中，电源功能系统334位于四旋翼飞行器模型的中心下方，为整个系统提供所需要的工作电压和能量。

如图2A，2B所示，微处理器控制单元2和运动姿态角检测单元4均位于飞行器模型33的中心上方。如前文所述，参数设定及观测单元1可以采用微型计算机来实现，则本发明所提出的实验系统的整体结构可以如图3所示，其中参数设定及观测单元1与设有微处理器控制单元2和运动姿态角检测单元4的执行单元3通过有线或者无线的方式通信。

电子调速器333电连接微处理器控制单元2中的信号处理模块22，信号处理模块22分别发送无刷电机331的基础转速给电子调速器333，通过电子调速器333使无刷电机331带动与之连接的旋翼332转动；由于四旋翼飞行器模型的旋翼旋转方向对角相同，相邻的相反，就抵消了由于旋翼的旋转而产生的反扭矩，并且通过控制四个电机的转速还可以利用这种反扭矩达到偏航的目的；由于四旋翼飞行器的四个电机是分别进行控制，可以利用四个旋翼产生的升力之差达到横滚和俯仰的控制，从而实现四旋翼飞行器姿态角的控制。

另外，运动姿态角检测单元4也电连接微处理器控制单元2中的信号处理模块22。微处理器控制单元2中的信号处理模块22发送开始工作的控制信号给运动姿态角检测单元4，运动姿态角检测单元4采集四旋翼飞行器在X、Y、Z三个方向的加速度以及横滚角速度、俯仰角速度、偏航角速度，并将采集的信号传送给微处理器控制单元2中的信号处理模块22进行处理。

微处理器控制单元2中的信号处理模块22所进行的处理主要分为两部分：

(1)所述信号处理模块22对从运动姿态角检测单元4接收的信息进行计算，得到四旋翼飞行器模型的姿态角，具体步骤如下：

(a)信号处理模块22根据公式 ${\mathrm{\θ}}_1=\arctan \frac{G_x}{G_z}$ 和公式 ${\mathrm{\φ}}_1=\arctan \frac{G_y}{\sqrt{{G_x}^2+{G_z}^2}},$ 计算得到估计的横摇角度φ₁和纵摇角度θ₁，其中，G_x，G_y，G_z分别为所述运动姿态检测单元检测到的四旋翼飞行器在X、Y、Z三个方向的加速度，需要说明的是，在该步骤(a)中，偏航角度ψ由磁力计测量得到，所以在此无需对偏航角度ψ进行估计。

(b)微处理器控制单元将接收到的陀螺仪测量的横摇角速度φ′，纵摇角速度θ′，偏航角速度ψ′进行积分得到角度增量Vθ、Vφ、Vψ。

(c)微处理器控制单元将通过步骤(a)的计算所得到的估计的横摇角度φ和纵摇角度θ以及由磁力计测量得到的偏航角度ψ与通过步骤(b)的计算所得到的角度增量Vθ、Vφ、Vψ进行融合，得到四旋翼飞行器模型的纵摇姿态角。

由于加速度计测得的值中有大量四旋翼飞行器震荡所产生的干扰加速度，所以得到的估计值包含有噪声，是不准确的，需要陀螺仪来进行校正，而磁力计测量得到值也包含外界磁场扰动的成分，也需要陀螺仪来进行校正。因为陀螺仪本身存在温度漂移的问题，因此其测得的值也不能直接反映四旋翼飞行器姿态的真实情况，需要和加速度计的值相互融合，才能得到真实的姿态角。常用的融合方式卡尔曼滤波，但是卡尔曼滤波计算量大，为其建立稳定可靠的更新方程比较困难，对处理器的运算速度和精度要求很高，不适合应用于本发明实验装置。本发明采用了另外一种姿态融合方法——互补滤波算法，互补滤波算法简单可靠，对姿态传感器的精度要求较低，其应用越来越广泛。其计算公式为g_n＝g_n1*p+g_n2*(1-p){p∈(0，1)}。

其中，g_n(n＝x，y，z)＝[θ φ ψ]为最终计算得到的四旋翼飞行器姿态角，g_n1(n＝x，y，z)＝[θ₁ φ₁ ψ₁]为加速度计估计和磁力计测量得到的姿态角，g_n2(n＝x，y，z)＝[Vθ Vφ Vψ]为陀螺仪积分得到的姿态角度增量，其中，p为互补滤波权重因子，一般取0.95。

(2)信号处理模块22在计算得到四旋翼飞行器的姿态角之后，根据用户所选择的自动控制算法计算得到控制量，将所述控制量发送给执行单元3，进而控制四旋翼无人飞行器的姿态，并将四旋翼飞行器的期望姿态角与实际姿态角的响应变化曲线通过通信模块22传送给参数设定及观测单元1中的人机交互模块11上，供用户观测实时数据信息和控制效果，其中，用户可以根据该响应曲线观测控制效果，反复调整该自动控制算法的参数和执行单元3的工作参数，直到观测到四旋翼飞行器的姿态角达到期望姿态角。

信号处理模块22根据用户选择的不同的控制算法进行相应处理的具体的控制过程如下所示：

若用户在人机交互界面11上选择了PID控制，在设定好四旋翼飞行器期望姿态角之后，首先设定P的参数为10(0-100可调)，I和D的值为0，先看只加一个P参数的控制效果，然后再逐渐增加D(0-100可调)值，最后在加上I(0-100可调)值，同时观测控制效果，根据控制效果不断地调整P、I、D三个参数，直到达到期望稳定的控制效果。

如图4所示，PID控制器的输入为r(t)用户通过人机交互模块11设定的期望姿态角，y(t)为四旋翼飞行器的实际角度，e(t)为r(t)与y(t)的角度误差，u(t)为计算的到无刷电机转速。

当运动姿态角检测单元4检测到实际的四旋翼无人飞行器的姿态角和期望姿态角不同时，微处理器单元2就会通过PID控制算法模块产生对四旋翼无人飞行器的控制量，在不断地通过运动姿态角检测单元4反馈然后调整的过程中，最终使四旋翼无人飞行器的姿态角和期望姿态角相同。

若用户在人机交互界面11上选择了模糊控制，则需在人机交互界面11上设定期望姿态角和设定论域，模糊集合，隶属度函数，模糊规则库，解模糊法。如图5所示，控制器的输入是期望姿态角。图5中，r(t)为系统的期望输入，e(t)和ec(t)分别为模糊控制器的输入变量，为系统误差和误差变化率，u(t)为模糊控制器的控制输出，经四旋翼飞行器后为实际的系统输出y(t)。

若用户在人机交互界面11上选择了模糊PID控制，则其需要设置的参数同模糊控制，但是对于模糊PID而言，模糊PID是综合了PID和模糊控制器的一种自适应控制器，通过模糊控制器得到PID控制器的三个参数，然后对四旋翼飞行器进行控制。相当于其内部有3个模糊控制器，因此，每种参数都有3个。如图6所示，r(t)为系统的期望输入，e(t)和ec(t)分别为模糊控制器的输入变量，为系统误差和误差变化率，经模糊控制器的输出为比例系数增量ΔKp、积分作用系数增量ΔKi和微分作用系数增量ΔKd，u(t)为PID控制器的控制输出，经四旋翼飞行器后为实际的系统输出y(t)。

本发明还提出了一种以姿态角为控制对象验证控制理论的实验方法，如图7所示，所述方法包括以下步骤：

(1)用户通过参数设定及观测单元1的人机交互界面选择欲验证的自动控制算法，并设置执行单元3的工作参数以及所述自动控制算法所对应的参数，所述参数设定及观测单元1将设置好的上述参数通过有线或无线的方式传送给微处理器控制单元2。

在上述实施例中，用户所设置的执行单元3的工作参数可以包括：四旋翼飞行器模型的期望姿态角(包括期望横滚角，期望俯仰角，期望偏航角)、无刷电机331的基础转速(使得旋翼332能够转动的初始条件)；用户设置的自动控制算法所对应的参数可以包括：如选择PID控制算法的话，需要设定比例P，积分I和微分D这3个基本参数；如选择模糊控制算法，需要设定论域，模糊集合，隶属度函数，模糊规则库，解模糊法这些基本参数；如选择模糊PID控制算法，需设定的参数与模糊控制算法相同。

(2)微处理器控制单元2通过通信模块21接收所述执行单元的工作参数和所述自动控制算法所对应的参数并将上述参数发送给信号处理模块22。

(3)微处理器控制单元2的信号处理模块22根据执行单元3的工作参数控制执行单元3的四旋翼飞行器模型33的转动，同时控制运动姿态角检测单元4采集四旋翼飞行器在X、Y、Z三个方向的加速度以及横滚角速度、俯仰角速度、偏航角速度，并且运动姿态角检测单元4将采集的数据传送给所述信号处理模块22。

具体的，所述信号处理模块22分别发送无刷电机331的基础转速给电子调速器333，通过电子调速器333使无刷电机331带动与之连接的旋翼332转动；同时，信号处理模块22发送开始工作的控制信号给运动姿态角检测单元4，运动姿态角检测单元4采集四旋翼飞行器在X、Y、Z三个方向的加速度以及横滚角速度、俯仰角速度、偏航角速度，并将采集的数据传送给所述信号处理模块22。

上述步骤(3)中，通常在最开始阶段会预先设定四旋翼飞行器的期望姿态角为0度，即四旋翼飞行器处于平衡状态并设定无刷电机331基础转速为低转速，在参数设定及观测单元1中无刷电机转速的设定范围为0-1000，即油门的大小范围为0-1000，首先设定油门大小为100，使无刷直流电机331以低转速旋转，然后再提升无刷电机331的基础转速，即增加油门大小到600，和改变期望姿态角，在参数设定及观测单元中1中设定期望姿态角均为10弧度，使四旋翼飞行器模型处于倾斜状态。

(4)所述信号处理模块22对从运动姿态角检测单元4接收的信息进行计算，得到四旋翼飞行器模型的姿态角。具体计算过程上文已经作出了详细的描述，在此不再赘述。

(5)所述信号处理模块22根据用户所选择的自动控制算法计算得到控制量，将所述控制量发送给执行单元3，进而控制四旋翼无人飞行器的姿态。对于控制量的计算上文已经作出了详细的说明，在此也不再赘述。

(6)将四旋翼飞行器的期望姿态角与实际姿态角的响应变化曲线通过通信模块22传送给参数设定及观测单元1中的人机交互模块11上，供用户观测实时数据信息和控制效果，其中，用户可以根据该响应曲线观测控制效果，反复调整该自动控制算法的参数和执行单元3的工作参数，直到观测到四旋翼飞行器的姿态角达到期望姿态角。

以上内容仅为本发明的较佳实施例，对于本领域的普通技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种以姿态角为控制对象验证控制理论的实验系统，其特征在于，所述系统包括参数设定及观测单元、微处理器控制单元、执行单元和运动姿态角检测单元，其中：

所述参数设定及观测单元包括人机交互模块和存储模块，所述存储模块用于存储自动控制算法数据，所述人机交互模块用于向用户提供人机交互界面，用户通过所述人机交互界面选择欲验证的自动控制算法，所述人机交互模块从所述存储模块中提取用户选择的自动控制算法，并且在人机交互界面上显示所述自动控制算法的名称，用户根据显示的所述自动控制算法的名称，通过所述人机交互界面设置执行单元的工作参数以及所述自动控制算法所对应的参数，所述参数设定及观测单元将设置好的上述参数通过有线或无线的方式传送给微处理器控制单元，其中，用户所设置的执行单元的工作参数可以包括：四旋翼飞行器模型的期望姿态角(包括期望横滚角，期望俯仰角，期望偏航角)、无刷电机的基础转速；

所述微处理器控制单元包括信号处理模块，所述信号处理模块将所述执行单元的工作参数传送给所述执行单元以使所述执行单元按照所述工作参数工作；

所述执行单元包括四旋翼飞行器模型，所述四旋翼飞行器模型采用十字交叉的结构，共有四个机臂，每一机臂上都有一个无刷电机，每一个无刷电机的转轴上电连接一个旋翼，且每一个无刷电机电连接一个电子调速器；

所述运动姿态角检测单元用于测量所述执行单元中的四旋翼飞行器模型在X、Y、Z三个方向的加速度和横滚角速度、俯仰角速度、偏航角速度；

其中，所述微处理控制单元中的信号处理模块对所述运动姿态角检测单元采集的所述四旋翼飞行器模型在X、Y、Z三个方向的加速度和横滚角速度、俯仰角速度、偏航角速度进行计算，得到所述四旋翼飞行器模型的姿态角。

2.根据权利要求1所述的系统，其特征在于，所述执行单元还包括稳定底座和球关节轴承，所述稳定底座和所述球关节轴承的一端固定，所述球关节轴承的另一端固定连接在所述四旋翼飞行器模型的底端。

3.根据权利要求1所述的系统，其特征在于，所述运动姿态角检测单元被设计为整合性6轴运动处理组件MPU6050和3轴电子罗盘HMC5883L，其中6轴运动处理组件MPU6050集成了3轴MEMS陀螺仪、3轴MEMS加速度计和一个可扩展的数字运动处理器DMP(Digital Motion Processor)，并且可通过I2C接口连接一个第三方的数字传感器。

4.根据权利要求3所述的系统，其特征在于，所述第三方的数字传感器为磁力计，其用于测量四旋翼飞行器的偏航角度。

5.根据权利要求1所述的系统，其特征在于，存储模块还通过人机交互模块向用户提供了二次开发的接口，用户可以自行编写自动控制算法，然后通过所述接口向存储模块添加、优化自己的自动控制算法。

6.根据权利要求1所述的系统，其特征在于，所述信号处理模块还根据用户所选择的自动控制算法计算得到控制量，将所述控制量发送给执行单元，进而控制四旋翼无人飞行器的姿态。

7.根据权利要求1所述的系统，其特征在于，在人机交互模块上显示四旋翼飞行器的期望姿态角与实际姿态角的响应变化曲线，以供用户观测实时数据信息和控制效果，其中，用户可以根据该响应曲线观测控制效果，反复调整该自动控制算法的参数和执行单元的工作参数，直到观测到四旋翼飞行器的姿态角达到期望姿态角。

8.一种以姿态角为控制对象验证控制理论的实验方法，所述方法包括以下步骤：

(1)用户通过参数设定及观测单元的人机交互界面选择欲验证的自动控制算法，并设置执行单元的工作参数以及所述自动控制算法所对应的参数，所述参数设定及观测单元将设置好的上述参数通过有线或无线的方式传送给微处理器控制单元，其中，用户所设置的执行单元的工作参数可以包括：四旋翼飞行器模型的期望姿态角(包括期望横滚角，期望俯仰角，期望偏航角)、无刷电机的基础转速；

(2)微处理器控制单元通过通信模块接收所述执行单元的工作参数和所述自动控制算法所对应的参数并将上述参数发送给信号处理模块。

(3)微处理器控制单元的信号处理模块根据执行单元的工作参数控制执行单元的四旋翼飞行器模型的转动，同时控制运动姿态角检测单元采集四旋翼飞行器在X、Y、Z三个方向的加速度以及横滚角速度、俯仰角速度、偏航角速度，并且运动姿态角检测单元将采集的数据传送给所述信号处理模块。

(4)所述信号处理模块对从运动姿态角检测单元接收的信息进行计算，得到四旋翼飞行器模型的姿态角；

(5)所述信号处理模块根据用户所选择的自动控制算法计算得到控制量，将所述控制量发送给执行单元，进而控制四旋翼无人飞行器的姿态；

(6)将四旋翼飞行器的期望姿态角与实际姿态角的响应变化曲线通过通信模块传送给参数设定及观测单元中的人机交互模块上，供用户观测实时数据信息和控制效果，其中，用户可以根据该响应曲线观测控制效果，反复调整该自动控制算法的参数和执行单元的工作参数，直到观测到四旋翼飞行器的姿态角达到期望姿态角。

9.根据权利要求8所述的方法，其特征在于，所述步骤(4)具体包括以下子步骤：

(a)根据公式和公式计算得到估计的橫摇角度φ₁和纵摇角度θ₁，其中，G_x，G_y，G_z分别为所述运动姿态角检测单元检测到的四旋翼飞行器在X、Y、Z三个方向的加速度；

(b)将接收到的陀螺仪测量的橫摇角速度φ′，纵摇角速度θ′，偏航角速度ψ′进行积分得到角度增量Vθ、Vφ、Vψ。

(c)将通过步骤(a)的计算所得到的估计的橫摇角度φ和纵摇角度θ以及由磁力计测量得到的偏航角度ψ与通过步骤(b)的计算所得到的角度增量Vθ、Vφ、Vψ进行融合，得到所述四旋翼飞行器模型的纵摇姿态角，其计算公式为g_n＝g_n1*p+g_n2*(1-p){p∈(0，1)}

其中，g_n(n＝x，y，z)＝[θ φ ψ]为最终计算得到的四旋翼飞行器姿态角，g_n1(n＝x，y，z)＝[θ₁ φ₁ ψ₁]为加速度计估计和磁力计测量得到的姿态角，g_n2(n＝x，y，z)＝[Vθ Vφ Vψ]为陀螺仪积分得到的姿态角度增量，其中，p为互补滤波权重因子。

10.根据权利要求8所述的方法，其特征在于，p一般取0.95。