基于Lattice规划与MPC协同的自动驾驶动态避障研究

Research on dynamic obstacle avoidance for autonomous driving based on Lattice planning and model predictive control coordination

摘要

在动态环境下， 针对使用集成式模型预测控制（model predictive control， MPC）方案一体化解决避障规划与控制问题时面临的优化问题求解复杂、系统鲁棒性差的问题， 提出了一种Lattice引导轨迹规划与MPC控制器协同的避障控制策略. 该策略采用Lattice规划生成一系列平滑的避障候选轨迹， 优选其一作为集成式MPC避障控制器的参考轨迹， 为避障控制提供前馈引导. 随后， 集成式MPC避障控制器将该参考轨迹作为优化基准进行二次优化求解. MPC避障控制器通过设计预测时域内恒定预测距离的动态采样策略， 以充分利用预测信息并保持优化问题规模的稳定性， 预测步长内根据障碍物的轨迹预测构建可行驶区域作为避障约束， 并进一步显式考虑车辆动力学约束及操纵稳定性约束， 最终得到优化控制指令. 结果表明：相比于传统集成式MPC， Lattice-MPC协同方法求解时间方面计算效率提升约36.5%；在横向控制上， 峰值侧向加速度降低约29.8%， 横摆角速度波动降低约12.2%， 保证了更高的行驶稳定性. 在纵向控制上， 加速度峰值波动下降57%， 具有更好的舒适性.

Abstract

In dynamic environments， when using an integrated model predictive control （MPC） approach to solve obstacle avoidance planning and control problems simultaneously， challenges arise in optimization complexity and system robustness. To address these issues， this paper proposes a coordinated obstacle avoidance control strategy combining Lattice-guided trajectory planning with an MPC controller. This strategy utilizes Lattice planning to generate a series of smooth candidate trajectories for obstacle avoidance and selects the optimal one as a reference trajectory for the integrated MPC obstacle avoidance controller， thereby providing feedforward guidance. Subsequently， the integrated MPC obstacle avoidance controller conducts secondary optimization based on this reference trajectory. The MPC obstacle avoidance controller employs a dynamic sampling strategy with constant prediction distances in the prediction horizon to fully utilize predictive information while maintaining optimization problem stability. Feasible driving regions are constructed as obstacle avoidance constraints within each prediction step based on obstacle trajectory predictions， while explicitly considering vehicle dynamics constraints and handling stability constraints， ultimately generating optimized control commands. Comparative experiments between the proposed coordinated method and a baseline method using only MPC for direct trajectory planning and obstacle avoidance control demonstrate that the collaborative Lattice-MPC method improves computational efficiency by approximately 36.5% in terms of solving time. In lateral control， the peak lateral acceleration is reduced by about 29.8%， and the fluctuation in yaw rate is lowered by 12.2%， ensuring higher driving stability. For longitudinal control， the peak acceleration fluctuation is reduced by 57%， leading to better ride comfort.

Graphical abstract

关键词

自动驾驶 / 避障控制 / 模型预测控制 / Lattice引导轨迹规划 / 代价函数 / 约束条件

Key words

autonomous driving / obstacle avoidance control / model predictive control（MPC） / Lattice-guided trajectory planning / cost function / constraint

传统自动驾驶的技术栈可分为感知定位、预测决策、规划控制三大模块， 其中规划控制模块负责规划出自车未来无碰撞的可行轨迹并操纵车辆进行轨迹跟踪， 对自动驾驶车辆的安全性和舒适性有着决定性的影响.

在规划与控制系统的研究中， 传统方法普遍采用分层架构， 将任务分解为独立的路径/轨迹规划与轨迹跟踪控制. 规划模块生成几何路径或时序轨迹， 控制模块则执行精确跟踪. 此类模块化设计显著简化了问题复杂度， 提高了系统可解释性及实现便捷性. 然而， 其固有缺陷在于层级间的误差累积、系统响应延迟以及规划与控制间的协同不足， 制约了系统的整体性能^［1］. 一个典型挑战是：控制系统通常假设参考轨迹具有动态可行性， 但上层规划器在生成避障轨迹时若未能充分考量车辆的复杂动力学特性与执行器限制， 该假设便难以维持， 可能导致跟踪精度降低甚至引发安全隐患^［2］.

为克服分层架构的局限性， 集成式规划与控制方法得以发展. 该方法将两者视为统一的问题， 同时解决路径规划与跟踪控制任务， 直接根据环境信息（如道路边界和障碍物预测位置）生成最优控制指令. 当前， 实现集成式规划控制主要存在两种技术路径：其一是基于深度学习的端到端方法， 尝试直接从原始传感器数据或特征化环境信息映射到最终的车辆控制指令^［3］. 尽管此类方法在非线性建模方面表现出显著能力， 但在模型的可解释性、泛化能力以及极端情况下的安全性保证方面仍面临挑战^［4］. 其二是基于优化的集成式框架， 将规划与控制问题建模为一个统一的约束优化问题进行求解. 其中， 模型预测控制凭借其显式处理系统模型、预测未来状态，以及灵活处理多重约束（如安全约束、动力学约束、舒适性约束）的能力， 被认为是实现集成规划与控制的一种极具潜力的有效工具^［5］. 近年来， MPC已被广泛应用于自动驾驶车辆的轨迹跟踪、路径规划与生成^［6-7］以及集成化的避障控制^［8］等任务中， 展现出良好的控制效果.

集成式MPC控制器将规划与控制融合为统一优化框架. 相比于传统方法先规划出一条完整的轨迹再让控制器去跟随， 其直接利用一个预测模型， 在每个控制周期内， 根据对未来一段时间环境和车辆状态的预测， 通过一次性优化计算， 直接求解出未来一系列最优的控制指令（如转向角和加速度）. 然而， 当直接应用集成式MPC来一体化解决动态环境下的避障规划与控制问题时， 面临着严峻挑战^［9］. 一方面， 包含复杂车辆动力学和密集动态障碍物约束的优化问题通常是高度非凸性的， 求解计算量巨大， 难以满足自动驾驶的实时性要求^［10］；另一方面， 优化过程对初始解敏感， 容易陷入局部最优^［11］， 导致生成的轨迹质量不高甚至规划失败， 使得系统在复杂多变的动态环境下的鲁棒性和稳定性不足^［12］.

一类研究致力于降低优化问题的计算复杂度. 例如， 通过对车辆模型进行线性化或分段仿射处理， 虽然显著提升了求解速度， 但在高速、大曲率等强非线性工况下， 模型失配问题会降低控制精度和安全性^［13］. 另一类研究则聚焦于提升优化的鲁棒性， 例如通过设计更合理的代价函数或引入软约束， 允许在必要时短暂违反非关键约束， 从而避免因求解无解而导致的规划失败， 但这并未从根本上解决陷入局部最优轨迹的风险^［14］. 对于如何更好地将规划与控制进行协同的研究较少， 针对上述集成式MPC在动态避障场景下优化求解复杂、鲁棒性不足的核心挑战， 本文提出了一种基于Lattice规划与模型预测控制协同的自动驾驶动态避障规划与控制方法. 该方法核心思想是充分利用Lattice规划的结构化采样与高计算效率特性， 为计算密集但精度更高的集成式MPC避障控制器优化问题提供高质量的初始化信息， 从而提升MPC优化问题求解的优化效率与质量， 提升其鲁棒性. 集成式MPC控制器将Lattice生成的最佳候选轨迹作为初始解， 而非简单的跟踪目标. MPC在其预测时域内， 将动态障碍物的最新预测位置、车辆动力学以及各类约束条件统一纳入一个优化问题中， 进行实时的轨迹重规划， 以保障观测与规划的一致性.

与采用Lattice规划器生成轨迹并由独立控制器跟踪的分层方案相比， 本文所提方法避免了简单分层架构的固有缺陷.Lattice规划虽在结构化道路中能够高效生成候选轨迹， 但其通常基于简化的车辆运动学模型， 未充分考虑车辆的非线性动力学特性、执行器限制和轮胎力学. 这种简化导致其生成的最优轨迹在实际高速或大曲率转向（即横向运动）时， 车辆难以精确、平稳地跟踪， 甚至可能在动力学上不可行，影响安全性和舒适性. 最后， 将所提出协同控制方法和集成式MPC避障控制器，与传统分层式方法作对比， 证明所提方法在动态避障场景下的鲁棒性，并通过CarSim & Simulink验证该方法在动态环境下的有效性.

1 研究框架

集成式MPC广泛应用于自动驾驶局部轨迹的重规划与控制. 在动态环境下使用集成式非线性模型预测控制方案解决避障规划控制问题时， 存在优化问题求解复杂、系统鲁棒性差的问题. 为此， 本文提出了一种基于Lattice引导轨迹规划与模型预测控制协同的自动驾驶动态避障控制方法， 技术路线图如图1所示.

首先， Lattice规划器基于道路中心线构建Frenet坐标系， 将障碍物状态映射至S-L图与S-T图， 根据巡航、跟驰、超车三种工况进行末状态采样， 基于五次多项式生成横纵向运动轨迹. 通过考虑安全性、舒适性等横纵向评价指标， 进行量化评估， 选取最优轨迹作为MPC优化问题的引导参考轨迹. Lattice规划已保证了初始轨迹的可行性， 从而为下层MPC提供了一个经过筛选的、高质量的规划起点， 可以降低MPC在进行二次优化时陷入不可行解或局部最优的风险.

其次， MPC控制器以此参考轨迹作为优化问题的初始解. 在其预测时域内， 采用固定预测时域、变步长采样的策略以最大化有效预测距离. 同时， 将动态障碍物的最新预测位置、车辆自身动力学模型以及各类物理约束（如轮胎附着极限、执行器限制等）统一纳入优化问题框架中， 进行实时的轨迹重规划， 最终求解并输出最优的控制量序列.

这种分层协作的方式， 使得整个避障规划控制过程在效率和鲁棒性上都得到了提升. 从二者的相互作用来看：Lattice规划为MPC的优化问题提供了高质量的初始解， 降低了其求解难度和陷入局部最优的风险；反之， MPC的二次优化则在动力学层面进一步平滑了控制输出， 确保了最终执行的轨迹在满足所有安全与物理约束的同时， 具备更高的乘坐舒适性.

2 Lattice引导轨迹规划

传统的模型预测控制方法在求解避障等非线性优化问题时， 其性能高度依赖于初始解的质量. 一个低质量的初始猜测解不仅会使求解器耗费大量迭代步数， 甚至可能导致其陷入局部最优或求解失败.

针对此问题， 本文提出的协同框架引入了Lattice引导轨迹规划模块. 其任务是在复杂的驾驶环境中， 快速生成一组结构化、平滑且初步满足安全约束的候选轨迹， 并从中筛选出最优的一条作为MPC的“高质量初始参考轨迹”. MPC控制器则将此轨迹作为优化问题的初始解， 在其预测时域内， 融合动态障碍物的预测位置作为避障约束、车辆动力学约束， 进行实时的轨迹重规划并直接输出控制指令. 相较于基于随机采样的RRT或A*等图搜索算法， Lattice方法天然的结构化特性及对车辆运动学的内蕴考虑， 使其成为引导MPC优化求解的理想选择.

首先， Lattice规划提供的轨迹是一条经过筛选的、接近最优的无碰撞时空轨迹， 这能够将MPC的优化起点直接置于可行域或其紧邻区域内. 其可以缩小MPC的数值求解空间， 从而提升计算效率， 并降低陷入局部最优解的风险.

其次， 由于Lattice生成的轨迹已初步避开障碍物， 这意味着在MPC的预测时域内， 车辆的初始状态远离了障碍物所构成的“不可行区域”. 这为MPC在处理动态避障约束时提供了更大的调整裕度.

Lattice引导轨迹规划以障碍物的S-T图、S-L图以及车辆初始状态信息为输入， 通过主车初始与终端状态采样、轨迹构建、轨迹评估及碰撞检测分析等， 输出满足运动学约束的避障参考轨迹. 随后， MPC以此轨迹为参考轨迹， 利用动态避障可行区域和车辆动力学约束对参考轨迹进行二次优化.

2.1 自车末状态采样

自车末状态采样涵盖纵向与横向两个维度空间. 针对纵向维度的采样策略， 首先确定车辆在特定时间区间内可达到的速度范围上下界， 以及纵向位置的最大值与最小值， 从而确定纵向采样的约束边界.

2.1.1 速度上限与下限

定速巡航的速度上下限为：

v_{\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}}(t)=\left\{\begin{array}{l}v(t_{\mathrm{0}})+a_{\mathrm{m}\mathrm{a}\mathrm{x}}\times (t-t_{\mathrm{0}})\\ v_{\mathrm{l}\mathrm{i}\mathrm{m}}\end{array}\right.（1）

v_{\mathrm{l}\mathrm{o}\mathrm{w}\mathrm{e}\mathrm{r}}(t)=\left\{\begin{array}{l}v(t_{\mathrm{0}})+a_{\mathrm{m}\mathrm{i}\mathrm{n}}\times (t-t_{\mathrm{0}}),t-t_{\mathrm{0}}\le \mathrm{\Delta }t\\ \mathrm{0},t-t_{\mathrm{0}}>\mathrm{\Delta }t\end{array}\right.（2）

式中：v_{\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}}(t)、 v_{\mathrm{l}\mathrm{o}\mathrm{w}\mathrm{e}\mathrm{r}}(t)分别表示在t时刻， 自车所能达到的速度上限与速度下限；v(t_{\mathrm{0}})表示自车的初始速度；a_{\mathrm{m}\mathrm{a}\mathrm{x}}表示自车的最大纵向加速度； a_{\mathrm{m}\mathrm{i}\mathrm{n}}表示自车的最大纵向减速度；设v_{\mathrm{l}\mathrm{i}\mathrm{m}}为所设置的最大巡航车速， 当v_{\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}}(t)>v_{\mathrm{l}\mathrm{i}\mathrm{m}}时， 将自车最大纵向速度限制为巡航车速. 自车从初始车速v(t_{\mathrm{0}})以最大纵向减速度a_{\mathrm{m}\mathrm{i}\mathrm{n}}进行紧急刹车到速度为0 m/s所需要的最小时间\mathrm{\Delta }t为：

\mathrm{\Delta }t=\frac{v(t_{\mathrm{0}})}{-a_{\mathrm{m}\mathrm{i}\mathrm{n}}}（3）

2.1.2 纵向位置极值

\mathrm{\Delta }t时间内， 自车所需的刹车距离s(t_{\mathrm{e}\mathrm{n}\mathrm{d}})为：

s(t_{\mathrm{e}\mathrm{n}\mathrm{d}})=s(t_{\mathrm{0}})+\frac{v^{\mathrm{2}}(t_{\mathrm{0}})}{-\mathrm{2}{a}_{\mathrm{m}\mathrm{i}\mathrm{n}}}（4）

式中：s(t_{\mathrm{0}})表示在初始时刻t_{\mathrm{0}}，自车的纵向位置；s(t_{\mathrm{e}\mathrm{n}\mathrm{d}})表示自车刹车到速度为0 m/s时刻t_{\mathrm{e}\mathrm{n}\mathrm{d}}，自车的纵向位置.

给定时间t，自车可以行驶的纵向距离s下限为：

s_{\mathrm{l}\mathrm{o}\mathrm{w}\mathrm{e}\mathrm{r}}(t)=

（5）

自车可以行驶的纵向距离s上限为：

s_{\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}}(t)=s\left(t_{\mathrm{0}}\right)+\frac{\mathrm{1}}{\mathrm{2}}\times a_{\mathrm{m}\mathrm{a}\mathrm{x}}\times {\left(t-t_{\mathrm{0}}\right)}^{\mathrm{2}}（6）

利用式（1）、式（2）得到自车末状态速度采样如图2所示， 末状态速度采样范围受到初速度的影响.

跟驰与超车条件下的纵向速度规划主要基于障碍物的S-T图进行采样， 如图3所示， 蓝色四边形为障碍物的S-T图， 表示随时间t的变化，障碍物在纵向s方向上的占用情况；红色点是不同时刻自车的纵向位置采样点. 采样点的相对位置反映了不同的规划意图：若自车的纵向位置在障碍物占用区域之上则对应超车， 如图3（a）所示；若自车位置在障碍物占用区域之下则对应跟驰， 如图3（b）所示.

自车末状态采样后，需对采样点进行有效性判断，舍弃纵向位置s异常值点，即对于某个时刻s(t_i)，若s(t_i)>s_{\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}}(t_i)或者，则此时刻的s(t_i)被舍弃.其中，s_{\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}}(t_i)与s_{\mathrm{l}\mathrm{o}\mathrm{w}\mathrm{e}\mathrm{r}}(t_i)表示自车在t_i时刻在Frenet坐标系下s方向可行驶区域的最大值与最小值.

2.2 横纵向轨迹生成

为保证车辆的平稳行驶， 要求规划出的轨迹足够平滑. 加速度的变化率jerk是评价轨迹平滑性的重要指标之一.故问题转换为在生成轨迹时要求一段时间的轨迹jerk最小，且期望在某段时间内满足给定的边界约束（例如起点、终点的位置、速度、加速度）.该问题为经典的泛函最优问题，其解是一条五次多项式曲线.利用五次多项式连接自车的初状态与末状态，分别生成车辆在纵向和侧向的轨迹. 对于纵向速度曲线的生成，根据其初状态\left(s(t_{\mathrm{0}}),\frac{\partial s}{\partial t}\left|{}_{t=t_{\mathrm{0}}}\right.,\frac{{\partial }^{\mathrm{2}}s}{\partial t^{\mathrm{2}}}\left|{}_{t=t_{\mathrm{0}}}\right.\right)与末状态\left(s(t_{\mathrm{1}}),\frac{\partial s}{\partial t}\left|{}_{t=t_{\mathrm{1}}}\right.,\frac{{\partial }^{\mathrm{2}}s}{\partial t^{\mathrm{2}}}\left|{}_{t=t_{\mathrm{1}}}\right.\right)可列方程如下：

\left\{\begin{array}{l}s(t_{\mathrm{0}})=a_{\mathrm{0}}+a_{\mathrm{1}}{t}_{\mathrm{0}}+a_{\mathrm{2}}{t}_{\mathrm{0}}^{\mathrm{2}}+a_{\mathrm{3}}{t}_{\mathrm{0}}^{\mathrm{3}}+a_{\mathrm{4}}{t}_{\mathrm{0}}^{\mathrm{4}}+a_{\mathrm{5}}{t}_{\mathrm{0}}^{\mathrm{5}}\\ \frac{\partial s}{\partial t}\left|{}_{t=t_{\mathrm{0}}}\right.=a_{\mathrm{1}}+\mathrm{2}{a}_{\mathrm{2}}{t}_{\mathrm{0}}+\mathrm{3}{a}_{\mathrm{3}}{t}_{\mathrm{0}}^{\mathrm{2}}+\mathrm{4}{a}_{\mathrm{4}}{t}_{\mathrm{0}}^{\mathrm{3}}+\mathrm{5}{a}_{\mathrm{5}}{t}_{\mathrm{0}}^{\mathrm{4}}\\ \frac{{\partial }^{\mathrm{2}}s}{\partial t^{\mathrm{2}}}\left|{}_{t=t_{\mathrm{0}}}\right.=\mathrm{2}{a}_{\mathrm{2}}+\mathrm{6}{a}_{\mathrm{3}}{t}_{\mathrm{0}}+\mathrm{12}{a}_{\mathrm{4}}{t}_{\mathrm{0}}^{\mathrm{2}}+\mathrm{20}{t}_{\mathrm{0}}^{\mathrm{3}}\\ s(t_i)=a_{\mathrm{0}}+a_{\mathrm{1}}{t}_i+a_{\mathrm{2}}{t}_i^{\mathrm{2}}+a_{\mathrm{3}}{t}_i^{\mathrm{3}}+a_{\mathrm{4}}{t}_i^{\mathrm{4}}+a_{\mathrm{5}}{t}_i\\ \frac{\partial s}{\partial t}\left|{}_{t=t_i}\right.=a_{\mathrm{1}}+\mathrm{2}{a}_{\mathrm{2}}{t}_i+\mathrm{3}{a}_{\mathrm{3}}{t}_i^{\mathrm{2}}+\mathrm{4}{a}_{\mathrm{4}}{t}_i^{\mathrm{3}}+\mathrm{5}{a}_{\mathrm{5}}{t}_i^{\mathrm{4}}\\ \frac{{\partial }^{\mathrm{2}}s}{\partial t^{\mathrm{2}}}\left|{}_{t=t_i}\right.=\mathrm{2}{a}_{\mathrm{2}}+\mathrm{6}{a}_{\mathrm{3}}{t}_i+\mathrm{12}{a}_{\mathrm{4}}{t}_i^{\mathrm{2}}+\mathrm{20}{a}_{\mathrm{5}}{t}_i^{\mathrm{3}}\end{array}\right.（7）

通过求解式（7）， 可以确定一条从初状态到末状态的五次多项式曲线. 特别地， 对于巡航速度曲线， 因自车巡航状态不需要指明末状态的s值， 故初状态和末状态共有五个约束条件， 可约束得到一条四次多项式曲线， 如图4（a）所示为巡航状态的纵向距离-时间多项式曲线， 图4（b）为纵向速度-时间多项式曲线.

基于采样的横向路径规划关注自车初状态与末状态横向偏移距离l以及\frac{\partial l}{\partial s},\frac{{\partial }^{\mathrm{2}}l}{{\partial }^{\mathrm{2}}s}三个状态量. 首先基于Frenet坐标系进行纵向s上的采样，其次为自车在每个纵向s采样点， 对横向偏移距离l进行采样， 采样过程中， 终端状态的\frac{\partial l}{\partial s},\frac{{\partial }^{\mathrm{2}}l}{{\partial }^{\mathrm{2}}s}均设为0， 以保证终端状态无侧向速度与侧向加速度， 横向路径采样结果及五次多项式路径生成如图5所示.

2.3 横纵向轨迹评估与碰撞检测

通过评价函数从插值得到的横纵向轨迹簇中分别优选出一条代价最低且安全的横纵向轨迹， 并将二者合成最终的引导轨迹.

2.3.1 纵向轨迹评价函数

纵向轨迹评价函数的设计旨在平衡安全性、舒适性和行驶效率这三个关键维度， 通过量化各方面的代价， 并赋予不同权重求和， 得到每条轨迹的综合代价， 最终选取综合代价最小的速度曲线.

1）纵向运动碰撞风险代价.纵向运动碰撞风险评估的核心逻辑为：只在纵向s方向一个维度进行考虑， 若在某一时刻， 自车与障碍物车辆在纵向上有空间上的重叠或趋于重叠， 则此状态的碰撞风险较高. 纵向运动碰撞风险代价的计算公式为：

\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}={\displaystyle \sum _{i=\mathrm{0}}^N}{\mathrm{e}}^{-\frac{(s(t_i)-s_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}(t_i{))}^{\mathrm{2}}}{\mathrm{2}{c}^{\mathrm{2}}}}（8）

式中：N为每条轨迹的采样点个数；s(t_i)、 s_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}(t_i)分别表示t_i时刻，自车和障碍物车辆在Frenet坐标系中纵向上的位置；c为权重系数.

2）纵向舒适性代价.对于自车速度曲线的舒适性代价，常用加速度的变化率jerk进行评估， jerk越大，舒适性应该越差， 则纵向运动的舒适性代价为：

\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{t}}={\displaystyle \sum _{i=\mathrm{0}}^N}\frac{\dot{a}(t_i)}{{\dot{a}}_{\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}\_\mathrm{b}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}}}（9）

式中：\dot{a}(t_i)表示t_i时刻的jerk；{\dot{a}}_{\mathrm{u}\mathrm{p}\mathrm{p}\mathrm{e}\mathrm{r}\_\mathrm{b}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}}表示jerk的最大值， 用以单位化价项， 消除量纲对代价项大小的影响.

3）纵向目标速度代价.对于纵向速度曲线的代价函数设计， 首先在理想状态下（前方无障碍物）， 一般希望自车以设置的巡航速度V_{x-\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{n}\mathrm{g}}进行行驶， 故以V_{\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{n}\mathrm{g}}为标准， 对每条速度曲线在离散时刻t_i进行速度差值计算并累加求和， 可得速度代价为：

\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{s}\mathrm{p}\mathrm{e}\mathrm{e}\mathrm{d}}=\frac{{\displaystyle \sum _{i=\mathrm{0}}^{N-\mathrm{1}}}{t}_i^{\mathrm{2}}\cdot (V_{\mathrm{x}}(t_i)-V_{x-\mathrm{c}\mathrm{u}\mathrm{r}\mathrm{i}\mathrm{s}\mathrm{i}\mathrm{n}\mathrm{g}}{)}^{\mathrm{2}}}{{\displaystyle \sum _{i=\mathrm{0}}^{N-\mathrm{1}}}{t}_i^{\mathrm{2}}+\epsilon }（10）

式中：V_x （t_i ）是t_i 时刻的车辆纵向速度；\epsilon是一个极小的非零值，避免分母为零.

4）纵向轨迹综合代价计算.将上述各代价进行加权求和， 得到纵向轨迹综合代价：

J_{\mathrm{l}\mathrm{o}\mathrm{n}}(j)=w_{\mathrm{o}\mathrm{b}\mathrm{s}}\cdot \mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}(j)+

w_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\_\mathrm{l}\mathrm{o}\mathrm{n}}\cdot \mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{t}}(j)+w_{\mathrm{s}\mathrm{p}\mathrm{e}\mathrm{e}\mathrm{d}}\cdot \mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{s}\mathrm{p}\mathrm{e}\mathrm{e}\mathrm{d}}(j)（11）

式中：w_{\mathrm{o}\mathrm{b}\mathrm{s}}、w_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\_\mathrm{l}\mathrm{o}\mathrm{n}}、w_{\mathrm{s}\mathrm{p}\mathrm{e}\mathrm{e}\mathrm{d}}分别是碰撞风险、纵向舒适性和速度代价的权重系数；\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{o}\mathrm{b}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{c}\mathrm{l}\mathrm{e}}(j)、\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{t}}(j)、\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{s}\mathrm{p}\mathrm{e}\mathrm{e}\mathrm{d}}(j)分别是第j条轨迹的纵向运动碰撞风险代价、纵向舒适性代价、纵向速度与距离代价.

2.3.2 横向轨迹评价函数设计

对于横向路径规划出的路径进行评价， 主要有横向偏移以及舒适性两个方面.

1）横向偏移代价.横向偏移代价用于量化规划轨迹与参考线之间的偏差， 其设计目标是使重规划轨迹尽可能贴合参考线：

\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{l}\mathrm{a}\mathrm{n}\_\mathrm{o}\mathrm{f}\mathrm{f}\mathrm{s}\mathrm{e}\mathrm{t}}={\displaystyle \sum _{i=\mathrm{0}}^{N-\mathrm{1}}}\frac{(l(s(i))-l_{\mathrm{r}\mathrm{e}\mathrm{f}\mathrm{e}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{c}\mathrm{e}}{)}^{\mathrm{2}}}{l_{\mathrm{b}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}\mathrm{a}\mathrm{r}\mathrm{y}}}（12）

式中：l(s(i))表示第 i个点处规划轨迹的实际横向偏移量；l_{\mathrm{r}\mathrm{e}\mathrm{f}\mathrm{e}\mathrm{r}\mathrm{e}\mathrm{n}\mathrm{c}\mathrm{e}}表示参考线的期望横向位置；l_{\mathrm{b}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}\mathrm{a}\mathrm{r}\mathrm{y}}表示横向边界限制， 代表车道宽度的一半.

2）舒适性代价.各离散时刻舒适度的评价指标\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{l}\mathrm{a}\mathrm{t}\_\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{t}}(t_i)设计， 在纵向速度很大的时候，侧向速度的剧烈变化不满足安全性与舒适性要求， 另外， 考虑到轮胎的附着椭圆， 在纵向加速的过程中， 应该尽量避免同时进行横向的转向操作， 故横向舒适性代价与纵向速度相关， 设计为：

\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{l}\mathrm{a}\mathrm{t}\_\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{t}}(t_i)=l^{\text{'}}(\mathrm{\Delta }s{(i))}^{\mathrm{2}}v(t_i{)}^{\mathrm{2}}+l^{″}(\mathrm{\Delta }s(i))a(t_i)（13）

式中：l^{\text{'}}与l^{″}分别表示基于Frenet坐标系， 横向偏移距离l对纵向距离s的一阶导数和二阶导数，在几何上， l^{\mathrm{\text{'}}}反映车辆相对于参考线的偏航角， l^{″}反映轨迹相对于参考线的曲率；v(t_i)和a(t_i)分别表示车辆在t_i时刻的纵向速度和纵向加速度；\mathrm{\Delta }s(i)表示从第i-1个点到第i个点，车辆沿着参考线往前走过的这一小段距离（步长）.

3）横向轨迹综合代价计算：将横向偏移代价和横向舒适性代价进行加权求和，得到横向轨迹的总代价：

J_{\mathrm{l}\mathrm{a}\mathrm{t}}(k)=w_{\mathrm{o}\mathrm{f}\mathrm{f}\mathrm{s}\mathrm{e}\mathrm{t}}\cdot \mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{l}\mathrm{a}\mathrm{t}\_\mathrm{o}\mathrm{f}\mathrm{f}\mathrm{s}\mathrm{e}\mathrm{t}}(k)+

w_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\_\mathrm{l}\mathrm{a}\mathrm{t}}\cdot \sum _i\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{l}\mathrm{a}\mathrm{t}\_\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{t}}(t_i,k)（14）

式中：w_{\mathrm{o}\mathrm{f}\mathrm{f}\mathrm{s}\mathrm{e}\mathrm{t}}和w_{\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\_\mathrm{l}\mathrm{a}\mathrm{t}}分别是横向偏移和横向舒适性的权重系数；\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{l}\mathrm{a}\mathrm{t}\_\mathrm{o}\mathrm{f}\mathrm{f}\mathrm{s}\mathrm{e}\mathrm{t}}(k)、\mathrm{c}\mathrm{o}\mathrm{s}{\mathrm{t}}_{\mathrm{l}\mathrm{a}\mathrm{t}\_\mathrm{c}\mathrm{o}\mathrm{m}\mathrm{f}\mathrm{o}\mathrm{r}\mathrm{t}}(t_i,k)分别是第k条轨迹的横向偏移代价、舒适性代价.

得到纵向和横向运动轨迹之后，对两个维度上的轨迹进行耦合.首先，从多个候选轨迹对中选择代价最小的横纵向运动组合.其次，基于选定的纵向运动多项式s\left(t\right)，在离散时刻t_i采样得到纵向状态\left(s_i,{\dot{s}}_i,{\ddot{s}}_i\right).再次，以纵向位移s_i为依据，从对应的横向五次多项式l\left(s\right)中提取横向状态\left(l_i,{\dot{l}}_i,{\ddot{l}}_i\right).由此获得各时刻t_i的Frenet状态.最后，将Frenet坐标点\left(s_i,{\dot{s}}_i,{\ddot{s}}_i,l_i,{\dot{l}}_i,\ddot{l}\right)序列转换为笛卡儿坐标表示，生成最终轨迹. 轨迹耦合流程如图6所示.

车辆在有障碍物的环境下行驶， 因此需要对合成的轨迹进行碰撞检测.根据分离轴定理， 判断两个凸多边形是否有碰撞时只需检查是否存在一条分离轴， 使得两者在该轴上的投影不重叠. 对于二维凸多边形， 其所有边的法线方向构成了充分的候选分离轴集合. 特别地， 当自车和障碍物均为矩形时， 仅需测试平行于两个矩形各自边方向的轴（共四个方向）即可完成碰撞检测， 如图7所示.

若两个投影区间的中心距离a_{\mathrm{p}}小于等于它们各自的“半径”之和b_{\mathrm{p}}+c_{\mathrm{p}}， 则可断定两区间存在重叠或临界接触， 表明在该投影轴上两障碍物无法完全分离， 判定条件为：

a_{\mathrm{p}}\le b_{\mathrm{p}}+c_{\mathrm{p}}（15）

式中：a_{\mathrm{p}}为两个障碍物的中心点间距离；b_{\mathrm{p}}、 c_{\mathrm{p}}分别为各自半宽度.

3 预测模型构建

MPC通过求解优化问题实现控制， 其优化问题的目标函数和约束条件均依赖于预测模型对系统未来行为的状态预测， 本节构建车辆动力学模型作为预测模型.

3.1 整车动力学模型

车辆动力学模型所用的变量定义如表1所示.

三自由度车身模型纵向、侧向、横摆运动方程可写为：

\left\{\begin{array}{l}m{\dot{V}}_x=m\dot{y}\dot{\phi }+\mathrm{2}{F}_{x\mathrm{f}}+\mathrm{2}{F}_{x\mathrm{r}}-F_{\mathrm{d}\mathrm{r}\mathrm{a}\mathrm{g}}\\ m{\dot{V}}_y=-m\dot{x}\dot{\phi }+\mathrm{2}{F}_{y\mathrm{f}}+\mathrm{2}{F}_{y\mathrm{r}}\\ I_z\ddot{\phi }=\mathrm{2}a{F}_{y\mathrm{f}}-\mathrm{2}b{F}_{y\mathrm{r}}\end{array}\right.（16）

式中：F_{\mathrm{d}\mathrm{r}\mathrm{a}\mathrm{g}}为空气阻力.

车轮的旋转运动进行扭矩平衡分析可得：

\left\{\begin{array}{l}{\dot{\omega }}_{\mathrm{f}}=\left(T_{\mathrm{f}}-R_{\mathrm{e}}{F}_{\mathrm{l}\mathrm{f}}\right)/J_{\mathrm{f}}\\ {\dot{\omega }}_{\mathrm{r}}=\left(T_{\mathrm{r}}-R_{\mathrm{e}}{F}_{\mathrm{l}\mathrm{r}}\right)/J_{\mathrm{r}}\end{array}\right.（17）

式中：R_{\mathrm{e}}为轮胎滚动半径.

车辆在大地坐标系XOY中的运动表示为：

\begin{array}{l}\dot{X}=V_x\mathrm{c}\mathrm{o}\mathrm{s}\phi -V_y\mathrm{s}\mathrm{i}\mathrm{n}\phi \\ \dot{Y}=V_x\mathrm{s}\mathrm{i}\mathrm{n}\phi +V_y\mathrm{c}\mathrm{o}\mathrm{s}\phi \end{array}（18）

3.2 轮胎模型

车辆的轮胎力可用魔术轮胎公式计算：

\begin{array}{c}F(X)=D_{}\mathrm{s}\mathrm{i}\mathrm{n}\left\{C\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left[BX-E\left(BX-\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}BX\right)\right]\right\}\end{array}（19）

式中：B、C、D、E为魔术轮胎公式模型的拟合参数；X为轮胎侧偏角.

将上式转换为状态空间方程：

\left\{\begin{array}{l}\dot{\mathit{x}}(t)=f(x(t),u(t)),t\ge \mathrm{0}\\ \mathit{y}(t)=g(x(t),u(t))\end{array}\right.（20）

根据状态空间方程相关理论， 定义系统的状态量为\mathit{x}=[V_x,V_y,\phi ,\omega ,X,Y,{\omega }_{\mathrm{f}},{\omega }_{\mathrm{r}}{]}^{\mathrm{T}}， 系统的状态输出量选取车辆的纵向速度、车辆横摆角以及侧向位移， 即\overline{y}=[V_x{,\phi ,Y]}^{\mathrm{T}}， 系统的输入控制量为\overline{u}=[T_{x\mathrm{f}},T_{x\mathrm{r}},{\delta }_{\mathrm{f}}{]}^{\mathrm{T}}.

4 模型预测控制器设计

基于MPC的避障约束， 可以将避障规划控制合并为一个问题， 但是求解复杂. Lattice规划器为MPC控制器提供初始避障参考轨迹， MPC控制器在此基础上考虑避障约束， 显式地考虑车辆的非线性动力学特性、执行器限制以及操纵稳定性等多重约束， 进行二次优化， 通过优化问题求解， 输出最终控制量.

4.1 预测模型

MPC的预测能力依赖于系统的动态模型. 预测模型利用前向欧拉法对如式（20）连续模型进行离散化， 所得结果为：

\begin{array}{l}\mathit{x}(k+\mathrm{1})=F(\mathit{x}(k),\mathit{u}(k))\\ \mathit{y}(k)=G(\mathit{x}(k),\mathit{u}(k))\end{array}（21）

式中：\mathit{x}\left(k+\mathrm{1}\right)\approx \mathit{x}\left(k\right)+T\times f\left(\mathit{x}\left(k\right),\mathit{u}\left(k\right)\right)， k代表离散时间步（k = 0， 1， 2，…）；T表示单个步长； u （k）表示在时间区间［kT， （k+1）T］内施加的控制输入向量； y （k）表示在时刻kT的输出向量. 控制量为\mathit{u}=[T_{x\mathrm{f}},T_{x\mathrm{r}},{\delta }_{\mathrm{f}}{]}^{\mathrm{T}}.

4.2 代价函数设计

代价函数的设计是指导控制器行为的关键， 该代价函数围绕Lattice提供参考轨迹进行构建， 旨在引导MPC实现精确、平滑的控制. 代价函数被设计为：

J={\displaystyle \sum _{i=\mathrm{1}}^{N_{\mathrm{p}}}}{‖{\overline{\mathit{y}}}_{\mathrm{c}}(k+i)-\mathit{r}(k+i)‖}_{\mathit{Q}}^{\mathrm{2}}+{\displaystyle \sum _{i=\mathrm{0}}^{N_{\mathrm{c}}-\mathrm{1}}}{‖\overline{\mathit{u}}(k+i)‖}_{\mathit{R}}^{\mathrm{2}}+

{\displaystyle \sum _{i=\mathrm{0}}^{N_c-\mathrm{1}}}{‖\mathrm{\Delta }\overline{\mathit{u}}(k+i)‖}_{\mathit{S}}^{\mathrm{2}}（22）

式中：N_p是预测时域；N_c是控制时域；式中第一项为轨迹跟踪引导项， {\overline{\mathit{y}}}_{\mathrm{c}}(k+i)表示未来k+i时刻车辆的预测状态， \mathit{r}(k+i)表示未来k+i时刻的参考状态. 在本文协同策略中， \mathit{r}(k+i)是由上游Lattice规划器生成的. 相比于集成式MPC从零开始规划或跟踪一个简单的几何路径， 采用此参考轨迹作为优化基准， 降低了集成式MPC优化问题的难度， 提高了优化效率和找到高质量解的可能性， 增强了对动态环境扰动的鲁棒性. 第二项为控制量约束项， \overline{\mathit{u}}(k+i)表示未来k+i时刻车辆的控制量， 目的是惩罚控制输入\mathit{u}的幅值.第三项为控制量变化率约束项， 目的为惩罚控制输入增量\mathrm{\Delta }\mathit{u}的大小， 保证控制平顺性. \mathit{Q}，\mathit{R}，\mathit{S}为各自权重矩阵.

4.3 约束条件设计

4.3.1 执行机构约束

为避免执行器饱和， 应对控制输入及其变化率进行约束， 具体表达为：

\begin{array}{l}\left\{\begin{array}{l}{\delta }_{\mathrm{f}\mathrm{m}\mathrm{i}\mathrm{n}}\le {\delta }_{\mathrm{f}}(k+i|k)\le {\delta }_{\mathrm{f}\mathrm{m}\mathrm{a}\mathrm{x}}\\ \mathrm{\Delta }{\delta }_{\mathrm{f}\mathrm{m}\mathrm{i}\mathrm{n}}\le \mathrm{\Delta }{\delta }_{\mathrm{f}}(k+i|k)\le \mathrm{\Delta }{\delta }_{\mathrm{f}\mathrm{m}\mathrm{a}\mathrm{x}}\\ T_{x,j\mathrm{m}\mathrm{i}\mathrm{n}}\le T_{x,j}(k+i|k)\le T_{x,j\mathrm{m}\mathrm{a}\mathrm{x}}\\ \mathrm{\Delta }{T}_{x,j\mathrm{m}\mathrm{i}\mathrm{n}}\le \mathrm{\Delta }{T}_{x,j}(k+i|k)\le \mathrm{\Delta }{T}_{x,j\mathrm{m}\mathrm{a}\mathrm{x}}\end{array}\right.\\ \forall i\in [\mathrm{0},N_{\mathrm{c}}-\mathrm{1}]\end{array}（23）

式中：{\delta }_{\mathrm{f}}(k+i|k)与\mathrm{\Delta }{\delta }_{\mathrm{f}}(k+i|k)分别表示在当前时刻k预测的未来第i步（k+i）的前轮转角及其增量.T_{x,j}(k+i|k)， \mathrm{\Delta }{T}_{x,j}(k+i|k)分别表示在当前时刻k预测的未来第i步施加于第j个执行器上的纵向驱动/制动力或力矩及其增量. 下标j代表前轴合力矩（j=\mathrm{f}）、后轴合力矩 （j=\mathrm{r}）.

4.3.2 避障可行驶区域约束

本文Lattice规划方法生成初步考虑了避障的参考轨迹， 但仅仅满足基本运动学约束， 而MPC则能基于避障约束与车辆动力学约束实现进一步精细的避障优化， 弥补了Lattice规划主要基于运动学、未能充分考虑车辆动力学能力的不足.

避障约束要求在预测时域内的所有时刻k， 车辆的每个角点都必须位于由道路边界和障碍物定义的可行驶区域内. 即对所有预测步i\in \{\mathrm{1,2},\dots ,N_{\mathrm{p}}\}， 车辆的预测几何包络［通常以其边界框\mathrm{B}\mathrm{o}\mathrm{x}(k+i|k)表示］须完全包含于预测的、时变的可行驶区域\mathrm{𝒟}(k+i)内. 该可行驶区域\mathrm{𝒟}(k+i)由道路边界及对环境中其他交通参与者预测位置所确定的安全空间共同界定.

1）避障边界生成.使用AABB包围盒表示障碍物. Lattice规划器确定轨迹绕行障碍物的方向， 从而将道路空间划分为障碍物占据区（含膨胀边界）和车辆可行驶区. 该可行驶区域随后被转化为MPC优化问题中的避障约束. MPC结合这些避障约束与车辆动力学约束， 对Lattice生成的初始轨迹进行精细化调整. 如图8所示， 蓝色与红色虚线分别代表先前与当前时刻的安全区域边界. 通过整合多个时间点（或预测步）及其对应的安全区域约束， 形成了适用于动态环境的时变避障约束集.

2）自车OBB包围盒模型构建.如图9所示， 设车辆的四个角点的在笛卡儿坐标系下的坐标分别为(X_{\mathrm{f}\mathrm{l}},Y_{\mathrm{f}\mathrm{l}})， (X_{\mathrm{f}\mathrm{r}},Y_{\mathrm{f}\mathrm{r}})， (X_{\mathrm{r}\mathrm{l}},Y_{\mathrm{r}\mathrm{l}})， (X_{\mathrm{r}\mathrm{r}},Y_{\mathrm{r}\mathrm{r}})，车辆的质心坐标为(X,Y)；a， b分别为质心到前轴的距离以及质心到后轴的距离；T_{\mathrm{r}}为汽车的宽度； \phi为横摆角.

故车辆的四个角点坐标可表示为：

\mathbf{B}\mathbf{o}\mathbf{x}=\mathbf{C}\mathbf{o}\mathbf{r}\mathbf{n}\mathbf{e}\mathbf{r}{\mathbf{s}}_{\mathrm{l}\mathrm{o}\mathrm{c}\mathrm{a}\mathrm{l}}\cdot {\mathit{R}}^{\mathrm{T}}+\mathrm{1}\cdot {\mathit{P}}_{\mathrm{c}\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{e}\mathrm{r}}（24）

对于MPC的预测时域内的每一个时间步k（k=1，2，…，N）， 四个角点的全局坐标矩阵\mathbf{B}\mathbf{o}{\mathbf{x}}_k（每行代表一个角点的［X， Y］坐标）计算如下：

\mathbf{B}\mathbf{o}{\mathbf{x}}_k=\mathbf{C}\mathbf{o}\mathbf{r}\mathbf{n}\mathbf{e}\mathbf{r}{\mathbf{s}}_{\mathrm{l}\mathrm{o}\mathrm{c}\mathrm{a}\mathrm{l}}\cdot {\mathit{R}}_k^{\mathrm{T}}+\mathrm{1}\cdot {\mathit{P}}_{\mathrm{c}\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{e}\mathrm{r},k}（25）

式中：\mathbf{B}\mathbf{o}\mathbf{x}={\left[\begin{array}{cccc}{X}_{\mathrm{f}\mathrm{l}}& X_{\mathrm{f}\mathrm{r}}& X_{\mathrm{r}\mathrm{l}}& X_{\mathrm{r}\mathrm{r}}\\ Y_{\mathrm{f}\mathrm{l}}& Y_{\mathrm{f}\mathrm{r}}& Y_{\mathrm{r}\mathrm{l}}& Y_{\mathrm{r}\mathrm{r}}\end{array}\right]}^{\mathrm{T}}表示自车的四个角点坐标；旋转矩阵\mathit{R}=\left[\begin{array}{cc}\mathrm{c}\mathrm{o}\mathrm{s}\phi & -\mathrm{s}\mathrm{i}\mathrm{n}\phi \\ \mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{c}\mathrm{o}\mathrm{s}\phi \end{array}\right]； \mathbf{C}\mathbf{o}\mathbf{r}\mathbf{n}\mathbf{e}\mathbf{r}{\mathbf{s}}_{\mathrm{l}\mathrm{o}\mathrm{c}\mathrm{a}\mathrm{l}}^{}={\left[\begin{array}{cccc}a& a& -b& -b\\ T_{\mathrm{r}}/\mathrm{2}& -T_{\mathrm{r}}/\mathrm{2}& T_{\mathrm{r}}/\mathrm{2}& -T_{\mathrm{r}}/\mathrm{2}\end{array}\right]}^{\mathrm{T}}为四个角点的坐标矩阵；自车质心的位置{\mathit{P}}_{\mathrm{c}\mathrm{e}\mathrm{n}\mathrm{t}\mathrm{e}\mathrm{r}}=(X,Y).

3）时变避障约束.为便于优化求解， 此几何约束通常转化为对车辆边界框顶点（角点）的约束. 假定存在一坐标系， 使得在预测时刻k+i， 可行驶区域\mathrm{𝒟}(k+i)可由横坐标X的下、上边界函数Y_{\mathrm{L}}(X,k+i)和Y_{\mathrm{U}}(X,k+i)界定. 据此， 对于边界框\mathbf{B}\mathbf{o}\mathbf{x}(k+i|k) 的任意顶点j\in \{\mathrm{f}\mathrm{l},\mathrm{f}\mathrm{r},\mathrm{r}\mathrm{l},\mathrm{r}\mathrm{r}\}（分别对应前左、前右、后左、后右）， 其预测坐标(X_j(k+i|k),Y_j(k+i|k))必须满足条件：

\begin{array}{l}{Y}_{\mathrm{L}}(X_j(k+i|k),k+i)\le Y_j(k+i|k)\le \\ Y_{\mathrm{U}}(X_j(k+i|k),k+i)\end{array}（26）

对所有四个角点的上述不等式写成向量形式，该约束可以紧凑地表示为：

\begin{array}{l}{\mathit{Y}}_{\mathrm{L}}({\mathit{X}}_{\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{n}\mathrm{e}\mathrm{r}\mathrm{s}}(k+i|k),k+i|k)\le \\ {\mathit{Y}}_{\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{n}\mathrm{e}\mathrm{r}\mathrm{s}}(k+i|k)\le \\ {\mathit{Y}}_{\mathrm{U}}({\mathit{X}}_{\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{n}\mathrm{e}\mathrm{r}\mathrm{s}}(k+i|k),k+i|k)\end{array}（27）

该约束对所有预测步i\in \{\mathrm{1,2},\dots ,N_{\mathrm{p}}\}均须满足. 式中， {\mathit{X}}_{\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{n}\mathrm{e}\mathrm{r}\mathrm{s}}(k+i|k)表示4×1的列向量， 包含\mathbf{B}\mathbf{o}\mathbf{x}(k+i|k)中四个角点的X坐标. {\mathit{Y}}_{\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{n}\mathrm{e}\mathrm{r}\mathrm{s}}(k+i|k)表示4×1的列向量， 包含\mathbf{B}\mathbf{o}\mathbf{x}(k+i|k)矩阵中四个角点的Y坐标. {\mathit{Y}}_{\mathrm{L}}(\cdot ,k+i|k)和{\mathit{Y}}_{\mathrm{U}}(\cdot ,k+i|k)表示将时变的下边界和上边界函数分别逐元素地应用于{\mathit{X}}_{\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{n}\mathrm{e}\mathrm{r}\mathrm{s}}(k+i|k)中的每个X坐标， 得到对应的Y坐标边界值向量.

4.3.3 稳定性约束

避障问题的解决需要考虑汽车的横摆稳定性， 车辆的横摆角速度{\omega }_{\mathrm{s}\mathrm{s}}所引起的离心力不应超过地面所提供的附着力的极限.

因此， 横摆角速度的稳定域边界为：

\left|{\omega }_{\mathrm{s}\mathrm{s}}\right|\le \frac{\sqrt[]{{\left(\mu mg\right)}^{\mathrm{2}}-F_x^{\mathrm{2}}}}{m{V}_x}（28）

式中：\left|{\omega }_{\mathrm{s}\mathrm{s}}\right|为横摆角速度稳定域边界；F_x为轮胎侧向力.

轮胎侧向力达到极限时轮胎侧偏角大小为{\alpha }_{\mathrm{r},\mathrm{p}\mathrm{e}\mathrm{a}\mathrm{k}}， 根据后轮轮胎的侧偏角计算公式， 可得质心侧偏角{\beta }_{\mathrm{s}\mathrm{s}}的限制范围为^［14］：

\left|{\beta }_{\mathrm{s}\mathrm{s}}\right|={\alpha }_{\mathrm{r},\mathrm{p}\mathrm{e}\mathrm{a}\mathrm{k}}+\frac{b\dot{\phi }}{V_x}（29）

式中：\left|{\beta }_{\mathrm{s}\mathrm{s}}\right|为质心侧偏角稳定域边界.

由式（28）与式（29）联合确定了一个封闭的包络线，分别表示横摆角速度和质心侧偏角的边界， 当车辆状态处于包络线之内时即可保证车辆的横摆稳定性.对于纵向速度变化的三自由度或五自由度动力学模型，稳定性域会随系统状态的变化而动态变化，且随速度的增加，稳定域趋于缩小，如图10所示.

4.4 预测时域变步长设计

在传统的MPC框架中， 通常采用固定时间步长T_s进行状态预测. 在车速恒定的条件下， 较大的预测时域使控制器能够预测更远的距离：

L_{\mathrm{p}\mathrm{r}\mathrm{e}}={\displaystyle \sum _{i=\mathrm{1}}^{N_p}}{T}_s(i)\cdot V_x(k+i|k)（30）

式中：L_{\mathrm{p}\mathrm{r}\mathrm{e}}表示预测模型的预测距离；V_x(k+i|k)表示在当前时刻k预测的未来第i步的纵向速度；T_s为固定的时间步长.

当车辆速度V_x降低时， L_{\mathrm{p}\mathrm{r}\mathrm{e}}会缩短， 可能导致控制器无法充分预见远期状态， 尤其是在需要长距离规划的场景， 从而降低控制系统的前瞻性和性能. 反之， 速度升高则预测距离过长， 可能增加不必要的计算负担或引入远期预测的不确定性.

为克服此问题，本文旨在维持预测时域内总的空间预测距离相对恒定，而非时间长度.为此，提出一种保持预测时域N_p固定，但动态调整每个预测时间步长T_s（k+i）的方法.其核心思想是使每个预测时间步内覆盖的空间距离保持为L/N_p.基于此目标，第i个预测时间步长T_s（k+i）（即从预测时刻k+i-1到k+i的时间间隔）的计算公式如下：

T_{\mathrm{s}}(k+i)=\frac{L/N_{\mathrm{p}}}{V_x(k+i-\mathrm{1}|k)}(i=\mathrm{1,2},\cdots ,N_{\mathrm{p}})（31）

式中：L为设定的目标总预测距离；N_{\mathrm{p}}为预测时域；V_x(k+i-\mathrm{1}|k)为在当前时刻k预测的、在第i个预测区间开始时（即预测时刻k+i-\mathrm{1}）的车辆纵向速度. 此公式确保了每个预测步对应的空间距离\mathrm{\Delta }{L}_i=T_{\mathrm{s}}(k+i)\cdot V_x(k+i-\mathrm{1}|k)=L/N_{\mathrm{p}}.

采用此变步长策略， 总预测距离为：

L={\displaystyle \sum _{i=\mathrm{1}}^{N_{\mathrm{p}}}}{T}_{\mathrm{s}}(k+i)\cdot V_x(k+i-\mathrm{1}|k)={\displaystyle \sum _{i=\mathrm{1}}^{N_p}}(L/N_p)（32）

该策略实现了恒定空间预测距离的目标. 如图11所示， 展示了该等距离预测策略：当预测速度V_x(k+i-\mathrm{1}|k)较大时， 对应的时间步长T_{\mathrm{s}}(k+i)缩短；当预测速度较小时， 时间步长T_{\mathrm{s}}(k+i)相应增长， 从而保证每个时间步在空间上推进相同的距离L/N_{\mathrm{p}}. 该方法增强了控制器在不同速度下预测范围的一致性和鲁棒性.

5 仿真试验

5.1 Lattice-MPC协同避障策略有效性验证

为验证所提出Lattice-MPC协同架构的优势， 本文设置传统的集成式MPC控制策略作为对照组. 仿真场景配置如下：自车初始纵向速度设定为25 m/s， 具体车辆参数如表2所示， 自车同车道前方设置两个动态障碍物， 长度为3 m，宽度为1.6 m. 近端障碍物距原点50 m， 纵向速度为10 m/s；远端障碍物距原点200 m， 纵向速度为7 m/s， 验证协同避障策略在高速多目标动态避障中的性能表现.

为排除预测模型与实际被控对象失配导致的避障控制性能下降问题， 有针对性地评估控制器模块设计的合理性， 本文在MATLAB环境中使用与预测模型相同的五自由度车辆动力学模型作为被控对象， 仿真结果如图12所示. 其中， 黑色实线表示基于道路边界与动静态障碍物信息构建并经平滑处理的避障边界约束， 蓝色区域代表障碍物车辆的占用空间， 绿色曲线表示自车的规划轨迹. 可以看出， 所生成的避障轨迹不仅保持了良好的平滑性和连续性， 且在完成避障机动后能高效回归至期望行驶车道.

利用Lattice-MPC协同避障控制策略和集成式MPC避障控制， 两种方案进行避障时，自车主要状态量变化对比结果如图13所示，求解耗时如图14所示.

对仿真结果进行分析， 可以得出：

1）所提出的Lattice-MPC协同避障策略在横向控制方面，平均峰值侧向加速度降低约29.8%， 平均横摆角速度波动降低约12.2%， 表现出更好的平滑性， 有效避免了集成式MPC避障控制方案中出现的状态突变、鲁棒性差的现象.

2）在纵向控制方面，相较于集成式MPC控制方案，加速度峰值波动下降57%， Lattice-MPC协同控制策略展现出更优的速度调节特性. 车辆纵向速度波动幅度显著降低， 在达到设定巡航速度后能够保持稳定的匀速运动状态；同时， 纵向加速度的变化更为平缓， 提升了行车舒适性.

3）计算效率方面，单步求解时长，Lattice-MPC分布在10 ms范围内，传统集成式MPC平均大于10 ms；总求解时长，基于Lattice-MPC协同的框架为8.65 s， 传统集成式MPC为13.63 s， 计算效率提升约36.5%.

5.2 CarSim & Simulink联合仿真验证

搭建如图15所示CarSim & Simulink联合仿真验证环境，其中绿色模块为本文所设计Lattice-MPC协同算法，紫色模块为所搭建的底层逆动力学模型， 上层控制器输出的驱动/制动力矩给下层逆动力学模型转化为加速与制动底层控制指令. 本节规划控制算法的实时仿真在配置为AMD Ryzen 7945 （3.2 GHz） CPU和16.0 GB RAM的计算机硬件平台上运行.

在CarSim中设置如图16所示连续避障场景， 仿真道路为单向四车道， 蓝色车辆为自车， 初始坐标设置为（0 m，-1.8 m）， 初始速度为28 km/h， 自车的巡航车速设置为72 km/h. 绿色车辆为障碍物车辆， 在自车当前车道前方40 m处和自车当前车道左侧车道前方100 m处以36 km/h初速度匀速行驶.其余两个障碍物为静止状态，自车参数与表2一致，实验场景的设计意图即模拟高速公路上，自车需加速以超越前方慢速车流的典型场景

连续换道避障测试场景仿真结果如下所示：

在连续换道避障过程中， 如图17（a）所示曲率平均峰值相比于Planning规划下降了32.4%， 这主要由Lattice规划层生成的换道轨迹未充分考虑车辆动力学约束Lattice-MPC协同避障策略对初始规划轨迹进行了动态调整所导致， 如图17（b）所示为Lattice规划轨迹与自车实际轨迹曲线， 可看出最终求解的实际轨迹相比于Lattice规划的引导避障轨迹更加平滑. 如图17（c）所示， 优化后的自车实际轨迹相比规划出的初始轨迹的横摆角变化率峰值降低约10%， 表明实际轨迹相比规划层给出的参考横摆角曲线具有更好的平顺性. 优化后的轨迹满足车辆动力学约束， 符合主动换道的操作要求， 验证了本文Lattice-MPC协同算法的有效性.

由图17（d）可得纵向控制方面，当车速低于预设巡航速度时，控制系统进行加速操作，直至车速达到目标值72 km/h. 在速度跟踪过程中出现了轻微的超调现象，但最大超调幅值维持在可接受范围内. 整个换道过程中，如图17（e）、17（f）所示， 前轮转角基本限制在1 rad内，车辆侧向加速度限制在0.25 m/s²以内， 满足自动驾驶换道操作对舒适性的要求.

综上， 本文所提出的控制算法能有效实现车辆在结构化道路环境下的自主避障功能，生成的运动轨迹满足车辆运动学约束条件， 验证了算法的可行性与有效性.

6 结 论

1）本文构建了一种Lattice引导轨迹规划与模型预测控制协同的避障控制策略， 旨在提升动态环境下集成式MPC自主避障控制的鲁棒性与安全性. 通过Lattice规划提供高避障引导参考轨迹， 使得MPC能够高效地在此基础上进行精细调整， 而集成式MPC优化问题中约束的处理则受益于良好的初始解， 使得整个协同框架在保证安全和性能的同时， 相比于集成式MPC避障方法， 有效提升了在动态避障场景下的安全性和鲁棒性.

2）对于集成式MPC控制的代价函数，利用Lattice规划所提供的避障引导轨迹.使得集成式MPC的避障优化过程起点更好，是实现两者高效协同的基础.

3）为克服车辆速度变化对预测时域的影响，设计了一种恒定预测距离的动态采样策略.通过固定预测时域N_p并动态调整时间步长T_s，确保MPC控制器始终具有稳定的前向预测距离，从而增强了系统在不同速度下的前瞻性和预测信息利用率，并稳定了优化问题的规模.

参考文献

[1] SANTOS M A， FERRAMOSCA A， RAFFO G V. Nonlinear model predictive control schemes for obstacle avoidance［J］. Journal of Control， Automation and Electrical Systems， 2023，34（5）： 891-906．

[2] 王国栋， 刘立， 孟宇， 等 ．一体式车辆避撞轨迹规划与跟踪控制［J］．交通运输系统工程与信息， 2022， 22（2）：127-136．

[3] WANG G D， LIU L， MENG Y， et al ．Integrated control of trajectory planning and tracking for vehicle collision avoidance［J］. Journal of Transportation Systems Engineering and Information Technology， 2022， 22（2）：127-136．（in Chinese）

[4] 金立生， 韩广德， 谢宪毅， 等 ．基于强化学习的自动驾驶决策研究综述［J］．汽车工程， 2023， 45（4）： 527-540．

[5] JIN L S， HAN G D， XIE X Y， et al. Review of autonomous driving decision-making research based on reinforcement learning［J］. Automotive Engineering， 2023， 45（4）：527-540．（in Chinese）

[6] GUAN Y， REN Y G， SUN Q， et al. Integrated decision and control：toward interpretable and computationally efficient driving intelligence［J］. IEEE Transactions on Cybernetics， 2023， 53（2）：859-873．

[7] BENDERS D， KÖHLER J， NIESTEN T， et al. Embedded hierarchical MPC for autonomous navigation［J］. IEEE Transactions on Robotics， 2025， 41：3556-3574．

[8] BONAB S A， EMADI A. Optimization-based path planning for an autonomous vehicle in a racing track［C］//IECON 2019-45th Annual Conference of the IEEE Industrial Electronics Society．October 14-17， 2019， Lisbon， Portugal. IEEE， 2019：3823-3828．

[9] HUANG C， LI B Y， KISHIDA M ．Model predictive approach to integrated path planning and tracking for autonomous vehicles［C］//2019 IEEE Intelligent Transportation Systems Conference （ITSC），October 27-30， 2019. Auckland， New Zealand．IEEE， 2019：1448-1453．

[10] MICHELI F， BERSANI M， ARRIGONI S， et al. NMPC trajectory planner for urban autonomous driving［J］. Vehicle System Dynamics， 2023， 61（5）：1387-1409．

[11] NEZAMI M， KARACHALIOS D S， SCHILDBACH G， et al. On the design of nonlinear MPC and LPVMPC for obstacle avoidance in autonomous driving［C］//2023 9th International Conference on Control， Decision and Information Technologies （CoDIT）， July 3-6， 2023， Rome， Italy． IEEE， 2023：1-6．

[12] YU S Y， SHENG E C， ZHANG Y J， et al ．Efficient nonlinear model predictive control of automated vehicles［J］．Mathematics， 2022， 10（21）：4163．

[13] KIM D， KIM G， KIM H， et al ．A hierarchical motion planning framework for autonomous driving in structured highway environments［J］．IEEE Access， 2022， 10：20102-20117．

[14] 何德峰， 徐广琪， 李海平 ．自主车辆高效滚动时域轨迹规划与跟踪控制［J］．浙江工业大学学报， 2021， 49（1）：16-23．

[15] HE D F， XU G Q， LI H P. Efficient rolling horizon trajectory planning and tracking control for autonomous vehicles［J］. Journal of Zhejiang University of Technology， 2021， 49（1）：16-23．（in Chinese）

[16] 柏硕 ．考虑驾驶员特性与路面附着系数的主动安全避撞策略研究［D］．南京：东南大学， 2022．

[17] BAI S ．Research on collision avoidance strategy of active safety considering characteristics of drivers and road adhesion coefficient［D］. Nanjing：Southeast University， 2022．（in Chinese）

[18] 颜秉卿 ．基于集成规划和控制的自动驾驶车辆避障系统研究［D］. 镇江： 江苏大学， 2019．

[19] YAN B Q. Integrated planning and control for autonomous vehicle collision avoidance systems［D］. Zhenjiang：Jiangsu University， 2019．（in Chinese）

基金资助

国家重点研发计划资助项目(2023YFC3009600)

National Key Research and Development Program of China(2023YFC3009600)