Track planning hot start method and device, equipment and storage medium

Abstract

The invention discloses a trajectory planning hot start method, apparatus and device, and a storage medium. The method comprises the steps of obtaining a previous cycle planning result of a previous cycle control sequence of a reference line corresponding to a current vehicle; when the planning result of the previous period meets a preset condition, generating a vehicle driving path according to the planning result of the previous period; and when the planning result of the previous cycle does not meet the preset condition, recalculating a plurality of initial solutions, and taking the trajectory corresponding to the initial solution with the minimum total cost as a final path, so that the solving efficiency and success rate of trajectory planning can be improved, and the speed and efficiency of trajectory planning hot start are improved.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a trajectory planning hot start method, device, equipment and storage medium. Background Art

[0002] The existing path trajectory planning method is to determine a preset trajectory point sequence for the target vehicle, which is used to input a double-loop iterative algorithm as an initial hot start item; determine a nonlinear function of the safety velocity (SV) speed limit, which is used to calculate the speed constraint condition of the double-loop iterative algorithm; and start the double-loop iterative algorithm to optimize and solve the preset trajectory point sequence to output the target path trajectory point sequence.

[0003] However, the existing trajectory planning methods only use a preset trajectory point sequence as the initial hot start option in the hot start scheme, without providing a detailed introduction to the specific method for determining the preset trajectory point sequence. This results in low trajectory planning solution efficiency and low trajectory planning solution success rate. Summary of the Invention

[0004] The main purpose of the present invention is to provide a trajectory planning hot start method, device, equipment and storage medium, aiming to solve the technical problem that the existing technology lacks a specific method for determining the trajectory point sequence, resulting in low trajectory planning solution efficiency and low trajectory planning solution success rate.

[0005] In a first aspect, the present invention provides a trajectory planning hot start method, the trajectory planning hot start method comprising the following steps: Obtain the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle; When the planning result of the previous cycle meets the preset conditions, generating a vehicle driving path according to the planning result of the previous cycle; When the planning result of the previous cycle does not meet the preset condition, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the minimum total cost is used as the final path.

[0006] Optionally, before obtaining the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle, the trajectory planning hot start method further includes: Obtain coordinate variables and constraints of the current vehicle in the current scene, and generate a reference line based on the coordinate variables and the constraints.

[0007] Optionally, obtaining coordinate variables and constraint conditions of the current vehicle in the current scene, and generating a reference line according to the coordinate variables and the constraint conditions includes: Get the coordinate variables and constraints of the current vehicle in the current scene; Optimizing the coordinate variables to obtain optimized variables and objective functions of the coordinate variables; Determining kinematic constraints, continuity constraints, and starting point constraints based on the constraints; Planning a smooth path from an initial point to a target lane centerline in a preset coordinate system according to the optimization variables, the objective function, the kinematic constraints, the continuity constraints, and the starting point constraints; A reference speed and a reference distance at each moment are obtained, target coordinates and a target heading angle optimized by the smooth path hot start are determined based on the reference speed and the reference distance, a spatiotemporal reference point is generated based on the target and the target heading angle, and a reference line is generated based on the spatiotemporal reference point.

[0008] Optionally, obtaining the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle includes: The lateral control variable and longitudinal control variable of the iterative linear quadratic regulation ILQR corresponding to the reference line of the current vehicle are calculated according to the following formula:

[0009]

[0010] in, is the horizontal distance from the current point to the reference point, is the angular deviation between the current point and the reference point, is the front wheel angle at the current point, is the angular velocity of the front wheel at the current point, 、 、 、 Corresponding to 、 、 、 The preset scaling factor of The control sequence of the previous cycle is determined according to the horizontal control variables and the vertical control variables, and the control sequence of the previous cycle is used as the late planning result of the previous week.

[0011] Optionally, when the planning result of the previous cycle meets a preset condition, generating a vehicle driving path according to the planning result of the previous cycle includes: If the planning result of the previous cycle is not empty, the ILQR optimization results of two or more consecutive cycles are normal, and the planning trajectory length of the planning result of the previous cycle is not close to 0, it is determined that the planning result of the previous cycle meets the preset conditions; The trajectory of the planning result of the previous cycle is used as the initial trajectory, and the vehicle driving path is generated according to the initial trajectory.

[0012] Optionally, when the planning result of the previous cycle does not meet the preset condition, recalculating multiple initial solutions and taking the trajectory corresponding to the initial solution with the minimum total cost as the final path includes: When the planning result of the previous cycle is empty, or the ILQR optimization results of two or more consecutive cycles are abnormal, or the planning trajectory length of the planning result of the previous cycle is close to 0, it is determined that the planning result of the previous cycle does not meet the preset conditions; Obtaining the state variables of the ILQR, recursively deducing the state variables according to a preset kinematic formula, and obtaining a state sequence; Traversing the state sequence, and calculating the running cost and terminal cost of each state according to preset constraint information; Accumulating and summing the operating cost and the terminal cost of each state to obtain a total cost of each control sequence; A target initial solution with the minimum total cost is selected from the state sequence according to the total cost, and a trajectory corresponding to the target initial solution with the minimum total cost is used as a final path for the current vehicle.

[0013] Optionally, the acquiring of the state variables of the ILQR and the recursive deduction of the state variables according to a preset kinematic formula to obtain a state sequence include: Get ILQR status variables , and recursively deduce the state variables according to the following preset kinematic formula to obtain the state sequence:

[0014]

[0015]

[0016]

[0017]

[0018]

[0019]

[0020]

[0021] Where x is the x-coordinate in the Cartesian coordinate system, and y is the y-coordinate in the Cartesian coordinate system. is the heading angle of the vehicle, is the front wheel turning angle, is the angular velocity of the front wheel, s is the distance to the center of the rear axle, v is the vehicle speed, a is the vehicle acceleration, alfa is the angular acceleration of the front wheel, jerk is the vehicle jerk, is the transpose operation, is the wheelbase.

[0022] In a second aspect, to achieve the above-mentioned purpose, the present invention further proposes a trajectory planning hot start device, the trajectory planning hot start device comprising: The planning result acquisition module is used to obtain the previous cycle planning result of the previous cycle control sequence of the reference line corresponding to the current vehicle; A path determination module, configured to generate a vehicle driving path according to the planning result of the previous cycle when the planning result of the previous cycle meets a preset condition; The path recalculation module is used to recalculate multiple initial solutions when the planning result of the previous cycle does not meet the preset conditions, and use the trajectory corresponding to the initial solution with the minimum total cost as the final path.

[0023] In the third aspect, in order to achieve the above-mentioned purpose, the present invention also proposes a trajectory planning hot start device, which includes: a memory, a processor, and a trajectory planning hot start program stored in the memory and runnable on the processor, and the trajectory planning hot start program is configured to implement the steps of the trajectory planning hot start method described above.

[0024] In a fourth aspect, in order to achieve the above-mentioned purpose, the present invention also proposes a storage medium, on which a trajectory planning hot start program is stored. When the trajectory planning hot start program is executed by a processor, the steps of the trajectory planning hot start method as described above are implemented.

[0025] The trajectory planning hot start method proposed in the present invention obtains the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle; when the previous cycle planning result meets the preset conditions, the vehicle driving path is generated according to the previous cycle planning result; when the previous cycle planning result does not meet the preset conditions, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the minimum total cost is used as the final path. This can improve the solution efficiency and success rate of trajectory planning, and improve the speed and efficiency of trajectory planning hot start. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 A schematic diagram of the device structure of the hardware operating environment involved in the embodiment of the present invention; Figure 2 This is a flow chart of the first embodiment of the trajectory planning hot start method of the present invention; Figure 3This is a flow chart of a second embodiment of the trajectory planning hot start method of the present invention; Figure 4 This is a flow chart of a third embodiment of the trajectory planning hot start method of the present invention; Figure 5 This is a flow chart of a fourth embodiment of the trajectory planning hot start method of the present invention; Figure 6 This is a flow chart of a fifth embodiment of the trajectory planning hot start method of the present invention; Figure 7 This is a functional module diagram of the first embodiment of the trajectory planning hot start device of the present invention.

[0027] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings and in conjunction with the embodiments. DETAILED DESCRIPTION

[0028] It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0029] The solution of the embodiment of the present invention is mainly: by obtaining the previous cycle planning result of the previous cycle control sequence of the reference line corresponding to the current vehicle; when the previous cycle planning result meets the preset conditions, the vehicle driving path is generated according to the previous cycle planning result; when the previous cycle planning result does not meet the preset conditions, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the smallest total cost is used as the final path, which can improve the solution efficiency and success rate of trajectory planning, improve the speed and efficiency of trajectory planning hot start, and solve the technical problems in the prior art of lacking a specific determination method for the trajectory point sequence, resulting in low trajectory planning solution efficiency and low trajectory planning solution success rate.

[0030] Reference Figure 1 , Figure 1 This is a schematic diagram of the device structure of the hardware operating environment involved in the embodiment of the present invention.

[0031] like Figure 1As shown, the device may include: a processor 1001, such as a CPU, a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. The communication bus 1002 is used to implement communication between these components. The user interface 1003 may include a display and an input unit such as a keyboard. Optionally, the user interface 1003 may also include a standard wired interface or a wireless interface. The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a Wi-Fi interface). The memory 1005 may be a high-speed RAM memory or a non-volatile memory, such as a disk storage. The memory 1005 may also be a storage device independent of the processor 1001.

[0032] Those skilled in the art will understand that Figure 1 The device structure shown in the figure does not constitute a limitation of the device, and may include more or fewer components than shown in the figure, or combine certain components, or arrange the components differently.

[0033] like Figure 1 As shown, the memory 1005 as a storage medium may include an operating device, a network communication module, a user interface module, and a trajectory planning hot start program.

[0034] The device of the present invention calls the trajectory planning hot start program stored in the memory 1005 through the processor 1001 and performs the following operations: Obtain the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle; When the planning result of the previous cycle meets the preset conditions, generating a vehicle driving path according to the planning result of the previous cycle; When the planning result of the previous cycle does not meet the preset condition, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the minimum total cost is used as the final path.

[0035] The device of the present invention calls the trajectory planning hot start program stored in the memory 1005 through the processor 1001, and further performs the following operations: Obtain coordinate variables and constraints of the current vehicle in the current scene, and generate a reference line based on the coordinate variables and the constraints.

[0036] The device of the present invention calls the trajectory planning hot start program stored in the memory 1005 through the processor 1001, and further performs the following operations: Get the coordinate variables and constraints of the current vehicle in the current scene; Optimizing the coordinate variables to obtain optimized variables and objective functions of the coordinate variables; Determining kinematic constraints, continuity constraints, and starting point constraints based on the constraints; Planning a smooth path from an initial point to a target lane centerline in a preset coordinate system according to the optimization variables, the objective function, the kinematic constraints, the continuity constraints, and the starting point constraints; A reference speed and a reference distance at each moment are obtained, target coordinates and a target heading angle optimized by the smooth path hot start are determined based on the reference speed and the reference distance, a spatiotemporal reference point is generated based on the target and the target heading angle, and a reference line is generated based on the spatiotemporal reference point.

[0037] The device of the present invention calls the trajectory planning hot start program stored in the memory 1005 through the processor 1001, and further performs the following operations: The lateral control variable and longitudinal control variable of the iterative linear quadratic regulation ILQR corresponding to the reference line of the current vehicle are calculated according to the following formula:

[0038]

[0039] in, is the horizontal distance from the current point to the reference point, is the angular deviation between the current point and the reference point, is the front wheel angle at the current point, is the angular velocity of the front wheel at the current point, 、 、 、 Corresponding to 、 、 、 The preset scaling factor of The control sequence of the previous cycle is determined according to the horizontal control variables and the vertical control variables, and the control sequence of the previous cycle is used as the late planning result of the previous week.

[0040] The device of the present invention calls the trajectory planning hot start program stored in the memory 1005 through the processor 1001, and further performs the following operations: If the planning result of the previous cycle is not empty, the ILQR optimization results of two or more consecutive cycles are normal, and the planning trajectory length of the planning result of the previous cycle is not close to 0, it is determined that the planning result of the previous cycle meets the preset conditions; The trajectory of the planning result of the previous cycle is used as the initial trajectory, and the vehicle driving path is generated according to the initial trajectory.

[0041] The device of the present invention calls the trajectory planning hot start program stored in the memory 1005 through the processor 1001, and further performs the following operations: When the planning result of the previous cycle is empty, or the ILQR optimization results of two or more consecutive cycles are abnormal, or the planning trajectory length of the planning result of the previous cycle is close to 0, it is determined that the planning result of the previous cycle does not meet the preset conditions; Obtaining the state variables of the ILQR, recursively deducing the state variables according to a preset kinematic formula, and obtaining a state sequence; Traversing the state sequence, and calculating the running cost and terminal cost of each state according to preset constraint information; Accumulating and summing the operating cost and the terminal cost of each state to obtain a total cost of each control sequence; A target initial solution with the minimum total cost is selected from the state sequence according to the total cost, and a trajectory corresponding to the target initial solution with the minimum total cost is used as a final path for the current vehicle.

[0042] The device of the present invention calls the trajectory planning hot start program stored in the memory 1005 through the processor 1001, and further performs the following operations: Get ILQR status variables , and recursively deduce the state variables according to the following preset kinematic formula to obtain the state sequence:

[0043]

[0044]

[0045]

[0046]

[0047]

[0048]

[0049]

[0050] Where x is the x-coordinate in the Cartesian coordinate system, and y is the y-coordinate in the Cartesian coordinate system. is the heading angle of the vehicle, is the front wheel turning angle, is the front wheel angular velocity, s is the distance to the center of the rear axle, v is the vehicle speed, a is the vehicle acceleration, alfa is the front wheel angular acceleration, and jerk is the vehicle jerk.

[0051] Through the above scheme, this embodiment obtains the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle; when the previous cycle planning result meets the preset conditions, the vehicle driving path is generated according to the previous cycle planning result; when the previous cycle planning result does not meet the preset conditions, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the smallest total cost is used as the final path, which can improve the solution efficiency and success rate of trajectory planning and improve the speed and efficiency of trajectory planning hot start.

[0052] Based on the above hardware structure, an embodiment of the trajectory planning hot start method of the present invention is proposed.

[0053] Reference Figure 2 , Figure 2 Schematic diagram of the flow of the first embodiment of the trajectory planning hot start method of the present invention.

[0054] In a first embodiment, the trajectory planning hot start method includes the following steps: Step S10: Obtain the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle.

[0055] It should be noted that after obtaining the reference line corresponding to the current vehicle, the control sequence of the previous cycle can be obtained based on the reference line, and then the planning result of the previous cycle corresponding to the control sequence can be obtained.

[0056] Furthermore, the step S10 specifically includes the following steps: The lateral control variable and longitudinal control variable of the iterative linear quadratic regulation ILQR corresponding to the reference line of the current vehicle are calculated according to the following formula:

[0057]

[0058] in, is the horizontal distance from the current point to the reference point, is the angular deviation between the current point and the reference point, is the front wheel angle at the current point, is the angular velocity of the front wheel at the current point, 、 、 、 Corresponding to 、 、 、 The preset scaling factor of The control sequence of the previous cycle is determined according to the horizontal control variables and the vertical control variables, and the control sequence of the previous cycle is used as the late planning result of the previous week.

[0059] In a specific implementation, a multi-objective feedback control algorithm can be used to calculate the lateral control variable alfa and the longitudinal control variable jerk respectively. After obtaining the lateral control variable and the longitudinal control variable, the control sequence of the previous cycle is determined based on the lateral control variable and the longitudinal control variable, and the control sequence of the previous cycle is used as the late planning result of the previous week.

[0060] Step S20: When the planning result of the previous cycle meets the preset conditions, a vehicle driving path is generated according to the planning result of the previous cycle.

[0061] It should be understood that when the planning result of the previous cycle meets the pre-set conditions, the vehicle driving path can be generated according to the planning result of the previous cycle.

[0062] Step S30: When the planning result of the previous cycle does not meet the preset condition, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the minimum total cost is used as the final path.

[0063] It is understandable that when the planning result of the previous cycle does not meet the preset conditions, multiple initial solutions can be recalculated to filter out the initial solution with the minimum total cost, and then obtain the corresponding trajectory as the final path.

[0064] Through the above scheme, this embodiment obtains the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle; when the previous cycle planning result meets the preset conditions, the vehicle driving path is generated according to the previous cycle planning result; when the previous cycle planning result does not meet the preset conditions, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the smallest total cost is used as the final path, which can improve the solution efficiency and success rate of trajectory planning and improve the speed and efficiency of trajectory planning hot start.

[0065] Furthermore, Figure 3 This is a flow chart of the second embodiment of the trajectory planning hot start method of the present invention. Figure 3 As shown, a second embodiment of the trajectory planning hot start method of the present invention is proposed based on the first embodiment. In this embodiment, before step S10, the trajectory planning hot start method further includes the following steps: Step S01: Obtain coordinate variables and constraint conditions of the current vehicle in the current scene, and generate a reference line according to the coordinate variables and the constraint conditions.

[0066] It should be noted that after obtaining the coordinate variables and constraint conditions of the current vehicle in the current scene, a reference line can be generated according to the coordinate variables and the constraint conditions.

[0067] Through the above scheme, this embodiment can generate a reference line based on the coordinate variables and constraints of the current vehicle in the current scene after obtaining the coordinate variables and the constraints, and can quickly determine the reference line, thereby improving the speed and efficiency of trajectory planning hot start.

[0068] Furthermore, Figure 4 This is a flow chart of the third embodiment of the trajectory planning hot start method of the present invention. Figure 4 As shown, a third embodiment of the trajectory planning hot start method of the present invention is proposed based on the second embodiment. In this embodiment, step S01 specifically includes the following steps: Step S011: Obtain the coordinate variables and constraints of the current vehicle in the current scene.

[0069] It should be noted that the coordinate variables and constraints of the current vehicle in the current scene are obtained.

[0070] Step S012: Optimize the coordinate variables to obtain optimized variables and objective functions of the coordinate variables.

[0071] It can be understood that after optimizing the coordinate variables, the optimized variables and objective functions of the coordinate variables can be obtained.

[0072] Step S013: determining kinematic constraints, continuity constraints, and starting point constraints according to the constraints.

[0073] It should be understood that kinematic constraints, continuity constraints and starting point constraints can be determined based on the constraints.

[0074] Step S014: planning a smooth path from the initial point to the center line of the target lane in a preset coordinate system according to the optimization variables, the objective function, the kinematic constraints, the continuity constraints, and the starting point constraints.

[0075] It is understandable that a smooth path from the initial point to the center line of the target lane can be planned in a preset coordinate system based on the optimization variables, the objective function, the kinematic constraints, the continuity constraints and the starting point constraints.

[0076] Step S015: Obtain a reference speed and a reference distance at each moment, determine the target coordinates and target heading angle optimized by the smooth path hot start based on the reference speed and the reference distance, generate a spatiotemporal reference point based on the target and the target heading angle, and generate a reference line based on the spatiotemporal reference point.

[0077] In a specific implementation, the reference speed and reference distance at each moment can be obtained by recursion with a fixed acceleration based on longitudinal kinematics, and each state can be traversed to obtain the reference speed and reference distance of the state according to the index. The coordinates and heading angle on warmstart_refline through the smooth path hot start optimization are determined based on the reference speed and the reference distance, and the spatiotemporal reference point SpaTemRefPoint is constructed. A reference line can be generated based on the spatiotemporal reference point. Hot start optimization is performed through the smooth path, which can improve the smoothness of the control sequence submodule when using the control algorithm to track the reference line.

[0078] In the specific implementation, the reference line mainly contains pose information, excluding related information such as acceleration and velocity. The PiecewiseJerk optimization algorithm is used to plan a smooth path warmstart_refline from the initial point init_point to the centerline of the target lane in the Frenet coordinate system to improve the smoothness of the control algorithm when tracking the reference line in the control sequence submodule. The mathematical model of the PiecewiseJerk optimization algorithm is as follows: Quadratic programming standard form:

[0079]

[0080] in, represents the minimization objective, is a quadratic term, is the transpose operation, is a linear term, is the coefficient vector, x is the decision variable vector, is a symmetric matrix representing the quadratic part of the objective function, Indicates that it is limited to is the constraint matrix, and are the lower and upper bound vectors respectively.

[0081] Optimization variables:

[0082] Where, :n points l coordinate :n points l The first derivative with respect to s :n points l The second derivative with respect to s Objective function:

[0083] Where, : Objective function close to the reference line

[0084] : Comfort objective function

[0085] :The objective function of the center of the drivable area

[0086] : End point similarity objective function

[0087] in, for The weight corresponding to the coordinate, For n points coordinate( ), for The weight corresponding to the coordinate, For n points The first derivative of s ( ), for The weight corresponding to the coordinate, For n points l The second derivative with respect to s , for The weight corresponding to the coordinate, For n points l The third derivative with respect to s , For each point reference l The weight corresponding to the coordinate, For each point on the reference line l coordinate, The end point on the reference line l The weight corresponding to the coordinate, The end point on the reference line l coordinate, The end point on the reference line The weight corresponding to the coordinate, The end point on the reference line coordinate, The end point on the reference line The weight corresponding to the coordinate, The end point on the reference line coordinate.

[0088] Constraints: The ego vehicle must be within the boundaries provided by the decision module

[0089] Kinematic constraints

[0090]

[0091] Continuity constraints

[0092]

[0093]

[0094] Start point constraint

[0095]

[0096]

[0097] in, For the i-th The lower bound of the coordinate-related constraints, For the i-th Upper bounds on coordinate-related constraints, For the i-th The lower bound of the coordinate-related constraints, For the i-th Upper bounds on coordinate-related constraints, For the i-th point coordinate, For the i-th point coordinate, For the i-th point coordinate, For the i-th point coordinate, is the i+1th point coordinate is the i+1th point coordinate, is the i+1th point coordinate, is the distance between the two optimized points. For the 0th optimization point coordinate, For the 0th optimization point coordinate, For the 0th optimization point coordinate, For the initial point coordinate, For the initial point coordinate, For the initial point coordinate.

[0098] This embodiment adopts the above scheme, obtains the coordinate variables and constraints of the current vehicle in the current scene; optimizes the coordinate variables to obtain the optimization variables and objective function of the coordinate variables; determines the kinematic constraints, continuity constraints and starting point constraints according to the constraints; plans a smooth path from the initial point to the center line of the target lane in a preset coordinate system according to the optimization variables, the objective function, the kinematic constraints, the continuity constraints and the starting point constraints, obtains the reference speed and reference distance at each moment, determines the target coordinates and target heading angle optimized by hot start of the smooth path according to the reference speed and the reference distance, generates a spatiotemporal reference point according to the target and the target heading angle, and generates a reference line according to the spatiotemporal reference point, which can improve the solution efficiency and success rate of trajectory planning and improve the speed and efficiency of trajectory planning hot start.

[0099] Furthermore, Figure 5 This is a flow chart of the fourth embodiment of the trajectory planning hot start method of the present invention. Figure 5 As shown, a fourth embodiment of the trajectory planning hot start method of the present invention is proposed based on the first embodiment. In this embodiment, step S20 specifically includes the following steps: Step S21: When the planning result of the previous cycle is not empty, the ILQR optimization results of two or more consecutive cycles are normal, and the planning trajectory length of the planning result of the previous cycle is not close to 0, it is determined that the planning result of the previous cycle meets the preset conditions.

[0100] It should be noted that if the planning result of the previous cycle is not empty, the iterative linear quadratic regulator (ILQR) optimization results of two or more consecutive cycles are normal, and the planning trajectory length of the planning result of the previous cycle is not close to 0, it can be determined that the planning result of the previous cycle meets the preset conditions.

[0101] Step S22: Using the trajectory of the previous cycle planning result as the initial trajectory, and generating a vehicle driving path according to the initial trajectory.

[0102] It is understandable that the trajectory of the planning result of the previous cycle can be used as the initial trajectory, and the vehicle driving path can be generated according to the initial trajectory.

[0103] Through the above scheme, this embodiment determines that the planning result of the previous cycle meets the preset conditions when the planning result of the previous cycle is not empty, the ILQR optimization results of two or more consecutive cycles are normal, and the length of the planned trajectory of the planning result of the previous cycle is not close to 0; uses the trajectory of the planning result of the previous cycle as the initial trajectory, and generates a vehicle driving path based on the initial trajectory, which can improve the solution efficiency and success rate of trajectory planning and improve the speed and efficiency of trajectory planning hot start.

[0104] Furthermore, Figure 6 FIG. 5 is a flow chart of the fifth embodiment of the trajectory planning hot start method of the present invention. Figure 6 As shown, a fifth embodiment of the trajectory planning hot start method of the present invention is proposed based on the first embodiment. In this embodiment, step S30 specifically includes the following steps: Step S31: When the planning result of the previous cycle is empty, or the ILQR optimization results of two or more consecutive cycles are abnormal, or the planning trajectory length of the planning result of the previous cycle is close to 0, it is determined that the planning result of the previous cycle does not meet the preset conditions.

[0105] It should be noted that the ILQR control variable is There are two ways to determine the control sequence. Under normal circumstances, the control sequence of the previous cycle is used. The control sequence is recalculated only in the following situations: The planning result of the previous cycle is empty. The ILQR optimization result status is not OK for more than two consecutive cycles. The trajectory length of the previous cycle is close to 0. Step S32: Obtain the state variables of the ILQR, recursively deducing the state variables according to the preset kinematic formula to obtain the state sequence.

[0106] It is understandable that after obtaining the state variables of the ILQR, the state variables can be recursively deduced according to a preset kinematic formula to obtain a state sequence.

[0107] Furthermore, the step S32 specifically includes the following steps: Get ILQR status variables , and recursively deduce the state variables according to the following preset kinematic formula to obtain the state sequence:

[0108]

[0109]

[0110]

[0111]

[0112]

[0113]

[0114]

[0115] Where x is the x-coordinate in the Cartesian coordinate system, and y is the y-coordinate in the Cartesian coordinate system. is the heading angle of the vehicle, is the front wheel turning angle, is the angular velocity of the front wheel, s is the distance to the center of the rear axle, v is the vehicle speed, a is the vehicle acceleration, alfa is the angular acceleration of the front wheel, jerk is the vehicle jerk, is the transpose operation, is the wheelbase.

[0116] In the specific implementation, the ILQR state variable is , according to the above kinematic formula, the entire state sequence is recursively obtained Step S33: traverse the state sequence and calculate the operation cost and terminal cost of each state according to preset constraint information.

[0117] It is understandable that the running_cost and terminal_cost of each state are calculated according to the constraint information while traversing the state sequence, that is, the running cost and terminal cost of each state are calculated according to the preset constraint information while traversing the state sequence.

[0118] Step S34: Accumulate and sum the operating cost and the terminal cost of each state to obtain the total cost of each control sequence.

[0119] It should be understood that these costs are accumulated and summed to obtain the total_cost of the control sequence, that is, the operating cost and the terminal cost of each state are accumulated and summed to obtain the total cost of each control sequence.

[0120] Step S35 , selecting a target initial solution with the minimum total cost from the state sequence according to the total cost, and using the trajectory corresponding to the target initial solution with the minimum total cost as the final path of the current vehicle.

[0121] It can be understood that, based on the total cost, the target initial solution with the minimum total cost is screened out from the state sequence, and the trajectory corresponding to the target initial solution with the minimum total cost can be used as the final path of the current vehicle.

[0122] In the specific implementation, there are two methods to recalculate the control sequence: 1. Calculate based on the control sequence of the previous cycle 2. Sample four accelerations [-2.5, 0.15, 0.4, 0.8] and calculate based on the fixed acceleration combined with the control algorithm.

[0123] The above two methods calculate 5 control sequences and state sequences, and calculate the cost of these 5 sequences in combination with the constraint information provided by the upstream constraint module. The sequence with the minimum cost is selected as the optimal sequence. The state sequence is traversed and the running_cost and terminal_cost of each state are calculated according to the constraint information. These costs are accumulated and summed to obtain the total_cost of the control sequence. , the entire state sequence can be deduced based on kinematics, but in most normal cases, the state sequence is not calculated using the control sequence of the previous cycle. According to the kinematic formula, the entire state sequence can be deduced recursively.

[0124] This embodiment uses the above scheme to determine that the planning result of the previous cycle does not meet the preset conditions when the planning result of the previous cycle is empty, or the ILQR optimization results of two or more consecutive cycles are abnormal, or the planning trajectory length of the planning result of the previous cycle is close to 0; obtain the ILQR state variable, recursively deduces the state variable according to the preset kinematic formula to obtain a state sequence; traverse the state sequence, and calculate the operating cost and terminal cost of each state according to preset constraint information; accumulate and sum the operating cost and the terminal cost of each state to obtain the total cost of each control sequence; select the target initial solution with the minimum total cost from the state sequence based on the total cost, and use the trajectory corresponding to the target initial solution with the minimum total cost as the final path of the current vehicle; can systematically and informationally quickly identify trial parts, which not only meets test requirements but also reduces trial production costs. It is suitable for the BOM structure of prototype vehicles for different test purposes, realizes standardization of subsequent project applications, avoids the phenomenon of low accuracy of manual component identification, improves the accuracy of component identification, reduces the development cost of prototype vehicles, and improves the speed and efficiency of trajectory planning hot start.

[0125] Accordingly, the present invention further provides a trajectory planning hot start device.

[0126] Reference Figure 7 , Figure 7 This is a functional module diagram of the first embodiment of the trajectory planning hot start device of the present invention.

[0127] In a first embodiment of the trajectory planning hot start device of the present invention, the trajectory planning hot start device includes: The planning result acquisition module 10 is used to obtain the previous cycle planning result of the previous cycle control sequence of the reference line corresponding to the current vehicle.

[0128] The path determination module 20 is configured to generate a vehicle driving path according to the planning result of the previous cycle when the planning result of the previous cycle meets a preset condition.

[0129] The path recalculation module 30 is used to recalculate multiple initial solutions when the planning result of the previous cycle does not meet the preset conditions, and use the trajectory corresponding to the initial solution with the minimum total cost as the final path.

[0130] The planning result acquisition module 10 is further configured to acquire coordinate variables and constraint conditions of the current vehicle in the current scene, and generate a reference line according to the coordinate variables and the constraint conditions.

[0131] The planning result acquisition module 10 is also used to obtain the coordinate variables and constraints of the current vehicle in the current scenario; optimize the coordinate variables to obtain the optimization variables and objective functions of the coordinate variables; determine the kinematic constraints, continuity constraints and starting point constraints based on the constraints; plan a smooth path from the initial point to the center line of the target lane in a preset coordinate system based on the optimization variables, the objective function, the kinematic constraints, the continuity constraints and the starting point constraints; obtain the reference speed and reference distance at each moment, determine the target coordinates and target heading angle optimized by hot start of the smooth path based on the reference speed and the reference distance, generate a spatiotemporal reference point based on the target and the target heading angle, and generate a reference line based on the spatiotemporal reference point.

[0132] The planning result acquisition module 10 is further configured to calculate the lateral control variable and the longitudinal control variable of the iterative linear quadratic regulation ILQR corresponding to the reference line of the current vehicle according to the following formula:

[0133]

[0134] in, is the horizontal distance from the current point to the reference point, is the angular deviation between the current point and the reference point, is the front wheel angle at the current point, is the angular velocity of the front wheel at the current point, 、 、 、 Corresponding to 、 、 、 The preset scaling factor of The control sequence of the previous cycle is determined according to the horizontal control variables and the vertical control variables, and the control sequence of the previous cycle is used as the late planning result of the previous week.

[0135] The path determination module 20 is further configured to determine that the planning result of the previous cycle meets preset conditions when the planning result of the previous cycle is not empty, the ILQR optimization results of two or more consecutive cycles are normal, and the length of the planned trajectory of the planning result of the previous cycle is not close to 0; use the trajectory of the planning result of the previous cycle as the initial trajectory, and generate a vehicle driving path based on the initial trajectory.

[0136] The path recalculation module 30 is further configured to determine that the planning result of the previous cycle does not meet a preset condition when the planning result of the previous cycle is empty, or the ILQR optimization results of two or more consecutive cycles are abnormal, or the planning trajectory length of the planning result of the previous cycle is close to 0; obtain the state variables of the ILQR, recursively deducing the state variables according to a preset kinematic formula to obtain a state sequence; traverse the state sequence, and calculate the operating cost and terminal cost of each state according to preset constraint information; accumulate and sum the operating cost and the terminal cost of each state to obtain the total cost of each control sequence; and screen out the target initial solution with the minimum total cost from the state sequence according to the total cost, and use the trajectory corresponding to the target initial solution with the minimum total cost as the final path of the current vehicle.

[0137] The path recalculation module 30 is also used to obtain the state variables of ILQR , and recursively deduce the state variables according to the following preset kinematic formula to obtain the state sequence:

[0138]

[0139]

[0140]

[0141]

[0142]

[0143]

[0144]

[0145] Where x is the x-coordinate in the Cartesian coordinate system, and y is the y-coordinate in the Cartesian coordinate system. is the heading angle of the vehicle, is the front wheel turning angle, is the angular velocity of the front wheel, s is the distance to the center of the rear axle, v is the vehicle speed, a is the vehicle acceleration, alfa is the angular acceleration of the front wheel, jerk is the vehicle jerk, is the transpose operation, is the wheelbase.

[0146] Among them, the steps implemented by each functional module of the trajectory planning hot start device can refer to the various embodiments of the trajectory planning hot start method of the present invention, and will not be repeated here.

[0147] In addition, an embodiment of the present invention further provides a storage medium, wherein a trajectory planning hot start program is stored on the storage medium. When the trajectory planning hot start program is executed by a processor, the following operations are performed: Obtain the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle; When the planning result of the previous cycle meets the preset conditions, generating a vehicle driving path according to the planning result of the previous cycle; When the planning result of the previous cycle does not meet the preset condition, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the minimum total cost is used as the final path.

[0148] Furthermore, when the trajectory planning hot start program is executed by the processor, the following operations are also implemented: Obtain coordinate variables and constraints of the current vehicle in the current scene, and generate a reference line based on the coordinate variables and the constraints.

[0149] Furthermore, when the trajectory planning hot start program is executed by the processor, the following operations are also implemented: Get the coordinate variables and constraints of the current vehicle in the current scene; Optimizing the coordinate variables to obtain optimized variables and objective functions of the coordinate variables; Determining kinematic constraints, continuity constraints, and starting point constraints based on the constraints; Planning a smooth path from an initial point to a target lane centerline in a preset coordinate system according to the optimization variables, the objective function, the kinematic constraints, the continuity constraints, and the starting point constraints; A reference speed and a reference distance at each moment are obtained, target coordinates and a target heading angle optimized by the smooth path hot start are determined based on the reference speed and the reference distance, a spatiotemporal reference point is generated based on the target and the target heading angle, and a reference line is generated based on the spatiotemporal reference point.

[0150] Furthermore, when the trajectory planning hot start program is executed by the processor, the following operations are also implemented: The lateral control variable and longitudinal control variable of the iterative linear quadratic regulation ILQR corresponding to the reference line of the current vehicle are calculated according to the following formula:

[0151]

[0152] in, is the horizontal distance from the current point to the reference point, is the angular deviation between the current point and the reference point, is the front wheel angle at the current point, is the angular velocity of the front wheel at the current point, 、 、 、 Corresponding to 、 、 、 The preset scaling factor of The control sequence of the previous cycle is determined according to the horizontal control variables and the vertical control variables, and the control sequence of the previous cycle is used as the late planning result of the previous week.

[0153] Furthermore, when the trajectory planning hot start program is executed by the processor, the following operations are also implemented: If the planning result of the previous cycle is not empty, the ILQR optimization results of two or more consecutive cycles are normal, and the planning trajectory length of the planning result of the previous cycle is not close to 0, it is determined that the planning result of the previous cycle meets the preset conditions; The trajectory of the planning result of the previous cycle is used as the initial trajectory, and the vehicle driving path is generated according to the initial trajectory.

[0154] Furthermore, when the trajectory planning hot start program is executed by the processor, the following operations are also implemented: When the planning result of the previous cycle is empty, or the ILQR optimization results of two or more consecutive cycles are abnormal, or the planning trajectory length of the planning result of the previous cycle is close to 0, it is determined that the planning result of the previous cycle does not meet the preset conditions; Obtaining the state variables of the ILQR, recursively deducing the state variables according to a preset kinematic formula, and obtaining a state sequence; Traversing the state sequence, and calculating the running cost and terminal cost of each state according to preset constraint information; Accumulating and summing the operating cost and the terminal cost of each state to obtain a total cost of each control sequence; A target initial solution with the minimum total cost is selected from the state sequence according to the total cost, and a trajectory corresponding to the target initial solution with the minimum total cost is used as a final path for the current vehicle.

[0155] Furthermore, when the trajectory planning hot start program is executed by the processor, the following operations are also implemented: Get ILQR status variables , and recursively deduce the state variables according to the following preset kinematic formula to obtain the state sequence:

[0156]

[0157]

[0158]

[0159]

[0160]

[0161]

[0162]

[0163] Where x is the x-coordinate in the Cartesian coordinate system, and y is the y-coordinate in the Cartesian coordinate system. is the heading angle of the vehicle, is the front wheel turning angle, is the angular velocity of the front wheel, s is the distance to the center of the rear axle, v is the vehicle speed, a is the vehicle acceleration, alfa is the angular acceleration of the front wheel, jerk is the vehicle jerk, is the transpose operation, is the wheelbase.

[0164] Those skilled in the art will understand that all or part of the steps in the above-mentioned implementation methods can be implemented by instructing related hardware through a program. The program is stored in a storage medium and includes a number of instructions for enabling a device (which may be a single-chip microcomputer, chip, etc.) or a processor to execute all or part of the steps of the method described in each embodiment of the present application; and the aforementioned storage medium is a computer-readable storage medium, including: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, and other media that can store program codes.

[0165] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article, or apparatus comprising the element.

[0166] The serial numbers of the above embodiments of the present invention are for description only and do not represent the advantages or disadvantages of the embodiments.

[0167] The above are only preferred embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention description and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A trajectory planning hot start method, characterized in that: The trajectory planning hot start method includes: Obtain the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle; When the planning result of the previous cycle meets the preset conditions, generating a vehicle driving path according to the planning result of the previous cycle; When the planning result of the previous cycle does not meet the preset condition, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the minimum total cost is used as the final path.

2. The trajectory planning hot start method according to claim 1, characterized in that: Before obtaining the last cycle planning result of the last cycle control sequence corresponding to the reference line of the current vehicle, the trajectory planning hot start method further includes: Obtain coordinate variables and constraints of the current vehicle in the current scene, and generate a reference line based on the coordinate variables and the constraints.

3. The trajectory planning hot start method according to claim 2, characterized in that: The acquiring of coordinate variables and constraint conditions of the current vehicle in the current scene, and generating a reference line according to the coordinate variables and the constraint conditions, includes: Get the coordinate variables and constraints of the current vehicle in the current scene; Optimizing the coordinate variables to obtain optimized variables and objective functions of the coordinate variables; Determining kinematic constraints, continuity constraints, and starting point constraints based on the constraints; Planning a smooth path from an initial point to a target lane centerline in a preset coordinate system according to the optimization variables, the objective function, the kinematic constraints, the continuity constraints, and the starting point constraints; A reference speed and a reference distance at each moment are obtained, target coordinates and a target heading angle optimized by the smooth path hot start are determined based on the reference speed and the reference distance, a spatiotemporal reference point is generated based on the target and the target heading angle, and a reference line is generated based on the spatiotemporal reference point.

4. The trajectory planning hot start method according to claim 1, characterized in that: The obtaining of the previous cycle planning result of the previous cycle control sequence corresponding to the reference line of the current vehicle includes: The lateral control variable and longitudinal control variable of the iterative linear quadratic regulation ILQR corresponding to the reference line of the current vehicle are calculated according to the following formula: in, is the horizontal distance from the current point to the reference point, is the angular deviation between the current point and the reference point, is the front wheel angle at the current point, is the angular velocity of the front wheel at the current point, 、 、 、 Corresponding to 、 、 、 The preset scaling factor of The control sequence of the previous cycle is determined according to the horizontal control variables and the vertical control variables, and the control sequence of the previous cycle is used as the late planning result of the previous week.

5. The trajectory planning hot start method according to claim 1, characterized in that: When the planning result of the previous cycle meets the preset conditions, generating a vehicle driving path according to the planning result of the previous cycle includes: If the planning result of the previous cycle is not empty, the ILQR optimization results of two or more consecutive cycles are normal, and the planning trajectory length of the planning result of the previous cycle is not close to 0, it is determined that the planning result of the previous cycle meets the preset conditions; The trajectory of the planning result of the previous cycle is used as the initial trajectory, and the vehicle driving path is generated according to the initial trajectory.

6. The trajectory planning hot start method according to claim 1, characterized in that: When the planning result of the previous cycle does not meet the preset conditions, multiple initial solutions are recalculated, and the trajectory corresponding to the initial solution with the minimum total cost is used as the final path, including: When the planning result of the previous cycle is empty, or the ILQR optimization results of two or more consecutive cycles are abnormal, or the planning trajectory length of the planning result of the previous cycle is close to 0, it is determined that the planning result of the previous cycle does not meet the preset conditions; Obtaining the state variables of the ILQR, recursively deducing the state variables according to a preset kinematic formula, and obtaining a state sequence; Traversing the state sequence, and calculating the running cost and terminal cost of each state according to preset constraint information; Accumulating and summing the operating cost and the terminal cost of each state to obtain a total cost of each control sequence; A target initial solution with the minimum total cost is selected from the state sequence according to the total cost, and a trajectory corresponding to the target initial solution with the minimum total cost is used as a final path for the current vehicle.

7. The trajectory planning hot start method according to claim 6, characterized in that: The step of obtaining the state variables of the ILQR and recursively deducing the state variables according to a preset kinematic formula to obtain a state sequence includes: Get ILQR status variables , and recursively deduce the state variables according to the following preset kinematic formula to obtain the state sequence: Where x is the x-coordinate in the Cartesian coordinate system, and y is the y-coordinate in the Cartesian coordinate system. is the heading angle of the vehicle, is the front wheel turning angle, is the angular velocity of the front wheel, s is the distance to the center of the rear axle, v is the vehicle speed, a is the vehicle acceleration, alfa is the angular acceleration of the front wheel, jerk is the vehicle jerk, is the transpose operation, is the wheelbase.

8. A trajectory planning hot start device, characterized in that: The trajectory planning hot start device comprises: The planning result acquisition module is used to obtain the previous cycle planning result of the previous cycle control sequence of the reference line corresponding to the current vehicle; A path determination module, configured to generate a vehicle driving path according to the planning result of the previous cycle when the planning result of the previous cycle meets a preset condition; The path recalculation module is used to recalculate multiple initial solutions when the planning result of the previous cycle does not meet the preset conditions, and use the trajectory corresponding to the initial solution with the minimum total cost as the final path.

9. A trajectory planning hot start device, characterized in that: The trajectory planning hot start device includes: a memory, a processor, and a trajectory planning hot start program stored in the memory and executable on the processor, wherein the trajectory planning hot start program is configured to implement the steps of the trajectory planning hot start method according to any one of claims 1 to 7.

10. A storage medium, characterized in that: The storage medium stores a trajectory planning hot start program, which, when executed by a processor, implements the steps of the trajectory planning hot start method according to any one of claims 1 to 7.

Images