一种四旋翼飞行器姿态控制系统及控制方法

1.一种四旋翼飞行器姿态控制系统的控制方法，该系统包括DC-DC电路、三轴加速度器、地磁计、陀螺仪、模数转换器和微处理器，所述三轴加速度器、地磁计、陀螺仪通过模数转换器与微处理器相连，三轴加速度器、地磁计、陀螺仪将检测到的模拟信号经模数转换后发送到微处理器进行处理和控制，其特征在于，包括以下步骤：
步骤一、获取当前飞行器的姿态角信息，所述姿态角包括滚转角、俯仰角和偏航角，具体为：
(1-1)建立飞行器的姿态角模型，其中包括滚转角模型、俯仰角模型和偏航角模型，所述滚转角模型和俯仰角模型相同均为：
式中，A为滚转角模型的系统矩阵，B为控制矩阵，C为输出矩阵，x为状态变量，y为输出变量，表示x的导数，J转动惯量，T为一个时间常数，K为空气阻力系数,k为比例系数，m为飞行器质量，g为重力加速度，状态变量x1,x2,x3,x4分别为角加速度、加速度、角度、传感器观测加速度，u为控制输入；
偏航角模型为：
式中Ay、By、Cy、xy、yy分别为偏航角模型的系统矩阵、控制矩阵、输出矩阵、状态变量、输出变量， 为xy的导数；Ts1、Ts2分别为两个惯性环节的时间常数，ks为两个惯性环节的增益的乘积，xy1，xy2为偏航角速度与角度，uy为偏航角控制输入；
(1-2)分别根据上述建立的模型设计相应的卡尔曼滤波器，估计出姿态角模型中的状态变量，其中滚转角模型/俯仰角模型中的状态变量包括姿态角、角加速度、角速度和观测加速度；偏航角模型中的状态变量包括偏航角速度与角度；
A、根据滚转角模型/俯仰角模型建立的卡尔曼滤波器为：
式中，表示状态变量x的估计值，y和 分别为系统的输出值和估计的输出值，Kz为滚转角/俯仰角模型的卡尔曼增益；
B、根据偏航角模型建立的卡尔曼滤波器为：
式中， 表示状态变量xy的估计值，yy和 分别为系统的输出值和估计的输出值，Kzy为偏航角模型的卡尔曼增益；
步骤二、分别设计滚转角、俯仰角和偏航角的控制器，其中滚转角与俯仰角的控制器相同，具体为：
(2-1)分别为滚转角模型、俯仰角模型、偏航角模型设置相应的参考模型其中，滚转角模型、俯仰角模型的参考模型如下：
式中r为滚转角或俯仰角的参考输入，Am、Bm、Cm为参考模型的状态矩阵，xm、ym为参考模型的状态变量和输出；
偏航角模型的参考模型如下：
式中ry为偏航角的参考输入，Amy、Bmy、Cmy为参考模型的状态矩阵，xmy、ymy为参考模型的状态变量和输出；
(2-2)、利用参考模型中的状态变量与飞行器姿态角模型中的状态变量的偏差来设计滑模控制器；
其中，滚转角、俯仰角控制器为：
u＝ueq+unl＝ueq+Knlf(σ)
式中，u为滚转角/俯仰角控制器得到的控制输入，ueq为等效输入，unl为滑模控制器的非线性输入，Knl为切换幅值，f(σ)为切换函数；
偏航角控制器为：
uy＝ueqy+unly＝ueqy+Knlyfy(σy)
式中uy为偏航角控制器得到的控制输入，ueqy为等效输入，unly为滑模控制器的非线性输入，Knly为切换幅值，fy(σy)为切换函数；
步骤三、对步骤二中获得的三个控制器的控制输入进行叠加后输出一个总的驱动信号从而驱动电机工作以控制飞行器姿态。

一种四旋翼飞行器姿态控制系统及控制方法\n技术领域\n[0001] 本发明涉及一种四旋翼飞行器姿态控制系统，属于飞行器控制技术领域。\n背景技术\n[0002] 早在上个世纪中叶，微小型多旋翼飞行器已经受到海外一些研究机构的瞩目，但多旋翼飞行器一般尺寸较小，负载能力相对较差，无法搭载传统的高精度传感器。直到本世纪初，MEMS传感器技术的发展使微小型多旋翼飞行器的研究得到突破。\n[0003] 姿态控制是多旋翼飞行控制的基础。为完成姿态控制，首先必须得到飞行器的姿态信息。目前飞行器姿态角的测量方法主要有：1、采用高精度的陀螺仪。虽然价格相对较低，但陀螺仪本身存在漂移，随时间增加，数据准确度会很差。2、采用加速度计和地磁计对陀螺仪数据进行修正，从而抑制传感器漂移。此方法的准确度与采用的补偿算法有关，一般有欧拉角、四元数、旋转矢量、扩展卡尔曼等。商业化的姿态测量系统主要有美国Crossbow公司的MNAV模块和荷兰Xsens公司的MTi，但他们的价格都高达数万人民币，严重影响了其应用范围。\n[0004] 3、采用图像处理的方法。此方法通过对图像的处理结合相应算法可得到较为准确的姿态信息，但图像处理数据量较大，数据更新频率慢，不易满足飞行器的需要。此外，因为图像处理一般需要安装图像采集设备和标志物等，受环境的制约较大，无法广泛采用。\n[0005] 目前，姿态控制器的设计方法较多，如PID控制、现代控制、模糊控制等，并取得了一定的控制效果。但以上控制方法大都对模型的准确性要求较高，当模型摄动或是受到外界干扰时其控制品质会明显下降，严重影响了多旋翼飞行器的应用范围。\n发明内容\n[0006] 本发明的目的在于提供一种结构简单、成本低的四旋翼飞行器姿态控制系统；同时，本发明还提出一种基于该控制系统的控制方法。\n[0007] 实现本发明目的的技术解决方案为：一种四旋翼飞行器姿态控制系统，包括DC-DC电路、三轴加速度器、地磁计、陀螺仪、模数转换器和微处理器，所述三轴加速度器、地磁计、陀螺仪通过模数转换器与微处理器相连，三轴加速度器、地磁计、陀螺仪将检测到模拟信号经模数转换后发送到微处理器进行处理和控制。\n[0008] 进一步地优化方案，本发明四旋翼飞行器姿态控制系统中，所述陀螺仪的数量为三个。\n[0009] 一种四旋翼飞行器姿态控制方法，包括以下步骤：\n[0010] 步骤一、获取当前飞行器的姿态角信息，所述姿态角包括滚转角、俯仰角和偏航角；\n[0011] 步骤二、分别设计滚转角、俯仰角和偏航角的控制器，其中滚转角与俯仰角的控制器相同；\n[0012] 步骤三、对步骤二中获得的三个控制器的控制量进行叠加后输出一个总的驱动信号从而驱动电机工作以控制飞行器姿态。\n[0013] 进一步地优化方案，所述四旋翼飞行器姿态控制方法中，步骤一中获取当前飞行器的姿态角信息，具体如下：\n[0014] （1-1）建立飞行器的姿态角模型，其中包括滚转角模型、俯仰角模型和偏航角模型，所述滚转角模型和俯仰角模型相同均为：\n[0015] \n[0016] 式中，A为滚转角/俯仰角模型的系统矩阵，B为控制矩阵，C为输出矩阵，x为状态变量，y为输出变量，表示x的导数J转动惯量，T为一个时间常数，k为比例系数，K为空气阻力系数,m为飞行器质量，g为重力加速度，状态变量x1,x2,x3,x4分别为角加速度、加速度、角度、传感器观测加速度，u为控制输入；\n[0017] 偏航角模型为：\n[0018] \n[0019] 式中Ay、By、Cy、xy、yy分别为偏航角模型的系统矩阵、控制矩阵、输出矩阵、状态变量、输出变量，为xy的导数。Ts1、Ts2分别为两个近似惯性环节的时间常数，ks为两个惯性环节的比例系数的乘积，xy1，xy2为偏航角速度与角度，uy为偏航角控制输入;\n[0020] （1-2）分别根据上述建立的模型设计相应的卡尔曼滤波器，估计出姿态角模型中的状态变量，其中滚转角模型/俯仰角模型中的状态变量包括姿态角、角加速度、角速度和观测加速度；偏航角模型中的状态变量包括偏航角速度与角度；\n[0021] A、根据滚转角模型/俯仰角模型建立的卡尔曼滤波器为：\n[0022] \n[0023] 式中，表示状态变量x的估计值，y和 分别为系统的输出值和估计的输出值，Kz为滚转角/俯仰角模型的卡尔曼增益；\n[0024] B、根据偏航角模型建立的卡尔曼滤波器为：\n[0025] \n[0026] 式中，表示状态变量xy的估计值，yy和 分别为系统的输出值和估计的输出值，Kzy为偏航角模型的卡尔曼增益；\n[0027] 进一步地优化方案，所述四旋翼飞行器姿态控制方法中，步骤二中设计滚转角、俯仰角和偏航角的控制器的方法具体为：\n[0028] （2-1）为滚转角模型、俯仰角模型、偏航角模型设置相应的参考模型[0029] 其中，滚转角模型、俯仰角模型的参考模型如下：\n[0030] \n[0031] 式中r为参考输入，Am、Bm、Cm为参考模型的状态矩阵，xm、ym为参考模型的状态变量和输出；\n[0032] 偏航角参考模型如下：\n[0033] \n[0034] 式中ry为偏航角的参考输入，Amy、Bmy、Cmy为参考模型的状态矩阵，xmy、ymy为参考模型的状态变量和输出；\n[0035] （2-2）、利用参考模型中的状态变量与飞行器姿态角模型中的状态变量的偏差来设计滑模控制器；其中，滚转角、俯仰角控制器为：\n[0036] u=ueq+unl=ueq+Knlf(σ)\n[0037] 式中，u为滚转角/俯仰角控制器得到的控制输入，ueq为等效输入，unl为滑模控制器的非线性输入，Knl为切换幅值，f(σ)为切换函数；\n[0038] 偏航角控制器为：\n[0039] uy=ueqy+unly=ueqy+Knlyfy(σy)\n[0040] 式中uy为偏航角控制器得到的控制输入，ueqy为等效输入，unly为滑模控制器的非线性输入，Knly为切换幅值，fy(σy)为切换函数。\n[0041] 本发明与现有技术相比，其显著优点：1）本发明采用陀螺仪结合加速度计，并根据飞行器姿态角模型设计出卡尔曼滤波器在线推测姿态角，同时还能得到系统的中间状态变量，具有一定的滤波效果；\n[0042] 2）本发明具有成本低、计算量小、精度高、得到的信息量大的优点；\n[0043] 3）本发明采用陀螺仪结合加速度计，并根据飞行器姿态角模型设计出卡尔曼滤波器在线推测姿态角，计算量较小，系统响应速度快。\n[0044] 4）本发明采用参考模型滑膜控制器姿态控制算法，具有较强的鲁棒性，飞行性能受环境的制约很小。\n附图说明\n[0045] 图1为本发明四旋翼飞行器姿态控制系统的结构框图；\n[0046] 图2为滚转角/俯仰角到测得的加速度之间的系统方框图；\n[0047] 图3为滚转角/俯仰角的卡尔曼滤波器方框图；\n[0048] 图4为滚转角/俯仰角的滑膜控制器的控制方框图；\n具体实施方式\n[0049] 下面结合附图对本发明作进一步详细描述。\n[0050] 如图1所示，一种四旋翼飞行器姿态控制系统，包括DC-DC电路1、三轴加速度器\n3、地磁计4、陀螺仪2、模数转换器5和微处理器6，所述三轴加速度器、地磁计、陀螺仪通过模数转换器与微处理器相连，三轴加速度器、地磁计、陀螺仪将检测到模拟信号经模数转换后发送到微处理器进行处理和控制。\n[0051] 1、DC-DC电路:DC-DC电路为直流到直流的电源转换模块，作用是将直流电源转化为需要的直流电压。本实例选用稳压芯片LM2575，其最大输入电压为45v，最大输出电流可达1A，输出电压3.3v、5v、12v、15v可调，稳压误差在4%以内。根据实际需要，采用两片LM2575得到3.3v和5v的稳压输出。\n[0052] 2．陀螺仪:所用的陀螺是ADI公司的一款功能完备、成本低廉的角速率传感器（陀螺仪），用于测量角速率，该陀螺仪测量范围达到±300°/s，抗干扰能力强，具有温度校准功能，漂移误差小，能够满足多旋翼飞行器的飞行控制性能需求。利用外接电容和片上电阻构成一个低通滤波器用于限制ADXRS610速率响应带宽，带宽设为361Hz。\n[0053] 3.三轴加速度器：加速度是MAV姿态测量以及分析MAV飞行性能的重要状态量，选择的ADXL335能以±3g的满量程范围测量加速度。通过调节输出引脚上的电容来选择合适的带宽。采用的加速度计的X轴和Y轴的带宽范围为0.5Hz至1600Hz，Z轴的带宽范围为0.5Hz至550Hz。为了减少噪音，将加速度传感器带宽设定为50HZ，即使用0.1uf滤波电容。\n[0054] 4．地磁计：选择飞思卡尔半导体首款地磁计MAG3110，是一款小型的低功耗、数字3轴地磁计，具有宽广的动态范围，能够在带有外部磁场的印刷电路板(PCB)中运行。\nMAG3110包括标准的I2C串行接口，能够测量高达10高斯的所在位置磁场，输出数据速率(ODR)可达80Hz。相应的输出数据速率可以从12ms到数秒钟的采样间隔内调整。\n[0055] 5．模数转换器：模数转换器使用了12位分辨率的MCP3204，带SPI串行接口，当供电电压为5V时；最大采样速率能够达到100Ksps。为了增强微型MAVS的抗风能力，需要提高ADC的采样率，设计的姿态测量系统将以400HZ的频率输出数据。信号以模拟和数字（RS-232）两种格式输出。模拟信号包括三轴加速度计电压，三轴磁强计电压，加速度计电压。数字量输出包括三轴角速率；滚转、俯仰、偏航角。留有GPS输入接口，ADC输入引脚留有扩展接口，方便根据实际需要增加相应的传感器。\n[0056] 6．微处理器：采用微处理器对传感器数据进行处理，推测出姿态角并进行姿态控制，控制频率为400Hz。选择具有较高性价比的AT91sam7，它是Atmel32位ARM RISC处理器，带有256k的高速Flash，处理速度能够满足姿态推测及姿态控制的需要。\n[0057] 一种四旋翼飞行器姿态控制系统的控制方法，包括以下步骤：\n[0058] 步骤一、获取当前飞行器的姿态角信息，所述姿态角包括滚转角、俯仰角和偏航角，具体为：\n[0059] (1-1)建立飞行器的姿态角模型，其中包括滚转角模型、俯仰角\n[0060] 模型和偏航角模型\n[0061] A、建立滚转角模型、俯仰角模型\n[0062] 通过理论推导和近似的线性化处理，飞行器的姿态角和转矩之间的关系可近似表示为：\n[0063] \n[0064] 式中τ为飞行器得到的转矩，J转动惯量，φ为姿态角。\n[0065] 现假设飞行器的控制指令信号到得到的转矩为一阶惯性环节，则可得到控制指令信号到姿态角的模型。\n[0066] \n[0067] 式中Guφ表示控制输入信号u到姿态角φ的传递函数，k为比例系数，T为一个时间常数。\n[0068] 为抑制由单一陀螺仪数据的漂移产生的姿态角φ推测误差，本专利提出引入加速度计来补偿，然后利用卡尔曼滤波器在线推测姿态角。为此，我们首先要将加速数据度引入飞行器模型。\n[0069] 加速度计的观测加速度am包括动加速度ad和静加速度，g为重力加速度，φ为姿态角，其关系可由下式给出。\n[0070] am=gφ+ad （3）\n[0071] 考虑空气阻力，且阻力系数与速度成正比，则有：\n[0072] mad=mgφ-K∫addt （4）\n[0073] 式中K为空气阻力系数。\n[0074] 对上式作拉普拉斯变换，得到姿态角到动加速度的传递函数：\n[0075] \n[0076] 从而得到滚转角/俯仰角到测得的加速度之间的系统方框图，如图2所示，Angular Velocity为输入角速度，Roll为得到的姿态角，g为重力加速度，Acc为静加速度，ad为动加速度，am为传感器观测加速度。\n[0077] 根据式（2）（3）（5），选择角速度和加速度作为系统输出，则可得到四旋翼飞行器滚转角或俯仰角的模型：\n[0078] \n[0079] 式中，A为滚转角模型的系统矩阵，B为控制矩阵，C为输出矩阵，x为状态变量，y为输出变量，为x的导数，J转动惯量，T为一个时间常数，K为空气阻力系数,k为比例系数，m为飞行器质量，g为重力加速度，状态变量x1,x2,x3,x4分别为角加速度、加速度、角度、传感器观测加速度，u为控制输入；\n[0080] B、建立偏航角模型\n[0081] 假设偏航角模型中控制输入到偏航角为两个一阶惯性环节的叠加，则可以得到偏航角的二阶模型。\n[0082] \n[0083] 式中Ay、By、Cy、xy、yy分别为偏航角模型的系统矩阵、控制矩阵、输出矩阵、状态变量、输出变量，为xy的导数。Ts1、Ts2分别为两个近似惯性环节的时间常数，ks为两个惯性环节的比例系数的乘积，xy1，xy2为偏航角速度与角度，uy为偏航角控制输入；\n[0084] （1-2）、分别根据上述建立的模型设计相应的卡尔曼滤波器，估计出姿态角模型中的状态变量，其中滚转角模型/俯仰角模型中的状态变量包括姿态角、角加速度、角速度和观测加速度；偏航角模型中的状态变量包括偏航角速度与角度；\n[0085] A、根据滚转角模型、俯仰角模型建立的卡尔曼滤波器表示为\n[0086] \n[0087] 式中 表示状态变量x的估计值，A、B为式（6）中系统状态方程的系统矩阵和输入矩阵，y和 分别为系统的输出值和估计的输出值，Kz为卡尔曼增益，可表示为[0088] Kz=PCTR-1 (8)\n[0089] 式中C为式（6）系统状态方程输出矩阵，P为代数里卡蒂方程的解[0090] AP+PAT+BQBT-PCTRCP=0 (9)\n[0091] 里卡蒂方程中矩阵Q,R分别为系统输出端（陀螺仪和加速度计数据）和控制输入端噪声的方差，代表了各个参数的置信度。通过调整矩阵的加权系数则可得到相对准确的姿态角信息。同时卡尔曼滤波器具有一定的滤波效果，可消除高频的传感器噪声，提高了推测的姿态角的准确度。如图3所示，y为传感器得到的角速度与加速度数据，u为控制器的输出控制量，A、B、C为式（6）表示的飞行器姿态角模型，Kz为式（8）得到的卡尔曼增益，则可得到系统的状态变量的估计\n[0092] B、根据偏航角模型建立的卡尔曼滤波器为：\n[0093] \n[0094] 式中，表示状态变量xy的估计值，yy和 分别为系统的输出值和估计的输出值，Kzy为偏航角模型的卡尔曼增益；偏航角模型的卡尔曼滤波器的设计与滚转角方法相同，只是卡尔曼增益Kzy的求解是依据偏航角模型和其对应的加权矩阵Qy,Ry得到。\n[0095] 步骤二、分别设计滚转角、俯仰角和偏航角的控制器，其中滚转角与俯仰角的控制器相同；\n[0096] （2-1）为滚转角模型、俯仰角模型、偏航角模型设置相应的参考模型[0097] A、滚转角模型、俯仰角模型的参考模型如下：\n[0098] \n[0099] 式中r为参考输入，Am、Bm、Cm为参考模型的状态矩阵，xm、ym为参考模型的状态变量和输出；\n[0100] 令Cm=C，e=xm-x，则反馈回路的偏差微分表达式可表示为：\n[0101] \n[0102] u为控制输入，假设此参考模型满足如下条件：\n[0103] Am-A=BK1,Bm=BK2 （12）\n[0104] 则式（10）可表示为：\n[0105] \n[0106] 引入B的广义逆矩阵B+，由式（11）得到K1表达式\n[0107] K1=B+(Am-A) （14）\n[0108] 为使参考模型的输出能够跟踪目标值r，需调整系统的直通增益为1。当时间近似无穷大时,系统的状态变量趋近稳定，即\n[0109] Amxm+Bmr=0，所以\n[0110] xm=-Am-1Bmr （15）\n[0111] 带入ym得\n[0112] ym=-CmAm-1Bmr=-CmAm-1BK2r （16）\n[0113] 从而可以得到K2的值\n[0114] \n[0115] 则Bm可由下式得到\n[0116] \n[0117] Am的值可通过仿真实验调节得到。\n[0118] B、偏航角参考模型如下：\n[0119] \n[0120] 式中ry为偏航角的参考输入，Amy、Bmy、Cmy为参考模型的状态矩阵，xmy、ymy为参考模型的状态变量和输出；偏航角模型的求解方法与滚转角/俯仰角的求解方法类似，只需将求解过程中滚转角/俯仰角模型的矩阵A、B、C替换为Ay、By、Cy即可得到Amy、Bmy、Cmy。\n[0121] （2-2）、利用参考模型中的状态变量与飞行器姿态角模型中的状态变量的偏差来设计滑模控制器；\n[0122] 由于滑模控制器（SMC）对外界干扰和建模误差具有较好的鲁棒性，因此采用滑模控制器设计方法实现对参考模型状态变量的追踪。在SMC控制中为实现目标值的准确跟踪，引入积分环节以提高系统的跟踪性能，为此我们引入一个新的状态变量εy为输出偏差ey的积分，即\n[0123] \n[0124] 则扩展的偏差系统为\n[0125] \n[0126] 式中us=-(K1x+K2r-u)\n[0127] 定义切换函数σ∈R为\n[0128] σ=Ses （21）\n[0129] \n[0130] 当系统到达理想的滑动模态时 其等效输入ueq可由式（23）得\n[0131] 到ueq=-(SBs)-1SAses+K1x+K2r （23）\n[0132] 将ueq带入式（19）得到\n[0133] \n[0134] 由上式可得系统具有稳定的零点。\n[0135] 为选择切换面，引入最优控制方法，即选择最优反馈增益F为切换面S。式中P为里卡蒂方程的解，S满足SBs>0。\n[0136] \n[0137] \n[0138] SMC的非线性输入\n[0139] unl=Knlf(σ) （27）\n[0140] 式中式中Knl为切换幅值，f(σ)为切换函数，通常选择号函数sign，但在实际应用中符号函数会产生很大抖动，影响控制品质。我们采用一个平滑函数\n[0141] \n[0142] 式中δ为平滑函数权重。\n[0143] 滚转角、俯仰角控制器表示为：\n[0144] u=ueq+unl=ueq+Knlf(σ)\n[0145] 式中，u为控制器得到的控制量，ueq为等效输入，unl为滑模控制器的非线性输入，Knl为切换幅值，f(σ)为切换函数；\n[0146] 通过选择合适的δ使状态变量接近切换面时相应的非线性控制量也相应减小，从而有效抑制了系统抖动。如图4所示，SMC为滑模控制器，Kalman为卡尔曼滤波器，Real System为实际飞行器。r为参考输入，卡尔曼滤波器推测的状态变量 和参考模型的状态变量x的差值，以及飞行器的输出角度y与参考模型输出角度差值的积分作为滑模控制器SMC的状态变量，然后输出控制输入u，从而完成飞行器的姿态控制。\n[0147] 偏航角控制器为：\n[0148] uy=ueqy+unly=ueqy+Knlyfy(σy)\n[0149] 式中uy为偏航角控制器得到的控制输入，ueqy为等效输入，unly为滑模控制器的非线性输入，Knly为切换幅值，fy(σy)为切换函数。\n[0150] 步骤三、对步骤二中获得的三个控制器的控制量进行叠加后输出一个总的驱动信号从而驱动电机工作以控制飞行器姿态。