第3章四旋翼飞行器的原理 3．1基本原理 四旋翼飞行器通过4个螺旋桨的相互配合，利用力的合成和分解原理，可以实现各种飞行动作，四旋翼飞行器通常被设计为两种模式，如图3．1中的“×”字模式和“十”字模式。 图3．1“×”字模式和“十”字模式 为了抵消旋翼在旋转过程中产生的反扭矩，例如１号位置的旋翼逆时针旋转时，空气会产生推动飞行器逆时针旋转的反扭矩，飞行器的相邻旋翼需要有不同的转向，即１、４号电机顺时针旋转而２、３号电机逆时针旋转，这样空气推动飞行器旋转的反扭矩刚好抵消，使得飞行器能够平稳飞行。由于飞行器的4个桨在旋转的过程中都要提供向上的升力，而相邻电机转向相反，因此在相邻电机上还要使用形状不同的螺旋桨，分别称为正桨和反桨，通常１、４号电机顺时针旋转搭载正桨，２、３号电机逆时针旋转搭载反桨。 3．2四旋翼飞行器的6个基本飞行动作 四旋翼飞行器可以沿着升降方向、前进后退方向和左右方向平移运动，以及沿着3个轴旋转运动，共6个基本飞行动作。 3．2．1升降运动 升降运动的实质就是四旋翼飞行器沿着Z轴的两个方向运动，如图3．2所示。只要4个电机转速同时增加，使飞行器的升力大于重力和阻力的和之后，就可以上升，反之，4个电机的转速同时下降，就可以使飞行器下降，当升力和重力达到平衡之后，飞行器就能实现空中悬停，在实际飞行中，悬停是一个动态稳定的过程，即使在悬停中，升力也是不断进行调整的。 图3．2升降运动 3．2．2俯仰运动 假定X轴是飞行器的正方向，俯仰运动就是绕Y轴的旋转运动； 而若飞行器在空中自由飞行，那么俯仰运动就伴随着前进和后退的运动； 若飞行器绕Y轴正方向逆时针旋转，飞行器就会向前运动，反之就会向后运动。保持1、3号电机的转速不变，增大0号电机的转速，减小2号电机的转速，飞行器就会绕Y轴顺时针旋转，这个动作称为仰； 反之，飞行器会绕Y轴逆时针旋转，称为俯。俯仰运动如图3．3所示。 图3．3俯仰运动 3．2．3横滚运动 横滚运动的原理和俯仰运动的原理相同，即保持0号和2号电机的转速不变，改变1、3号电机的转速，使飞行器绕X轴进行旋转，称为横滚运动，如图3．4所示。 图3．4横滚运动 3．2．4偏航(自旋)运动 偏航运动又被称为自旋运动，是飞行器绕Z轴旋转的运动，如图3．5所示。在一个螺旋桨旋转时，飞行器会受到一个反扭矩的影响，导致飞行器沿螺旋桨转向的相反方向旋转，而四旋翼飞行器采用了4个螺旋桨，其中2个顺时针旋转，另外2个逆时针旋转抵消了这种反扭矩。那么，为了让飞行器能够产生自旋运动，只要让由于4个电机旋转受到的反扭矩无法自相抵消即可。当0、2号电机的转速上升，1、3号电机的转速下降，飞行器就会沿Z轴顺时针旋转，反之，则会沿Z轴逆时针旋转。通常在进行偏航运动时，为了保证飞行器升力不变，其中两个电机下降的转速和另外两个电机上升的转速相同。 图3．5偏航(自旋)运动 3．3姿态解算 姿态解算是飞行器正常飞行的基础，只有飞行器能够正确掌握飞行姿态，飞行器才能够通过动态平衡调节保持飞行的稳定性。要进行姿态解算，需要对传感器获取的多种不同的数据进行滤波和融合，选择合适的方式描述飞行器的姿态，以及用一种方法让飞行器的姿态与姿态数据一一对应。 3．3．1姿态表示 欧拉角是一种最为常用的描述旋转的办法，它用三次旋转来表示一个物体的姿态，分别是章动角φ、旋进角θ以及自旋角ψ，三次旋转的顺序有多种选择，飞行器的姿态描述中，多用zyx的旋转顺序进行描述。 图3．6航空次序欧拉角 zyx的取法又被称为航空次序欧拉角，分为Roll(横滚角)、Pitch(俯仰角)和Yaw(偏航角)，如图3．6所示。坐标系首先绕Z轴旋转ψ，将ψ称为偏航角再绕Y轴旋转θ，将θ称为俯仰角； 最后绕X轴旋转φ，将φ称为横滚角。 为了保证旋转描述的一致性，俯仰角被限制在-90°~90°，其他两个角度则被限制在-180°~180°。 欧拉角描述法存在明显的不足。首先在俯仰角大小接近90°时会出现奇点，具体的现象有当俯仰角在90°时横滚角与俯仰角会发生突变，以及在奇点附近，可能出现某一自由度的角速度分量很大甚至无穷大的问题。另外，欧拉角仅可以用于描述一次旋转，当进行第二次旋转时，很有可能产生万向节死锁问题，具体表现为，考虑第一次旋转时，俯仰角旋转了90°，另外两个角度均为0°，如果在此基础上实现第二次旋转，绕最初的Z轴转动一个角度，这是无法实现的，因为第一次旋转后，横滚角与偏航角的转轴重合，即系统丢失了一个自由度，只剩下两个自由度。鉴于欧拉角姿态描述中这种无法回避的奇异现象的存在，采用四元素与欧拉角的相互转换构建姿态控制率比较适合工程应用。 3．3．2数据滤波 数据滤波是去除噪声、还原真实数据的一种数据处理技术。 1． 维纳滤波 维纳滤波是一种平稳随机过程的最佳滤波理论，换句话说就是在滤波过程中系统的状态参数(或信号的波形参数)是稳定不变的。维纳滤波仅在理论上有意义，在实际应用中有局限性，其表现在： 不适用于非平稳随机过程的滤波； 要用到所有时刻的采样数据，需要的数据存储容量大； 求解维纳霍夫方程时要用到高阶数矩阵的求逆运算，计算量大，而且实际数据下的维纳霍夫方程可能无解。 2． 卡尔曼滤波 卡尔曼滤波不仅适用于平稳随机过程，也适用于非平稳随机过程。它将系统的状态迁移用状态方程来表述，并用固定维数的矩阵运算递推式代替了维纳滤波解维数大的线性方程组。克服了维纳滤波的一系列局限性而获得了成功应用。在应用中，卡尔曼滤波区别于递归加权最小二乘法的关键是考虑了系统噪声和测量噪声，建立了包括状态方程和观测方程在内的准确加权融合系统模型。 3．3．3数据融合 在传感器获取一系列数据后，通过数据融合一系列具有不同特性的、存在误差的数据经过计算，得到最接近实际的值。 对于姿态解算而言，需要将陀螺仪获得的三轴角速度数据和加速度计获得的三轴加速度数据进行融合，获取飞行器的姿态角数据。 陀螺仪测得的数据是三轴的角速度数据，将角速度进行积分，就可以获取三轴的角度数据。陀螺仪受飞行器振动的影响较小，噪声较低，但由于使用的是积分的方法，时间久以后容易产生累积误差，且无法修复，对飞行器的姿态控制影响较大。 加速计测得的数据是重力加速度和飞行器本身的加速度在三轴上的分量。当飞行器静止时，利用测得的三轴加速度数据可以唯一确定角度，求出的是一组绝对量，不存在累积误差。但在飞行器实际飞行的过程中，电机的振动会让加速度计的数据包含大量的噪声，难以滤除，同时，由于飞行器自身的加速度造成的角度计算偏差，也是加速度计本身无法排除的。 因此，可见通过陀螺仪计算出的角度数据瞬时比较准，长时间会产生累积误差，而通过加速度计算出的角度值瞬时误差较大，长时间则比较稳定，因此，需要通过一定的算法将这两种数据进行融合，以获得更接近真实角度的角度值。 常用于加速度计和陀螺仪数据融合的算法有一阶互补滤波算法、二阶互补滤波算法和卡尔曼滤波算法。 1． 一阶互补滤波 一阶互补滤波指的是对加速度计和陀螺仪计算而得的角度数据进行简单加权。实现一阶互补滤波的程序代码如下： K = 0．075; //对加速度计取值的权重 float A = K /（K + dt); Com_angle = A *（Com_angle + omega * dt) +（1-A) * angleA; //Com_angle为融合角度值，omega为角速度，dt为采样间隔，angleA为加速度计求得角度 2． 二阶互补滤波 二阶互补滤波则是将加速度信号作为参考值，将融合计算得到的角度与加速度计算出的角度的偏差量加入积分之中。代码实现如下： K = 0．5; float x1 =（angleA - Com2_angle) * K * K; y1 = y1 + x1 * dt; float x2 = y1 + 2 * K *(angleA - Com2_angle) + omega; Com2_angle = Com2_angle + x2 * dt; 3． 卡尔曼滤波 卡尔曼滤波针对各种随机过程都十分有效。相比维纳滤波，卡尔曼滤波能够用于非平稳随机过程，克服了其局限性。 卡尔曼滤波器被称为最优化自回归数据处理算法，对于很多应用，它是最好的方法，效率可以说是最高的。它是一个迭代预测器，每次只需输入上一时刻的数据，就能预测下一时刻的数据，在利用加速度值计算出角度后得到后验估计误差，从而不断迭代修正，得出最优解。 从理论上来说，卡尔曼滤波器能得到最好的结果，但对于飞行器而言，由于在飞行过程中振动十分剧烈，会对卡尔曼滤波器的预测产生很大的影响，从而导致在手持飞行器进行测试时，卡尔曼滤波器具有最好的表现，一阶互补滤波效果较差，而二阶互补滤波器数据比较平滑，但收敛较慢，角度跟踪性能差。但在实际飞行时，二阶互补滤波器飞行最为稳定。 3．3．4姿态解算 对于飞行器的姿态解算，通常采用获取陀螺仪和加速度计的数据，利用这些数据进行四元数运算，直接求出航空次序欧拉角。四元数相对于其他各种旋转描述法有着明显的优势。 (1) 巧妙的设计避免了处理缓慢的三角函数计算，运算效率提升。 (2) 四元数在全角范围内无奇点，没有万向节死锁问题。 常用的旋转描述有矩阵法、欧拉角以及四元数。 1． 矩阵法 图3．7绕Z轴旋转 (1) 绕Z轴旋转，如图3．7所示。 x′=xcost-ysint y′=xsint+ycost z′=z 矩阵表示为： （x′y′z′1）=(xyz1)cosγsinγ00 -sinγcosγ00 0010 0001绕Z轴旋转 (2) 绕X轴旋转，如图3．8所示。 y′=ycost-zsint z′=ysint+zcost x′=x 矩阵表示为： （x′y′z′1）=(xyz1)1000 0cosαsinα0 0-sinαcosα0 0001绕X轴旋转 (3) 绕Y轴旋转，如图3．9所示。 z′=zcost-xsint x′=zsint+xcost y′=y 图3．8绕Y轴旋转 图3．9绕Z轴旋转 矩阵表示为： （x′y′z′1）=(xyz1)cosβ0-sinβ0 0100 sinβ0cosβ0 0001绕Y轴旋转 矩阵法计算量最大，既要计算三角函数，计算三维旋转要用到四阶的矩阵运算，在实际使用中并不常用。 2． 四元数 欧拉角是一种最直观的描述旋转的方法，它以三次旋转的合成来描述空间任意旋转。但如3．3．1节所述欧拉角姿态描述中存在无法回避的奇异现象，需要结合四元素法构建姿态算法。 四元数是简单的超复数，一个实部，三个虚部。 四元数一般定义如下： q=w+xi+yj+zk 其中,w、x、y、z是实数。同时，有 i*i=-1 j*j=-1 k*k=-1 四元数也可以表示为： q=［v，w］ 其中，v=(x，y，z)是矢量，w是标量，虽然v是矢量，但不能简单地理解为三维空间的矢量，它是四维空间中的矢量。 通俗地讲，一个四元数(quaternion)描述了一个旋转轴和一个旋转角度。创建这个旋转轴和这个角度可以通过 Quaternion::ToAngleAxis转换得到，它可以返回一个绕轴线axis旋转angle角度的四元数变换。当然也可以随意指定一个角度一个旋转轴来构造一个四元数。这个角度是相对于单位四元数而言的，也可以说是相对于物体的初始方向而言的。 四元数项对于其他各种旋转描述法有着明显的优势。 (1) 虽然较欧拉角表示法多了一个参量，但是四元数的运算却巧妙地绕开了复杂耗时的三角函数运算，有更高的运算效率。 (2) 四元数在全角范围内无奇点，不会出现死锁问题。 3．3．5PID平衡算法 PID是在自动控制领域非常常用的一种算法。在飞行器的控制中，PID控制非常重要，使用在对飞行器姿态角的控制、飞行器的定高控制、定点悬停控制等。 1. PID算法的理解 如果要让飞行器从地面起飞在空中某一高度悬停，就要有向上的升力来克服地球引力，这个力是需要控制的量，飞行器高度有它现在的“当前值”，也有期望的“目标值”，当两者差距较大时，就让电机开足马力，尽快让飞行器到达目标高度附近，而当飞行器接近目标高度时，就让电机稍稍用力即可。这就是P的作用，P就是比例的意思。实际写程序时，就让偏差(目标值减去当前值)与调节装置的“调节力度”建立一个一次函数的关系，就可以实现最基本的“比例”控制了。P越大，调节作用越激进，P调小会让调节作用更缓慢。 有了P的作用后会发现，只有P好像不能让飞行器稳定，飞行器总是在某一高度处上下波动。设想一个弹簧挂一重物，现在在平衡位置上拉它一下然后松手，这时它会振荡起来。因为阻力很小，它可能会振荡很长时间，才会重新停在平衡位置。要是它浸没在水里，同样拉它一下，那么重新停在平衡位置的时间就短得多。所以需要一个控制作用，让被控制的物理量的“变化速度”趋于0，即类似于“阻尼”的作用。当比较接近目标时，P的控制作用就比较小了，越接近目标，P的作用越小。但还是有惯性让飞行器冲过目标高度，D的作用就是让物理量的速度趋于0，只要这个量具有了速度，D就向相反的方向用力，尽力抑制这个变化。D参数越大，向速度相反方向刹车的力就越强。 飞行器加上P和D两种控制作用，如果参数调节合适，飞行器就应该可以悬停在空中了。但看了一眼返回的高度值可能会发现一个不好的情况，稳定的高度值实际还没达到目标值，也许飞行器太重，这时上升的动力和重力已经相等了，这可怎么办？P认为和目标已经很近了，只需要轻轻加力就可以了，D认为高度没有波动，好像没我什么事了，于是高度永远也到不了目标值。根据常识知道，应该进一步增加上升动力，于是就要引入参数I了。增加一个积分量，只要高度偏差存在，就不断地对偏差进行积分(累加)，并反映在调节力度上。这样一来，即使高度相差不太大，但是随着时间的推移，只要没达到目标高度，这个积分量就不断增加。系统就会慢慢意识到： 还没有到达目标高度，该增加功率了。到了目标高度后，假设高度没有波动，积分值就不会再变动。这时，上升动力仍然等于重力，但高度是稳稳地定在了目标值上。I的值越大，积分时乘的系数就越大，积分效果越明显。所以，I的作用就是减小静态情况下的误差，让受控物理量尽可能接近目标值。I在使用时还有个问题： 需要设定积分限制，以防止在刚开始上升时就把积分量积得太大，之后难以控制。 2. PID算法的使用 在飞行器的控制中采用数字式PID，数字PID控制算法主要有位置式PID和增量式PID两种。位置式 PID控制的基本算法框图如图3．10所示。 图3．10PID基本算法框图 计算公式为： u(k)=KPe(k)+TTi∑ij=0e(j)+TdT(e(k)-e(k-1)) (3．1) 其中,e(k)代表的是设定目标和当前值的偏差值，若e(k)是设定当前横滚角和设定横滚角的差值，那么u(k)指的就是横滚方向的电机输出量。 将式（3.1）展开，可以分为比例项、微分项和积分项。 比例项控制是十分直接的控制，根据差值立即对油门输出量作出改变，将飞行器向差值的反方向调整就行。通过调整比例项的参数，可以让飞行器的角度在一定范围内等幅摆动。 微分项使用的变量是上一次和本次的差值之差，以横滚为例就是横滚方向的转动速度，在比例项的基础上加上微分项参数，主要是为了限制飞行器旋转的速度，通过比例项和微分项的联合作用，可以让飞行器在该角度上形成动态稳定的结果。 积分项的存在是为了消除稳态误差，如果飞行器存在重心的偏差，或者飞行器的电机动力有所不同，那么飞行器通过比例项和微分项维持稳定之后，有可能和设定目标有一个固定的误差，通过加入积分项，可以消除这种稳态误差。 u(k)=KPe(k)+TTi∑kj=0e(j)+TdT(e(k)-e(k-1)） (3．2) u(k－1)=KPe(k－1)+TTi∑k－1j=0e(j)+TdT(e(k－1)－e(k－2))(3．3) 数字式PID调节还有一种增量式PID调节，由上面两式，根据递推原理可以得到： A=KP1+TTi+TdT B=KP1+2TdT C=KPTdT 增量式PID求出的是油门的增量，与当前的油门值叠加，就可以实现对飞行器的控制，增量式PID相对于位置式PID具有以下优点。 (1) 因为PID的输出是油门增量，因此短暂出现错误动作、错误数据时影响较小，需要时还能够用逻辑判断消除。 (2) 计算公式中没有积分量，输出量只和最近几次采样的数据有关，方便程序进行处理，没有积分的累积误差。