Stratified Sampling Based Multi-Dynamic Window Trajectory Planner for Autonomous Driving on Highway
-
摘要: 高速公路无人驾驶轨迹规划面临着实时性强、安全性高的挑战. 本文提出了一种分层抽样多动态窗口的轨迹规划算法(Stratied sampling based multi-dynamic window trajectory planner, SMWTP). 首先, 用多动态窗口表征可行轨迹的搜索空间, 并基于贝叶斯网络构建了车辆轨迹分布模型. 其次, 采用先速度后路径的分层抽样策略生成符合动态场景约束的候选轨迹集合. 最后, 利用引入障碍车辆速度估计不确定性的责任敏感安全模型(Responsibility sensitive safety, RSS)从中选择最优轨迹. 大量仿真实验和实际交通场景测试验证了算法的有效性, 对比实验结果表明所提算法性能显著优于人工势场最优轨迹规划算法和多动态窗口模拟退火轨迹规划算法.Abstract: This paper proposes a stratified sampling based multi-dynamic window trajectory planner for unmanned vehicles on highway. Firstly, the search space of feasible trajectories is constructed with multi-dynamic windows. Then, the Bayesian network is used to derive the probability distribution of human-like driving trajectories. Besides, the stratified sampling strategy where speed is sampled before path makes generated candidate trajectories meet the constraints in dynamic scenes. Finally, the uncertainty of traffic participant vehicles' speed estimation is embedded into Responsibility Sensitive Safety model to select the optimal trajectory. A large number of simulation experiments and real traffic scenario tests have verified the effectiveness of the algorithm. Its performance is significantly better than the optimal trajectory planning algorithm based on artificial potential fields and simulated annealing-optimized trajectory planning algorithm.
-
Key words:
- Autonomous driving /
- trajectory planning /
- motion planning /
- Bayesian network
-
图 13 2017年IVFC无人车行驶中一段航拍视频. 图中红色圆圈中心的机动车为无人车, (1)(2)(10)(11)为跟车行驶,(3)−(9) 为向右换道, (12)−(18)为向左换道
Fig. 13 A continuous aerial view of unmanned vehicles driven in IVFC in 2017. The motor vehicle in the center of the red circle is the unmanned vehicle, (1)(2)(10)(11) show car-following, (3)−(9) show lane-right, (12)−(18) show lane-left
表 1 SMWTP参数设置
Table 1 Parameters of SMWTP
$ k $ $\sigma_{v} $ ${\rm{m/s}}$ $\Delta {v}_{\mathrm{thr}} $ ${\rm{m/s}}$ $\omega _{\mathrm{yawr}} $ $\omega _{\mathrm{safe}} $ $ \omega _{\mathrm{acc}} $ $ \omega _{s1} $ $ \omega _{s2} $ 1.5 2 5 20 5 3 1 0.5 表 2 TP-ATP参数设置
Table 2 Parameters of TP-ATP
$\omega _{\mathrm{s}} $ $\omega _{\mathrm{d}} $ $ \omega _{\mathrm{c}} $ $ \omega _{\mathrm{p}} $ $ c _{\mathrm{j},\mathrm{s}} $ $ c _{\mathrm{v},\mathrm{s}} $ 5 5 0.5 0.005 1 0.2 $ c _{T,\mathrm{s}} $ $ c _{\mathrm{j},\mathrm{d}} $ $ c _{T,\mathrm{d}} $ $ D_0 $ $\tau$ 0.1 1.5 0.1 10 4 表 3 不同障碍车辆速度估计误差下的规划结果
Table 3 Planning results with different uncertainty in speed estimation of obstacle vehicles
$v_{\mathrm{g}} $ ${\rm{m/s}}$ $s_{\mathrm{g}} $ $m$ $d_{\mathrm{g}} $ $s$ $T $ $m$ $v_{lim} $ ${\rm{m/s}}$ 决策 安全概率% $\sigma_m = 0.5 \; {\rm{m/s}}$ 22.5 167.3 5.6 7.5 25 LC 91.1 $\sigma_m = 1 \;{\rm{ m/s}}$ 20.5 105 1.85 5.1 21 LK 95.9 表 4 2018-2019年IVFC比赛中SMWTP规划情况概览
Table 4 An overview of SMWTP's performance in IVFC in year 2018-2019
年 行驶时长$ {\rm{min}} $ 平均速度${\rm{ m/s}} $ 平均安全
概率%最低安全
概率%平均耗时$ {\rm{ms}} $ 2018 20 13.8 91.3 80 35.1 2019 30 13.2 93.6 80 33.5 表 5 虚线车道线下的纵向安全避让规划结果对比
Table 5 Comparison of planning results for longitudinal risk avoidance with dashed lane marking
场景一 $v_{\mathrm{g}} $ ${\rm{m/s}}$ $s_{\mathrm{g}} $ ${\rm{m}}$ $d_{\mathrm{g}} $ ${\rm{m}}$ $T $ ${\rm{s}}$ $v_{lim} $ ${\rm{m/s}}$ 决策 TP-ATP 20 111.4 1.85 5.4 20 LK SMWTP 19.5 160 5.6 8 25 LC 表 6 实线车道线下的纵向安全避让规划结果对比
Table 6 Comparison of planning results for longitudinal risk avoidance with solid lane marking
场景二 $v_{\mathrm{g}} $ ${\rm{m/s}}$ $s_{\mathrm{g}} $ ${\rm{m}}$ $d_{\mathrm{g}} $ ${\rm{m}}$ $T $ ${\rm{s}}$ $v_{\mathrm{lim}} $ ${\rm{ m/s}}$ 决策 TP-ATP 20 107 1.85 5.2 20 LK SMWTP 18 113 1.85 5.6 20 LK 表 7 横向安全避让规划结果对比
Table 7 Comparison of planning results for lateral risk avoidance
场景三 $v_{\mathrm{g}} $ ${\rm{m/s}}$ $s_{\mathrm{g}} $ ${\rm{m }}$ $d_{\mathrm{g}} $ ${\rm{m}}$ $T $ ${\rm{s}}$ $v_{\mathrm{lim}} $ ${\rm{m/s}}$ 决策 TP-ATP 20 80 1.85 4.1 20 LK SMWTP 19.5 103 1.3 5.3 20 LK 表 8 动态交通流中TP-ATP多帧规划结果
Table 8 Performance of TP-ATP in dynamic traffic flow
TP-ATP $v_{\mathrm{g}} $ ${\rm{m/s}}$ $s_{\mathrm{g}} $ ${\rm{m}}$ $d_{\mathrm{g}} $ ${\rm{m}}$ $T $ $s$ $v_{\mathrm{lim}} $ ${\rm{m/s}}$ 决策 $ t=0 \; {\rm{s}} $ 21 90 1.85 4.0 21 LK $ t=12.5 \; {\rm{s}} $ 25 159 5.6 6.9 25 LC 表 9 动态交通流中SMWTP规划结果
Table 9 Performance of SMWTP in dynamic traffic flow
SMWTP $v_{\mathrm{g}} $ ${\rm{m/s}}$ $s_{\mathrm{g}} $ ${\rm{m}}$ $d_{\mathrm{g}} $ ${\rm{m}}$ $T $ $s$ $v_{\mathrm{lim}} $ ${\rm{m/s}}$ 决策 $ t=0 \; {\rm{s }}$ 22.8 156 5.6 6.7 25 LC $ t=4.5 \; {\rm{s}} $ 26.1 172 5.6 7 25 LK $ t=24.5 \; {\rm{s}} $ 25.1 169.4 1.85 6.8 33.3 LC 表 10 SMWTP与SA-TP规划结果对比
Table 10 Comparison of SMWTP and SA-TP's planning results
测试场景 $v_{\mathrm{g}} $ ${\rm{m/s}}$ $ \sigma_{\mathrm{g}} $ ${\rm{m/s}}$ $s_{\mathrm{g}} $ ${\rm{m}}$ $d_{\mathrm{g}} $ ${\rm{m}}$ $ T $ ${\rm{s}}$ 安全概率/% 决策 SA-TP 23.4 2.47 136 5.6 5.2 100 LK SMWTP 25 0.19 150 5.6 5.7 100 LK 表 11 SMWTP与SA-TP实时性比较
Table 11 Comparison of SMWTP and SA-TP's real-time performance
算法耗时 平均耗时${\rm{ms}}$ 标准差${\rm{ms}}$ 最大耗时${\rm{ms}}$ 最小耗时${\rm{ms}}$ SA-TP 72 10 99 61 SMWTP 34 2 49 31 -
[1] Claussmann L, Revilloud M, Gruyer D, Glaser S. A review of motion planning for highway autonomous driving. IEEE Transactions on Intelligent Transportation Systems, 2020, 21(5):1826-1848 doi: 10.1109/TITS.2019.2913998 [2] Rasekhipour Y, Khajepour A, Chen S, Litkouhi B. A potential field-based model predictive path-planning controller for autonomous road vehicles. IEEE Transactions on Intelligent Transportation Systems, 2017, 18(5): 1255-1267 doi: 10.1109/TITS.2016.2604240 [3] Kim D, Kim H, Huh K. Trajectory Planning for Autonomous Highway Driving Using the Adaptive Potential Field. Proceedings of IEEE International Conference on Intelligent Transportation Systems. Maui, HI, USA: IEEE, 2018. 1069−1074 [4] Wolf M, Burdick J W. Artificial potential functions for highway driving with collision avoidance. Proceedings of IEEE International Conference on Robotics & Automation.California, USA: IEEE, 2008. 3731–3736 [5] Claussmann L, Revilloud M and Glaser S. Simulated Annealing-optimized Trajectory Planning within Non-Collision Nominal Intervals for Highway Autonomous Driving. Proceedings of International Conference on Robotics and Automation. Montreal, QC, Canada: IEEE, 2019. 5922−5928 [6] Paden B, Cap M, Yong S Z, Yershov D, Frazzoli E. A Survey of Motion Planning and Control Techniques for Self-Driving Urban Vehicles. IEEE Transactions on Intelligent Vehicles, 2016, 1(1): 33-55 doi: 10.1109/TIV.2016.2578706 [7] Claussmann L, Revilloud M, Glaser S, Gruyer D. A study on AI-based approaches for high-level decision making in highway autonomous driving. Proceedings of IEEE International Conference on Systems, Man, and Cybernetics (SMC). Banff, Canada: IEEE, 2017. 3671−3676 [8] Werling M, Ziegler J, Kammel S, Thrun S. Optimal trajectory generation for dynamic street scenarios in a Frenét Frame. Proceedings of IEEE International Conference on Robotics and Automation. Anchorage, AK: IEEE, 2010. 987−993 [9] 苏锑, 杨明, 王春香, 唐卫, 王冰. 一种基于分类回归树的无人车汇流决策方法[J]. 自动化学报, 2018, 44(1): 35-43Su Ti, Yang Ming, Wang Chun-xiang, Tang Wei, Wang Bing. Classification and regression tree based traffic merging for method self-driving vehicles. Acta Automatica Sinica, 2018, 44(1): 35-43 [10] Ziegler J, Stiller C. Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios. Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems. St Louis MO, USA: IEEE, 2009. 1879−1884 [11] Mcnaughton M, Urmson C, Dolan J M, Lee J. Motion planning for autonomous driving with a conformal spatiotemporal lattice. Proceedings of IEEE International Conference on Robotics and Automation. ShangHai, China: IEEE, 2011. 4889−4895 [12] 袁静妮, 杨林, 唐晓峰, 陈傲文. 基于改进RRT*与行驶轨迹优化的智能汽车运动规划. 自动化学报, 2020, 46(x):1-10Yuan Jing-Ni, Yang Lin, Tang Xiao-Fen, Chen Ao-Wen, Autonomous vehicle motion planning based on improved RRT* algorithm and trajectory optimization. Acta Automatica Sinica, 2020, 46(x):1-10 [13] Yue M, Hou X, Zhao X and Wu X. Robust Tube-Based Model Predictive Control for Lane Change Maneuver of Tractor-Trailer Vehicles Based on a Polynomial Trajectory. IEEE Transactions on Systems, 2020, 50(12): 5180-5188 [14] Zhou Y, Cholette M E, Bhaskar A and Chung E. Optimal Vehicle Trajectory Planning With Control Constraints and Recursive Implementation for Automated On-Ramp Merging. IEEE Transactions on Intelligent Transportation Systems, 2019, 20(9): 3409-3420 doi: 10.1109/TITS.2018.2874234 [15] Liu C, Lee S, Varnhagen S, Tseng H E. Path planning for autonomous vehicles using model predictive control. Proceedings of IEEE Intelligent Vehicles Symposium. Los Angeles, USA, 2017. 174–179 [16] Plessen M, Lima P, Martensson J, Bemporad A, Wahlberg B. Trajectory planning under vehicle dimension constraints using sequential linear programming. Proceedings of IEEE International Conference on Intelligent Transportation Systems. Yokohama, Japan: IEEE, 2017. 1–6 [17] Werling M, Kammel S, Ziegler J, Gröll L. Optimal Trajectories for Time-Critical Street Scenarios Using Discretized Terminal Manifolds. The International Journal of Robotics Research, 2012, 31(3): 346–359 doi: 10.1177/0278364911423042 [18] Zhan W, Chen J, Chan C, Liu C and Tomizuka M. Spatially-partitioned environmental representation and planning architecture for on-road autonomous driving. Proceedings of IEEE Intelligent Vehicles Symposium, Los Angeles, USA: IEEE, 2017. 632−639 [19] Kant K, Zucker S W. Toward Efficient Trajectory Planning: The Path-Velocity Decomposition. The International Journal of Robotics Research, 1986, 5(3): 72-89 doi: 10.1177/027836498600500304 [20] Gu T Y, Atwood J, Dong C Y, Dolan J M, Lee J W. Tunable and stable real-time trajectory planning for urban autonomous driving. In: Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems. Hamburg, Germany: IEEE, 2015. 250−256 [21] González D, Milanés V, Pérez J and Nashashibi F. Speed profile generation based on quintic Bézier curves for enhanced passenger comfort. Proceedings of IEEE International Conference on Intelligent Transportation Systems. Rio de Janeiro, Brazil : IEEE, 2016: 814−819 [22] Lima P F, Trincavelli M, Martensson J, Wahlberg B. Clothoid-Based Speed Profiler and Control for Autonomous Driving. Proceedings of IEEE International Conference on Intelligent Transportation Systems. Las Palmas, Spain: IEEE, 2015. 2194−2199 [23] Liu C, Zhan W and Tomizuka M. Speed profile planning in dynamic environments via temporal optimization. Proceedings of IEEE Intelligent Vehicles Symposium. Los Angeles, USA: IEEE, 2017: 154−159 [24] Wang Y, Chardonnet J and Merienne F. Speed Profile Optimization for Enhanced Passenger Comfort: An Optimal Control Approach. Proceedings of IEEE International Conference on Intelligent Transportation Systems (ITSC). Maui, USA: IEEE, 2018: 723−728 [25] Xu W D, Wei J Q, Dolan J M, Zhao H J, Zha H B. A real-time motion planner with trajectory optimization for autonomous vehicles. Proceedings of IEEE International Conference on Robotics and Automation. MN, USA: IEEE, 2012. 2061−2067 [26] Fan H Y, Zhu F, Liu C, Zhang L, Zhuang L, et al. Baidu Apollo EM Motion Planner.arXiv: Robotics, 2018 [27] Zheng Z. Recent developments and research needs in modeling lane changing. Transportation Research Part B: Methodological, 2014, 60, 16-32 doi: 10.1016/j.trb.2013.11.009 [28] 聂建强, 高速公路车辆自主性换道行为建模研究. 东南大学, 中国, 2017.Xie J. Research on modelling discretionary lane-changing behaviore of vehicles in freeway[Ph. D. dissertation]. Southeast University, China, 2017 [29] Shalev-Shwartz S, Shammah S, Shashua A. On a Formal Model of Safe and Scalable Self-driving Cars. arXiv: Robotics, 2017. [30] 符锌砂, 胡嘉诚, 何石坚. 基于交通状况及行驶速度的高速公路换道时间研究[J]. 公路交通科技, 2020, 37(04): 133-139Fu Xin-Sha, Hu Jia-Cheng, He Shi-Jian. Study on Expressway Lane-changing Time Based on Traffic Condition and Driving Speed. Journal of Highway and Transportation Research and Development. 2020, 37(04): 133-139 [31] Yang D, Zhu L, Ran B, Pu Y, Hui P. Modeling and Analysis of the Lane-Changing Execution in Longitudinal Direction. IEEE Transactions on Intelligent Transportation Systems, 2016, 17(10): 2984-2992 doi: 10.1109/TITS.2016.2542109 [32] Toledo T, Zohar D. Modeling Duration of Lane Changes.Transportation Research Record, 2007, 10.3141/1999−08 [33] K. Kawabata, L. Ma, J. Xue and N. Zheng. A path generation method for automated vehicles based on Bezier curve. Proceedings of IEEE/ASME International Conference on Advanced Intelligent Mechatronics. NSW, Australia: IEEE, 2013. 991−996 [34] Ziegler J, Bender P, Dang T, Stiller C. Trajectory planning for Bertha — A local, continuous method. Proceedings of IEEE Intelligent Vehicles Symposium. Michigan, USA: IEEE, 2014. 450−457 [35] Li, Li and Wang, Xiao and Wang et al. Parallel testing of vehicle intelligence via virtual-real interaction. Science Robotics. 2019, 10.1126/scirobotics.aaw4106 [36] F. -Y. Wang et al. China's 12-Year Quest of Autonomous Vehicular Intelligence: The Intelligent Vehicles Future Challenge Program. IEEE Intelligent Transportation Systems Magazine 2021. 6−19
计量
- 文章访问数: 561
- HTML全文浏览量: 481
- 被引次数: 0