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倍大的推力，导致电机转速瞬间饱和。

下面是参数含义速查表，方便对号入座：

如果只做轨迹跟踪，质量、推力系数最关键；如果对比姿态控制，惯性矩不能拍脑袋，要用三线摆或理论计算。另外，代码里可能把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_{k=0}^{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:k+N-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:k+N-1)会用到未来N步参考值，如果轨迹长度不够，需要在最后进行尾部重复或悬停处理，否则索引越界。

3.3 参数化编程：怎么改预测时域和控制时域

预测时域N的物理含义是“目光放多远”。N越大，控制器越有前瞻性，在参考轨迹急转弯时能提前减速，但优化变量数量随N*Nu线性增长，fmincon每次迭代都要调用模型预测函数，计算量爆炸。控制时域Nu表示未来优化变量中只有前Nu个变化自由度，之后保持最后的值。下表给出一个在普通四旋翼模型上的参考设置：

如果主频不高，建议把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倍，否则会产生耦合振荡。常见参数如下：

这里的比例值只是初始参照，实际还要根据无人机质量和传感器噪声调整。外环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，不建议只画一张轨迹图就下结论。我们通常统计三个指标：

计算代码很直接：

err = x_log - ref_log; rmse = sqrt(mean(err.^2, 2)); max_err = max(abs(err), [], 2);

注意所有状态要统一单位，比如姿态角要转成弧度再算，否则位置和角度混在一起算RMSE没意义。我一般分别统计位置RMSE和姿态RMSE，而不是合成一个标量。

5.3 遇到不收敛或发散先查这五个参数

NMPC跑起来最折磨人的就是不收敛。根据我拆这套代码的经验，优先排查下面五个参数：

1. 采样时间Ts。Ts = 0.01s时预测15步只有0.15s的视野，飞机根本看不到远处弯道；Ts = 0.2s时模型离散误差又太大。先用0.05~0.1起步。
2. 预测时域N乘Ts的有效视野。保证有效视野覆盖参考轨迹最剧烈的变化周期。如果轨迹周期是10s，视野至少要有1~2s。
3. 权重矩阵Q和R的比例。位置误差权重如果比控制量权重大1000倍以上，fmincon容易追求零误差而输出饱和；反过来又跟踪跟不上。通常先固定位置权重为1，然后调R使得控制量在饱和边界附近。
4. 模型中的状态归一化。位置（米）和姿态角（弧度）直接放进一个代价函数，数值尺度差100倍时，优化器会忽略小数值的项。要对状态做对角归一化。
5. 初始可行解。冷启动时用全零控制量往往不可行（升力小于重力），至少设一个悬停油门初值，等效于将每个电机转速设为sqrt(mg/(4k_thrust))。

只要这五个点不踩歪，NMPC通常比PID在急转弯时少30%的峰值误差。最后附一个实用做法：画图时把NMPC和PID的误差曲线纵轴统一，别用自动缩放，否则看不出差异。

本文还有配套的精品资源，点击获取