Parking path planning method and device, storage medium and electronic equipment

By using vehicle kinematics models to generate initial expansion trees in narrow parking spaces and obstacle-intensive environments, and combining RRT algorithm for path search, the problem of automatic parking trajectory search failure is solved, and the search efficiency and success rate are improved.

CN118896624AActive Publication Date: 2024-11-05GUANGZHOU AUTOMOBILE GROUP CO LTD

Patent Information

Application Number
CN202310493773.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2023-05-04
Publication Date
2024-11-05
Estimated Expiration
2043-05-04

AI Technical Summary

Technical Problem

In scenarios where there are many narrow parking spaces and obstacles, automatic parking trajectory search is prone to failure in search.

Method used

A parking path planning method is adopted to obtain surrounding environment information, and the first path search is performed based on the preset vehicle kinematic model to generate the initial expansion tree, and the second path search is performed in combination with the RRT algorithm to improve the random expansion tree The growth process reduces the number of invalid expansions and avoids search failures caused by collision detection.

Benefits of technology

It effectively solves the problem of parking trajectory search failure in scenarios with many narrow parking spaces and obstacles, and improves the search rate and success rate.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118896624A_ABST
    Figure CN118896624A_ABST
Patent Text Reader

Abstract

The invention relates to a parking path planning method and device, a storage medium and electronic equipment, and the method comprises the steps: obtaining surrounding environment information from a target parking space to a current position of a vehicle, and obtaining obstacle information, a starting point and a terminal point according to the surrounding environment information; based on a preset vehicle kinematics model, performing first path search according to the obstacle information, the starting point and the ending point to obtain an initial extension tree; on the basis of the initial extension tree, performing a second path search according to the obstacle information, the starting point, the ending point and a preset RRT algorithm to obtain a random extension tree; wherein the random extension tree comprises a plurality of nodes, and node information of each node comprises a position coordinate and a course angle corresponding to the node; and obtaining a parking planning path according to the random extension tree. According to the method and the device, the problem that parking track search is easy to fail in a scene with a narrow parking space and many obstacles at present can be solved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present application relates to the technical field of automatic parking, and in particular to a parking path planning method and device, storage medium, and electronic device thereof. Background Art

[0002] Automatic parking is a vehicle-assisted driving technology that uses sensors and computer algorithms to identify the target parking space and control the vehicle to perform automatic driving operations, which can help the driver park the vehicle in the target parking space. The automatic parking system can measure the size of the target parking space, calculate the best parking path and steering angle based on the vehicle size and environmental conditions, and then automatically control the steering wheel, accelerator and brake to achieve automatic parking of the vehicle. Automatic parking technology can reduce the driver's stress and driving skill requirements when parking, improve parking efficiency and safety, and reduce the risk of collision between the vehicle and surrounding objects. Automatic parking technology is currently widely used in many car brands, and with the continuous development and popularization of technology, it will become an important part of future car driving.

[0003] The RRT (Rapidly-exploring Random Tree) algorithm for automatic parking is a path planning algorithm based on a tree data structure. It can search for valid paths in high-dimensional space and find the optimal path from the starting point to the end point. The RRT algorithm is an incremental sampling search algorithm. Its advantage is that it tends to expand to open unexplored areas and has probabilistic completeness. When the time is long enough and the number of iterations is large enough, all areas will be explored. However, the RRT algorithm is a pure random search algorithm that is insensitive to the type of environment. When the space contains a large number of obstacles or narrow channel constraints, the algorithm converges slowly and its efficiency drops significantly. Therefore, in scenarios with narrow parking spaces and many obstacles, the parking trajectory search based on the RRT algorithm is prone to search failure. Summary of the invention

[0004] The purpose of the present application is to propose a parking path planning method and device thereof to solve the problem that parking trajectory search is prone to search failure in the current scenario of narrow parking spaces and many obstacles.

[0005] To achieve the above objectives, an embodiment of the present application provides a parking path planning method, the method comprising:

[0006] Acquire the surrounding environment information from the target parking space to the current position of the vehicle, and obtain the obstacle information, the starting point and the end point according to the surrounding environment information; wherein the starting point is the position of the target parking space, and the end point is the current position of the vehicle;

[0007] Based on a preset vehicle kinematic model, a first path search is performed according to the obstacle information, the starting point and the end point to obtain an initial expansion tree; wherein the initial expansion tree includes a plurality of nodes, and the node information of each node includes the position coordinates and the heading angle corresponding to the node;

[0008] Based on the initial expansion tree, a second path search is performed according to the obstacle information, the starting point and the end point, and a preset RRT algorithm to obtain a random expansion tree; wherein the random expansion tree includes a plurality of nodes, and the node information of each node includes the position coordinates and the heading angle corresponding to the node;

[0009] A parking planning path is obtained according to the random expansion tree.

[0010] The method provided in the embodiment of the present application, in order to adapt to the uneven distribution of obstacle density in wider driving lanes and narrower parking spaces, the method uses the position of the target parking space as the starting point and the current position of the vehicle as the end point to perform a reverse search for the path; and in the search process, a first path search is performed based on the vehicle kinematic model, an initial expansion tree is pre-generated, and then a second path search is performed based on the initial expansion tree in combination with the RRT algorithm, thereby accelerating the growth process of the random expansion tree, avoiding the problem that the RRT algorithm has difficulty growing in the parking space during the reverse search and even causes planning failure. The pre-generation of the initial expansion tree can reduce the number of invalid expansions, avoid the random expansion tree growth in a narrow parking space, which will frequently trigger collision detection and cause invalid expansion, and greatly improve the search rate, thereby effectively solving the problem that the parking trajectory search is prone to search failure in the current scenario of narrow parking spaces and many obstacles.

[0011] In some implementations, the first path search based on the preset vehicle kinematics model and the starting point and the end point specifically includes:

[0012] Using the starting point as the root node of the initial expansion tree, node expansion is performed in multiple expansion directions, each expansion direction corresponds to a preset wheel turning angle;

[0013] When performing node expansion in any expansion direction, the vehicle moves a preset step length from the current node according to the preset wheel angle, and performs collision prediction according to the obstacle information; if no collision occurs after the movement, the position after the movement is used as a new node in the expansion direction, and it is determined whether to end the node expansion in the expansion direction; if a collision occurs after the movement, the vehicle moves a preset step length in a straight line from the current node, and the position after the movement is used as a new node in the expansion direction, and it is determined whether to end the node expansion in the expansion direction;

[0014] If the position of the new node meets the preset conditions, the expansion is terminated. If the position of the new node does not meet the preset conditions, the node expansion in the expansion direction is continued with the new node as the current node.

[0015] In some implementations, performing a first path search according to the starting point and the end point specifically further includes:

[0016] When a new node is obtained, the first position coordinates and first heading angle corresponding to the parent node of the new node, the first wheel turning angle and the moving speed of the vehicle moving from the parent node to the new node are obtained, and based on the preset vehicle kinematic model, the position coordinates and heading angle of the new node are calculated according to the first position coordinates, first heading angle, first wheel turning angle and moving speed.

[0017] In some implementations, the preset condition includes:

[0018] When the vehicle moves to the new node, the vehicle is satisfied to drive out of the target parking space; wherein, the judgment condition for driving out of the target parking space is that the vehicle body completely drives out of the parking space line of the target parking space, or the front of the vehicle touches the lane boundary above the parking space.

[0019] In some implementations, the multiple preset wheel turning angles corresponding to the multiple extension directions are:

[0020] φ max , -φ max ;

[0021] Among them, φ mac It is the preset maximum wheel turning angle of the vehicle.

[0022] In some implementations, the performing of a second path search based on the initial expansion tree, according to the obstacle information, the starting point and the end point, and a preset RRT algorithm to obtain a random expansion tree specifically includes:

[0023] The multiple nodes of the initial expansion tree are used as multiple basic nodes of the random expansion tree, and the random expansion tree is grown at least once based on the multiple basic nodes until there is a node in the random expansion tree whose distance cost to the end point is less than a preset threshold, then the growth is stopped and the second path search is ended.

[0024] In some implementations, the distance cost between any node of the random expansion tree and the end point is determined according to the following method:

[0025] If the vehicle does not enter the lane when it moves to any node of the random expansion tree, the second heading angle, the first distance between the vehicle and the center line of the lane, and the inclination angle of the center line of the lane when the vehicle is located at the any node are obtained, and the distance cost between the any node and the end point is determined according to the second heading angle, the inclination angle of the center line of the lane, and the first distance;

[0026] If the vehicle has entered the driving lane when it moves to any node of the random expansion tree, the third heading angle and the second position coordinates of the vehicle when it is located at the any node, and the terminal heading angle and the terminal position coordinates of the vehicle when the vehicle is located at the terminal are obtained, and the distance cost between the any node and the terminal is determined according to the third heading angle, the second position coordinates, the terminal heading angle, and the terminal position coordinates.

[0027] In some implementations, each growth of the random expansion tree includes:

[0028] Perform random sampling to obtain a temporary sampling point, and select a node from the existing nodes of the random expansion tree as a neighboring node of the temporary sampling point;

[0029] From the neighboring node, move a preset step length in multiple directions according to multiple preset wheel angles, and perform collision prediction according to the obstacle information;

[0030] If no collision occurs after moving in any direction, the position after moving in any direction is used as a candidate child node;

[0031] If there is at least one candidate child node, a candidate child node with the smallest distance cost to the end point is selected from the at least one candidate child node as a new node of the random expansion tree, whose parent node is the neighboring node, thus completing one growth.

[0032] In some implementations, selecting a candidate child node with the smallest distance cost to the end point from the at least one candidate child node specifically includes:

[0033] If the vehicle does not enter the lane when it is located at any candidate subnode, the fourth heading angle when the vehicle is located at the any candidate subnode, the second distance between the vehicle and the centerline of the lane, and the inclination angle of the centerline of the lane are obtained, and the distance cost between the any candidate subnode and the end point is determined according to the fourth heading angle, the second distance, and the end point heading angle;

[0034] If the vehicle has entered the driving lane when it is located at any candidate subnode, the fifth heading angle and the third position coordinates when the vehicle is located at the any candidate subnode, the terminal heading angle and the terminal position coordinates when the vehicle is located at the terminal are obtained, and the distance cost between the any candidate subnode and the terminal is determined based on the fifth heading angle, the third position coordinates, the terminal heading angle and the terminal position coordinates.

[0035] In some implementations, the plurality of preset wheel angles include:

[0036]

[0037] Among them, φ max It is the preset maximum wheel turning angle of the vehicle.

[0038] An embodiment of the present application further provides a parking path planning device, comprising a module for executing the parking path planning method as described above.

[0039] An embodiment of the present application further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the parking path planning method as described above is implemented.

[0040] The embodiment of the present application also provides an electronic device, including a processor, a memory, and a computer program stored in the memory and executable on the processor, wherein the processor implements the parking path planning method as described above when executing the computer program. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0042] Figure 1 The present invention is a flowchart of a parking path planning method in one embodiment of the present application.

[0043] Figure 2 This is a schematic diagram of a parking space coordinate system in one embodiment of the present application.

[0044] Figure 3 This is a schematic diagram of the path of a vehicle driving out of a target parking space in one embodiment of the present application.

[0045] Figure 4 This is a flowchart of the first path search in one embodiment of the present application.

[0046] Figure 5 Schematic diagram of a parking path planning device in one embodiment of the present application. DETAILED DESCRIPTION

[0047] The detailed description of the accompanying drawings is intended as an illustration of the currently preferred embodiment of the present application, and is not intended to represent the only form in which the present application can be implemented. It should be understood that the same or equivalent functions can be accomplished by different embodiments intended to be included in the spirit and scope of the present application.

[0048] An embodiment of the present application provides a parking path planning method, see Figure 1 , the method of this embodiment includes the following steps:

[0049] Step S100, obtaining the surrounding environment information from the target parking space to the current position of the vehicle, and obtaining obstacle information, a starting point and an end point according to the surrounding environment information; wherein the starting point is the position of the target parking space, and the end point is the current position of the vehicle.

[0050] Specifically, this embodiment uses a vehicle-mounted camera to collect lane and parking space environment data, and uses a vehicle-mounted RTK device to perform real-time positioning to obtain the UTM coordinates x of the vehicle and surrounding obstacles. u ,y u , perform coordinate rotation transformation to obtain the coordinate x in the parking space coordinate system p ,y p The parking space coordinate system is a right-hand coordinate system with the starting point P as the origin and the target parking space vertically outward as the Y axis, such as Figure 2 As shown in formula (1), where x pu ,y pu is the UTM coordinate of the location point P, θ pu is the azimuth of the target parking space under UTM;

[0051]

[0052] In this embodiment, it is defined that when the geometric center of the target parking space coincides with the geometric center of the vehicle, the position of the center point of the rear axle of the vehicle is the starting point P, and the end point is the current position of the center point of the rear axle of the vehicle.

[0053] Step S200, based on a preset vehicle kinematic model, a first path search is performed according to the obstacle information, the starting point and the end point to obtain an initial expansion tree; wherein the initial expansion tree includes a plurality of nodes, and the node information of each node includes the position coordinates and the heading angle corresponding to the node.

[0054] Specifically, the vehicle kinematic model in this embodiment may be a bicycle model, from which the following formula (2) may be obtained:

[0055]

[0056] in, are the rate of change of the horizontal coordinate of the rear axle center of the vehicle, the rate of change of the vertical coordinate of the rear axle center of the vehicle, and the rate of change of the heading angle, v is the speed of the rear axle center of the vehicle, θ c is the heading angle of the vehicle before it moves, and φ is the front wheel turning angle during the movement.

[0057] It should be noted that the first path search in this embodiment is performed based on the parking space coordinate system. During the path search process, more nodes are searched with the starting point as the root node. An edge is formed between the nodes. Finally, multiple edges are connected to obtain a path out of the target parking space, that is, an initial expansion tree is obtained. The movement law of the vehicle is related to the vehicle kinematic model. Therefore, during the path search process, this embodiment predicts the movement of the vehicle based on the vehicle kinematic model. For path planning, it is necessary to obtain the position coordinates and heading angle of each trajectory point on the path to guide the automatic driving of the vehicle. Specifically, in this embodiment, the position coordinates and heading angle corresponding to each node can be obtained based on formula (2).

[0058] Step S300, based on the initial expansion tree, a second path search is performed according to the obstacle information, the starting point and the end point, and a preset RRT algorithm to obtain a random expansion tree; wherein the random expansion tree includes multiple nodes, and the node information of each node includes the position coordinates and heading angle corresponding to the node.

[0059] Specifically, the root node of the random expansion tree of this embodiment is the starting point, and the second path search of this embodiment is still based on the parking space coordinate system. The RRT (Rapidly-exploring Random Tree) algorithm is an algorithm commonly used in robot path planning. It searches for the optimal path by continuously expanding the tree structure. When the conventional RRT algorithm performs path search, it is necessary to initialize the random expansion tree, randomly select a starting point as the root node of the random expansion tree, and add it to the tree, and start expanding the tree structure based on the root node. In this embodiment, the initial expansion tree is used as the basis, that is, before the random expansion tree is expanded, the random expansion tree already has multiple nodes, which speeds up the growth process of the random expansion tree, avoids the growth of the random expansion tree in a narrow parking space, and frequently triggers collision detection, resulting in invalid expansion, greatly improving the search rate.

[0060] Step S400: obtaining a parking planning path according to the random expansion tree.

[0061] Specifically, after obtaining the random expansion tree, starting from the leaf node of the random expansion tree (the distance cost between it and the end point is less than a preset threshold), trace back to the root node along the generated edges (two nodes are connected to form an edge), and you can get a path from the starting point to the end point. This path is the parking planning path. The node information of each node includes the position coordinates and heading angle of the corresponding node. According to the position coordinates and heading angles of multiple nodes on the parking planning path, the vehicle's automatic driving can be controlled.

[0062] The method provided in the embodiment of the present application, in order to adapt to the uneven distribution of obstacle density in wider driving lanes and narrower parking spaces, the method uses the position of the target parking space as the starting point and the current position of the vehicle as the end point to perform a reverse search for the path; and in the search process, a first path search is performed based on the vehicle kinematic model, an initial expansion tree is pre-generated, and then a second path search is performed based on the initial expansion tree in combination with the RRT algorithm, thereby accelerating the growth process of the random expansion tree, avoiding the problem that the RRT algorithm has difficulty growing in the parking space during the reverse search and even causes planning failure. The pre-generation of the initial expansion tree can reduce the number of invalid expansions, avoid the random expansion tree growth in a narrow parking space, which will frequently trigger collision detection and cause invalid expansion, and greatly improve the search rate, thereby effectively solving the problem that the parking trajectory search is prone to search failure in the current scenario of narrow parking spaces and many obstacles.

[0063] In some embodiments, the step S200 specifically includes:

[0064] Using the starting point as the root node of the initial expansion tree, node expansion is performed in multiple expansion directions, each expansion direction corresponds to a preset wheel turning angle;

[0065] When performing node expansion in any expansion direction, the vehicle moves a preset step length from the current node according to the preset wheel angle, and performs collision prediction according to the obstacle information; if no collision occurs after the movement, the position after the movement is used as a new node in the expansion direction, and it is determined whether to end the node expansion in the expansion direction; if a collision occurs after the movement, the vehicle moves a preset step length in a straight line from the current node, and the position after the movement is used as a new node in the expansion direction, and it is determined whether to end the node expansion in the expansion direction;

[0066] If the position of the new node meets the preset conditions, the expansion is terminated. If the position of the new node does not meet the preset conditions, the node expansion in the expansion direction is continued with the new node as the current node.

[0067] Specifically, the moving step length refers to the distance that the center of the rear axle of the vehicle moves in one step. The length of the edge formed by connecting the parent node and the child node is one moving step length. In this embodiment, the center of the rear axle of the vehicle is used as the location point of the vehicle.

[0068] The collision prediction refers to predicting whether the vehicle will collide with an obstacle after moving in a certain direction based on the vehicle body parameters, vehicle moving speed and the obstacle information; the obstacle information includes information such as the structure, position, moving speed, and moving direction of the obstacle.

[0069] By expanding the leaf nodes, multiple paths that can exit the parking space may be obtained. Each path that can exit the parking space starts from the root node. For example, Figure 3 As shown, Figure 3 In the example, the nodes are expanded from 6 expansion directions, and finally 6 paths that can exit the parking space are obtained. The search process of each path can be found in Figure 4 In the process of searching a path in each expansion direction, the straight-line steps straight_count and the turning steps turn_count are recorded. After each new node is obtained, it is determined whether the target parking space has been driven out according to the new node. If not, the new node is continued to be expanded. If so, the process ends and the path generated by the straight-line steps straight_count and the turning steps turn_count is added to the initial expansion tree.

[0070] In some embodiments, the first path search based on the starting point and the end point specifically further includes:

[0071] When a new node is obtained, the first position coordinates and first heading angle corresponding to the parent node of the new node, the first wheel turning angle and the moving speed of the vehicle moving from the parent node to the new node are obtained, and based on the preset vehicle kinematic model, the position coordinates and heading angle of the new node are calculated according to the first position coordinates, first heading angle, first wheel turning angle and moving speed.

[0072] Specifically, the position coordinates and heading angle of each new node can be based on the vehicle kinematic model, that is, the content of the aforementioned formula (2), according to the moving speed v of the center of the vehicle rear axle, the heading angle θ of the current node c , the first wheel turning angle φ during the movement can be used to calculate the rate of change of the horizontal coordinate The rate of change of the vertical axis Rate of change of heading angle According to the change rate and the first position coordinates before the movement (including the horizontal coordinate x c , vertical coordinate y c ), the first heading angle θ c That is, the horizontal coordinate, vertical coordinate, and heading angle of the new node after movement can be calculated, and the position coordinates include the horizontal coordinate and the vertical coordinate.

[0073] In some embodiments, the preset conditions include:

[0074] When the vehicle moves to the new node, the vehicle is driven out of the target parking space; wherein, the judgment condition for driving out of the target parking space is that the vehicle body completely drives out of the parking space line of the target parking space, or the front of the vehicle touches the lane boundary above the parking space, for example Figure 3 shown.

[0075] In some embodiments, in order to balance the search efficiency and the search success rate, the multiple expansion directions include six directions, and the multiple preset wheel angles corresponding to the six directions may be, but are not limited to, respectively:

[0076] φ max , -φ max ;

[0077] Among them, φ max It is the preset maximum wheel turning angle of the vehicle. Assuming that the left is the positive direction, when the steering wheel turns left, the wheel turns left, and the turning angle is a positive number. Conversely, when the steering wheel turns right, the wheel turns right, and the turning angle is a negative number.

[0078] In some embodiments, the step S300 specifically includes:

[0079] The multiple nodes of the initial expansion tree are used as multiple basic nodes of the random expansion tree, and the random expansion tree is grown at least once based on the multiple basic nodes until there is a node in the random expansion tree whose distance cost to the end point is less than a preset threshold, then the growth is stopped and the second path search is ended.

[0080] Specifically, in the step, the initial expansion tree is used as the basis for growing a random expansion tree, and a path that can connect the starting point and the end point is found by continuously randomly generating nodes and trying to connect the nodes to the random expansion tree. In the process of connecting the nodes, it is necessary to determine whether the path collides with an obstacle. If so, the path is not added.

[0081] There are many ways to represent the distance cost, and this embodiment is not limited to any one way. For example, it can be the distance between the node and the end point. The preset threshold is a very small value. If the distance cost between the node and the end point is less than the preset threshold, it means that the extended node is very close to the end point, and the vehicle can obtain a path from the node to park in the target parking space.

[0082] Before the first growth of the random expansion tree, the random expansion tree already has a plurality of the basic nodes, thus speeding up the growth process of the random expansion tree, avoiding the frequent triggering of collision detection and resulting in invalid expansion due to the growth of the random expansion tree in a narrow parking space, and greatly improving the search rate.

[0083] In some embodiments, the distance cost between any node of the random expansion tree and the end point may be determined, but not limited to, in the following manner:

[0084] If the vehicle does not enter the lane when it moves to any node of the random expansion tree, the second heading angle, the first distance between the vehicle and the center line of the lane, and the inclination angle of the center line of the lane when the vehicle is located at the any node are obtained, and the distance cost cost between the any node and the end point is determined according to the second heading angle, the inclination angle of the center line of the lane, and the first distance distance ; can be expressed as the following formula (3);

[0085] cost distance =k1·dist(c,l r ) 2 +k2·(θ l -θ c2 ) 2 (3)

[0086] In formula (3), k1 and k2 are preset weight parameters, dist(c,l r ) is the first distance between the vehicle and the center line of the lane, c represents the center of the rear axle of the vehicle, l r represents the center line of the lane, θ l is the inclination angle of the center line of the lane (the center line of the lane is usually horizontal), θ c2 is the second heading angle;

[0087] If the vehicle has entered the driving lane when it moves to any node of the random expansion tree, the third heading angle and the second position coordinates of the vehicle at the arbitrary node, the terminal heading angle and the terminal position coordinates of the vehicle at the terminal are obtained, and the distance cost cost between the arbitrary node and the terminal is determined according to the third heading angle, the second position coordinates, the terminal heading angle, and the terminal position coordinates. distance ; can be expressed as the following formula (4);

[0088] cost distance =k3·(x g -x c ) 2 +k4·(y g -y c ) 2 +k5·(θ g -θ c3 ) 2 (4)

[0089] In formula (4), k3, k4 and k5 are preset weight parameters, θ g is the terminal heading angle, θc3 The second position coordinate includes the lateral coordinate x of the center of the rear axle of the vehicle. c and the vertical coordinate y c The end point position coordinates include the horizontal coordinate x g and the vertical coordinate y g .

[0090] Specifically, for path planning in parking scenarios, planning within the lane is relatively easy, and the difficulty and focus lies in path planning from the parking space to the lane. Therefore, different search strategies should be adopted before and after the parking space enters the lane. The vehicle is defined as entering the lane after the vehicle outline completely leaves the parking space. When the vehicle is driving on the lane and starts planning, most of the time it is in a nearly horizontal state, that is, the vehicle is pointing parallel to the lane and perpendicular to the parking space. Therefore, the planning goal before entering the lane from the parking space is set to coincide with the center line of the lane, focusing on the vehicle's longitudinal coordinate and heading angle, but not the horizontal coordinate; the planning goal after entering the lane is the end point of the planning, that is, the current position of the vehicle, and all posture quantities should be paid attention to at this time.

[0091] It should be noted that the present embodiment adopts a variable distance cost, adds the attraction of the end point and the lane centerline on the growth direction of the expansion tree, and reduces the time cost of the expansion tree due to random and aimless growth.

[0092] In some embodiments, each growth of the random expansion tree comprises:

[0093] Step S301, random sampling is performed to obtain a temporary sampling point, and a node is selected from the existing nodes of the random expansion tree as a neighboring node of the temporary sampling point; specifically, the neighboring node may be a node closest to the temporary sampling point, and the range of random sampling may be the target parking space plus a part of the lane adjacent to the parking space, and the range may be appropriately expanded outward or contracted inward from the main body according to the actual debugging situation;

[0094] Step S302, from the neighboring node, move a preset step length in multiple directions according to multiple preset wheel angles, and perform collision prediction according to the obstacle information; specifically, multiple initial child nodes can be obtained by moving in multiple directions, and then collision prediction is performed on the multiple points. The principle of collision prediction can be found in the previous content and will not be repeated here;

[0095] Step S303: if no collision occurs after the movement in any direction of step S302, the position after the movement in any direction (initial child node) is used as a candidate child node; the candidate child node indicates that it can be used as a new node;

[0096] Step S304, if there is at least one candidate child node, then select a candidate child node with the smallest distance cost to the end point from the at least one candidate child node as a new node of the random expansion tree, whose parent node is the neighboring node, and one growth is completed; specifically, if there is no candidate child node, it means that no suitable new node has been searched, and it is necessary to re-grow the next time, and still execute steps S301 to S304. After multiple growths, finally find a node with a distance cost less than a preset threshold value between it and the end point, and complete the growth of the random expansion tree.

[0097] In some embodiments, selecting a candidate subnode having the smallest distance cost to the end point from the at least one candidate subnode specifically includes:

[0098] If the vehicle is located at any candidate subnode and has not entered the lane, the fourth heading angle of the vehicle at the any candidate subnode, the second distance between the vehicle and the centerline of the lane, and the inclination angle of the centerline of the lane are obtained, and the distance cost cost between the any candidate subnode and the end point is determined according to the fourth heading angle, the second distance, and the end point heading angle. distance ; can be expressed as the following formula (5);

[0099] cost distance =k1·dist(c,l r ) 2 +k2·(θ l -θ c4 ) 2 (5)

[0100] In formula (3), k1 and k2 are preset weight parameters, dist(c,l r ) is the first distance between the vehicle and the center line of the lane, c represents the center of the rear axle of the vehicle, l r represents the center line of the lane, θ l is the inclination angle of the center line of the lane (the center line of the lane is usually horizontal), θ c4 is the second heading angle;

[0101] If the vehicle has entered the driving lane when it is located at any candidate subnode, the fifth heading angle and the third position coordinates when the vehicle is located at any candidate subnode, the terminal heading angle and the terminal position coordinates when the vehicle is located at the terminal are obtained, and the distance cost between any candidate subnode and the terminal is determined according to the fifth heading angle, the third position coordinates, the terminal heading angle and the terminal position coordinates.

[0102] cost distance ; can be expressed as the following formula (4);

[0103]

[0104] In formula (4), k3, k4 and k5 are preset weight parameters, θ g is the terminal heading angle, θ c5 is the fifth heading angle; the third position coordinate includes the lateral coordinate x of the center of the rear axle of the vehicle c and the vertical coordinate y c The end point position coordinates include the horizontal coordinate x g and the vertical coordinate y g .

[0105] Specifically, for path planning in parking scenarios, planning within the lane is relatively easy, and the difficulty and focus lies in path planning from the parking space to the lane. Therefore, different search strategies should be adopted before and after the parking space enters the lane. The vehicle is defined as entering the lane after the vehicle outline completely leaves the parking space. When the vehicle is driving on the lane and starts planning, most of the time it is in a nearly horizontal state, that is, the vehicle is pointing parallel to the lane and perpendicular to the parking space. Therefore, the planning goal before entering the lane from the parking space is set to coincide with the center line of the lane, focusing on the vehicle's longitudinal coordinate and heading angle, but not the horizontal coordinate; the planning goal after entering the lane is the end point of the planning, that is, the current position of the vehicle, and all posture quantities should be paid attention to at this time.

[0106] It should be noted that the present embodiment adopts a variable distance cost, adds the attraction of the end point and the lane centerline on the growth direction of the expansion tree, and reduces the time cost of the expansion tree due to random and aimless growth.

[0107] In some embodiments, in order to balance the search efficiency and the search success rate, the multiple preset wheel angles when searching for new nodes based on neighboring nodes may be, but are not limited to, respectively:

[0108] φ max , -φ max ;

[0109] Among them, φ max It is the preset maximum wheel turning angle of the vehicle. Assuming that the left is the positive direction, when the steering wheel turns left, the wheel turns left, and the turning angle is a positive number. Conversely, when the steering wheel turns right, the wheel turns right, and the turning angle is a negative number.

[0110] Another embodiment of the present application further provides a parking path planning device, comprising a module for executing the parking path planning method described in the above embodiment;

[0111] like Figure 5As shown, the module includes:

[0112] Information acquisition module 1, used to acquire the surrounding environment information from the target parking space to the current position of the vehicle, and obtain obstacle information, a starting point and an end point according to the surrounding environment information; wherein the starting point is the position of the target parking space, and the end point is the current position of the vehicle;

[0113] A first path search module 2 is used to perform a first path search based on a preset vehicle kinematic model and according to the obstacle information, the starting point and the end point to obtain an initial expansion tree; wherein the initial expansion tree includes a plurality of nodes, and the node information of each node includes a position coordinate and a heading angle corresponding to the node;

[0114] A second path search module 3 is used to perform a second path search based on the initial expansion tree, according to the obstacle information, the starting point and the end point, and a preset RRT algorithm to obtain a random expansion tree; wherein the random expansion tree includes a plurality of nodes, and the node information of each node includes the position coordinates and the heading angle corresponding to the node; and

[0115] The path generation module 4 is used to obtain a parking planning path according to the random expansion tree.

[0116] The parking path planning device of the above-described embodiment is merely illustrative, wherein the modules described as separate components may or may not be physically separated, and the components of the modules may or may not be physical modules, that is, they may be located in one place, or may be distributed on multiple network modules. Some or all of the modules may be selected according to actual needs to achieve the purpose of the parking path planning device of the embodiment.

[0117] It should be noted that the parking path planning device of the above embodiment corresponds to the parking path planning method of the above embodiment. Therefore, the undetailed parts of the parking path planning device of the above embodiment can be obtained by referring to the contents of the parking path planning method of the above embodiment, that is, the specific steps recorded in the parking path planning method of the above embodiment can be understood as the functions that can be implemented by the parking path planning device of the above embodiment, which will not be repeated here.

[0118] Furthermore, if the parking path planning device of the above embodiment is implemented in the form of a software function module and sold or used as an independent product, it can be stored in a computer-readable storage medium.

[0119] Another embodiment of the present application further provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the parking path planning method as described in the above embodiment is implemented.

[0120] Specifically, the computer-readable storage medium may include: any entity or recording medium that can carry the computer program instructions, a USB flash drive, a mobile hard disk, a magnetic disk, an optical disk, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium, etc.

[0121] Another embodiment of the present application provides an electronic device, including a processor, a memory, and a computer program stored in the memory and executable on the processor, wherein the processor implements the parking path planning method described in the above embodiment when executing the program.

[0122] Wherein, the electronic device may also include a bus connecting different components (including memory and processor). The memory may include a computer-readable medium in the form of a volatile memory, such as a random access memory (RAM) and / or a cache memory. The memory may also include at least one program product, which has a set of (e.g., at least one) program modules, which are configured to perform the functions of the various embodiments of the present application. The electronic device may also communicate with one or more external devices (e.g., keyboards, pointing devices, displays, etc.), may also communicate with one or more devices that enable a user to interact with the electronic device, and / or communicate with any device (e.g., a network card) that enables the electronic device to communicate with one or more other computing devices, such communication may be performed through an input / output (I / O) interface, and the electronic device may also communicate with one or more networks (e.g., local area networks (LANs), wide area networks (WANs) and / or public networks, such as the Internet) through a network adapter.

[0123] The embodiments of the present application have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and changes will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The selection of terms used herein is intended to best explain the principles of the embodiments, practical applications, or technical improvements in the market, or to enable other persons of ordinary skill in the art to understand the embodiments disclosed herein.

Claims

1. A parking path planning method, characterized in that: include: Acquire the surrounding environment information from the target parking space to the current position of the vehicle, and obtain the obstacle information, the starting point and the end point according to the surrounding environment information; wherein the starting point is the position of the target parking space, and the end point is the current position of the vehicle; Based on a preset vehicle kinematic model, a first path search is performed according to the obstacle information, the starting point and the end point to obtain an initial expansion tree; wherein the initial expansion tree includes a plurality of nodes, and the node information of each node includes the position coordinates and the heading angle corresponding to the node; Based on the initial expansion tree, a second path search is performed according to the obstacle information, the starting point and the end point, and a preset RRT algorithm to obtain a random expansion tree; wherein the random expansion tree includes a plurality of nodes, and the node information of each node includes the position coordinates and the heading angle corresponding to the node; A parking planning path is obtained according to the random expansion tree.

2. The parking path planning method according to claim 1, characterized in that: The first path search based on the preset vehicle kinematic model and according to the starting point and the end point specifically includes: Using the starting point as the root node of the initial expansion tree, node expansion is performed in multiple expansion directions, each expansion direction corresponds to a preset wheel turning angle; When performing node expansion in any expansion direction, the vehicle moves a preset step length from the current node according to the preset wheel angle, and performs collision prediction according to the obstacle information; if no collision occurs after the movement, the position after the movement is used as a new node in the expansion direction, and it is determined whether to end the node expansion in the expansion direction; if a collision occurs after the movement, the vehicle moves a preset step length in a straight line from the current node, and the position after the movement is used as a new node in the expansion direction, and it is determined whether to end the node expansion in the expansion direction; If the position of the new node meets the preset conditions, the expansion is terminated. If the position of the new node does not meet the preset conditions, the node expansion in the expansion direction is continued with the new node as the current node.

3. The parking path planning method according to claim 2, characterized in that: The first path search according to the starting point and the end point specifically includes: When a new node is obtained, the first position coordinates and first heading angle corresponding to the parent node of the new node, the first wheel turning angle and the moving speed of the vehicle moving from the parent node to the new node are obtained, and based on a preset vehicle kinematic model, the position coordinates and heading angle of the new node are calculated according to the first position coordinates, first heading angle, first wheel turning angle and moving speed.

4. The parking path planning method according to claim 2, characterized in that: The preset conditions include: When the vehicle moves to the new node, the vehicle is satisfied to drive out of the target parking space; wherein, the judgment condition for driving out of the target parking space is that the vehicle body completely drives out of the parking space line of the target parking space, or the front of the vehicle touches the lane boundary above the parking space.

5. The parking path planning method according to claim 2, characterized in that: The multiple preset wheel turning angles corresponding to the multiple expansion directions are respectively: Among them, φ max It is the preset maximum wheel turning angle of the vehicle.

6. The parking path planning method according to claim 1, characterized in that: Based on the initial expansion tree, a second path search is performed according to the obstacle information, the starting point and the end point and the preset RRT algorithm to obtain a random expansion tree, specifically including: The multiple nodes of the initial expansion tree are used as multiple basic nodes of the random expansion tree, and the random expansion tree is grown at least once based on the multiple basic nodes until there is a node in the random expansion tree whose distance cost to the end point is less than a preset threshold, then the growth is stopped and the second path search is ended.

7. The parking path planning method according to claim 6, characterized in that: The distance cost between any node of the random expansion tree and the end point is determined according to the following method: If the vehicle does not enter the lane when it moves to any node of the random expansion tree, the second heading angle, the first distance between the vehicle and the center line of the lane, and the inclination angle of the center line of the lane when the vehicle is located at the any node are obtained, and the distance cost between the any node and the end point is determined according to the second heading angle, the inclination angle of the center line of the lane, and the first distance; If the vehicle has entered the driving lane when it moves to any node of the random expansion tree, the third heading angle and the second position coordinates of the vehicle when it is located at the any node, and the terminal heading angle and the terminal position coordinates of the vehicle when the vehicle is located at the terminal are obtained, and the distance cost between the any node and the terminal is determined according to the third heading angle, the second position coordinates, the terminal heading angle, and the terminal position coordinates.

8. The parking path planning method according to claim 6, characterized in that: Each growth of the random expansion tree includes: Perform random sampling to obtain a temporary sampling point, and select a node from the existing nodes of the random expansion tree as a neighboring node of the temporary sampling point; From the neighboring node, move a preset step length in multiple directions according to multiple preset wheel angles, and perform collision prediction according to the obstacle information; If no collision occurs after moving in any direction, the position after moving in any direction is used as a candidate child node; If there is at least one candidate child node, a candidate child node with the smallest distance cost to the end point is selected from the at least one candidate child node as a new node of the random expansion tree, whose parent node is the neighboring node, thus completing one growth.

9. The parking path planning method according to claim 8, characterized in that: The selecting a candidate subnode having the smallest distance cost to the end point from the at least one candidate subnode specifically includes: If the vehicle does not enter the lane when it is located at any candidate subnode, the fourth heading angle when the vehicle is located at the any candidate subnode, the second distance between the vehicle and the centerline of the lane, and the inclination angle of the centerline of the lane are obtained, and the distance cost between the any candidate subnode and the end point is determined according to the fourth heading angle, the second distance, and the end point heading angle; If the vehicle has entered the driving lane when it is located at any candidate subnode, the fifth heading angle and the third position coordinates when the vehicle is located at the any candidate subnode, the terminal heading angle and the terminal position coordinates when the vehicle is located at the terminal are obtained, and the distance cost between the any candidate subnode and the terminal is determined based on the fifth heading angle, the third position coordinates, the terminal heading angle and the terminal position coordinates.

10. The parking path planning method according to claim 8, characterized in that: The plurality of preset wheel turning angles include: Among them, φ max It is the preset maximum wheel turning angle of the vehicle.

11. A parking path planning device, characterized in that: The method comprises a module for executing the parking path planning method according to any one of claims 1 to 10.

12. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the parking path planning method according to any one of claims 1 to 10 is implemented.

13. An electronic device comprising a processor, a memory, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the parking path planning method according to any one of claims 1 to 10 is implemented.

Citation Information

Patent Citations

  • Improved D*Lite vehicle dynamic path planning method

    CN108073176A

  • Unmanned ship weighted iteration path planning method and device based on multi-tree RRT

    CN112904869A

  • Vehicle control method and apparatus

    WO2021168669A1

Cited By

  • Robot unknown environment exploration method based on adaptive KMeans algorithm

    CN119148721A