穿越机控制思想介绍

林大

飞控的控制思想其实归纳起来，很简单。就是状态监测、控制和输出。但是控制出来的效果却是有好有怀，就是上面三大块中的差别和优化程度了。
穿越机上，状态监测就是使用更好的传感器；控制就是使用鲁棒性更好的算法；输出就是使用延迟更小效率更高的电机。
这其中还有个隐藏的优化项，那就是三者间的延迟，从状态监测到控制间的延迟和从控制到输出间的延迟，后者由于技术的原因无法提高了，但是前者却是存在着优化的空间。
传感器数据的读取到控制时间应该是要越短越好。但是由于操作系统的加入，这方面做得越来越不好了。程序的不同控制流程，会有不同的延时。

早期Betaflight（cleanflight）是专门为穿越机和小型四旋翼开发的飞控程序，使用CC3D的硬件，麻雀虽小五脏俱全。Betaflight没有使用操作系统，使用的是程序调度，应该是非抢占式的，也有优先级的概念。主要的控制框架分为三个部分：主循环、加速度值采集和姿态解算。
早期Betaflight的主循环采用100Hz循环，最高优先级。设计的相当巧妙，陀螺仪数据的读取和控制放在了一起，通常情况下陀螺仪的数据相当于角速度，相当于姿态控制中的微分部分，直接乘以微分系数就可以了，由于微分本来就是用来超前调节的，所以对实时性要求很高，这里读取陀螺仪数据后直接控制。至于直接读取陀螺仪值当做角速度相比于利用角度值进行微分哪个效果好就不得而知了。不过有个不理解的地方，那就是弄了个控制计数，也就是4次主循环才进行一次控制，这样控制周期就变成了25Hz，这样的设定很不理解。控制采用自适应积分的PID控制。
加速度计值的采集采用1000Hz，这里采用高频率采集主要是用来滤波，不过滤波的效果不得而知。
姿态解算部分也设计的也比较巧妙，将姿态解算另外做了一个线程，姿态角用来做P运算，通常不需要很高的实时性，况且姿态解算本身也不是那么可靠，看是将其拉到控制外面，确实可以大大的减少其控制延迟，最大程度的减少了延迟。姿态解算采用的Mahong算法，占用资源小，速度快。
穿越机具体的控制原理，其简化图如下所示：
飞行的动力由四个桨叶提供，通过改变四个螺旋桨的转速对四旋翼进行控制。四个电机产生的总升力可以使四旋翼在垂直方向运动，四个电机升力的差异导致四旋翼俯仰和偏航的变化进而改变运动方向，扭矩力的差异将导致四旋翼的偏航。根据四旋翼的飞行方式将四旋翼的飞行方式划分为四种基本的控制方式：垂直飞行控制、翻滚控制、俯仰控制和偏航控制。

通常四旋翼的飞行模型分为“+”型和“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系数表如下：



作为一款飞控，四旋翼模型的数据是未知的，甚至会因为植保或者喷洒等应用变得更加不确定。因此针对不同的情形使用不同的控制策略是比较好的办法，当模型数据确定时，尽可以使用依赖模型的控制算法，可以大大的提高控制精度；当模型数据不确定时，可以采用PID、模型预测与自抗扰等控制算法。因此控制模块需要进行模块化。
控制分为位置和姿态控制两个部分，位置控制定时10ms，优先级比姿态控制低，其中的控制任务中如果需要内环和外环不同的频率，则在里面设置时间标志位。控制算法全部写成黑盒子形式的API接口，保持参数和返回值形式相同。控制算法形式和参数都可以设置，包括：控制算法的选取，角速度使用角度微分或是陀螺仪值，控制的周期。其中控制算法包括：双环PID控制，ADRC控制等。