无人机动力学建模与模型预测控制(MPC)实践

1. 无人机动力学建模基础

无人机动力学建模是飞行控制算法设计的基石，其核心在于准确描述飞行器在空间中的运动规律。四旋翼无人机作为典型的欠驱动系统，其动力学特性由平移运动和旋转运动两部分组成。

1.1 坐标系定义与转换

无人机运动分析需要建立两个关键坐标系：

• 惯性坐标系（地面坐标系）：固定于地面，Z轴垂直向上
• 机体坐标系：固连于无人机质心，X轴指向机头方向

两坐标系间的转换通过旋转矩阵实现。欧拉角(φ,θ,ψ)分别表示滚转、俯仰和偏航角度，对应的旋转矩阵R将机体坐标系下的向量转换为惯性坐标系下的表达：

R = Rz(ψ)Ry(θ)Rx(φ) = ⎡cosψcosθ cosψsinθsinφ-sinψcosφ cosψsinθcosφ+sinψsinφ⎤ ⎢sinψcosθ sinψsinθsinφ+cosψcosφ sinψsinθcosφ-cosψsinφ⎥ ⎣-sinθ cosθsinφ cosθcosφ ⎦

注意：当俯仰角θ=±90°时会出现万向节锁死现象，此时需改用四元数表示姿态。实际工程中通常限制俯仰角在±80°以内以避免此问题。

1.2 推力与扭矩方程

四旋翼无人机的动力来源于四个旋翼产生的升力。设第i个旋翼转速为ωi，则总推力uT和机体扭矩τ可表示为：

uT = kt(ω₁² + ω₂² + ω₃² + ω₄²)·[0;0;1]
τ = [ lkt(ω₄² - ω₂²) ; lkt(ω₃² - ω₁²) ; kb(ω₂² - ω₁² + ω₃² - ω₄²) ]

其中：

• kt为推力系数（N·s²/rad²）
• kb为反扭矩系数（N·m·s²/rad²）
• l为旋翼到质心的距离（m）

经验提示：kt和kb需通过实验标定，常见方法是在测力平台上测量不同转速下的推力和扭矩，再用最小二乘法拟合。

1.3 六自由度动力学方程

基于牛顿-欧拉公式，可得无人机在惯性系下的运动方程：

平移动力学： m·dv/dt = R·uT - m·g - kd·v
其中：

• m为无人机质量（kg）
• g=[0;0;9.8]为重力加速度（m/s²）
• kd为空气阻力系数（N·s/m）

旋转动力学： I·dωB/dt = -ωB×(I·ωB) - Jr·ωB·ωr + τ
其中：

• I=diag(Ixx,Iyy,Izz)为转动惯量矩阵（kg·m²）
• Jr为旋翼转动惯量（kg·m²）
• ωr = ω₂ - ω₁ + ω₄ - ω₃为有效旋翼转速（rad/s）

2. 模型预测控制(MPC)设计

2.1 MPC基本原理

模型预测控制是一种基于优化的先进控制策略，其核心思想是：

1. 建立预测模型描述系统动态
2. 在每个控制周期求解有限时域最优控制问题
3. 仅实施控制序列的第一个元素
4. 下一周期重新测量并优化，形成滚动时域框架

对于无人机系统，MPC的优势在于：

• 显式处理状态和输入约束
• 天然适合多输入多输出系统
• 可协调响应速度与控制能耗

2.2 预测模型离散化

将连续动力学方程离散化为状态空间形式： x(k+1) = f(x(k),u(k))

采用前向欧拉法离散化（采样周期T=0.1s）： x(k+1) = x(k) + T·f(x(k),u(k))

状态向量x包含12个元素： x = [px py pz vx vy vz φ θ ψ p q r]ᵀ

2.3 目标函数设计

典型二次型代价函数： J = ∑(x-x_ref)ᵀQ(x-x_ref) + ∑(u-u_ref)ᵀR(u-u_ref)

其中：

• Q为状态权重矩阵（半正定）
• R为输入权重矩阵（正定）
• x_ref为参考轨迹
• u_ref为悬停所需输入（ω₁=ω₂=ω₃=ω₄=√(mg/4kt)）

调试技巧：初始建议设置Q=diag([10,10,10,1,1,1,0.1,0.1,0.1,0,0,0])，R=0.1I₄，再根据实际响应调整。

2.4 约束处理

无人机运行需满足物理限制：

• 电机转速：ω_i ∈ [0, 630] rad/s（对应最大推力）
• 速度限制：v_x,v_y ∈ [-2,2] m/s，v_z ∈ [-1,1] m/s
• 姿态角限制：φ,θ ∈ [-π/4,π/4] rad，ψ无约束
• 角速度限制：p,q,r ∈ [-π/2,π/2] rad/s

MPC中通过硬约束或软约束处理：

# 软约束示例（Python伪代码） slack = model.addVar(lb=0) model.addConstr(state <= limit + slack) cost += penalty * slack**2

3. 参数辨识与自适应控制

3.1 系统参数不确定性

无人机实际参数θ与标称值θ₀存在偏差： ∥θ - θ₀∥ ≤ 0.5∥θ∥

关键待辨识参数包括： θ = [kt kb kd Ixx Iyy Izz Jr]ᵀ

3.2 递归最小二乘(RLS)算法

RLS通过实时更新来估计参数，其迭代公式为：

1. 增益计算：K = P·φᵀ/(λ + φPφᵀ)
2. 参数更新：θ̂ = θ̂ + K(y - φθ̂)
3. 协方差更新：P = (I - Kφ)P/λ

其中：

• φ为回归矩阵（包含状态和输入）
• y为观测输出
• λ∈(0,1]为遗忘因子（常取0.95-0.99）

实现细节：需添加协方差重置机制，当tr(P)>1e6时令P=1e3I，避免数值发散。

3.3 联合学习与控制框架

整体算法流程：

1. 初始化：设置θ̂=θ₀，P=1e3I
2. 在线循环： a. 采集当前状态x(k) b. RLS更新参数估计θ̂ c. 基于θ̂更新MPC模型 d. 求解MPC问题得到u(k) e. 应用u(k)并等待下一周期

收敛性保障：

• 持续激励条件：轨迹需包含足够丰富的频率成分
• 参数协方差监控：确保P矩阵保持正定

4. 实现案例与性能分析

4.1 悬停控制

测试条件：

• 初始位置：(0,0,0)m
• 目标位置：(0,0,2)m
• 参数误差：∥θ₀-θ∥=0.5∥θ∥

结果对比：

4.2 八字轨迹跟踪

轨迹方程： x_ref = 3sin(0.2t) y_ref = 3sin(0.4t) z_ref = 2

跟踪性能：

• 平均位置误差：0.15m
• 最大位置误差：0.28m
• 姿态角波动：±0.12rad

4.3 抗扰测试

施加突发风扰（持续时间1s）：

• 风速：3m/s（X方向）
• 恢复时间：1.8s
• 最大偏离：0.45m

5. 工程实现要点

5.1 实时性保障

计算耗时分解（N=12预测步长）：

5.2 参数调试流程

1. 先调悬停控制器（仅位置环）
   • 增大Q对角元素直至无静差
   • 调整R避免电机饱和
2. 加入姿态环调试
   • 保证内环带宽>外环3倍
3. 最后调辨识算法
   • 从λ=1.0开始逐步减小
   • 监控参数变化率

5.3 常见故障排查

问题1：MPC求解失败

• 检查：预测模型雅可比矩阵
• 对策：松弛约束或缩短时域

问题2：参数估计发散

• 检查：回归矩阵φ的秩
• 对策：注入小幅激励信号

问题3：高频抖动

• 检查：代价函数中的Δu惩罚项
• 对策：增加输入变化率权重

6. 进阶优化方向

1. 非线性MPC：直接处理全阶非线性模型，避免线性化误差
2. 鲁棒MPC：考虑估计误差边界，设计最坏情况保证
3. 学习型MPC：结合深度神经网络提升模型精度
4. 分布式架构：分离姿态与位置控制器降低计算负担

实际飞行测试表明，经过200次迭代训练后，控制器的闭环成本可降低35%，参数估计误差小于1%。这种数据驱动的方法为复杂环境下的无人机控制提供了可靠解决方案。