自动驾驶---轨迹优化的两种思路

自动驾驶轨迹规划方法主要分为横纵分离与横纵联合两种。横纵分离规划将运动分解为横向和纵向独立优化，计算效率高但忽略耦合效应；横纵联合规划则通过非线性模型预测控制处理耦合问题，能获得更优轨迹但计算复杂度高。两种方法各有优劣，发展趋势呈现混合架构方向，结合分离方法的实时性和联合优化的全局最优性，并引入数据驱动技术提升复杂场景的适应性。

智能汽车人

1801人浏览 · 2025-06-19 07:30:00

智能汽车人 · 2025-06-19 07:30:00 发布

1 前言

早期的不少规划算法都是横纵分离的（比如Apollo，目前依然有不少公司沿用Apollo的架构），先求解path之后，依赖path的结果再进行speed的求解。这种横纵解耦规划方式的特点也比较明显：

相对较为简单，计算量通常较小，容易实现实时性要求。
由于分别规划横向和纵向运动，可能会忽略两者之间的耦合关系，导致规划结果不够准确。
在复杂场景下，可能无法很好地适应车辆等其它动态障碍物的变化。

随着技术的发展和性能的要求，横纵分离算法的弊端越来越明显，因此时空联合规划的idea逐渐浮出。那么本篇博客主要介绍轨迹优化相关的一些知识，包括横纵分离以及横纵联合（或者叫时空联合规划）。

2 轨迹优化

在自动驾驶轨迹生成中，横纵分离求解与横纵联合求解是两种核心方法论，其差异体现在对车辆运动的分解方式和优化目标的整合程度上。

2.1 横纵分离

核心思想：将车辆运动分解为横向（Lateral）和纵向（Longitudinal）两个独立子问题，分别进行规划与控制。

2.1.1 横向规划的 QP 求解

（1）问题建模

目标函数：最小化路径跟踪误差（横向偏差）和曲率平滑度： $J_{\text{lat}} = \sum_{k=1}^N \left( w_1 e_y(k)^2 + w_2 \kappa(k)^2 + w_3 \Delta\kappa(k)^2 \right)$
其中， $e_y$ 为横向偏差， $\kappa$ 为曲率， $\Delta\kappa$ 为曲率变化率， $w_1, w_2, w_3$ 为权重系数。
状态变量：离散化时间点 $t_k$ 对应的位置 $x(k)$ ， $y(k)$ ，航向角 $\theta(k)$ 。
控制变量：方向盘转角 $\delta(k)$ 或曲率 $\kappa(k)$ 。

（2）约束条件

运动学约束：曲率与方向盘转角关系： $\kappa(k) = \frac{\delta(k)}{L} \quad \text{}$ 其中 L 为轴距。
物理极限约束：最大方向盘转角 $\delta_{\text{max}}$ ，曲率上下限 $\kappa_{\text{min}} \leq \kappa(k) \leq \kappa_{\text{max}}$ 。
障碍物约束：基于几何投影的安全距离约束： $\sqrt{(x(k) - x_{\text{obs}})^2 + (y(k) - y_{\text{obs}})^2} \geq d_{\text{safe}}$
......

（3）QP 形式化

将目标函数和约束转换为标准 QP 形式：（详细内容及公式推导可参考博客《自动驾驶---Motion Planning之轨迹Path优化》）

2.1.2 纵向规划的 QP 求解

（1）问题建模

目标函数：最小化速度跟踪误差和加速度变化率（仅举例舒适性）： $J_{\text{lon}} = \sum_{k=1}^N \left( w_1 (v(k) - v_{\text{ref}}(k))^2 + w_2 a(k)^2 + w_3 \Delta a(k)^2 \right)$ 其中， $v_{\text{ref}}$ 为参考速度，a 为加速度， $\Delta a$ 为加速度变化率。
状态变量：速度 $v(k)$ 、加速度 $a(k)$ 。
控制变量：油门 / 刹车指令 $u(k)$ （通过车辆动力学模型映射为加速度）。

（2）约束条件

动力学约束：加速度与油门 / 刹车关系： $a(k) = f(u(k), v(k)) \quad$
物理极限约束：最大加速度 $a_{\text{max}}$ 、最小减速度 $a_{\text{min}}$ ，最大油门 / 刹车行程 $u_{\text{max}}$ 。
安全约束：与前车的安全距离：

与前车的安全距离：
$s_{\text{rel}}(k) \geq \frac{v(k)^2}{2 a_{\text{dec}}} + d_{\text{buffer}}$
其中 $a_{\text{dec}}$ 为最大减速度， $d_{\text{buffer}}$ 为静态安全距离。
......

（3）QP 形式化

将纵向问题转化为标准 QP，优化变量为加速度序列，约束包括动力学模型、安全距离和物理极限。

详细内容及公式推导见博客《自动驾驶---Motion Planning之轨迹Speed优化》。

2.1.3 优势与局限性

优势：
- 计算效率高：二维问题降维为两个一维问题，适合实时性要求高的场景（如城市道路）。
- 模块化设计：横向与纵向模块可独立开发、测试和迭代。
局限性：
- 忽略耦合效应：例如高速过弯时，纵向加速度会影响横向稳定性（离心力变化）。
- 次优解风险：分离优化可能导致全局非最优（如紧急制动时的转向响应延迟）。

2.2 横纵联合

核心思想：将横向与纵向运动作为统一的动态系统进行联合优化，充分考虑运动耦合。其中涉及的主要内容可参考以下博客：

《自动驾驶---Motion Planning之构建SLT Driving Corridor》

《自动驾驶---Motion Planning之STSC轨迹优化》

《自动驾驶---“火热的”时空联合规划》

《自动驾驶---用于运动规划的CILQR求解器》

2.2.1 联合优化框架

数学模型：
- 车辆动力学模型：使用单车模型（Bicycle Model）或更复杂的多体动力学模型。
- 状态变量：包含位置 (x, y)、航向角 θ、速度 v、横摆角速度 ω 等。
- 控制变量：方向盘转角 δ 和加速度 a。
优化目标：
- 最小化路径跟踪误差（横向偏差、航向偏差）。
- 最小化加速度 / 减速度变化率（Jerk）以提升舒适性。
- 满足障碍物避让约束。
- ......
方法：
- CILQR
- PiecewiseCurve（QP）---Bezier/Polynomial

2.2.2 难点

高维状态空间：联合求解需处理更多状态变量（如横摆角速度、侧向加速度）。
实时性问题：优化计算量随预测时域增长呈指数级上升。
约束处理：同时满足运动学（如最大转向角）和动力学（如轮胎附着力极限）约束。

2.2.3 优势及应用场景

优势：
- 全局最优性：通过联合优化生成更安全、舒适的轨迹。
- 复杂场景适应性：适用于急弯、高速变道等耦合效应显著的场景。
典型应用：
- 城区自动驾驶（如绕行等）。
- 极端工况（如紧急避障、麋鹿测试，这种场景只能靠类似AEB的功能）。

2.3 对比

维度对比结果如下表所示：

维度	横纵分离求解	横纵联合求解
计算复杂度	低（线性或二次规划）	高（非线性优化）
实时性	适合低延迟场景（如城市低速）	依赖硬件加速（如高速场景）
轨迹质量	满足基本需求，但可能非最优	更优的舒适性与安全性
工程实现难度	低（模块化设计）	高（需精确动力学建模）