车道跟踪和动态避障是移动机器人、低速自动驾驶里最常见的两个需求。我最早做的版本是“路径规划跟踪控制”分层设计规划层先绕开障碍物生成一条参考线控制层再用MPC去追。结果在动态障碍物场景里反复翻车——规划层绕障绕得很漂亮但跟踪层因为转向角饱和、加速度限制追不上最后横向误差大得离谱。后来我换成基于CASADI的集成方案把局部路径规划与跟踪控制合到一个模型预测控制问题里滚动优化时同时输出局部路径和下一步执行控制量动态避障时的稳定性提升非常明显。这篇文章把整套系统的建模、避障约束处理、MATLAB实现和调参经验完整写出来。目标是让正在搭类似系统的你能直接照着把代码落下来而不是再去翻一堆零散的论文和仓库。1. 为什么车道跟踪与动态避障必须“集成”着做1.1 分层的路径规划跟踪控制到底断在哪在介绍集成方案之前先说说我最初用的分层架构。上层全局规划负责在拓扑地图里找到一条从起点到终点的粗略路径中层局部规划把这条路径平滑化并且根据激光雷达点云做避障调整底层跟踪控制负责输出油门刹车和转向角。这套结构在静态场景里其实挺好用但一旦障碍物在动麻烦就来了。第一个问题出在“接口失真”。规划层做避障时用的往往是简化的质点模型或者低速运动学模型它根本不知道底层车辆的真实转向角上限和加速度上限。于是规划层给出一条绕障路径跟踪层一看弯太急转角饱和了追不上等到跟踪层终于绕过去障碍物已经走到另一个位置整个避障动作变成了一次危险的赌博。动态场景下这类规划与控制的“脱节”会被放大得很明显。第二个问题是“信息没有回环”。规划层计算路径时通常不反馈当前控制量是否受限跟踪层只能被动修正不能提前把执行边界反馈给规划。两者各自求解谁都不知道对方下一步的极限在哪里。这就是为什么很多时候规划轨迹画出来很漂亮实际跑起来却是另一种表现。在低速园区场景问题还不算致命速度一上来比如7到8米每秒误差就会迅速累积。所以我在那次项目里决定不再打补丁而是把规划和跟踪合并成一个问题。1.2 集成优化一条数学形式统一的道路集成方案的核心思想不复杂把“规划一条局部轨迹”和“跟踪这辆车的参考车道”合并成一个滚动优化问题。在每个控制周期求解器以当前车辆状态为起点在预测时域内同时搜索一条状态轨迹和一个控制序列目标函数同时惩罚车道跟踪误差、控制量和控制增量约束同时包含车辆运动学、执行机构边界以及动态障碍物距离边界。这比分层方案的“接力”多了什么规划结果是带物理可行性的。因为转向角、加速度、障碍物距离全部写在同一个约束里求解器给出的轨迹天然满足车辆的动态能力跟踪层不再需要去追一条本身就跑不了的路径。另一个好处是动态避障的“预测”可以和“控制”组合在同一个时域内——障碍物往前走控制器在每一步提前调整转向而不是等障碍物进入某个阈值后才触发一次重规划。代价也很直接问题规模变大非线性约束变多求解难度和实时性压力都上来了。这也正是CASADI这类工具的价值所在。1.3 CASADI在这个系统中的位置我对CASADI的理解是它不是一个“规划器”也不是一个“控制器”而是一个优化问题的建模和求解工具箱。你只需要把目标函数、约束条件用符号变量写出来CASADI负责自动求导、组装雅可比和海森矩阵再交给底层求解器去解。对于非线性MPC来说这一步省掉了手写导数的绝大部分体力活。我用的是它的Opti接口风格跟写数学公式很像opti.variable声明变量opti.minimize写目标opti.subject_to写约束最后opti.solve求解。在MATLAB环境下符号对象和数值矩阵可以混用调试起来比纯C路径顺手很多。对于科研原型、低速车载系统、课程项目这套组合相当合适。下面就从数学模型开始一步一步把问题搭起来。2. 模型与误差定义车道跟踪MPC的第一步2.1 车辆模型为什么不用质点模型路径规划里常用质点模型控制里常用自行车模型集成方案只能选一个我选的是运动学自行车模型。状态量取车辆在大地坐标系下的位置、航向角和车速状态x [px; py; theta; v]控制u [a; delta]其中a是纵向加速度delta是前轮转角运动学方程是px_dot v * cos(theta) py_dot v * sin(theta) theta_dot v * tan(delta) / L v_dot a其中L是前后轴轴距。这个模型不考虑轮胎侧偏但保留了最重要的两个物理约束转弯半径与轴距、转向角的关系以及加速度对车速的影响。对园区低速车和室内机器人来说这个精度完全够用。为什么不直接上更精细的动力学模型因为集成MPC本身已经有预测时域、避障约束、控制边界这些非线性东西模型每多一个状态求解难度就上一个台阶。运动学模型已经能反映“当前车速下能不能转过这个弯”这一点对避障规划才是最关键的。等你在仿真里跑通了再根据实际车辆把模型升级为带侧偏刚度的动力学模型也不迟。2.2 参考车道线的误差定义车道跟踪的本质是让车辆到参考车道线的横向偏差最小、航向偏差最小、车速偏差最小。参考车道线用一组离散点表示每个点包含位置(rx, ry)和期望航向rth。对每个预测步都需要找一个参考点来计算误差。我用的误差定义如下dx_g px - rx dy_g py - ry e_lat -sin(rth) * dx_g cos(rth) * dy_g e_yaw atan2(sin(theta - rth), cos(theta - rth)) e_v v - v_ref这里的e_lat是横向偏差正的表示车辆偏在参考线左侧e_yaw是航向偏差e_v是速度偏差。计算e_lat时为什么用-sin(rth)和cos(rth)做坐标变换因为要把全局坐标下的位置差投影到参考点的局部坐标系里。参考点航向rth是该点切线的方向角沿切线方向的分量是纵向误差垂直切线方向的分量就是横向误差。投影公式写起来简单但它是整个车道跟踪效果的基础写错一个符号MPC就会朝反方向打方向盘。注意e_yaw必须用atan2包一下角度差。如果不处理航向角在pi和-pi之间跳变时误差会出现一个2*pi的跳跃优化器会被这个虚假的大误差带偏导致转向抖动。这是最容易被忽视的细节之一。2.3 RK4离散化别在离散这步省时间MPC是离散时间优化必须先把连续模型离散化。最简单的是欧拉法x_{k1} x_k dt * f(x_k, u_k)。我一开始也这么干后来发现预测时域稍微拉长欧拉法的误差就会明显影响避障判断。举个例子车速8米每秒控制周期0.05秒一个预测步前进0.4米。看起来不多但20个预测步就是8米。欧拉法在急转弯处的航向误差累积起来会让MPC“看到”的轨迹和真实轨迹不一致避障约束明明算出来距离安全实车却差点贴上障碍物。所以我用了四阶Runge-KuttaRK4做离散化。RK4的核心思想是用多个中间斜率加权平均代替欧拉法只取起点斜率的方式。代码实现如下function dx vehicle_dynamics(x, u, L) % 运动学自行车模型 theta x(3); v x(4); a u(1); delta u(2); dx [v*cos(theta); v*sin(theta); (v/L)*tan(delta); a]; end function x_next rk4_step(x, u, dt, L) k1 vehicle_dynamics(x, u, L); k2 vehicle_dynamics(x dt/2*k1, u, L); k3 vehicle_dynamics(x dt/2*k2, u, L); k4 vehicle_dynamics(x dt*k3, u, L); x_next x dt/6 * (k1 2*k2 2*k3 k4); end在CASADI里x和u可以是符号变量vehicle_dynamics返回的是符号表达式RK4算完之后得到的x_next也是符号表达式。用opti.subject_to(X(:,k1) x_next)把这个离散递推关系作为等式约束就完成了动力学约束的建模。3. 动态避障约束建模非凸问题的工程化解法3.1 障碍物预测模型怎么选动态避障关键是“动态”两个字。如果障碍物位置固定把约束写成当前车辆到障碍物中心的距离大于安全半径就行可障碍物在动MPC必须把未来每个预测步的障碍物位置都估计出来才能提前规划绕行。我用的预测模型是最常见的匀速模型CV模型。设障碍物初始位置为(obs_x0, obs_y0)速度为(obs_vx, obs_vy)则在第k个预测步opx obs_x0 obs_vx * (k-1) * dt opy obs_y0 obs_vy * (k-1) * dt匀速模型简单可靠对横穿马路的行人、直行的车辆都能给出不错的估计。但要注意它假设障碍物方向不变如果遇到转弯的障碍物预测会失真。更高级的可以用CTRV模型恒定横摆角速度状态里加一个角速度项位置预测公式变复杂但能描述转弯。我的建议是先从匀速模型做起把整套避障逻辑跑通再根据实际场景升级预测模型。障碍物本身的形状用圆形包络。把车辆和障碍物都“膨胀”成圆车辆半径r_veh、障碍物半径r_obs再加上一个安全余量margin得到约束距离d_safe r_veh r_obs margin这个圆的半径怎么定很讲究。太保守会让MPC不敢接近障碍物在窄路里直接死机太激进会在真实场景里擦碰。低速园区我一般留0.5米余量普通道路留1米以上。3.2 距离约束的非凸性避障约束写出来是sqrt((px - opx)^2 (py - opy)^2) d_safe这个约束看起来简单但它是一个非凸约束。原因很直观满足“到障碍物中心距离大于d_safe”的点的集合是整个平面挖掉一个圆。这个集合不是凸集——你在圆的左侧和右侧各取一个可行点连线会穿过禁入区域。非凸约束对求解器非常不友好优化器可能陷入局部解或者在“从左边绕还是从右边绕”的抉择中反复震荡。工程上最常见的做法是线性化。设当前迭代中车辆位置为(px0, py0)到障碍物的距离为d0则距离函数在该点附近做一阶泰勒展开d ≈ d0 (px0 - opx)/d0 * (px - px0) (py0 - opy)/d0 * (py - py0)把这条线性化的距离约束替代原来的非线性约束约束就变成了仿射约束整个问题也更接近凸问题。线性化的展开点取上一次求解得到的解也就是SQP的思想每次迭代重新线性化一次求解器逐步逼近真实解。CASADI的符号求导能力在这里很省力不需要手算雅可比。如果障碍物不止一个线性化约束的数量等于“障碍物数乘以预测步数”计算量会涨得很快。我的经验是先对每个预测步都建约束然后在下一次迭代时只保留那些离车辆较近的障碍物约束远处的省掉求解时间能减少不少。3.3 软约束与多重松弛策略即使加了线性化多个动态障碍物场景下IPOPT这类内点法求解器还是会经常找不到可行解。一个实用的办法是把避障写成软约束而不是硬约束。具体做法是引入一个松弛变量s_k 0把约束变成d_k d_safe - s_k同时在目标函数里加上对松弛变量的惩罚比如w * s_k^2。这样当硬约束实在无解时求解器宁可稍微侵入安全距离也不会直接返回失败。它的实际意义是你可以接受一次“轻微逼近”的优化解然后用下一帧感知数据重新规划而不是让车辆在障碍物面前停下来。我强烈建议你采用“硬约束为主、软约束兜底”的组合平时s_k保持在零附近真遇到无法回避的拥挤场景时松弛变量自动放大保证控制器始终输出有效控制量。这个策略在我的实车调试里救了很多次。4. MATLABCASADI代码骨架从Opti问题到主循环4.1 环境配置CASADI的MATLAB版本是一个预编译的库文件下载后解压在MATLAB里把路径加进去就能用addpath(genpath(casadi-3.6.5-linux-matlab-mex));注意版本要和你的系统平台对应Windows、Linux、macOS的包名不一样。配置完成后运行以下代码验证是否正常import casadi.* disp(CasadiMeta.version());能输出版本号就说明环境没问题。我一般把addpath写进MATLAB的startup.m这样每次启动自动加载。推荐直接用Opti接口而不是底层SX加Function的方式。原因有三个Opti更接近“写数学问题”的直觉变量、约束、目标分开声明不易出错。约束可以命名调试时能直接打印某个约束的值。opti.value()可以很方便地把符号求解结果转成数值适合滚动时域循环。4.2 核心优化问题构建下面这段代码是车道跟踪加动态避障MPC的核心骨架。变量声明、目标函数、动力学约束、控制边界、避障约束都在这里。import casadi.* dt 0.05; % 控制周期 N 20; % 预测时域 L 2.5; % 车辆轴距 v_ref 8; % 期望巡航速度 d_safe 1.5; % 安全距离 nx 4; nu 2; opti casadi.Opti(); % 优化变量状态轨迹与控制轨迹 X opti.variable(nx, N1); U opti.variable(nu, N); % 当前状态设为参数方便滚动时域中反复赋值 x_init opti.parameter(nx); opti.subject_to(X(:,1) x_init); % 参考车道线与动态障碍物在主循环里更新 refs casadi.MX.sym(refs, 3, N1); % 占位说明实际可用数值数组替代 obs_x0 casadi.MX.sym(obs_x0); obs_y0 casadi.MX.sym(obs_y0); obs_vx casadi.MX.sym(obs_vx); obs_vy casadi.MX.sym(obs_vy); % 目标函数 Q diag([8, 1, 0.5]); % 横向误差、航向误差、速度误差 R diag([0.1, 2]); % 加速度、转角的惩罚 Rd diag([0.05, 8]); % 控制增量的惩罚 cost 0; for k 1:N % 参考点由外部传入 rx refs(1,k); ry refs(2,k); rth refs(3,k); % 横向误差与航向误差 dxg X(1,k) - rx; dyg X(2,k) - ry; e_lat -sin(rth)*dxg cos(rth)*dyg; e_yaw atan2(sin(X(3,k)-rth), cos(X(3,k)-rth)); e_v X(4,k) - v_ref; cost cost [e_lat, e_yaw, e_v] * Q * [e_lat, e_yaw, e_v] ... U(:,k) * R * U(:,k); % 控制增量惩罚 if k 1 du U(:,k) - U(:,k-1); cost cost du * Rd * du; end end opti.minimize(cost); % 动力学约束RK4离散 for k 1:N xk X(:,k); uk U(:,k); k1 vehicle_dynamics(xk, uk, L); k2 vehicle_dynamics(xk dt/2*k1, uk, L); k3 vehicle_dynamics(xk dt/2*k2, uk, L); k4 vehicle_dynamics(xk dt*k3, uk, L); x_next xk dt/6 * (k1 2*k2 2*k3 k4); opti.subject_to(X(:,k1) x_next); end % 控制边界 opti.subject_to(-3 U(1,:) 2); % 加速度范围 opti.subject_to(-0.5 U(2,:) 0.5); % 转向角范围 % 动态避障约束 for k 1:N t (k-1) * dt; opx obs_x0 obs_vx * t; opy obs_y0 obs_vy * t; dist sqrt((X(1,k)-opx)^2 (X(2,k)-opy)^2); opti.subject_to(dist d_safe); end % 求解器配置 opti.solver(ipopt, struct(print_level,0), struct(print_level,0));这段代码里的refs和障碍物参数在实际代码中应该用数值变量传入我为了控制篇幅用了符号说明。你在搭建时要把参考线和障碍物运动参数在主循环里以数值数组方式代入。4.3 滚动求解主循环热启动与状态更新MPC不是只解一次而是每个控制周期都解一次。主循环要做四件事设置当前状态参数、设置初值、求解、取第一个控制量执行。x_cur [0; 0; 0; v_ref]; % 车辆初始状态 u_last [0; 0]; X_sol repmat(x_cur, 1, N1); U_sol zeros(nu, N); history []; for step 1:300 % 1. 在当前车辆位置附近找最近的参考点索引截取预测时域内的参考线 base_idx find_nearest_idx(x_cur(1:2), ref_all); refs ref_all(:, base_idx : base_idx N); % 2. 更新当前状态参数 opti.set_value(x_init, x_cur); % 3. 热启动把上一帧的解作为初值 opti.set_initial(X, X_sol); opti.set_initial(U, U_sol); % 4. 求解 try sol opti.solve(); solve_time toc; % 配合tic使用 u_apply opti.value(U(:,1)); u_last u_apply; X_sol opti.value(X); U_sol opti.value(U); catch warning(MPC solve failed, use last valid control); u_apply u_last; end % 5. 用RK4仿真车辆状态 x_cur rk4_step(x_cur, u_apply, dt, L); % 记录日志 history [history, [x_cur; u_apply]]; end热启动是关键。如果把初值都设为全零IPOPT每次都要从头搜索动态避障场景下大概率在“左绕还是右绕”上反复试探求解时间飙升。把上一帧的可行轨迹平移一列作为当前初值求解通常在几轮迭代内收敛。find_nearest_idx是我自己写的一个最近点搜索函数遍历参考线点集找最近包络点。如果参考线点间距很密可以加一步均匀采样缩小搜索范围省掉每周期全量遍历的时间。5. 参数整定与踩坑复盘5.1 参数初值与调参顺序MPC参数太多没有一套“万能配置”。我给出一个相对通用的初值组合并说明调节方向。参数推荐初值调节方向控制周期dt0.05秒减小可提高响应但增大求解负担预测时域N15~25过短反应慢过长求解慢Q(1) 横向误差权重5~10避障时过大会导致急转Q(2) 航向误差权重1保持航向平顺Q(3) 速度权重0.5让车速贴合期望速度R(1) 加速度权重0.1主要靠它控制纵向波动R(2) 转向角权重2防止转向角过大Rd(2) 转向增量权重5~10越大转向越平顺d_safe1.0~2.0按车辆宽度和场景调整调参顺序有讲究。我建议先关掉避障约束或者把safe设得很小纯车道跟踪跑通确认横向误差和航向误差都能收敛后再逐步放开避障约束。一来就上全功能出了问题根本不知道是哪一层参数导致的。先调Q再调R最后调Rd。Q决定“跟踪优先级”R决定“控制代价”Rd决定“转向平顺性”。直线跟踪不稳先加大Q(1)弯道太快先加大R(2)转向抖再加大Rd(2)。5.2 三个最容易踩的坑第一个坑是航向误差不做角度包裹。前面已经提过theta - rth直接进入代价函数遇到角度从pi跳变到-pi时误差从2*pi跳回2*pi优化器以为车辆航向偏差巨大会输出剧烈的反向转向。用atan2(sin(...), cos(...))包一层三秒解决。这是我见到过的最常见的MPC“灵异现象”。第二个坑是参考点索引不随车速更新。如果参考线按固定距离采样而你只用固定索引范围车速一变化预测时域里的参考点就跟真实位置错位误差计算出来就是歪的。正确做法是根据期望车速估算每个预测步的前进距离换算成参考点索引偏移量或者在主循环里实时找最近点。第三个坑是避障约束和车道误差约束的拉扯。避障要求车辆远离障碍物车道跟踪要求车辆回到参考线两者在同一个优化问题里相互竞争。如果Q(1)太大车辆会在障碍物边缘急拉回车道线产生一个大S弯如果避障权重太大车又长期偏离车道。我的经验是先把横向误差权重控制在8附近再通过加大转向增量惩罚Rd(2)来抑制急剧的横向修正动作。绕障过程允许短时间离开参考线但转向角变化要受控。5.3 实时性评估与降级方案用tic/toc把每个控制周期的求解耗时打出来你会看到明显的统计规律。低速场景下IPOPT求解非线性MPC通常在50到200毫秒之间。如果超过200毫秒先看预测时域是不是太大把N从20降到15通常就能达标。如果N减小后仍然超时说明问题不在规模而在收敛困难。这时候我一般做两件事一是把避障约束做线性化处理把非凸问题转成近似凸问题收敛速度会快很多二是限制IPOPT的最大迭代次数即使没完全收敛也把当前迭代点对应的控制量拿出来用。后者听起来粗暴但配合软约束兜底实际表现比“求解失败直接刹车”好得多。另外提醒一下仿真里状态更新用的是rk4_step真车状态下应该换成实车传感器反馈和底层控制器执行结果。MPC下发的是加速度和转向角实际车辆执行还有延迟这个延迟如果比较大需要在动力学约束里补偿一阶惯性环节否则会出现持续的小幅震荡。最后再分享一个我个人的实操体会整套系统跑通后最难的不是写CASADI代码而是理解“预测、约束、代价”三者怎么拧在一起。CASADI帮你省去了手写求导的苦但系统设计、参数标定、异常降级这些“理解问题”的部分它替不了你。如果你正打算做类似的项目我强烈建议你也从纯车道跟踪MPC起步先把一个简化问题调明白再逐步加入动态障碍物和更复杂的场景。这个顺序看起来慢实际是通往稳定系统的最近路径。