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

目录

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

1 原理剖析

1.1 安全飞行走廊（SFC）

大规模和非凸的防撞约束使路径规划的计算复杂度过高，在无人机路径规划领域，提出了安全飞行走廊（SFC）这种方式，通过铺设安全飞行走廊使得无人机与周围的所有障碍物分开，从而最优控制问题的规模完全独立于环境的复杂性，提高了路径规划的速度。

安全飞行走廊的好处：

（1）例如你选择Route2，但原来的路径规划会考虑你不能撞上O1，O2，实际上这些离得较远的障碍物不需要考虑，因此安全飞行走廊可以减少最优控制中的冗余避障约束。

（2）你在Route2 中通过铺设安全飞行走廊，就相当于忽视了障碍物的高复杂度，只需保证飞行轨迹在安全飞行走廊内即可，如上图所示。

1.2 安全行驶走廊（STC）

但是，安全飞行走廊（safe flight corridor）不能直接应用于车辆的路径规划，因为汽车的形状能够看成质点或是一个圆盘，因此车辆的不同部分可能停留在不同的走廊上。因此根据车辆的外形特性，在自动驾驶领域引入提出了安全行驶走廊（safe travel corridor）。

1.2.1 车辆外形建模

因为行车时整辆车都需要在安全行驶走廊内部，但去计算车辆的矩形外形与障碍物有没有交集的计算量较大，若车辆由圆形构成，计算量会较少，因此，为了减少计算量但同时保证冗余（即由圆形构成的车辆比原始车辆大很多）不会过大，我们选择用两个圆盘去代替一辆车。

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

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

两个圆心分别是   ，新型车辆的各个参数可以由原始车辆的参数去确定，公式如下

 

1.2.2 环境建模

这时候其实就已经可以让车辆收缩半径，看作两个质点，但需要将障碍物去膨化，示意图如下

 

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

1.2.3 安全行驶走廊的构建

首先将车辆后轴中心 (x,y) 进行A*或者hybrid A* 的搜索，从而得到车辆后轴中心的粗略轨迹，不熟悉 A* 或者 hybrid A* 的读者请参考【自动驾驶轨迹规划之A*算法】_无意2121的博客-CSDN博客【自动驾驶轨迹规划之hybrid A*算法】_无意2121的博客-CSDN博客

车辆后轴中心的粗略轨迹确定，那么两个代替车辆的圆盘的圆心也就确定了他们的粗略轨迹，以质点 Pf 为例来介绍构建安全行驶走廊的方法。首先得到由A*或者hybrid A* 的搜索完成路径规划的总耗时 t ，对 t 进行均匀采样，目的是取出相对均匀的轨迹点，在采样得到的轨迹点上分别去扩展一个多边形，多个多边形就连成了上述的安全行驶走廊。这里的多边形选择矩形。

 

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

原始论文中采用局部矩形区域生成的办法，依次选取采样点，按照逆时针或顺时针分别沿着矩形四条边的方向不断向外扩展  个单位（ 过大导致矩形区域较小，容易丢失最优或较优轨迹， 过小导致计算量过大，路径规划速度慢），直到矩形与障碍物有重叠，但是矩形的长宽不能超过自己设定的一个最大值，防止在一个较大空区域上耗费过多计算量。矩形生成图如下

 

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

1.4 建立约束限制

在经过上述的处理，现在就可以真正进行安全行驶走廊的约束，我们首先对时间采样，但这里的采样不是之前的采样，现在的采样分辨率需要较高，才能保证轨迹都在安全行驶走廊内。对时间均匀采样得到了 Pf 与 Pr 的采样点，于是我们可以建立 Pf 与 Pr 的采样点都在安全行驶走廊边界内的约束不等式，这都是线性不等式，约束复杂度低。

1.5最优控制

我们通过建立车辆运动学约束、两点边值约束、安全行驶走廊约束，再设定合理的评价函数，就可以完成一个完整的最优控制问题，详情参考【自动驾驶轨迹规划之最优控制】_无意2121的博客-CSDN博客

这中控制是针对于静态环境的路径规划，如果是动态环境，我们可以利用滚动优化的方式进行在线轨迹重规划完成，以后的文章会进行讲解，本文聚焦于静态环境。

2 测试结果