A method, storage medium, device, and computer program product for reverse Y-shaped parking path planning in restricted areas.

Abstract

This invention provides a reverse Y-shaped parking path planning method for restricted areas, comprising: performing structured modeling of the parking lot environment; performing a forward Hybrid A search with the parking start pose as the starting node and the parking end pose as the target node, and simultaneously performing a reverse Hybrid A search with the parking end pose as the starting node and the parking start pose as the target node, expanding the forward and reverse search nodes; selecting the current forward and reverse search nodes, establishing an RS motion model connecting the two nodes based on RS curves, and terminating the bidirectional search when an RS curve path satisfying collision-free constraints is detected; discretizing and sampling the collision-free RS curve path to obtain the reverse Y-shaped parking path. This invention also provides a storage medium, device, and computer program product for implementing the above method. This invention can improve path planning efficiency, reduce vehicle steering amplitude and direction switching frequency, ensure the smoothness and efficiency of the parking process, and improve parking safety.

Description

Technical Field

[0001] This invention belongs to the field of intelligent vehicle automatic parking and path planning technology, specifically relating to a reverse Y-shaped parking path planning method, storage medium, device and computer program product for restricted areas, which is applicable to automatic parking scenarios in parking lots with complex structures, narrow passages and dense obstacles. Background Technology

[0002] With the acceleration of urbanization and the continuous growth of motor vehicle ownership, the problem of parking resource shortage is becoming increasingly prominent. Typical parking environments such as underground parking lots and multi-story parking garages are generally characterized by limited lane width, small turning radius, dense obstacles, and high coupling of vehicle driving space, which places higher demands on the path planning capabilities of automatic parking systems.

[0003] Existing methods for automated parking path planning mainly include rule-based methods, search-based methods, and learning-based methods. Rule-based methods typically rely on predefined parking trajectory templates and lack adaptability to complex environments. While learning-based methods exhibit good generalization ability in some scenarios, they suffer from strong dependence on training data, poor model interpretability, and difficulty in meeting real-time requirements, making large-scale deployment in engineering challenging. Search-based path planning methods, due to their good interpretability and engineering feasibility, are widely used in automated parking systems. Typical methods include A... Algorithm, Hybrid A Algorithms and their improved forms. Hybrid A The algorithm introduces vehicle kinematic constraints during the search process, making the planned path feasible and giving it high application value in parking scenarios. However, within restricted areas, traditional Hybrid A... Algorithms typically employ a unidirectional search strategy, resulting in a large search space and numerous node expansions, which can easily lead to decreased planning efficiency. Furthermore, the generated parking paths frequently switch between forward and reverse movement, resulting in complex path structures that affect the smoothness and safety of the parking process. In addition, Reeds-Shepp curves, as an analytical path model that satisfies the minimum turning radius constraint, are often used for end-connection or path smoothing in parking paths. However, in existing technologies, Reeds-Shepp curves are mostly used as a post-processing method and are not deeply integrated with the search process, making it difficult to effectively constrain the overall path structure during the search phase.

[0004] Therefore, how to fully utilize the geometric relationship between the starting and ending points of parking, reduce the search scale and the number of direction changes, and generate a parking path with clear structure and good smoothness while ensuring the kinematic feasibility of the vehicle remains a key technical problem that urgently needs to be solved in the automatic parking path planning of restricted areas. Summary of the Invention

[0005] To overcome the problems existing in the prior art, the present invention provides a reverse Y-shaped parking path planning method, storage medium, device and computer program product for restricted areas, so as to realize the rapid generation of parking paths with strong executability, few reversals and good smoothness in limited space.

[0006] To achieve the above objectives, the present invention adopts the following technical solution:

[0007] (I) This invention provides a reverse Y-shaped parking path planning method for restricted areas, comprising the following steps: S1. Perform structured modeling of the parking lot environment where the parking task is located; S2. In the constructed parking environment model, execute a forward Hybrid A algorithm with the parking start pose as the starting node and the parking end pose as the target node. The search expands to include several forward search nodes; simultaneously, a reverse Hybrid A algorithm is executed, starting with the parking endpoint pose and targeting the parking origin pose. Search, expand several reverse search nodes; S3. Periodically select the current forward search node and the reverse search node to form a candidate node pair. After normalization, use Reeds-Shepp curves to generate a Reeds-Shepp motion model connecting the two nodes. Perform collision detection on each path point in the Reeds-Shepp motion model. When a Reeds-Shepp curve connection path that satisfies the collision-free constraint is detected, terminate the bidirectional search. S4. Discretize the Reeds-Shepp curve without collision constraints to obtain the coordinates and heading angles of each discrete path point. After inverse normalization of all discrete path points, the reverse Y-shaped parking path is obtained.

[0008] Further, step S1 specifically involves: acquiring the parking start and end poses, road boundaries, parking spaces, and environmental obstacle information to complete the geometric abstraction of the parking lot environment; establishing a Cartesian coordinate system in the parking area, uniformly mapping the abstracted environmental elements to this coordinate system, and constructing a parking lot environment model.

[0009] Further, in step S2, expanding the forward / reverse search nodes specifically involves: using the parking start point pose as the starting node for the forward search, constructing a forward Hybrid A... The search tree is constructed by using the parking endpoint pose as the starting node for the reverse search, thus creating a reverse Hybrid A. Search tree; in forward and reverse Hybrid A During the search process, for each parent node to be expanded, the steering angle is discretely sampled in combination with the vehicle's maximum steering angle, and a corresponding child node is generated in a single sampling. Multiple path points are inserted between the parent node and the child node and collision detection is performed. When all path points meet the collision detection requirements, the expansion result from the parent node to the child node is valid; otherwise, the node expansion operation is re-executed.

[0010] Furthermore, in step S2, forward and reverse HybridA nodes are constructed respectively in the expanded forward / reverse search nodes. The algorithm's cost function selects the node with the smallest cost function value as the next expansion parent node during each node expansion, enabling bidirectional search to generate parking paths with an inverted Y-shaped structure; the cost function includes the actual cost function and / or the heuristic cost function.

[0011] Furthermore, the actual cost function includes at least the following cost terms: reversing cost term, reversing cost term, steering angle magnitude cost term, and steering switching cost term, specifically:

[0012] in, cost The cumulative cost of the parent node; This represents the distance from the current node to its parent node; u This indicates the sampling angle corresponding to the current node. Indicates sampling to the left (abs) u ), Indicates sampling to the right (abs) u ), where abs() is the absolute value symbol; father.steer Indicates the sampling angle of the parent node; C.BACKWARD_COST , C.GEAR_COST , C.STEER_ANGLE_COST as well as C.STEER_CHANGE_COST These represent the reversing penalty factor, the direction-changing penalty factor, the steering angle penalty factor, and the steering switching penalty factor, respectively.

[0013] Furthermore, the construction process of the heuristic cost function is as follows: For positive Hybrid A Algorithm, when the starting heading angle Greater than the destination heading angle And when the heading angle of the path curve connecting the starting point and the ending point is not decreasing, or when the heading angle of the starting point is... Less than the destination heading angle Furthermore, when the heading angle of the path curve connecting the start and end points is not increasing, a heuristic penalty factor is added to the cost of the child nodes. ; For reverse Hybrid A Algorithm, when the starting heading angle Greater than the destination heading angle And when the heading angle of the path curve connecting the starting point and the ending point is not increasing, or when the heading angle of the starting point is... Less than the destination heading angle Furthermore, when the heading angle of the path curve connecting the start and end points is not decreasing, a heuristic penalty factor is added to the cost of the child nodes. .

[0014] Further, in step S3, the normalization process includes: synchronously translating and rotating the forward search node and the reverse search node in the candidate node pair, so that the lateral coordinate, longitudinal coordinate, and heading angle of the forward search node are all 0; scaling the translated and rotated forward search node and the reverse search node by a scaling amount of... , It is the minimum turning radius of the vehicle's equivalent two-wheeled bicycle model.

[0015] Further, in steps S2 and S3, the collision detection method is as follows: taking the rear axle center of the vehicle as the current path point position, the vehicle center coordinates are calculated based on the rear axle center coordinates and the vehicle heading angle; a circular first-level collision detection area is divided with the vehicle center as the center and the maximum distance from the vehicle center to the vehicle outline vertex as the radius, and it is determined whether there is an obstacle within the first-level collision detection area; when there is no obstacle within the first-level collision detection area, the path point is determined to meet the no-collision requirement; when there is an obstacle within the first-level collision detection area, a vector from the vehicle center to the obstacle is first constructed, the vehicle center is placed at the origin of the coordinate system through translation transformation, and then the vehicle heading is aligned with the coordinate axis through rotation transformation, thereby obtaining the coordinates of the obstacle in the vehicle coordinate system. When abs( Less than half the width of the vehicle or abs ( If the obstacle is less than half the length of the vehicle, then the obstacle is located within the vehicle's outline, and the path point is determined not to meet the no-collision requirement.

[0016] Furthermore, the horizontal and vertical coordinates of the vehicle center are:

[0017] in, This indicates the vehicle's heading angle at the current moment; b p x , b p x These are the x and y coordinates of the vehicle's rear axle center at the current moment. c p x , c px These are the x and y coordinates of the vehicle's center at the current moment. It refers to the vehicle's semi-wheelbase.

[0018] Furthermore, the calculation process for the translation and rotation transformations is as follows: ; ; in, These are the assumed coordinates of the obstacle. The vector from the vehicle's center to the obstacle; Represents a vector The angle of clockwise rotation; This represents the coordinates of the obstacle in the vehicle coordinate system, where This indicates the position of the obstacle relative to the center of the vehicle in a direction perpendicular to the vehicle's flight path. This indicates the position of the obstacle relative to the center of the vehicle in a direction parallel to the vehicle's flight path.

[0019] (ii) The present invention also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the reverse Y-shaped parking path planning method for restricted areas as described in (i) of the present invention.

[0020] (III) The present invention also provides an electronic device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the reverse Y-shaped parking path planning method for restricted areas as described in the present invention (I).

[0021] (iv) The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the reverse Y-shaped parking path planning method for restricted areas as described in the present invention (i).

[0022] The beneficial effects of this invention are: (1) This invention employs a bidirectional search strategy and connection mechanism, simultaneously utilizing Hybrid A at the parking start and end points. The algorithm searches and uses RS curves to detect and connect bidirectional search nodes in real time, thereby reducing the expansion of redundant nodes. Compared with traditional search strategies, this design can reduce search time and improve the efficiency of path planning. (2) This invention designs an inverted Y-shaped path structure. Addressing the issue of frequent changes in driving direction during parking within restricted areas, a bidirectional Hybrid A is designed. The algorithm's cost function guides the generation of an inverse Y-shaped parking path, thereby reducing the vehicle's steering radius and the number of direction changes during the parking process, ensuring a smooth and efficient parking process. (3) The present invention establishes a novel graded collision detection model. Through geometric expansion and layered detection, it achieves a smaller computational load to determine the safe distance between the vehicle and the obstacle, thus ensuring the safety of the parking process in the restricted area. Attached image description: Figure 1 This is a flowchart of the reverse Y-shaped parking path planning method applicable to restricted areas according to the present invention; Figure 2 This is a schematic diagram illustrating the principle of solving the RS curve for parking path planning in this invention. Figure 3 This is a schematic diagram and visualization of the design principle of the heuristic cost function based on geometric principles of this invention. Figure 4 This is a schematic diagram and design principle diagram of the hierarchical collision detection model of the present invention; Figure 5 The reverse Y-shaped parking path planning method proposed in this invention differs from the traditional Hybrid A The parking path curves obtained by the algorithm in simulations for different parking spaces; Figure 6 These are the ROS real vehicle modeling diagram and experimental scenario modeling diagram of this invention; Figure 7 A slice diagram of the actual vehicle driving process along the path planned in this invention; Figure 8 This is a comparison chart of parking path curves obtained from physical system verification and simulation verification of the present invention. Detailed Implementation

[0023] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions 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, 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.

[0024] Example 1 This invention provides a reverse Y-shaped parking path planning method for restricted areas. Figure 1 The flowchart of this method includes the following steps: Step 1: Perform structured modeling of the parking lot environment where the parking task takes place. A structured model of the parking lot environment is performed to provide a unified spatial representation for subsequent path search and collision detection. Specifically, key information such as the starting and ending poses of the vehicle, road boundaries, parking space locations, and environmental obstacles within the parking area is acquired to provide a geometric abstract description of the parking lot environment. Simultaneously, a Cartesian coordinate system is established within the parking area to uniformly map vehicle pose information and environmental elements to the same coordinate system, thus providing a consistent spatial reference for bidirectional path search, Reeds-Shepp curve connection, and collision detection.

[0025] Step Two: Bidirectional Hybrid A Path search Introduce bidirectional Hybrid A into the existing parking environment model. A path search mechanism is implemented to improve the search efficiency and feasibility of parking path planning, specifically as follows: S1. Constructing constraints: In bidirectional Hybrid A Before path search, construct forward and reverse Hybrid A respectively. The algorithm's cost function selects the node with the smallest cost function value as the next expansion parent node during each node expansion. This enables bidirectional search to generate parking paths with an inverted Y-shaped structure and allows the search process to generate feasible path segments that satisfy the vehicle's minimum turning radius and forward / backward constraints, thus creating conditions for subsequent bidirectional path connections.

[0026] The cost function includes an actual cost function and a heuristic cost function. By introducing cost terms such as reversing, changing direction, steering angle, and steering change into the actual cost function to constrain the smoothness of the local path, and by incorporating the geometric relationship between the parking start and parking end points into the heuristic cost function to guide the search direction, the path tends to reduce the steering amplitude and the number of direction changes in its overall structure, thereby improving the stability and efficiency of the parking process.

[0027] (1) Actual cost function: includes reversing cost, reversing cost, steering angle amplitude cost, and steering switching cost, specifically:

[0028] in, cost The cumulative cost of the parent node; This represents the distance from the current node to its parent node; u This indicates the sampling angle corresponding to the current node. Indicates sampling to the left (abs) u ), Indicates sampling to the right (abs) u ), where abs() is the absolute value symbol; father.steer Indicates the sampling angle of the parent node; C.BACKWARD_COST , C.GEAR_COST , C.STEER_ANGLE_COST as well as C.STEER_CHANGE_COST These represent the reversing penalty factor, the direction-changing penalty factor, the steering angle penalty factor, and the steering switching penalty factor, respectively.

[0029] By constructing a forward Hybrid A Algorithms and Reverse Hybrid A The actual cost function shared by the algorithm aims to reduce operations that are detrimental to parking smoothness and efficiency, such as frequent reversing, changing direction, excessive steering angles, and frequent steering switching during the path planning process.

[0030] (2) The process of constructing the heuristic cost function is as follows: For positive Hybrid A Algorithm, if the starting heading angle Greater than the destination heading angle Then the heading angle of the path curve connecting the starting point and the ending point should gradually decrease; if the heading angle of the starting point is... Less than the destination heading angle The heading angle of the path curve connecting the starting and ending points should gradually increase. For a reverse Hybrid A... Algorithm, if the starting heading angle Greater than the destination heading angle Then the heading angle of the path curve connecting the starting point and the ending point should gradually increase; if the heading angle of the starting point is... Less than the destination heading angle The heading angle of the path curve connecting the starting point and the ending point should gradually decrease.

[0031] like Figure 3 As shown, Figure 3 As shown by the blue line, based on the geometric characteristics of the inverted Y-shaped curve, for a forward Hybrid A... In terms of the algorithm, if the starting heading angle Greater than the destination heading angle Then the heading angle of the path curve connecting the starting point and the ending point should gradually decrease. Taking the vehicle's orientation at the parent node as a reference, the node's expansion direction should be right front and left rear. At this point, it should be in the left front and right rear directions of the parent node (…). Figure 3 middle The surrounding pink area A heuristic penalty factor is added to the cost of the child nodes of ). Conversely, if the starting heading angle is... Less than the destination heading angle Then it should be in the direction of the parent node's right front and left back ( Figure 3 The white area in the middle Accumulate in the cost of the child nodes of ) ; Forward Hybrid A The heuristic cost calculation process is as follows:

[0032] in, The heading angle is the final pose. The starting position and heading angle; The cost of the current node is the sum of the actual cost and the heuristic cost. This represents a heuristic penalty factor, which introduces a heuristic penalty term into the cost of the current node for node expansion directions that are not conducive to forming an anti-Y path structure.

[0033] For reverse Hybrid A Algorithm, and forward Hybrid A The algorithm logic is the same, but the solution process is reversed. If the starting heading angle... Greater than the destination heading angle Then the heading angle of the path curve connecting the starting point and the ending point should gradually increase; conversely, if the heading angle of the starting point is... Less than the destination heading angle The heading angle of the path curve connecting the starting point and the ending point should gradually decrease.

[0034] By constructing a heuristic cost function, the aim is to guide the search direction from the overall path structure level, so that the generated parking path presents an inverse Y-shaped geometric feature, thereby reducing the number of times the vehicle changes direction in the entire path.

[0035] (3) The actual cost function constructed above is fused with the heuristic cost function to form a function for bidirectional Hybrid A The combined cost function for algorithm search and node expansion is calculated as follows:

[0036] Where g(n) represents the actual cost of the current node; h(n) represents the heuristic cost based on the inverse Y-shape.

[0037] S2, Expanding forward / reverse search nodes Using the parking start pose as the starting node of the forward search and the parking end pose as the target node of the forward search, perform a forward Hybrid A algorithm. Search, build a positive Hybrid A Search tree. Simultaneously, reverse Hybrid A is performed using the parking endpoint pose as the starting node for the reverse search and the parking origin pose as the target node. Search and build a reverse Hybrid A Search tree. In a reverse search, the vehicle's direction of travel is opposite to that in a forward search.

[0038] For each node to be expanded, the steering angle is discretely sampled based on the vehicle's maximum steering angle. Each sample can expand one node, with the original node and the expanded node having a parent-child relationship. The expansion direction of the node is consistent with the vehicle's driving direction, including right turn forward, right turn backward, left turn forward, left turn backward, forward, and backward. Left turn and forward are both positive directions.

[0039] Let the maximum steering angle of the vehicle be The number of sampling steps is , If the distance between nodes is represented by the angle, then the step size for sampling the steering angle is:

[0040] S3. Multiple path points are generated between the parent and child nodes in the forward / backward expansion. During node expansion, Hybrid A The algorithm inserts multiple path points between the parent and child nodes through sampling, assuming the node... The corresponding sampling angle is Then from the parent node to The process of generating path points:

[0041] in, and They represent the first The nodes sampled and their corresponding turning angles; , and These represent the horizontal coordinates, vertical coordinates, and heading angle of the current path point, respectively. , and These represent the horizontal coordinate, vertical coordinate, and heading angle of the next path point of the current node, respectively. Indicates the direction of vehicle movement; when the vehicle is moving forward, When the vehicle reverses, ; BF This indicates the vehicle's wheelbase. The value of is very small and can be regarded as a straight line. Therefore, it can be considered that the position coordinates of the next path point are calculated using a linearization method. The calculation result is the instantaneous turning radius of the vehicle. According to the arc length theorem, The product of these is the vehicle's steering angle, and the heading angle of the next path point is calculated.

[0042] Repeat the above path point update process iteratively. Once, and in each iteration, , and Updated to the score obtained in the previous round. , and This will give you the path from the parent node to the node. All path points.

[0043] S4, Collision Detection During node expansion, the safe distance between vehicles and environmental obstacles is effectively constrained to ensure the safety of the planned parking path. By introducing a hierarchical collision detection method, potential collision risks are first quickly screened using a simplified geometric model, and then the screened possible collision scenarios are accurately detected, thereby reducing the overall computational complexity while ensuring detection accuracy. Nodes with collision risks are promptly removed to prevent unsafe paths from participating in subsequent search and connection processes.

[0044] Specifically, collision detection is performed on multiple path points from the parent node to the child node during forward / backward expansion. If all path points meet the collision detection requirements, the expansion result from the parent node to the child node is valid; otherwise, the node expansion operation is re-executed. The collision detection method is as follows: Using the center of the vehicle's rear axle as the current path point, calculate the vehicle's center coordinates based on the coordinates of the rear axle center and the vehicle's heading angle.

[0045] The horizontal and vertical coordinates of the vehicle center are:

[0046] in, This indicates the vehicle's heading angle at the current moment; b p x , b p x These are the x and y coordinates of the vehicle's rear axle center at the current moment. c p x , c p x These are the x and y coordinates of the vehicle's center at the current moment. It refers to the vehicle's semi-wheelbase.

[0047] Distance from vehicle center to the apex of vehicle profile The calculation process is as follows:

[0048] in, and These represent half the vehicle length and half the vehicle width, respectively.

[0049] A circular primary collision detection area is defined with the vehicle center as the center and the distance from the vehicle center to the apex of the vehicle outline as the radius. The primary collision detection area is used to detect whether there are obstacles. If there are no obstacles in the primary collision detection area, the path point is determined to meet the no-collision requirement. If there are obstacles in the primary collision detection area, the obstacles located in the area are output as possible collision points and enter the secondary collision detection. The secondary collision detection accurately determines the possible collision points.

[0050] First, a vector is constructed pointing from the vehicle's center to the obstacle (potential collision point). A translation transformation is then used to place the vehicle's center at the origin. Next, a rotation transformation is used to align the vehicle's heading with the coordinate axes, thus obtaining the obstacle's coordinates in the vehicle's coordinate system. ; The calculation process for translation and rotation transformations is as follows: ; ; in, These are the assumed coordinates of the obstacle. The vector from the vehicle's center to the obstacle; Represents a vector The angle of clockwise rotation; This represents the coordinates of the obstacle in the vehicle coordinate system, where This indicates the position of the obstacle relative to the center of the vehicle in a direction perpendicular to the vehicle's flight path. This indicates the position of the obstacle relative to the center of the vehicle in a direction parallel to the vehicle's flight path.

[0051] When abs( Less than half the width of the vehicle or abs ( When the length of the obstacle is less than half the vehicle length, the obstacle lies within the vehicle's outline and will collide with the vehicle. In this case, the path point where the center of the vehicle's rear axle is located is considered not to meet the no-collision requirement. When abs( ) greater than half the vehicle width or abs( If the obstacle is greater than half the vehicle length, then the obstacle is located outside the vehicle outline, and the path point is determined to meet the no-collision requirement.

[0052] Step 3: Perform an analytical connection between the Reeds-Shepp curve and the two nodes. In the forward Hybrid A Search and Reverse Hybrid A During the search process, it is determined whether a valid connection can be achieved through a Reeds-Shepp curve that satisfies vehicle kinematic constraints. The pose relationship between forward and reverse search nodes is detected, and a Reeds-Shepp curve is used to analytically connect the two nodes. Discretization and collision detection are then performed on the connection path. When a Reeds-Shepp curve connection path that satisfies no collision constraints exists, the forward search path segment, the reverse search path segment, and the connection path segment are concatenated into a complete parking path, thus terminating the bidirectional search process. The specific steps are as follows: S1. Select candidate node pairs In the forward Hybrid A Search tree and reverse Hybrid A During the search tree process, the current forward search node and the reverse search node are periodically selected to form a candidate node pair, where each candidate node contains the vehicle's position coordinates and heading angle information.

[0053] S2, Normalization Processing Assume the poses of the starting and ending points of the path planning are respectively and The minimum turning radius of the vehicle's equivalent two-wheeled bicycle model is Before solving the RS curve, the starting pose and ending pose are sequentially transformed by translation, rotation, and scaling.

[0054] 1) Perform a translation transformation: The amount of the translation transformation is The starting point of the path planning is moved to the origin, and the poses of the starting and ending points are updated after the translation. and ,in , ; 2) Perform rotation transformation: rotate counterclockwise The heading angle of the starting point is changed to 0, and the poses of the starting and ending points are updated after translation and rotation transformations. The specific calculation process for rotation transformation is as follows:

[0055]

[0056] 3) Perform scaling transformation: The scaling change is... The minimum turning radius of the vehicle is normalized to 1. The specific calculation process for scaling transformation is as follows:

[0057] in, This indicates the final updated endpoint pose.

[0058] After the above transformation, the standardized starting and ending poses for solving the RS curve are obtained; S3, Reeds-Shepp Curve Analysis For the normalized starting and ending poses, a Reeds-Shepp curve is introduced as the connection model. The Reeds-Shepp curve consists of circular arc segments and straight line segments with fixed turning radii, allowing the vehicle to move forward or backward under kinematic constraints. The basic motions of the Reeds-Shepp curve include six basic motion forms: left turn forward, left turn backward, right turn forward, right turn backward, straight forward, and straight backward. Left turn and forward are defined as positive directions, and right turn and backward are defined as negative directions. The normalized starting and ending poses are successively substituted into the analytical motion models corresponding to each basic motion of the Reeds-Shepp curve. By establishing a system of equivalent equations between the basic motion parameters, the arc lengths of each motion segment constituting the RS curve are solved. Let the transformed starting pose be... The calculation formulas for the basic motions that make up the RS curve are shown in Table 1.

[0059] Table 1 - Calculation Formulas for the Components of the RS Curve

[0060] The basic motion includes the form of motion and the direction of motion, where the form of motion refers to the direction of motion along the circumference, including... , and The symbols indicate right turn, left turn, and straight ahead, respectively; the direction of movement includes "+" and "-", indicating forward and backward movement, respectively. ~ The arc lengths of the six basic motions are represented as variables to be determined, and a Reeds-Shepp motion model connecting two nodes is generated.

[0061] S4, Collision Detection Perform collision detection for each path point in the Reeds-Shepp curve path to determine whether the vehicle has collided with the road boundary, parking space boundary or obstacle. The collision detection method is the same as in step two (S4).

[0062] When any Reeds-Shepp curve connection path that satisfies both kinematic and collision constraints is detected, a positive Hybrid A is determined. Search tree and reverse Hybrid A The search trees successfully merge and the bidirectional search process terminates. If no feasible solution is found after trying all the basic forms of RS curves and their transformations, it is determined that there is no feasible RS curve connection path between the current forward search node and the reverse search node.

[0063] Step 4: Generate a reverse Y-shaped parking path The Reeds-Shepp curve connection path generated above cannot be directly used as the driving path of the intelligent vehicle. It needs to be discretized through sampling to meet the vehicle's kinematic characteristics. Then, an inverse transformation corresponding to the aforementioned pose normalization process is applied to the feasible path to restore the RS curve path to the original parking lot coordinate system. Specifically, this includes: The Reeds-Shepp curve without collision constraints is discretized to obtain the coordinates and heading angles of each discrete path point. Assume the currently sampled motion is... The initial pose of the movement is The sampling step size is Then the first Results of the second sampling:

[0064] in Indicates the first The arc length of the second sampling is calculated because the RS path is solved with the vehicle model having a turning radius of 1. It can also represent the first The radian of the next sample; and They represent the first The increments of the position coordinates obtained from the sub-sampling in directions parallel and perpendicular to the vehicle's motion; and They represent the first The horizontal and vertical coordinates of the pose obtained from the second sampling are obtained by summing the increments in the vehicle's motion direction after rotation to a Cartesian coordinate system with the initial position coordinates. Indicates the first The heading angle of the pose obtained from the second sampling is the sum of the initial heading angle and the sampled radian angle.

[0065] Since the RS curve solves for the path between the starting and ending points after pose transformation, an inverse pose transformation is also required for the discrete path points to ultimately obtain the inverse Y-shaped parking path from the starting point to the ending point. The inverse Y-shaped parking path starts from the forward Hybrid A... Algorithm-generated path segments, reverse Hybrid A The path segments generated by the algorithm, along with the path segments formed by connecting them via Reeds-Shepp curves, together form an overall inverted Y-shaped structure. By sequentially piecing together these path segments, a complete parking path that satisfies both vehicle kinematic and safety constraints is obtained, serving as the final planning result output of the automatic parking system.

[0066] In one embodiment of the present invention, a computer-readable storage medium is also provided, storing a computer program that causes a computer to execute the reverse Y-shaped parking path planning method applicable to restricted areas.

[0067] In one technical solution of the present invention, an electronic device is also provided, including: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the reverse Y-shaped parking path planning method applicable to restricted areas.

[0068] In one technical solution of the present invention, a computer program product is also provided, including a computer program that, when executed by a processor, implements the reverse Y-shaped parking path planning method applicable to restricted areas.

[0069] The computer storage medium can be a tangible medium that contains or stores programs for use by or in conjunction with an instruction execution system, apparatus, or device. The computer storage medium can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination thereof. As more specific examples, the computer storage medium can include electrical connections based on one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, or portable compact disc read-only memory (CD). ROM, optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0070] Example 2: Algorithm Simulation Verification An experimental verification platform was built and parking experiments were conducted to verify the feasibility and effectiveness of the reverse Y-shaped parking path planning method in Example 1.

[0071] Two parking spaces with different orientations were selected, and the above-mentioned reverse Y-shaped parking path planning method and the traditional Hybrid A method were compared. The algorithm was used to simulate parking path planning, and the results were as follows: Figure 5 The simulation results are shown below. Figure 5As shown, the area bordered in yellow represents the restricted parking zone. Both methods plan collision-free paths when facing the same type of parking space, but compared to the traditional Hybrid A... The algorithm, an inverse Y-shaped parking path planning method, reduces the number of direction changes during the parking process and results in a smoother overall curve.

[0072] Furthermore, to verify the efficiency of the inverse Y-shaped parking path planning method, in Figure 5 For the parking scenario of parking space 1, five parking path planning simulations were performed on a single computer using two different methods. The time consumption of the two methods in the five simulations is shown in Table 2.

[0073] Table 2 - Time consumption for parking path planning simulation using two methods

[0074] Method 1 represents the inverse Y-shaped parking path planning method, and Method 2 represents the traditional Hybrid A algorithm.

[0075] As shown in Table 2, the reverse Y-shaped parking path planning method has a shorter and more stable time consumption than the traditional Hybrid A method. The algorithm's time consumption. Further calculations show that in 5 parking path planning simulations, the traditional Hybrid A... The algorithm's average execution time is 0.404s, while the inverse Y-shaped parking path planning method's average execution time is 0.131s, which is faster than the traditional Hybrid A The algorithm was 67.6% lower.

[0076] Example 3: Physical System Verification To verify the proposed bidirectional Hybrid A To assess the feasibility and effectiveness of the inverse Y-shaped parking path planning method, a mobile robot experimental platform based on the Robot Operating System (ROS) was built, and an experimental environment corresponding to a real parking lot scenario was constructed. First, a ROS-based vehicle platform was established, and a vehicle kinematic model was constructed to ensure that the path planning process met the actual steering constraints of the vehicle. Then, the experimental scenario was scaled down according to the actual parking lot dimensions to construct an experimental environment model including parking spaces, road boundaries, and obstacle information. Based on this, the proposed parking path planning algorithm was deployed. By setting the starting and ending poses of the parking, an inverse Y-shaped parking path was generated, and the vehicle was driven to complete the parking operation according to the planned path. The feasibility and parking effect of the proposed method were experimentally verified by recording the vehicle's motion trajectory and the parking process.

[0077] Specifically, the steps include the following: Step 1: Build a ROS mobile robot platform to validate the path planning method.

[0078] The platform consists of a motion chassis, sensor modules, actuators, and a main control computer. The main control computer runs the Robot Operating System (ROS), enabling functions such as environmental perception, path planning, and control execution. The experimental system uses a ROS node mechanism for module communication: sensor nodes collect environmental information and generate map data, navigation nodes handle path planning and control command generation, and chassis control nodes drive the vehicle according to these commands. Experimental scenarios were established to verify the accuracy of the vehicle steering model establishment and simplification methods, as well as the inverse Y-shaped parking path planning method. According to the industry standard "Garage Building Design Code," the length and width of a microcar are 3.8 m and 1.6 m, respectively, while the length and width of a standardized parking space are 5.3 m and 2.6 m, respectively, and the lane width is 6 m. Following the principle of proportional scaling, when the length and width of the ROS car are 0.78 m and 0.55 m, respectively, the length and width of the parking space should be 1.1 m and 0.9 m, respectively, and the lane width should be 2.1 m. Furthermore, since the inverse Y-shaped parking path planning method expands the obstacle by 0.2 m, to ensure the ROS car can park smoothly, the length and width of the parking space should each be expanded by 0.2 m, resulting in a length and width of 1.3 m and 1.1 m respectively. Figure 6 As shown.

[0079] Step 2: Place the ROS car at the starting point of the path planning within the experimental scenario. Log in to the car on the virtual machine, start the navigation stack and RViz, and publish the poses of the starting and ending points of the path planning to the navigation stack. The navigation stack subscribes to the map topic, calls the custom path planner, plans the path from the starting point to the ending point, and publishes a control topic. The chassis node subscribes to the control topic and controls the car to drive according to the planned path within the experimental scenario. The driving process is as follows: Figure 7 As shown.

[0080] Step 3: Record the vehicle's driving trajectory using the RViz visualization tool and compare it with the simulated path. Experimental results show that the vehicle can smoothly enter the target parking space along the planned trajectory. The shape of the experimental path is basically consistent with the simulation result, verifying the feasibility and engineering application value of the parking path planning method proposed in this invention. The experimental path and simulation results are shown below. Figure 8 As shown.

[0081] This invention relates to a reverse Y-shaped parking path planning method applicable to restricted areas. It employs a bidirectional search strategy and connection mechanism, utilizing Hybrid A at both the parking start and end points. The algorithm searches and uses RS curves to detect and connect bidirectional search nodes in real time, thereby reducing the expansion of redundant nodes. Compared with traditional search strategies, this design can reduce search time and improve the efficiency of path planning. This invention designs an inverted Y-shaped path structure and, addressing the problem of frequent changes in driving direction during parking in restricted areas, designs a bidirectional Hybrid A The algorithm's cost function guides the generation of an inverted Y-shaped parking path. This reduces the vehicle's steering radius and the number of direction changes during parking, ensuring a smooth and efficient parking process. Furthermore, this invention establishes a novel hierarchical collision detection model. Through geometric expansion and layered detection, it achieves a relatively low computational cost in determining the safe distance between the vehicle and obstacles, ensuring the safety of parking within restricted areas.

[0082] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0083] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A reverse Y-shaped parking path planning method for restricted areas, characterized in that, Includes the following steps: S1. Perform structured modeling of the parking lot environment where the parking task is located; S2. In the constructed parking environment model, execute a forward Hybrid A algorithm with the parking start pose as the starting node and the parking end pose as the target node. Search and expand the forward search nodes; simultaneously, perform reverse Hybrid A with the parking endpoint pose as the starting node and the parking origin pose as the target node. Search, expand to several search nodes; S3. Periodically select the current forward search node and the reverse search node to form a candidate node pair. Use Reeds-Shepp curves to generate a Reeds-Shepp motion model connecting the two nodes. Perform collision detection on each path point in the Reeds-Shepp motion model. When a Reeds-Shepp curve connection path that satisfies the collision-free constraint is detected, terminate the bidirectional search. S4. Discretize the Reeds-Shepp curve without collision constraints to obtain the coordinates and heading angles of each discrete path point, thus obtaining the reverse Y-shaped parking path.

2. The reverse Y-shaped parking path planning method for restricted areas according to claim 1, characterized in that: In step S2, expanding the forward / reverse search nodes specifically involves: Using the parking origin pose as the starting node for the forward search, a forward Hybrid A is constructed. The search tree is constructed by using the parking endpoint pose as the starting node for the reverse search, thus creating a reverse Hybrid A. Search tree; In forward and reverse Hybrid A During the search process, for each parent node to be expanded, the steering angle is discretely sampled in combination with the vehicle's maximum steering angle, and a corresponding child node is generated in a single sampling. Insert multiple path points between the parent node and the child node and perform collision detection. If all path points meet the collision detection requirements, the expansion result from the parent node to the child node is valid; otherwise, re-execute the node expansion operation.

3. The reverse Y-shaped parking path planning method for restricted areas according to claim 1, characterized in that: In step S2, forward and reverse Hybrid A nodes are constructed respectively in the expanded forward / reverse search nodes. The algorithm's cost function selects the node with the smallest cost function value as the next parent node for expansion each time a node is expanded. The cost function includes the actual cost function and / or the heuristic cost function. The actual cost function includes the following cost items: reversing cost item, reversing cost item, steering angle amplitude cost item, and steering switching cost item, specifically: in, cost The cumulative cost of the parent node; This represents the distance from the current node to its parent node; u This indicates the sampling angle corresponding to the current node. Indicates sampling to the left. Indicates sampling to the right; father.steer Indicates the sampling angle of the parent node; C.BACKWARD_COST , C.GEAR_COST , C.STEER_ANGLE_COST as well as C.STEER_CHANGE_COST These represent the reversing penalty factor, the direction-changing penalty factor, the steering angle penalty factor, and the steering switching penalty factor, respectively.

4. The reverse Y-shaped parking path planning method for restricted areas according to claim 3, characterized in that: The process of constructing the heuristic cost function is as follows: For forward Hybrid A Algorithm, when the starting heading angle Greater than the destination heading angle And when the heading angle of the path curve connecting the starting point and the ending point is not decreasing, or when the heading angle of the starting point is... Less than the destination heading angle Furthermore, when the heading angle of the path curve connecting the start and end points is not increasing, a heuristic penalty factor is added to the cost of the child nodes. ; For reverse Hybrid A Algorithm, when the starting heading angle Greater than the destination heading angle And when the heading angle of the path curve connecting the starting point and the ending point is not increasing, or when the heading angle of the starting point is... Less than the destination heading angle Furthermore, when the heading angle of the path curve connecting the start and end points is not decreasing, a heuristic penalty factor is added to the cost of the child nodes. .

5. The reverse Y-shaped parking path planning method for restricted areas according to claim 2, characterized in that: In steps S2 and S3, the collision detection method is as follows: Using the rear axle center of the vehicle as the current path point, calculate the vehicle center coordinates based on the rear axle center coordinates and the vehicle heading angle; divide the area into a circular first-level collision detection zone with the vehicle center as the center and the maximum distance from the vehicle center to the apex of the vehicle outline as the radius, and determine whether there are obstacles within the first-level collision detection zone. If there are no obstacles within the primary collision detection area, the path point is determined to meet the no-collision requirement. When an obstacle exists within the Level 1 collision detection area, a vector pointing from the vehicle's center to the obstacle is first constructed. A translation transformation is then used to place the vehicle's center at the origin, followed by a rotation transformation to align the vehicle's heading with the coordinate axes, thus obtaining the obstacle's coordinates in the vehicle's coordinate system. When abs( Less than half the width of the vehicle or abs ( If the obstacle is less than half the length of the vehicle, then the obstacle is located within the vehicle's outline, and the path point is determined not to meet the no-collision requirement.

6. The reverse Y-shaped parking path planning method for restricted areas according to claim 5, characterized in that: The horizontal and vertical coordinates of the vehicle center are: in, This indicates the vehicle's heading angle at the current moment; b p x , b p x These are the x and y coordinates of the vehicle's rear axle center at the current moment. c p x , c p x These are the x and y coordinates of the vehicle's center at the current moment. It refers to the vehicle's semi-wheelbase.

7. The reverse Y-shaped parking path planning method for restricted areas according to claim 5, characterized in that: The calculation process for the translation and rotation transformations is as follows: ； ； in, These are assumed obstacle coordinates; The vector from the vehicle's center to the obstacle; Represents a vector The angle of clockwise rotation; This represents the coordinates of the obstacle in the vehicle coordinate system, where This indicates the position of the obstacle relative to the center of the vehicle in a direction perpendicular to the vehicle's flight path. This indicates the position of the obstacle relative to the center of the vehicle in a direction parallel to the vehicle's flight path.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the reverse Y-shaped parking path planning method for restricted areas as described in any one of claims 1 to 7.

9. An electronic device, characterized in that, include: The system includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the reverse Y-shaped parking path planning method for restricted areas as described in any one of claims 1 to 7.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the reverse Y-shaped parking path planning method for restricted areas as described in any one of claims 1 to 7.

Images