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: 分别代表车辆的速度和加速度。 和 代表前轮转向角和角速度。 表示车辆的轴距。此外,在车辆的实际运行过程中,应遵守物理或机械约束,满足以下条件: