车道跟踪配合动态避障，这种需求放在真实的自动驾驶项目里，远比听上去复杂。很多人一开始的思路是分开做：上游跑一个全局路径规划，下游用纯跟踪或者PID把车拉回参考线，中间再套一层障碍物判断。真跑起来就会发现问题——规划出来的路径不见得满足车辆动力学，跟踪控制器反应不够快，动态障碍物场景下根本来不及重新规划。这个项目把路径规划和控制拧进了同一个MPC（模型预测控制）优化框架里，再用CASADI做非线性求解，在Matlab里完成闭环仿真和验证，属于很典型的“规划控制一体化”实现思路。

我最初接触这个题目的时候，第一反应是“CASADI是什么”，第二反应是“MPC怎么把避障和跟踪一起做进去”。这两个问题搞清楚之后，剩下的就全是工程细节了。这篇内容我会从整体设计、核心原理、Matlab代码实现、常见坑点四个部分展开，尽量把每一步的原理和操作都讲透，代码部分可以直接拿来跑。

1. 项目整体设计与思路拆解

1.1 车道跟踪与动态避障为什么要集成

先说需求。车道跟踪解决的是“车在车道里怎么走”的问题，目标是贴着重参考路径走；动态避障解决的是“前方有障碍物怎么躲”的问题，目标是安全绕开。这两个问题如果分层处理，通常的流程是：感知层给出障碍物信息 → 全局规划重新规划一条绕行路径 → 控制层跟踪这条新路径。

问题就出在“重新规划”这个环节。全局规划一般用采样、搜索或者曲线插值的方法，它不一定会考虑车辆本身的运动学约束，比如最小转弯半径、最大横向加速度。规划出来的路径可能是一条几何上很漂亮、但车辆物理上根本走不出来的曲线。控制层去跟踪这种路径时，就会出现大幅度转向、误差收敛慢甚至发散的情况。

另外，在动态障碍物场景下，全局规划的更新频率往往不够。如果一个行人或者车辆突然出现在预测路径的前方，等全局规划跑一轮再传下去，控制层再响应，时间上已经来不及了。

把跟踪和避障放进同一个MPC优化问题里，就没有这些割裂问题。优化目标里同时有参考路径跟踪误差项、控制量平滑项、避障惩罚项，约束里同时有车辆动力学约束、控制量边界约束、障碍物安全距离约束。求解一次优化，得到的控制指令本身就是一条满足车辆动力学、同时避开障碍物的可行轨迹。而且MPC是滚动时域的，每步都重新优化，天然具备对动态障碍物的实时响应能力。

1.2 技术选型：CASADI + MPC的组合逻辑

MPC本身不是一个很新的概念，但传统上大家会倾向于用Matlab自带的优化工具箱，或者手写梯度下降法。这里选择CASADI，核心原因是它把“建模”和“求解”两件事都做到了极致。

CASADI是一个开源的非线性优化工具，主打的特性就是符号计算和自动微分。你只需要把车辆模型、代价函数、约束条件用符号变量写出来，CASADI会自动帮你求出目标函数对每个决策变量的梯度、海森矩阵等导数信息，然后送进IPOPT这种非线性求解器去求解。整个过程中不需要你手动推公式，更不需要手写数值差分。

这对MPC意义重大。MPC每次求解本质上都是一个非线性规划问题，求解器依赖梯度信息来迭代逼近最优解。如果梯度算得不准或者太慢，求解效率和稳定性都会很差。CASADI的自动微分能精确计算出梯度，速度和准确度远超数值差分方法。

选Matlab的原因更直接——做算法验证、做仿真可视化、处理数据都方便。CASADI官方提供了完整的Matlab接口，安装配置也简单。我见过不少开发者用Python和C++做同样的事，但Matlab在快速原型阶段的效率确实有优势，尤其是调试优化器参数时，可视化反馈非常直观。后面如果想做实车部署，CASADI还支持把优化问题生成C代码，可以平移嵌到嵌入式平台，这也是它比单纯用Matlab工具箱更被工业界接受的原因。

1.3 系统框架与模块划分

整个项目的框架大概分四层：感知层、规划控制层、车辆执行层、仿真环境层。

感知层在完整项目里负责输出障碍物位置、速度、车道线信息，但在这个研究项目里，我选择简化处理：预定义障碍物的初始位置和运动速度，直接以结构化数据的形式输入给MPC模块，这样能把研究重点集中在规划控制算法本身，不会被感知噪声干扰。

规划控制层就是整个MPC模块，也是代码的核心部分。输入是当前车辆状态、参考路径、障碍物信息、上一时刻的控制指令，输出是每个控制周期内的转向角序列和加速度序列，但真正执行的只有当前控制步的第一个控制量，然后整个优化窗口向前滚动。

车辆执行层我用的是二自由度自行车模型，后面会详细推导。它表达的是“给一个转向角和一个加速度，车辆在短时间内怎么运动”这件事，MPC的预测就是基于这个模型外推出来的。

仿真环境层负责把每一步的执行结果反馈给MPC，形成闭环。每一步仿真里，MPC求解出控制量，把控制量用于更新车辆状态，再把新状态传回MPC，一直循环到仿真结束。

2. 核心算法原理与模型推导

2.1 车辆运动学模型：二自由度自行车模型

模型是整个MPC预测的基础。选二自由度自行车模型而不是更复杂的动力学模型，是因为它足够简洁、参数少（只有一个轴距和两个状态方程），计算开销小，中低速场景下精度也够用。在城区道路和高速公路上，大部分工况都能满足这个假设。

自行车模型的思路是：把车辆的前后轮分别合并成一个轮子，等效成一辆自行车。状态量取：

• (x)：车辆在大地坐标系下的横向位置
• (y)：车辆在大地坐标系下的纵向位置
• (\psi)：车辆航向角
• (v)：纵向车速

控制量取：

• (\delta)：前轮转向角
• (a)：纵向加速度

模型的微分方程组是：

[ \dot{x} = v \cos\psi ] [ \dot{y} = v \sin\psi ] [ \dot{\psi} = \frac{v}{L} \tan\delta ] [ \dot{v} = a ]

其中 (L) 是前后轴轴距。第一个和第二个方程描述了车辆的位置变化，第三个方程是航向角变化率，核心逻辑是“车速越高、轴距越短、转向角越大，车头转得就越快”。第四行描述的是速度随加速度的变化。

这里有个值得注意的点：模型里把轮胎侧偏、横摆动力学都忽略了，所以对于高速、急转向极限工况，这个模型的预测会和实际差别比较大。但在项目验证阶段，我们关心的主要是控制算法的逻辑完整性，自行车模型完全够用。

在Matlab代码里，这个模型会改写成适用于MPC的离散化形式。具体来说，用前向欧拉法把微分方程转成差分方程：

[ x_{k+1} = x_k + T_s \cdot v_k \cos\psi_k ] [ y_{k+1} = y_k + T_s \cdot v_k \sin\psi_k ] [ \psi_{k+1} = \psi_k + T_s \cdot \frac{v_k}{L} \tan\delta_k ] [ v_{k+1} = v_k + T_s \cdot a_k ]

(T_s) 是采样时间。这个离散化形式会以约束的形式出现在MPC优化问题里，保证每一步的决策变量都满足车辆运动规律。

2.2 避障约束如何抽象成数学表达

避障问题放在MPC里，本质上就是一个（或一组）不等式约束。

障碍物在二维平面里通常用圆形来近似，圆心坐标是 ((x_{\text{obs}}, y_{\text{obs}}))，半径是 (R_{\text{obs}})。为了保证安全，车辆和障碍物之间应该保持一个大于安全阈值的距离。如果用车辆的质心位置 ((x, y)) 来代表整个车，那么这个约束可以写成：

[ (x - x_{\text{obs}})^2 + (y - y_{\text{obs}})^2 \geq (R_{\text{obs}} + d_{\text{safe}})^2 ]

(d_{\text{safe}}) 是额外增加的安全距离余量。样子上看，这个约束要求车辆质心和障碍物中心之间的距离永远保持在碰撞半径加安全余量之上。

但直接写这个硬约束在实际求解中经常出问题。如果障碍物恰好出现在当前车辆的正前方，同时车速又比较高、预测时域比较短，那么优化器可能真的找不到满足所有约束的解——因为无论怎么打方向，在很短的时间内都没办法让横向位移足够大。这就是“不可行解问题”。

比较稳妥的做法是引入松弛变量。把不等式约束放宽成：

[ (x - x_{\text{obs}})^2 + (y - y_{\text{obs}})^2 \geq (R_{\text{obs}} + d_{\text{safe}})^2 - \varepsilon ]

其中 (\varepsilon \geq 0) 是松弛变量，会被以很高的权重加进代价函数里。优化器只有在实在无法满足安全距离约束时，才会被迫让 (\varepsilon) 取一个较大值，相当于“破例”靠近障碍物，但每破一次例都会付出代价。这个机制被称为“软约束”，可以显著提高求解器在复杂场景下的可行性，我在实际项目里几乎不会用纯硬约束版本。

对于动态障碍物，关键是在预测时域内正确估计障碍物的未来位置。最常见的假设是匀速直线运动：

[ x_{\text{obs}}(k) = x_{\text{obs}}(0) + v_{\text{obs},x} \cdot k \cdot T_s ] [ y_{\text{obs}}(k) = y_{\text{obs}}(0) + v_{\text{obs},y} \cdot k \cdot T_s ]

也就是在第 (k) 步预测时刻，障碍物会从初始位置出发，以恒定速度朝某个方向移动一段距离。这个方法简单可靠，对于低速运动障碍物（行人、低速车辆）的预测精度足够。如果障碍物运动更复杂，理论上可以接入更高层的轨迹预测模型，把预测结果作为MPC参数输入，但原理上是一样的。

2.3 MPC优化问题构建与代价函数设计

在MPC框架下，整个问题被变成一个“在预测时域内找最优控制序列”的优化问题。

设定预测时域为 (N) 步，那么决策变量包括：

• 状态序列：(X_0, X_1, \dots, X_N)，每个 (X_k) 都是四维向量 ((x, y, \psi, v))
• 控制序列：(U_0, U_1, \dots, U_{N-1})，每个 (U_k) 都是二维向量 ((\delta, a))
• 松弛变量：每个预测步对应一个 (\varepsilon)

代价函数我一般设计成三部分叠加：

第一部分是跟踪参考路径的误差项：

[ J_{\text{track}} = \sum_{k=0}^{N} (X_k - X_{\text{ref},k})^T Q (X_k - X_{\text{ref},k}) ]

(X_{\text{ref},k}) 是参考路径在第 (k) 步的目标状态，(Q) 是对角权重矩阵，对角线上的元素分别对应位置误差、航向误差、速度误差的权重。位置误差权重通常最大，航向次之。

第二部分是控制输入平滑项：

[ J_{\text{input}} = \sum_{k=0}^{N-1} U_k^T R U_k + \sum_{k=1}^{N-1} (U_k - U_{k-1})^T R_{\Delta} (U_k - U_{k-1}) ]

前一半惩罚控制量本身的大小，避免转向过大、加速度过猛；后一半惩罚控制量的变化率，让输出更平滑。这个项非常重要，没有它的话，优化器容易给出前后两步差别很大的控制指令，车辆会抖动。

第三部分是松弛变量惩罚：

[ J_{\text{safe}} = \rho \sum_{k=0}^{N} \varepsilon_k ]

(\rho) 是软约束权重，要设得比较大，让优化器优先考虑可解性而不是随意靠近障碍物。

最终代价函数是这三者之和。这里没有把避障作为惩罚项直接加入代价函数，而是用约束表达式体现——原因是“距离越远罚越轻”的方式会导致车辆在可行空间内贴着障碍物走，不合适；而硬性要求保持安全距离更符合实际驾驶行为。

2.4 滚动时域与闭环反馈的配合

MPC“预测、优化、执行、再预测”这套循环逻辑是它适合控制车辆的关键。每次优化只取控制序列的第一个控制量下发，到了下一个采样周期，车辆状态会被重新测量或估计，优化窗口整体往前滚一步，再次求解。

这意味着MPC本质上是一个带反馈的闭环控制器，而且因为每步都在重新优化，它对模型误差和外部扰动有一定鲁棒性。即使参考路径、障碍物信息发生变化，下一拍求解时自然会被考虑到，不需要额外写一整套“重规划”逻辑。

预测时域 (N) 的选择是系统工程。(N) 太小，车辆只看得到很近的未来，绕障时容易撞上前方的障碍物；(N) 太大，计算量成倍增加，实时性受影响。我调试下来感觉比较合适的区间是10到20步，采样时间 (T_s = 0.1) 秒左右，对应着1到2秒的预测时长。这个窗口长度对一般的规避和跟踪需求都够用。

3. Matlab + CASADI 实操实现

3.1 环境准备与CASADI安装

做这个项目之前，先把环境配好。Matlab版本建议用R2019b以上，太老版本对CASADI接口支持不好。

CASADI的Matlab版本直接在官网下载，选择与操作系统和Matlab版本对应的包，解压后会得到一个包含casadi文件夹的目录。然后添加路径：

addpath('/你的路径/casadi-matlab');

为了系统性的接入后续代码，最好在Matlab启动配置里把这一行固定下来，具体做法是编辑startup.m文件并放在MATLAB路径下。想确认安装是否成功，可以运行：

import casadi.* disp(Function('f', {SX.sym('x')}, {SX.sym('x')^2}))

如果正常输出了符号函数信息，那就说明CASADI已经可以使用了。

另外建议确认一下IPOPT求解器是否随CASADI附带。CASADI的Matlab发行版通常内置了对IPOPT的支持，不需要额外安装。如果你后面在求解时发现报错说“cannot find solver ipopt”，大概率是CASADI包版本问题，换一个对应版本即可。

3.2 代码结构总览

我习惯把代码拆成几个文件，各司其职：

• main_mpc_casadi.m：主脚本，负责整个仿真循环
• vehicle_model.m：返回车辆模型的微分方程组函数体
• update_reference.m：根据当前时刻生成参考路径点
• update_obstacles.m：输出每个预测时刻障碍物的位置
• mpc_solve_step.m：单步MPC求解，核心优化逻辑所在

这种拆分的好处是单个文件逻辑清晰，也方便单独测试某个模块。

核心仿真循环大致是这样：

for k = 1 : total_steps ref_traj = update_reference(k); obs_traj = update_obstacles(k); [delta_opt, a_opt] = mpc_solve_step(x_current, ref_traj, obs_traj, u_prev); u_current = [delta_opt, a_opt]; x_current = vehicle_update(x_current, u_current, Ts); % 记录并绘制轨迹... end

mpc_solve_step.m是整个文件中最需要花心思的部分，下面详细说。

3.3 核心代码段讲解与实现要点

mpc_solve_step.m我直接放一份完整可跑的框架代码（基于 CASADI 的 Opti 接口），然后逐段解释：

function [delta_opt, a_opt, sol] = mpc_solve_step(x0, ref_traj, obs, u_prev, Ts, N, L, params) import casadi.* % 模型参数与权重参数 L = 2.5; % 轴距 Ts = 0.1; % 采样时间 N = 15; % 预测时域步数 Q = diag([10; 10; 1; 1]); % 状态权重：x, y 位置权重最大 R = diag([0.1; 0.1]); % 控制量权重 Rd = diag([0.5; 0.5]); % 控制增量权重 rho = 1000; % 避障松弛变量惩罚 % 创建Opti对象 opti = casadi.Opti(); % 决策变量 X = opti.variable(4, N+1); % 状态序列：x, y, psi, v U = opti.variable(2, N); % 控制序列：delta, a eps_safe = opti.variable(1, N+1); % 避障松弛变量序列 % 参数（外部输入） P_x0 = opti.parameter(4, 1); P_ref = opti.parameter(4, N+1); P_obs = opti.parameter(2, size(obs,2)); % 障碍物初始位置 P_obs_vel = opti.parameter(2, size(obs,2)); % 障碍物速度 P_u_prev = opti.parameter(2, 1); % 目标函数 J = 0; for k = 1 : N+1 J = J + (X(:,k) - P_ref(:,k))' * Q * (X(:,k) - P_ref(:,k)); end for k = 1 : N J = J + U(:,k)' * R * U(:,k); end for k = 1 : N-1 U_diff = U(:,k+1) - U(:,k); J = J + U_diff' * Rd * U_diff; end J = J + rho * sum(eps_safe); % 松弛变量罚项 opti.minimize(J); % 动力学约束（前向欧拉离散） for k = 1 : N xk = X(:,k); uk = U(:,k); f_val = [ xk(4)*cos(xk(3)); xk(4)*sin(xk(3)); xk(4)/L * tan(uk(1)); uk(2) ]; opti.subject_to( X(:,k+1) == xk + Ts * f_val ); end % 控制量边界约束 opti.subject_to( -0.5 <= U(1,:) <= 0.5 ); % 前轮转角上下限 opti.subject_to( -3.0 <= U(2,:) <= 3.0 ); % 加速度上下限 % 初始状态约束 opti.subject_to( X(:,1) == P_x0 ); % 避障约束 for k = 1 : N+1 for j = 1 : size(obs,2) % 障碍物第k时刻的位置（匀速假设） obs_k = P_obs(:,j) + P_obs_vel(:,j) * (k-1) * Ts; dist_sq = (X(1,k) - obs_k(1))^2 + (X(2,k) - obs_k(2))^2; opti.subject_to( dist_sq >= (1.0)^2 - eps_safe(k) ); end end % 松弛变量非负约束 opti.subject_to( eps_safe >= 0 ); % 配置求解器 opti.solver('ipopt', struct('print_time', false), struct('print_level', 0)); % 赋值参数 opti.set_value(P_x0, x0); opti.set_value(P_ref, ref_traj); opti.set_value(P_obs, obs(1:2, :)); opti.set_value(P_obs_vel, obs(3:4, :)); opti.set_value(P_u_prev, u_prev); % 求解 sol = opti.solve(); % 返回第一个控制量 U_sol = sol.value(U); delta_opt = full(U_sol(1, 1)); a_opt = full(U_sol(2, 1)); end

这段代码有几个地方值得单独讲。

第一，搭优化问题用的是casadi.Opti()接口而不是原始的Function+nlpsol接口。Opti接口的抽象层次更高，写起来更接近数学原型的思路，约束、目标、变量可以直接通过opti.subject_to、opti.minimize、opti.variable来添加，代码可读性好很多，调试也方便。入门阶段推荐直接用Opti，不要一开始就去啃底层接口。

第二，注意前向欧拉离散化的写法。f_val是车辆模型连续微分方程，离散化后直接用X(:,k+1) == xk + Ts * f_val写成等式约束。CASADI求解器会把这个等式约束处理成残差的形式在迭代中满足，不需要你自己做数值积分。

第三，避障约束里用了eps_safe(k)做软约束。每个预测时刻都有自己的松弛变量，因为某个时刻可能安全距离够，另一时刻被逼得很紧——分开给变量能让优化器更灵活地权衡。

第四，full()函数。CASADI在Matlab中返回的变量并非常规Matlab数组，是MX/SX类型，要用full()转成普通double数组再往下传。

3.4 仿真参数选取与运行效果

参考路径初始阶段我用了一条带弯道的车道中心线。用一组离散角度与弧长采样生成路径点，保证路径包含直线段和弯曲段，能测试车道跟踪在不同曲率下的表现。参考状态序列ref_traj里，每个预测时刻对应的目标点是参考路径上距离当前车辆最近点之后若干弧长处的点。

这种“最近点投影+前向取点”的做法，比直接用全局索引取点更稳健，因为车辆在绕障过程中会出现横向偏移，如果坚持用固定的参考序列，参考误差会很大。最邻近投影后按弧长取前向点的逻辑，是实际调通绕障场景的关键细节。

障碍物场景设置上，我放了两个动态障碍物：一个从右侧向左侧横穿，一个在车道前方同向慢速行驶。前者考验MPC对横向穿行障碍物的规避能力，后者考验纵向减速和变道绕行能力。

仿真跑下来，比较理想的参数组合是：

• (T_s = 0.1) 秒
• (N = 15)
• Q 的位置权重设到 10，航向权重 1，速度权重 1
• R 设到 0.1，Rd 设到 0.5
• 松弛变量惩罚 (\rho = 1000)
• 安全距离阈值 1.0 米

在这个组合下，MPC能在0.01到0.05秒之间完成一次求解（取决于障碍物数量），车辆能平稳绕开动态障碍物并回到参考路径，绕障过程中的转向角变化也比较光滑。

4. 常见问题与调试技巧实录

4.1 优化无解 / 不可行问题

这是MPC项目里遇到最多的典型问题，表现形式就一句话：IPOPT 报Infeasible Problem。

我第一次遇到时也很懵，明明约束都写对了，怎么就没解了。后来逐项排查，发现其实无外乎几个原因。

第一个原因是避障硬约束太紧。车辆前方正中有一个障碍物，速度又压不下来，预测时域内无论怎么转方向盘，横向位移都不够在一两步内拉开到安全距离。这种情况下优化器没法让约束全部满足。解决办法就是我前面说的，引入松弛变量做软约束，让求解器在“无法安全避障”时能先“凑合往前开”，把问题留给下一步再解。在实际系统里，这种情况说明感知或上游规划已经给出了一条没有可行解的指令，MPC能做的就是以最小代价尝试逼近约束边界。

第二个原因是预测时域太长，累计误差放大导致约束无法满足。特别是前向欧拉离散化，步数越多误差累积越大，动力学约束本身可能就计算不准。可以缩短 (N)，或者换成更精确的离散化方法（比如二阶RK4），但后者会明显增加求解时间，需要权衡。

第三个原因是约束自相矛盾。比如参考路径本身穿过障碍物，而状态跟踪权重又设得非常大，优化器会同时被方向相反的两个力量拉拽，最终找不到平衡点。这种问题要回到场景层面检查：参考路径的分段生成逻辑里，绕行段应该由MPC自然生成，而不是强塞一条穿过障碍物的参考线下去。

排查不可行问题，我一般先把松弛变量和软约束加上去，再看是不是约束本身有冲突；都不行的话，把每条约束逐组注释掉，最小化复现问题，定位是哪一组约束导致的不可行。

4.2 跟踪震荡与控制不平滑

震荡的典型表现是车辆在参考路径附近来回摆动、转向角高频抖动。这个问题往往不是MPC原理问题，而是权重配比问题。

最常见的原因是位置误差权重 (Q) 设得太大，同时控制量权重 (R) 设得太小。优化器为了把位置误差压到极小，宁可给出大幅度和高频率的转向。解决方式就是在降低位置权重的同时，提高 (R_{\Delta})（控制增量权重）。我自己的调参经验是：R_Delta对平滑性的影响非常直接，想要车辆不抖，优先调它。

其次，模型离散化步长和采样时间不匹配会导致预测偏差。比如车辆模型本身用的是连续方程，控制周期是0.05秒，但离散化采用了0.1秒，那预测轨迹就会和真实轨迹有偏差，控制器很容易过冲。

第三个原因是参考点的提取逻辑不对。如果用固定的全局索引取参考点，车辆横向偏移后参考点会滞后或者突变，导致MPC每一步都在追一个前后跳动的目标。正确做法是前面提到的“最近点投影+前向取点”。

4.3 CASADI在Matlab中的使用细节

有几个坑我觉得值得记一下，新手很容易踩。

第一个是矩阵类型的转换。CASADI在Matlab中的变量类型不是常见的double，而是它自己的SX/MX类型。你把X(:,k)传给Matlab的内置函数时，经常报类型错误。要判断类型是否转换成功，可以用isa(var, 'casadi.MX')查一下，必要时用full()强制转成double。

第二个是符号变量规模的控制。CASADI强大的同时也意味着你把它拉进来参与计算的表达式越多，求解器负担越重。我试过把预测时域一下子设到50，Matlab直接卡住不动。大的预测时域要用高效写法，尽量避免用循环包裹不必要的重复计算。实际项目中N在10到20范围内更合适。

第三个是版本兼容性。CASADI的接口不同版本差异不算小，尤其是Opti接口的某些功能在老版本里可能不完全支持。建议直接下最新稳定版，别用太旧的包，否则不易对照文档排查问题。

第四个是IPOPT求解器的输出信息。看似啰嗦的一堆迭代日志，其实是有价值的。我调试时发现车辆无解或者振荡，都会看sum(cert) <= 999等退出信息和迭代中的可行性数据，判断当前主要卡在动力学约束还是障碍物约束上。开发阶段建议打开print_level到5左右，方便观察收敛过程，而不是全程设0，不然问题发生的时候你完全是“睁眼瞎”。

5. 还能往哪些方向扩展

代码能跑通之后，这个项目其实只完成了第一步。后面可以扩展的方向还挺多的。

一个是换成更实际的车辆模型。自行车模型忽略的轮胎侧偏在高速场景下会显现误差，可以升级成考虑轮胎力的动力学模型，但代价是状态维度和非线性程度都会明显增加，求解时间和数值稳定性都是一个挑战，需要配合更好的初值策略。

另一个是接入感知模块的真实输出。目前我是把障碍物初始位置和速度直接作为参数输入，如果接上目标检测和跟踪算法，让MPC的输入从“真值”变成“估计值”，那就接近一个完整的自动驾驶决策规划系统了。

还有一个方向是部署验证。CASADI支持代码生成，可以把求解过程编译成C代码，在实车控制器或者硬件在环仿真平台上跑。这个是从仿真走向落地的关键一步，很多学校项目都止步在Matlab仿真层面，企业实际需求中，能不能在嵌入式平台上实时运行是整个方案能否被采纳的分水岭。

如果你想把这个项目继续做下去，我建议先尝试增加更多复杂场景，例如连续弯道+多个动态障碍物的组合，以及车辆间交互场景。MPC的滚动优化特性决定了它对这种场景的适应能力很强，值得测试到极限工况。