Stratified Sampling Based Multi-dynamic Window Trajectory Planner for Autonomous Driving on Highway
-
摘要: 高速公路无人驾驶轨迹规划面临着实时性强、安全性高的挑战. 为此, 提出一种分层抽样多动态窗口的轨迹规划算法(Stratified sampling based multi-dynamic window trajectory planner, SMWTP). 首先, 用多动态窗口表征可行轨迹的搜索空间, 并基于贝叶斯网络构建轨迹概率分布模型. 其次, 采用先速度后路径的分层抽样策略生成符合动态场景约束的候选轨迹集合. 最后, 利用引入障碍车辆速度估计不确定性的责任敏感安全模型(Responsibility sensitive safety, RSS)从中选择最优轨迹. 大量仿真实验和实际交通场景测试验证了算法的有效性, 对比实验结果表明, 所提算法性能显著优于人工势场最优轨迹规划算法和多动态窗口模拟退火轨迹规划算法.Abstract: Autonomous driving trajectory planning on highways faces challenges of strong real-time performance and safety. This paper proposes a stratified sampling based multi-dynamic window trajectory planner (SMWTP) 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 model of trajectories. Secondly, 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 (RSS) model to select the optimal trajectory. A large number of simulation experiments and real traffic scenario tests have verified the effectiveness of the algorithm. The comparative experimental results show that the performance of the proposed algorithm is significantly better than the optimal trajectory planning algorithm based on artificial potential fields and multi-dynamic window simulated annealing-optimized trajectory planning algorithm.
-
Key words:
- Autonomous driving /
- trajectory planning /
- motion planning /
- Bayesian network
-
图 5 旁道动态窗口内期望速度的概率密度分布
Fig. 5 Probabilistic density distribution of desired speed in side lane＇s dynamic window
图 6 当前道动态窗口内期望速度的概率密度分布
Fig. 6 Probabilistic density distribution of desired speed in current lane＇s dynamic window
图 7 无人车相对于车辆$ c_i $的纵向安全概率
Fig. 7 The longitudinal safety probability of ego vehicle with respect to vehicle $ c_i $
图 10 不考虑障碍车辆速度估计不确定性时轨迹代价与生成概率之间的关系
Fig. 10 The relationship between the trajectory cost and the generation probability when the uncertainty of the speed estimation of the obstacle is not considered
图 11 考虑障碍车辆速度估计不确定性时轨迹代价与生成概率之间的关系
Fig. 11 The relationship between the trajectory cost and the generation probability when the uncertainty of the speed estimation of the obstacle is considered
图 12 障碍车辆速度估计不确定性对轨迹规划的影响(红色方框所示轨迹为规划轨迹)
Fig. 12 Impact of uncertainty in speed estimation of obstacle vehicles on trajectory planning (The trajectory shown in the red box is the planned trajectory)
图 13 2017年IVFC无人车行驶中一段航拍视频(红色圆圈中心的机动车为无人车, (a), (b), (j), (k)为跟车行驶,(c) ~ (i) 为向右换道, (l) ~ (r)为向左换道)
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, (a), (b), (j), (k) show car-following, (c) ~ (i) show lane-right, (l) ~ (r) show lane-left)
图 14 2018年IVFC中SMWTP规划结果示例(橙色矩形为无人车, 蓝色曲线为规划轨迹)
Fig. 14 Performance of SMWTP planning results in IVFC in 2018 (The orange rectangle represents ego vehicle, and the blue curve is the trajectory planned by SMWTP)
图 16 2019年IVFC中SMWTP规划结果示例(橙色矩形为无人车, 蓝色曲线为规划轨迹)
Fig. 16 Performance of SMWTP planning results in IVFC in 2019 (The orange rectangle represents ego vehicle, and the blue curve is the trajectory planned by SMWTP)
图 18 SMWTP规划重型牵引车换道轨迹示例(橙色矩形为无人车, 蓝色曲线为规划轨迹)
Fig. 18 Lane-change trajectories for heavy tractor planned by SMWTP (The orange rectangle represents ego vehicle, and the blue curve is the planned trajectory)
图 23 动态交通流中TP-ATP规划结果
Fig. 23 Performance of TP-ATP planning results in dynamic traffic flow
图 24 动态交通流中SMWTP规划结果
Fig. 24 Performance of SMWTP planning results in dynamic traffic flow
表 1 SMWTP参数设置
Table 1 Parameters of SMWTP
参数名称 参数值 $ k $ 1.5 $\sigma_{v}\;({\rm{m/s} })$ 2 $\Delta {v}_{\mathrm{thr} }\;({\rm{m/s} })$ 5 $\omega _{\mathrm{yawr}} $ 20 $\omega _{\mathrm{safe}} $ 5 $ \omega _{\mathrm{acc}} $ 3 $ \omega _{s1} $ 1 $ \omega _{s2} $ 0.5 
下载: 导出CSV
表 2 TP-ATP参数设置
Table 2 Parameters of TP-ATP
参数名称 参数值 $\omega _{\mathrm{s}} $ 5 $\omega _{\mathrm{d}} $ 5 $ \omega _{\mathrm{c}} $ 0.5 $ \omega _{\mathrm{p}} $ 0.005 $ c _{\mathrm{j},\mathrm{s}} $ 1 $ c _{\mathrm{v},\mathrm{s}} $ 0.2 $ c _{T,\mathrm{s}} $ 0.1 $ c _{\mathrm{j},\mathrm{d}} $ 1.5 $ c _{T,\mathrm{d}} $ 0.1 $ D_0 $ 10 $\tau$ 4
下载: 导出CSV
表 3 不同障碍车辆速度估计误差下的规划结果
Table 3 Planning results with different errors in speed estimation of obstacle vehicles
$\sigma_{ {\rm{m} } }\; ({\rm{m/s} })$ $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g} }\;({\rm{m} })$ $T\;({\rm{s} })$ $v_{ {\rm{lim} } }\;({\rm{m/s} })$ 决策 安全
概率
(%)$0.5$ 22.5 167.3 5.60 7.5 25 LC 91.1 $1.0$ 20.5 105.0 1.85 5.1 21 LK 95.9
下载: 导出CSV
表 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
下载: 导出CSV
表 5 虚线车道线下的纵向安全避让规划结果对比
Table 5 Comparison of planning results for longitudinal safety avoidance with dashed lane
场景1 $v_{\mathrm{g} }\;({\rm{m/s} })$ $s_{\mathrm{g} }\;({\rm{m} })$ $d_{\mathrm{g}} \;({\rm{m} })$ $T\;({\rm{s} })$ $v_{ {\rm{lim} } }\;({\rm{m/s} })$ 决策 TP-ATP 20.0 111.4 1.85 5.4 20 LK SMWTP 19.5 160.0 5.60 8.0 25 LC
下载: 导出CSV
表 6 实线车道线下的纵向安全避让规划结果对比
Table 6 Comparison of planning results for longitudinal safety avoidance with solid lane markings
场景2 $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
下载: 导出CSV
表 7 横向安全避让规划结果对比
Table 7 Comparison of planning results for lateral safety avoidance
场景3 $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.0 80 1.85 4.1 20 LK SMWTP 19.5 103 1.30 5.3 20 LK
下载: 导出CSV
表 8 动态交通流中TP-ATP多帧规划结果
Table 8 Performance of TP-ATP multi-frame planning results in dynamic traffic flow
t (s) $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} })$ 决策 $0$ 21 90 1.85 4.0 21 LK $12.5$ 25 159 5.60 6.9 25 LC
下载: 导出CSV
表 9 动态交通流中SMWTP多帧规划结果
Table 9 Performance of SMWTP multi-frame planning results in dynamic traffic flow
t (s) $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} })$ 决策 $0$ 22.8 156.0 5.60 6.7 25.0 LC $4.5$ 26.1 172.0 5.60 7.0 25.0 LK $24.5$ 25.1 169.4 1.85 6.8 33.3 LC
下载: 导出CSV
表 10 SMWTP与SA-TP规划结果对比
Table 10 Comparison of SMWTP and SA-TP 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 0.19 150 5.6 5.7 100% LK
下载: 导出CSV
表 11 SMWTP与SA-TP实时性比较
Table 11 Comparison of SMWTP and SA-TP real-time performance
测试场景 平均耗时$({\rm{ms} })$ 标准差$({\rm{ms} })$ 最大耗时$({\rm{ms} })$ 最小耗时$({\rm{ms} })$ SA-TP 72 10 99 61 SMWTP 34 2 49 31
下载: 导出CSV
-
[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 K, 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. In: Proceedings of the 21st International Conference on Intelligent Transportation Systems (ITSC). Maui, USA: IEEE, 2018. 1069−1074 [4] Wolf M T, Burdick J W. Artificial potential functions for highway driving with collision avoidance. In: Proceedings of the IEEE International Conference on Robotics and Automation. Pasadena, USA: IEEE, 2008. 3731−3736 [5] Claussmann L, Revilloud M, Glaser S. Simulated annealing-optimized trajectory planning within non-collision nominal intervals for highway autonomous driving. In: Proceedings of the International Conference on Robotics and Automation (ICRA). Montreal, Canada: IEEE, 2019. 5922−5928 [6] Paden B, Čáp 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 al-based approaches for high-level decision making in highway autonomous driving. In: Proceedings of the 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. In: Proceedings of the IEEE International Conference on Robotics and Automation. Anchorage, USA: IEEE, 2010. 987−993 [9] 苏锑, 杨明, 王春香, 唐卫, 王冰. 一种基于分类回归树的无人车汇流决策方法. 自动化学报, 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. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems. St. Louis, USA: IEEE, 2009. 1879−1884 [11] McNaughton M, Urmson C, Dolan J M, Lee J W. Motion planning for autonomous driving with a conformal spatiotemporal lattice. In: Proceedings of the IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE, 2011. 4889−4895 [12] 袁静妮, 杨林, 唐晓峰, 陈傲文. 基于改进RRT* 与行驶轨迹优化的智能汽车运动规划. 自动化学报, 2022, 48(12): 2941−2950Yuan Jing-Ni, Yang Lin, Tang Xiao-Feng, Chen Ao-Wen. Autonomous vehicle motion planning based on improved RRT* algorithm and trajectory optimization. Acta Automatica Sinica, 2022, 48(12): 2941−2950 [13] Yue M, Hou X Q, Zhao X D, Wu X M. Robust tube-based model predictive control for lane change maneuver of tractor-trailer vehicles based on a polynomial trajectory. IEEE Transactions on Systems, Man, and Cybernetics: Systems, 2020, 50(12): 5180−5188 doi: 10.1109/TSMC.2018.2867807 [14] Zhou Y, Cholette M E, Bhaskar A, 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. In: Proceedings of the IEEE Intelligent Vehicles Symposium (IV). Los Angeles, USA: IEEE, 2017. 174−179 [16] Plessen M G, Lima P F, Mårtensson J, Bemporad A, Wahlberg B. Trajectory planning under vehicle dimension constraints using sequential linear programming. In: Proceedings of the IEEE 20th International Conference on Intelligent Transportation Systems (ITSC). 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 Y, Chan C Y, Liu C L, Tomizuka M. Spatially-partitioned environmental representation and planning architecture for on-road autonomous driving. In: Proceedings of the IEEE Intelligent Vehicles Symposium (IV). 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 the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Hamburg, Germany: IEEE, 2015. 250−256 [21] González D, Milanés V, Pérez J, Nashashibi F. Speed profile generation based on quintic Bézier curves for enhanced passenger comfort. In: Proceedings of the 19th IEEE International Conference on Intelligent Transportation Systems (ITSC). Rio de Janeiro, Brazil: IEEE, 2016. 814−819 [22] Lima P F, Trincavelli M, Mårtensson J, Wahlberg B. Clothoid-based speed profiler and control for autonomous driving. In: Proceedings of the 18th IEEE International Conference on Intelligent Transportation Systems. Gran Canaria, Spain: IEEE, 2015. 2194−2199 [23] Liu C L, Zhan W, Tomizuka M. Speed profile planning in dynamic environments via temporal optimization. In: Proceedings of the IEEE Intelligent Vehicles Symposium (IV). Los Angeles, USA: IEEE, 2017. 154−159 [24] Wang Y Y, Chardonnet J R, Merienne F. Speed profile optimization for enhanced passenger comfort: An optimal control approach. In: Proceedings of the 21st 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. In: Proceedings of the IEEE International Conference on Robotics and Automation. Saint Paul, USA: IEEE, 2012. 2061−2067 [26] Fan H Y, Zhu F, Liu C C, Zhang L L, Zhuang L, Li D, et al. Baidu apollo EM motion planner. arXiv preprint arXiv: 1904.04671, 2019. [27] Zheng Z D. 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.Nie Jian-Qiang. Research on Modeling 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 preprint arXiv: 1708.06374, 2017. [30] 符锌砂, 胡嘉诚, 何石坚. 基于交通状况及行驶速度的高速公路换道时间研究. 公路交通科技, 2020, 37(4): 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(4): 133−139 [31] Yang D, Zhu L 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: Journal of the Transportation Research, 2007, 1999(1): 71−78 doi: 10.3141/1999-08 [33] Kawabata K, Ma L, Xue J R, Zheng N N. A path generation method for automated vehicles based on Bezier curve. In: Proceedings of the IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Wollongong, Australia: IEEE, 2013. 991−996 [34] Ziegler J, Bender P, Dang T, Stiller C. Trajectory planning for Bertha——A local, continuous method. In: Proceedings of the IEEE Intelligent Vehicles Symposium. Dearborn, USA: IEEE, 2014. 450−457 [35] Li L, Wang X, Wang K F, Lin Y L, Xin J M, Chen L, et al. Parallel testing of vehicle intelligence via virtual-real interaction. Science Robotics, 2019, 4(28): Article No. eaaw4106 doi: 10.1126/scirobotics.aaw4106 [36] Wang F Y, Zheng N N, Li L, Xin J M, Wang X, Xu L H, et al. China's 12-year quest of autonomous vehicular intelligence: The intelligent vehicles future challenge program. IEEE Intelligent Transportation Systems Magazine, 2021, 13(2): 6−19 doi: 10.1109/MITS.2021.3058623 -
计量
- 文章访问数: 942
- HTML全文浏览量: 599
- PDF下载量: 174
- 被引次数: 0
