NMPC与PID无人机轨迹跟踪对比：Matlab实现与调参技巧

简介无人机轨迹跟踪的非线性模型预测控制NMPC与基线反馈控制器对比研究资源面向自动控制、无人机导航及机器人方向的学生与工程师以MATLAB为核心实现覆盖建模、参数识别、动态仿真与控制性能分析全流程适合课程设计、毕业设计及算法验证。压缩包共116个文件其中63个.m主程序脚本、31个.mat数据文件、14个.pdf说明文档另有备份与相关文档总大小7.69MB结构清晰便于查阅。已有90人学习下载。内容包含无人机运动学与动态模型参数、参数辨识验证、NMPC及动态控制器实机/标称仿真等子模块代码采用参数化编程、注释明细可方便更改参数并直接运行同时附赠案例数据帮助使用者快速复现不同控制律的跟踪效果深入理解NMPC相较基线方法的性能优势。1. 为什么拿NMPC和基线反馈控制器比无人机轨迹跟踪无人机做轨迹跟踪时大家最熟悉的控制器就是PID但在大角度倾斜和快速转向场景下外环位置输出和内环姿态跟踪之间常常出现相位滞后甚至会诱发振荡。NMPC非线性模型预测控制把轨迹跟踪建模成有限时域最优控制问题每个控制周期用无人机模型预测未来一段轨迹再求解一个带约束的优化问题所以对模型依赖强但一旦调好跟踪质量和约束处理能力都明显优于固定增益反馈。这份配套Matlab代码同时实现了NMPC和带自适应估计的动力学反馈控制器并提供了参数文件、辨识验证脚本和真实模型下的对比结果能在Matlab 2014/2019a/2021a中直接运行。它比较适合两类人一是做无人机控制方向课程设计、毕业设计的学生二是刚接触NMPC但不想从零搭优化框架的算法工程师。2. 无人机动力学模型与Matlab参数初始化从Drone_Parameters.m开始2.1 运动学与动力学的差异代码里两套模型怎么选先观察仓库里的两个.asv文件T_UAV_Cinematica.asv 和 T_UAV_DynamicCom_EstimAdaptative.asv。前者处理的是运动学Cinematica后者是带自适应估计的动力学补偿。运动学模型只描述位置、速度、姿态角的变化关系不涉及质量、惯性矩和推力系数表达式形如线速度直接积分角速度通过转换矩阵映射到欧拉角速率。动力学模型则需要完整的力和力矩方程电机转速通过推力系数产生升力通过反扭矩系数产生偏航力矩。NMPC做轨迹跟踪时如果控制频率高且飞行速度低运动学模型已经够用如果你要模拟大机动、受风扰就必须上动力学模型。代码中Drone_Parameters.m同时给两套模型供给参数所以要先弄明白每个脚本用的是哪一套。我一般会在脚本开头加一行注释标明“本脚本使用运动学模型”或“本脚本使用动力学模型”避免后续对比时把两套模型的结果混在一起。这个工程里两个.asv文件分别对应两套模型但实际运行时通过Drone_Parameters.m统一初始化算是一个不错的习惯。2.2 Drone_Parameters.m 参数逐项解读典型的Drone_Parameters.m会写成参数化脚本我一般习惯用section组织方便多个脚本共用。下面是一个简化但能跑的版本% Drone_Parameters.m - 四旋翼模型参数 % 所有控制器脚本通过 run(Drone_Parameters.m) 加载 %% 物理常量 g 9.81; % 重力加速度 m/s^2 m 1.2; % 无人机总质量 kg %% 惯性矩 Ix 0.015; % 绕x轴转动惯量 kg*m^2 Iy 0.015; % 绕y轴转动惯量 kg*m^2 Iz 0.028; % 绕z轴转动惯量 kg*m^2 %% 电机-旋翼模型简化线性 k_thrust 8.0e-6; % 推力系数 N/(rad/s)^2 k_drag 2.0e-7; % 反扭矩系数 N*m/(rad/s)^2 %% 控制限幅 omega_min 0; % 电机最低转速 rad/s omega_max 1200; % 电机最高转速 rad/s这段代码把参数集中到一起后续脚本直接用变量名访问。注意推力系数单位会因为仿真步长和风洞实验而略有差别建议使用自己模型辨识得到的结果。参数一旦改错最直接的表现是控制器以为自己输出很大但仿真位移却很小。例如质量m从1.2误写成12相同的期望加速度会被分配成10倍大的推力导致电机转速瞬间饱和。下面是参数含义速查表方便对号入座参数符号典型值单位作用质量m1.2kg平移动力分配转动惯量Ix/Iy/Iz0.015/0.015/0.028kg·m²姿态动力学计算推力系数k_thrust8e-6N/(rad/s)²电机转速→升力反扭矩系数k_drag2e-7N·m/(rad/s)²差速偏航力矩如果只做轨迹跟踪质量、推力系数最关键如果对比姿态控制惯性矩不能拍脑袋要用三线摆或理论计算。另外代码里可能把g写成本地重力加速度在不同海拔或纬度下需要修正到位。2.3 从asv文件到可复用脚本T_UAV_Cinematica.asv 里藏了什么.asv是Matlab自动保存文件说明作者写代码过程中崩溃或手动保存过。把.asv后缀改成.m就可以当脚本用但里面可能遗留了调试脏代码。我拿到这个文件后一般会先搜索断点、或者把分号漏掉的大数组输出清理掉。T_UAV_Cinematica.asv 里面常见内容是一个简化的运动学传播函数用来生成状态转移矩阵的离散形式function [x_next] uav_kinematics(x, u, Ts) % x [px; py; pz; phi; theta; psi] % u [vx; vy; vz; p; q; r] 期望速度与角速度 px x(1); py x(2); pz x(3); phi x(4); theta x(5); psi x(6); % 线速度直接积分NED坐标系 px_next px u(1)*Ts; py_next py u(2)*Ts; pz_next pz u(3)*Ts; % 角速度到欧拉角速率的转换矩阵 R [1 sin(phi)*tan(theta) cos(phi)*tan(theta); 0 cos(phi) -sin(phi); 0 sin(phi)/cos(theta) cos(phi)/cos(theta)]; euler_dot R * u(4:6); x_next [px_next; py_next; pz_next; ... x(4)euler_dot(1)*Ts; x(5)euler_dot(2)*Ts; x(6)euler_dot(3)*Ts]; end这段代码把线速度直接作为欧拉积分欧拉角速率则用转换矩阵乘上机体角速度。注意这种运动学模型忽略了科氏力和陀螺效应适合慢速飞行或作为NMPC内部预测模型。如果用在快速翻转场景预测结果会失真这时候就需要换T_UAV_DynamicCom_EstimAdaptative.asv里的动力学传播。另一点容易踩坑欧拉角在θ接近±90°时转换矩阵奇异必须在预测循环里对θ做限幅。2.4 模型辨识与参数验证的衔接除了Drone_Parameters.m仓库里的Results_Identification_validation.m专门做参数辨识效果验证。它一般把辨识出的参数代回模型输入一段测试信号然后把仿真输出和真实输出画在一起。验证合格的标准是RMSE小于某个阈值且预测误差不随时间发散。如果发散多半是积分步长过大或模型结构不对。这里先提一句参数文件里所有带下划线的变量名尽量保持一致否则后续十几个脚本之间互相引用时会不断报“Undefined function or variable”。3. NMPC轨迹跟踪实现把优化问题写进代码3.1 NMPC核心预测模型、代价函数、约束NMPC 和普通PID的本质区别在于PID是根据当前误差计算反馈量NMPC会利用模型预测未来N步的误差然后求解一组最优控制输入u0...uN-1使得代价函数最小。代价函数通常写成J sum_{k0}^{N-1} (x_k - x_ref_k) Q (x_k - x_ref_k) u_k R u_k terminal_cost其中Q是对状态误差的权重R是对控制量的惩罚。约束包括电机转速限幅、姿态角限幅以及可能的最大推力。Matlab实现时我一般使用fmincon作为底层求解器虽然慢但对教学和验证算法足够。如果你做实时飞控建议换CasADi Ipopt或用MEX加速。NMPC真正麻烦的不是公式而是预测模型怎么离散化、代价函数里数值尺度怎么平衡。3.2 Results_NMPC_Nominal.m 代码结构与关键段这个脚本应该是NMPC在名义模型下跑仿真并出图。核心流程可以浓缩成下面这段循环% Results_NMPC_Nominal.m 核心循环段 Ts 0.05; % 控制周期 50ms N 15; % 预测时域 Nu 5; % 控制时域 % 使用运动学模型作为预测模型 model (x,u) uav_kinematics(x,u,Ts); % 参考轨迹圆形 t_all 0:Ts:20; ref [2*sin(0.2*t_all); 2*cos(0.2*t_all); zeros(size(t_all))]; x0 [0; 2; 0; 0; 0; 0]; % 初始位置 u0 zeros(6,1); % 初始控制 for k 1:length(t_all)-1 % 从当前状态和参考轨迹构造非线性优化问题 costfun (U) nmpc_cost(U, x0, ref(:,k:kN-1), model, Q, R); nonlcon (U) nmpc_constraints(U, omega_max, omega_min); U_opt fmincon(costfun, repmat(u0, [1 Nu]), [], [], [], [], ... [], [], nonlcon, optimoptions(fmincon,Display,off)); % 执行第一个控制量 u_apply U_opt(1:6); x0 model(x0, u_apply); log{k} x0; end注意这里把6个控制量压平成一个向量传给fmincon所以U的长度是6*Nu。nmpc_cost内部要用预测模型迭代计算未来N步状态再累加权误差。nmpc_constraints返回非线性约束c(x)0和ceq[]最简单的约束是电机转速在[omega_min, omega_max]之间。但为了演示NMPC的约束处理能力我一般写成nonlcon这样后续换复杂约束不用改主循环。循环里ref(:,k:kN-1)会用到未来N步参考值如果轨迹长度不够需要在最后进行尾部重复或悬停处理否则索引越界。3.3 参数化编程怎么改预测时域和控制时域预测时域N的物理含义是“目光放多远”。N越大控制器越有前瞻性在参考轨迹急转弯时能提前减速但优化变量数量随N*Nu线性增长fmincon每次迭代都要调用模型预测函数计算量爆炸。控制时域Nu表示未来优化变量中只有前Nu个变化自由度之后保持最后的值。下表给出一个在普通四旋翼模型上的参考设置场景NNuTs计算耗时(fmincon)慢速巡航1030.1s约30ms快速轨迹1550.05s约80ms精确悬停820.05s约15ms如果主频不高建议把N设在10~20之间。还可以在代价函数里加入控制增量惩罚例如权重矩阵R加上udiff * Rdu * udiff可以防止相邻周期控制量跳变太大。具体到代码中就是把costfun里的u扩展成[U(k); U(k-1)]后再做差分。此外NMPC对初始解敏感。在Results_NMPC_real.m里即带真实模型或实际噪声我一般把上一周期的最优解作为当前周期的初始值也就是热启动。热启动能显著减少迭代次数反之冷启动可能出现第一次迭代位置误差大但控制器不敢动的现象。调预测时域的时候建议先保持权重不变只改N跑完一组轨迹后打印累计误差和求解失败次数再决定N是增大还是减小。4. 基线反馈控制器设计PID/LQR对比实现4.1 为什么选级联PID作为基线基线反馈控制器不能选得太弱否则对比结果没有说服力也不能选得太复杂否则无法体现NMPC的优势。业界最常用的基线就是级联PID外环位置控制器算出期望加速度再映射成期望姿态角内环姿态控制器跟踪期望角。这个结构在PX4/ArduPilot里已经被验证烂了作为对照基线非常合适。NMPC可以直接一步输出电机转速或期望姿态指令而级联PID必须分成内外环带宽搭配很关键。外环一般10Hz内环50~100Hz内外环时间间隔要拉开3~5倍否则会产生耦合振荡。常见参数如下控制环控制器比例积分微分外环位置PID0.50.050.1内环姿态PID3.00.10.2这里的比例值只是初始参照实际还要根据无人机质量和传感器噪声调整。外环P太大会导致期望姿态角抖动进而带着内环一起振荡。我一般先调内环再调外环否则两个环同时调会分不清是谁在振。4.2 T_UAV_DynamicCom_EstimAdaptative.asv 里的估计与补偿文件名里的EstimAdaptative暗示这个脚本包含了模型不确定性和扰动的在线估计。常见做法是给动力学方程加一个扩张状态观测器ESO估计总和扰动再在控制律里前馈补偿。代码段示意% T_UAV_DynamicCom_EstimAdaptative.asv 中的ESO估计 function [z1_hat, z2_hat] eso_update(t, x, u_actual, z1, z2, w0, b0) % 简化一阶ESO把总扰动作为扩张状态 e z1 - x; z1_dot z2 - 2*w0*e b0*u_actual; z2_dot -w0*w0*e; z1_hat z1 z1_dot*Ts; z2_hat z2 z2_dot*Ts; end这里的w0是观测器带宽越大跟踪扰动越快但会放大噪声。b0是控制增益的估计值如果模型参数不准b0和真实值偏差20%以内还能用超过50%观测器容易振荡。在对比实验里给这个基线控制器加上同样的风扰NMPC虽然不知道风扰是具体的正弦还是常值但它可以通过预测模型中的状态误差间接反映出来。不过因为没有积分项纯NMPC常常会在常值扰动下留稳态误差。这就是为什么有些NMPC工程实现会在外部再加一个抗扰观测器。代码文件中同时给出动态补偿和自适应估计我觉得原作者就是想对比“带扰动补偿的PID”和“纯NMPC”之间的性能差距。4.3 对比实验设置相同轨迹、相同扰动为了公平两个控制器需要完全相同的参考轨迹、初始状态和采样时间。我写对比脚本时通常先生成一个统一的参考轨迹结构% 生成对比用参考轨迹8字轨迹 Ts 0.05; t 0:Ts:20; w 0.3; ref_px 3*sin(w*t); ref_py 2*sin(2*w*t); ref_pz 1 0.2*sin(0.5*t); ref_p [ref_px; ref_py; ref_pz];然后对每个控制器在主循环里加上同样的风扰向量wind [0.5*sin(0.8*t(k)), 0.3*cos(1.2*t(k)), 0]; % 阵风最后把两个结果保存成结构体画出同一张三维轨迹对比图。这里有个非常关键的细节PID控制器每一步接收到的参考姿态解算频率必须和NMPC一致。如果PID内环跑100Hz而外环只10Hz代码里要用零阶保持器把期望姿态保持住否则内环看到的是阶梯波会产生额外超调。我在Results_Dynamic_real.m和Results_NMPC_real.m里看到类似的处理逻辑都是先把参考量插值到同一个时间轴跑完再做误差分析。5. 仿真结果分析与NMPC调参技巧5.1 从Results_Identification_validation.m 看模型辨识验证模型参数质量直接决定NMPC预测准不准。Results_Identification_validation.m 做的事情是把辨识出的参数代回模型输入一段测试信号然后把仿真输出和真实输出画在一起。验证一般看两点一是RMSE是否小于某个阈值二是预测误差是否随时间发散。如果发散多半是积分步长过大或模型结构不对。pred_err sim_out - real_out; rmse sqrt(mean(pred_err.^2)); if rmse 0.1 warning(模型精度不足建议重新辨识); end这里0.1只是示例阈值具体要和轨迹长度、单位对应。若单位是米0.1m的误差对于2m幅度轨迹可以接受。如果验证时使用开环预测误差发散是正常的关键是看发散速度。预测时域N对应的视野内误差不明显增大就说明模型可用。5.2 误差指标与收敛性判断对比NMPC和基线PID不建议只画一张轨迹图就下结论。我们通常统计三个指标指标公式意义RMSEsqrt(mean((x-x_ref).^2))整体偏差水平MaxErrmax(abs(x-x_ref))最大瞬时偏差SettleTime误差进入±5%后不再离开的时间收敛速度计算代码很直接err x_log - ref_log; rmse sqrt(mean(err.^2, 2)); max_err max(abs(err), [], 2);注意所有状态要统一单位比如姿态角要转成弧度再算否则位置和角度混在一起算RMSE没意义。我一般分别统计位置RMSE和姿态RMSE而不是合成一个标量。5.3 遇到不收敛或发散先查这五个参数NMPC跑起来最折磨人的就是不收敛。根据我拆这套代码的经验优先排查下面五个参数采样时间Ts。Ts 0.01s时预测15步只有0.15s的视野飞机根本看不到远处弯道Ts 0.2s时模型离散误差又太大。先用0.05~0.1起步。预测时域N乘Ts的有效视野。保证有效视野覆盖参考轨迹最剧烈的变化周期。如果轨迹周期是10s视野至少要有1~2s。权重矩阵Q和R的比例。位置误差权重如果比控制量权重大1000倍以上fmincon容易追求零误差而输出饱和反过来又跟踪跟不上。通常先固定位置权重为1然后调R使得控制量在饱和边界附近。模型中的状态归一化。位置米和姿态角弧度直接放进一个代价函数数值尺度差100倍时优化器会忽略小数值的项。要对状态做对角归一化。初始可行解。冷启动时用全零控制量往往不可行升力小于重力至少设一个悬停油门初值等效于将每个电机转速设为sqrt(mg/(4k_thrust))。只要这五个点不踩歪NMPC通常比PID在急转弯时少30%的峰值误差。最后附一个实用做法画图时把NMPC和PID的误差曲线纵轴统一别用自动缩放否则看不出差异。本文还有配套的精品资源点击获取