四旋翼飞行器是近来各大科研机构研究的热门方向,由于其控制算法经典而又复杂,现已成为各大科研院校作为学生实践能力提升的一项重要实践课。自以来,在每届全国大学生电子设计竞赛中,四旋翼飞行器题也已成为参赛学生热门首选题目[1]。然而,由于四旋翼飞行器具有非线性、强耦合、欠驱动及多变量等特点,使得飞行器的设计及其控制都难以达到预定要求。
1 四旋翼飞行器物理数学模型
1.1 坐标轴选取
本文选取载体坐标系来描述飞行器的运动规律,如图1所示,载体坐标系其原点位于机体质心,选取右前上坐标系,即其X轴沿机体横轴向右,Y轴沿机体纵轴向前,Z轴沿机体竖轴向上[2]。根据载体坐标系和导航坐标系之间的相对关系来定义载体的航向角Yaw、俯仰角Pitch、横滚角Roll 3个姿态角。
1.2 姿态解算方案
当前很多学者对四旋翼飞行器采用欧拉角法、方向余弦法进行姿态解算,经仔细分析可知这些姿态解算方案存在缺陷[3-4]。欧拉角微分方程仅有3个未知数,但每个方程都包含三角函数运算,当θ=90°时方程将出现奇点,此时会导致飞行器姿态解算错误,编程人员只能在θ=90°时强制修正姿态角以避开解算错误,因此欧拉角法不能全姿态工作;方向余弦法虽然可以全姿态工作,但微分方程组高达9维,计算量大,对CPU计算速度要求高,而且不可避免地会产生非正交化误差。本文将采用四元数法进行飞行器全姿态解算[5],可以保证全姿态工作,不受限制,且微分方程只有四维,相比方向余弦法计算量小,四元数法得到的方向余弦矩阵的性能优于方向余弦法。
四元数由1个实数单位和3个虚数单位i,j,k构成4个元的数,可表示为:
则导航坐标系n到载体坐标系b的四元数变换为:
至此,四旋翼的物理数学模型建立完毕,在程序设计中将通过软件算法实现以上数据姿态解算。
2 系统硬件电路设计
2.1 系统电路设计方案
控制系统电路结构包括单片机、显示模块、光流模块、惯性检测、按键模块以及电机、电调等部分。采用STM32F103单片机作为主控[7],其主频达到72 MHz,保证了姿态数据的快速解算,提高了系统的响应速度。
采用GPS定位模块与光流模块相结合的方式来实现定位与悬停,这可以满足室内外均可飞行的需要。惯性检测单元采用MPU6050[8]模块,用来实现对飞行器当前的姿态角进行检测,从而控制飞行器的飞行姿态。电调采用大功率MOS管形成电子调速器,电机采用飓风U2208无刷电机,保证系统的稳定性及可靠性,系统电路结构图如图2所示。
2.2 硬件电路原理图设计
根据图2所示系统电路结构图,设计的电路原理图如图3所示[9]。采用意法半导体公司微处理器STM32F103C8T6作为主控单元,惯性处理单元采用全球首例整合性6轴运动处理组件MPU6050,整合了陀螺仪和加速度计传感器为一体,免除了二者时间轴之差的问题[8];根据MPU6050的I2C接口扩展磁场方案,扩展了磁场传感器HMC5883,实现了九轴运动处理;设计了4路采用MOS管Q1实现的电子速度调节器,以实现对4路无刷电机速度的控制调节等。
3 系统软件程序编写
3.1 数据滤波程序设计
飞行器在运行时,电机高速转动引起的机体高频振动、环境电磁干扰以及温漂等均会产生干扰噪声信号,这些信号会被灵敏的惯性传感器MPU6050采集到,给后期控制带来干扰误差,因此需要设计滤波器将其去除或降至最低。由于下一时刻的角度、角速度数据具有可预测性,因此本文将采用卡尔曼滤波利用系统线性状态方程对系统观测数据、状态进行实时最优估计,以滤去这些干扰噪声信号。设计的卡尔曼滤波器详细参数如下[10-11]:
飞行器上电后,将卡尔曼滤波前后数据通过无线模块传递给上位机进行结果观察,飞行器在各轴平衡位置附近来回运动时,卡尔曼滤波很好地滤去了这些干扰信号,其中X轴滤波前后波形图如图4所示。从图中可以看出,卡尔曼滤波滤去这些干扰噪声,X轴角度数据输出稳定,其他轴数据与此类似。
3.2 串级PID控制器设计
卡尔曼滤波后的角度数据、角速度数据最终作为控制系统实时姿态信息。由于四旋翼飞行器运动过程复杂,单级PID调节无法保证控制系统稳定性,参数过大时,姿态角由大角度偏差恢复到平衡位置时极易引起震荡;参数过小时,姿态角由小角度偏差恢复到平衡位置时又显得力度不够。此问题主要是因为在单级PID调节中,角度恢复速度无法跟随角度大小变化来控制导致。因此本文将考虑采用串级PID调节方案[12-13],将PID控制器分内环与外环两个层面来考虑。如图5所示,内环主要对角速度进行PID调节,期望值为外环角度。这可保证系统从偏离期望角向平衡位置转动的角速度与角度成正比,即:角度偏差较大时,转动角速度大;角度偏差小时,转动角速度也小。这一方面保证了恢复到平衡位置的速度,另一方面避免了在平衡位置震荡问题。
设3路PID控制器的输出分别为RollQut、PitchOut、YawOut,每个电机得到驱动电路给出的PWM线性组合为:
X轴Pitch左端电机:Throttle+roll_out+yaw_out;
X轴Pitch右端电机:Throttle-roll_out+yaw_out;
Y轴Roll前端电机:Throttle-roll_out-yaw_out;
Y轴Roll后端电机:Throttle+pitch_out-yaw_out。
将飞行器分别沿X轴、Y轴固定,以调整内外环PID参数。首先调整内环PID参数,此时设置外环输出为零,使内环期望值为零,调整PID参数,随意沿X轴或Y轴转动飞行器至某一角度,当其能自动停止在该角度且不震荡时为内环最佳PID参数。然后调整外环PID参数,直至外环角度恢复有力且不震荡时为外环最佳PID参数。至此串级PID控制器设计完毕。
4 实验验证及分析
4.1 实验系统验证[14]
根据以上设计,将陀螺仪、加速度计数据经卡尔曼滤波器融合滤波后,将数据通过串口发送至PC上位机(波特率为9 600 b/s),通过匿名科创提供的地面站软件,对飞行器横滚角解算、俯仰角解算均进行了3D观察校验。当手动旋转X轴到某一角度时,上位机显示俯仰角会跟随偏转而其他角度不变;当手动旋转Y轴到某一角度时,上位机显示横滚角会跟随偏转而其他角度不变,过程平稳,响应速度快,证明此设计符合要求。其中俯仰角解算结果如图6所示,其他与此类似。
4.2 实验结果分析
通过室外飞行可以看出,飞行器能够平稳地在空中飞行。通过对横滚角、俯仰角、偏航角3种姿态角控制,可以实现飞行器在空中的稳定悬停、前进、后退、偏航等姿态。
5 结论
四旋翼飞行器是一种较为复杂的控制系统,本文通过对四旋翼飞行器进行物理数学模型创建,分析了几种姿态解算方案的不足,设计了四元数姿态解算方案,消除了运算中的转动不可交换性误差,设计卡尔曼滤波对加速度计、陀螺仪数据进行融合滤波,保证了数据的稳定性及可靠性。同时,对四旋翼飞行器进行了电路设计和程序编写,实验结果表明,该系统能够在空中实现稳定悬停并能按预定方向平稳飞行。
参考文献
[1] 王莹.全国大学生电子设计竞赛落幕,4.3万名学生参赛[J].电子产品世界,(1):85-86.
[2] 潘锦珊.气体动力学基础(3版)[M].西安:西北工业大学出版社,1995.
[3] 野莹莹,张艳珠,邹心宇,等.欧拉角姿态解算的改进[J].装备制造技术,,279(3):110-111,140.
[4] 王勇军,李智,李翔.采用组合滤波算法的无人机航向测量系统研究[J].电子技术应用,,44(2):39-43.
[5] 叶锃锋,冯恩信.基于四元数和卡尔曼滤波的两轮车姿态稳定方法[J].传感技术学报,,25(4):524-528.
[6] 张德先,聂桂根.基于分解四元数的自适应姿态四元数卡尔曼滤波[J].控制理论与应用,(3):367-374.
[7] 孙威,殷兴辉,王新君.基于STM32的频谱测量系统的设计[J].电子技术应用,,45(3):102-105,109.
[8] 黄瑞敏,李建奇,张斌,等.四旋翼飞行器追踪地面移动目标控制策略研究[J].电子技术应用,,44(5):73-77,82.
[9] 华成英.模拟电子技术基本教程[M].北京:清华大学出版社,.
[10] 乔少杰,韩楠,朱新文,等.基于卡尔曼滤波的动态轨迹预测算法[J].电子学报,,46(2):418-423.
[11] 梁龙凯,张丽英,何文超,等.概率神经网络多模型卡尔曼滤波定位导航算法[J].电子技术应用,,44(6):66-68,73.
[12] 林巍,王亚刚.串级控制系统闭环辨识及PID参数整定[J].控制工程,(1):11-18.
[13] 张涛,张晓宇,王辉俊.基于PWM和PID的直流电动机控制系统设计与制作[J].实验室研究与探索,,33(7):134-138.
[14] 江卉.四旋翼无人机姿态角回路稳定性控制仿真[J].计算机仿真,(1):70-73.
作者信息:
田 睿1,2,孙迪飞1
(1.河南工业职业技术学院 基础科学部,河南 南阳473000;2.郑州大学 物理工程学院,河南 郑州450001)