4. SITL中的姿态控制架构：内环角速率控制、外环角度控制、控制链路分析

姿态控制，说白了就是让飞机在空中“听话”。你让它平飞，它就不能侧着身子；你让它转弯，它就得乖乖滚转。在SITL仿真里，这个“听话”的过程是靠一套经典的双环控制架构实现的。

我个人习惯把姿态控制想象成两个人在配合干活。外环是“指挥官”，负责看大方向——比如“机身要倾斜30度”。内环是“执行队长”，负责具体操作——比如“用多大的力气去转电机”。这两个环配合得好，飞机飞起来才稳当。

4.1 双环控制架构：外环角度 vs 内环角速率

先看一张整体架构图，我把它画成了SVG，方便你理解信号是怎么流的。

你看这个图，信号从左往右流。期望角度进来，外环先算一遍，输出一个期望角速率。内环拿到这个角速率，再算一遍，最后输出给电机。两个环都有反馈，但反馈的东西不一样——外环看角度，内环看角速率。

4.2 外环角度控制：P控制器的艺术

外环角度控制，我习惯叫它“位置环”。它不关心飞机转得有多快，只关心“转没转到目标角度”。

在ArduPilot和PX4的SITL实现里，外环几乎清一色用P控制器。为什么不用PID？你想想看，角度误差本身就有积分效果——你偏了10度，持续偏着，误差一直存在，控制器就会一直输出。再加上I项反而容易引起超调和震荡。

外环的数学表达式很简单：

期望角速率 = P_angle × (期望角度 - 当前角度)

这里的P_angle就是外环的P增益。我在项目中遇到过一个问题：P增益调得太大，飞机在悬停时高频抖动，像“帕金森”一样。后来发现是外环输出太猛，内环跟不上。

4.3 内环角速率控制：PID控制器的战场

内环才是真正“干活”的地方。它直接面对电机，响应速度要求极高。在SITL里，内环通常跑在200Hz以上，而外环可能只有50Hz。

内环用的是完整的PID控制器：

电机输出 = P_rate × 角速率误差 + I_rate × 积分项 + D_rate × 微分项

每个参数都有自己的脾气：

参数	作用	调太大	调太小
P_rate	快速响应角速率误差	高频震荡，电机发烫	反应迟钝，转弯拖泥带水
I_rate	消除稳态误差	低频摆动，飞机“晃悠”	有静差，回不到中位
D_rate	抑制超调，增加阻尼	噪声放大，电机“滋滋”响	超调大，过冲明显

我曾经在调一个四轴时，内环P给到0.25，飞机起飞后直接翻了个跟头。嗯，后来查日志发现角速率反馈有200ms的延迟——SITL仿真里如果CPU负载高，也会出现类似问题。

4.4 控制链路分析：信号怎么走

理解了内外环各自的工作，我们再看看整个控制链路是怎么串起来的。说白了，就是一条“期望→计算→执行→反馈”的闭环链条。

在SITL里，这个链路每一步都有对应的代码和数据结构。我以ArduPilot为例，把链路拆成5步：

1. 获取期望角度：来自遥控器摇杆或航线指令，经过限幅和速率限制
2. 外环计算：用当前姿态和期望姿态算出角度误差，乘以P增益，输出期望角速率
3. 内环计算：用陀螺仪读到的实际角速率和期望角速率算误差，经过PID输出电机指令
4. 混控输出：把内环输出的滚转、俯仰、偏航指令混合成4个电机的PWM值
5. 状态更新：陀螺仪和姿态估计算法更新当前角速率和角度，反馈给内外环

这5步在SITL里每5ms（200Hz）跑一轮。你想想看，一秒钟跑200轮，每一轮都要完成从传感器到控制器的全链路计算。这就是为什么SITL对电脑性能有要求——CPU跟不上，仿真时间就不真实。

4.5 内外环的配合关系

内外环不是各自为战，它们有严格的配合关系。我总结了一个“快慢原则”：

• 内环要快：响应速度至少是外环的3倍。外环抖一下，内环能迅速跟上
• 外环要稳：输出不能突变，否则内环会饱和。外环输出要加限幅和低通滤波
• 带宽分离：内外环的带宽要拉开距离。如果外环带宽2Hz，内环至少6Hz

这个原则在SITL里怎么验证？很简单。给一个阶跃期望角度，看响应曲线。如果外环响应慢但内环有震荡，说明内环带宽太高。如果外环响应快但内环跟不上，说明内环带宽不够。

我个人习惯是先调内环，再调外环。内环调稳了，外环怎么调都不会太差。反过来，外环调得再好，内环不稳也是白搭。