CN115158299A - Parking path planning method and device, vehicle and readable storage medium - Google Patents
Parking path planning method and device, vehicle and readable storage medium Download PDFInfo
- Publication number
- CN115158299A CN115158299A CN202210985461.3A CN202210985461A CN115158299A CN 115158299 A CN115158299 A CN 115158299A CN 202210985461 A CN202210985461 A CN 202210985461A CN 115158299 A CN115158299 A CN 115158299A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- parked
- path
- cost
- map
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 230000007613 environmental effect Effects 0.000 claims abstract description 13
- 238000004422 calculation algorithm Methods 0.000 claims description 38
- 238000001514 detection method Methods 0.000 claims description 18
- 238000004590 computer program Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 2
- 230000004888 barrier function Effects 0.000 description 11
- 238000010586 diagram Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 4
- 230000004807 localization Effects 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000003238 somatosensory effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/06—Automatic manoeuvring for parking
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Traffic Control Systems (AREA)
Abstract
The embodiment of the application provides a parking path planning method, a parking path planning device, a vehicle and a readable storage medium. The parking method and the parking system can avoid the problems of parking scene limitation and excessive dependence on environmental information, are suitable for all parking scenes, and can improve the body feeling of a user.
Description
Technical Field
The invention relates to the field of parking path planning, in particular to a parking path planning method, a parking path planning device, a vehicle and a readable storage medium.
Background
During automatic parking, the sensing system identifies the parking space and the obstacles through the camera or the ultrasonic radar, then transmits the identification result to the planning decision system, the planning decision system firstly judges whether the parking space meets the requirements, and if so, the planning decision system plans the parking path from the initial position and the posture to the parking space. The existing automatic parking path planning generally adopts a geometric method consisting of circular arc and/or straight line connection, but the method strongly depends on environment information such as aisle width, parking starting point to target parking space distance and the like, and has no universality.
Disclosure of Invention
The invention aims to provide a parking path planning method, a parking path planning device, a vehicle and a readable storage medium.
In a first aspect, the present invention provides a parking path planning method, applied to a vehicle to be parked, the method including:
acquiring an initial pose and a target pose of the vehicle to be parked;
converting the grid map into a cost map through an A star algorithm based on the starting pose and the target pose;
planning an extension path of the vehicle to be parked in the grid map through a hybrid A star algorithm based on the cost map;
and planning an RS path of the vehicle to be parked by using an RS geometric algorithm, combining the expanded path and the RS path, and performing collision detection on the combined path to obtain a planned parking path.
In an optional embodiment, before acquiring the start pose and the target pose of the vehicle to be parked, the method further includes:
acquiring environmental information on a preset map and around a vehicle to be parked;
and converting the preset map into a grid map according to the preset grid resolution and the environment information.
In an optional embodiment, the planning an extended path of the vehicle to be parked in the grid map by a hybrid a-star algorithm based on the cost map includes:
determining a plurality of candidate extension nodes in a grid map according to a preset distance, a preset angle and a preset step length by taking the initial pose of the vehicle to be parked as an initial father node;
determining whether the vehicle to be parked collides when driving to each candidate expansion node through collision detection;
if collision occurs at the candidate expansion node, terminating searching for an expansion path;
if the candidate expansion nodes do not collide, determining the candidate expansion nodes as expandable nodes, calculating the total cost value of each expandable node according to a cost strategy and the cost map, and determining the expandable node with the minimum total cost value in all the expandable nodes as the latest expansion node;
determining whether the latest expansion node meets a preset termination condition;
when the latest extension node does not meet a preset termination condition, taking the latest extension node as the father node, and re-executing the step of determining a plurality of extension nodes according to a preset distance, a preset angle and a preset step length;
and when the latest expansion node meets a preset termination condition, terminating searching for an expansion path and outputting the latest expansion node and a series of father nodes thereof as the expansion path.
In an alternative embodiment, the process of collision detection for the vehicle to be parked includes:
determining four boundary points of the vehicle to be parked under a vehicle coordinate system;
converting the four boundary points in the vehicle coordinate system into four boundary points in a world coordinate system;
determining a rectangular frame forming the vehicle to be parked based on four boundary points under the world coordinate system;
and determining whether an obstacle line intersected with the rectangular frame exists according to the obstacle line on the grid map and the rectangular frame.
In an optional embodiment, the calculating a total cost value of the extensible node according to the cost policy and the cost map includes:
determining a dissipation cost of the expandable node based on a cost policy;
and determining a heuristic cost based on a cost map, and determining a total cost value of the expandable node based on the dissipation cost and the heuristic cost.
In an optional embodiment, the determining whether the latest extension node meets a preset termination condition includes:
determining whether the list storing the candidate extension nodes is empty;
when the list is empty, determining that the latest extended node meets the preset termination condition;
when the candidate extension node exists in the list, determining that the latest extension node does not meet the preset termination condition.
In an optional embodiment, the preset map is any one of a high-precision map and positioning and map building.
In a second aspect, the present invention provides a parking path planning apparatus for a vehicle to be parked, the apparatus comprising:
the acquisition module is used for acquiring the starting pose and the target pose of the vehicle to be parked;
the cost module is used for converting the grid map into a cost map through an A star algorithm based on the starting pose and the target pose;
the extended path module is used for planning an extended path of the vehicle to be parked in the grid map through a hybrid A star algorithm based on the cost map;
and the RS path module is used for planning the RS path of the vehicle to be parked by utilizing an RS geometric algorithm, combining the expanded path and the RS path and performing collision detection on the combined path to obtain a planned parking path.
In a third aspect, the present invention provides a vehicle, including a memory and a processor, where the memory stores a computer program, and the computer program, when executed on the processor, executes the parking path planning method.
In a fourth aspect, the present invention provides a readable storage medium, which stores a computer program that, when executed on a processor, performs the parking path planning method.
The embodiment of the invention has the beneficial effects that:
the embodiment of the application provides a parking path planning method, which comprises the steps of obtaining an initial pose and a target pose of a vehicle to be parked, converting a raster map into a cost map through an A star algorithm based on the initial pose and the target pose, planning an extension path of the vehicle to be parked in the raster map through a mixed A star algorithm based on the cost map, planning an RS path of the vehicle to be parked through an RS geometric algorithm, combining the extension path and the RS path, and performing collision detection on the combined path to obtain a planned parking path. The parking method and the parking system can avoid the problems of parking scene limitation and excessive dependence on environmental information, are suitable for all parking scenes, and can improve the body feeling of a user.
In order to make the aforementioned objects, features and advantages of the present application comprehensible, preferred embodiments accompanied with figures are described in detail below.
Drawings
In order to more clearly illustrate the technical solution of the present invention, the drawings required to be used in the embodiments will be briefly described below, and it should be understood that the following drawings only illustrate some embodiments of the present invention, and therefore should not be considered as limiting the scope of the present invention. Like components are numbered similarly in the various figures.
Fig. 1 shows a first flowchart of a parking path planning method according to an embodiment of the present application;
fig. 2 is a second flow chart of a parking path planning method according to an embodiment of the present application;
fig. 3 is a schematic flow chart illustrating a hybrid a-star algorithm in a parking path planning method according to an embodiment of the present application;
fig. 4 is a schematic flow chart illustrating collision detection in a parking path planning method according to an embodiment of the present application;
fig. 5 is a schematic flow chart illustrating a calculation of a total cost value in a parking path planning method according to an embodiment of the present application;
fig. 6 is a schematic flow chart illustrating a parking path planning method according to an embodiment of the present application to determine whether a preset termination condition is met;
fig. 7 is a schematic structural diagram of a parking path planning apparatus according to an embodiment of the present application.
Description of the main element symbols:
10-parking path planning means; 11-an acquisition module; 12-a cost module; 13-an extended path module; 14-RS path module.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments.
The components of embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present invention, presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments of the present invention without making any creative effort, shall fall within the protection scope of the present invention.
Hereinafter, the terms "including", "having", and their derivatives, which may be used in various embodiments of the present invention, are only intended to indicate specific features, numbers, steps, operations, elements, components, or combinations of the foregoing, and should not be construed as first excluding the existence of, or adding to, one or more other features, numbers, steps, operations, elements, components, or combinations of the foregoing.
Furthermore, the terms "first," "second," "third," and the like are used solely to distinguish one from another and are not to be construed as indicating or implying relative importance.
Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which various embodiments of the present invention belong. The terms (such as terms defined in a commonly used dictionary) will be construed to have the same meaning as the contextual meaning in the related art and will not be construed to have an idealized or overly formal meaning unless expressly so defined in various embodiments of the present invention.
Example 1
Referring to fig. 1, an embodiment of the present application provides a parking path planning method, which is applied to a vehicle, and can plan a parking path of a vehicle to be parked by using the parking path planning method, so as to implement automatic parking of the vehicle to be parked. Exemplarily, the parking path planning method includes steps S100 to S300.
Step S100: and acquiring the starting pose and the target pose of the vehicle to be parked.
It can be understood that the initial pose of the vehicle to be parked is obtained as an initial point, when a user sends an automatic parking instruction through a human-computer interaction interface or voice, and the vehicle to be parked receives the automatic parking instruction, automatic parking is started. After the vehicle to be parked starts automatic parking, the vehicle to be parked is driven at low speed to search for parking spaces, after optional parking spaces around the vehicle to be parked are searched, the grid map provides corresponding number information and corresponding parking space position and pose information of each parking space, at the moment, the vehicle to be parked stops driving, and a user selects a target parking space. Normally, the position where the vehicle to be parked will stop running is 4-5 meters away from the target parking space. And after the user selects the target parking space, taking the parking space pose corresponding to the determined target parking space as the target pose. The position and the attitude are represented, the starting point of the vehicle to be parked is the position of the vehicle to be parked when the vehicle stops driving after the parking space is searched, the position can be automatically positioned and determined according to a GPS module of the vehicle to be parked, and the attitude corresponding to the starting point is the starting attitude of the vehicle to be parked.
In one embodiment, as shown in fig. 2, steps S500 to S600 are further included before step S100.
Step S500: and acquiring environmental information on a preset map and around the vehicle to be parked.
In the embodiment of the present application, the preset Map is a Map scanned by a vehicle to be parked, and may be a high-precision Map (HAD Map) or a Map such as SLAM (Simultaneous Localization and Mapping, also referred to as CML, current Localization and Localization, and instant positioning and Map construction).
In the embodiment of the application, the environmental information around the vehicle to be parked can be obtained through a preset map, and the environmental information around the vehicle to be parked can also be obtained through an ultrasonic sensor, a look-around camera or a laser radar arranged on the vehicle to be parked, that is, the position information of the surrounding environmental information in world coordinates is obtained, and the environmental information includes but is not limited to a lane line, the position information of a static obstacle, a vehicle position line and the like, wherein the static obstacle includes a vehicle, a ground lock, a cone and the like. Exemplarily, when the preset map is a high-precision map, the high-precision map may provide information such as geographical coordinate information of a lane, a crosswalk, a traffic light, and a lane line.
Step S600: and converting the preset map into a grid map according to the preset grid resolution and the environment information.
It can be understood that corresponding barrier lines are set according to the environmental information around the vehicle to be parked, and after the user selects the target parking space, the scanned preset map is converted into a grid map according to the preset grid resolution and the environmental information, that is, the world coordinate of the world coordinate system environmental information is converted into the grid coordinate. The preset grid resolution can be set according to actual requirements, the grid resolution can influence the number of the grids occupied by the obstacles, and therefore the preset grid resolution with smaller resolution can be selected according to requirements under the condition that the preset calculation time is met, so that the success rate of parking of the vehicle to be parked is improved.
In other words, the preset map is divided according to grids with preset sizes to obtain an initial grid map, and barrier lines are set on the initial grid map according to the environment information, that is, grids are set through corresponding numbers to obtain a corresponding grid map. Exemplarily, the grid value corresponding to the obstacle grid may be set to 1, and the grid values of the other grids may be set to 0, so as to form the obstacle line in the grid map. The lane line of the vehicle to be parked entering the selected target parking space from the lane where the vehicle is located cannot be used as the barrier line, and static barriers, aisle lane lines, vehicle location lines and the like existing in other environment information are connected to form the barrier line on the grid map so as to form a drivable area of the vehicle to be parked.
Step S200: and converting the grid map into a cost map by an A star algorithm based on the starting pose and the target pose.
In this embodiment, after determining the start pose and the target pose of the vehicle to be parked, filling in inf (infinity) to the corresponding obstacle grid in the cost map by using the start pose of the vehicle to be parked as a start point and the target pose as a target grid in the grid map so as to represent that the point is unreachable; and calculating the cost value from each grid except the obstacle grid to the target grid in the grid map by using an A-star algorithm, and filling the calculated cost value into the corresponding grid to obtain the corresponding cost map. The cost map represents the cost value from each grid to the target grid, the direction of the vehicle to be parked which reaches the target pose as soon as possible is determined according to the cost map, namely the direction can be used as a global reference path, and the direction of an extended path is guided for the hybrid A-star algorithm.
Exemplarily, if the grid value corresponding to the grid in the grid map is 1, that is, the grid is the obstacle grid, inf is filled in the grid in the cost map; and when the grid value corresponding to the grid in the grid map is 0, calculating the cost value from the grid to the target grid through an A star algorithm, and filling the obtained cost value into the corresponding grid in the cost map.
Step S300: and planning an expansion path of the vehicle to be parked in the grid map through a hybrid A-star algorithm based on the cost map.
It can be understood that, after the approximate direction of the parking path of the vehicle to be parked is determined through the cost map, an extended path, in which the vehicle to be parked can avoid the obstacle, is planned on the grid map through the hybrid a-star algorithm.
In one embodiment, as shown in fig. 3, step S300 further includes performing substeps S310-S300 with the initial pose of the vehicle to be parked as the initial parent.
Substep S310: and determining a plurality of candidate extension nodes in the grid map according to the preset distance, the preset angle and the preset step length.
In this embodiment, a plurality of candidate extension nodes are determined in a grid map according to a preset distance, a preset angle and a preset step length, with a start pose, that is, a start point, of a vehicle to be parked as an initial parent node and with the initial parent node as a center. The preset angle refers to a deflection angle of a front wheel of the vehicle to be parked. The preset step length, the preset distance and the preset angle are set according to actual scenes.
Substep S320: whether a collision occurs when the vehicle to be parked runs to each candidate expansion node is determined through collision detection.
After a plurality of candidate expansion nodes corresponding to the initial parent node are determined, collision detection is carried out on each candidate expansion node based on the pose corresponding to each candidate expansion node, so that whether a vehicle to be parked collides after the vehicle runs to the candidate expansion node is determined. When the collision test shows that the vehicle to be parked is about to collide at the candidate expansion node, the hybrid star A algorithm is not solved, and the substep S370 is executed; if the vehicle to be parked is not collided at the candidate expansion node through the collision test, the substep S330 is executed.
In one embodiment, as shown in fig. 4, the process of collision detection for a vehicle to be parked includes substeps S321 to S324.
Substep S321: and determining four boundary points of the vehicle to be parked in the vehicle coordinate system.
In the implementation of the application, four boundary points of the vehicle to be parked in a vehicle coordinate system are determined through sensors arranged on the vehicle, namely positions corresponding to the left front, the right front, the left rear and the right rear of the vehicle to be parked are calculated. The vehicle coordinate system is a special moving coordinate system used for describing the motion of the automobile; its origin and barycenter coincidence are in quiescent condition when the vehicle on the horizontal road surface, and X axle is on a parallel with ground pointing to vehicle the place ahead, and the Z axle passes through car barycenter and points to the top, and the Y axle points to driver's left side.
Substep S322: and converting the four boundary points in the vehicle coordinate system into four boundary points in the world coordinate system.
And converting the positions corresponding to the left front, the right front, the left rear and the right rear of the vehicle to be parked under the vehicle coordinate system to the world coordinate system to obtain the positioning information of the vehicle to be parked under the world coordinate system. The vehicle to be parked takes the center of the rear axle in the initial pose as an original point, and the heading angle of the vehicle is a positive direction.
Substep S323: and determining a rectangular frame forming the vehicle to be parked based on the four boundary points in the world coordinate system.
And forming four line segments of a rectangular frame corresponding to the vehicle to be parked through the positions of four boundary points of the vehicle to be parked in the world coordinate system, namely the positions of the left front, the right front, the left rear and the right rear.
Substep S324: and determining whether an obstacle line intersected with the rectangular frame exists according to the obstacle line and the rectangular frame on the grid map.
In the embodiment, the barrier lines arranged in the grid map are traversed, whether four line segments of the rectangular frame corresponding to the vehicle to be parked are intersected with the barrier lines or not is determined when the vehicle to be parked runs to each candidate expansion node, and if the barrier lines which are intersected with the set barrier lines and the four line segments corresponding to the vehicle to be parked exist, the vehicle to be parked is shown to collide with the barriers in the surrounding environment of the vehicle to be parked when the vehicle to be parked runs to the candidate expansion nodes; if the four line segments corresponding to the vehicle to be parked are not intersected with any set barrier line, it is indicated that the vehicle to be parked does not collide when the vehicle runs to the candidate expansion node.
The rectangular frame formed by the length and the width of the vehicle is used for collision detection, the whole vehicle body of the vehicle is completely covered, and the vehicle is safe and has no dead angle.
Substep S330: and determining candidate expansion nodes as expandable nodes, calculating the total cost value of each expandable node according to the cost strategy and the cost map, and determining the expandable node with the minimum total cost value in all the expandable nodes as the latest expansion node.
When the vehicle to be parked does not collide with the candidate expansion nodes, the candidate expansion nodes are used as the expandable nodes, the total cost value corresponding to each expandable node is calculated according to the cost map and the cost strategy, and the expandable node with the minimum total cost value in all the expandable nodes is used as the latest expansion node.
In one embodiment, as shown in fig. 5, the total cost value of the scalable node is calculated according to the cost strategy and the cost map, and includes substeps S331 to S332.
Substep S331: determining a dissipation cost of the scalable node based on the cost policy.
In this embodiment, the dissipation cost G corresponding to the expandable node is calculated according to cost strategies, where the cost strategies include a few shift costs, an impatient turn cost, and a discontinuous turn cost.
The shift cost is calculated through a formula of cost1= f + configuration, switch _ cost _ distance, wherein f represents the cost calculated by the expandable node according to the A star algorithm, the switch _ cost represents the shift cost gain, and the distance represents the distance to the next node. And when less gear shifting is needed, setting a larger value for switch _ cost, and selecting the expandable node if the calculated gear shifting cost is larger. The turning cost is calculated by the formula cost2= f + config. Steer _ cost abs (delta), where cost represents the steering cost gain, delta represents the steering angle, and config. Steer _ cost is set to a larger value when turning is not sharp. The continuous turn cost will be calculated by the formula cost3= f + configuration. Step _ connection _ cost _ abs (delta-parent. Delta), which represents the continuous turn cost gain, delta-parent. Delta represents the turn change amount, and when turning is not continuous, the configuration. Step _ connection _ cost is set to a larger value.
Substep S332: and determining a heuristic cost based on the cost map, and determining the total cost value of the extensible node based on the dissipation cost and the heuristic cost.
It can be understood that the cost map obtained by the a-star algorithm is used as the heuristic cost H, where the heuristic cost H is calculated by using the cost map, and the specific formula is H = costmap (node. The approximate direction of the searched parking path of the vehicle to be parked is represented by the cost map, so that the problem that the direction of expansion is incorrect in the process of expanding the nodes by the mixed A-star algorithm is avoided, and the searching process can be accelerated. After the dissipation cost and the heuristic cost are determined, the total cost value corresponding to each expandable node is calculated through a corresponding formula.
Specifically, the total cost value corresponding to each expandable node is calculated by the following formula:
f = G + H, F representing the total cost value corresponding to the expandable node, G representing the dissipation cost, and H representing the heuristic cost.
Substep S340: and determining whether the latest extension node meets a preset termination condition.
And judging whether the latest expansion node meets a preset termination condition, if not, executing the substep S350, and if so, executing the substep S360.
In one embodiment, as shown in FIG. 6, determining whether the latest expansion node satisfies the preset termination condition includes substeps S341-S343.
Sub-step S341: it is determined whether the list in which the candidate expansion nodes are stored is empty.
In this embodiment, an open list and a close list are generated in advance, the open list is used for storing candidate extension nodes, the list is empty at the beginning, the start point is placed in the open list, the start point in the open list is used as a central point to extend 8 child nodes to the periphery, the path from the reference point to each child node is sequentially calculated, the shortest child node is determined, and the shortest child node is written into the open list. When there is no candidate extension node in the open list, indicating that the list is empty, substep S342 is performed, and when the list is not empty, substep S343 is performed.
Substep S342: and determining that the latest extension node meets a preset termination condition.
And when the expandable node does not exist in the list in which the candidate expansion nodes are stored, considering that the latest expansion node meets the preset termination condition.
Substep S343: and determining that the latest extension node does not meet the preset termination condition.
And when the candidate extension nodes exist in the list, the latest extension node is considered not to meet the preset termination condition.
Substep S350: the latest extension node is taken as the parent node, and the substep S310 is re-executed.
Substep S360: the output latest extension node and its series of parent nodes are taken as extension paths, and substep S370 is performed.
And when the latest extension node meets the preset termination condition, outputting the latest extension node and a series of father nodes thereof as extension path points.
Substep S370: the search for the expanded path is terminated.
Step S400: and planning an RS path of the vehicle to be parked by using an RS geometric algorithm, combining the expanded path and the RS path, and performing collision detection on the combined path to obtain a planned parking path.
In the embodiment of the application, after the extension path of the vehicle to be parked is determined, RS path planning is performed based on the end point of the extension path of the vehicle to be parked, in other words, when the RS path of the vehicle to be parked is planned through an RS geometric algorithm, the RS path planning is performed with the last latest extension node of the extension path as the start point of the RS path and the target pose as the end point.
And after an RS path planned through an RS geometric algorithm is obtained, combining the RS path with an expanded path obtained through a hybrid A-star algorithm, performing collision detection on the combined path, determining whether a corresponding rectangular frame intersects with a set obstacle line or not when the vehicle to be parked runs on the combined path, and taking the path after the collision detection as a planned parking path. So as to avoid the inconsistency of the planned path direction and the target direction when the vehicle to be parked reaches the target parking space. The RS geometric algorithm is a route planning method based on the geometric algorithm, a path from a starting point to an end point (namely a target point) can be quickly planned, the path planned through the RS geometric algorithm is an RS curve, namely a Reeds-Shepp curve is short, and the curve can not only ensure that a vehicle can reach the end point, but also ensure that the angle of the vehicle can reach an expected angle at the end point.
The method and the device can avoid the problems of parking scene limitation and excessive dependence on environmental information, are suitable for all parking scenes, and can improve the somatosensory degree of users.
Based on the parking path planning method in the foregoing embodiment, fig. 7 shows a schematic structural diagram of a parking path planning apparatus 10 provided in the embodiment of the present application, which is applied to a vehicle to be parked. The parking path planning apparatus 10 includes:
the acquisition module 11 is used for acquiring an initial pose and a target pose of a vehicle to be parked;
the cost module 12 is used for converting the grid map into a cost map through an A star algorithm based on the initial pose and the target pose;
the extended path module 13 is used for planning an extended path of the vehicle to be parked in the grid map through a hybrid A-star algorithm based on the cost map;
and the RS path module 14 is configured to plan an RS path of the vehicle to be parked by using an RS geometric algorithm, combine the extended path and the RS path, and perform collision detection on the combined path to obtain a planned parking path.
The parking path planning device 10 of the present embodiment is used for executing the parking path planning method of the foregoing embodiment, and the embodiments and the advantageous effects related to the foregoing embodiment are also applicable to the present embodiment, and are not described herein again.
The embodiment of the application further provides a vehicle, which comprises a vehicle body and a parking path planning device 10, wherein the parking path planning device is arranged on the vehicle body, and the parking path planning device is used for planning a parking path of the vehicle to be parked.
The embodiment of the present application further provides a computer-readable storage medium, which stores a computer program, and when the computer program is executed on a processor, the method for planning a parking path is implemented.
In the several embodiments provided in the present application, it should be understood that the disclosed apparatus and method may be implemented in other manners. The apparatus embodiments described above are merely illustrative and, for example, the flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present invention. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
In addition, each functional module or unit in each embodiment of the present invention may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
The functions, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored in a computer-readable storage medium. Based on such understanding, the technical solution of the present invention or a part of the technical solution that contributes to the prior art in essence can be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a smart phone, a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk, or an optical disk, and various media capable of storing program codes.
The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and the changes or substitutions should be covered within the scope of the present invention.
Claims (10)
1. A parking path planning method applied to a vehicle to be parked, the method comprising:
acquiring an initial pose and a target pose of the vehicle to be parked;
converting the grid map into a cost map through an A star algorithm based on the starting pose and the target pose;
planning an extension path of the vehicle to be parked in the grid map through a hybrid A star algorithm based on the cost map;
and planning an RS path of the vehicle to be parked by using an RS geometric algorithm, combining the expanded path and the RS path, and performing collision detection on the combined path to obtain a planned parking path.
2. The parking path planning method according to claim 1, wherein before the acquiring the start pose and the target pose of the vehicle to be parked, the method further comprises:
acquiring environmental information on a preset map and around a vehicle to be parked;
and converting the preset map into a grid map according to the preset grid resolution and the environment information.
3. The parking path planning method according to claim 1, wherein the planning of the extended path of the vehicle to be parked in the grid map by a hybrid a-star algorithm based on the cost map includes:
determining a plurality of candidate extension nodes in a grid map according to a preset distance, a preset angle and a preset step length by taking the initial pose of a vehicle to be parked as an initial father node;
determining whether the vehicle to be parked collides when running to each candidate expansion node through collision detection;
if collision occurs at the candidate expansion node, terminating searching for an expansion path;
if the candidate expansion nodes do not collide, determining the candidate expansion nodes as expandable nodes, calculating the total cost value of each expandable node according to a cost strategy and the cost map, and determining the expandable node with the minimum total cost value in all the expandable nodes as the latest expansion node;
determining whether the latest expansion node meets a preset termination condition;
when the latest expansion node does not meet a preset termination condition, taking the latest expansion node as the father node, and re-executing the determination of the plurality of expansion nodes according to a preset distance, a preset angle and a preset step length;
and when the latest expansion node meets a preset termination condition, terminating searching for an expansion path and outputting the latest expansion node and a series of father nodes thereof as the expansion path.
4. The parking path planning method according to claim 1 or 3, wherein the process of performing collision detection on the vehicle to be parked includes:
determining four boundary points of the vehicle to be parked in a vehicle coordinate system;
converting the four boundary points in the vehicle coordinate system into four boundary points in a world coordinate system;
determining a rectangular frame forming the vehicle to be parked based on four boundary points under the world coordinate system;
and determining whether an obstacle line intersected with the rectangular frame exists according to the obstacle line on the grid map and the rectangular frame.
5. The parking path planning method according to claim 3, wherein the calculating the total cost value of the expandable nodes according to the cost strategy and the cost map comprises:
determining a dissipation cost of the expandable node based on a cost policy;
and determining a heuristic cost based on a cost map, and determining a total cost value of the expandable node based on the dissipation cost and the heuristic cost.
6. The parking path planning method according to claim 3, wherein the determining whether the latest expansion node satisfies a preset termination condition includes:
determining whether the list storing the candidate extension nodes is empty;
when the list is empty, determining that the latest extended node meets the preset termination condition;
when the candidate extension node exists in the list, determining that the latest extension node does not meet the preset termination condition.
7. The parking path planning method according to claim 2, wherein the preset map is any one of a high-precision map and positioning and map construction.
8. A parking path planning apparatus applied to a vehicle to be parked, the apparatus comprising:
the acquisition module is used for acquiring the starting pose and the target pose of the vehicle to be parked;
the cost module is used for converting the grid map into a cost map through an A star algorithm based on the starting pose and the target pose;
the extended path module is used for planning an extended path of the vehicle to be parked in the grid map through a hybrid A star algorithm based on the cost map;
and the RS path module is used for planning the RS path of the vehicle to be parked by utilizing an RS geometric algorithm, combining the expanded path and the RS path and performing collision detection on the combined path to obtain a planned parking path.
9. A vehicle comprising a memory and a processor, the memory storing a computer program which, when run on the processor, performs the parking path planning method of any of claims 1 to 7.
10. A readable storage medium, characterized in that it stores a computer program which, when executed on a processor, executes the parking path planning method according to any one of claims 1 to 7.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210985461.3A CN115158299A (en) | 2022-08-17 | 2022-08-17 | Parking path planning method and device, vehicle and readable storage medium |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210985461.3A CN115158299A (en) | 2022-08-17 | 2022-08-17 | Parking path planning method and device, vehicle and readable storage medium |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115158299A true CN115158299A (en) | 2022-10-11 |
Family
ID=83479591
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210985461.3A Pending CN115158299A (en) | 2022-08-17 | 2022-08-17 | Parking path planning method and device, vehicle and readable storage medium |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115158299A (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115826437A (en) * | 2022-12-22 | 2023-03-21 | 镁佳(北京)科技有限公司 | Automatic vehicle parking simulation method, system, device and electronic equipment |
-
2022
- 2022-08-17 CN CN202210985461.3A patent/CN115158299A/en active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115826437A (en) * | 2022-12-22 | 2023-03-21 | 镁佳(北京)科技有限公司 | Automatic vehicle parking simulation method, system, device and electronic equipment |
CN115826437B (en) * | 2022-12-22 | 2024-01-30 | 镁佳(北京)科技有限公司 | Automatic vehicle parking simulation method, system, device and electronic equipment |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11680821B2 (en) | Map information system | |
CN106257242B (en) | Unit and method for adjusting road boundaries | |
CN112964271A (en) | Multi-scene-oriented automatic driving planning method and system | |
JP6664813B2 (en) | Driving support device and driving support method | |
EP2074380B1 (en) | A method of analysing the surroundings of a vehicle | |
CN110816548A (en) | Sensor fusion | |
CN112590775B (en) | Automatic parking method and device, vehicle and storage medium | |
JP7059888B2 (en) | Assistance control system | |
JP2023531831A (en) | Autonomous parking using hybrid search for parking spaces | |
Rabe et al. | Ego-lane estimation for lane-level navigation in urban scenarios | |
JP6892887B2 (en) | Vehicle control device and vehicle | |
CN115230719B (en) | Driving track planning method and device | |
JP2018138402A (en) | Vehicle driving assisting system and method | |
JP2019095210A (en) | Vehicle controller, method for controlling vehicle, and program | |
EP4177570A1 (en) | Standard-definition to high-definition navigation route determination | |
CN118235180A (en) | Method and device for predicting drivable lane | |
CN115158299A (en) | Parking path planning method and device, vehicle and readable storage medium | |
US20220204041A1 (en) | Vehicle and Method of Controlling Cut-In Response | |
CN115900738A (en) | Path planning method, device, vehicle and storage medium | |
CN113504782B (en) | Obstacle collision prevention method, device and system and moving tool | |
CN111204342B (en) | Map information system | |
CN111650934A (en) | Method for planning local path of automatic driving system | |
JP2020166123A (en) | Map data preparation method and map data preparation device | |
JP7344182B2 (en) | information processing equipment | |
CN113147783A (en) | Control method and control device for unmanned vehicle and unmanned vehicle |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |