企业官网建设流程全解析

车道跟踪和动态避障，是移动机器人、低速自动驾驶里最常见的两个需求。我最早做的版本是“路径规划+跟踪控制”分层设计：规划层先绕开障碍物生成一条参考线，控制层再用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_{k+1} = x_k + dt * f(x_k, u_k)。我一开始也这么干，后来发现预测时域稍微拉长，欧拉法的误差就会明显影响避障判断。

举个例子：车速8米每秒，控制周期0.05秒，一个预测步前进0.4米。看起来不多，但20个预测步就是8米。欧拉法在急转弯处的航向误差累积起来，会让MPC“看到”的轨迹和真实轨迹不一致，避障约束明明算出来距离安全，实车却差点贴上障碍物。

所以我用了四阶Runge-Kutta（RK4）做离散化。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(:,k+1) == 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. MATLAB+CASADI代码骨架：从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的方式。原因有三个：

1. Opti更接近“写数学问题”的直觉，变量、约束、目标分开声明，不易出错。
2. 约束可以命名，调试时能直接打印某个约束的值。
3. 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, N+1); U = opti.variable(nu, N); % 当前状态设为参数，方便滚动时域中反复赋值 x_init = opti.parameter(nx); opti.subject_to(X(:,1) == x_init); % 参考车道线与动态障碍物（在主循环里更新） refs = casadi.MX.sym('refs', 3, N+1); % 占位说明，实际可用数值数组替代 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(:,k+1) == 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, N+1); 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参数太多，没有一套“万能配置”。我给出一个相对通用的初值组合，并说明调节方向。

调参顺序有讲究。我建议先关掉避障约束，或者把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起步，先把一个简化问题调明白，再逐步加入动态障碍物和更复杂的场景。这个顺序看起来慢，实际是通往稳定系统的最近路径。