Post-processing method, system and device for iLQR planning and readable storage medium

Abstract

A post-processing method, system and device for iLQR planning, and a readable storage medium relate to the technical field of trajectory post-processing, and specifically comprise the steps of controlling to enter a target mode based on whether the duration, the trajectory length, the transverse iLQR planning state of the current period and the iLQR planning state of the last period are abnormal or not; the target mode comprises a parking track construction mode, a track construction mode according to an iLQR result of a previous period and a track construction mode according to an iLQR result of a current period, the duration is the duration corresponding to a track which is not constructed based on the iLQR result of the current period, and the track length is the track length planned based on the iLQR result of the previous period; and determining a target trajectory sequence based on the target mode. The transverse stability of the vehicle is improved.

Description

Technical Field

[0001] The present application relates to the field of trajectory post-processing technology, and in particular to a post-processing method, system, device and readable storage medium for iLQR planning. Background Art

[0002] In the trajectory planning scheme based on the basic iLQR (Iterative Linear Quadratic Regulator), since the constraints need to be processed as cost terms of the objective function, the iLQR output results cannot be guaranteed to meet the original constraints. Improper superposition boundary settings may cause the iLQR solution to time out or even fail. Therefore, post-processing of the planned trajectory becomes more necessary.

[0003] Traditional trajectory post-processing primarily uses the previously planned trajectory as a fallback in the event of a planning failure. However, processing based solely on the previous trajectory can lead to poor lateral stability. Therefore, improving the vehicle's lateral stability is an urgent issue that needs to be addressed. Summary of the Invention

[0004] The present application provides a post-processing method, system, device and readable storage medium for iLQR planning, which can improve the lateral stability of a vehicle.

[0005] In a first aspect, an embodiment of the present application provides a post-processing method for iLQR planning, the post-processing method for iLQR planning comprising:

[0006] Entering a target mode is controlled based on the duration, trajectory length, and abnormality of the current cycle lateral iLQR planning state and the previous cycle iLQR planning state. The target modes include parking trajectory construction mode, trajectory construction mode based on the previous cycle iLQR result mode, and trajectory construction mode based on the current cycle iLQR result mode. The duration is the duration corresponding to the trajectory not being constructed based on the current cycle iLQR result, and the trajectory length is the length of the trajectory planned based on the previous cycle iLQR result.

[0007] A target trajectory sequence is determined based on the target pattern.

[0008] In conjunction with the first aspect, in one embodiment, the control of entering the target mode based on the duration, trajectory length, current cycle lateral iLQR planning state, and abnormality of the previous cycle iLQR planning state includes:

[0009] If it is detected that the lateral iLQR planning state of the current cycle is abnormal, the duration is less than the preset duration threshold, and the trajectory length is less than the preset length threshold, the control enters the parking trajectory construction mode;

[0010] If it is detected that the horizontal iLQR planning state of the current cycle is abnormal and the duration is less than the preset duration threshold, and the iLQR planning state of the previous cycle is normal, the control enters the trajectory construction mode based on the iLQR result of the previous cycle;

[0011] If it is detected that the horizontal iLQR planning status of the current cycle is normal or the duration is not less than the preset duration threshold, or the iLQR planning status of the previous cycle is abnormal and the trajectory length is not less than the preset length threshold, or the horizontal iLQR planning status of the current cycle is abnormal and the duration is less than the preset duration threshold, the control enters the trajectory construction mode based on the iLQR result of the current cycle.

[0012] In combination with the first aspect, in one embodiment, before the step of controlling entry into the target mode based on the duration, trajectory length, current cycle lateral iLQR planning state, and abnormality of the previous cycle iLQR planning state, the following steps are further included:

[0013] If it is detected that the sum of the comfort cost function and the lateral safety cost function is greater than the preset threshold, the lateral iLQR planning state of the current cycle is determined to be abnormal;

[0014] If it is detected that the sum of the comfort cost function and the lateral safety cost function is not greater than the preset threshold, the lateral iLQR planning state of the current cycle is determined to be normal.

[0015] In conjunction with the first aspect, in one embodiment, determining the target trajectory sequence based on the target pattern includes:

[0016] If the target mode is parking trajectory construction mode, a target trajectory sequence is determined based on preset planning starting point attributes and preset target values, where the preset target values ​​are used to represent the magnitude of speed, acceleration, and acceleration derivatives;

[0017] If the target mode is to build a mode based on the results of the previous cycle, the target trajectory sequence is determined based on the iLQR planning results of the previous cycle;

[0018] If the target mode is to build a mode based on the results of the current cycle, the target trajectory sequence is determined based on the iLQR planning results of the current cycle.

[0019] In conjunction with the first aspect, in one embodiment, determining the target trajectory sequence based on the preset planning starting point attribute and the preset target value includes:

[0020] Set the lateral attributes of all target trajectory points in the parking trajectory construction mode to the preset planning starting point attributes;

[0021] The velocity, acceleration, and acceleration derivative of all target trajectory points are set to preset target values ​​to determine the target trajectory sequence.

[0022] In conjunction with the first aspect, in one embodiment, after the step of determining the target trajectory sequence based on the target pattern, the method further includes:

[0023] If it is detected that the speed of the target track point in the target track sequence is less than a preset speed threshold or the curvature is greater than a preset curvature threshold, the speed of the target track point and the track points thereafter is set to the preset speed threshold;

[0024] If it is not detected that the speed of the target trajectory point in the target trajectory sequence is less than the preset speed threshold or the curvature is greater than the preset curvature threshold, the speed of the target trajectory point is controlled to remain unchanged.

[0025] In conjunction with the first aspect, in one embodiment, after the step of determining the target trajectory sequence based on the target pattern, the method further includes:

[0026] Control the vehicle to travel based on the target trajectory sequence;

[0027] During the driving process of the ego vehicle, obstacle collision detection is performed based on whether the boundary box of the obstacle and the ego vehicle overlaps;

[0028] Alternatively, physical boundary collision detection is performed based on the distance between the boundary points in the ego-vehicle bounding box and the preset physical bounding box and a preset distance threshold.

[0029] In a second aspect, an embodiment of the present application provides a post-processing system for iLQR planning, the post-processing system for iLQR planning comprising:

[0030] A first processing module, configured to control entry into a target mode based on a duration, a trajectory length, a current-cycle lateral iLQR planning state, and whether the previous-cycle iLQR planning state is abnormal. The target modes include a parking trajectory construction mode, a trajectory construction mode based on the previous-cycle iLQR results, and a trajectory construction mode based on the current-cycle iLQR results. The duration is the duration corresponding to a trajectory not being constructed based on the current-cycle iLQR results, and the trajectory length is the length of a trajectory planned based on the previous-cycle iLQR results.

[0031] The second processing module is configured to determine a target trajectory sequence based on the target pattern.

[0032] In a third aspect, an embodiment of the present application provides a post-processing device for iLQR planning, wherein the post-processing device for iLQR planning includes a processor, a memory, and a post-processing program for iLQR planning stored on the memory and executable by the processor, wherein when the post-processing program for iLQR planning is executed by the processor, the steps of the post-processing method for iLQR planning as described in any of the foregoing items are implemented.

[0033] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, on which a post-processing program for iLQR planning is stored. When the post-processing program for iLQR planning is executed by a processor, the steps of the post-processing method for iLQR planning as described in any of the foregoing items are implemented.

[0034] The beneficial effects of the technical solutions provided in the embodiments of the present application include:

[0035] The target mode is entered by controlling the duration corresponding to the trajectory not constructed based on the iLQR results of the current cycle, the length of the trajectory planned based on the iLQR results of the previous cycle, the abnormality of the lateral iLQR planning state of the current cycle and the iLQR planning state of the previous cycle; and the target trajectory sequence is determined based on the target mode. Compared with the traditional post-processing method that only relies on the trajectory of the previous cycle, this multi-mode switching mechanism can select the most appropriate trajectory planning post-processing method according to different scenarios, that is, it can comprehensively consider the data of the previous cycle and the data of the current cycle to perform trajectory planning post-processing, thereby effectively improving the lateral stability of the vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 This is a flow chart of an embodiment of the post-processing method for iLQR planning of this application;

[0037] Figure 2 For this application Figure 1 Detailed flow chart of step S10;

[0038] Figure 3 For this application Figure 1 Detailed flow chart of step S20;

[0039] Figure 4 Schematic diagram of the hardware structure of the post-processing equipment of the iLQR planning involved in the embodiment of the present application. DETAILED DESCRIPTION

[0040] In order to enable those skilled in the art to better understand the present invention, the following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0041] In order to make the objectives, technical solutions and advantages of this application clearer, the implementation methods of this application will be further described in detail below with reference to the accompanying drawings.

[0042] In a first aspect, an embodiment of the present application provides a post-processing method for iLQR planning.

[0043] In one embodiment, referring to Figure 1 , Figure 1 This is a flow chart of an embodiment of the post-processing method of iLQR planning in this application. Figure 1 As shown in Figure 2, the post-processing methods for iLQR planning include:

[0044] Step S10: Based on the duration, trajectory length, the current cycle lateral iLQR planning state, and whether the previous cycle iLQR planning state is abnormal, the target mode is controlled to enter. The target mode includes a parking trajectory construction mode, a trajectory construction mode based on the previous cycle iLQR result mode, and a trajectory construction mode based on the current cycle iLQR result mode. The duration is the duration corresponding to the trajectory not being constructed based on the current cycle iLQR result, and the trajectory length is the length of the trajectory planned based on the previous cycle iLQR result.

[0045] Exemplarily, in an embodiment of the present application, the current cycle lateral iLQR planning state represents the planning state of the lateral trajectory of the iLQR controller in the current time period, wherein the planning state includes abnormal state and normal state, and the duration refers to the duration during which the trajectory is not constructed based on the iLQR result of the current cycle; the previous cycle iLQR planning state represents the planning state of the iLQR controller in the previous cycle, and the trajectory length refers to the length of the trajectory planned based on the iLQR result of the previous cycle, which reflects the trajectory planning progress in the previous cycle.

[0046] It can be understood that the target modes include parking trajectory construction mode, trajectory construction based on previous cycle iLQR results mode, and trajectory construction based on current cycle iLQR results mode. The parking trajectory construction mode is used to meet parking needs, the trajectory construction based on previous cycle iLQR results mode is used to plan future paths based on historical control results, and the trajectory construction based on current cycle iLQR results mode is used to plan future paths based on real-time control results. The decision on which target mode to enter is made by analyzing and judging the duration, trajectory length, and the iLQR planning status of the previous and current cycles, ensuring that the appropriate control strategy is selected under different vehicle conditions.

[0047] Step S20: Determine a target trajectory sequence based on the target pattern.

[0048] Exemplarily, in an embodiment of the present application, the target modes include a parking trajectory construction mode, a trajectory construction mode based on the iLQR results of the previous cycle, and a trajectory construction mode based on the iLQR results of the current cycle. After determining which mode the vehicle is to be controlled to enter, corresponding trajectory planning post-processing measures can be adopted for different target modes to obtain a target trajectory sequence, so as to ensure that the system makes the most appropriate response in a dynamically changing environment, thereby ensuring the safety and smooth driving of the vehicle.

[0049] The present application controls entry into a target mode including a parking trajectory construction mode, a trajectory construction mode based on the previous cycle iLQR results, and a trajectory construction mode based on the current cycle iLQR results by the duration corresponding to the trajectory not being constructed based on the current cycle iLQR results, the trajectory length planned based on the previous cycle iLQR results, the current cycle lateral iLQR planning state, and whether the previous cycle iLQR planning state is abnormal; and determines a target trajectory sequence based on the target mode. Compared with the traditional post-processing method that only relies on the previous cycle trajectory, this multi-mode switching mechanism can select the most appropriate trajectory planning post-processing method according to different scenarios, that is, it can comprehensively consider the previous cycle data and the current cycle data to perform trajectory planning post-processing, thereby effectively improving the lateral stability of the vehicle.

[0050] Furthermore, in one embodiment, referring to Figure 2 As shown, the control of entering the target mode based on the duration, trajectory length, current cycle lateral iLQR planning state and the abnormality of the previous cycle iLQR planning state includes:

[0051] Step S101: If it is detected that the current period lateral iLQR planning state is abnormal, the duration is less than the preset duration threshold, and the trajectory length is less than the preset length threshold, the control enters the parking trajectory construction mode;

[0052] Step S102: If it is detected that the horizontal iLQR planning state of the current cycle is abnormal and the duration is less than the preset duration threshold and the iLQR planning state of the previous cycle is normal, the control enters the trajectory construction mode based on the iLQR result of the previous cycle;

[0053] Step S103: If it is detected that the horizontal iLQR planning state of the current cycle is normal or the duration is not less than the preset duration threshold, or the iLQR planning state of the previous cycle is abnormal, the trajectory length is not less than the preset length threshold, or the horizontal iLQR planning state of the current cycle is abnormal and the duration is less than the preset duration threshold, then the control enters the trajectory construction mode according to the iLQR result of the current cycle.

[0054] For example, in the embodiment of the present application, the specific values ​​of the preset duration threshold and the preset length threshold can be determined according to actual needs and are not limited here; the abnormality of the lateral iLQR planning state of the current cycle indicates that the trajectory planning of the vehicle in the current cycle has deviated or become unstable, which may cause the vehicle to be unable to travel along the expected path; the duration of the abnormality being less than the preset duration threshold indicates that the abnormality is short-lived, that is, it is not a persistent trajectory problem; the trajectory length being less than the preset length threshold indicates that the current driving trajectory is relatively short and the vehicle has not completed the scheduled driving task. At this time, a more conservative operation can be taken, that is, controlling the vehicle to enter the parking trajectory construction mode. In summary, when these three conditions are met at the same time, it means that the current abnormal condition is not sufficient to restore the normal driving trajectory, but in order to avoid further risks or accidents, the control system will actively enter the parking trajectory construction mode to ensure that the vehicle can park safely.

[0055] It should be noted that if the iLQR planning status of the previous cycle is normal, it means that there is no problem with the trajectory planning of the previous cycle, that is, the vehicle's driving performance in the previous cycle is normal; if it is also detected that the lateral iLQR planning status of the current cycle is abnormal and the duration is less than the preset duration threshold, it means that the trajectory planning of the previous cycle is normal and the lateral iLQR planning of the current cycle is abnormal and the abnormal duration is relatively short. The system can construct a new driving trajectory based on the trajectory planning results of the previous cycle to ensure that the vehicle can maintain relatively stable driving under abnormal conditions and avoid drastic trajectory adjustments, thereby ensuring that the vehicle can continue to drive to the target area or stop safely.

[0056] It can be understood that if the current period lateral iLQR planning status is normal, it means that there is no abnormality in the trajectory planning of the current period, the vehicle can drive normally, and can be controlled to enter the trajectory construction mode based on the iLQR results of the current period; if the duration is not less than the preset duration threshold, it means that the current state has lasted for a certain period of time and is stable, and can be controlled to enter the trajectory construction mode based on the iLQR results of the current period.

[0057] It should be noted that if the iLQR planning status of the previous cycle is abnormal, it indicates that there was a problem with the trajectory planning of the previous cycle. If the trajectory length is greater than or equal to the preset length threshold, it means that the current trajectory is already longer, and the system can determine that the vehicle has completed a certain driving task. Based on this, if it is detected that the iLQR planning status of the previous cycle is abnormal, the trajectory length is not less than the preset length threshold, and the lateral iLQR planning status of the current cycle is abnormal and the duration is less than the preset duration threshold, the system can enter the trajectory construction mode based on the iLQR results of the current cycle through control to perform local trajectory corrections, ensuring that the system can quickly restore stability in abnormal situations and buy time for further comprehensive corrections. The above approach makes the system more flexible and can maintain a certain level of safety and stability in uncertain environments, ensuring that the vehicle can adapt to real-time changes in road conditions and maintain stability, thereby ensuring that the vehicle can drive efficiently and safely in the current environment.

[0058] Furthermore, in one embodiment, before the step of controlling entry into the target mode based on the duration, trajectory length, current cycle lateral iLQR planning state, and previous cycle iLQR planning state, the method further includes:

[0059] If it is detected that the sum of the comfort cost function and the lateral safety cost function is greater than the preset threshold, the lateral iLQR planning state of the current cycle is determined to be abnormal;

[0060] If it is detected that the sum of the comfort cost function and the lateral safety cost function is not greater than the preset threshold, the lateral iLQR planning state of the current cycle is determined to be normal.

[0061] For example, in the embodiment of the present application, the specific value of the preset threshold value can be determined according to actual needs and is not limited here; the two key indicators of comfort and lateral safety during vehicle driving can be obtained through the comfort cost function and the lateral safety cost function, and the system can obtain the values ​​of the comfort cost function and the lateral safety cost function of the current cycle respectively through real-time calculation; the values ​​of the two cost functions are added together to obtain a sum, and then the sum is compared with the preset threshold value; if the sum is greater than the preset threshold value, it means that the driving state of the vehicle in the current cycle is abnormal, that is, there may be excessive lateral deviation or inappropriate comfort performance, then the lateral iLQR planning state of the current cycle can be determined to be abnormal, and then the corresponding post-processing mechanism can be triggered; if the sum is not greater than the preset threshold value, indicating that the comfort and safety of the vehicle are as expected, then the lateral iLQR planning state of the current cycle can be determined to be normal, indicating that there is no problem with the lateral trajectory planning of the vehicle in this cycle, and the predetermined trajectory control task can continue to be executed. Through the above-mentioned judgment mechanism, the system can monitor the dynamic state of the vehicle in real time, ensure that it strikes a balance between comfort and safety, and respond promptly when an abnormality occurs; it should be noted that the expressions of the comfort cost function and the lateral safety cost function are common knowledge in this field, and for the sake of brevity, they will not be repeated here.

[0062] Furthermore, in one embodiment, referring to Figure 3 As shown, the target trajectory sequence is determined based on the target pattern, including:

[0063] Step S201: If the target mode is the parking trajectory construction mode, a target trajectory sequence is determined based on preset planning starting point attributes and preset target values, where the preset target values ​​are used to represent the magnitude of speed, acceleration, and acceleration derivatives;

[0064] Step S202: If the target mode is a mode constructed according to the results of the previous cycle, the target trajectory sequence is determined according to the iLQR planning results of the previous cycle;

[0065] Step S203: If the target mode is to construct a mode based on the current cycle results, the target trajectory sequence is determined based on the iLQR planning results of the current cycle.

[0066] For example, in the embodiment of the present application, the preset target value generally includes the speed, acceleration and acceleration derivative (i.e., jerk) restrictions required by the vehicle during the parking process. These values ​​are used to ensure the smoothness and safety of the parking process. The preset planning starting point attributes and the preset target value can be determined according to actual needs and are not limited here. For example, the preset target value can preferably be 0. When the target mode is the parking trajectory construction mode, the system can generate a target trajectory sequence according to the preset planning starting point attributes and the preset target value. When the target mode is the construction mode based on the results of the previous cycle, the system can refer to the iLQR planning results of the previous cycle to determine the target trajectory sequence, that is, the iLQR planning results of the previous cycle can be converted to the target trajectory sequence by coordinate transformation. The QR planning results are converted to the current vehicle coordinate system to obtain the first lateral trajectory sequence. The trajectory points to the right of the origin of the current vehicle coordinate system are then filtered out from the first lateral trajectory sequence, and the filtered trajectory points are used as the target trajectory sequence. When the target mode is to build a mode based on the current cycle results, the system directly uses the iLQR planning results of the current cycle to generate the target trajectory sequence. The trajectory information can be extracted from the control variable sequence and state variable sequence of the iLQR planning results of the current cycle, and the extracted trajectory information is used as the target trajectory sequence. The principle of extracting trajectory information from the control variable sequence and state variable sequence is common knowledge in the field and will not be repeated here for the sake of brevity. Therefore, through different target modes, the system can flexibly adjust the trajectory generation strategy according to actual needs and status, thereby achieving more optimized and dynamic path planning.

[0067] Furthermore, in one embodiment, determining the target trajectory sequence based on the preset planning starting point attribute and the preset target value includes:

[0068] Set the lateral attributes of all target trajectory points in the parking trajectory construction mode to the preset planning starting point attributes;

[0069] The velocity, acceleration, and acceleration derivative of all target trajectory points are set to preset target values ​​to determine the target trajectory sequence.

[0070] For example, in an embodiment of the present application, in parking trajectory construction mode, the lateral attributes of all target trajectory points can be set to the starting point attributes based on the preset planned starting point attributes to ensure that the lateral position of the target trajectory is consistent with the starting point, thereby ensuring that the vehicle smoothly parks along the planned path. At the same time, the velocity, acceleration, and acceleration derivative of all target trajectory points can be set to 0 to ensure that each trajectory point in the parking process has a stable state of zero velocity and zero acceleration, thereby achieving a target trajectory sequence in which the vehicle ultimately parks smoothly at the target location. The above process, based on the smoothness and stationary requirements of the parking trajectory, ensures that the vehicle does not generate additional dynamic responses when completing the parking task, thereby improving parking accuracy and safety.

[0071] Furthermore, in one embodiment, after the step of determining the target trajectory sequence based on the target pattern, the method further includes:

[0072] If it is detected that the speed of the target track point in the target track sequence is less than a preset speed threshold or the curvature is greater than a preset curvature threshold, the speed of the target track point and the track points thereafter is set to the preset speed threshold;

[0073] If it is not detected that the speed of the target trajectory point in the target trajectory sequence is less than the preset speed threshold or the curvature is greater than the preset curvature threshold, the speed of the target trajectory point is controlled to remain unchanged.

[0074] For example, in the embodiment of the present application, the specific values ​​of the preset speed threshold and the preset curvature threshold can be determined according to actual needs and are not limited here; for example, the preset speed threshold can preferably be 0 km / h; the main purpose of the vehicle speed validity processing and curvature validity detection is to ensure the safety of the vehicle in trajectory planning. Specifically, the planned speed curve of the vehicle is not a curve that gradually decreases to 0, but a curve that first decreases to 0 and then accelerates to the target speed. However, if the planned acceleration requires the vehicle to reverse (negative acceleration), the vehicle will encounter problems, because negative acceleration means that the vehicle needs to travel in the opposite direction, which may cause the vehicle to unexpectedly retreat, thereby increasing the risk of collision with other obstacles.

[0075] It should be noted that in order to avoid the above situation, the vehicle should limit the acceleration to be greater than or equal to 0 when planning the speed, that is, the vehicle is only allowed to move forward but not backward. Therefore, if the speed of a target trajectory point is detected to be less than 0, all trajectory points with a speed less than 0 and thereafter need to be set as stationary points (that is, the speed is set to 0); at the same time, when the curvature is detected to exceed the motion limit (that is, the curvature is detected to be greater than the preset curvature threshold), it means that the current plan cannot be actually executed. If the vehicle continues to drive along the trajectory that exceeds the motion limit, it may lose control or encounter unpredictable risks; therefore, the subsequent trajectory points can be set to stationary points (that is, the speed of the target trajectory point is set to 0) to avoid these potential dangers and ensure that the vehicle operates within the control range.

[0076] It is understandable that if no target trajectory point in the target trajectory sequence is detected to have a speed less than a preset speed threshold or a curvature greater than a preset curvature threshold, it means that the vehicle speed and curvature of all trajectory points are within the normal range and no adjustment is required, and the speed of the target trajectory point can be controlled to remain unchanged.

[0077] Furthermore, in one embodiment, after the step of determining the target trajectory sequence based on the target pattern, the method further includes:

[0078] Control the vehicle to travel based on the target trajectory sequence;

[0079] During the driving process of the ego vehicle, obstacle collision detection is performed based on whether the boundary box of the obstacle and the ego vehicle overlaps;

[0080] Alternatively, physical boundary collision detection is performed based on the distance between the boundary points in the ego-vehicle bounding box and the preset physical bounding box and a preset distance threshold.

[0081] For example, in the embodiment of the present application, the preset distance threshold and the preset physical bounding box can be determined according to actual needs and are not limited here. When the ego vehicle is driving based on the target trajectory sequence, it is first necessary to plan and control according to the target trajectory to ensure that the vehicle drives normally along the planned path. At the same time, to avoid collisions, the system can monitor the relative position between the ego vehicle and surrounding obstacles in real time and perform collision detection based on the overlap between the obstacle and the ego vehicle bounding box. Specifically, the system can generate a corresponding ego vehicle bounding box based on each planned trajectory point, and obtain the obstacle bounding box at the corresponding time point by predicting the obstacle trajectory, and then use standard Box and Box collision detection methods (AABB (Axis-Aligned Bounding Box, axis-aligned bounding box), OBB (Oriented Bounding Box, oriented bounding box) collision detection, etc.) to determine whether the ego vehicle will have a collision risk with the obstacle. Among them, the principle of standard Box and Box collision detection is common knowledge in the field and will not be repeated here for the sake of brevity.

[0082] In addition, to further ensure safety, the system can also perform collision detection on physical boundaries, where the physical bounding box refers to the geometric shape or area boundary in an object or scene used to define collision detection and reaction calculations; specifically, a self-vehicle bounding box can be generated for each planned trajectory point, and a physical boundary Frenet (lane) coordinate system can be constructed; by calculating the coordinates (S, L) of each boundary point in the self-vehicle bounding box in the Frenet coordinate system, where S represents the arc length coordinate and L represents the lateral offset coordinate, it is determined whether the distance between these boundary points and the physical bounding box is less than a preset distance threshold; if the distance is less than the preset distance threshold, it indicates that there is a physical boundary collision risk, and corresponding measures must be taken to ensure safe driving and avoid collisions; if the distance is not less than the preset distance threshold, it indicates that there is no physical boundary collision risk.

[0083] It should be noted that regardless of the risk of physical boundary collision or obstacle collision, the longitudinal parking trajectory can be controlled to avoid vehicle collision; the purpose of determining the longitudinal parking trajectory is to stop the vehicle 1m before the collision point. Specifically, the coordinate data of the current position can be obtained, and the current position can be used as the starting point to deduce a trajectory sequence of a preset number of target points based on the kinematic formula until the last point in the trajectory sequence meets the requirement of stopping 1m before the collision point. Finally, a longitudinal parking trajectory sequence (i.e., a Fallback trajectory sequence) can be obtained; the specific value of the preset number can be determined according to actual needs and is not limited here.

[0084] It is understandable that after determining the longitudinal parking trajectory, since the number of elements in the target trajectory sequence and the longitudinal parking trajectory sequence generated in the aforementioned trajectory construction mode is equal, the target trajectory sequence and the fallback trajectory sequence can be traversed simultaneously. For each fallback trajectory sequence point, the two preceding and succeeding target trajectory points are found on the target trajectory sequence. Then, new trajectory points that match the fallback trajectory sequence points are calculated through linear interpolation, and the trajectory sequence composed of these new trajectory points is used as the final trajectory sequence.

[0085] In a second aspect, an embodiment of the present application further provides a post-processing system for iLQR planning, the post-processing system for iLQR planning comprising:

[0086] A first processing module, configured to control entry into a target mode based on a duration, a trajectory length, a current-cycle lateral iLQR planning state, and whether the previous-cycle iLQR planning state is abnormal. The target modes include a parking trajectory construction mode, a trajectory construction mode based on the previous-cycle iLQR results, and a trajectory construction mode based on the current-cycle iLQR results. The duration is the duration corresponding to a trajectory not being constructed based on the current-cycle iLQR results, and the trajectory length is the length of a trajectory planned based on the previous-cycle iLQR results.

[0087] The second processing module is configured to determine a target trajectory sequence based on the target pattern.

[0088] Furthermore, in one embodiment, the first processing module is specifically configured to:

[0089] If it is detected that the lateral iLQR planning state of the current cycle is abnormal, the duration is less than the preset duration threshold, and the trajectory length is less than the preset length threshold, the control enters the parking trajectory construction mode;

[0090] If it is detected that the horizontal iLQR planning state of the current cycle is abnormal and the duration is less than the preset duration threshold, and the iLQR planning state of the previous cycle is normal, the control enters the trajectory construction mode based on the iLQR result of the previous cycle;

[0091] If it is detected that the horizontal iLQR planning status of the current cycle is normal or the duration is not less than the preset duration threshold, or the iLQR planning status of the previous cycle is abnormal and the trajectory length is not less than the preset length threshold, or the horizontal iLQR planning status of the current cycle is abnormal and the duration is less than the preset duration threshold, the control enters the trajectory construction mode based on the iLQR result of the current cycle.

[0092] Furthermore, in one embodiment, the first processing module is further configured to:

[0093] If it is detected that the sum of the comfort cost function and the lateral safety cost function is greater than the preset threshold, the lateral iLQR planning state of the current cycle is determined to be abnormal;

[0094] If it is detected that the sum of the comfort cost function and the lateral safety cost function is not greater than the preset threshold, the lateral iLQR planning state of the current cycle is determined to be normal.

[0095] Furthermore, in one embodiment, the second processing module is specifically configured to:

[0096] If the target mode is parking trajectory construction mode, a target trajectory sequence is determined based on preset planning starting point attributes and preset target values, where the preset target values ​​are used to represent the magnitude of speed, acceleration, and acceleration derivatives;

[0097] If the target mode is to build a mode based on the results of the previous cycle, the target trajectory sequence is determined based on the iLQR planning results of the previous cycle;

[0098] If the target mode is to build a mode based on the results of the current cycle, the target trajectory sequence is determined based on the iLQR planning results of the current cycle.

[0099] Furthermore, in one embodiment, the second processing module is further configured to:

[0100] Set the lateral attributes of all target trajectory points in the parking trajectory construction mode to the preset planning starting point attributes;

[0101] The velocity, acceleration, and acceleration derivative of all target trajectory points are set to preset target values ​​to determine the target trajectory sequence.

[0102] Furthermore, in one embodiment, the second processing module is further configured to:

[0103] If it is detected that the speed of the target track point in the target track sequence is less than a preset speed threshold or the curvature is greater than a preset curvature threshold, the speed of the target track point and the track points thereafter is set to the preset speed threshold;

[0104] If it is not detected that the speed of the target trajectory point in the target trajectory sequence is less than the preset speed threshold or the curvature is greater than the preset curvature threshold, the speed of the target trajectory point is controlled to remain unchanged.

[0105] Furthermore, in one embodiment, the second processing module is further configured to:

[0106] Control the vehicle to travel based on the target trajectory sequence;

[0107] During the driving process of the ego vehicle, obstacle collision detection is performed based on whether the boundary box of the obstacle and the ego vehicle overlaps;

[0108] Alternatively, physical boundary collision detection is performed based on the distance between the boundary points in the ego-vehicle bounding box and the preset physical bounding box and a preset distance threshold.

[0109] The present application controls entry into a target mode including a parking trajectory construction mode, a trajectory construction mode based on the previous cycle iLQR results, and a trajectory construction mode based on the current cycle iLQR results by the duration corresponding to the trajectory not being constructed based on the current cycle iLQR results, the trajectory length planned based on the previous cycle iLQR results, the current cycle lateral iLQR planning state, and whether the previous cycle iLQR planning state is abnormal; and determines a target trajectory sequence based on the target mode. Compared with the traditional post-processing method that only relies on the previous cycle trajectory, this multi-mode switching mechanism can select the most appropriate trajectory planning post-processing method according to different scenarios, that is, it can comprehensively consider the previous cycle data and the current cycle data to perform trajectory planning post-processing, thereby effectively improving the lateral stability of the vehicle.

[0110] Among them, the functional implementation of each module in the above-mentioned iLQR planning post-processing system corresponds to the various steps in the above-mentioned iLQR planning post-processing method embodiment, and their functions and implementation processes will not be repeated here one by one.

[0111] In a third aspect, an embodiment of the present application provides a post-processing device for iLQR planning. The post-processing device for iLQR planning may be a personal computer (PC), a laptop computer, a server, or other device with data processing capabilities.

[0112] Reference Figure 4 , Figure 4 Schematic diagram of the hardware structure of the post-processing device of iLQR planning involved in the embodiment of the present application. In the embodiment of the present application, the post-processing device of iLQR planning may include a processor, a memory, a communication interface and a communication bus.

[0113] The communication bus may be of any type and is used to interconnect the processor, memory, and communication interface.

[0114] Communication interfaces include input / output (I / O) interfaces, physical interfaces, and logical interfaces, which interconnect components within the iLQR planning post-processing equipment and connect the iLQR planning post-processing equipment to other devices (such as other computing devices or user devices). Physical interfaces can be Ethernet, fiber, or ATM interfaces; user devices can be displays or keyboards.

[0115] The memory can be various types of storage media, such as random access memory (RAM), read-only memory (ROM), non-volatile RAM (NVRAM), flash memory, optical storage, hard disk, programmable ROM (PROM), erasable PROM (EPROM), electrically erasable PROM (EEPROM), etc.

[0116] The processor may be a general-purpose processor, which may call the post-processing program of the iLQR plan stored in the memory and execute the post-processing method of the iLQR plan provided in the embodiment of the present application. For example, the general-purpose processor may be a central processing unit (CPU). The method executed when the post-processing program of the iLQR plan is called may refer to the various embodiments of the post-processing method of the iLQR plan of the present application, and will not be repeated here.

[0117] Those skilled in the art will understand that Figure 4 The hardware structure shown in the figure does not constitute a limitation to the present application and may include more or fewer components than shown in the figure, or a combination of certain components, or a different arrangement of components.

[0118] In a fourth aspect, an embodiment of the present application also provides a readable storage medium.

[0119] The readable storage medium of the present application stores a post-processing program for iLQR planning, wherein when the post-processing program for iLQR planning is executed by a processor, the steps of the post-processing method for iLQR planning as described above are implemented.

[0120] Among them, the method implemented when the post-processing program of iLQR planning is executed can refer to the various embodiments of the post-processing method of iLQR planning in this application, and will not be repeated here.

[0121] The terms "including" and "having" and any variations thereof in the specification and claims of this application and the above-mentioned drawings are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but optionally includes steps or units that are not listed, or optionally includes other steps or units inherent to these processes, methods, products or devices. The terms "first", "second" and "third" are used to distinguish different objects, etc., and do not represent a sequence, nor do they limit the "first", "second" and "third" to different types.

[0122] In the description of the embodiments of this application, the words "exemplary," "for example," or "for example" are used to indicate examples, illustrations, or descriptions. Any embodiment or design described as "exemplary," "for example," or "for example" in the embodiments of this application should not be construed as being preferred or advantageous over other embodiments or designs. Rather, the use of words such as "exemplary," "for example," or "for example" is intended to present the relevant concepts in a concrete manner.

[0123] In the description of the embodiments of the present application, unless otherwise specified, “ / ” means or, for example, A / B can mean A or B; “and / or” in the text is merely a description of the association relationship of associated objects, indicating that three relationships may exist, for example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. In addition, in the description of the embodiments of the present application, “multiple” refers to two or more than two.

[0124] In some processes described in the embodiments of the present application, multiple operations or steps are included that appear in a specific order. However, it should be understood that these operations or steps may not be performed in the order in which they appear in the embodiments of the present application or may be performed in parallel. The sequence numbers of the operations are only used to distinguish between different operations, and the sequence numbers themselves do not represent any order of execution. In addition, these processes may include more or fewer operations, and these operations or steps may be performed in sequence or in parallel, and these operations or steps may be combined.

[0125] It should be noted that the serial numbers of the above-mentioned embodiments of the present application are for description only and do not represent the advantages or disadvantages of the embodiments.

[0126] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus the necessary general hardware platform, of course, it can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes a number of instructions for enabling a terminal device to execute the methods described in each embodiment of the present application.

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

Claims

1. A post-processing method for iLQR planning, characterized in that: The post-processing method of the iLQR planning includes: Entering a target mode is controlled based on the duration, trajectory length, and abnormality of the current cycle lateral iLQR planning state and the previous cycle iLQR planning state. The target modes include parking trajectory construction mode, trajectory construction mode based on the previous cycle iLQR result mode, and trajectory construction mode based on the current cycle iLQR result mode. The duration is the duration corresponding to the trajectory not being constructed based on the current cycle iLQR result, and the trajectory length is the length of the trajectory planned based on the previous cycle iLQR result. A target trajectory sequence is determined based on the target pattern.

2. The post-processing method for iLQR planning according to claim 1, wherein: The control of entering the target mode based on the duration, trajectory length, current cycle lateral iLQR planning state, and abnormality of the previous cycle iLQR planning state includes: If it is detected that the lateral iLQR planning state of the current cycle is abnormal, the duration is less than the preset duration threshold, and the trajectory length is less than the preset length threshold, the control enters the parking trajectory construction mode; If it is detected that the horizontal iLQR planning state of the current cycle is abnormal and the duration is less than the preset duration threshold, and the iLQR planning state of the previous cycle is normal, the control enters the trajectory construction mode based on the iLQR result of the previous cycle; If it is detected that the horizontal iLQR planning status of the current cycle is normal or the duration is not less than the preset duration threshold, or the iLQR planning status of the previous cycle is abnormal and the trajectory length is not less than the preset length threshold, or the horizontal iLQR planning status of the current cycle is abnormal and the duration is less than the preset duration threshold, the control enters the trajectory construction mode based on the iLQR result of the current cycle.

3. The post-processing method for iLQR planning according to claim 1, wherein: Before the step of controlling entry into the target mode based on the duration, trajectory length, current cycle lateral iLQR planning state, and abnormality of the previous cycle iLQR planning state, the method further includes: If it is detected that the sum of the comfort cost function and the lateral safety cost function is greater than the preset threshold, the lateral iLQR planning state of the current cycle is determined to be abnormal; If it is detected that the sum of the comfort cost function and the lateral safety cost function is not greater than the preset threshold, the lateral iLQR planning state of the current cycle is determined to be normal.

4. The post-processing method for iLQR planning according to claim 1, wherein: Determining a target trajectory sequence based on the target pattern includes: If the target mode is parking trajectory construction mode, a target trajectory sequence is determined based on preset planning starting point attributes and preset target values, where the preset target values ​​are used to represent the magnitude of speed, acceleration, and acceleration derivatives; If the target mode is to build a mode based on the results of the previous cycle, the target trajectory sequence is determined based on the iLQR planning results of the previous cycle; If the target mode is to build a mode based on the results of the current cycle, the target trajectory sequence is determined based on the iLQR planning results of the current cycle.

5. The post-processing method of iLQR planning according to claim 4, characterized in that: The step of determining a target trajectory sequence based on a preset planning starting point attribute and a preset target value includes: Set the lateral attributes of all target trajectory points in the parking trajectory construction mode to the preset planning starting point attributes; The velocity, acceleration, and acceleration derivative of all target trajectory points are set to preset target values ​​to determine the target trajectory sequence.

6. The post-processing method for iLQR planning according to claim 1, wherein: After the step of determining the target trajectory sequence based on the target pattern, the method further includes: If it is detected that the speed of the target track point in the target track sequence is less than a preset speed threshold or the curvature is greater than a preset curvature threshold, the speed of the target track point and the track points thereafter is set to the preset speed threshold; If it is not detected that the speed of the target trajectory point in the target trajectory sequence is less than the preset speed threshold or the curvature is greater than the preset curvature threshold, the speed of the target trajectory point is controlled to remain unchanged.

7. The post-processing method for iLQR planning according to claim 1, wherein: After the step of determining the target trajectory sequence based on the target pattern, the method further includes: Control the vehicle to travel based on the target trajectory sequence; During the driving process of the ego vehicle, obstacle collision detection is performed based on whether the boundary box of the obstacle and the ego vehicle overlaps; Alternatively, physical boundary collision detection is performed based on the distance between the boundary points in the ego-vehicle bounding box and the preset physical bounding box and a preset distance threshold.

8. A post-processing system for iLQR planning, characterized in that: The post-processing system of the iLQR plan includes: A first processing module, configured to control entry into a target mode based on a duration, a trajectory length, a current-cycle lateral iLQR planning state, and whether the previous-cycle iLQR planning state is abnormal. The target modes include a parking trajectory construction mode, a trajectory construction mode based on the previous-cycle iLQR results, and a trajectory construction mode based on the current-cycle iLQR results. The duration is the duration corresponding to a trajectory not being constructed based on the current-cycle iLQR results, and the trajectory length is the length of a trajectory planned based on the previous-cycle iLQR results. The second processing module is configured to determine a target trajectory sequence based on the target pattern.

9. A post-processing device for iLQR planning, characterized in that: The post-processing device of the iLQR planning includes a processor, a memory, and a post-processing program of the iLQR planning stored on the memory and executable by the processor, wherein when the post-processing program of the iLQR planning is executed by the processor, the steps of the post-processing method of the iLQR planning as described in any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a post-processing program for iLQR planning, wherein when the post-processing program for iLQR planning is executed by a processor, the steps of the post-processing method for iLQR planning according to any one of claims 1 to 7 are implemented.

Images