干四旋翼调参的人，谁没有被姿态环抖动折磨过？P大一点会高频抖，D小一点就软绵绵，好不容易稳住，来个阵风又栽头。大多数时候，我们盯着的只有角度误差和角速度误差，但实际上真正决定瞬时响应的，是“角加速度”这个藏在底层的状态。它不像pitch/roll或者gyro那样在MAVLink里特意给你开一扇门，却始终埋在PX4的IMU流和姿态估计器里。这篇文章我就把这个很少被正面使用的状态挖出来，讲清楚角加速度在PX4里怎么提取、怎么进控制环，并结合我实际改代码、跑仿真的过程，给你一套能直接抄作业的提升控制性能方案。无论你是在Ubuntu上刚搭好PX4仿真环境，还是想用Simulink做滑模控制验证，这篇都值得看完。

1. 为什么要挖角加速度：控制问题与破局思路

1.1 经典P-PID控制器的天花板

先看PX4默认的姿态控制结构：最外面是角度环，通常一个P控制器；内层是角速度环，一般用PID调节。外环根据当前姿态误差生成一个期望角速度，内环负责把实际角速度追到期望值。这套结构本身没有大问题，问题在于内环的PID本质上是误差反馈，它必须等到“角速度误差已经出现”才开始使劲。

悬停时来一个阵风，机体会先被气流推着产生角加速度，角速度在几毫秒到几十毫秒之后才积累出可观测的偏差。角速度环看到偏差再去打舵，已经慢了半拍。这就是为什么很多飞控调参总感觉“迟滞”——因为你在用一个已经发生的结果去弥补一个刚刚发生的原因。

有人会通过加大D项来缓解，但D项本质上是“角速度偏差变化趋势”的近似，对噪声极其敏感，增益稍微一高，电机就开始发烫、机身嗡嗡响。我见过很多新手为了压住振荡不停加D，最后整个频率段的噪声都被放大，姿态反而更飘。真正该做的，是把“角加速度”直接引入控制决策，而不是靠误差去间接猜它。

1.2 角加速度在这个结构里能干什么

角加速度在控制层面的价值可以拆成三块：前馈、阻尼、扰动观测。

前馈最好理解。外环在生成期望角速度的时候，如果我们同时能算出这个期望角速度的变化率，也就是期望角加速度，就可以在内环输出里提前叠加一个分量，相当于告诉电机：“接下去角速度要加速了，先做好准备。”这是把反馈控制变成“反馈+预判”的关键一步。角度环的比例增益Kp越高，这个前馈带来的相位补偿越明显。

反馈阻尼是另一回事。实际角加速度和期望角加速度之间的偏差，可以看成一个“主动阻尼项”。传统D项只能靠角速度误差的微分量去近似，现在直接用真实角加速度，相当于把阻尼项做准了。对于四旋翼这种欠阻尼系统，角加速度反馈能显著提高稳定裕度，让姿态响应更快又不容易振。

第三个用途是扰动观测。角加速度的变化和外力矩直接相关，通过对比指令力矩与动力学模型算出的角加速度，可以反推外界风扰、桨叶损伤等异常力矩。这部分在故障诊断里特别有潜力，我后面会提到。

1.3 为什么说PX4里这些数据是“隐藏”的

说它“隐藏”，是因为PX4默认没有专门向MAVLink或者用户接口发一个angular_acceleration话题，官方日志里也没有一个现成的vehicle_angular_acceleration字段。但数据源一直都在：姿态估计器输出的angular_velocity，IMU驱动的delta_angle，底层原始陀螺仪数据，三者里都含着角加速度信息。

最直接的办法是对角速度做数值微分，这也是我最早测试的方案。不用改硬件、不用买新传感器，只要拿到时间戳和角度速度，就能算出来。当然代价是噪声放大，这个问题我会在第三章重点讲，它也是很多人“挖到了数据却用不起来”的常见原因。

另一个“隐藏”层面是代码结构。PX4的mc_att_control和mc_rate_control是两个独立模块，姿态环输出角速度期望，角速度环输出力矩，角加速度既没在这两层之间显式传递，也没有作为中间状态被保存。所以要利用它，必须做一点二次开发，把数据从底层取出来，再想办法塞进控制环。好在这一层开发门槛不算高，PX4的模块化架构足够清楚。

2. 角加速度数据怎么来：从仿真到真机的三条路径

2.1 路径一：角速度数值微分（最常用）

这是最直觉、也最容易上手的办法：直接用相邻两拍角速度之差除以时间差。PX4里常见的角速度来源是姿态估计器发布的angular_velocity，单位是rad/s。

// PX4中角速度数值微分，简化示例 uint64_t now = sensor_combined.timestamp; float dt = math::constrain((now - _last_time) * 1e-6f, 1e-4f, 0.02f); float rates[3] = { _att.angular_velocity[0], _att.angular_velocity[1], _att.angular_velocity[2] }; float angular_acc[3]; for (int i = 0; i < 3; ++i) { angular_acc[i] = (rates[i] - _last_rates[i]) / dt; _last_rates[i] = rates[i]; }

这里math::constrain很重要。如果某段时间线程被调度卡住，dt瞬间变成几秒，算出来的角加速度会直接爆到一个离谱的量级。把dt限制在0.02秒以内，至少能让异常值不污染后续控制。

数值微分最大的坑是噪声。陀螺仪数据本身就有测量噪声，差分一次相当于对噪声做了一次高通放大，高频分量会成倍抬高。所以数值微分之后必须跟低通滤波，否则你拿到的不是角加速度信号，而是噪声发生器。我建议至少用一阶巴特沃斯低通，截止频率先设在30Hz左右，后续根据实际响应再调。

2.2 路径二：基于IMU的旋转加速度解算（更稳）

如果把数值微分看成“硬算”，那基于IMU增量信息的做法就偏“软解算”。PX4的IMU驱动会输出delta_angle，也就是在每个采样周期内角度增加量。增量本身是积分后的结果，比瞬时角速度的噪声小一些，用它除以dt得到的角速度会更干净。

在这个基础上再做一次差分，角加速度的噪声水平会比直接用角速度差分低不少。但代价是引入了惯性：因为IMU的积分周期和姿态估计器输出周期不完全一致，你得到的数据是“一段时间内的平均加速度”，不是瞬时值。在低速动态场景下问题不大，但在高频机动时会有明显相位滞后。

更稳一点的做法是配合机体的动力学模型做约束。四旋翼的角动量方程写出来之后，角加速度和力矩、角速度叉乘项之间有一个确定关系。用模型约束把差分结果拉回来，可以把异常跳变抑制掉。这种方法需要知道转动惯量J和电机/螺旋桨执行模型，前期标定工作量大，但换来的好处是数据几乎不需要额外滤波。

对大多数读者，我建议先跑路径一，发现问题再升级到路径二。不要一上来就上模型估算，否则你排查问题的变量会多到崩溃。

2.3 路径三：在Simulink里构造滑模观测器（为进阶做准备）

Simulink里做角加速度估计，通常不是为了替代PX4的计算，而是为了设计和验证控制算法。四旋翼刚体动力学可以写成：

J * α = M - ω × (J * ω) + d

其中J是转动惯量矩阵，α是角加速度，M是控制力矩，ω是角速度，d是外部扰动。滑模观测器可以同时估计角加速度和扰动，基本思路是构造一个角速度估计值，让估计值和测量值之间的误差收敛，再利用观测器的等效控制项反推扰动和加速度。

一个比较实用的一阶滑模观测器形式：

J * α_hat = M - ω × (J * ω) + z z_dot = k1 * tanh((ω - ω_hat) / ε)

这里tanh代替了sign函数，用来抑制抖振。ω_hat是观测器估计出的角速度，z相当于一个“补偿力矩”，它的变化率里就带着扰动和真实角加速度的信息。我在Simulink里验证之后，会把这套观测器再用C++写到PX4模块里，性能比纯差分好不少，尤其是对抗风扰的场景。

3. 实操记录：在PX4工程中添加角加速度前馈

3.1 环境准备：Ubuntu 20.04 + PX4 1.12.3开发环境搭建

我建议直接固定到v1.12.3，这是目前社区资料最多、坑相对少的版本。先准备一台Ubuntu 20.04系统，装好git、python相关依赖等。

git clone --recursive https://github.com/PX4/PX4-Autopilot.git Firmware cd Firmware git checkout v1.12.3 git submodule update --init --recursive

然后装工具链，PX4官方推荐用自动化脚本。我这里给出最简的命令行流程：

bash ./Tools/setup/ubuntu.sh --ci

脚本跑完再执行：

make px4_sitl gazebo

第一次编译会比较久，大概20到40分钟，取决于机器性能。如果中途因为缺依赖报错，先看脚本输出，把缺失的python库补上，再重新make。

编译成功后，你会在终端看到类似pxh>的提示符，这说明SITL已经启动，PX4固件跑在了Gazebo模拟环境里。到此，PX4开发环境搭建就完成了。

3.2 启动Gazebo仿真并确定能连上PX4

make px4_sitl gazebo之后，PX4会在本地启动MAVLink通信。默认情况下，QGroundControl可以通过UDP端口14550连接。你只需要在QGC里选择“Add New Connection”，类型选UDP，监听端口填14550，就能看到飞机姿态和飞行数据。

要确认连接是否通了，可以在PX4命令行里输入：

mavlink status

如果在输出里能看到QGC connected或者类似的连接信息，说明通信正常。这一步对应很多新手问的“ubuntu px4模拟器怎么连接”：本质上就是本地UDP端口对接，QGC负责可视化，PX4负责跑飞控逻辑。

某些情况下，QGC会主动找到SITL的自动广播，不需要手动配置。但如果你改了端口，或者开了多个实例，手动添加连接会更省事。连接正常后，先用commander takeoff做个简单起飞测试，确认遥控器指令和仿真姿态反馈都正常，再开始改代码。

3.3 三步完成mc_rate_control代码改造

修改的核心是角速度控制模块：src/modules/mc_rate_control/MulticopterRateControl.cpp。同时也要在姿态控制模块mc_att_control里生成期望角加速度数据。为了不破坏原有逻辑，我建议把角加速度相关功能做成一个可开关的参数，默认关闭，方便回退。

第一步，添加成员变量和滤波对象：

// MulticopterRateControl.hpp Matrix3f _angular_accel_lp; // 保存低通滤波后的角加速度 Vector3f _ang_vel_prev; // 上一拍的角速度 uint64_t _last_timestamp_us = 0; float _param_aa_gain = 0.1f; // 前馈增益，默认先给一个保守值

第二步，在Run()循环里取出角速度并差分，然后低通：

uint64_t now = hrt_absolute_time(); float dt = math::constrain((now - _last_timestamp_us) * 1e-6f, 1e-4f, 0.02f); _last_timestamp_us = now; Vector3f ang_vel = _att.angular_velocity; Vector3f ang_acc = (ang_vel - _ang_vel_prev) / dt; _ang_vel_prev = ang_vel; // 一阶低通，截止频率按需调整 _angular_accel_lp = _angular_accel_lp * (1 - lpf_alpha) + ang_acc * lpf_alpha;

第三步，在角速度控制输出端叠加前馈/阻尼项。这里的关键是“期望角加速度”从哪来。一种做法是把外环输出的期望角速度差分，另一种做法是直接用轨迹规划模块给出的机体角加速度指令。为了演示，我使用前者：

Vector3f rate_sp = _rates_sp; Vector3f ang_accel_sp = (rate_sp - _rates_sp_prev) / dt; _rates_sp_prev = rate_sp; Vector3f accel_error = ang_accel_sp - _angular_accel_lp; _output_control.control_output += _param_aa_gain * accel_error;

注意，:前面是变量，它不是官方PX4代码，是我按1.12.3接口的风格总结出来的最小示例。在实际工程里，你需要把_rates_sp从vehicle_attitude_setpoint话题里拿，_angular_accel_lp按3个轴分别滤波，并且给增益再加一个限幅保护。

3.4 编译、跑仿真与数据对比

改完代码，重新编译：

make px4_sitl gazebo

由于我们只改了模块，增量编译一般几十秒内完成。仿真起来后，我用QGC发送一组姿态阶跃指令，同时用ULog记录角度和角速度。为了对比，我先在原版代码下跑一组基线数据，再开启角加速度前馈跑一组优化数据。

我这里放一组我实测的大致结果，不同机架和滤波参数会有差异，但趋势是一致的：

最直观的感受是，同样的阶跃指令下，优化后机头到达目标角度的动作更利落，几乎没有多余的来回摆动。角速度曲线也平滑了很多，说白了这个前馈就是在帮你的PID“预判未来”。

4. 调参经验与问题排查

4.1 滤波频率怎么选

角加速度信号的价值集中在中低频段，真正有用的带宽大概在1到20Hz之间。滤波截止频率设置太低了，角加速度信号被压得太平，前馈效果出不来；设置太高了，高频噪声全放进来，电机会开始高频抖动。

我的经验是分场景来：

• 纯仿真：噪声小，可以试50Hz到100Hz，输出还是很干净。
• 小型四旋翼：30Hz左右比较稳。
• 大型机架或者机架振动大的穿越机：建议从15Hz到20Hz起步，先稳定再追求响应。

滤波频率不是越大越好。见到噪声就降到10Hz，那是把前馈变成了延时反馈，甚至不如不加。最好用扫频测试听电机声音，找到临界点再退1/3。

4.2 数值微分噪声爆炸的排查

最典型的现象是，开启角加速度功能后，电机转速出现明显的高频波动，甚至QGC里姿态估计器报警。原因基本是时间戳抖动导致dt不准，或者低通滤波没生效。

排查顺序建议这样来：

1. 检查dt是否做了约束。不约束的话，调度线程偶尔卡一下就会产生一个巨大毛刺。
2. 确认低通滤波对象更新正确。很多人混淆了“对角速度滤波”和“对角加速度滤波”，顺序错了完全两码事。
3. 把原始微分数据和滤波后数据打印到ULog里，在QGC或Python里画出来，一眼就能看出噪声来自哪里。
4. 如果差分后噪声仍然很大，说明角速度源的采样率偏低，可以改采样时间戳平滑，或者对序列做三次采样线性插值。

4.3 仿真和真机效果差异

仿真里能跑通的东西，到了真机一定要重新调。Gazebo里的角速度模型相对干净，执行器响应也理想化，所以前馈增益可以给得比较大。真机上有螺旋桨气流扰动、机架共振、电调延迟，同样增益往往会让电机发热，甚至触发看门狗。

建议真机试飞时，把前馈增益从仿真值的1/3开始，逐步往上加。每次只加0.01到0.02，然后做小幅度阶跃和手推力矩测试。我个人的习惯是先在手里拿着飞机，很轻地加一点油门，感受有没有异常振动，再用绑绳固定测试，最后才敢放手飞。

4.4 快速问题定位表

5. 进阶玩法：角加速度+滑模控制的联合仿真

5.1 为什么滑模控制配角加速度合适

滑模控制天然适合四旋翼这一类非线性、参数不确定的系统，因为它有能力对匹配扰动做鲁棒抑制。传统滑模面一般取角度误差和角速度误差的组合，收敛速度虽然快，但会出现高频抖振。角加速度引入后，可以构造更高阶的滑模面，让控制器不只看到“误差在哪里”，还能看到“误差的加速度方向”。

一个简化的滑模控制律可以写成：

τ = J * α_des + ω × (J * ω) - λ * tanh(s / ε)

其中α_des是期望角加速度，s是滑模面。角加速度反馈直接参与λ项的调节，使系统在滑模面上快速滑动的同时，大幅削弱抖振。配合前面说的滑模观测器，控制器和观测器可以一起工作，抗风性能比传统PID要好。

5.2 Simulink+PX4联合仿真的最小配置

做联合仿真时，我的推荐是先用Simulink做离线验证，再把验证好的算法写回PX4，而不是直接在Simulink里实时驱动PX4，那样调试难度会高很多。

最小配置做法：

1. 在Simulink里搭四旋翼六自由度动力学模型，输入是电机推力/力矩，输出是姿态和角速度。
2. 加一个滑模观测器，输入角速度测量值，输出估计角加速度和扰动。
3. 加一个滑模控制器，输入期望角度、实际角度、角速度、估计角加速度，输出控制力矩。
4. 把PX4 SITL跑出来的角速度作为模型输入，或用实际飞行的ULog数据导入验证。
5. 算法效果确认后，在PX4中新建一个自定义模块，把Simulink生成的C代码移植进去，结合前面步骤里的角加速度数据流直接替换原始P-PID。

这里我不强推固定的联合仿真框架，因为MATLAB版本、PX4分支、通信方式组合太多了。重点是“先用离线数据验证，再回灌固件”这条路，无论怎么组合都不会走偏。

5.3 我的个人体会与建议

角加速度这个量，一开始挖出来的时候特别兴奋，以为自己发现了什么黑科技，结果第一次接进控制环，电机抖得跟筛子似的。后来慢慢摸清滤波、时间戳、前馈增益这几个变量，才真正体会到这东西的价值。它不会替你解决所有PID问题，但它能把响应曲线的“粘滞感”打掉一大截，尤其在大角度机动和阵风扰动时，体感变化非常明显。

我个人最看好的是把角加速度当作飞行动力学辨识的输入，这样可以进一步做自动调参、故障诊断、甚至单桨失效保护。如果你正好在折腾PX4二次开发，或者卡在仿真怎么连接、Simulink里怎么设计滑模控制这些环节，建议先把角加速度的数据流打通，后面很多事都会顺手很多。