【自动驾驶轨迹规划之安全行驶走廊】

本文提出了一种基于安全飞行走廊（SFC）和安全行驶走廊（STC）的路径规划方法，用于自动驾驶车辆的动态环境路径规划。通过将车辆建模为两个圆盘，可以有效减少计算量，同时确保车辆与障碍物之间的安全距离。在路径规划过程中，车辆后轴中心的轨迹通过A或hybrid A算法生成，并结合障碍物环境进行建模，生成安全行驶走廊。通过局部矩形区域的扩展，构建多边形路径，确保车辆在动态环境中能够安全避障。测试结果表明，该方法在复杂环境中能够有效规划出安全且高效的路径。

欢迎大家关注我的B站：

Zheng同学的个人空间，专注于薯片的爱好者。该个人主页链接至哔哩哔哩视频平台，访问地址为：bilibili.com/1353527764?spm_id_from=333.1007.0.0

目录

1 原理剖析

1.1 安全飞行走廊（SFC）

1.2 安全行驶走廊（STC）

1.2.1 车辆外形建模

1.2.2 环境建模

1.3 生成安全行驶走廊的伪代码

1.4 建立约束限制

1.5最优控制

2 测试结果

---

本文参考论文链接如下：供感兴趣的读者参考原始论文[Maneuver Planning for Automatic Parking with Safe Travel Corridors: A Numerical Optimal Control Approach | IEEE Conference Publication | IEEE Xplore](https://ieeexplore.ieee.org/document/9143786 "Maneuver Planning for Automatic Parking with Safe Travel Corridors: A Numerical Optimal Control Approach | IEEE Conference Publication | IEEE Xplore)

1 原理剖析

1.1 安全飞行走廊（SFC）

大规模且非凸的碰撞约束显著提升了路径规划的计算复杂度，在无人机路径规划问题中，开发了安全飞行走廊（SFC）这一方法。通过铺设安全飞行走廊，无人机与周围的所有障碍物保持足够的安全距离，从而使得最优控制问题的规模完全不受环境复杂性的程度影响，显著提高了路径规划的速度。

安全飞行走廊的好处：

比如，采用Route2路径，但路径规划会包含这些障碍物O1和O2在内。实际上，那些距离较远的障碍物不需要被包含在路径规划中，因此，安全飞行走廊可以减少最优控制中冗余的避障约束条件。

在Route2路径上，通过建立安全飞行通道，等同于规避了障碍物的复杂性，只需确保飞行路径位于安全飞行通道内即可，如图所示。

1.2 安全行驶走廊（STC）

然而，飞行安全走廊（safe flight corridor）无法直接应用于车辆的路径规划，因为汽车的形状可以被视为质点或圆盘，因此车辆的不同部分可能分布在不同的安全走廊上。鉴于此，在自动驾驶领域提出了行驶安全走廊（safe travel corridor）。

1.2.1 车辆外形建模

在行驶过程中，整辆车都需要在安全行驶走廊内部运行，然而计算车辆的矩形外形与障碍物是否存在交集所需的时间较大。如果采用圆形来表示车辆，计算量则会显著减少。因此，在减少计算量的同时，保证冗余率（即由圆形构成的车辆比原始车辆大得多）不会过于庞大，我们选择用两个圆盘来替代一辆车。

下面是原始车辆 的外形建模参数

经过两个圆盘去代替一辆车，新型车辆 的外形建模参数如下

两个圆心分别是

，新型车辆的各个参数可以由原始车辆的参数去确定，公式如下

1.2.2 环境建模

实际上，这时候车辆收缩半径已经可以被视为两个质点，然而必须将障碍物去除非结构部分，如示意图所示。

这样就可以将车辆的路径规划问题转化为两个质点的避障轨迹

1.2.3 安全行驶走廊的构建

自动驾驶轨迹规划之A*算法

首先，为了实现车辆后轴中心的精确搜索，需要对目标位置进行A或hybrid A算法的优化。通过这种方法，可以有效获得车辆后轴中心的初步轨迹。如对A或hybrid A算法不熟悉，建议参考以下链接获取详细信息：

1. 【自动驾驶轨迹规划之A*算法

2. 【自动驾驶轨迹规划之hybrid A*算法

通过定位车辆后轴中心的粗略轨迹，我们可以确定两个替代车辆的圆盘圆心，从而确定各自的粗略轨迹。以质点Pf为例，介绍构建安全行驶走廊的具体方法。首先，通过A算法或混合A算法进行路径规划，获得路径的总耗时t。随后，对t进行等间隔采样，以获取均匀分布的采样点。在这些采样点上分别生成一个多边形，多个多边形连接起来就形成了上述的安全行驶走廊。这里选择多边形为矩形。

但是怎么生成这样的多边形呢？

在原始论文中，采用了基于局部矩形区域生成的方法，系统地选取采样点，按照逆时针或顺时针两种方式，沿着矩形的四条边依次向外扩展。

个单位（

过大导致矩形区域较小，容易丢失最优或较优轨迹 ，

当矩形过于狭窄时，不仅会导致计算量的急剧增加，而且会严重影响路径规划的效率。这种增长过程会持续进行，直到矩形与障碍物发生接触。然而，矩形的长和宽必须保持在我们设定的一个最大值以内，以避免在较大的空区域中进行过多的计算。矩形生成图如下所示。

1.3 生成安全行驶走廊的伪代码

1.4 建立约束限制

在完成上述处理后，我们就可以真正实现安全行驶走廊的约束机制。值得注意的是，这里的采样采用了更高的分辨率，这在保证所有轨迹都位于安全行驶走廊内方面具有重要意义。通过对时间进行均匀采样，我们获得了 Pf 和 Pr 的采样点。这些采样点需要满足位于安全行驶走廊边界内的约束条件，这些条件均为线性不等式，且约束条件较为简单。

1.5最优控制

自动驾驶轨迹规划之最优控制

该控制方案专为静态环境设计，当系统进入动态环境时，建议采用滚动优化算法进行在线轨迹的动态调整，具体实现细节将在后续文章中详细阐述，本文重点介绍静态环境下的路径规划方案。

2 测试结果