模型预测控制在汽车路径跟随中的应用与工程实践

简介一套面向自动驾驶与智能车辆方向学习者的Simulink仿真资料围绕模型预测控制MPC在汽车路径规划与路径跟随中的应用展开。资料基于MATLAB/Simulink搭建完整的汽车路径规划仿真系统涵盖路径生成、平滑处理和MPC控制器设计等关键环节适合研究车辆横向控制、轨迹跟踪或需要快速搭建MPC仿真平台的工程师与研究人员。压缩包共4个文件包含Simulink模型.mdl、数据文件.mat、脚本.m及参数配置文件.cpar分别用于仿真主模型、车辆与路径数据、轨迹规划算法实现和控制器参数设置整体仅1.81MB结构紧凑便于二次开发。该资源已有1284人学习下载内容提供了可直接运行的MPC路径跟随基础框架能够帮助读者快速理解建模思路与控制流程。在此基础上使用者可进行参数调试、算法改进或扩展为更复杂的自动驾驶控制策略是兼顾理论理解与工程实践的实用参考资料。1. 为什么汽车路径跟随要单独谈 MPC 控制路径规划负责说“往哪开”路径跟随负责回答“方向盘现在打多少”。低速缓弯用纯跟踪或 PID 还能凑合一旦进入匝道、倒车入库这种曲率连续变化的场景只看当前位置偏差打方向必然滞后车会在参考线两侧画龙。模型预测控制MPC是汽车工程应对这类问题的主流做法每个控制周期用车辆模型往后推演十几步把整段预测轨迹对参考路径的误差和转向、加速度限制一起交给优化器求解再滚动执行第一组控制量。下面把路径规划、路径跟随、MPC控制三者的数据链路拆开讲从模型建立讲到动态避障和实车验证适合做自动驾驶、线控底盘、类车机器人运动控制的工程师。2. 模型预测控制做路径跟随预测模型与滚动优化2.1 用运动学自行车模型描述车辆状态MPC 的控制品质很大程度取决于预测模型是否抓准了车辆的运动本质。汽车工程做路径跟随最常见的第一版模型是运动学自行车模型kinematic bicycle model。它把前后轴各自合并到一条轴线用前后轮平均转角近似前轮转向角 δ状态取大地坐标系下的位置 (x, y)、横摆角 ψ 和纵向速度 vx v cos ψ y v sin ψ ψ v / L tan δ v a这个模型成立的前提是轮胎侧偏可以忽略车身绕瞬时转向中心旋转。低速园区、泊车、匝道跟随场景误差足够小参数只有一个轴距 L标定成本低。如果车速长期超过 60 km/h、频繁出现大侧向加速度运动学模型会明显乐观需要换成带前后轴侧偏刚度的动力学自行车模型。从业者通常的做法是先用运动学模型把控制链路打通再根据实测状态误差决定是否升级。下面是把离散运动学模型写成一个可复用的 Python 函数输入当前状态和控制量输出下一步状态。这段函数同时是后面 MPC 预测循环的核心import numpy as np def kinematic_step(x, u, dt, L): # x: [x, y, psi, v]u: [a, delta] x_next np.zeros(4) x_next[0] x[0] dt * x[3] * np.cos(x[2]) x_next[1] x[1] dt * x[3] * np.sin(x[2]) x_next[2] x[2] dt * x[3] / L * np.tan(u[1]) x_next[3] x[3] dt * u[0] return x_next这里 dt 是采样周期L 是轴距米。注意 ψ 的增量用 v/L·tan(δ) 计算而不是直接对 δ 积分。很多第一次写 MPC 的人把航向角变化量写成 δ 或 v·δ/L低速下两者数值接近但在大转角时会差出几十厘米级的位置误差。运动学模型对 v0 的状态不可逆因此倒车入库场景要么用专门的倒车参考路径加反向速度符号处理要么换成可逆的类车模型。2.2 预测时域中的优化问题目标函数与约束有了离散模型MPC 的优化问题可以写成一个标准的有限时域最优控制问题。预测时域取 N 步每一步用上一节的状态转移函数向后递推。目标函数分为三部分预测轨迹与参考轨迹的状态偏差、控制量本身的幅值、以及控制量的变化率。控制量变化率惩罚项在工程上非常重要它抑制方向盘抖振也是解决“求解器每拍输出突变”的第一道关口。约束按物理特性和安全要求设置。最常见的是执行器限幅前轮转角 δ 有范围加速度 a 有上下限如果控制器直接下发纵向速度还要加 v 的限幅。目标函数的紧凑写法是min Σ ( z_k − z_ref,k )ᵀ Q ( z_k − z_ref,k ) u_kᵀ R u_k ( u_k − u_{k−1} )ᵀ S ( u_k − u_{k−1} )约束条件为z_{k1} f(z_k, u_k)δ_min ≤ δ_k ≤ δ_maxa_min ≤ a_k ≤ a_max。Q 通常取对角矩阵权重分别对应横向位置、纵向位置、横摆角、速度。横向位置权重要远大于纵向位置因为路径跟随的核心指标是横向偏差横摆角权重负责让车头朝向与路径方向一致入弯时尤其起作用。R 惩罚控制幅值S 惩罚控制变化率系数一般都比 Q 小一个数量级否则跟踪会变成“慢慢悠悠不跟上”。2.3 与纯跟踪和 LQR 的对比MPC 的钱花在哪纯跟踪pure pursuit用当前点到前方预瞄点画一段圆弧转弯半径由预瞄距离决定。它对参数不敏感、鲁棒但预瞄距离是车速和道路曲率的函数弯道里跟踪误差无法显式控制。LQR 是线性控制里最接近 MPC 的对手无限时域无约束最优控制反应快、实现轻但处理不了执行器饱和和动态避障这类显式约束。MPC 的优势就在这三件事上有预测能力、有约束处理、参考轨迹可以随时换。方法预测能力约束处理计算开销典型场景纯跟踪单点预瞄不处理极低低速巡逻、机器人底盘跟随LQR无限时域仅线性限幅低高速车道保持、姿态稳定MPC有限时域滚动硬/软约束中到高泊车、主动避障、变曲率匝道表格里三个方法不是互斥关系。实车上常见的是用 LQR 做高速工况基础控制用 MPC 做泊车、避障等需要约束的工况两套控制器之间做切换矩阵。MPC 的代价是每个周期要解一个非线性规划NLP需要专门的求解器与实时性保障这个问题在第四章和第五章具体展开。3. 路径规划输出什么MPC 控制器才跟得住3.1 全局路径与局部路径的分工路径规划通常分两层。全局规划在已知地图上做较粗的路径搜索给出一段从起点到目标点的参考走廊常见方法包括 A*、RRT*、混合 A* 等输入是栅格地图或拓扑路网输出是每 520 米一个的路径点。局部规划在全局轨迹附近处理动态障碍物和车辆运动学约束输出的是百米级、逐帧更新的局部轨迹也就是真正送给 MPC 的参考序列。全局规划和局部规划的分工边界要刻进接口里否则 MPC 拿到的参考轨迹一会跳到全局直方图、一会跳到局部避障线控制器会不断画龙。动态避障小车路径规划正是这个接口里最常见的业务。低速小车的运动学约束比整车宽松但探测范围小、障碍物出现突然局部规划器必须在下游给 MPC 留出“可执行速度包线”。比较稳妥的做法是局部规划器只输出一条已满足运动学约束的轨迹MPC 不再做避障大决策只负责把横向偏差和速度偏差收敛到零。这样责任边界清晰实车出问题时查起来也快。3.2 参考路径的离散化位置、航向、曲率、目标速度规划器输出的原始路径点通常只是 (x, y) 或 (x, y, θ)这对路径可视化够了但 MPC 要吃更结构化的数据。我在接口层一般维护一个参考轨迹类字段如下字段含义典型来源t参考时间戳规划器每帧生成时刻x, y大地坐标位置全局/局部规划输出psi期望横摆角由路径方向角计算kappa曲率差分或样条导数v_ref目标速度曲率和法规限制联合计算obstacle_list障碍物边界点感知模块输出库里只需要一份离散化好的参考点序列采样间距视场景调整。高速车道保持用 0.51.0 米间距泊车和低速避障用 0.10.2 米间距。间距过大会让曲率估计抖动过小又会让大曲率段的数据冗余、求解器每次扫描的参考点过多。对 MPC 来说真正关心的不是点有多密而是参考航向和参考速度要平滑。原始规划点里偶发的噪声点必须在进控制器前做一次平滑常见手段是三次样条拟合或曲率滤波器。3.3 从规划器到控制器的接口参考轨迹序列MPC 每个周期需要未来 N 步的参考状态而规划器往往只在有新障碍物、新任务或周期性刷新时才输出新轨迹。两者之间要有一层“参考序列提取”逻辑控制器维护一个索引按当前车速和采样周期外推出未来若干步的参考位姿。下面是一个最小接口实现的 Python 片段重点展示参考序列组装和索引推进def extract_ref_sequence(path, state, N, dt, ds0.5): # path: 已平滑的参考轨迹列表每项含 x, y, psi, v # state: 当前车辆状态 [x, y, psi, v] # N: 预测时域dt: 采样周期ds: 参考轨迹采样间距 idx nearest_index(path, state[0], state[1]) seq [] for k in range(N 1): # 按车速外推预测时刻走过的弧长再折算成路径点索引 arc state[3] * k * dt j min(idx int(arc / ds), len(path) - 1) seq.append(path[j]) return seq索引前移距离用车速乘以预测时间计算避免用固定时间步采样导致高速时参考序列原地踏步。近邻搜索可以用 KDTree 或简单的线性扫描N 在 20 左右时线性扫描的开销可以忽略。这个接口看起来简单但实际踩坑最多局部规划输出的轨迹坐标系可能切换车体系到地图系参考速度可能与当前车速差太多MPC 会在第一个周期就请求一个不可达的加速度。接口层必须把参考速度做斜坡限幅保证预测轨迹可达。4. 用 Python 和 CasADi 跑通最小 MPC 路径跟随系统4.1 用 CasADi 搭非线性预测问题Python 里做 MPC 最常见的方式是用 CasADi 的 Opti 栈。它把优化变量、约束、目标函数用符号对象描述求解器负责把 NLP 交给 IPOPT 等底层求解器。下面这段代码构建了一个运动学模型下的 MPC 问题预测时域 20 步采样周期 0.1 秒。import casadi as ca import numpy as np dt 0.1 N 20 L 2.7 opti ca.Opti() # 决策变量状态轨迹、控制轨迹、控制增量 X opti.variable(4, N 1) # [x, y, psi, v] U opti.variable(2, N) # [a, delta] DU opti.variable(2, N) # 参数初始状态、参考轨迹、上一拍控制输入 x0 opti.parameter(4) ref opti.parameter(4, N 1) u_prev opti.parameter(2, 1) opti.set_value(x0, [0.0, 0.0, 0.0, 5.0]) opti.set_value(ref, np.zeros((4, N 1))) opti.set_value(u_prev, [0.0, 0.0]) Q ca.diag([8.0, 8.0, 2.0, 1.0]) # 位置、航向、速度权重 R ca.diag([1.0, 1.0]) # 控制幅值权重 S ca.diag([0.5, 2.0]) # 控制增量权重 opti.subject_to(X[:, 0] x0) for k in range(N): x, y, psi, v X[0, k], X[1, k], X[2, k], X[3, k] a, delta U[0, k], U[1, k] x_next x dt * v * ca.cos(psi) y_next y dt * v * ca.sin(psi) psi_next psi dt * v / L * ca.tan(delta) v_next v dt * a opti.subject_to(X[:, k 1] ca.vertcat(x_next, y_next, psi_next, v_next)) if k 0: opti.subject_to(DU[:, k] U[:, k] - u_prev) else: opti.subject_to(DU[:, k] U[:, k] - U[:, k - 1]) cost 0 for k in range(N 1): cost (X[:, k] - ref[:, k]).T Q (X[:, k] - ref[:, k]) for k in range(N): cost U[:, k].T R U[:, k] cost DU[:, k].T S DU[:, k] opti.minimize(cost) opti.solver(ipopt)代码先定义了状态轨迹和控制轨迹两个符号变量再把运动学模型写成等式约束。DU 表示控制增量第一个周期与上一时刻的实际控制输入做差避免把“方向盘还在原来位置”当成新增量。目标函数用矩阵乘法求二次型Q 对位置的惩罚占大头S 对 delta 增量的惩罚放在 2.0抑制方向盘高频抖动。4.2 转向和加速度约束怎么加物理限制直接加到符号变量上即可。转向角一般限在 ±0.6 rad 左右加速度按动力性和舒适性给 ±2 m/s²更高车速时还可以在不等式约束里加入参考速度的软上限opti.subject_to(opti.bounded(-2.0, U[0, :], 2.0)) opti.subject_to(opti.bounded(-0.6, U[1, :], 0.6)) opti.subject_to(X[3, :] 0.0) # 倒车场景按需放开注意 bounded 的写法会对整段轨迹同时加约束如果某个控制量在某一步超限求解器会通过优化调整所有相关步。这里最容易犯的错是把加速度约束加在参考轨迹上而不是控制轨迹上导致求解出的速度曲线和约束不一致。MPC 的约束处理能力只有在“预测轨迹上的每一步”都满足要求才算数这也是它优于直接限幅控制的地方。4.3 滚动优化循环只执行第一拍求解只是第一步MPC 的滚动机制在于每周期用最新状态重新求解取第一个控制量执行下一周期重复。下面是把上面问题封装成函数后的循环骨架state np.array([0.0, 0.0, 0.0, 5.0]) last_u np.array([0.0, 0.0]) for step in range(300): new_ref load_reference(step) # 从规划器拿参考序列 opti.set_value(x0, state) opti.set_value(ref, new_ref) opti.set_value(u_prev, last_u) sol opti.solve() u_cmd sol.value(U[:, 0]) state kinematic_step(state, u_cmd, dt, L) last_u u_cmd每次 solve 之前要把 x0、ref、u_prev 三个参数重新赋值它们都是 4.1 节里定义的 parameter滚动循环的本质就是不断更新这些参数。求解之后取 U 的第一列作为控制量再用运动学模型推进状态。实际项目里求解返回后还要检查求解状态标志处理超时、不可行等异常分支。滚动循环的更新频率就是采样周期 dt0.1 秒对应 10 Hz对低速路径跟随足够高速工况一般做到 2050 Hz对求解器性能要求更高。4.4 参数初值表与第一版结果预期拿到第一版能跑的 MPC建议先看三个指标横向偏差 RMS、方向盘增量最大值、求解耗时。表格里是一组从低速小车到乘用车都比较稳的经验初值参数低速小车乘用车园区高速变道N152020302540dt0.050.1 s0.1 s0.05 sQ 横向位置5108151020S 转向增量13120.51求解目标耗时 20 ms 30 ms 40 ms如果横向偏差持续稳定在 0.1 米量级但不收敛到零多半是参考航向和参考点位对不齐先去检查坐标平滑如果方向盘频繁换向再增大 S。求解耗时超过去一半采样周期就要降低 N 或简化约束否则求解器退化风险很高这正好是第五章要展开的问题。5. 参数整定与隐藏坑预测时域、离散化和求解时延5.1 预测时域 N 的取舍N 决定控制器的“眼光”有多远。单纯把 N 拉大并不总是更好N 大预测覆盖时域变长高速变道时更稳定但非线性模型在远时域上的线性化误差累积也大求解难度上升最坏求解时间线性甚至超线性增长。判断 N 是否合适的经验依据是 N·dt 要覆盖车辆完成一个典型机动的时间。入弯减速到出弯大约需要 2 到 3 秒那么 N·dt 至少要有 2.5 秒LQR 失效的主要场景往往就是预测长度不够覆盖整条弯道。控制时域如果和预测时域一样长控制变量的自由度太多会让求解器在尾部产生无用动作。常见做法是只对前 Nc 步比如 Nc58优化控制量后面步用零增量或保持前一步值把多余的自由度省掉。这个细节对求解时间的影响非常明显N30 时把控制时域压到 8IPOPT 的迭代次数经常能下降一半。5.2 权重矩阵的调整顺序权重调整最怕一次同时改好几个系数回头根本不知道是哪个改动起效。通常按三步走先调 Q 里的横向位置权重到 810让基础跟踪不发散再调航向权重直到入弯时车头朝向误差小于 5 度最后才动 S。R 一般保持较小因为纯幅值惩罚过大会让控制器“舍不得踩油门打方向”跟踪响应变慢。判断权重是否过大的直接信号是预测轨迹在参考线附近出现周期性的小幅震荡像是橡皮筋在拉伸。此时减小 Q 或增大 S 都能缓解优先动 S。5.3 前向欧拉和 RK4 的差别第四章代码用前向欧拉离散写起来方便但采样周期大时误差明显。dt0.1 s、车速 10 m/s 时前向欧拉在 1 秒后能累积出十几厘米的位置偏差对 MPC 来说这个偏差会被优化器当成“跟踪误差”努力去补反而制造多余的转向动作。把运动学模型改成四阶龙格库塔RK4只需在模型函数里多写几行def kinematics(x, u, L): # 返回状态导数 [dx, dy, dpsi, dv] return np.array([ x[3] * np.cos(x[2]), x[3] * np.sin(x[2]), x[3] / L * np.tan(u[1]), u[0] ]) def kinematic_rk4(x, u, dt, L): k1 kinematics(x, u, L) k2 kinematics(x dt/2 * k1, u, L) k3 kinematics(x dt/2 * k2, u, L) k4 kinematics(x dt * k3, u, L) return x dt/6 * (k1 2*k2 2*k3 k4)CasADi 的符号变量同样支持这种写法只是多了几个临时变量。如果整条预测轨迹都用 RK4求解时间会涨 30%50%但换来的是高速工况下横向偏差更小、控制更平滑。低速泊车场景通常不必要前向欧拉配合 0.05 秒采样周期已经够用。5.4 求解时延补偿与约束松弛控制周期里最隐蔽的坑是求解时延。传感器采到状态到求解器返回控制量中间隔了几十毫秒车已经沿着旧控制往前跑了。简单粗暴的做法是把状态外推用上一帧解出的控制量按模型外推到求解完成时刻再作为当前周期的状态初值。更稳的替代方案是用上一帧解出的“预测轨迹”来初始化当前帧的状态变量相当于给求解器一个很好的热启动收敛更快也天然带上了时延信息。约束不可行是第二个常见崩溃点。感知模块给出一个极窄的避障走廊时硬约束会让 NLP 找不到可行解求解器直接报错。业界标准做法是引入松弛变量把关键的不等式约束改写成软约束让求解器在“违反一点点”和“整体不可行”之间选择前者slack opti.variable() opti.subject_to(slack 0) opti.subject_to(distance_to_obstacle d_min - slack) cost 1000 * slack松弛系数给到 1000 左右正常情况下 slack 会保持为 0碰到极端场景时允许短暂侵入安全距离车辆不会因为规划器抖动而突然失控。注意松弛变量只能用于安全边界不能用于执行器限幅这类硬物理约束否则求解器会把转向角超限作为常态。6. 动态避障路径跟随把障碍物写进 MPC 约束6.1 障碍物约束的两种建模方式动态避障是小车路径规划和车辆路径跟随的常用交集。MPC 里最常见的做法是把障碍物离散成若干圆形用圆心距离约束车辆轨迹点与最近障碍物圆心的距离大于安全半径。在 CasADi 里写成sqrt((x - ox)^2 (y - oy)^2) d_min再配一个松弛变量。另一种做法是把避障写进目标函数距离越近代价越大适合障碍物检测噪声大的场景。两种方式不冲突纵向大半径避障用软代价横向紧急避让用硬约束。6.2 规划器与控制器的责任边界给 MPC 加避障约束不意味着规划器就可以偷懒。局部规划器负责找到绕开障碍物的曲线路径MPC 的避障约束只补足“路径和物理边界之间的安全富余”。如果每个周期都让 MPC 自己去绕新检测到的障碍物求解器反复在可行域边缘震荡乘用车方向盘会出现明显的驾驶不自然感。混合 A* 这类考虑运动学的规划算法在泊车路径规划里把责任边界画得很清楚规划器保证几何可行控制器保证动态可行。6.3 实车验证先做这三件事第一用实车录制的参考轨迹和传感器时间戳做离线回放把 MPC 的输出与真实方向盘作对比能快速暴露坐标系和时间同步问题。第二给求解器加最坏情况求解时间统计连续三次超时就走安全停车逻辑不要让调度兜底。第三把参考速度按曲率做一个 v_max_safe sqrt(a_lat_max / |kappa|) 的斜坡限幅再喂给 MPC避免入弯减速太晚导致横向加速度超限。提示MPC 的求解器状态收敛、超时、不可行必须作为诊断消息记录到日志里。只看控制效果调参遇到求解退化时会非常难定位。把最坏情况求解时间打进诊断消息连续三次超时就切换到安全停车逻辑这一条能救回大多数实车联调现场。本文还有配套的精品资源点击获取