基于MPC的四旋翼高度动力学及X-Y平面位置控制设计的实践与仿真

基于MPC的四旋翼高度动力学以及x-y平面位置控制设计 简介本项目侧重于MPC控制器设计用于控制四旋翼的高度动力学以及x-y平面位置 就方向动力学而言使用了定制的离散PID(DPID)控制器 该项目在MATLAB 2022b中进行了完全编码和仿真 此外优化问题使用SQP求解器和使用ODE15s求解器求解quadrotor的全plant动力学 最后评估控制器性能的基准轨迹是3D螺旋 代码为纯matlab脚本无附带参考文献四旋翼在三维空间里画螺旋线这事儿听起来浪漫实际调起控制器来能把人逼疯。传统PID在简单轨迹跟踪上还能应付但遇到螺旋上升这种耦合了高度变化和平面运动的场景参数调得再好也容易顾此失彼。这次我们试着用模型预测控制MPC来搞定z轴高度和x-y平面方向控制则交给定制版离散PID看看能不能让这铁家伙优雅地跳个空中华尔兹。先看核心的MPC设计。预测模型直接从牛顿力学扒公式更靠谱别整那些花哨的。状态向量选了[x; y; z; vx; vy; vz]控制量自然是四个电机的总推力分配。这里有个取巧的设定——把姿态动力学交给后面的DPID处理MPC只管整体推力分配% 状态方程离散化 A_cont [zeros(3) eye(3); zeros(3) zeros(3)]; B_cont [zeros(3,1); [0;0;1/mass]]; % 仅考虑z轴推力 sysc ss(A_cont,B_cont,eye(6),0); sysd c2d(sysc, dt); % 零阶保持离散化代价函数设计得简单粗暴预测时域内既要跟紧目标轨迹又要防止推力突变。特别加了终端代价防止最后几步摆烂Q diag([10,10,20,1,1,5]); % z轴误差惩罚加倍 R 0.1; N 20; % 预测步长 [~,S] dlqr(sysd.A,sysd.B,Q,R); % 用LQR计算终端代价求解环节用MATLAB自带的fmincon配合SQP算法。注意约束设置别太贪心四旋翼的推力上下限得卡死否则求解器分分钟给你整出反重力操作options optimoptions(fmincon,Algorithm,sqp,MaxIterations,50); [u_opt,~,exitflag] fmincon((u) costFunc(u,x0,ref,Q,R,S,N),u_guess,[],[],[],[],lb,ub,[],options);方向控制那边用的DPID有点意思。普通PID在离散时间域容易积分饱和这里给积分项加了动态限幅function [u, integrator] dpid(eta_err, dt, Kp, Ki, Kd, prev_err, integrator) integrator integrator eta_err * dt; integrator max(min(integrator, 2*pi), -2*pi); % 防饱和 derivative (eta_err - prev_err) / dt; u Kp*eta_err Ki*integrator Kd*derivative; end仿真用ODE15s解算动力学方程时发现个坑——四元数微分方程对步长敏感必须把相对误差容限调到1e-6以下才能保证姿态解算不崩。后来改成用旋转矩阵参数化才解决果然数值积分的水深得很。基于MPC的四旋翼高度动力学以及x-y平面位置控制设计 简介本项目侧重于MPC控制器设计用于控制四旋翼的高度动力学以及x-y平面位置 就方向动力学而言使用了定制的离散PID(DPID)控制器 该项目在MATLAB 2022b中进行了完全编码和仿真 此外优化问题使用SQP求解器和使用ODE15s求解器求解quadrotor的全plant动力学 最后评估控制器性能的基准轨迹是3D螺旋 代码为纯matlab脚本无附带参考文献测试用的3D螺旋轨迹参数故意设得比较极端半径1米螺距2米最大爬升速度3m/s。跑完整个仿真看到四旋翼的轨迹误差图时松了口气——x-y平面最大偏差0.15米z轴误差始终压在0.1米以内。代价是求解MPC耗时平均每步80ms离实时控制还有点距离不过离线验证算是达标了。调试过程中有个意外发现当MPC的预测时域超过25步后控制效果反而变差。后来想明白是模型误差累积导致的毕竟把姿态动力学完全解耦处理预测步长太长相当于在错误的方向上越走越远。这也提醒我们MPC不是预测越远越好得在模型精度和计算开销之间找平衡。最后留个尾巴尝试把方向控制也整合进MPC框架时求解时间直接飙到500ms以上看来要玩全状态MPC还得换更高效的求解器。不过现在的分层设计已经能让四旋翼在办公室里画出标准螺旋线暂时够跟老板交差了——当然得忽略那偶尔抽搐的机头朝向。