四轴飞行器报告(高级篇).pdf

1、四轴飞行器报告 (高 级 篇 ) 姓 名 : 阿力木江 艾合买提江 高瞻 完成日期 : 2014 年 12 月 29 日 星期 一 报告内容 1. 姿态 解算 用到的 常用 数学方法和处理手段 2. 自动 控制 原理 PID 和 系统 建模 姿态 解算 用到的 常用 数学方法和处理手段 姿态的数学表示方法 姿态有多种数学表示方式，常见的是四元数，欧拉角，矩阵和轴角。他们各自有其自身的优点，在不同的领域使用不同的表示方式。在四轴飞行器中使用到 了 四元数 和 欧拉角 。 四元数 四元数是由爱尔兰数学家威廉 卢云 哈密顿在 1843年发现的数学概念。从明确地角度而言，四元数是复数的不可交换延伸。如

2、把四元数的集合考虑成多维实数空间的话，四元数就代表着一个四维空间，相对于复数为二维空间。 四元数大量用于电脑绘图（及相关的图像分析）上表示三维物件的旋转及方位。四元数亦见于控制论、信号处理、姿态控制、物理和轨道力学，都是用来表示旋转和方位。 相对于另几种旋转表示法（矩阵，欧拉角，轴角），四元数具有某些方面的优势，如速度更快、提供平滑插值、有效避免万向锁问题、存储空间较小等等。 以上部分摘 自 维基百科 -四元数 。 欧拉角 莱昂哈德 欧拉用欧拉角来描述刚体在三维欧几里得空间的取向。对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的。

3、而坐标系则固定于刚体，随着刚体的旋转而旋转。 以上部分摘 自 维基百科 -欧拉角 。下面我们通过图例来看看欧拉角是如何产生的，并且分别对应哪个角度。 姿态解算为什么要用四元数和欧拉角 姿态解算的核心在于旋转，一般旋转有 4种表示方式：矩阵表示、欧拉角表示、轴角表示和四元数表示。矩阵表示适合变换向量，欧拉 角最直观，轴角表示则适合几何推导，而在组合旋转方面，四元数表示最佳。因为姿态解算需要频繁组合旋转和用旋转变换向量，所以采用四元数保存组合姿态、辅以矩阵来变换向量的方案。 总结来说，在 飞行器 中，姿态解算中使用四元数来保存飞行器的姿态，包括旋转和方位。在获得四元数之后，会将其转化为欧拉角，然后

4、输入到姿态控制算法中。 姿态控制算法的输入参数必须要是欧拉角。 AD值是指 MPU6050 的陀螺仪和加速度值， 3个维度的陀螺仪值和 3个维度的加速度值，每个值为 16 位精度。 AD值必须先转化为四元数，然后通过四元数转化 为欧拉角。这个四元数可能是软解，主控芯片（ STM32）读取到 AD值，用软件从 AD值算得，也可能是通过 MPU6050中的 DMP硬解，主控芯片（ STM32）直接读取到四元数。具体参考 MPU60x0的四元数生成方式介绍。 下面就是四元数软解过程，可以由下面这个框图表示： 扩展阅读 -四元数的运算 下面介绍一下四元数，然后给出几种旋转表示的转换。这些运算在 飞行器

5、 的代码中都会遇到。 四元数可以理解为一个实数和一个向量的组合，也可以理解为四维的向量。这里用一个圈表示 q是一个四元数（很可能不是规范的表示方式）。 四元数的长度（模）与普通向量相似。 下面是对 四元数的单位化，单位化的四元数可以表示一个旋转。 四元数相乘，旋转的组合就靠它了。 旋转的 “轴角表示 ”转 “四元数表示 ”。这里创造一个运算 q(w,)，用于把绕单位向量 w 转 角的旋转表示为四元数。 通过 q(w,)，引伸出一个更方便的运算 q(f,t)。有时需要把向量 f的方向转到向量 t的方向，这个运算就是生成表示对应旋转的四元数的（后面会用到）。 然后是 “四元数表示 ”转 “矩阵表示

6、 ”。再次创造运算，用 R(q)表示四元数 q对应的矩阵（后面用到）。 多个旋转的组合可以用四元数的乘法来实现。 “四元数表示 ”转 “欧拉角表示 ”。用于显示。 软件姿态解算 使用 MPU6050硬件 DMP解算姿态是非常简单的，下面介绍由三轴陀螺仪和加速度计的值来使用四元数软件解算姿态的方法。 我们先来看看如何用欧拉角描述一次平面旋转 (坐标变换 )： 设坐标系绕旋转 角后得到坐标系 ,在空间中有一个矢量在坐标系中的投影为 ,在内的投影为由于旋转绕进行，所以 Z坐标 未变，即有。 转换成矩 阵形式表示为： 整理一下： 所以从旋转到可以写成 上面仅仅是绕一根轴的旋转，如果三维空间中的欧拉角旋

7、转要转三次 : 上面得到了一个表示旋转的方向余弦矩阵。 不过要想用欧拉角解算姿态，其实我们套用欧拉角微分方程就行了： 上式中左侧 ,是本次更新后的欧拉角 ,对应 row,pit,yaw。右侧，是上个周期测算出来的角度，三个角速度由直接安装在四轴飞行器的三轴陀螺仪在这个周期转动的角度，单位为弧度，计算间隔时 T陀螺角速度，比如 0.02秒 0.01弧度 /秒 =0.0002弧度。间因此求解这个微分方程就能解算出当前的欧拉角。 前面介绍了什么是欧拉角，而且欧拉角微分方程解算姿态关系简单明了，概念直观容易理解，那么我们为什么不用欧拉角来表示旋转而要引入四元数呢？ 一方面是因为欧拉角微分方程中包含了大

8、量的三角运算，这给实时解算带来了一定的困难。而且当俯仰角为 90 度时方程式会出现神奇的 “GimbalLock”。所以欧拉角方法只适用于水平姿态变化不大的情况，而不适用于全姿态飞行器的姿态确定。 四元数法只求解四个未知量的线性微分方程组，计算量小，易于操作，是比较实用的工程方法。 我们知道在平面 (x,y)中的旋转可以用复数来表示，同样的三维中 的旋转可以用单位四元数来描述。我们来定义一个四元数： 我们可以把它写成 ,其中 ,。那么是矢量，表示三维空间中的旋转轴。 w 是标量，表示旋转角度。那么就是绕轴旋转 w度，所以一个四元数可以表示一个完整的旋转。只有单位四元数才可以表示旋转，至于为什么

9、，因为这就是四元数表示旋转的约束条件。 而刚才用欧拉角描述的方向余弦矩阵用四元数描述则 为： 所以在软件解算中，我们要首先把加速度计采集到的值 (三维向量 )转化为单位向量 ,即向量除以模，传入参数是陀螺仪x,y,z 值和加速度计 x,y,z值： void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) float norm; float vx, vy, vz; float ex, ey, ez; norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; a

10、y = ay / norm; az = az / norm; 下面把四元数换算成方向余弦中的第三行的三个元素。刚好 vx,vy,vz 其实就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量。 / estimated direction of gravity vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; axyz 是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量。 axyz 是测量得到的重力向量， vxyz是陀螺积 分后的姿态来推算出

11、的重力向量，它们都是机体坐标参照系上的重力向量。 那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。 向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示， exyz就是两个重力向量的叉积。 这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。 / integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt +

12、 ey*Ki; ezInt = ezInt + ez*Ki; 用叉积误差来做 PI 修正陀螺零偏 / integral error scaled integral gain exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; / adjusted gyroscope measurements gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; 四元数微分方程，其中 T为测量周期，为陀螺仪角速度，以下都是已知量，这

13、里使用了一阶龙哥库塔求解四元数微分方程： / integrate quaternion rate and normalise q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; 最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角： 所以有： Q_ANGLE.Yaw = atan2(2 * q1 * q2 +

14、 2 * q0 * q3, -2 * q2*q2 - 2 * 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 * q1 - 2 * q2* q2 + 1)* 57.3; / roll 硬件姿态解算 四轴的姿态解算无疑是最繁琐的步骤没有之一，但是自从 MPU6050 出现了硬件 DMP的时候，大妈都能完成姿态解算了！ 飞行器 使用了 MPU6050自带的硬件四元数单元，可以

15、通过 IIC 直接读取四元数，省却了软件解算繁琐的算法步骤，非常方便易用。 这里还是要首先介绍下四元数，四元数要说的实在太多，因为它的优点很多，利用起来很方便，但是理解起来就有点蹩脚了。我们百度四元数，一开始看到的就是四元数来历，还有就是四元数的基本计算。对于来历，还是想说一下，四元数（ Quaternions）是由威廉 卢云 哈密尔顿 (William Row anHamilton,1805-1865）在 1843 年爱尔兰发现的数学概念。 将实数域扩充 到复数域，并用复数来表示平面向量，用复数的加、乘运算表示平面向量的合成、伸缩和旋，这就是我们熟知的复数的二维空间含义，所以人们会继续猜想，

16、利用三维复数不就可以表达三维空间的变换了吗，历史上有很多数学家试图寻找过三维的复数，但后来证明这样的三维复数是不存在的。有关这个结论的证明，我没有查到更明确的版本，据古今数学思想中的一个理由，三维空间中的伸缩旋转变换需要四个变量来决定：两个变量决定轴的方向，一个变量决定旋转角度，一个变量决定伸缩比例。这样，只有三个变量的三维复数无法满足这样的要求。但是历史上得到的应该 是比这个更强的结论，即使不考虑空间旋转，只从代数角度来说，三维的复数域作为普通复数域的扩张域是不存在的。并且，据古今数学思想叙述，即使像哈密尔顿后来引入四元数那样，牺牲乘法交换律，这样的三维复数也得不到。经过一些年的努力之后，

17、Hamilton 发现自己被迫应作两个让步，第一个是他的新数包含四个分量，而第二个是他必须牺牲乘法交换律。（ 古今数学思想第三册 177 页）但是四元数用作旋转的作用明显，简化了运算，而且避免了 Gimbal Lock，四元数是最简单的超复数，我们不能把四元数简单的理解为 3D 空 间的矢量，它是 4 维空间中的的矢量，也是非常不容易想像的。 我们知道在平面 (x,y)中的旋转可以用复数来表示，同样的三维中的旋转可以用单位四元数来描述。我们来定义一个四元数： 我们可以把它写成 ，其中 ， 。那么 V是矢量，表示三维空间中的旋转轴。 w 是标量，表示旋转角度。那么 就是绕轴 V旋转 w 度，所以

18、一个四元数可以表示一个完整的旋转。只有单位四元数才可以表示旋转，至于为什么，因为这就是四元数表示旋转的约束条件。 所以大家可以理解为，单位四元数能够表示旋转。我们给出一组单位四元数和欧拉角的转换关系，通 过这个关系来将采集到的四元数转化成欧拉角，原理将在软件解算中给出，这里直接使用就可以了： 所以在四轴飞行器中，传感器读取到四元数，首先应先将它归一化成单位四元数： norm = dmpinvSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 * norm; q1 = q1 * norm; q2 = q2 * norm; q3 = q3 * norm; 归一

19、化后根据四元数和欧拉角转换公式把四元数转化为欧拉角， OK,硬件姿态解算完成！ DMP_DATA.dmp_roll = (atan2(2.0*(q0*q1 + q2*q3), 1 - 2.0*(q1*q1 + q2*q2)* 180/M_PI; / we let safe_asin() handle the singularities near 90/-90 in pitch DMP_DATA.dmp_pitch = dmpsafe_asin(2.0*(q0*q2 - q3*q1)* 180/M_PI; DMP_DATA.dmp_yaw = -atan2(2.0*(q0*q3 + q1*q2)

20、, 1 - 2.0*(q2*q2 + q3*q3)* 180/M_PI; 自动 控制 原理 PID 和 系统 建模 PID 控制算法 四轴如何起飞的原理 四轴飞行器的螺旋桨与空气发生相对运动，产生了向上的升力，当升力大于四轴的重力时四轴就可以起飞了。 四轴飞行器飞行过程中如何保持水平： 由于四个电机转向相同，四轴会发生旋转。我们控制四轴电机 1和电机 3同向，电机 2电机 4反向，刚好抵消反扭矩，巧妙的实现了平衡： 实际上由于电机和螺旋桨本身制造的差异我们无法做到四个电机转速完全相同，如果我们控制同样的转速很有可能飞 行器起飞之后就侧翻了。 由于电机的不平衡，在人眼的观察下发现飞机向右侧翻，我

21、们控制右侧电机 1电机 2提高转速增加升力，飞机归于平衡。由于 飞机是一个动态系统，在接下来我们会一直重复： 观察 -大脑计算 -控制 -观察 -大脑计算 -控制 这个过程。 但事实上这是不可能的，因为人无法长时间精确的同时控制四个电机。我们需要一个自动反馈系统替代人操作来完成飞机的自稳定，我们人只需要控制飞机的方向和高度就可以了。这个系统中反馈由姿态传感器替代眼睛，而大脑则由单片机来替代。这时候该 运用 PID 控制系 统 什么是 PID? PID控制器由偏差的比例（ P） 、积分（ I）和微分（ D）来对被控对象进行控制，是应用最为广泛的一种自动控制器。 比例（ P）控制 比例控制是一种最

22、简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（ Steady-state error）。 积分（ I）控制 在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的 或简称有差系统（ System w ith Steady-state Error）。为了消除稳态误差，在控制器中必须引入 “积分项 ”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积 分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零

23、。因此，比例 +积分 (PI)控制器，可以使系统在进入稳态后无稳 态误差。 积分 项输出： 微分（ D）控制 微分调节就是偏差值的变化率。使用微分环节能够实现系统的超前控制。如果输入偏差值线性变化，则在调节器输出侧叠加一个恒定的调节量。大部分控制系统不需要调节微分时间。因为只有时间滞后的系统才需要附加这个参数。如果画蛇添足加上这个参数反而会使系统的控制受到影响。微分项输出： 综上所述得到一个一条公式，这个就是模拟 PID： 而 PID中又可以只使用 PI 项构成比例 -积分控制器，使用 PD 项构成比例 -微分控制器。在 飞行器 四轴飞行器中我们使用了增量式 PD 控制，以 ROLL方向角度控

24、制为例： 测得 ROLL轴向偏差： 偏差 =目标期望角度 -传感器实测角度 DIF_ANGLE.X = EXP_ANGLE.X - Q_ANGLE.Roll; 比例项的计算： 比例项输出 = 比例系数 P * 偏差 Proportion = PID_Motor.P * DIF_ANGLE.X; 微分项计算： 由于陀螺仪测得的是 ROLL轴向旋转角速率，角速 率积分就是角度，那么角度微分即角速率，所以微分量刚好是陀螺仪测得的值。 微分输出 =微分系数 D*角速率 DifferentialCoefficient = PID_Motor.D * DMP_DATA.GYROx; 整合结果总输出为： R

25、OLL方向总控制量 =比例项输出 +微分量输出 ROLL 和 PIT轴向按照以上公式计算 PID输出，但 YAW轴比较特殊，因为偏航角法线方向刚好和地球重力平行，这个方向的角度无法由加速度计直接测得，需要增加一个电子罗盘来替代加速度计。如果不使用罗盘的话，我们可以单纯的通过角速度积分来测得偏航角，缺点是由于积分环节中存在积分漂移，偏航角随着时间的推移会偏差越来越大。我们不使用罗盘就没有比例项，只仅使用微分环节来控制。 YAW轴输出： 微分输出 =微分系数 D*角速率 YAW方向控制量 = PID_YAW.D * DMP_DATA.GYROz; 电机的输出： 油门控制 Throttle是电机输出

26、的基准值，增加油门即可提高四轴高度。最后整合ROLL/PIT/YAW三轴输出量进行电机控制： Motor2 = (int16_t)(Thr - Pitch - Roll - Yaw ); /M3 Motor0 = (int16_t)(Thr + Pitch + Roll - Yaw ); /M1 Motor3 = (int16_t)(Thr - Pitch + Roll + Yaw ); /M4 Motor1 = (int16_t)(Thr + Pitch - Roll + Yaw ); /M2 如图四轴绕 ROW轴向右下倾斜 5度，那么电机 1电机 2应该提高升力，电机 3电机 0减小升力恢

27、复平衡状态，所以有以下规则： Roll方向旋转，则电机 1电机 2同侧出力，电机 0电机 3反向出力 Pitch方向旋转，则电机 2电机 3同侧出力，电机 0电机 1反向出力 Yaw 方向旋转，则电机 1电机 3同侧出力，电机 0电机 2反向出力 参考 : 软件 架构 部分 :小马论坛 ,匿名 论坛 , 圆点 博士论坛 ,crazyfile 论坛 控制 PID部分 :中山大学 四轴俱乐部 ,crazyfile论坛 硬件 参考 :众多大神匿名提供的资料和 解决 方案 ,还有 许多热心帮助的 爱好者 . 参考书籍 : 实时 操作系统 CSD方法 UC/OS-III 基于 STM32 移植 由 入门到 精通 吃透 PID 高频 电子线路 设计 参考 文献： 基于非线性滤波的初始对准研究 卡尔曼滤波器介绍 捷联式惯性导航系统 四旋翼飞行器的动力学建模及 pid控制 四旋翼几种控制方法研究