A two-stage parallel parking path planning algorithm considering tracking error accumulation

Abstract

The application discloses a two-stage parallel parking path planning algorithm considering tracking error accumulation, and comprises the following steps: S1, periodically acquiring vehicle operation data, parking space coordinate information and obstacle coordinate information; S2, performing path search and obstacle avoidance constraint through a Hybrid A* algorithm, and planning an optimal path between a starting point and an ending point of parking in real time; S3, judging a lateral error of the vehicle at a detection point, and if the error exceeds a threshold value, re-planning a parking path in a garage; and S4, forming a complete automatic parking planning path. The application firstly performs path search through a Hybrid A* algorithm in combination with an RS curve, secondly further plans in combination with an environment in the parking space at a planning midpoint, and finally detects a trajectory tracking error of the vehicle by using the detection point, so that the vehicle can be successfully parked.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of intelligent control, in particular to a two-stage parallel parking path planning algorithm considering tracking error accumulation. BACKGROUND

[0002] With the improvement of economic level, the number of cars is rapidly increasing and has a rising trend, but at the same time, it also causes the problem of "parking difficulty". On the one hand, the number of parking spaces is small, and the random parking of some vehicles makes the parking difficulty problem more prominent. On the other hand, even if the driver finds a suitable parking space, the small parking space and the limited field of view of the driver make it difficult for the driver to successfully park in the garage in a short time. Therefore, studying a reasonable automatic parking path planning method helps to improve the success rate and parking efficiency of vehicle automatic parking.

[0003] As a depth-first algorithm, the A* algorithm adds a heuristic function in the cost function, which guides the algorithm to search in the direction close to the end point, improving the search efficiency. The hybrid A* algorithm considers whether the planning algorithm meets the vehicle driving constraint, so that the nodes are no longer connected by straight lines. This algorithm is mostly applied to the automatic parking scene. Both the A* algorithm and the hybrid A* algorithm need to expand many nodes, the search range is wide, the calculation amount is large, and a long time is needed for calculation, and the vehicle-mounted computer lacks computing power, which leads to a long waiting time and poor experience. In addition, the path planned by this method is not smoothed, which will cause the vehicle to turn in place during tracking control, which will have a large load on the steering motor and wear the tires. SUMMARY

[0004] In view of the problems in the prior art, the present application provides a two-stage parallel parking path planning algorithm considering tracking error accumulation. First, a hybrid A* algorithm is used to search the path with an RS curve. Second, the environment inside the parking space is combined at the planning midpoint for further planning. Finally, the tracking error of the vehicle trajectory is detected by using detection points to ensure that the vehicle can successfully park.

[0005] To achieve the above purpose, the present application adopts the following technical solutions:

[0006] The present application provides a two-stage parallel parking path planning algorithm considering tracking error accumulation, including the following steps:

[0007] S1, periodically acquire vehicle running data, parking space coordinate information and obstacle coordinate information, the vehicle running data including vehicle starting point and vehicle ending point;

[0008] S2, based on vehicle operation data, parking space coordinate information and obstacle coordinate information, the Hybrid A* algorithm is used for path search, obstacle avoidance constraint, real-time planning of the optimal path from the starting point to the parking end point of the vehicle, and the path from the starting point to the parking end point of the vehicle is composed of the parking path outside the garage and the parking path inside the garage, and the connection point of the parking path outside the garage and the parking path inside the garage is the planning midpoint;

[0009] S3, detecting the lateral error of the vehicle at the detection point, and if the error exceeds the threshold, re-planning the parking path inside the garage;

[0010] S4, connecting the planned parking path outside the garage and the final parking path inside the garage to generate a complete automatic parking planning path.

[0011] Preferably, in S2, the planning midpoint and the heading angle of the vehicle at the planning midpoint are first found, and the optimal path from the starting point to the parking end point of the vehicle is planned in real time through the planning midpoint and the heading angle of the vehicle at the planning midpoint. In the planning process, the RS curve extending through the planning midpoint from the current grid node to the parking end pose is expanded at a fixed frequency.

[0012] Preferably, the method for finding the planning midpoint is as follows: taking the straight line of the garage boundary line as the X-axis, taking the straight line of the garage tail boundary line as the Y-axis, and taking the intersection of the X-axis and the Y-axis as the origin O to establish a coordinate system, and the X-axis coordinate x m of the planning midpoint (x m , 0) is calculated.

[0013]

[0014] In the formula, R min is the minimum turning radius of the rear axle center of the vehicle, x s is the horizontal coordinate of the parking end point, and y s is the vertical coordinate of the parking end point.

[0015] x m，max is determined by the initial pose and the parking space boundary.

[0016] Preferably, the calculation method of x m，max is as follows:

[0017] x m,max = min(x m,max1 , x m,max2 )

[0018]

[0019] When the vertical coordinate of the starting point is less than the critical value:

[0020]

[0021] When the starting point ordinate is greater than the critical value:

[0022]

[0023] In the above formula, Kc is the garage length, R C,min is the minimum turning radius of the right front corner of the vehicle, R D,min represents the minimum turning radius of the right rear corner of the vehicle, Kw is the parking space width, W is the vehicle width, L is the vehicle wheelbase, L f is the front suspension length of the vehicle, L r is the rear suspension length of the vehicle, x0 is the starting point horizontal coordinate of parking, and θ0 is the starting point heading angle of parking.

[0024] Preferably, it is characterized in that the calculation method of the heading angle of the planning midpoint is:

[0025]

[0026] In the above formula, W is the vehicle width, L r is the rear suspension length of the vehicle, R min is the minimum turning radius of the rear axle center of the vehicle.

[0027] θ min is determined by the right boundary of the parking space, the parking end point, and the parking starting point.

[0028] Preferably, it is characterized in that the calculation method of θ min is as follows:

[0029] θ min = max(θ min1 , θ min2 , θ min3 )

[0030]

[0031] In the above formula, P m O r is the distance between the center of the trajectory circle when the vehicle reverses at the starting point with the minimum turning radius and the planning midpoint.

[0032] Preferably, the RS curve is composed of at least three segments, which are circular arcs C and straight line segments S combined by the minimum turning radius, and are represented as follows:

[0033] CCC←{C|C|C C∣CC CC∣C CC β ∣C β C C∣C β C β ∣C}

[0034] CSC←{CSC C∣C π / 2 SC CSCπ / 2 | C | C | π / 2 SC π / 2 | C}

[0035] where the symbol | represents the reversal of direction, C π / 2 represents an arc length of π / 2 walked to the left or right.

[0036] Preferably, the method for planning the optimal path between the start and end points of the vehicle is specifically as follows:

[0037] S201, initialize the start node and the target node, and add the start node to the Open list;

[0038] S202, when the Open list is not empty, perform the following steps:

[0039] a. Select the node with the lowest generation value from the Open list as the current node, and remove it from the Open list and mark it as visited;

[0040] b. If the current node is the target node, it means that the optimal path has been found, and the search is ended;

[0041] c. If the current node is not the target node, add the current node to the Closed list.

[0042] d. Generate the neighbor nodes of the current node, and perform the following operations for each neighbor node:

[0043] i. If the neighbor node is already in the Closed list, skip this node;

[0044] ii. If the neighbor node is not in the Closed list and not in the Open list, add it to the Open list, update its previous node to the current node, and calculate its heuristic cost and actual cost;

[0045] iii. If the neighbor node is not in the Closed list but in the Open list, and the cost calculated through the path of the current node is lower, update the previous node and the cost of the neighbor node;

[0046] e. Calculate the RS curve between the end point, perform collision detection on the RS curve, if there is no collision, the optimal path is found, start from the target node and trace back along the previous node pointer to the start node, and the optimal path can be obtained;

[0047] S203, if the Open list is empty, it means that the path cannot be found, and the search fails. Preferably, the node cost function expression is as follows:

[0048] f(s) = g(s) + h(s)

[0049] wherein g(s) represents the actual cost from the start to the current node, and h(s) represents the heuristic cost from the current node to the end.

[0050] Preferably, the detection point is a discrete point along the parking-out path from the planning midpoint by a distance of x h meters, which is immediately adjacent to the point from the planning midpoint by a distance of x h meters, x h The calculation method is as follows:

[0051] x h = v·t ps

[0052] In the above formula, x h is the distance from the planning midpoint to the detection point along the parking-out path, v is the vehicle speed when parking into the garage, and t ps is the planning time consumed by the planner from the planning midpoint to the parking end.

[0053] Compared with the prior art, the present application has the following beneficial effects:

[0054] (1) The present application introduces a RS curve accelerator at a fixed frequency on the basis of the Hybrid A* algorithm, solves the shortcomings of the traditional search algorithm that the calculation amount is large and the time consumption is long, proposes a two-section automatic parking path planning idea, determines the planning midpoint position coordinates under the conditions of considering various obstacle avoidance constraints, start and end pose constraints, and turning radius constraints, further adjusts the parking end according to the change of the parking space environment, realizes parking obstacle avoidance and improves the parking success rate.

[0055] (2) The present application also considers that if the vehicle is stopped at the planning midpoint to observe the parking space environment and then the second section parking planning is performed, time waste and complex operation will be caused, therefore, the scheme of planning in advance is proposed to solve the problem, the concept of “detection point” and the calculation of its position are proposed, considering the existence of errors, the judgment criterion of the vehicle reaching the point is proposed, and the lateral error of the vehicle from the point is calculated when reaching the point, if the error exceeds a certain range, the planning start point of the parking-in path is reselected. BRIEF DESCRIPTION OF DRAWINGS

[0056] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or prior art description. Obviously, the drawings in the following description only some embodiments of the present application, and for those skilled in the art, other drawings can also be obtained from the structures shown in the drawings without creative labor.

[0057] Figure 1 The figure shows the planning midpoint.

[0058] Figure 2 for the parking end point constraint schematic diagram;

[0059] Figure 3 for the parking end point constraint schematic diagram;

[0060] Figure 4 for the parking end point constraint schematic diagram;

[0061] Figure 5 for the planning midpoint schematic diagram when the longitudinal offset is small;

[0062] Figure 6 for the planning midpoint schematic diagram when the longitudinal offset is large;

[0063] Figure 7 for the parking end point constraint schematic diagram;

[0064] Figure 8 for the parking end point constraint schematic diagram;

[0065] Figure 9 for the parking end point constraint schematic diagram;

[0066] Figure 10 for the parking end point constraint schematic diagram;

[0067] Figure 11 wherein (a) is the time used by the traditional A* search algorithm, and (b) is the time used by the node search algorithm provided by the present application;

[0068] Figure 12 for the planning midpoint algorithm test result diagram provided by the present application for the case of an obstacle appearing in the parking space, (a) is the case of an obstacle existing at the bottom of the parking space requiring the selection of a new end point, and (b) is the case of the partial position of the parking space being occupied due to the failure of other vehicles to park in a standard manner;

[0069] Figure 13 for the detection point algorithm test result diagram provided by the present application for the case of trajectory tracking error, (a) is the trajectory of vehicle displacement change, and (b) is the trajectory of vehicle heading angle change. DETAILED DESCRIPTION

[0070] The embodiments of the present application will be described in detail below with reference to the drawings, but those skilled in the art will understand that the following examples are only used to illustrate the present application and should not be regarded as limiting the scope of the present application.

[0071] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the specific embodiments of the present application will be described in detail below with reference to the drawings.

[0072] In the following description specific details are set forth in order to provide a thorough understanding of the application. However, the application can be practiced without the specific details. In other instances, well-known methods, procedures, components, and circuits have not been described in detail so as not to obscure the application. Accordingly, the application is not limited to the specific embodiments described below.

[0073] The embodiment proposes a two-stage parallel parking path planning algorithm considering tracking error accumulation, including the following steps:

[0074] S1, periodically acquire vehicle running data, parking space coordinate information and obstacle coordinate information, the vehicle running data including vehicle starting point and vehicle ending point;

[0075] S2, first find the planning midpoint and the heading angle of the vehicle at the planning midpoint, based on the vehicle running data, the parking space coordinate information, the obstacle coordinate information, the planning midpoint and the heading angle of the vehicle at the planning midpoint, the optimal path between the vehicle starting point and the parking ending point is planned in real time by the Hybrid A* algorithm, the path between the vehicle starting point and the parking ending point is composed of the parking path outside the garage and the parking path inside the garage, and the connection point of the parking path outside the garage and the parking path inside the garage is the planning midpoint. In the planning process, the RS curve expanding the planning midpoint from the current grid node to the parking ending point pose is expanded at a fixed frequency;

[0076] Specifically, referring to Figure 1 , the method for finding the planning midpoint is as follows: taking the straight line of the garage boundary line as the X-axis, taking the straight line of the garage tail boundary line as the Y-axis, and taking the intersection of the X-axis and the Y-axis as the origin O to establish a coordinate system, the X-axis coordinate x m of the planning midpoint Pm(x m is calculated as follows:

[0077]

[0078] In the formula, R min is the minimum turning radius of the center of the rear axle of the vehicle, x s is the horizontal coordinate of the parking ending point, y s is the vertical coordinate of the parking ending point;

[0079] x m,max =min(x m,max1 ,x m,max2 )

[0080]

[0081]

[0082] When the starting point vertical coordinate is less than the critical value:

[0083]

[0084] When the starting ordinate is greater than the critical value:

[0085]

[0086] In the above formula, Kc is the length of the garage, and R C,min R is the minimum turning radius of the vehicle's right front corner. D,min This indicates the minimum turning radius of the vehicle's right rear corner. Kw is the parking space width, W is the vehicle width, and L is the vehicle wheelbase. f L represents the front overhang length of the vehicle. r θ is the rear overhang length of the vehicle, x0 is the x-coordinate of the parking starting point, and θ0 is the heading angle of the parking starting point.

[0087] In this embodiment, the midpoint of the plan is used to distinguish the two paths before and after warehousing, where the paths before and after warehousing, as well as the midpoint of the plan, are defined. m The specific location is as follows Figure 1 As shown in the figure. This step mainly calculates the position and angle range of the planned midpoint under various constraints during the vehicle's parking process, and finally determines the optimal planned midpoint.

[0088] The purpose of the midpoint in the planning is to divide the entire parking process into two parts: before and after parking. The midpoint is selected at the entrance of the parking space, specifically when the center of the vehicle's rear axle is at a vertical coordinate of 0. The range of the horizontal coordinate of the midpoint mainly considers three aspects: the constraints of the parking endpoint, the constraints of the parking space boundaries, and the positional constraints of the parking starting point.

[0089] The first analysis focuses on the constraints of the parking endpoint. The top left corner of the parking space is defined as the origin. With the endpoint coordinates determined, a reverse calculation is performed. At the endpoint, as the vehicle moves out of the garage, its x-coordinate reaches its minimum when it reaches y=0, assuming it is moving at its maximum turning angle. For example... Figure 2 As shown, at this moment, the vehicle is moving from the parking end point towards the outside of the garage with the minimum turning radius. At this moment, P... m The x-coordinate of the point is the minimum value. This scenario does not consider whether the rear right corner D of the vehicle will collide with the vehicle's lower boundary, i.e., when the y-coordinate of the parking endpoint is... s If the value is too small, the vehicle cannot exit at its maximum turning angle. That is, when the vehicle exits at the maximum turning angle that prevents it from colliding with the lower boundary of the parking space, the x-coordinate at the center of the rear wheel will be at its minimum value, where the ordinate is zero.

[0090] According to j=y s +Kw+R min -R Dmin Make a judgment, where Kw is the width of the parking space, and y s R is the ordinate of the parking endpoint. min R is the minimum turning radius of the vehicle's rear axle center.D min The minimum turning radius of the right rear point of the vehicle. When j is greater than or equal to zero, it means that turning directly at the minimum turning radius will not collide with the lower boundary of the parking space, and the minimum value is obtained, which is shown in Fig. 2. The minimum value is calculated by the following formula, wherein x Figure 2 s is the horizontal coordinate of the end of parking.

[0091]

[0092] When j is less than zero, the turning radius at this time satisfies R D s + Kw. Since R D and R D satisfy the relationship R 2 = (R + W / 2) 2 + Lr 2 , wherein W is the width of the vehicle, and Lr is the rear suspension length of the vehicle. Therefore, the turning radius R of the rear wheel center at this time can be obtained, and the minimum value of the horizontal coordinate of the planning midpoint at this time is calculated as follows:

[0093]

[0094] Regarding the analysis of the boundary restrictions of the parking space, the right boundary of the parking space will limit the horizontal coordinate of the planning midpoint within a certain range, and therefore the maximum value of the horizontal coordinate is calculated by analyzing the collision between the vehicle and the right boundary of the garage. It can be known through analysis that the horizontal coordinate will reach the maximum when the vehicle is about to collide with the boundary.

[0095] As shown in Fig. 3, the longitudinal coordinate of the rear wheel center of the vehicle at this time is exactly zero, and the limit horizontal coordinate value at this time is the maximum horizontal coordinate of the planning midpoint on the parking line. Through calculation, the horizontal coordinate of the midpoint is obtained as follows: Figure 3

[0096]

[0097] wherein Kc is the length of the parking space, R is the turning radius of the rear wheel center, R C is the turning radius of the right front point of the vehicle, and angle θ is the heading angle of the rear wheel center of the vehicle on the parking line. It can be seen from the formula that the horizontal coordinate is related to angle θ. The derivative of the formula is calculated to obtain:

[0098]

[0099] Since R C > R, i.e.

[0100]

[0101] Therefore,​​ i.e. x m increases with the increase of the vehicle heading angle θ, i.e. the maximum midpoint abscissa x m is obtained when the heading angle θ reaches the maximum. Since there are the restrictions of the lower boundary of the parking space, the end of the parking process and the limit condition of the right front point of the vehicle, the heading angle θ cannot increase infinitely under the restrictions of the three constraints. It is easy to know that under the conditions of the parking end pose and the parking space, the greater the front wheel steering angle during the parking process, the closer the parking position to the lower boundary of the parking space, and the greater the vehicle body orientation angle θ at the entrance of the parking space, i.e. the greater the vehicle body orientation angle θ at the midpoint of the path.

[0102] As shown in FIG. 6, at this time, the vehicle travels at the maximum front wheel steering angle, and the right rear point D of the vehicle just does not collide with the lower boundary of the parking space, and the right front point C of the vehicle just does not collide with the boundary of the parking space, and the abscissa of the planning midpoint obtained at this time is the maximum. Through calculation, the maximum value is: Figure 4

[0103]

[0104] where R C,min is the minimum turning radius of the right front point C of the vehicle, and R D,min represents the minimum turning radius of the right rear point D of the vehicle, and the sizes are respectively:

[0105]

[0106] Regarding the restriction of the initial pose, the forward adjustment pose of the vehicle is ignored, and the vehicle directly starts to reverse at the parking start point. In the previous calculation, the maximum abscissa value of the planning midpoint is obtained, and when the vehicle is close to the parking space, the vehicle cannot reach the maximum value. That is, the vehicle cannot reach the maximum abscissa by directly reversing at the start point with the maximum steering angle. When the longitudinal coordinate of the vehicle start point is too large, directly reversing with the maximum steering angle will cause the vehicle to reach the parking line with a too large heading angle, which cannot complete the subsequent parking operation, and therefore it is necessary to calculate the maximum abscissa value of the planning midpoint by reaching the front steering wheel and changing from right turning to left turning. First, the critical value is calculated, i.e. the vehicle reverses with the maximum front wheel steering angle, and the heading angle when reaching the parking line is just the angle on the parking line when the vehicle exits from the parking end with the maximum front wheel steering angle. Through calculation, the critical value of the longitudinal coordinate of the vehicle start point is:

[0107] y cri = R min × cos |θ0| - R min - y s

[0108] ​When the ordinate of the starting point is less than the critical value, the maximum abscissa of the midpoint in the planning is as shown in Figure 5 ; while when it is greater than the critical value, the maximum abscissa of the midpoint in the planning is as shown in Figure 6 .

[0109] The coordinate of the starting parking point is (x0, y0), and the heading angle is θ0. The centers of the respective circular arcs are denoted by O r , O r1 , and O r2 , respectively. Through calculation, when the ordinate of the starting point is less than the critical value, the maximum abscissa of the midpoint is:

[0110]

[0111] When the ordinate of the starting point is greater than the critical value, the maximum abscissa of the midpoint calculated is:

[0112]

[0113] Taking all of the above into consideration, the maximum abscissa of the midpoint in the path planning is:

[0114] x m,max = min(x m,max1 , x m,max2 )

[0115] Thus, the range of the position of the midpoint in the planning can be determined as: x m,min < x m < x m,max , y m = 0.

[0116] For safety, x m is preferably the middle value between x m，mim and x m，max , i.e.

[0117] The calculation method of the heading angle θ of the vehicle at the midpoint in the planning is:

[0118]

[0119] In the above formula, W is the width of the vehicle, L r is the rear overhang length of the vehicle, and R min is the minimum turning radius of the center of the rear axle of the vehicle;

[0120] θ min = max(θ min1 , θ min2 , θ min3 )

[0121]

[0122] In the above formula, P m O r is the distance between the center of the trajectory circle when the vehicle backs up at the starting point with the minimum turning radius and the midpoint of the plan.

[0123] Since the heading angle at each point directly affects the success rate of automatic parking, first analyze the impact of the lower boundary of the parking space on the heading angle at the midpoint of the plan. In the case of a determined turning radius, the closer the vehicle is to the lower boundary of the parking space, the larger the heading angle of the vehicle when it reaches the midpoint of the plan. As shown in FIG. 6, when the vehicle reaches the left boundary line of the parking space, the heading angle is θ, and it can be observed that when the vehicle approaches the lower boundary of the parking space, the corresponding driving arc length of the vehicle driving out of the garage is longer, and under the same radius, the corresponding heading angle is larger. In the case of a determined distance between the vehicle and the bottom of the parking space, the smaller the turning radius, the larger the corresponding heading angle of the vehicle when it reaches the midpoint of the plan. The analysis of the lower boundary of the parking space ignores the change in the corresponding longitudinal coordinate of the vehicle when it is about to collide with the lower boundary of the parking space under different turning radii. The limit distance is set to a fixed value for easy calculation. Figure 7 The value of s at this time is calculated as follows:

[0124]

[0125] The maximum value of the heading angle under the constraint of the lower boundary of the parking space is calculated as follows:

[0126]

[0127] Regarding the impact of the right boundary of the parking space on the heading angle at the midpoint of the plan, to avoid collision between the vehicle and the right boundary of the parking space at the midpoint of the plan, there should be a minimum heading angle limit when the midpoint position is determined. Through the constraint of the boundary of the parking space on the midpoint position, the relationship between the horizontal coordinate of the midpoint of the plan and the heading angle is obtained, where x m is the horizontal coordinate value of the midpoint of the plan, and through back calculation, the following can be obtained:

[0128]

[0129] In the formula, R C represents the turning radius of the right front point of the vehicle, which is directly related to the turning radius R.

[0130]

[0131] By combining the two formulas, the following can be obtained:

[0132]

[0133]

[0134] ​By observing the formula, it can be found that only the radius R is a variable on the right side of the equation, and the angle θ is positively correlated with sinθ in the value range, so the turning radius R is negatively correlated with the heading angle θ, that is, when the minimum heading angle value is calculated, the turning radius R needs to be infinite, that is, the vehicle is straight at this time, and the front wheel turning angle is 0, as shown in Figure 8 , at this time, the minimum heading angle is obtained, and the value is:

[0135]

[0136] Regarding the restriction of the parking end pose on the heading angle at the midpoint, when the midpoint and the end position are determined, the path of the vehicle driving out from the end position will be in the form of a combination of a circular arc and a straight line. Using the form of a circular arc will cause the heading angle of the vehicle to increase, so that the vehicle has a smaller heading angle when reaching the parking space line, the vehicle should turn as little as possible and drive as straight as possible.

[0137] As shown in Figure 9 , the vehicle drives out, considering the limited position in the parking space, the vehicle turns with the minimum turning radius, drives out of the parking space in the direction of the tangent, and the driving out position corresponds. It can be calculated that the minimum heading angle at this time is:

[0138]

[0139] Regarding the influence of the parking start pose on the planned midpoint heading angle, similar to the restriction of the parking end, the reversing situation is as shown in Figure 10 :

[0140] In order to make the heading angle of the vehicle at the planned midpoint as small as possible, the vehicle reverses at the start point P0 with the minimum turning radius, and then continues to reverse in the direction of the tangent of the circular arc to reach the planned midpoint position. Similar to the calculation method of θ min2 in the above, the point O r is connected with the point P m . It is calculated that:

[0141]

[0142] By comprehensively considering the parking space lower boundary constraint, the parking space right boundary constraint, the vehicle parking end constraint, and the vehicle parking start constraint, the range of the heading angle at the planned midpoint is obtained.

[0143] The minimum heading angle at the midpoint is determined by three parts, and the value is:

[0144] θ min = max(θ min1 , θ min2 , θ min3 )

[0145] The range of the heading angle is obtained, and for safety, θ takes the intermediate value of θ min and θ max , that is,

[0146] Figure 12 The test result graph of the midpoint algorithm of the application for the case of an obstacle appearing in the parking space;

[0147] Specifically, in the path planning process, the RS curve is composed of at least three segments, and the three-segment path is composed of a circular arc C formed by the minimum turning radius and a straight line segment S, and is expressed as follows:

[0148] CCC←{C|C|C C∣CC CC∣C CC β ∣C β C C∣C β C β ∣C}

[0149] CSC←{CSC C∣C π / 2 SC CSC π / 2 ∣C C∣C π / 2 SC π / 2 ∣C}

[0150] Wherein the symbol | represents the reversal of the direction, C π / 2 represents that the arc length walked to the left or right is π / 2. Therefore, there is a distinction between left and right turns, wherein L represents a left turn, R represents a right turn, the symbol + represents the forward movement of the vehicle, the symbol - represents the backward movement of the vehicle, and the character subscript represents the arc length turned, wherein C π / 2 represents that the arc length walked to the left or right is π / 2. By bringing each symbol into the equation, 48 combinations can be obtained, and the shortest path from the starting point to the end point must be one of them. The combinations are shown in Table 1.

[0151] Table 1 48 RS curve combination modes

[0152]

[0153]

[0154] Specifically, the planning method of the optimal path of the vehicle from the starting point to the end point is specifically:

[0155] S201, initializing the starting node and the target node, and adding the starting node to the Open list;

[0156] S202, when the Open list is not empty, the following steps are performed:

[0157] a. Select the node with the lowest generation value from the Open list as the current node, and remove it from the Open list, mark it as visited;

[0158] Wherein, the node cost function expression is as follows: (the cost includes heuristic cost and actual cost)

[0159] f(s) = g(s) + h(s)

[0160] Wherein g(s) represents the actual cost from the starting point to the current node, and h(s) represents the heuristic cost from the current node to the end point.

[0161] b. If the current node is the target node, it means that the optimal path has been found, and the search is ended;

[0162] c. If the current node is not the target node, the current node is added to the Closed list.

[0163] d. Generate the neighbor nodes of the current node, and perform the following operations on each neighbor node:

[0164] i. If the neighbor node is already in the Closed list, skip this node;

[0165] ii. If the neighbor node is not in the Closed list and not in the Open list; add it to the Open list, update its previous node as the current node, and calculate its heuristic cost and actual cost.

[0166] iii. If the neighbor node is not in the Closed list but in the Open list, and the cost calculated through the current node path is lower, update the previous node and the cost of the neighbor node.

[0167] e. Calculate the RS curve between the end point, and perform collision detection on the RS curve, if there is no collision, the optimal path is found, and the optimal path can be obtained by starting from the target node and tracing back to the starting node along the previous node pointer.

[0168] S203, if the Open list is empty, it means that the path cannot be found, and the search fails.

[0169] Figure 11 The comparison chart of the time used by the node search algorithm provided by the application and the conventional search algorithm;

[0170] S3. After planning the optimal path from the parking start point to the parking end point, passing the planned midpoint, the vehicle reverses into the parking space according to the optimal path. When the center of the vehicle's rear axle reaches the detection point, the lateral error of the vehicle is judged. If the error exceeds the threshold, the parking path inside the garage is replanned, starting from the intersection of the vehicle and the X-axis. The detection point is the parking path outside the garage, with a distance of x from the planned midpoint. h The discrete point between the point at the meter and the planned midpoint, x h The calculation method is as follows:

[0171] x h =v·t ps

[0172] In the above formula, x h Let v be the distance between the midpoint of the planned parking path outside the garage and the detection point, v be the vehicle speed when parking, and t be the vehicle speed when entering the garage. ps The planning time taken by the planner from the midpoint of the planning process to the parking endpoint.

[0173] Because the parking path consists of a series of discrete points, it is impossible to accurately obtain the location of the detection points. Therefore, the distance x from the planned midpoint is used. h The nearest discrete point at a distance of meters is taken as the detection point; Figure 13 The image shows the test results of the detection point algorithm provided by this invention for addressing trajectory tracking errors.

[0174] S4. Connect the planned parking path outside the garage with the final parking path inside the garage to generate a complete automatic parking planning path.

[0175] Although the present invention has been described in detail in this specification with general description and specific embodiments, some modifications or improvements can be made to it, which will be obvious to those skilled in the art. Therefore, all such modifications or improvements made without departing from the present invention are within the scope of protection claimed by the present invention.

Claims

1. A two-stage parallel parking path planning algorithm considering the accumulation of tracking errors, characterized in that, Includes the following steps: S1. Periodically acquire vehicle operation data, parking space coordinate information and obstacle coordinate information, wherein the vehicle operation data includes the vehicle start point and the vehicle end point; S2. Based on vehicle operation data, parking space coordinate information, and obstacle coordinate information, the Hybrid A* algorithm is used for path search and obstacle avoidance constraints to plan the optimal path from the vehicle's starting point to the parking endpoint in real time. The path from the vehicle's starting point to the parking endpoint consists of a parking path outside the garage and a parking path inside the garage. The connection point between the parking paths outside the garage and the parking paths inside the garage is the planning midpoint. The method for real-time planning of the optimal path from the vehicle's starting point to the parking endpoint is as follows: First, find the planning midpoint and the vehicle's heading angle at the planning midpoint. Then, use the planning midpoint and the vehicle's heading angle at the planning midpoint to plan the optimal path from the vehicle's starting point to the parking endpoint in real time. During the planning process, the RS curve is extended from the current grid node to the parking endpoint pose at a fixed frequency, passing through the planning midpoint. The method for finding the planning midpoint is as follows: A coordinate system is established with the straight line where the garage boundary line is located as the X-axis, the straight line where the garage tail boundary line is located as the Y-axis, and the intersection of the X-axis and Y-axis as the origin O. The planning midpoint (x m The x-axis coordinate of (0) m The calculation method is as follows: In the formula, R min x is the minimum turning radius of the vehicle's rear axle center. s Let y be the x-coordinate of the parking endpoint. s The vertical coordinate of the parking endpoint; x m,max Determined by initial pose and parking space boundaries; S3. At the detection point, the lateral error of the vehicle is judged. If the error exceeds the threshold, the parking path in the garage is replanned. S4. Connect the planned parking path outside the garage with the final parking path inside the garage to generate a complete automatic parking planning path.

2. The two-segment parallel parking path planning algorithm considering tracking error accumulation according to claim 1, characterized in that, x m,max The calculation method is as follows: x m,max =min(x m,max1 ,x m,max2 ) When the starting ordinate is less than the critical value: When the starting ordinate is greater than the critical value: In the above formula, Kc is the length of the garage, and R C,min R is the minimum turning radius of the vehicle's right front corner. D,min This indicates the minimum turning radius of the vehicle's right rear corner. Kw is the parking space width, W is the vehicle width, and L is the vehicle wheelbase. f L represents the front overhang length of the vehicle. r θ is the rear overhang length of the vehicle, x0 is the x-coordinate of the parking starting point, and θ0 is the heading angle of the parking starting point.

3. The two-segment parallel parking path planning algorithm considering tracking error accumulation according to claim 2, characterized in that, The method for calculating the heading angle of the midpoint in the plan is as follows: In the above formula, W is the vehicle width, and L... r R is the rear overhang length of the vehicle. min The minimum turning radius of the vehicle's rear axle center; θ min The parking space is determined by its right boundary, the end point of parking, and the starting point of parking.

4. The two-segment parallel parking path planning algorithm considering tracking error accumulation according to claim 3, characterized in that, θ min The calculation method is as follows: i min =max(θ min1 ,i min2 ,i min3 ) In the above formula, P m O r This is the distance between the center of the circle on which the vehicle reverses at the starting point with the minimum turning radius and the planned midpoint.

5. The two-segment parallel parking path planning algorithm considering tracking error accumulation according to claim 1, characterized in that, The RS curve consists of at least three segments, which are composed of a circular arc C formed by the minimum turning radius and a straight line segment S, as shown below: CCC←{C|C|C C∣CC CC∣C CC β ∣C β C C∣C β C β ∣C} CSC←{CSC C∣C π / 2 SC CSC π / 2 ∣CC∣C π / 2 SC π / 2 ∣C} The symbol | represents the reversal of direction, C π / 2 This indicates that the arc length traveled to the left or right is π / 2.

6. The two-segment parallel parking path planning algorithm considering tracking error accumulation according to claim 1, characterized in that, The specific method for planning the optimal path between the vehicle's origin and destination is as follows: S201. Initialize the starting node and the target node, and add the starting node to the Open list; S202. When the Open list is not empty, perform the following steps: a. Select the node with the lowest cost value from the Open list as the current node, remove it from the Open list, and mark it as visited; b. If the current node is the target node, it means that the optimal path has been found, and the search ends; c. If the current node is not the target node, add the current node to the Closed list; d. Generate the neighbor nodes of the current node, and perform the following operations on each neighbor node: i. If the neighboring node is already in the Closed list, skip that node; ii. If a neighboring node is not in the Closed list and not in the Open list, add it to the Open list, update its previous node to the current node, and calculate its heuristic cost and actual cost. iii. If a neighboring node is not in the Closed list but in the Open list, and the cost calculated through the current node's path is lower, update the neighboring node's previous node and cost; e. Calculate the RS curve between the target node and the destination node, perform collision detection on the RS curve, and if there is no collision, find the optimal path. Starting from the target node, backtrack along the pointer of the previous node to the starting node to obtain the optimal path. S203. If the Open list is empty, it means that the path could not be found and the search failed.

7. The automatic parking path planning method according to claim 6, characterized in that, The node cost function expression is as follows: f(s) = g(s) + h(s) Where g(s) represents the actual cost from the starting point to the current node, and h(s) represents the heuristic cost from the current node to the destination.

8. The automatic parking path planning method according to claim 1, characterized in that, The detection point is located along the parking path outside the garage, at a distance x from the planned midpoint. h A discrete point located at a distance of x meters between the planned midpoint and the planned midpoint. This discrete point is adjacent to a point whose distance from the planned midpoint is x. h The point at the meter, x h The calculation method is as follows: x h =v·t ps In the above formula, x h Let v be the distance between the midpoint of the planned parking path outside the garage and the detection point, v be the vehicle speed when parking, and t be the vehicle speed when entering the garage. ps The planning time taken by the planner from the midpoint of the planning process to the parking endpoint.

Images