覃龙光我们平时做运动控制的人，一听到“MPC”这个词，脑子里大概率蹦出来的是Model Predictive Control——但你要是上网搜资料，很容易先搜到那个叫Media Player Classic的视频播放器，搜索体验非常酸爽。今天我要聊的MPC，是模型预测控制，而且是把“模型预测”真正落到运动控制器设计上的那种做法。
我在入门阶段走过不少弯路：最早在MATLAB里用MPC Toolbox做轨迹跟踪，仿真很顺，工具箱封得干干净净，点两下鼠标就能拿到一个控制器。但等到想换一个非线性模型、想在预测时域里加自己的约束条件，工具箱就变成了黑盒，文档翻烂也隔着一层。直到后来接触了CasADi，才感觉把MPC这层窗户纸捅破了。这篇文章就围绕“基于CasADi工具箱的MPC轨迹跟踪运动控制器设计”这条主线，把我从原理到代码、从调参到部署踩坑的完整思路整理出来，希望能帮到正准备入坑MPC的同行。
1. 为什么轨迹跟踪控制器要从PID走向MPC
1.1 轨迹跟踪控制器到底在解决什么问题
先说清楚轨迹跟踪是个什么任务。无论是移动机器人、自动驾驶车辆还是机械臂末端，轨迹跟踪控制器的目标是让系统状态沿着一条预先给定的时间序列或几何路径运动。以差速小车为例，状态通常是位置和航向角，控制指令是线速度和角速度，控制器要做的就是从当前状态出发，把小车的实际轨迹拉回到参考轨迹上。
这个任务听起来朴素，做起来却不简单。参考轨迹可能包含高速段、急弯段、甚至倒车段；真实系统有速度上限、加速度受限、转向能力有限这些物理约束。你给的指令太激进，轮子根本执行不出来；指令太保守，又会落后参考轨迹一大截。所以轨迹跟踪控制器本质上是个“带着锁链跳舞”的问题——要在物理约束内把跟踪误差压到最小。
1.2 传统方法的瓶口在哪
PID是最常见的起点，但拿PID直接做轨迹跟踪，很多人会把轨迹跟踪理解成“同时跟踪期望位置和期望航向”，于是位置环加航向环叠一个PID。实测下来问题很明显：小车在直线段还好，一进弯道就往外飘，因为PID不管未来路径怎么弯曲，只看当下误差，方向角误差和位置误差在弯道处还会互相打架。
几何方法比如纯跟踪也是常用方案，它根据前视距离选一个目标点，计算出圆弧半径，再转换成转向指令。优点是很稳、调一个前视距离就行，但缺点也明显：前视距离本质上是“拍脑袋参数”，遇到复杂轨迹需要在线变速变前视距离，约束处理更是无从谈起。
这些传统方法的共同瓶颈是“没有预测能力”。它们都是看到当前误差再去反应，属于被动纠偏，遇到大曲率、强耦合的系统，纠偏速度和稳定性很难兼得。
1.3 MPC的滚动优化思路
MPC的思路不一样。它在每个控制周期里，基于当前状态和模型预测未来一小段时间的系统行为，然后在线求解一个带约束的优化问题，把这一段预测窗口内的控制序列算出来之后，只执行第一个控制量，下一个周期重新来一遍，这叫滚动时域优化。
打个比方：普通PID像开车时只看车头前面两三米，发现偏了赶紧打方向；MPC像开了高德导航，提前看到前方五百米有个连续弯道，还没入弯就开始调整速度和方向。这种“预判-决策-反馈”的闭环结构，让MPC天然适合轨迹跟踪，而且约束能力是PID和几何方法不具备的——你可以在优化问题里直接写“速度不超过0.5m/s”“转向不超过1rad/s”。
当然，MPC不是没有代价，代价就是每个周期都要在线解一个非线性优化问题，实时性需要精心优化。这正好解释了为什么现在做运动控制的人越来越多转向CasADi：它把符号建模、自动微分、非线性求解器这几个环节串在了一起，让MPC的落地开发效率高了不少。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 先搞懂CasADi的四个核心概念，再写代码
2.1 SX和MX：两种符号变量选谁
CasADi里有两个最基础的符号类型：SX和MX。很多初学者上来就卡在这里，搞不清区别，我可以给你一个比较粗暴的理解方向：SX是把所有表达式展开成标量级别的稀疏运算，MX则是把整个矩阵操作当成一个独立的节点去构建表达式图。
拿MPC来说，我们通常要沿着预测时域写N步循环，在循环里不断拼接动力学表达式。我最早用SX写了一个预测时域N=30的小车NMPC，构造问题的时候就开始卡顿，求解时更是慢得发指。后来把符号变量换成MX，同样的N值，问题构造速度和内存占用明显改善。原因很简单：MX把每一步的矩阵运算作为整体保存，表达式图不会无脑膨胀；SX则会把所有标量运算展开，预测步数一多就爆炸。
所以我的建议是：刚入手做MPC，直接默认用MX就行。SX也有它的优势场景，比如想对某个表达式做更细粒度的稀疏性分析时，但那是比较后期的话题，入门阶段用MX能少踩很多坑。
2.2 Function：把表达式封装成可调用对象
CasADi的Function就像一个“函数外壳”，你定义好输入和输出表达式，它就能被反复调用，可以被编译，还能被嵌入到其他优化问题里。
举个例子：
python复制import casadi as ca
x = ca.MX.sym('x', 3)
u = ca.MX.sym('u', 2)
x_dot = ca.vertcat(u[0] * ca.cos(x[2]), u[0] * ca.sin(x[2]), u[1])
f = ca.Function('f', [x, u], [x_dot])
定义完成之后，f就是一个可调用对象，输入状态和控制，输出导数。如果后续想做变步长仿真，还可以直接把f传给积分器：
python复制integrator = ca.integrator('integrator', 'rk4', {'x': x, 'p': u, 'ode': x_dot}, {'tf': 0.1})
比起每次手写欧拉离散，这种封装更接近“面向问题的编程”，代码可读性也高很多。Function还有个重要用途是在生成C代码时作为导出单元，到部署阶段非常关键。
2.3 Opti：用最自然的方式搭优化问题
CasADi早期全是nlpsol这种底层接口，写起来像在做填空题，变量、约束、目标函数全都要塞进一堆字典里，格式要求严苛，报错信息也不友好。后来CasADi推出了Opti栈式接口，风格一下子向YALMIP靠拢，写MPC问题像写数学公式：
python复制opti = ca.Opti()
X = opti.variable(3, N+1)
U = opti.variable(2, N)
opti.subject_to(X[:, 0] == X0)
opti.subject_to(X[:, k+1] == X[:, k] + dt * f(X[:, k], U[:, k]))
opti.minimize(cost)
opti.solver('ipopt')
sol = opti.solve()
变量声明、约束装载、目标函数声明、求解器选择，每一步都很直观。对入门者来说，Opti接口极大地降低了理解成本，而且它在求解后会保存上次的求解结果，下一次solve()时自动作为初始猜测，这就是我们做滚动时域MPC时求之不得的热启动特性。
2.4 自动微分：MPC里最省心的部分
NLP求解器（比如IPOPT）在迭代过程中需要目标函数和约束的梯度信息，手动推导这些导数非常痛苦，模型一复杂就基本不可能。CasADi的核心能力之一是算法微分（Algorithmic Differentiation，AD），它基于你构造的符号表达式自动生成导数，精确到机器精度，没有有限差分的截断误差。
这个能力对MPC的意义巨大。你在代码里写了一个包含航向角、控制量、变化率的代价函数，CasADi能自动得到它对所有决策变量的雅可比矩阵，IPOPT拿着这些信息去搜索最优解。我见过不少初学者尝试自己推导MPC的梯度公式，然后手写雅可比，模型稍微复杂一点就推导错、代码也bug百出。用CasADi做MPC的大原则就是：你把问题建明白，导数交给工具箱去算。
3. 运动学模型、目标函数和约束：搭出MPC控制器的三块积木
3.1 差速小车模型与离散化
做轨迹跟踪运动控制器，第一件事是把被控对象的运动学模型写出来。这里选最经典的差速小车模型，状态取(px, py, theta)，控制取(v, omega)：
code复制px_dot = v * cos(theta)
py_dot = v * sin(theta)
theta_dot = omega
模型很简洁，但对MPC来说已经足够有代表性。注意这个模型是非线性的，因为状态导数里的cos(theta)和sin(theta)与状态本身耦合。如果只用线性MPC，每步都要在参考点做线性化，实现起来多一层；而用CasADi可以之间做非线性MPC，省掉线性化那一步。
关于离散化，入门阶段我建议先用一阶欧拉法：
python复制x_next = x + dt * f(x, u)
dt=0.1s，预测步数N，控制器预测的未来总时长就是N*dt。欧拉法优势是代码简单、和MPC预测模型一致，缺点是精度随dt增大而下降。等基础跑通之后，可以把离散化换成RK4或者直接用CasADi的integrate接口，提升预测精度。
3.2 参考轨迹生成与跟踪误差的定义
为了验证控制器，我习惯用一个匀速圆周作为参考轨迹。半径取1m，角速度0.3rad/s，那么期望线速度就是0.3m/s：
python复制r = 1.0
w_ref = 0.3
v_ref = r * w_ref # 0.3 m/s
px_ref(t) = r * cos(w_ref * t)
py_ref(t) = r * sin(w_ref * t)
theta_ref(t) = atan2(d(py_ref)/dt, d(px_ref)/dt)
参考航向角取的是速度方向，也就是轨迹的切线方向。这里有个隐藏细节需要特别留心：atan2返回的航向角落在[-pi, pi]区间，而小车的实际航向角可以连续地从pi转到-pi，两者之间的差会在某个时刻忽然跳到接近2*pi。如果你直接把这个差值放到代价函数里，目标函数会出现一个明显的突变，MPC求解器会被这个突变折磨得够呛。
所以一定要做角度差归一化（wrap）：
python复制err_theta = atan2(sin(theta - theta_ref), cos(theta - theta_ref))
这个操作把角度误差限制在[-pi, pi]内，代价函数就连续了。
3.3 代价函数设计：误差、控制量与变化率
MPC的目标函数就好比控制器的“性格”，不同的权重组合决定了控制器是激进还是保守。我习惯用三项代价：
code复制J = sum_k ( e_k' Q e_k + du_k' R du_k + dv_k' Rd dv_k )
其中e_k是状态误差（位置误差+归一化后的航向误差），du_k是控制量相对期望值的偏差，dv_k是控制量变化率。Q对应跟踪精度，R对应控制能耗，Rd对应控制动作的平滑性。
各项权重矩阵取什么数量级，需要结合物理单位看。位置单位是米，航向单位是弧度，线速度单位是m/s，角速度单位是rad/s，这些量纲不同的量放进同一个代价函数时必须做权重匹配：
python复制Q = ca.diag([5.0, 5.0, 0.1]) # 位置权重高，航向权重低
R = ca.diag([0.1, 0.1]) # 控制量本身的惩罚
Rd = ca.diag([0.5, 0.5]) # 控制变化率惩罚
我第一次调的时候把位置权重设成1、航向权重也设成1，结果小车在圆周轨迹上追得很费劲，原因是航向误差一个弧度给的是1的代价，位置误差0.2米才0.2的代价，控制器宁可让横向误差大到0.5米也不愿意转一下头。后来把位置权重提到5，航向压到0.1，跟踪效果立刻正常了。所以调权重不是玄学，本质上是做“单位对齐+优先级权衡”。
3.4 约束条件与航向角周期性的处理
MPC相比PID最大的优势就是能显式处理约束。这里至少加两类约束：
一是执行机构的物理限制：
python复制v_min <= v <= v_max
omega_min <= omega <= omega_max
二是在执行层面的平滑性限制，即控制变化率的约束：
python复制-v_du_max <= v(k+1) - v(k) <= v_du_max
-omega_du_max <= omega(k+1) - omega(k) <= omega_du_max
这样即使代价函数里的Rd权重不够，控制器也不会发出阶跃式的猛打方向指令，对真实执行机构很友好。
约束加多了以后，一个需要注意的问题是可行性。如果约束设得太紧，比如巡航速度要求0.5m/s但同时约束速度必须小于0.4m/s，优化问题可能直接报无解。解决这个问题的常见做法是把硬约束中的一部分改成“软约束”——即在代价函数里加一个惩罚项，允许约束轻微越界但付代价。入门阶段可以先不加软约束，等遇到求解失败再回来优化也不迟。
4. 完整闭环仿真：CasADi代码分段拆解
4.1 初始化与参数设置
直接上完整可运行的代码。这个例子里，我让小车从偏离参考轨迹的位置出发，看它能不能在MPC的驱动下收敛到圆周轨迹上。
python复制import numpy as np
import casadi as ca
import matplotlib.pyplot as plt
dt = 0.1 # 采样时间
N = 25 # 预测时域步数
sim_len = 200 # 仿真步数
r = 1.0
w_ref = 0.3
v_ref = r * w_ref
v_min, v_max = -0.5, 0.5
w_min, w_max = -1.0, 1.0
dv_max = 0.3
domega_max = 0.6
Q = ca.diag([5.0, 5.0, 0.1])
R = ca.diag([0.1, 0.1])
Rd = ca.diag([0.5, 0.5])
4.2 MPC问题构造
接下来是核心的MPC问题搭建。这里把参考状态和参考输入都做成opti.parameter，这样滚动时域求解时每次只需更新参数，不用重新构造问题：
python复制opti = ca.Opti()
# 决策变量：状态序列 + 控制序列
X = opti.variable(3, N + 1)
U = opti.variable(2, N)
# 参数：当前状态、参考状态窗口、参考输入窗口
X0 = opti.parameter(3, 1)
X_ref = opti.parameter(3, N + 1)
U_ref = opti.parameter(2, N)
# 代价函数
cost = 0
for k in range(N):
err_x = X[0, k] - X_ref[0, k]
err_y = X[1, k] - X_ref[1, k]
err_theta = ca.atan2(ca.sin(X[2, k] - X_ref[2, k]),
ca.cos(X[2, k] - X_ref[2, k]))
e = ca.vertcat(err_x, err_y, err_theta)
cost += ca.bilin(Q, e, e)
du = U[:, k] - U_ref[:, k]
cost += ca.bilin(R, du, du)
# 控制变化率代价（从第1步到第N-1步，跨邻域计算）
for k in range(N - 1):
dU = U[:, k + 1] - U[:, k]
cost += ca.bilin(Rd, dU, dU)
opti.minimize(cost)
# 初始状态约束
opti.subject_to(X[:, 0] == X0)
# 动力学约束（欧拉法离散）
def dynamics(x, u):
return ca.vertcat(u[0] * ca.cos(x[2]),
u[0] * ca.sin(x[2]),
u[1])
for k in range(N):
x_next = X[:, k] + dt * dynamics(X[:, k], U[:, k])
opti.subject_to(X[:, k + 1] == x_next)
# 输入幅值约束
for k in range(N):
opti.subject_to(opti.bounded(v_min, U[0, k], v_max))
opti.subject_to(opti.bounded(w_min, U[1, k], w_max))
# 输入变化率约束
for k in range(N - 1):
opti.subject_to(opti.bounded(-dv_max, U[0, k + 1] - U[0, k], dv_max))
opti.subject_to(opti.bounded(-domega_max, U[1, k + 1] - U[1, k], domega_max))
# 求解器配置
opti.solver('ipopt', {
'print_time': False,
'ipopt': {
'print_level': 0,
'max_iter': 200,
'tol': 1e-4,
}
})
这段代码有几个点需要解释。
第一，ca.bilin(Q, e, e)等价于e^T Q e，CasADi直接提供了这个接口，写起来很简洁。第二，动力学约束用循环加了N条等式约束，这是MPC最标准的做法：把状态序列的相邻两步用离散动力学方程绑定。第三，输入变化率约束是从第0步到第N-2步的相邻控制量差值，最后一步没有约束，因为优化变量里没有N时刻之后的新控制量。
4.3 滚动求解与仿真推进
仿真循环的核心逻辑是：更新参数、求解MPC、取第一个控制量、推进真实模型、继续下一个周期。
python复制# 初始状态：故意偏离参考轨迹
x_cur = np.array([0.2, -0.2, 0.1]).reshape(3, 1)
x_log = [x_cur.flatten().copy()]
u_log = []
for k in range(sim_len):
t_cur = k * dt
# 当前预测窗口内的参考状态与参考输入
ref_win = np.zeros((3, N + 1))
ref_input_win = np.zeros((2, N))
for j in range(N + 1):
t = t_cur + j * dt
ref_win[0, j] = r * np.cos(w_ref * t)
ref_win[1, j] = r * np.sin(w_ref * t)
vx = -r * w_ref * np.sin(w_ref * t)
vy = r * w_ref * np.cos(w_ref * t)
ref_win[2, j] = np.arctan2(vy, vx)
for j in range(N):
ref_input_win[0, j] = v_ref
ref_input_win[1, j] = w_ref
opti.set_value(X0, x_cur)
opti.set_value(X_ref, ref_win)
opti.set_value(U_ref, ref_input_win)
try:
sol = opti.solve()
except RuntimeError:
print(f'Step {k}: solver failed, using initial guess fallback')
sol = opti.debug.value(opti.x, opti.initial())
break
u_opt = sol.value(U)[:, 0]
# 用真实模型推进一步（这里以欧拉法模拟真实被控对象）
theta = x_cur[2, 0]
x_cur = np.array([
x_cur[0, 0] + dt * u_opt[0] * np.cos(theta),
x_cur[1, 0] + dt * u_opt[0] * np.sin(theta),
x_cur[2, 0] + dt * u_opt[1]
]).reshape(3, 1)
x_log.append(x_cur.flatten().copy())
u_log.append(u_opt.copy())
这里有一个经验值得反复强调：参考状态窗口必须随当前时刻滑动。如果我图省事，在构造问题时就把参考轨迹写死为从t=0开始的序列，那么当小车跑到第5秒的时候，预测窗口内仍然是t=0时刻的参考，控制器就会拼命想把小车拉回起点附近，整个跟踪就乱套了。正确做法就是上面代码里那种：每步根据当前时间t_cur重新生成预测窗口内的参考状态，并通过opti.set_value更新参数。
另外，CasADi Opti在每次solve()之后会把上一帧的最优解自动作为下一帧的初始猜测，这其实就是热启动。我在循环里没有手动设置初值，但实际收敛速度依然不错。如果你发现某一帧求解很慢，可以考虑手动把上一帧的sol.value(X)和sol.value(U)显式传给opti.set_initial，效果会更稳。
4.4 结果可视化与验证
跑完仿真之后，把轨迹和控制量画出来看看：
python复制x_log = np.array(x_log)
u_log = np.array(u_log)
t_plot = np.arange(sim_len) * dt
ref_x = [r * np.cos(w_ref * t) for t in t_plot]
ref_y = [r * np.sin(w_ref * t) for t in t_plot]
plt.figure(figsize=(8, 6))
plt.plot(x_log[:, 0], x_log[:, 1], 'b-', label='actual')
plt.plot(ref_x, ref_y, 'r--', label='reference')
plt.xlabel('x')
plt.ylabel('y')
plt.legend()
plt.axis('equal')
plt.show()
如果你仿真跑下来图像正常，会看到小车从偏离点开始，绕了几步就贴到参考圆上，之后维持稳定跟踪。误差曲线应该是一个先下降后维持在零附近的过程。控制量曲线不应该出现剧烈跳变，因为我们在代价函数里加了变化率惩罚，又在约束里限制了变化率上限。
我跑这个例子时经常发现，初始几步的求解时间比稳态时更长，原因在于初始偏离大，优化问题的最优解在可行域边界附近徘徊，IPOPT需要更多迭代。这属于正常现象，不必过度担心。
5. 调试与调参的实战心得：权重、时域和约束设置
5.1 两组高频报错的排查思路
用CasADi+IPOPT跑MPC，第一次能一次跑通其实是少数，多数人会在报错中挣扎。我遇到最多的是这两类。
第一类是维度不匹配。比如在代价函数里把e写成了1x3的行向量，而Q是3x3，然后ca.bilin就会报维度错误。排查思路很直接：在报错前打印出相关表达式的形状，用CasADi自带的.shape属性检查。
第二类是求解器返回Infeasible Problem Detected。这通常是因为约束太紧或初值太差。我的排查路径是：先把输入变化率约束或幅值约束放松一些，看看问题能不能可行；如果放松约束以后依然不可行，就检查动力学约束是不是写错了，比如dt忘乘了、cos里的状态索引写错。我最开始犯过一个低级错误：动力学约束里把X[:, k]写成了X[:, k+1]，导致约束变成悖论式地要求同一时刻的状态既等于自己又等于前进了一步，当然永远无解。这类问题靠单纯看代码不容易发现，建议把单步动力学单独拿出来验证：给一个已知状态和控制，手动算一下下一步，再和CasADi表达式的结果对比。
5.2 权重Q/R/Rd的经验法则
调权重算得上MPC里最“玄学”的部分，但也不是完全没规律可循。按我自己的经验，有一个非常好用的调参顺序：
- 先固定R和Rd，只调Q中的位置权重，让跟踪误差达到可以接受的水平。
- 如果控制器动作太猛、靠向约束边界，再调大R；如果动作太软、响应偏慢，调小R。
- 最后用Rd来优化平滑性。Rd的作用是抑制控制量高频变化，很多跟踪震荡问题并不是Q不够大，而是