CN112985445B - Lane-level precision real-time motion planning method based on high-precision map - Google Patents
Lane-level precision real-time motion planning method based on high-precision map Download PDFInfo
- Publication number
- CN112985445B CN112985445B CN202110421757.8A CN202110421757A CN112985445B CN 112985445 B CN112985445 B CN 112985445B CN 202110421757 A CN202110421757 A CN 202110421757A CN 112985445 B CN112985445 B CN 112985445B
- Authority
- CN
- China
- Prior art keywords
- path
- lane
- point
- precision map
- planning
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 230000008569 process Effects 0.000 claims abstract description 24
- 238000005070 sampling Methods 0.000 claims description 22
- 238000009499 grossing Methods 0.000 claims description 12
- 238000005457 optimization Methods 0.000 claims description 8
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 238000004364 calculation method Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 238000003672 processing method Methods 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 6
- 230000008859 change Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 230000002457 bidirectional effect Effects 0.000 description 2
- 150000001875 compounds Chemical class 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
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/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
-
- 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/3453—Special cost functions, i.e. other than distance or default speed limit of road segments
- G01C21/3484—Personalized, e.g. from learned user behaviour or user-defined profiles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Social Psychology (AREA)
- Aviation & Aerospace Engineering (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a lane-level precision real-time motion planning method based on a high-precision map, which comprises the following specific steps of: s1, constructing a lane relation adjacency list according to the high-precision map; judging whether the current position is located in the coverage area of the high-precision map, if so, turning to the step S2, otherwise, turning to the step S3 first and then turning to the step S2; s2, determining a starting point and an end point of driving, searching at least one passable path from the starting point to the end point according to a breadth-first principle, and obtaining a global path; judging whether lane changing or obstacle avoidance is needed or not in the driving process, and driving from the global path to the terminal point if the lane changing or obstacle avoidance is not needed; if yes, go to step S4; s3, planning at least one local path going to the target lane area by taking the lane which is closest to the current position and is positioned in the area covered by the high-precision map as a target; and S4, planning the local dynamic path by taking the global path planning as a guide, and returning the planned local dynamic path to the global path to drive to the terminal.
Description
Technical Field
The invention belongs to the field of high-precision map unmanned driving, and particularly relates to a lane-level precision real-time motion planning method based on a high-precision map.
Background
In recent years, along with the rapid development of technologies such as high-precision maps, 5G, artificial intelligence, Internet of things and the like, unmanned services are more and more close to the lives of people. However, the high-precision map coverage in China is not enough at present, and the running environment of the unmanned vehicle may comprise both a structured road and an unstructured wild. From the perspective of planning, it is relatively easy for the unmanned vehicle to make decisions and plan on a normal structured and closed road, and still faces a huge challenge to an unstructured and open road scene, so that a better planning method needs to be explored to help the unmanned vehicle adapt to different scenes.
Therefore, there is a need to develop a lane-level precision real-time motion planning method based on a high-precision map, which is based on the high-precision map, takes global path planning as guidance and dynamically plans and optimizes local paths.
Disclosure of Invention
The invention aims to solve the technical problem of providing a lane-level precision real-time motion planning method based on a high-precision map, which takes global path planning as guidance and dynamically plans and optimizes local paths on the basis of the high-precision map.
In order to solve the technical problems, the invention adopts the technical scheme that: the lane-level precision real-time motion planning method based on the high-precision map specifically comprises the following steps:
s1: constructing a lane relation adjacency list according to the high-precision map; judging whether the current position of the unmanned vehicle is located in the coverage area of the high-precision map, and if so, performing step S2; if the map is not located in the area covered by the high-precision map, the step S3 is executed, and then the step S2 is executed;
s2: determining a starting point and an end point of driving, searching at least one passable path from the starting point to the end point according to a breadth-first principle, and selecting an optimal passable path to obtain a global path; judging whether lane changing or obstacle avoidance is needed or not in the driving process, and driving from the global path to the terminal point if the lane changing or obstacle avoidance is not needed; if yes, go to step S4;
s3: taking a lane which is closest to the current position and is positioned in the area covered by the high-precision map as a target, and planning at least one local path going to a target lane area;
s4: and planning a local dynamic path by taking the global path planning as a guide, and returning the planned local dynamic path to the global path from the local dynamic path to the destination after the local dynamic path is obtained.
By adopting the technical scheme, the global path planning is firstly carried out according to the high-precision map, and then the motion path is dynamically planned in real time according to the road condition in the running process of the unmanned vehicle, so that the unmanned vehicle can autonomously run from the set starting point to the set terminal point. The invention selects the optimal passable path as the global path with optimal time, thereby finishing the planning of the global path and providing a guiding function for the subsequent local motion planning.
As a preferred technical solution of the present invention, the specific construction method for constructing the lane relation adjacency list in step S1 includes: and respectively searching the backward lanes of each lane section, taking the current lane code as a key, and taking the set of codes of all the backward lanes of the current lane as a value to construct a search dictionary table. In addition, the high-precision map used by the method is based on an optional-format high-precision map, and other attribute information is added to the content of the high-precision map, wherein the high-precision map comprises topological relation, adjacent relation, passable and impassable area and other road condition information among all sections of lanes; the topological relation among the lanes exists in the high-precision map, namely, each lane section has a forward lane and a backward lane, and each lane section has a unique code, so that a lane relation adjacency list can be constructed according to the unique codes; in the process of constructing the dictionary table, lane codes which have an impassable relation with the current lane in all the lanes in the backward direction need to be eliminated.
As a preferred technical solution of the present invention, the specific search method for searching for at least one passable path from the starting point to the end point according to the breadth-first principle in step S2 is as follows:
s21: judging the position of the current vehicle according to the positioning, recording the code of the current lane if the vehicle is in the area covered by the high-precision map, and adding the code of the current lane into a search queue;
s22: searching a first lane code in the search queue, judging whether the first lane code belongs to a target lane code, and if not, removing the first lane code; then, the value of the corresponding key of the current lane code is searched through the lane relation adjacency list in the step S1, and all adjacency codes of the current lane code are added into a search queue; meanwhile, marking the current lane code as traversed, and storing the front lane code information of the current lane code by taking the current lane code as an index value of a container;
s23: turning to step S22, repeatedly judging until the first lane code in the search queue is the code where the target lane is located, or the queue is empty, namely the passing path from the starting point to the destination is not found;
s24: the lane codes included in all the passing paths are found by the preceding lane code information stored in the container of step S22 from the lane code reverse search where the end point is located in a recursive manner, and the optimal path is selected as the global path according to the global path cost model.
As a preferred embodiment of the present invention, if the vehicle is not located in the area covered by the high-precision map in step S1, the method proceeds to step S3, and the step S3 uses an improved RRT algorithm to plan a local path to the target lane area, with the lane closest to the current position and located in the area covered by the high-precision map as the target, specifically includes:
s31: firstly, growing a random tree by taking the current position of the unmanned vehicle as a root node;
s32: generating a random point A in a detectable space of the unmanned vehicle, and simultaneously ensuring that in the process of generating the improved RRT algorithm random tree, a lane which is closest to the current position and is positioned in the coverage area of the high-precision map is used as a guide, namely every n random sampling points, so that the point A is ensured to fall into a target lane section area, and the convergence speed of the random tree to the target lane is accelerated;
s33: finding the point closest to the point A on the random tree, and recording as a point B;
s34: backtracking an optimal path from the point B to the starting point, judging whether the BA direction is more than 135 degrees with the last edge of the path, if so, turning to the step S35, otherwise, repeating the steps S32-S34;
s35: the point B grows towards the direction of the point A according to a set step length, and if the point B does not touch an obstacle, the growing edge is added to the point B to form a newly generated edge;
s36: then, selecting tree nodes in a set range by taking the end points of the newly generated edges as centers, and reselecting father nodes and the routing random tree of the newly generated edges by taking the cost optimal principle as a principle;
s37: repeating the steps S31-S37 in the process of the unmanned vehicle running until the unmanned vehicle runs to the target place, and obtaining a planned path;
s38: and simplifying and smoothing the obtained random tree of the planned path so as to obtain a local path.
As a preferred embodiment of the present invention, the method for performing the simplification and smoothing process in step S38 includes:
s381: deleting nodes positioned at even numbers from the starting point of the planned path, and if the last node is an even number, reserving the node to form a new passing path;
s382: judging whether each section of the new passing route in the step S381 collides with an obstacle, and if so, recovering the nodes of the section of the new passing route deleted in the step S381; meanwhile, whether the included angle between the section of the route and the adjacent route is smaller than 135 degrees is judged, and if the included angle is smaller than 135 degrees, nodes deleted in the step S381 on the next section of the road of the section of the route are recovered;
s383: repeating the steps 381-382 until the simplification cannot be realized;
s384: and performing polynomial curve fitting on the simplified path, and after the unmanned vehicle passes through the simplified path and drives to the target lane segment, then turning to the steps S21-S24 to plan a global path from the starting point to the end point. A polynomial curve fit of degree 3 or 5 is generally performed.
As a preferred embodiment of the present invention, in step S4, if it is detected that there is a running vehicle or an obstacle ahead in the front running route during the running of the unmanned vehicle, the local dynamic route is planned in real time with the global route planning as guidance, and the local dynamic route is obtained to avoid the running vehicle or the obstacle ahead; the method for planning the local dynamic path by taking the global path planning as the guide specifically comprises the following steps:
s41: the planning method comprises the steps of adopting an s-t coordinate system defined in a high-precision map format, decomposing the current motion of the unmanned vehicle to the s direction and the t direction in the s-t coordinate system, sampling in the s direction and the t direction to obtain a plurality of sampling points, and combining the sampling points in pairs to obtain a multi-section path leading to a target lane;
s42: simplifying and smoothing the multi-section path to the target lane obtained in the step S41 by adopting the simplifying and smoothing method in the step S38, performing multiple polynomial curve fitting on the simplified path, and performing quadratic optimization to obtain a local dynamic path;
s43: when the unmanned vehicle returns to the global path from the local dynamic path, the distance between the unmanned vehicle and the rear vehicle in the s direction is larger than 10-15 m; when the unmanned vehicle returns to the global route again, the unmanned vehicle is driven to the destination according to the global route planning performed in step S2.
Considering the safety and comfort of the vehicle in the lane changing process, the random falling points in the s direction and the t direction need to be respectively limited in the sampling process as follows:
(a) in general, the included angle between the unmanned vehicle and the marked line is ensured to be as small as possible in the lane changing process, so that the displacement change ratio of the unmanned vehicle in the t direction and the s direction in unit time is required to be controlled to be less than 1;
(b) in order to ensure that the transverse offset of the vehicle is close to the center line of the lane in the driving process, the falling points in the t direction are all fallen in the feasible lane section area, and every 15 sampling points must have one point in the t direction to fall on the center line of the adjacent lane;
(c) for a bidirectional lane, a sampling point in the direction of t must be deviated to one side of the center line of the current lane, namely t is ensured to be always larger than or smaller than 0 in the sampling process;
(d) in order to keep the distance between vehicles, the safety distance from the sampling drop point position to the front vehicle or the barrier in the s direction is 10-15 m, and the safety distance can be set to be different according to different scenes;
as a preferred embodiment of the present invention, the method of performing the secondary optimization processing in step S42 includes:
if points with too large deviation from the center line of the lane exist in the simplified and smoothed multi-section path, if P is a point with large deviation, the BC path is taken as a chord, a circle is drawn by the radius R, n bisector points are inserted into the inferior arc BC to determine an alternative local dynamic path, and the radius calculation formula is as follows:
in the formula (I), the compound is shown in the specification,
representing the radius of curvature, which may vary according toSelecting different value ranges for the scene and the driving speed;
s422: respectively calculating cost values of the local dynamic path and the alternative local dynamic path, and reserving the path with lower cost, wherein the cost function is as follows:
wherein c represents a cost value; k is a radical of1，k2The weighting coefficients are respectively, the sum of the weighting coefficients is 1, and each weighting coefficient can be set according to the actual situation; n represents the number of nodes of each path; diRepresenting the vertical distance between each path and the obstacle in each path; t is tjIndicating the displacement of each point of each path in the direction of t.
Compared with the prior art, the lane-level precision real-time motion planning method based on the high-precision map fully considers the indexes of human driving behavior habits, vehicle dynamics constraints, safety and the like, combines the overall planning of global path planning and the detailed advantages of local path planning, and can realize comfortable and safe autonomous navigation motion of unmanned vehicles in most structural and unstructured environments.
Drawings
FIG. 1 is a schematic flow diagram of a lane-level precision real-time motion planning method based on a high-precision map according to the present invention;
FIG. 2 is a schematic diagram of an improvement of the RRT algorithm in the high-precision map-based lane-level precision real-time motion planning method of the present invention;
FIG. 3 is a schematic diagram of a path simplification method in the lane-level precision real-time motion planning method based on a high-precision map according to the present invention;
FIG. 4 is a schematic diagram of an s-t coordinate system in the lane-level precision real-time motion planning method based on a high-precision map;
FIG. 5 is a schematic diagram of a path optimization method in the lane-level precision real-time motion planning method based on a high-precision map according to the present invention;
FIG. 6 is an effect diagram after local dynamic path optimization in the lane-level precision real-time motion planning method based on the high-precision map.
Detailed Description
The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the drawings of the embodiments of the present invention.
Example (b): as shown in fig. 1, the method for planning the real-time lane-level precision motion based on the high-precision map specifically includes the following steps:
s1: constructing a lane relation adjacency list according to the high-precision map; judging whether the current position of the unmanned vehicle is located in the coverage area of the high-precision map, and if so, performing step S2; if the map is not located in the area covered by the high-precision map, the step S3 is executed, and then the step S2 is executed;
the specific construction method for constructing the lane relation adjacency list in step S1 is as follows: respectively searching the backward lanes of each lane section, taking the current lane code as a key, and taking the set of codes of all the backward lanes of the current lane as a value to construct a search dictionary table;
s2: determining a starting point and an end point of driving, searching at least one passable path from the starting point to the end point according to a breadth-first principle, and selecting an optimal passable path to obtain a global path; judging whether lane changing or obstacle avoidance is needed or not in the driving process, and driving from the global path to the terminal point if the lane changing or obstacle avoidance is not needed; if yes, go to step S4;
the specific search method for searching for at least one passable path from the starting point to the end point according to the breadth-first principle in step S2 includes:
s21: judging the position of the current vehicle according to the positioning, recording the code of the current lane if the vehicle is in the area covered by the high-precision map, and adding the code of the current lane into a search queue;
s22: searching a first lane code in the search queue, judging whether the first lane code belongs to a target lane code, and if not, removing the first lane code; then, the value of the corresponding key of the current lane code is searched through the lane relation adjacency list in the step S1, and all adjacency codes of the current lane code are added into a search queue; meanwhile, marking the current lane code as traversed, and storing the front lane code information of the current lane code by taking the current lane code as an index value of a container;
s23: turning to step S22, repeatedly judging until the first lane code in the search queue is the code where the target lane is located, or the queue is empty, namely the passing path from the starting point to the destination is not found;
s24: through the front lane code information stored in the container in the step S22, the lane codes included in all the passing paths are found by reverse search from the lane code where the end point is located in a recursive manner, and the optimal path is selected as the global path according to the global path cost model;
s3: taking a lane which is closest to the current position and is positioned in the area covered by the high-precision map as a target, and planning at least one local path going to a target lane area;
if the vehicle is not located in the area covered by the high-precision map in the step S1, go to step S3, and the step S3 uses an improved RRT algorithm to target the lane closest to the current position and located in the area covered by the high-precision map, and the specific step of planning a local path to the target lane area is as follows:
s31: firstly, growing a random tree by taking the current position of the unmanned vehicle as a root node;
s32: generating a random point A in a detectable space of the unmanned vehicle, and simultaneously ensuring that in the process of generating the improved RRT algorithm random tree, a lane which is closest to the current position and is positioned in the coverage area of the high-precision map is used as a guide, namely every n random sampling points, so that the point A is ensured to fall into a target lane section area, and the convergence speed of the random tree to the target lane is accelerated;
s33: finding the point closest to the point A on the random tree, and recording as a point B;
s34: backtracking an optimal path from the point B to the starting point, judging whether the BA direction is more than 135 degrees with the last edge of the path, if so, turning to the step S35, otherwise, repeating the steps S32-S34;
s35: the point B grows towards the direction of the point A according to a set step length, and if the point B does not touch an obstacle, the growing edge is added to the point B to form a newly generated edge;
s36: then, selecting tree nodes in a set range by taking the end points of the newly generated edges as centers, and reselecting father nodes and the routing random tree of the newly generated edges by taking the cost optimal principle as a principle;
s37: repeating the steps S31-S37 in the process of the unmanned vehicle running until the unmanned vehicle runs to the target place, and obtaining a planned path;
s38: and simplifying and smoothing the obtained random tree of the planned path so as to obtain a local path.
The method for performing the simplification and smoothing processing in step S38 includes the specific steps of:
s381: deleting nodes positioned at even numbers from the starting point of the planned path, and if the last node is an even number, reserving the node to form a new passing path;
s382: judging whether each section of the new passing route in the step S381 collides with an obstacle, and if so, recovering the nodes of the section of the new passing route deleted in the step S381; meanwhile, whether the included angle between the section of the route and the adjacent route is smaller than 135 degrees is judged, and if the included angle is smaller than 135 degrees, nodes deleted in the step S381 on the next section of the road of the section of the route are recovered; as shown in fig. 3: assuming that an obstacle triangle exists between points 1 to 8, the initial path is planned as shown by a black solid line in the figure, and the route is simplified to be 1-5-8;
s383: repeating the steps 381-382 until the simplification cannot be realized;
s384: performing polynomial curve fitting on the simplified path, and after the unmanned vehicle passes through the simplified path and drives to a target lane segment, turning to the steps S21-S24 to plan a global path from the starting point to the end point;
s4: planning a local dynamic path by taking the global path planning as a guide, and returning the planned local dynamic path to the global path to drive to a terminal point after obtaining the local dynamic path;
if the unmanned vehicle needs to avoid an obstacle or change lanes in the step S2, go to step S4, as shown in fig. 5, when it is detected that a front driving vehicle or an obstacle exists in the front driving path in the driving process of the unmanned vehicle, plan a local dynamic path in real time with global path planning as guidance, and obtain a local dynamic path to avoid the front driving vehicle or the obstacle in the step S4; the method for planning the local dynamic path by taking the global path planning as the guide specifically comprises the following steps:
s41: adopting an s-t coordinate system defined in a high-precision map format, decomposing the current motion of the unmanned vehicle to the s direction and the t direction in the s-t coordinate system, sampling in the s direction and the t direction to obtain a plurality of sampling points, and then combining the sampling points in pairs to obtain a multi-section path leading to a target lane; because roads are mostly curved, in order to simplify calculation and meet the real-time requirement, the invention uses an s-t coordinate system defined in an openrive high-precision map format, as shown in fig. 4, the s coordinate is along the direction of a road reference line, the t coordinate is positioned at the lateral position of the reference line, the left is positive, and the right is negative; firstly, determining a current lane section according to the position of an unmanned vehicle, secondly determining a passable lane area according to lane adjacent relation information in a high-precision map and a perception system of the vehicle, and finally determining the final lane section area to be driven by taking global path planning as guidance;
considering the safety and comfort of the vehicle in the lane changing process, the random falling points in the s direction and the t direction need to be respectively limited in the sampling process as follows:
(a) in general, the included angle between the unmanned vehicle and the marked line is ensured to be as small as possible in the lane changing process, so that the displacement change ratio of the unmanned vehicle in the t direction and the s direction in unit time is required to be controlled to be less than 1;
(b) in order to ensure that the transverse offset of the vehicle is close to the center line of the lane in the driving process, the falling points in the t direction are all fallen in the feasible lane section area, and every 15 sampling points must have one point in the t direction to fall on the center line of the adjacent lane;
(c) for a bidirectional lane, a sampling point in the direction of t must be deviated to one side of the center line of the current lane, namely t is ensured to be always larger than or smaller than 0 in the sampling process;
(d) in order to keep the distance between vehicles, the safety distance from the sampling drop point position to the front vehicle or the barrier in the s direction is 10-15 m, and the safety distance can be set to be different according to different scenes;
s42: simplifying and smoothing the multi-section path to the target lane obtained in the step S41 by adopting the simplifying and smoothing method in the step S38, performing multiple polynomial curve fitting on the simplified path, and performing quadratic optimization to obtain a local dynamic path;
the steps of the method of the second optimization processing in step S42 include:
s421: if points with too large deviation from the center line of the lane exist in the simplified and smoothed multi-segment path, if P is a point with large deviation, the BC path is taken as a chord, a circle is drawn by using the R radius, and n bisectors are inserted into the minor arc BC to determine an alternative local dynamic path, as shown in FIG. 6, the path is initially planned by A-B-P-C-D, the path is initially planned by A-B-P1-P2-P3-C-D, the path is optimized, and the radius calculation formula is as follows:
in the formula (I), the compound is shown in the specification,
the curvature radius is expressed, and different value ranges can be selected according to different scenes and running speeds;
s422: respectively calculating cost values of the local dynamic path and the alternative local dynamic path, and reserving the path with lower cost, wherein the cost function is as follows:
wherein c represents a cost value; k is a radical of1，k2The weighting coefficients are respectively, the sum of the weighting coefficients is 1, and each weighting coefficient can be set according to the actual situation; n represents the number of nodes of each path; diRepresenting the vertical distance between each path and the obstacle in each path; t is tjIndicating the displacement of each point of each path in the direction of t.
S43: when the unmanned vehicle returns to the global path from the local dynamic path, the distance between the unmanned vehicle and the rear vehicle in the s direction is larger than 10-15 m; when the unmanned vehicle returns to the global route again, the unmanned vehicle is driven to the destination according to the global route planning performed in step S2.
The above description is only exemplary of the present invention and should not be taken as limiting the invention, as any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (5)
1. A lane-level precision real-time motion planning method based on a high-precision map is characterized by specifically comprising the following steps of:
s1: constructing a lane relation adjacency list according to the high-precision map; judging whether the current position of the unmanned vehicle is located in the coverage area of the high-precision map, and if so, performing step S2; if the map is not located in the area covered by the high-precision map, the step S3 is executed, and then the step S2 is executed;
s2: determining a starting point and an end point of driving, searching at least one passable path from the starting point to the end point according to a breadth-first principle, and selecting an optimal passable path to obtain a global path; judging whether lane changing or obstacle avoidance is needed or not in the driving process, and driving from the global path to the terminal point if the lane changing or obstacle avoidance is not needed; if yes, go to step S4;
s3: taking a lane which is closest to the current position and is positioned in the area covered by the high-precision map as a target, and planning at least one local path going to a target lane area;
s4: planning a local dynamic path by taking the global path planning as a guide, and returning the planned local dynamic path to the global path to drive to a terminal point after obtaining the local dynamic path;
the specific construction method for constructing the lane relation adjacency list in step S1 is as follows: respectively searching the backward lanes of each lane section, taking the current lane code as a key, and taking the set of codes of all the backward lanes of the current lane as a value to construct a search dictionary table;
the specific search method for searching for at least one passable path from the starting point to the end point according to the breadth-first principle in step S2 includes:
s21: judging the position of the current unmanned vehicle according to the positioning, recording the code of the current lane if the unmanned vehicle is in the area covered by the high-precision map, and adding the code of the current lane into a search queue;
s22: searching a first lane code in the search queue, judging whether the first lane code belongs to a target lane code, and if not, removing the first lane code; then, the value of the corresponding key of the current lane code is searched through the lane relation adjacency list in the step S1, and all adjacency codes of the current lane code are added into a search queue; meanwhile, marking the current lane code as traversed, and storing the front lane code information of the current lane code by taking the current lane code as an index value of a container;
s23: turning to step S22, repeatedly judging until the first lane code in the search queue is the code where the target lane is located, or the queue is empty, namely the passing path from the starting point to the destination is not found;
s24: the lane codes included in all the passing paths are found by the preceding lane code information stored in the container of step S22 from the lane code reverse search where the end point is located in a recursive manner, and the optimal path is selected as the global path according to the global path cost model.
2. The method for lane-level precision real-time movement planning based on high-precision map according to claim 1, wherein when the vehicle is not located in the coverage area of the high-precision map in step S1, the step S3 is performed, and the step S3 uses the modified RRT algorithm to target the lane closest to the current position and located in the coverage area of the high-precision map, and the specific step of planning a local path to the target lane area is:
s31: firstly, growing a random tree by taking the current position of the unmanned vehicle as a root node;
s32: generating a random point A in a detectable space of the unmanned vehicle, and simultaneously ensuring that in the process of generating the improved RRT algorithm random tree, a lane which is closest to the current position and is positioned in the coverage area of the high-precision map is used as a guide, namely every n random sampling points, so that the point A is ensured to fall into a target lane section area, and the convergence speed of the random tree to the target lane is accelerated;
s33: finding the point closest to the point A on the random tree, and recording as a point B;
s34: backtracking an optimal path from the point B to the starting point, judging whether the BA direction is more than 135 degrees with the last edge of the path, if so, turning to the step S35, otherwise, repeating the steps S32-S34;
s35: the point B grows towards the direction of the point A according to a set step length, and if the point B does not touch an obstacle, the growing edge is added to the point B to form a newly generated edge;
s36: then, selecting tree nodes in a set range by taking the end points of the newly generated edges as centers, and reselecting father nodes and the routing random tree of the newly generated edges by taking the cost optimal principle as a principle;
s37: repeating the steps S31-S37 in the process of the unmanned vehicle running until the unmanned vehicle runs to the target place, and obtaining a planned path;
s38: and simplifying and smoothing the obtained random tree of the planned path so as to obtain a local path.
3. The high-precision map-based lane-level precision real-time motion planning method according to claim 2, wherein the method for simplifying and smoothing in step S38 includes the following specific steps:
s381: deleting nodes positioned at even numbers from the starting point of the planned path, and if the last node is an even number, reserving the node to form a new passing path;
s382: judging whether each section of the new passing route in the step S381 collides with an obstacle, and if so, recovering the nodes of the section of the new passing route deleted in the step S381; meanwhile, whether the included angle between the section of the route and the adjacent route is smaller than 135 degrees is judged, and if the included angle is smaller than 135 degrees, nodes deleted in the step S381 on the next section of the road of the section of the route are recovered;
s383: repeating the steps 381-382 until the simplification cannot be realized;
s384: and performing polynomial curve fitting on the simplified path, and after the unmanned vehicle passes through the simplified path and drives to the target lane segment, then turning to the steps S21-S24 to plan a global path from the starting point to the end point.
4. The method for lane-level precision real-time movement planning based on high-precision map according to claim 3, wherein in step S4, if it is detected that there is a running vehicle or an obstacle ahead in the running path ahead during the unmanned vehicle running, the global path plan is used as a guide to plan the local dynamic path in real time, and the local dynamic path is obtained to avoid the running vehicle or the obstacle ahead; the method for planning the local dynamic path by taking the global path planning as the guide specifically comprises the following steps:
s41: the planning method comprises the steps of adopting an s-t coordinate system defined in a high-precision map format, decomposing the current motion of the unmanned vehicle to the s direction and the t direction in the s-t coordinate system, sampling in the s direction and the t direction to obtain a plurality of sampling points, and combining the sampling points in pairs to obtain a multi-section path leading to a target lane;
s42: simplifying and smoothing the multi-section path to the target lane obtained in the step S41 by adopting the simplifying and smoothing method in the step S38, performing multiple polynomial curve fitting on the simplified path, and performing quadratic optimization to obtain a local dynamic path;
s43: when the unmanned vehicle returns to the global path from the local dynamic path, the distance between the unmanned vehicle and the rear vehicle in the s direction is larger than 10-15 m; when the unmanned vehicle returns to the global route again, the unmanned vehicle is driven to the destination according to the global route planning performed in step S2.
5. The high-precision map-based lane-level precision real-time motion planning method according to claim 4, wherein the step of the quadratic optimization processing method in the step S42 comprises:
s421: if points with too large deviation from the center line of the lane exist in the simplified and smoothed multi-section path, if P is a point with large deviation, the BC path is taken as a chord, a circle is drawn by the radius R, n bisector points are inserted into the inferior arc BC to determine an alternative local dynamic path, and the radius calculation formula is as follows:
in the formula, kappa represents the curvature radius, and different value ranges can be selected according to different scenes and driving speeds;
s422: respectively calculating cost values of the local dynamic path and the alternative local dynamic path, and reserving the path with lower cost, wherein the cost function is as follows:
wherein c represents a cost value; k is a radical of1，k2The weighting coefficients are respectively, the sum of the weighting coefficients is 1, and each weighting coefficient can be set according to the actual situation; n represents the number of nodes of each path; diRepresenting the vertical distance between each path and the obstacle in each path; t is tjIndicating the displacement of each point of each path in the direction of t.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110421757.8A CN112985445B (en) | 2021-04-20 | 2021-04-20 | Lane-level precision real-time motion planning method based on high-precision map |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110421757.8A CN112985445B (en) | 2021-04-20 | 2021-04-20 | Lane-level precision real-time motion planning method based on high-precision map |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112985445A CN112985445A (en) | 2021-06-18 |
| CN112985445B true CN112985445B (en) | 2021-08-13 |
Family
ID=76341238
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110421757.8A Active CN112985445B (en) | 2021-04-20 | 2021-04-20 | Lane-level precision real-time motion planning method based on high-precision map |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112985445B (en) |
Families Citing this family (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114003026A (en) * | 2021-06-22 | 2022-02-01 | 的卢技术有限公司 | Improved lane change mechanism based on Apollo framework |
| CN113435650A (en) * | 2021-06-29 | 2021-09-24 | 上海东普信息科技有限公司 | Planning method, device and equipment for vehicle driving path and storage medium |
| CN113483775B (en) * | 2021-06-30 | 2024-06-14 | 上海商汤临港智能科技有限公司 | Path prediction method and device, electronic equipment and computer readable storage medium |
| CN113393055B (en) * | 2021-07-05 | 2023-07-25 | 苏州清研捷运信息科技有限公司 | Pretreatment and use method of truck navigation along-route data |
| CN113721608A (en) * | 2021-08-16 | 2021-11-30 | 河南牧原智能科技有限公司 | Robot local path planning method and system and readable storage medium |
| CN113778102B (en) * | 2021-09-22 | 2023-05-12 | 重庆长安汽车股份有限公司 | AVP global path planning system, method, vehicle and storage medium |
| CN113865600B (en) * | 2021-09-28 | 2023-01-06 | 北京三快在线科技有限公司 | High-precision map construction method and device |
| CN114353797A (en) * | 2021-12-13 | 2022-04-15 | 哈尔滨工业大学芜湖机器人产业技术研究院 | Real-time path planning method and AGV |
| CN114413926B (en) * | 2021-12-13 | 2023-10-13 | 武汉中海庭数据技术有限公司 | Map display method based on mapbox engine osm data and high-precision data |
| CN114323041B (en) * | 2021-12-24 | 2024-09-20 | 深圳一清创新科技有限公司 | Key point diagram construction method and device and electronic equipment |
| CN114326737A (en) * | 2021-12-30 | 2022-04-12 | 深兰人工智能(深圳)有限公司 | Path planning method and device, electronic equipment and computer readable storage medium |
| CN114594772A (en) * | 2022-03-09 | 2022-06-07 | 深圳市普渡科技有限公司 | Robot, path planning method, device and storage medium |
| CN114724377B (en) * | 2022-06-01 | 2022-10-18 | 华砺智行(武汉)科技有限公司 | Unmanned vehicle guiding method and system based on vehicle-road cooperation technology |
| CN115493609B (en) * | 2022-09-27 | 2023-05-23 | 禾多科技（北京）有限公司 | Lane-level path information generation method, device, equipment, medium and program product |
| CN116539058B (en) * | 2023-05-08 | 2024-06-21 | 山东省交通规划设计院集团有限公司 | Navigation system with line planning function |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE19808111B4 (en) * | 1997-02-28 | 2007-04-05 | Aisin AW Co., Ltd., Anjo | Car navigation system |
| CN107664503A (en) * | 2016-07-29 | 2018-02-06 | 上海汽车集团股份有限公司 | Vehicle path planning method and device |
| CN108007471B (en) * | 2016-10-28 | 2021-06-29 | 北京四维图新科技股份有限公司 | Vehicle guide block acquisition method and device and automatic driving method and system |
| KR102395283B1 (en) * | 2016-12-14 | 2022-05-09 | 현대자동차주식회사 | Apparatus for controlling automatic driving, system having the same and method thereof |
| CN107702716B (en) * | 2017-08-31 | 2021-04-13 | 广州小鹏汽车科技有限公司 | Unmanned driving path planning method, system and device |
| CN107992050B (en) * | 2017-12-20 | 2021-05-11 | 广州汽车集团股份有限公司 | Method and device for planning local path motion of unmanned vehicle |
| CN108519773B (en) * | 2018-03-07 | 2020-01-14 | 西安交通大学 | Path planning method for unmanned vehicle in structured environment |
| CN111289006B (en) * | 2020-03-20 | 2022-03-29 | 上海商汤临港智能科技有限公司 | Lane navigation path generation method and device and driving control method and device |
-
2021
- 2021-04-20 CN CN202110421757.8A patent/CN112985445B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN112985445A (en) | 2021-06-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112985445B (en) | Lane-level precision real-time motion planning method based on high-precision map | |
| US11460311B2 (en) | Path planning method, system and device for autonomous driving | |
| US11747826B2 (en) | Method for route optimization based on dynamic window and redundant node filtering | |
| US8121749B1 (en) | System for integrating dynamically observed and static information for route planning in a graph based planner | |
| CN110333659B (en) | Unmanned vehicle local path planning method based on improved A star search | |
| CN110231824B (en) | Intelligent agent path planning method based on straight line deviation method | |
| CN111694356B (en) | Driving control method and device, electronic equipment and storage medium | |
| CN111375205B (en) | Processing method and device of path finding path in game, electronic equipment and storage medium | |
| KR102425741B1 (en) | Autonomous Driving Method Adapted for a Recognition Failure of Road Line and a Method for Building Driving Guide Data | |
| CN115713856A (en) | Vehicle path planning method based on traffic flow prediction and actual road conditions | |
| CN110487290B (en) | Unmanned vehicle local path planning method based on variable step size A star search | |
| CN112033426B (en) | Driving path planning method and device and electronic equipment | |
| CN114115298B (en) | Unmanned vehicle path smoothing method and system | |
| CN111879307A (en) | Vehicle path planning method based on vehicle body parameters and engineering construction information | |
| CN114740898B (en) | Three-dimensional track planning method based on free space and A-algorithm | |
| CN110849385B (en) | Track planning method and system based on double-layer heuristic search conjugate gradient descent | |
| CN113515111B (en) | Vehicle obstacle avoidance path planning method and device | |
| CN115079702A (en) | Intelligent vehicle planning method and system under mixed road scene | |
| CN108919805A (en) | A kind of unmanned auxiliary system of vehicle | |
| CN117371760B (en) | Layered passenger ship personnel emergency evacuation method considering deadline and congestion relief | |
| CN117970928A (en) | Route planning method based on cooperation of multiple mobile robots of ROS system | |
| CN116560360A (en) | Method and system for planning real-time dynamic path of medical care robot facing complex dynamic scene | |
| CN116610109A (en) | Gradient-based forward ant colony algorithm unmanned vehicle path planning method | |
| CN115326070A (en) | Unmanned aerial vehicle flight path planning method based on three-dimensional tangent line graph method in urban low-altitude environment | |
| Moreira | Deep Reinforcement Learning for Automated Parking |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CP01 | Change in the name or title of a patent holder |
Address after: 210042 8 Blocks 699-22 Xuanwu Avenue, Xuanwu District, Nanjing City, Jiangsu Province Patentee after: Speed Technology Co.,Ltd. Address before: 210042 8 Blocks 699-22 Xuanwu Avenue, Xuanwu District, Nanjing City, Jiangsu Province Patentee before: SPEED TIME AND SPACE INFORMATION TECHNOLOGY Co.,Ltd. |
|
| CP01 | Change in the name or title of a patent holder |