第一篇：新手四旋翼算法总结
新手四旋翼算法总结 一．姿态结算（匿名版程序）
首先，程序中一般用了两种求解姿态的方法，一种为欧拉角法，一种为四元数法
（1）欧拉角法静止状态，或者总加速度只是稍微大于g时，由加计算出的值比较准确。
使用欧拉角表示姿态，令Φ,θ和Φ代表ZYX 欧拉角，分别称为偏航角、俯仰角和横滚角。载体坐标系下的 加 速 度(axB,ayB,azB)和参考坐标系下的加速度(axN, ayN, azN)之间的关系可表示为(1)。其中 c 和 s 分别代表 cos 和 sin。axB,ayB,azB就是mpu读出来的三个值。
这个矩阵就是三个旋转矩阵相乘得到的，因为矩阵的乘法可以表示旋转。
ccaxBayBcsssc azBsscsccsccssssccsssaxNayN(1)scccazNaxN0ayN0（2）azNg飞行器处于静止状态，此时参考系下的加速度等于重力加速度，即
把（2）代入（1）可以解的
arctg(axBaa2yB2zB)（3）
arctgayB（4）azB即为初始俯仰角和横滚角，通过加速度计得到载体坐标系下的加速度即可将其解出，偏航角可以通过电子罗盘求出。
（2）四元数法（通过处理单位采样时间内的角增量(mpu的陀螺仪得到的就是角增量)，为了避免噪声的微分放大，应该直接用角增量-------抄的书）
上匿名的程序
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){
float norm;// float hx, hy, hz, bx, bz;
float vx, vy, vz;// wx, wy, wz;
float ex, ey, ez;
// 先把这些用得到的值算好
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;// float q0q3 = q0*q3;
float q1q1 = q1*q1;// float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)
return;
norm = sqrt(ax*ax + ay*ay + az*az);
//acc数据归一化
ax = ax /norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity and flux(v and w)
vx = 2*(q1q3q1q1az*vy);
//向量外积在相减得到差分就是误差
ey =(az*vxay*vx);
exInt = exInt + ex * Ki;
//对误差进行积分
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
//将误差PI后补偿到陀螺仪，即补偿零点漂移
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减
// integrate quaternion rate and normalise
//四元素的微分方程
q0 = q0 +(-q1*gxq3*gz)*halfT;
q1 = q1 +(q0*gx + q2*gzq1*gz + q3*gx)*halfT;
q3 = q3 +(q0*gz + q1*gy2 * q3* q3 + 1)* 57.3;// yaw
Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;// pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1q0q2);
/
vy = 2*(q0q1 + q2q3);
vz = q0q0q2q2 + q3q3;可以看到vx，vy，vz为CRb的最后一列的三项，四元数矩阵带入（1）式得vx，vy，vz分别是axB，ayB，azB每一项g前的系数。且静止情况下vx，vy，vz组成向量模长基本可以认为为1.3.norm = sqrt(ax*ax + ay*ay + az*az);
//acc数据归一化
ax = ax /norm;
ay = ay / norm;
az = az / norm;以上已说，由四元数倒推回去的加速度，向量模长为1，为了比较误差进行归1化，算的由加计得出的向量。4.ex =(ay*vzax*vz);
ez =(ax*vyq2*gyq3*gy)*halfT;
q2 = q2 +(q0*gyq2*gx)*halfT;对四元数进行跟新，这里用的是方程的数值解法，求得的解释近似解，总之就是跟新了四元数 8．
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;对四元数进行规范化，即化为模长为1，因为只有规范化的四元数才能表示刚体旋转。9．
Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;// pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1ACC_OFFSET.X;MPU6050_ACC_LAST.Y=((((int16_t)mpu6050_buffer[2])<< 8)| mpu6050_buffer[3])ACC_OFFSET.Z;MPU6050_GYRO_LAST.X=((((int16_t)mpu6050_buffer[8])<< 8)| mpu6050_buffer[9])GYRO_OFFSET.Y;MPU6050_GYRO_LAST.Z=((((int16_t)mpu6050_buffer[12])<< 8)| mpu6050_buffer[13])-GYRO_OFFSET.Z;
这里还要说一点，这里加速计的数据用的是滑动平均值滤波法 void Prepare_Data(void){ static uint8_t filter_cnt=0;int32_t temp1=0,temp2=0,temp3=0;uint8_t i;
MPU6050_Read();MPU6050_Dataanl();
ACC_X_BUF[filter_cnt] = MPU6050_ACC_LAST.X;ACC_Y_BUF[filter_cnt] = MPU6050_ACC_LAST.Y;ACC_Z_BUF[filter_cnt] = MPU6050_ACC_LAST.Z;for(i=0;i temp1 += ACC_X_BUF[i]; temp2 += ACC_Y_BUF[i]; temp3 += ACC_Z_BUF[i];} ACC_AVG.X = temp1 / FILTER_NUM;ACC_AVG.Y = temp2 / FILTER_NUM;ACC_AVG.Z = temp3 / FILTER_NUM;filter_cnt++;if(filter_cnt==FILTER_NUM)filter_cnt=0; GYRO_I.X += MPU6050_GYRO_LAST.X*Gyro_G*0.0001;GYRO_I.Y += MPU6050_GYRO_LAST.Y*Gyro_G*0.0001;GYRO_I.Z += MPU6050_GYRO_LAST.Z*Gyro_G*0.0001;} 资料在附带文件中。 2012年辽宁省普通高等学校大学生 机械设计大赛参赛作品说明书 参赛单位：辽东学院 设计课题：四轴碟形飞行器 参赛学生：吴家帅、李成祥、石 岩、祁 勇、杨惠麟 指导教师：刘 瀛、刘广达 -***782这款四轴飞行器适合多种型号的飞控板配合使用。有利于批量生产，四轴飞行器又一亮点是成本远远低于功能相同的航模价格。此外，工人易操作，使用简单，维护方便，用途多样。1.2国内外相关研究现状 在微型飞行器的研究方面,目前欧美等发达国家远远走在全球的前列,且成功研究了固定翼式、旋翼式和扑翼式各类微型飞行器。相比较之下，我国研究所和各大高校的研究人员虽然也开始展开了相关研究,但更多停留在理论设计阶段，实践的成功案例较为少见。产品说明书 2.1设计方案 现有的四轴飞行器存在着操作困难、安全保障低、价格昂贵等原因，受人们的喜爱程度较低，而且最主要原因是不能解决人员的安全问题。但是，性能好的四轴飞行器的价格比较昂贵，这种因素的限制之下，航模的市场很难满足飞行操作者的欲望要求。本产品恰恰从这个角度出发，为航模爱好者量身打造具有价格低廉，操作简单，维护方便，人身伤害比较小，有较为广泛的应用的四轴碟形飞行器。 为了实现安全飞行的目的我们小组起初设想了以下几种方案： 方案一：机架“十字形”方案 方案二：碟形外壳“十字镶嵌”方案 方案三：模具制作外壳、将电机、桨、电路板嵌在壳体内方案 通过反复的实验，我们小组决定采用第三种方案。 首先，与方案一相比较，方案三在外壳制作及组合安装上更容易，可以完成批量生产。从具体实施以及可行性角度分析，方案三更加具体完整。由于螺旋桨、电池、电路板等都是内镶的，根据空气动力学可知，碟形外壳更加有利于飞行稳定性，大大提高安全系数。 其次，与方案二相比较，方案三壳体较轻，负载比较小，容易固定，协调性也有了很大的改善。最后，与前两个方案相比较，方案三飞行效果及安全性更好，动作灵敏度更高，飞行动力更加持久，抗风抗干扰性能力更强，能够有效的完成各种动作。 综上所述，我们改进方案 一、方案 二、决定采用方案三为最终的作品方案。理论设计计算 3.1作品内部重要零部件介绍 四轴碟形飞行器中各参数说明表 标准配置 总体 螺旋桨 电子调速器 电机 遥控器 摄像头 四轴飞行器 描述 700*700 正反2对90*50桨；尺寸在200mm至300mm之间；总体载重量在600g至1000g之间 好赢2212 10A 新西达2212 KV980 天地飞6通道2.4GHz 300万像素 总体质量在1500g至2500g之间 表1 图1电机 图2螺旋桨 图3 电池 图5 飞行控制主板 图7 接收设备 图4 电调 图6 电压测试器 图8 拍摄设备 动力源提供装置： 采用直流电源3S锂电池，该电池的容量大，有充放电的保护，电池的质量轻，其质量为191.9克。电子调速器： 由于我们需要四轴碟形飞行器在空中完成各种动作，所以需要一个调速的装置进行调速，当操作者想要改变方向时，电子调速器会将从接收器得来的信号进行处理将直流电源转换成交流带信号，这样电机就会接收到信号，自动将速度进行改变，进而改变飞行器的姿态及位置。 拍摄及接收装置： 拍摄的摄像头采用可以发射出模拟信号，将拍摄到的流畅彩色视频及声音进行无线传输，调制模式采用AM调制，有效传输距离120m，工作时间可达2小时。其质量为32克。 接收装置采用支持带UHF波段模拟电视接收功能的多种电子设备终端,包括手机,电视,笔记本电脑,MP4,数码相框等电子设备，我们的作品采用的是手机。 遥控器及接收器 遥控器采用天地飞六通道2.4GHz，有效传输范围在200m左右。接收器是天地飞遥控器配套的接收器，其质量为8克。 工作流程图 飞行流程图： 飞行前的准备→遥控器及飞行器解锁→起飞→操作遥控器慢慢退下油门使飞行器慢慢减速降落 左右转弯流程图： 起飞→向左（右）旋转→遥控器方向舵向左（右）打→飞行器实现向左（右）飞行 前后流程图： 起飞→向前（后）飞行→遥控器方向舵向前（后）打→飞行器实现向前（后）飞行 其他的动作（翻滚、俯仰、横滚）参考以上的流程图即可实现。创新点及应用 1）操作和控制简便。集合了原有四轴飞行器的优点，碟形的设计更加美观，在平衡方面更加有优势。可以进行危险作业的同时还具有很强的趣味性，自主研发和独到的设计理念给航模爱好者耳目一新的感觉，与此同时在娱乐方面也增长了知识。 2）安全指数高。相对简单的机械构造，使安全指数大幅度提高，无论是作为航空模型还是作为遥控平台，安全永远是第一位的。 3）成本低廉。飞行器外壳采用的是模具成型技术，为工业开发其商业用途奠定了必要的基础。4）适用于进行危险性系数比较大的作业。在发生震后塌方和泥石流等自然灾害的时候，监控室的监控人员可以实现对飞行器无线的遥控，当飞行器飞行到灾后现场上空时，通过所携带的摄像头将图像资料无线传输给监控室,给现场决策提供帮助，减少伤害的发生。 5）适用于航测，军事应用等。足够的稳定性和抗风性，可以满足很多要求悬停能力的测量及军事应用。 目前在国内，四轴碟形飞行器的发展尚未成熟，很多不足都需要改进，因此我们独到的设计可以引领四轴碟形飞行器进入一个创新领域。 参考文献 [1] JOEL M.,MATTHEW T.Development of the Black Widow Micro Air Vehicle[R].AIAA-01-0127,2001.[2] WILSON J.R.MicroSAR Meets MAV[J].Aerospace American,1999,10(2):32-35.[3] PORNSIN-SISIRAK T.N.,LEE S.W.,NASSEF H.,GRASMEYER J.,etc.MEMS Wing Technology for A Battery-Powered Ornithopter.Micro Electro Mechanical Systems,2000,MEMS 2000,IEEE[C].2000,1:799-804.[4]刘晓杰.基于视觉的微小型四旋翼飞行器位姿估计研究与实现[D].长春:吉林大学,2009.[5]范承志,王字峰,林小娥等.一种位置无传感器无刷直流电动机驱动电路[J].微电机,2001(3):19-20,24.[6]曹杰,史金飞,戴敏.基于MEGA8单片机的无传感器无刷直流电动机控制系统设计[J].自动化仪表,2005,26(12):13-16.[7]刘焕晔.小型四旋翼飞行器飞行控制系统研究与设计[D].上海:上海交通大学,2009. 关于四旋翼飞行器的心得 对于飞行器或者航模之类的映像，是在高中时期，学校有航模小组，经常可以看到拿着航模的学生在进行试飞，当时心中感觉“航模”是非常有意思并且“高科技”。如今已经历高考进入大学，在学校的为我们安排的导师制计划中，非常幸运的加入无人机航拍飞行器小组，关于四旋翼飞行器，在查阅了相关资料后，有了一定的了解。 四旋翼飞行器也称为四旋翼直升机，是一种有4个螺旋桨且螺旋桨呈十字形交叉的飞行器。Seraphi 是一款可用于空中拍摄的一体化多旋翼飞行器，它外观时尚精美，做工精湛，还拥有集成了自身研发的飞行动力系统，并配置专业的无线电遥控系统。Seraphi集成易作、易维护的稳定设计，在出厂前已经设置并调试所有的飞行参数及功能，具有免安装、免调试的快速飞行模式。Seraphi 携带方便，可以搭配GoPro或者其它微型相机录制空中视频。 记得在TED的讲座中，有一期叫做“TED-红遍全球的的炫酷飞行器”，这个讲座说明了四旋翼飞行器的一些特点。1.时尚精美、做工精湛。Seraphi外观时尚精美，做工精湛，还拥集成了自身研发的飞行动力系统，并配置专业的无线电遥控系统。2.集成易作、易维护的稳定设计。Seraphi集成易作、易维护的稳定设计。Seraphi 携带方便，可以搭配GoPro或者其它微型相机录制空中视频。3.自由切换多种飞行模式。Seraphi内置自身研发的飞行控制系统，具备多种飞行模式，可以根据不同的飞行需要以及不同的飞行环境进行实时的智能切换以达到不一样的飞行体验。4.方向控制灵活。Seraphi具备自身研发飞控系统，方向控制灵活。在通常飞行过程中，可以根据需要，进行灵活纵。 制作航拍飞行器能够让培养我们的团队合作意识，拓宽我们的知识领域，同时让我们动手实践的能力得到提升，相信这次经历肯定能成为我的大学生活中最值得回忆的事情之一。 动态系统建模仿真 实验报告（2） 四旋翼飞行器仿真 2012 1实验内容 基于Simulink建立四旋翼飞行器的悬停控制回路，实现飞行器的悬停控制； 建立UI界面，能够输入参数并绘制运动轨迹； 基于VR Toolbox建立3D动画场景，能够模拟飞行器的运动轨迹。 2实验目的通过在Matlab 环境中对四旋翼飞行器进行系统建模，使掌握以下内容： 四旋翼飞行器的建模和控制方法 在Matlab下快速建立虚拟可视化环境的方法。 3实验器材 硬件：PC机。 工具软件：操作系统：Windows系列；软件工具：MATLAB及simulink。 4实验原理 4.1四旋翼飞行器 四旋翼飞行器通过四个螺旋桨产生的升力实现飞行，原理与直升机类似。 四个旋翼位于一个几何对称的十字支架前，后，左，右四端，如图 所示。旋翼由电机控制；整个飞行器依靠改变每个电机的转速来实现飞行姿态控制。 图1四旋翼飞行器旋转方向示意图 在图 中，前端旋翼 和后端旋翼 逆时针旋转，而左端旋翼 和右端的旋翼 顺时针旋转，以平衡旋翼旋转所产生的反扭转矩。 由此可知，悬停时，四只旋翼的转速应该相等，以相互抵消反扭力矩；同时等量地增大或减小四只旋翼的转速，会引起上升或下降运动；增大某一只旋翼的转速，同时等量地减小同组另一只旋翼的转速，则产生俯仰、横滚运动；增大某一组旋翼的转速，同时等量减小另一组旋翼的转速，将产生偏航运动。 4.2建模分析 四旋翼飞行器受力分析,如图 所示 图2四旋翼飞行器受力分析示意图 旋翼机体所受外力和力矩为： 重力mg,机体受到重力沿方向； 四个旋翼旋转所产生的升力 (i= 1,2,3,4)，旋翼升力沿方向； 旋翼旋转会产生扭转力矩 (i= 1,2,3,4)。垂直于叶片的旋翼平面，与旋转矢量相反。 力模型为：，旋翼通过螺旋桨产生升力。是电机转动力系数，可取，为电机转速。旋翼旋转产生旋转力矩Mi(i=1,2,3,4)，力矩Mi的旋向依据右手定则确定。力矩模型为，其中是电机转动力系数，可取为电机转速。当给定期望转速后，电机的实际转速需要经过一段时间才能达到。实际转速与期望转速之间的关系为一阶延迟：响应延迟时间可取0.05s(即)。期望转速则需要限制在电机的最小转速和最大转速之间，范围可分取[1200rpm，7800rpm]。 飞行器受到外界力和力矩的作用，形成线运动和角运动。线运动由合外力引起，符合牛顿第二定律： r为飞机的位置矢量。 角运动由合力矩引起。四旋翼飞行器所受力矩来源于两个方面：1）旋翼升力作用于质心产生的力矩；2）旋翼旋转产生的扭转力矩。角运动方程如下式所示。其中，L 为旋翼中心建立飞行器质心的距离，I 为惯量矩阵。 4.3控制回路设计 控制回路包括内外两层。外回路由Position Control 模块实现。输入为位置误差，输出为期望的滚转、俯仰和偏航角。内回路由Attitude Control 模块实现，输入为期望姿态角，输出为期望转速。Motor Dynamics 模块模拟电机特性，输入为期望转速，输出为力和力矩。Rigid Body Dynamics 是被控对象，模拟四旋翼飞行器的运动特性。 图3包含内外两个控制回路的控制结构 （1）内回路：姿态控制回路 对四旋翼飞行器，我们唯一可用的控制手段就是四个旋翼的转速。因此，这里首先对转速产生的作用进行分析。假设我们希望旋翼1的转速达到，那么它的效果可分解成以下几个分量： ：使飞行器保持悬停的转速分量； ：除悬停所需之外，产生沿ZB轴的净力； ：使飞行器负向偏转的转速分量； ：使飞行器正向偏航的转速分量； 因此，可以将期望转速写成几个分量的线性组合： 其它几个旋翼也可进行类似分析，最终得到： 在悬浮状态下，四个旋翼共同的升力应抵消重力，因此： 此时，可以把旋翼角速度分成几个部分分别控制，通过“比例-微分”控制律建立如下公式： 综合以上三式可得到期望姿态角-期望转速之间的关系，即内回路。 外回路：位置控制回路 外回路采用以下控制方式：通过位置偏差计算控制信号（加速度）；建立控制信号与姿态角之间的几何关系；得到期望姿态角，作为内回路的输入。期望位置记为。可通过PID 控制器计算控制信号： 是目标悬停位置是我们的目标悬停位置(i=1,2,3)，是期望加速度，即控制信号。注意：悬停状态下线速度和加速度均为0，即。 通过俯仰角和滚转角控制飞行器在XW和YW平面上的运动，通过控制偏航角，通过控制飞行器在ZB轴上的运动。可得： 根据上式可按照以下原则进行线性化： （1）将俯仰角、滚转角的变化作为小扰动分量，有，，（2）偏航角不变，有，其中初始偏航角，为期望偏航角（3）在悬停的稳态附近，有 根据以上原则线性化后，可得到控制信号（期望加速度）与期望姿态角之间的关系： 则内回路的输入为： 5实验步骤与结果 （1） 根据控制回路的结构建立simulink模型； （2） 为了便于对控制回路进行参数调整，利用Matlab软件为四旋翼飞行器创建GUI参数界面； （3） 利用Matlab的VR Toolbox建立四旋翼飞行器的动画场景 （4） 根据系统的结构框图，搭建Simulink模块以实现模拟飞行器在指定位置的悬停。使用默认数据，此时xdes=3，ydes=4，zdes=5，开始仿真，可以得到运动轨迹x、y、z的响应函数，同时可以得到在xyz坐标中的空间运动轨迹。然后点击GUI中的VR按钮使simulink的工作空间中载入系统仿真所需的参数，把x、y、z的运动轨迹和Roll，Pitch，Yaw输入至VR中的模拟飞行器中，观察飞行器的运动轨迹和运动姿态，然后再使用一组新的参数xdes=-8，ydes=3，zdes=6进行四旋翼飞行器运动进行仿真模拟，可以看出仿真结果和动画场景相吻合。 6实验总结与心得 此次MATLAB实验综合了SIMULINK、GUI和VR场景等多个部分，对四旋翼飞行器运动进行了仿真模拟。由仿真结果可以看出，四旋翼飞行器最终位置达到了期望给定的位置，三个方向的响应曲线最终平稳，对应飞行器悬停在期望位置，达到了控制要求。 本次试验收获很多，学习到了很多知识，首先是熟悉了SIMULINK由简至繁搭建系统的过程，学习了利用VR建立虚拟模型，并在SIMULINK中连接。其次是熟悉了MATLAB GUI界面的编写和搭建过程。Matlab提供了强大的用户图形界面，以帮助用户不必编写底层程序而直接在软件包基础上进行自行开发，这点在诸多软件中都有所体现。另外通过实验，对四旋翼飞行器的受力分析、模型建立、控制回路设计等有了较为细致的了解。 动态系统建模（四旋翼飞行器仿真） 实验报告 院（系）名称 大飞机班 学号 学生姓名 任课教师 2011年 X月 四旋翼飞行器的建模与仿真 一、实验原理 I．四旋翼飞行器简介 四旋翼飞行器通过四个螺旋桨产生的升力实现飞行，原理与直升机类似。四个旋翼位于一个几何对称的十字支架前、后、左、右四端，如图1-1所示。旋翼由电机控制；整个飞行器依靠改变每个电机的转速来实现飞行姿态控制。 在图1-1中，前端旋翼1 和后端旋翼3 逆时针旋转，而左端旋翼2 和右端的旋翼4 顺时针旋转，以平衡旋翼旋转所产生的反扭转矩。由此可知，悬停时，四只旋翼的转速应该相等，以相互抵消反扭力矩；同时等量地增大或减小四只旋翼的转速，会引起上升或下降运动；增大某一只旋翼的转速，同时等量地减小同组另一只旋翼的转速，则产生俯仰、横滚运动；增大某一组旋翼的转速，同时等量减小另一组旋翼的转速，将产生偏航运动。 图1-1 四旋翼飞行器旋翼旋转方向示意图 从动力学角度分析，四旋翼飞行器系统本身是不稳定的，因此，使系统稳定的控制算法的设计显得尤为关键。由于四旋翼飞行器为六自由度的系统（三个角位移量，三个线位移量），而其控制量只有四个（4 个旋翼的转速），这就意味着被控量之间存在耦合关系。因此，控制算法应能够对这种欠驱动（under-actuated）系统足够有效，用四个控制量对三个角位移量和三个线位移量进行稳态控制。本实验针对四旋翼飞行器的悬浮飞行状态进行建模。 II．飞行器受力分析及运动模型 （1）整体分析 如图1-2所示，四旋翼飞行器所受外力和力矩为： Ø 重力mg，机体受到重力沿-Zw方向 Ø 四个旋翼旋转所产生的升力Fi(i=1,2,3,4)，旋翼升力沿ZB方向 Ø 旋翼旋转会产生扭转力矩Mi (i=1,2,3,4)，Mi垂直于叶片的旋翼平面，与旋转矢量相反。 图1-2 四旋翼飞行器受力分析 （2）电机模型 Ø 力模型 （1.1） 旋翼通过螺旋桨产生升力。是电机转动力系数，可取，为电机转速。 Ø 力矩模型 旋翼旋转产生旋转力矩Mi(i=1,2,3,4)，力矩Mi的旋向依据右手定则确定。 （1.2） 是电机转动力系数，可取为电机转速。 Ø 转速模型 当给定期望转速后，电机的实际转速需要经过一段时间才能达到。实际转速与期望转速之间的关系为一阶延迟： （1.3） 响应延迟时间可取0.05s(即)。期望转速则需要限制在电机的最小转速和最大转速之间，范围可分取[1200rpm，7800rpm]。 （3）运动方程 飞行器受到外界力和力矩的作用，形成线运动和角运动。线运动由合外力引起，符合牛顿第二定律，如公式(1.4)所示： （1.4） r为飞机的位置矢量。注意：公式（1.4）是在地平面坐标系中进行描述的。 角运动由合力矩引起。四旋翼飞行器所受力矩来源于两个方面：1）旋翼升力作用于质心产生的力矩；2）旋翼旋转产生的扭转力矩。角运动方程如公式（1.5）所示。其中，L 为旋翼中心建立飞行器质心的距离，I 为惯量矩阵。 （1.5） III．控制回路设计 控制回路包括内外两层。外回路由Position Control 模块实现。输入为位置误差，输出为期望的滚转、俯仰和偏航角。内回路由Attitude Control 模块实现，输入为期望姿态角，输出为期望转速。Motor Dynamics 模块模拟电机特性，输入为期望转速，输出为力和力矩。Rigid Body Dynamics 是被控对象，模拟四旋翼飞行器的运动特性。如图1-3 图1-3 包含内外两个控制回路的控制结构 （1）内回路：姿态控制回路 对四旋翼飞行器，我们唯一可用的控制手段就是四个旋翼的转速。因此，这里首先对转速产生的作用进行分析。假设我们希望旋翼1的转速达到，那么它的效果可分解成以下几个分量： ：使飞行器保持悬停的转速分量； ：除悬停所需之外，产生沿ZB轴的净力； ：使飞行器负向偏转的转速分量； ：使飞行器正向偏航的转速分量； 因此，可以将期望转速写成几个分量的线性组合： （1.6） 其它几个旋翼也可进行类似分析，最终得到： （1.7） 在悬浮状态下，四个旋翼共同的升力应抵消重力，因此： （1.8） 此时，可以把旋翼角速度分成几个部分分别控制，通过“比例-微分”控制律建立如下公式： （1.9） 综合式（1.7）、（1.8）、（1.9）可得到期望姿态角-期望转速之间的关系，即内回路。 （2）外回路：位置控制回路 外回路采用以下控制方式： Ø 通过位置偏差计算控制信号（加速度）； Ø 建立控制信号与姿态角之间的几何关系； Ø 得到期望姿态角，作为内回路的输入。 期望位置记为。可通过PID 控制器计算控制信号： （1.10） 是目标悬停位置是我们的目标悬停位置(i=1,2,3)，是期望加速度，即控制信号。注意：悬停状态下线速度和加速度均为0，即。 通过俯仰角和滚转角控制飞行器在XW和YW平面上的运动，通过控制偏航角，通过控制飞行器在ZB轴上的运动。对（1.4）进行展开，可得到： （1.11） 根据上式可按照以下原则进行线性化： （1）将俯仰角、滚转角的变化作为小扰动分量，有，，（2）偏航角不变，有，其中初始偏航角，为期望偏航角（3）在悬停的稳态附近，有 根据以上原则线性化后，可得到控制信号（期望加速度）与期望姿态角之间的关系： （1.12） 根据式(1.10)已经通过PID 控制器得到了作为控制信号的期望加速度，因此，将（1.12）式反转，由期望加速度计算期望姿态角，作为内回路的输入： （1.13） 二、实验步骤 I．搭建Simulink仿真控制回路 根据实验原理中运动方程及控制回路设计，搭建Simulink控制回路，如图2-1所示。主要分为五个部分：Position Control（由期望的位置误差通过控制律设计计算出期望的姿态角），Attitude Control（由姿态角信息和各轴角速度信息通过控制律计算出给电机的控制信号），Motor Dynamics（通过给电机的控制信号由电机模型计算出每个电机的输出力和力矩），Rigid Body Dynamics为四旋翼飞行器的仿真模型，由产生的力和力矩计算出仿真模型的姿态和位置信息，VR Sink为四旋翼飞行器的虚拟显示模型。 图2-1 仿真Simulink模型 下面给出每个子系统的仿真结构图及控制律设计部分。 图2-2 Position Control子系统 图2-3 位置PID控制器结构 图2-4 Attitude Control子系统 图2-5 姿态角和三轴角速度之间的转换关系 图2-6 Motor Dynamics子系统输出力及力矩模型 图2-7 Rigid Body Dynamics子系统 II．利用V-Realm Builder建立四旋翼飞行器的虚拟模型 利用V-Realm Builder建立四旋翼飞行器的大致虚拟模型，并建立四个父类分别为Simulink输入提供质心位移信息和机体姿态信息，如图2-8所示。 图2-8 四旋翼飞行器虚拟模型 III．利用MATLAB GUI建立四旋翼飞行器仿真的控制界面 利用MATLAB GUI建立仿真控制界面，所建立的控制界面如图2-9所示。 图2-9 MATLAB GUI仿真控制界面 界面主要分为四个部分，Struct Parameters Panel设置飞行器的结构参数和外部变量，Desired Position Panel设置期望控制飞行器所到达的位置，Control Parameters Panel设置PID控制律所需的增益参数和仿真时间，Plot Panel显示仿真结果图形并对图形效果进行简单的控制。 三、仿真结果 运行GUI，输入所需参数或者采用默认参数，点击load data按钮分别将三组参数载入，点击Start按钮，仿真开始运行。跳出VR显示，并在仿真结束后绘制飞行器三方向的坐标信息曲线和飞行器位置曲线。VR显示过程中某一时刻如图3-1所示，仿真结束后控制界面显示的曲线如图3-2所示。期望达到的目标点设置为（10,15,20）。 图3-1 VR显示四旋翼飞行器运动状态 图3-2 四旋翼飞行器控制平台 四、总结与体会 由仿真结果可以看出，四旋翼飞行器最终位置达到了期望给定的位置，三个方向的响应曲线最终平稳，对应飞行器悬停在期望位置，达到了控制要求。本次试验收获很多，学习到了很多知识，熟悉了SIMULINK由简至繁搭建系统的过程，学习了利用V-Realm Builder建立虚拟模型，并在SIMULINK中连接，也熟悉了MATLAB GUI界面的编写和搭建过程。第二篇：四旋翼产品说明书最终稿（范文）
第三篇：关于四旋翼飞行器的心得
第四篇：动态系统建模仿真实验报告四旋翼仿真
第五篇：动态系统建模（四旋翼飞行器仿真）实验报告
点击咨询 