控制大师

概述

高级控制算法是无人机性能优化的核心。在本项目中，你将学习如何实现 LQR 和 MPC 控制算法，提升无人机的飞行稳定性和响应速度。

你将学到什么

• LQR（线性二次调节器）原理
• MPC（模型预测控制）原理
• 系统建模与辨识
• 控制器设计与实现

所需材料

物品	数量	说明
已组装好的无人机	1	-
电脑	1	安装 VS Code + ESP-IDF 环境
USB 数据线	1	用于编程
MATLAB 或 Python	1	用于算法设计和仿真

步骤 1：了解高级控制算法

LQR（线性二次调节器）

通过最小化二次代价函数，设计最优控制器。

MPC（模型预测控制）

基于系统模型，预测未来状态并优化控制序列。

步骤 2：系统建模

1. 建立无人机的线性化模型
2. 包括姿态动力学和位置动力学
3. 使用 MATLAB 或 Python 进行系统辨识和模型验证

步骤 3：算法设计

使用 MATLAB 的 lqr() 函数或 Python 的 control 库设计 LQR 控制器：

from control import lqr

# 系统矩阵
A = [[...]]  # 状态矩阵
B = [[...]]  # 输入矩阵
Q = [[...]]  # 状态权重矩阵
R = [[...]]  # 控制权重矩阵

# 计算 LQR 增益
K, S, E = lqr(A, B, Q, R)

步骤 4：代码实现

在 components/core/crazyflie/modules/src/controller 目录下创建新的控制器文件：

controller_lqr.c

void controllerLQR(control_t *control, setpoint_t *setpoint, const state_t *state) {
    // 计算状态误差
    float error[12];
    error[0] = setpoint->position.x - state->position.x;
    error[1] = setpoint->position.y - state->position.y;
    error[2] = setpoint->position.z - state->position.z;
    // ... 其他状态误差

    // LQR 控制律: u = -K * error
    float control_output[4];
    for (int i = 0; i < 4; i++) {
        control_output[i] = 0;
        for (int j = 0; j < 12; j++) {
            control_output[i] -= K[i][j] * error[j];
        }
    }

    // 应用控制输出
    control->thrust = control_output[0];
    control->roll = control_output[1];
    control->pitch = control_output[2];
    control->yaw = control_output[3];
}

步骤 5：编译烧录并测试

1. 编译烧录代码
2. 飞行测试，比较高级控制器与 PID 控制器的性能差异

故障排除

控制器不稳定

• 检查系统模型是否准确
• 调整权重矩阵 Q 和 R

响应速度慢

• 增大状态权重 Q
• 减小控制权重 R

成就感

恭喜你！你已经实现了高级控制算法，这是无人机控制的核心技术！

下一步

在下一个项目中，你将学习如何开发多传感器数据融合系统。