Space-time joint trajectory planning method for off-road environment

Abstract

The application discloses a space-time joint trajectory planning method for off-road environment, relates to the technical field of vehicle automatic driving and trajectory planning control, and realizes unified modeling of a vehicle motion state and external environment characteristics by establishing a vehicle dynamics model and fusing environment information such as terrain structure, obstacle distribution and passing area, so that the trajectory planning process can simultaneously consider dynamics constraints and environment constraints, thereby realizing global feasibility and smoothness of a trajectory in an off-road or other unstructured scene. By constructing a space-time joint planning model, the method guarantees dynamic continuity of vehicle motion in the time dimension and ensures safety and passability of a path in the space dimension, and effectively solves the problem of the prior art that trajectory optimization only depends on a static geometric path and is difficult to reflect dynamic constraints and terrain characteristics.

Description

Technical Field

[0001] This application relates to the field of vehicle autonomous driving and trajectory planning and control technology, and in particular to a spatiotemporal joint trajectory planning method for off-road environments. Background Technology

[0002] With the rapid development of autonomous driving technology, vehicles have achieved relatively mature path planning and trajectory tracking capabilities in structured road scenarios, relying on high-precision maps and comprehensive traffic rules. However, in unstructured off-road environments such as mountains, forests, and deserts, the terrain is undulating, road boundaries are irregular and time-varying, and obstacles are diverse and may be in motion, significantly increasing the uncertainty of environmental perception. To achieve safe, smooth, and efficient passage of unmanned vehicles in such complex scenarios, it is urgent to establish a spatiotemporal joint trajectory planning method that can simultaneously consider spatial terrain characteristics and temporal evolution patterns.

[0003] Existing technologies mainly include path planning methods based on graph search / sampling (such as A, D, RRT, etc.) and model-based optimization methods (such as MPC, iLQR, etc.). The former relies on manually modeled cost maps or coarse-grained grids in unstructured scenarios, making it difficult to accurately represent the coupling relationship between terrain accessibility and vehicle dynamics constraints. The latter, although capable of smooth optimization in a continuous domain, has limited ability to handle complex nonlinear inequality constraints (such as safe distances for dynamic obstacles, lateral acceleration and rate of change of acceleration, and joint constraints of curvature and velocity), often relying on strong approximation or penalty strategies, resulting in a difficulty in simultaneously achieving convergence stability and real-time performance. Furthermore, the accessibility information of off-road terrain contains uncertainties and noise, and there is still a lack of a unified and effective modeling and solution framework for how to integrate terrain gradient / normal features in a continuously differentiable manner during trajectory optimization.

[0004] Therefore, in the spatiotemporal joint trajectory planning of off-road autonomous driving, the challenges of integrating the uncertainty and continuous differentiability of terrain accessibility information, the synergistic satisfaction of multiple constraints on vehicle dynamics and safety and comfort, and the limitations of existing methods in handling nonlinear inequality constraints and their solution efficiency have become urgent problems to be solved. Summary of the Invention

[0005] This application provides a spatiotemporal joint trajectory planning method for off-road environments, aiming to solve the problems of uncertainty and continuous differentiability fusion of terrain accessibility information, synergistic satisfaction of multiple constraints of vehicle dynamics and safety and comfort, and the limited ability and solution efficiency of existing methods to handle nonlinear inequality constraints in spatiotemporal joint trajectory planning for off-road unmanned driving.

[0006] A spatiotemporal joint trajectory planning method for off-road environments, the method comprising: Establish a dynamic model that reflects the vehicle's motion characteristics to describe the changes in the vehicle's spatial position, attitude, and motion state; The dynamic model is fused with environmental information, including terrain structure, obstacle distribution, and traffic area, to construct a comprehensive model that reflects the coupling relationship between the vehicle and the environment. Based on the comprehensive model, a spatiotemporal joint trajectory planning problem is constructed, which simultaneously considers vehicle motion constraints, environmental constraints, and path continuity constraints. The trajectory planning problem is solved by iterative optimization to obtain a vehicle motion trajectory that satisfies the constraints in both time and space dimensions. The trajectory is output to guide the vehicle's autonomous driving in complex terrain environments.

[0007] Optionally, in the above scheme, the dynamic model is a nonlinear kinematic model of a vehicle. The model includes position, heading angle, curvature, velocity and acceleration state variables, as well as curvature change rate and acceleration change rate control variables, which are used to describe the nonlinear relationship between vehicle attitude change and motion behavior.

[0008] In the above scheme, optionally, the environmental information is obtained by fusing a terrain accessibility map with an obstacle model. The accessibility map is generated from elevation data or point cloud information, and the obstacle model is approximated by an ellipse or polygon shape. The dynamic safety zone is determined by combining the movement direction and speed of the obstacle.

[0009] In the above scheme, optionally, the objective function of the spatiotemporal joint trajectory planning problem includes a terminal cost term and a stage cost term. The terminal cost term is used to limit the deviation between the vehicle's final state and the target state. The stage cost term includes a control input smoothing term, a curvature change term, a speed deviation term, a path offset term, and a terrain traversability penalty term.

[0010] Optionally, in the above scheme, the constraints include: Road boundary constraints are used to ensure that the center of the vehicle maintains a safe distance from the road boundary; Obstacle constraints are used to prevent vehicles from entering the envelope of obstacles. Dynamic constraints are used to limit the range of values ​​for velocity, acceleration, curvature, and their rate of change. The heading angle constraint is used to control the deviation angle between the vehicle's attitude and the road direction.

[0011] Optionally, in the above scheme, the iterative optimization method adopts a two-layer structure based on the enhanced Lagrange method and the iterative linear quadratic adjustment algorithm, wherein: The inner layer linearizes and approximates the system model, and uses an iterative linear quadratic adjustment algorithm to calculate the control law gain and feedforward correction. The outer layer introduces Lagrange multipliers and penalty parameters through the enhanced Lagrange method to augment and dynamically adjust the constraints.

[0012] Optionally, in the above scheme, the enhanced Lagrange outer layer iteration updates the penalty coefficient by detecting the degree of constraint violation. When the degree of constraint satisfaction is high, the penalty parameter is reduced, and when the degree of violation is large, the penalty parameter is increased, so as to achieve dynamic convergence of the constraint conditions.

[0013] In the above scheme, optionally, the iterative linear quadratic adjustment algorithm calculates the linear gain matrix of the control law and the feedforward correction vector in the backpropagation stage, updates the state sequence according to the control law in the forward simulation stage, and performs a line search after each iteration to determine the trajectory update step size.

[0014] Optionally, in the above scheme, the control correlation matrix is ​​regularized during the solution process. When the matrix does not meet the positive definite condition, it is corrected by introducing a positive definite adjustment parameter to maintain the invertibility of the matrix and improve the numerical stability.

[0015] Optionally, in the above scheme, the final output planning result includes the spatial position points, attitude angles, velocities, accelerations and corresponding control input sequences of the vehicle in a discrete time series. The result can be directly used for trajectory tracking and control execution of unmanned vehicles.

[0016] Compared with the prior art, this application has at least the following beneficial effects: Based on further analysis and research of existing technical problems, this application recognizes the challenges in spatiotemporal joint trajectory planning for off-road autonomous driving, including the uncertainties and challenges of fusing terrain accessibility information with continuous differentiability, the synergistic satisfaction of multiple constraints related to vehicle dynamics and safety and comfort, and the limitations of existing methods in handling nonlinear inequality constraints and their solution efficiency. By establishing a vehicle dynamics model and fusing it with environmental information such as terrain structure, obstacle distribution, and traversable areas, this application achieves unified modeling of vehicle motion state and external environmental characteristics. This allows the trajectory planning process to simultaneously consider dynamic and environmental constraints, thereby achieving global feasibility and smoothness of the trajectory in unstructured scenarios such as off-road driving. By constructing a spatiotemporal joint planning model, this method ensures the dynamic continuity of vehicle motion in the time dimension and the safe traversability of the path in the spatial dimension, effectively solving the problem that existing technologies rely solely on static geometric paths for trajectory optimization, making it difficult to reflect dynamic constraints and terrain characteristics. This method is based on an iterative optimization solution framework, which can quickly obtain feasible trajectories that meet spatiotemporal constraints under complex constraints, dynamic changes in obstacles, and significant terrain undulations. This overcomes the problems of trajectory discontinuity, unstable solution, and insufficient representation of trafficability in traditional planning algorithms in unstructured terrain, and achieves high reliability and real-time executability for autonomous driving of vehicles in complex environments. Attached Figure Description

[0017] Figure 1 This is one of the flowcharts illustrating a spatiotemporal joint trajectory planning method for off-road environments provided in an embodiment of this application; Figure 2 This is a second flowchart illustrating a spatiotemporal joint trajectory planning method for off-road environments, provided as an embodiment of this application. Figure 3 This is a schematic diagram of a vehicle kinematics model provided in one embodiment of this application. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0019] In the description of this application: unless otherwise stated, "a plurality of" means two or more. The terms "first," "second," "third," etc., in this application are intended to distinguish the objects referred to and do not have any special meaning in terms of technical connotation (e.g., they should not be construed as an emphasis on importance or order). Expressions such as "including," "comprising," and "having" also mean "not limited to" (certain units, components, materials, steps, etc.).

[0020] While current autonomous driving technology has made significant progress in structured road scenarios, it still faces challenges in unstructured off-road scenarios. Fundamental challenges. Autonomous driving applications in off-road scenarios face multiple challenges, including rugged terrain, unknown accessibility, narrow spaces, and instability, which place higher demands on environmental understanding, trajectory planning, and control strategies.

[0021] Off-road trajectory planning is an extension of robot trajectory planning. It requires attention to efficient trajectory planning methods in unstructured scenarios and in-depth exploration of the integration of map accessibility information with trajectory planning. While current numerical optimization-based trajectory planning methods have demonstrated strong theoretical advantages and application potential in autonomous driving, they urgently need to address challenges in off-road scenarios, such as the inability to quickly process highly nonlinear grid accessibility maps and weak dynamic obstacle interaction capabilities. This will provide theoretical and engineering support for unmanned platforms in off-road environments.

[0022] In one embodiment, such as Figure 1 and Figure 2 As shown, a spatiotemporal joint trajectory planning method for off-road environments is provided, including the following steps: Establish a dynamic model that reflects the vehicle's motion characteristics to describe the changes in the vehicle's spatial position, attitude, and motion state; The dynamic model is fused with environmental information, including terrain structure, obstacle distribution, and traffic area, to construct a comprehensive model that reflects the coupling relationship between the vehicle and the environment. Based on the comprehensive model, a spatiotemporal joint trajectory planning problem is constructed, which simultaneously considers vehicle motion constraints, environmental constraints, and path continuity constraints. The trajectory planning problem is solved by iterative optimization to obtain a vehicle motion trajectory that satisfies the constraints in both time and space dimensions. The trajectory is output to guide the vehicle's autonomous driving in complex terrain environments.

[0023] This embodiment provides a spatiotemporal joint trajectory planning method for vehicle motion planning in complex terrain environments, applicable to autonomous vehicle motion planning in complex scenarios such as off-road, mountainous, and unstructured roads. This method can achieve joint optimization trajectory generation in the time and spatial domains by fusing vehicle dynamics characteristics and environmental terrain information, thereby ensuring the safety, stability, and feasibility of the vehicle in complex environments.

[0024] First, a dynamic model of the vehicle is established to describe the dynamic relationship between its spatial position, attitude angles, and motion state. This dynamic model includes basic state variables reflecting the vehicle's position, heading, velocity, and acceleration, and uses steering input or rate of change of acceleration as control variables, thus characterizing the continuous changes in the vehicle's attitude during turning, acceleration, or deceleration. The model can be configured with different levels of precision depending on the vehicle type, such as using a nonholonomic constraint model or a simplified two-degree-of-freedom kinematic model, to achieve a balance between motion characteristics and computational complexity.

[0025] Secondly, to enable the trajectory planning process to adapt to the environment, the vehicle dynamics model is fused with external environmental information. This environmental information includes terrain undulation characteristics, traffic availability distribution, obstacle location and shape, road boundaries, and inaccessible areas. By incorporating environmental features into the dynamic equations as extended states or additional constraints, the system can simultaneously consider vehicle motion characteristics and environmental influences, achieving coupled vehicle-terrain modeling. This fused model can dynamically adjust the vehicle's feasible area during the planning process, thereby avoiding paths traversing impassable areas such as steep slopes, potholes, or obstacles.

[0026] Based on the fusion model, a spatiotemporal joint trajectory planning problem is constructed. This problem optimizes the trajectory globally by comprehensively considering vehicle dynamic constraints, path smoothness constraints, and environmental safety constraints, taking into account both time and space dimensions. The objective function typically includes multiple indicators such as vehicle attitude deviation, control smoothness, path offset, speed error, and terrain traversal cost, used to balance driving efficiency and safety; at the same time, a terminal cost term is introduced to ensure that the vehicle's final state is consistent with the target state (such as target position or attitude).

[0027] In the solution phase, an iterative optimization-based framework is employed. This method transforms the nonlinear system into an iteratively solvable optimization subproblem through linearization and quadratic approximation. During iteration, state variables and control inputs are updated simultaneously to achieve synchronous trajectory optimization in both the time and spatial domains. In each iteration, the constraint conditions are checked for satisfaction, and constraint augmentation parameters or penalty coefficients are dynamically adjusted as needed to ensure the physical feasibility of the trajectory and constraint consistency. Finally, after the optimization process converges, a discrete spatiotemporal trajectory sequence is output, including the vehicle's position, attitude angles, velocity, and control variables at each time step, for trajectory tracking and path control by the vehicle control execution module.

[0028] Through the above technical solution, this embodiment can effectively solve a number of key problems existing in the background technology, and has significant practical value and engineering advantages.

[0029] First, addressing the problem in the background technology that the terrain of off-road environments is complex and the accessibility information is difficult to integrate into the planning model, this method establishes a unified state description framework by integrating environmental features with vehicle dynamics models. This enables the planning system to respond to terrain changes and access areas at the modeling level, thereby avoiding the limitation of traditional algorithms that are based only on two-dimensional paths and cannot reflect three-dimensional terrain features.

[0030] Secondly, to address the problem that traditional trajectory planning cannot simultaneously consider dynamic constraints and path smoothness, this invention introduces continuous constraints such as acceleration and rate of curvature change in the time dimension and path offset and boundary safety distance control in the spatial dimension through spatiotemporal joint modeling. This enables the trajectory to simultaneously possess dynamic feasibility and geometric continuity, thereby improving the stability and comfort of vehicles in complex terrain.

[0031] Furthermore, to address the issues of complex obstacle distribution and unstable planning results, this invention dynamically updates obstacle positions and safety distances during the optimization process, transforming obstacle constraints into continuously differentiable constraint functions. This allows for a smooth transition of the trajectory during obstacle avoidance, avoiding path abrupt changes and optimization non-convergence in traditional methods.

[0032] Furthermore, the design based on the iterative optimization solution framework enables the algorithm to possess high convergence and stability when dealing with nonlinear constraint problems. Regardless of the implementation method used, such as linearization approximation, quadratic optimization, or constraint augmentation, the generated trajectory can be guaranteed to simultaneously satisfy the constraint requirements under both dynamic and environmental conditions.

[0033] In summary, this embodiment constructs a unified vehicle-environment fusion model and adopts a spatiotemporal joint optimization trajectory solution method to achieve dynamic, feasible, continuous, and obstacle-avoidance-safe trajectory planning for vehicles under complex terrain conditions. It effectively overcomes the problems of insufficient terrain representation, imperfect constraint handling, and unstable real-time performance in existing technologies, and provides reliable technical support for autonomous driving of unmanned vehicles in off-road, unstructured, and dynamic environments.

[0034] In this embodiment, the dynamic model is a nonlinear kinematic model of the vehicle. The model includes position, heading angle, curvature, velocity and acceleration state variables, as well as curvature change rate and acceleration change rate control variables, which are used to describe the nonlinear relationship between vehicle attitude change and motion behavior.

[0035] In this embodiment, the environmental information is obtained by fusing a terrain accessibility map with an obstacle model. The accessibility map is generated from elevation data or point cloud information, and the obstacle model is approximated by an elliptical or polygonal shape. The dynamic safety zone is determined by combining the movement direction and speed of the obstacles.

[0036] In this embodiment, the objective function of the spatiotemporal joint trajectory planning problem includes a terminal cost term and a stage cost term. The terminal cost term is used to limit the deviation between the vehicle's final state and the target state. The stage cost term includes a control input smoothing term, a curvature change term, a speed deviation term, a path offset term, and a terrain traversability penalty term.

[0037] In this embodiment, the constraints include: Road boundary constraints are used to ensure that the center of the vehicle maintains a safe distance from the road boundary; Obstacle constraints are used to prevent vehicles from entering the envelope of obstacles. Dynamic constraints are used to limit the range of values ​​for velocity, acceleration, curvature, and their rate of change. The heading angle constraint is used to control the deviation angle between the vehicle's attitude and the road direction.

[0038] In this embodiment, the iterative optimization method adopts a two-layer structure based on the enhanced Lagrange method and the iterative linear quadratic adjustment algorithm, wherein: The inner layer linearizes and approximates the system model, and uses an iterative linear quadratic adjustment algorithm to calculate the control law gain and feedforward correction. The outer layer introduces Lagrange multipliers and penalty parameters through the enhanced Lagrange method to augment and dynamically adjust the constraints.

[0039] In this embodiment, the enhanced Lagrange outer layer iteration updates the penalty coefficient by detecting the degree of constraint violation. When the degree of constraint satisfaction is high, the penalty parameter is reduced, and when the degree of violation is large, the penalty parameter is increased, so as to achieve dynamic convergence of the constraint conditions.

[0040] In this embodiment, the iterative linear quadratic adjustment algorithm calculates the linear gain matrix of the control law and the feedforward correction vector during the backpropagation stage, updates the state sequence according to the control law during the forward simulation stage, and performs a line search after each iteration to determine the trajectory update step size.

[0041] In this embodiment, the control correlation matrix is ​​regularized during the solution process. When the matrix does not meet the positive definite condition, a positive definite adjustment parameter is introduced for correction, so as to maintain the invertibility of the matrix and improve the numerical stability.

[0042] In this embodiment, the final output planning result includes the vehicle's spatial position points, attitude angles, velocity, acceleration, and corresponding control input sequences in a discrete time series. This result can be directly used for trajectory tracking and control execution of unmanned vehicles. By establishing a vehicle dynamics model and fusing it with environmental information such as terrain structure, obstacle distribution, and passable areas, a unified modeling of the vehicle's motion state and external environmental characteristics is achieved. This allows the trajectory planning process to simultaneously consider dynamic constraints and environmental constraints, thereby achieving global feasibility and smoothness of the trajectory in unstructured scenarios such as off-road driving. By constructing a spatiotemporal joint planning model, this method ensures the dynamic continuity of vehicle motion in the time dimension and the safe traversability of the path in the spatial dimension, effectively solving the problem that existing technologies rely solely on static geometric paths for trajectory optimization, making it difficult to reflect dynamic constraints and terrain characteristics. This method is based on an iterative optimization solution framework, which can quickly obtain feasible trajectories that meet spatiotemporal constraints under complex constraints, dynamic changes in obstacles, and significant terrain undulations. This overcomes the problems of trajectory discontinuity, unstable solution, and insufficient representation of trafficability in traditional planning algorithms in unstructured terrain, and achieves high reliability and real-time executability for autonomous driving of vehicles in complex environments.

[0043] This embodiment provides a spatiotemporal joint trajectory planning method for off-road environments, including the following steps: Step 1: Construct a high-precision discrete vehicle kinematics model for off-road environments (1) Construct a continuous-time vehicle kinematics model: In planning problems, vehicle kinematics models are commonly used to describe vehicle motion. Compared to point mass models, kinematic models consider the relationship between the front wheel steering angle and the vehicle's yaw rate, effectively avoiding the planning of trajectories with stationary turning characteristics, thus preventing a disconnect between planning and control. For example... Figure 3 In the kinematic model shown, state variables are generally used. To describe the state of the vehicle, among which x and y This represents the Cartesian coordinates of the vehicle's rear axle center in the planned coordinate system. For vehicle heading, Indicates the front wheel steering angle. v and a These represent velocity and acceleration, respectively.

[0044] The following continuous-time vehicle kinematics model can be obtained: Formula (1) in, For the turning curvature, For the rate of change of turning curvature, This represents the rate of change of acceleration.

[0045] (2) Construct a discrete-time vehicle kinematics model: To enable vehicle kinematics models to be solved by computers The continuous-time vehicle kinematics model obtained in discretization (1) uses the fourth-order Runge-Kutta method for integral approximation of the nonlinear expressions in the model. Meanwhile, considering that optimal control in off-road scenarios requires the integration of a grid-based accessibility map, as described in step one, an expanded grid state needs to be introduced into the vehicle kinematics model. express The traffic flow at a given location, combined with the traffic flow at that location, yields a discrete-time vehicle kinematics model that considers the expanded grid state: Formula (2) Wherein, let the state variable be The control quantity is The discrete vehicle kinematics model can be represented as .

[0046] Step 2: Constructing a spatiotemporal joint trajectory planning problem in an off-road environment The spatiotemporal joint trajectory planning problem in off-road scenarios can be described as follows: Formula (3) in, For the first The system state vector of the step, For the corresponding control input vector, For the stage cost function, For terminal state cost, and These represent the allowed range of values ​​for the status and control inputs, respectively.

[0047] The objective function and constraints are modeled in detail below to improve the performance of the spatiotemporal joint trajectory planning algorithm in off-road environments.

[0048] (1) Design the objective function: In off-road scenarios, trajectory planning problems mainly need to consider off-road accessibility, trajectory quality, risks or costs of interacting with other obstacles, and terminal status. This embodiment designs an objective function that comprehensively considers control input, trajectory smoothness, trajectory smoothness, collision risk, and off-road accessibility.

[0049] (a) Cost of control quantity: To control the input angle, the goal is to satisfy various constraints with the smallest possible control input and reduce the cost function value. Therefore, the following cost is designed: Formula (4) in, This is the weight coefficient matrix.

[0050] (b) Lateral motion cost: The generated trajectory should minimize lateral acceleration and its rate of change to improve motion perception. Smaller lateral acceleration and rate of change can also alleviate pressure on the control layer and reduce lateral control error. The following cost is designed: Formula (5) in, This is the lateral acceleration weighting coefficient. This is the lateral acceleration weighting coefficient.

[0051] (c) Lateral offset cost: To improve the robustness of the planning problem to abnormal inputs, a Huber-type cost function is introduced: Formula (6) in, This is a hyperparameter.

[0052] Combining the Huber-type cost function, the following cost is designed: Formula (7) in, for Vehicle coordinates at time Projection points on the reference path ,function This represents the normal distance from the vehicle's position to the projection point of the reference path.

[0053] (d) Cost of vehicle speed error: The vehicle speed of the generated trajectory also needs to be more robust to abnormal inputs to maintain the balance of the optimization problem. Therefore, the following cost is designed, also incorporating a Huber-type cost function: Formula (8) (e) Collision time cost: To fully consider the interaction between the vehicle and obstacle vehicles, a Time to Collision (TTC) cost is introduced into the objective function of trajectory planning. Furthermore, to soften the TTC constraint and avoid its strong nonlinearity affecting the convergence of the optimization problem, a differentiable LeakyReLU function is introduced, and the following cost is designed: Formula (9) in, To ensure the coefficient approaches 0, avoid... Satisfying constraints affects the value of the objective function. To constrain the violation function: Formula (10) in, For vehicles Center of mass position and vehicles The position of the center of mass The relative vector, For vehicles and vehicles The derivative of the coordinates with respect to time and The relative vector, This is the TTC threshold.

[0054] (f) Off-road accessibility cost: By setting a secondary traffic penalty, the planned trajectory can be kept away from high-risk areas. At the same time, the speed of vehicles in high-risk areas needs to be penalized to further improve safety. The following cost is designed: Formula (11) in, This is the off-road accessibility weighting coefficient. This is the speed weighting coefficient for off-road accessibility.

[0055] By comprehensively considering the above six cost functions, the objective function of the spatiotemporal joint trajectory planning algorithm in off-road environments can be finally obtained: Formula (12) in, To control the cost, For the cost of lateral movement, For the cost of lateral offset, For the cost of vehicle speed error, For the time cost of collision, The price paid for off-road accessibility.

[0056] (2) Design constraints: Off-road environments present complex road conditions, requiring consideration of road conditions, vehicle kinematics and dynamics, and external obstacle information when generating trajectories. This embodiment designs a constraint condition that considers road boundary constraints, obstacle constraints, and vehicle motion capability constraints.

[0057] (a) Road boundary constraints: To improve the optimization effect of trajectory planning problems in off-road environments, a pseudo-distance constraint with continuous differentiability for static road boundary construction is designed.

[0058] First, we model the road boundaries. By connecting the discrete road boundary points, we can construct a polyline. Each segment on the polyline can be represented by a tuple. It indicates. Among them. and Let be the coordinates of the two endpoints of the line segment. and These are the two tangent vectors relative to the vertices of the polygon. The tangent vector at any point on the boundary can be obtained by linear interpolation from the tangent vectors at the two ends of the corresponding line segment. Formula (13) Then, pseudo-distance constraints with continuously differentiable properties can be constructed on the static road boundary: Formula (14) in, The symbolic distance function, Let be the radius of the vehicle's envelope circle.

[0059] (b) Obstacle constraints: Considering information such as the safe distance from obstacles, the obstacle is modeled as an ellipse. The major and minor axes of the ellipse correspond to the longitudinal and lateral dimensions of the obstacle vehicle, respectively. The longitudinal axis is expanded to account for the obstacle vehicle's speed, while the minor axis is directly expanded based on the collision dimensions. The major and minor axes of the ellipse are represented as follows: Formula (15) in, and These are the longitudinal and lateral dimensions of the obstacle, respectively. For the speed of the obstacle, , , , These are time, longitudinal collision size, lateral collision size, and the radius of the vehicle's envelope circle.

[0060] According to the formula for an ellipse, the point The constraint outside the ellipse can be written as: Formula (16) The obstacle state is represented as follows: , Elliptic coefficient matrix , Rotation matrix (c) Vehicle motion capability constraints: To ensure the planned trajectory meets vehicle dynamics requirements and is accurately followed by the controller, vehicle traffic capacity needs to be limited. Vehicle speed, acceleration, rate of change of acceleration, and curvature can all be set as linear box constraints. Formula (17) To improve the lateral feel, constraints need to be placed on lateral acceleration and the rate of change of lateral acceleration. Formula (18) To reduce the exploration space of the trajectory optimization problem and enable faster convergence: Formula (19) Step 3: Construct an expanded state EAL-iLQR trajectory optimization solver for off-road accessibility maps. The path or trajectory optimization problem in the planning module of an autonomous driving system can be described as a numerical optimal control problem of a Markov system: Formula (20) in, For indexes of discrete time periods, and These represent terminal cost and stage cost, respectively. and Let the state variables and control variables of the Markov system be respectively, and let their state transition relations satisfy the equation. ; These are various inequality constraints.

[0061] In the Markov architecture, to fully utilize the computational efficiency advantage of the indirect optimal control method in autonomous driving planning tasks and improve the method's ability to handle nonlinear constraints, this embodiment combines the Augmented Lagrangian method with the representative indirect optimal control solution method iLQR. Furthermore, to accommodate the grid map-based accessibility map, grid map information is introduced as an extended state variable. Therefore, an iLQR solution method based on extended grid state and the Augmented Lagrangian method, EAL-iLQR (Extended Grid State Augmented Lagrangian iLQR), is designed.

[0062] (1) Construct an unconstrained optimal control problem solver based on iLQR: First, given the initial control input sequence and initial state, the initial state trajectory is generated through forward simulation based on the vehicle model, and the initial total cost is calculated. Then, a backward propagation is performed, recursively calculating the gain matrix of the control increment for the entire trajectory from the terminal time step by step. and Simultaneously, the optimal value function at each time step is updated; then, the forward simulation and line search are performed using the control strategy obtained from backpropagation to generate a new control input sequence and state trajectory, and the total cost is re-evaluated. If the line search fails to reduce the Lagrangian function value after a certain number of iterations, regularization is applied and the next step is continued; finally, convergence is judged by comparing the changes in the old and new costs. If the convergence condition is not met, the control sequence is iterated and updated repeatedly until the objective function converges to a local optimum.

[0063] Based on the above basic process, solving the unconstrained optimal control problem based on iLQR mainly includes backpropagation, forward simulation and line search, and regularization. The mathematical descriptions of each process are constructed in detail below.

[0064] (a) Backpropagation: Consider the unconstrained numerical optimal control problem: Formula (21) Given the performance metrics, the optimal residual cost (Cost-to-go) function at time k. Based on the iterative relationship of Bellman's optimality principle, we can derive: Formula (22) in, This is the action value function.

[0065] For the residual cost function and action value function in the initial solution Performing a Taylor expansion nearby yields: Formula (23) The expanded residual cost and action value function are written as... Substituting the incremental form into the Taylor expansion yields the residual cost increment. With the increase in action value The expression: Formula (24) Substituting the analytical expression for the stage cost into the incremental expression yields the following result. , and Relationship: Formula (25) in, express For state variables The first-order partial derivative, express right The second-order partial derivative, express For variables The first-order partial derivative, express For variables and variables The second-order partial derivatives of .

[0066] Introducing variables: Formula (26) but and It can be expanded into the following form: Formula (27) For computational simplicity, the second-order terms are ignored. Furthermore, for this unconstrained optimization problem, its first-order terms can be written out. The optimality condition is used to obtain the optimal control increment. : Formula (28) make , ,Will Substitute return From this, we can obtain and the value of its derivative: Formula (29) Based on the derivation of dynamic programming, the residual cost of the terminal is independent of the decision variables at time N, that is... time and If all relevant derivatives are 0, then: Formula (30) Given an initial control sequence, the state variable sequence can be derived through forward simulation using the system's state equations. From the final time... Initially, the gain matrix of the control increment for the entire trajectory can be calculated sequentially using this forward simulation state sequence. and .

[0067] (b) Forward simulation and line search Given the optimal feedback gain matrix sequence The nominal trajectory is updated through forward dynamics simulation. Since the initial state is fixed, the entire forward simulation can be described as the following iterative process: Formula (31) in, and These are the updated nominal state variables and control variables. The step size parameter for the line search represents the increment of the optimal control quantity. The level of acceptance.

[0068] The ratio of actual decline to expected decline The expression is as follows: Formula (32) in, The cost function at step size The expected decline.

[0069] pass The size of this value can measure the size of the preceding search step. Below, the difference between the actual and expected function value decreases is used to define an index. If and only if The system accepts search results in real time. If the results are not satisfactory, the step size needs to be updated proportionally. Formula (33) in, As the backtracking factor, it is usually taken as , This represents the number of iterations for the line search.

[0070] (c) Regularization If the line search strategy in (b) fails to reduce the value of the Lagrange function after a certain number of iterations, or even if the function value gradually diverges, the forward propagation process will be terminated, and the backward propagation process will be updated. Add a regularization term to the matrix. To transform it into a positive definite identity matrix: Formula (34) During backpropagation, if the optimal control increment is calculated... Hessian matrix at time If the value is not rank, a regularization term needs to be added. Then restart the entire backpropagation process. The value of the regularization term is only reduced after successful backpropagation. The scaling factor for the regularization term is typically set to 1.5. 2.0.

[0071] (2) Construct a constrained optimal control problem solver based on augmented Lagrange-iLQR: This embodiment incorporates the augmented Lagrange multiplier method into iLQR, modifies iLQR, and constructs a constrained optimal control problem solver based on augmented Lagrange iLQR, enabling it to obtain high-performance constraint handling capabilities.

[0072] (a) Constructing an unconstrained optimization problem based on the augmented Lagrangian method For general constrained optimization problems: Formula (35) To enable iLQR to solve constrained optimization problems, augmented Lagrangian methods are introduced, transforming the problems solved by iLQR into unconstrained optimization problems. Augmented Lagrangian methods improve upon the penalty function method by explicitly maintaining estimates of the constraint-related Lagrange multipliers.

[0073] Formula (36) in, Let Lagrange multiplier vectors be used. Penalty term coefficients, matrix The definition is as follows: Formula (37) By constructing two loops, the unconstrained optimization problem L(x) is solved in the inner loop, and the Lagrange multipliers and penalty function gain are continuously updated in the outer loop until the iteration termination condition is met, thus obtaining the local optimal solution of the original constrained optimal control problem.

[0074] (b) Constructing the optimal control problem From the perspective of the augmented Lagrange method, the optimal control problem can be solved by constructing a method with Lagrange multipliers for each stage k. Sum of penalty function gains augmentation items Similarly, the terminal can also establish an augmented term that is independent of the control variable. : Formula (38) Formula (39) Substituting into equation (20), we obtain the complete unconstrained optimal control problem: Formula (40) (3) Constructing an expansion state dynamics update mechanism based on local plane fitting In off-road scenarios, vehicle motion is not only limited by its own nonholonomic constraints (such as Ackerman steering), but also strongly constrained by terrain traversability. Traditional iLQR algorithms rely on analytical gradient information, but off-road traversability maps are usually in the form of discrete grids, lacking continuous differentiable gradients, causing the optimizer to be unable to perceive "which direction is easier to travel".

[0075] To address this issue, this invention specifically modifies the forward simulation and backpropagation processes of the standard AL-iLQR, constructing an expanded state update mechanism based on local plane fitting: (a) Local micro-differentiation of raster maps To incorporate map derivative information into the algorithm, this invention introduces Principal Component Analysis (PCA) to perform local plane fitting on the grid trafficability within the vehicle's current location neighborhood. By calculating the covariance matrix of the neighborhood point cloud and performing eigenvalue decomposition, the eigenvector corresponding to the minimum eigenvalue is extracted as the normal vector, thereby constructing a locally continuous plane equation: Formula (44) in This is a constant term.

[0076] (b) Constructing the dynamic equations of the extended state In order to enable iLQR to actively guide the trajectory to converge toward the high traversability region by utilizing the above-mentioned second derivative information when calculating the Riccati equation during backward propagation, this invention does not simply treat traversability as an external cost, but rather as an extended state variable of the system.

[0077] Formula (45) in, and These are the actual state variables and control variables of the system. This represents the true dynamic equations of the system.

[0078] Through the above construction, combined with the framework built in (2), a complete EAL-iLQR (Extended GridState Augmented Lagrangian iLQR) solver is formed. When solving the spatiotemporal joint planning problem described in step two, this solver can generate a high-quality trajectory that is both smooth and actively adapts to off-road terrain by utilizing terrain gradient information in the extended state, while satisfying vehicle kinematics and obstacle avoidance constraints.

[0079] To address the numerical computational challenges of spatiotemporal joint trajectory planning in off-road scenarios, this embodiment proposes an EAL-iLQR trajectory optimization method that combines grid state extension with principal component analysis derivative approximation. By incorporating accessibility as an extended state into system dynamics and combining it with local principal component analysis to approximate gradients, the efficiency and stability of grid-based trajectory optimization are significantly improved. This method not only performs excellently in path planning problems but has also been systematically validated in high-dimensional spatiotemporal joint trajectory planning problems, fully demonstrating its practicality and superiority in complex off-road terrain environments.

[0080] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

Claims

1. A spatiotemporal joint trajectory planning method for off-road environments, characterized in that, The method includes: Establish a dynamic model that reflects the vehicle's motion characteristics to describe the changes in the vehicle's spatial position, attitude, and motion state; The dynamic model is fused with environmental information, including terrain structure, obstacle distribution, and traffic area, to construct a comprehensive model that reflects the coupling relationship between the vehicle and the environment. Based on the comprehensive model, a spatiotemporal joint trajectory planning problem is constructed, which simultaneously considers vehicle motion constraints, environmental constraints, and path continuity constraints. The trajectory planning problem is solved by iterative optimization to obtain a vehicle motion trajectory that satisfies the constraints in both time and space dimensions. The trajectory is output to guide the vehicle's autonomous driving in complex terrain environments.

2. The method according to claim 1, characterized in that, The dynamic model is a nonlinear kinematic model of the vehicle. The model includes state variables such as position, heading angle, curvature, velocity, and acceleration, as well as control variables such as the rate of change of curvature and the rate of change of acceleration, which are used to describe the nonlinear relationship between the vehicle's attitude change and motion behavior.

3. The method according to claim 1, characterized in that, The environmental information is obtained by fusing a terrain accessibility map with an obstacle model. The accessibility map is generated from elevation data or point cloud information, and the obstacle model is approximated by an elliptical or polygonal shape. The dynamic safety zone is determined by combining the movement direction and speed of the obstacles.

4. The method according to claim 1, characterized in that, The objective function of the spatiotemporal joint trajectory planning problem includes a terminal cost term and a stage cost term. The terminal cost term is used to limit the deviation between the vehicle's final state and the target state. The stage cost term includes a control input smoothing term, a curvature change term, a speed deviation term, a path offset term, and a terrain traversability penalty term.

5. The method according to claim 1, characterized in that, The constraints include: Road boundary constraints are used to ensure that the center of the vehicle maintains a safe distance from the road boundary; Obstacle constraints are used to prevent vehicles from entering the envelope of obstacles. Dynamic constraints are used to limit the range of values ​​for velocity, acceleration, curvature, and their rate of change. The heading angle constraint is used to control the deviation angle between the vehicle's attitude and the road direction.

6. The method according to claim 1, characterized in that, The iterative optimization method adopts a two-layer structure based on the enhanced Lagrange method and the iterative linear quadratic adjustment algorithm, wherein: The inner layer linearizes and approximates the system model, and uses an iterative linear quadratic adjustment algorithm to calculate the control law gain and feedforward correction. The outer layer introduces Lagrange multipliers and penalty parameters through the enhanced Lagrange method to augment and dynamically adjust the constraints.

7. The method according to claim 6, characterized in that, The enhanced Lagrange outer layer iteration updates the penalty coefficient by detecting the degree of constraint violation. When the constraint is satisfied to a high degree, the penalty parameter is reduced, and when the degree of violation is large, the penalty parameter is increased, so as to achieve dynamic convergence of the constraint conditions.

8. The method according to claim 6, characterized in that, The iterative linear quadratic adjustment algorithm calculates the linear gain matrix of the control law and the feedforward correction vector during the backpropagation stage, updates the state sequence according to the control law during the forward simulation stage, and performs a line search after each iteration to determine the trajectory update step size.

9. The method according to claim 1, characterized in that, During the solution process, the control correlation matrix is ​​regularized. When the matrix does not meet the positive definite condition, a positive definite adjustment parameter is introduced for correction to maintain the matrix invertibility and improve numerical stability.

10. The method according to claim 1, characterized in that, The final output planning results include the vehicle's spatial position points, attitude angles, velocity, acceleration, and corresponding control input sequences in the vehicle's discrete time series. These results can be directly used for trajectory tracking and control execution of unmanned vehicles.

Images