Multi-algorithm combined trajectory planning method under unstructured road

Abstract

The invention discloses a multi-algorithm combined trajectory planning method under an unstructured road, which belongs to the technical field of automatic driving, and comprises the following steps: generating an initial search trajectory by using a Hybrid A * algorithm, performing segmentation parallel optimization on the initial search trajectory by using a QP algorithm and an OBCA algorithm, performing post-trajectory check on the obtained optimized trajectory, if the check is passed, outputting the corresponding optimized trajectory, and if the check is passed, outputting the corresponding optimized trajectory; and if the QP and OBCA are both successfully optimized but the post-check of the trajectory is not passed, determining the final trajectory according to the priority of whether the collision is superior to whether the start-end point constraint is met or not and superior to whether the curvature constraint is met or not. According to the method, through combined use of multiple algorithms, the success rate of trajectory planning is improved, optimization results are comprehensively and dynamically screened through trajectory comparison, a better trajectory is finally output, comprehensive improvement of trajectory planning performance under an unstructured road is achieved, and the method is suitable for large-scale popularization and application. Therefore, the technical problem that the real-time performance and the track quality of track planning cannot be considered at the same time is solved.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving technology, and specifically relates to a trajectory planning method combining multiple algorithms on unstructured roads, which is constructed by integrating Hybrid A * , QP and OBCA algorithms to achieve efficient and robust trajectory planning, which is suitable for autonomous vehicle driving (such as parking in and out of the garage, U-turn, etc.) in closed or complex scenarios such as parking lots and roads without clear markings. Background Art

[0002] In the field of autonomous driving, planning refers to generating a trajectory for the vehicle based on its current location and surrounding conditions. Due to the diversity of road scenarios, the technical solutions used also vary. Road scenarios are mainly divided into two types: structured roads and unstructured roads.

[0003] Structured road scenarios refer to road environments with regular, clear lane markings and road signs. These typically occur in open spaces, such as urban roads and highways, and have well-defined traffic rules and driving standards. Vehicles on structured roads typically travel at high speeds, and only forward motion is considered, with no consideration given to reverse movements. This makes planning in structured road scenarios highly dependent on reference lines. However, the advantage is that planning is quick, typically in the 100ms range, enabling real-time planning. Quadratic Programming (QP) is the mainstream algorithm for structured road scenarios.

[0004] Unstructured road scenarios differ from structured ones in that they lack clear lane markings, road signs, or traffic regulations. These scenarios typically occur in enclosed spaces, such as parking lots, and are subject to complex scenarios and environments, resulting in significant uncertainty. Furthermore, vehicles in unstructured road scenarios typically travel at slow speeds, requiring simultaneous forward and reverse maneuvers. Currently, the mainstream algorithmic framework for unstructured road scenarios is search-and-optimize, typically using the Hybrid A* + OBCA algorithm. This algorithm first uses a search algorithm to obtain an initial trajectory, then applies an optimization algorithm to smooth the initial solution and obtain the final trajectory. However, as a non-convex, nonlinear optimization algorithm, OBCA takes a long time to plan in complex scenarios, typically requiring 1-3 seconds. This results in poor real-time trajectory planning and makes it difficult to quickly respond to environmental changes. While using the QP algorithm for optimization significantly reduces planning time, the QP algorithm cannot effectively constrain curvature, resulting in poor trajectory quality. In other words, for unstructured road scenarios, in order to pursue trajectory quality, the real-time performance of trajectory planning will be poor, and the pursuit of real-time performance will lead to poor trajectory quality.

[0005] Therefore, for unstructured road scenarios, it is very necessary to design a trajectory planning method that can take into account both the real-time performance and trajectory quality of trajectory planning, that is, to achieve a balance between the real-time performance and trajectory quality of trajectory planning. Summary of the Invention

[0006] In view of this, the purpose of the present invention is to provide a trajectory planning method combining multiple algorithms under unstructured roads, which combines Hybrid A * , QP and OBCA algorithms to solve the problems of poor real-time performance and poor trajectory quality in trajectory planning in unstructured road scenarios, and improve the robustness, success rate and system stability of trajectory planning.

[0007] In order to achieve the above-mentioned purpose of the invention, the technical solutions adopted are as follows:

[0008] The present invention provides a trajectory planning method combining multiple algorithms on unstructured roads, comprising the following steps:

[0009] S1, based on the planned start and end points, adopts Hybrid A * The algorithm performs trajectory planning. If the planning is successful, the initial search trajectory is generated and the process proceeds to step S2. If the planning fails, the process is terminated.

[0010] S2, based on the initial search trajectory generated in step S1, the QP algorithm is used to perform segmented parallel smooth optimization on it. If the optimization is successful, a QP optimized trajectory is generated and the process proceeds to step S3; if the optimization fails, the process proceeds to step S4;

[0011] S3, perform a post-trajectory check on the QP optimized trajectory obtained in step S2. If the check passes, the QP optimized trajectory is directly output and the process is terminated; if the check fails, proceed to step S4;

[0012] S4, based on the initial search trajectory generated in step S1, the OBCA algorithm is used to perform segmented parallel smooth optimization on it. If the optimization is successful, the OBCA optimized trajectory is generated and the process proceeds to step S5; if the optimization fails, the process proceeds to step S6;

[0013] S5, the OBCA optimized trajectory obtained in step S4 is subjected to the same trajectory post-check as in step S3. If the check passes, the OBCA optimized trajectory is directly output and the process is terminated; if the check fails, step S6 is entered;

[0014] S6, based on the optimization results of the QP algorithm and the OBCA algorithm and the results of the post-check of the trajectories obtained in steps S3 and S5, the trajectories are compared, and the final trajectory is dynamically determined and output.

[0015] Specifically, firstly, according to the planned starting point and end point, Hybrid A * The algorithm generates the initial search trajectory. If Hybrid A * If the generation fails, the process is terminated; if the generation succeeds, the optimization phase begins.

[0016] The initial search trajectory is first segmented by shift points. The shift points are determined by judging whether the difference between the vector angles of adjacent trajectory points A and B and the heading angle of point B is greater than 90°. Subsequently, an asynchronous thread task is started to parallel optimize each segment of the trajectory. If the QP algorithm is used, its low time consumption (100ms level) is utilized to quickly generate the optimized trajectory corresponding to the QP algorithm. If the planning (optimization) is successful and the optimized trajectory passes the post-trajectory check including collision check, start and end point check, and curvature check, the optimized trajectory corresponding to the QP algorithm is directly output; if the QP planning fails or its post-trajectory check does not pass, the OBCA algorithm is switched to.

[0017] When using the OBCA algorithm, as a non-convex, nonlinear optimization algorithm, it can strictly constrain trajectory curvature, collision risk, and driving direction, making it suitable for complex scenarios. If OBCA planning (optimization) is successful, and the trajectory satisfies the nonlinear constraints and passes the post-trajectory check, the optimized trajectory corresponding to the OBCA algorithm is directly output. If OBCA planning fails or the post-trajectory check fails, the process proceeds to the trajectory comparison phase in step S6.

[0018] This invention improves the planning efficiency of simple scenarios through the low time consumption characteristics of the QP algorithm, uses the high-precision optimization of the OBCA algorithm to solve the quality problems of complex scenarios, and combines the segmented parallel mechanism to shorten the overall time consumption; the trajectory post-check and priority selection mechanism ensure the safety and reliability of the trajectory; the collaborative complementarity of multiple algorithms enhances the system stability and success rate. It is suitable for complex scenarios of low-speed and multi-directional driving on unstructured roads, providing an efficient and robust trajectory planning solution for autonomous driving.

[0019] Furthermore, the post-trajectory check at least includes a collision check for checking whether the trajectory overlaps with an obstacle, and sets it as a priority; the step S6 includes: S601, if both the QP algorithm and the OBCA algorithm fail to be optimized, then directly output the Hybrid A-based * Initial search trajectory of the algorithm; S602, if only one of the QP algorithm and the OBCA algorithm is optimized successfully, then when the result of the corresponding collision check is a collision, the output is based on Hybrid A * If the collision check result of the algorithm is no collision, the optimized trajectory corresponding to the successful algorithm is output; S603, if both the QP algorithm and the OBCA algorithm are successfully optimized, the final trajectory is determined and output based on all the inspection results of the trajectory post-check involved in steps S3 and S5.

[0020] Furthermore, it should be noted that in the QP algorithm and the OBCA algorithm, if each algorithm successfully plans (optimizes), then its corresponding representative has an optimized trajectory. That is, if both the QP algorithm and the OBCA algorithm plan successfully, there are two optimized trajectories, corresponding to one of the QP algorithm and the OBCA algorithm respectively. In the case of an optimized trajectory after successful planning, during the post-trajectory inspection process for the corresponding optimized trajectory in steps S3 and S5, only the optimized trajectory that passes all inspections can be directly output. Step S6 is for the case where neither the QP algorithm nor the OBCA algorithm plans successfully, one of them plans successfully but fails the post-trajectory inspection, or both plans successfully but both pass the post-trajectory inspection.

[0021] Specifically, when both the QP algorithm and the OBCA algorithm fail to plan (optimize), the output is directly based on Hybrid A. * The initial search trajectory of the algorithm is used as the final trajectory; if one of the QP algorithm and the OBCA algorithm is successfully planned (optimized), the final trajectory to be output is determined based on the result of the collision check in the trajectory post-check for the optimized trajectory of the corresponding successful algorithm. That is, if the result of the corresponding collision check is a collision, the output is based on HybridA. *The initial search trajectory of the algorithm. If the result of the collision check is no collision, the optimized trajectory corresponding to the successful planning algorithm is output.

[0022] Furthermore, the post-trajectory check also includes a start-end point check to check whether the start and end points of the trajectory are consistent with the start and end points planned in step S1, and a curvature check to check the curvature of each trajectory point on the trajectory to calculate the trajectory length that exceeds the maximum curvature limit of the vehicle; in step S603, the inspection result response order is preset in such a manner that the collision check takes precedence over the start-end point check, and the start-end point check takes precedence over the curvature check. According to the preset inspection result response order, the final trajectory is determined and output based on the results of the post-trajectory check in steps S3 and S5.

[0023] Further, in step S603, the following steps are performed in order: determine whether the optimized trajectories of the QP algorithm and the OBCA algorithm collide (collision check result); if both trajectories collide, output the result based on Hybrid A. * The initial search trajectory of the algorithm is used as the final trajectory; if only one of the trajectories collides, the result of the collision check is output as the optimized trajectory without collision, which is used as the final trajectory; if neither of the two trajectories collides, the next step is performed; determine whether the optimized trajectories of the QP algorithm and the OBCA algorithm meet the start and end point constraints (start and end point check results): if only one of the two trajectories meets the start and end point constraints, the result of the start and end point check is output as the optimized trajectory that meets the start and end point constraints, which is used as the final trajectory; if both trajectories meet the start and end point constraints or neither of them meets the start and end point constraints, the next step is performed; determine the optimized trajectories of the QP algorithm and the OBCA algorithm Whether the trajectory satisfies the curvature constraint (curvature check result): Under the condition that both trajectories satisfy the start and end point constraints, if both optimized trajectories do not satisfy the curvature constraint, then the average curvature of both optimized trajectories is calculated, and the optimized trajectory with smaller average curvature is output as the final trajectory; Under the condition that both trajectories do not satisfy the start and end point constraints, if both optimized trajectories satisfy the curvature constraint or neither of them satisfies the curvature constraint, then the average curvature of both optimized trajectories is calculated, and the optimized trajectory with smaller average curvature is output as the final trajectory; and, if only one of the two optimized trajectories satisfies the curvature constraint, then the optimized trajectory that satisfies the curvature constraint is output as the final trajectory.

[0024] In the present invention, when both the QP algorithm and the OBCA algorithm are successfully planned, but the corresponding optimized trajectories fail the post-trajectory check, the output logic of the final trajectory is: first determine the results of the collision check in the post-trajectory check in steps S3 and S5, then determine the results of the start and end point checks in the post-trajectory check in steps S3 and S5, and finally determine the results of the curvature check in the post-trajectory check in steps S3 and S5.

[0025] Specifically, in the process of determining the results of the collision check in the trajectory post-check in step S3 and step S5, the final trajectory is output according to the set conditions, that is, if there is a collision between the two trajectories, the output is based on Hybrid A. * The algorithm's initial search trajectory is used as the final trajectory. If only one of the trajectories collides, the collision check result is output as the non-collision optimized trajectory as the final trajectory. If neither of the trajectories collides, the next step is to determine the results of the start and end point checks in step S3 and step S5.

[0026] In the present invention, if both the QP algorithm and the OBCA algorithm fail to plan, the system will directly output Hybrid A * In unstructured road scenarios, complex environments and uncertainty constraints may cause the QP and OBCA algorithms to be unable to generate optimized trajectories, while Hybrid A * The algorithm generates an initial trajectory based on the start and end points through search. Although it has not been smoothed or fully considered complex constraints, it constructs a basic path framework from the start point to the end point. This strategy has many significant advantages: first, it guarantees the basic driving function of the vehicle, ensuring that the autonomous driving system still has a reference driving trajectory when the algorithm planning fails, avoiding vehicle stagnation and maintaining basic system operation; second, it improves the robustness of the system. By using the initial trajectory as a backup solution, the system can respond quickly to algorithm failures in complex environments, avoiding crashes and enhancing stability and reliability; third, it saves computing resources, eliminating the need for other complex attempts or recalculations after all algorithms fail, and can focus resources on key tasks such as subsequent driving control; finally, it provides a basis for subsequent optimization. Although the initial trajectory has shortcomings, it can be locally optimized based on real-time environmental information and vehicle status during vehicle driving, gradually improving trajectory quality.

[0027] Next, in determining the results of the start-end point checks in the post-trajectory check in steps S3 and S5, if only one of the two trajectories satisfies the start-end point constraints, the result of the start-end point check is output as the optimized trajectory that satisfies the start-end point constraints, which serves as the final trajectory. If both trajectories satisfy the start-end point constraints or neither satisfies the start-end point constraints, the next step is to determine the results of the curvature check in the post-trajectory check in steps S3 and S5. It should be noted that when only one of the QP and OBCA algorithms succeeds in planning, the post-trajectory check for that optimized trajectory fails. Otherwise, that trajectory would be directly output in the post-trajectory check in steps S3 or S5, and the dynamic selection process in step S6 would not be entered. At this time, the system dynamically selects the output trajectory based on the collision check results in the post-trajectory check. The core logic is to prioritize driving safety and rationally utilize the optimization results. The specific rules of its operation are as follows: if the QP algorithm planning is successful and the OBCA algorithm planning fails, the system calls the collision check result of step S3: if the optimized trajectory of the QP algorithm has no collision, it directly outputs the trajectory because it has been optimized by the algorithm and has more path rationality and efficiency advantages under the premise of safety; if the optimized trajectory of the QP algorithm has a collision, it outputs the Hybrid A * The initial search trajectory generated by the algorithm is based on the starting and ending points and is a theoretically feasible backup solution. If the OBCA algorithm successfully plans and the QP algorithm fails, the system will check the collision results of step S5: if the optimized trajectory of the OBCA algorithm has no collision, it will directly output the trajectory because the OBCA algorithm has more advantages in considering vehicle dynamics and environmental constraints; if the optimized trajectory of the OBCA algorithm has a collision, it will also output the Hybrid A * Initial search trajectory to ensure basic system functions.

[0028] The effects of this trajectory selection rule are: first, safety is prioritized, and dangerous trajectories are filtered through collision checks to ensure that the output trajectory is collision-free, thereby improving system safety in complex environments; second, resources are efficiently utilized, and when a single algorithm planning is successful and safe, the optimization result is directly adopted, reducing calculation time and improving response efficiency; third, fault tolerance is enhanced, and when the algorithm planning fails, the Hybrid A * The initial trajectory is a backup plan to prevent the system from being paralyzed due to the failure of a single algorithm; the fourth is to balance quality and stability, by optimizing the trajectory to ensure path quality and the initial trajectory to support system stability, thus achieving a dynamic balance between safety and functionality.

[0029] Next, in the process of determining the results of the curvature check in the post-trajectory check in step S3 and step S5, whether the two estimates in the previous step satisfy the start and end point constraints must be used as a prerequisite. Specifically, under the condition that both trajectories satisfy the start and end point constraints, if both optimized trajectories do not satisfy the curvature constraint, then the average curvature of the two optimized trajectories is calculated, and the optimized trajectory with a smaller average curvature is output as the final trajectory; and under the condition that both trajectories do not satisfy the start and end point constraints, there are two situations. One is that if both optimized trajectories satisfy the curvature constraint or neither of them satisfies the curvature constraint, then the average curvature of the two optimized trajectories is calculated, and the optimized trajectory with a smaller average curvature is output as the final trajectory; the other is that if only one of the two optimized trajectories satisfies the curvature constraint, then the optimized trajectory that satisfies the curvature constraint is output as the final trajectory.

[0030] In this invention, when both the QP algorithm and the OBCA algorithm plan successfully but fail the post-trajectory check, comparing trajectories to dynamically select the optimal trajectory can improve the performance of the autonomous driving system in many ways, as follows:

[0031] In terms of safety, the rule uses collision-free as the core screening condition. If only one optimized trajectory has no collision, the trajectory is directly output; if both trajectories collide with obstacles, since driving safety cannot be guaranteed, the HybridA trajectory is output. * The algorithm's initial search trajectory. This strategy avoids collision risks, maximizes the safety of the vehicle and its surroundings, reduces the accident rate, and significantly improves the system's safety performance in complex environments.

[0032] For driving accuracy, the rules prioritize satisfying start-end constraints. When two trajectories are collision-free, if one meets the constraints and the other does not, the trajectory that meets them is output. This ensures the vehicle drives precisely from the start point to the end point, avoiding driving errors caused by position or direction deviations and enhancing the reliability of path planning.

[0033] From the perspective of driving stability and comfort, the rule optimizes trajectory smoothness through curvature constraints. Trajectories that meet the curvature constraints are prioritized. If both trajectories meet or do not meet the curvature constraints, the average curvature is calculated and the smaller trajectory is output. This mechanism ensures smoother vehicle steering, reduces handling difficulties and ride roughness caused by excessive curvature, and improves driving stability and comfort.

[0034] From the perspective of overall system performance, the rules dynamically select tracks based on collision scenarios, start-end point constraints, and curvature constraints. This combines the strengths of different algorithms (the real-time performance of the QP algorithm and the trajectory quality of the OBCA algorithm) to overcome the shortcomings of a single algorithm. This enables the system to flexibly select the optimal trajectory based on the complex environment of unstructured roads, improving planning success rates and system stability, and comprehensively enhancing the environmental adaptability and overall performance of the autonomous driving system.

[0035] Furthermore, in step S2 and step S4, the initial search trajectory is segmented and smoothly optimized in parallel, including: a trajectory segmentation step, which is constructed by segmenting the initial search trajectory with the shift point as the dividing point, wherein the shift point is determined based on the following method: setting the front and rear trajectory points as A and B, judging the vector angle passing through trajectory point A and trajectory point B and the heading of trajectory point B, and if the difference between the two is greater than 90 degrees, setting trajectory point B as the shift point to indicate that the vehicle needs to shift gears to change the driving direction when it reaches the corresponding trajectory point; an asynchronous parallel optimization step, which is constructed by simultaneously starting multiple asynchronous thread tasks with the same number of segments as the segmented trajectory, and using the corresponding algorithm to perform parallel optimization on each segmented trajectory; a trajectory combination step, which is constructed by splicing the segmented trajectories head to tail with the shift point as the reference, and combining them into the optimized trajectory of the corresponding algorithm.

[0036] Furthermore, the implementation steps of the collision check in the post-trajectory check include: constructing an ego vehicle box for each trajectory point in the optimized trajectory formed by the corresponding algorithm and performing an expansion process; checking whether the expanded ego vehicle box coincides with the obstacle; if the expanded ego vehicle boxes corresponding to all trajectory points do not coincide with the obstacle, the check result is determined to be non-collision, indicating that the optimized trajectory passes the collision check; otherwise, the check result is collision.

[0037] The collision check process is divided into the following steps: First, a vehicle box is constructed based on each trajectory point in the optimized trajectory. These boxes represent the approximate outline of the vehicle at that location. Subsequently, the ego vehicle box is expanded. This is done because in real-world driving scenarios, vehicles are not ideal points and are subject to errors and sway during driving. This expansion process makes the detection range slightly larger than the actual vehicle outline, providing a safety margin and effectively preventing collisions with obstacles caused by calculation errors.

[0038] Next, the expanded vehicle box is compared with surrounding obstacles. These obstacles include other vehicles in the parking lot, buildings, roadblocks, and more. This comparison determines whether the vehicle will physically come into contact with surrounding obstacles at various locations while following its current trajectory.

[0039] Finally, the optimized trajectory is determined to have passed the collision check based on the comparison results. The optimized trajectory is considered to have passed the collision check only if the inflated ego vehicle boxes corresponding to all trajectory points on the optimized trajectory do not overlap with any obstacles. This indicates that the vehicle can follow this trajectory from the starting point to the end point without colliding with any detected obstacles, and the trajectory is safe and feasible. Conversely, if the inflated ego vehicle box corresponding to any trajectory point overlaps with an obstacle, it indicates that the trajectory poses a collision risk, cannot guarantee safe driving, and therefore fails the collision check. In this case, the trajectory needs to be further optimized or replanned.

[0040] Furthermore, the implementation steps of the start and end point check in the post-trajectory check include: performing distance error and heading error checks on the start point and planned start point, and the end point and planned end point of the optimized trajectory of the corresponding algorithm, respectively, wherein the distance error is the position deviation between the trajectory point and the planned point, and the heading error is the orientation angle deviation between the trajectory point and the planned point; if the distance error is less than a preset distance error threshold and the heading error is less than a preset heading error threshold, then it is determined that the optimized trajectory of the corresponding algorithm meets the start and end point constraints, otherwise it is determined that the start and end point constraints are not met.

[0041] The purpose of the start-end check is to verify the accuracy of the optimized trajectory in terms of starting and ending positions and directions, ensuring that the vehicle can accurately start from the designated starting point and drive towards the target destination in the desired direction according to actual driving requirements. The specific inspection is divided into two parts: the trajectory starting point and the planned starting point inspection, and the trajectory end point and the planned end point inspection. For the trajectory starting point and the planned starting point inspection, it is necessary to measure and compare the distance error and heading error between the two. Among them, the distance error reflects the deviation between the actual starting position and the expected starting position, and the heading error reflects the difference between the vehicle's starting driving direction and the planned direction. These errors are compared with the preset distance error threshold (0.05m in this invention) and the preset heading error threshold (2° in this invention) to determine whether the trajectory starting point meets the requirements. The process of the trajectory end point and the planned end point inspection is similar to the starting point inspection. The distance error and heading error are also measured and compared for the trajectory end point and the planned end point to ensure that the actual end point position reached by the vehicle is close to the planned end point and that the driving direction upon arrival is also as expected. The judgment criteria are still the preset distance error threshold and the preset heading error threshold.

[0042] The entire trajectory is considered to meet the start and end constraints only when both the start and end points meet the error requirements (i.e., the distance error is less than the preset distance error threshold, and the heading error is less than the preset heading error threshold). If any of these checks fail, such as if the distance error at the start point exceeds the threshold or the heading error at the end point is too large, it means that the trajectory does not meet the planned settings at the start or end point, and the trajectory needs to be adjusted or replanned to ensure the accuracy and reliability of vehicle operation.

[0043] Furthermore, the curvature check in the post-trajectory check includes the following steps: checking the curvature of each trajectory point in the optimized trajectory of the corresponding algorithm and calculating the trajectory length that exceeds the maximum curvature limit of the vehicle; if the trajectory length is less than or equal to a preset length threshold, the optimized trajectory of the corresponding algorithm is determined to meet the curvature constraint; otherwise, it is determined that the curvature constraint is not met.

[0044] Furthermore, when calculating the curvature of the trajectory point, the vehicle kinematic model is used, and the calculation is performed according to the vehicle steering wheel angle and the vehicle wheelbase. The formula is as follows:

[0045]

[0046] Where δ is the vehicle steering wheel angle, and L is the vehicle wheelbase.

[0047] In this invention, curvature constraint checking is a crucial step in ensuring vehicle safety and stability. The specific process involves first calculating the curvature of each point on the optimized trajectory using a specific curvature calculation formula, by obtaining the corresponding steering wheel angle and vehicle wheelbase data. This is done to quantify the severity of the vehicle's steering at each position, providing a data foundation for subsequent evaluation.

[0048] Because each vehicle has a different mechanical structure and handling characteristics, each has its own maximum curvature limit. The calculated curvature of each trajectory point is compared with the vehicle's own maximum curvature limit to filter out trajectory points where the curvature exceeds the limit. These points indicate that the vehicle's steering operation at that location may be beyond safe or reasonable ranges, posing a potential risk.

[0049] Next, the trajectory lengths between the trajectory points whose curvature exceeds the maximum curvature limit of the vehicle are accumulated. The total trajectory length obtained reflects the trajectory range where the vehicle may oversteer during the entire driving trajectory.

[0050] Finally, a preset length threshold (1m in this example) is set and the calculated total trajectory length exceeding the vehicle's maximum curvature limit is compared with this threshold. If the total trajectory length does not exceed the preset threshold, it means that the vehicle's oversteering is within an acceptable range during driving. The optimized trajectory satisfies the vehicle's curvature constraint, ensuring that the vehicle will not experience steering difficulties or loss of control due to excessive trajectory curvature during actual driving, and can travel safely and stably. Conversely, if the total trajectory length exceeds the preset threshold, it is determined that the optimized trajectory does not meet the curvature constraint, and the trajectory needs to be adjusted or replanned to ensure the safety and stability of the vehicle.

[0051] In this paper, average curvature is a key indicator of the curvature of the entire track. By calculating the average curvature, we can assess the frequency and intensity of steering maneuvers along a specific track from a macro perspective, thereby determining the impact of that track on vehicle handling.

[0052] During the critical phase of trajectory comparison and selection, when both optimized trajectories present no collision risk and the results of the start / end point check and curvature constraint check are identical, average curvature becomes the key factor in determining the final output trajectory. In this case, the trajectory with the smaller average curvature is selected. This is because a smaller average curvature means smoother steering during driving, lowering the requirements for vehicle handling, and thus providing greater stability and comfort during driving.

[0053] Furthermore, the pass criteria for the post-trajectory check in steps S3 and S5 are set to require that all checks satisfy the constraints. If any check result fails to satisfy the constraints, the post-trajectory check is considered to have failed. In the present invention, the post-trajectory check has strict pass criteria. The entire post-trajectory check is considered to have passed only if the collision check, the start and end point constraint check, and the curvature constraint check all pass.

[0054] A passing collision check ensures that the vehicle will not collide with surrounding obstacles while following the trajectory, ensuring safe driving. A passing start-end constraint check indicates that the trajectory's start and end points meet planned position and orientation requirements, allowing the vehicle to accurately drive from the designated start point to the target destination. A passing curvature constraint check indicates that the trajectory's curvature is within the vehicle's controllable range and that the vehicle can travel smoothly. Only when all three requirements are met can the trajectory be considered safe, accurate, and drivable and output as a reliable driving path. Conversely, if any of these three checks fails, the entire trajectory will fail the post-check. For example, during the collision check, if the inflated ego vehicle box corresponding to a trajectory point overlaps with an obstacle, the trajectory will fail the check even if the other two checks meet the requirements. Similarly, if the start-end distance error or heading error exceeds a threshold, or if the total trajectory length exceeds the ego vehicle's maximum curvature limit by more than a preset threshold, the trajectory has issues and cannot be directly output. At this point, subsequent measures need to be taken, such as using other algorithms to re-optimize the trajectory, or comparing and screening multiple trajectories to determine a more suitable driving trajectory, so as to ensure that a reliable driving path is planned for the vehicle.

[0055] The beneficial effects of the present invention compared to the prior art are as follows:

[0056] The present invention effectively balances the real-time performance and quality of trajectory planning and improves the robustness of the system by integrating multiple algorithms and optimization strategies. Specifically: on the one hand, a trajectory segmentation parallel optimization mechanism is introduced into the QP and OBCA algorithms. After the trajectory is segmented with the shift point as the delimiter, it is processed in parallel by asynchronous threads, and multiple trajectories can be planned at the same time, which significantly improves the planning speed and real-time performance; on the other hand, the algorithm is adapted for different scenarios - the short-lived QP algorithm is used first in simple scenarios, and the trajectory quality is ensured through post-trajectory inspection (including collision, start and end points, and curvature inspection), reducing the redundant calculation of the OBCA algorithm; for complex scenarios, the OBCA algorithm is enabled for deep optimization to make up for the defect that the QP algorithm cannot constrain curvature. In addition, the combined use of multiple algorithms not only improves the success rate of trajectory planning and system stability, but also uses the trajectory comparison module to perform comprehensive dynamic screening of the optimization results, and finally outputs a better trajectory, achieving a comprehensive improvement in trajectory planning performance on unstructured roads.

[0057] The following describes in detail the trajectory planning method and system combining multiple algorithms for unstructured roads of the present invention with reference to the embodiments shown in the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 This is an overall flow chart of the trajectory planning method combining multiple algorithms on unstructured roads of the present invention;

[0059] Figure 2This is a flow chart of the present invention for dynamically selecting output trajectories based on the optimization results of the QP algorithm and the OBCA algorithm. DETAILED DESCRIPTION

[0060] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the drawings in the embodiments of the present invention. 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 making creative work are within the scope of protection of the present invention. It should be noted that the embodiments in this application and the features in the embodiments can be combined with each other unless there is a conflict.

[0061] In typical unstructured road scenarios such as parking lots, vehicles face many challenges, such as complex environment layout, lack of clear lane markings, and frequent acceleration, deceleration, and steering operations. In such scenarios, the trajectory planning technology of the present invention is particularly important.

[0062] Figure 1 The overall flow chart of the trajectory planning method of the present invention is shown, and the "Hybrid A * A hierarchical planning framework consisting of initial search—QP fast optimization—OBCA deep optimization—multi-dimensional post-check—dynamic trajectory selection. * The initial search trajectory generated by the algorithm forms the basis for subsequent optimization, providing a preliminary direction for the entire planning process. The QP algorithm, with its fast optimization capabilities, can quickly generate trajectories in simple scenarios. The OBCA algorithm is used to deeply optimize trajectories in complex scenarios. Multi-dimensional post-inspection is key to ensuring safe, accurate, and feasible trajectories. The dynamic trajectory selection mechanism selects the most suitable trajectory for the vehicle based on different optimization results and inspection conditions.

[0063] Figure 2 This paper presents a flow chart for dynamic trajectory selection based on the optimization results of the QP and OBCA algorithms, clarifying the trajectory selection rules for different optimization results. This rule plays a key role in ensuring vehicle safety and improving driving accuracy and stability.

[0064] Based on the technical logic shown in the above flowchart, the trajectory planning method and system of the present invention are further explained through specific embodiments below.

[0065] like Figure 1As shown, the present invention discloses a trajectory planning method combining multiple algorithms under unstructured roads, comprising the following steps: S1, based on the planned starting point and end point, adopting the Hybrid A* algorithm to perform trajectory planning, if the planning is successful, generating an initial search trajectory, and entering step S2; if the planning fails, terminating the process; S2, based on the initial search trajectory generated in step S1, adopting the QP algorithm to perform segmented parallel smooth optimization on it, if the optimization is successful, generating a QP optimized trajectory, and entering step S3; if the optimization fails, entering step S4; S3, performing a trajectory post-check on the QP optimized trajectory obtained in step S2, if the check passes, directly outputting the QP optimized trajectory, and terminating the process; if the check fails, entering step S4; S4, based on the initial search trajectory generated in step S1, The OBCA algorithm is used to perform segmented parallel smoothing optimization. If the optimization is successful, the OBCA optimized trajectory is generated and the process proceeds to step S5. If the optimization fails, the process proceeds to step S6. In step S5, the OBCA optimized trajectory obtained in step S4 is subjected to the same trajectory post-check as in step S3. If the check passes, the OBCA optimized trajectory is directly output and the process terminates. If the check fails, the process proceeds to step S6. In step S6, based on the optimization results of the QP algorithm and the OBCA algorithm and the results of the trajectory post-check obtained in steps S3 and S5, the trajectories are compared, and the final trajectory is dynamically determined and output.

[0066] The post-trajectory check at least includes a collision check to check whether the trajectory overlaps with an obstacle, and sets it as a priority; the step S6 includes: S601, if both the QP algorithm and the OBCA algorithm fail to optimize, then directly output the Hybrid A-based * Initial search trajectory of the algorithm; S602, if only one of the QP algorithm and the OBCA algorithm is optimized successfully, then when the result of the corresponding collision check is a collision, the output is based on Hybrid A * If the collision check result of the algorithm is no collision, the optimized trajectory corresponding to the successful algorithm is output; S603, if both the QP algorithm and the OBCA algorithm are successfully optimized, the final trajectory is determined and output based on all the inspection results of the trajectory post-check involved in steps S3 and S5.

[0067] The post-trajectory check also includes a start-end point check to check whether the start and end points of the trajectory are consistent with the start and end points planned in step S1, and a curvature check to check the curvature of each trajectory point on the trajectory to calculate the trajectory length that exceeds the maximum curvature limit of the vehicle. In step S603, the check result response order is preset in such a manner that the collision check takes precedence over the start-end point check, and the start-end point check takes precedence over the curvature check. According to the preset check result response order, the final trajectory is determined and output based on the results of the post-trajectory check in steps S3 and S5.

[0068] In a specific embodiment, in step S603, a priority order is preset, that is, when both QP and OBCA are optimized successfully but the trajectory post-check fails, the final trajectory is determined in the order of whether there is a collision > whether the start and end point constraints are satisfied > whether the curvature constraint is satisfied.

[0069] Specifically, in step S603, the following steps are performed in order: First, determine whether the optimized trajectories of the QP algorithm and the OBCA algorithm collide: If both trajectories collide, then output the Hybrid A-based * The initial search trajectory of the algorithm is used as the final trajectory; if only one of the trajectories collides, the result of the collision check is output as the optimized trajectory without collision, which is used as the final trajectory; if neither of the two trajectories collides, the next step is to determine whether the optimized trajectories of the QP algorithm and the OBCA algorithm meet the start and end point constraints. If only one of the two trajectories meets the start and end point constraints, the result of the start and end point check is output as the optimized trajectory that meets the start and end point constraints as the final trajectory; if both trajectories meet the start and end point constraints or neither of them meets the start and end point constraints, the next step is to determine the optimized trajectory of the QP algorithm and the OBCA algorithm. Whether the trajectory satisfies the curvature constraint: Under the condition that both trajectories satisfy the start and end point constraints, if neither optimized trajectories satisfies the curvature constraint, then the average curvature of both optimized trajectories is calculated, and the optimized trajectory with smaller average curvature is output as the final trajectory; Under the condition that both trajectories do not satisfy the start and end point constraints, if both optimized trajectories satisfy the curvature constraint or neither of them satisfies the curvature constraint, then the average curvature of both optimized trajectories is calculated, and the optimized trajectory with smaller average curvature is output as the final trajectory; and, if only one of the two optimized trajectories satisfies the curvature constraint, then the optimized trajectory that satisfies the curvature constraint is output as the final trajectory.

[0070] Based on the above disclosed steps, the present invention takes into account both planning real-time performance and trajectory quality by rationally combining the QP algorithm and the OBCA algorithm. In simple scenarios, the QP algorithm is used first to quickly optimize the trajectory, while in complex scenarios, the OBCA algorithm is used to ensure quality. At the same time, a strict post-trajectory inspection mechanism including collision, start and end point constraints, and curvature constraint checks is set up. Only trajectories that pass all the checks will be output, thereby ensuring safe, accurate, and smooth driving. In the overall planning system, Hybrid A * The algorithm generates the initial trajectory, and the QP and OBCA algorithms collaborate to optimize, improving planning success rates and system stability. Trajectory selection is dynamically prioritized based on collision avoidance, meeting start and end point constraints, meeting curvature constraints, and minimizing average curvature. This further enhances the scientific and rational nature of planning, effectively ensuring safe and efficient vehicle operation on unstructured roads.

[0071] In a specific embodiment, the QP algorithm or the OBCA algorithm is used to * The operation of segmented parallel smooth optimization of the initial search trajectory generated by the algorithm is as follows: first, the initial trajectory is segmented by the shift point. The shift point is determined by judging whether the difference between the vector angles of the previous and subsequent trajectory points A and B and the heading angle of point B is greater than 90°. The trajectory is divided into n segments, and the starting and ending points of each trajectory are both shift points. Then, n asynchronous thread tasks are started to perform corresponding algorithm optimization on each trajectory segment. If the QP algorithm is used for segmented parallel smooth optimization of the trajectory, its short planning time is used to improve real-time performance, which is suitable for simple scenarios. After successful planning, the trajectory is checked for collision, start and end points, and curvature. If it passes, the QP trajectory is output. If it fails or the check fails, the OBCA algorithm is triggered for optimization. If the OBCA algorithm is used for segmented parallel smooth optimization of the trajectory, it is suitable for complex scenarios. After successful planning, the trajectory is checked for similar types (collision, start and end points, curvature). If it passes, the OBCA trajectory is output. If it fails or the check fails, the trajectory comparison process is entered to determine the final output. During the asynchronous parallel optimization step, the starting and ending points (i.e., shift points) of each segmented trajectory remain unchanged. After optimization, both algorithms sequentially concatenate the n segments through the shift points to form a complete optimized trajectory. The QP algorithm prioritizes real-time performance, while the OBCA algorithm ensures trajectory quality through curvature checks.

[0072] like Figure 2 As shown, in a preferred embodiment of the present invention, in step S6, when both the QP algorithm and the OBCA algorithm are successfully planned, the rules for dynamically selecting and outputting the optimized trajectory are as follows: 1) If both optimized trajectories collide, output HybridA * The algorithm generates the initial search trajectory; 2) If only one optimized trajectory has a collision, output the collision-free optimized trajectory; 3) If both optimized trajectories do not collide, further judgment is made: if only one meets the start-end constraint, output the optimized trajectory that meets the start-end constraint; if neither meets the start-end constraint, continue to judge the curvature constraint: if only one meets the curvature constraint, output the optimized trajectory that meets the curvature constraint; if both meet or do not meet the curvature constraint, calculate the average curvature of the two and output the optimized trajectory with the smaller average curvature; if both meet the start-end constraint, directly calculate the average curvature of the two and output the optimized trajectory with the smaller average curvature.

[0073] In this embodiment, if both the QP algorithm and the OBCA algorithm are successfully planned, the system dynamically selects the output trajectory according to specific rules. This rule has significant effects in many aspects: At the safety level, the rule uses collision-free as the core screening condition. If only one optimized trajectory is collision-free, it is directly output; if both trajectories collide with obstacles, since driving safety cannot be guaranteed, the Hybrid A trajectory is output. * The algorithm's initial search trajectory. This strategy avoids collision risks, maximizes vehicle and surrounding safety, reduces accident rates, and significantly improves system safety performance in complex environments. Regarding driving accuracy, the rule prioritizes satisfying start-end constraints. When two trajectories are collision-free, if one satisfies the start-end constraints and the other does not, the trajectory that satisfies the constraints is output. This ensures the vehicle accurately navigates from the start to the end, avoids driving errors caused by position or direction deviation, and enhances path planning reliability. Regarding driving stability and comfort, the rule optimizes trajectory smoothness using curvature constraints. Trajectories that satisfy the curvature constraints are prioritized. If both trajectories satisfy or neither satisfy the curvature constraints, the average curvature is calculated and the smaller trajectory is output. This mechanism ensures smoother vehicle steering, reduces handling difficulties and ride roughness caused by excessive curvature, and improves driving stability and comfort. From the perspective of overall system performance, the rule dynamically selects a trajectory based on collision conditions, start-end constraints, and curvature constraints. This combines the strengths of different algorithms (the real-time performance of the QP algorithm and the trajectory quality of the OBCA algorithm) and overcomes the shortcomings of individual algorithms. This enables the system to flexibly select the optimal trajectory based on the complex environment of unstructured roads, improve planning success rate and system stability, and comprehensively enhance the environmental adaptability and overall performance of the autonomous driving system.

[0074] like Figure 2 As shown, in a preferred embodiment of the present invention, in step S6, when only one of the QP algorithm and the OBCA algorithm succeeds in planning, the rules for dynamically selecting and outputting the optimized trajectory are as follows: if the QP algorithm successfully plans and the OBCA algorithm fails, the optimized trajectory is selected based on the collision check in the trajectory post-check in step S3. If no collision is found, the QP optimized trajectory is output; if a collision is found, the Hybrid A optimized trajectory is output. * The initial search trajectory generated by the algorithm; if the OBCA algorithm planning is successful and the QP algorithm planning fails, the optimized trajectory is selected based on the collision check in the post-trajectory check in step S5. If there is no collision, the OBCA optimized trajectory is output; if there is a collision, the Hybrid A is output. * Initial search trajectory generated by the algorithm.

[0075] In this embodiment, when only one of the QP algorithm and the OBCA algorithm is successful, the system dynamically selects the output trajectory based on the collision check result in the trajectory post-check. The core logic is to prioritize driving safety and rationally utilize the optimization results. The specific rules are as follows: If the QP planning is successful and the OBCA planning fails, the system calls the collision check result of step S3: If the QP optimized trajectory has no collision, it is directly output because it has been optimized by the algorithm and has more path rationality and efficiency advantages under the premise of safety; if the QP optimized trajectory has a collision, the Hybrid A is output. * The initial search trajectory generated by the algorithm is based on the starting and ending points and is a theoretically feasible backup solution. If OBCA planning succeeds but QP planning fails, the system will check the collision results in step S5: if the OBCA optimized trajectory has no collision, it will directly output the trajectory, because the OBCA algorithm has more advantages in considering vehicle dynamics and environmental constraints; if the OBCA optimized trajectory has a collision, it will also output the Hybrid A * Initial search trajectory to ensure basic system functions.

[0076] The effects of this trajectory selection rule are: first, safety is prioritized, and dangerous trajectories are filtered through collision checks to ensure that the output trajectory is collision-free, thereby improving system safety in complex environments; second, resources are efficiently utilized, and when a single algorithm planning is successful and safe, the optimization result is directly adopted to reduce calculation time and improve response efficiency; third, fault tolerance is enhanced, and when the algorithm planning fails, the Hybrid A * The initial trajectory is a backup plan to prevent the system from being paralyzed due to the failure of a single algorithm; the fourth is to balance quality and stability, by optimizing the trajectory to ensure path quality and the initial trajectory to support system stability, thus achieving a dynamic balance between safety and functionality.

[0077] like Figure 2 As shown, in a preferred embodiment of the present invention, in step S6, when both the QP algorithm and the OBCA algorithm fail to plan, the Hybrid A algorithm is directly output. * Initial search trajectory generated by the algorithm.

[0078] In this embodiment, if both the QP algorithm and the OBCA algorithm fail to plan, the system will directly output Hybrid A * The algorithm generates the initial search trajectory. Unstructured road environments are complex and volatile, with numerous uncertainties and constraints, which can render these two optimization algorithms ineffective. However, vehicles must have a planned path. While the initial search trajectory, while not meticulously processed and lacking full consideration of complex constraints, does establish a basic path framework from the starting point to the destination.

[0079] This strategy has many significant advantages: first, it guarantees the basic driving function of the vehicle, ensuring that the autonomous driving system still has a reference driving trajectory when the algorithm planning fails, avoiding vehicle stagnation and maintaining basic system operation; second, it improves the robustness of the system. By using the initial trajectory as a backup plan, the system can respond quickly to algorithm failures in complex environments, avoiding crashes and enhancing stability and reliability; third, it saves computing resources. There is no need to make other complex attempts or recalculations after all algorithms fail, and resources can be concentrated on key tasks such as subsequent driving control; finally, it provides a basis for subsequent optimization. Although the initial trajectory has shortcomings, it can be locally optimized based on real-time environmental information and vehicle status during vehicle driving, gradually improving the trajectory quality.

[0080] In a preferred embodiment of the present invention, the collision check in the post-trajectory check includes: expanding the ego vehicle box formed by each trajectory point in the optimized trajectory; checking whether the expanded ego vehicle box coincides with the obstacle; if the expanded ego vehicle boxes corresponding to all trajectory points do not coincide with the obstacle, then the optimized trajectory is determined to have passed the collision check.

[0081] In this embodiment, collision checking is performed as follows: a self-vehicle box representing the vehicle's outline is constructed based on each trajectory point of the optimized trajectory, and then expanded. In actual driving, vehicles may experience driving errors and wobbles. Expansion can expand the detection range, provide a safety margin, and avoid collisions caused by calculation errors. The expanded self-vehicle box is then compared with other vehicles, buildings, roadblocks, and other obstacles in the parking lot to determine whether the vehicle will physically contact any obstacles while following the current trajectory. Finally, based on the comparison results, a collision check is determined to determine whether the trajectory passes. Only when the expanded self-vehicle box corresponding to all trajectory points does the entire trajectory pass the check and is safe for driving. If the expanded self-vehicle box of any trajectory point overlaps with an obstacle, the trajectory presents a collision risk and fails the check, requiring further optimization or replanning.

[0082] In a preferred embodiment of the present invention, the start-end constraint check in the post-trajectory check includes: checking the distance error and heading error between the trajectory start point and the planned start point, and between the trajectory end point and the planned end point, respectively; the distance error is the position deviation between the trajectory point and the planned point, and the heading error is the orientation angle deviation between the trajectory point and the planned point; if the distance error of the trajectory start point is less than a preset distance error threshold and the heading error is less than a preset heading error threshold, and the distance error and heading error of the trajectory end point both meet the same threshold requirements, then the trajectory is determined to meet the start-end constraint.

[0083] In this embodiment, the start-end constraint check is designed to verify the accuracy of the optimized trajectory's starting and ending positions and directions, ensuring that the vehicle travels from the specified starting point to the target destination as required. This check is divided into two parts: a check of the trajectory's starting point against the planned starting point, and a check of the trajectory's end point against the planned end point. When checking the trajectory's starting point, the distance error (reflecting positional deviation) and heading error (reflecting directional difference) are measured and compared with the planned starting point. These errors are then compared to preset distance error thresholds (0.05m in this embodiment) and heading error thresholds (2° in this embodiment) to determine whether they meet the requirements.

[0084] The trajectory endpoint check process is similar. The distance and heading errors between the trajectory endpoint and the planned endpoint are measured and compared, and judgments are made based on the same thresholds. Only when the distance error between the start and end points of the trajectory is less than the threshold, and the heading error is also less than the threshold, does the entire trajectory meet the start and end constraints. If either condition is not met, such as the start distance error exceeding the threshold or the end heading error being too large, the trajectory must be adjusted or replanned to ensure accurate and reliable vehicle operation.

[0085] In a preferred embodiment of the present invention, the curvature constraint check in the post-trajectory check includes: calculating the curvature of each trajectory point in the optimized trajectory; counting the trajectory points whose curvature exceeds the maximum curvature limit of the ego vehicle; accumulating the trajectory lengths between the counted trajectory points to obtain the total trajectory length exceeding the maximum curvature limit of the ego vehicle; if the total trajectory length does not exceed a preset length threshold, then the optimized trajectory is determined to meet the curvature constraint. The trajectory point curvature is calculated based on the vehicle kinematic model, the vehicle steering wheel angle, and the vehicle wheelbase, and the formula is as follows:

[0086]

[0087] Where δ is the vehicle steering wheel angle, and L is the vehicle wheelbase.

[0088] In this embodiment, curvature constraint checking is crucial to ensuring vehicle driving safety and stability. First, using the formula (where δ is the steering wheel angle of the vehicle, and L is the wheelbase of the vehicle), calculate the curvature of each point on the optimized trajectory, and quantify the severity of the vehicle's steering. Due to differences in the mechanical structure and handling performance of different vehicles, each has a maximum curvature limit. Compare the calculated curvature of each point with the vehicle's own limitations, and filter out the trajectory points that exceed the limit, which are at risk of steering. Then, add up the trajectory lengths between the trajectory points where the curvature exceeds the limit, and the total length reflects the trajectory range of oversteering during vehicle driving. Finally, compare the total length with the preset length threshold (1m in this embodiment). If the threshold is not exceeded, it indicates that the oversteering situation is acceptable, the trajectory meets the curvature constraint, and the vehicle is safe and stable; if the threshold is exceeded, the trajectory does not meet the constraint and needs to be adjusted or replanned to ensure safe and stable vehicle driving.

[0089] In this paper, average curvature is a key indicator of the curvature of the entire track. By calculating the average curvature, we can assess the frequency and intensity of steering maneuvers along a specific track from a macro perspective, thereby determining the impact of that track on vehicle handling.

[0090] In a specific embodiment, the calculation method is to first use the formula (where δ is the vehicle steering wheel angle and L is the vehicle wheelbase), calculate the curvature value of each point on the trajectory, then add up the curvature values ​​of all trajectory points, and finally divide this sum by the total number of trajectory points. For example, if a trajectory contains n trajectory points, the curvatures of each point are k1, k2, ..., k n , then the mean curvature of the trajectory

[0091] During the critical phase of trajectory comparison and selection, when both optimized trajectories present no collision risk and the results of the start / end point check and curvature constraint check are identical, average curvature becomes the key factor in determining the final output trajectory. In this case, the trajectory with the smaller average curvature is selected. This is because a smaller average curvature means smoother steering during driving, lowering the requirements for vehicle handling, and thus providing greater stability and comfort during driving.

[0092] In a preferred embodiment of the present invention, the criteria for passing the post-trajectory check in step S3 and step S5 are: passing the collision check, the start and end point constraint check, and the curvature constraint check; the criteria for failing the post-trajectory check is: failing any one of the checks.

[0093] In this embodiment, the post-trajectory inspection criteria are stringent. Only when all three checks—collision, start / end point constraints, and curvature constraints—pass can the entire trajectory be considered a pass. A passing collision check ensures vehicle safety and avoids collisions with obstacles. A passing start / end point constraint check indicates that the trajectory's start and end points are aligned with the plan, allowing the vehicle to travel accurately. A passing curvature constraint check indicates that the trajectory's curvature is within the vehicle's controllable range, ensuring smooth travel. Only when all three criteria are met can the trajectory qualify as safe, accurate, and drivable, and be output as a reliable path.

[0094] Conversely, if any one of the three checks fails, the entire trajectory will fail the final check. For example, during a collision check, if the inflated ego vehicle box corresponding to a trajectory point overlaps with an obstacle, if the distance or heading error between the start and end points exceeds a threshold, or if the total trajectory length exceeds the maximum curvature limit of the ego vehicle and exceeds a preset threshold, the trajectory is problematic and cannot be directly output. In this case, measures such as re-optimization of the conversion algorithm or screening among multiple trajectories are necessary to determine a more appropriate driving trajectory and ensure the vehicle's reliable driving path.

[0095] The present invention constructs "Hybrid A * The hierarchical planning framework of "initial search - QP fast optimization - OBCA deep optimization - multi-dimensional post-check - dynamic trajectory selection" effectively balances the real-time performance and trajectory quality of trajectory planning on unstructured roads: First, the Hybrid A * An initial feasible trajectory is generated, and the fast QP algorithm is used to achieve rapid optimization in simple scenarios. Qualified trajectories are then screened based on multiple dimensions, including collision, start-end constraints, and curvature constraints. If QP optimization fails to meet requirements, the OBCA algorithm is used for further optimization. Ultimately, the optimal trajectory is dynamically selected based on a preset priority (collision-free > start-end constraints > curvature constraints > average curvature). This method not only improves planning efficiency (100ms optimization) through the QP algorithm, but also ensures trajectory quality (curvature accuracy) in complex scenarios with the OBCA algorithm. Combining a post-trajectory checking mechanism with a multi-algorithm complementary strategy significantly enhances the robustness of the planning system, ensuring that the output trajectory meets actual driving requirements in terms of safety (collision-free), geometric feasibility (curvature compliance), and positional accuracy (start-end point error <0.05m, heading error <2°). This effectively addresses the existing problem of a single algorithm struggling to balance efficiency and accuracy, providing a reliable technical solution for autonomous driving applications in complex, unstructured scenarios such as parking lots.

[0096] In the present invention, Hybrid A * The algorithm is also called the hybrid A algorithm, which is based on the classic A * This path planning method is developed based on the QP algorithm. This algorithm not only considers obstacle avoidance in a static environment but also fully accounts for constraints imposed by factors such as turning radius during actual driving. The QP algorithm, also known as the Quadratic Programming (QP) algorithm, is a mathematical optimization problem with a quadratic objective function and linear constraints. The OBCA algorithm is also known as the Optimization-based Collision Avoidance (OBCA) algorithm.

[0097] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. A trajectory planning method combining multiple algorithms on unstructured roads, characterized by: The following steps are involved: S1, based on the planned start and end points, adopts Hybrid A * The algorithm performs trajectory planning. If the planning is successful, the initial search trajectory is generated and the process proceeds to step S2. If the planning fails, the process is terminated. S2, based on the initial search trajectory generated in step S1, the QP algorithm is used to perform segmented parallel smooth optimization on it. If the optimization is successful, a QP optimized trajectory is generated and the process proceeds to step S3; if the optimization fails, the process proceeds to step S4; S3, perform a post-trajectory check on the QP optimized trajectory obtained in step S2. If the check passes, directly output the QP optimized trajectory and terminate the process; If the check fails, proceed to step S4; S4, based on the initial search trajectory generated in step S1, the OBCA algorithm is used to perform segmented parallel smooth optimization on it. If the optimization is successful, the OBCA optimized trajectory is generated and the process proceeds to step S5; If the optimization fails, proceed to step S6; S5, performing the same post-trajectory check as in step S3 on the OBCA optimized trajectory obtained in step S4. If the check passes, the OBCA optimized trajectory is directly output and the process is terminated; If the check fails, proceed to step S6; S6, based on the optimization results of the QP algorithm and the OBCA algorithm and the results of the post-check of the trajectories obtained in steps S3 and S5, the trajectories are compared, and the final trajectory is dynamically determined and output.

2. The trajectory planning method according to claim 1, characterized in that: The post-trajectory check at least includes a collision check for checking whether the trajectory overlaps with an obstacle, and setting it as a priority; the step S6 includes: S601, if both the QP algorithm and the OBCA algorithm fail to optimize, then directly output the Hybrid A-based * The algorithm's initial search trajectory; S602: If only one of the QP algorithm and the OBCA algorithm is optimized successfully, then when the result of the corresponding collision check is a collision, the Hybrid A-based * The algorithm's initial search trajectory. If the collision check result is no collision, the optimized trajectory corresponding to the successful planning algorithm is output; S603: If both the QP algorithm and the OBCA algorithm are optimized successfully, a final trajectory is determined and output according to all inspection results in the trajectory post-inspection involved in steps S3 and S5.

3. The trajectory planning method according to claim 2, characterized in that: The post-trajectory check also includes a start-end check to check whether the start and end points of the trajectory are consistent with the start and end points planned in step S1 and a curvature check to check the curvature of each trajectory point on the trajectory to calculate the trajectory length that exceeds the maximum curvature limit of the vehicle; In step S603, the inspection result response order is preset in such a manner that collision check takes precedence over start and end point check, and start and end point check takes precedence over curvature check. According to the preset inspection result response order, based on the results of the post-trajectory check in steps S3 and S5, the final trajectory is determined and output.

4. The trajectory planning method according to claim 3, characterized in that: In step S603, the following steps are performed in order: Determine whether the optimization trajectories of the QP algorithm and the OBCA algorithm collide: If there is a collision between the two trajectories, the output is based on Hybrid A * The algorithm’s initial search trajectory serves as the final trajectory; If only one of the trajectories collides, the collision check result is output as the non-collision optimized trajectory as the final trajectory; If there is no collision between the two trajectories, proceed to the next step; Determine whether the optimization trajectories of the QP algorithm and the OBCA algorithm meet the start and end point constraints: If only one of the two trajectories satisfies the start and end point constraints, the output result of the start and end point check is the optimized trajectory that satisfies the start and end point constraints as the final trajectory; If both trajectories satisfy the start and end point constraints or neither of them satisfies the start and end point constraints, proceed to the next step; Determine whether the optimized trajectories of the QP algorithm and the OBCA algorithm satisfy the curvature constraint: Under the condition that both trajectories satisfy the start and end point constraints, if both optimized trajectories do not satisfy the curvature constraint, the average curvature of the two optimized trajectories is calculated, and the optimized trajectory with the smaller average curvature is output as the final trajectory; Under the condition that both trajectories do not meet the start and end point constraints, if both optimized trajectories meet the curvature constraint or neither meets the curvature constraint, the average curvature of the two optimized trajectories is calculated, and the optimized trajectory with the smaller average curvature is output as the final trajectory; and if only one of the two optimized trajectories meets the curvature constraint, The optimized trajectory that satisfies the curvature constraint is output as the final trajectory.

5. The trajectory planning method according to claim 4, characterized in that: In step S2 and step S4, the initial search trajectory is subjected to segmented parallel smooth optimization, including: The trajectory segmentation step is constructed by segmenting the initial search trajectory using the shift point as a dividing point, wherein the shift point is determined based on the following method: setting the previous and next trajectory points as A and B, judging the vector angle passing through trajectory point A and trajectory point B and the heading of trajectory point B, and if the difference between the two is greater than 90 degrees, then setting trajectory point B as the shift point to indicate that the vehicle needs to shift gears to change the driving direction when it reaches the corresponding trajectory point; The asynchronous parallel optimization step is constructed as follows: based on the number of segments of the segmented trajectory, multiple asynchronous thread tasks with the same number of segments are simultaneously started, and each segmented trajectory is optimized in parallel using the corresponding algorithm; The trajectory combination step is constructed by splicing the segmented trajectories head to tail based on the shift point to form an optimized trajectory of the corresponding algorithm.

6. The trajectory planning method according to claim 5, characterized in that: The implementation steps of the collision check in the trajectory post-check include: A vehicle box is constructed for each trajectory point in the optimized trajectory generated by the corresponding algorithm and then expanded. Check whether the expanded vehicle box overlaps with the obstacle; If the expanded vehicle boxes corresponding to all trajectory points do not overlap with the obstacles, the check result is judged as non-collision, which means that the optimized trajectory passes the collision check. Otherwise, the check result is collision.

7. The trajectory planning method according to claim 6, characterized in that: The implementation steps of the start and end point inspection in the post-track inspection include: Check the distance error and heading error between the starting point and the planned starting point of the optimized trajectory of the corresponding algorithm, and the end point and the planned end point respectively, where the distance error is the position deviation between the trajectory point and the planned point, and the heading error is the direction angle deviation between the trajectory point and the planned point; If the distance error is less than the preset distance error threshold and the heading error is less than the preset heading error threshold, the optimized trajectory of the corresponding algorithm is determined to meet the start and end point constraints; otherwise, it is determined that the start and end point constraints are not met.

8. The trajectory planning method according to claim 7, characterized in that: The implementation steps of the curvature inspection in the post-track inspection include: Check the curvature of each trajectory point in the optimized trajectory of the corresponding algorithm and calculate the trajectory length that exceeds the maximum curvature limit of the vehicle; If the trajectory length is less than or equal to the preset length threshold, the optimized trajectory of the corresponding algorithm is determined to meet the curvature constraint; otherwise, it is determined that the curvature constraint is not met.

9. The trajectory planning method according to claim 8, characterized in that: When calculating the curvature of a trajectory point, the vehicle kinematic model is used, based on the vehicle steering wheel angle and the vehicle wheelbase. The formula is as follows: Where k is the curvature of the trajectory point, δ is the vehicle steering wheel angle, and L is the vehicle wheelbase.

10. The trajectory planning method according to claim 1, characterized in that: The criterion for passing the post-trajectory inspection in step S3 and step S5 is that all inspections satisfy the constraints. If the inspection result of any inspection does not satisfy the constraints, it is determined that the post-trajectory inspection has failed.

Images