无人机动力学建模与模型预测控制(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为无人机质量kgg[0;0;9.8]为重力加速度m/s²kd为空气阻力系数N·s/m旋转动力学 I·dωB/dt -ωB×(I·ωB) - Jr·ωB·ωr τ其中Idiag(Ixx,Iyy,Izz)为转动惯量矩阵kg·m²Jr为旋翼转动惯量kg·m²ωr ω₂ - ω₁ ω₄ - ω₃为有效旋翼转速rad/s2. 模型预测控制(MPC)设计2.1 MPC基本原理模型预测控制是一种基于优化的先进控制策略其核心思想是建立预测模型描述系统动态在每个控制周期求解有限时域最优控制问题仅实施控制序列的第一个元素下一周期重新测量并优化形成滚动时域框架对于无人机系统MPC的优势在于显式处理状态和输入约束天然适合多输入多输出系统可协调响应速度与控制能耗2.2 预测模型离散化将连续动力学方程离散化为状态空间形式 x(k1) f(x(k),u(k))采用前向欧拉法离散化采样周期T0.1s x(k1) 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)调试技巧初始建议设置Qdiag([10,10,10,1,1,1,0.1,0.1,0.1,0,0,0])R0.1I₄再根据实际响应调整。2.4 约束处理无人机运行需满足物理限制电机转速ω_i ∈ [0, 630] rad/s对应最大推力速度限制v_x,v_y ∈ [-2,2] m/sv_z ∈ [-1,1] m/s姿态角限制φ,θ ∈ [-π/4,π/4] radψ无约束角速度限制p,q,r ∈ [-π/2,π/2] rad/sMPC中通过硬约束或软约束处理# 软约束示例Python伪代码 slack model.addVar(lb0) model.addConstr(state limit slack) cost penalty * slack**23. 参数辨识与自适应控制3.1 系统参数不确定性无人机实际参数θ与标称值θ₀存在偏差 ∥θ - θ₀∥ ≤ 0.5∥θ∥关键待辨识参数包括 θ [kt kb kd Ixx Iyy Izz Jr]ᵀ3.2 递归最小二乘(RLS)算法RLS通过实时更新来估计参数其迭代公式为增益计算K P·φᵀ/(λ φPφᵀ)参数更新θ̂ θ̂ K(y - φθ̂)协方差更新P (I - Kφ)P/λ其中φ为回归矩阵包含状态和输入y为观测输出λ∈(0,1]为遗忘因子常取0.95-0.99实现细节需添加协方差重置机制当tr(P)1e6时令P1e3I避免数值发散。3.3 联合学习与控制框架整体算法流程初始化设置θ̂θ₀P1e3I在线循环 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∥θ∥结果对比指标初始控制器训练后控制器改进率稳定时间(s)3.22.134%超调量(%)15847%能耗(J)12510814%4.2 八字轨迹跟踪轨迹方程 x_ref 3sin(0.2t) y_ref 3sin(0.4t) z_ref 2跟踪性能平均位置误差0.15m最大位置误差0.28m姿态角波动±0.12rad4.3 抗扰测试施加突发风扰持续时间1s风速3m/sX方向恢复时间1.8s最大偏离0.45m5. 工程实现要点5.1 实时性保障计算耗时分解N12预测步长模块时间(ms)优化建议状态估计0.5使用EKF替代UKFRLS更新0.3固定点运算MPC求解8.2热启动代码生成总计9.0需10ms(100Hz)5.2 参数调试流程先调悬停控制器仅位置环增大Q对角元素直至无静差调整R避免电机饱和加入姿态环调试保证内环带宽外环3倍最后调辨识算法从λ1.0开始逐步减小监控参数变化率5.3 常见故障排查问题1MPC求解失败检查预测模型雅可比矩阵对策松弛约束或缩短时域问题2参数估计发散检查回归矩阵φ的秩对策注入小幅激励信号问题3高频抖动检查代价函数中的Δu惩罚项对策增加输入变化率权重6. 进阶优化方向非线性MPC直接处理全阶非线性模型避免线性化误差鲁棒MPC考虑估计误差边界设计最坏情况保证学习型MPC结合深度神经网络提升模型精度分布式架构分离姿态与位置控制器降低计算负担实际飞行测试表明经过200次迭代训练后控制器的闭环成本可降低35%参数估计误差小于1%。这种数据驱动的方法为复杂环境下的无人机控制提供了可靠解决方案。