Path trajectory planning method, terminal and computer-readable storage medium

Abstract

This application provides a path trajectory planning method, including determining a preset trajectory point sequence for a target vehicle, which is used as an initial warm-up term for a dual-loop iterative algorithm; determining a nonlinear function for the speed limit (SV), which is used to calculate the speed constraint conditions of the dual-loop iterative algorithm; and starting the dual-loop iterative algorithm to optimize and solve the preset trajectory point sequence to output the target path trajectory point sequence. This application accelerates the solution of nonlinear programming problems and shortens the path trajectory planning time by constructing a dual-loop iterative algorithm framework.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a path trajectory planning method, a terminal, and a computer-readable storage medium. Background Technology

[0002] The target vehicle system can reduce the driver's driving stress and related responsibilities. It relies on its own sensors and processors to acquire driving information and navigate the vehicle to the target location. Decision planning is a key part of the target vehicle system. In order to handle driving tasks, it needs to rely on decision planning to provide a safe, comfortable and reasonable trajectory. However, decision planning requires some complex algorithms to calculate the planned trajectory and process it in real time. Therefore, the execution time of decision planning is crucial. Summary of the Invention

[0003] In view of this, the present invention provides a path trajectory planning method, a terminal, and a computer-readable storage medium, which accelerates the solution of nonlinear programming problems and shortens the path trajectory planning time.

[0004] This application provides a path trajectory planning method, including the following steps:

[0005] A preset trajectory point sequence for the target vehicle is determined, and the preset trajectory point sequence is used as an initial hot start item for inputting a double loop iterative algorithm;

[0006] A nonlinear function for determining the SV speed limit is used to calculate the speed constraint conditions of the double-loop iterative algorithm.

[0007] A double-loop iterative algorithm is initiated to optimize and solve the preset trajectory point sequence, so as to output the target path trajectory point sequence.

[0008] Optionally, determining the preset trajectory point sequence of the target vehicle includes:

[0009] Determine at least one route reference line based on the target vehicle's location information, travel task, and map information;

[0010] The reference line is laterally and / or longitudinally planned based on obstacle information to generate a spatiotemporal route for the target vehicle from its current position to the desired position.

[0011] The spatiotemporal path is optimized to obtain a preset trajectory point sequence.

[0012] Optionally, the optimization solution for the spatiotemporal path includes:

[0013] The spatiotemporal path is optimized and solved based on the first objective function and preset constraints;

[0014] The first objective function is:

[0015] Total cost = Position cost + Speed ​​cost + Acceleration cost + Acceleration cost + Following vehicle cost

[0016] Wherein, position cost is the square of the difference between each position and each position sr on the longitudinal displacement line in the SL diagram, multiplied by the weight W_s; velocity cost is the square of the difference between each velocity and each velocity vr on the longitudinal displacement line in the SL diagram, multiplied by the weight W_v; acceleration cost is the square of each acceleration, multiplied by the weight W_a; jerk cost is the square of each jerk, multiplied by the weight W_jerk; and following cost is the longitudinal coordinate position of each target vehicle in the SL diagram minus the distance d_stop between each position and the stop distance, minus the time and velocity difference required for the target vehicle to collide with the target vehicle. The velocity difference is each velocity multiplied by 2 minus the velocity of the target vehicle.

[0017] Optionally, the nonlinear function for determining the SV speed limit includes:

[0018] The SV speed limit is determined based on preset speed limit information and the location information of the target vehicle. The preset speed limit information includes at least one of map information, scene information, and risk information.

[0019] The SV speed limit is smoothed to obtain a nonlinear function of the SV speed limit.

[0020] Optionally, the smoothing process for the SV speed limit includes:

[0021] The SV speed limit is smoothed according to the second objective function;

[0022] The second objective function is:

[0023] SV objective function = velocity cost + cost of velocity's first derivative with respect to position + cost of velocity's second derivative with respect to position + cost of velocity's third derivative with respect to position

[0024] Wherein, the speed cost is the square of the difference between the solved speed V_o and each V in the SV speed limit multiplied by the weight W_0; the cost of the speed with respect to the first derivative with respect to the position is the square of the solved speed V_o with respect to the first derivative with respect to the position in the SV speed limit multiplied by the weight W_1; the cost of the speed with respect to the second derivative with respect to the position is the square of the solved speed V_o with respect to the second derivative with respect to the position in the SV speed limit multiplied by the weight W_2; the cost of the speed with respect to the third derivative with respect to the position is the square of the solved speed V_o with respect to the third derivative with respect to the position in the SV speed limit multiplied by the weight W_3.

[0025] Optionally, the step of initiating a double-loop iterative algorithm to optimize the preset trajectory point sequence includes:

[0026] Start the outer loop, and run the inner loop within the outer loop;

[0027] The preset trajectory point sequence is iteratively updated in the inner loop;

[0028] In the outer loop, convergence is determined and slack variables are adjusted based on the solution results of two consecutive iterations in the inner loop.

[0029] When the outer loop ends, the solution results are output as a sequence of target path trajectory points.

[0030] Optionally, the iterative update of the preset trajectory point sequence includes:

[0031] The preset trajectory point sequence is optimized and solved according to the third objective function and preset constraints. The third objective function is used to solve for one or more of the following: position cost, velocity cost, acceleration cost, jerk cost, following cost, position relaxation cost, and velocity relaxation cost.

[0032] If the solution is successful, the solution result sequence is used as the hot start term, and the dual variable is set for the next iteration;

[0033] If the sequence of solutions obtained from two consecutive iterations in the inner loop meets the inner loop convergence threshold, then the inner loop is exited.

[0034] Optionally, the step of determining convergence and adjusting slack variables based on the solution results of two consecutive iterations in the inner loop includes:

[0035] If the solution result sequence of two consecutive inner loops satisfies the outer loop convergence condition threshold, then exit the outer loop;

[0036] If the slack variables output by the inner loop exceed a preset threshold, then the weight of the slack variables in the third objective function is increased.

[0037] This application also provides a terminal, including: a memory and a processor, wherein the memory stores a speed planning program, and when the speed planning program is executed by the processor, it implements the steps of the path trajectory planning method as described above.

[0038] This application also provides a computer storage medium storing computer program instructions; when the computer program instructions are executed by a processor, they implement the path trajectory planning method described above.

[0039] In summary, the path trajectory planning method, terminal, and computer-readable storage medium provided by this invention include: determining a preset trajectory point sequence for a target vehicle, wherein the preset trajectory point sequence is used as an initial hot start term for a dual-loop iterative algorithm; determining a nonlinear function for the SV speed limit, wherein the nonlinear function for the SV speed limit is used to calculate the speed constraint conditions of the dual-loop iterative algorithm; and starting the dual-loop iterative algorithm to optimize and solve the preset trajectory point sequence to output a target path trajectory point sequence. This application accelerates the solution of nonlinear programming problems and shortens the path trajectory planning time by constructing a dual-loop iterative algorithm framework.

[0040] The above description is merely an overview of the technical solution of the present invention. In order to better understand the technical means of the present invention and to implement it in accordance with the contents of the specification, and to make the above and other objects, features and advantages of the present invention more apparent and understandable, preferred embodiments are described in detail below with reference to the accompanying drawings. Attached Figure Description

[0041] Figure 1 This is a flowchart illustrating the path trajectory planning method according to an embodiment of the present invention;

[0042] Figure 2 This is a schematic diagram illustrating the relationship between SV speed limits in an embodiment of the present invention;

[0043] Figure 3 This is a logical schematic diagram of the double loop iterative algorithm shown in an embodiment of the present invention;

[0044] Figure 4 This is a schematic diagram illustrating the specific process of the path trajectory planning method in an embodiment of the present invention;

[0045] Figure 5 This is a schematic diagram of the path trajectory planning system shown in an embodiment of the present invention. Detailed Implementation

[0046] To further illustrate the technical means and effects adopted by the present invention in order to achieve the intended purpose, the present invention will be described in detail below with reference to the accompanying drawings and preferred embodiments.

[0047] First Embodiment

[0048] Figure 1 This is a flowchart illustrating a path trajectory planning method according to an embodiment of the present invention.

[0049] Please refer to Figure 1 This invention provides a path trajectory planning method, including:

[0050] Step 201: Determine the preset trajectory point sequence of the target vehicle. The preset trajectory point sequence is used as the initial hot start item for inputting the double loop iterative algorithm.

[0051] In one embodiment, determining a preset sequence of trajectory points for the target vehicle includes:

[0052] Determine at least one route reference line based on the target vehicle's location information, travel task, and map information;

[0053] Based on obstacle information, the reference line is laterally and / or longitudinally planned to generate the spatiotemporal route of the target vehicle from its current position to its desired position.

[0054] The spatiotemporal path is optimized and solved to obtain a preset sequence of trajectory points.

[0055] In this embodiment, the path planning system is used to implement a path trajectory planning method for a target vehicle. The path trajectory planning system includes, but is not limited to, a perception module, a positioning module, a map module, a prediction module, a routing module, a decision-making and planning module, and a control module. The system comprises the following modules: a perception module, a positioning module, and a control module. The perception module determines the environmental information surrounding the target vehicle, including but not limited to information about surrounding obstacles (such as vehicles, pedestrians, cones, and non-motorized vehicles). Combined with positioning and map information, it can identify traffic lights, traffic signs, and lane information. The positioning module determines the current location of the target vehicle and manages its travel data. The map module stores map information, including but not limited to road shape, lane information, merging and diverging information, intersection information, pedestrian crossing information, and speed bumps. The routing module generates globally planned path information based on the positioning and map modules, as well as the target vehicle's task. The prediction module generates predicted trajectories for obstacles based on obstacle information from the perception module and map information from the map module. The decision-making and planning module generates a trajectory that meets vehicle dynamics constraints and ensures safety and comfort based on the predicted obstacle trajectories from the prediction module, the vehicle's localization from the positioning module, and the target vehicle's travel task from the routing module. The control module outputs corresponding steering wheel angles and drive or braking torques based on the trajectory from the decision-making and planning module to control the vehicle's trajectory.

[0056] When the decision planning module receives path information and a pncmap (plan and control map) from the routing module, it generates one or more motion reference lines based on the current position provided by the positioning module. These reference lines are part of the path information or local paths accessible from adjacent lanes. After receiving the obstacle prediction trajectory and its own positioning from the prediction module, the decision planning module makes lateral and longitudinal decisions regarding each obstacle, without being limited to the order of these decisions. Once the obstacle decisions are made, the lateral planning module plans the path from the target vehicle's current position to the desired position. This path is extended laterally based on the reference lines to avoid obstacles. The longitudinal planning module generates an ST map based on the laterally planned path and the obstacle's motion trajectory, and forms upper and lower boundaries of s with respect to time t based on the obstacle decisions: the upper boundary S_max and the lower boundary S_min at each time t. Within these upper and lower boundaries with respect to time t, the module generates the spatiotemporal route from the target vehicle's current position to the desired position, regardless of whether the decision method or dynamic programming method is used.

[0057] In one embodiment, when the longitudinal planning module is in front of the lateral planning module, the longitudinal planning module generates an ST diagram based on the coarse path or reference line path of the lateral decision and the movement trajectory of the obstacle, and forms upper and lower boundaries of s with respect to time t based on the decision on the obstacle, namely the upper boundary S_max and the lower boundary S_min of s at each time t; within the upper and lower boundaries with respect to time t, the spatiotemporal route of the target vehicle from the current position to the desired position is generated, which is not limited to the decision method and dynamic planning method.

[0058] In one embodiment, optimizing the spatiotemporal path includes:

[0059] The spatiotemporal path is optimized and solved based on the first objective function and preset constraints;

[0060] The first objective function is:

[0061] Total cost = Position cost + Speed ​​cost + Acceleration cost + Acceleration cost + Following vehicle cost

[0062] Wherein, position cost is the square of the difference between each position and each position sr on the longitudinal displacement line in the SL diagram, multiplied by the weight W_s; velocity cost is the square of the difference between each velocity and each velocity vr on the longitudinal displacement line in the SL diagram, multiplied by the weight W_v; acceleration cost is the square of each acceleration, multiplied by the weight W_a; jerk cost is the square of each jerk, multiplied by the weight W_jerk; and following cost is the longitudinal coordinate position of each target vehicle in the SL diagram minus the distance d_stop between each position and the stop distance, minus the time and velocity difference required for the target vehicle to collide with the target vehicle. The velocity difference is each velocity multiplied by 2 minus the velocity of the target vehicle.

[0063] In this embodiment, based on the obtained spatiotemporal route of the target vehicle from its current position to its desired position, a quadratic programming optimization is performed using an objective function to generate the path trajectory. Considered constraints include jerk constraints, acceleration constraints, speed constraints, and following constraints. The jerk, acceleration, and speed constraints require that each jerk, acceleration, and speed be within the vehicle's dynamic performance range; the following constraint requires that the distance between the solved position and the target vehicle be within the safe distance d_safe. A trajectory point sequence is calculated based on the objective function and constraints and input into a dual-loop system as the initial hot-start term for the inner loop. Thus, by adding a following term to the first objective function when optimizing the spatiotemporal route, the problem of adaptively adjusting the distance to the vehicle ahead during following is addressed in existing technologies. This allows for adaptation to different speeds and the following habits of drivers with different driving styles. By adding a following term to the objective function of trajectory optimization, the safe distance and stopping distance of the vehicle during following are considered, enabling adaptive adjustment of the following distance while ensuring following safety.

[0064] Step 202: Determine the nonlinear function of the SV speed limit. The nonlinear function of the SV speed limit is used to calculate the speed constraint conditions of the double loop iterative algorithm.

[0065] In one embodiment, determining the nonlinear function of the SV speed limit includes:

[0066] The SV speed limit is determined based on the preset speed limit information and the location information of the target vehicle. The preset speed limit information includes at least one of map information, scene information, and risk information.

[0067] The SV speed limit is smoothed to obtain a nonlinear function of the SV speed limit.

[0068] In this embodiment, the target vehicle considers, but is not limited to, speed limits based on map information, speed limits based on scene recognition, and speed limits based on risk information, depending on its location. As shown in Table 1, speed limits based on map information include, but are not limited to, road speed limits, zebra crossing speed limits, speed bump speed limits, intersection speed limits, ramp speed limits, and curvature speed limits; speed limits based on scene recognition include, but are not limited to, speed limits due to congestion in adjacent lanes, speed limits for pedestrians at zebra crossings, speed limits based on the movement trend of the previous cycle, speed limits based on openspace information, and speed limits for stationary vehicles in front of zebra crossings; speed limits based on risk information include, but are not limited to, speed limits for vehicles decelerating in adjacent lanes, speed limits due to vehicle system malfunctions, and speed limits for environments where functions are enabled. Considering speed limits based on map information, scene recognition, and risk information, and integrating them into nonlinear constraints on position and speed, helps to complete location-based speed limits in various scenarios and ensure driving safety. In this embodiment, speed limits can also be applied to locations of interest within the ODD (operational design domain), and then a nonlinear problem about speed constraints can be solved to address the problem that existing technologies cannot impose speed limits at locations of interest.

[0069] Table 1

[0070]

[0071] It's worth noting that the final speed limit generated based on the above speed limit conditions is the minimum speed limit value corresponding to each of the above speed limit information at each sampling location. The number of sampling points is denoted as N. Each sampling location S_point corresponds to a speed limit V_limit. The number of samples N for S_point is determined by the selected interval delta_s, where N is the ratio of the difference between the current position and the desired position S of the target vehicle to delta_s. The final output is as follows: Figure 2 The following is a series of SV speed limits.

[0072] In one embodiment, smoothing the SV speed limit includes:

[0073] The SV speed limit is smoothed according to the second objective function;

[0074] The second objective function is:

[0075] SV objective function = velocity cost + cost of velocity's first derivative with respect to position + cost of velocity's second derivative with respect to position + cost of velocity's third derivative with respect to position

[0076] Wherein, the speed cost is the square of the difference between the solved speed V_o and each V in the SV speed limit multiplied by the weight W_0; the cost of the speed with respect to the first derivative with respect to the position is the square of the solved speed V_o with respect to the first derivative with respect to the position in the SV speed limit multiplied by the weight W_1; the cost of the speed with respect to the second derivative with respect to the position is the square of the solved speed V_o with respect to the second derivative with respect to the position in the SV speed limit multiplied by the weight W_2; the cost of the speed with respect to the third derivative with respect to the position is the square of the solved speed V_o with respect to the third derivative with respect to the position in the SV speed limit multiplied by the weight W_3.

[0077] In this embodiment, for the SV speed limit, to obtain the higher-order derivative of each point and represent it using a nonlinear function, a smooth SV function is generated by optimizing the objective function using quadratic programming. For example... Figure 2 Since the relationship between S and V in the SV speed limit is non-linear, quadratic programming is used to optimize the smoothing process in order to represent the function as a continuous function.

[0078] Step 203: Start the double loop iterative algorithm to optimize and solve the preset trajectory point sequence, so as to output the target path trajectory point sequence.

[0079] Please refer to Figure 3 In this embodiment, a dual-loop system is initiated to perform final optimization of the spatiotemporal route of the target vehicle from its current position to its desired position, resulting in a smooth trajectory that satisfies the vehicle's movement. Before initiating the dual-loop system, the iteration counts for the outer loop (iters_out) and the inner loop (iters_in), the position slack variable thresholds (s_threshold) and velocity slack variable thresholds (v_threshold), and the convergence condition thresholds for the outer loop (violate_out) and the inner loop (violate_in) are set. When initiating the dual-loop system, the outer loop is started first, and the inner loop, along with convergence and slack constraint checks, is primarily run within the outer loop. This lightweight dual-loop iterative framework, by establishing convergence conditions, a warm-start term, and modifications to the dual variables within the dual loop, accelerates the solution of the nonlinear programming problem.

[0080] In one embodiment, a double-loop iterative algorithm is initiated to optimize and solve the preset trajectory point sequence, including:

[0081] Start the outer loop, and run the inner loop within the outer loop;

[0082] Iteratively update the preset trajectory point sequence within the inner loop;

[0083] In the outer loop, convergence is determined and slack variables are adjusted based on the solution results of two consecutive iterations in the inner loop;

[0084] When the outer loop ends, the solution results are output as a sequence of target path trajectory points.

[0085] In one embodiment, iteratively updating a preset trajectory point sequence includes:

[0086] The preset trajectory point sequence is optimized and solved according to the third objective function and preset constraints. The third objective function is used to solve for one or more of the following: position cost, velocity cost, acceleration cost, jerk cost, following cost, position relaxation cost, and velocity relaxation cost.

[0087] If the solution is successful, the solution result sequence is used as the hot start term, and the dual variable is set for the next iteration;

[0088] If the sequence of solutions obtained from two consecutive iterations in the inner loop meets the inner loop convergence threshold, then the inner loop is exited.

[0089] In this embodiment, the nonlinear problem is mainly expanded linearly at various positions in the inner loop to solve multiple linearized problems, making the solution of the linearized problem approximate the solution of the nonlinear problem. The preset trajectory point sequence solved in the first objective function is used as the initial hot-start term, and the constraint matrix and constraint terms of the quadratic programming are updated according to the position, velocity, and acceleration of the initial hot-start term.

[0090] It is worth mentioning that slack variables, including velocity slack variables and position slack variables, are introduced in the inner loop. When solving nonlinear problems, setting slack variables for position and velocity ensures that the solution will not fail due to the position exceeding the position limit or the velocity exceeding the speed limit, thereby increasing the success rate and enabling the solution to handle situations where the vehicle speed exceeds the speed limit and the position limit is not a hard requirement.

[0091] Specifically, the third objective function is:

[0092] Total cost' = Position cost' + Speed ​​cost' + Acceleration cost' + Jet acceleration cost' + Following cost' + Position slack cost' + Speed ​​slack cost'

[0093] Wherein, position cost' is the square of the difference between each position and each position sr on the longitudinal displacement line in the SL diagram, multiplied by the weight W_s; velocity cost' is the square of the difference between each velocity and each velocity vr on the longitudinal displacement line in the SL diagram, multiplied by the weight W_v; acceleration cost' is the square of each acceleration, multiplied by the weight W_a; jerk cost' is the square of each jerk, multiplied by the weight W_jerk; following term cost' is the longitudinal coordinate position of the following target in the SL diagram minus the distance d_stop between each position and the following stop, and then minus the time and velocity difference required for the target vehicle to collide with the following target, where the velocity difference is each velocity multiplied by 2 minus the velocity of the following target; position relaxation term cost' is the square of the position relaxation variable multiplied by the weight W_slack_s; velocity relaxation term cost' is the square of the velocity relaxation variable multiplied by the weight W_slack_v.

[0094] The constraints considered include jerk constraints, acceleration constraints, velocity constraints, position constraints, variable equality constraints, and following-term constraints. Among these, the jerk constraint requires that each jerk be within the vehicle's dynamic performance range; the velocity constraint has two parts: one is that the velocity is greater than 0, and the other is that the difference between the velocity and the velocity slack variable is less than the first-order Taylor expansion at each position at the hot start point; the position constraint also includes two parts: one part is that the solved position is greater than the lower position limit Smin, and the other part is that the difference between the solved position and the position slack variable is less than the difference between the upper position limit Smax and the safe distance d_safe; the variable equality constraints include the equations between velocity and jerk, and between position and jerk, as shown in the following equations:

[0095] V t+T =V t +a t *T+0.5*j t *T 2

[0096] S ++T =S t +V t +0.5*T 2 +1 / 6*j t *T 3

[0097] Among them, V t+T， V t S t+T S t , a t j t T represents the velocity at time t+T, the velocity at time t, the position at time t+T, the position at time t, the acceleration at time t, the jerk at time t, and the time interval, respectively.

[0098] If the solution is successful, the result is used as the warm-up term, and a dual variable is set for the next iteration. Convergence in the inner loop is determined when the cumulative value of the position difference and velocity difference in the solution result sequences of two consecutive inner loop iterations is less than the threshold `violate_in`, indicating convergence and exiting the inner loop. If the optimizer reports an error, such as an out-of-bounds error or no solution, both the inner and outer loops are exited simultaneously, ending the double-loop iteration, and the current solution result is output as the target path trajectory point sequence.

[0099] In one implementation, convergence determination and relaxation variable adjustment are performed based on the solution results of two consecutive iterations in the inner loop, including:

[0100] If the solution result sequence of two consecutive inner loops satisfies the outer loop convergence condition threshold, then exit the outer loop;

[0101] If the slack variables output by the inner loop exceed a preset threshold, then the weight of the slack variables in the third objective function is increased.

[0102] In this embodiment, the convergence judgment in the outer loop is as follows: for two adjacent nonlinear solution result sequences, if the cumulative value of each position difference and velocity difference is less than the threshold violent_out, then convergence is judged and the outer loop is exited; otherwise, the outer loop continues.

[0103] External loop slack variable assessment: When the position slack variable exceeds the threshold s_threshold, increase the position slack variable weight W_slack_s; when the velocity slack variable exceeds the threshold v_threshold, increase the velocity slack variable weight W_slack_v.

[0104] When the outer loop ends, the solution result serves as the final path trajectory point, navigating the target vehicle from its current location to its target location.

[0105] The path trajectory planning method of this invention includes: determining a preset trajectory point sequence for a target vehicle, wherein the preset trajectory point sequence is used as an initial hot start term for a dual-loop iterative algorithm; determining a nonlinear function for the speed limit (SV), wherein the nonlinear function for the speed limit (SV) is used to calculate the speed constraint conditions of the dual-loop iterative algorithm; and starting the dual-loop iterative algorithm to optimize and solve the preset trajectory point sequence to output a target path trajectory point sequence. The path trajectory planning method of this application accelerates the solution of nonlinear programming problems and shortens the path trajectory planning time by constructing a dual-loop iterative algorithm framework.

[0106] Second Embodiment

[0107] Please refer to Figure 4In the path trajectory planning method of this embodiment, a spatiotemporal route from the current position to the desired position of the target vehicle is determined within the ODD region. For this route, a path trajectory is generated by quadratic programming optimization using an objective function, and this trajectory is input into the dual loop as a warm-up value. The target vehicle's speed limit (SV) from the current position to the desired position is determined based on its current location. For the SV speed limit, a smooth SV function is generated by quadratic programming optimization using an objective function, and this function is input into the dual loop. The dual loop is then activated, with the inner loop executing within the outer loop. Convergence and constraint relaxation are determined based on the results of the inner loop. In the inner loop, nonlinear quadratic programming optimization using an objective function is performed to generate a path trajectory that satisfies the vehicle's motion and constraints. The dual loop framework is used to optimize the path trajectory and navigate the target vehicle from its current position to the desired position.

[0108] For details on the specific process of executing the above method steps in this embodiment, please refer to the relevant descriptions in the above embodiments, which will not be repeated here.

[0109] Third Embodiment

[0110] Please refer to Figure 5 This invention also provides a path trajectory planning system, characterized in that it includes a perception module, a positioning module, a map module, a prediction module, a routing module, a decision planning module, and a control module; wherein,

[0111] The perception module is used to acquire input information, including obstacle information, target vehicle location information, and map information;

[0112] The routing module is used to generate globally planned path information based on the travel task of the target vehicle;

[0113] The prediction module is used to generate predicted trajectories of obstacles based on obstacle information and map information;

[0114] The decision planning module is used to generate the target trajectory based on the predicted trajectory, location information, and travel task;

[0115] The control module is used to control the movement of the target vehicle according to the target trajectory.

[0116] The target vehicle's path planning system includes, but is not limited to, a perception module, a positioning module, a map module, a prediction module, a routing module, a decision-making and planning module, and a control module. The perception module determines the environmental information surrounding the target vehicle, including but not limited to information about surrounding obstacles (such as vehicles, pedestrians, cones, non-motorized vehicles, etc.), and, combined with positioning and map information, can determine traffic lights, traffic signs, lane information, etc. The positioning module determines the target vehicle's current location and manages its travel data. The map module stores map information, including but not limited to road shape, lane information, merging and diverging information, intersection information, pedestrian crossing information, speed bumps, etc. The routing module generates globally planned path information based on the positioning and map modules, as well as the target vehicle's task. The prediction module generates predicted trajectories for obstacles based on obstacle information from the perception module and map information from the map module. The decision-making and planning module generates a trajectory that meets vehicle dynamics constraints and is safe and comfortable, based on the predicted obstacle trajectories from the prediction module, the vehicle's localization from the positioning module, and the target vehicle's travel task from the routing module. The control module outputs corresponding steering wheel angles and drive or braking torques based on the trajectory from the decision-making and planning module to control the vehicle's trajectory.

[0117] For details on the specific process of executing the above method steps in this embodiment, please refer to the relevant descriptions in the above embodiments, which will not be repeated here.

[0118] This application also provides a terminal, including: a memory and a processor, wherein the memory stores a speed planning program, and when the speed planning program is executed by the processor, it implements the steps of the path trajectory planning method as described above.

[0119] For details on the specific process of executing the above method steps in this embodiment, please refer to the relevant descriptions in the above embodiments, which will not be repeated here.

[0120] This application also provides a computer-readable storage medium storing computer program instructions; when the computer program instructions are executed by a processor, they implement the path trajectory planning method described above.

[0121] For details on the specific process of executing the above method steps in this embodiment, please refer to the relevant descriptions in the above embodiments, which will not be repeated here.

[0122] 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.

[0123] In this document, the terms “comprising,” “including,” or any other variations thereof are intended to cover non-exclusive inclusion, which includes not only the elements listed but also other elements not expressly listed.

[0124] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A path trajectory planning method, characterized in that, Includes the following steps: A preset trajectory point sequence for the target vehicle is determined, and the preset trajectory point sequence is used as an initial hot start item for inputting a double loop iterative algorithm; A nonlinear function for determining the SV speed limit is used to calculate the speed constraint conditions of the double-loop iterative algorithm. A double-loop iterative algorithm is initiated to optimize and solve the preset trajectory point sequence, so as to output the target path trajectory point sequence; The determination of the preset trajectory point sequence of the target vehicle includes: Determine at least one route reference line based on the target vehicle's location information, travel task, and map information; Based on obstacle information, the reference line is laterally and longitudinally planned to generate a spatiotemporal route for the target vehicle from its current position to its desired position; the lateral planning plans the path from the target vehicle's current position to its desired position; the longitudinal planning generates an ST diagram based on the laterally planned path and the movement trajectory of the obstacles, and forms upper and lower boundaries of s with respect to time t based on the decision regarding the obstacles. The spatiotemporal path is optimized and solved to obtain a preset trajectory point sequence; The initiation of the dual-loop iterative algorithm to optimize the solution of the preset trajectory point sequence includes: Start the outer loop, and run the inner loop within the outer loop; The preset trajectory point sequence is iteratively updated in the inner loop; In the outer loop, convergence is determined and slack variables are adjusted based on the solution results of two consecutive iterations in the inner loop. When the outer loop ends, the solution results are output as a sequence of target path trajectory points.

2. The path trajectory planning method according to claim 1, characterized in that, The optimization solution for the spatiotemporal path includes: The spatiotemporal path is optimized and solved based on the first objective function and preset constraints; The first objective function is: Total cost = Position cost + Speed ​​cost + Acceleration cost + Jet acceleration cost + Following vehicle cost Wherein, position cost is the square of the difference between each position and each position sr on the longitudinal displacement line in the SL diagram, multiplied by the weight W_s; velocity cost is the square of the difference between each velocity and each velocity vr on the longitudinal displacement line in the SL diagram, multiplied by the weight W_v; acceleration cost is the square of each acceleration, multiplied by the weight W_a; jerk cost is the square of each jerk, multiplied by the weight W_jerk; and following cost is the longitudinal coordinate position of each target vehicle in the SL diagram minus the distance d_stop between each position and the stop distance, minus the time and velocity difference required for the target vehicle to collide with the target vehicle. The velocity difference is each velocity multiplied by 2 minus the velocity of the target vehicle.

3. The path trajectory planning method according to claim 1, characterized in that, The nonlinear function for determining the SV speed limit includes: The SV speed limit is determined based on preset speed limit information and the location information of the target vehicle. The preset speed limit information includes at least one of map information, scene information, and risk information. The SV speed limit is smoothed to obtain a nonlinear function of the SV speed limit.

4. The path trajectory planning method according to claim 3, characterized in that, The smoothing process for the SV speed limit includes: The SV speed limit is smoothed according to the second objective function; The second objective function is: SV objective function = velocity cost + cost of the first derivative of velocity with respect to position + cost of the second derivative of velocity with respect to position + cost of the third derivative of velocity with respect to position Wherein, the speed cost is the square of the difference between the solved speed V_o and each V in the SV speed limit multiplied by the weight W_0; the cost of the speed with respect to the first derivative with respect to the position is the square of the solved speed V_o with respect to the first derivative with respect to the position in the SV speed limit multiplied by the weight W_1; the cost of the speed with respect to the second derivative with respect to the position is the square of the solved speed V_o with respect to the second derivative with respect to the position in the SV speed limit multiplied by the weight W_2; the cost of the speed with respect to the third derivative with respect to the position is the square of the solved speed V_o with respect to the third derivative with respect to the position in the SV speed limit multiplied by the weight W_3.

5. The path trajectory planning method according to claim 1, characterized in that, The iterative update of the preset trajectory point sequence includes: The preset trajectory point sequence is optimized and solved according to the third objective function and preset constraints. The third objective function is used to solve for one or more of the following: position cost, velocity cost, acceleration cost, jerk cost, following cost, position relaxation cost, and velocity relaxation cost. If the solution is successful, the solution result sequence is used as the hot start term, and the dual variable is set for the next iteration; If the sequence of solutions obtained from two consecutive iterations in the inner loop meets the inner loop convergence threshold, then the inner loop is exited.

6. The path trajectory planning method according to claim 5, characterized in that, The convergence judgment and relaxation variable adjustment based on the solution results of two consecutive iterations in the inner loop include: If the solution result sequence of two consecutive inner loops satisfies the outer loop convergence condition threshold, then exit the outer loop; If the slack variables output by the inner loop exceed a preset threshold, then the weight of the slack variables in the third objective function is increased.

7. A terminal, characterized in that, The terminal includes a memory and a processor, wherein the memory stores a path trajectory planning program, and when the path trajectory planning program is executed by the processor, it implements the steps of the path trajectory planning method as described in any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer program instructions; when executed by a processor, the computer-readable storage medium implements the path trajectory planning method as described in any one of claims 1-6.

Images