四旋翼无人机线性与非线性MPC控制对比与实践

1. 四旋翼控制问题概述

四旋翼飞行器的轨迹跟踪控制一直是无人机控制领域的关键技术难题。作为一名从事飞行控制算法开发多年的工程师，我经常需要在精确跟踪和实时响应之间寻找平衡点。模型预测控制（MPC）因其优秀的约束处理能力和预测特性，成为解决这一问题的理想选择。

在实际工程应用中，我们通常会面临两种MPC方案的选择：基于线性模型的MPC和基于非线性模型的MPC。前者计算效率高但精度有限，后者控制精度高但计算复杂。本文将基于我参与的多个四旋翼控制项目经验，详细解析这两种方法的实现细节和性能差异。

2. 四旋翼建模基础

2.1 非线性动力学模型构建

四旋翼的完整动力学模型需要考虑六个自由度的运动特性。在我的实际项目中，通常采用牛顿-欧拉方程建立模型。以机体坐标系为例，关键的动力学方程包括：

平移运动：

math复制m\begin{bmatrix}
\ddot{x}\\ 
\ddot{y}\\ 
\ddot{z}
\end{bmatrix} = 
\begin{bmatrix}
0\\ 
0\\ 
-mg
\end{bmatrix} + 
R(\phi,\theta,\psi)
\begin{bmatrix}
0\\ 
0\\ 
T
\end{bmatrix}

旋转运动：

math复制I\begin{bmatrix}
\ddot{\phi}\\ 
\ddot{\theta}\\ 
\ddot{\psi}
\end{bmatrix} = 
\begin{bmatrix}
\tau_\phi\\ 
\tau_\theta\\ 
\tau_\psi
\end{bmatrix} - 
\begin{bmatrix}
\dot{\theta}\dot{\psi}(I_z-I_y)\\ 
\dot{\phi}\dot{\psi}(I_x-I_z)\\ 
\dot{\phi}\dot{\theta}(I_y-I_x)
\end{bmatrix}

其中R为旋转矩阵，将机体坐标系转换到惯性坐标系。这个模型完整描述了四旋翼的非线性特性，但直接用于控制设计会导致计算量过大。

实际建模经验：在项目实践中，我发现忽略某些次要耦合项（如陀螺效应）可以简化模型而不显著影响控制性能，这对实时性要求高的应用特别重要。

2.2 线性化模型推导

为了获得线性模型，我们通常在悬停状态附近进行泰勒展开。假设姿态角较小（sinθ≈θ，cosθ≈1），可以得到简化的线性模型：

状态空间形式：

math复制\dot{x} = Ax + Bu

其中状态向量x通常包含位置、速度、姿态角和角速度，控制输入u为四个电机的转速。具体矩阵形式为：

math复制A = \begin{bmatrix}
0 & I_3 & 0 & 0\\
0 & 0 & g\cdot S & 0\\
0 & 0 & 0 & I_3\\
0 & 0 & 0 & 0
\end{bmatrix}, \quad
B = \begin{bmatrix}
0\\ 
0\\ 
0\\ 
I_3
\end{bmatrix}

这里S是姿态角的斜对称矩阵。这个线性模型虽然简化了物理关系，但在小角度范围内能很好地近似系统行为。

3. MPC控制器设计

3.1 线性MPC实现

线性MPC的核心是求解如下优化问题：

math复制\min_U \sum_{k=0}^{N_p-1} (x_k^T Q x_k + u_k^T R u_k) + x_N^T P x_N

math复制\text{s.t.} \quad x_{k+1} = Ax_k + Bu_k

在实际项目中，我通常使用以下参数配置：

• 预测时域N_p：15-20步
• 控制时域N_c：5-8步
• 状态权重Q：对角矩阵，位置误差权重>速度误差权重
• 控制权重R：根据电机特性调整

Python实现示例（使用CVXPY）：

python复制def solve_mpc(A, B, x0, Q, R, N):
    nx, nu = B.shape
    x = cvxpy.Variable((nx, N+1))
    u = cvxpy.Variable((nu, N))
    
    cost = 0
    constraints = [x[:,0] == x0]
    
    for t in range(N):
        cost += cvxpy.quad_form(x[:,t], Q) + cvxpy.quad_form(u[:,t], R)
        constraints += [x[:,t+1] == A @ x[:,t] + B @ u[:,t]]
    
    prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
    prob.solve(solver=cvxpy.OSQP)
    
    return u[:,0].value

3.2 非线性MPC实现

非线性MPC的优化问题形式类似，但使用完整非线性模型：

math复制x_{k+1} = f(x_k, u_k)

在项目中我常用CasADi框架实现：

python复制def setup_nmpc():
    opti = casadi.Opti()
    
    # 定义决策变量
    X = opti.variable(nx, N+1)
    U = opti.variable(nu, N)
    
    # 定义代价函数
    J = 0
    for k in range(N):
        J += X[:,k].T @ Q @ X[:,k] + U[:,k].T @ R @ U[:,k]
    
    # 添加动力学约束
    for k in range(N):
        opti.subject_to(X[:,k+1] == f(X[:,k], U[:,k]))
    
    # 求解器配置
    opts = {'ipopt.print_level':0, 'print_time':0}
    opti.solver('ipopt', opts)
    
    return opti, X, U

计算效率提示：非线性MPC的求解时间通常是线性MPC的10-100倍，在实际项目中需要仔细权衡精度和实时性要求。

4. 仿真对比分析

4.1 圆形轨迹跟踪测试

在相同仿真条件下（半径5m，速度2m/s），两种控制器的表现：

4.2 阶跃响应测试

对z轴高度指令的响应对比：

5. 工程实践建议

根据多个项目的实施经验，我总结出以下选择原则：

1. 线性MPC适用场景：

• 计算资源有限（如嵌入式飞控）
• 飞行包线内姿态变化小（<15°）
• 对实时性要求极高（>100Hz）

2. 非线性MPC适用场景：

• 高性能计算平台可用（如机载计算机）
• 需要大机动飞行（如特技飞行）
• 对跟踪精度要求极高（<0.1m误差）

实际项目中的折中方案：

• 使用线性MPC作为主控制器
• 在关键机动阶段切换至非线性MPC
• 采用事件触发机制降低计算负荷

6. 常见问题排查

问题1：MPC求解不收敛

• 检查预测时域是否过短
• 验证系统模型的可控性
• 调整权重矩阵的相对大小

问题2：实际飞行出现振荡

• 检查MPC采样时间是否匹配系统动态
• 验证状态估计的延迟和噪声
• 考虑添加输出速率约束

问题3：计算时间过长

• 尝试减少预测时域
• 使用condensing技术减少变量
• 考虑显式MPC或近似方法

在最近的一个工业检测无人机项目中，我们最终采用了混合架构：平飞阶段使用线性MPC（100Hz），检测目标时切换至非线性MPC（20Hz）。这种方案在保持精度的同时将平均CPU使用率控制在65%以下。