汇总！自动驾驶路径规划中的障碍物处理方法总结

作者 | 西涯先生  编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/635971229

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【规划控制】技术交流群

后台回复【规划控制综述】获取自动驾驶、智能机器人规划控制最新综述论文！

1 引言

无论是在Frenet坐标系亦或在笛卡尔坐标系下展开路径规划，障碍物约束的处理是整个自动驾驶规划问题中最具挑战性的一个环节。（P.S. 这里指基于优化的方法，基于采样+剪枝以及网格搜索的方法不在讨论范围内）

一般来说，好的障碍物约束的处理需要满足：精确、可微、计算复杂度低、局部极值点数量少等特点。

现有的自动驾驶相关文献中，障碍物约束的处理方法大致可以分为以下几类：

1. 基于混合整数线性规划的处理方法

2. OBCA方法

3. 基于凸包通行走廊的处理方法

4. 基于符号距离场（SDF）的处理方法

下面，我会逐一介绍每种方法的核心思想，并且对比他们的优劣。

2 障碍物约束的一般表述

首先，定义二维平面上的坐标点为 p=[x,y] ，那么可以定义代表障碍物占据空间的集合，以及代表主车占据空间的集合，进而对障碍物约束进行数学上的准确定义。

本文中假定障碍物可以由一个凸多边形或者多个凸多边形来包络，使得障碍物约束的描述得到一定的简化。这个假设是合理的，也是大多数论文中采用的。即便是非常复杂的障碍物，也可以通过少量的约束放松，使得其可以用多个凸多边形表述。一个凸多边形障碍物就可以用如下符号表示：

其中， A 矩阵的每一行代表一条障碍物多边形一条边的法向量， b 向量的每一行代表障碍物的这条边到原点的距离。所以， n 也代表了障碍物凸包的边的数量。注意，这里的含义，假定了 A 矩阵每一行的模长为 1 。

主车的占据空间一般采用多个包络圆或者单个矩形进行表述。当采用包络圆表述时，可以将包络圆缩小为一个质点，而将障碍物扩展一个圆的半径，从而主车可以缩减为一条线段或者是多个离散点（忽略了包络圆圆心的固定关系），进而障碍物约束问题就变成了避免一个点出现在凸包内，或者避免一个线段和凸包有交点的问题。而当采用单个矩形时，障碍物约束的问题就是避免矩形和障碍物凸包有交集（重叠部分）。

2 基于混合整数线性规划的处理方法

思考一个问题，对于一个点 p 而言，它不在一个障碍物凸包内，意味着什么呢？

事实上，只需要点 p 违反障碍物的任意一条边的约束，也就是不等式Ap<=b在任意一行上不成立即可。

所以，要使一个点远离障碍物，只需要用到障碍物凸包的任意一条边或者任意几条边即可。

为此，可以定义一系列的二值变量，记作 ,其中，i=1,......,m代表的是障碍物的数量，j=1,......,n代表的障碍物凸包里边缘的数量。因此，的总数量是  m* n。进一步地，当 =0的时候，意味着点 p 满足该边的约束。当 =1 的时候，意味着点 p 不满足该边的约束，也就是说这是真正生效的障碍物约束。

基于上述定义，首先考虑单个点p的情况，可以把障碍物的约束表达如下：

其中， 和 代表的是第 i 个障碍物的第 j 条边的相关参数。另外，第三个不等式意味着，对于第 i 个障碍物来说，至少要有一条边的约束是生效的，否则就会发生碰撞。

进一步地，考虑更一般的情况，例如一条线段或者一个矩形的碰撞约束问题。

为了保证一条线段或者一个矩形不发生碰撞，我们需要线段或矩形的所有端点同时满足约束，也就是如下表达式：

其中，k 代表线段或矩形的端点个数。

评论：
基于混合整数线性规划的碰撞约束建模方法引入了大量二值变量，产生了大量的约束，计算复杂度非常高，但是胜在碰撞约束建模的准确性。另外，一般来说小规模的混合整数线性规划问题可以求出全局最优解，但是也需要耗费大量的计算时间。

3 OBCA方法

OBCA方法的核心是利用强对偶性，把主车和障碍物的符号距离约束转化为了一般的光滑的非线性约束。

然后，定义两个集合的符号距离函数如下：

其中：

显然，上述函数均是主车状态 x 的相关函数。dist函数表达的是，主车最少需要移动多少距离，就可以和障碍物发生碰撞。pen函数描述的则是，主车最少需要移动多少距离，才可以不和障碍物发生碰撞。举个例子，如果主车已经和障碍物碰撞了，dist=0，如果没有碰撞，那么pen=0。因此，当主车和障碍物已经发生碰撞，符号距离sd代表的就是障碍物侵入主车的距离。当主车和障碍物没有碰撞，符号距离sd代表的就是主车相对障碍物的距离。

因此，基于符号距离的定义，障碍物约束可以描述为：

其中， d 代表障碍物避碰的距离阈值。

由于上面描述的符号距离函数本身也是约束优化问题，因此可以找到其最优解的KKT条件。另外，根据优化理论，最终可以把符号距离约束，转换为一般的非线性约束：

上式将原本的障碍物约束转换为了五个约束，其中三个不等式约束，两个等式约束。并且引入了 和 作为辅助变量。从形式上来看，上面的几个约束关于 x 、  和 的非线性函数，因此求解的时候需要用到一些非线性优化的工具。

评论：
OBCA方法把离散不连续的障碍物约束精确地转化为了连续光滑的非线性约束，因此能够用一般的非线性优化器进行求解。然而，OBCA方法也引入了大量的辅助变量（ 的元素个数等于障碍物数量乘以障碍物的边数），求解复杂度较大。另外，OBCA方法仍然是个非凸规划问题，所以不可避免的有局部极值点，因此求解结果依赖于初始解的质量。

4 基于凸包通行走廊的处理方法

据我观察，基于凸包通行走廊的障碍物约束处理方法是工程实践中最常用的一类方法。它具有原理简单，求解结果稳定，求解速度快的一些优势。

基于凸包通行走廊的障碍物约束处理方法一般可以分为三个步骤：

• 一，根据障碍物空间位置，构建出一条由凸包构成的通行走廊

• 备注1：凸包的形状可以是多样的，例如圆形、矩形、梯形、以及其他凸多边形

• 备注2：为了保证最终Path的安全性，相邻凸包间通常需要有重叠，并且凸包的并集应该完整覆盖从起点到终点的通行区域

• 二，Path点和凸包进行一一匹配，也就是，规定每个Path点允许的活动范围

• 三，障碍物避碰约束转换为Path点在凸包内部的线性约束

上述方法的具体原理说起来很简单，但是实际操作起来每一步都会涉及到不少问题。

第一点，关于构建凸走廊的方法，大多数方案都需要提前通过某些方法确定初始解，并围绕初始解上的Path点去拓展出凸包。因此，这就需要初始解不能太差，否则最终也绝对不可能得到一个好的规划结果。其次，即便初始解质量不错，在产生凸包的时候也需要尽可能保证凸包的范围大，并且覆盖了更多的通行空间，以保证最终Path有较大的调整空间。此外，当求解空间就非常狭窄，可能无法构建出一条凸包足够大且相互重叠的通行走廊时，如何放松约束，保证问题的可解性，也需要考虑。

第二点，关于Path点和凸包的匹配问题，绝大多数方法都是在定义优化问题的时候就把Path点和凸包进行一一绑定了，并且在整个问题求解和优化的过程中都是固定不变的。这样做的好处就是可以把障碍物避碰约束完全转换为凸约束，提升Path的求解速度和稳定性。然而，这种方式依赖于初始解的质量，如果初始解的Path点一开始都落在凸包内，那么进行Path点和凸包的一一匹配是比较容易。但要是初始解一开始就不在约束范围内，那么如何确定Path点在哪个凸包内移动就会比较困难了。因此，也有一些方法提供了迭代优化的思路，一开始先给定一组Path点和凸包的对应关系，然后随着Path的迭代，再逐步切换这种对应关系，最终达到稳定。

最后，如何在凸包通行走廊处理方法的基础上考虑主车包络也是一个值得思考的问题。

如果将主车看做多个包络圆的集合，那么可以先用包络圆半径对障碍物进行膨胀，相当于回缩凸走廊的边界。同时，主车也可以被看做为一条线段。后续就是通过优化保证每一条主车线段都落在同一个凸包范围内。P.S. 这要求凸走廊里每个凸包的范围大一些，至少可以容纳下这一条线段。当然，如果无法保证生成这么大的凸包，让线段上的点落在不同凸包内也是可以的，不过这样的约束往往会更加严格，损失一定的通行能力。

如果将主车看做矩形，那么要处理这种情况，需要引入除了XY坐标以外的第三维度信息，也就是Heading信息。因为主车是否和障碍物碰撞，除了和主车的位置有关，和它的Heading也有密切关系。引入Heading信息后，可以建立X-Y-Heading三维空间内的凸包通行走廊。另外，由于主车的外形轮廓是已知的，在建立X-Y-Heading凸包通行走廊时可以考虑这一信息，使得在生成走廊的基础上，能够将主车视作为质点。

比如，下图这个例子，主车在A点和B点，是道路边界内的两个极限可行位置，在这两个位置处Heading的可行范围只有0度。而在C点，主车的Heading可行范围是正负Pi。因此，在X-Y-Heading空间上，可以构建一个类似下图右侧的三维凸空间，用于描述通行空间，并且主车可以被视作为质点。

评论：
基于凸包通行走廊的障碍物约束方法采取了逆向思维，把主车不能侵入障碍物这件事，描述为主车需要待在一个安全区域之中，这样的思维方式使得障碍物约束的数量得到了显著的简化（障碍物数量可能非常大，但是凸包数量是可控的）。此外，由于安全区域是一个凸空间，因此把避碰问题转换为了凸问题，极大的提高了求解速度和求解的稳定性。然而，这种简化的代价就是我们需要事先提供高质量的凸走廊和初始解。如果凸走廊本身不合理，求解出来的Path自然也会很奇怪。

5 基于符号距离场（SDF）的处理方法

规划领域常用的是欧几里得符号距离场，这个距离场可以构建在二维或者三维空间，距离场在空间中任意点的取值，代表着该点到最近的障碍物（几何体）的距离。之所以叫符号距离场，是为了定义障碍物内部空间。当空间点处于障碍物内部时，距离取负值。反之，距离就是正常的正数。

在工程实践中，我们会将二维或三维空间进行网格化，然后计算每一个网格的符号距离值，从而建立一个完整的符号距离场，并且存储在内存空间中。基于事先建立的符号距离场，我们能够实现高效的碰撞检测，同时能够迅速的计算碰撞约束产生的梯度信息和二阶导数信息。更重要的是，发生碰撞时，障碍物内部的碰撞信息也有良好的定义，因此更利于优化算法的顺利进行。

根据上面的描述，对于单点来说，符号距离场的定义是非常容易的，也是非常便于使用的。例如，无人机通常会用一个圆形来表述其轮廓，通过对障碍物的膨胀操作，就可以进一步把无人机简化为一个点。因此，无人机运动规划中，常常可以见到符号距离场的身影。

然而，对于车辆来说，将轮廓简化为单点的操作误差太大了，完全无法接受。如果暴力地直接使用车辆矩形轮廓的四个角点，利用其符号距离来判断碰撞，也不能保证其安全性。很容易构造出一类场景，即四个角点都没有发生碰撞，但是其中有一条边发生了碰撞。因此，要想使用这种方法，就必须要定义出从障碍物到车辆矩形轮廓的符号距离场。这个距离场显然不仅和车辆的位置相关，与车辆的姿态也直接相关。也就是说，要保证车辆规划结果的安全性，需要定义与X、Y以及主车Heading相关的三维符号距离场。目前这方面的工作，我也几乎没有看到过，如果有朋友了解的，欢迎评论区指出。

评论：
基于欧几里得符号距离场的障碍物约束方法需要在优化运算前花费大量计算资源和内存空间进行符号距离场的构建，在优化过程中则可以非常高效的进行Cost的评估和导数的计算。另外，基于符号距离场的规划方法通常对规划主体的外形轮廓有比较强烈的限制，通常是看出一个质点，因此精确性不高。最后，符号距离场建立了一个全局的势场函数，描述了整体的外部环境。其他的方法通常只会考虑局部的障碍物，是对整体环境的一个局部近似。因此，在符号距离场下进行规划，通常能够取得在整体环境下的更优值，但是它依然强烈依赖于初始解。

6 综合对比

综合前文的介绍，可以给上述四类方法进行一个总结与对比

7 参考文献

[1] Michael P. Vitus† et al. Tunnel-MILP: Path Planning with Sequential Convex Polytopes. 2008

[2] Xiaojing Zhang et al. Autonomous Parking using Optimization-Based Collision Avoidance. 2018.

[3] Henrik Andreasson et al. Fast, Continuous State Path Smoothing to Improve Navigation Accuracy. 2015.

[4] Zhichao Han et al. Differential Flatness-Based Trajectory Planning for Autonomous Vehicles. 2022.

[5] Heechan Shin et al. Kinodynamic Comfort Trajectory Planning for Car-like Robots. 2018.

往期回顾

史上最全综述 | 3D目标检测算法汇总！（单目/双目/LiDAR/多模态/时序/半弱自监督）

（一）视频课程来了！

自动驾驶之心为大家汇集了毫米波雷达视觉融合、高精地图、BEV感知、多传感器标定、传感器部署、自动驾驶协同感知、语义分割、自动驾驶仿真、L4感知、决策规划、轨迹预测等多个方向学习视频，欢迎大家自取（扫码进入学习）

（扫码学习最新视频）

视频官网：www.zdjszx.com

（二）国内首个自动驾驶学习社区

近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、Occpuancy、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

（三）【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称