基于NMPC的移动机器人点镇定与避障仿真实现

做移动机器人控制的人，十有八九都被一个问题折磨过：车已经开到目标点附近，不撞墙、也不触障，却总在那一带画圈，姿态怎么都拧不过来，最后干脆把控制器调成一个"原地转圈器"。这个问题的学名叫点镇定（point stabilization），看似简单，却是非完整约束系统里的经典硬骨头。而用非线性模型预测控制（NMPC）来啃这块骨头，最大的好处是能把"到不到得了"和"撞不撞得到"统一放进一个优化问题里，既管终点精度，又管路径安全。这篇文章就围绕这个项目标题，把NMPC解决静态与动态障碍物环境下的点镇定问题，从原理、Matlab代码到Simulink仿真，完整拆一遍，顺便把我在实际调参中踩过的坑都抖出来。适合正在做移动机器人、无人车或相关课程项目的同学参考，也适合想从轨迹跟踪转向镇定问题的人做个铺垫。

如果你手头已经有PID或者LQR版本的点镇定程序，那这篇文章的价值可能更大。因为你会发现：很多在PID/LQR里近乎无解的矛盾，放到NMPC里反而成了几个约束条件的事。

1. 任务拆解：点镇定问题与NMPC的匹配逻辑

1.1 点镇定为什么"小"却"难"

点镇定的问题描述非常简单：给定一个非完整约束的轮式机器人（例如常见的单轮车模型），状态是平面位置和朝向角 (x, y, \theta)，控制输入是线速度 (v) 和角速度 (\omega)，任务是从任意初始状态出发，最终让机器人停在指定的目标点 ((x_{ref}, y_{ref})) 并且有一个期望的最终朝向 (\theta_{ref})。

轨迹跟踪不难，镇定到原点却一直是个理论难题。这里有一个非常出名的结论：对于非完整约束系统，不存在连续光滑的时不变状态反馈律能同时保证平衡点的渐近稳定。换句话说，你想写一个 (u = f(x)) 的光滑函数，一劳永逸地实现 "只要车没到目标点，就一定有控制量把它推过去"，这在数学上已经不成立了。这也就是为什么PID在实际调的时候总会出现"接近目标点之后控制量急剧减小，然后车子在终点附近缓慢漂移"的现象。不是你的PID参数没调好，是这个系统结构限制了光滑反馈的镇定能力。

那NMPC为什么能干这件事？因为它是"有限时域优化"，每一次控制量的计算都基于当前状态重新求解一个开环最优控制问题。这个求解过程相当于每时每刻都在构造一个"新的反馈律"，允许控制策略随时间变化，也就绕开了光滑时不变反馈不能镇定的理论限制。所以哪怕系统本身不能用简单的 (u = kx) 来镇定，NMPC依然可以通过反复滚动优化把状态压到目标点。

1.2 相对于轨迹跟踪和LQR，NMPC的核心优势

很多人一开始会有疑问：既然LQR也能处理约束，为什么非要NMPC？实际上LQR在点镇定场景下有两个明显的痛点。

第一个痛点是LQR本质上假设系统在参考点附近线性化，而点镇定问题的初始状态往往距离目标点很远。如果直接把非线性模型在目标点附近做线性化，而出发点偏离又大，那线性化误差在一开始就非常可观。为了压制这种误差，你只能把增益调得特别小，结果就是控制效率极低，甚至出现"慢吞吞地爬向目标点"的观感。

第二个痛点是约束处理。LQR处理输入饱和、状态限幅这类约束相当别扭，往往要引入anti-windup或者额外的条件判断逻辑，而且一旦同时考虑多个障碍物约束，LQR基本上就没有规范化手段了。NMPC从问题定义上把约束当成一等公民：输入限幅直接写进优化问题的约束项，障碍物信息也作为非线性不等式约束加进去，整个求解过程是统一且可解释的。

动态障碍物场景下，NMPC还有一个隐藏优势：它天然适配"预测"这个概念。动态障碍物不是静止的，它每一时刻都在移动。NMPC在做预测时可以将障碍物的运动模型并入预测模型，例如假设障碍物保持当前速度匀速运动，然后在预测时域内的每个采样时刻计算出对应的"动态禁入区域"。这种放在LQR里几乎不知道怎么建模的东西，在NMPC里只是一个额外的时变约束。

所以你可以把NMPC理解成一个"每次控制都重新做全局规划"的控制器。代价是计算量上升，换来的是约束处理能力和对非线性的容忍度，特别适合这类"既要镇定精度、又要避障安全"的综合任务。

2. NMPC方案的核心数学设计与参数选型

2.1 运动学模型与离散化方法

点镇定问题使用的被控对象一般是一个简化运动学模型。我这里以单轮车模型为例：

[ \begin{cases} \dot{x} = v \cos\theta \ \dot{y} = v \sin\theta \ \dot{\theta} = \omega \end{cases} ]

其中 (x, y) 是机器人在地图中的位置，(\theta) 是航向角，(v) 为线速度，(\omega) 为角速度。这里我刻意不引入动力学模型，因为项目标题里的重点是镇定和避障控制，而不是电机特性辨识。如果你的硬件平台有明显的加速延迟，再往状态量里加上速度状态即可。

NMPC在做预测控制时，需要把连续时间模型离散化。最常见的做法是欧拉法，一步差分得到：

[ \begin{cases} x_{k+1} = x_k + T v_k \cos\theta_k \ y_{k+1} = y_k + T v_k \sin\theta_k \ \theta_{k+1} = \theta_k + T \omega_k \end{cases} ]

(T) 是控制周期，我常用的取值是0.05s到0.2s。欧拉法实现简单，但如果你发现预测轨迹存在明显失真，可以改成二阶Runge-Kutta（中点法），代码上不过多花几行，却能大幅提高预测精度。仿真里我用的是二阶RK法，因为当控制周期偏大时（大于0.1s），欧拉法会让预测出来的转弯轨迹和实际Simulink模型差别较大，导致NMPC算出的控制量在实物上不是最优的。

2.2 代价函数、终端代价与约束

NMPC的优化问题是一个典型的带约束非线性优化问题。整体形式如下：

[ \min_{u_{0:N-1}} J = \sum_{k=0}^{N-1} \left( \Delta x_k^T Q \Delta x_k + u_k^T R u_k \right) + \Delta x_N^T Q_N \Delta x_N ]

其中 (\Delta x_k = [x_k - x_{ref},\ y_k - y_{ref},\ \theta_k - \theta_{ref}]^T)，(Q) 是状态误差权重，(R) 是控制量权重，(Q_N) 是终端代价权重。

终端代价这一项特别关键。纯靠有限时域的stage cost，机器人往往在预测时域的末端"还没到目标点就松懈了"，因为后面没有惩罚了。加了终端代价之后，才能把“最终能不能停在目标点”这个目标显式地塞进优化问题里。我常用的做法是令 (Q_N = Q)，或者把终端权重调得比 (Q) 大一些，比如10~50倍，用来强化终点精度。不过终端权重也不能无脑加大，加得太大可能导致优化问题在接近目标点时过度拥挤，求解器反复抖动。

约束分为两类：

一是输入约束：

[ |v| \le v_{max}, \quad |\omega| \le \omega_{max} ]

这个没有任何争议，仿真里通常取 (v_{max} = 1.0\text{ m/s})，(\omega_{max} = 1.5\text{ rad/s})。注意角速度上限不要给得太紧，否则点镇定末段朝向调整会非常慢。

二是避障约束。对于静态障碍物，用圆形禁入区的近似：

[ (x_k - x_o)^2 + (y_k - y_o)^2 \ge r_{safe}^2 ]

(r_{safe}) 是机器人半径和障碍物半径之和再加上一个安全裕量。对动态障碍物，我采用"预测位置禁入"的原则，即障碍物在以当前速度和方向匀速运动的前提下，未来每个预测时刻有一个对应的位置约束。

2.3 障碍物建模和动态避障的核心技巧

静态障碍物处理起来最直接，但动态障碍物处理起来稍微讲究。动态障碍物在仿真中的状态一般用 ((x_o, y_o, v_{ox}, v_{oy})) 表示，位置和速度都属于已知信息。预测一步：

[ x_{o,k} = x_o + v_{ox} \cdot k T,\quad y_{o,k} = y_o + v_{oy} \cdot k T ]

然后把每个预测时刻 (k) 的障碍物位置代入避障约束。这样NMPC在求解时就相当于在预测时域内对一条"正在移动的障碍物轨迹"避让。这里有一个重要技巧：不要在约束里直接用 ((x_k - x_{o,k})^2 + (y_k - y_{o,k})^2 \ge r_{safe}^2) 这种形式硬解，最好引入一个松弛变量，把避障约束转成软约束：

[ r_{safe}^2 - \left[(x_k - x_{o,k})^2 + (y_k - y_{o,k})^2\right] - \epsilon_k \le 0,\quad \epsilon_k \ge 0 ]

然后把 (\epsilon_k) 加一个较大的惩罚系数放进代价函数。这个技巧的核心原因是NMPC的求解器对初始可行解非常敏感。如果某个时刻机器人的初始预测轨迹已经穿过了障碍物，而避障约束又是硬约束，那么求解器在物理意义上就直接无解，fmincon这类局部优化器很容易陷入inf。引入软约束后，求解器至少能找到一个让机器人和障碍物距离尽量大的可行解，等预测轨迹调整过来后，惩罚项自然会收敛到零。我在实际仿真中，把这个技巧称作"避障约束的保底机制"。

另一个动态避障的关键点是障碍物运动的预测模型。真实环境中障碍物可能会突然加速或转弯，不确定度会累积。仿真阶段可以先假设匀速，但要注意让障碍物的速度不要设计得太过刁钻，否则难度主要来自"预测失灵"而不是控制算法本身。合理的动态障碍物测试场景是：障碍物沿一条直线或者缓慢曲线移动，机器人需要判断"直接冲过去会不会撞"以及"如果要绕，从左边绕还是右边绕"，这个逻辑会在预测轨迹中反映出来。

3. Matlab实现流程：从伪代码到可跑工程

3.1 主程序框架与初始化

Matlab这边的实现，核心不复杂，但工程组织要清晰。我推荐把整个工程分成几个文件：

• main_simulation.m：主脚本，负责参数初始化、循环求解、状态更新、画图。
• nmpc_solve.m：NMPC核心优化函数，输入当前状态、障碍物状态、参考点，输出最优控制序列。
• single_track_model.m：被控对象的离散化运动学方程。
• dyn_obstacle_trajectory.m：动态障碍物轨迹生成函数。

主程序里最关键的是初始化。仿真中我们先给机器人一个远离目标点的初始状态，比如：

x0 = [-2; -1.5; pi/3]; % 初始位置和航向角 x_ref = [2; 2; 0]; % 目标点位置和航向角

同时定义一个障碍物结构体，因为静态和动态障碍物数量不多，我习惯用结构体数组存储：

obstacles_static = struct('x', [0.0, 1.0], 'y', [1.5, 0.2], 'r', [0.3, 0.3]); obstacles_dyn = struct('x0', [0.5], 'y0', [0.8], 'vx', [0.3], 'vy', [0.15], 'r', [0.3]);

这里静态障碍物给了两个，动态障碍物给了一个。注意动态障碍物的x0, y0是初始位置，vx, vy是速度分量。

主循环里面做的事比较机械：得到当前状态 → 调用NMPC求解器 → 取出第一个控制量 → 用仿真模型更新状态 → 记录数据。我一般会做一个固定步长的循环，步长设为 (T)，总共仿真 (N_{sim}) 步。如果想更接近Simulink的风格，也可以把主循环改成ode45+零阶保持，但固定步长循环的调试效率更高，输出也更好分析。

3.2 NMPC核心函数与求解器配置

nmpc_solve.m是整个工程的心脏。在这个函数里，你要完成三件事：预测状态轨迹、构建代价函数、求解非线性优化问题。Matlab自带的fmincon可以直接用来求解小规模NMPC，最适合教学和验证。我这里给出一个骨架：

function [u_opt, x_pred] = nmpc_solve(x_current, x_ref, obstacles, param) N = param.N; % 预测时域长度 T = param.T; % 控制周期 v_max = param.v_max; w_max = param.w_max; % 优化变量：每个时刻的 v 和 omega，共 2*N 个 u_init = zeros(2*N, 1); % 代价函数内部需要重构状态轨迹 cost_fun = @(u) nmpc_cost(u, x_current, x_ref, obstacles, param); % 约束函数 cons_fun = @(u) nmpc_constraints(u, x_current, obstacles, param); options = optimoptions('fmincon', ... 'Algorithm', 'sqp', ... 'MaxIterations', 150, ... 'MaxFunctionEvaluations', 2e4, ... 'OptimalityTolerance', 1e-6, ... 'Display', 'off'); % 变量边界 lb = [-v_max * ones(N,1); -w_max * ones(N,1)]; ub = [ v_max * ones(N,1); w_max * ones(N,1)]; [u_opt, fval, exitflag] = fmincon(cost_fun, u_init, [], [], [], [], lb, ub, cons_fun, options); end

这里几个点你需要特别留意。

第一，(u_{init}) 的初始猜测。仿真中每一时刻可以拿上一时刻的最优控制序列平移一个周期来作为初值，这样求解速度会快很多。但首次求解时没有上一时刻的序列，我用的是全零初值。实测下来，在障碍物复杂度不高的情况下，全零初值也能缓过来，只是第一个周期求解时间会长一点。

第二，为什么用sqp算法而不是默认的interior-point。NMPC的问题经常带有较强的非线性约束，sqp在处理非凸约束时相对更稳定，不容易被卡在那些"约束抖动"的位置。但这也不是绝对的，如果遇到fmincon的sqp提示约束不收敛，改成interior-point再试试。项目里我两个都跑过，最终sqp的综合表现更好。

第三，约束函数chap里需要把障碍物和输入限幅都传进去。输入限幅其实可以在lb/ub里直接处理，我仍保留在约束里是为了让目标函数更容易理解。但实际写的时候，lb/ub就够了，约束函数里只写避障约束即可，避免重复约束导致求解器行为怪异。

3.3 静态与动态障碍物的生成逻辑

再展开说一下障碍物在代码里怎么处理。

静态障碍物比较简单。当预测轨迹解到某时刻状态 (x_k)，把所有静态障碍物逐一检查距离：

for k = 1:N pos_k = [x_pred(1,k); x_pred(2,k)]; for i = 1:num_static d2 = sum((pos_k - [obs_s(i).x; obs_s(i).y]).^2); con = con + (r_safe(i)^2 - d2); end end

写成不等式约束 (g \le 0) 的形式，所以上面返回的con是约束值。如果约束违反，con > 0。

动态障碍物的处理略复杂一点。关键在于每一个预测时刻 (k)，对应的障碍物位置可能都不同。我单独写一个函数计算障碍物在第 (k) 步的位置，避免在约束里写重复代码：

function p_obstacle = get_obstacle_pos(obs_dyn, k, T) t = k * T; p_obstacle = [obs_dyn.x0 + obs_dyn.vx * t; obs_dyn.y0 + obs_dyn.vy * t]; end

然后循环所有预测时刻，把当前时刻的障碍物位置代入避障约束。这里有一个经验：对动态障碍物，约束要同时检查所有障碍物（静态+动态），不要把静态和动态分开处理，否则障碍物数量一多，循环里容易漏约束。

还有一点要强调：障碍物半径不要选择过小。仿真中看起来机器人能"擦着边"过去，但实际控制有延迟和建模误差，擦边操作很容易撞上。我设为安全半径 = 障碍物半径 + 机器人半径 + 0.1m 裕量。这个0.1m在点镇定末端可能会让收敛路径显得保守一点，但换来的是全程不容易出现“看起来没撞、实际上已穿过禁入区”的约束违规。

3.4 从NMPC求解器回传轨迹的调试技巧

一个非常实用的调试技巧是让NMPC求解器每次除了回传控制量，还把最优预测轨迹也输出。在仿真主循环里把这些预测轨迹直接叠加画到机器人运动轨迹图上，你会看到"当前时刻机器人未来几步打算怎么走"。这个图对判断约束失效非常有帮助。如果某一次求解失败，你能立刻看到fmincon返回的轨迹是否穿过了障碍物。

同时，exitflag主要检查三个值：1表示函数收敛到局部最小，0表示迭代次数超出，-2表示无可行解。仿真实战中我见到最多的是-2，原因几乎都是避障约束在某个预测时刻过紧，或者障碍物速度设置导致预测轨迹没有可行通道。这时把避障约束中的 (\epsilon) 软约束权重调大即可缓解。

4. Simulink仿真架构与联调过程

4.1 顶层架构设计

如果你决定把控制器在Simulink里跑，顶层架构一般分三大部分：控制器模块、被控对象模块、数据记录模块。控制器模块最实用的是用一个MATLAB Function块，里面直接调用你自己写的nmpc_solve函数。被控对象模块可以直接用积分器搭建运动学模型，或者用S-Function实现离散模型。数据记录模块则用Scope和To Workspace把轨迹输出到Matlab工作区用于绘图。

Simulink的采样时间设置非常关键。NMPC控制器需要按控制周期 (T) 来触发计算，被控对象模型最好用连续时间积分器或采用更小的积分步长。我常用两种配置：

• 控制器采样时间 (T_c = 0.1s)，模型积分器使用变步长，最大步长限制在0.01s。
• 控制器采样时间 (T_c = 0.1s)，模型也使用离散求解器，步长0.01s，控制器模块每10步触发一次。

两种方式跑出来的结果基本一致。不过第一种方式更容易在Simulink里接入连续时间被控对象，适合后续加电机动力学或传感器模型。

MATLAB Function块里面状态量通过输入端口读入。一个常见坑是MATLAB Function块默认不支持全局变量，所以nmpc_solve内部用到的参数结构体要么作为输入端口传进去，要么写成外部函数的附加参数。我习惯把参数定义成一个基底工作区变量，在MATLAB Function块内用coder.extrinsic声明，或者干脆把参数硬编码进NMPC求解函数内部。考虑到教学项目，我建议直接把参数硬编码在nmpc_solve里，少一个端口少一份麻烦。

4.2 被控对象建模与可视化输出

被控对象模型我用积分器搭了一个单轮车运动学模型：

function [x_dot, y_dot, theta_dot] = kinematic_model(x, y, theta, v, omega) x_dot = v * cos(theta); y_dot = v * sin(theta); theta_dot = omega; end

在Simulink里把 (x, y, \theta) 分别用三个Integrator模块，控制输入 (v, \omega) 从控制器模块出来，经过一个零阶保持器送给积分器。积分器初值就是机器人的初始状态。

动态障碍物模块我用一个独立的子系统实现，输入仿真时间 (t)，输出障碍物的当前坐标。在这个子系统中定义一个Signal Editor或者MATLAB Function，实现匀速直线运动。如果想做得更丰富，可以让障碍物在运动中徘徊或者做正弦轨迹运动，但要注意保证障碍物轨迹本身不穿过机器人的必经通道太长时间，否则求解器几乎永远找不到可行解，仿真一跑就崩。

Simulink仿真开始后，要注意把输出轨迹数据用To Workspace保存。我在仿真结束后，会有一段统一的后处理代码，把机器人轨迹、障碍物轨迹、参考点、控制量曲线全部画在一张图上，用不同颜色区分。这个图的纵坐标不需要统一，因为控制量和位置量纲不同。我一般画两个子图，一个平面轨迹图，一个控制量随时间变化图。平面轨迹图里把障碍物画成圆，再把动态障碍物的轨迹虚线画出来，一眼就能看出机器人是否成功绕开。

4.3 Simulink与Matlab版本差异的兼容性问题

做Simulink仿真最容易遇到的问题就是版本兼容。MATLAB Function块里如果用了较新的语法，在老版本Matlab上会报错。比如用arguments关键词声明函数参数，或者在约束函数里用了optimvar，老版本都不支持。我的建议是：仿真项目尽量使用最朴素的函数定义方式，不要贪图新语法，能用function [u, fval] = ...这种经典形式就别用arguments。毕竟项目的核心是控制算法本身，不是函数定义模式。如果要在不同版本的Matlab间共用，可以使用isMATLABReleaseOlderThan来做条件判断，但这个函数本身只在新版本才有，所以最稳妥的做法还是选择低版本兼容的写法。

另外，如果你装了CasADi或者IPOPT，Matlab版本之间也可能有兼容问题。我的建议是先用fmincon把整套逻辑跑通，再考虑是否切换高性能求解器。对静态/动态障碍物数量不多、预测时域不超过10的仿真场景，fmincon完全够用，单个控制周期求解时间大概在几十毫秒到两三百毫秒之间，足够满足慢速机器人仿真。

5. 调参、踩坑与常见问题速查

5.1 最容易出问题的三个环节

我调试这个项目期间走了不少弯路，总结下来有三大类问题出现频率最高。

第一类：预测时域N与权重Q/R之间的配合失误。如果N太小，比如N=5，机器人在预测时域内根本看不到障碍物，避障约束相当于虚设。N=10时，预测轨迹能提前看到0.5s后的障碍物，避障效果已经比较明显。但如果N=30，优化变量数量达到60个，fmincon求解时间直线上升，而且预报时间长导致终端代价权重对轨迹影响过大，机器人容易出现"一开始就猛转弯去找未来目标点"的奇怪路径。所以我最终把N固定在10~15之间，平衡了求解时间和前瞻性。

权重矩阵Q/R的调节，最基本的原则是：状态误差权重越大，机器人越积极地向目标点靠拢；控制量权重越大，控制动作越平缓，但收敛也越慢。如果Q中的朝向角权重给得很大，机器人会优先调整姿态，位置收敛变慢。反过来，朝向角权重太小，末端可能长时间停留在目标点附近但方向不对。我调试过程中发现，位置权重和朝向角权重取2：1的关系比较自然，然后在R里留一个小的控制量权重（0.1左右），保证控制量不至于抖得特别厉害。

第二类：约束求解发散。NMPC最常见的问题是fmincon返回无可行解，然后机器人就停在原地不动了。我的对策是上节提到的软约束松弛变量。将避障约束转成软约束后，几乎不会再遇到"完全无解"的情况，最差情况是机器人以很小的代价贴近障碍物。此时你会看到预测轨迹和障碍物产生轻微重叠，但这也提醒你：安全裕量还不够大，或者动态障碍物太难。两者稍微调一下就能解决。

第三类：动态障碍物预测模型和被控对象模型的不匹配。动态障碍物的轨迹有时候会和机器人路径出现"抱死"状态——预测时域内看起来反正能绕过，实际多走几步发现障碍物已经把路堵死。这本质上是设置障碍物速度时没考虑机器人的机动能力。比如机器人最大线速度1m/s、最大角速度1.5rad/s，动态障碍物速度却有0.8m/s，且运动方向和机器人路线呈夹角，那难度就非常高。我的经验是动态障碍物速度设置在机器人最大速度的30%以下，同时给机器人留出至少1倍车身长度的横向绕行空间，仿真效果才会好看，否则就成 "恶意避障" 了。

5.2 常用参数表与调参心得

下面这个表是我在仿真里最终使用的参数组合，实测下来在静态障碍物和单个动态障碍物环境下，机器人基本都能完成点镇定任务。

调参的心得是“先松后紧”。先把Q、R都调得比较温和，给一个较慢的收敛速度，然后逐步加大权重。一开始就上高强度权重，很容易在仿真前2秒出现剧烈抖动，然后约束失效，问题还特别难排查。先确认系统能稳定收敛到目标点，再加障碍物；先加静态障碍物，观察避障轨迹是否平滑；确认无误后再加动态障碍物。这样一步步来，每个阶段的问题都非常清晰。

还有一个冷门但重要的心得体会：不要盲目相信NMPC的 "最优性"。仿真中经常出现机器人选择了一条“最优但很绕”的路径。它明明可以直接从障碍物旁边绕过去，却绕了一个大圈。这是因为NMPC的求解器只在局部搜索，它对全局路径优劣没有感知。如果你对这种绕路行为不满意，可以适当增大位置误差权重，让机器人更“急躁”一点。但也要接受一个事实：在局部优化的框架下，全局最优路径本来就不是NMPC的承诺。

5.3 常见问题速查

6. 应用扩展与个人体会

6.1 从仿真到实车需要注意的几个点

如果你不是只想交一个仿真Demo，而是想把这个NMPC点镇定控制器移植到真实移动机器人上，那么有三件事必须在仿真阶段就提前考虑。

第一是计算延迟。fmincon在仿真里几十毫秒求解一次无所谓，但真实控制器如果运行在嵌入式处理器上，几十毫秒的延迟可能已经影响控制稳定性。解决办法是先在线下把NMPC求解函数用代码生成工具转换为C代码，然后部署到实时目标机上。需要注意的是，实时目标机上同样的NMPC问题求解时间可能会是Matlab里的几倍到十几倍，所以预测时域N在实机上要相应减小，或者换用更高效的序列二次规划求解器。

第二是状态估计。仿真里机器人位置直接来自积分器，没有噪声。实际中位置和航向角往往来自里程计或视觉定位，存在延迟和漂移。这些误差会直接进入NMPC的预测模型，导致控制效果明显变差。我在做真机验证时，通常会在仿真里给状态加一个高斯白噪声，模仿里程计误差。加上噪声之后，原本仿真里非常完美的NMPC控制器会暴露出一系列问题，比如终端位置误差变大、避障路径变得犹豫。遇到这种情况，可以把控制周期适当缩短，或者在代价函数里加一个对状态的滤波项，但这又是一个大坑，需要单独写一篇文章来聊。

第三是执行器的限幅和响应延迟。仿真里 (v, \omega) 可以直接跳变，但真实电机有加速度限制。如果你在仿真阶段把输入约束写成单纯的限幅，实机上会出现“控制命令很激进但电机跟不上”的落差。稳妥的做法是在运动学模型里再加入一阶惯性环节，模拟电机响应延迟。加入之后NMPC的预测轨迹会更接近实际运动轨迹，避障效果也更可信。

6.2 这个项目后续可以怎么扩展

从NMPC点镇定出发，延展方向其实非常丰富。如果你后续想继续往这个方向做，我建议先考虑这几个扩展方向。

第一个方向是把目标点换成动态目标点。点镇定的目标点是固定的，而动态目标点是追踪一个运动目标，这其实和轨迹跟踪又不太一样，它更接近“追击稳定”问题。实现上只需要让 (x_{ref}) 随时间变化，然后每步NMPC里更新参考点位置和姿态。代价函数的写法几乎不用改，但动态参考点会让保守避障的约束更容易冲突，因为目标在动，机器人路径也一直在变。

第二个方向是引入编队镇定。把多个机器人各自跑一个点镇定控制器，然后在NMPC里加入机器人之间的间距约束，就变成了编队控制问题。这里的难点在于多机器人共享地图信息后，预测时域内的互撞约束会让求解问题规模和复杂度急剧上升。如果做双机器人编队，一个NMPC控制器同时控制两台车也是一种可行方案，不过预测时域要缩短，否则优化变量数量太多，求解时间不可接受。

第三个方向是切换成轨迹规划+跟踪的两层架构。NMPC点镇定本质上是 "直接控制层做规划" ，而如果你把上层换成一个全局路径规划器（比如A*或RRT），下层再让NMPC去跟踪路径，那么整个系统的实时性和鲁棒性都会好很多。但要注意这样处理会让点镇定问题退化为路径跟踪问题，理论上的“非完整约束镇定”难点就被绕开了，少了一点“挑战感”，但工程上更实用。

我个人在实际操作中的体会是，NMPC点镇定项目最容易被低估的是调参和调试成本，而不是算法公式本身。公式写出来不过几行，但真正让仿真跑得稳、避障避得漂亮，需要反复实验各种参数组合。如果你第一次跑出来效果不理想，一定不要急着怀疑NMPC方法有问题，先检查是不是预测时域太短，或者安全半径太小。把这些基础因素排除之后，再决定要不要换求解器。我最后再分享一个小技巧：调试NMPC仿真时，把每一时刻的预测轨迹都画出来，就像看着机器人“心里在想什么”一样。只要你能直观看到它预测的未来路径，很多诡异问题都会立刻暴露出来。这个方法比分析一堆求解器日志高效得多。