贝莎的轨迹规划—一种局部，连续的方法

泡泡航行天下，带你精读决策规划领域顶级会议文章

标题：Trajectory Planning for BERTHA—a Local, Continuous Method

作者：Julius Ziegler, Philipp Bender, Thao Dang and Christoph Stiller

来源：IV2014

编译：邬杨明

审核：张鲁

欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权

摘要

大家好，今天为大家带来的文章是--贝莎的轨迹规划—一种局部，连续的方法，该文章发表于2014 IEEE Intelligent Vehicles Symposium (IV)，谷歌学术显示目前已经被引192次。

贝莎-本茨，是汽车发明者卡尔-本茨先生的妻子。130 年前，她在没有告知丈夫且没得到当局批准的情况下，把本茨先生制造的汽车Patent-Motorwagen Model III 开了出去。从曼海姆到普福尔茨海姆的这次 103 公里 “非法上路” 就成了历史上第一次汽车长途行驶，而贝莎也成了世界上第一位真正意义的汽车驾驶员。为了纪念，德国卡尔斯鲁厄理工学院（KIT）和奔驰公司将自己的开发的一辆无人车也叫做贝莎，2013年8月，贝莎全程自主行驶完成了这条103公里的路线。并且贝莎也赢得好几次欧洲的大型合作驾驶挑战赛(GCDC)。

图 1 贝莎-本茨与第一辆奔驰

这篇文章根据变分法，提出一种局部连续的方法。本文设计了能够表达动力学可行性和舒适度的目标函数，最优轨迹是目标函数达到极值时的解。静态和动态障碍物约束都表达为多边形的形式。本文精心设计的约束和目标函数确保解收敛到一个唯一的全局最优解。

主要贡献

1、放弃组合式方法，转而采用一种不必离散工作空间的连续的优化方法，因而离散方法带来的次优性、概率完备性或分辨率完备性这种采样类算法固有的缺陷也就去除了，此外，这个方法的计算复杂度并不随着状态空间维数的增加而成指数性增长。

2、引入行驶通道，将障碍物处理成凸包，并凸化整个工作空间，使得算法在不给定好的初值的情况下也能够快速收敛到工作空间最优。

算法流程

1、目标函数

设计目标函数，目标函数的惩罚项包括：与行驶通道的距离，与参考速度的偏离，加速度，加加速度，角速度。这五项线性组合，通过积分组成泛函形式的目标函数。巧妙的是，本文离散时间轴，采样固定步长的时间戳，以与时间戳相互映射的position(x,y)做为决策变量，通过position的差分，目标函数可以转换成差分的有限项的和的形式。

2、约束函数

接下来，将车辆的内部约束和外部约束分别整理，做为轨迹优化的约束函数。特别指出的是，本文以N个圆做为车体的包络圆，对于每一个圆的圆心都有到障碍物的距离约束。

图 2 车体的包络圆表示

3、工作空间凸化

在对待距离约束的时候，我们需要精心处理障碍物。我们知道对于规划的状态空间，无论是欧氏空间还是速度空间，它都是非凸的，对于优化算法来说，除非给出一个最优解的领域解，否则很容易陷入驻点，导致优化的结果不能令人满意。本文将离散的障碍物先给处理成凸包，然后结合驾驶通道的边缘，又将障碍物给凸化，使得工作空间成为凸的。

图 3 轨迹优化的驻点（a) 无决策过程。（b) 有决策过程。

图 4 根据传感器建立约束多边形

4、伪距离

本文又设计了一个距离函数，引入了具有连续可微性质的伪距离。通过对多边形角点的插值，构建了伪梯度场，这样我们就能很快解出position到多边形的距离。

图 5 伪距离和伪梯度场

5、SQP优化

最后形成一个带约束的非线性规划（NLP）问题，根据KKT条件，使用序列二次规划（SQP）解这个规划问题。

主要结果

图 6 红色表示车辆轨迹，蓝色表示障碍物和约束多边形，黑色表示边界约束

图6a显示了快速通过环形路。该方法显示出较好的动力学性能，在环道的入口，中点和出口有最小的横向加速度。图8b、8c、8d为通道示意图同一个右转弯，不同位置设置障碍物。蓝色的十字架是体素的位置，累积超过5帧，蓝色的多边形是它们的多边形包络。一个平滑的轨迹能够被发现，即使只有很少的自由空间。图8e显示面对较慢的迎面而来的车辆的一个急转向行为。预测的迎面而来的物体的轨迹用深蓝色表示，以及由此产生的淡蓝色障碍多边形。障碍物多边形是时间相关的，圆表示车辆相遇的时刻。

本文提出的轨迹规划器已经证明了非常有能力应付所遇到的各种挑战。精心设计一个单一的目标函数，精心处理多边形约束，并注意数值优化方案的细节。得到的系统非常通用，并且自然地实现了许多操作，没有任何外部启发，车辆在急转弯或掉头时能够减速。驾驶风格被乘客描述为愉快和自然。

Abstract

In this paper, we present the strategy for trajectory planning that was used on-board the vehicle that completed the 103 km of the Bertha-Benz-Memorial-Route fully autonomously. We suggest a local, continuous method that is derived from a variational formulation. The solution trajectory is the constrained extremum of an objective function that is designed to express dynamic feasibility and comfort. Static and dynamic obstacle constraints are incorporated in the form of polygons. The constraints are carefully designed to ensure that the solution converges to a single, global optimum.

如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。