这一部分是核心内容，讲解姿态角的串级PID控制。在智能小车、四旋翼、四足狗子等等一系列机器人的控制系统中，姿态控制（俯仰角、滚转角、偏航角）都是核心内容，它决定了小车开得直不直，飞机飞得稳不稳。虽然现在先进的、智能的控制算法有很多，如自适应控制、神经网络控制、模糊控制等在机器人控制系统的设计上有了很多应用，但是最常用的最好用的依然是PID控制器，搞通了PID控制器就能够应付绝大多数场合了。

本文续接上一篇STM32实现四驱小车（三）传感任务——姿态角解算。

PID控制器的原理图如图所示。 PID控制器是一种线性控制器，根据给定值和实际输出值的偏差构成控制偏差 e ( t ) = y d ( t ) − y ( t ) e(t)={{y}_{d}}(t)-y(t) e(t)=yd​(t)−y(t) PID的控制率为 u ( t ) = k p [ e ( t ) + 1 T I ∫ 0 t e ( t ) d t + T D d e ( t ) d t ] u(t)={{k}_{p}}\left[ e(t)+\frac{1}{{{T}_{I}}}\int_{0}^{t}{e(t)dt+{{T}_{D}}\frac{de(t)}{dt}} \right] u(t)=kp​[e(t)+TI​1​∫0t​e(t)dt+TD​dtde(t)​] 其中， k p k_p kp​为比例系数， T I T_I TI​为积分时间常数， T D T_D TD​为微分时间常数。PID控制器各校正环节的作用为： （1）比例环节：成比例的反应控制系统的偏差信号e(t)，偏差一旦产生，控制器立即产生控制作用，以减少偏差。但是比例环节不能消除稳态误差。 （2）积分环节：主要是消除静差，提高系统的无差度。积分作用的强弱取决于积分时间常数 T I T_I TI​， T I T_I TI​越大，积分作用越弱，反之则越强。 （3）微分环节：反映偏差信号的变化趋势（变化速率），并能在偏差信号变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减少调节时间。

如何调节PID参数是实现PID控制器的核心内容，以笔者的经验，比例环节是起主要调节作用的，从小到大逐渐调整，直到系统有发散的趋势，然后往回取一个适中的值；积分环节的作用是消除误差，确定了比例系数后，从小到大增大积分系数（减少积分时间常数），直到系统有发散的趋势，积分环节不需要取得很大，记住它的作用是消除误差。微分环节的作用是超前校正，但是在噪声较大的情况下会放大噪声，引起系统不稳定，所以对于延迟没有太高要求的场合可以不加微分环节。

在实际中我们都是用的离散系统，所以我们关心数字PID控制的实现。在应用中一般有位置式PID控制和增量式PID控制 。

位置式PID的算法为： u ( k ) = k p e ( k ) + k i ∑ j = 0 k e ( j ) T + k d e ( k ) − e ( k − 1 ) T u(k)={{k}_{p}}e(k)+{{k}_{i}}\sum\limits_{j=0}^{k}{e(j)}T+{{k}_{d}}\frac{e(k)-e(k-1)}{T} u(k)=kp​e(k)+ki​j=0∑k​e(j)T+kd​Te(k)−e(k−1)​ 式中，T为采样周期，也就是单片机的控制周期。k为采样序列，e(k)和e(k-1)分别是第k次和第k-1次所得的偏差信号。

当执行机构需要的是控制量的增量时（例如驱动步进电机），应该采用增强式PID控制。由位置式PID的算法： u ( k − 1 ) = k p e ( k − 1 ) + k i ∑ j = 0 k − 1 e ( j ) T + k d e ( k − 1 ) − e ( k − 2 ) T u(k-1)={{k}_{p}}e(k-1)+{{k}_{i}}\sum\limits_{j=0}^{k-1}{e(j)}T+{{k}_{d}}\frac{e(k-1)-e(k-2)}{T} u(k−1)=kp​e(k−1)+ki​j=0∑k−1​e(j)T+kd​Te(k−1)−e(k−2)​ 得到增量式PID算法为： Δ u ( k ) = u ( k ) − u ( k − 1 ) = k p [ e ( k ) − e ( k − 1 ) ] + k i e ( k ) + k d [ e ( k ) − 2 e ( k − 1 ) + e ( k − 2 ) ] \Delta u(k)=u(k)-u(k-1)=k_{p}[e(k)-e(k-1)]+k_{i} e(k)+k_{d}[e(k)-2 e(k-1)+e(k-2)] Δu(k)=u(k)−u(k−1)=kp​[e(k)−e(k−1)]+ki​e(k)+kd​[e(k)−2e(k−1)+e(k−2)]

对于姿态角的控制，我们希望给定姿态角机器人能够跟随给定的输入，其实这就是一个位置跟踪问题。按照单级PID的思路应该是这样的： 但是这里不用这种方式，而是采用串级PID，也就是一个PID套一个PID，外面是角度环，里面是角速度环。这样做的好处是增加了控制系统的响应速度和稳态精度，具体的原理大家可以去找文章专门研究，这里不过多讲解。

原理弄明白之后其实实现起来很简单，PID的控制算法是通用的，完全可以移植，只要调整三个系数以适应自己做的东西就可以了，这里我们一起写一下，建立一个pid.h和一个pid.c文件，添加到工程中。 pid.h的内容如下，定义PID的结构体和一些数据结构、声明函数。

pid.c当中实现函数：

这一部分代码大家自行阅读，很好理解，另外大家如果嫌函数太多可以用C++来用对象实现PID结构体。（网上有，不想自己写去copy也行）

由于我们要使用串级PID控制航向角，仅仅有上面的PID控制器代码还不够，咱们继续创建一个attitude_control.h和一个attitude_control.c文件，用来实现串级PID控制。

attitude_control.h文件内容如下：

attitude_control.c文件内容如下：

attitude_control.c文件一开始声明并初始化了一个结构体变量configParamCar ，类型为configParam(在pid.h中定义的)，里面保存的就是小车所有PID的参数值，后续要做的就是对这个结构体进行PID调参。

大家可能注意到了attitudeControlInit(), attitudeRatePID(), attitudeAnglePID里面全部都有三轴的角度，只不过我屏蔽掉了俯仰角和滚装角，因为对于小车来说我们只需要航向角。后期实现四旋翼我们依然用的这一套代码框架，届时只需要使能其他两个角度就能实现四旋翼的姿态控制了。

有了上面的驱动代码和PID算法，下面我们写main.c文件里面的StabilizationTask，实现姿态控制任务。

在上一篇STM32实现四驱小车（三）传感任务——姿态角解算的基础上，补充StabilizationTask函数的内容如下：

这里面while循环里面的步骤为，首先根据读到的遥控器的方向摇杆的值更新期望偏航角，期望偏航角来自于方向摇杆的积分。然后根据速度档位按钮的值确定当前的油门量（低速与高速模式）。之后判断遥控器油门摇杆与方向摇杆的位置，如果都居中说明机器人应该静止，此时复位所有PID，PID输出置零。如果任何一个摇杆不是中间位置，说明是在前进后退或者原地转弯状态，此时使能串级PID控制，控制器的输出送入到混合控制器（注意这个词，在飞控中还会用到），由于四驱车的模型很简单，其实就是一侧加上这个控制量加速，一侧减去这个控制量减速，从而实现差速，控制机器人转弯。

这里面有一个数组set_speed[4]，存储的是各个电机的速度，这个速度值在下一篇电机伺服任务中我们要用到，它作为期望速度值，作为电机速度伺服的PID控制器输入。

这里做下说明，本系列文章笔者重在分享思想、算法，在讲解上会弱化一些基本知识（比如单片机各个外设的原理、单片机编程的基本知识等），在代码的粘贴上会忽视一些底层的驱动代码和无关紧要的部分，事实上上面的代码我都经过删减了，只留下了干货。所以可以说面向的是中高级选手，拿来主义者可以打道回府了，本系列文章不开源，不提供源码，请见谅。