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

Abstract

The present invention discloses a parking path planning method and device, electronic device and storage medium, the planning method comprises the following steps: obtaining a rasterized map of the surrounding environment of the automobile; obtaining the current point and the target position of the automobile, performing an extended search process from the current point and obtaining a target point set containing all points under exploration, obtaining a first cost minimum point from the target point set, and when the distance between the first cost minimum point and the target position is less than or equal to a preset threshold, using an RS curve algorithm to calculate a first RS curve trajectory connecting the first cost minimum point and the target position, and performing an obstacle collision test on the first RS curve trajectory based on the rasterized map, if the obstacle collision test is passed, the parking path of the automobile is formed by splicing the first cost minimum point and the first RS curve trajectory. The planning method can automatically avoid obstacles.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a parking path planning method and device, electronic equipment and storage medium. Background Art

[0002] With the development of automobile intelligence and networking, the automatic parking system is an important achievement in modern urban traffic management, which not only improves the efficiency and convenience of parking, but also ensures driving safety. With the continuous advancement of technology and continuous improvement of services, the automatic parking system will serve people's travel needs more perfectly and efficiently.

[0003] In practice, there are some obstacles on the ground (for example, ground locks in a closed state, etc.), which are generally unable to move. Therefore, how to avoid obstacles during automatic parking becomes a problem that needs to be solved urgently. Summary of the invention

[0004] In view of this, the main purpose of the present invention is to provide a parking path planning method and device, electronic device and storage medium.

[0005] To achieve the above-mentioned purpose, the technical solution of the present invention is implemented as follows: a method for planning a parking path for a car, comprising the following steps: obtaining a rasterized map of the surrounding environment of the car, the rasterized map containing obstacle information; obtaining the current point and target position of the car, performing an extended search process from the current point and obtaining a target point set containing all points under exploration, obtaining a first minimum cost point from the target point set, and when the distance between the first minimum cost point and the target position is ≤ a preset threshold, using an RS curve algorithm to calculate a first RS curve trajectory connecting the first minimum cost point and the target position, and performing an obstacle collision test on the first RS curve trajectory based on the rasterized map. If the obstacle collision test is passed, the parking path of the car is spliced ​​by the first minimum cost point and the first RS curve trajectory.

[0006] As an improvement of an embodiment of the present invention, the following steps are also included: when the distance between the first minimum cost point and the target position is greater than a preset threshold, or the obstacle collision test is not passed, the first minimum cost point is deleted from the target point set, several neighbor points of the first minimum cost point are obtained, and the several neighbor points are added to the target point set, and the second minimum cost point is obtained from the target point set; when the distance between the second minimum cost point and the target position is less than or equal to a preset threshold, the second RS curve trajectory connecting the second minimum cost point and the target position is calculated using an RS curve algorithm, and an obstacle collision test is performed on the second RS curve trajectory based on the rasterized map; if the obstacle collision test is passed, the parking path of the car is formed by splicing the second minimum cost point and the second RS curve trajectory.

[0007] As an improvement of an embodiment of the present invention, the step of performing an extended search process from the current point and obtaining a point with the minimum cost in the boundary specifically includes: based on a Hybrid A* algorithm, performing an extended search process from the current point and obtaining a point with the minimum cost in the boundary.

[0008] As an improvement of an embodiment of the present invention, the rasterized map performs an obstacle collision test on the first RS curve trajectory, specifically including: based on the vehicle heading angle and the front wheel orientation angle of the car, generating a first boundary line formed by the border of the car when the car moves along the first RS curve trajectory, and a second boundary line formed by each wheel of the car; when there is an obstacle in the area enclosed by the first boundary line and the height of the obstacle is ≥ the chassis height of the car, or there is an obstacle in the area enclosed by any second boundary line, the obstacle collision test fails; otherwise, the obstacle collision test passes; the rasterized map performs an obstacle collision test on the second RS curve trajectory, specifically including: based on the vehicle heading angle and the front wheel orientation angle of the car, generating a third boundary line formed by the border of the car when the car moves along the second RS curve trajectory, and generating a fourth boundary line formed by each wheel of the car; when there is an obstacle in the area enclosed by the third boundary line and the height of the obstacle is ≥ the chassis height of the car, or there is an obstacle in the area enclosed by any fourth boundary line, the obstacle collision test fails; otherwise, the obstacle collision test passes.

[0009] As an improvement of an embodiment of the present invention, the car is provided with four wheels; the planning method further comprises the following steps: taking the midpoint of the rear axle as the origin, the direction of the front of the car as the positive direction of the x-axis, and the direction of the left side of the car as the positive direction of the y-axis, creating a local plane coordinate system of the car; in the local plane coordinate system of the car, the left vertex of the rear of the border of the car The coordinates of , the front left vertex The coordinates of , the rear right vertex The coordinates of , the front right vertex The coordinates of ;vertex Coordinates in the world coordinate system , ,in, , is the vehicle heading angle; in the local plane coordinate system of the vehicle, the left wheel behind the frame of the vehicle The coordinates of the center point are , front left wheel The coordinates of the center point are , rear right wheel The coordinates of the center point are , front right wheel The coordinates of the center point are The wheel The coordinates of the vertex on the left side of the corresponding projection matrix are , the coordinates of the front left vertex of the corresponding projection matrix are , the coordinates of the right vertex behind the corresponding projection matrix are , the coordinates of the front right vertex of the corresponding projection matrix are , where shadow is a constant and wheel_width is the wheel width; generating the first boundary line formed by the border of the car when the car moves along the first RS curve trajectory based on the vehicle heading angle and the front wheel orientation angle of the car specifically includes: obtaining multiple first trajectory points along the first RS curve trajectory, and performing the following processing on each first trajectory point: obtaining the coordinates of the first trajectory point in the world coordinate system ,make , based on the formula Get the vertex corresponding to the first trajectory point Coordinates in the world coordinate system ,make , based on the formula , , , , ; , is the front wheel heading angle; , and get the corresponding value of the first trajectory point , this processing is completed; then, the first boundary line is generated based on all vertices corresponding to all first trajectory points, and the first boundary line is generated based on all coordinates corresponding to all first trajectory points. Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The method comprises: obtaining a plurality of second trajectory points along the second RS curve trajectory, and performing the following processing on each second trajectory point: obtaining the coordinates of the second trajectory point in the world coordinate system; generating a third boundary line formed by the frame of the vehicle when the vehicle moves along the second RS curve trajectory based on the vehicle heading angle and the front wheel heading angle of the vehicle, and generating a plurality of fourth boundary lines formed by all wheels of the vehicle. ,make , based on the formula Get the vertex corresponding to the second trajectory point Coordinates in the world coordinate system ,make , based on the formula , , , , ; , is the turning angle of any front wheel; , and get the corresponding value of the second trajectory point , this processing is completed; then, the third boundary line is generated based on all the vertices corresponding to all the second trajectory points, and the third boundary line is generated based on all the coordinates corresponding to all the second trajectory points. Generate Wheel The corresponding fourth boundary line is based on all coordinates corresponding to all second trajectory points Generate Wheel The corresponding fourth boundary line is based on all coordinates corresponding to all second trajectory points Generate Wheel The corresponding fourth boundary line is based on all coordinates corresponding to all second trajectory points Generate Wheel The corresponding fourth boundary line.

[0010] As an improvement of the embodiment of the present invention, , , , , where rear_axle_dist is the distance from the rear end of the car to the rear axle, width is the width of the car, and length is the length of the car.

[0011] As an improvement of the embodiment of the present invention, , , , , where wheel_dist is the distance between the left and right wheels, and wheel_base is the distance between the front and rear axles.

[0012] The embodiment of the present invention further provides a parking path planning device for a car, comprising the following modules:

[0013] An information acquisition module is used to obtain a rasterized map of the surrounding environment of the automobile, wherein the rasterized map includes obstacle information; a processing module is used to obtain a current point and a target position of the automobile, perform an extended search process from the current point and obtain a target point set including all points under exploration, obtain a first minimum cost point from the target point set, and when the distance between the first minimum cost point and the target position is ≤ a preset threshold, use an RS curve algorithm to calculate a first RS curve trajectory connecting the first minimum cost point and the target position, and perform an obstacle collision test on the first RS curve trajectory based on the rasterized map. If the obstacle collision test is passed, the parking path of the automobile is formed by splicing the first minimum cost point and the first RS curve trajectory.

[0014] An embodiment of the present invention further provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the computer program.

[0015] An embodiment of the present invention further provides a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a processor, the steps of the above method are implemented.

[0016] The parking path planning method and device, electronic device and storage medium provided by the embodiment of the present invention have the following advantages: The embodiment of the present invention discloses a parking path planning method and device, electronic device and storage medium, the planning method comprises the following steps: obtaining a rasterized map of the surrounding environment of the car; obtaining the current point and the target position of the car, performing an extended search process from the current point and obtaining a target point set containing all points under exploration, obtaining a first cost minimum point from the target point set, and when the distance between the first cost minimum point and the target position is ≤ a preset threshold, using an RS curve algorithm to calculate a first RS curve trajectory connecting the first cost minimum point and the target position, and performing an obstacle collision test on the first RS curve trajectory based on the rasterized map, if the obstacle collision test is passed, the parking path of the car is formed by splicing the first cost minimum point and the first RS curve trajectory. The planning method can automatically avoid obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 is a schematic flow chart of a method for planning a parking path for a car in an embodiment;

[0018] Figure 2 is a schematic structural diagram of a car in an embodiment;

[0019] Figure 3 The experimental results in the examples are shown in FIG. DETAILED DESCRIPTION

[0020] The present invention will be described in detail below in conjunction with the embodiments shown in the accompanying drawings. However, the embodiments do not limit the present invention, and any structural, methodological, or functional changes made by a person skilled in the art based on the embodiments are all within the protection scope of the present invention.

[0021] The following description and accompanying drawings fully illustrate the specific embodiments of this article so that those skilled in the art can practice them. Parts and features of some embodiments may be included in or replace parts and features of other embodiments. The scope of the embodiments of this article includes the entire scope of the claims, as well as all available equivalents of the claims. Herein, the terms "first", "second", etc. are only used to distinguish one element from another, without requiring or implying any actual relationship or order between these elements. In fact, the first element can also be called the second element, and vice versa. Moreover, the terms "include", "comprise" or any other variant thereof are intended to cover non-exclusive inclusion, so that the structure, device or equipment including a series of elements includes not only those elements, but also other elements that are not explicitly listed, or also include elements inherent to such structure, device or equipment. In the absence of more restrictions, the elements defined by the sentence "including one..." do not exclude the existence of other identical elements in the structure, device or equipment including the elements. Each embodiment is described in a progressive manner herein, and each embodiment focuses on the differences from other embodiments, and the same and similar parts between the embodiments can be referred to each other.

[0022] The terms "longitudinal", "lateral", "upper", "lower", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc. in this document indicate the orientation or position relationship based on the orientation or position relationship shown in the drawings, and are only for the convenience of describing this document and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation, and therefore cannot be understood as a limitation on the present invention. In the description of this document, unless otherwise specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a mechanical connection or an electrical connection, it can also be the internal communication of two elements, it can be a direct connection, or it can be indirectly connected through an intermediate medium. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0023] Embodiment 1 of the present invention provides a method for planning a parking path for a car, such as Figure 1 As shown, the following steps are included:

[0024] Step 101: Obtain a rasterized map of the surrounding environment of the vehicle, wherein the rasterized map includes obstacle information;

[0025] Here, the raster map is a grid-based map representation method, in which the map area is divided into uniform grid cells, and specific attribute information is assigned to each grid cell. Each grid cell can represent different geographical features, types of objects, heights, obstacles and other information. The rasterized map can use a two-dimensional vector data structure, in which the assignment of ordinary obstacle point elements and obstacle point elements is different for easy distinction. Optionally, the obstacle can be a ground lock, a pillar, a stone pier, etc., among which some obstacles can be passed by the chassis of the car but not by the wheels (for example, a closed ground lock, etc.).

[0026] Here, a laser radar and a camera may be provided on the car. The laser radar is used to measure the distance information between the space where the smart car is located and the surrounding obstacles. The camera is used to identify the obstacle type and specific status in the space where the smart car is located. After that, the processor starts to execute the relevant recognition algorithm, thereby finally generating the rasterized map.

[0027] Step 102: Obtain the current point and target position of the car, perform an extended search process from the current point and obtain a target point set containing all points under exploration, obtain a first minimum cost point from the target point set, and when the distance between the first minimum cost point and the target position is ≤ a preset threshold, use an RS curve algorithm to calculate a first RS curve trajectory connecting the first minimum cost point and the target position, and perform an obstacle collision test on the first RS curve trajectory based on the rasterized map. If the obstacle collision test is passed, the parking path of the car is spliced ​​by the first minimum cost point and the first RS curve trajectory.

[0028] Here, the current node can be understood as the node or state currently being considered during the search process, specifically referring to the state quantities such as forward or backward, the position coordinates of the rear axle center of the car, the heading angle, and the front wheel turning angle. Specifically, the current point can be selected as the starting point in the path planning process.

[0029] When the search algorithm is applied to the map, each point in the map point set belongs to one of three states, namely "explored", "being explored" and "unexplored". The search frontier is the set of points that are "being explored". Expanding the search frontier means that the algorithm continuously changes the state of the current point from being explored to being explored, and then changes the state of the unexplored neighboring points around the current point to being explored, which is intuitively the process of expanding the exploration frontier from the starting point to the outside. In practice, a candidate set and a closed set can be set. The candidate set contains all the points being explored, and the closed set contains all the points that have been explored. After that, the point with the minimum cost is popped out from the candidate set as the current point, and the current point is recorded in the closed set.

[0030] Here, the cost can be understood as the sum of the length of the path traveled and the heuristic value. The heuristic value is an estimate of the distance to be traveled from the current point to the end point. Optionally, the cost is calculated using the Manhattan distance.

[0031] The RS curve algorithm was published by JA Reeds and LA Shepp in a paper (optimal path for a car that goes both forward and backwards) in 1990. This method is based on the Dubins algorithm and improves it by adding reverse motion (the car is allowed to go backward and put in reverse gear) to the planning, which can produce a better solution than the Dubins curve in some cases.

[0032] here, Figure 3 A graph of experimental results is shown.

[0033] In this embodiment, the following steps are also included: when the distance between the first minimum cost point and the target position is greater than a preset threshold, or the obstacle collision test is not passed, the first minimum cost point is deleted from the target point set, several neighbor points of the first minimum cost point are obtained, and the several neighbor points are added to the target point set, and the second minimum cost point is obtained from the target point set; when the distance between the second minimum cost point and the target position is less than or equal to a preset threshold, the second RS curve trajectory connecting the second minimum cost point and the target position is calculated using an RS curve algorithm, and an obstacle collision test is performed on the second RS curve trajectory based on the rasterized map; if the obstacle collision test is passed, the parking path of the car is formed by splicing the second minimum cost point and the second RS curve trajectory.

[0034] Here, the neighbor points of the point can be enumerated according to the preset steering range and step length, and the forward and backward step length to meet the requirements of vehicle dynamics, that is, all possible curves for the car to travel are enumerated to obtain multiple neighbor points. For example, if the preset steering range is ±60°, the step length is 15°, and the forward and backward step length is 1 meter, then a certain point has 18 neighbor points, namely, forward 1 meter with a left turn of 60°, forward 1 meter with a left turn of 45°...backward 1 meter with a right turn of 45°, and backward 1 meter with a right turn of 60°. All of the above neighbor points meet the requirements of vehicle dynamics and can be reached by moving forward or backward for a certain distance under a certain steering.

[0035] In this embodiment, performing an extended search process from the current point and obtaining a minimum cost point in the boundary specifically includes: performing an extended search process from the current point and obtaining a minimum cost point in the boundary based on a Hybrid A* algorithm.

[0036] Here, the ‌Hybrid A* algorithm is a path planning algorithm that combines discrete and continuous space search techniques. It expands the search space to three dimensions by introducing the heading angle, which is more in line with the actual vehicle kinematic model. The Hybrid A* algorithm searches in a two-dimensional grid, but considering the steering and speed limits of the car, it is able to find a better path in continuous space.

[0037] In this embodiment, the rasterized map performs an obstacle collision test on the first RS curve trajectory, specifically including: based on the vehicle heading angle and the front wheel orientation angle of the car, generating a first boundary line formed by the border of the car when the car moves along the first RS curve trajectory, and a second boundary line formed by each wheel of the car; when there is an obstacle in the area enclosed by the first boundary line and the height of the obstacle is ≥ the chassis height of the car, or there is an obstacle in the area enclosed by any second boundary line, the obstacle collision test is not passed; otherwise, the obstacle collision test is passed; the rasterized map performs an obstacle collision test on the second RS curve trajectory, specifically including: based on the vehicle heading angle and the front wheel orientation angle of the car, generating a third boundary line formed by the border of the car when the car moves along the second RS curve trajectory, and generating a fourth boundary line formed by each wheel of the car; when there is an obstacle in the area enclosed by the third boundary line and the height of the obstacle is ≥ the chassis height of the car, or there is an obstacle in the area enclosed by any fourth boundary line, the obstacle collision test is not passed; otherwise, the obstacle collision test is passed.

[0038] Here, when there is an obstacle in the area enclosed by the first boundary line, and the height of the obstacle is greater than or equal to the chassis height of the car, the obstacle may scratch the chassis of the car, causing damage to the car, and thus, the obstacle collision test is not passed. Similarly, when there is an obstacle in the area enclosed by the fourth boundary line, the car tire may hit the obstacle, causing damage to the tire, and thus, the obstacle collision test is not passed.

[0039] In this embodiment, Figure 2 As shown, the car is provided with four wheels; the planning method further comprises the following steps: taking the midpoint of the rear axle as the origin, the direction of the front of the car as the positive direction of the x-axis, and the direction of the left side of the car as the positive direction of the y-axis, creating a local plane coordinate system of the car; in the local plane coordinate system of the car, the left vertex of the rear side of the border of the car The coordinates of , the front left vertex The coordinates of , the rear right vertex The coordinates of , the front right vertex The coordinates of ;vertex Coordinates in the world coordinate system , ,in, , is the vehicle heading angle; in the local plane coordinate system of the vehicle, the left wheel behind the frame of the vehicle The coordinates of the center point are , front left wheel The coordinates of the center point are , rear right wheel The coordinates of the center point are , front right wheel The coordinates of the center point are The wheel The coordinates of the vertex on the left side of the corresponding projection matrix are , the coordinates of the front left vertex of the corresponding projection matrix are , the coordinates of the right vertex behind the corresponding projection matrix are , the coordinates of the front right vertex of the corresponding projection matrix are , where shadow is a constant and wheel_width is the wheel width; generating the first boundary line formed by the border of the car when the car moves along the first RS curve trajectory based on the vehicle heading angle and the front wheel orientation angle of the car specifically includes: obtaining multiple first trajectory points along the first RS curve trajectory, and performing the following processing on each first trajectory point: obtaining the coordinates of the first trajectory point in the world coordinate system ,make , based on the formula Get the vertex corresponding to the first trajectory point Coordinates in the world coordinate system ,make , based on the formula , , , , ; , is the front wheel heading angle; , and get the corresponding value of the first trajectory point , this processing is completed; then, the first boundary line is generated based on all vertices corresponding to all first trajectory points, and the first boundary line is generated based on all coordinates corresponding to all first trajectory points. Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The method comprises: obtaining a plurality of second trajectory points along the second RS curve trajectory, and performing the following processing on each second trajectory point: obtaining the coordinates of the second trajectory point in the world coordinate system; generating a third boundary line formed by the frame of the vehicle when the vehicle moves along the second RS curve trajectory based on the vehicle heading angle and the front wheel heading angle of the vehicle, and generating a plurality of fourth boundary lines formed by all wheels of the vehicle. ,make , based on the formula Get the vertex corresponding to the second trajectory point Coordinates in the world coordinate system ,make , based on the formula , , , , ; , is the turning angle of any front wheel; , and get the corresponding value of the second trajectory point , this processing is completed; then, the third boundary line is generated based on all the vertices corresponding to all the second trajectory points, and the third boundary line is generated based on all the coordinates corresponding to all the second trajectory points. Generate Wheel The corresponding fourth boundary line is based on all coordinates corresponding to all second trajectory points Generate Wheel The corresponding fourth boundary line is based on all coordinates corresponding to all second trajectory points Generate Wheel The corresponding fourth boundary line is based on all coordinates corresponding to all second trajectory points Generate Wheel The corresponding fourth boundary line.

[0040] Here, we can first establish a local plane coordinate system for the car, with the midpoint of the rear axle as the origin, the direction of the front of the car (i.e. the front of the car) as the positive direction of the x-axis, and the direction of the left side of the car as the positive direction of the y-axis.

[0041] The rear left vertex of the car's bounding box The coordinates of , the front left vertex The coordinates of , the rear right vertex The coordinates of , the front right vertex The coordinates of ; where rear_axle_dist is the distance from the rear end of the car to the rear axle, width is the width of the car, and length is the length of the car.

[0042] The left wheel behind the frame of the car The coordinates of the center point are , front left wheel The coordinates of the center point are , rear right wheel The coordinates of the center point are , front right wheel The coordinates of the center point are ; Among them, wheel_dist is the distance between the left and right wheels, and wheel_base is the distance between the front and rear axles.

[0043] A projection length hyperparameter is set based on factors such as the wheel radius and the height of general obstacles (such as ground locks). This parameter, together with the wheel width, specifies the length and width of a projection rectangle. The center of the projection rectangle coincides with the wheel. If it coincides with an obstacle, it is considered a risk of crushing. Based on this parameter and the wheel width, the center coordinates of the left rear, left front, right front, and right rear wheels of a certain wheel are calculated.

[0044] The left wheel behind the frame of the car The coordinates of the vertex on the left side of the corresponding projection matrix are , the coordinates of the front left vertex of the corresponding projection matrix are , the coordinates of the right vertex behind the corresponding projection matrix are , the coordinates of the front right vertex of the corresponding projection matrix are .

[0045] The front left wheel of the frame of the vehicle The coordinates of the vertex on the left side of the corresponding projection matrix are , the coordinates of the front left vertex of the corresponding projection matrix are , the coordinates of the right vertex behind the corresponding projection matrix are , the coordinates of the front right vertex of the corresponding projection matrix are .

[0046] The rear right wheel of the frame of the vehicle The coordinates of the vertex on the left side of the corresponding projection matrix are , the coordinates of the front left vertex of the corresponding projection matrix are , the coordinates of the right vertex behind the corresponding projection matrix are , the coordinates of the front right vertex of the corresponding projection matrix are .

[0047] The front right wheel of the frame of the vehicle The coordinates of the vertex on the left side of the corresponding projection matrix are , the coordinates of the front left vertex of the corresponding projection matrix are , the coordinates of the right vertex behind the corresponding projection matrix are , the coordinates of the front right vertex of the corresponding projection matrix are .

[0048] Multiply the rotation matrix R by the coordinate vectors of the car's bounding box vertices in the car's local plane coordinate system, and add the offset Z from the local coordinate system to the world coordinate system or The offset is equal to the position of the rear axle center of the car in the world coordinate system. The multiplication result and the offset are added to complete the coordinate system conversion and obtain the vertex coordinates of the car's frame in the world coordinate system. .

[0049] Calculate the world coordinate position of the geometric center of the four wheels of the car, that is, first set the rotation matrix R by the current heading angle, multiply it by the coordinate vectors of the geometric center of the four wheels in the local coordinate system of the car, and then calculate the offset from the local coordinate system to the world coordinate system. The offset is numerically equal to the position of the rear axle center of the car in the world coordinate system. Add the multiplication result and the offset to complete the coordinate system conversion and get the geometric center coordinate of a wheel in the world coordinate system. .

[0050] The rotation matrix Multiply the vertex coordinates of the wheel frame in the local coordinate system of a wheel, and then calculate the offset from the local coordinate system to the world coordinate system. The offset is equal to the position of the geometric center of the wheel in the world coordinate system. Add the multiplication result and the offset to complete the coordinate system conversion and get the vertex coordinates of the front wheel projection frame in the world coordinate system. and .

[0051] The front wheel turning angle should be the turning angle value when enumerating neighbors in the hybrid A* algorithm (such as 60° left turn, 45° left turn, etc. in the previous example); the front wheel turning angle is obtained by dividing the radius of the arc segment by the front and rear wheelbase parameters of the car. The arc segment here refers to the arc segment in the RS curve. Generally, the RS curve uses a fixed radius arc and a straight line segment to match the start and end points. The radius is a hyperparameter specified by the RS curve algorithm, such as 1 meter.

[0052] For the calculation of the world coordinates of the vertex of the projection frame of a certain wheel of the rear wheel, first set the rotation matrix R by the current heading angle, multiply it by the vertex coordinates of the wheel frame in the local coordinate system of a certain wheel, and then calculate the offset from the local coordinate system to the world coordinate system. The offset is numerically equal to the position of the geometric center of the wheel in the world coordinate system. The coordinate system conversion is completed by adding the multiplication result and the offset to obtain the vertex coordinates of the projection frame of a certain rear wheel in the world coordinate system. and .

[0053] In this embodiment, , , , , where rear_axle_dist is the distance from the rear end of the car to the rear axle, width is the width of the car, and length is the length of the car.

[0054] In this embodiment, , , , , where wheel_dist is the distance between the left and right wheels, and wheel_base is the distance between the front and rear axles.

[0055] Based on the above scheme, the planning method in the embodiment of the present invention analyzes or calculates the front wheel angle, and then determines whether rolling occurs after calculating the wheel position through coordinate conversion, thereby reducing damage to obstacles and tires. When considering avoiding low obstacles, the planning method in the embodiment of the present invention also considers the passability of the vehicle chassis, that is, the chassis other than the wheels can still pass over the low obstacles, which expands the feasible area of ​​the planned path, thereby improving the efficiency of the planned path and improving the path quality. When calculating the real-time position of the wheel, the influence of the front wheel angle factor is accurately considered, divided into two cases of hybrid A* and RS curves, and the theoretical value of the front wheel angle is read or calculated respectively, so as to more accurately calculate the real-time wheel projection position, and the obstacle collision judgment is less missed and less wrong.

[0056] Embodiment 2 of the present invention provides a parking path planning device for a car, comprising the following modules:

[0057] An information acquisition module is used to obtain a rasterized map of the surrounding environment of the automobile, wherein the rasterized map includes obstacle information; a processing module is used to obtain a current point and a target position of the automobile, perform an extended search process from the current point and obtain a target point set including all points under exploration, obtain a first minimum cost point from the target point set, and when the distance between the first minimum cost point and the target position is ≤ a preset threshold, use an RS curve algorithm to calculate a first RS curve trajectory connecting the first minimum cost point and the target position, and perform an obstacle collision test on the first RS curve trajectory based on the rasterized map. If the obstacle collision test is passed, the parking path of the automobile is formed by splicing the first minimum cost point and the first RS curve trajectory.

[0058] Embodiment 3 of the present invention provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method in embodiment 1 when executing the computer program.

[0059] Embodiment 4 of the present invention provides a computer-readable storage medium, in which a computer program is stored. When the computer program is executed by a processor, the steps of the method in embodiment 1 are implemented.

[0060] It should be noted that although the above describes the various steps in a specific order, it does not mean that the various steps must be executed in the above specific order. In fact, some of these steps can be executed concurrently or even in a different order as long as the required functions can be achieved.

[0061] The present invention may be a system, a method and / or a computer program product. The computer program product may include a readable storage medium carrying computer readable program instructions for causing a processor to implement various aspects of the present invention.

[0062] The readable storage medium can be a tangible device that holds and stores instructions used by the instruction execution device. The readable storage medium can include, for example, but is not limited to, an electrical storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any suitable combination thereof. More specific examples (non-exhaustive list) of readable storage media include: a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a static random access memory (SRAM), a portable compact disk read-only memory (CD-ROM), a digital versatile disk (DVD), a memory stick, a floppy disk, a mechanical encoding device, such as a punch card or a protruding structure in a groove on which instructions are stored, and any suitable combination thereof.

[0063] The embodiments of the present invention have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations 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 method for planning a parking path for a car, wherein the car is provided with four wheels, characterized in that: The following steps are involved: Acquire a rasterized map of the surrounding environment of the vehicle, wherein the rasterized map includes obstacle information; The current point and the target position of the car are obtained, an extended search process is performed from the current point to obtain a target point set including all points under exploration, a first minimum cost point is obtained from the target point set, and when the distance between the first minimum cost point and the target position is less than or equal to a preset threshold, a first RS curve trajectory connecting the first minimum cost point and the target position is calculated using an RS curve algorithm, and based on the vehicle heading angle and the front wheel heading angle of the car, a first boundary line formed by the border of the car when the car moves along the first RS curve trajectory and a second boundary line formed by each wheel of the car are generated. When there is an obstacle in the area enclosed by the first boundary line and the height of the obstacle is ≥ the chassis height of the car, or when there is an obstacle in the area enclosed by any second boundary line, the obstacle collision test is not passed, otherwise, the obstacle collision test is passed. If the obstacle collision test is passed, the parking path of the car is formed by splicing the first cost minimum point and the first RS curve trajectory; a local plane coordinate system of the car is created with the midpoint of the rear axle as the origin, the direction of the front of the car as the positive direction of the x-axis, and the direction of the left side of the car as the positive direction of the y-axis; in the local plane coordinate system of the car, the left vertex of the rear of the border of the car The coordinates of , the front left vertex The coordinates of , the rear right vertex The coordinates of , the front right vertex The coordinates of ;vertex Coordinates in the world coordinate system , ,in, , is the vehicle heading angle; in the local plane coordinate system of the vehicle, the left wheel behind the frame of the vehicle The coordinates of the center point are , front left wheel The coordinates of the center point are , rear right wheel The coordinates of the center point are , front right wheel The coordinates of the center point are The wheel The coordinates of the vertex on the left side of the corresponding projection matrix are , the coordinates of the front left vertex of the corresponding projection matrix are , the coordinates of the right vertex behind the corresponding projection matrix are , the coordinates of the front right vertex of the corresponding projection matrix are , where shadow is a constant and wheel_width is the wheel width; generating the first boundary line formed by the border of the car when the car moves along the first RS curve trajectory based on the vehicle heading angle and the front wheel orientation angle of the car specifically includes: obtaining multiple first trajectory points along the first RS curve trajectory, and performing the following processing on each first trajectory point: obtaining the coordinates of the first trajectory point in the world coordinate system ,make , based on the formula Get the vertex corresponding to the first trajectory point Coordinates in the world coordinate system ,make , based on the formula , , , , ; , is the front wheel heading angle; , and get the corresponding value of the first trajectory point , this processing is completed; then, the first boundary line is generated based on all vertices corresponding to all first trajectory points, and the first boundary line is generated based on all coordinates corresponding to all first trajectory points. Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line.

2. The planning method according to claim 1, characterized in that: The following steps are also included: When the distance between the first minimum cost point and the target position is greater than a preset threshold, or the obstacle collision test is not passed, the first minimum cost point is deleted from the target point set, several neighbor points of the first minimum cost point are obtained, and the several neighbor points are added to the target point set, and the second minimum cost point is obtained from the target point set. When the distance between the second minimum cost point and the target position is less than or equal to a preset threshold, the RS curve algorithm is used to calculate a second RS curve trajectory connecting the second minimum cost point and the target position, and an obstacle collision test is performed on the second RS curve trajectory based on the rasterized map. If the obstacle collision test is passed, the parking path of the car is spliced ​​by the second minimum cost point and the second RS curve trajectory.

3. The planning method according to claim 1, characterized in that: Performing an extended search process from the current point and obtaining a minimum cost point in the search boundary specifically includes: Based on the Hybrid A* algorithm, an extended search process is performed from the current point to obtain the minimum cost point in the search boundary.

4. The planning method according to claim 2, characterized in that: The obstacle collision test on the second RS curve trajectory based on the rasterized map specifically includes: based on the vehicle heading angle and the front wheel heading angle of the car, generating a third boundary line formed by the border of the car when the car moves along the second RS curve trajectory, and generating a fourth boundary line formed by each wheel of the car; when there is an obstacle in the area enclosed by the third boundary line and the height of the obstacle is ≥ the chassis height of the car, or there is an obstacle in the area enclosed by any fourth boundary line, the obstacle collision test fails; otherwise, the obstacle collision test passes.

5. The planning method according to claim 4, characterized in that: The method of generating a third boundary line formed by the border of the vehicle when the vehicle moves along the second RS curve trajectory based on the vehicle heading angle and the front wheel heading angle of the vehicle, and generating a fourth boundary line formed by each wheel of the vehicle specifically includes: obtaining a plurality of second trajectory points along the second RS curve trajectory, and performing the following processing on each second trajectory point: obtaining the coordinates of the second trajectory point in the world coordinate system; ,make , based on the formula Get the vertex corresponding to the second trajectory point Coordinates in the world coordinate system ,make , based on the formula , , , , ; , is the turning angle of any front wheel; , and get the corresponding value of the second trajectory point , this processing is completed; then, the third boundary line is generated based on all the vertices corresponding to all the second trajectory points, and the third boundary line is generated based on all the coordinates corresponding to all the second trajectory points. Generate Wheel The corresponding fourth boundary line is based on all coordinates corresponding to all second trajectory points Generate Wheel The corresponding fourth boundary line is based on all coordinates corresponding to all second trajectory points Generate Wheel The corresponding fourth boundary line is based on all coordinates corresponding to all second trajectory points Generate Wheel The corresponding fourth boundary line.

6. The planning method according to claim 5, characterized in that: , , , , where rear_axle_dist is the distance from the rear end of the car to the rear axle, width is the width of the car, and length is the length of the car.

7. The planning method according to claim 6, characterized in that: , , , , where wheel_dist is the distance between the left and right wheels, and wheel_base is the distance between the front and rear axles.

8. A parking path planning device for a car, wherein the car is provided with four wheels, characterized in that: Includes the following modules: An information acquisition module, used to acquire a rasterized map of the surrounding environment of the vehicle, wherein the rasterized map includes obstacle information; a processing module, configured to obtain a current point and a target position of the automobile, perform an extended search process from the current point and obtain a target point set including all points under exploration, obtain a first minimum cost point from the target point set, and when the distance between the first minimum cost point and the target position is less than or equal to a preset threshold, use an RS curve algorithm to calculate a first RS curve trajectory connecting the first minimum cost point and the target position, and based on the vehicle heading angle and the front wheel heading angle of the automobile, generate a first boundary line formed by the border of the automobile when the automobile moves along the first RS curve trajectory, and a first boundary line formed by each wheel of the automobile Second boundary line, when there is an obstacle in the area enclosed by the first boundary line and the height of the obstacle is ≥ the chassis height of the car, or there is an obstacle in the area enclosed by any second boundary line, the obstacle collision test is not passed, otherwise, the obstacle collision test is passed. If the obstacle collision test is passed, the parking path of the car is spliced ​​by the first minimum cost point and the first RS curve trajectory; the car local plane coordinate system is created with the midpoint of the rear axle as the origin, the direction of the front of the car as the positive direction of the x-axis, and the left side of the car as the positive direction of the y-axis; in the car local plane coordinate system, the left vertex of the rear of the border of the car The coordinates of , the front left vertex The coordinates of , the rear right vertex The coordinates of , the front right vertex The coordinates of ;vertex Coordinates in the world coordinate system , ,in, , is the vehicle heading angle; in the local plane coordinate system of the vehicle, the left wheel behind the frame of the vehicle The coordinates of the center point are , front left wheel The coordinates of the center point are , rear right wheel The coordinates of the center point are , front right wheel The coordinates of the center point are The wheel The coordinates of the vertex on the left side of the corresponding projection matrix are , the coordinates of the front left vertex of the corresponding projection matrix are , the coordinates of the right vertex behind the corresponding projection matrix are , the coordinates of the front right vertex of the corresponding projection matrix are , where shadow is a constant and wheel_width is the wheel width; generating the first boundary line formed by the border of the car when the car moves along the first RS curve trajectory based on the vehicle heading angle and the front wheel orientation angle of the car specifically includes: obtaining multiple first trajectory points along the first RS curve trajectory, and performing the following processing on each first trajectory point: obtaining the coordinates of the first trajectory point in the world coordinate system ,make , based on the formula Get the vertex corresponding to the first trajectory point Coordinates in the world coordinate system ,make , based on the formula , , , , ; , is the front wheel heading angle; , and get the corresponding value of the first trajectory point , this processing is completed; then, the first boundary line is generated based on all vertices corresponding to all first trajectory points, and the first boundary line is generated based on all coordinates corresponding to all first trajectory points. Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line is based on all coordinates corresponding to all first trajectory points Generate Wheel The corresponding second boundary line.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method described in any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, wherein the computer program implements the steps of the method described in any one of claims 1 to 7 when executed by a processor.

Images