CN115077553A - Method, system, automobile, equipment and medium for planning track based on grid search - Google Patents
Method, system, automobile, equipment and medium for planning track based on grid search Download PDFInfo
- Publication number
- CN115077553A CN115077553A CN202210754909.0A CN202210754909A CN115077553A CN 115077553 A CN115077553 A CN 115077553A CN 202210754909 A CN202210754909 A CN 202210754909A CN 115077553 A CN115077553 A CN 115077553A
- Authority
- CN
- China
- Prior art keywords
- grid
- line
- obstacle
- passable
- column
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
Abstract
The invention discloses an obstacle avoidance track planning method based on grid search, which relates to the technical field of automatic driving, wherein a grid is generated according to a reference datum line and sensing information, a high-precision map boundary is added in the grid, an obstacle is projected in the grid and expanded, the grid is processed according to the current vehicle position and kinematics constraint, whether a completely passable 'column' exists is judged, if a completely passable line exists, anti-jitter processing is carried out on a safety line, a passable path is generated, the passable path is subjected to smooth processing, if a completely passable line does not exist, whether a left-side or right-side obstacle-bypassing safety line exists is searched, if the left-side or right-side obstacle-bypassing safety line exists, anti-jitter processing is carried out on the safety line, the passable path is generated and the safety processing is carried out, if the left-side or right-side obstacle-bypassing safety line does not exist, the last time path is output as the current path, the method is suitable for planning the dynamic track of the low-speed running vehicle in the complex environment.
Description
Technical Field
The invention relates to an automatic driving technology of an automobile, in particular to a dynamic trajectory planning method for a low-speed running automobile in a complex environment, which comprises real-time path planning such as obstacle avoidance and deviation in a lane.
Background
The trajectory planning algorithm is used as the basis and the technical core of an automatic driving system and has the function of generating a set path connecting a starting point and a terminal point of a vehicle on the basis of complying with space-time constraint conditions such as vehicle dynamics and kinematics, driving road environment, traffic regulations and the like. Among them, graph search is a commonly used method in trajectory planning. Graph searching typically generates a grid from high-precision map information and sensor-identified environment information, and then searches for a path on the generated grid using a search algorithm. The invention patent application with the publication number of CN111857160A entitled unmanned vehicle path planning method and device introduces an artificial potential field method to obtain the repulsion coefficient of each grid, adopts a mixed A star to search the path, adopts a gradient descent algorithm to smooth the track, and the core of the patent is still based on the mixed A star. The introduction of the artificial potential field can improve the searching efficiency, but in a complex environment, the repulsion coefficient of each grid needs to be calculated, the calculation force requirement is high, and meanwhile, the calculation force requirement of the hybrid A star algorithm is also high. The whole scheme is difficult to realize dynamic real-time planning in engineering. The chinese patent application, publication No. CN113267199A entitled "method and apparatus for planning driving track", generates multiple paths by offsetting a reference line, then calculates a cost function of each candidate path, and finally selects an optimal path, thereby being capable of realizing planning of a dynamic scene, and the chinese patent application, publication No. CN108241370B entitled "method and apparatus for obtaining an obstacle avoidance path through a grid map", discloses a method for obtaining an obstacle avoidance path through a grid map, and obtains a static global map of an area needing obstacle avoidance; taking each grid in the static grid map as a grid point; determining an obstacle area in a static grid map; determining obstacle cost values of grid points in the reachable area; and determining an obstacle avoidance path according to the positions of the starting point and the end point in the static grid map and the obstacle cost value of each grid point in the reachable area. However, the core idea of the method is closer to sampling optimization, planning is only performed based on a grid map, no search algorithm is used, and the calculation power is low. The invention patent application with the publication number CN113561968A "a vehicle horizontal parking trajectory planning algorithm and device, a vehicle and a storage medium" is a parking planning method based on geometry, and is not suitable for planning a driving scene. The invention patent application of publication No. CN 113467456 a, "a path planning algorithm for specific target search in unknown environment" also proposes a new path planning algorithm, except that the core algorithm of path planning is an RRT planning algorithm using artificial potential field guidance.
Therefore, it is necessary to develop a trajectory planning method that can make full use of grids to conveniently represent the surrounding environment, has low calculation power requirements, is convenient for engineering, has strong anti-interference capability, and can meet the requirement of real-time dynamic planning in the driving process.
Disclosure of Invention
The invention provides an obstacle avoidance trajectory planning method based on grid search, aiming at the problems that in the prior art, the traveling trajectory planning under a complex environment has high calculation power requirement, weak anti-interference capability, inconvenient engineering application and the like. In urban automatic driving and underground garage automatic driving scenes, due to the fact that various irregular obstacles such as walls, columns and road edges exist, the obstacles are difficult to be represented by the attributes of conventionally extracted obstacle targets, and the obstacles are usually represented by obstacle point clouds. The method provided by the invention can realize rasterization of the obstacle point cloud under the condition of small computational power.
The technical scheme for solving the technical problems is as follows: the method comprises the steps of detecting information such as types, speeds and contours of obstacle targets through a sensor, positioning a reference datum line required by current local path planning through a positioning system and global path planning, integrating the reference datum line and the information of the obstacle targets, searching an optimal path in a grid diagram searching mode, smoothing and optimizing the path in a spline fitting mode and the like, outputting an executable planned path finally, and realizing follow control of a track through a control module. The method mainly comprises the steps of generating grids under a Frenet coordinate system on the basis of a reference datum line, wherein the direction along the reference datum line is defined as a row, the direction perpendicular to the reference datum line is defined as a column, and meanwhile, map boundary information and sensor perception information are projected into the grids to form a planning space. When graph searching is carried out, searching is carried out on nodes one by one without adopting a searching mode such as A-x, in order to improve efficiency, columns of a planning space are directly searched, the optimal columns are searched, a safety datum line is formed, then the optimal value of grids of each row is calculated according to the current vehicle position and the conditions of the passable areas of the grids, and finally smooth optimization is carried out on the path generated by searching. The search mode is improved, so that the search can be completed in a shorter period, a simpler algorithm can be applied to the dynamic planning of the vehicle running path, the calculation is saved, the engineering realization is easy, and the purpose of dynamically planning the vehicle path in a complex path is realized. The method specifically comprises the following steps: a method for planning obstacle avoidance tracks based on grid search comprises the steps of matching points on a global path according to the current position of a vehicle, selecting path points with a fixed distance from the forward direction of the current vehicle position as reference lines of local path planning, planning tracks according to the reference lines and vehicle sensing information, outputting planned tracks to a transverse and longitudinal control module, and controlling the whole vehicle through a whole vehicle executing mechanism, wherein the track planning specifically comprises the following steps: generating a grid according to a reference datum line and perception information, adding a high-precision map boundary in the grid, projecting obstacles in the grid and expanding the obstacles, processing the grid according to the current vehicle position and kinematic constraint, judging whether a completely passable 'column' exists, if the completely passable column exists, performing anti-shaking processing on a safety line, generating a passable path, performing smoothing processing on the passable path, if the completely passable column does not exist, searching whether a left-side or right-side obstacle-detouring safety line exists, if the left-side or right-side obstacle-detouring safety line exists, performing anti-shaking processing on the safety line, generating the passable path and performing smoothing processing, and if the left-side or right-side obstacle-detouring safety line does not exist, outputting a previous time path as the current path.
Preferably, the sensing module detects that the obstacle information is subjected to sensing fusion and environment cognition processing, and outputs the information to the local path planning module, and the local path planning module generates a grid in a Frenet coordinate system according to a reference track, wherein the grid is defined as a line along a reference datum line direction, the line interval is the distance between two points of the reference datum line, and the grid is defined as a column along a vertical reference datum line direction. The distance between the rows is determined according to the vehicle speed and the calculation capability of the controller, and the distance between the columns is determined according to the width of the road. The ratio of the "row" pitch to the "column" pitch meets certain constraint requirements, which are derived from vehicle kinematics requirements.
Further preferably, the "rows" of the grid are gradually increased from the current position of the vehicle to the reference line direction, the "columns" of the grid are gradually increased from left to right, the value of the passable grid is set to 1, and the value of the nonpassable grid is set to 0.
More preferably, the adding of the high-precision map boundary to the grid is to project the boundary of the high-precision map on the grid, the grid outside the boundary is an impassable area, the grid inside the boundary is an impassable area, the sensing obstacle and the obstacle point cloud are expanded and then put into the grid, and the grid shielded by the obstacle is the impassable area. The safety line is a line obtained by translating the reference datum line in the column direction after the translation distance is determined based on searching.
Further preferably, the generating the passable path specifically includes: generating a passable grid based on the pose of the current vehicle according to the position of the current vehicle in the grid and the kinematic constraint of the vehicle; and calculating the grid coordinate with the minimum cost from each 'line' in the passable grid to the safety line based on the current vehicle pose, and forming a searched path as a passage path.
Further preferably, projecting the obstacle in the grid comprises: acquiring the type, the dynamic and static properties and the vertex position of a target obstacle from a sensor module; converting the top point of the obstacle to a coordinate system of a reference datum line; searching grids corresponding to each vertex coordinate in the barrier; and expanding the grids corresponding to the coordinate vertex points, wherein the grids in the expansion range are all impassable areas.
Further preferably, projecting the obstacle in the grid comprises: converting the obstacle point cloud into a coordinate system of a reference datum line; selecting rows and columns of the grid to generate a closed area according to an automatic driving scene; the obstacle point clouds in the closed area are effective point clouds, and the perpendicular distance between each effective point cloud and the reference datum line and the corresponding longitudinal distance are calculated; classifying the effective point clouds according to the distance values of the vertical lines; sorting according to the longitudinal distance of the point cloud corresponding to the reference datum line; and merging and projecting all the point clouds into a grid according to the distance from the effective point clouds to the reference datum line, classification and sequencing conditions.
Further preferably, the step of judging whether completely passable columns exist specifically comprises the step of forming a completely passable column set if all rows corresponding to the current column in the grid are 1 passable columns; forming a search list which is sorted to the left side and the right side according to the size of the vertical distance by using the 'column' where the reference datum line is located; and searching in the complete accessible column set according to the limited search width according to the search list.
Preferably, the step of searching whether the safety line with the left side or the right side obstacle avoidance exists further comprises the steps of selecting an area with a column value smaller than a reference datum line, and judging and searching the minimum feasible column value in each row in the area, wherein the column is the left side obstacle avoidance safety line. And selecting an area with the column value larger than the reference datum line, judging and searching the maximum feasible column value in each row in the area, wherein the column is the right obstacle-detouring safety line.
The invention also provides an obstacle avoidance trajectory planning system based on grid search, which comprises: the system comprises a sensing module, a positioning module, a global path planning module, a local path planning module, a transverse and longitudinal control module, a reference datum line module and a whole vehicle executing mechanism, wherein the reference datum line module is used for matching points on a global path determined by the global path planning module according to the current position of a vehicle provided by the positioning module, selecting path points with a fixed distance in the advancing direction of the current position of the vehicle as reference datum lines for local path planning, the local path planning module is used for planning a track according to the reference datum lines and sensing information provided by the sensing module, outputting the track to the transverse and longitudinal control module, and then controlling the whole vehicle through the whole vehicle executing mechanism, wherein the track planning is as follows: generating a grid according to a reference datum line and perception information, adding a high-precision map boundary in the grid, projecting obstacles in the grid and expanding the obstacles, processing the grid according to the current vehicle position and kinematic constraint, judging whether a completely passable 'column' exists, if the completely passable column exists, performing anti-shaking processing on a safety line, generating a passable path, performing smoothing processing on the passable path, if the completely passable column does not exist, searching whether a left-side or right-side obstacle-detouring safety line exists, if the left-side or right-side obstacle-detouring safety line exists, performing anti-shaking processing on the safety line, generating the passable path and performing smoothing processing, and if the left-side or right-side obstacle-detouring safety line does not exist, outputting a previous time path as the current path.
Preferably, the sensing module senses the obstacle information, performs sensing fusion and environment cognition processing, outputs the obstacle information to the local path planning module, and the local path planning module generates a grid in a Frenet coordinate system according to a reference track, wherein the grid is defined as a line along a reference datum line direction, the line spacing is the distance between two points of the reference datum line, the vertical reference datum line direction is defined as a column, the line spacing is determined according to the vehicle speed and the calculation capability of the controller, and the column spacing is determined according to the width of the road. The ratio of the "row" pitch to the "column" pitch satisfies certain constraint requirements derived from vehicle kinematics requirements.
Further preferably, projecting the obstacle in the grid comprises: acquiring the type, the dynamic and static properties and the vertex position of a target obstacle from a sensor module; converting the top point of the obstacle to a coordinate system of a reference datum line; searching grids corresponding to each vertex coordinate in the barrier; and expanding the grids corresponding to the coordinate vertex points, wherein the grids in the expansion range are all impassable areas.
Further preferably, projecting the obstacle in the grid comprises: converting the obstacle point cloud into a coordinate system of a reference datum line; selecting rows and columns of the grid to generate a closed area according to an automatic driving scene; the obstacle point clouds in the closed area are effective point clouds, and the perpendicular distance between each effective point cloud and the reference datum line and the corresponding longitudinal distance are calculated; classifying the effective point clouds according to the distance values of the vertical lines; sorting according to the longitudinal distance of the point cloud corresponding to the reference datum line; and merging and projecting all the point clouds into a grid according to the distance from the effective point clouds to the reference datum line, classification and sequencing conditions.
In a third aspect, the invention further provides an automobile, which includes the above obstacle avoidance trajectory planning system based on grid search.
In a fourth aspect, the present invention also provides a control apparatus, comprising: a processor and a memory; wherein the processor executes a program corresponding to the executable program code by reading the executable program code stored in the memory, so as to implement the obstacle avoidance trajectory planning method based on grid search.
In a fifth aspect, the present invention provides a computer-readable storage medium, on which a program or instructions are stored, where the program or instructions can be loaded and executed by a processor to execute the method for planning an obstacle avoidance trajectory based on grid search as described in the above.
In the process of realizing the obstacle avoidance planning and planning, each node is not searched in a traversing way, but columns are searched, so that the searching times are reduced, the safety line is found, and then the grid with the minimum cost from each row of the passable grid to the safety line is calculated, so that the planning path is formed. The method can save the operation resources of the controller to the maximum extent and realize the real-time planning of the path under the condition of a dynamic scene.
Drawings
FIG. 1 is a diagram of an obstacle avoidance trajectory planning system architecture based on grid search;
FIG. 2 is a flow chart of the obstacle avoidance trajectory planning based on grid search according to the present invention;
FIG. 3 is a schematic view of a passable grid.
Detailed Description
For the convenience of understanding of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying 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. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The invention will be further explained with reference to the drawings.
Fig. 1 shows an architecture diagram of an obstacle avoidance trajectory planning system based on grid search, which includes a sensing module (1), a positioning module (2), a global path planning module (3), a reference datum line module (4), a local path planning module (5), a transverse and longitudinal control module (6), and a vehicle actuator (7). The reference datum line module matches points on the global path determined by the global path planning module according to the current position of the vehicle provided by the positioning module, so that path points with a fixed distance in the forward direction from the current position of the vehicle are selected to serve as reference datum lines for local path planning, the local path planning module plans a track according to the reference datum lines and sensing information provided by the sensing module, the track is output to the transverse and longitudinal control module, and the whole vehicle is controlled through a whole vehicle execution mechanism, so that the automatic driving function is realized.
The sensing system is mainly based on sensors such as a vision sensor, a laser radar, a millimeter wave radar and an ultrasonic radar, and is used for detecting information such as an obstacle target and a lane line; the positioning system is mainly used for providing the position and the course information of the current vehicle; the global path system is mainly used for planning a path connecting the starting point to the end point according to the starting point, the end point and the map information; the reference line module is used for matching points on the global path according to the current position of the vehicle so as to select path points with fixed distances from the forward direction of the current vehicle position as reference lines for local path planning; the local path planning module is used for planning the tracks such as obstacle avoidance, longitudinal speed and the like according to the sensing information and the reference line information; the transverse and longitudinal control is carried out according to the track planned by the local path, so that the vehicle is ensured to follow the track within a certain error range; the whole vehicle actuating mechanism comprises an EMS (electronic message service), an ESP (electronic stability program), an EPS (electric power steering), gears, a BCM (binary coded modulation), and the like, wherein the EPS receives transverse control angle request information, the EMS receives longitudinal control torque request information, and the ESP receives longitudinal control deceleration request information.
Fig. 2 is a flow chart of obstacle avoidance trajectory planning based on grid search according to the present invention. Generating grids according to the reference datum lines and the perception information, adding high-precision map boundaries in the grids, projecting obstacles in the grids and expanding the grids, processing whether the grids have completely passable columns according to the current vehicle position and kinematic constraints, and if the grids have completely passable rows and columns, performing anti-shaking processing on safety lines, generating paths and smoothing the paths. If the completely passable column does not exist, searching whether a left side or a right side safety line exists, if the left side or the right side obstacle-detouring safety line exists, performing anti-jitter processing on the safety line, generating a path and performing smoothing processing, and if the left side or the right side safety line does not exist, outputting the last time path.
When the vehicle is in an automatic driving mode; the sensing module detects that the obstacle information is subjected to sensing fusion and environment cognition processing and then is output to the local path planning module, and meanwhile, the reference datum line module outputs a reference track to the local path planning module; the local path planning module generates grids in a Frenet coordinate system, wherein the direction along a reference datum line is defined as a line, the distance between two points of the reference datum line is the distance related to the vehicle speed and the calculation capability of a controller, the direction perpendicular to the reference datum line is defined as a line, the proportion relation between the distance between the line and the distance between the line needs to consider the kinematic information of the vehicle, and meanwhile, the total number of the lines considers the width of a vehicle driving area. In the present embodiment, the "rows" of the grid are gradually increased from the current position of the vehicle to the front of the reference line, the "columns" of the grid are gradually increased from left to right, the value of the passable grid is set to 1, and the value of the nonpassable grid is set to 0;
projecting the boundary in the high-precision map in a grid, wherein the grid outside the boundary is an impassable area, and the grid inside the boundary is a passable area;
and (4) putting the sensed obstacles and the obstacle point clouds into the grids after expansion, wherein the grids shielded by the obstacles are the impassable areas.
The formed grid is processed according to vehicle kinematic constraints. The principle of the main processing is that based on the current passable area, the grid is reachable by the vehicle under the condition that the vehicle kinematic constraint is met, otherwise, even if the grid is in the high-precision map boundary, the grid is still treated as a non-passable grid without being filled with obstacles;
the search determines the security thread. The safety line is a line obtained by translating the reference datum line according to the column direction, and the specific translation value of the safety line is realized based on searching.
In order to prevent the instability of the obstacle from causing the fluctuation of the safety line, the safety line is subjected to anti-shaking processing, and the processing method can adopt a conventional anti-shaking processing method adopted by a person skilled in the art.
And generating a passable grid based on the pose of the current vehicle according to the position of the current vehicle in the grid and the processing of the kinematic constraint of the vehicle.
And calculating the grid coordinate with the minimum cost from each 'line' in the passable grid to the safety line based on the current vehicle pose to form a searched path.
The generated path is smoothed by, but not limited to, a method such as spline interpolation or least square method.
Because the complex scene is difficult to be completely characterized by the obstacle, the complex scene needs to be supplemented by point cloud, the grid projection of the obstacle is divided into two parts, and the obstacle can be projected to a grid in the following way, namely, the first way is a vertex projection way:
(1) acquiring the type, the dynamic and static properties and the vertex position of a target obstacle from a sensor module;
(2) converting the target vertex to a coordinate system of a reference datum line;
(3) searching a grid corresponding to each vertex coordinate in each target;
(4) and expanding the grids corresponding to the coordinate vertex points, and processing the grids in the expansion range into impassable areas.
In the second embodiment, a point cloud projection method is used, and the obstacle point cloud is projected to a grid.
(1) Converting the point cloud of the obstacle into a coordinate system corresponding to the reference path;
(2) selecting rows and columns of the grid to generate a closed area according to different automatic driving scenes, for example, selecting the 2 nd to 30 th columns of the grid, and selecting the 1 st to 50 th rows of the grid to form the closed area;
(3) the method comprises the following steps of calculating the vertical distance between each point cloud and a reference datum line and the longitudinal distance corresponding to the reference datum line, wherein the left side of the vertical distance between each point cloud and the reference datum line is negative, and the right side of the vertical distance between each point cloud and the reference datum line is positive;
(4) classifying the sensing point cloud left and right according to the positive and negative values of the vertical line distance;
(5) sorting from near to far according to the longitudinal distance of the point cloud corresponding to the reference datum line, wherein the priority of the point cloud closer to the vehicle is higher;
(6) merging all the point clouds according to the calculated distance, classification and sorting conditions;
(7) and after collision processing is carried out on the point cloud, the point cloud is projected into the grid.
For the method of searching and determining the security thread, the following searching method may be specifically adopted in this embodiment:
(1) it is determined whether there are completely passable "columns" that are each passable for every "row" on the "column" grid. The specific judgment method is based on the reference datum line, and forms a search list which is arranged in a column where the reference datum line is located and is sorted to the left side and the right side according to the size of the vertical distance. And searching according to the search list, if the search is successful, performing anti-shake processing on the safety line, and otherwise, executing the next search. The detailed search steps for the safe and accessible "column" are as follows:
the first step is as follows: and searching for completely passable columns in the formed grid, and specifically checking whether the rows corresponding to each column of the grid are all 1 or not, if so, the column is a passable column, and finally forming a completely passable column set.
The second step is that: and generating a search list according to the reference datum line sequencing, wherein the principle of generating the list is that left side search is prior, and the left side and the right side are sequenced from the column of the current reference datum line. For example, the number of the column where the current reference line is located is 21, and one of the generated search lists is 21, 20, 22, 19, 23, 17 … ….
The third step: and searching in the generated completely passable column set according to the search list and the limited search width, if the completely passable column set exists in the column range searched currently, indicating that the search is successful, carrying out anti-shake treatment on the safety line, and if the search is unsuccessful, carrying out the next search. The following is illustrated as an example: the search width refers to a range extending from left to right of the current column, for example, the search width is 2, and the range of columns to be searched is {19, 20, 21, 22, 23} if there are 19, 20, 21, 22, 23 in the fully accessible columns, the search is successful, otherwise the search is unsuccessful,
(2) and the left side detour is prior, namely whether a safety line exists on the left side of the reference datum line is searched preferentially. The specific method is to select the left interesting area, namely the area with the column value smaller than the reference datum line. And judging and searching a minimum passable column value in each row in the area, wherein the column is a safety line with the barrier on the left side, if the search is successful, carrying out anti-shake treatment on the safety line, and if the search is unsuccessful, carrying out the next search.
(3) And (4) bypassing the right side, namely searching whether a safety line exists on the right side of the reference datum line. The specific method is to select the right interested area, namely the area with the column value larger than the reference datum line. And judging and searching the maximum passable row value in each row in the area, wherein the row is the safety line which is the right barrier, if the search is successful, carrying out anti-shake treatment on the safety line, otherwise, keeping the path at the previous moment and executing parking operation, wherein the current area is the impassable area.
The safety line is subjected to anti-shake processing for preventing the instability of the obstacle from causing the fluctuation of the safety line. And generating a passable grid based on the pose of the current vehicle according to the position of the current vehicle in the grid and the processing of the kinematic constraint of the vehicle.
One method of calculating a passable grid based on current vehicle position and vehicle kinematic constraints is described as follows:
(1) according to the vehicle positioning information, the position in the grid is matched,
(2) starting from the current grid, the vehicle can reach three grids at the left, the middle and the right of the next row at most,
fig. 3 is a schematic diagram of a passable grid, in which the number 2 indicates the position of the current vehicle, the number 3 indicates a non-passable grid due to obstacle filling, the number 1 indicates a passable grid, and the number 0 indicates a different passable grid generated according to vehicle kinematic constraints. When generating the final traversable grid in the train, the number 2 can only be moved to the grid in the left middle 3 positions in the next column, so that only the 3 grid values are 1, and the other grids in the column are filled with 0 even though not occupied by the obstacle and are treated as the non-traversable grids.
While the invention has been described in detail in connection with only a limited number of embodiments, it is not intended to be limited to the specific embodiments shown, and is intended to be exhaustive or otherwise limited to the invention in any suitable manner. Modifications, additions and substitutions as may be readily made by those skilled in the art are therefore not to be considered as limited by the foregoing description without departing from the general concept as defined by the claims and their equivalents.
Claims (23)
1. A method for planning obstacle avoidance tracks based on grid search is characterized in that points on a global path are matched according to the current position of a vehicle, path points with a fixed distance from the forward direction of the current vehicle position are selected to serve as reference lines of local path planning, track planning is carried out according to the reference lines and vehicle perception information, a planned track is output to a transverse and longitudinal control module, and control over the whole vehicle is achieved through a whole vehicle execution mechanism, and the track planning specifically comprises the following steps: generating a grid according to a reference datum line and sensing information, adding a high-precision map boundary in the grid, projecting obstacles in the grid and expanding the obstacles, processing the grid according to the current vehicle position and kinematic constraint, judging whether a completely passable 'column' exists, if the completely passable column exists, taking the completely passable column as a safety line, performing anti-shaking processing on the safety line, generating a passable path and performing smoothing processing, if the completely passable column does not exist, searching whether a left-side or right-side obstacle-detouring safety line exists, if the left-side or right-side obstacle-detouring safety line exists, performing anti-shaking processing on the safety line, generating the passable path and performing smoothing processing, and if the left-side or right-side obstacle-detouring safety line does not exist, outputting the last time path as the current path.
2. The method according to claim 1, wherein the sensing module detects obstacle information, performs sensing fusion and environment recognition processing, and outputs the obstacle information to the local path planning module, and the local path planning module generates a grid in a Frenet coordinate system according to a reference track, wherein a direction along a reference datum line is defined as a "row", a distance between two points of the reference datum line is defined as a "line", a direction perpendicular to the reference datum line is defined as a "column", the distance between the "row" is determined according to a vehicle speed and a controller computing capability, and the distance between the "column" is determined according to a road width.
3. The method of claim 2, wherein the rows of the grid are gradually increased from the current position of the vehicle to the reference line direction, the columns of the grid are gradually increased from left to right, the value of the passable grid is set to 1, and the value of the impassable grid is set to 0.
4. The method according to claim 1, wherein the adding of the high-precision map boundary to the grid is specifically that the boundary of the high-precision map is projected in the grid, the grid outside the boundary is an impassable area, the grid inside the boundary is an impassable area, the sensing obstacle and the obstacle point cloud are expanded and then put into the grid, and the grid shielded by the obstacle is the impassable area.
5. The method of claim 1, wherein the safety line is a line translated in a "column" direction with respect to the reference line after determining the translation distance based on the search.
6. The method according to claim 1, wherein the generating a traversable path specifically comprises: generating a passable grid based on the position and the pose of the current vehicle according to the position of the current vehicle in the grid and the kinematic constraint of the vehicle; and calculating the grid coordinate with the minimum cost from each 'line' in the passable grid to the safety line based on the current vehicle pose, and forming a searched path as a passable path.
7. The method of any one of claims 1-6, wherein projecting the obstruction in the grid comprises: acquiring the type, the dynamic and static properties and the vertex position of a target obstacle from a sensor module; converting the top point of the obstacle to a coordinate system of a reference datum line; searching grids corresponding to each vertex coordinate in the barrier; and expanding the grids corresponding to the coordinate vertex points, wherein the grids in the expansion range are all impassable areas.
8. The method of one of claims 1-6, wherein projecting the obstacle in the grid comprises: converting the obstacle point cloud into a coordinate system of a reference datum line; selecting rows and columns of the grid to generate a closed area according to an automatic driving scene; the obstacle point clouds in the closed area are effective point clouds, and the perpendicular distance between each effective point cloud and the reference datum line and the corresponding longitudinal distance are calculated; classifying the effective point clouds according to the distance values of the vertical lines; sorting according to the longitudinal distance of the point cloud corresponding to the reference datum line; and merging and projecting all the point clouds into a grid according to the distance from the effective point clouds to the reference datum line, classification and sequencing conditions.
9. The method of claim 3, wherein determining whether there are completely passable "columns" specifically comprises forming a completely passable column set if all rows corresponding to a current column in the grid are 1 passable columns; forming a search list which is sorted to the left side and the right side according to the size of the vertical distance by using the 'column' where the reference datum line is located; and searching in the complete passable column set according to the limited search width according to the search list.
10. The method of any one of claims 1-6, 9, wherein finding whether a left or right detour security line exists further comprises selecting an area having a "column" value less than a reference line, determining to find a "column" value that is the smallest possible in each "row" in the area, the "column" being the left detour security line, selecting an area having a "column" value greater than the reference line, determining to find a "column" value that is the largest possible in each "row" in the area, the "column" being the right detour security line.
11. An obstacle avoidance trajectory planning system based on grid search is characterized by comprising: the system comprises a sensing module, a positioning module, a global path planning module, a local path planning module, a transverse and longitudinal control module, a reference datum line module and a whole vehicle executing mechanism, wherein the reference datum line module is used for matching points on a global path determined by the global path planning module according to the current position of a vehicle provided by the positioning module, selecting path points with a fixed distance in the advancing direction of the current position of the vehicle as reference datum lines for local path planning, the local path planning module is used for planning a track according to the reference datum lines and sensing information provided by the sensing module, outputting the track to the transverse and longitudinal control module, and then controlling the whole vehicle through the whole vehicle executing mechanism, wherein the track planning is as follows: generating a grid according to a reference datum line and perception information, adding a high-precision map boundary in the grid, projecting obstacles in the grid and expanding the obstacles, processing the grid according to the current vehicle position and kinematic constraint, judging whether a completely passable 'column' exists, if the completely passable column exists, performing anti-shaking processing on a safety line, generating a passable path, performing smoothing processing on the passable path, if the completely passable column does not exist, searching whether a left-side or right-side obstacle-detouring safety line exists, if the left-side or right-side obstacle-detouring safety line exists, performing anti-shaking processing on the safety line, generating the passable path and performing smoothing processing, and if the left-side or right-side obstacle-detouring safety line does not exist, outputting a previous time path as the current path.
12. The system according to claim 11, wherein the sensing module detects obstacle information, performs sensing fusion and environment recognition processing, and outputs the information to the local path planning module, and the local path planning module generates a grid in a Frenet coordinate system according to a reference trajectory, wherein a direction along a reference line is defined as a "row", a pitch of the "row" is a distance between two points of the reference line, a direction perpendicular to the reference line is defined as a "column", the pitch of the "row" is determined according to a vehicle speed and a controller calculation capability, and the pitch of the "column" is determined according to a road width.
13. The system of claim 12, wherein the rows of the grid gradually increase from the current position of the vehicle to the reference line, the columns of the grid gradually increase from left to right, the value of the passable grid is set to 1, and the value of the non-passable grid is set to 0.
14. The system according to claim 11, wherein the adding of the high-precision map boundary to the grid is to project the boundary of the high-precision map on the grid, the grid outside the boundary is an impassable area, the grid inside the boundary is a passable area, the sensing obstacle and the obstacle point cloud are expanded and then put into the grid, and the grid blocked by the obstacle is the impassable area.
15. The system of claim 11, wherein the safety line is a line that is translated in a "column" direction with respect to the reference line after determining the translation distance based on the search.
16. The system according to claim 11, wherein the generating a traversable path specifically comprises: generating a passable grid based on the position and the pose of the current vehicle according to the position of the current vehicle in the grid and the kinematic constraint of the vehicle; and calculating the grid coordinate with the minimum cost from each 'line' in the passable grid to the safety line based on the current vehicle pose, and forming a searched path as a passage path.
17. The system of any of claims 11-16, wherein projecting obstacles in a grid comprises: acquiring the type, the dynamic and static properties and the vertex position of a target obstacle from a sensor module; converting the top point of the obstacle to a coordinate system of a reference datum line; searching grids corresponding to each vertex coordinate in the barrier; and expanding the grids corresponding to the coordinate vertex points, wherein the grids in the expansion range are all impassable areas.
18. The system of any of claims 11-16, wherein projecting obstacles in a grid comprises: converting the obstacle point cloud into a coordinate system of a reference datum line; selecting rows and columns of the grid to generate a closed area according to an automatic driving scene; the obstacle point clouds in the closed area are effective point clouds, and the perpendicular distance between each effective point cloud and the reference datum line and the corresponding longitudinal distance are calculated; classifying the effective point clouds according to the distance values of the vertical lines; sorting according to the longitudinal distance of the point cloud corresponding to the reference datum line; and merging and projecting all the point clouds into a grid according to the distance from the effective point clouds to the reference datum line, classification and sequencing conditions.
19. The system of claim 13, wherein determining whether there are completely passable "columns" specifically comprises forming a completely passable column set if all rows corresponding to a current column in the grid are 1 passable columns; forming a search list which is sorted to the left side and the right side according to the size of the vertical distance by using the 'column' where the reference datum line is located; and searching in the complete accessible column set according to the limited search width according to the search list.
20. The system of any one of claims 11-16, 19, wherein finding the presence of a left-or right-sided obstacle-detonated security thread further comprises selecting an area having a "column" value less than a reference line, determining to find a "column" value of minimum passable in each "row" in the area, the "column" being the left-sided obstacle-detonated security thread, selecting an area having a "column" value greater than the reference line, determining to find a "column" value of maximum passable in each "row" in the area, the "column" being the right-sided obstacle-detonated security thread.
21. An automobile, characterized by comprising the grid search-based obstacle avoidance trajectory planning system according to any one of claims 11 to 20.
22. A control apparatus, characterized by comprising: a processor and a memory; wherein the processor executes a program corresponding to the executable program code by reading the executable program code stored in the memory, so as to implement the obstacle avoidance trajectory planning method based on grid search according to any one of claims 1 to 10.
23. A computer-readable storage medium, on which a program or instructions are stored, which can be loaded and executed by a processor to perform the method for planning an obstacle avoidance trajectory based on a grid search according to any one of claims 1 to 10.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210754909.0A CN115077553A (en) | 2022-06-30 | 2022-06-30 | Method, system, automobile, equipment and medium for planning track based on grid search |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210754909.0A CN115077553A (en) | 2022-06-30 | 2022-06-30 | Method, system, automobile, equipment and medium for planning track based on grid search |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115077553A true CN115077553A (en) | 2022-09-20 |
Family
ID=83255993
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210754909.0A Pending CN115077553A (en) | 2022-06-30 | 2022-06-30 | Method, system, automobile, equipment and medium for planning track based on grid search |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115077553A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115855095A (en) * | 2022-12-01 | 2023-03-28 | 酷黑科技（北京）有限公司 | Autonomous navigation method and device and electronic equipment |
| CN115965682A (en) * | 2022-12-16 | 2023-04-14 | 镁佳(北京)科技有限公司 | Method and device for determining passable area of vehicle and computer equipment |
| CN116533993A (en) * | 2023-07-07 | 2023-08-04 | 广汽埃安新能源汽车股份有限公司 | Parking control method and device |
-
2022
- 2022-06-30 CN CN202210754909.0A patent/CN115077553A/en active Pending
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115855095A (en) * | 2022-12-01 | 2023-03-28 | 酷黑科技（北京）有限公司 | Autonomous navigation method and device and electronic equipment |
| CN115965682A (en) * | 2022-12-16 | 2023-04-14 | 镁佳(北京)科技有限公司 | Method and device for determining passable area of vehicle and computer equipment |
| CN115965682B (en) * | 2022-12-16 | 2023-09-01 | 镁佳(北京)科技有限公司 | Vehicle passable area determining method and device and computer equipment |
| CN116533993A (en) * | 2023-07-07 | 2023-08-04 | 广汽埃安新能源汽车股份有限公司 | Parking control method and device |
| CN116533993B (en) * | 2023-07-07 | 2023-09-22 | 广汽埃安新能源汽车股份有限公司 | Parking control method and device |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115077553A (en) | Method, system, automobile, equipment and medium for planning track based on grid search | |
| CN114234998A (en) | Unmanned multi-target-point track parallel planning method based on semantic road map | |
| CN112810630B (en) | Method and system for planning track of automatic driving vehicle | |
| CN111301409A (en) | Parking path planning method and device, vehicle and storage medium | |
| US20220080961A1 (en) | Control system and control method for sampling based planning of possible trajectories for motor vehicles | |
| JP2017016645A (en) | Semiautonomous vehicle and method for controlling semiautonomous vehicle | |
| CN111397622B (en) | Intelligent automobile local path planning method based on improved A-algorithm and Morphin algorithm | |
| CN112965476B (en) | High-speed unmanned vehicle trajectory planning system and method based on multi-window model | |
| CN111830979A (en) | Trajectory optimization method and device | |
| CN112284393B (en) | Global path planning method and system for intelligent mobile robot | |
| CN112256034B (en) | Autonomous obstacle avoidance planning method and device | |
| CN111768647A (en) | Autonomous parking method and device based on mobile edge calculation | |
| CN110782707A (en) | Automobile lane change control method and device, automobile and storage medium | |
| KR20210121595A (en) | Optimal route finding device and operating method thereof | |
| CN115140096A (en) | Spline curve and polynomial curve-based automatic driving track planning method | |
| CN114932901A (en) | Self-adaptive speed planning method and device and domain controller | |
| US11353878B2 (en) | Soft-boundary based path optimization for complex scenes for autonomous driving vehicles | |
| CN114620070A (en) | Driving track planning method, device, equipment and storage medium | |
| CN116653963B (en) | Vehicle lane change control method, system and intelligent driving domain controller | |
| CN116476840B (en) | Variable-lane driving method, device, equipment and storage medium | |
| US11878712B2 (en) | Trajectory planning with obstacle avoidance for autonomous driving vehicles | |
| CN115840454B (en) | Multi-vehicle track collaborative planning method and device for unstructured road conflict area | |
| CN115903854B (en) | Automatic driving real-time track planning method for dynamic structured road | |
| CN114750782A (en) | Path planning method, device and equipment and vehicle control method, device and equipment | |
| EP3857327B1 (en) | Implementation of dynamic cost function of self-driving vehicles |
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 |