A restricted space trajectory planning method considering vehicle body volume

Abstract

The application discloses a kind of limited space trajectory planning method considering vehicle volume, comprising: first, construct two-dimensional occupancy grid map and adopt multiple circle surrounding rectangular frame model to parameterize the representation of vehicle;Using non-uniform B spline curve parameterization reference trajectory and constructing optimization problem;Along the trajectory, calculate minimum safety distance point by point to locate interference control point, generate curvature flat weight of Gaussian distribution with interference point as center, and negatively correlate coupling between flat weight and preset reference weight, obtain the reference weight after dynamic relaxation;Construct joint objective function including smoothing cost, weighted obstacle avoidance cost and weighted deviation cost, iteratively update trajectory control points by calling numerical solver through inner and outer double-loop architecture.The application can output collision-free, high-order smooth and dynamic feasible trajectory, improve the safety, smoothness and solution robustness of trajectory planning in limited space.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to a method for trajectory planning in a constrained space that takes into account the size of the vehicle body. Background Technology

[0002] As autonomous driving technology extends from structured roads (such as highways and urban arterial roads) to unstructured, confined spaces (such as closed parks, narrow mining roads, old underground parking garages, and construction areas), local trajectory planning, as a core functional module of autonomous driving systems, faces increasingly stringent technical challenges. In these extreme scenarios, limited space, chaotic obstacle distribution, and dramatic changes in curve curvature place higher demands on the safety, smoothness, and real-time performance of the planned trajectory.

[0003] Currently, mature local trajectory planning solutions in the industry typically employ the following technical approach: continuous parametric representation of the trajectory using B-spline curves or Bézier curves; local obstacle avoidance and trajectory smoothing optimization using algorithms such as Incremental Path Flattening (IPF); collision detection using vehicle geometric bounding boxes (such as single-circle models or directed bounding boxes); and finally, numerical optimization using a nonlinear programming solver. These methods have been widely validated and applied in structured road scenarios.

[0004] However, existing trajectory planning schemes still have the following technical shortcomings when dealing with unstructured, constrained spaces:

[0005] First, traditional obstacle avoidance thrust generation methods employ a rigid step-like strategy, which applies pulse-like correction forces to the trajectory based on discrete collision detection results. This approach can easily lead to abrupt curvature changes or geometric angles in the optimized trajectory, failing to guarantee high-order smoothness and thus affecting the vehicle's ride comfort and stability.

[0006] Second, most solutions employ a fixed-weight multi-objective optimization framework, in which obstacle avoidance penalty and reference trajectory fidelity terms antagonize each other. When the weight configurations of these two terms are not suited to the characteristics of the scenario, the optimization system is prone to deadlock: on the one hand, obstacle avoidance requirements drive the trajectory away from obstacles, while on the other hand, fidelity constraints prevent the trajectory from deviating from the original path, ultimately leading to planning failure or the output of a suboptimal trajectory.

[0007] Third, the vehicle geometry modeling and trajectory optimization processes are disconnected. Existing solutions generally use point mass models, single-circle approximation models, or directed bounding boxes for collision detection. Point mass or single-circle models cannot accurately represent the sweep volume expansion characteristics of a vehicle during turning (especially the inner wheel difference effect), which can easily lead to vehicle body scraping interference. While directed bounding boxes have higher accuracy, their distance function is not differentiable, making it difficult to directly integrate them into gradient-based numerical optimization frameworks, thus limiting the efficiency and quality of optimization solutions.

[0008] Fourth, existing single-layer optimization architectures lack robustness when dealing with non-convex constrained spaces. Due to the lack of an active escape mechanism for local optimum traps, the solver is prone to premature convergence to infeasible solutions in complex environments, leading to trajectory planning failure. Furthermore, globally indiscriminate optimization strategies have high computational redundancy, making it difficult to meet real-time requirements.

[0009] In summary, designing a trajectory planning method that can balance modeling accuracy, optimized smoothness, and robust solution for the special needs of unstructured constrained spaces is a technical problem that urgently needs to be solved in this field. Summary of the Invention

[0010] The purpose of this invention is to provide a trajectory planning method for confined spaces that takes into account the vehicle body volume, thereby solving the problems of abrupt changes in trajectory curvature, deadlock in obstacle avoidance and path preservation optimization, and inaccurate representation of vehicle sweep volume in existing methods in unstructured narrow scenarios.

[0011] Technical solution: A constrained space trajectory planning method considering vehicle volume, comprising the following steps:

[0012] Acquire environmental perception data and vehicle status, construct a two-dimensional occupancy grid map, and use a multi-circle bounding rectangle model to parametrically represent the vehicle body to obtain the vehicle geometric model;

[0013] Discrete reference trajectories are collected, and non-uniform B-spline curves are used for parameterization to construct an initial nonlinear programming problem.

[0014] Calculate the minimum distance from the multi-circle bounding rectangle model to the occupied raster map point by point along the current trajectory, compare it with the safety threshold, mark the interference control points, and generate an interference control point index set;

[0015] Based on the set of interferometric control point indices, Gaussian-distributed curvature flattening weights are generated with the interferometric control points as the center, and the maximum envelope value is taken for the overlapping area of ​​multiple interferometric points to obtain the final flattening weight of each control point.

[0016] The final flattened weights are negatively correlated with the preset reference weights, and the minimum envelope value is taken for the overlapping area of ​​multiple interference points to obtain the final reference weight of each control point.

[0017] A joint objective function is constructed, which includes curvature smoothing cost, obstacle avoidance cost, and reference deviation cost. The obstacle avoidance cost is weighted by the final flattening weight, and the reference deviation cost is weighted by the final reference weight. The numerical optimization solver is called to perform iterative solution and update the trajectory control points.

[0018] Repeat the above steps until the collision-free condition is met or the maximum number of iterations is reached, and output the final non-uniform B-spline trajectory.

[0019] Beneficial effects: By accurately modeling a multi-circle-enclosed rectangular frame and coupling it with Gaussian flexible curvature flattening depth, and by introducing a negative correlation dynamic weight relaxation mechanism, this invention can efficiently generate collision-free, high-order smooth and dynamically feasible driving trajectories in unstructured confined spaces. Attached Figure Description

[0020] Figure 1 This is a flowchart of the overall solution of the present invention.

[0021] Figure 2 This is a flowchart of the process of obtaining the vehicle geometric model according to the present invention.

[0022] Figure 3 This is a flowchart illustrating the initial nonlinear programming problem constructed according to the present invention.

[0023] Figure 4 This is a flowchart illustrating the process of generating the set of interference control point indices according to the present invention.

[0024] Figure 5 This is a flowchart illustrating how the final flattened weights of each control point are obtained according to the present invention.

[0025] Figure 6 This is a flowchart illustrating how the final reference weights for each control point are obtained in this invention.

[0026] Figure 7 This is a flowchart of the final non-uniform B-spline trajectory output by the present invention. Detailed Implementation

[0027] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0028] like Figures 1-7 As shown, this embodiment describes in detail the data processing flow of a constrained space trajectory planning method considering vehicle volume, specifically including:

[0029] S1: Acquire environmental perception data and vehicle status, construct a two-dimensional occupancy grid map, and use a multi-circle bounding rectangle model to parametrically represent the vehicle body to obtain the vehicle geometric model.

[0030] In this embodiment, environmental perception and vehicle physical modeling operations are performed first to establish the basic data environment required for subsequent trajectory planning.

[0031] Specifically, the onboard computing unit acquires 3D point cloud data output by the LiDAR in real time. The acquired point cloud data is filtered to remove outliers and noise, and ground point cloud segmentation is performed to remove ground point clouds that do not belong to obstacles. Further, the preprocessed 3D point cloud data is projected onto a 2D plane to construct a 2D occupancy grid map. In the 2D occupancy grid map, each grid cell is assigned one of two states: occupied, indicating the presence of an obstacle within the grid; or vacant, indicating that the grid is a passable area. Preferably, to balance computational accuracy and real-time requirements, the resolution of the grid cells can be set between 0.1 meters and 0.2 meters.

[0032] Simultaneously, the physical dimensional parameters of the vehicle itself are collected. Specifically, the accurate length and width values ​​of the vehicle are obtained. According to one aspect of this application, a multi-circle bounding rectangular frame model is used to parametrically represent the vehicle body. For example, this model uses three equally sized disks, corresponding to the front, middle, and rear of the vehicle, respectively. The disk radius is calculated as half the vehicle width to ensure that the disk diameter completely covers the width of the vehicle body. The longitudinal distance between the centers of adjacent disks is calculated based on the vehicle length and the determined disk radii, so that the three disks are arranged sequentially in the longitudinal direction, and their envelope contour exactly covers the entire length of the rectangular vehicle body. The calculated disk radii and center-to-center distances are used as model parameters to form a complete vehicle geometric model.

[0033] S2: Collect discrete reference trajectories, parameterize them using non-uniform B-spline curves, and construct an initial nonlinear programming problem.

[0034] In this embodiment, a discrete reference trajectory point sequence output by the global planning module is obtained. A non-uniform B-spline curve is used to fit the discrete reference trajectory point sequence to obtain a continuous parameterized trajectory defined by a series of control points. To maintain the pose continuity at the trajectory's start and end points, the coordinates of the first three and last three control points in the control point sequence are fixed and not included in subsequent optimization. The coordinates of the remaining internal control points are marked as decision variables to be optimized, forming the initial set of optimized control points.

[0035] Furthermore, to associate the trajectory with vehicle dynamics, a time parameter pre-assignment operation is performed. Specifically, based on the total length of the original reference trajectory and the preset expected average vehicle speed, the time step between adjacent control points is calculated. In this scheme, the time step remains constant throughout all subsequent iterations of optimization, thus simplifying the complex spatiotemporal joint optimization problem into a pure spatial domain optimization problem. Finally, based on the time step and the derivative characteristics of the non-uniform B-spline curve, the velocity, acceleration, and lateral acceleration at each point on the trajectory are calculated, and vehicle physical limit constraints are applied, forming a complete description of the initial nonlinear programming problem.

[0036] S3: Calculate the minimum distance from the multi-circle bounding rectangle model to the occupied grid map point by point along the current trajectory, compare it with the safety threshold, mark the interference control points, and generate an interference control point index set.

[0037] After completing the initial optimization problem framework, a collision detection operation is performed on the current trajectory to accurately locate trajectory segments with collision risks and their corresponding control points.

[0038] In this embodiment, the set of control points for the current iteration is obtained. Based on the set of control points, a complete continuous and smooth trajectory is reconstructed using the basis function expression of the non-uniform B-spline curve. For discretized collision detection, the trajectory needs to be discretized according to a certain sampling strategy. Preferably, an adaptive step-size sampling strategy can be adopted: a smaller sampling step size is used in areas with large curvature changes, and a larger sampling step size is used in areas with gentle curvature changes. The specific value of the sampling step size can be dynamically adjusted according to the vehicle wheelbase or a preset curvature threshold to balance detection accuracy and computational efficiency.

[0039] For each trajectory sampling point, it is necessary to determine the precise spatial occupancy of the vehicle at that point. Specifically, based on the vehicle's orientation angle at the trajectory sampling point, and combined with the parameters of the constructed multi-circle bounding rectangle model (i.e., the radius of the circles and the distance between their centers), the coordinates of the centers of the front, middle, and rear circles in the global coordinate system are calculated respectively.

[0040] After obtaining the center coordinates of the three disks, distance queries are performed between them and a two-dimensional occupancy grid map. For each center, the nearest grid cell marked as "occupied" is searched in the occupancy grid map, and the Euclidean distance between the center and the nearest occupied grid cell is calculated. The query operation is performed on the front, middle, and rear disks respectively, obtaining three distance values. The minimum value among them is taken as the minimum safe distance for the trajectory sampling point.

[0041] The calculated minimum safe distance is then compared with a pre-set collision warning threshold. For example, the collision warning threshold can be set to a safety margin slightly larger than the disk radius, such as the disk radius plus 0.1 meters. If the minimum safe distance is less than the collision warning threshold, the trajectory sampling point is determined to be a collision interference point.

[0042] Finally, a mapping relationship from trajectory sampling points to control points is established. For each trajectory sampling point marked as a collision interference point, the local support properties of the non-uniform B-spline curve are used to determine several control points that contribute most to the shape of the sampling point (usually several control points near the node interval where the sampling point is located; for example, in a cubic B-spline curve, each sampling point is affected by at most four control points). The indices of these control points are collected, and after removing duplicates, an interference control point index set is formed. This set accurately identifies the local regions in the current trajectory that need to be corrected, laying the foundation for subsequent targeted optimization.

[0043] S4: Based on the set of interferometric control point indices, Gaussian distribution curvature flattening weights are generated with the interferometric control points as the center, and the maximum envelope value is taken for the overlapping area of ​​multiple interferometric points to obtain the final flattening weight of each control point.

[0044] After locating the set of interference control point indices, this scheme further performs the calculation of curvature flattening weights: generating a continuous and smooth thrust field near the interference control points to guide the trajectory to smoothly deviate from the obstacle, rather than producing abrupt geometric angles.

[0045] According to one aspect of this application, for each interferometric control point index in the set of interferometric control point indices, a one-dimensional Gaussian distribution function is constructed centered on that index. The Gaussian distribution function is used to calculate the flattened weight contribution value of that interferometric control point to any control point index on the trajectory.

[0046] Specifically, the Gaussian distribution function is expressed as follows: the weight contribution value equals the preset flattening penalty peak multiplied by an exponential function with the natural constant as the base, where the exponent of the exponential function is negative (the square of the index difference) divided by (the square of twice the standard deviation of the Gaussian distribution). Here, the index difference refers to the algebraic difference between the index of the interfering control point and the index of the control point to be calculated. The standard deviation of the Gaussian distribution is an adjustable parameter used to control the width of the flattening influence range. The larger the standard deviation, the wider the range of neighboring control points that a single interfering control point can influence, and the softer the flattening effect; the smaller the standard deviation, the more confined the influence range, and the more concentrated the flattening effect.

[0047] In a preferred embodiment, for each interferometric control point, its influence range is limited to a specific window centered on that control point. For example, the window radius can be set to three times the standard deviation, and the weight contribution received by control points outside the window can be approximated as zero, thereby reducing unnecessary computation.

[0048] Since a typical control point may fall within the influence range of multiple interfering control points simultaneously, it is necessary to address the issue of multi-source weight superposition. In this scheme, an envelope maximum value strategy is employed for weight fusion. Specifically, for each control point index on the trajectory, all interfering points in the interfering control point index set are traversed, and the flattened weight value contributing to that control point by each interfering point is calculated. Then, the maximum value is selected as the final flattened weight for that control point.

[0049] A control point should be assigned a high flat weight if it is affected by any strong interfering point, rather than being diluted by averaging due to its distance from multiple interfering points. Through traversal and maximum value operations, a final flat weight vector with a length equal to the total number of control points is generated. In this vector, control points near the interfering region have higher weight values, while the weight values ​​of control points far from the interfering region approach zero, forming a continuous and smooth weight field distribution.

[0050] S5: The final flattened weights are negatively correlated with the preset reference weights, and the minimum envelope value is taken for the overlapping area of ​​multiple interference points to obtain the final reference weight of each control point.

[0051] After obtaining the final flattened weights for each control point, this scheme further performs a dynamic relaxation operation on the reference trajectory similarity weights. This operation aims to break the inherent contradiction between obstacle avoidance thrust and path-keeping pull in traditional trajectory optimization, giving the vehicle the flexibility to "borrow" obstacles in narrow spaces.

[0052] Specifically, a preset baseline reference weight is first obtained. The baseline reference weight represents the constraint strength under ideal, obstacle-free environmental conditions, which aims to make the optimized trajectory as close as possible to the original reference path. Typically, this value is set to a high positive number to ensure that the trajectory maintains good path-following performance in open areas.

[0053] According to one of the core improvements in this embodiment, the final reference weight of each control point is designed to be negatively correlated with its final flattening weight. In other words, the reference weight of a control point with a higher flattening weight (i.e., a point in a high-risk collision zone) should be reduced; the reference weight of a control point with a lower flattening weight (i.e., a point in a safe zone) should be restored to or maintained at a higher level.

[0054] In one optional implementation, the specific calculation method for negative correlation coupling is as follows: For each control point index, firstly, the ratio of its final flattening weight to the peak value of the flattening penalty is calculated. This ratio reflects the relative intensity of the flattening thrust experienced by the control point. Then, this ratio is subtracted from the value 1 to obtain a relaxation coefficient. The relaxation coefficient ranges from 0 to 1: when the flattening weight reaches its peak value, the relaxation coefficient approaches 0; when the flattening weight is 0, the relaxation coefficient equals 1. Finally, the baseline reference weight is multiplied by this relaxation coefficient to obtain the relaxed reference weight of the control point.

[0055] Furthermore, this step employs an envelope minimum strategy when handling the cumulative effects of multiple interference points. This is because the relaxation of reference weights exhibits the "barrel effect": if a control point is affected by any one interference point and needs to be relaxed, its reference constraints should be fully released. Therefore, for control point indices that may be jointly affected by multiple interference control points, the relaxed reference weights under the independent action of each interference control point are calculated separately, and then the minimum value is selected as the final reference weight for that control point.

[0056] Through the above operations, a final reference weight vector is generated. This vector complements the generated final flattened weight vector: in spatial distribution, the reference weight automatically decreases in areas with higher flattened weights; in areas with zero flattened weights, the reference weight is restored to its nominal value. This dynamic relaxation mechanism does not require manually setting trigger thresholds or switching rules, but is naturally implemented at the optimization level through weight coupling, thereby effectively avoiding the deadlock problem in multi-objective optimization.

[0057] S6: Construct a joint objective function that includes curvature smoothing cost, obstacle avoidance cost, and reference deviation cost, where the obstacle avoidance cost is weighted by the final flattening weight and the reference deviation cost is weighted by the final reference weight. Call the numerical optimization solver to perform iterative solution and update the trajectory control points.

[0058] After completing the calculation and allocation of the flattening weights and reference weights, this scheme performs the core numerical optimization solution operation to update the trajectory control points.

[0059] According to one aspect of this application, a joint objective function is first constructed. The joint objective function is a weighted sum of three sub-cost terms: curvature smoothing cost, obstacle avoidance cost, and reference path deviation cost. Each sub-cost term has a clear physical meaning.

[0060] Specifically, curvature smoothing cost measures the degree of curvature of the trajectory and its rate of change. This cost term is calculated by integrating the second (curvature) or third (rate of curvature change, i.e., jerk) derivative of the non-uniform B-spline curve. A lower smoothing cost corresponds to a smoother trajectory with greater passenger comfort.

[0061] Obstacle avoidance cost measures the proximity of a trajectory to surrounding obstacles. In this scheme, the calculation of this cost term incorporates the final flattened weight vector. Specifically, for each control point, its distance to the nearest obstacle is calculated based on a multi-circle bounding rectangle model, and a penalty function (e.g., a reciprocal function or a quadratic penalty function) is constructed that increases sharply as the distance decreases. The penalty function value of the control point is then multiplied by the corresponding flattened weight to obtain the weighted obstacle avoidance cost for that control point. The summation of the weighted obstacle avoidance costs for all control points yields the overall obstacle avoidance cost. Control points with higher flattened weights contribute a larger obstacle avoidance cost, thus driving the solver to prioritize pushing these control points away from obstacles.

[0062] The reference path deviation cost measures the deviation between the optimized trajectory and the original reference trajectory. This cost term is calculated by incorporating the final reference weight vector. Specifically, for each control point, the squared Euclidean distance between its current coordinates and the corresponding coordinates on the original reference trajectory is calculated, and then multiplied by the corresponding reference weight to obtain the weighted deviation cost for that control point. Control points with high reference weights (i.e., control points in safe areas) are subject to stronger path-keeping constraints; control points with low reference weights (i.e., control points in interference areas) are allowed to deviate from the original path in exchange for obstacle avoidance space.

[0063] The total cost function is obtained by multiplying each of the three cost terms by its respective overall weight coefficient and then summing the results. The overall weight coefficient can be calibrated according to the specific application scenario. For example, the weight of obstacle avoidance cost can be appropriately increased in extremely confined environments, and the weight of smoothing cost can be appropriately increased in high-speed driving scenarios.

[0064] Furthermore, the automatic differentiation framework is invoked to perform analytical gradient calculations on the total cost function and various constraints (endpoint constraints, velocity constraints, acceleration constraints, etc.). Then, the interior-point solver is invoked to perform an iterative search based on the gradient information to find the control point locations that minimize the total cost and satisfy all constraints. After the solver converges, it outputs the updated set of control points.

[0065] In a preferred embodiment, to prevent the solution process from getting stuck in local optima, this scheme also introduces an adaptive penalty multiplication mechanism. Specifically, after an inner-layer optimization is completed, the optimization results are checked: if the trajectory corresponding to the updated set of control points still has collision interference points, and the changes in all control points are less than a preset small threshold (indicating that the solver can no longer improve), then it is determined that the current optimization has gotten stuck in a local optimum. In this case, the overall weight coefficient of obstacle avoidance cost is automatically multiplied by a factor greater than 1 (e.g., 1.5 or 2.0), thereby artificially enhancing the dominance of obstacle avoidance thrust. Subsequently, the process returns to step S2, and collision detection, weight allocation, and numerical optimization are re-executed using the enhanced weight coefficient, guiding the solver to escape the current local optimum.

[0066] S7: Repeat the above steps until the collision-free condition is met or the maximum number of iterations is reached, and output the final non-uniform B-spline trajectory.

[0067] Specifically, the execution flow of each outer loop is as follows: First, based on the current set of control points, step S2 is executed to reconstruct the trajectory and perform collision detection, generating an interferometric control point index set; then, steps S3 and S4 are executed to update the flattened weight vector and reference weight vector according to the interferometric point index; next, step S5 is executed to construct the total cost function for the current iteration and call the solver to update the control point set; after one loop ends, the updated control point set is used as the input for the next loop.

[0068] At the end of each iteration, it is determined whether the loop termination condition is met. If not, the next iteration continues; if so, the loop exits and proceeds to step S7. This two-layer loop architecture has significant advantages: the outer loop dynamically adjusts the optimization weights based on environmental perception results, reflecting the real-time interaction between the vehicle and the environment; the inner loop solves for the optimal control point position under given weights, reflecting the precise search capability of the mathematical optimization algorithm. The two loops are nested and work together, allowing the trajectory to gradually approach the collision-free, highly smooth optimal solution in each iteration.

[0069] In each outer loop, in addition to performing collision detection and weight updates, it is also necessary to simultaneously perform the operation of judging the iteration termination condition.

[0070] Specifically, after completing the collision detection operation at the beginning or end of a loop, the minimum safe distance of all sampling points on the current trajectory is obtained. This minimum safe distance is compared to a preset absolute safety threshold. The absolute safety threshold is typically set to a value greater than the collision warning threshold to ensure that the final output trajectory not only has no collisions but also retains sufficient safety margin. For example, the absolute safety threshold can be set to the disk radius plus 0.3 meters. If the minimum safe distance of all sampling points is greater than the absolute safety threshold, the current trajectory is determined to meet the collision-free condition.

[0071] Simultaneously, the number of outer loop iterations executed is accumulated and compared with the preset maximum number of iterations. The maximum number of iterations is an empirical parameter that can be set according to the computing power of the onboard computing unit and the real-time requirements of the plan; for example, it can be set to 30 or 50 iterations.

[0072] In one alternative implementation, the termination condition uses an "OR" logic: the iteration terminates as long as either of the two conditions is met. Specifically, if the collision-free condition is met, it indicates that the algorithm has successfully found a safe and feasible trajectory, and the loop exits in a successful state. If the maximum number of iterations is reached but the collision-free condition is not met, it indicates that a safe path cannot be found within the limited computing resources, and the loop exits in a failed state.

[0073] Based on the termination status determination, different subsequent operations are executed. If the loop exits successfully, the latest non-uniform B-spline control point set and the pre-allocated and unchanged time step parameters are packaged to generate a complete trajectory command that can be directly sent to the vehicle's underlying actuators. The trajectory command typically includes information such as time sequence, position sequence, velocity sequence, and acceleration sequence.

[0074] If the loop exits in a failed state, a planning failure warning signal is output. According to a further improvement of this embodiment, a preset degradation strategy can be triggered simultaneously with the output of the planning failure signal. The degradation strategy includes, but is not limited to: controlling the vehicle to perform an emergency braking operation to bring the vehicle to a complete stop within a safe distance; or sending a request for assistance to a remote monitoring center, requesting remote manual intervention or issuing an alternative driving plan. Through the above failure handling mechanism, the safety and controllability of the autonomous driving system are ensured in extreme scenarios.

[0075] In summary, the confined space trajectory planning method considering vehicle volume provided by this invention organically integrates precise modeling of multi-circle enclosed rectangular frames, Gaussian distribution flexible curvature flattening, negative correlation weight dynamic relaxation, and inner and outer double-layer iterative optimization architecture. This method can efficiently generate collision-free, high-order smooth, and dynamically feasible vehicle driving trajectories in unstructured confined spaces.

[0076] According to another aspect of this application, a vehicle geometric model is obtained, comprising:

[0077] S11: Collect the vehicle's length and width physical parameters.

[0078] In this embodiment, to construct an accurate multi-circle bounding rectangle model, it is first necessary to obtain the intrinsic physical dimension parameters of the vehicle to be planned.

[0079] Specifically, the onboard computing unit reads parameter information stored in the vehicle's electronic control unit via the controller area network bus. This parameter information includes the vehicle's body geometry data calibrated at the factory. Preferably, the vehicle's outline can also be scanned in real time using onboard sensors (such as ultrasonic radar or lidar) to verify or supplement the parameter values ​​read from the electronic control unit, ensuring that the modeling data matches the actual vehicle state.

[0080] Furthermore, two key values ​​are extracted from the acquired parameter information: vehicle length and vehicle width. Vehicle length is defined as the horizontal distance from the foremost point of the front bumper to the rearmost point of the rear bumper; vehicle width is defined as the horizontal distance between the outermost edges of both sides of the vehicle body (excluding the rearview mirrors). These two parameters are the basic inputs for subsequent model calculations.

[0081] In one alternative implementation, considering that vehicle suspension compression under different load conditions may cause changes in vehicle height, but length and width remain basically constant, this solution only needs to collect the above two parameters once, and there is no need to update them repeatedly in the subsequent planning process.

[0082] S12: Calculate the radius of a single disk based on the vehicle width, so that the disk diameter is equal to the vehicle width.

[0083] After obtaining the vehicle width parameters, this step performs the calculation of the radius of a single disk in the multi-circle bounding rectangle model.

[0084] According to one aspect of this application, to ensure the complete coverage of the vehicle geometry model, a rectangular vehicle body is represented by three equally sized disks arranged sequentially. The diameter of each disk is set to be equal to the width of the vehicle. The reason for this design is that when the diameter of the disk is equal to the width of the vehicle, a single disk can completely cover the transverse section of the vehicle; after the three disks are arranged sequentially in the longitudinal direction, their overall envelope contour can precisely cover the entire rectangular vehicle body area.

[0085] Based on the above design principles, the formula for calculating the disk radius is: the disk radius is equal to half the vehicle width. For example, if the vehicle width is 1.8 meters, the disk radius is calculated to be 0.9 meters. This value ensures that the lateral coverage of the disk is strictly consistent with the vehicle width, eliminating both uncovered blind spots (caused by an excessively small radius) and unnecessary conservatism due to excessive redundancy (caused by an excessively large radius).

[0086] Furthermore, the calculated disk radius will be stored as one of the core parameters of the multi-circle bounding rectangle model, and used for the calculation of the circle center coordinates, collision distance detection, and construction of the obstacle avoidance cost function in subsequent steps.

[0087] S13: Calculate the longitudinal spacing between the centers of adjacent disks based on the vehicle length and disk radius, so that the front, middle and rear disks are arranged in sequence and completely cover the rectangular outline of the vehicle.

[0088] After determining the disk radius, this step further calculates the longitudinal spacing between the centers of adjacent disks to achieve complete coverage of the vehicle's entire length by the three disks.

[0089] Specifically, three discs are deployed at the front, middle, and rear of the vehicle, respectively. The center of the front disc is located at the center of the front of the vehicle, the center of the rear disc at the center of the rear, and the center of the middle disc at the midpoint of the line connecting the front and rear centers. To ensure that the overall envelope of the three discs precisely covers the entire length of the vehicle, the following geometric relationship must be satisfied: the vehicle length equals the radius of the front disc, the radius of the rear disc, and the sum of the two longitudinal spacings. Since the radii of the front and rear discs are equal and equal to the disc radius, we can conclude that the vehicle length equals twice the disc radius plus the sum of the two longitudinal spacings.

[0090] According to the calculation method of this embodiment, the longitudinal spacing between the centers of adjacent discs is determined as follows: the longitudinal spacing is equal to (vehicle length minus twice the disc radius) divided by two. This calculation result ensures that the front edge of the front disc is flush with the front of the vehicle, the rear edge of the rear disc is flush with the rear of the vehicle, and the middle disc overlaps with both the front and rear discs, thus forming a seamless continuous coverage.

[0091] In a preferred embodiment, because the front, middle, and rear disks overlap, the overlapping area can transition smoothly when the vehicle turns, avoiding collision detection blind spots caused by a single disk model at vehicle corners. This longitudinal spacing parameter is also stored as one of the core parameters of the multi-circle bounding rectangle model.

[0092] S14: Use the disk radius and longitudinal spacing as parameters for the multi-circle bounding rectangle model to generate the vehicle geometry model.

[0093] This step integrates and encapsulates the obtained parameters to generate a complete vehicle geometry model that can be used for subsequent collision detection.

[0094] Specifically, a data structure is created that contains the following fields: model type identifier, marked as a multi-circle enclosed rectangle model; number of disks, a fixed value of three; disk radius, taken from the calculation result of step S12; longitudinal distance between the centers of adjacent disks, taken from the calculation result of step S13; and optional auxiliary parameters, such as the offset of the disk center relative to the rear axle center or the geometric center of the vehicle.

[0095] Furthermore, the vehicle geometry model also needs to define a calculation method: given any vehicle pose (i.e., the global coordinates of the vehicle reference point and the vehicle orientation angle), it should be able to output the global coordinates of the centers of the three disks. For example, if the vehicle reference point is set as the rear axle center, the local coordinates of the centers of the front, middle, and rear disks relative to the reference point can be pre-calculated and stored. When it is necessary to query the coordinates of the disk centers at a certain trajectory sampling point, the local coordinates can simply be transformed to the global coordinate system through rotation and translation transformations.

[0096] In this embodiment, the generated vehicle geometry model will be stored in the memory of the onboard computing unit and reused throughout the trajectory planning lifecycle without needing to be rebuilt. This model provides an accurate geometric representation basis for calculating the minimum safe distance point by point along the trajectory in subsequent steps.

[0097] According to another aspect of this application, an initial nonlinear programming problem is constructed, including:

[0098] S21: Fit the discrete reference trajectory points into a non-uniform B-spline curve to obtain the control point sequence.

[0099] After completing the vehicle geometry modeling, this step begins to parametrically represent the reference trajectory.

[0100] Specifically, the discrete reference trajectory point sequence output by the global planning module is obtained. This sequence is typically sampled at equal or adaptive intervals, with each point containing position coordinates and optionally orientation angle information. These discrete points only represent the approximate direction of the path, lacking curvature continuity and parameterized representation, and cannot be directly used for optimization solutions under dynamic constraints.

[0101] According to one aspect of this application, a third-order (i.e., cubic) non-uniform B-spline curve is used to fit a discrete reference trajectory point sequence. The fitting process is as follows: taking the discrete points as the approximation target, a set of control points is solved such that the B-spline curve defined by these control points is closest to the original discrete points in the least squares sense. In specific implementation, firstly, the node vectors are determined based on the discrete point sequence, and the values ​​of the node vectors can be obtained using the chord length parameterization method or the centripetal parameterization method; then, a system of linear equations is constructed, where the unknowns are the coordinates of the control points; finally, the control point sequence is obtained by solving this system of linear equations.

[0102] For example, if there are M discrete reference trajectory points, the number of control points obtained after fitting is typically M plus 2 (for cubic B-spline curves). The first and last control points in the control point sequence usually coincide with or are very close to the start and end points of the discrete reference trajectory.

[0103] S22: The coordinates of the first three control points and the last three control points in the fixed control point sequence are used as the coordinates of the remaining internal control points as the decision variables to be optimized, thus obtaining the initial set of optimized control points.

[0104] In order to maintain the pose continuity of the trajectory start and end points during the optimization process, this step applies endpoint fixed constraints to the control point sequence.

[0105] Specifically, obtain the generated complete sequence of control points, denoted as P0, P1, P2, ..., Pn, where n+1 is the total number of control points. Lock the coordinates of the first three control points P0, P1, and P2 in the sequence, preventing subsequent optimization operations from modifying them. Similarly, lock the coordinates of the last three control points P(n-2), P(n-1), and Pn. The locking operation is implemented by passing the coordinates of these control points as constant parameters when constructing the nonlinear programming problem, rather than as decision variables.

[0106] Based on the local support properties of non-uniform B-spline curves, the starting position and tangent direction of the curve are jointly determined by P0, P1, and P2, while the ending position and tangent direction are jointly determined by P(n-2), P(n-1), and Pn. Fixing three control points at the beginning and end ensures that the trajectory before and after optimization has the same pose (position and orientation) at the starting and ending points, thus ensuring a smooth connection with the global path.

[0107] Furthermore, in addition to the six fixed control points mentioned above, the coordinates of the remaining internal control points P3, P4, ..., P(n-3) are labeled as decision variables to be optimized. The initial values ​​of these decision variables are set to the fitted original coordinate values, forming an initial set of optimized control points. This set will be iteratively updated during subsequent numerical optimization.

[0108] S23: Based on the total length of the original reference path and the preset average vehicle speed, pre-allocate the time step between adjacent control points and keep the time step unchanged in subsequent optimizations.

[0109] In order to transform a pure geometric trajectory into a driving trajectory with time attributes, this step performs a time parameter pre-assignment operation.

[0110] Specifically, the total length of the original reference path is first calculated. The original reference path is formed by connecting discrete reference trajectory points sequentially, and the total length can be obtained by accumulating the Euclidean distances between adjacent points. Secondly, a desired average vehicle speed is preset according to the scenario requirements. For example, in a narrow park environment, the average vehicle speed can be set between 2 m / s and 5 m / s; in extreme environments such as parking lots, it can be set between 1 m / s and 2 m / s.

[0111] For any two adjacent control points P in the control point sequence i and P i+1 Calculate the Euclidean distance between them. Then, divide this distance by the preset average vehicle speed to obtain the time step of the adjacent control point interval. Perform the above calculation for each adjacent control point interval to obtain a set of time step sequences delta. t0 delta t1 , …, delta t(n-1) .

[0112] According to a key constraint in this embodiment, the time step sequence remains constant throughout all iterations of the trajectory optimization. This design choice reflects profound engineering considerations: if the time step also participates in the optimization, the problem becomes a joint spatiotemporal optimization, doubling the number of variables and drastically increasing the difficulty of solving, making it difficult to meet real-time requirements. By freezing the time step, the problem is simplified to a pure spatial domain optimization, reducing computational complexity while ensuring dynamic feasibility.

[0113] S24: Based on the time step and the derivative characteristics of the B-spline curve, calculate the velocity, acceleration, and lateral acceleration at the trajectory points, and set the corresponding vehicle physical limit constraints to form the initial nonlinear programming problem.

[0114] After obtaining a fixed time step, this step constructs the dynamic constraints of the trajectory.

[0115] Specifically, non-uniform B-spline curves possess excellent derivative computability properties. Given a set of control points and a fixed time step, the position of any point on the curve can be obtained using the de Boer recursive formula. The velocity vector at that point is equal to the first derivative of the position with respect to time, and the acceleration vector is equal to the second derivative. Since the time step is known, these derivatives can be efficiently calculated using the difference operations of the control points.

[0116] Furthermore, the magnitude of velocity (i.e., speed) can be extracted from the velocity vector; the magnitude and direction of acceleration can be extracted from the acceleration vector. Lateral acceleration (also known as side acceleration) refers to the component of the acceleration vector perpendicular to the velocity direction. Its magnitude reflects the degree of lateral force experienced by the vehicle during cornering and is closely related to ride comfort and driving safety.

[0117] Based on the vehicle's physical limits, the following constraints are set: the speed should be between the minimum and maximum speeds; the acceleration should be between the maximum deceleration and maximum acceleration; and the lateral acceleration should be less than a preset safety threshold (for example, a comfort threshold of 2 m / s² and a safety threshold of 3 m / s²). These constraints are added to the nonlinear programming problem in the form of inequalities.

[0118] In addition, endpoint equality constraints are included. Combining all constraints with the cost function to be constructed subsequently forms a complete description of the initial nonlinear programming problem. This problem uses the set of control points to be optimized as decision variables, dynamic constraints and safety constraints as the feasible region boundaries, and the cost function to be minimized as the optimization objective.

[0119] According to another aspect of this application, an interferometric control point index set is generated, including:

[0120] S31: In the current iteration, reconstruct the non-uniform B-spline trajectory based on the current set of control points, and perform discrete sampling of the trajectory with a fixed step size or an adaptive step size to obtain the trajectory sampling point sequence.

[0121] At the start of each outer loop, this step reconstructs the trajectory based on the latest set of control points and performs discrete sampling.

[0122] Specifically, the set of control points for the current iteration is obtained. If it's the first iteration, the initial optimized control point set is used; if it's the k-th iteration, the control point set updated by the previous solver is used. Using the basis function expression of the non-uniform B-spline curve, combined with a fixed node vector, a continuous and smooth trajectory curve is reconstructed.

[0123] To perform collision detection on a trajectory, the continuous trajectory needs to be discretized into a series of sampling points. In one alternative implementation, a fixed-step sampling strategy is used: sampling is performed at constant intervals (e.g., 0.2 meters or 0.5 meters) along the trajectory's arc length. This strategy is simple to implement and has stable computational overhead.

[0124] In another preferred embodiment, an adaptive step-size sampling strategy is employed: first, the curvature distribution of the trajectory is calculated; a smaller sampling step size is used in areas with greater curvature (i.e., sharper turns) to ensure that the collision risk caused by the vehicle's outward swing can be captured; a larger sampling step size is used in areas with less curvature (i.e., approximately straight travel) to reduce computational load. The adaptive step size can be dynamically adjusted according to a curvature threshold. Through the above sampling operations, an ordered sequence of trajectory sampling points is generated, with each sampling point containing position coordinates and orientation angle information.

[0125] S32: For each trajectory sampling point, calculate the global coordinates of the centers of the front, middle, and rear disks in the multi-circle bounding rectangle model based on the vehicle's orientation angle.

[0126] After obtaining the sequence of trajectory sampling points, this step maps the vehicle geometry model to each sampling point.

[0127] Specifically, for any trajectory sampling point, the position of the vehicle reference point and the vehicle orientation angle at that point are known. The vehicle reference point is usually set as the rear axle center, but can also be set to other reference positions depending on the actual vehicle configuration. From the generated vehicle geometry model, the local coordinate offsets of the centers of the three disks relative to the reference point are read.

[0128] According to the calculation method of this embodiment, the coordinates of the center of the front disc in the vehicle's local coordinate system can be pre-calculated as (longitudinal offset, 0), where the longitudinal offset is equal to (half the vehicle length minus the disc radius) or specifically calibrated according to the vehicle structure. The local coordinates of the center of the middle disc are (0, 0) or set according to actual conditions. The local coordinates of the center of the rear disc are (negative longitudinal offset, 0). These local coordinates are fixed constants.

[0129] Then, the local coordinates are transformed to the global coordinate system through a two-dimensional coordinate transformation: the global coordinates are equal to the reference point coordinates plus the rotation matrix multiplied by the local coordinates. The rotation matrix consists of the sine and cosine values ​​of the vehicle's orientation angle. The transformation is performed on the front, middle, and rear disks respectively to obtain the global coordinate values ​​of the centers of the three disks.

[0130] For example, if the vehicle's facing angle is θ and the rotation matrix is ​​[[cos(θ), -sin(θ)], [sin(theta),cos(tθ)]], then multiplying the local coordinates by this matrix and adding it to the reference point coordinates yields the global coordinates.

[0131] S33: In the two-dimensional occupancy grid map, query the distance from the center of each circle to the nearest occupied grid cell, and take the minimum value among the three distances as the minimum safe distance for the trajectory sampling point.

[0132] After calculating the global coordinates of the centers of the three disks, this step performs the actual distance query operation.

[0133] Specifically, the center coordinates of the three disks (front, middle, and rear) are input into a constructed 2D occupancy grid map for querying. For each center coordinate, its corresponding grid index is first determined (i.e., obtained by dividing the coordinate by the grid resolution and rounding). Then, using this grid as the center, a breadth-first search or a pre-calculated distance transformation method is used to find the nearest grid cell marked as "occupied". The Euclidean distance between the center of the disk and the center of this occupied grid cell is calculated.

[0134] In a preferred embodiment, to improve query efficiency, a distance transformation algorithm can be pre-executed on the occupied grid map. This algorithm pre-calculates the distance to the nearest obstacle for each grid cell in the map. Thus, for any given circle center, only the distance transformation value of its corresponding grid cell needs to be read, eliminating the need for real-time searching and improving collision detection speed.

[0135] Let d be the distance value obtained from each of the three circle centers. front d mid d rear The minimum of these three values ​​is taken as the minimum safe distance for the trajectory sampling point. The reason for taking the minimum value is that if any disc (i.e., any part of the vehicle body) gets too close to the obstacle, the entire vehicle is at risk of collision. Therefore, the most dangerous part determines the overall safety status.

[0136] S34: Compare the minimum safe distance with the preset collision warning threshold. If it is less than the threshold, then determine that the trajectory sampling point is a collision interference point.

[0137] After calculating the minimum safe distance between the trajectory sampling points, this step compares it with a preset collision warning threshold.

[0138] According to one aspect of this application, the collision warning threshold is a safety margin value greater than or equal to the radius of the disk. Specifically, the threshold can be set as the radius of the disk plus a positive value δ. The value of δ can be set according to the scenario requirements: in a conservative scenario, δ can be 0.2 meters to 0.3 meters to ensure greater safety redundancy; in an extremely narrow passage scenario, δ can be 0.05 meters to 0.1 meters to allow the vehicle to pass closer to the obstacle.

[0139] Perform a comparison operation: if the minimum safe distance is less than the collision warning threshold, the trajectory sampling point is determined to be in the collision danger zone and marked as a collision interference point; otherwise, if the minimum safe distance is greater than or equal to the collision warning threshold, the point is determined to be a safe point and no further processing is required.

[0140] All sampling points marked as collision interference points are collected to form a list of collision interference points. This list identifies the locations of segments on the current trajectory that are too close to obstacles.

[0141] S35: Find the non-uniform B-spline control point index corresponding to the collision interference point, and add all the marked control point indices to the interference control point index set.

[0142] After identifying the collision interference points, this step maps the interference information at the sampling point level to the control point level.

[0143] Specifically, for each trajectory sampling point marked as a collision interference point, it is necessary to determine the control point in the non-uniform B-spline curve that contributes the most to the shape of that sampling point. Due to the local support properties of the non-uniform B-spline curve, the position of a sampling point is determined only by the nearest control points. For a third-order (cubic) B-spline curve, each sampling point is influenced by at most four consecutive control points.

[0144] By using the node vector and the curve parameter values ​​corresponding to the sampling point, the range of control point indices affecting that sampling point can be found. For example, if the parameter corresponding to the sampling point is u, and it is located within the k-th node interval, then the control point indices affecting that point are k-2, k-1, k, k+1 (after appropriate boundary processing).

[0145] These control point indices are collected and added to a set data structure. This process is repeated for each collision interference point. Since different sampling points may map to the same control point index, the set data structure automatically removes duplicates, ultimately generating a unique set of interference control point indices. This set precisely points to the control points corresponding to the trajectory segments that need correction, providing spatial localization for subsequent weight allocation and targeted optimization.

[0146] According to another aspect of this application, the final flattening weight of each control point is obtained, including:

[0147] S41: For each interferometric control point index in the set of interferometric control point indices, construct a one-dimensional Gaussian distribution function centered on it, and calculate its flattened weight contribution value to all control point indices on the trajectory.

[0148] After obtaining the set of interferometric control point indices, this step begins to construct the curvature flattened weight field of the Gaussian distribution.

[0149] Specifically, let the index of an interferometric control point in the set of interferometric control point indices be m. Define a one-dimensional Gaussian distribution function centered at m, with the independent variable being any control point index i. The function value represents the flattened weighted contribution of interferometric point m to control point i.

[0150] In this scheme, the Gaussian distribution function takes the following form: the weight contribution value equals the preset flattening penalty peak multiplied by an exponential function with the natural constant as the base, where the exponent of the exponential function is negative (the square of the index difference) divided by (the square of twice the standard deviation of the Gaussian distribution). The preset flattening penalty peak is a positive constant representing the maximum flattening intensity applied at the center of the interference point. The standard deviation of the Gaussian distribution is an adjustable parameter that controls the rate at which the weights decay from the center outwards.

[0151] For each interferometer index m, iterate through all possible control point indices i (ranging from 0 to n) and calculate the Gaussian function value as described above. The calculated value is the flattened weight contribution of interferometer m to control point i. For i that is far from m, due to the exponential decay effect, the contribution value rapidly approaches zero.

[0152] S42: Set the standard deviation parameter of the Gaussian distribution to control the range of the flattening effect.

[0153] This step further configures the standard deviation parameter of the Gaussian distribution.

[0154] The standard deviation parameter is a positive number that defines the characteristic width of weight decay. When the standard deviation is small, the Gaussian distribution curve is relatively steep, and the weights are mainly concentrated on a few control points near the interference point, resulting in a more localized flattening effect. When the standard deviation is large, the Gaussian distribution curve is relatively flat, and the weights can spread to a wider range of control points, resulting in a smoother and more global flattening effect.

[0155] According to the preferred configuration of this embodiment, the value of the standard deviation parameter is related to the density of the trajectory control points. The higher the control point density (i.e., the smaller the distance between adjacent control points), the smaller the standard deviation can be (e.g., 2 to 5); the lower the control point density (the larger the distance), the larger the standard deviation can be (e.g., 5 to 10). It can also be calibrated according to the characteristics of the scene: in a narrow continuous turning scene, appropriately increasing the standard deviation helps to produce a smoother overall curvature adjustment.

[0156] In this approach, the standard deviation parameter is set during algorithm initialization and remains constant throughout the iteration process. Users or system developers can adjust this parameter according to their specific application scenarios.

[0157] S43: For each control point index, traverse all interferometric control point indices, and take the maximum value among the received flattening weight contribution values ​​as the final flattening weight of that control point to form the final flattening weight vector.

[0158] After calculating the contribution of all interference points to all control points, this step performs a weight fusion operation.

[0159] Since the same control point i may be affected by multiple interference points m1, m2, m3, etc., it will receive multiple weight contribution values. Simply adding these contribution values ​​may lead to excessive weight accumulation, causing the weight of non-interference regions to also increase, thus affecting the optimization effect.

[0160] According to the innovative design of this application, an envelope maximum value strategy is adopted for fusion. Specifically, for each control point index i, each interference point m in the set of interference control point indices is traversed to obtain the calculated weight contribution value, and then the maximum value is selected. This maximum value is used as the final flattened weight of control point i. It is important to note that as long as a control point is within the influence range of any strong interference point, its obstacle avoidance requirement is significant and should be assigned a higher weight; the superposition of multiple interference points does not change this fact. Taking the maximum value instead of the sum value avoids the meaningless expansion of the weight value and maintains the physical interpretability of the weight field.

[0161] The final flattened weights of all control points are organized into a vector according to their index order, i.e., the final flattened weight vector. The length of this vector is equal to the total number of control points. In the vector, the elements near the interference region have larger values, while the elements in the safe region have zero or close to zero values, forming a continuous, unimodal weight field distribution.

[0162] According to another aspect of this application, the final reference weight for each control point is obtained, including:

[0163] S51: Obtain the preset baseline reference weight, which represents the strength of the fit to the original reference path under unobstructed conditions.

[0164] Before starting to dynamically relax the reference weights, this step first defines a baseline reference weight.

[0165] The baseline reference weight is a scalar constant used to quantify the constraint strength required for the optimized trajectory to closely approximate the original reference path within a safe area free of or far from obstacles. A larger value indicates that the optimizer is more inclined to pull control points back to the vicinity of the original reference path; a smaller value indicates a greater tolerance for trajectory deviation from the original path.

[0166] According to the typical settings of this embodiment, the reference weight can be between 0.5 and 2.0. In open, high-speed scenarios, a larger value (such as 1.5 or 2.0) can be used to ensure lane-keeping performance; in narrow park or parking lot scenarios, a medium or smaller value (such as 0.5 or 1.0) can be used to give obstacle avoidance operations greater freedom. This parameter can be preset by the system developer in the algorithm configuration file. The reference weight is a global constant, independent of the location of specific control points and interference conditions. Subsequent dynamic relaxation will be adjusted up or down around this reference value.

[0167] S52: For each control point index, calculate the negative correlation coupling factor based on the final flattening weight, so that the final reference weight and the final flattening weight are negatively correlated.

[0168] After determining the baseline reference weights, this step independently calculates the reference weights after dynamic relaxation for each control point.

[0169] Specifically, let w be the final flattening weight of a certain control point index i. flat_i Since the final flattening weight ranges from 0 to the preset flattening penalty peak, the normalized flattening intensity is defined as w. flat_i Divide by the peak value of the flattening penalty. This normalized value is between 0 and 1: 0 indicates no flattening requirement (safe area), and 1 indicates maximum flattening requirement (strong interference center).

[0170] According to the negative correlation coupling mechanism of this application, the relaxation coefficient is defined as 1 minus the normalized flattening strength. When the flattening strength is 0, the relaxation coefficient is 1, indicating that no relaxation is performed; when the flattening strength is 1, the relaxation coefficient is 0, indicating that the reference constraint is completely released. This relaxation coefficient is a continuous variable between 0 and 1.

[0171] Then, the relaxed reference weight of control point i is calculated: the relaxed reference weight is equal to the baseline reference weight multiplied by the relaxation coefficient. Since the relaxation coefficient decreases as the flattening weight increases, a clear negative correlation is formed between the final reference weight and the final flattening weight. This indicates that in high-risk collision zones, the reference constraints are automatically weakened or even reduced to zero, allowing the trajectory to deviate freely to complete obstacle avoidance; in safe zones, the reference constraints are restored to the baseline strength.

[0172] S53: For a control point index that may be affected by multiple interfering control points, take the minimum value among the negative correlation coupling results calculated independently by all interfering control points as the final reference weight of the control point, and form a reference weight vector.

[0173] Unlike the fusion strategy of flat weights, the dynamic relaxation of reference weights requires the use of the envelope minimum strategy.

[0174] According to a further improvement of this embodiment, the reference constraints of a control point should be released as long as it is within the influence range of any interference point. This is a "weakest link effect": if any of the multiple interference points requires the release of constraints, the overall constraints should be released to the most relaxed extent to avoid obstacle avoidance failure due to conservative constraints.

[0175] Therefore, for each control point index i, it is necessary to traverse all interfering points m in the set of interferometric control point indices. For each interfering point m, a relaxed reference weight value is independently calculated (i.e., the reference weight that control point i should have if only that interfering point exists). Then, the minimum value among all these calculation results is selected as the final reference weight of control point i.

[0176] For example, if control point i is simultaneously affected by two interference points, and the calculated relaxed reference weights are 0.3 and 0.6 respectively, then the smaller value of 0.3 is taken as the final value. This ensures that the constraints are fully released under the most stringent requirements.

[0177] The final reference weights of all control points are organized into a vector according to their index order, namely the final reference weight vector. This vector forms a complementary spatial distribution with the final flattened weight vector: regions with high flattened weights have low reference weights, and regions with low flattened weights have high reference weights. The two work together to guide the optimizer to actively avoid obstacles in the collision zone and regress the path in the safe zone.

[0178] According to another aspect of this application, the final non-uniform B-spline trajectory is output, including:

[0179] S71: In one outer loop, reconstruct the trajectory based on the current set of control points and perform dense sampling, and calculate the minimum safe distance of all sampling points.

[0180] During each outer loop execution, this step is responsible for evaluating whether the current trajectory meets the final safe passage conditions.

[0181] Specifically, the latest set of control points in the current iteration is obtained. The continuous trajectory is reconstructed, and dense sampling is performed. Unlike the sampling density used for collision detection, the sampling used for termination decisions should be denser to fully cover all potential risk points on the trajectory. In a preferred embodiment, the sampling step size can be set to half the time step size, or a more refined adaptive strategy can be used.

[0182] For each sampling point, calculate its minimum safe distance based on the multi-circle bounding rectangle model. Record the minimum safe distances of all sampling points in a list.

[0183] S72: Determine whether all minimum safety distances are greater than the preset absolute safety threshold. If so, determine that the no-collision condition is met.

[0184] After obtaining the minimum safe distance for all sampling points, this step executes the core judgment of the termination condition.

[0185] The absolute safety threshold is a pre-set safe distance value, typically greater than the collision warning threshold. The purpose of setting an absolute safety threshold is to ensure that the final output trajectory not only avoids obstacles but also retains sufficient safety margin to cope with sensor noise, control errors, and dynamic environmental changes.

[0186] For example, if the radius of the disk is 0.9 meters, the collision warning threshold can be set to 1.0 meter (i.e., the disk radius plus 0.1 meters), while the absolute safety threshold can be set to 1.2 meters (i.e., the disk radius plus 0.3 meters). The absolute safety threshold can be adjusted according to the safety level requirements of the scenario: a larger value should be taken in manned environments or high-speed scenarios; a smaller value can be taken in unmanned enclosed parks or extremely low-speed scenarios.

[0187] The algorithm iterates through all recorded minimum safe distance values. If every value is strictly greater than the absolute safe threshold, the current trajectory is considered to have met the collision-free condition, and the algorithm can terminate successfully. If the minimum safe distance of any sampling point is less than or equal to the absolute safe threshold, the condition is not yet met, and iteration continues.

[0188] S73: Simultaneously accumulate the number of iterations in the outer loop and determine whether the preset maximum number of iterations has been reached.

[0189] To avoid infinite iterations when no feasible solution can be found, this step introduces a maximum number of iterations as a safe termination condition.

[0190] Specifically, a counter variable is maintained, initialized to 0. The counter is incremented by 1 after each iteration of the outer loop. At each termination check, the current value of the counter is read and compared with the preset maximum number of iterations.

[0191] The maximum number of iterations is an empirical parameter that needs to be set based on the computing power of the onboard computing unit and the real-time requirements of the planning. For example, if the planning cycle is required to be no more than 100 milliseconds, and the average time of each outer loop is 10 milliseconds, then the maximum number of iterations can be set to 10. In practical applications, considering safety redundancy, the maximum number of iterations is usually set between 20 and 50.

[0192] If the cumulative number of iterations reaches the preset maximum number of iterations, the iteration should terminate regardless of whether the collision-free condition is met. This ensures that the algorithm can return a result within a finite amount of time under any circumstances, avoiding infinite loops that could cause the system to hang.

[0193] S74: If the no-collision condition is met, the current set of control points and the pre-allocated time step are packaged to generate an executable trajectory instruction and output.

[0194] This step is the output operation after successful termination.

[0195] When the collision-free condition is met, it indicates that the algorithm has successfully found a safe, feasible, and smooth trajectory. At this point, the latest set of non-uniform B-spline control points and the pre-assigned time step sequence are obtained. Since the time steps remain unchanged during the optimization process, these time steps are still valid.

[0196] The control point set and time step sequence are input into the trajectory generation module. This module uses the evaluation formula for non-uniform B-spline curves to calculate the position, velocity, and acceleration of the trajectory at any given time. Specifically, the trajectory can be discretized at fixed time intervals (e.g., 0.1 seconds) to generate a series of path points, each containing a timestamp, coordinates, orientation angle, instantaneous velocity, and instantaneous acceleration information.

[0197] These waypoints are organized into a trajectory command message in chronological order. The trajectory command message uses a predefined data format (such as Protobuf or ROS Message) to facilitate transmission to downstream lateral and longitudinal controllers. Finally, the onboard computing unit sends the trajectory command to the vehicle's underlying execution system (including steering, braking, and drive subsystems) via the controller area network bus or Ethernet interface to complete the trajectory planning task.

[0198] S75: If the maximum number of iterations is reached but the collision-free condition is not met, output a planning failure signal and trigger a degradation strategy of safe parking or remote intervention.

[0199] When the preset maximum number of iterations is reached, and the minimum safe distance of at least one sampling point on the current trajectory is less than or equal to the absolute safe threshold, this step will perform a failure handling operation.

[0200] This situation typically occurs when the environment is too congested, the reference path itself is infeasible, or the algorithm parameters are improperly set. To ensure the safety and controllability of the autonomous driving system, it is neither possible to simply ignore the risk of collision and continue driving, nor can we wait indefinitely for the algorithm to converge.

[0201] According to the degradation strategy in this embodiment, a clear planning failure warning signal is first output. This signal can be a Boolean flag or an enumerated value containing a failure reason code. The signal is sent to the vehicle's safety monitoring module.

[0202] Subsequently, a preset safety response behavior is triggered. In one optional implementation, a safe stopping operation is triggered: the vehicle immediately brakes at a preset maximum deceleration, coming to a smooth stop within the shortest distance. After stopping, the vehicle remains stationary, awaiting remote manual intervention or a system reset.

[0203] In another alternative implementation, if the vehicle is equipped with a remote monitoring system, it sends a distress request to a remote monitoring center. The operator at the remote monitoring center can assess the environmental conditions using video and sensor data, and then decide whether to issue a manually corrected reference path or instruct the vehicle to perform maneuvers such as reversing to find new passage space.

[0204] Through a failure handling mechanism, this invention ensures that even in extreme circumstances, the vehicle will not blindly follow a trajectory that poses a collision risk, thereby maximizing driving safety.

[0205] Compared with existing technologies, this invention has the following significant advantages: First, by deeply coupling precise vehicle body modeling with a multi-circle bounding rectangle (CER) with Gaussian-distributed flexible curvature flattening weights, it abandons the traditional rigid step-type obstacle avoidance thrust and replaces discrete pulse correction with a continuously differentiable thrust field. This eliminates abrupt changes in trajectory curvature and geometric angle problems, giving the output trajectory high-order geometric continuity and improving the vehicle's driving smoothness and passenger comfort in narrow curves. Second, it introduces a negative correlation dynamic relaxation mechanism between the flattening weights and the reference trajectory weights. Without manually setting complex rule trigger thresholds, it can automatically release path following constraints in high-risk collision zones and automatically restore constraints in safe zones, breaking the multi-objective optimization deadlock between obstacle avoidance and path preservation. This gives the vehicle the flexibility to use external lanes for obstacle avoidance and effectively improves the planning success rate in extreme scenarios. Third, it adopts a local collision-targeted triggering optimization mechanism. Each iteration only performs trajectory correction on the interference control point and its adjacent area, while the safe section does not participate in the optimization calculation at all. This significantly reduces redundant computation and ensures the real-time response capability of the onboard computing unit in complex environments. Fourth, by constructing an inner and outer double-layer iterative loop architecture combined with an adaptive penalty doubling strategy, the invention can effectively identify and proactively escape local optima traps, improving the robustness and stability of nonlinear programming solutions in nonconvex constrained spaces. In summary, this invention achieves substantial progress in trajectory safety, smoothness, computational efficiency, and solution robustness, reliably supporting autonomous driving vehicles in extreme scenarios such as narrow roads in parks, old underground parking garages, and mining areas.

[0206] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the specific details of the above embodiments. Within the scope of the technical concept of the present invention, various equivalent transformations can be made to the technical solutions of the present invention, and these equivalent transformations all fall within the protection scope of the present invention.

Claims

1. A constrained spatial trajectory planning method considering vehicle volume, characterized in that, include: Acquire environmental perception data and vehicle status, construct a two-dimensional occupancy grid map, and use a multi-circle bounding rectangle model to parametrically represent the vehicle body to obtain the vehicle geometric model; Discrete reference trajectories are collected, and non-uniform B-spline curves are used for parameterization to construct an initial nonlinear programming problem. Calculate the minimum distance from the multi-circle bounding rectangle model to the occupied raster map point by point along the current trajectory, compare it with the safety threshold, mark the interference control points, and generate an interference control point index set; Based on the set of interferometric control point indices, Gaussian-distributed curvature flattening weights are generated with the interferometric control points as the center, and the maximum envelope value is taken for the overlapping area of ​​multiple interferometric points to obtain the final flattening weight of each control point. The final flattened weights are negatively correlated with the preset reference weights, and the minimum envelope value is taken for the overlapping area of ​​multiple interference points to obtain the final reference weight of each control point. A joint objective function is constructed, which includes curvature smoothing cost, obstacle avoidance cost, and reference deviation cost. The obstacle avoidance cost is weighted by the final flattening weight, and the reference deviation cost is weighted by the final reference weight. The numerical optimization solver is called to perform iterative solution and update the trajectory control points. Repeat the above steps until the collision-free condition is met or the maximum number of iterations is reached, and output the final non-uniform B-spline trajectory.

2. The method according to claim 1, characterized in that, The vehicle geometry model is obtained, including: Collect the vehicle's length and width physical parameters; Calculate the radius of a single disk based on the vehicle width, so that the diameter of the disk is equal to the vehicle width; Calculate the longitudinal spacing between the centers of adjacent disks based on the vehicle length and disk radius, so that the front, middle and rear disks are arranged in sequence and completely cover the rectangular outline of the vehicle. The vehicle geometry model is generated by using the radius and longitudinal spacing of the disk as parameters of the multi-circle bounding rectangle model.

3. The method according to claim 1, characterized in that, Construct the initial nonlinear programming problem, including: The discrete reference trajectory points are fitted into a non-uniform B-spline curve to obtain the control point sequence; The coordinates of the first three control points and the last three control points in the fixed control point sequence are used as the coordinates of the remaining internal control points as the decision variables to be optimized, thus obtaining the initial set of optimized control points. Based on the total length of the original reference path and the preset average vehicle speed, a time step is pre-allocated between adjacent control points, and the time step remains unchanged in subsequent optimizations. Based on the time step and the derivative properties of the B-spline curve, the velocity, acceleration, and lateral acceleration at the trajectory points are calculated, and corresponding vehicle physical limit constraints are set to form an initial nonlinear programming problem.

4. The method according to claim 1, characterized in that, Generate a set of interferometric control point indices, including: In the current iteration, the non-uniform B-spline trajectory is reconstructed based on the current set of control points, and the trajectory is discretely sampled with a fixed step size or an adaptive step size to obtain the trajectory sampling point sequence. For each trajectory sampling point, calculate the global coordinates of the centers of the front, middle, and rear disks in the multi-circle bounding rectangle model based on the vehicle's orientation angle; In the 2D occupancy grid map, query the distance from the center of each circle to the nearest occupied grid cell, and take the minimum of the three distances as the minimum safe distance for the trajectory sampling point; The minimum safe distance is compared with a preset collision warning threshold. If it is less than the threshold, the trajectory sampling point is determined to be a collision interference point. Find the non-uniform B-spline control point index corresponding to the collision interference point, and add all the marked control point indices to the interference control point index set.

5. The method according to claim 1, characterized in that, The final flattening weights for each control point are obtained, including: For each interferometric control point index in the set of interferometric control point indices, construct a one-dimensional Gaussian distribution function centered on it, and calculate its flattened weight contribution value to all control point indices on the trajectory. Set the standard deviation parameter of the Gaussian distribution to control the range of the flattening effect; For each control point index, iterate through all interferometric control point indices and take the maximum value among the received flattening weight contribution values ​​as the final flattening weight of that control point, forming the final flattening weight vector.

6. The method according to claim 1, characterized in that, The final reference weights for each control point are obtained, including: Obtain the preset baseline reference weight, which represents the strength of the fit to the original reference path under unobstructed conditions; For each control point index, a negative correlation coupling factor is calculated based on the final flattening weight, so that the final reference weight and the final flattening weight are negatively correlated. For a control point index that may be affected by multiple interfering control points, the minimum value among the negative correlation coupling results calculated independently by all interfering control points is taken as the final reference weight of the control point, forming a reference weight vector.

7. The method according to claim 1, characterized in that, The numerical optimization solver is invoked for iterative solution, updating the trajectory control points, including: Construct a total cost function, which includes the curvature change rate cost, obstacle avoidance cost weighted by flattening weights, and reference path deviation cost weighted by reference weights. The automatic differentiation framework is invoked to calculate the gradient of the total cost function and constraints. The interior point solver is invoked to search for control point locations that minimize the total cost based on gradient information, resulting in an updated set of control points. If the trajectory corresponding to the updated set of control points still has collisions and the change in control points is less than the preset threshold, it is determined that it has fallen into a local optimum. The overall weight coefficient of obstacle avoidance cost is automatically multiplied, and the collision detection and weight update are re-executed.

8. The method according to claim 1, characterized in that, The final non-uniform B-spline trajectory is output, including: In one outer loop, the trajectory is reconstructed based on the current set of control points and dense sampling is performed. The minimum safe distance of all sampling points is calculated. Determine whether all minimum safe distances are greater than the preset absolute safety threshold; if so, determine that the no-collision condition is met. Simultaneously, accumulate the number of iterations in the outer loop and determine whether the preset maximum number of iterations has been reached; If the no-collision condition is met, the current set of control points and the pre-allocated time step are packaged to generate an executable trajectory instruction and output. If the maximum number of iterations is reached but the collision-free condition is not met, a planning failure signal is output and a degradation strategy of safe parking or remote intervention is triggered.

Images