基于图搜索与优化的动态非结构环境智能车辆轨迹规划

doi:10.19562/j.chinasae.qcgc.2024.09.004

摘要/Abstract

摘要：

针对动态非结构环境下的智能车辆轨迹规划，提出了一种基于图搜索和优化的轨迹规划方法。首先，采用图搜索方法对智能车辆运动基元进行搜索，获取符合运动学特性的初始轨迹；然后，基于非线性模型预测控制方法对轨迹进行优化，以获得更平滑、更安全的轨迹。为在动态非结构环境下实现基元的快速且安全的拓展，提出了一种基元碰撞检测的方法。该方法通过障碍物膨胀和栅格离散运动基元，对非规则障碍物进行静态碰撞检测，引入速度障碍物概念，在速度空间对动态障碍物进行动态碰撞检测。在ROS/Gazebo环境下进行了算法仿真比较，并通过场地试验进行了测试评价。结果表明，相较于TEB算法，所提轨迹规划方法在满足计算实时性要求的同时，平均避障成功率提高了18%，展现出了更高的安全避障能力和可行性。

关键词: 智能车辆, 非结构环境, 动态环境, 避障

Abstract:

A trajectory planning method based on graph search and optimization is proposed for intelligent vehicle trajectory planning in dynamic unstructured environments. Firstly， the graph search method is employed to search for motion primitives for intelligent vehicles to obtain initial trajectories that conform to kinematic characteristics. Then， based on nonlinear model predictive control methods， the trajectory is optimized to obtain smoother and safer trajectories. In order to achieve rapid and secure expansion of primitives in dynamic unstructured environments， a method for primitive collision detection is proposed. This method uses obstacle expansion and grid discrete motion elements to perform static collision detection on irregular obstacles， and introduces in the concept of velocity obstacles to perform dynamic collision detection on dynamic obstacles in velocity space. The proposed algorithm is compared by simulations in ROS/Gazebo environment， and is evaluated by field tests. The results show that compared to the TEB algorithm， the proposed trajectory planning method improves the average obstacle avoidance success rate by 18% while meeting the real-time computing requirements， demonstrating higher safety obstacle avoidance ability and feasibility.

Key words: intelligent vehicle, unstructured environment, dynamic environment, obstacle avoidance

杨秀建,白永瑞. 基于图搜索与优化的动态非结构环境智能车辆轨迹规划[J]. 汽车工程, 2024, 46(9): 1564-1575.

Xiujian Yang,Yongrui Bai. Trajectory Planning for Intelligent Vehicle in Dynamic Unstructured Environment Based on the Graph Search and Optimization Methods[J]. Automotive Engineering, 2024, 46(9): 1564-1575.

图/表 17

图1

图2

图3

图4

图5

表1

图6

图7

图8

图9

图10

图11

表2

表3

图12

表4

图13

参考文献 21

1	PADEN B， CAP M， YONG S Z， et al. A survey of motion planning and control techniques for self-driving urban vehicles［J］. IEEE Transactions on Intelligent Vehicles， 2016， 1（1）： 33-55.
2	CHIANG H T， HOMCHAUDHURI B， SMITH L， et al. Safety， challenges， and performance of motion planners in dynamic environments［C］// AMATO N M， HAGER G， THOMAS S， et al. Robotics research. Cham： Springer International Publishing， 2020： 793-808.
3	WANG H， HUANG Y， KHAJEPOUR A， et al. Crash mitigation in motion planning for autonomous vehicles［J］. IEEE Transactions on Intelligent Transportation Systems， 2019， 20（9）： 3313-3323.
4	田洪清，丁峰，郑讯佳，等. 基于势能场虚拟力的智能网联车辆运动规划［J］. 汽车工程， 2021， 43（4）： 518-526.
	TIAN H Q， DING F， ZHENG X J， et al. Motion planning based on virtual force of potential field for intelligent connected vehicles［J］. Automotive Engineering， 2021， 43（4）： 518-526.
5	KIM J， KIM K， JO K. Obstacle avoidance in dynamic environment using particle swarm optimization and Kalman filter［C］. 2023 23rd International Conference on Control， Automation and Systems （ICCAS）， 2023： 1199-1203.
6	郝琨，张慧杰，李志圣，等. 基于改进避障策略和双优化蚁群算法的机器人路径规划［J］. 农业机械学报， 2022， 53（8）： 303-312.
	HAO K， ZHANG H J， LI Z S， et al. Path planning of mobile robot based on improved obstacle avoidance strategy and double optimization ant colony algorithm［J］.Transactions of the Chinese Society for Agricultural Machinery，2022，53（8）：303-312.
7	胡小建，周琼，宋旭东，等. 基于时间窗约束的多人多储位拣选路径优化模型及改进遗传算法应用研究［J］. 计算机集成制造系统， 2022， 28（11）： 3354-3364.
	HU X J， ZHOU Q， SONG X D， et al. Optimal model of multi-person and multi-location picking path based on time window constraint and application of improved genetic algorithm［J］. Computer Integrated Manufacturing System， 2022， 28（11）： 3354-3364.
8	JIAN Z， YAN Z， LEI X， et al. Dynamic control barrier function-based model predictive control to safety-critical obstacle-avoidance of mobile robot［C］. 2023 IEEE International Conference on Robotics and Automation （ICRA）. IEEE， 2023： 3679-3685.
9	李军，周伟，唐爽. 基于自适应拟合的智能车换道避障轨迹规划［J］. 汽车工程， 2023， 45（7）： 1174-1183.
	LI J， ZHOU W， TANG S. Lane change and obstacle avoidance trajectory planning of intelligent vehicle based on adaptive fitting［J］. Automotive Engineering， 2023， 45（7）： 1174-1183.
10	LI G， ZHANG X， GUO H， et al. Real-time optimal trajectory planning for autonomous driving with collision avoidance using convex optimization［J］. Automotive Innovation， 2023， 6（3）： 481-491.
11	SINTOV A， SHAPIRO A. Time-based RRT algorithm for rendezvous planning of two dynamic systems［C］. 2014 IEEE International Conference on Robotics and Automation （ICRA）， 2014： 6745-6750.
12	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.
13	齐尧，朱彦齐，李永乐，等. 面向动静态混合环境的智能车运动规划方法［J］. 交通运输系统工程与信息， 2022， 22（4）： 293-301.
	QI Y， ZHU Y Q， LI Y L， et al. Motion planning method for intelligent vehicles under constrained dynamic and static environment［J］. Journal of Transportation Systems Engineering and Information Technology， 2022， 22（4）： 293-301.
14	胡杰，张志豪，陈瑞楠，等. 基于改进混合A*的智能汽车时空联合规划方法［J］. 汽车工程， 2023， 45（7）： 1123-1133.
	HU J， ZHANG Z H， CHEN R N， et al. Spatio-temporal joint planning method of intelligent vehicles based on improved hybrid A*［J］. Automotive Engineering， 2023， 45（7）： 1123-1133.
15	PETTI S， FRAICHARD T. Safe motion planning in dynamic environments［C］. 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE， 2005： 2210-2215.
16	AMANATIDES J， WOO A. A fast voxel traversal algorithm for ray tracing［C］. Eurographics， 1987， 87（3）： 3-10.
17	BAUTIN A， MARTINEZ-GOMEZ L， FRAICHARD T. Inevitable collision states： a probabilistic perspective［C］. 2010 IEEE International Conference on Robotics and Automation， 2010： 4022-4027.
18	LIN J， ZHOU T， ZHU D， et al. Search-based online trajectory planning for car-like robots in highly dynamic environments［C］. 2021 IEEE International Conference on Robotics and Automation （ICRA）. IEEE， 2021： 8151-8157.
19	ANDERSSON J A E， GILLIS J， HORN G， et al. CasADi-a software framework for nonlinear optimization and optimal control［J］. Mathematical Programming Computation， 2019， 11（1）： 1-36.
20	WÄCHTER A， BIEGLER L T. On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming［J］. Mathematical Programming， 2006， 106： 25-57.
21	YAN Z， DUCKETT T， BELLOTTO N. Online learning for 3D LiDAR-based human detection： experimental analysis of point cloud clustering and classification methods［J］. Autonomous Robots， 2020， 44（2）： 147-164.

参数	数值
车长/m	1.92
轴距L/m	1.33
车宽/m	1.02
单个栅格边长/m	0.20
剪枝网格边长/m	0.20
拓展步长τ/s	0.8
速度范围/（m·s^-1）	［-3， 3］
最大搜索时间/ms	100
离散加速度集合/（m·s^-2）	｛-0.5， -0.25， 0.0， 0.25， 0.5｝
离散前轮转角集合/（°）	｛-28， -21， -14， -7， 0， 7， 14， 21， 28｝

场景	评价过程	平均耗时/ms
仿真动态场景	TEB算法全过程	23
	本文算法初始轨迹搜索过程	8
	本文算法轨迹优化过程	50
	本文算法全过程	58
单障碍物场景	TEB算法全过程	10
	本文算法初始轨迹搜索过程	5
	本文算法轨迹优化过程	36
	本文算法全过程	41

剪枝网格边长/m	耗时	搜索过程耗时/ms	优化过程耗时/ms	全过程耗时/ms
0.4	平均耗时	4	52	56
	最大耗时	101	136	178
	最小耗时	1	20	21
0.2	平均耗时	8	50	58
	最大耗时	101	86	156
	最小耗时	1	26	27
0.1	平均耗时	14	53	67
	最大耗时	65	119	132
	最小耗时	3	27	30

参数	数值
车长/m	1.92
轴距L/m	1.33
车宽/m	1.02
单个栅格边长/m	0.20
剪枝网格边长/m	0.20
拓展步长τ/s	0.8
速度范围/（m·s^-1）	［-1.5， 1.5］
最大搜索时间/ms	100
离散加速度集合/（m·s^-2）	｛-0.5， -0.25， 0.0， 0.25， 0.5｝
离散前轮转角集合/（°）	｛-28， -21， -14， -7， 0， 7， 14， 21， 28｝

[1]	范晓临,张旭东,邹渊,尹鑫,刘颖群. 一种基于简化可视图的建图和规划方法[J]. 汽车工程, 2024, 46(7): 1249-1258.
[2]	周俊宇,李克强,任晗啸,于杰,罗禹贡. 基于场景划分的智能网联汽车协同换道避障方法[J]. 汽车工程, 2024, 46(5): 745-753.
[3]	赵树恩,王盛,冷姚. 智能汽车轨迹跟踪多目标显式模型预测控制[J]. 汽车工程, 2024, 46(5): 784-794.
[4]	吴晓建,廖平伟,雷耀,江会华,王爱春,胡家琦. 面向结构化道路的智能驾驶轨迹规划一致性研究[J]. 汽车工程, 2024, 46(3): 383-395.
[5]	高锋,冯德福,胡秋霞. 面向NMPC运动规划系统的数值优化加速技术[J]. 汽车工程, 2023, 45(8): 1438-1447.
[6]	李军, 周伟, 唐爽. 基于自适应拟合的智能车换道避障轨迹规划[J]. 汽车工程, 2023, 45(7): 1174-1183.
[7]	严凉,吴晓东,胡川. 面向密集障碍规避的人车共享转向控制系统[J]. 汽车工程, 2023, 45(12): 2222-2233.
[8]	吕彦直,魏超,何元浩. 基于GCN和CIL的端到端自动驾驶换道方法[J]. 汽车工程, 2023, 45(12): 2310-2317.
[9]	胡丹丹,尹鹏飞,牛国臣,赵金聚. 非结构化道路下离轴式拖挂车辆主动避障控制研究[J]. 汽车工程, 2023, 45(12): 2318-2329.
[10]	唐斌,许占祥,江浩斌,蔡英凤,胡子添,杨铮奕. 基于分段优化的车辆换道避障轨迹规划[J]. 汽车工程, 2022, 44(6): 831-841.
[11]	黄圣杰,胡满江,周云水,殷周平,秦晓辉,边有钢,贾倩倩. 动态场景下基于语义分割与运动一致性约束的车辆视觉SLAM[J]. 汽车工程, 2022, 44(10): 1503-1510.
[12]	杨彬,宋学伟,高振海. 考虑车辆运动约束的最优避障轨迹规划算法[J]. 汽车工程, 2021, 43(4): 562-570.
[13]	陈吉清,翁楚滨,兰凤崇. 智能车辆换道潜在冲突分析与风险量化方法[J]. 汽车工程, 2021, 43(11): 1565-1576.
[14]	章军辉,付宗杰,郭晓满,李庆,陈大鹏,赵野. 基于HS⁃FCM模糊聚类的快速多目标车辆跟踪算法[J]. 汽车工程, 2021, 43(10): 1419-1426.
[15]	宋晓琳,盛鑫,曹昊天,李明俊,易滨林,黄智. 基于模仿学习和强化学习的智能车辆换道行为决策[J]. 汽车工程, 2021, 43(1): 59-67.