056、位置环与速度环的串级PID实现

056、位置环与速度环的串级PID实现

一、从一次炸机事故说起

去年调试四轴飞行器，在室内用光流定位做悬停。位置环输出直接给了电机油门，结果飞机像抽风一样上下抖动，最后侧翻炸机。事后分析日志，发现位置环输出变化率太大，电机响应跟不上，导致积分项疯狂累积——这就是典型的单级位置PID的缺陷。

后来换成位置环+速度环的串级结构，同样的参数，飞机稳得像钉在空中。今天就把这个坑填上。

二、为什么需要串级PID

单级PID的问题在于：位置环直接输出油门值，但油门和加速度之间隔着电机响应、空气动力学、姿态变化等多重非线性环节。位置环的P项稍微大一点，电机就会过冲；I项大了，积分饱和能把飞机送上房顶。

串级PID的思路是：外环（位置环）输出期望速度，内环（速度环）输出实际油门。这样外环只负责“我要多快”，内环负责“怎么达到这个速度”。内环的响应速度比外环快得多，能及时抑制扰动。

这里踩过坑：有人把内外环频率设成一样，结果两个环互相打架。正确做法是内环频率至少是外环的5-10倍。

三、代码实现（C语言，STM32平台）

3.1 数据结构定义

typedefstruct