A parking trajectory evaluation method

Abstract

This invention discloses a parking trajectory evaluation method, comprising: generating multiple candidate obstacle avoidance paths represented by fourth-order Bézier curves; utilizing the convex hull property of Bézier curves, a graded collision detection mechanism is used to screen candidate paths with no collision risk through preliminary axial bounding box judgment and precise judgment using the separating axis theorem; for safe candidate paths, regression gravity cost, ride comfort cost, and comprehensive safety cost are calculated by analyzing the geometric relationship of control points, wherein the regression gravity cost is based on the alignment degree between the control point evaluation at the curve endpoint and the heading and curvature of the reference path, and the ride comfort cost is calculated based on the discrete bending energy model of the control polygon; the comprehensive evaluation cost is obtained by weighted summation of the costs of the above three dimensions, and the path with the minimum cost is selected as the execution trajectory, wherein the weight coefficients are dynamically adjusted according to the width of the lateral passable space and the vehicle speed. This invention reduces the computational power consumption of planning and achieves smooth regression with the memorized path after obstacle avoidance.

Description

Technical Field

[0001] This invention relates to the field of parking technology, and more particularly to a parking trajectory evaluation method. Background Technology

[0002] With the development of autonomous driving technology, advanced parking assistance systems have become one of the core functions of intelligent vehicles. In a typical workflow of an advanced parking assistance system, the system first learns from a human driver and generates a global reference path, then controls the vehicle to drive along this reference path when performing parking tasks autonomously. However, real parking lot environments are highly dynamic, and vehicles often encounter dynamic or static obstacles that were not present during the teaching process when autonomously driving along the memorized route. In such cases, the system must possess local real-time obstacle avoidance capabilities and return to the reference path after bypassing the obstacles.

[0003] To achieve smooth obstacle avoidance, existing local path planners typically generate multiple candidate obstacle avoidance trajectories based on the vehicle's current pose and the target reference line, using algorithms such as Bézier curves. Trajectory evaluation is then performed using discrete point sampling and scoring, where the curve is discretized into a series of trajectory points, and collision detection and curvature calculation are performed point-by-point. In extreme obstacle avoidance scenarios, this traditional method requires geometric intersection calculations on dozens of discrete points on each candidate curve, resulting in high-frequency computational consumption and easily overloading the vehicle's underlying computing power, causing delays and frame drops in the planning system. Furthermore, the dense sampling derivative calculations performed to evaluate trajectory smoothness further slow down the overall evaluation efficiency. In addition, traditional cost functions often only penalize the lateral positional deviation between the obstacle avoidance trajectory and the reference line, causing the vehicle to abruptly switch back to the memorized path with a large yaw angle and curvature after avoiding the obstacle, resulting in erratic vehicle movement and sudden steering wheel abrupt changes, severely impacting ride comfort. Summary of the Invention

[0004] The purpose of this invention is to disclose a parking trajectory evaluation method that solves the technical problems of low computational efficiency due to discrete sampling and the "dragon-like" vehicle body when returning to the path after bypassing obstacles.

[0005] To achieve the above objectives, this invention discloses a parking trajectory evaluation method, comprising: Step 1: Based on the vehicle's current pose and a preset reference path, generate multiple candidate obstacle avoidance paths, where each candidate obstacle avoidance path is represented by a smooth curve, and the smooth curve is uniquely determined by a set of sequentially arranged control points. Step 2: For each candidate obstacle avoidance path, perform graded collision detection based on the convex hull formed by its control points and the expanded obstacle area to screen out candidate paths with no collision risk. Step 3: For the candidate paths with no collision risk, calculate the cost function values ​​of multiple dimensions for evaluating the quality of the path by directly analyzing the geometric relationship between its control points. The multiple dimensions include at least the gravity cost of the path regressing to the reference path, the smoothness cost of the path itself, and the comprehensive safety cost related to obstacles. Step 4: Based on the cost function values ​​of the multiple dimensions, calculate the comprehensive evaluation cost of each candidate path, and select the candidate path with the smallest comprehensive evaluation cost as the final execution trajectory output.

[0006] Specifically, the smooth curve is a fourth-order Bézier curve, determined by the first to fifth control points P0~P4; wherein, the first control point P0 is the current position of the vehicle, the fifth control point P4 is the regression point selected on the reference path based on the position of the obstacle, the second control point P1 is obtained by sampling on the extension line of the current heading of the vehicle, and the fourth control point P3 is obtained by sampling on the reverse extension line of the tangent of the reference path at the fifth control point P4.

[0007] Specifically, the graded collision detection in step 2 includes: Step 201: Perform preliminary intersection determination based on axial bounding boxes: If the axial bounding box of the convex hull of the current candidate path does not intersect with the axial bounding box of the expanded obstacle region, the path is directly determined to be safe. Step 202: If the axial bounding boxes intersect, perform a precise intersection determination based on the separation axis theorem: by projecting the convex hull and the expanded obstacle region onto multiple potential separation axes, if there is a non-overlapping projection interval on any separation axis, the path is determined to be safe; otherwise, it is determined to be at risk of collision.

[0008] Specifically, the regression gravity cost in step 3 is calculated through the following sub-steps: Step 301: Based on the last two control points that constitute the end point of the curve, calculate the deviation between the direction of the end point of the curve and the reference direction of the reference path at the regression point, as the first-order direction deviation cost. Step 302: Based on the last three control points that constitute the endpoint of the curve, calculate the deviation between the curvature of the endpoint of the curve and the reference curvature of the reference path at the regression point, as the second-order curvature deviation cost. Step 303: The first-order directional deviation cost, the second-order curvature deviation cost, and the lateral position deviation of the endpoint relative to the reference path are weighted and summed to obtain the regression gravity cost.

[0009] Specifically, the first-order directional deviation cost is directly determined by the direction of the line connecting the fourth and fifth control points; the second-order curvature deviation cost is directly calculated from the coordinates of the third, fourth, and fifth control points using the curvature formula.

[0010] Specifically, the smoothness cost in step 3 is calculated through the following sub-steps: Step 311: Based on all the control points that constitute the curve, construct a series of control vectors between adjacent control points; Step 312: Calculate the discrete angle abruptness cost used to characterize the path curvature based on the angle between adjacent control vectors; Step 313: Calculate the trajectory inflation penalty cost, which characterizes the path redundancy length, based on the ratio of the total length of all control vectors to the straight-line distance between the first and last control points. Step 314: The discrete angle mutation cost and trajectory dilation penalty cost are weighted and summed to obtain the smoothness cost.

[0011] Specifically, the comprehensive safety cost in step 3 includes static safety cost and dynamic safety cost; the static safety cost is calculated based on the maximum projection distance between the path convex hull and the expanded obstacle region on the separation axis obtained from the graded collision detection; the dynamic safety cost is calculated based on the temporal convex hull intersection test results of the predicted trajectory of the dynamic obstacle and the candidate path.

[0012] Specifically, the comprehensive evaluation cost in step 4 is obtained by weighted summation of the regression gravity cost, ride comfort cost, and comprehensive safety cost, and the weight coefficients of each item are dynamically adjusted according to the current driving environment of the vehicle.

[0013] Specifically, the dynamic adjustment of the weight coefficients includes: calculating the original safety weight, the original smoothness weight, and the original regression weight based on the real-time perceived width of the lateral passable space and the current vehicle speed, and then normalizing the original weights to obtain the final weights used to calculate the comprehensive evaluation cost.

[0014] Specifically, step 3 also includes risk prediction of dynamic obstacles: by predicting the position of dynamic obstacles in the future time domain and performing a time domain intersection test between the expanded region and the local convex hull of the candidate path, if the collision time of all candidate paths is less than a preset safety threshold, an emergency intervention mechanism is triggered.

[0015] The beneficial effects of this invention are as follows: By utilizing the convex hull characteristics of Bézier curves to construct a hierarchical collision detection mechanism, this invention only requires preliminary determination of the axial bounding box and precise determination of the separation axis theorem for the convex hull formed by five control points, which can achieve rapid and safe screening without sampling, significantly reducing the computational power consumption in multi-obstacle scenarios; at the same time, by directly analyzing the geometric relationship between control points, a fast calculation method for regression gravity cost and smoothness cost is constructed, which achieves seamless and smooth regression with the memory path after bypassing obstacles, completely eliminating the "dragon drawing" phenomenon caused by hard cutback in traditional methods, and significantly improving the real-time planning, driving smoothness and ride comfort of advanced parking assistance systems in complex dynamic environments. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0017] Figure 1 This is a flowchart illustrating the parking trajectory evaluation method of the present invention. Detailed Implementation

[0018] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. 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 are within the scope of protection of the present invention.

[0019] In this invention, the terms "upper," "lower," "left," "right," "front," "rear," "top," "bottom," "inner," "outer," "middle," "vertical," "horizontal," "lateral," and "longitudinal" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. These terms are primarily for the purpose of better describing the invention and its embodiments, and are not intended to limit the indicated devices, elements, or components to having a specific orientation, or to be constructed and operated in a specific orientation.

[0020] Furthermore, in addition to indicating direction or positional relationship, some of the aforementioned terms may also have other meanings. For example, the term "above" may also be used in certain situations to indicate a dependency or connection. Those skilled in the art can understand the specific meaning of these terms in this invention based on the specific circumstances.

[0021] Furthermore, the terms "installation," "setup," "equipped with," "connection," and "linked" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral structure; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium, or an internal connection between two devices, components, or parts. Those skilled in the art can understand the specific meaning of these terms in this invention based on the specific circumstances.

[0022] Furthermore, the terms "first," "second," etc., are primarily used to distinguish different devices, components, or parts (which may be the same or different in specific type and construction), and are not intended to indicate or imply the relative importance or quantity of the indicated devices, components, or parts. Unless otherwise stated, "a plurality of" means two or more.

[0023] The technical solution of the present invention will be further described below with reference to the embodiments and accompanying drawings.

[0024] like Figure 1 As shown, this embodiment provides a parking trajectory evaluation method, including: Step 1: Generate candidate obstacle bypass paths.

[0025] First, based on the vehicle's current pose and a preset reference path, multiple candidate obstacle avoidance paths are generated. The reference path is a global path that is pre-stored or generated in real time by the advanced parking assistance system.

[0026] In this embodiment, step 1 includes: Step 101: Construct candidate paths using a smooth curve generation algorithm.

[0027] In this embodiment, a quartic Bézier curve is used as the mathematical representation of the path, and each curve is uniquely determined by five sequentially arranged control points. Let the five control points be the first control point P0, the second control point P1, the third control point P2, the fourth control point P3, and the fifth control point P4. Then, the point B(t) corresponding to any parameter t on the curve satisfies:

[0028] Step 102: Determine the positions of the first control point P0 and the fifth control point P4.

[0029] in: The first control point P0 is set as the vehicle's current position coordinates, which are obtained in real time by the on-board positioning module; The fifth control point P4 is set as the return point for the vehicle to rejoin the reference path after bypassing the obstacle. The return point is selected by searching for the nearest passable location behind the obstacle on the reference path and adjusting the location according to vehicle dynamics constraints (such as the maximum steering angle).

[0030] Step 103: Generate candidate sets for the second control point P1 and the fourth control point P3. Specifically, this includes: a. The second control point P1 is constrained to the extension of the vehicle's current heading, i.e., P1 = P0 + α. d curr , where d curr d represents the unit direction vector of the vehicle's current heading. curr =(cosθ curr sinθ curr ), θ curr The vehicle's current actual heading angle; α is a preset sampling distance parameter. Multiple candidate points P1 are generated by sampling α within a preset range at set step sizes. b. The fourth control point P3 is constrained to the reverse extension of the tangent direction of the reference path starting from the fifth control point P4, i.e., P3 = P4. β t ref , where t ref Let P4 be the unit tangent vector of the reference path, and β be a preset sampling distance parameter. Multiple candidate points P3 are generated by sampling β within a preset range with a set step size.

[0031] Step 104: Obtain the reference direction parameters and reference curvature parameters at the fifth control point P4.

[0032] The reference path consists of a series of discrete path points {Q} j =(x j ,y j The discrete path points are constructed using numerical methods, with the following structure: (j=0,1,...,n) a. Reference direction parameter θ ref Take the two consecutive points Q on the reference path that are closest to P4. k Q k+1 Calculate the direction angle of the line connecting them; In this embodiment, the reference direction parameter θ ref The calculation formula is:

[0033] Among them, (x k ,y k ) represents Q k The coordinates, (xk+1 ,y k+1 ) represents Q k+1 The coordinates.

[0034] b. Reference curvature parameter κ ref Take the three consecutive points (Q) on the path that are closest to P4. k 1,Q k Q k+1 The geometric curvature is calculated using the three-point circle method or curve fitting method.

[0035] Step 2: Perform static collision detection on the candidate paths.

[0036] This step employs a collision detection method based on control point convex hull and hierarchical filtering mechanism to quickly screen candidate paths that pose no risk of collision with static obstacles.

[0037] In this embodiment, step 2 includes: Step 201: Perform spatial expansion preprocessing for obstacles, specifically including: a. Determine the vehicle's equivalent safety radius R safe The equivalent safety radius includes half the vehicle width and a dynamically adjusted safety margin; The safety margin is adjusted based on the vehicle's current driving state and / or obstacle attributes. For example, when adjusting based on vehicle speed, 0.2m is used when the vehicle speed is 5 km / h or lower, 0.3m is used when the vehicle speed is higher than 5 km / h but lower than or equal to 10 km / h, and 0.4m is used when the vehicle speed is higher than 10 km / h; when adjusting based on obstacle attributes, 0.15m to 0.25m is used for static rigid obstacles, 0.25m to 0.35m for static flexible obstacles, and 0.4m to 0.6m for dynamic obstacles. b. Shift the boundary of the obstacle detected by the sensing module outward by the equivalent safety radius R. safe This generates an expanded obstacle region.

[0038] Step 202: Construct the convex hull of the control points for the candidate paths. For each candidate path, extract its five control points P0, P1, P2, P3, and P4. Use the convex hull algorithm to calculate the smallest convex polygon enclosing these five points, denoted as the path convex hull. According to the convex hull property of Bézier curves, all points on the entire path must lie inside the path convex hull.

[0039] In specific implementation, the convex hull algorithm can adopt Graham's scan method with polar angle sorting, or other algorithms can be used. This invention does not limit this.

[0040] Step 203: Perform preliminary intersection determination based on axial bounding boxes.

[0041] Calculate the axial bounding box of the path's convex hull and obtain its minimum x-coordinate in the global coordinate system. H_min Maximum x-coordinate H_max Minimum ordinate Y H_min and the maximum ordinate Y H_max Simultaneously, obtain the axial bounding box boundary [X] of the expanded obstacle region. O_min ,X O_max ] and [Y O_min ,Y O_max If any of the following conditions are met, the path convex hull is determined to have no possibility of intersecting with the obstacle region, the corresponding candidate path is marked as safe, and subsequent filtering steps are skipped, proceeding directly to step 3:

[0042] Step 204: Perform precise intersection determination based on the separation axis theorem.

[0043] If the axial bounding boxes in step 203 overlap, then a precise intersection test is performed: a. Construct a set of potential separating axes: Obtain the normal vectors of each edge of the path's convex hull and each edge of the expanded obstacle region to form a set of separating axes. ; b. For each separating axis in set A, project all vertices of the path convex hull and all vertices of the obstacle region onto that axis, respectively, to obtain two projection intervals. , ; c. If there exist two non-overlapping projection intervals on a certain separation axis (i.e.) or If the path convex hull does not intersect with the obstacle region according to the separation axis theorem, the corresponding candidate path is marked as safe, and the test of the remaining separation axes is terminated. d. If the projection intervals on all separation axes overlap, the path convex hull is determined to intersect with the obstacle region. At this time, based on the current computing power load of the system, if the computing power is lower than the preset threshold, the candidate path is sampled and verified using discrete points. If the verification confirms no collision, the path is marked as safe and proceeds to step 3. If the verification confirms a collision, the path is eliminated. If the computing power is higher than the preset threshold, the candidate path is directly eliminated.

[0044] This embodiment employs a two-stage filtering mechanism in the collision detection stage: the first stage checks the AABB boundary. O (1) The complexity is eliminated, and the second level is to perform polygon projection through the Separating Axis Theorem (SAT).

[0045] In existing technologies, to determine whether a 10-meter-long Bézier curve has collided, based on a sampling of 0.1 meters, 100 collision calculations between the point and the OBB are required. However, this embodiment only requires one SAT projection calculation for the convex hull formed by 5 control points. In extreme parking scenarios with dense surrounding obstacles (HPA), this embodiment not only eliminates the risks of frame drops and computing power fluctuations, but also ensures 100% safety (no missed detections) at the mathematical level.

[0046] Step 3: Perform risk prediction for dynamic obstacles.

[0047] For dynamic obstacles identified by the perception module, this step adds temporal collision detection based on motion prediction on the basis of static collision detection.

[0048] Step 301: Estimate the motion state of the dynamic obstacle and predict its future trajectory.

[0049] The system acquires the current position, velocity, and direction of motion of dynamic obstacles, and uses Kalman filtering or uniform motion models to predict their trajectory within a preset future time period, generating multiple expanded obstacle regions corresponding to the predicted time points.

[0050] Step 302: Perform temporal convex hull intersection test.

[0051] The candidate path is regarded as a trajectory that changes over time. For each prediction time, the local convex hull formed by the corresponding position on the path and its neighboring control points is extracted and intersected with the expanded obstacle region at that time using the separating axis theorem.

[0052] Step 303: Output dynamic risk indicators.

[0053] If an intersection is detected at any predicted time, the predicted collision time corresponding to that time is recorded, and the minimum value (TTC) among all predicted times is taken. min As a dynamic risk indicator for this candidate path, if no intersection is detected at any time, a TTC is marked. min =∞.

[0054] Step 304: Trigger the emergency intervention mechanism. If the dynamic risk indicators of all candidate paths are less than the preset safety threshold, emergency braking is triggered and the driver is requested to take over control of the vehicle.

[0055] Step 4: Conduct a multi-dimensional comprehensive cost evaluation of the candidate paths.

[0056] In this embodiment, step 4 includes: Step 401: Calculate the regression gravity cost of the path and the reference path.

[0057] This sub-step evaluates the degree of fit between the candidate path and the reference path at the endpoint, achieving rapid calculation by utilizing the boundary properties of Bézier curves. Specifically, it includes: a. Calculate the cost of first-order directional deviation.

[0058] The first derivative of the quartic Bézier curve at the endpoint is determined by the fourth control point P3 and the fifth control point P4:

[0059] Therefore, the direction angle of the candidate path endpoint .

[0060] Let the reference direction angle of the reference path at point P4 be θ. ref The first-order directional deviation cost is defined as:

[0061] Normalize it:

[0062] b. Calculate the cost of second-order curvature deviation.

[0063] The second derivative of the quartic Bézier curve at the endpoint is determined by the third control point P2, the fourth control point P3, and the fifth control point P4:

[0064] Let the first derivative components be:

[0065] Second derivative components

[0066] Then the curvature κ of the candidate path endpoint bezier for:

[0067] Let the reference direction angle of the reference path at point P4 be κ. ref The cost of second-order curvature deviation is then defined as:

[0068] Normalize it:

[0069] Among them, κ max This indicates the maximum curvature allowed by vehicle dynamics.

[0070] c. Calculate the comprehensive regression gravity cost.

[0071] The lateral position deviation cost, the first-order directional deviation cost, and the second-order curvature deviation cost are weighted and summed:

[0072] in, , as the end point Normalized vertical and lateral errors to the reference path; D max The maximum permissible lateral deviation is preset; k1, k2, and k3 are the internal weights of the regression graviton cost, used to balance the importance of lateral offset, heading deviation, and curvature deviation, satisfying k1+k2+k3=1.

[0073] Step 402: Calculate the smoothness cost of the path.

[0074] This sub-step, based on the geometric features of the control polygon, constructs a smoothness evaluation method that does not require dense sampling, specifically including: a. Construct the control vector sequence. For five control points P0, P1, P2, P3, and P4, calculate the vectors between adjacent control points sequentially:

[0075] b. Calculate the cost of the change in direction. Evaluate the path's curvature using the cosine of the angle between adjacent vectors:

[0076] c. Calculate the path length inflation cost. Assess the detour level of the path by controlling the ratio of the total polygon length to the straight-line distance between the first and last ends:

[0077] d. Calculate the overall smoothness cost. Weight the cost of direction change and the cost of length expansion:

[0078] Wherein, c1 and c2 are system calibration coefficients. Typical values ​​are c1=0.7 and c2=0.3, indicating that the impact of angle changes on ride comfort is slightly higher than that of trajectory length. In practical applications, the values ​​of c1 and c2 can be optimized using parameter identification methods through subjective evaluation of actual vehicles or collection of expert driving data.

[0079] Normalize it:

[0080] Among them, J smooth_max The maximum acceptable smoothness cost can be obtained by statistically analyzing a large number of typical trajectories. For example, in the sample trajectories, J smooth The 95th percentile is approximately 4.2, so we can take J as the value.smooth_max = 5.0, ensuring that the normalized value of the vast majority of trajectories is less than 1.

[0081] Step 403: Calculate the overall security cost of the path.

[0082] This sub-step integrates static collision detection results and dynamic risk prediction results to construct a multi-dimensional safety cost function, specifically including: a. Calculate the static safety cost. For candidate paths determined to be safe in step 2, use the projection spacing g on each separation axis obtained from the separation axis theorem test. i Calculate the minimum distance between the convex hull of the path and the expanded obstacle region:

[0083] Define the static security cost as the reciprocal of the minimum distance:

[0084] in To prevent extremely small constants with a denominator of zero.

[0085] Normalize it using the Sigmoid function:

[0086] Where, α s To control the steepness of the punishment, β s The bias threshold that triggers the dangerous penalty. A typical value is α. s =2.0, β s =2.0, α can be adjusted according to vehicle model and safety strategy in practical applications. s β s .

[0087] b. Calculate the dynamic security cost. Based on the dynamic risk index TTC output in step 303. min Define the dynamic security cost:

[0088] If TTC min If the time is less than the preset safe time threshold, Jsafe_dynamic will be set to the preset large value.

[0089] Normalize it using the Sigmoid function:

[0090] Where, α d To control the steepness of the punishment, β d The bias threshold that triggers the dangerous penalty. A typical value is α. d =2.0, βd =1.0.

[0091] c. Calculate the overall security cost. The normalized static security cost and dynamic security cost are weighted and summed:

[0092] Among them, w s w d Let w be the weighting coefficient. s +w d =1. Typical value w s =0.7, w d =0.3 indicates that static safety takes precedence. In practical applications, this can be dynamically adjusted according to the scenario (e.g., appropriately increasing w in densely populated pedestrian areas). d ).

[0093] Step 404: Calculate the comprehensive evaluation cost of each candidate path.

[0094] The costs of the above three dimensions are weighted and summed:

[0095] Among them, w1, w2, and w3 are dynamically adjusted weight coefficients.

[0096] Step 405: Dynamically adjust the weighting coefficients based on the current driving environment.

[0097] Specifically, it includes: a. The lateral driving space width of the current vehicle location is obtained in real time through the sensing system. (i.e., the lateral distance between the left and right obstacles) and the current vehicle speed V (unit: m / s).

[0098] b. Calculate the original security weights w 1_original When space is extremely limited, safety has an overwhelming veto power:

[0099] Where, k 1a The maximum magnitude of the control weight, k 1b Control the decay rate. Typical value k 1a =1.0, k 1b =2.0, such that when W free When W = 0.5m, w1 ≈ 0.37. free When m = 1.0, w1 ≈ 0.14.

[0100] c. Calculate the original smoothing weights w 2_original In open areas or at higher speeds, passengers are extremely sensitive to sudden steering wheel movements.

[0101] Where, k 2a k is the speed influence coefficient. 2b This is the influence coefficient on spatial width. A typical value for k is... 2a =0.1s / m, k 2b =0.2m 1 This makes it possible to achieve the following when V = 5 m / s and W = 5 m / s. free When the value is 2m, w2≈0.5+0.4=0.9.

[0102] It is important to note that w 2_original Possibly with V and W free The value increases beyond a reasonable range. Therefore, in practical applications, w can be adjusted accordingly. 2_original Truncate the data, for example, to the range [0, 3], before normalizing it to avoid any one factor having too large a weight.

[0103] d. Calculate the original regression weights w 3_original When the vehicle has deviated significantly from the HPA memory route due to obstacle avoidance (deviation distance) (Larger), must be forcibly pulled back:

[0104] Among them, w 3_base The basic regression weights are γ, which is the deviation sensitivity coefficient. Typical values ​​for γ are... 3_base =0.2, γ=0.3, such that when D offset When the radius is 1.0m, w3≈0.2+0.3×0.76=0.43.

[0105] e. Normalize the original weights:

[0106]

[0107] Step 5: Select the optimal path.

[0108] The candidate path with the lowest overall evaluation cost is selected as the obstacle avoidance trajectory output of the advanced parking assistance system.

[0109] The technical means disclosed in this invention are not limited to those disclosed in the above embodiments, but also include technical solutions composed of any combination of the above technical features. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of this invention, and these improvements and modifications are also considered within the scope of protection of this invention.

Claims

1. A parking trajectory evaluation method, characterized in that, Includes the following steps: Step 1: Based on the vehicle's current pose and a preset reference path, generate multiple candidate obstacle avoidance paths, where each candidate obstacle avoidance path is represented by a smooth curve, and the smooth curve is uniquely determined by a set of sequentially arranged control points. Step 2: For each candidate obstacle avoidance path, perform graded collision detection based on the convex hull formed by its control points and the expanded obstacle area to screen out candidate paths with no collision risk. Step 3: For the candidate paths with no collision risk, calculate the cost function values ​​of multiple dimensions for evaluating the quality of the path by directly analyzing the geometric relationship between its control points. The multiple dimensions include at least the gravity cost of the path regressing to the reference path, the smoothness cost of the path itself, and the comprehensive safety cost related to obstacles. Step 4: Based on the cost function values ​​of the multiple dimensions, calculate the comprehensive evaluation cost of each candidate path, and select the candidate path with the smallest comprehensive evaluation cost as the final execution trajectory output.

2. The method according to claim 1, characterized in that, In step 1, the smooth curve is a fourth-order Bézier curve, determined by the first to fifth control points P0~P4; wherein, the first control point P0 is the current position of the vehicle, the fifth control point P4 is the regression point selected on the reference path based on the position of the obstacle, the second control point P1 is obtained by sampling on the extension line of the current heading of the vehicle, and the fourth control point P3 is obtained by sampling on the reverse extension line of the tangent of the reference path at the fifth control point P4.

3. The method according to claim 1, characterized in that, The graded collision detection in step 2 specifically includes: Step 201: Perform preliminary intersection determination based on axial bounding boxes: If the axial bounding box of the convex hull of the current candidate path does not intersect with the axial bounding box of the expanded obstacle region, the path is directly determined to be safe. Step 202: If the axial bounding boxes intersect, perform a precise intersection determination based on the separation axis theorem: by projecting the convex hull and the expanded obstacle region onto multiple potential separation axes, if there is a non-overlapping projection interval on any separation axis, the path is determined to be safe; otherwise, it is determined to be at risk of collision.

4. The method according to claim 1, characterized in that, The regression gravity cost in step 3 is calculated through the following sub-steps: Step 301: Based on the last two control points that constitute the end point of the curve, calculate the deviation between the direction of the end point of the curve and the reference direction of the reference path at the regression point, as the first-order direction deviation cost. Step 302: Based on the last three control points that constitute the endpoint of the curve, calculate the deviation between the curvature of the endpoint of the curve and the reference curvature of the reference path at the regression point, as the second-order curvature deviation cost. Step 303: The first-order directional deviation cost, the second-order curvature deviation cost, and the lateral position deviation of the endpoint relative to the reference path are weighted and summed to obtain the regression gravity cost.

5. The method according to claim 4, characterized in that, The first-order directional deviation cost is directly determined by the direction of the line connecting the fourth and fifth control points; the second-order curvature deviation cost is directly calculated from the coordinates of the third, fourth, and fifth control points using the curvature formula.

6. The method according to claim 1, characterized in that, The smoothness cost in step 3 is calculated through the following sub-steps: Step 311: Based on all the control points that constitute the curve, construct a series of control vectors between adjacent control points; Step 312: Calculate the discrete angle abruptness cost used to characterize the path curvature based on the angle between adjacent control vectors; Step 313: Calculate the trajectory inflation penalty cost, which characterizes the path redundancy length, based on the ratio of the total length of all control vectors to the straight-line distance between the first and last control points. Step 314: The discrete angle mutation cost and trajectory dilation penalty cost are weighted and summed to obtain the smoothness cost.

7. The method according to claim 1, characterized in that, The comprehensive safety cost in step 3 includes static safety cost and dynamic safety cost; the static safety cost is calculated based on the maximum projection distance between the path convex hull and the expanded obstacle region on the separation axis obtained from the graded collision detection. The dynamic safety cost is calculated based on the temporal convex hull intersection test results of the predicted trajectory of the dynamic obstacle and the candidate path.

8. The method according to claim 1, characterized in that, The comprehensive evaluation cost in step 4 is obtained by weighted summation of the regression gravity cost, ride comfort cost, and comprehensive safety cost, and the weight coefficients of each item are dynamically adjusted according to the current driving environment of the vehicle.

9. The method according to claim 8, characterized in that, The dynamic adjustment of the weight coefficients includes: calculating the original safety weight, the original smoothness weight, and the original regression weight based on the real-time perceived width of the lateral passable space and the current vehicle speed, and then normalizing the original weights to obtain the final weights used to calculate the comprehensive evaluation cost.

10. The method according to any one of claims 1 to 9, characterized in that, Step 3 also includes risk prediction for dynamic obstacles: by predicting the position of dynamic obstacles in the future time domain and performing a time domain intersection test between the expanded region and the local convex hull of the candidate path, if the collision time of all candidate paths is less than a preset safety threshold, an emergency intervention mechanism is triggered.

Images