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

doi:10.19562/j.chinasae.qcgc.2025.05.003

摘要/Abstract

摘要：

针对自动驾驶汽车所应用的时空分离轨迹规划方法易导致车辆灵活性不足，甚至无法在复杂工况下规划出可行轨迹，而现有时空联合轨迹规划方法难以满足结构化道路应用要求的问题，本文提出了一种基于动态规划与数值优化算法的时空联合规划方法。首先，在Frenet坐标系下使用动态规划算法生成时空耦合粗轨迹，过程中采用确定性采样法进行子节点拓展。然后，以粗轨迹为参考在笛卡尔坐标系下构建可行驶时空走廊，建立NMPC优化模型求解最终轨迹。最后，通过仿真验证算法有效性，结果表明，所提出的方法对结构化道路的适应性良好，相较于其他时空联合规划算法，能够更好地平衡通行效率、轨迹舒适性、算法实时性的要求。

关键词: 时空联合轨迹规划, 动态规划, 确定性采样, 可行驶时空走廊, NMPC优化

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

胡杰,郑嘉辰,周思龙,赵文龙,张志凌,姚茂嘉. 结构化道路下智能车时空联合轨迹规划方法[J]. 汽车工程, 2025, 47(5): 820-828.

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.

图/表 22

图1

图2

表1

时空域动态规划伪代码"

算法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

表1

图3

表 2

时空节点拓展伪代码"

算法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$

表 2

图4

图5

图6

表3

实验相关参数"

参数	数值
车长/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

表3

图7

图8

图9

图10

图11

表4

图12

图13

图14

图15

图16

表5

表6

参考文献 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

[1]	李兴坤,王国晖,卢紫旺,王玉海,王语风,田光宇. 基于IDP的重型商用车自适应距离域预见性巡航控制策略[J]. 汽车工程, 2024, 46(8): 1346-1356.
[2]	马彦,李佳怡,马乾,陈明超. 基于迭代动态规划的动力电池组热管理优化策略[J]. 汽车工程, 2022, 44(5): 709-721.
[3]	张哲,丁海涛,张袅娜,郭孔辉. 智能网联电动汽车经济性巡航速度规划[J]. 汽车工程, 2022, 44(4): 609-616.
[4]	李跃娟,齐巍,王成,张博,卢强. 并联混合动力汽车ECMS的时变等效因子提取算法的研究[J]. 汽车工程, 2021, 43(2): 181-188.
[5]	冷江昊,孙超,卢兵. 智能车辆多信号灯路口快速节能车速规划[J]. 汽车工程, 2021, 43(10): 1442-1447.
[6]	赵治国, 唐旭辉, 付靖, 范佳琦. 功率分流混合动力系统发动机停机优化控制^*[J]. 汽车工程, 2020, 42(8): 993-999.
[7]	赵治国, 范佳琦, 蒋蓝星, 唐旭辉, 付靖. 复合功率分流系统发动机起动H_∞鲁棒优化控制^*[J]. 汽车工程, 2020, 42(4): 417-424.
[8]	金辉, 张俊. 智能车经济性起步车速规划研究^*[J]. 汽车工程, 2020, 42(2): 270-277.
[9]	金辉, 张子豪. 基于自适应动态规划的HEV能量管理研究综述^*[J]. 汽车工程, 2020, 42(11): 1490-1496.
[10]	王玉海, 李兴坤, 张鹏雷, 郑旭光. 基于ADAS地图的载货车预见巡航实时优化算法^*[J]. 汽车工程, 2020, 42(10): 1335-1339.
[11]	解少博，刘通，李会灵，辛宗科. 基于马尔科夫链的并联PHEB预测型能量管理策略研究[J]. 汽车工程, 2018, 40(8): 871-877.
[12]	解少博，刘通，李会灵，魏朗. 双电机一体化纯电动客车动力分配策略优化[J]. 汽车工程, 2018, 40(7): 749-.
[13]	解少博，辛宗科，李会灵，刘通，魏朗. 插电式混合动力公交车电池配置和能量管理策略协同优化的研究[J]. 汽车工程, 2018, 40(6): 625-.
[14]	金辉,丁峰. 智能车辆换道行驶的经济车速研究[J]. 汽车工程, 2018, 40(5): 542-546.