1. 四旋翼飞行器MPC控制的核心挑战

四旋翼飞行器的多目标航点导航控制是一个典型的非线性系统控制问题。与传统PID控制相比，MPC算法需要解决三个关键挑战：

1. 强耦合动力学特性：四旋翼的6自由度运动（x/y/z位置和roll/pitch/yaw姿态）通过4个电机转速控制，存在严重的输入输出耦合。我在实际飞控开发中发现，俯仰角（pitch）变化会同时影响x和z轴运动，这种耦合关系必须在预测模型中准确表达。

2. 实时性要求：MPC需要在每个控制周期（通常20-50ms）内完成优化计算。当预测时域N=20时，即使线性化模型也需要求解包含120个变量的优化问题。实测表明，在树莓派4B上运行标准QP求解器需要约15ms，留给其他任务的时间所剩无几。

3. 约束处理复杂性：飞行中需要同时满足：

   • 输入约束（电机转速在[0, max_rpm]）
   • 状态约束（姿态角不超过±30°）
   • 环境约束（避障安全距离）

2. 动力学建模的关键细节

2.1 坐标系定义

采用右手坐标系：

• 惯性系$O_I$：东北天坐标系（ENU），X轴指向东，Y轴指向北，Z轴垂直向上
• 机体系$O_B$：X轴指向机头，Y轴指向右侧，Z轴垂直向下

2.2 刚体动力学方程

通过牛顿-欧拉方程推导得到：

平移动力学：
$$
m\ddot{\boldsymbol{p}} = m\boldsymbol{g} + \boldsymbol{R}_B^I \boldsymbol{F}_B
$$
其中$\boldsymbol{R}_B^I$是从机体系到惯性系的旋转矩阵，升力$\boldsymbol{F}B = [0,0,T]^T$，$T = k_t\sum^4 \omega_i^2$

旋转动力学：
$$
\boldsymbol{J}\dot{\boldsymbol{\omega}} + \boldsymbol{\omega}\times\boldsymbol{J}\boldsymbol{\omega} = \boldsymbol{\tau}
$$
其中$\boldsymbol{\tau}$是机体力矩，与电机转速平方成正比：
$$
\begin{bmatrix}
\tau_x \
\tau_y \
\tau_z
\end

\begin{bmatrix}
lk_t(\omega_4^2-\omega_2^2) \
lk_t(\omega_3^2-\omega_1^2) \
k_d(\omega_1^2-\omega_2^2+\omega_3^2-\omega_4^2)
\end{bmatrix}
$$

关键参数经验值：

• 升力系数$k_t$：1.5e-5 N·s²
• 阻力系数$k_d$：3e-7 N·m·s²
• 电机到重心距离$l$：0.2 m

3. MPC实现的核心技术栈

3.1 模型离散化

采用前向欧拉法离散化，采样时间$T_s=0.02s$：
$$
\boldsymbol{x}_{k+1} = \boldsymbol{x}_k + T_s f(\boldsymbol{x}_k, \boldsymbol{u}_k)
$$
状态向量$\boldsymbol{x} = [p_x,v_x,p_y,v_y,p_z,v_z,\phi,\theta,\psi,\omega_x,\omega_y,\omega_z]^T$

3.2 代价函数设计

采用二次型代价函数：
$$
J = \sum_{k=0}^{N-1} (\boldsymbol{x}_k^T\boldsymbol{Q}\boldsymbol{x}_k + \boldsymbol{u}_k^T\boldsymbol{R}\boldsymbol{u}_k) + \boldsymbol{x}_N^T\boldsymbol{Q}_f\boldsymbol{x}_N
$$

权重矩阵经验值：

matlab复制Q = diag([1e4, 1e2, 1e4, 1e2, 1e4, 1e2, 1e3, 1e3, 1e2, 1e1, 1e1, 1e1]);
R = diag([1e2, 1e2, 1e2]);

3.3 约束处理技巧

1. 输入约束：将电机转速约束转化为PWM信号范围

   matlab复制u_min = [0; 0; 0; 0];  % 最小PWM占空比
   u_max = [1; 1; 1; 1];  % 最大PWM占空比
   

2. 状态约束：通过松弛变量处理姿态角约束

   matlab复制A_roll = [0 0 0 0 0 0 1 0 0 0 0 0;
             0 0 0 0 0 0 -1 0 0 0 0 0];
   b_roll = [30*pi/180; 30*pi/180];  % ±30度约束
   

4. 航点管理策略优化

4.1 航点切换逻辑

采用距离-速度双条件判断：

matlab复制function [target_reached] = check_waypoint_reached(pos, waypoint)
    dist_threshold = 0.2;  % 距离阈值[m]
    vel_threshold = 0.1;   % 速度阈值[m/s]
    
    dist = norm(pos(1:3) - waypoint(1:3));
    vel = norm(pos(4:6));
    
    target_reached = (dist < dist_threshold) && (vel < vel_threshold);
end

4.2 参考轨迹生成

采用三次多项式插值：
$$
\boldsymbol{p}_{ref}(t) = \boldsymbol{a}_0 + \boldsymbol{a}_1 t + \boldsymbol{a}_2 t^2 + \boldsymbol{a}_3 t^3
$$
系数通过边界条件求解：

matlab复制A = [1 t0 t0^2 t0^3;
     0 1 2*t0 3*t0^2;
     1 tf tf^2 tf^3;
     0 1 2*tf 3*tf^2];
b = [p0; v0; pf; vf];
a = A\b;

5. 实际调试中的关键发现

1. 预测时域选择：通过实测发现当时域N=15（对应0.3s）时，在计算耗时和控制性能间取得最佳平衡。N<10会导致响应迟缓，N>20则引起振荡。

2. 权重调整技巧：

   • 增大Q中位置权重可提高跟踪精度，但会导致控制量突变
   • 降低姿态角权重可增强抗风性，但会牺牲位置精度
   • 实测建议初始比例：位置:速度:姿态 = 100:10:1

3. 数值稳定性处理：

   • 对QP求解器添加正则化项：$J_{reg} = \epsilon||\boldsymbol{u}||^2$，$\epsilon=1e-6$
   • 采用warm-start策略，用上一周期解作为初始猜测

6. 完整MATLAB实现框架

matlab复制%% 主控制循环
for k = 1:sim_steps
    % 1. 获取当前状态
    x_current = get_sensor_data();
    
    % 2. 航点状态更新
    if check_waypoint_reached(x_current, waypoints(current_wpt,:))
        current_wpt = current_wpt + 1;
        ref_traj = generate_trajectory(x_current, waypoints(current_wpt,:));
    end
    
    % 3. MPC求解
    [u_opt, x_pred] = solve_mpc(x_current, ref_traj, model_params);
    
    % 4. 执行控制
    apply_control(u_opt(1,:));
    
    % 5. 数据记录
    log_data(x_current, u_opt);
end

%% MPC求解函数
function [u_opt, x_pred] = solve_mpc(x0, ref, params)
    % 构造优化问题
    prob = struct();
    prob.H = blkdiag(kron(eye(params.N), params.R), params.Qf);
    prob.f = zeros(params.N*params.nu + params.nx, 1);
    
    % 构造约束矩阵
    A_eq = build_equality_constraints(params);
    A_ineq = build_inequality_constraints(params);
    
    % 求解QP
    options = optimoptions('quadprog', 'Display', 'off');
    sol = quadprog(prob.H, prob.f, A_ineq.A, A_ineq.b, ...
                   A_eq.A, A_eq.b, [], [], [], options);
    
    % 提取结果
    u_opt = reshape(sol(1:params.N*params.nu), params.nu, params.N);
    x_pred = reshape(sol(params.N*params.nu+1:end), params.nx, params.N+1);
end

7. 性能优化实战技巧

1. 代码加速：

   • 使用Coder将关键函数转换为MEX文件
   • 对QP求解采用Hot-QP算法，实测速度提升3倍

   matlab复制codegen solve_mpc -args {x0, ref, params}
   

2. 内存预分配：

   matlab复制% 预分配记录数组
   log.x = zeros(nx, sim_steps);
   log.u = zeros(nu, sim_steps);
   

3. 并行计算：

   matlab复制parfor i = 1:num_tests
       results(i) = run_simulation(scenarios(i));
   end
   

8. 典型问题排查指南

9. 进阶改进方向

1. 非线性MPC：采用CasADi框架实现

   matlab复制import casadi.*
   opti = casadi.Opti();
   x = opti.variable(nx, N+1);
   u = opti.variable(nu, N);
   

2. 鲁棒MPC：考虑风扰模型

   matlab复制% 风扰作为有界扰动
   w = opti.variable(3, N);
   opti.subject_to(norm(w,2) <= w_max);
   

3. 学习型MPC：结合神经网络拟合残差

   matlab复制% 使用NN预测模型误差
   delta_f = predict_nn(x, u);
   x_next = f(x,u) + delta_f;