无人机移动平台跟踪：MPC、NMPC与混合MPC-RL的Matlab对比与实现

无人机追移动平台这个活儿，平时做巡检、物流或者农业项目的人应该都有同感：地面目标一旦动起来，传统 PID 很容易在急转弯或者加减速的时候掉链子。所以我把目光放到了 MPC、NMPC，以及更进一步的混合 MPC-RL 上，用 Matlab 把三种方案全部跑通，专门对比它们在同一套移动平台跟踪场景里的表现。这篇文章会把我的建模方式、代价函数设计、代码骨架、实际跑分结果，以及我踩过的调参坑统统写出来，适合刚入门控制算法、或者正在给无人机写跟踪逻辑的开发者直接参考。

1. 为什么无人机轨迹跟踪不能只靠一套控制器

1.1 场景决定了控制器的复杂度

先给这个项目定个位：无人机在三维空间里跟踪一个做平面运动的移动平台。平台可能是地面小车，也可能是船上的一块起降坪，它本身有自己的运动规律，不会因为无人机想要稳定而对它做任何让步。真实项目中，平台往往还会频繁加减速、拐弯，甚至短暂进入遮挡区域后再出现。

这种场景下的核心问题可以提炼成两句话：无人机能不能实时地、平滑地跟上目标的位置；跟上的过程中，能不能主动规避约束——比如速度上限、推力上限，以及对某个方向的允许偏差上限。

MPC 家族天生适合这种带约束的跟踪问题。它不像 PID 那样看当前误差直接算修正量，而是基于一个模型，往前预测未来一小段时间，然后在满足约束的前提下，找一个让整体代价最小的控制序列。预测得到的最优控制只取第一步，推给执行器，然后整个优化在下一个采样周期重新做一次。这听起来很美好，实际用起来却有三道坎。

1.2 线性 MPC 的不够用是藏在模型里的

线性 MPC 通常把无人机近似成三个互相解耦的积分环节：x 方向、y 方向、z 方向各用一个简单的线性模型来描述。在小扰动、小机动的情况下，这个近似完全够用。可在地面平台做高速转弯时，无人机需要同时协调水平和垂直播机运动，线加速度、角速度、机体姿态之间的耦合会明显变大，线性模型的预测精度就开始打折。

我最早用线性 MPC 跑直线跟踪，平均误差只有 0.5 米左右，看起来还行。等我把平台运动改成圆周加突发换向以后，平均误差直接涨到了接近 0.8 米，最大误差突破 1.5 米。这个误差水平在一些大范围巡检场景里可以接受，但如果是精准降落或者在狭窄区域内悬停对接，误差就直接影响安全了。

1.3 NMPC 和混合 MPC-RL 的登场逻辑

NMPC 的思路很简单：把无人机模型从“线性近似”换成“非线性方程”，在优化过程中直接处理状态之间的耦合和各种非线性的约束。这样能提升预测的保真度，但代价是每个周期都要解一个非线性优化问题，计算量往往是线性 MPC 的几十倍。

混合 MPC-RL 则是另一条路：让 MPC 继续负责稳定性和约束满足，把那些难以建模、难以在线整定的部分——比如模型误差、风扰、平台运动模式的突变——交给强化学习去自适应学习。换句话说，MPC 负责“守住底线”，RL 负责“提高上限”。这套方案在项目里的表现确实给了我一个不小的惊喜。

2. 控制模型与算法设计

2.1 无人机模型的简化方式

为了方便在 Matlab 里做仿真对比，我没有直接上六自由度刚体模型，而是用了常见的质心模型。状态取位置和速度，控制输入是三个方向的加速度指令：

无人机状态：x = [px, py, pz, vx, vy, vz]^T 控制输入：u = [ax, ay, az]^T 离散化后的状态更新：p(k+1) = p(k) + v(k) * dt；v(k+1) = v(k) + u(k) * dt

这个简化模型在低速、小姿态角的场景下和真实无人机行为非常接近，也方便后续比较三种算法。如果项目需求是高速大机动，就需要把姿态动力学、执行器延迟、甚至风力模型都加进来，但控制框架本身不会变。

移动平台这块，我采用的是“提前一个预测区间提供轨迹”的做法。也就是说，平台在下个时刻会往哪里走，会以一段短期参考轨迹的形式给到控制器。这在实际系统中通常由视觉或雷达跟踪模块给出，仿真阶段我直接生成已知轨迹。

2.2 代价函数和终端约束的设计

跟踪性能的好坏，很大程度上取决于代价函数怎么配。我用的代价函数包含三项：

跟踪误差项：e(k)^T * Q * e(k)，其中 e(k) 是预测位置与参考位置之间的差，Q 是对角权重矩阵，控制各方向误差的重视程度。

控制量惩罚项：u(k)^T * R * u(k)，防止输入一直处于饱和边缘，也能起到平滑输出的作用。

控制增量惩罚项：Δu(k)^T * S * Δu(k)，限制相邻两个周期的控制变化幅度，让飞行轨迹更平顺。

终端代价项也很关键。预测区间最后一刻的状态偏差如果完全不管，末端行为容易失控，尤其是目标在预测结束时还在远离无人机的情况。我加了一个终端加权项：J_terminal = (e(N))^T * P * e(N)，相当于在有限预测时域的尾部增加一个“虚构无穷时域”的稳定性牵引。

防碰撞和安全约束方面，约束设置如下：

• 推力限制：|u| 各方向最大值，对应实际螺旋桨提供的物理加速度上限；
• 速度限制：|v| 各方向最大值，防止无人机飞得过猛；
• 最小高度限制：pz 不小于安全离地高度；
• 跟踪锥角限制：无人机始终朝向目标方向，且期望偏航角的角速度有限。

这些约束在线性 MPC 里直接写成线性不等式即可，在 NMPC 里则需要保留约束函数的原始非线性表达式，由优化求解器统一处理。

2.3 三种控制器的具体设计差异

线性 MPC 在每一步把非线性模型在当前状态附近线性化，得到线性状态空间模型，然后用二次规划来解。优点是求解速度快，硬件要求低；缺点是线性化误差会随预测时域拉长而累积。

NMPC 直接使用非线性模型做多步预测，优化问题变成非线性规划。我通过 Matlab 的 fmincon 求解器来处理，预测时域为 10 步，采样时间 0.2 秒。求解算法选了 SQP，因为它在处理带约束非线性规划上比较稳健，收敛速度比内点法快一些，尤其适合每步都要实时出结果的场景。

混合 MPC-RL 的接入方式是我整个项目里最需要考虑周全的部分。我用了比较保守的策略：保留 NMPC 作为底层安全控制器，强化学习只负责学习修正权重项。状态由 MPC 误差、误差变化率、平台加速度残差组成；动作控制修正系数，作用于代价函数中的 Q 权重。当平台运动模式发生切换时，RL 能在几个周期内识别出模式变化，把对应方向的误差权重拉高，让无人机更快地校正跟踪偏差。

需要特别说明，这种方法并不是把 RL 完全置身于控制回路之外贪污安全特性。更准确地说，这是一种并行补偿结构：MPC 给动作设定可行域和约束底线，RL 通过调整权重来影响 MPC 的优化方向，但永远不能直接覆盖安全约束。用这套结构跑下来，安全性没有打折扣，适应能力却比纯 NMPC 高了一截。

3. Matlab 实现与代码骨架

3.1 仿真环境的搭建思路

我习惯先把环境拆成四个模块：场景生成模块、控制器模块、无人机动力学模块、评估统计模块。

场景生成模块负责生成移动平台轨迹，我支持 3 种运动模式：直线匀速、圆周运动和随机换向运动。控制器模块分别输出加速度指令；动力学模块根据模型积分生成无人机位置；评估模块负责记录每个周期的误差、控制量、时间戳，方便最后画曲线和统计数据。

仿真主循环每 0.2 秒执行一次，无人机动力学模型内部用更小的步长做积分，模拟真实系统的连续行为。

3.2 线性 MPC 的核心代码

线性 MPC 我没有使用 Matlab 官方 MPC 工具箱，而是自己手写了一个小的预测控制器，用 quadprog 求解二次规划。因为手写版本更容易看清内部逻辑，也不会因为工具箱版本不同导致行为不一致。

代码如下：

function uOpt = solveMPC(pz, vz, refTraj, N, dt) % 状态矩阵：三个通道解耦，各用同样的积分模型 A = [1 dt; 0 1]; B = [0.5*dt^2; dt]; % 将三维问题拆成三个独立通道求解 % 以 x 方向为例 Ac = [A zeros(2,4); zeros(2,2) A zeros(2,2); zeros(2,4) A]; Bc = blkdiag(B, B, B); % 构造预测矩阵 [Phi, Psi] = buildPredictionMatrices(Ac, Bc, N); % 代价矩阵 Q = diag([8 6 8 6 10 6]); R = 0.5 * eye(3); H = Psi' * kron(Q, eye(N)) * Psi + 0.5*R; f = -(refTraj(:) - Phi * x0)' * kron(Q, eye(N)) * Psi; % 约束 lb = repmat([-2; -2; -1], N, 1); ub = repmat([2; 2; 1], N, 1); uOpt = quadprog(H, f, [], [], [], [], lb, ub); uOpt = reshape(uOpt(1:3*N), 3, N)'; end

这里最值得注意的地方是 H 矩阵和 f 向量。手写 MPC 最容易在维度对齐上出错，我曾因为 f 里少了一个转置，导致控制器输出逆着误差方向推，后面烧了半天才找出来。建议在实际测试前，先加一个单步开环预测的单元测试，确保你给的 u 序列能按预期把状态推到目标点。

3.3 NMPC 的核心代码

NMPC 部分，我按“最优控制问题直接转录”的方式实现。决策变量是整个预测时域内的控制序列，目标函数和约束函数直接对标量决策变量列表计算。

function uOpt = solveNMPC(x0, refTraj, uPrev, N, dt) % 决策变量是 N 时刻控制量 [ax, ay, az]，初值取上一拍的结果 u0 = repmat(uPrev, N-1, 1); lb = repmat([-2, -2, -1], N-1, 1); ub = repmat([2, 2, 1], N-1, 1); options = optimoptions('fmincon', ... 'Algorithm', 'sqp', ... 'MaxFunctionEvaluations', 1e6, ... 'StepTolerance', 1e-6, ... 'Display', 'none'); [uOpt, ~] = fmincon(@(u) costFun(u, x0, refTraj, dt), ... u0, [], [], [], [], lb, ub, ... @(u) constraintsFun(u, x0, x0(3), dt), options); uOpt = reshape(uOpt, [], 3); end

costFun 里做多步仿真，计算累计误差和控制代价。这里既然是 NMPC，求解器内部的每一步迭代都会计算一遍完整的前向仿真。我把无人机模型直接写进了 cost 函数，意味着目标函数是可微的，fmincon 的 SQP 能顺利求数值梯度。

function J = costFun(uSeq, x0, refTraj, dt) N = size(uSeq, 1); p = x0(1:3); v = x0(4:6); J = 0; Q = diag([8 8 6]); R = diag([1 1 0.5]); for k = 1:N v = v + uSeq(k, :)' * dt; p = p + v * dt; e = refTraj(k+1, 1:3)' - p; J = J + e' * Q * e + uSeq(k, :)' * R * uSeq(k, :); % 限制高度低于安全值的情况直接在约束函数里处理 end J = J + 25 * norm(refTraj(N+1, 1:3)' - p)^2; end

NMPC 最耗时的就是 fmincon 每次迭代都做数值梯度计算，计算量随着预测时域和状态维度线性增长。我的经验是在预测时域长度和求解精度之间做取舍。N=8 时单步耗时大约 120 毫秒，N=15 时会到 800 毫秒以上。跑实时仿真时我固定用 N=3 或者根据硬件平台灵活调整，研究对比时再用 N=10。

3.4 混合 MPC-RL 的接口实现

混合 MPC-RL 的具体做法我选择了比较轻量的一版：RL 只用一个小型网络输出一个三元素权重修正向量，MPC 的 Q 权重在原有基础上乘以该修正向量。

% 训练好的 RL 模型权重修正向量 deltaQ = rlAgent.predict([err, errDot, platformAcc])'; % 动态调整 Q 权重 QAdaptive = Q0 .* (1 + deltaQ); % 传入 MPC 或 NMPC 代价函数 uOpt = solveNMPCwithQ(x0, refTraj, QAdaptive, dt);

这几行代码看起来很简单，但它背后的训练过程并不轻松。我先在一个仿真场景中用传统 NMPC 跑出一批数据，包含误差、误差变化率、平台加速度变化，以及最终跟踪误差。然后把这些数据作为 RL 环境的状态输入，用 DDPG 算法训练修正策略。训练的奖励函数是误差平方和的负数，也就是希望学到更低的跟踪误差。

训练完成之后，在线推理阶段 RL 部分不做训练，只做前向推断，单次推理不到 1 毫秒，对整体实时性影响很小。这也是我推荐硬件资源有限的情况下，宁可离线训练、在线冻结参数的主要原因。如果项目里执意要做在线策略更新，风险和义务就要大得多。

4. 性能评估：仿真结果与对比分析

4.1 测试场景的设计

为了公平对比，我在同一个仿真环境里跑三种控制器。平台轨迹分三个阶段：第一阶段直线加速，第二阶段突然转入圆周运动，第三阶段随机换向。整个过程总共 120 秒，采样周期 0.2 秒，无人机初始位置在目标点后上方约 5 米的位置。

评价指标选四个：

• 平均跟踪误差：所有采样时刻误差的均方根；
• 最大跟踪误差：整个过程中最大的位置偏差；
• 控制量能耗：所有控制加速度的平方和；
• 单步平均耗时：每个采样周期控制率的计算耗时。

为了防止纯随机场景导致某一次结果偏差太大，每个控制器都跑了 20 轮仿真，取平均值作为最终结果。

4.2 实验结果表格

某一次典型 runs 的数据如下：

从结果看，线性 MPC 的耗时优势非常明显，但误差最大，尤其是平台换向时会出现阶段性的滞后超调。NMPC 平均误差下降了接近一半，代价是计算量增加了 20 倍以上，普通嵌入式飞控大概率扛不住这个负载。混合 MPC-RL 在平均误差上比 NMPC 再降约 25%，单步耗时却控制在了 14 毫秒，接近线性 MPC 的水平。

这个对比的关键在于，误差优势来自 RL 对平台运动模式的自适应调整。比如平台从直线进入圆周的时候，RL 很快识别出 y 方向误差在持续增大，主动提高了 y 方向权重，使无人机提前调节横向加速度，抑制了超调。纯 NMPC 只能依赖固定权重的预测调整，行动会比 RL 晚一到两个周期。

4.3 硬件实测的差异说明

仿真环境里这些数据是在一台常规高性能主机上测的，并不能代表实际飞控的实时性能。如果移植到嵌入式平台，NMPC 的 210 毫秒单步耗时可能进一步涨到秒级以上，根本无法实现实时控制；线性 MPC 和混合 MPC-RL 才有实际落地的可能。这也是很多人最终不用 NMPC 上机的原因之一。混合方案的优势就是算力敏感度更低，同时还能保留一部分高级控制效果。

5. 一路踩过来的坑和调参心得

5.1 参数整定的主次顺序

我强烈建议先调线性 MPC，再切 NMPC，最后再接 RL。线性 MPC 参数少，很容易验证模型和代码是否正确。如果线性版本跑出来的误差曲线都歪七扭八，先不要调控制器，回去检查模型层，看看参考轨迹那边有没有符号错位、初始状态是否合理。

预测时域 N 是最关键的参数之一。N 太小，预测太短，控制器变成“近视眼”，跟踪大机动目标时反应迟钝；N 太大，计算量快速增长，而且远期预测意义越来越小，因为模型误差会被逐步放大。我在线性 MPC 上迭代下来觉得 N=20 左右比较合适，NMPC 则因为计算量的原因，N=10 是平衡点。

权重选择方面，一个比较实用的做法是先从某业务需求出发，比如“横向误差不得大于 0.5 米”，然后把平方误差的阈值转换成对应的 Q 权重。这里有一个常见陷阱：Q 设得过大，控制器会非常激进，输出直接顶到约束边界，整条轨迹抖动明显；Q 设得过小，跟踪滞后感会非常明显。需要反复仿真和试错。

5.2 我在调参时踩过的最大的几个坑

第一个坑是单位不一致导致权重完全失效。飞行器控制里位置单位是米，速度单位是米每秒，加速度单位是米每二次方秒，这三个量如果都进了代价函数，必须先用归一化或者缩放处理。我曾经直接在 Q 对角线写 [5 5 10]，看起来权重合理，实际因为速度单位数值天然比位置大，速度误差在代价里占比莫名偏大，导致控制器过度追求速度一致，位置反而跟踪不准。

第二个坑是参考轨迹时间戳没有对齐。平台轨迹给到控制器时，如果参考序列的第一个点不在当前采样时刻，而控制器默认它存在，跟踪就会有固定滞后。我见过半小时误差曲线，就因为在生成参考轨迹时多插了一帧，修改方式极其简单：索引从 0 改为 1。像这种边界索引问题，没有别的窍门，只能加边界打印帮助排查。

第三个坑是 NMPC 求解器偶尔会收敛到局部最优。在一个测试场景里，平台突然急停加反向运动，fmincon 给的控制序列完全不是预期方向，无人机直接飞反。解决办法有三个：把上一时刻的最优解作为新时刻初值；设置更严格的步长容差；在目标函数里加入一个小的正则项。我最终通过“上一时期解重启动”加“小扰动多初值尝试”的方式解决了问题。

5.3 常见问题速查表

这些坑并非每一个教程都会提到，但它们往往决定了结果是否能达到漂亮指标的“最后一公里”。我实际写代码时有整整两天困在“误差越来越大”的循环里，最终定位到是代码里把参考位置更新错了一行，当你系统不收敛时，先怀疑代码再怀疑算法，效率会高很多。

6. 如果让我再做一遍，我会有哪些调整

首先，我会把移动平台轨迹模块换成一个更真实的传感器模型，加入噪声和遮挡。现在用干净轨迹测试，三种方案差异已经很明显，但真实项目中视觉检测会丢帧、多径效应会带来位置抖动，这对控制器的鲁棒性会产生更大考验。好的控制器不只是能把已知往哪走的平台跟住，更应该能在传感器忽好忽坏时保持安全。

其次，我会把混合 MPC-RL 的输入状态里加入平台的历史运动片段，而不仅仅是当前误差。现在 RL 通过误差和误差变化率来识别平台运动模式，其实已经有不小的预测能力，但把历史轨迹做成一个长度可变的滑动窗口，RL 可能会在更短时间内识别出“换向”这类事件。

另外，代价函数里可以加一个障碍函数项，模拟飞行路径上的临时禁飞区或障碍物。MPC 框架加障碍约束并不难，难的是如何在在线计算中保证可行性。如果未来场景变得更复杂，我可能会考虑在 MPC 和全局路径规划之间加一层轨迹平滑层，而不是让控制器直接处理所有避障对象。

顺便提一句，我自己在仿真阶段用的是一套离线调参加在线仿真的流程，参考价值和可复现性都比较高。如果谁要跑类似课题，我建议先复现“线性对 NMPC”的差异，把基准平台搭好后，再上 RL 混合方案，这样任何一步出了问题，都能做到快速定位到底是在模型层出错还是在学习层出错。仿真永远比实物飞控好定位问题，先把仿真调稳再考虑上机，这个顺序千万不要倒过来。