车道跟踪配合动态避障这种需求放在真实的自动驾驶项目里远比听上去复杂。很多人一开始的思路是分开做上游跑一个全局路径规划下游用纯跟踪或者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_{k1} x_k T_s \cdot v_k \cos\psi_k ] [ y_{k1} y_k T_s \cdot v_k \sin\psi_k ] [ \psi_{k1} \psi_k T_s \cdot \frac{v_k}{L} \tan\delta_k ] [ v_{k1} 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_{k0}^{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_{k0}^{N-1} U_k^T R U_k \sum_{k1}^{N-1} (U_k - U_{k-1})^T R_{\Delta} (U_k - U_{k-1}) ]前一半惩罚控制量本身的大小避免转向过大、加速度过猛后一半惩罚控制量的变化率让输出更平滑。这个项非常重要没有它的话优化器容易给出前后两步差别很大的控制指令车辆会抖动。第三部分是松弛变量惩罚[ J_{\text{safe}} \rho \sum_{k0}^{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); % 记录并绘制轨迹... endmpc_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, N1); % 状态序列x, y, psi, v U opti.variable(2, N); % 控制序列delta, a eps_safe opti.variable(1, N1); % 避障松弛变量序列 % 参数外部输入 P_x0 opti.parameter(4, 1); P_ref opti.parameter(4, N1); 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 : N1 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(:,k1) - 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(:,k1) 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 : N1 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()接口而不是原始的Functionnlpsol接口。Opti接口的抽象层次更高写起来更接近数学原型的思路约束、目标、变量可以直接通过opti.subject_to、opti.minimize、opti.variable来添加代码可读性好很多调试也方便。入门阶段推荐直接用Opti不要一开始就去啃底层接口。第二注意前向欧拉离散化的写法。f_val是车辆模型连续微分方程离散化后直接用X(:,k1) 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速度权重 1R 设到 0.1Rd 设到 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强大的同时也意味着你把它拉进来参与计算的表达式越多求解器负担越重。我试过把预测时域一下子设到50Matlab直接卡住不动。大的预测时域要用高效写法尽量避免用循环包裹不必要的重复计算。实际项目中N在10到20范围内更合适。第三个是版本兼容性。CASADI的接口不同版本差异不算小尤其是Opti接口的某些功能在老版本里可能不完全支持。建议直接下最新稳定版别用太旧的包否则不易对照文档排查问题。第四个是IPOPT求解器的输出信息。看似啰嗦的一堆迭代日志其实是有价值的。我调试时发现车辆无解或者振荡都会看sum(cert) 999等退出信息和迭代中的可行性数据判断当前主要卡在动力学约束还是障碍物约束上。开发阶段建议打开print_level到5左右方便观察收敛过程而不是全程设0不然问题发生的时候你完全是“睁眼瞎”。5. 还能往哪些方向扩展代码能跑通之后这个项目其实只完成了第一步。后面可以扩展的方向还挺多的。一个是换成更实际的车辆模型。自行车模型忽略的轮胎侧偏在高速场景下会显现误差可以升级成考虑轮胎力的动力学模型但代价是状态维度和非线性程度都会明显增加求解时间和数值稳定性都是一个挑战需要配合更好的初值策略。另一个是接入感知模块的真实输出。目前我是把障碍物初始位置和速度直接作为参数输入如果接上目标检测和跟踪算法让MPC的输入从“真值”变成“估计值”那就接近一个完整的自动驾驶决策规划系统了。还有一个方向是部署验证。CASADI支持代码生成可以把求解过程编译成C代码在实车控制器或者硬件在环仿真平台上跑。这个是从仿真走向落地的关键一步很多学校项目都止步在Matlab仿真层面企业实际需求中能不能在嵌入式平台上实时运行是整个方案能否被采纳的分水岭。如果你想把这个项目继续做下去我建议先尝试增加更多复杂场景例如连续弯道多个动态障碍物的组合以及车辆间交互场景。MPC的滚动优化特性决定了它对这种场景的适应能力很强值得测试到极限工况。