Trajectory planning method, controller, vehicle and robot

Abstract

This disclosure relates to a trajectory planning method, controller, vehicle, and robot, and pertains to the field of autonomous driving technology, enabling rapid response to complex dynamic environmental changes. The trajectory planning method includes: determining an initial trajectory based on an obstacle map, target location, and the position of the moving subject; determining weight coefficients of a preset cost function based on the initial trajectory, and inputting the weight coefficients, the preset cost function, and preset constraints into a quadratic programming solver to obtain multiple intermediate trajectories output by the solver; determining the advantage value of each intermediate trajectory at each time step, and the probability ratio of each intermediate trajectory to the initial trajectory, and iteratively adjusting the weight coefficients based on the multiple advantage values ​​and the multiple probability ratios until a preset iteration stopping condition is met to obtain a target weight coefficient; and inputting the target weight coefficient, the preset cost function, and the preset constraints into the quadratic programming solver to obtain the target trajectory output by the solver.

Description

Technical Field

[0001] This disclosure relates to the field of autonomous driving technology, and more specifically, to a trajectory planning method, a controller, a vehicle, and a robot. Background Technology

[0002] Trajectory planning is the process of planning an efficient and safe driving path in autonomous driving based on environmental information, vehicle status, and kinematic constraints.

[0003] In related technologies, trajectory planning typically generates a driving trajectory by directly searching for the trajectory in three-dimensional space. While this method can effectively calculate the path, it is less adaptable to problems with multiple constraints and struggles to respond quickly to changes in complex dynamic environments, resulting in low efficiency and unreliable planning results. Summary of the Invention

[0004] To address the aforementioned technical problems, this disclosure provides a trajectory planning method, a controller, a vehicle, and a robot.

[0005] To achieve the above objectives, firstly, this disclosure provides a trajectory planning method, comprising: Obtain the obstacle map, the target location, and the location of the moving subject; and determine the initial trajectory of the moving subject to move to the target location based on the obstacle map, the target location, and the location of the moving subject. The weight coefficients of the preset cost function are determined based on the initial trajectory, and the weight coefficients, the preset cost function, and the preset constraints are input into the quadratic programming solver to obtain multiple intermediate trajectories of the moving subject moving to the target position, output by the quadratic programming solver. The advantage value of each intermediate trajectory at each time step and the probability ratio of each intermediate trajectory to the initial trajectory are determined respectively. The weight coefficient is iteratively adjusted according to the multiple advantage values ​​and multiple probability ratios until a preset iteration stop condition is met to obtain the target weight coefficient. The target weight coefficient, the preset cost function, and the preset constraints are input into the quadratic programming solver to obtain the target trajectory of the moving subject moving to the target position, which is output by the quadratic programming solver.

[0006] Secondly, this disclosure provides a controller, including: The first memory, on which the computer program is stored; A first processor is configured to execute the computer program in the first memory to implement the method described in the first aspect.

[0007] Thirdly, this disclosure provides a vehicle including the controller described in the second aspect.

[0008] Fourthly, this disclosure provides a robot including the controller described in the second aspect.

[0009] Fifthly, this disclosure provides a computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements the method described in the first aspect.

[0010] In a sixth aspect, this disclosure provides a computer program product, including a computer program that, when executed by a processor, implements the method described in the first aspect.

[0011] The above technical solution determines the initial trajectory of the moving subject to the target location based on the obstacle map, target location, and the position of the moving subject. This ensures the initial planning has a certain degree of rationality and feasibility, avoiding inefficiency or failure to achieve the target caused by blind planning. Since the preset cost function and constraints can be flexibly set according to different application scenarios and actual needs, the weight coefficients of the cost function determined based on the initial trajectory, the preset cost function, and the preset constraints are input into the quadratic programming solver. The quadratic programming solver can determine multiple feasible intermediate trajectories while satisfying various practical constraints, and can adapt to different scenarios and needs. Each intermediate trajectory is determined separately in each... The system calculates the advantage value at each time step and the probability ratio of each intermediate trajectory to the initial trajectory. It iteratively adjusts the weight coefficients based on the advantage value and probability ratio until a preset iteration stopping condition is met, yielding the target weight coefficients. This allows the trajectory planning process to adapt to environmental changes in real time, improving its flexibility and efficiency. The target weight coefficients, preset cost function, and preset constraints are input into a quadratic programming solver to obtain the target trajectory of the moving subject to the target location. By updating the obstacle map and the moving subject's position in real time, the trajectory planning results are updated accordingly, enabling the moving subject to respond promptly to environmental changes and maintain a reasonable movement trajectory, thus improving the robustness and reliability of the trajectory planning.

[0012] Other features and advantages of this disclosure will be described in detail in the following detailed description section. Attached Figure Description

[0013] The accompanying drawings are provided to further illustrate the present disclosure and form part of the specification. They are used together with the following detailed description to explain the present disclosure, but do not constitute a limitation thereof. In the drawings: Figure 1 This is a flowchart illustrating a trajectory planning method according to an exemplary embodiment of the present disclosure.

[0014] Figure 2This is another flowchart illustrating a trajectory planning method according to an exemplary embodiment of the present disclosure.

[0015] Figure 3 This is a block diagram of a controller according to an exemplary embodiment of the present disclosure.

[0016] Figure 4 This is a block diagram of a vehicle according to an exemplary embodiment of the present disclosure. Detailed Implementation

[0017] The specific embodiments of this disclosure will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit this disclosure.

[0018] As mentioned in the background section, although various path planning methods have been proposed, they still have some significant shortcomings in complex and unstructured scenarios. Among related technologies, decoupling-based planning methods typically require decomposing the three-dimensional space into a two-dimensional space for processing, sacrificing some solution space. This decoupling often leads to the loss of solution space information, resulting in local optima and ultimately a non-optimal trajectory solution. Traditional spatiotemporal joint trajectory planning methods, while capable of directly searching for trajectories in three-dimensional space, often fail to balance accuracy and efficiency under multiple constraints and real-time requirements. This makes it difficult to meet the demands of efficiency and reliability in practical applications, and they often struggle to respond quickly to changes in complex dynamic environments, resulting in low trajectory planning efficiency. Planning methods based on static environment rules lack the ability to respond to real-time dynamic environments.

[0019] In view of this, this disclosure provides a trajectory planning method, controller, vehicle, and robot that can quickly respond to changes in complex dynamic environments, improving the robustness and reliability of trajectory planning.

[0020] Figure 1 This is a flowchart illustrating a trajectory planning method according to an exemplary embodiment of the present disclosure, which can be applied to robots or vehicles. Figure 1 As shown, the trajectory planning method may include the following steps: In step S11, the obstacle map, target location, and position of the moving subject are obtained, and the initial trajectory of the moving subject to move to the target location is determined based on the obstacle map, target location, and position of the moving subject.

[0021] In step S12, the weight coefficients of the preset cost function are determined based on the initial trajectory, and the weight coefficients, the preset cost function, and the preset constraints are input into the quadratic programming solver to obtain multiple intermediate trajectories of the moving subject moving to the target position, as output by the quadratic programming solver.

[0022] In step S13, the advantage value of each intermediate trajectory at each time step and the probability ratio of each intermediate trajectory to the initial trajectory are determined respectively. The weight coefficients are iteratively adjusted according to multiple advantage values ​​and multiple probability ratios until the preset iteration stop condition is met, and the target weight coefficients are obtained.

[0023] In step S14, the target weight coefficient, the preset cost function, and the preset constraints are input into the quadratic programming solver to obtain the target trajectory of the moving subject moving to the target position, which is output by the quadratic programming solver.

[0024] It is worth noting that the obstacle map includes, but is not limited to, occupancy grid map, cost map and environment map. The obstacle map includes, but is not limited to, the spatial occupancy status of each cell in the map (including occupancy status or vacancy status, occupancy probability of each cell), geometric structure (including height information or terrain information, surface normal vector), obstacle boundary (including expansion layer), obstacle type, motion status, cost value (or cost value), timestamp and confidence level (or number of observations).

[0025] In this embodiment, the moving subject includes, but is not limited to, robots, vehicles, and drones. The preset cost function and preset constraints can be flexibly adjusted and set according to the application scenario and planning requirements. The quadratic programming solver includes, but is not limited to, the osqp-eigen quadratic programming solver. osqp-eigen is based on the open-source solver framework OSQP (Operator Splitting Quadratic Program), and its internal implementation uses ADMM (Alternating Direction Method of Multipliers). By transforming the original quadratic trajectory programming (QP) problem into a series of subproblems and solving them iteratively and alternately, it efficiently approximates the optimal solution. The solution process supports sparse matrix format, has the characteristics of small memory footprint, high numerical stability, and fast convergence speed, and is suitable for trajectory optimization tasks with high real-time requirements.

[0026] In this embodiment, the trajectory optimization task is converted into a standard form of quadratic trajectory programming (QP) problem. The weight coefficients, preset cost function and preset constraints are input into the osqp-eigen quadratic programming solver. The QP problem uses the state vector of discrete trajectory points as decision variables and uses sparse matrix form to model the objective function and constraints.

[0027] Specifically, let the variable vector be x, where x represents the position of the trajectory point to be optimized, the heading angle, and the time parameter, x∈R.n The objective function is as follows: , Here, matrix H represents a symmetric positive definite sparse matrix, used to define the quadratic loss, H∈R nxn f represents the initial image and is used to describe the trajectory reference offset, f∈R n .

[0028] This OP problem is also subject to inequality constraints, as follows: stl≤Ax≤u, Here, matrix A represents the sparse constraint matrix, used to express the constraints at the start and end points, kinematic constraints, real-time reference constraints, and maximum steering angle constraints, where A∈R. nxn Vector l represents the lower bound of each constraint, and u represents the upper bound of each constraint, where l, u ∈ R. mxn .

[0029] In the above technical solution, the initial trajectory of the moving subject to the target location is determined based on the obstacle map, target location, and the position of the moving subject. This ensures that the initial planning has a certain degree of rationality and feasibility, avoiding problems such as inefficiency or failure to achieve the target caused by blind planning. Since the preset cost function and constraints can be flexibly set according to different application scenarios and actual needs, the weight coefficients of the cost function determined based on the initial trajectory, the preset cost function, and the preset constraints are input into the quadratic programming solver. Using the quadratic programming solver, multiple feasible intermediate trajectories can be determined while satisfying various practical constraints, and it can adapt to different scenarios and needs. Each intermediate trajectory is determined separately in each... The system calculates the advantage value at each time step and the probability ratio of each intermediate trajectory to the initial trajectory. It iteratively adjusts the weight coefficients based on the advantage value and probability ratio until a preset iteration stopping condition is met, yielding the target weight coefficients. This allows the trajectory planning process to adapt to environmental changes in real time, improving its flexibility and efficiency. The target weight coefficients, preset cost function, and preset constraints are input into a quadratic programming solver to obtain the target trajectory of the moving subject to the target location. By updating the obstacle map and the moving subject's position in real time, the trajectory planning results are updated accordingly, enabling the moving subject to respond promptly to environmental changes and maintain a reasonable movement trajectory, thus improving the robustness and reliability of the trajectory planning.

[0030] To facilitate a better understanding of the trajectory planning method provided in this disclosure by those skilled in the art, the trajectory planning method will be described in detail below.

[0031] In one feasible embodiment, in step S11, determining the initial trajectory of the moving subject to move to the target position based on the obstacle map, the target position, and the position of the moving subject may include: The obstacle map is discretized to obtain a raster map, and the initial path between the position of the moving subject and the target position is determined based on the raster map; The initial path is thinned and smoothed according to a preset expansion factor to obtain the initial trajectory of the moving subject moving to the target position.

[0032] In this embodiment, preset kinematic equations, node expansion methods, and heuristic functions are used. Based on these preset kinematic equations, node expansion methods, and heuristic functions, the Anytime Repairing A* (ARA*) algorithm is employed to quickly search for an initial path from the obstacle map according to the target position and the position of the moving subject. Then, based on a preset inflation factor (esplison), the Douglas-Peucker (DP) algorithm is used to thin the initial path, resulting in a thinned path. When the esplison of an intermediate path is 1.0, the thinned path is considered the optimal path, and no further simplification is performed. When the esplison of a thinned path is 2.0, it is considered a suboptimal path. At this point, the preset inflation factor can be adjusted until the tolerance of the thinned path is less than the preset tolerance, thus obtaining the optimal path. The path tolerance can be calculated using the following formula: tolerance=0.1+(esplison-1.0)×0.1, tolerance characterizes the tolerance factor, while esplison characterizes the expansion factor.

[0033] The key points (turning points) in the thinning path are used as control points and input into a cubic B-spline curve to smooth the thinning path and generate the initial trajectory for the moving subject to move to the target position.

[0034] In the above technical solution, based on the ARA* algorithm, an initial path between the target location and the location of the moving subject is quickly generated according to the obstacle target map, and then thinned and smoothed. The thinning process can remove unnecessary redundant points in the initial path, making the path more indirect and direct, thereby reducing the moving distance and time of the moving subject. The smoothing process can reduce the situation of sharp turns and sudden acceleration and deceleration of the moving subject, improve the continuity and stability of the motion, reduce energy consumption and mechanical wear, and obtain the initial trajectory of the moving subject moving to the target location in an unstructured environment.

[0035] In a feasible embodiment, determining the weight coefficients of the preset cost function based on the initial trajectory in step S12 may include: The initial trajectory point sequence is obtained by sampling based on the curvature of the initial trajectory; The initial trajectory point sequence is input into the policy network model to obtain the weight coefficients of the preset cost function output by the policy network model. The weight coefficients include smoothing cost weight, energy cost weight, path deviation cost weight, curvature change cost weight, and safety cost weight. The policy network model is constructed based on near-end policy optimization.

[0036] It is worth noting that adaptive curvature time sampling is performed on the initial path to obtain the initial trajectory point sequence. During adaptive curvature time sampling, to ensure sampling accuracy, interpolation sampling points are performed at locations with higher curvature (kappa) of the smooth trajectory, while larger intervals are selected between sampling points at locations with lower curvature to reduce computational load. The specific sampling strategy is as follows: .

[0037] In this embodiment, Proximal Policy Optimization (PPO) is an advanced reinforcement learning algorithm with good stability and convergence. The policy network model built based on PPO can effectively control the update magnitude during training, avoiding performance degradation or instability caused by excessively large updates. This allows the model to more reliably learn the mapping relationship from the initial trajectory point sequence to the optimal weight coefficients, improving the accuracy and consistency of the output, thus providing more reliable weight parameters for trajectory planning.

[0038] In the above technical solution, adaptive curvature time sampling of the initial trajectory can reasonably adjust the distribution of sampling points according to the curvature changes of the trajectory, ensuring that the subsequently generated trajectory is within the motion capability range of the moving subject. This avoids situations where the trajectory cannot be tracked due to excessive curvature, improving system efficiency and resource utilization while meeting motion requirements. Furthermore, different application scenarios and tasks have different requirements for the trajectory. By introducing a policy network model and using the initial trajectory point sequence as input, the policy network model can fully extract information from it to determine the weight coefficients of the cost function. This ensures that trajectory planning takes into account channel limitations, avoids collisions, and can flexibly adjust the weight allocation of the cost function according to the characteristics of specific scenarios and tasks. The policy network model can adapt to these diverse needs, providing appropriate weight coefficients for different scenarios and enhancing the system's adaptability.

[0039] In one feasible embodiment, the preset cost function may include: ; Where cost represents the total cost of the trajectory, w1 represents the weighting coefficient of the smoothing cost, and J smooth The smoothing cost is represented by w2, which represents the weighting coefficient of the energy cost. energyw3 represents the energy cost, and w3 represents the weighting coefficient of the deviation cost. deviation The cost of the deviation is represented by w4, which is a weighting coefficient representing the cost of curvature variation. Curvature_Change The cost of curvature variation is represented by w5, which is the weighting coefficient for the safety cost. safety Characterize the cost of security.

[0040] Among them, the smoothing cost J smooth It can be obtained through the following calculation formula: , Where N represents the number of trajectory points, x i+1 The x-axis coordinate of the (i+1)th trajectory point is y i+1 The ordinate of the (i+1)th trajectory point is represented by the x-axis. i The x-axis coordinate of the i-th trajectory point, y i The y-axis coordinate of the i-th trajectory point, t i+1 The timestamp representing the (i+1)th trajectory point, t i The timestamp representing the i-th trajectory point.

[0041] Energy cost J energy It can be obtained through the following calculation formula: , Here, Δt represents the difference between the timestamp of the (i+1)th trajectory point and the timestamp of the ith trajectory point.

[0042] Deviation Cost J deviation It can be obtained through the following calculation formula: , Where, x ref,i The x-axis coordinate of the i-th trajectory point on the reference line is y. ref,i θ represents the ordinate of the i-th trajectory point on the reference line. i θ represents the heading angle of the i-th trajectory point. ref,i The turning angle of the i-th trajectory point.

[0043] Curvature change cost J Curvature_Change It can be obtained through the following calculation formula: .

[0044] Security Cost J safety It can be obtained through the following calculation formula: , Where, d safe The minimum safe distance corresponding to the i-th trajectory point is d(x). i ,y i) represents the i-th path point (x) i ,y i ( ) The shortest distance to the obstacle.

[0045] For the cost function, max(0,d) safe -d(x i ,y i This characterizes the situation where the distance between the i-th path point and the obstacle is less than d. safe Only then will a cost be incurred, and the cost increases as the distance between the path and the obstacle decreases.

[0046] Accordingly, the following cost function can be obtained:

[0047]

[0048]

[0049]

[0050] .

[0051] It is worth noting that the path obtained by trajectory planning is constrained by the starting point (the position of the moving subject) and the ending point (the target position), and must satisfy kinematic constraints and real-time reference constraints. Therefore, the following constraints can be constructed: Start and End Constraints: , The above-mentioned starting point and ending point constraints constrain the position, angle, and time of the starting point and ending point during the trajectory planning process.

[0052] Kinematic Constraint: , -θ max ≤θ i ≤θ max , in, Characterizing the maximum velocity in the x-direction, Characterizing the maximum velocity in the y-direction, Characterizing acceleration in the x-direction, Characterizing acceleration in the y-direction, Characterizing jerk in the x-direction, θ represents the acceleration in the y-direction. max Characterizes the maximum steering angle of the moving subject.

[0053] Real-Time Reference Constraint: , Among them, t ref,i Representing the time for i trajectory points, Characterizes the reference tolerance.

[0054] The real-time reference constraints described above can be used to ensure that the trajectory does not deviate too far from the reference path during the trajectory planning process.

[0055] In the above technical solution, the preset cost function and constraints can be flexibly set according to different application scenarios and actual needs, which can ensure that various practical constraints are met during the trajectory planning process, adapt to different scenarios and needs, and thus improve the accuracy and reliability of subsequent trajectory planning results.

[0056] In one feasible embodiment, the policy network model may include an input layer, a first hidden layer, a second hidden layer, and an output layer, wherein the first hidden layer is larger than the second hidden layer; The input layer is used to extract state feature vectors based on the input initial trajectory point sequence. The state feature vectors include the maximum curvature value of the initial trajectory, the energy consumption of the initial trajectory, the maximum deviation of the initial trajectory from the reference trajectory, the minimum distance between the initial trajectory and the obstacle, and the total duration of the initial trajectory. The first hidden layer is used to extract the first higher-order features between the state feature vector and the weights of the preset cost function through the activation function; The second hidden layer is used to extract the second higher-order features between the weights of the state feature vector and the preset cost function based on the first higher-order features through an activation function; The output layer is used to output the weight coefficients of the preset cost function based on the second higher-order features.

[0057] The input layer includes a state extraction module, which extracts the state feature vector of the trajectory quality at a speed of seconds. The output of the first hidden layer serves as the input of the second hidden layer. The sizes of the first and second hidden layers can be preset according to the trajectory planning requirements. In this embodiment, the first hidden layer is a fully connected layer with 128 nodes, and the second hidden layer is a fully connected layer with 64 nodes.

[0058] For example, the initial trajectory sequence is input into the policy network model. The input layer of the policy network model extracts the maximum curvature value s1 of the initial trajectory, the energy consumption s2 of the initial trajectory, the maximum deviation of the initial trajectory from the reference trajectory s3, the minimum distance s4 between the initial trajectory and the obstacle, and the total duration s5 of the initial trajectory from the initial trajectory sequence, as detailed below: S = {s1, s2, s3, s4, s5}; The first hidden layer extracts multiple 128 first higher-order features from the state features using the ReLU activation function. The second hidden layer extracts 64 second higher-order features from the multiple 128 first higher-order features using an activation function. The output layer outputs the action space A based on the multiple 64 second higher-order features. The action space A is as follows: A = {w1, w2, w3, w4, w5}.

[0059] In the above technical solution, the state feature vector extracted from the initial trajectory point sequence by the input layer of the policy network model contains multiple key information, enabling subsequent network layers to learn and compute in a relatively stable and consistent data environment, thus improving the stability and reliability of the model. The structure of a first hidden layer larger than the second hidden layer, and the design of extracting different higher-order features from the two hidden layers, achieves hierarchical feature extraction and abstraction of the input state feature vector. This gradually strips away irrelevant information, focusing on key features closely related to the weight coefficients, helping the model to more accurately learn the complex mapping relationship between state features and weight coefficients. Furthermore, the smaller size of the second hidden layer compared to the first helps reduce the number of model parameters and lowers the risk of overfitting. The output layer directly outputs the weight coefficients of the preset cost function based on the second higher-order feature, avoiding error accumulation and information loss that may occur in intermediate steps, and providing clear and usable parameters for subsequent trajectory planning.

[0060] In a feasible embodiment, in step S13, determining the dominance value of each intermediate trajectory at each time step, and the probability ratio of each intermediate trajectory to the initial trajectory, may include: For each intermediate trajectory, an error index is determined, and the corresponding scalar reward value is determined based on the error index and the preset reward function. The error index includes the sum of jerk, the maximum offset from the preset reference trajectory, the sum of the changes in trajectory curvature, and the inverse ratio of the minimum distance between the trajectory point and the obstacle. Substituting the scalar reward value into the preset dominance function, we obtain the dominance value of the intermediate trajectory at each time step; Determine the first probability of the initial trajectory and the second probability of the intermediate trajectory, and use the ratio of the second probability to the first probability as the probability ratio of the intermediate trajectory to the initial trajectory.

[0061] For example, for each intermediate trajectory, the sum of the accelerations (jerk) of that intermediate trajectory, the maximum offset D from the preset reference trajectory, the sum of the changes in trajectory curvature K, and the inverse ratio R of the minimum distance between the trajectory point and the obstacle are calculated. safe Substituting the following preset reward function, we obtain the scalar reward value R: , in, The adjustment coefficients that characterize the reward function.

[0062] Substituting the scalar reward value R into the following preset advantage function, we obtain the advantage value A(S) of the intermediate trajectory at each time step in a certain state. t ,a t ): , Wherein, Q(S) t ,a t R(S) represents the expected reward of taking an action in a certain state on an intermediate trajectory. t ) represents the scalar reward value in this state.

[0063] The first probability π for determining the initial trajectory θold (a t |s t The second probability π of the intermediate trajectory θ (a t |s t ), and the second probability π θ (a t |s t ) and the first probability π θold (a t |s t The ratio of the intermediate trajectory to the initial trajectory is used as the probability ratio r. t (θ): . s In a feasible embodiment, step S13, iteratively adjusting the weighting coefficients based on multiple dominance values ​​and multiple probability ratios, may include: Substitute the advantage value and probability ratio corresponding to each intermediate trajectory into the following preset shearing objective function to obtain the loss value; With the goal of minimizing the loss value, the gradient descent algorithm is used to iteratively adjust the weight coefficients until a preset iteration stopping condition is met, thereby obtaining the target weight coefficients. The preset iteration stopping conditions include reaching a preset number of iterations, or the change in loss value between two adjacent iterations being less than a preset convergence threshold; The preset shearing objective function includes: , The loss value is represented by clip(), the shearing function is represented by clip(), and the expectation operator is represented by E[]. This represents the probability ratio between the θ-th intermediate trajectory and the initial trajectory at time t. The dominant value of the intermediate trajectory at time t represents the value of the intermediate trajectory at time t. Characterize the shear hyperparameters, and ∈ (0.1~0.2).

[0064] The hyperparameters are used to control the limitation of the update magnitude. In this embodiment, the hyperparameters can be set to... =0.2. Iteration stopping conditions include, but are not limited to, reaching a preset number of iterations, the change in loss value between two adjacent iterations being less than a preset convergence threshold, and the improvement in trajectory quality being lower than a preset quality value or the improvement in cost function being lower than a preset value. The preset convergence threshold can be (1e-5), and the clipping function `clip()` is used to limit the input value to 1- To 1+ Within the range, the expectation operator E[ ] is used to take the expectation of all time steps t and all intermediate trajectories θ.

[0065] In this embodiment, a pruning mechanism is introduced to accelerate the convergence speed of the training process while ensuring model stability. The pruning objective function limits the magnitude of each policy update, ensuring that the new trajectory planning policy does not deviate too far from the old one. The model parameters of the policy network model are updated using the loss value determined by the pruning objective function, and the new policy is used for the next round of interaction and data collection. This process iteratively updates and optimizes the trajectory sequence until the iteration stops. During each iteration, the weights of the policy network model are adjusted based on the new state and reward, thereby improving the trajectory planning results and gradually increasing the quality of trajectory generation, making the motion trajectory of the moving subject smoother, more energy-efficient, and safer.

[0066] The trajectory planning method provided in this disclosure is described below with a complete embodiment, such as... Figure 2 As shown, the trajectory planning method includes: In step S11, the obstacle map, target location, and position of the moving subject are obtained, and the initial trajectory of the moving subject to move to the target location is determined based on the obstacle map, target location, and position of the moving subject.

[0067] In step S121, sampling is performed based on the curvature of the initial trajectory to obtain the initial trajectory point sequence.

[0068] In step S122, the initial trajectory point sequence is input into the policy network model to obtain the weight coefficients of the preset cost function output by the policy network model. The weight coefficients include smoothing cost weight, energy cost weight, path deviation cost weight, curvature change cost weight, and safety cost weight. The policy network model is constructed based on near-end policy optimization.

[0069] In step S131, for each intermediate trajectory, an error index is determined, and the corresponding scalar reward value is determined based on the error index and the preset reward function. The error index includes the sum of jerk, the maximum offset from the preset reference trajectory, the sum of the changes in trajectory curvature, and the inverse ratio of the minimum distance between the trajectory point and the obstacle.

[0070] In step S132, the scalar reward value is substituted into the preset advantage function to obtain the advantage value of the intermediate trajectory at each time step.

[0071] In step S133, the first probability of the initial trajectory and the second probability of the intermediate trajectory are determined respectively, and the ratio of the second probability to the first probability is used as the probability ratio of the intermediate trajectory to the initial trajectory.

[0072] In step S134, the advantage value and probability ratio corresponding to each intermediate trajectory are substituted into the preset shearing objective function to obtain the loss value. With the goal of minimizing the loss value, the gradient descent algorithm is used to iteratively adjust the weight coefficients until the preset iteration stopping condition is met to obtain the target weight coefficient. The preset iteration stopping condition includes reaching the preset number of iterations or the change in loss value between two adjacent iterations being less than the preset convergence threshold.

[0073] In step S14, the target weight coefficient, the preset cost function, and the preset constraints are input into the quadratic programming solver to obtain the target trajectory of the moving subject moving to the target position, which is output by the quadratic programming solver.

[0074] The trajectory planning method disclosed herein uses the ARA* algorithm for initial search, then thins and smooths the coarse path found through search, and adaptively samples the curvature time to obtain a high-quality initial trajectory. Subsequently, a spatiotemporal joint planning proposition is established, and the cost function weights of the quadratic programming are adaptively adjusted using the deep reinforcement learning PPO algorithm. The trajectory is further optimized through a state extraction module, iteratively updating the cost function weights and continuously improving the quality of trajectory planning. Compared with traditional planning methods, this disclosure considers the joint planning of vehicles or UAVs in the spatiotemporal dimensions and designs a reward mechanism that dynamically adjusts the cost function weights. This adaptive adjustment of the cost function weights in the quadratic programming design allows the trajectory planning to adapt to environmental changes in real time, improving its flexibility and efficiency, and solving the problem of insufficient adaptability caused by fixed cost function weights in traditional methods. Furthermore, the initial search using the ARA* algorithm can find feasible solutions more quickly, improving planning efficiency.

[0075] Based on the same inventive concept, this disclosure provides a controller, such as Figure 3 As shown, the controller includes: The first memory 301 stores computer programs; The first processor 302 is configured to execute the computer program in the first memory to implement the trajectory planning method described above.

[0076] The first processor 302 controls the overall operation of the controller 300 to complete all or part of the steps in the trajectory planning method described above. The first memory 301 stores various types of data to support the operation of the controller 300. This data may include, for example, instructions for any application or method operating on the controller 300, and application-related data such as obstacle maps, target locations, the location of the moving subject, weight coefficients, intermediate trajectories, preset cost functions, and preset constraints. The first memory 301 can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read-Only Memory (EPROM), Programmable Read-Only Memory (PROM), Read-Only Memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk.

[0077] In an exemplary embodiment, the controller 300 may be implemented by one or more application-specific integrated circuits (ASICs), digital signal processors (DSPs), digital signal processing devices (DSPDs), programmable logic devices (PLDs), field-programmable gate arrays (FPGAs), controllers, microcontrollers, microprocessors, or other electronic components to execute the trajectory planning method described above.

[0078] In another exemplary embodiment, a computer-readable storage medium including program instructions is also provided, which, when executed by a processor, implement the steps of the trajectory planning method described above. For example, the computer-readable storage medium may be the first memory 301 including the program instructions, which may be executed by the first processor 302 of the controller 300 to complete the trajectory planning method described above.

[0079] In another exemplary embodiment, a computer program product is also provided, which includes a computer program executable by a processor, which, when executed by the processor, implements the steps of the trajectory planning method described above.

[0080] Based on the same inventive concept, this disclosure also provides a vehicle including the aforementioned controller.

[0081] Figure 4 This is a block diagram illustrating a vehicle 400 according to an exemplary embodiment. For example, vehicle 400 can be a hybrid vehicle, a non-hybrid vehicle, an electric vehicle, a fuel cell vehicle, or other type of vehicle. Vehicle 400 can be an autonomous vehicle or a semi-autonomous vehicle. (Refer to...) Figure 4 The vehicle 400 may include various subsystems, such as an infotainment system 410, a perception system 420, a decision control system 430, a drive system 440, and a computing platform 450. The vehicle 400 may also include more or fewer subsystems, and each subsystem may include multiple components. Furthermore, each subsystem and component of the vehicle 400 can be interconnected via wired or wireless means. In some embodiments, the infotainment system 410 may include a communication system, an entertainment system, and a navigation system. The perception system 420 may include several sensors for sensing information about the environment surrounding the vehicle 400. For example, the perception system 420 may include a Global Positioning System (GPS, BeiDou, or other positioning systems), an Inertial Measurement Unit (IMU), lidar, millimeter-wave radar, ultrasonic radar, and a camera device. The decision control system 430 may include a computing system, a vehicle controller, a steering system, a throttle, a braking system, and the aforementioned controllers.

[0082] The drive system 440 may include components that provide powered motion to the vehicle 400. In one embodiment, the drive system 440 may include an engine, an energy source, a transmission system, and wheels. The engine may be one or a combination of internal combustion engines, electric motors, and compressed air engines. The engine is capable of converting energy provided by the energy source into mechanical energy. Some or all of the functions of the vehicle 400 are controlled by a computing platform 450. The computing platform 450 may include at least one second processor 451 and a second memory 452, the second processor 451 being capable of executing instructions 453 stored in the second memory 452. The second processor 451 may be any conventional processor, such as a commercially available CPU. The processor may also include, for example, a Graphics Processing Unit (GPU), a Field Programmable Gate Array (FPGA), a System on Chip (SOC), an Application Specific Integrated Circuit (ASIC), or a combination thereof. The second memory 452 can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk. In addition to instructions 453, the second memory 452 can also store data, such as road maps, route information, vehicle position, direction, speed, etc. The data stored in the second memory 452 can be used by the computing platform 450. In this embodiment of the disclosure, the second processor 451 can execute instructions 453 to complete all or part of the steps of the trajectory planning method described above, or complete all or part of the steps of the trajectory planning method described above through the controller in the decision control system 430.

[0083] The preferred embodiments of this disclosure have been described in detail above with reference to the accompanying drawings. However, this disclosure is not limited to the specific details of the above embodiments. Within the scope of the technical concept of this disclosure, various simple modifications can be made to the technical solutions of this disclosure, and these simple modifications all fall within the protection scope of this disclosure.

[0084] It should also be noted that the various specific technical features described in the above embodiments can be combined in any suitable manner without contradiction. To avoid unnecessary repetition, this disclosure will not describe the various possible combinations separately.

[0085] Furthermore, various different embodiments of this disclosure can be combined in any way, as long as they do not violate the spirit of this disclosure, they should also be regarded as the content disclosed in this disclosure.

Claims

1. A trajectory planning method, characterized in that, include: Obtain the obstacle map, the target location, and the location of the moving subject; and determine the initial trajectory of the moving subject to move to the target location based on the obstacle map, the target location, and the location of the moving subject. The weight coefficients of the preset cost function are determined based on the initial trajectory, and the weight coefficients, the preset cost function, and the preset constraints are input into the quadratic programming solver to obtain multiple intermediate trajectories of the moving subject moving to the target position, output by the quadratic programming solver. The advantage value of each intermediate trajectory at each time step and the probability ratio of each intermediate trajectory to the initial trajectory are determined respectively. The weight coefficient is iteratively adjusted according to the multiple advantage values ​​and multiple probability ratios until a preset iteration stop condition is met to obtain the target weight coefficient. The target weight coefficient, the preset cost function, and the preset constraints are input into the quadratic programming solver to obtain the target trajectory of the moving subject moving to the target position, which is output by the quadratic programming solver.

2. The trajectory planning method according to claim 1, characterized in that, The step of determining the weight coefficients of the preset cost function based on the initial trajectory includes: The initial trajectory point sequence is obtained by sampling based on the curvature of the initial trajectory; The initial trajectory point sequence is input into the policy network model to obtain the weight coefficients of the preset cost function output by the policy network model. The weight coefficients include smoothing cost weight, energy cost weight, path deviation cost weight, curvature change cost weight, and safety cost weight. The policy network model is constructed based on near-end policy optimization.

3. The trajectory planning method according to claim 2, characterized in that, The policy network model includes an input layer, a first hidden layer, a second hidden layer, and an output layer, wherein the first hidden layer is larger than the second hidden layer; The input layer is used to extract a state feature vector based on the input initial trajectory point sequence. The state feature vector includes the maximum curvature value of the initial trajectory, the energy consumption of the initial trajectory, the maximum deviation of the initial trajectory from the reference trajectory, the minimum distance between the initial trajectory and the obstacle, and the total duration of the initial trajectory. The first hidden layer is used to extract a first higher-order feature between the weights of the state feature vector and the preset cost function through an activation function; The second hidden layer is used to extract a second higher-order feature between the weights of the state feature vector and the preset cost function based on the first higher-order feature and through the activation function; The output layer is used to output the weight coefficients of the preset cost function based on the second higher-order feature.

4. The trajectory planning method according to claim 1, characterized in that, The step of determining the dominance value of each intermediate trajectory at each time step, and the probability ratio of each intermediate trajectory to the initial trajectory, includes: For each intermediate trajectory, an error index is determined for the intermediate trajectory, and a corresponding scalar reward value is determined based on the error index and a preset reward function. The error index includes the sum of jerk, the maximum offset from the preset reference trajectory, the sum of the changes in trajectory curvature, and the inverse ratio of the minimum distance between the trajectory point and the obstacle. Substituting the scalar reward value into the preset advantage function, we obtain the advantage value of the intermediate trajectory at each time step; A first probability of the initial trajectory and a second probability of the intermediate trajectory are determined respectively, and the ratio of the second probability to the first probability is taken as the probability ratio of the intermediate trajectory to the initial trajectory.

5. The trajectory planning method according to claim 1, characterized in that, The step of iteratively adjusting the weight coefficients based on a plurality of said dominance values ​​and a plurality of said probability ratios includes: Substitute the advantage value and probability ratio corresponding to each intermediate trajectory into the following preset shearing objective function to obtain the loss value; With the goal of minimizing the loss value, the gradient descent algorithm is used to iteratively adjust the weight coefficients until a preset iteration stopping condition is met, thereby obtaining the target weight coefficients. The preset iteration stopping conditions include reaching a preset number of iterations, or the change in loss value between two adjacent iterations being less than a preset convergence threshold. The preset shearing objective function includes: ， The loss value is represented by clip(), the shearing function is represented by clip(), and the expectation operator is represented by E[]. This represents the probability ratio between the θ-th intermediate trajectory and the initial trajectory at time t. The dominant value of the intermediate trajectory at time t represents the value of the intermediate trajectory at time t. Characterize the shear hyperparameters, and ∈ (0.1~0.2).

6. The trajectory planning method according to claim 1, characterized in that, The preset cost function includes: ； Where cost represents the total cost of the trajectory, w1 represents the weighting coefficient of the smoothing cost, and J smooth The smoothing cost is represented by w2, which represents the weighting coefficient of the energy cost. energy w3 represents the energy cost, and w3 represents the weighting coefficient of the deviation cost. deviation The cost of the deviation is represented by w4, which is a weighting coefficient representing the cost of curvature variation. Curvature_Change The cost of curvature variation is represented by w5, which is the weighting coefficient for the safety cost. safety Characterize the cost of security.

7. The trajectory planning method according to claim 1, characterized in that, Determining the initial trajectory of the moving subject to the target location based on the obstacle map, the target location, and the position of the moving subject includes: The obstacle map is discretized to obtain a grid map, and the initial path between the position of the moving subject and the target position is determined based on the grid map. The initial path is thinned and smoothed according to a preset expansion factor to obtain the initial trajectory of the moving subject moving to the target position.

8. A controller, characterized in that, include: The first memory, on which the computer program is stored; A first processor is configured to execute the computer program in the first memory to implement the method of any one of claims 1-7.

9. A vehicle, characterized in that, Includes the controller as described in claim 8.

10. A robot, characterized in that, Includes the controller as described in claim 8.

Images