结构化道路下智能车时空联合轨迹规划方法

doi:10.19562/j.chinasae.qcgc.2025.05.003

Abstract

Abstract:

For the problem that the spatio-temporal separation trajectory planning method used in autonomous vehicles is prone to insufficient vehicle flexibility， and even cannot generate feasible trajectories under complex working conditions， while the existing spatio-temporal unified trajectory planning method is difficult to meet the requirements of structured road application， a spatio-temporal unified planning method based on dynamic programming and numerical optimization algorithm is proposed. Firstly， the spatio-temporal unified coarse trajectory is generated by dynamic programming algorithm in Frenet coordinate system. In the process， deterministic sampling method is used to expand the child nodes. Then， taking the coarse trajectory as reference， the feasible spatio-temporal corridor is constructed in Cartesian coordinate system， and the NMPC optimization model is established to generate the final trajectory. Finally， the algorithm is verified by simulation. The results show that the proposed algorism has good adaptability to structured road， and can better balance the requirements of traffic efficiency， trajectory comfort and time consumption than other spatio-temporal unified algorithms.

Key words: spatio-temporal unified planning, dynamic planning, deterministic sampling, feasible spatio-temporal corridor, NMPC optimization

Jie Hu,Jiachen Zheng,Silong Zhou,Wenlong Zhao,Zhiling Zhang,Maojia Yao. Spatio-Temporal Unified Planning Method for Intelligent Vehicles on Structured Road[J].Automotive Engineering, 2025, 47(5): 820-828.

Figures/Tables 22

算法1 时空域动态规划算法流程

输入：车辆状态信息，道路信息，时空节点网格

输出：时空耦合粗轨迹

1. Initialize the start node of $n o d e_g r a p h$

2. For each $t i m e_l a y e r ∈ n o d e_g r a p h$

3. For each $n o d e i ∈ n o d e_g r a p h$

4. take $n o d e i$ as parent node；

5. execute child node extension from parent node

6. calculate the cost of child node and filter the node with lowest cost in each grid

7. update the connection status of nodes in $n o d e_g r a p h$

8. End For

9. select the node with lowest cost in latest layer as target node

10. trackback from the target node to the planning start node and

construct the coarse trajectory

11. End For

12. return the coarse trajectory

算法2 时空节点拓展算法流程

输入：车辆配置参数，道路信息，障碍物信息，父节点 $N i$ ，时空节点网格 $n o d e_g r a p h$

输出：更新后的时空节点网格

13. For each $a ∈ s a m p l e_a_l i s t$

14. calculate the longitudinal quadratic polynomial；

15. update longitudinal position $s i$ ；

16. calculate $A j$ of the sample node through the $S D N A M$

17. generate $s a m p l e_l_l i s t$

18. For each $l ∈ s a m p l e_l_l i s t$

19. calculate the lateral cubic polynomial；

20. update lateral position；

21. combine the longitudinal and lateral polynomial to form the

trajectory from $N i$ to sample node $N i + 1$ ；

22. If $N i + 1$ is in collision or kinematic inaccessible

23. continue；

24. End

25. calculate the total cost of node $N i + 1$ as $C i + 1$ ；

26. calculate the $g r i d_i d$ of $N i + 1$ in the $n o d e_g r a p h$ ；

27. If $C i + 1$ < the total cost at $n o d e_g r a p h (g r i d_i d)$ ；

28. replace the node at $n o d e_g r a p h g r i d i d$ with $N i + 1$ ；

29. End

30. End For

31. End For

32. return the updated $n o d e_g r a p h$

参数	数值
车长/m	4.6
车宽/m	1.8
轴距/m	2.7
最大前轮转角/（ $°$ ）	40
最高车速/（m·s^-1）	15
期望车速/（m·s^-1）	14
加速度范围/（m·s^-2）	［-4，4］
离散加速度候选集 $A$	$- 4, - 3, - 2, - 1,0, 1,2, 3,4$
规划时域 $T$ /s	7
车道宽度/m	3.5

References 21

1	FAN H， ZHU F， LIU C， et al. Baidu apollo em motion planner［J］. arxiv preprint arXiv：， 2018.
2	LIM W， LEE S， SUNWOO M， et al. Hierarchical trajectory planning of an autonomous car based on the integration of a sampling and an optimization method［J］. IEEE Transactions on Intelligent Transportation Systems， 2018， 19（2）： 613-626.
3	ZHANG Y， SUN H， ZHOU J， et al. Optimal vehicle path planning using quadratic optimization for baidu apollo open platform［C］. 2020 IEEE Intelligent Vehicles Symposium （IV）. IEEE， 2020： 978-984.
4	MILLER C， PEK C， ALTHOFF M. Efficient mixed-integer programming for longitudinal and lateral motion planning of autonomous vehicles［C］. 2018 IEEE Intelligent Vehicles Symposium （IV）. IEEE， 2018： 1954-1961.
5	DEOLASEE S， LIN Q， LI J， et al. Spatio-temporal motion planning for autonomous vehicles with trapezoidal prism corridors and bézier curves［C］. 2023 American Control Conference （ACC）. IEEE， 2023： 3207-3214.
6	胡杰，张志豪，陈瑞楠，等. 基于改进混合A*的智能汽车时空联合规划方法［J］. 汽车工程， 2023， 45（7）： 1123-1133.
	HU J， ZHANG Z H， CHEN R N， et al. Spatio-temporal unified planning method of intelligent vehicles based on improved hybrid A*［J］. Automotive Engineering， 2023， 45（7）： 1123-1133.
7	ZHANG T， FU M， SONG W， et al. Trajectory planning based on spatio-temporal map with collision avoidance guaranteed by safety strip［J］. IEEE Transactions on Intelligent Transportation Systems， 2020， 23（2）： 1030-1043.
8	LI B， ZHANG Y， OUYANG Y， et al. Fast trajectory planning for AGV in the presence of moving obstacles： a combination of 3-dim A* search and QCQP［C］. 2021 33rd Chinese Control and Decision Conference （CCDC）. IEEE， 2021： 7549-7554.
9	XIN L， KONG Y， LI S E， et al. Enable faster and smoother spatio-temporal trajectory planning for autonomous vehicles in constrained dynamic environment［J］. Proceedings of the Institution of Mechanical Engineers， Part D： Journal of Automobile Engineering， 2021， 235（4）： 1101-1112.
10	LI B， KONG Q， ZHANG Y， et al. On-road trajectory planning with spatio-temporal RRT* and always-feasible quadratic program［C］. 2020 IEEE 16th International Conference on Automation Science and Engineering （CASE）. IEEE， 2020： 942-947.
11	CHANG Y， LIANG H， ZHAO P， et al. On-road trajectory planning with spatio-temporal informed RRT［C］. 2022 IEEE International Conference on Mechatronics and Automation （ICMA）. IEEE， 2022： 1425-1431.
12	EIRAS F， HAWASLY M， ALBRECHT S V， et al. A two-stage optimization-based motion planner for safe urban driving［J］. IEEE Transactions on Robotics， 2021， 38（2）： 822-834.
13	DEOLASEE S， LIN Q， LI J， et al. Spatio-temporal motion planning for autonomous vehicles with trapezoidal prism corridors and Bézier curves［C］. 2023 American Control Conference （ACC）. IEEE， 2023： 3207-3214.
14	DE GROOT O， FERRANTI L， GAVRILA D M， et al. Topology-driven parallel trajectory optimization in dynamic environments［J］. IEEE Transactions on Robotics， 2024.
15	SADAT A， REN M， POKROVSKY A， et al. Jointly learnable behavior and trajectory planning for self-driving vehicles［C］. 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. IEEE， 2019： 3949-3956.
16	ZIEGLER J， STILLER C. Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios［C］. 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE， 2009： 1879-1884.
17	DIXIT S， MONTANARO U， DIANATI M， et al. Trajectory planning for autonomous high-speed overtaking in structured environments using robust MPC［J］. IEEE Transactions on Intelligent Transportation Systems， 2019， 21（6）： 2310-2323.
18	LI B， OUYANG Y， LI L， et al. Autonomous driving on curvy roads without reliance on frenet frame： a cartesian-based trajectory planning method［J］. IEEE Transactions on Intelligent Transportation Systems， 2022， 23（9）： 15729-15741.
19	ZIEGLER J， BENDER P， DANG T， et al. Trajectory planning for Bertha—a local， continuous method［C］. 2014 IEEE Intelligent Vehicles Symposium Proceedings. IEEE， 2014： 450-457.
20	MICHELI F， BERSANI M， ARRIGONI S， et al. NMPC trajectory planner for urban autonomous driving［J］. Vehicle System Dynamics， 2023， 61（5）： 1387-1409.
21	MCNAUGHTON M， URMSON C， DOLAN J M， et al. Motion planning for autonomous driving with a conformal spatiotemporal lattice［C］. 2011 IEEE International Conference on Robotics and Automation. IEEE， 2011： 4889-4895.

算法	纵向通行距离/m	纵向加速度峰值/ 平均值/（m·s^-2）	横向加速度峰值/ 平均值/（m·s^-2）
本文算法粗轨迹	96.4	1.00/0.57	2.95/1.25
本文算法优化后	96.4	0.84/0.45	2.13/1.11
Lattice算法	84.0	0.00/0.00	1.17/0.45
Hybrid A*算法	94.3	0.94/0.51	2.77/1.56

算法	纵向通行距离/m	纵向加速度峰值/ 平均值/（m·s^-2）	横向加速度峰值/ 平均值/（m·s^-2）
本文算法粗轨迹	96.4	2.00/0.57	3.93/1.96
本文算法优化后	96.4	1.79/0.54	2.87/1.81
Lattice算法	84	0.50/0.26	1.60/1.30
Hybrid A*算法	92.1	1.47/0.58	3.89/1.88

场景	平均耗时/ms
场景	本文算法	Hybrid A*^［6］	Lattice^［21］
场景1	90（18+72）	76	34
场景2	96（21+75）	89	39

Spatio-Temporal Unified Planning Method for Intelligent Vehicles on Structured Road

RichHTML

PDF (PC)

Like

Knowledge

Abstract

Cite this article

share this article

Figures/Tables 22

References 21

Related Articles 0

Metrics

Comments

Recommended 0