道路场景中的风险最小化轨迹规划

本文提出了一种基于A*算法的用于道路场景的轨迹规划方法，该方法中，使用了图形搜索近似，并且通过一种新的推理技术来考虑其他车辆的不确定预测，这种新的推理技术允许高效计算一组模拟场景类型的危险结果的概率。研究者相信本文提出的几何推断方法可以很好地适应模型传感器范围和遮挡限制。后台回复【轨迹规划】获取论文原文。

什么是路径规划和轨迹规划？ 

车辆自主驾驶系统从本质上讲是一个智能控制机器，其研究内容大致可分为信息感知、行为决策及操纵控制三个子系统。 

路径规划是智能车辆导航和控制的基础，是从轨迹决策的角度考虑的，可分为局部路径规划和全局路径规划。全局路径规划的任务是根据全局地图数据库信息规划出自起始点至目标点的一条无碰撞、可通过的路径。由于全局路径规划所生成的路径只能是从起始点到目标点的粗略路径，并没有考虑路径的方向、宽度、曲率、道路交叉以及路障等细节信息，加之智能车辆在行驶过程中受局部环境和自身状态的不确定性的影响，会遇到各种不可测的情况。 

因此，在智能车辆的行驶过程中，必须以局部环境信息和自身状态信息为基础，规划出一段无碰撞的理想局部路径，这就是局部路径规划。通常路径规划的方法有:空间搜索法、层次法、动作行为法、势场域法、栅格法、模糊逻辑法和神经网络法等。 

汽车自动驾驶任务可以分为三层，如图所示，每层执行不同任务，包括上层路径规划，中层行驶行为规划和下层轨迹规划。

上层路径规划在已知电子地图、路网以及宏观交通信息等先验信息下，根据某优化目标得到两点之间的最优路径，完成路径规划的传感信息主要来自于GPS定位信息以及电子地图。 

中层行驶行为规划是指根据主车感兴趣区域内道路、交通车等环境信息，决策出当前时刻满足交通法规、结构化道路约束的最优行驶行为，动态规划的行驶行为序列组成宏观路径。行为规划的传感信息主要来自车载传感器如雷达、照相机等，用以识别道路障碍、车道线、道路标识信息和交通信号灯信息等。 

下层轨迹规划是指在当前时刻，以完成当前行车行为为目标，考虑周围交通环境并满足不同约束条件，根据最优目标动态规划决策出的最优轨迹。同时，车辆的动力学约束也会在下层得到体现，下层轨迹规划除了必要的外部环境信息外，还需要对主车状态信息进行测量或估计。 

车辆路径规划问题中的几个关键点：路网模型、路径规划算法和交通信息的智能预测，涉及的方面较多。本文主要探讨轨迹规划。 

用于道路场景的轨迹规划方法 

A概述

本文提出了一种基于A*算法的用于道路场景的轨迹规划方法，该方法中，使用了图形搜索近似，并且通过一种新的推理技术来考虑其他车辆的不确定预测，这种新的推理技术允许高效计算一组模拟场景类型的危险结果的概率。同时研究者还将这种轨迹规划方法与基于蒙特卡罗方法的POMDP求解器进行了比较，发现虽然POMDP求解器可以规划比该研究的方法更保守更复杂的行为，但是它遭受粒子剥夺，这可能会导致危险的行为。这是因为蒙特卡罗方法有时不会有足够多的样本用于罕见但危险的未来情况。相反，我们的方法使用了对未来信念状态的参数化描述，并且没有遭受这个问题的困扰，并且运行时间明显更长。 

图1:道路行驶功能系统架构

图1显示了用于自主道路操作的模块的高级视图。全局规划模块处理任务，为轨迹规划模块提供路线，这也需要定位、附近车道模型和描述附近物体状态和行为的模型。轨迹规划模块应该为自主车辆(称为ego车辆)找到合适的轨迹，以便遵循公式： 

其中xe是自我车辆的状态，Th是预测范围的长度。这是通过优化成本函数来实现的，其中高风险的轨迹具有高成本。通过分析由 i 指数的附近其他车辆的不确定预测来估计风险。在不确定性下的这种计划将在该方法中以固定间隔重复，而当前最佳计划由下级控制模块执行。 

这里，我们通过借用[13]中的符号和定义简要概述不确定条件下的轨迹规划问题。我们假设环境状态x =（xe，xi），i = 0，..，K在计划范围的离散时间步长中演变：

其中xik是车辆 i 在时刻tnow +kΔtp的状态，uk是对于自我车辆的控制动作，ωk是扰动，并且N = Th /Δtp其中Δtp是时间离散化。我们每次都会对环境进行嘈杂的测量：

其中νk模拟测量噪声。一般来说，我们不能直接观察xk，而是总结信息状态Ik中的所有可用信息。目标是为计划范围找到最优策略Π* = {π*0（I0），...，π*N-1（IN-1）}，给出我们的控制动作，在给定信息状态的情况下最小化未来预期成本，对于附加成本函数：

为了找到最优政策，我们计划在信念空间中，并对未来的测量做出不同的假设，以获得易处理的问题。在信念状态bk被定义为bk = p（xk | Ik）并且Ik是历史I状态的情况下：Ik =（I0，u0：k-1，y0：k-1）。这个问题可以重新表述为随机动态规划（SDP）问题。 

我们可以使用部分可观察的决策过程（POMDP）方法来逼近SDP问题的解决方案，或者我们可以以后退的方式解决一系列控制动作，忽略未来的测量。我们在本文中使用的后一种方法称为开环后退水平控制（OLRHC），这里信念状态是“对象的开环预测分布”[13]，反馈是通过重新规划实现的。 

如[5] [14]中所述，我们的轨迹规划算法描述了自主车辆的状态，在道路对齐的坐标系中，横坐标对应于沿参考路径的弧长s，纵坐标表示有符号的横向距离。路径d。我们假设我们可以精确地测量与参考路径（se0，de0）相关的当前状态，并且可以获得其他车辆的开环预测列表。设xik是惯性参考系中车辆 i 的姿态和速度，xik =（x，y，θ，v），xik是与参考路径 r 相关的状态，xik,r =（sik,r , dik,r , sik,r）和g（xik,r）= xik是在它们之间进行映射的函数。 对每个其他车辆的预测表示为高斯混合序列，其中每个高斯模型表示车辆状态相对于特定路线的分布：

其中αj是高斯prj（xik,r(j)）=N（μir(j),k ，Σir(j),k）的混合分量权重，描述了假设 j 的路径 r（j）的车辆 i 的状态分布。该表示允许我们表达单个车辆的多个运动假设，例如，我们模拟车辆是否将使用不同的αj值转向两个假设的不确定性，每个假设属于不同的路线。分布N（μir(j),k ，Σir(j),k）可以用不同的方法估计，例如[15]，或者诸如执行卡尔曼滤波器的预测步骤的简单方法。我们还假设我们知道每条车辆沿其路线的纵向范围。 

轨迹规划算法可以访问道路模型，在该道路模型中，轨道规划算法可以快速确定两条路线是否相邻，例如，检查车辆是否在我们的左侧或右侧，如果一辆车与另一辆车合并或者路线是否交叉。本文描述风险评估方法主要基于两条车辆在路线上占据相同纵向位置的距离和车辆的相对速度。 

B图形搜索近似 

决定什么时候改变车道，或者当驾驶到两个车道合并的道路的一部分时是一个非凸的问题，特别是我们经常可以选择在另一辆车前面或后面，每个车辆都有一个局部最佳值选择。 通过允许为我们的解决方案用一组采样状态值，我们将轨迹优化转换为图搜索问题：从有限集中找出状态值的序列，其具有最低成本，满足我们的约束并达到状态预测范围的结束。我们通过对和时间进行采样来构造一个格子超过允许状态，并定义模拟我们的控制动作的运动原语u从时间k的状态（s，d，v）转换到时间k的另一个状态 k+1，k后的Δtp秒，从而形成可能的状态序列的图形表示。我们使用了一个简化的模型来说明车辆在道路对齐坐标系中的移动方式： 

其中运动原语由不同的预定义加速度和横向速度组成。速度v被限制为在上面被vmax限制为非负，并且d被缩放到1和1之间，其中1是参考路径右边的一个车道宽度和左边一个车道宽度。允许速度的集合是从v = 0开始可达到的任何速度，其是非负的并且在规划步骤的整个时间期间使用A中的任何加速度小于vmax，假设步骤之间的加速度瞬时变化。允许纵向位置的集合是时间步长k = 0，...，N 从 s = 0 开始以初始速度进入并且使用A中的加速度行进的距离的集合。在我们的简化模型中，我们允许加速度的瞬时变化这个误差假设最多是dmin的纵向位置误差，它是从执行模块转换为非完整运动的距离中减去的。 

对于我们在A *算法中的启发式，我们使用进入结束状态（k = N）的最优成本，假设如果dk不在车道的中心并且在时间步长k，k + 1之间将发生横向移动，并且没有其他车辆。 这些成本是针对每个s，v，k组合离线计算的并存储在查找表中。 这低估了达到目标状态的成本，因为cl和cd总是大于或等于零，启发式是可以接受的。 

总结

自动驾驶车辆的轨迹规划应试图在传感器数据噪声和不确定预测的情况下最小化预期风险。本文提出了一种基于图搜索近似的道路场景轨迹规划方法。其他车辆的不确定预测是由一种新的推理技术来解释的，这种推理技术可以有效地计算建模情况类型集的危险结果的概率。对于高速公路合并场景，这里使用简单的恒速预测，研究者并不考虑传感器范围和遮挡。未来的工作将考虑更复杂的预测模型，研究者相信本文提出的几何推断方法可以很好地适应模型传感器范围和遮挡限制。例如，为了对超出传感器范围的可能车辆所构成的风险进行建模，研究者可以在给定未知车辆速度和位置的统一概率的情况下，整合约束违反概率。 

参考资料 

[1] D. Gonz´alez, J. P´erez, V. Milan´es, and F. Nashashibi, “A review of motion planning techniques for automated vehicles,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 4, pp. 1135–1145, 2016.

[2] C. Hubmann, M. Aeberhard, and C. Stiller, “A generic driving strategy for urban environments,” in Intelligent Transportation Systems (ITSC), 2016 IEEE 19th International Conference on. IEEE, 2016, pp. 1010–1016.

[3] J. Ziegler, P. Bender, T. Dang, and C. Stiller, “Trajectory planning for berthaa local, continuous method,” in Intelligent Vehicles Symposium Proceedings, 2014 IEEE. IEEE, 2014, pp. 450–457.

[4] J. Nilsson, J. Silvlin, M. Brannstrom, E. Coelingh, and J. Fredriksson,“If, when, and how to perform lane change maneuvers on highways,” IEEE Intelligent Transportation Systems Magazine, vol. 8, no. 4, pp. 68–78, 2016.

[5] M. McNaughton, C. Urmson, J. M. Dolan, and J.-W. Lee, “Motion planning for autonomous driving with a conformal spatiotemporal lattice,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011, pp. 4889–4895.

[6] M. Bahram, A. Wolf, M. Aeberhard, and D. Wollherr, “A predictionbased reactive driving strategy for highly automated driving function on freeways,” in Intelligent Vehicles Symposium Proceedings, 2014 IEEE. IEEE, 2014, pp. 400–406.

[7] S. Ulbrich and M. Maurer, “Towards tactical lane change behavior planning for automated vehicles,” in 2015 IEEE 18th International Conference on Intelligent Transportation Systems. IEEE, 2015, pp.989–995.

[8] M. Althoff, O. Stursberg, and M. Buss, “Model-based probabilistic collision detection in autonomous driving,” IEEE Transactions on Intelligent Transportation Systems, vol. 10, no. 2, pp. 299–310, 2009.

[9] W. Zhan, J. Chen, C.-Y. Chan, C. Liu, and M. Tomizuka, “Spatiallypartitioned environmental representation and planning architecture for on-road autonomous driving,” in Intelligent Vehicles Symposium (IV), 2017 IEEE. IEEE, 2017, pp. 632–639.

[10] C. Hubmann, M. Becker, D. Althoff, D. Lenz, and C. Stiller, “Decision making for autonomous driving considering interaction and uncertain prediction of surrounding vehicles,” in Intelligent Vehicles Symposium (IV), 2017 IEEE. IEEE, 2017, pp. 1671–1678.

[11] W. Liu, S.-W. Kim, S. Pendleton, and M. H. Ang, “Situation-aware decision making for autonomous driving on urban road using online pomdp,” in Intelligent Vehicles Symposium (IV), 2015 IEEE. IEEE, 2015, pp. 1126–1133.

[12] D. Klimenko, J. Song, and H. Kurniawati, “Tapir: A software toolkit for approximating and adapting pomdp solutions online,” in Proceedings of the Australasian Conference on Robotics and Automation, Melbourne, Australia, vol. 24, 2014.

[13] N. E. Du Toit and J. W. Burdick, “Robot motion planning in dynamic, uncertain environments,” IEEE Transactions on Robotics, vol. 28, no. 1, pp. 101–115, 2012.

[14] M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenarios in a frenet frame,” in Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010, pp. 987–993.

[15] Q. Tran and J. Firl, “online maneuver recognition and multimodal trajectory prediction for intersection assistance using non-parametric regression,” in Intelligent Vehicles Symposium Proceedings, 2014 IEEE. IEEE, 2014, pp. 918–923.

[16] S. Ulbrich and M. Maurer, “Situation assessment in lane change behavior planning for automated vehicles,” in Proceedings of the 18th International IEEE Annual Conference on Intelligent Transportation Systems (ITSC 2015), 2015.

[17] M. Althoff and R. L¨osch, “Can automated road vehicles harmonize with traffic flow while guaranteeing a safe distance?” in Intelligent Transportation Systems (ITSC), 2016 IEEE 19th International Conference on. IEEE, 2016, pp. 485–491.

[18] O. Amidi and C. E. Thorpe, “Integrated mobile robot control,” in Fibers’ 91, Boston, MA. International Society for Optics and Photonics, 1991, pp. 504–523.

[19] E. Ward, N. Evestedt, D. Axehill, and J. Folkesson, “Probabilistic model for interaction aware planning in merge scenarios,” IEEE Transactions on Intelligent Vehicles, vol. 2, no. 2, pp. 133–146, 2017.

[20] P. F. Lima, M. Trincavelli, J. M°artensson, and B. Wahlberg,“Clothoidbased speed profiler and control for autonomous driving,” in Intelligent Transportation Systems (ITSC), 2015 IEEE 18th International Conference on. IEEE, 2015, pp. 2194–2199.

作者：Erik Ward, John Folkesson

The research leading to these results has been carried out within the iQMatic project funded by FFI/VINNOVA.

1Robotics Perception and Learning Lab, KTH, Stockholm, Sweden

(e-mail: eward@kth.se, johnf@kth.se)