带约束NMPC车辆轨迹跟踪仿真：从模型到Matlab实践

写这个项目之前，我先说两句实在话：非线性模型预测控制（NMPC）在车辆动力学仿真里真是个“看起来高大上、跑起来谁调谁知道”的模块。不少同学拿到题目第一反应是“那我是不是要写一堆复杂方程？是不是要用特别牛的工具箱？”其实真做起来，核心就三件事：把车模型建对、把约束和目标函数写明白、把求解器喂好。我这个项目正是用 Matlab 完整实现了一套带约束的 NMPC 车辆轨迹跟踪仿真，从车辆模型推导、控制器设计到代码实现和调参都走了一遍，下面把整个过程和踩过的坑分享出来。

这个项目适合三类人看：刚入门 MPC 想找个具体工程场景练手的研究生、做自动驾驶控制仿真需要一套可复现 baseline 的工程师、以及被“仿真老发散”折磨到怀疑人生的同学。我会把每一步为什么这么做讲清楚，也会贴出可以直接改着用的 Matlab 代码片段，不说废话，只讲干货。

1. 为什么选带约束的 NMPC 做车辆轨迹跟踪

1.1 线性 MPC 与 NMPC 的分水岭

预测控制（MPC）本身不是新东西，工业界用了快五十年，核心思想就一句话：在每一个控制周期，基于当前状态，在线求解一个有限时域的最优控制问题，只执行第一个控制量，下一周期滚动重复。

传统线性 MPC 把车辆模型在工作点附近泰勒展开，得到一个线性时不变或线性时变模型，然后用二次规划（QP）求解。这样做的好处是求解快、理论成熟，但问题也很明显：车辆动力学本质上是强非线性的，尤其是车速变化大、轮胎工作点靠近附着极限的时候，线性化带来的模型失配会让预测轨迹“跑偏”，控制器输出自然就跟着出问题。

NMPC 的逻辑则直接得多——不丢掉非线性项，直接在非线性模型上做滚动优化。代价是求解从 QP 变成了非线性规划（NLP），计算量成倍上涨，但对车辆这种工作范围变化剧烈的对象来说，换来的控制精度和稳定性提升是很值的。

我的项目里选的一条典型场景是：车辆以 20m/s 左右速度做双移线工况，过程中车速有波动、横摆角速度变化很大，这种情况下线性 MPC 需要频繁重新线性化才能勉强工作，而 NMPC 用一套模型从头算到尾，逻辑上更干净。

1.2 车辆控制里那些绕不开的约束

车辆控制如果完全不管约束，控制器给出的指令往往在实车上根本执行不了。我把约束分成三类处理：

第一类是执行器硬约束。前轮转角有物理限位，一般在 ±0.5rad 左右；纵向加速度受发动机/制动系统限制，我按加速 2m/s²、制动 -4m/s² 来设。这些约束必须写进优化问题里，否者求解器给出的控制量可能方向盘根本打不到那个角度。

第二类是安全约束。比如车辆横向偏移不能越过车道边界、质心侧偏角不能太大以免失稳、横摆角速度要限制在物理可承受范围内。这类约束代表了对“安全包络”的定义，正是“带约束”三个字区别于普通跟踪控制的关键。

第三类是舒适性约束，严格说这不一定作为硬约束，更多是加在目标函数里作为惩罚，比如控制量变化率（Jerk）、加速度变化率。真当成硬约束容易导致可行域太小，求解器找不到解，所以我的做法是分主次：硬约束只保留执行器和安全边界，软约束放进代价函数。

1.3 项目整体技术路线

我这个项目的完整链路是：建立非线性车辆动力学模型 → 确定状态量、控制量和可测输出 → 设计带约束的 NMPC 优化问题（目标函数+动力学约束+边界约束）→ 在 Matlab 中用非线性优化求解器滚动求解 → 双移线工况仿真验证 → 调权值调参数。整条链路我画了个特别简单的流动顺序：模型 → 预测 → 优化 → 执行 → 更新状态。

仿真时我做了一个很重要的验证动作：同一个工况下，分别跑一次“无约束预测控制”（只是目标函数里不写约束项）和“带约束 NMPC”，对比控制量和轨迹的差异。对比结果非常直观：没加约束的控制量在某些时刻会冲到离谱的数值，加了约束后虽然跟踪误差略大一点，但控制量全程在执行器合理范围内，这才是真正可部署的结果。

2. 车辆动力学模型搭建与离散化

2.1 模型选型：别上来就上八自由度

车辆动力学模型复杂度从二自由度自行车模型到几十自由度整车模型都有。作为控制算法验证，我强烈建议从**单轨动力学模型（自行车模型）**起步。八自由度和 CarSim 联合仿真那些是后话，不是第一版该干的事。

理由很简单：NMPC 每个采样周期要反复积分预测模型，模型每加一个自由度，求解时间可能翻一倍。自行车模型保留了横摆、侧向两个最重要的车辆运动特征，在大范围工况下已经能反映非线性，对验证 NMPC 算法足够用了。等算法和调参跑通了，再替换成更高精度的模型，这时改的只是模型函数接口，控制器代码完全不用动。

2.2 单车模型的状态方程与参数

我采用的自行车模型假设车辆左右对称，把前轮和后轮各合并成一个等效车轮，忽略悬架运动和空气阻力（或只加一个简单的纵向阻力）。状态量我取六个：

• X、Y：车辆在大地坐标系下的位置（m）
• psi：横摆角（rad）
• vx：纵向车速（m/s）
• vy：侧向车速（m/s）
• omega：横摆角速度（rad/s）

控制量是两个：前轮转角 delta（rad）和纵向加速度 a（m/s²）。

连续时间状态方程如下：

code复制dX/dt     = vx*cos(psi) - vy*sin(psi)
dY/dt     = vx*sin(psi) + vy*cos(psi)
dpsi/dt   = omega
dvx/dt    = a
dvy/dt    = (-C_f*(vy + l_f*omega)/vx - C_r*(vy - l_r*omega)/vx)/m - vx*omega
domega/dt = (-l_f*C_f*(vy + l_f*omega)/vx + l_r*C_r*(vy - l_r*omega)/vx)/I_z

这里 C_f、C_r 是前后轮等效侧偏刚度，l_f、l_r 是质心到前后轴的距离，m 是整车质量，I_z 是横摆转动惯量。注意方程里分母中有 vx，这意味着车速接近零时方程会奇异，所以仿真时我给 vx 加了一个很小的保护下限，比如 0.5m/s 以下不启动控制器，这在实际调试里非常重要。

参数我按常见轿车量级设置：

这套参数比较接近一辆前置前驱家用车，双移线工况下能看出明显的侧向动态特性，又不会因为参数太极端导致仿真不好收敛。

2.3 离散化方法：欧拉法与 RK4 的取舍

NMPC 预测模型必须是离散的，因为求解器需要把状态一步步往前推。离散化我用过两种，分别说下感受。

前向欧拉法最直观：x(k+1) = x(k) + Ts * f(x(k), u(k))。它实现简单、计算量最小，但要注意采样时间不能取太大。我测试下来，Ts 取 0.05s（20Hz）时欧拉法还能凑合，取到 0.1s 时预测偏差就开始明显了，轨迹误差会放大。

**四阶龙格库塔（RK4）**精度高很多，在同样 Ts 下预测准得多，代价是每一步要算四次模型函数，耗时约为欧拉法的四倍。在 NMPC 里这四倍时间未必划算，因为你可以用更小的 Ts 配合欧拉法来达到相近精度。

我的最终方案是：预测时域内部用欧拉法，Ts 选 0.05s，预测步数 N 取 20（总预测时域 1s）。实际测试下来这个组合在跟踪精度和求解速度之间比较均衡。如果你发现某个工况下模型失配严重，先考虑减小 Ts，而不是急着换 RK4。

3. NMPC 控制器设计要点

3.1 滚动时域优化的核心逻辑

NMPC 每一拍都在解下面这个带约束的最优控制问题：

code复制min   J = 终端代价 + sum(跟踪误差代价 + 控制量代价 + 控制增量代价)
u(0)...u(N-1)

subject to:
x(k+1) = f_discrete(x(k), u(k))    模型约束
u_min <= u(k) <= u_max              控制量边界
g(x(k)) <= 0                        状态约束
x(0) = x_current                    当前实测状态

关键在于“只执行第一步”：解出完整的最优控制序列 [u0, u1, ..., u_{N-1}] 后，只把 u0 发给车辆模型，下一个采样周期用新的状态重新求解。这就是滚动优化的意义——它让控制器始终基于最新状态做决策，对模型误差和外部扰动有天然的反馈抑制能力。

3.2 目标函数的四个组成项

我的目标函数分四块，每一项都有明确物理含义：

• 终端代价：希望预测时域结束时，状态能落在参考状态附近。加这一项能显著提升稳定性，防止因为预测时域截断导致末段“不管不顾”。
• 过程跟踪误差：预测时域内每个采样点的位置偏差、横摆角偏差和速度偏差。位置偏差权重最高。
• 控制量代价：抑制控制量的幅值，避免剧烈操纵。
• 控制增量代价：惩罚两步之间控制量的变化量 delta_u，这是抑制输出抖动的关键。我强烈建议加这一项，不加的话控制器输出容易高频震荡，实车上没人敢用。

Matlab 里我用函数句柄的方式把目标函数写成单一标量函数，求解器每次迭代会频繁调用它，所以这里我做了个优化：预先分配矩阵、避免在循环里动态增长数组，减少开销。

3.3 约束的梳理与权重初设

约束的数学表达不难，难的是设置合理边界。我列出这个项目实际使用的约束表：

权重矩阵初设我遵循一个经验法则：先让单位统一，再按重要性调倍数。位置单位是米，角度单位是弧度，速度单位是米每秒，如果不做归一化，数值大的项天然占优，权重就白设了。我的初值 Q 矩阵对角线设为 [50, 50, 10, 1]（对应 X、Y、psi、vx 四个跟踪量），R 矩阵取 diag([10, 10])，S 矩阵（增量惩罚）取 diag([1, 1])。这个初值不是最优的，但能保证第一次仿真不会立即发散，后续再微调。

3.4 求解器怎么选：fmincon 还是 CasADi

这是很多新手最容易卡住的环节。Matlab 里做 NMPC 最常见的三条路：

1. fmincon + 手写模型函数：Optimization Toolbox 自带，优缺点都明显。优点是零额外安装、上手快，缺点是求解速度慢，预测步数大了之后很吃力，而且给的解有时会停在局部极小值。
2. CasADi + IPOPT：专门为最优控制设计的工具，符号化建模、自动求导、求解效率高，缺点是 Windows 下配置有点麻烦，需要装 Python 或编译好的二进制包。没有额外工具箱的情况下，这是认真做 NMPC 的首选。
3. MPC Toolbox 自带模块：内置 nlmpc 对象，封装很完整，缺点是“黑盒”感强，出了问题不好排查内部细节。

我这个项目为了把原理讲透，选了 fmincon 手写全部函数。如果你的目标是工程落地或跑大型仿真，建议直接换 CasADi。后者求解效率能快一个数量级，但今天这篇文章我只展开 fmincon 的实现路径，CasADi 的用法以后有机会再单独写。

4. Matlab 仿真实现与代码拆解

4.1 主仿真循环框架

主程序核心结构很清晰：初始化参数 → 生成参考轨迹 → 进入控制循环（更新状态 → 调 NMPC 求解器 → 提取第一个控制量 → 积分模型推进一个采样周期）→ 记录数据 → 绘图。伪代码流程如下：

matlab复制% 参数初始化
Ts = 0.05;               % 采样周期
N = 20;                  % 预测时域步数
T_end = 8;               % 仿真总时长
steps = T_end / Ts;      % 总步数

% 状态初值
x0 = [0; 0; 0; 20; 0; 0];  % X, Y, psi, vx, vy, omega
x = x0;

% 参考轨迹生成（双移线）
[X_ref, Y_ref, psi_ref, vx_ref] = generateDoubleLaneReference(...);

% 数据记录
log_state = zeros(6, steps);
log_u = zeros(2, steps);

% 主循环
for k = 1:steps
    % 求解 NMPC，得到最优控制序列
    [u_opt, ~] = solveNMPC(x, X_ref(k:k+N), Y_ref(k:k+N), ...);
    
    % 只取第一个控制量
    u = u_opt(:, 1);
    
    % 用车辆模型推进采样周期
    x = vehicleModelRK4(x, u, Ts);
    
    % 记录
    log_state(:, k) = x;
    log_u(:, k) = u;
end

这里的 solveNMPC 是核心模块，它内部做三件事：把当前状态和参考轨迹打包、调用 fmincon、把解拆成控制序列返回。函数签名建议设计得干净一点，方便后续替换成 CasADi 版本。

4.2 动力学函数、代价函数和非线性约束代码

动力学函数是整个仿真和预测共用的基础模块。我把它写成独立的 m 函数，保证“仿真推进”和“预测模型”用的是同一套方程，避免两边模型不一致导致“仿真里明明没问题、预测却一塌糊涂”的尴尬：

matlab复制function x_next = vehicleModelEuler(x, u, Ts, p)
    % 状态: x = [X; Y; psi; vx; vy; omega]
    % 控制: u = [delta; a]
    delta = u(1); a = u(2);
    vx = x(4); vy = x(5); omega = x(6);
    
    % 防止低速奇异
    vx = max(vx, 0.5);
    
    f = zeros(6, 1);
    f(1) = vx*cos(x(3)) - vy*sin(x(3));
    f(2) = vx*sin(x(3)) + vy*cos(x(3));
    f(3) = omega;
    f(4) = a;
    f(5) = (-p.Cf*(vy + p.lf*omega)/vx - p.Cr*(vy - p.lr*omega)/vx)/p.m - vx*omega;
    f(6) = (-p.lf*p.Cf*(vy + p.lf*omega)/vx + p.lr*p.Cr*(vy - p.lr*omega)/vx)/p.Iz;
    
    x_next = x + Ts * f;
end

代价函数需要把整个预测时域的状态轨迹都算出来，然后累加各项误差。这是 fmincon 优化时调用最频繁的函数，所以我把状态预测和代价累加放在同一个函数里，省掉重复调用模型带来的冗余计算。下面是简化版的结构：

matlab复制function cost = nmpcCost(u_seq, x0, ref, p, Q, R, S, Ts)
    N = size(u_seq, 2);
    x = x0;
    cost = 0;
    % 终端代价权重 P，这里简化为放大 Q
    P = Q * 10;
    
    for k = 1:N
        % 状态预测一步
        u = u_seq(:, k);
        x_next = vehicleModelEuler(x, u, Ts, p);
        
        % 参考偏差
        e = x_next - ref(:, k+1);
        cost = cost + e' * Q * e;
        cost = cost + u' * R * u;
        
        if k > 1
            du = u_seq(:, k) - u_seq(:, k-1);
            cost = cost + du' * S * du;
        end
        
        x = x_next;
    end
    
    % 终端代价
    eN = x - ref(:, N+1);
    cost = cost + eN' * P * eN;
end

非线性约束函数是“带约束”的直接体现。fmincon 支持通过 nonlcon 参数传入非线性约束的等式和不等式函数。这里面要注意：控制量上下限直接在 ub/lb 里声明，比写在 nonlcon 里更高效；状态约束则必须写成 nonlcon：

matlab复制function [c, ceq] = nmpcConstraints(u_seq, x0, ref, p, Ts)
    N = size(u_seq, 2);
    x = x0;
    c = [];
    ceq = [];
    
    for k = 1:N
        u = u_seq(:, k);
        x_next = vehicleModelEuler(x, u, Ts, p);
        
        % 状态约束：侧偏角限制、横向偏移限制
        vx = max(x_next(4), 0.5);
        slip_angle = atan(x_next(5) / vx);
        c = [c; slip_angle - 0.15; -0.15 - slip_angle];
        c = [c; x_next(2) - ref(2, k+1) - 0.5; -(x_next(2) - ref(2, k+1)) - 0.5];
        
        x = x_next;
    end
end

主循环里调用 fmincon 的部分要特别处理初值问题：上一个采样周期求出的控制序列就是下一个周期最好的初始猜测。这种“热启动”策略能让求解速度提升非常多：

matlab复制function u_opt = solveNMPC(x_current, ref_segment, u_prev, p, params)
    N = params.N;
    lb = [repmat([-0.5; -4], 1, N)]';
    ub = [repmat([0.5; 2], 1, N)]';
    
    % 热启动：用上一拍的控制序列作为初值
    if isempty(u_prev)
        u_init = zeros(2, N);
    else
        u_init = [u_prev(:, 2:end), u_prev(:, end)];
    end
    
    options = optimoptions('fmincon', ...
        'Algorithm', 'sqp', ...         % 序列二次规划法
        'MaxIterations', 200, ...
        'OptimalityTolerance', 1e-4, ...
        'ConstraintTolerance', 1e-5, ...
        'Display', 'off');              % 仿真过程中关掉输出
    
    fun = @(u_seq) nmpcCost(u_seq, x_current, ref_segment, p, params.Q, params.R, params.S, params.Ts);
    nlcon = @(u_seq) nmpcConstraints(u_seq, x_current, ref_segment, p, params.Ts);
    
    [u_opt, ~] = fmincon(fun, u_init(:), [], [], [], [], lb(:), ub(:), nlcon, options);
    u_opt = reshape(u_opt, 2, N);
end

这段代码里我用了 sqp 算法。我的经验是 interior-point 和 sqp 都能用于 NMPC，但 sqp 在热启动时表现更稳，不容易因为初值偏离可行域而不收敛。

4.3 权重参数与仿真工况设置

参考轨迹我采用经典双移线（Double Lane Change）工况，模拟车辆高速变道超车再变回原车道。轨迹由两段回旋线和直线段拼接而成，横向总偏移 3.5m。车速参考设常值 20m/s。

权重矩阵我经过几轮调试后最终定成：

code复制Q = diag([30, 80, 10, 1])
R = diag([8, 8])
S = diag([2, 2])

解释一下为什么 Y 方向的权重最高：双移线场景里核心任务是横向精准变道，Y 偏差直接影响是否压线。X 方向权重次之，是因为它主要跟着车速走，不用死磕。psi 角权重也不能太低，否则车身姿态会在变道过程中失衡。vx 权重最低，因为纵向速度基本恒定，不需要控制器花太多“精力”去拽它。

仿真结果从数据上看，最大横向跟踪误差在 0.12m 左右，前轮转角峰值 0.03rad 左右，全程没有超出约束边界，控制器输出的横摆角速度曲线比较平滑，没有出现高频抖振。相比同一组参数但去掉约束项的实验，带约束版的 u 序列全程落在执行器范围内，不带约束版在某些时刻会算出一个超大的前轮转角——这在实车上根本执行不了。

5. 调参经验与典型问题排查

5.1 仿真发散的排查顺序

做 NMPC 仿真，十次有八次会在“发散”上栽跟头。我自己的排查顺序是这样的，供你参考：

第一看采样周期是否过大。很多发散纯粹是离散化误差积累导致的，把 Ts 从 0.1s 调到 0.02s 可能立刻就好了。第二看状态初值是不是给得乱七八糟，尤其是横摆角 psi 初始值和参考轨迹起点差距太大，预测模型很容易在第一步就算出不可行解。第三检查预测时域内是否出现约束互斥，比如参考轨迹 Y 突然跳变太大，而我给横向偏移约束 0.5m，这时可行域可能是空的，求解器返回的“最优解”根本不满足约束。我常用这招排查：把约束暂时放松十倍，如果能跑通，问题就出在约束打架上，而不是模型写错了。

还有一个很容易被忽略的源头：低速奇异。我说的自行模型里那个 vx 除法，如果车辆状态里 vx 接近零，方程直接爆掉。这是一个特别经典的坑。

5.2 求解慢的三个解决思路

NMPC 慢是常态，关键看慢得能不能接受。fmincon 版的求解时间在我的机器上大概 0.2~0.5s，而我的采样周期只有 0.05s，也就是说实时性完全不够，但作为离线仿真验证是没问题的。如果你想让它在实时环境里跑，按下面三步优化：

1. 降低决策变量维度：把预测时域 N 从 20 减到 10，或对末段控制量做参数化（比如末 5 步控制量保持不变），决策变量直接减半。
2. 提供解析梯度：fmincon 默认用有限差分算梯度，代价是每个变量额外跑若干次模型。我用 CasADi 对目标函数自动求导后，求解时间降到手写 fmincon 的十分之一不到。这就是为什么真正做工程的人更愿意用 CasADi。
3. 更优的热启动策略：不只是把上一拍的控制序列顺延，还可以把上一步最优的拉格朗日乘子传进去，fmincon 支持这种高级初始值设置，收敛会快不少。

5.3 跟踪误差大的调权技巧

如果你发现轨迹跟踪误差偏大，又排除了模型和约束的问题，那基本就是目标函数权重没调到位。我摸索出一个不算特别严谨但很实用的调参方法：先固定 R 和 S，只调 Q；跑通之后再反过来固定 Q，稍微动 R 和 S。千万别一次动三个矩阵，否则出了问题根本不知道是谁引起的。

调 Q 的时候观察两个指标：如果 Y 误差大，就把 Q(2,2) 加大；如果车身姿态不正、进入弯道有明显滞后感，就增大 Q(3,3)。R 和 S 的调节逻辑相反：R 增大控制量更平缓但跟踪变差，S 增大控制增量更平滑但响应变迟钝。我最后是从 Q(2,2)=40 一路调到 80，才在跟踪精度和控制平滑度之间找到满意的平衡点。

常见问题和调试方向我整理了一个速查表：

最后再分享一个小技巧：写 NMPC 仿真时，第一版千万别追求复杂工况。我每次起步都是直线轨迹、低车速、大约束范围，先把算法链路跑通，确认控制器能稳定工作，再逐渐提高车速、收窄约束、换成双移线。这样做最大的好处是，一旦发散，你能立刻判断问题出在哪个环节，而不是在一堆复杂设置里大海捞针。

这个项目做完之后我的体会是，NMPC 的难点其实不在“非线性”这三个字上，而在“约束”与“实时性”之间的平衡。约束加得多，可行域变小，求解变难；约束加得少，控制结果没有实用价值。把每一类约束想清楚加在目标函数里还是硬约束里，把每一步求解的初值喂好，整个控制器基本就能稳定工作了。后续如果你想扩展，可以把线性轮胎模型换成魔术公式轮胎模型，或者把预测模型换成 CarSim 导出接口，控制器的召回结构不需要大改，这恰恰是 NMPC 框架的扩展性优势所在。