【空心杯四旋翼TinyLeaf】姿态控制入门

家父昨日已经取消休假返回医院工作，望所有医护人员平安归来！武汉加油，中国加油！

                                                                                                                   ——2020.1.27

目录

什么是控制

什么是闭环控制

姿态控制的目的

PID算法简介

航向控制

本文将结合PID控制算法介绍四旋翼姿态环的闭环控制思想和算法。

• 什么是控制

        控制是一个宽泛的概念，笔者对控制的理解是“让被控制对象按照期望运行”，下文也将按照此思想论述。

       如上图所示，控制的作用就是让被控对象的实际输出尽量符合期望/给定（给定：控制论术语，即期望的输出数据，比如“控温空调设定在26℃”，这里的“26℃”就是给定），期望可以是单个或多个，输出也可以是单个或多个。期望和实际输出之间的差值叫做误差。控制的意义就在于让误差及时的收敛。

       实现让输出跟随给定的算法叫做控制算法。控制算法有很多种，PID算法是一种非常经典的控制算法，也是本文着重介绍的算法，此外，还有诸如滑模控制（SMC），自抗扰控制(ADRC)等控制算法等。

• 什么是闭环控制

       闭环和开环相对，他们的差别是：闭环控制会采集被控对象的被控输出和期望比较，并通过一些算法对控制器修正，从而使输出更接近给定。在控制框图上，实际输出会返回到被控对象执行机构的控制量之前，以某种形式参与被控对象执行机构的控制量的计算，使得控制框图出现一个环，这就是闭环控制的由来。一般来说，只有闭环控制可以实现无误差的控制，并具有良好的抗干扰性能；开环控制一般做不到无误差控制（步进电机的控制是一个特例）。

       PID算法就是一种经典的闭环控制算法：

• 姿态控制的目的

       姿态控制是四旋翼飞行器控制的基础。没有控制好飞行器的姿态，飞行器就会变成一颗“布朗运动弹”，后续的高度控制，航线控制也就无从谈起。

       通常，我们常用欧拉角来描述飞行器的姿态。关于欧拉角的介绍，可以看我先前的博文【空心杯四旋翼TinyLeaf】四旋翼基础。欧拉角包括横滚（roll）、俯仰（pitch）、航向（yaw）,航向比较特殊，笔者会在本文下方另开一章介绍。

       显而易见，四旋翼在水平情况（四个旋翼轴向与地面垂直）下，升力和重力在一条直线上，也只有这个时候，随着升力的加大，四旋翼得以垂直起飞。当飞机倾斜时，总升力F的垂直分力与重力相抵消，总升力F的水平分力为四旋翼提供了一个水平方向的加速度。这个加速度为我们后续的位置控制提供了帮助。

        因此，我们把水平情况下的姿态定为零点，也就是俯仰角为零，横滚角为零。姿态控制的目的是让四旋翼保持水平，也就是让四旋翼的姿态服从给定(给定为零点)，让俯仰Pitch和横滚Roll去跟踪给定零点（Pitch = 0;Roll = 0）。这就是四旋翼的姿态控制。

• PID算法简介

       首先亮出PID的公式：

       PID分别代表了比例（Proportion）、积分（Integration）、微分（Differentiation）三个环节（忘记微积分的同学需要复习一下高等数学）。

       上方的框图我为了符合公式的写法，作的是时域框图，所以在积分环节和微分环节那，就只好用一个计算符号来表示环节这样不正规的做法了。大家在书本上或论文中，主要用的都是频域框图，那样更加准确。

       PID算法的实质，就是通过误差e(t)来调节执行机构的控制量 u(t)。

       控制量u(t)由比例、积分、微分三个部分构成：

• 比例：输出和误差成正比，系数是 Kp，比例项没有滞后，误差越大，比例项提供的修正量越大；
• 积分：输出和误差的积分程成正比，系数是 Ki，积分项对误差响应有滞后，同向误差持续存在时间越长，积分项提供的修正量越大；
• 微分：输出和误差的微分（导数）成正比，系数是 Kd，微分项对误差响应有提前，误差的变化程度越剧烈，微分项提供的修正量越大。

       从上面的框图我们可以看到，如果输入和输出一致

                                                                                                    c(t) = r(t)

       那么误差就为0

                                                                                             e(t) = r(t) - c(t) = 0

       如果误差、误差的积分、误差的微分（导数）都为0，这么U(t)也就为0，即执行器不会动作。这点很好理解：输出已经和所希望的一致了，自然就不需要动作了，只有输出和期望出现了偏差（误差e(t)），执行器才会动作以消除偏差。

关于PID算法的详细介绍我将在之后新开一篇博客介绍。

• 航向控制

       之所以要把航向控制取出来特别讲，是因为航向的控制有多种方式，也涉及到四旋翼的飞行模式。俯仰、横滚轴的控制参数是基本一致的，它们决定了飞行器与水平面的夹角。在整定好俯仰轴的PID参数后，这套参数可以照搬到横滚轴。而航向则比较特殊，航向轴的变化不影响飞行器与水平面的夹角。

       笔者刚开始写四旋翼飞控的时候，对于航向轴的闭环是和控制俯仰，横滚轴类似的：给定水平时的初始方向为零点。然后就发现了一个问题：四旋翼起飞时机头朝南，我使用遥控器打航向左转。机头朝东。遥控器航向归中，机头又自动回到了朝南。这就和笔者玩的穿越机不太一样了（正常应该是打方向旋转，归中就停在当前角度）。为什么呢？实际上，大多数四旋翼对航向轴采取的是角速度闭环，也就是说，给定是角速度为0而不是角度为0。

• PID的参数整定

        一直以来，PID的参数整定对于很多人来说都是一个老大难问题。笔者在开始学习自抗扰控制前，控制一直使用的是PID算法,包括四旋翼，倾转翼，动量轮，倒立摆，风力摆，平衡车等等。在这些系统的控制中，笔者总结了一些调试PID参数的技巧和注意点，这部分内容会在我讲述PID算法的专门博文中介绍，在此只介绍一些四旋翼飞行器控制的相关的技巧。

        我知道很多人写代码都是CtrlCVer，不过笔者还是建议初学PID的朋友，按照公式自己写一些，这对于理解PID的算法有很多好处。

       在四旋翼的的姿态控制中，PID正常情况下起作用的主要是比例和微分，也就是PD控制。

       为什么积分环节一般不常用呢？主要是两个原因，一个是积分的作用总是非常迟缓，而四旋翼的姿态控制对快速性的要求很高，另一方面，积分环节的作用主要是消除静差，而姿态环很少出现静差，出现静差说明你的四旋翼出现了一些问题，有可能是陀螺仪的安装固定出现的明显的倾斜，或者四旋翼的重心没有与合升力在一条直线上（重力和升力不是共点力，就会产生力矩，这就是静差的来源）。

        对于姿态环的参数整定，笔者的建议是：

1. 先调一个轴，先调节比例系数Kp,即纯P控制,从小往大调节，调节到四旋翼可以自己恢复到平衡点但不至于产生振荡。
2. 然后将Kp置0，调节微分系数Kd，这个的感觉需要自己拿捏，不同的机型参数差异比较大。这个参数也是从小往大调节，微分是抑制高频扰动的，什么属于高频扰动？用杆子捅一下四旋翼，对四旋翼产生的扰动就属于高频扰动，Kd的调节因性能要求而异，唯一的要求是不能出现自激振荡（在扰动后自己产生的振荡也算是自激振荡）。
3. 调到自己觉得满意之后。再使用PD控制，注意要让之前的Kd稍微减小一些，建议将系数乘以0.7~0.95。然后开始综合调节，视情况在加上一些积分作用（Ki）。

语言总是无力的，纸上得来终觉浅，绝知此事要躬行。有条件的朋友，建议准备一个调试架。

       笔者能力有限，错漏之处在所难免，如有错误，可在下方评论指出，也可邮箱交流：read_air@163.com。

码字不易，给个赞可好？