1. 项目背景与核心需求
四旋翼飞行器的轨迹跟踪控制一直是无人机控制领域的热点问题。传统PID控制器在简单直线轨迹跟踪时表现尚可,但遇到复杂曲线轨迹(特别是圆形这种需要连续变化控制输入的轨迹)时,往往会出现超调量大、响应滞后等问题。而模型预测控制(MPC)因其能够处理多输入多输出系统、显式考虑约束条件以及优化未来一段时间内的系统行为等优势,成为解决这类问题的理想选择。
这个项目的核心在于设计一个能够精确跟踪圆形轨迹的MPC控制器。圆形轨迹看似简单,实则对控制器提出了几个关键挑战:需要同时控制四个旋翼的转速来维持飞行器在三维空间中的姿态和位置;需要考虑空气动力学效应带来的非线性;还需要处理传感器噪声和执行器延迟等实际问题。MPC控制器需要在这些约束条件下,实时计算出最优控制输入,使飞行器能够平滑、精确地沿着预定圆形轨迹飞行。
2. MPC控制理论基础
2.1 MPC基本原理
模型预测控制的核心思想可以概括为"滚动优化"加"反馈校正"。具体来说,在每个控制周期内:
- 基于当前系统状态和模型,预测未来一段时间(预测时域)内的系统行为
- 求解一个优化问题,得到使预测输出尽可能接近期望轨迹的控制序列
- 只实施控制序列中的第一个控制输入
- 下一个周期重复上述过程,实现滚动优化
这种控制策略特别适合四旋翼飞行器这类需要实时调整的系统,因为它能够:
- 显式处理系统的输入输出约束(如电机转速限制)
- 考虑系统的动态特性(如惯性、耦合效应)
- 平衡跟踪精度与控制能耗
2.2 四旋翼动力学建模
要设计有效的MPC控制器,首先需要建立准确的四旋翼动力学模型。我们采用牛顿-欧拉方法建立六自由度模型:
平移运动方程:
mẍ = (cosϕsinθcosψ + sinϕsinψ)u₁ - kₓẋ
mÿ = (cosϕsinθsinψ - sinϕcosψ)u₁ - kᵧẏ
mz̈ = (cosϕcosθ)u₁ - mg - k_zż
旋转运动方程:
Iₓϕ̈ = θ̇ψ̇(Iᵧ - I_z) + lu₂ - k_ϕϕ̇
Iᵧθ̈ = ϕ̇ψ̇(I_z - Iₓ) + lu₃ - k_θθ̇
I_zψ̈ = ϕ̇θ̇(Iₓ - Iᵧ) + u₄ - k_ψψ̇
其中:
- m为飞行器质量
- Iₓ, Iᵧ, I_z为转动惯量
- kₓ, kᵧ, k_z, k_ϕ, k_θ, k_ψ为空气阻力系数
- u₁为总升力,u₂,u₃,u₄为三个方向的力矩
- l为电机到质心的距离
2.3 模型线性化处理
由于MPC通常基于线性模型设计,我们需要对上述非线性模型进行线性化处理。选择悬停状态(ϕ=θ=ψ=0,高度恒定)作为平衡点,进行小扰动线性化:
状态向量x = [x y z ẋ ẏ ż ϕ θ ψ ϕ̇ θ̇ ψ̇]ᵀ
控制输入u = [u₁ u₂ u₃ u₄]ᵀ
线性化后的状态空间方程:
ẋ = Ax + Bu
y = Cx
其中A、B矩阵可通过在平衡点处求雅可比矩阵得到,C矩阵根据输出选择确定(通常位置和姿态角作为输出)。
3. MPC控制器设计
3.1 预测模型构建
基于线性化模型,我们构建离散时间的预测模型(采样时间Tₛ):
x(k+1) = A_d x(k) + B_d u(k)
y(k) = C_d x(k)
其中A_d = e^(ATₛ),B_d = ∫₀^Tₛ e^(Aτ)Bdτ,可通过零阶保持法离散化得到。
预测时域为N_p步,控制时域为N_c步(N_c ≤ N_p)。在k时刻,预测未来状态:
x(k+i|k) = A_d^i x(k) + Σ_{j=0}^{i-1} A_d^j B_d u(k+i-1-j|k)
y(k+i|k) = C_d x(k+i|k)
3.2 优化问题构建
在每个时刻k,求解如下优化问题:
min J = Σ_{i=1}^{N_p} ||y(k+i|k) - r(k+i)||²_Q + Σ_{i=0}^{N_c-1} ||Δu(k+i|k)||²_R
s.t.
x(k+i+1|k) = A_d x(k+i|k) + B_d u(k+i|k)
y(k+i|k) = C_d x(k+i|k)
u_min ≤ u(k+i|k) ≤ u_max
Δu_min ≤ Δu(k+i|k) ≤ Δu_max
x_min ≤ x(k+i|k) ≤ x_max
其中:
- r(k+i)为参考轨迹(圆形)
- Q为输出误差权重矩阵
- R为控制增量权重矩阵
- Δu(k+i|k) = u(k+i|k) - u(k+i-1|k)
3.3 圆形参考轨迹生成
圆形轨迹参数方程:
x_ref(t) = R cos(ωt + φ₀) + x₀
y_ref(t) = R sin(ωt + φ₀) + y₀
z_ref(t) = h (恒定高度)
ψ_ref(t) = ωt + ψ₀ (机头方向沿切线)
其中:
- R为圆半径
- ω为角速度
- φ₀为初始相位
- (x₀,y₀)为圆心坐标
- h为飞行高度
- ψ₀为初始偏航角
在离散时间中,参考信号为:
r(k) = [x_ref(kTₛ) y_ref(kTₛ) z_ref(kTₛ) ψ_ref(kTₛ)]ᵀ
3.4 优化问题求解
将预测模型代入目标函数,可将优化问题转化为标准的二次规划(QP)问题:
min 1/2 Uᵀ H U + fᵀ U
s.t. L ≤ A U ≤ U
其中:
- U = [u(k|k)ᵀ ... u(k+N_c-1|k)ᵀ]ᵀ为优化变量
- H为Hessian矩阵
- f为梯度向量
- A为约束矩阵
使用有效集法或内点法求解该QP问题。考虑到实时性要求,可以采用热启动策略(将上一时刻的解作为当前时刻的初始猜测)来加速求解。
4. 实现细节与参数整定
4.1 控制器参数选择
关键参数及其影响:
- 采样时间Tₛ:通常选择10-50ms,需要在计算复杂度和控制精度间权衡
- 预测时域N_p:一般覆盖系统主要动态,对于四旋翼可取20-30步
- 控制时域N_c:通常小于N_p,可取5-10步
- 权重矩阵Q:对角线矩阵,位置误差权重通常大于角度误差
- 权重矩阵R:控制增量权重,防止过于剧烈的控制动作
实际调试建议:先调Q确保跟踪性能,再调R平滑控制输入,最后调整时域参数平衡实时性与预测能力。
4.2 状态估计与滤波
由于实际中无法直接测量所有状态,需要设计状态观测器。常用的方案:
-
传感器融合:
- IMU(加速度计+陀螺仪)提供高频姿态信息
- 光学流/视觉提供位置信息
- 气压计/超声波提供高度信息
- GPS提供全局定位(室外)
-
卡尔曼滤波器:
x̂(k|k-1) = A_d x̂(k-1|k-1) + B_d u(k-1)
P(k|k-1) = A_d P(k-1|k-1) A_dᵀ + Q_k
K(k) = P(k|k-1) C_dᵀ (C_d P(k|k-1) C_dᵀ + R_k)⁻¹
x̂(k|k) = x̂(k|k-1) + K(k)(y(k) - C_d x̂(k|k-1))
P(k|k) = (I - K(k)C_d)P(k|k-1)
4.3 抗扰措施
实际飞行中会遇到各种干扰,需要在控制器设计中考虑:
-
风扰建模:在状态方程中加入风扰项d(k):
x(k+1) = A_d x(k) + B_d u(k) + B_d d(k)
通过扩张状态观测器估计并补偿 -
积分动作:在目标函数中加入误差积分项,消除稳态误差
-
鲁棒设计:使用min-max MPC考虑最坏情况扰动
5. 仿真与实验结果
5.1 仿真环境搭建
使用MATLAB/Simulink搭建仿真平台:
- 非线性四旋翼模型(基于Simulink Multibody或Aerospace Blockset)
- MPC控制器模块(使用Model Predictive Control Toolbox)
- 轨迹生成模块
- 可视化模块(3D动画)
仿真参数示例:
- 飞行器质量m = 1.2 kg
- 惯性矩Iₓ = Iᵧ = 0.03 kg·m², I_z = 0.04 kg·m²
- 电机距离l = 0.2 m
- 采样时间Tₛ = 0.02 s
- 预测时域N_p = 25 (0.5 s)
- 控制时域N_c = 5
- Q = diag([10 10 5 1])
- R = diag([0.1 0.1 0.1 0.1])
5.2 仿真结果分析
圆形轨迹跟踪性能指标:
- 位置跟踪误差RMS:
- x方向:0.08 m
- y方向:0.07 m
- z方向:0.03 m
- 姿态角误差RMS:
- 滚转角:1.2°
- 俯仰角:1.0°
- 偏航角:2.5°
- 控制输入变化率:Δu < 15% of full range per step
与传统PID控制器对比:
- 超调量减少60%
- 稳态误差降低75%
- 抗风扰能力显著提升
5.3 实际飞行测试
硬件平台配置:
- 机架:450mm轴距碳纤维框架
- 飞控:Pixhawk 4
- 处理器:STM32H7 + FPGA(用于QP求解加速)
- 传感器:IMU(BMI088)、光学流(PMW3901)、超声波(MB1040)
实测性能:
- 跟踪半径2m的圆形轨迹,平均误差0.15m
- 最大位置偏差出现在逆风段,约0.3m
- 电机响应时间约50ms
- 控制器计算时间15ms(满足实时性)
6. 常见问题与调试技巧
6.1 控制器不稳定
可能原因及解决方案:
-
模型不准确:
- 重新测量飞行器参数(质量、惯性矩)
- 进行系统辨识实验获取更精确模型
- 考虑未建模动态(如电机延迟)
-
预测时域过短:
- 增加N_p至覆盖系统主要动态
- 但需注意计算负担
-
权重设置不当:
- 增大状态误差权重Q
- 减小控制权重R
6.2 跟踪误差大
优化方向:
-
加入积分动作:
J += Σ||Σ(y-r)||²_Q_i -
调整参考轨迹:
- 降低角速度ω
- 增大圆半径R
-
改进状态估计:
- 校准传感器
- 调整卡尔曼滤波器参数
6.3 计算延迟问题
实时性优化技巧:
-
代码优化:
- 使用ARM CMSIS-DSP库加速矩阵运算
- 定点数运算替代浮点
-
硬件加速:
- FPGA实现QP求解
- 使用GPU加速(如NVIDIA Jetson)
-
简化策略:
- 减少预测时域
- 稀疏化Hessian矩阵
6.4 实际飞行中的经验
-
电池电压补偿:
在u₁计算中加入电压补偿项:
u₁_actual = u₁_desired * (V_nominal / V_actual) -
电机混控优化:
根据电机特性调整混控矩阵,考虑非线性效应 -
安全保护:
- 设置控制输入限幅
- 添加紧急降落逻辑
- 监控计算超时
7. 进阶优化方向
-
非线性MPC:
- 直接基于非线性模型设计
- 使用序列二次规划(SQP)求解
- 需要更强的计算能力
-
学习增强MPC:
- 使用神经网络学习模型误差
- 在线调整预测模型
- 提高对未建模动态的适应性
-
分布式实现:
- 位置控制与姿态控制分层
- 减少单个QP问题规模
- 提高实时性
-
多机协同:
- 扩展状态向量包含多机状态
- 考虑避碰约束
- 实现编队飞行