基于STM32与FreeRTOS的无人机姿态控制与串级PID调优实战

1. 项目概述与核心设计思路

四轴无人机看似简单，但想要稳定飞行，背后的技术门道可不少。我自己折腾这个项目的时候，深刻体会到软硬件协同设计的重要性——光有好的传感器不够，还得有靠谱的算法和实时系统来支撑。这次我就以STM32F103C8T6为主控，搭配MPU6050传感器和FreeRTOS系统，带你一步步实现无人机的姿态控制和串级PID调优。

为什么选择这样的硬件组合？ STM32F103C8T6虽然不是什么高端芯片，但72MHz主频加上20KB RAM和64KB Flash，对于四轴飞行器来说完全够用。MPU6050更是个性价比神器，一百多块就能买到，集成了三轴加速度计和三轴陀螺仪，足够我们做姿态解算。实际用下来发现，MPU6050的原始数据噪音比较大，直接用它内置的DMP效果不太理想，所以我选择自己写四元数解算算法，精度反而更高。

软件架构的设计思路：我用了FreeRTOS来管理多个任务，这样可以把传感器数据采集、姿态解算、PID控制和通信这些任务分开，每个任务专注做一件事。实测下来，这种多任务设计比裸机轮询稳定多了，特别是在处理2.4G通信这种偶尔有延迟的任务时，不会影响到关键的姿态控制任务。

2. 传感器数据采集与处理实战

MPU6050的数据采集可不是简单的读寄存器就行，这里面的坑我都踩过一遍了。首先要配置采样率，根据香农采样定理，采样频率至少要高于信号最高频率的两倍。MPU6050的陀螺仪输出频率在开启DLPF后是1kHz，我把采样率设为500Hz，这样既能保证数据准确又不会给MCU太大压力。

零偏校准是重中之重。一开始我没做校准，无人机一上电就飘得厉害。后来加了校准流程：先判断无人机是否静止（30次采样内角速度变化小于5），然后计算各轴的偏差平均值。校准后的数据稳定多了，特别是Z轴加速度，理想值应该是16384（对应1g重力加速度），校准后误差可以控制在±50以内。

数据滤波是关键环节。加速度计噪音大，我用了一维卡尔曼滤波；角速度数据相对平滑，用一阶低通滤波就够了。这里有个小技巧：滤波系数要根据实际效果调整。一开始我把卡尔曼滤波的Q值设得太大，导致响应延迟，飞机变得很"迟钝"。后来反复试验，最终Q=0.001，R=0.543时效果最好。

// 卡尔曼滤波实现
void Com_Kalman_1(struct _1_ekf_filter *ekf, float input) {
    ekf->Now_P = ekf->LastP + ekf->Q;
    ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
    ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
    ekf->LastP = (1 - ekf->Kg) * ekf->Now_P;
}

3. 四元数姿态解算详解

刚开始我尝试用欧拉角做姿态解算，很快就遇到了万向节死锁问题。后来改用四元数，不仅解决了死锁问题，计算效率也更高了。四元数听起来很高大上，其实理解起来并不难——它就是用一个四维向量来表示旋转状态，比欧拉角更简洁高效。

实现四元数解算的关键步骤：首先要初始化四元数，我通常设为q0=1, q1=q2=q3=0，表示没有旋转。然后每次采样时，用陀螺仪的数据更新四元数：

// 四元数更新核心代码
float q0_temp = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
float q1_temp = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
float q2_temp = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
float q3_temp = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

更新完后一定要做归一化处理，否则数值会发散。最后把四元数转换成欧拉角，这样更直观：

// 转换为欧拉角
angle.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // 弧度转角度
angle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1*q1 - 2 * q2*q2 + 1) * 57.3;
angle.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3*q3 + 1) * 57.3;

在实际测试中，四元数解算的精度能达到0.5度以内，完全满足飞行控制的需求。

4. 串级PID控制算法实现

单级PID在四轴飞行器上效果不太理想，因为角度和角速度的响应特性完全不同。角度变化相对缓慢，而角速度变化很快。我用串级PID来解决这个问题：外环控制角度，内环控制角速度。这样设计的好处是响应更快，系统也更稳定。

外环PID负责角度控制。以俯仰角为例，期望角度来自遥控器输入，测量角度来自MPU6050解算。外环PID输出的是期望角速度，作为内环的输入。这里P系数不能太大，否则会超调；但也不能太小，否则响应太慢。我调试时发现KP=2.5效果不错。

内环PID负责角速度控制。期望值来自外环输出，测量值直接来自陀螺仪。内环的P系数可以设得大一些，让系统快速响应。我用的KP=0.8，KI=0.02，KD=0.01。D项能抑制 overshoot，但太大会引入噪声。

// 串级PID计算
void Com_PID_Cascade(sPID *outer, sPID *inner, float dt) {
    Com_PID_Update(outer, dt);    // 计算外环
    inner->expect = outer->out;   // 外环输出作为内环期望
    Com_PID_Update(inner, dt);    // 计算内环
}

参数整定经验分享：先调内环再调外环，先调P再调I和D。内环调试时把飞机拿在手上，轻轻晃动，观察电机响应是否及时。外环调试时让飞机实际飞行，观察姿态稳定性。记得每次只调一个参数，小幅度调整，记录下效果。

5. FreeRTOS多任务设计与优化

在FreeRTOS中，我把不同优先级的任务合理安排，确保关键任务及时执行。姿态控制任务优先级最高（9），每2ms执行一次；通信任务也是优先级9，但每4ms一次；辅助任务优先级6，50ms一次；电源管理任务优先级10，10秒一次。

任务间通信通过全局变量实现，但要注意数据保护。我在访问共享数据时用了临界区保护：

taskENTER_CRITICAL();
App_Flight_GetMpuData(); // 读取传感器数据
taskEXIT_CRITICAL();

内存管理也很重要。每个任务的堆栈大小要留足余量，我给姿态控制任务分配了128字，实际使用约80字，这样即使有堆栈波动也不会溢出。FreeRTOS的堆大小设为4096字，足够所有任务使用。

任务调度策略：我用了时间片轮转调度，相同优先级的任务公平分享CPU时间。但姿态控制任务用了vTaskDelayUntil来保证精确的2ms周期，这是飞行控制的关键。

6. 无线通信与遥控器实现

无线通信用的Si24R1模块（兼容NRF24L01），性价比很高。通信距离实测能达到50米左右，完全满足小型无人机的需求。数据格式设计很关键：我用了帧头（0xAA 0xAF）+功能字+长度字+数据+校验和的格式，确保数据传输可靠。

遥控器端的摇杆处理：ADC采集到的原始数据是0-4095，需要转换成1000-2000的范围，这是航模标准。还要做死区处理，摇杆在中位时输出1500，避免因微小抖动导致飞机漂移。

// 摇杆数据处理
remote.THR = 2000 - 0.25 * adc_values[0]; // 转换范围
remote.THR = LIMIT(remote.THR, 1000, 2000); // 限幅

滑动窗口滤波：对油门值做滤波处理，防止突变。我用的是8点滑动窗口滤波，效果很好：

void App_Remote_WindowFilter(void) {
    thrWindow.windowSum -= thrWindow.windowArray[index];
    thrWindow.windowArray[index] = remote.THR;
    thrWindow.windowSum += thrWindow.windowArray[index];
    remote.THR = thrWindow.windowSum / WINDOW_SIZE;
}

失控保护：这是必须的！我设计了30秒无信号保护，失联后会自动降低油门并回中姿态。还有低电压保护，电池电压低于3.5V时会闪灯报警，低于3.3V会自动降落。

7. PID参数整定实战案例

调参是个耐心活，我花了整整两个周末才调出满意的参数。首先调内环角速度PID，因为内环是基础。把飞机拿在手上，轻轻晃动，听电机声音变化。如果响应迟钝就加大P，如果振荡就加大D。

俯仰轴调参过程：最初P=0.5，飞机反应迟钝。逐步增加到2.0，响应快了但有超调。加入D=0.5，超调抑制了。最后加I=0.01消除静差。最终参数：KP=2.0，KI=0.01，KD=0.5。

横滚轴调参：与俯仰轴类似，但由于电机布局不同，参数略有差异。最终KP=2.2，KI=0.01，KD=0.6。

偏航轴调参：比较难调，因为偏航响应较慢。KP=1.5，KI=0.02，KD=0.2。I值稍大些是为了消除静差。

高度控制PID：外环高度PID的P不能太大，否则会振荡。KP=0.8，KI=0.001，KD=0.1。内环速度PID的P可以大些：KP=1.2，KI=0.005，KD=0.3。

调参时最好用匿名上位机观察数据曲线，这样更直观。记得改变参数后要飞起来试，手持测试和实际飞行效果还是有差别的。

8. 系统安全与稳定性优化

无人机的安全性绝对不能马虎。我设计了多重保护机制：上电后需要特定解锁序列才能启动电机（油门最低->油门最高保持1秒->油门最低）。这样避免了意外启动。

电池监测：通过ADC检测电池电压，低于3.5V时LED闪烁报警，低于3.3V时自动降落。这个功能很实用，避免了过放损坏电池。

软件看门狗：FreeRTOS的看门狗任务监控其他任务是否卡死。如果有任务超过预期时间没有运行，看门狗会重启系统。

电机输出限制：总油门不超过900（最大1000），留出100给PID调整用。这样即使PID计算输出很大，也不会让电机过载。

// 电机输出限制
moto1 = LIMIT(moto1, 0, 1000);
moto2 = LIMIT(moto2, 0, 1000);
moto3 = LIMIT(moto3, 0, 1000);
moto4 = LIMIT(moto4, 0, 1000);

姿态容错处理：当检测到异常姿态（角度大于45度）时，会忽略遥控器输入，优先恢复平衡。这是为了避免新手操作失误导致炸机。

9. 常见问题与调试技巧

电机振动问题：如果电机振动大，会影响传感器读数。我在电机座加了橡胶减震垫，效果很好。软件上也可以加大滤波系数。

飞行漂移问题：通常是加速度校准不准或PID参数不合适。先检查校准数据，再调整PID的I值。

响应迟钝问题：加大P值，减小滤波系数。但要注意平衡，过大的P值会导致振荡。

电池电压影响：电池电压下降时，电机转速会变慢，导致控制效果变化。可以考虑根据电压动态调整PID参数。

调试建议：

• 先用USB线连接电脑，用串口打印数据调试
• 调参时一次只调一个参数，小幅度调整
• 做好数据记录，记下每次参数变化的效果
• 安全第一，调试时螺旋桨可以暂时取下

工具推荐：

• 匿名上位机：观察数据曲线
• VOFA+：自定义数据协议监控
• ST-Link V2：调试和下载程序
• 万用表：检查电源和信号

整个项目做下来，最深体会是调试占用了70%的时间。但看到无人机稳稳悬停的那一刻，所有的努力都值了。最重要的是耐心，不要指望一次就能调好，多次迭代才能找到最佳参数。