Trajectory Planning for Autonomous Driving in Unstructured Scenarios Based on Graph Neural Network and Numerical Optimization 基于图神经网络和数值优化的非结构化场景下的自动驾驶轨迹规划
Sumin Zhang, Kuo Li, Rui He, Zhiwei Meng, Yupeng Chang, Xiaosong Jin, Ri Bai
Abstract 摘要
In unstructured environments, obstacles are diverse and lack lane markings, making trajectory planning for intelligent vehicles a challenging task. Traditional trajectory planning methods typically involve multiple stages, including path planning, speed planning, and trajectory optimization. These methods require the manual design of numerous parameters for each stage, resulting in significant workload and computational burden. While end-to-end trajectory planning methods are simple and efficient, they often fail to ensure that the trajectory meets vehicle dynamics and obstacle avoidance constraints in unstructured scenarios. Therefore, this paper proposes a novel trajectory planning method based on Graph Neural Networks (GNN) and numerical optimization. The proposed method consists of two stages: (1) initial trajectory prediction using the GNN, (2) trajectory optimization using numerical optimization. First, the graph neural network processes the environment information and predicts a rough trajectory, replacing traditional path and speed planning. This predicted trajectory serves as the initial solution for the numerical optimization stage, which optimizes the trajectory to ensure compliance with vehicle dynamics and obstacle avoidance constraints. We conducted simulation experiments to validate the feasibility of the proposed algorithm and compared it with other mainstream planning algorithms. The results demonstrate that the proposed method simplifies the trajectory planning process and significantly improves planning efficiency. 在非结构化环境中,障碍物种类繁多且缺乏车道标记,这使得智能车辆的轨迹规划成为一项具有挑战性的任务。传统的轨迹规划方法通常涉及多个阶段,包括路径规划、速度规划和轨迹优化。这些方法需要为每个阶段手动设计大量参数,造成了巨大的工作量和计算负担。虽然端到端轨迹规划方法简单高效,但在非结构化场景中,它们往往无法确保轨迹满足车辆动力学和避障约束条件。因此,本文提出了一种基于图神经网络(GNN)和数值优化的新型轨迹规划方法。该方法包括两个阶段:(1)使用图神经网络进行初始轨迹预测;(2)使用数值优化进行轨迹优化。首先,图神经网络处理环境信息并预测粗略轨迹,取代传统的路径和速度规划。该预测轨迹是数值优化阶段的初始解,数值优化阶段对轨迹进行优化,以确保符合车辆动力学和避障约束条件。我们进行了仿真实验,验证了所提算法的可行性,并将其与其他主流规划算法进行了比较。结果表明,提出的方法简化了轨迹规划过程,并显著提高了规划效率。
TRAJECTORY planning for intelligent vehicles is a crucial topic in the field of autonomous driving, requiring consideration of safety, comfort, and driving efficiency [1]. Current trajectory planning research mainly focuses on structured environments such as highways and unstructured environments such as parking lots. In contrast to structured environments, which have lane markings as driving references, unstructured environments generally lack such references and feature more complex and diverse obstacles, making trajectory planning significantly more challenging [2], [3]. A feasible trajectory should connect the vehicle's initial position and orientation with its target position and orientation, comply with vehicle dynamic constraints, and ensure that the control system avoids collisions with obstacles during trajectory tracking. 智能车辆的轨迹规划是自动驾驶领域的一个重要课题,需要考虑安全性、舒适性和驾驶效率 [1]。目前的轨迹规划研究主要集中在高速公路等结构化环境和停车场等非结构化环境。与有车道标记作为驾驶参考的结构化环境相比,非结构化环境通常缺乏此类参考,而且障碍物更加复杂多样,因此轨迹规划的难度大大增加 [2],[3]。可行的轨迹应将车辆的初始位置和方向与其目标位置和方向连接起来,符合车辆动态约束条件,并确保控制系统在轨迹跟踪过程中避免与障碍物发生碰撞。
The objective of the trajectory planner is to establish a correspondence between the unstructured environment and the trajectory, generating a feasible path based on the environmental information. Traditional trajectory planning algorithms employ mathematical methods to identify such correspondences, typically involving multiple stages of mathematical modeling [2], [3]. These stages include path planning, path optimization, and speed planning to derive a feasible trajectory. This paper aims to simplify this process by leveraging deep learning techniques to identify the relationship between environmental information and the trajectory through large amounts of data. Subsequently, an optimal control method is used to generate a locally optimal trajectory that satisfies multiple constraints. 轨迹规划器的目标是在非结构化环境和轨迹之间建立对应关系,根据环境信息生成可行路径。传统的轨迹规划算法采用数学方法来确定这种对应关系,通常涉及多个阶段的数学建模 [2]、[3]。这些阶段包括路径规划、路径优化和速度规划,以得出可行的轨迹。本文旨在利用深度学习技术,通过大量数据识别环境信息与轨迹之间的关系,从而简化这一过程。随后,使用优化控制方法生成满足多个约束条件的局部最优轨迹。
II. RELATED WORKS II.相关作品
Existing trajectory planning algorithms can be categorized based on different criteria. In line with the research content of this paper, trajectory planning methods are divided into three categories: 1) combinations of traditional planning algorithms, 2) optimization-based methods, and 3) end-to-end methods. 现有的轨迹规划算法可以根据不同的标准进行分类。根据本文的研究内容,轨迹规划方法分为三类:1) 传统规划算法的组合;2) 基于优化的方法;3) 端到端方法。
Traditional planning algorithms, originating in the 1970s, have been widely applied in the field of robotics [4]. These algorithms typically seek a feasible or optimal solution based on certain manually defined rules. For example, graph searchbased algorithms such as Dijkstra's [5] and A* [6], and sampling-based algorithms such as RRT* [7] and its variants, are commonly used. The paths solved by these algorithms often exhibit discontinuous curvature and do not consider vehicle kinematic characteristics, making them more suitable for global path planning. When used for local path planning, polynomial curves like quintic polynomials or Bézier curves are often used to fit the planned path. To simulate vehicle kinematics, the Hybrid A* [8] extends graph search algorithms by incorporating the vehicle's movement direction, resulting in smoother paths that adhere to vehicle kinematic constraints. To generate a trajectory, it is necessary to add time information to the path to represent the vehicle's speed. Speed planning typically employs S-T graph-based algorithms. The combination of traditional planning algorithms usually requires the manual setting of multiple stages. For instance, in graph search algorithms, the map must be gridded, and the grid resolution must be set. Larger grids can lead to poorer path quality, while smaller grids increase the computational cost of the algorithm. In sampling-based algorithms, the step size and the number of iterations need to be set. A smaller step size 传统的规划算法起源于 20 世纪 70 年代,已广泛应用于机器人领域 [4]。这些算法通常根据某些人工定义的规则寻求可行或最优的解决方案。例如,Dijkstra's [5] 和 A* [6] 等基于图搜索的算法,以及 RRT* [7] 及其变体等基于采样的算法,都是常用的算法。这些算法求解出的路径通常曲率不连续,且不考虑车辆运动特性,因此更适合全局路径规划。用于局部路径规划时,通常使用五次多项式或贝塞尔曲线等多项式曲线来拟合规划路径。为了模拟车辆运动学,Hybrid A* [8] 扩展了图搜索算法,将车辆的运动方向纳入其中,从而得到更平滑、更符合车辆运动学约束的路径。要生成轨迹,必须在路径中添加时间信息,以表示车辆的速度。速度规划通常采用基于 S-T 图的算法。传统规划算法的组合通常需要手动设置多个阶段。例如,在图搜索算法中,必须对地图进行网格划分,并设置网格分辨率。网格越大,路径质量越差,而网格越小,算法的计算成本越高。在基于采样的算法中,需要设置步长和迭代次数。步长越小
and more iterations can improve path quality but also increase computational cost, and vice versa. 更多的迭代可以提高路径质量,但也会增加计算成本,反之亦然。
Optimization-based methods generally refer to the design of a numerical optimization cost function with objectives such as comfort or energy consumption, and constraints such as collision avoidance or physical limitations. By minimizing this cost function, a locally optimal trajectory can be obtained. For example, Lim et al. [9] used a hierarchical trajectory planning method combining sampling and numerical optimization. They obtained a rough behavioral trajectory through a sampling algorithm, then established a cost function and constraints, and performed numerical optimization using Sequential Quadratic Programming (SQP) [10]. When the continuous variables of the optimization problem are partially or fully discretized and then solved directly, it is typically referred to as an optimal control method [11]. Optimal control methods model the trajectory planning problem as an Optimal Control Problem (OCP), usually consisting of an objective function and multiple constraints. The variables are discretized and converted into a Nonlinear Programming (NLP) problem for solving, using methods such as SQP and Interior Point Methods (IPM) [12]. Li et al. [13] and Li and Wang [14], for example, formulated the trajectory planning task with constraints on vehicle dynamics, obstacles, and initial positions, optimizing performance metrics such as trajectory smoothness and obstacle avoidance to obtain an optimal trajectory. Lian et al. [3] proposed a two-stage trajectory planning method. In the first stage, they used an improved Hybrid algorithm to obtain an initial trajectory. In the second stage, they performed segmented optimization of the optimal control problem. This method achieved good results in narrow parking environments by reducing optimization time. 基于优化的方法通常是指设计一个数值优化成本函数,其目标包括舒适度或能耗,以及避免碰撞或物理限制等约束条件。通过最小化该成本函数,可以获得局部最优轨迹。例如,Lim 等人[9] 使用了一种结合采样和数值优化的分层轨迹规划方法。他们通过采样算法获得了粗略的行为轨迹,然后建立了成本函数和约束条件,并使用序列二次编程(SQP)[10]进行了数值优化。如果将优化问题的连续变量部分或全部离散化,然后直接求解,通常称为最优控制方法 [11]。最优控制方法将轨迹规划问题建模为最优控制问题(OCP),通常由一个目标函数和多个约束条件组成。使用 SQP 和内点法 (IPM) 等方法将变量离散化并转换为非线性编程 (NLP) 问题进行求解 [12]。例如,Li 等人[13] 以及 Li 和 Wang [14]在制定轨迹规划任务时,对车辆动力学、障碍物和初始位置进行了约束,优化了轨迹平滑度和避障等性能指标,以获得最佳轨迹。Lian 等人 [3] 提出了一种两阶段轨迹规划方法。在第一阶段,他们使用改进的混合 算法获得初始轨迹。在第二阶段,他们对最优控制问题进行了分段优化。这种方法通过缩短优化时间,在狭窄的停车环境中取得了良好的效果。
End-to-end autonomous driving technology can be distinguished in terms of learning methods as imitation learning (IL) and reinforcement learning (RL). IL learns from a large number of expert demonstrations, mimicking expert behavior across various driving scenarios, while RL accumulates rewards or penalties through interaction with the environment, aiming to maximize cumulative rewards [15]. Although RL does not require pre-prepared training data, its efficiency during the training process tends to be lower. End-to-end autonomous driving traces its origins back to ALVINN [16] in 1988, which utilized a fully connected neural network to process data from cameras and radar sensors and output steering values. In recent years, with the continuous advancement in the field of deep learning and the increase in computational power, various methods for end-to-end autonomous driving have emerged, exhibiting differences in input formats, intermediate processes, and output formats. Hawke et al. [17] trained a neural network model using real-world traffic environment data, inputting data from three different directions of cameras and directly outputting control signals. TransFuser [18], [19] utilized the attention mechanism from Transformers [20], combining camera inputs and LIDAR data through multimodal fusion within the CARLA simulator [21], outputting 2D waypoints, and utilizing PID controllers for lateral and longitudinal control. PlanT [22] takes target-level environment as input, employs Transformers for fusion, and outputs 2D waypoints. It visualizes attention weights to demonstrate the interpretability of driving decisions. Some studies modularize end-to-end models, such as Sadat et al. [23] and Hu et al. [24], who predict the motion trends of other participants in the environment between the model input and output, enhancing safety during model planning and interpretability during decision-making. IL requires large datasets during training to enhance the model's generalization ability but may struggle to adapt to significantly different new scenarios from the training dataset. To alleviate the limitations posed by insufficiently comprehensive datasets, some studies enrich training datasets by acquiring driving data from other vehicles on the road. For instance, Zhang et al. [25] and Chen et al. [26] create more diverse driving datasets by capturing the driving behaviors of other vehicles. 端到端自动驾驶技术在学习方法上可分为模仿学习(IL)和强化学习(RL)。模仿学习从大量的专家示范中学习,模仿专家在各种驾驶场景中的行为,而强化学习则通过与环境的交互累积奖励或惩罚,旨在最大化累积奖励[15]。虽然 RL 不需要预先准备训练数据,但其在训练过程中的效率往往较低。端到端自动驾驶最早可追溯到 1988 年的 ALVINN [16],它利用全连接神经网络处理来自摄像头和雷达传感器的数据并输出转向值。近年来,随着深度学习领域的不断进步和计算能力的提高,出现了各种端到端自动驾驶方法,它们在输入格式、中间过程和输出格式等方面表现出差异。Hawke 等人[17]利用真实世界的交通环境数据训练了一个神经网络模型,从三个不同方向的摄像头输入数据并直接输出控制信号。TransFuser [18]、[19] 利用了 Transformers [20] 中的注意力机制,通过 CARLA 模拟器 [21] 中的多模态融合将摄像头输入和激光雷达数据结合起来,输出 2D 航点,并利用 PID 控制器进行横向和纵向控制。PlanT [22] 将目标级环境作为输入,采用变换器进行融合,并输出二维航点。它将注意力权重可视化,以展示驾驶决策的可解释性。一些研究将端到端模型模块化,如 Sadat 等人[23] 和 Hu 等人[24]。 [24],他们在模型输入和输出之间预测环境中其他参与者的运动趋势,从而提高了模型规划过程中的安全性和决策过程中的可解释性。IL 在训练过程中需要大量数据集,以增强模型的泛化能力,但可能难以适应与训练数据集明显不同的新场景。为了缓解数据集不够全面所带来的限制,一些研究通过获取道路上其他车辆的驾驶数据来丰富训练数据集。例如,Zhang 等人[25]和 Chen 等人[26]通过捕捉其他车辆的驾驶行为创建了更多样化的驾驶数据集。
There are also studies that combine the aforementioned methods. For instance, Wang et al. [27] proposed the Neural RRT* algorithm, which utilizes neural networks to predict the probability distribution of the optimal path on the map and employs RRT* for searching. Zhang et al. [28] enhanced the performance of Neural RRT* in predicting the probability distribution of the optimal path by utilizing Generative Adversarial Networks (GAN). Although such methods enhance path planning efficiency, the planned paths cannot be directly followed by the vehicle control. Zhao et al. [29] employed GAN to learn the relationship between starting points, endpoints, and sequences of control actions in unstructured road environments. They integrated the RRT algorithm to extend the planned trajectory length. Although the algorithm can generate high-quality trajectories, they are not necessarily optimal. Li et al. [30] utilized convolutional neural networks to predict priority search areas in grid maps, followed by Monte-Carlo tree search (MCTS) for path planning. After path optimization and velocity planning, a feasible trajectory is generated. Du et al. [31] addressed lane-changing scenarios by using two neural networks: one predicts the feasibility of lane-changing, while the other predicts the lane-changing time and longitudinal displacement to determine the lane-changing trajectory 还有一些研究结合了上述方法。例如,Wang 等人[27] 提出了神经 RRT* 算法,该算法利用神经网络预测地图上最优路径的概率分布,并采用 RRT* 进行搜索。Zhang 等人[28]利用生成对抗网络(GAN)提高了神经 RRT* 预测最优路径概率分布的性能。虽然这些方法提高了路径规划效率,但车辆控制无法直接遵循规划的路径。Zhao 等人[29] 利用 GAN 学习非结构化道路环境中起点、终点和控制行动序列之间的关系。他们集成了 RRT 算法,以扩展计划的轨迹长度。虽然该算法能生成高质量的轨迹,但并不一定是最优的。Li 等人[30]利用卷积神经网络预测网格图中的优先搜索区域,然后利用蒙特卡洛树搜索(MCTS)进行路径规划。经过路径优化和速度规划后,生成可行的轨迹。Du 等人[31]使用两个神经网络处理变道场景:一个预测变道的可行性,另一个预测变道时间和纵向位移,以确定变道轨迹
Based on the above methods, in unstructured environments, traditional trajectory planning methods, although reliable, necessitate manual design of algorithm parameters and often result in high time and space complexity during the solving process. Trajectory planning methods based on optimal control can generate high-quality trajectories while considering vehicle kinematic constraints, however, they rely on a feasible initial solution to reduce optimization time. The quality of the initial feasible solution directly impacts the optimization rate [3], [13], [14]. In unstructured environments, end-to-end trajectory planning requires longer trajectories compared to structured environments. The end-to-end prediction trajectory is difficult to satisfy the vehicle's kinematics and has error fluctuations, making it unsuitable for vehicle control module tracking. Additionally, obstacles in such environments exhibit diverse forms, necessitating a fusion of complementary approaches with traditional methods to accomplish the task [27][29]. 基于上述方法,在非结构化环境中,传统的轨迹规划方法虽然可靠,但必须手动设计算法参数,而且在求解过程中往往会导致较高的时间和空间复杂度。基于最优控制的轨迹规划方法可以在考虑车辆运动学约束的同时生成高质量的轨迹,但它们依赖于可行的初始解来减少优化时间。初始可行解的质量直接影响优化率 [3]、[13]、[14]。在非结构化环境中,与结构化环境相比,端到端轨迹规划需要更长的轨迹。端到端预测轨迹很难满足车辆的运动学要求,并且存在误差波动,因此不适合用于车辆控制模块的跟踪。此外,此类环境中的障碍物形式多样,需要将互补方法与传统方法融合起来才能完成任务[27][29]。
Drawing upon the advantages and disadvantages of the aforementioned algorithms, this study proposes a novel trajec- 本研究借鉴了上述算法的优缺点,提出了一种新颖的跟踪算法。
Fig. 1. Overall framework 图 1.总体框架
tory planning approach that integrates the concise and efficient nature of end-to-end methods with the ability of optimal control methods to learn driving trajectories while considering vehicle kinematic constraints and collision avoidance constraint. We employ a neural network model to not only learn 2D path point information but also the control inputs and state information of the vehicle, including vehicle position, heading angle, velocity, and front-wheel steering angle. The output values of the neural network serve as the initial solution for the optimal control algorithm, which refines them to yield a locally optimal trajectory. Simultaneously, the locally optimal solution serves as the ground truth for training the neural network. From the perspective of optimal control, the neural network efficiently provides an initial solution, thereby accelerating the optimization iterations of the optimal control problem. From the standpoint of end-to-end trajectory planning, the optimal control proposition ensures that the trajectory adheres to the vehicle's kinematic constraints, facilitating the tracking by the control system. Consequently, this research presents a novel approach to trajectory planning by combining endto-end trajectory planning with optimal control propositions, leveraging their complementary strengths to enhance planning efficiency and trajectory feasibility. 路径规划方法将端到端方法的简洁高效与最优控制方法的学习驾驶轨迹能力相结合,同时考虑车辆运动学约束和避免碰撞约束。我们采用神经网络模型,不仅学习二维路径点信息,还学习车辆的控制输入和状态信息,包括车辆位置、航向角、速度和前轮转向角。神经网络的输出值可作为最优控制算法的初始解,该算法会对其进行细化,以生成局部最优轨迹。同时,局部最优解也是训练神经网络的基本事实。从优化控制的角度来看,神经网络能有效提供初始解,从而加速优化控制问题的优化迭代。从端到端轨迹规划的角度来看,最优控制命题可确保轨迹遵守飞行器的运动学约束,从而促进控制系统的跟踪。因此,本研究提出了一种新的轨迹规划方法,将端到端轨迹规划与最优控制命题相结合,利用二者的互补优势提高规划效率和轨迹可行性。
A. Contributions A.捐款
The main contributions of this paper are as follows: 本文的主要贡献如下:
We integrate neural networks with optimal control to devise a novel trajectory planning architecture, demonstrating the feasibility of this approach and enhancing both the efficiency and quality of trajectory planning. 我们将神经网络与最优控制相结合,设计出一种新颖的轨迹规划架构,证明了这种方法的可行性,并提高了轨迹规划的效率和质量。
We utilize graph neural networks to extract and encode information from unstructured environments, leading to a more streamlined and efficient neural network structure. 我们利用图神经网络从非结构化环境中提取和编码信息,从而形成了一种更精简、更高效的神经网络结构。
Online Prediction 在线预测
3) Neural networks predict trajectories as initial solutions for optimal control, simplifying the process of obtaining initial solutions and thereby improving trajectory planning efficiency. 3) 神经网络预测轨迹作为优化控制的初始解,简化了获取初始解的过程,从而提高了轨迹规划效率。
4) Optimal control optimizes the trajectory predicted by the neural network, ensuring compliance with vehicle kinematic constraints and obstacle avoidance constraints. 4) 优化控制可优化神经网络预测的轨迹,确保符合车辆运动学约束和避障约束。
B. Organization B.组织结构
The remaining structure of this paper is outlined as follows. In Section III, we provide a brief description of trajectory planning propositions and introduce the algorithmic framework of this study. Section IV elaborates on the neural network structure and optimal control algorithm employed in the planning algorithm. Section presents the implementation details of our algorithm and compares it with mainstream planning algorithms through experimental evaluation. Section VI concludes our research findings. 本文的其余结构概述如下。在第三节中,我们简要介绍了轨迹规划命题,并介绍了本研究的算法框架。第四节阐述了规划算法中采用的神经网络结构和优化控制算法。 节介绍了我们算法的实现细节,并通过实验评估将其与主流规划算法进行了比较。第六节总结了我们的研究成果。
III. System Framework Overview III.系统框架概述
A. Trajectory Planning Problem A.轨迹规划问题
The core objective of trajectory planning lies in identifying a safe and feasible trajectory given known environmental information and the starting point. Vehicle trajectory planning must initially satisfy obstacle avoidance constraints and vehicle kinematic constraints. In this paper, a trajectory is defined as . The trajectory should connect the initial state with the goal state . Under a certain cost function, 轨迹规划的核心目标是在已知环境信息和起点的情况下确定一条安全可行的轨迹。车辆轨迹规划最初必须满足避障约束和车辆运动学约束。本文将轨迹定义为 。轨迹应连接初始状态 和目标状态 。在一定的成本函数下
Fig. 2. Network architecture schematic diagram 图 2.网络架构示意图
the trajectory should be locally optimal. Therefore, the path planning proposition can be defined as follows: 轨迹应该是局部最优的。因此,路径规划命题可定义如下:
represents the set of all feasible trajectories, denotes the optimal trajectory, and signifies the free space where collision with obstacles is avoided. 表示所有可行轨迹的集合, 表示最优轨迹, 表示避免与障碍物碰撞的自由空间。
B. System Framework B.系统框架
Based on graph neural networks and the optimal control trajectory planning algorithm, as depicted in Fig. 11 the process consists of two operational phases: offline training and online prediction. 如图 11 所示,基于图神经网络和最优控制轨迹规划算法,该过程包括两个运行阶段:离线训练和在线预测。
offline training: Offline training aims to enable the neural network to learn how to generate an initial trajectory under various tasks, serving as the initial solution for the optimal control problem. We prepared a large number of environmental maps and randomly specified the initial and target positions of the vehicle. Each pair of starting positions and environmental configurations constitutes a task. Subsequently, we utilized the Hybrid A* algorithm to solve each task and obtained feasible trajectories through velocity planning. These feasible trajectories serve as the initial solutions for the optimal control problem, which are refined to yield locally optimal trajectories through optimization. The combination of tasks and trajectories forms a sample, and the accumulation of all samples constitutes the training dataset for the neural network. 离线训练:离线训练旨在使神经网络学会如何在各种任务下生成初始轨迹,作为最优控制问题的初始解。我们准备了大量的环境地图,并随机指定了车辆的初始位置和目标位置。每一对初始位置和环境配置构成一个任务。随后,我们利用混合 A* 算法来解决每个任务,并通过速度规划获得可行的轨迹。这些可行轨迹可作为优化控制问题的初始解,通过优化对其进行细化,从而获得局部最优轨迹。任务和轨迹的组合构成一个样本,所有样本的累积构成神经网络的训练数据集。
online prediction: Online prediction involves replacing the path planning and velocity planning stages with the trained neural network to enhance planning efficiency. The starting position, target position, and environment configuration of vectorization are used as inputs for graph neural networks. 在线预测:在线预测是指用训练有素的神经网络取代路径规划和速度规划阶段,以提高规划效率。矢量化的起始位置、目标位置和环境配置被用作图神经网络的输入。
Through network inference, the predicted vehicle trajectory is outputted. This predicted trajectory serves as the initial solution for the optimal control problem, which is further refined to yield a locally optimal trajectory through optimization. The output of the neural network prediction determines the success of the optimal control problem's solution and the efficiency of finding the optimal value. High-quality prediction results will shorten the solution time of the optimal control problem. If the optimal control problem fails to converge, a trajectory will be solved using the rule-based Hybrid A* and Speed planner before reattempting optimization. 通过网络推理,输出预测的车辆轨迹。该预测轨迹可作为最优控制问题的初始解,并通过优化进一步细化以获得局部最优轨迹。神经网络预测的输出结果决定了最优控制问题解决方案的成功与否,以及找到最优值的效率。高质量的预测结果将缩短优化控制问题的求解时间。如果优化控制问题无法收敛,则将使用基于规则的混合 A* 和速度规划器求解轨迹,然后再重新尝试优化。
IV. Proposed Trajectory Planner IV.拟议的轨迹规划器
In this section, we introduce two critical phases of trajectory planning in this paper: 1) employing neural networks to predict trajectories as initial solutions for OCP, and 2) optimizing the initial solutions provided by the neural networks to generate a locally optimal trajectory. The specific details are outlined below. 在本节中,我们将介绍本文中轨迹规划的两个关键阶段:1) 利用神经网络预测轨迹,作为 OCP 的初始解;2) 优化神经网络提供的初始解,生成局部最优轨迹。具体细节概述如下。
A. Learning-based Initial Solution Generation A.基于学习的初始解决方案生成
Our neural network architecture draws inspiration from VectorNet [32] and PlanT. Unlike CNN models that take grid-based pixel points as input, we represent tasks using object-level representations and extract features through Graph Neural Networks (GNNs). In addition, our network structure is relatively simple and there is enough room for improvement. The network structure is depicted in Fig. 2 . 我们的神经网络架构汲取了 VectorNet [32] 和 PlanT 的灵感。与将基于网格的像素点作为输入的 CNN 模型不同,我们使用对象级表示法来表示任务,并通过图神经网络(GNN)来提取特征。此外,我们的网络结构相对简单,有足够的改进空间。网络结构如图 2 所示。
Task vectorization and subgraph generation: Before vectorization, to standardize the initial state of each task, we convert the absolute positions of obstacles and target states in the Cartesian coordinate system into relative positions with respect to the initial state. The vectorization process is illustrated in Fig. 3 The start and end vectors contain the vehicle's initial coordinates and heading angle, forming the starting vector and the target vector . The vertices of each obstacle 任务矢量化和子图生成:在矢量化之前,为了使每个任务的初始状态标准化,我们将直角坐标系中障碍物和目标状态的绝对位置转换为相对于初始状态的相对位置。矢量化过程如图 3 所示。起点和终点矢量包含车辆的初始坐标和航向角,形成起点矢量 和目标矢量 。每个障碍物的顶点
are connected to each other to form obstacle vectors . To enable the GNN to efficiently acquire task information, we interconnect obstacle vectors, with each obstacle forming an obstacle subgraph. Simultaneously, the starting point and the endpoint form a state subgraph. All subgraphs constitute the task graph. 相互连接,形成障碍向量 。为了让 GNN 有效获取任务信息,我们将障碍向量相互连接,每个障碍形成一个障碍子图。同时,起点和终点构成一个状态子图。所有子图构成任务图。
Fig. 3. Vectorization 图 3.矢量化
Neural network structure: To fully utilize the positional information contained within each subgraph, we perform a three-layer feature fusion for each subgraph, which includes the feedforward phase of a fully connected neural network and graph feature concatenation. The single-layer feature fusion propagated in each subgraph can be defined as follows: 神经网络结构:为了充分利用每个子图中包含的位置信息,我们对每个子图进行了三层特征融合,其中包括全连接神经网络的前馈阶段和图特征串联。在每个子图中传播的单层特征融合可定义如下:
Here, represents the subgraph features of the -th subgraph in the -th layer. The is a multi-layer perceptron consisting of three fully connected layers with 128 neurons in the hidden layer. The denotes the feature aggregation layer, and represents the feature concatenation layer. After three layers of feature fusion, to extract important features from the subgraphs within the task, we perform max pooling on each subgraph as follows: 这里, 表示 层中 个子图的子图特征。 是一个多层感知器,由三个全连接层组成,隐藏层中有 128 个神经元。 表示特征聚合层, 表示特征串联层。经过三层特征融合后,为了提取任务中子图的重要特征,我们对每个子图进行最大池化处理,具体如下:
represents the max pooling layer. 表示最大池化层。
The max-pooled vector for each subgraph represents its most salient features. These features are concatenated to form a new graph structure, which then undergoes three rounds of feature aggregation to complete the feature extraction of the task graph. 每个子图的最大集合向量代表其最突出的特征。这些特征被串联起来形成一个新的图结构,然后经过三轮特征聚合,完成任务图的特征提取。
Following the feature fusion and extraction of the task graph, to reduce the size of the network, the structure only predicts five variables that constitute the vehicle's trajectory, denoted as . Here, represent the position coordinates, heading angle, speed, and front-wheel steering angle at the -th trajectory point, respectively. Unpredicted variables, such as vehicle acceleration and front-wheel steering angular velocity , are computed from and . 在对任务图进行特征融合和提取后,为减小网络规模,该结构只预测构成车辆轨迹的五个变量,记为 。在这里, 分别代表 轨迹点的位置坐标、航向角、速度和前轮转向角。车辆加速度 和前轮转向角速度 等未预测变量由 和 计算得出。
B. Optimize the Initial Trajectory B.优化初始轨迹
In this section, based on previous research [2], [14], [33], we formulate an OCP using neural network outputs as the initial solution. This includes defining the cost function to be minimized and the various constraints to be applied. Finally, we briefly introduce the solution approach for the OCP. 在本节中,我们将在先前研究[2]、[14]、[33]的基础上,利用神经网络的输出结果作为初始解决方案,制定一个 OCP。这包括定义要最小化的成本函数和要应用的各种约束条件。最后,我们简要介绍 OCP 的求解方法。
Problem Overview: Usually, the OCP for trajectory planning can be described in the following standard form: 问题概述:通常,轨迹规划的 OCP 可以用下面的标准形式来描述:
In this context, represents the cost function that includes both control and state variables. denotes the derivative of the state variable with respect to time. is the control variable that provides inputs to the vehicle. represents the relationship between the change in vehicle state and the current control variables, also known as the vehicle kinematic equation. and represent the initial and final states of the vehicle, respectively. The terms and denote the maximum and minimum control inputs. The workspace for trajectory planning is defined as , where represents the obstacle space. The function maps the vehicle's state and inputs to the workspace, presenting them as a path that should not intersect with obstacles, i.e., . The above defines the cost function and various constraints. Next, we will elaborate on these components in detail. 在这里, 表示成本函数,其中包括控制变量和状态变量。 表示状态变量 相对于时间的导数。 是控制变量,为车辆提供输入。 表示车辆状态变化与当前控制变量之间的关系,也称为车辆运动方程。 和 分别代表车辆的初始状态和最终状态。术语 和 表示最大和最小控制输入。轨迹规划的工作空间定义为 ,其中 表示障碍物空间。函数 将车辆的状态和输入映射到工作空间,并将其显示为一条不应与障碍物相交的路径,即 。以上定义了成本函数和各种约束条件。接下来,我们将详细阐述这些组件。
2) Various constraints: The constraints include kinematic constraints, initial and final state constraints, and obstacle constraints. 2) 各种约束条件:约束条件包括运动学约束条件、初始和最终状态约束条件以及障碍物约束条件。
kinematic constraints: In unstructured environments, vehicles typically need to operate at low speeds. Therefore, this paper employs the bicycle model to represent the vehicle's kinematics. A schematic diagram of the vehicle parameters is shown in Fig. 4. The equation in equation (5) is expanded as follows: 运动限制:在非结构化环境中,车辆通常需要低速行驶。因此,本文采用自行车模型来表示车辆的运动学。车辆参数示意图如图 4 所示。公式 (5) 中的 扩展如下:
and represent the coordinates of the vehicle's rear axle center in the Cartesian coordinate system. denotes the vehicle's heading angle, while and represent the 和 表示车辆后轴中心在直角坐标系中的坐标。 表示车辆的航向角,而 和 表示
Fig. 4. Geometric parameters related to vehicle kinematics. 图 4.与车辆运动学有关的几何参数。
vehicle's velocity and acceleration, respectively. and stand for the front-wheel steering angle and angular velocity. denotes the wheelbase of the vehicle. Additionally, in the practical operation of the vehicle, it should adhere to physical or mechanical constraints, satisfying: 分别代表车辆的速度和加速度。 和 代表前轮转向角和角速度。 表示车辆的轴距。此外,在车辆的实际运行过程中,应遵守物理或机械约束,满足以下条件:
State constraints: When solving the OCP problem, it's essential to meet the fundamental requirements of the vehicle's initial and final states in the trajectory planning task. As in equation (5), the initial and final states should satisfy: 状态约束:在求解 OCP 问题时,必须满足轨迹规划任务中对飞行器初始状态和最终状态的基本要求。如式(5)所示,初始状态和最终状态应满足:
In this article, we assume that at the beginning and end of a trajectory planning task, the vehicle has no input effect, i.e 在本文中,我们假设在轨迹规划任务的开始和结束时,车辆没有输入效应,即
Obstacle constraints: In unstructured environments, obstacles exhibit diverse forms, and the establishment of obstacle avoidance constraints is crucial for the completion of trajectory planning tasks. In previous research, [13] utilized the triangle area method to establish collision avoidance constraints. To reduce the number of constraints, [2] and [3] introduced collision-free corridors, confining the vehicle within these corridors. In this paper, to simplify the processing procedure, we adopt the triangle area method to establish constraints. 障碍物约束在非结构化环境中,障碍物的形式多种多样,建立避障约束对于完成轨迹规划任务至关重要。在之前的研究中,[13] 利用三角形区域法建立了避撞约束。为了减少约束条件的数量,[2]和[3]引入了无碰撞走廊,将车辆限制在这些走廊内。在本文中,为了简化处理程序,我们采用三角形区域法来建立约束条件。
The triangle area method is used to determine whether a point lies inside a polygon. As illustrated in Fig. 54 the relationship holds for the triangle formed by point and the vertices of polygon : 三角形面积法用于确定点是否位于多边形内部。如图 54 所示,对于点 和多边形 的顶点所构成的三角形,该关系成立:
Fig. 5. Triangle area method 图 5.三角形面积法
When point lies inside polygon , then: 当点 位于多边形 内时,则
otherwise: 否则
We establish collision avoidance constraints by considering the four vertices of the vehicle's quadrilateral and each obstacle polygon, as well as by considering each vertex of the obstacle polygons and the vehicle's quadrilateral. This dual approach ensures comprehensive collision avoidance. 我们通过考虑车辆四边形的四个顶点和每个障碍多边形,以及考虑障碍多边形的每个顶点和车辆四边形,来建立避免碰撞的约束条件。这种双重方法可确保全面避免碰撞。
3) Cost function: The purpose of establishing a cost function is to find a locally optimal solution that minimizes the cost within the set of constraint-satisfying solutions. Constructing the cost function for the trajectory planning problem should consider multiple criteria, including the smoothness of the vehicle's trajectory, safety, and the time required to complete the task. Different weight coefficients should be assigned to these criteria to reflect their relative importance. 3) 成本函数:建立成本函数的目的是找到一个局部最优解,在满足约束条件的解集中使成本最小。构建轨迹规划问题的成本函数应考虑多个标准,包括车辆轨迹的平滑性、安全性和完成任务所需的时间。应为这些标准分配不同的权重系数,以反映其相对重要性。
To enhance the smoothness of the trajectory, sudden changes in speed and front-wheel steering angle should be minimized. This contributes to ride comfort and the effectiveness of the vehicle's control module in tracking the planned path. Therefore, let: 为了提高行驶轨迹的平稳性,应尽量减少速度和前轮转向角的突然变化。这有助于提高驾驶舒适性和车辆控制模块跟踪计划路径的有效性。因此,让
Regarding the safety of the trajectory, it should maintain a safe distance from obstacles. Assuming the geometric center of each obstacle is , the performance metric can be designed as follows: 关于轨迹的安全性,它应与障碍物保持安全距离。假设每个障碍物的几何中心为 ,性能指标可设计如下:
Here, represents the squared distance between the vehicle and the center of obstacle. The larger the distance, the smaller the cost function. Assuming that each planned task consumes the same amount of time, i.e., is a constant, the in (4) can be expressed as: 这里, 表示车辆与障碍物中心之间距离的平方。距离越大,成本函数越小。假设每项计划任务消耗的时间相同,即 为常数,则 (4) 中的 可表示为:
and represent the weight coefficients of the corresponding indicators. 和 代表相应指标的权重系数。
4) Solving OCP: The solution of trajectory planning OCP requires the discretization of continuous variables, thus forming a NLP. When solving NLP, the establishment of initial solutions will affect whether the problem can be successfully solved and the speed of solving. Therefore, in previous studies, some scholars [2], [3], [33] used the Hybrid A* algorithm and its variants as the basis to construct initial solutions. Building upon this foundation, this paper proposes the use of GNN to predict trajectories for constructing initial solutions. For NLP with existing initial solutions, we utilize the open-source solver IPOPT for solution, thereby completing the entire trajectory planning process. Some detailed basic parameters are listed in Table II 4) OCP 的求解:轨迹规划 OCP 的求解需要将连续变量离散化,从而形成 NLP。在求解 NLP 时,初始解的建立将影响问题能否顺利求解以及求解速度。因此,在以往的研究中,一些学者[2]、[3]、[33]将混合 A* 算法及其变体作为构建初始解的基础。在此基础上,本文提出使用 GNN 预测轨迹来构建初始解。对于已有初始解的 NLP,我们利用开源求解器 IPOPT 进行求解,从而完成整个轨迹规划过程。表 II 列出了一些详细的基本参数
TABLE I 表 I
BASIC PARAMETERS 基本参数
Parameter 参数
Description 说明
Value 价值
Vehicle wheelbase 车辆轴距
2.8 m
Vehicle front hang length 车辆前悬挂长度
0.96 m
Vehicle rear hang length 车辆后悬挂长度
0.929 m
Vehicle width 车辆宽度
1.9 m
Maximum acceleration 最大加速度
Maximum angular velocity 最大角速度
Maximum velocity 最大速度
Maximum steering angle 最大转向角
Number of GNN prediction points GNN 预测点数
30
The xoy resolution for Hybrid 混合动力 的 xoy 分辨率
Heading resolution for Hybrid 混合 的航向分辨率
0.785
Penalty for 对 的处罚
1.0
Penalty for 对 的处罚
1.0
Number of optimization points 优化点数量
120
V. EXPERIMENT ANALYSIS V.实验分析
A. Implementation Details A.实施细节
To validate the effectiveness of the proposed trajectory planning algorithm, a series of simulation experiments were conducted in typical scenarios. Detailed training procedures and simulation parameters are provided below. 为了验证所提出的轨迹规划算法的有效性,我们在典型场景中进行了一系列模拟实验。下面提供了详细的训练程序和仿真参数。
Training Details: We prepared 800 maps with different random distributions of obstacles, and for each map, 15 pairs of random start and goal positions were selected. Each pair was solved using the Hybrid algorithm, and the resulting trajectory was optimized using IPOPT, forming one training sample. Due to the incompleteness of Hybrid A*, there were instances where no solution was found during task solving. After solving, a total of 10,426 local optimal trajectories were obtained as samples. of the samples were used as the training set, while the remaining samples were used as the test set. During training, the Adam optimizer was utilized with a learning rate of 0.001. Training was conducted on an RTX3090 GPU for 200 epochs, with a warm-up period of 50 epochs, and a batch size of 32 . 训练细节我们准备了 800 幅不同障碍物随机分布的地图,并为每幅地图随机选择了 15 对起点和目标位置。使用混合 算法求解每一对,并使用 IPOPT 优化得到的轨迹,形成一个训练样本。由于混合 A* 算法的不完整性,在任务求解过程中会出现找不到解的情况。求解后,共获得 10,426 条局部最优轨迹作为样本。其中 个样本作为训练集,其余样本作为测试集。在训练过程中,亚当优化器的学习率为 0.001。在 RTX3090 GPU 上进行了 200 次训练,预热期为 50 次,批量大小为 32。
Simulation Details: During algorithm simulation testing, to facilitate subsequent comparison with mainstream planning algorithms, GNN inference was conducted on an Intel i9 CPU to align with other computations. The simulation platform utilized Python for implementation. 模拟细节:在算法模拟测试期间,为了便于随后与主流规划算法进行比较,GNN 推理在英特尔 i9 CPU 上进行,以便与其他计算保持一致。模拟平台使用 Python 实现。
Fig. 6. Algorithm testing environment 图 6.算法测试环境
B. Analysis of the Proposed Planner B.对拟议规划者的分析
This section demonstrates the performance of the proposed method in different obstacle environments. 本节展示了拟议方法在不同障碍物环境中的性能。
We tested the algorithm in four different obstacle environments as shown in Fig. 6, 6(a) and 6(b) consist of rectangular obstacles and irregular obstacles, while 6(c) and 6(d) comprise several irregular obstacles. These scenarios highlight the algorithm's ability to plan longer trajectories and handle complex obstacles. In all four scenarios, the proposed algorithm successfully completes the trajectory planning tasks, with the green points in the figures representing the trajectory points formed by the vehicle. 如图 6 所示,我们在四种不同的障碍物环境中测试了该算法,其中 6(a) 和 6(b) 由矩形障碍物和不规则障碍物组成,而 6(c) 和 6(d) 则由多个不规则障碍物组成。这些场景凸显了算法规划较长轨迹和处理复杂障碍物的能力。在所有四个场景中,所提出的算法都成功完成了轨迹规划任务,图中的绿色点代表了车辆形成的轨迹点。
Due to space limitations, we use 6(a) as a representative example to illustrate the trajectory generation process in detail, as shown in Fig. 7 Given the known environmental information, we use the trained neural network model to predict the vehicle trajectory. The inputs include environmental information, starting point information, and target point information. Through the GNN, the model outputs predicted trajectory points. In Fig. 7. Fig. 7(a) represents the predicted path points, consisting of 30 discrete points. Each discrete point includes values for the vehicle's orientation angle , steering angle , and velocity . The steering angle velocity and acceleration required for the OCP problem are calculated from the steering angle and velocity. To match the number of predicted points with the optimization points required by the OCP, we perform linear interpolation between every two trajectory points to add three additional trajectory points, as shown in 7 (b). The interpolated trajectory in 7(b) includes and , which will serve as the initial solution for the IPOPT iterative optimization. 77(c) shows the results after solving the OCP, and 由于篇幅有限,我们以 6(a) 为代表,详细说明轨迹生成过程,如图 7 所示 在已知环境信息的基础上,我们使用训练有素的神经网络模型来预测车辆轨迹。输入包括环境信息、起点信息和目标点信息。通过 GNN,模型输出预测的轨迹点。如图 7 所示。图 7(a) 表示预测的轨迹点,由 30 个离散点组成。每个离散点包括车辆的方向角 、转向角 和速度 。根据转向角和速度计算出 OCP 问题所需的转向角速度 和加速度 。为了使预测点的数量与 OCP 所需的优化点相匹配,我们在每两个轨迹点之间进行线性插值,以增加三个额外的轨迹点,如 7 (b) 所示。7(b) 中的插值轨迹包括 和 ,它们将作为 IPOPT 迭代优化的初始解。77(c) 显示了 OCP 求解后的结果,以及
7(d) compares the paths before and after optimization. 7(d) 比较了优化前后的路径。
Fig. 7. Trajectory generation and optimization process 图 7.轨迹生成和优化过程
Additionally, Fig 8 provides a detailed comparison of the trajectory parameters between the predicted trajectory and the local optimal trajectory from Fig 7 (d). Fig 8 (a), Fig 8 (b), Fig 8 (c), and Fig 8 (d) respectively show the comparisons of the path, heading angle, velocity, and steering angle between the predicted trajectory and the local optimal trajectory. 此外,图 8 详细比较了预测轨迹与图 7 (d) 中的局部最优轨迹之间的轨迹参数。图 8 (a)、图 8 (b)、图 8 (c) 和图 8 (d) 分别显示了预测轨迹与局部最优轨迹之间的路径、航向角、速度和转向角的比较。
It can be observed that the prediction from the neural network closely matches the results after OCP optimization. Therefore, the network output, after interpolation processing, can serve as the initial solution for the OCP problem. 可以看出,神经网络的预测结果与 OCP 优化后的结果非常吻合。因此,经过插值处理后的网络输出可以作为 OCP 问题的初始解。
C. Comparison with mainstream planners C.与主流规划者的比较
In this section, the neural network combined with optimal control proposed in this paper is compared with current mainstream non-structured road trajectory planning algorithms combined with optimal control, including Hybrid A* and RRT* combined with optimal control, in both the initial trajectory generation and trajectory optimization stages. The testing environment is shown in Figure 6. The specific comparison results are shown in Table II 在本节中,本文提出的结合最优控制的神经网络与目前主流的结合最优控制的非结构化道路轨迹规划算法(包括结合最优控制的混合 A* 算法和结合最优控制的 RRT* 算法)在初始轨迹生成和轨迹优化阶段进行了比较。测试环境如图 6 所示。具体比较结果如表 II 所示
In the first stage of planning, the initial solution is provided by the algorithm or the neural network. It can be observed that the RRT* algorithm exhibits the highest efficiency and the shortest time consumption due to its disregard for the vehicle's kinematic constraints. The Hybrid algorithm consumes more time, particularly in environments like map (a) and map (b) where the starting point is relatively distant from the target, as graph search algorithms require step-by-step expansion from the starting point to the endpoint, resulting in lower efficiency with longer distances. In comparison, 在规划的第一阶段,由算法或神经网络提供初始解决方案。可以看出,由于 RRT* 算法不考虑车辆的运动学约束,因此效率最高,耗时最短。混合 算法消耗的时间较多,特别是在地图(a)和地图(b)这样起点距离目标相对较远的环境中,因为图搜索算法需要从起点到终点逐步扩展,导致距离越远效率越低。相比之下
Fig. 8. Comparison between GNN trajectory prediction and optimization trajectory 图 8.GNN 轨迹预测与优化轨迹的比较
TABLE II 表 II
PERFORMANCE COMPARISON 性能比较
Cases 案例
Method 方法
刨削时间
Planing
time(s)
选择时间
Opt.
time(s)
总时间
Total
time(s)
map(a) 映射
Hybrid A*-OCP 混合 A*-OCP
4.487
5.549
10.036
RRT*-OCP
0.167
23.641
23.808
GNN-OCP
0.695
6.652
8.347
Hybrid A*-OCP 混合 A*-OCP
3.988
4.870
8.858
RRT*-OCP
0.284
15.904
16.188
GNN-OCP
0.709
6.956
7.665
map(c) 映射
Hybrid A*-OCP 混合 A*-OCP
1.089
8.134
9.223
RRT*-OCP
0.162
9.675
9.837
GNN-OCP
0.691
2.874
3.565
Hybrid A*-OCP 混合 A*-OCP
1.756
8.137
9.893
RRT*-OCP
0.152
13.948
14.100
GNN-OCP
0.687
8.507
9.194
the time consumption of the GNN remains stable at around 700 ms . This stability arises from the fact that, given the same computational unit, the inference speed of the neural network is only determined by the network structure. GNN 的耗时稳定在 700 毫秒左右。之所以如此稳定,是因为在计算单元相同的情况下,神经网络的推理速度只取决于网络结构。
In the second stage of planning, i.e., the OCP optimization stage, the Hybrid -OCP exhibits the shortest time consumption among the map(a), map(b), and map(d) experimental scenarios, while the RRT*-OCP has the longest time consumption among all experimental scenarios. The GNN-OCP slightly surpasses the Hybrid -OCP. In this stage, the duration of OCP optimization largely depends on the quality of the initial solution, the closer the initial solution to the optimal solution, the lower the time consumption and the faster the optimization rate. Among the four proposed experimental scenarios, the GNN-OCP proposed in this paper exhibits the shortest total time, significantly improving trajectory planning efficiency. 在规划的第二阶段,即 OCP 优化阶段,混合 -OCP 在 map(a)、map(b) 和 map(d) 实验方案中耗时最短,而 RRT*-OCP 在所有实验方案中耗时最长。GNN-OCP 略微超过混合 -OCP。在这一阶段,OCP 优化的持续时间主要取决于初始解的质量,初始解越接近最优解,耗时越短,优化速度越快。在本文提出的四种实验方案中,GNN-OCP 的总时间最短,显著提高了轨迹规划效率。
To delve deeper into the performance analysis of the algorithms presented in Table II, we computed the differences between the planned trajectories in the first stage and the local optimal solutions in the second stage for each algorithm, 为了深入分析表 II 所列算法的性能,我们计算了每种算法第一阶段计划轨迹与第二阶段局部最优解之间的差异、
as shown in Fig. 9 Fig. 9(a) illustrates the cumulative sum of distances between the planned trajectory and the local optimal trajectory for each trajectory point. Figures 9 (b), 9 (c), and 9 d) depict the cumulative sums of differences for each trajectory point in terms of , and , respectively. It can be seen that the trajectory points predicted by the GNN are significantly closer to the locally optimal solution compared to the other algorithms. However, as shown in Table II in the experimental environments of , and , the optimization time of GNN-OCP is slightly higher than that of Hybrid A*. This is because the GNN faces a similar issue as the RRT* algorithm: it cannot completely satisfy the vehicle's kinematic constraints during trajectory prediction. The GNN predicts , and independently, ensuring each value adheres to its maximum and minimum constraints but without establishing correlations to meet the vehicle's kinematic equations. Compared to RRT*, the GNN's predicted trajectories are closer to the feasible solution range dictated by the vehicle's kinematic constraints, resulting in performance that is nearly on par with Hybrid A*-OCP. 如图 9 所示 图 9(a) 显示了每个轨迹点的计划轨迹与局部最优轨迹之间的距离累积和。图 9(b)、图 9(c)和图 9(d) 分别以 和 表示每个轨迹点的累计差值之和。可以看出,与其他算法相比,GNN 预测的轨迹点明显更接近局部最优解。然而,如表 II 所示,在 和 的实验环境中,GNN-OCP 的优化时间略高于 Hybrid A*。这是因为 GNN 面临着与 RRT* 算法类似的问题:在轨迹预测过程中无法完全满足车辆的运动学约束。GNN 对 和 进行独立预测,确保每个值都遵守其最大和最小约束,但没有建立相关性以满足车辆的运动方程。与 RRT* 相比,GNN 预测的轨迹更接近由车辆运动学约束条件决定的可行解决方案范围,因此性能几乎与混合 A*-OCP 相当。
Fig. 9. The difference distance between the first stage planning trajectory and the local optimal solution 图 9.第一阶段规划轨迹与局部最优解之间的差分距离
D. Parameter analysis and model defects D.参数分析和模型缺陷
As mentioned in Section IV, we employed a simple yet effective network. However, there is room for improvement in the network structure, and the algorithm parameters can be adjusted in various ways. This section will provide a brief analysis of the algorithm parameters. 如第四节所述,我们采用了一个简单而有效的网络。然而,网络结构仍有改进的余地,算法参数也可以通过各种方式进行调整。本节将对算法参数进行简要分析。
In selecting the number of trajectory points predicted by the neural network, we chose to use 30 prediction points. Due to inherent prediction errors in the neural network, the number of prediction points determines the smoothness of the predicted trajectory and the optimization rate. As shown in Fig. 10, Fig. 10. (a) illustrates a trajectory with 120 predicted points, where the trajectory's smoothness is compromised due to prediction errors. When the number of prediction points is reduced, as shown in Fig. 10 b), and linear interpolation is applied between the prediction points, the final result is displayed in Fig. 10. (c). 在选择神经网络预测的轨迹点数量时,我们选择使用 30 个预测点。由于神经网络固有的预测误差,预测点的数量决定了预测轨迹的平滑度和优化率。如图 10 所示,图 10. (a) 展示了一条有 120 个预测点的轨迹,由于预测误差,轨迹的平滑度大打折扣。当减少预测点数量(如图 10 b),并在预测点之间应用线性插值时,最终结果如图 10 c)所示。
Fig. 10. Prediction node interpolation processing 图 10.预测节点插值处理
When using neural networks, their performance tends to deteriorate when handling tasks significantly different from the training dataset, which is an inherent drawback of neural networks. In this study, the prediction error for trajectories increases with the length of the trajectory; the further from the starting point, the greater the error. As shown in Fig. 11 , Fig. 11 a) illustrates the predicted trajectory and the positions of the trajectory points. Fig. 11(b) depicts the distance between each predicted trajectory point and its corresponding true trajectory label point, starting from the initial point. The distance gradually increases with the length of the trajectory. Nevertheless, as long as the error remains within a reasonable range, it will not affect the calculation of the locally optimal trajectory during the optimization phase. 使用神经网络时,当处理的任务与训练数据集有显著差异时,其性能往往会下降,这是神经网络固有的缺点。在本研究中,轨迹预测误差随着轨迹长度的增加而增加;距离起点越远,误差越大。如图 11 所示,图 11 a) 显示了预测的轨迹和轨迹点的位置。图 11(b) 描述了从初始点开始,每个预测轨迹点与其对应的真实轨迹标签点之间的距离。距离随着轨迹长度的增加而逐渐增大。不过,只要误差保持在合理范围内,就不会影响优化阶段局部最优轨迹的计算。
VI. CONCLUSION VI.结论
This paper proposes a two-stage trajectory planning method based on GNN and numerical optimal control. In the first stage, the GNN efficiently extracts environmental task information to predict an initial trajectory. In the second stage, 本文提出了一种基于 GNN 和数值优化控制的两阶段轨迹规划方法。在第一阶段,GNN 有效提取环境任务信息,预测初始轨迹。在第二阶段
Fig. 11. Change in distance between predicted trajectory and actual trajectory 图 11.预测轨迹与实际轨迹之间距离的变化
optimal control computation is employed to optimize the trajectory, ensuring it meets vehicle kinematic constraints and collision avoidance requirements. This method simplifies the traditional trajectory planning process. Unlike conventional planning algorithms that require separate steps for path planning, speed planning, and trajectory optimization, the GNNbased initial planning benefits from the simplicity and efficiency of end-to-end trajectory planning. This reduces the need for extensive parameter tuning in traditional planning algorithms while enhancing the overall planning efficiency. 采用最优控制计算来优化轨迹,确保符合车辆运动学约束和避免碰撞的要求。这种方法简化了传统的轨迹规划过程。传统的规划算法需要分别进行路径规划、速度规划和轨迹优化,而基于 GNN 的初始规划则不同,它得益于端到端轨迹规划的简单性和高效性。这减少了传统规划算法中大量参数调整的需要,同时提高了整体规划效率。
The neural network structure proposed in this paper is simple and efficient, but there is significant room for improvement. Future work will consider larger datasets and deeper network structures to enhance the predictive capabilities of the neural network. In terms of prediction results, this paper uses parallel multilayer perceptrons to predict different trajectory parameters. While this approach achieves good results, it does not simultaneously incorporate different results into the vehicle kinematic model, which reduces the optimization process speed. Nonetheless, in the initial trajectory acquisition stage, the algorithm demonstrates sufficient efficiency to reduce the overall running time and simplify the workflow. The planning framework provided in this paper also offers a novel perspective for end-to-end trajectory planning: using numerical optimization methods to improve the trajectory quality predicted by neural networks. 本文提出的神经网络结构简单高效,但仍有很大的改进空间。未来的工作将考虑更大的数据集和更深的网络结构,以增强神经网络的预测能力。在预测结果方面,本文使用并行多层感知器来预测不同的轨迹参数。虽然这种方法取得了良好的效果,但它并没有将不同的结果同时纳入车辆运动学模型,从而降低了优化过程的速度。不过,在初始轨迹获取阶段,该算法表现出了足够的效率,缩短了整体运行时间,简化了工作流程。本文提供的规划框架还为端到端轨迹规划提供了一个新的视角:使用数值优化方法来提高神经网络预测的轨迹质量。
REFERENCES 参考文献
[1] Y. Guo, Z. Guo, Y. Wang, D. Yao, B. Li, and L. Li, "A survey of trajectory planning methods for autonomous driving - part i: Unstructured scenarios," IEEE Transactions on Intelligent Vehicles, pp. . [1] Y. Guo、Z. Guo、Y. Wang、D. Yao、B. Li 和 L. Li,"自动驾驶轨迹规划方法研究--第一部分:非结构化场景",IEEE Transactions Intelligent Vehicles,第 页:IEEE Transactions on Intelligent Vehicles, pp.
[2] B. Li, T. Acarman, Y. Zhang, Y. Ouyang, C. Yaman, Q. Kong, X. Zhong, and X. Peng, "Optimization-based trajectory planning for autonomous parking with irregularly placed obstacles: A lightweight iterative framework," IEEE Transactions on Intelligent Transportation Systems, vol. 23, no. 8, pp. 11970-11981, 2022. [2] B. Li, T. Acarman, Y. Zhang, Y. Ouyang, C. Yaman, Q. Kong, X. Zhong, and X. Peng, "Optimization-based trajectory planning for autonomous parking with irregularly placed obstacles:A lightweight iterative framework," IEEE Transactions on Intelligent Transportation Systems, vol. 23, no. 8, pp.
[3] J. Lian, W. Ren, D. Yang, L. Li, and F. Yu, "Trajectory planning for autonomous valet parking in narrow environments with enhanced hybrid a* search and nonlinear optimization," IEEE Transactions on Intelligent Vehicles, vol. 8, no. 6, pp. 3723-3734, 2023. [3] J. Lian, W. Ren, D. Yang, L. Li, and F. Yu, "Trajectory planning for autonomous valet parking in narrow environments with enhanced hybrid a* search and nonlinear optimization," IEEE Transactions on Intelligent Vehicles, vol. 8, no. 6, pp.
[4] W. Chen, Y. Chen, S. Wang, F. Kong, X. Zhang, and H. Sun, "Motion planning using feasible and smooth tree for autonomous driving," IEEE Transactions on Vehicular Technology, pp. 1-13, 2023. [4] W. Chen、Y. Chen、S. Wang、F. Kong、X. Zhang 和 H. Sun,"Motion planning using feasible and smooth tree for autonomous driving," IEEE Transactions on Vehicular Technology,第 1-13 页,2023 年。
[5] E. W. Dijkstra, "A note on two problems in connexion with graphs," in Edsger Wybe Dijkstra: His Life, Work, and Legacy, 2022, pp. 287-290. [5] E. W. Dijkstra,"A note on two problems in connexion with graphs",载于《Edsger Wybe Dijkstra:他的生活、工作和遗产》,2022 年,第 287-290 页。
[6] P. E. Hart, N. J. Nilsson, and B. Raphael, "A formal basis for the heuristic determination of minimum cost paths," IEEE transactions on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100-107, 1968. [6] P. E. Hart、N. J. Nilsson 和 B. Raphael,"最小成本路径启发式确定的形式基础",《电气和电子工程师学会系统科学与控制论事务》,第 4 卷第 2 期,第 100-107 页,1968 年。
[7] S. M. LaValle and J. J. Kuffner Jr, "Randomized kinodynamic planning," The international journal of robotics research, vol. 20, no. 5, pp. 378400, 2001. [7] S. M. LaValle and J. J. Kuffner Jr, "Randomized kinodynamic planning," The international journal of robotics research, vol. 20, no.5, pp.
[8] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, "Practical search techniques in path planning for autonomous driving," Ann Arbor, vol. 1001, no. 48105, pp. 18-80, 2008. [8] D. Dolgov、S. Thrun、M. Montemerlo 和 J. Diebel,"自动驾驶路径规划中的实用搜索技术",《安阿伯》,第 1001 卷,第 48105 期,第 18-80 页,2008 年。48105, pp.
[9] W. Lim, S. Lee, M. Sunwoo, and K. Jo, "Hierarchical trajectory planning of an autonomous car based on the integration of a sampling and an optimization method," IEEE Transactions on Intelligent Transportation Systems, vol. 19, no. 2, pp. 613-626, 2018. [9] W. Lim、S. Lee、M. Sunwoo 和 K. Jo,《基于采样和优化方法集成的自动驾驶汽车分层轨迹规划》,《IEEE 智能交通系统论文集》,第 19 卷第 2 期,第 613-626 页,2018 年。
[10] P. T. Boggs and J. W. Tolle, "Sequential quadratic programming," Acta numerica, vol. 4, pp. 1-51, 1995. [10] P. T. Boggs and J. W. Tolle, "Sequential quadratic programming," Acta numerica, vol. 4, pp.
[11] B. Li, "Research on computational optimal control methods for automated vehicle motion planning problems with complicated constraints," Ph.D. dissertation, Zhejiang University, 2018. [11] B. Li:《复杂约束条件下自动驾驶车辆运动规划问题的计算最优控制方法研究》,浙江大学博士论文,2018。
[12] A. Wächter and L. T. Biegler, "On the implementation of an interiorpoint filter line-search algorithm for large-scale nonlinear programming," Mathematical programming, vol. 106, pp. 25-57, 2006. [12] A. Wächter and L. T. Biegler, "On implementation of an interiorpoint filter line-search algorithm for large-scale nonlinear programming," Mathematical programming, vol. 106, pp.
[13] B. Li and Z. Shao, "A unified motion planning method for parking an autonomous vehicle in the presence of irregularly placed obstacles," Knowledge-Based Systems, vol. 86, pp. 11-20, 2015. [13] B. Li和Z. Shao:《存在不规则障碍物时自动驾驶车辆停车的统一运动规划方法》,《基于知识的系统》,第86卷,第11-20页,2015年。
[14] B. Li, K. Wang, and Z. Shao, "Time-optimal maneuver planning in automatic parallel parking using a simultaneous dynamic optimization approach," IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 11, pp. 3263-3274, 2016. [14] B. Li, K. Wang, and Z. Shao, "Time-optimal maneuver planning in automatic parallel parking using a simultaneous dynamic optimization approach," IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 11, pp.
[15] P. S. Chib and P. Singh, "Recent advancements in end-to-end autonomous driving using deep learning: A survey," IEEE Transactions on Intelligent Vehicles, vol. 9, no. 1, pp. 103-118, 2024. [15] P. S. Chib 和 P. Singh,"使用深度学习的端到端自动驾驶的最新进展:A survey," IEEE Transactions on Intelligent Vehicles, vol. 9, no. 1, pp.
[16] D. A. Pomerleau, "Alvinn: An autonomous land vehicle in a neural network," Advances in neural information processing systems, vol. 1, 1988. [16] D. A. Pomerleau,"Alvinn:神经信息处理系统进展》,第 1 卷,1988 年。
[17] J. Hawke, R. Shen, C. Gurau, S. Sharma, D. Reda, N. Nikolov, P. Mazur, S. Micklethwaite, N. Griffiths, A. Shah, and A. Kndall, "Urban driving with conditional imitation learning," in 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020, pp. 251-257. [17] J. Hawke、R. Shen、C. Gurau、S. Sharma、D. Reda、N. Nikolov、P. Mazur、S. Micklethwaite、N. Griffiths、A. Shah 和 A. Kndall,"利用条件模仿学习的城市驾驶",2020 年电气和电子工程师学会机器人与自动化国际会议(ICRA),2020 年,第 251-257 页。
[18] A. Prakash, K. Chitta, and A. Geiger, "Multi-modal fusion transformer for end-to-end autonomous driving," in 2021 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 2021, pp. 70737083. [18] A. Prakash、K. Chitta 和 A. Geiger,"端到端自动驾驶的多模式融合变换器",2021 年 IEEE/CVF 计算机视觉与模式识别会议(CVPR),2021 年,第 70737083 页。
[19] K. Chitta, A. Prakash, B. Jaeger, Z. Yu, K. Renz, and A. Geiger, "Transfuser: Imitation with transformer-based sensor fusion for autonomous driving," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 45, no. 11, pp. 12878-12895, 2023. [19] K. Chitta、A. Prakash、B. Jaeger、Z. Yu、K. Renz 和 A. Geiger,"Transfuser:基于变压器的自动驾驶传感器融合模仿",《电气和电子工程师学会模式分析与机器智能论文集》,第 45 卷,第 11 期,第 12878-12895 页,2023 年。
[20] A. Vaswani, N. Shazeer, N. Parmar, J. Uszkoreit, L. Jones, A. N. Gomez, Ł. Kaiser, and I. Polosukhin, "Attention is all you need," Advances in neural information processing systems, vol. 30, 2017. [20] A. Vaswani、N. Shazeer、N. Parmar、J. Uszkoreit、L. Jones、A. N. Gomez、Ł.Kaiser, and I. Polosukhin, "Attention is all you need," Advances in neural information processing systems, vol. 30, 2017.
[21] A. Dosovitskiy, G. Ros, F. Codevilla, A. Lopez, and V. Koltun, "Carla: An open urban driving simulator," in Conference on robot learning. PMLR, 2017, pp. 1-16. [21] A. Dosovitskiy、G. Ros、F. Codevilla、A. Lopez 和 V. Koltun,"Carla:开放式城市驾驶模拟器",机器人学习会议。PMLR, 2017, pp.
[22] K. Renz, K. Chitta, O.-B. Mercea, A. Koepke, Z. Akata, and A. Geiger, "Plant: Explainable planning transformers via object-level representations," arXiv preprint arXiv:2210.14222, 2022. [22] K. Renz、K. Chitta、O.-B.Mercea、A. Koepke、Z. Akata 和 A. Geiger,"Plant:Plant: Explainable Planning transformers via object-level representation," arXiv preprint arXiv:2210.14222, 2022.
[23] A. Sadat, S. Casas, M. Ren, X. Wu, P. Dhawan, and R. Urtasun, "Perceive, predict, and plan: Safe motion planning through interpretable semantic representations," in Computer Vision-ECCV 2020: 16th European Conference, Glasgow, UK, August 23-28, 2020, Proceedings, Part XXIII 16. Springer, 2020, pp. 414-430. [23] A. Sadat、S. Casas、M. Ren、X. Wu、P. Dhawan 和 R. Urtasun,"感知、预测和规划:通过可解释语义表征实现安全运动规划",《计算机视觉-ECCV 2020:第 16 届欧洲会议》,英国格拉斯哥,2020 年 8 月 23-28 日,论文集,第 XXIII 16 部分。Springer, 2020, pp.
[24] Y. Hu, J. Yang, L. Chen, K. Li, C. Sima, X. Zhu, S. Chai, S. Du, T. Lin, W. Wang et al., "Planning-oriented autonomous driving," in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2023, pp. . [24] Y. Hu、J. Yang、L. Chen、K. Li、C. Sima、X. Zhu、S. Chai、S. Du、T. Lin、W. Wang 等人,"面向规划的自动驾驶",《IEEE/CVF 计算机视觉和模式识别会议论文集》,2023 年,第 页。
[25] J. Zhang and E. Ohn-Bar, "Learning by watching," in Proceedings of the IEEE/CVF conference on computer vision and pattern recognition, 2021, pp. 12711-12721. [25] J. Zhang and E. Ohn-Bar, "Learning by watching," in Proceedings of the IEEE/CVF conference on computer vision and pattern recognition, 2021, pp.
[26] D. Chen and P. Krähenbühl, "Learning from all vehicles," in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2022, pp. 17222-17231. [26] D. Chen 和 P. Krähenbühl,《从所有车辆中学习》,《IEEE/CVF 计算机视觉与模式识别会议论文集》,2022 年,第 17222-17231 页。
[27] J. Wang, W. Chi, C. Li, C. Wang, and M. Q.-H. Meng, "Neural rrt*: Learning-based optimal path planning," IEEE Transactions on Automation Science and Engineering, vol. 17, no. 4, pp. 1748-1758, 2020. [27] J. Wang、W. Chi、C. Li、C. Wang 和 M. Q.-H. Meng,"Neural rrt*: Neural rrt*: Neural rrt*: Neural rrt*.Meng, "Neural rrt*:基于学习的最优路径规划》,《电气与电子工程师学会自动化科学与工程论文集》,第 17 卷,第 4 期,第 1748-1758 页,2020 年。4, pp.
[28] T. Zhang, J. Wang, and M. Q.-H. Meng, "Generative adversarial network based heuristics for sampling-based path planning," IEEE/CAA Journal of Automatica Sinica, vol. 9, no. 1, pp. 64-74, 2022. [28] T. Zhang, J. Wang, and M. Q.-H. Meng, "Generative adversarial network based heuristics for sampling-based path planning," IEEE/CAA Journal Automatica Sinica, vol. 9, no.Meng, "Generative adversarial network based heuristics for sampling-based path planning," IEEE/CAA Journal of Automatica Sinica, vol. 9, no. 1, pp.
[29] C. Zhao, Y. Zhu, Y. Du, F. Liao, and C.-Y. Chan, "A novel direct trajectory planning approach based on generative adversarial networks [29] C. Zhao, Y. Zhu, Y. Du, F. Liao, and C.-Y. Chan, "A novel direct trajectory planning approach based on generative adversarial networks.基于生成式对抗网络的新型直接轨迹规划方法
and rapidly-exploring random tree," IEEE Transactions on Intelligent Transportation Systems, vol. 23, no. 10, pp. 17910-17921, 2022. 和快速探索随机树",《IEEE 智能交通系统论文集》,第 23 卷,第 10 期,第 17910-17921 页,2022 年。
[30] H. Li, P. Chen, G. Yu, B. Zhou, Y. Li, and Y. Liao, "Trajectory planning for autonomous driving in unstructured scenarios based on deep learning and quadratic optimization," IEEE Transactions on Vehicular Technology, vol. 73, no. 4, pp. 4886-4903, 2024. [30] H. Li, P. Chen, G. Yu, B. Zhou, Y. Li, and Y. Liao, "Trajectory planning for autonomous driving in unstructured scenarios based on deep learning and quadratic optimization," IEEE Transactions on Vehicular Technology, vol. 73, no.4, pp.
[31] H. Du, Y. Sun, Y. Pan, Z. Li, and P. Siarry, "A lane-changing trajectory re-planning method considering conflicting traffic scenarios," Engineering Applications of Artificial Intelligence, vol. 127, p. 107264, 2024. [31] H. Du、Y. Sun、Y. Pan、Z. Li 和 P. Siarry,"考虑冲突交通场景的变道轨迹重新规划方法",《人工智能工程应用》,第 127 卷,第 107264 页,2024 年。
[32] J. Gao, C. Sun, H. Zhao, Y. Shen, D. Anguelov, C. Li, and C. Schmid, "Vectornet: Encoding hd maps and agent dynamics from vectorized representation," in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2020, pp. . [32] J. Gao、C. Sun、H. Zhao、Y. Shen、D. Anguelov、C. Li 和 C. Schmid,"Vectornet:Vectornet: Encoding hd maps and agent dynamics from vectorized representation," in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2020, pp.
[33] B. Li, Z. Yin, Y. Ouyang, Y. Zhang, X. Zhong, and S. Tang, "Online trajectory replanning for sudden environmental changes during automated parking: A parallel stitching method," IEEE Transactions on Intelligent Vehicles, vol. 7, no. 3, pp. 748-757, 2022. [33] B. Li, Z. Yin, Y. Ouyang, Y. Zhang, X. Zhong, and S. Tang, "Online trajectory replanning for sudden environmental changes during automated parking:A parallel stitching method," IEEE Transactions on Intelligent Vehicles, vol. 7, no.3, pp.