飞行姿态控制（精选7篇）
飞行姿态控制 篇1
1 飞行姿态描述
■1.1坐标系
清楚的描述四轴飞行器的飞行姿态是飞行控制工作的第一步。为了便于准确的分析与研究四轴飞行器的动力学特性,定义一系列的坐标系是非常必要的。考虑到飞行姿态建模的有效性,在本研究中,我们选取机体坐标系和地面坐标系来描述飞行状态,并且假定地球表面是平面,即假定其曲率为零,如图1所示。
(1)机体坐标系B (oxyz)
机体坐标系 (Aircraft-body coordinate frame),其原点O取在四旋翼直升机的重心上,坐标系与飞机固连,x轴在飞机对称平面内并平行于直升机的纵轴线,即前后旋翼连线指向机头;y轴垂直于飞机对称平面平行于左右旋翼的连线指向机身左方;z轴在飞机对称平面内,与x轴垂直并指向机身上方。
(2)地面坐标系E(OXYZ)
地面坐标系 (Earth-surface inertial reference frame) 用于研究四旋翼直升机相对与地面的运动状态,确定飞机的空间位置坐标。在地面上选一点O,作为直升机起飞位置。X轴是在水平面内并指向某一方向,Z轴垂直于地面并指向空中,Y轴在水平面内垂直于X轴,其指向按右手定则确定。
■1.2欧拉角
如图2所示,我们利用欧拉角可以将上述两个坐标系联系起来。在此问题中,引入三个概念角度:横滚角φ,俯仰角θ与偏航角ψ,如图3所示。
横滚角φ:机体坐标系相对地面坐标系沿x轴变化的角度;
俯仰角θ:机体坐标系相对地面坐标系沿y轴变化的角度;
偏航角ψ:机体坐标系相对地面坐标系沿z轴变化的角度。
■1.3矩阵转换
我们利用矩阵工具来表示两个坐标系之间的关系。(1) 式、(2)式、(3) 式分别描述了机体坐标系与地面坐标系各个轴之间的转换关系。
结合 (1) (2) (3) 式,我们可以得到机体坐标系B到地面坐标系E的转换矩阵,如 (4) 式。
2 模型建立
■2.1基本假设
为了更有效地建立模型,我们作出如下假设:
(1)地面坐标系为惯性坐标系,如建立坐标系时的描述,略地球曲率,将地球表面假设成一张平面
(2)不考虑地球公转和自转对直升机运动的影响
(3)将直升机看做一个刚体,忽略弹性形变
(4)质心位于机体中心
(5)四个螺旋桨轴都能很好的与Z轴平行排列
■2.2模型推导
本模型依据牛顿第二定律及动力学方程建立,基本方程如 (6)式和 (7) 式。
由基本假设5可知,飞行器通过螺旋桨产生的升力仅分布在z轴方向,在x轴和y轴方向没有分力,故可以将机体坐标系下的飞行器升力 ( 机体力 ) 写为 (8) 式。
其中Fi为四旋翼直升机每个旋翼 ( 共有四个 ) 转动产生的上升力。跟据刚体定轴转动定律,刚体所受的对于某定轴的合外力矩等于刚体对此定轴的转动惯量与刚体在此合外力矩作用下所获得的角加速度的乘积。基于以上讨论,我们得到了四轴飞行器的非线性运动方程,写为 (9) 式。
其中分别为x方向、y方向和z方向的转动惯量,p为每个螺旋桨中心到直升机质心的系数,F4-F2为滚转控制量,F3-F1为俯仰控制量,F2 + F4-F1-F3为偏航控制量。
3 PID算法
PID (Proportion-Integral-Differential) 算法包括三个方面,即比例控制、积分控制和微分控制。PID算法原理图如图4所示。
图 4 PID 算法原理框图
从图4中可以看出,对于给定的输入量r(t),与上一状态的输出量进行比较,得到了此时的偏差e(t),将偏差e(t) 通过PID整合算法进行处理 ( 分别通过比例控制、积分控制和微分控制后相加 )后输入给被控对象,即四轴飞行器,得到修正后飞行器下一时刻的状态。再将此时的状态与输入量进行比较,进行循环计算处理。最终通过PID算法可以达到预定的目标状态。PID算法表达式可以写为 (10) 式。
其中,为比例系数,为积分时间常数,为微分时间常数。
■3.1比例控制
比例控制是一种最简单的控制方式,其控制器的输出与输入误差信号成比例关系,当仅有比例控制时系统输出存在稳态误差,表达式为 (11) 式。
■3.2积分控制
在积分控制中,控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统,如果在进入稳态后存在稳态误差,则称这个控制系统是有稳态误差的或简称有差系统。为了消除稳态误差,在控制器中必须引入“积分项”。积分项对误差取决于时间的积分,随着时间的增加,积分项会增大,表达式为 (12) 式。这样,即便误差很小,积分项也会随着时间的增加而加大,它推动控制器的输出增大使稳态误差进一步减小,直到等于零。因此,“比例 +积分 (PI)”控制器,可以使系统在进入稳态后无稳态误差。
■3.3微分控制
在微分控制中,控制器的输出与输入误差信号的微分 ( 即误差的变化率 ) 成正比关系,表达式为 (13) 式。自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。其原因是由于存在有较大惯性组件 ( 环节 ) 或有滞后 (delay) 组件,具有抑制误差的作用,其变化总是落后于误差的变化。解决的办法是使抑制误差的作用的变化“超前”,即在误差接近零时,抑制误差的作用就应该是零。这就是说,在控制器中仅引入“比例”项往往是不够的,比例项的作用仅是放大误差的幅值,而目前需要增加的是“微分项”,它能预测误差变化的趋势,这样,具有“比例 + 微分”的控制器,就能够提前使抑制误差的控制作用等于零,甚至为负值,从而避免了被控量的严重超调。所以对有较大惯性或滞后的被控对象,“比例 + 微分 (PD)”控制器能改善系统在调节过程中的动态特性。
摘要：四轴飞行器是一个模块化、具有较高硬件灵活性和较好操控性的平台装置,这个平台装置能够为科学实验、工程监控、气象监测、灾害预警等提供很好的应用平台。再本论文中主要研究并讨论了四轴飞行器的飞行原理和模型构建,并且着重介绍了PID算法,以实现飞行器的飞行状态控制。
关键词：四轴飞行器,飞行姿态,PID算法
飞行姿态控制 篇2
关于四轴飞行器的姿态动力学建模
作者：邓矛
来源：《科技创新导报》2012年第09期
飞行姿态控制 篇3
STM32是ST公司的高性能、低成本、低功耗嵌入式处理器，应用ARM Cortex-M3低功耗高速内核。STM32丰富的片上资源可满足各类传感器通讯需求。基于STM32的飞行控制器与传统的飞行控制器相比可大大降低系统的开发成本、节约资源[3]。
姿态解算是姿态参考系统的关键技术，算法的优劣直接决定了解算效率和系统的精度[4,5]。目前姿态描述的主要方式有:欧拉角、方向余弦、四元素法[6]。欧拉角物理意义明确，但存在“奇点”。方向余弦计算没有“奇点”，但三角函数运算量大，不适合实时运算。四元素法没有“奇点”，而且运算为一般代数运算，运算量小，方法简单，易于操作[7]。采用四元素法进行姿态解算是理想的选择。
1 四旋翼飞行器工作原理
四旋翼飞行器在结构布局上有十字形、X形和H形。本文主要讨论X形布局，如图1所示。4个电机分别安装在对称结构的X形支架的4个顶点，电机1和3逆时针旋转，电机2和4顺时针旋转，当飞行器平衡飞行时，陀螺效应和空气动力扭矩效应均被抵消。四旋翼飞行器在空间中有6个自由度(分别沿3个坐标轴作平移和旋转动作)，可通过调节4个电机的转速来实现控制。基本运动状态:垂直运动、俯仰运动、滚转运动、偏航运动、前后运动、侧向运动。
垂直运动:同相改变四个电机的输出功率，使总的拉力改变，便实现飞行器沿z轴的垂直运动。当升力等于自重时，飞行器便保持悬停状态。
俯仰运动和滚转运动:电机1、4的转速上升，电机2、3的转速下降。产生的不平衡力矩使机身绕轴旋转，实现飞行器的俯仰运动。横滚运动与俯仰运动的原理相同。
偏航运动:四旋翼偏航运动可以借助旋翼产生的反扭矩来实现。四个旋翼对角线上的两个旋翼转动方向相同，当两条对角线上的电机转速不完全相同时，不平衡的反扭矩引起四旋翼转动，实现偏航运动。
前后运动和倾向运动:为实现水平面内运动，须在水平面内对飞行器施加一定的力。使飞行器做横滚或俯仰。飞行器发生一定程度的倾斜，从而使旋翼拉力产生水平分量，实现飞行器的水平运动。
2 系统硬件设计
飞行器控制器通过处理微型MEMS惯性器件和三维地磁传感器采集的数据，计算飞行器的姿态角，并根据飞行指令，结合相应的控制律给出适当的控制信号，控制飞行器姿态和位置[8]。
本文采用模块化设计，四旋翼飞行控制系统主要由主控模块、数据采集模块、电机驱动模块等组成。采用STM32芯片作为主控芯片。系统硬件结构如图2所示。
2.1 主控模块
本文采用基于ARM Cortex M3内核的STM32F103VCTE微控制器为主控芯片。STM32微控制器具有多路信号采集通道和多路PWM输出通道，方便接收传感器数据和对电机进行控制，72 MHz的主频满足系统对于运算速度的要求，且功耗低。
2.2 数据采集模块图
采用拥有3轴加速度计和3轴陀螺仪的组合传感器MPU-6050来测量加速度和角速度，3轴地磁传感器HMC5883L测量地磁场，气压计MS5611测量气压。
MPU-6050集成了3轴MEMS陀螺仪，3轴MEMS加速度计。陀螺仪可测范围和加速度计可测范围可根据实际应用调整。与设备寄存器之间的通信采用400 kHz的I2C接口。
HMC5883L是一种带有数字接口的弱磁传感器芯片，测量范围从毫高斯到8高斯(Gauss)，罗盘精度在1°～2°，以I2C总线接口与主控芯片进行通信。
MS5611气压传感器是一款高分辨率气压传感器，分辨率可达到10 cm。拥有I2C总线接口与微控制器连接。
2.3 电机驱动模块
采用4个ST2210无刷直流电机并用ST20A电调对电机进行驱动。STM32通过配置GPIO接口输出PPM信号到电调控制电机转速。ST20A电调接受20～400 Hz，脉宽1～2 ms的PWM信号，电平支持3.3 V和5 V。0%～100%的油门由PPM信号1～2 ms的脉宽表示，即0%油门对应1 ms脉宽，100%油门对应2 ms脉宽，以达到调节电机转速的目的。
3 系统软件设计
系统软件设计在Keil MDK-ARM开发环境里编写。MDK-ARM软件为基于Cortex-M、Cortex-R4、ARM7、ARM9处理器设备提供了一个完整的C/C++开发环境。程序流程如图3所示。
3.1 姿态解算
对姿态角的测量和解算是飞行控制系统中重要的组成部分。姿态解算的速度和准确度直接影响到姿态控制效果。本文采用运算速度快，精度高的四元素互补滤波算法对姿态进行解算，能够达到飞行控制的要求。
姿态角是通过陀螺仪、加速度计和数字罗盘等传感器测量得到的。MEMS陀螺仪具有温度漂移特性，长期积分运算存在累积误差，短期数据精度高[9]。飞行器在飞行过程中机体振动会对加速度计产生影响，同时数字罗盘是一种磁阻传感器，很容易受到外部磁场的干扰，两者不适合短期测量，但其测量误差为静态误差，不随时间的累加而变化，可用以长时间姿态角采集[10]。
四元素互补滤波方法将这几种传感器的优势互补。此方法的主要思想是用加速度计和数字罗盘的测量姿态修正陀螺仪的测量姿态。加速度计和数字罗盘测量出来的数据是地球重力与地球磁场在机体空间坐标上的分量，可认为是实际姿态。陀螺仪测量出来的姿态经实际姿态修正后输出给控制器以计算控制量。
欧拉角、四元素和方向余弦矩阵间的关系如式(1)所示。
根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量转到机体坐标系为第三行三个元素，即:
式(2)中，aX,aY,aZ为加速度计测量出的重力向量在各个轴向上的分量，即实际测出来的重力向量;vX,vY,vZ为陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标系上的重力向量。它们之间的误差，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。计算前aX,aY,aZ需规范化处理。
式(3)中，eX,eY,eZ为两个重力向量的叉积，即误差。用此误差修正陀螺仪，如式(4)。
式(4)中eXInt,eYInt,eZInt为误差积分，如式(5)所示。
陀螺仪各轴转动的角速度经修正后，再以一阶龙格库塔算法进行积分更新四元素，如式(6)所示。
式(6)中，q0(n+1),q1(n+1),q2(n+1),q3(n+1)为更新的四元素;q0(n),q1(n),q2(n),q3(n)为这一时刻的四元素;ωX(n)，ωY(n)，ωZ(n)为采集到的角速度;T为采样周期。新的四元素需经过规范化处理，与式(3)同理。再由反三角函数运算得到各个姿态角，航向角ψ，横滚角和俯仰角θ如式(7)。
3.2 模型建立
定义两个坐标系，分别为地面坐标系E(X,Y,Z)和机体坐标系B(x,y,z)，如图1所示。机体坐标系B到地面坐标系E的转换矩阵为式(1)的第二项。
将四旋翼飞行器的运动过程分解为线运动和角运动的组合。运用牛顿定律和欧拉方程，建立由线运动位移模型与小角度角运动姿态模型共同构成的飞行器非线性动力学模型[11]。考虑到实际飞行条件，试飞时飞行器速率较低，可忽略空气阻力[1]，得到飞行器简易非线性化模型，如式(8)所示:
式(8)中，和u(1),u(2),u(3),u(4)均为增量;u(2),u(3),u(4)分别表示总升力，横滚力矩，俯仰力矩和偏航力矩增量;Ix,Iy,Iz为对应轴转动惯量;l为飞行器质心到电机轴之间的臂长。
采用LPV(linear parameter-varying)法线性化模型，该方法忽略力和力矩之间的耦合。四旋翼飞行系统的状态空间方程为:
实验是在小角度范围内进行，可以近似认为cosθ≈1,得到四旋翼飞行系统简化线性模型如式(10)。
根据电机的电枢回路电压平衡方程，小电机的电感系数L很小可以忽略[12]。近似认为电机的模型为一节惯性模型[1]:
以上模型将带入参数即为实际模型。
3.3 控制律设计
PID(比例-积分-微分)控制器简单易懂，使用中不需精确的系统模型等先决条件，因而成为应用最为广泛的控制器。
四旋翼飞行器姿态的调节是通过对4个带桨电机的转速调节实现的，4个电机不同的转速组合，能够实现飞行器俯仰、横滚、偏航等不同的飞行动作。通过在3个姿态控制通道分别设计PID控制律，实现所需的控制功能[13]。
针对本系统动态要求较高的特点，采用PID控制器的改进型控制器———串级PID控制器进行，控制结构框图如图4所示。
主控制器采用PID控制器对姿态角进行精确调整，副控制器采用PD控制器对角速度的变化趋势进行快速调整，提高了系统动态扰动下的控制能力，保证了系统控制性能和飞行姿态的稳定。
在Simulink搭建模型进行仿真，整定串级PID控制器参数，横滚角和俯仰角的阶跃响应仿真曲线如图5(a)、(b)所示。从图5可以看出，串级PID具有很好的控制能力，能够发到控制要求。
四旋翼飞行器实物如图6所示。通过实验验证采用四元素融合滤波和串级PID算法控制下，四旋翼飞行器在空中悬停时各个姿态角的稳定性。实验结果表明，本文所设计的飞行控制器能够稳定控制四旋翼飞行器的各个姿态角，在空中悬停的过程中使各个姿态角都在合理的范围内波动，符合控制的要求。
4 结语
飞行姿态控制 篇4
我国地域广大, 自然条件复杂, 是世界上自然灾害最为严重的国家之一。地震等自然灾害不仅给救灾带来不利, 也进一步增加了施救的难度, 加大了施救者受伤的危险程度。虽然自然灾害的发生无法阻止, 但是减少灾害带来的二次伤害是可以实现的[1], 例如可以开发微型无人驾驶飞行器, 对上述不可达的危险地域进行探测和搜寻即可减少伤害的发生, 尤其是在地势险要或者人员无法到达的地区, 能够准确地把侦查到的信息传送回来。而四轴飞行器能以灵活性、多功能性以及高效的处理能力来胜任复杂环境中的信息获取的角色。同时, 与传统的搜救直升机相比, 四轴飞行器以其简单的机械结构、更小的螺旋桨、优良的飞行安全性能和先进控制算法的应用等优势具有更广泛的前景。
本文基于STM32F103C8T6微处理器 , 采用MPU9150惯性测量模块, 设计了四轴飞行器姿态控制系统。STM32系列处理器是ST公司推出的一款基于高性能、低成本、低功耗、外设丰富的嵌入式处理器, 应用专门设计的低功耗高速内核丰富的片上资源可满足各类传感器通讯需求, 与传统的飞行控制器相比可大大降低系统的开发成本、节约资源。MPU9150系统集成度高, 包含了三轴加速度计、三轴陀螺仪和三轴磁力计, 电路简单可靠, 测量范围广, 准确度高, 动态响应快, 体积小、功耗低, 能够快速的融合陀螺仪和加速度计数据得出姿态四元数, 适合用于四轴飞行器的姿态控制系统。
1 四轴飞行器结构与原理
四轴飞行器的基本结构如图1所示, 飞行器在空中运动中, 为了使整个机体转矩平衡, 采用正反桨设计, 即对角线的两组桨相同, 相邻的两个桨相反, 分为顺时针旋转 (1、3) 和逆时针旋转 (2、4) , 这样正常飞行时两个桨正转两个桨反转, 转矩抵消, 避免飞行器打转[2], 当然, 旋转时需加大两个正浆或两个反浆来改变总的转矩, 从而改变偏航角, 控制对角线上的一组桨的转速不同, 使机体倾斜一个角度产生水平分力推动飞行器平移, 飞行速度可以由俯仰角的大小与电机的转速来控制, 最终实现飞行器的偏航运动、上下飞行与前后飞行。
2 姿态控制系统功能
四轴飞行器有6个自由度, 而只有4个控制输入, 因此, 四轴飞行器的控制问题属于欠驱动控制问题, 具有不稳定和强耦合等特点。除了受自身机械结构和旋翼空气动力学影响外, 还很容易受到外界干扰。其中姿态控制是四轴飞行器研究的重点, 姿态传感器数据的采集并对数据进行融合处理都是姿态控制的关键, 要求飞行器具有快速响应能力, 及时进行自适应调整, 以确保飞行器姿态稳定。
本文主要研究对象是四轴飞行器的姿态, 根据姿态控制子系统的数学模型[3], 有两个基本坐标系:“地理”坐标系和“载体”坐标系。地理坐标系即当地水平坐标系, 也称NED坐标系, 而“载体”坐标系指的是四轴自己的坐标系。姿态的数据来源有五个:重力、地磁、陀螺仪、加速度计和电子罗盘。其中前两个来自“地理”坐标系, 后三个来自“载体”坐标系。在“地理”坐标系中, 重力的值始终是 (0, 0, 1g) , 地磁的值始终是 (0, 1, x) , 这值就是由放置在四轴上的传感器测量出来的。姿态控制系统需要检测的状态有:飞行器在机体坐标系下3个轴向的角速度、角度和相对地面的高度。机体坐标系如图2所示, 该坐标系固定在机体上, 原点在飞行器重心, 轴OX与前后螺旋桨连线平衡, 前方指向X轴正方向;轴OY与左右螺旋桨连线平衡, 右方指向Y轴正方向;轴OZ与轴OY、OX所在平面垂直, 并与轴OY、轴OX组成右手坐标系。
飞行器运动过程可以分解成机体绕三个轴运动来描述:当飞行器升降运动时, 飞行器即做俯仰运动, 飞行器在X-Z平面进行绕Y轴运动, 产生的角度即俯仰角 (pitch) ;当飞行器发生转向运动时, 飞行器将产生偏航, 飞行器在X-Y平面进行绕Z轴运动, 产生的角度即航向角 (yaw) ;当让飞行器侧身移动时, 飞行器将要做横滚运动, 飞行器在Y-Z平面进行绕X轴转动, 产生的角度即航滚角 (roll) 。
整个姿态控制系统担负着传感器信息采集、数据融合及姿态解算等各种任务, 其主要工作过程是主控制器能快速获得各传感器的数据, 实时检测无人机的状态, 包括姿态、位置、速度等信息, 并对数据进行处理;在计算出自身姿态之后, 飞行器需要控制电机来执行相应的姿态调整, 采用PID控制器来获得电机控制量, 以PWM的方式输出驱动电机, 以实现对其姿态的控制。
3 硬件电路设计
3.1 MPU9150性能分析
随着微机械加工技术和微电子技术的不断发展与相互融合, 各种基于MEMS (微机电系统) 技术的元器件应运而生, 它们具有功耗低、可靠性、工作效率高及成本低等特点, 易于实现智能化、数字化及批量化。MPU9150芯片就是一种MEMS传感器芯片, 其具有MEMS的抗冲击能力强、系统集成度高、体积小、成本低、功耗低, 性能优良的特点。MPU9150是全球第一款九轴惯性传感芯片, 包含了三轴加速度计、三轴陀螺仪和三轴磁力计, 其中加速度和陀螺仪的精度为16bit, 磁力计的精度为13bit, 保证了测量的精确度, 其加速度计的量程有±2g, ±4g, ±8g, ±16g可选, 陀螺仪的量程为±250, ±500, ±1000, 和±2000O/sec可选, 磁力计的满量程为±1200u T, 并且内置DMP用于姿态融合[4]。使MPU9150在无人机飞行控制系统中使用非常方便。MPU9150引脚功能描述如表1所示, 典型接线图如图3所示。
3.2 硬件电路结构
为了实现飞行器姿态控制, 需要得到飞行器的俯仰角和滚转角作为姿态反馈, 形成闭环控制, 为提高其飞行稳定性, 需加入角速率反馈以增加阻尼, 飞行器的飞行姿态通过惯性测量单元来获取, 系统采用整合了3轴陀螺仪、3轴加速计、3轴磁力计的9轴运动处理组件MPU-9150, 消除了组合陀螺仪与加速计时存在的轴差问题;并引入磁力传感器互补数据, 可采用四元数来描述姿态, 可以避免欧拉角的奇异问题;另外陀螺仪具备增强偏置和温度稳定的功能, 减少了用户校正操作, 且具备改进的低频噪声性能;加速计则具备可编程中断和自由降落中断的功能。MPU-9150姿态检测电路图如图4所示。
4 姿态解算与数据融合分析
4.1 姿态解算
姿态解算部分是飞行器控制系统的核心, 主要负责读取姿态传感器数据, 从中解算出飞行器姿态, 并将其作为姿态反馈, 形成闭环控制, 将期待的姿态与当前姿态的数据差值提供给PID控制器, 从而计算得到电机的控制量。
姿态解算主要用到陀螺仪、加速度计和磁力计三个传感器的数据。由于陀螺仪输出的角速度瞬时数据积分后存在较大累积误差, 且积分角度偏差随着时间增大而增大;而加速度计对绕重力加速度轴的转动无法感知, 因此长时间的自旋运动将无法估计, 这就需要引入磁力计来弥补这一不足。因此, 用加速度计对陀螺仪进行不断的校正, 引入磁力计互补数据, 融合三者的数据才能更准确的反应出当前飞行器的姿态[5]。
目前常用的姿态解算方法主要有梯度下降法, 互补滤波法, 卡尔曼融合DMP输出和磁场数据。考虑到MPU9150传感器有一个内置的DMP, 能够快速融合陀螺仪和加速度计的输出, 而且输出的姿态数据很稳定, 动态性能很好, 虽然DMP中算法没有融合磁场数据, 导致航向角在使用一段时间后会不可逆转的偏离正确位置, 但可以采用卡尔曼滤波的方法, 对两个航向角进行滤波, 同时将磁场数据融合进去, 以纠正航向角的偏差, 从而既保证了其动态性能, 又保证长时间运行航向角不会出现偏差。因此这里采用卡尔曼融合DMP输出和磁场数据的飞行姿态解算方法。算法流程图如图5所示。
4.2 数据融合分析
4.2.1 静态效果分析
实验方法为先将MPU9150模块静止放置20s, 然后再对其连续采样12s, 采样频率为50Hz。数据处理方法是, 对每个轴输出的欧拉角数据求平均值。
然后再求方差:
通过静止时方差的大小来分析其静态性能。卡尔曼融合DMP和磁场数据算法的核心在于确定卡尔曼滤波中的系统自协方差Q和测量值的自协方差R两个系数。不同Q值和R值的静态航向角方差结果如表2所示。
从上图中可以明显看出航向角的静态方差随着R的增大而变小, 随着Q的变小而变小, 且Q越大方差随R变化效果越明显。最后列出Q=0.00001, R=0.9静态效果如图7所示。
从静态效果图可以看出, 卡尔曼融合DMP和磁场数据的方法中航向角的误差最小可达到0.00113。表明在保证纠正效果--即姿态角不漂移的前提下卡尔曼融合DMP输出和磁场数据的算法静态效果理想。
4.2.2 动态效果分析
在确保静态性能良好的条件下, 采用一个将航向角快速旋转180°的方法来验证卡尔曼融合DMP输出和磁场数据算法的动态性能。结果如图8所示。
由图8可以看出卡尔曼融合DMP和磁场数据的算法能很好的反应真实的旋转过程。
综合静态和动态性能分析, 卡尔曼融合DMP和磁场数据的算法不仅在静态性能上表现很好, 而且在动态性能方面也表现良好。可以得到稳定的姿态数据, 实现良好的姿态控制, 从而使飞行器达到稳定飞行的效果。
5 结束语
四轴飞行器是一种特殊结构的飞行器, 其在民用及军事方面都有着广阔的应用前景[6]。本文根据四轴飞行器姿态控制系统的数学模型, 对姿态控制系统的功能要求进行了分析, 以STM32为主控制器, 采用MUP-9150芯片进行姿态控制, 并采用卡尔曼融合DMP和磁场数据的算法进行姿态解算, 实现了飞行器良好的姿态控制。整个姿态控制系统集成度高, 精度高, 在测量精度、动态性能以及硬件电路方面均能满足要求, 为飞行器姿态控制系统的设计提供了一种高效稳定的实现方法[6]。实验结果表明, 系统满足四轴飞行器飞行姿态控制的要求。
参考文献
[1]张鹏.一种新型四轴搜救飞行器设计[J].科技广场.2010 (9) :145-146.
[2]郭晓鸿.一种基于S T M 3 2的四旋翼飞行器[J].应用科技.2011.38 (7) .36-37.
[3]Pau Segui-Gasco.A Novel Actuation Concept for a Multi Rotor UAV[A].2013 International Conference on Unmanned Aircraft Systems (ICUAS) [C].2013, Atlanta, GA.373-376.
[4]Inven Sense Inc.MPU9150 Product Specification Revision 4.0[S].Inven Sense Inc, 2012.
[5]秦永元.惯性导航[M].科学出版社, 2006:287-327.
飞行姿态控制 篇5
飞行器在大气层内的运动具有非线性、强耦合、不确定等特性,难以建立精确的数学模型,并且易受风干扰等随机干扰的影响,气动参数变化剧烈。因此,设计一个可靠的飞行器姿态控制系统是保证使其按预定轨迹运动的必要条件。目前应用较普遍的飞行器姿态控制技术是采用PID控制,简单可靠,性能稳定。但在某些恶劣环境下,对飞行器的姿态控制精度和稳定性要求较高,仅靠PID或改进的PID控制技术难以满足。
因此,本文将PID控制和Fuzzy控制结合起来应用在飞行器姿态控制系统中。Fuzzy控制是一种仿人思维的智能控制方法,不依赖于精确的数学模型,可以较好地解决由于飞行器模型不精确和随机干扰引起的控制问题。但Fuzzy控制很难解决系统本身存在的稳态误差,PID控制正好能够弥补这一不足。同时,为了改善Fuzzy控制器的性能,加入自动修正因子对其参数进行在线调整,保证控制系统能在大范围内获得最优的动态性能。
1 Fuzzy-PID控制器设计
1.1 总体设计
Fuzzy-PID控制的基本原理如图1所示。
控制系统由两部分组成:Fuzzy控制器和PID控制器。选取误差和误差变化率作为系统输入,输出为系统控制量u。控制系统根据偏差e的大小来决定采用何种控制算法。当误差过大或较大时,采用Fuzzy控制算法,加大控制作用抑制超调,提高系统的响应速度,使系统实际响应尽快达到给定值;反之,采用PID控制算法,减小系统稳态误差,改善静态特性。它比单独Fuzzy控制或者PID控制都有更好的控制性能。PID控制器的设计在本文不再赘述。
图1 Fuzzy-PID控制基本原理图
1.2 带自动修正因子的Fuzzy控制器设计
在设计Fuzzy控制器的过程中,主要分为5个部分
设输入量e、ec和输出量u的论域分别为E、EC和U,其模糊子集通常用{负大(NB),负中(NM),负小(NS),零(0),正小(PS),正中(PM),正大(PB)}来表示。选取控制量的一般原则是:当误差大或较大时,选取控制量以尽快消除误差为主;当误差较小时,选择控制量要以系统的稳定性为主,防止系统超调。根据知识经验加以总结得到模糊控制器的控制规则,见表1。
表1 模糊控制规则表
设偏差e和偏差变化率ec的量化因子分别为K1和K2,控制量U的比例因子为K3,它们在很大程度上影响模糊控制器的性能。K1越大,系统的超调量越大,过渡过程也越长;反之,则系统变化越慢,稳态精度降低。K2越大,系统超调量越小,输出变化率越小,但系统变化越慢;反之,则系统反应加快,但超调增大。K3主要影响系统的动态性能,与实际控制对象有关。
在实际工作中为了使系统快速响应,减小超调,在常规模糊控制器上引入修正因子n,在控制过程中对n值作调整,实时在线改变偏差E和偏差变化率EC的加权程度,从而取得更优的控制效果。
设模糊化的变量为N,模糊集为{AB,AS,OK,CS,CB},子集中元素分别代表高放、低放、不变、小缩、大缩;加入修正因子n经调整后得到新的量化因子、比例因子M1=n K1、M2=n K2、M3=K3/n。其调整原则是:当e和ec较大时,选取较大的控制量,即增大M3、减小M1和M2,减小偏差、加快动态响应;当e和ec较小时,即系统接近稳态值时,应减小M3,增大M1和M2,减小超调量、提高系统稳定性。因此,随着e和ec的变化,修正因子n实时自动调整。依据经验,得到修正因子n调整规则表如表1所示。
2 飞行器姿态控制系统
典型的飞行器姿态控制系统分别对纵向、横侧向、航向三个通道进行协调控制。本文以某飞行器为例,采用带自动修正因子的Fuzzy-PID控制方法对飞行器三个通道进行设计。对于俯仰通道,Fuzzy-PID控制器的输入为eφ=△φ,,偏航通道Fuzzy-PID控制器的输入为eΨ=△Ψ,,滚转通道,Fuzzy-PID控制器的输入为eγ=△γ,,俯仰舵偏δφ、偏航舵偏δΨ、滚转舵偏δγ为系统控制量。控制目标是尽可能提高飞行姿态的控制精度和稳定性,消除姿态角偏差,克服随机干扰。三通道Fuzzy-PID控制器结构如图2所示。
图2 三通道Fuzzy-PID控制结构图
Fuzzy控制器中,选取输入变量的论域为E,EC={-5,-4,-3,-2,-1,0,1,2,3,4,5};控制量U的论域U={-3,-2,-1,0,1,2,3}。其模糊子集为{NB,NM,NS,ZO,PS,PM,PB}。模糊控制规则表如表1所示。修正因子n的论域为{1/4,1/2,1,2,4},其模糊子集为{AB,AS,OK,CS,CB}。修正因子n调整规则表和隶属表分别如表2和表3所示。
3 数学仿真
在MATLAB7.0/Simulink环境下,采用上述设计的带修正因子的Fuzzy-PID控制系统应用到飞行器俯仰、横侧、航向三通道的姿态控制中,并进行数学仿真。系统仿真框图如图3所示。图4给出了俯仰舵偏、俯仰角速率、迎角及俯仰角的仿真结果。图5给出了偏航舵偏、侧滑角、偏航角速率以及偏航角的仿真结果。图6给出了滚转舵偏、侧滑角、滚转角速率以及滚转角的仿真结果。
表2 修正因子n调整规则表
表3 修正因子n隶属表
图3 系统仿真框图
图4 俯仰通道仿真曲线
图5 偏航通道仿真曲线
图6 滚转通道仿真曲线
设俯仰初始姿态角偏差为5°,偏航通道预定偏航角度为30°,滚转初始姿态角偏差为5°。从系统仿真结果中可知,在初始阶段姿态角偏差较大时,飞行器姿态控制系统切换到模糊控制,控制量输出较大,各通道均能够在13 s之内使姿态角偏差迅速减小至稳态附近,姿态角速度在允许的范围内,取得较好的控制效果。当姿态角偏差在零附近或较小范围变化时,飞行器姿态控制系统切换至PID控制,三通道的姿态角稳态精度均控制在5%以下,很好地解决了各种随机干扰以及模型不精确等因素带来的稳态误差。由此可以看出本文提出的控制方法集Fuzzy控制的优点和PID控制的优点于一体,能够迅速减小姿态角偏差,具有很好的控制性能。
为了验证本文所提带自动修正因子的Fuzzy-PID控制方法的准确性,本文以俯仰通道为例,与常规FuzzyPID控制进行了对比,如图4所示。在同一坐标系下,可看出纵向控制系统在引入自动修正因子n后,俯仰姿态角的调节时间和超调量都有大幅度减小,系统响应时间提高了20%,超调量减小了85%;系统过渡过程更加平稳,俯仰角速率的峰值也有所减小。由此可知带自动修正因子的Fuzzy-PID控制能够有效改善飞行器姿态角的动态响应特性,比常规Fuzzy-PID控制更能适应飞行器复杂飞行环境的要求。
4 结论
本文将Fuzzy控制和PID控制相结合,设计了带自动修正因子的Fuzzy-PID控制系统,并将其应用到飞行器姿态控制系统中。从仿真结果中可以看出该控制方法可以快速减小姿态角偏差。同时,为了改善模糊控制器的控制性能,利用修正因子对模糊控制器的参数进行实时在线修改,增强了参数在线自调整能力,稳态误差在5%以下,在抗干扰方面具有很好的效果,具有较强的自适应能力。
参考文献
[1]诸静.模糊控制原理与应用[M].北京:机械工业出版社,2005.
[2]刘朝英,宋哲英,宋雪玲.MATLAB在模糊控制系统仿真中的应用[J].计算机仿真,2001,18(3):11-14.
[3]SALIH A L.Modelling and PID controller design for a quadrotor unmanned air vehicle[J].IEEE AQTR.,2010,39(5):697-699.
[4]陈向坚.微型飞行器姿态的智能控制方法研究[D].长春:中国科学院研究生院(长春光学精密机械与物理研究所),2012.
[5]吴森堂,费玉华.飞行控制系统[M].北京:航空工业出版社,2006.
[6]彭晓华,屠兴汉.求解模糊非线性系统[J].辽宁工程技术大学学报:自然科学版,2010,29(5):776-780.
[7]张义捷,钟友武,许得军.基于模糊控制技术的飞行器姿态控制系统设计[J].航天控制,2013(2):28-32.
飞行姿态控制 篇6
人造的和自然界的各种电磁波充斥着地球表面和太空,这些电磁波信息与接收平台姿态具有密切关联性,电磁波提供的姿态基准不受运动平台姿态及其运动影响,可以像利用地磁场指南那样利用电磁波测量运动平台姿态。国内外都研究在地球卫星导航系统接收端,利用多点接收、三角计算的方法感知姿态[1,2,3,4,5,6,7,8,9],缺点是体积大、精度低、系统复杂和速度慢,影响了该技术的推广应用。本文利用3 个正交电( 磁) 场传感器接收电磁波三维电( 磁) 信号测量运动平台姿态,可以单点测量飞行器姿态,接收信号既可以相互独立,也可以相干,信号数任意,波达方向和极化状态任意,各信号强弱任意,因此不受多径效应困扰,既可以利用主动发射电磁波信号,也可以被动接收空间分布的广播电磁波信号测量姿态,提高了系统的隐蔽性,进一步揭示了电磁波测量姿态的能力。
1 信号模型
电磁波信号沿- un方向传播,如图1( a) 所示,n = 1,2,…,N。 电磁波空间到达方向用参量(θ,φ) 表示,分别表示仰角和方位角,- π /2 ≤ θ ≤π /2,- π < φ≤π,则图1( a) 矢量
用极化椭圆描述子( γn,ηn)表示电磁波的极化属性,如图1( b) 所示,极化角- π/2 < γn≤π /2,极化椭圆率- π/4≤ηn≤π /4。ηn= 0 时,极化椭圆压缩为长轴所在的直线,ηn= ± π /4 时,极化椭圆为圆形。如图1( b) 所示,以电场极化椭圆为参照,向量un,描述了波达方向和极化状态的电磁波空间结构信息,分别为电场极化椭圆的长短轴方向向量,电磁波结构向量un,相互正交,波结构向量可作为姿态测量的参照,以波结构向量为3 个坐标轴构成的直角坐标系称为波结构坐标系。
电磁矢量传感器接收单元示意图如图2 所示。全电磁矢量传感器包含3 个正交电场和3 个正交磁场接收单元。
3 个正交电场传感器为3 正交偶极子天线,在基准姿态从第n信号源接收到的信号导向矢量表达为[10]:
式( 1) 信号导向矢量可进一步表示为:
式中,Bn为波结构坐标系( 见图1) 到平台天线坐标系的转换矩阵,
2 姿态测量基本原理
电磁矢量传感器安装在姿态变化的运动平台上,选取运动平台的某一姿态为基准姿态,标定好基准姿态平台天线坐标系与大地坐标系的关系。接收机天线平台运动会改变天线姿态,使空间姿态有关参数( θn,φn,γn)改变; 但 ηn与空间姿态变化无关,不会变。在运动平台基准姿态,全电磁矢量传感器的3 个正交电场传感器从n个固定信号源接收到的信号表达为: Y( t) = AS( t) + e( t) ,其中,Y( t) =[y1( t) ,y2( t) ,y3( t) ]T,t = 1,…,K为时间采样点。A = [a1,…,an] 为3 × N矩阵,S ( t ) =[s1( t) ,…,sn( t) ]T,各固定信号源可以独立,也可以相干。e( t) 为随机噪声。因一般信号为2 个极化信号的叠加,所以上述表达式可表示一般信号。忽略噪声,接收信号的互相关矩阵为:
平稳的固定信号源的RY为常数阵。
当运动平台姿态变化,天线的接收信号不同于基准姿态信号,实时姿态时接收信号为: Y'( t') =μA'S( t') + e' ( t') ,其中,Y' ( t) = [y1' ( t') ,y2' ( t') ,y'3( t') ]T,A' =[a'1,…,a'n]为3 × N矩阵,e'( t') 为随机噪声,μ 为接收机因姿态变化形成的信号复衰减因子。3 个正交电场传感器从第n信号源接收到的信号导向矢量表达为:
其中波结构坐标系到实时天线坐标系的转换矩阵
注意2 种姿态下同一信号波结构向量相对于大地不变,以第n信号波结构向量作为参照,得实时天线坐标系到基准天线坐标系的转换矩阵Ce= Bn' Bn- 1,Bn'= CeBn,于是,
根据三维空间旋转理论[11]知三维旋转矩阵可以用单一旋转向量 φ = [ф1ф2ф3]T描述,其中[ф1ф2ф3]为旋转向量 φ 在图1 坐标系中坐标,旋转矩阵就等于:
且,其中i = 1,2,3。
实时姿态时接收信号互相关矩阵为:
式中,λ = μμH为衰减因子。
对于时不变信号:E[S(t')SH(t')]=E[S(t)SH(t)],这样,K个快拍信号Y'(t')符合正态分布:,未知参数向量[φT,σ,λ]T,其中,因FISH阵元素
符号tr[]表示求矩阵的迹,所以可算得估计 φ 的CRB( 克拉美—罗界) 为[12,13,14]:
3 参数估计算法
估计的算法有多种,因RY'= λ CeRYCeH+ σ2I,所以RY'和CeRYCeH形成的向量近似平行,根据
可得,
式中,符号vec( ) 表示将矩阵各列组合为向量。
子空间正交法或称类MUSIC法,也可估计,因Ce为实正交阵,所以和有近似相同的特征值比例,对和进行特征谱分解:
Λ 中对角元素较大值对应特征向量U1和U'1分别张成广义信号子空间,较小特征值对应特征向量U2和U'2张成广义噪声子空间。由此得
接收信号可以为相干信号,不受多径效应困扰,信号总数N可以为1 到无穷大,波达方向和极化状态任意,各信号强弱任意。对接收电磁波环境的要求是式( 3) 确定的CRB为有限值; 接收极化三维电( 磁) 信号互相关矩阵在解除接收姿态旋转变化影响后保持平稳,即运动平台测量姿态所处时、空、频、极化域,信号二次统计特性不变。因此本文既可以主动利用发射电磁波信号,也可以被动利用空间分布的广播电磁波信号测量姿态,提高了系统的隐蔽性。
4 姿态测量具体实施步骤
姿态测量具体实施步骤如下:
① 确定运动平台基准姿态坐标系与大地坐标系转换关系。
② 在运动平台基准姿态以极化敏感的电磁矢量传感器中的3 个正交电( 磁) 场传感器接收电磁波信号,估计三维电( 磁) 信号互相关矩阵。计算姿态参数估计的CRB( 克拉美—罗界) 是否为有限值,判断该环境电磁波信号能否用于运动平台姿态测量。
该步骤对电( 磁) 极化三维特征进行学习评估,在单一接收点以极化敏感的电磁矢量传感器中的3 个正交电( 磁) 场传感器在基准姿态接收电磁波信号,选取时间t中的K个采样点采样k = 1,2,…,K次,计算估计值为:
充分采样才能获得精确的估计值,对于主动发射的电磁波,也可以通过理论计算估计。
③ 在运动平台实时姿态以极化敏感的电磁矢量传感器中的3 个正交电( 磁) 场传感器接收电磁波信号,计算估计三维电( 磁) 信号互相关矩阵。对实时姿态和基准姿态互相关矩阵RY'和RY,进行特征植分解,根据各自特征值比例是否近似相等,初略判断该环境电磁波信号是否平稳。
④ 根据实时姿态与基准姿态互相关矩阵RY'和RY估计旋转向量 φ = [ф1ф2ф3]T,计算得到实时天线坐标系到基准天线坐标系的转换矩阵Ce。
⑤ 用步骤④得到转换矩阵将实时姿态坐标系下的运动平台姿态,转换到基准姿态坐标系下,再转换到地理坐标系下,完成运动平台姿态感知。
在接收机平台实时姿态坐标系中,平台姿态方向向量为单位阵
的3 个列向量,用左乘运动平台姿态矩阵,将实时姿态坐标系下的运动平台姿态向量矩阵,转换到基准姿态坐标系下。利用步骤①得到的运动平台基准姿态坐标系与大地坐标系转换关系,将基准姿态坐标系下运动平台姿态矩阵转换到地理坐标系下,完成运动平台姿态感知[15,16]。
根据电磁信号对称性,可单独利用电或磁信息测量运动平台姿态,也可融合电磁信息提高测量姿态精度。
5 仿真实验
实验设定测控基站发射一完全极化测控信号,飞行平台接收电磁波空间到达方向用参量(θ0,φ0)表示,分别表示仰角和方位角,用极化椭圆描述子( γ0,η0)表示电磁波的极化属性,如图1( b)所示,选取波结构坐标系作为基准坐标系,设接收信号功率为p0,理论计算得基准姿态互相关矩阵:
式中,p = λp0。
代入式( 3) 得:
式中,ρ = p /σ2,这个结果与文献[10]一致。由此式知,η0= ± 45°的圆极化随机信号的CRB(ф1) = ∞,不能用于测量姿态。
仿真实验设定独立完全极化测控信号电磁波空间到达方向的仰角和方位角(170°,40°),电磁波的极化属性(20°,15°)。设定接收平台处p0= 1,信噪比10 d B,选取波结构坐标系作为基准坐标系,实时天线坐标系到基准天线坐标系的转换矩阵Ce对应的旋转向量 φ = [40°- 30°- 75°]T,根据理论计算得RY,根据数据仿真得RY',采用矩阵向量平行法得出的谱对数随 ф1和 ф2变化的曲面如图3( a)所示,它的等位线图如图3 ( b) 所示,由图可见峰值出现在正确的参数位置。
采用子空间正交法得出的谱对数随 ф1和 ф2变化的曲面如图4( a) 所示,它的等位线图如图4 ( b)所示,由图可见峰值也出现在正确的参数位置,且峰形更尖锐。在完全极化测控电磁波基础上增加寄生极化波,寄生电磁波的极化属性(- 70°,15°),幅度为原完全极化测控电磁波幅度的0. 3 倍,寄生电磁波也从基站发出,此时测控信号为部分极化波,再增加一个由此部分极化波多径反射形成的部分极化波,幅度为原部分极化波幅度的0. 3 倍,入射方向(- 10°,10°) 。采用图4 同样的子空间正交法算法得出的谱对数随 ф1和 ф2变化的曲面如图5( a) 所示,它的等位线图如图5 ( b) 所示,由图可见峰值出现在有较大误差的参数位置。仿真通过在基准姿态接收电磁波信号,估计基准互相关矩阵,替代根据理论计算得RY,进行姿态参数估计,校正基准姿态互相关矩阵后,子空间正交法谱对数随 ф1和 ф2变化图形如图6( a) 所示,它的等位线图如图6 ( b) 所示,由图可见峰值回到正确参数位置。
6 结束语
测控技术利用电磁波时域信息测距,频域信息测速,空域信息测方位,极化域信息却没有得到深入研究和利用,本文正是在这一领域展开探索,只需运动平台上有单一接收点,就可实现姿态/航向感知,系统可微型化,信号不受多径效应困扰,既可以利用主动发射电磁波信号,也可以被动接收空间分布的广播电磁波信号测量姿态,提高了系统的隐蔽性。适用于空或天飞行器。
摘要：以电磁波为参照的飞行器姿态测量研究,可弥补空间参照物缺乏,丰富姿态测量手段。利用3个正交电(磁)场传感器接收电磁波三维电(磁)信号,测量运动平台姿态。该方法根据姿态参数估计的CRB(克拉美—罗界)是否为有限值,判断该环境电磁波信号能否用于运动平台姿态测量;根据基准姿态与实时姿态互相关矩阵变化,估计得到实时天线坐标系到基准天线坐标系的转换矩阵,据此将实时姿态坐标系下的运动平台姿态,转换到基准姿态坐标系下,再转换到地理坐标系下,完成运动平台姿态感知。电磁波信号可以为独立或相干信号,波达方向和极化状态任意,可不受多径效应困扰。仿真试验表明该方法稳定有效。
飞行姿态控制 篇7
姿态最优估算是飞行器自主飞行的先决条件, 在飞行过程中, 实时准确地获得飞行器的姿态信息, 可以决定飞行器的控制精度和稳定性[1]。 随着微电子技术的发展,飞行器姿态解算系统普遍采用低成本的航姿参考系统(AHRS), 其主要包括: 基于微机电系统(MEMS) 的三轴陀螺仪、三轴加速度计和三轴磁罗盘。 由于系统机械结构和传感器自身三轴并非完全正交,引入了不正交误差,以及数据积分产生的发散误差和外部干扰产生的随机误差。 因此要估计飞行器姿态的最优信息必须对多传感器进行误差处理,并寻求最佳数据融合算法。
目前国内对MEMS传感器姿态解算系统研究的常用算法有:扩展卡尔曼滤波、梯度下降法、互补滤波等。 扩展卡尔曼滤波应用非常广泛,但为其建立可靠稳定的状态方程、确定合适的量测噪声和过程噪声协方差矩阵都比较困难。 梯度下降法的好处在于姿态解算过程只有普通的乘法和加法运算,普通的微控制器即能满足算法要求,但难点在于构造合适的目标函数[2]。 本系统在Mahony[3]提出的补偿滤波器的基础上进行优化,采用双环补偿滤波算法, 外环引用重力场和地磁场进行补偿, 内环引用重力场进行补偿,并将修正后的陀螺仪和磁罗盘进行互补滤波,以期得到最优姿态输出。
1 飞行器姿态描述
飞行器常用的姿态描述方法有欧拉角、 方向余弦矩阵和四元数三种。 三维空间中,飞行器有3 个转动自由度, 分别可以通过3 个角度参数来描述飞行器的姿态,分别定义为: 绕机体坐标系的Z轴转动为偏航角 Ψ;绕X轴转动为横滚角准; 绕Y轴转动为俯仰角 θ[4]。
如图1 所示,飞行器在飞行过程中,地理坐标系b通过绕不同坐标轴三次连续转动来实现对机体坐标n的转换。 第一次转动,OX0Y0Z0坐标系即地理坐标系ENU绕Z0轴转动 Ψ 角,得到坐标系OX1Y1Z0; 第二次转动,OX1Y1Z0坐标系绕OX1轴转动准角,得到坐标系OX1Y2Z2; 第三次转动,OX1Y2Z2坐标系绕OY2轴转动 θ 角, 得到坐标系OXYZ , OY2轴即OY轴[5]。 这三次坐标轴旋转可以用数学方法表述为3 个独立的方向余弦矩阵。 两个坐标系的转换可以用这3 个独立变换矩阵的乘积Cbn表示:
由于载体的运动, 四元数Q =[q0q1q2q3]T是变量,q0、 q1、 q2、 q3是时间函数。 刚体绕瞬时转轴转过 σ角度来实现坐标的转换。为参考坐标系下的一个单位矢量,其角速度为:
机体坐标系n和地理坐标系b之间的变换也可以用四元数的三角形式表示[6]:
在忽略地球的自转和公转运动对四旋翼飞行器造成的影响, 并假设飞行器为刚体运动且质量分布对称、均匀, 中心为重心位置且不随着运动而发生变化[7], 则四元数矩阵表达式Q的微分方程解为:
式(5) 中 ωxb、 ωyb、 ωzb分别表示机体坐标系相对于地理坐标系沿各个轴向的角速度分量。
2 飞行器姿态解算系统构建
2 . 1 系统硬件
本姿态解算系统硬件主要包括主控制器和航姿参考系统(AHRS)。 其中主控器采用ST(意法半导体)公司的Cortex-M4 为内核的32 位ARM微控制器STM32F405RGT6;航姿参考系统采用2 款高精度的姿态传感器, 分别为:Inven Sense公司的MPU - 6500 和ST公司的LSM303D , 构成了一个九轴的姿态测量系统。 传感器均选用数字芯片,内部均集成有高精度的ADC。 主控制器通过SPI总线, 读取出传感器采集到的原始数据, 然后通过四元数的坐标换算、 姿态误差的双环PI控制和互补滤波解算出欧拉角。 姿态解算系统框图如图2 所示。
2 . 2 传感器数据标定
在数据融合之前需要对传感器测量得到的原始数据进行标定,从而消除传感器的零点偏置误差。 陀螺仪消除偏置误差的方法是保持器件水平位置不变,多次采集后求取输出平均值;加速度计消除偏置方法为八面校准法, 将飞行器姿态解算系统保持机头方向不变, 正面朝上、下、左、 右、 前、 后和机头朝上、 机头朝下。 在这八种状态下分别测量三轴加速度计ADC输出的X、Y、Z三轴的最大最小值,并与±1 g(重力加速度)所对应的ADC数值进行校准;磁罗盘零点数据误差采用椭圆假设法进行标定, 将姿态解算系统分别在坐标系下XOY和XOZ两个平面内360°旋转,并将采集到的数据用MATLAB软件拟合成椭圆,然后修正椭圆的偏心和半径。
3 系统数据融合与姿态解算
系统姿态解算分为两个部分。 第一部分是双环PI控制器修正数据融合后传感器偏差。 第二部分为修正后的三轴陀螺仪与三轴磁罗盘进行互补滤波,消除磁罗盘的高频干扰,提高磁罗盘的响应能力和抗干扰能力。 姿态解算流程图如图3 所示。
3 . 1 双环PI控制器
陀螺仪可以快速响应机体的旋转,短时间误差较小、可信度高, 但是存在温漂和零漂, 以及积分误差随时间累积等因素影响。 加速度计在静止的状态下漂移很小,倾角求解过程不存在积分误差, 但是受到飞行过程中电机和机架的振动以及转动和运动加速度的干扰。 磁罗盘测量得到的地磁向量在一定地理范围内可以认为不会发生改变,但是,磁罗盘容易受到硬磁场和软磁场的干扰。
因此,系统外环采用九轴姿态传感器( 三轴加速度计、 三轴磁罗盘和三轴陀螺仪)数据融合。 由于磁罗盘容易受到干扰,有可能导致外环九轴数据融合后依旧存在较大误差, 所以, 内环采用六轴姿态传感器( 三轴加速度计和三轴陀螺仪) 数据融合,对数据融合后的传感器姿态偏差进行二次修正。
外环九轴姿态传感器数据融合, 记在飞行器机体坐标系下an=[ax ay az]T和mn=[mx my mz]T分别为加速度计和磁罗盘实际测量得到的重力向量和地磁向量。记vn=[vx vy vz]T和wn=[wx wy wz]T是将地理坐标系下重力向量kb=[0 0 1g]T和地磁向量nb=[nx 0 nz]T( 不考虑地理磁偏角因素, 将机头固定向北) 通过四元数坐标换算成机体坐标系下的重力向量和地磁向量。 向量之间的误差为坐标轴的旋转误差, 可以用向量的叉积en= [ ex ey ez ]T表示,如式(6)所示。 由于陀螺仪是对机体直接积分, 所以, 陀螺仪的误差可以体现为机体坐标的误差。 因此修正坐标轴的误差可以达到修正陀螺仪误差的目的, 从而将加速度计和磁罗盘进行修正陀螺仪,实现了九轴的数据融合。 即如果陀螺仪按照叉积误差的轴, 转动叉积误差的角度, 就可以消除机体坐标上实际测量的重力向量和地磁向量和坐标换算后的重力向量和地磁向量之间的误差。
PI调节器的比例部分用于迅速纠正陀螺仪误差, 积分部分用于消除稳态偏差[7]。 通过大量实验验证后取PI调节器的比例系数为1.0,积分系数为0.2。 陀螺仪经过外环PI控制器修正姿态误差后输出值为gn=[gx gy gz]T:
内环的六轴姿态传感器数据融合是将地理坐标系下的重力场向量与加速度计在机体坐标系下采集到的重力向量进行叉乘,求出两者向量误差。 并通过PI控制器修正向量误差,从而达到修正外环九轴数据融合后的陀螺仪的偏差的目的。 在每个姿态解算周期读取出机体坐标系下双环PI控制后的陀螺仪的角速率 ωxb, ωyb, ωzb,求解式(4)、式(5)中的四元数微分方程,即可得到姿态四元数q, 从而求得坐标转换矩阵Cbn, 然后根据姿态矩阵和欧拉角对应关系,求出欧拉角。
3 . 2 互补滤波
本系统姿态解算的第二部分是磁罗盘的修正, 主要采用互补滤波算法。 互补滤波算法简单可靠且对器件精度要求低。 互补滤波的目的在于将陀螺仪和磁罗盘各自在频域角度上进行互补。 因为磁罗盘动态响应能力差且易受外界环境的干扰,而陀螺仪动态响应快,故可在频域上将二者形成互补,即对磁罗盘低通滤波,对陀螺仪进行高通滤波。 如图4 所示,将修正后得到的陀螺仪数据与磁罗盘数据进行互补滤波,即可消除磁罗盘的高频干扰。
如式(10) 所示,r为互补滤波中的权值,r值越大, 证明陀螺仪占的权重越大。
4 姿态解算结果分析
为了验证姿态解算系统在多旋翼飞行器实际飞行过程中的性能, 搭建了一个半实物仿真平台。 将基于STM32 的姿态解算系统的四旋翼飞行器机头朝向地理正北方, 机架底部水平固定在万向节一端, 万向节另外一端固定在水平的桌面上。 启动四旋翼飞行器电机,将飞行器油门通道值保持在空中悬停状态,及四个螺旋桨产生的向上的合力等于飞行器自身重力。 然后,通过串口读取出传感器采集到的原始数据以及姿态解算后的数据,并用MATLAB进行数据分析。
图5 中采集的是模拟定点悬停时俯仰角和横滚角。 由于单纯地通过陀螺仪采集得到的角速度进行积分得到角度会产生积分误差, 并且积分误差会随着时间进行累加,所以未进行姿态解算所得到的姿态角零点误差会不断变大。 图中的2 条线分别代表姿态解算后的横滚角和俯仰角。 由于采用了双环PI修正陀螺仪,所以姿态角零点误差与解算前相比不会随着时间累计不断变大。 但是由于姿态解算系统处于四旋翼飞行器模拟定点悬停状态, 所以,电机引起的机架振动、载体重力分布不均、机械连接处阻尼问题等因素,导致零点误差相对于水平放置在桌面上时的静态零点误差,偏差较大且波动相对厉害。 零点误差基本稳定在±2.2°以内。 随着时间的推移,零点误差逐步趋近平稳,可以满足四旋翼飞行器对姿态数据的要求。
图6 中测试结果分别代表磁罗盘姿态解算前采集的数据和姿态解算后测量的数据。 本系统采用的互补滤波算法中权值r取0.15,偏航角以磁罗盘采集的数据为主,数据融合后的陀螺仪只进行辅助修正。 可以观测到细线代表的是没有使用滤波算法的磁罗盘测量的偏航角, 零点误差相对较大, 达到了 ±3° 左右, 而且上下波动也很大,表示磁罗盘受到外界环境严重干扰。 粗线代表磁罗盘和陀螺仪经过互补滤波后采集得到的偏航角,消除了部分干扰信号, 上下波动明显减弱, 并且误差稳定保持在±1.7°以内。 由于磁罗盘自身具有不可忽略的零位、 灵敏度及非正交误差, 因此通过软件算法无法彻底消除零点误差。
5 结论