1. 项目背景与核心需求
四旋翼飞行器的轨迹跟踪控制一直是无人机控制领域的热点问题。传统PID控制器在简单直线轨迹跟踪中表现尚可，但当遇到复杂曲线轨迹（特别是圆形这种需要持续改变姿态的路径）时，往往会出现超调量大、响应滞后等问题。而模型预测控制（MPC）凭借其滚动优化和反馈校正的特性，特别适合处理这类具有动态约束的非线性控制问题。
这个项目的核心目标，就是设计一个能够精确跟踪圆形轨迹的MPC控制器。与常规的轨迹跟踪不同，圆形轨迹对控制器的要求更高——它需要飞行器在保持恒定高度的同时，持续调整横滚和俯仰角来维持圆周运动。这涉及到对四旋翼动力学模型的精确建模、状态变量的合理选择，以及控制时域的优化策略。
2. 系统建模与问题描述
2.1 四旋翼动力学模型
四旋翼的动力学模型通常分为位置动力学和姿态动力学两部分。在惯性坐标系下，位置动力学可以表示为：
$$
\begin{cases}
\ddot{x} = (\sin\psi\sin\phi + \cos\psi\sin\theta\cos\phi)\frac{U_1}{m} \
\ddot{y} = (-\cos\psi\sin\phi + \sin\psi\sin\theta\cos\phi)\frac{U_1}{m} \
\ddot{z} = -g + (\cos\theta\cos\phi)\frac{U_1}{m}
\end{cases}
$$
其中$(x,y,z)$是飞行器在惯性系中的位置，$(\phi,\theta,\psi)$分别是横滚、俯仰和偏航角，$U_1$是总升力，$m$为飞行器质量，$g$为重力加速度。
姿态动力学则可以用欧拉方程描述：
$$
\begin{cases}
\ddot{\phi} = \dot{\theta}\dot{\psi}\left(\frac{I_y-I_z}{I_x}\right) + \frac{U_2}{I_x} \
\ddot{\theta} = \dot{\phi}\dot{\psi}\left(\frac{I_z-I_x}{I_y}\right) + \frac{U_3}{I_y} \
\ddot{\psi} = \dot{\phi}\dot{\theta}\left(\frac{I_x-I_y}{I_z}\right) + \frac{U_4}{I_z}
\end{cases}
$$
$I_x,I_y,I_z$分别是绕三个轴的转动惯量，$U_2,U_3,U_4$是三个方向的力矩。
2.2 圆形轨迹参数化
目标圆形轨迹可以用以下参数方程表示：
$$
\begin{cases}
x_{ref}(t) = R\cos(\omega t + \phi_0) + x_c \
y_{ref}(t) = R\sin(\omega t + \phi_0) + y_c \
z_{ref}(t) = h
\end{cases}
$$
其中$R$是圆半径，$\omega$是角速度，$(x_c,y_c)$是圆心坐标，$h$是恒定高度，$\phi_0$是初始相位角。
3. MPC控制器设计
3.1 状态空间模型离散化
为了应用MPC，首先需要将连续时间模型离散化。选择状态向量为：
$$
\mathbf{x} = [x,\dot{x},y,\dot{y},z,\dot{z},\phi,\dot{\phi},\theta,\dot{\theta},\psi,\dot{\psi}]^T
$$
控制输入为：
$$
\mathbf{u} = [U_1,U_2,U_3,U_4]^T
$$
使用一阶欧拉方法离散化，得到：
$$
\mathbf{x}_{k+1} = A_k\mathbf{x}_k + B_k\mathbf{u}_k
$$
其中$A_k$和$B_k$是通过在采样点线性化得到的系统矩阵。
3.2 优化问题构建
在每个时间步$k$，MPC求解如下优化问题：
$$
\min_{\mathbf{u}{k|k},...,\mathbf{u}{k+N-1|k}} \sum_{i=0}^{N-1} |\mathbf{x}{k+i|k} - \mathbf{x}|Q^2 + |\mathbf{u}|_R^2
$$
满足：
$$
\begin{cases}
\mathbf{x}{k+i+1|k} = f(\mathbf{x},\mathbf{u}{k+i|k}) \
\mathbf{u} \leq \mathbf{u}{k+i|k} \leq \mathbf{u} \
\mathbf{x}{min} \leq \mathbf{x} \leq \mathbf{x}_{max}
\end{cases}
$$
其中$N$是预测时域，$Q$和$R$是权重矩阵，$f(\cdot)$是离散化的系统动态。
3.3 实时线性化与QP求解
由于四旋翼模型本质上是非线性的，我们采用实时线性化策略：
- 在当前状态$\mathbf{x}_k$处线性化系统模型
- 将非线性优化问题转化为二次规划(QP)问题
- 使用QP求解器(如qpOASES)求解最优控制序列
- 仅应用第一个控制输入$\mathbf{u}_{k|k}^*$
- 下一时刻重复上述过程
4. 实现细节与参数调优
4.1 权重矩阵选择
权重矩阵$Q$和$R$的选择对控制性能至关重要。经过多次实验，我们采用以下设置：
$$
Q = \text{diag}([10,1,10,1,5,0.1,0.5,0.1,0.5,0.1,0.2,0.05])
$$
$$
R = \text{diag}([0.1,0.05,0.05,0.05])
$$
这种设置强调对位置跟踪的精度，同时适当放宽对姿态角速度的要求。
4.2 预测时域与采样时间
预测时域$N$和采样时间$T_s$的选择需要权衡计算复杂度和控制性能：
- 采样时间$T_s = 0.05s$ (20Hz)
- 预测时域$N = 20$ (对应1秒的预测范围)
这种配置在普通计算硬件上可以实现实时控制(计算时间<$T_s$)。
4.3 约束处理
考虑到四旋翼的物理限制，设置以下约束：
- 最大倾斜角：$|\phi|,|\theta| \leq 30^\circ$
- 最大高度变化：$|z-h| \leq 0.5m$
- 电机推力限制：$0 \leq U_1 \leq 2mg$
- 力矩限制：$|U_2|,|U_3| \leq 0.5mgL$, $|U_4| \leq 0.2mgL$
其中$L$是电机到重心的距离。
5. 仿真与实验结果
5.1 仿真环境设置
使用ROS/Gazebo搭建仿真环境，主要参数：
- 飞行器质量$m=1.2kg$
- 转动惯量$I_x=I_y=0.034kg\cdot m^2$, $I_z=0.065kg\cdot m^2$
- 圆轨迹半径$R=3m$，高度$h=2m$
- 角速度$\omega=0.5rad/s$ (周期约12.57秒)
5.2 跟踪性能指标
定义以下性能指标：
- 位置误差：$e_{pos} = \sqrt{(x-x_{ref})^2 + (y-y_{ref})^2}$
- 高度误差：$e_z = |z-h|$
- 姿态角误差：$e_{att} = \sqrt{\phi^2 + \theta^2}$
5.3 结果分析
在仿真中，MPC控制器表现出色：
- 平均位置误差：0.12m
- 最大位置误差：0.25m (出现在轨迹起始阶段)
- 高度保持误差：<0.05m
- 姿态角误差：<5°
相比之下，传统PID控制器在相同条件下的平均位置误差达到0.35m，且在轨迹转折处会出现明显的超调。
6. 实际部署中的挑战与解决方案
6.1 计算延迟问题
在实际硬件上，MPC的计算延迟可能影响控制性能。我们采用以下策略缓解：
- 使用更高效的QP求解器(如OSQP)
- 减少预测时域$N$到15
- 在计算未完成时使用上一时刻的控制输入
6.2 模型失配补偿
实际飞行器的动力学参数可能与模型存在差异。我们通过：
- 在线参数估计(使用扩展卡尔曼滤波)
- 在MPC中增加误差积分项
- 定期更新线性化点
6.3 风扰抑制
室外环境中风扰是主要挑战。我们的解决方案：
- 在状态估计中融合风速估计
- 在MPC模型中增加风扰项
- 适当增大权重矩阵$Q$的位置相关项
7. 进阶优化方向
7.1 非线性MPC实现
当前方案依赖于实时线性化，可以考虑：
- 直接非线性MPC(NMPC)框架
- 使用SQP(序列二次规划)方法
- 利用C/GMRES等快速求解算法
7.2 学习增强MPC
结合机器学习方法：
- 使用神经网络学习模型误差
- 强化学习优化MPC参数
- 数据驱动预测模型
7.3 多飞行器编队扩展
将单机控制扩展到编队控制：
- 分布式MPC架构
- 碰撞避免约束
- 通信延迟补偿
8. 关键代码实现片段
8.1 MPC问题构建
python复制def build_mpc_problem():
# 系统矩阵
A = compute_linearized_model()
B = compute_input_matrix()
# 构建QP矩阵
P = build_quadratic_cost(Q, R, N)
q = build_linear_cost(x_ref, Q)
# 构建约束
G, h = build_inequality_constraints(x_lim, u_lim)
A_eq, b_eq = build_equality_constraints(A, B, x0, N)
return P, q, G, h, A_eq, b_eq
8.2 实时控制循环
python复制while not rospy.is_shutdown():
# 获取当前状态
x = get_current_state()
# 更新参考轨迹
x_ref = generate_circular_trajectory(t)
# 构建并求解QP
P, q, G, h, A_eq, b_eq = build_mpc_problem(x, x_ref)
u_opt = solve_qp(P, q, G, h, A_eq, b_eq)
# 应用控制
apply_control(u_opt[0])
# 等待下一周期
rate.sleep()
t += dt
9. 性能优化技巧
-
稀疏矩阵利用：QP问题的Hessian矩阵通常是稀疏的，使用稀疏矩阵存储和运算可以显著提高效率。
-
热启动：利用上一时刻的解作为当前优化的初始猜测，可以减少迭代次数。
-
并行计算：将矩阵构建和QP求解分配到不同线程，充分利用多核处理器。
-
代码生成：使用CVXGEN等工具生成特定问题的优化代码，避免运行时解释开销。
-
降低精度要求：在满足控制需求的前提下，适当降低QP求解的精度要求以加快计算。
10. 常见问题排查
10.1 控制器发散
可能原因：
- 模型参数不准确
- 采样时间过长
- 约束设置不合理
解决方案：
- 重新校准系统参数
- 减小采样时间
- 检查约束可行性
10.2 计算超时
可能原因：
- 预测时域过长
- QP求解器效率低
- 处理器性能不足
解决方案：
- 减少预测时域$N$
- 更换更高效的求解器
- 优化代码或升级硬件
10.3 轨迹跟踪振荡
可能原因：
- 权重矩阵选择不当
- 存在未建模动态
- 传感器噪声过大
解决方案：
- 调整$Q$和$R$权重
- 增加模型复杂度
- 改进状态估计
在实际部署中，我发现MPC控制器的性能很大程度上依赖于模型的准确性。即使是很小的参数误差，经过预测时域的累积也会导致明显的跟踪偏差。因此，定期校准和在线参数估计是保证长期稳定运行的关键。另一个重要经验是：在调试阶段，可以先用较短的预测时域和较大的采样时间，待基本功能验证后再逐步提高要求，这样能显著减少调试难度。