EEDrone开源四旋翼从零开始（10）--四旋翼控制分析
上期回顾：EEDrone开源四旋翼从零开始（9）——控制框架对比分析
这次来聊聊四旋翼具体的控制问题，其简化图如下所示：
飞行的动力由四个桨叶提供，通过改变四个螺旋桨的转速对四旋翼进行控制。四个电机产生的总升力可以使四旋翼在垂直方向运动，四个电机升力的差异导致四旋翼俯仰和偏航的变化进而改变运动方向，扭矩力的差异将导致四旋翼的偏航。根据四旋翼的飞行方式将四旋翼的飞行方式划分为四种基本的控制方式：垂直飞行控制、翻滚控制、俯仰控制和偏航控制。
通常四旋翼的飞行模型分为“+”型和“X”型，分别如下图左和右所示：
其实本质上两者是没有什么区别的，仅仅是体坐标系的选择不同而已，两种模式中，“+”模式控制起来比较直观，常用于理论分析。“X”模在实际中采用较多，符合人们对四旋翼前后的直观认识，同时也方便摄像头的安装。
这里以“+”型模型为例，定义四旋翼的高度，俯仰，翻转，偏航的控制分别为u1、u2、u3和u4。在“+”模型中，u1是四个旋翼升力的总和，可以控制四旋翼的高度；u2是旋翼2和4的升力差，可以控制四旋翼的俯仰角；u3是旋翼1和3的升力差，可以控制四旋翼的翻滚觉；u4是四个旋翼的转矩力和，可以控制四旋翼的偏航。
“+”型四旋翼的控制量与电机转速的关系为：
但在实际控制中只需要知道控制量与旋翼旋转产生的扭矩力以和控制量与旋翼旋转产生的升力的关系即可，电机的转速并不是我们需要的，所以如果将控制量和旋翼产生的升力做模型辨识的话，会得到下图的曲线：
可以发现控制量从20到80大致为线性关系，所以可以将上面的式子转化为：
其中Cn就是各个电机的控制量，这是个很有趣的转换，将四个基本的控制量与各个电机的控制量联系起来了，两者的联系就是关系矩阵B+，当使用“X”类型或者6旋翼等机型时候，只需要乘以不同的关系矩阵就好。因此，通常只对U进行控制。
控制的目的就是控制执行器使当前的状态变成期望的状态，这里就涉及到了状态，通常说四旋翼的六自由度，说的就是四旋翼的三个姿态与三维空间中坐标中的三个位置。通常所说的四旋翼状态还包括基本状态的导数。根据状态量与期望值得出电机的控制量就叫做控制算法的设计，由于四旋翼涉及到体坐标与地坐标所以经常需要在两者坐标系间进行转换。
目前开源四旋翼多使用的PID控制算法或者基于PID的控制算法，除了PID是经典的原因外，还因为PID控制算法是一种不依赖模型的控制算法，也就是数据驱动控制算法，它的最大的好处就是可以不需要很精确的对象模型就可以有较好的鲁棒性；现代控制理论能够很好的分析控制系统的结构性质，并且有很好的分析工具和较完备的设计方法，但是设计的控制器结构复杂且需要精确的模型。这里谈谈我对PID的理解，比例（P）是主要的纠正误差手段，积分（I）是在将误差积累使得控制力度不断增大，微分（D）利用运动的趋势来控制，像缰绳拉住奔跑的马。通常并不是任何情况下都需要将三者线性叠加的，例如积分，它的不断的叠加会导致灾难性后果，通常在不会出现残差的情况下不使用它，例如小车的位置控制，只要电机给点就会移动，这就是无残差；但是当小车上放了重物，小电压小车不动，这就叫做残差了，需要积分来不断的增加控制量，但是积分很容易导致超调。微分看似挺好，但是很多时候由于测量值的跳变会导致微分值剧烈的跳变，这叫扩大了高频扰动，这样控制效果自然不会好，通常会在微分前加一个滤波，但是通常滤波是有延迟的，这又降低了微分的时效性。所以需要根据实际情况合理的选择采用PID中的哪几个来控制。PID解释清楚了放到代码里又分为位置式PID和增量式PID，看上去代码不同，但是本质上效果是一样的，位置式不需要积分限幅，但需要整体限幅。
前面介绍过四旋翼整个流程为：传感器数据读取、姿态解算与位置获取、控制、电机控制量分配。这次讨论就是其中的控制，控制之前是已经知道了姿态与位置的。下面来看看两款具有代表性的开源四旋翼是如何处理的，
APM
APM的姿态采用的是PID双环控制策略，借用官网的一张图如下:
首先是期望角度限幅，然后与实际角度的差值经过比例运算，这里的比例运算使用了一个开平方的策略，具体可以参考下面的图，角度的差值在一定的范围内使用线性的，大于阈值后就使用差值的开方，这样做的好处就是压缩大值，扩大小值，记得大学老师经常将学生成绩开个根号再乘10吗？这里角度的误差乘以P后当做角速度的期望值，然后和实际的角速度比较，将误差再进行PID运算，最后计算出来给电机。图中是姿态的控制，位置控制实际上也是要转化为姿态控制的，控制方式和姿态类似也有开方的过程，位置控制是通过姿态控制实现的，所以四旋翼的姿态控制尤为关键。
CF2
CF2位置控制和速度控制全部放到了一个函数里面了，使用的方法是在函数内设置标志位来判断是否执行，姿态控制的频率是500Hz，位置控制的频率是100Hz，控制的流程如下图：
从流程图可看出是P，PI和PID控制算法相结合的，全部使用位置式的传统PID，一共是四组PID串联。PID系数表如下：
MY
作为一款飞控，四旋翼模型的数据是未知的，甚至会因为植保或者喷洒等应用变得更加不确定。因此针对不同的情形使用不同的控制策略是比较好的办法，当模型数据确定时，尽可以使用依赖模型的控制算法，可以大大的提高控制精度；当模型数据不确定时，可以采用PID、模型预测与自抗扰等控制算法。因此控制模块需要进行模块化，飞控的控制模块结构定为如下：
控制分为位置和姿态控制两个部分，位置控制定时10ms，优先级比姿态控制低，其中的控制任务中如果需要内环和外环不同的频率，则在里面设置时间标志位。控制算法全部写成黑盒子形式的API接口，保持参数和返回值形式相同。控制算法形式和参数都可以设置，包括：控制算法的选取，角速度使用角度微分或是陀螺仪值，控制的周期。其中控制算法包括：双环PID控制，ADRC控制等。