Intersection path planning and tracking method based on three-level positioning and two-level path planning mechanism

Abstract

The present invention relates to a method for intersection path planning and tracking based on a three-level positioning and two-level path planning mechanism, and belongs to the field of autonomous driving technology. This method does not require additional sensors and only relies on the existing L2 autonomous driving vehicle's onboard low-precision GPS system, onboard autonomous driving camera, and lightweight intersection pre-built map. It uses a three-level positioning module to achieve precise positioning of the vehicle at the intersection, and designs a two-level path planning strategy to ensure that the vehicle can complete steering operations safely and efficiently when passing through the intersection. This method can still ensure the accuracy of vehicle positioning and realize timely updates of path planning even when computing power is limited, significantly improving the availability and safety of L2 autonomous driving vehicles in complex intersection scenarios, while maintaining the technical advantages of low cost and high efficiency.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving and relates to an intersection path planning and tracking method based on a three-level positioning and a two-level path planning mechanism. Background Art

[0002] In urban traffic environments, lane markings are often missing in scenarios such as intersections, reducing the integrity of data acquired by the perception system and, in turn, its reliability. In such situations, the risk of failure in core algorithms such as trajectory prediction and decision planning increases significantly, directly impacting the proper functioning of the autonomous driving system. Existing Level 2 autonomous driving functions (such as Lane Centering Control (LCC)) typically automatically exit autonomous driving mode when encountering missing lane markings, requiring the driver to immediately take over. This not only limits the adaptability of the assisted driving system but also reduces the user's driving experience and satisfaction.

[0003] To improve the performance of autonomous driving systems in lane-missing scenarios, pre-built maps serve as a powerful auxiliary tool for perception systems, providing critical prior information about the traffic environment, such as lane markings, zebra crossings, and other important road signs. This prior information supports trajectory prediction and decision-making, compensating for insufficient perception data, reducing the risk of algorithm failure, and ensuring the continued effective operation of Level 2 autonomous vehicles in complex urban road scenarios. Therefore, by combining pre-built maps with existing perception systems, it is expected to significantly improve the adaptability and reliability of Level 2 autonomous vehicles in lane-missing scenarios. Summary of the Invention

[0004] In view of this, the purpose of the present invention is to provide an intersection path planning and tracking method based on a three-level positioning and two-level path planning mechanism. The method includes a three-level positioning module and a two-level path planning module. Compared with existing intersection autonomous driving methods, the present invention does not require additional sensors or increase vehicle costs, and can effectively solve the problem of existing L2 autonomous driving vehicles automatically exiting when turning at intersections without lane lines. Even when computing power is limited, the present invention can still accurately calculate the vehicle's positioning information and safely and efficiently guide the vehicle to complete the intersection turning operation.

[0005] In order to achieve the above object, the present invention provides the following technical solutions:

[0006] A method for intersection path planning and tracking based on a three-level positioning and two-level path planning mechanism, the method comprising:

[0007] Establish a three-level positioning module, including:

[0008] Establish a primary positioning module: obtain coarse positioning information based on a low-precision GPS system, and retrieve a vector map of the corresponding intersection based on the coarse positioning information;

[0009] Establish a secondary positioning module: When a vehicle approaches an intersection, it uses the vehicle's real-time perception data to extract scene features of the current road, matches them with the pre-built map, and calculates the vehicle's precise planar positioning at the intersection;

[0010] Establish a three-level positioning module: When the vehicle is about to complete the turn at the intersection, the scene features of the road after the turn are extracted based on the vehicle's real-time perception data, and matched with the pre-built map to complete the vehicle repositioning;

[0011] Establish a two-level path planning module, including:

[0012] Establish a first-level path planning module: When a vehicle approaches an intersection, it plans a connecting route from the vehicle's current position to the intersection's turning path based on precise positioning information and a pre-built intersection vector map;

[0013] Establish a secondary path planning module: When the vehicle is about to complete a turn at an intersection, based on the repositioning information and combined with the pre-built vector map, a connecting path from the vehicle's current position to the lane after the turn is planned.

[0014] Furthermore, the scene features include lane lines, road boundaries, zebra crossings and lane markings, and the scene features are matched with the pre-built map through rigid body transformation and minimizing mean square distance or maximizing overlapping area.

[0015] Furthermore, the primary positioning module uses low-precision GPS positioning information to match the intersection vector map, determines the intersection the vehicle is about to enter, and calls out the corresponding vector map.

[0016] Furthermore, the secondary positioning module achieves accurate planar positioning of the vehicle by comparing scene features extracted from real-time perception data with features of pre-built maps.

[0017] Furthermore, the primary path planning module connects the current posture of the vehicle with the starting point of the map turning path through a quintic polynomial curve, and selects the final connection path according to the principle of minimum curvature.

[0018] Furthermore, in the first-level path planning module, a path tracking module based on dynamic model feedback is established, and virtual vehicle posture information is generated through trajectory calculation and fed back to the tracking algorithm to achieve closed-loop control.

[0019] Furthermore, the secondary path planning module connects the current position of the vehicle with the predetermined driving lane of the rear lane through a fifth-order polynomial curve to ensure smooth connection of the vehicles.

[0020] Furthermore, in the secondary path planning module, a path tracking module based on perception data feedback is established, and closed-loop control is achieved using real-time perception data to ensure that the vehicle accurately travels along the planned secondary path.

[0021] The beneficial effects of the present invention are:

[0022] (1) The present invention only relies on the existing L2 autonomous driving vehicle's onboard low-precision GPS system, onboard autonomous driving camera, and lightweight intersection pre-built map. No additional sensor equipment is required, which avoids upgrading the vehicle hardware and thus reduces the overall cost.

[0023] (2) Currently, Level 2 autonomous vehicles often automatically exit autonomous driving mode when passing through intersections without lane markings due to a lack of accurate navigation information, severely limiting their application on urban roads. This invention addresses this shortcoming by optimizing path planning and tracking algorithms, enabling Level 2 autonomous vehicles to maintain autonomous driving in complex intersection scenarios.

[0024] (3) This invention innovatively proposes a three-level positioning mechanism. First, a low-precision vehicle-mounted GPS system is used for coarse positioning. When the vehicle approaches an intersection, real-time scene features are matched with a pre-built map to achieve precise positioning. When the vehicle is about to pass the intersection, repositioning is completed again by associating scene features with the map. This mechanism ensures that the vehicle's accurate position can be obtained even with limited computing power.

[0025] (4) The present invention employs a two-level path planning strategy. When a vehicle approaches an intersection, primary path planning is performed based on precise positioning information and a pre-built map. When the vehicle is about to pass through the intersection, secondary path planning is performed based on re-positioning information and a pre-built map. This method ensures timely updates to the path planning.

[0026] Through the above design, the present invention significantly improves the availability and safety of L2 autonomous driving vehicles in complex intersection scenarios, while maintaining the technical advantages of low cost and high efficiency.

[0027] Other advantages, objects, and features of the present invention will be described in part in the following description and, in part, will be apparent to those skilled in the art upon examination of the following description or may be learned from practice of the present invention. The objects and other advantages of the present invention may be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below with reference to the accompanying drawings, in which:

[0029] Figure 1 A framework diagram of an intersection path planning and tracking method based on a three-level positioning and two-level path planning mechanism;

[0030] Figure 2 is the road scene feature data;

[0031] Figure 3 Vector map of the intersection you are about to enter;

[0032] Figure 4 Schematic diagram of the matching between lane lines and road boundaries;

[0033] Figure 5 This is a schematic diagram of the matching between zebra crossings and lane markings;

[0034] Figure 6 This is a schematic diagram of the first-level path planning;

[0035] Figure 7 This is a schematic diagram of the three-level positioning;

[0036] Figure 8 This is a schematic diagram of the secondary planning. DETAILED DESCRIPTION

[0037] The following describes the embodiments of the present invention by means of specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0038] Among them, the accompanying drawings are only for illustrative purposes and represent only schematic diagrams rather than actual pictures, and should not be understood as limiting the present invention. In order to better illustrate the embodiments of the present invention, some parts of the accompanying drawings may be omitted, enlarged or reduced, and do not represent the dimensions of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions may be omitted in the accompanying drawings.

[0039] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "front", "back", etc. indicating directions or positional relationships, they are based on the directions or positional relationships shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operate in a specific direction. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0040] The present invention provides a method for intersection path planning and tracking based on a three-level positioning and two-level path planning mechanism, which specifically includes the following steps:

[0041] 1. Establish a three-level positioning module

[0042] Pre-built maps: First, pre-build a vector map covering the urban intersections within the smart car's driving area. The map information includes key traffic elements such as lane lines, road boundaries, zebra crossings, and lane markings.

[0043] Level 1 positioning module (coarse positioning): obtains coarse positioning information based on the low-precision GPS system equipped in the L2 autonomous driving vehicle, and retrieves the vector map of the corresponding intersection based on the coarse positioning information.

[0044] Secondary positioning module (precise positioning): When a vehicle approaches an intersection, it uses the vehicle's real-time perception data to extract scene features of the current road (such as lane lines, road boundaries, zebra crossings, and lane markings), and matches them with the pre-built map to calculate the vehicle's precise planar positioning at the intersection.

[0045] Level 3 positioning module (repositioning): When the vehicle is about to complete a turn at an intersection, the vehicle's real-time perception data is used again to extract the scene features of the road after the turn, and the features are matched with the pre-built map to complete the vehicle's repositioning.

[0046] Through this three-level positioning mechanism, the precise position information of the vehicle in the intersection can be accurately obtained, providing a basic guarantee for path planning.

[0047] 2. Establish a two-level path planning module

[0048] Level 1 Path Planning: When a vehicle approaches an intersection, it plans a route from the vehicle's current position to the intersection's turning path based on precise positioning information and a pre-built intersection vector map. Dead reckoning is performed based on the vehicle's dynamic model to ensure the vehicle safely passes through the intersection along the planned path.

[0049] The secondary path planning module: When the vehicle is about to complete a turn at an intersection, it plans a connecting path from the vehicle's current position to the lane after the turn, based on relocalization information and a pre-built vector map. By establishing an error model between the vehicle's positioning and the secondary path, the vehicle is guided to continue the autonomous driving operation along the updated path.

[0050] Data Acquisition and Processing Module: This module is responsible for acquiring positioning information and perception data from the vehicle's low-precision GPS system and camera, vectorizing the perception data, and extracting key features of the road scene (such as lane lines, road boundaries, zebra crossings, and lane markings). Data first flows from the Data Acquisition and Processing Module to the primary positioning module, then to the secondary positioning module, the primary path planning module, the tertiary positioning module, and the secondary path planning module, forming a closed-loop data flow to ensure accurate vehicle positioning at intersections and effective path planning.

[0051] like Figure 1 As shown in FIG, a method for intersection path planning and tracking based on a three-level positioning and two-level path planning mechanism is implemented as follows:

[0052] 1. Data acquisition and processing:

[0053] Acquire the positioning information of the low-precision GPS system on the vehicle in real time and retrieve the corresponding intersection vector map (including lane lines, road boundaries, zebra crossings, lane markings and other elements). At the same time, obtain the real-time perception data of the vehicle camera, vectorize the perception data, and extract the key features of the road scene (such as lane lines, road boundaries, zebra crossings and lane markings, see Figure 2 ).

[0054] 2. Primary positioning module:

[0055] By comparing the low-precision GPS positioning information with the intersection vector map, the intersection that the vehicle is about to enter is determined, and the corresponding vector map (such as Figure 3 shown).

[0056] 3. Secondary positioning module:

[0057] Before a vehicle enters an intersection, the scene features of the current road (including lane lines, road boundaries, zebra crossings, lane markings, etc.) are extracted based on real-time perception data and matched with the pre-built map. In this way, accurate planar positioning is achieved (i.e., the fine positioning operation in the secondary positioning module).

[0058] 3.1 Matching of Lane Lines and Road Boundaries

[0059] Lane lines and road boundaries are composed of multiple line segments. The matching process is as follows: Figure 4 shown.

[0060] The lane lines of the vector map can be expressed as:

[0061]

[0062] in, is the set of line segments that make up the lane lines of the vector map, represents the starting point and end point of each line segment, and m represents the total number of line segments.

[0063] The road boundaries of a vector map can be represented as:

[0064]

[0065] in, is the set of line segments that make up the road boundaries of the vector map, represents the starting point and end point of each line segment, and n represents the total number of line segments.

[0066] The lane line of the scene feature can be expressed as:

[0067]

[0068] in, is the set of line segments that make up the scene feature lane lines, represents the starting point and end point of each line segment, and a represents the total number of line segments.

[0069] Discretize the lane lines of the scene features to obtain the discretized lane line set:

[0070]

[0071] in, is the point set that constitutes the scene feature lane line, Represents the coordinates of each point, Indicates the total number of points.

[0072] The road boundary of the scene feature can be expressed as:

[0073]

[0074] in, is the set of line segments that make up the road boundary of the scene feature, represents the starting point and end point of each line segment, and b represents the total number of line segments.

[0075] Discretize the road boundaries of the scene features to obtain the discretized road boundary set:

[0076]

[0077] in, is the point set that constitutes the scene feature road boundary, Represents the coordinates of each point, Indicates the total number of points.

[0078] for Assumptions After the rigid body transformation, the transformed coordinates are

[0079]

[0080] in, Represents the transformed point, (x,y) is the translation vector, and θ is the rotation vector.

[0081] Similarly, for After the rigid body transformation, the transformed coordinates are

[0082] for exist Find The nearest point is for exist Find The nearest point is

[0083] The optimization goal is to minimize the mean square distance between the vector map and the lane line of the scene feature:

[0084]

[0085] 3.2 Matching of Zebra Crossings and Lane Signs

[0086] Each zebra crossing is extracted as a polygon, and each lane sign (left turn, right turn, straight ahead, etc.) is extracted as a polygon. The matching process is as follows: Figure 5 shown.

[0087] The zebra crossing of a vector map can be represented as:

[0088]

[0089] in, A collection of polygons that make up the zebra crossing vector map. represents each polygon, and c represents the total number of polygons.

[0090] The lane markings of the vector map can be expressed as:

[0091]

[0092] in, A collection of polygons that make up the lane markings of a vector map. Represents each polygon, index(map,j) identifies the corresponding lane sign meaning (left turn, right turn, straight ahead, etc.), and d represents the total number of polygons.

[0093] The zebra crossing of the scene feature can be expressed as:

[0094]

[0095] in, is the polygon set that constitutes the scene feature zebra crossing, Represents each polygon, Indicates the total number of polygons.

[0096] The lane marking of the scene feature can be expressed as:

[0097]

[0098] in, is a set of polygons that make up the lane markings of the scene features, Represents each polygon, index(camera,j) represents the meaning of the corresponding lane sign (left = turn left, right = turn right, straight = go straight, etc.), Indicates the total number of polygons.

[0099] for Assumptions express Each vertex of is transformed by the rigid body transformation to obtain the transformed vertex

[0100]

[0101] Among them, (x, y) is the translation vector, θ is the rotation vector. Note that the vertex after transformation is The polygons composed of

[0102] Similarly, for After the rigid body transformation, the transformed polygon is Where index(camera,j) remains unchanged.

[0103] for exist Find The polygon with the closest center distance is denoted as for exist Search for the lane with the same meaning as index(camera,j) and is The polygon closest to the center

[0104]

[0105] The optimization goal is to maximize the overlap between the zebra crossing and lane markings in the vector map and scene features. That is, the negative overlap area is minimized:

[0106]

[0107] 3.3 Planar positioning of vehicles

[0108] The vehicle plane positioning optimization goal is established as:

[0109] minE(x,y,θ)=E1+E2

[0110] 4. Level 1 path planning:

[0111] Before a vehicle enters an intersection, it is necessary to plan the vehicle's path based on the current precise positioning information, such as Figure 6 The current vehicle posture information (including position and direction) is obtained through the precise positioning module. Since the actual vehicle posture may differ from the starting point of the turning path in the pre-built map in terms of position and direction, a connection path must be planned connecting the current vehicle posture and the starting point of the turning path in the map.

[0112] The connection path is calculated as follows:

[0113] Discretize the map turning path:

[0114]

[0115] in, is the discrete point set that constitutes the map turning path, represents the coordinates of each point, and m represents the total number of points.

[0116] According to the current posture of the vehicle, calculate the current posture of the vehicle to The fifth-degree polynomial curve of :

[0117]

[0118] in, The path set is formed by connecting all quintic polynomial paths. represents each quintic polynomial curve, and m represents the total number of curves.

[0119] exist Find the path with the minimum curvature, denoted as This path is the final planned connection path. The planned connection path and the map turning path are combined to form a first-level path.

[0120] 5. Path tracking based on dynamic model feedback:

[0121] When a vehicle tracks a primary path, the camera cannot capture information like lane markings at intersections, so the vehicle's true position information cannot be fed back. This makes it difficult for the tracking algorithm to form a closed-loop control. To address this issue, a vehicle dynamics model is established, and virtual vehicle position information is generated through dead reckoning and fed back to the tracking algorithm. This allows the tracking algorithm to close the loop based on the virtual vehicle position information, ensuring the vehicle continues to execute the autonomous driving mission along the planned primary path, despite the lack of real-world perception data.

[0122] 6. Three-level positioning module:

[0123] When the vehicle is about to exit the intersection, the camera can reacquire the road information after turning, such as Figure 7 As shown in Figure 1, 3D images of the vehicle's position and posture can be obtained. However, in path tracking based on dynamic model feedback, the vehicle's positioning is derived from the dynamic model, which inevitably results in a certain degree of error. This requires repositioning using the three-level positioning module.

[0124] The third-level localization module uses the vehicle's real-time perception data to extract scene features of the road after the turn (such as lane lines, road boundaries, zebra crossings, lane markings, etc.) and matches them with a pre-built intersection vector map to accurately calculate the vehicle's planar position after the turn. This relocalization process is essentially the same as the second-level localization module and will not be further described here.

[0125] 7. Secondary path planning:

[0126] When the vehicle is about to exit the intersection, Figure 8 As shown in Figure 1, based on the relocalization information and the pre-built intersection vector map, secondary path planning is performed. This path planning requires a smooth connection between the vehicle's current position and the intended driving lane of the road after the turn.

[0127] The connection path is calculated as follows:

[0128] Based on the vehicle's current precise position, a fifth-order polynomial is used to calculate a path from the vehicle's current position to the desired lane. This path, the result of secondary path planning, ensures the vehicle can smoothly connect to the lane after turning.

[0129] 8. Path tracking based on perception data feedback:

[0130] When the vehicle is tracking a secondary path, the camera can directly capture lane markings and other road markings, providing timely feedback to the tracking algorithm. This allows the tracking algorithm to perform closed-loop control based on real-time perception data, ensuring the vehicle accurately follows the planned secondary path and completing autonomous driving operations.

[0131] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should all be included in the scope of the claims of the present invention.

Claims

1. A method for intersection path planning and tracking based on a three-level positioning and two-level path planning mechanism, characterized by: The method includes: Establish a three-level positioning module, including: Establish a primary positioning module: obtain coarse positioning information based on a low-precision GPS system, and retrieve a vector map of the corresponding intersection based on the coarse positioning information; Establish a secondary positioning module: When a vehicle approaches an intersection, it uses the vehicle's real-time perception data to extract scene features of the current road, matches them with the pre-built map, and calculates the vehicle's precise planar positioning at the intersection; Establish a three-level positioning module: When the vehicle is about to complete the turn at the intersection, the scene features of the road after the turn are extracted based on the vehicle's real-time perception data, and matched with the pre-built map to complete the vehicle repositioning; Establish a two-level path planning module, including: Establish a first-level path planning module: When a vehicle approaches an intersection, it plans a connecting route from the vehicle's current position to the intersection's turning path based on precise positioning information and a pre-built intersection vector map; Establish a secondary path planning module: When the vehicle is about to complete a turn at an intersection, based on the repositioning information and combined with the pre-built vector map, a connecting path from the vehicle's current position to the lane after the turn is planned.

2. The intersection path planning and tracking method based on the three-level positioning and two-level path planning mechanism according to claim 1 is characterized by: The scene features include lane lines, road boundaries, zebra crossings and lane markings, and are matched with the pre-built map by rigid body transformation and minimizing mean square distance or maximizing overlapping area.

3. The intersection path planning and tracking method based on three-level positioning and two-level path planning mechanism according to claim 1 is characterized by: The primary positioning module uses low-precision GPS positioning information to match the intersection vector map, determines the intersection the vehicle is about to enter, and calls out the corresponding vector map.

4. The intersection path planning and tracking method based on three-level positioning and two-level path planning mechanism according to claim 1 is characterized by: The secondary positioning module achieves accurate planar positioning of the vehicle by comparing scene features extracted from real-time perception data with features of pre-built maps.

5. The intersection path planning and tracking method based on three-level positioning and two-level path planning mechanism according to claim 1 is characterized by: The first-level path planning module connects the vehicle's current posture with the starting point of the map steering path through a quintic polynomial curve, and selects the final connection path based on the principle of minimum curvature.

6. The intersection path planning and tracking method based on three-level positioning and two-level path planning mechanism according to claim 5, characterized in that: In the first-level path planning module, a path tracking module based on dynamic model feedback is established, and virtual vehicle posture information is generated through trajectory calculation and fed back to the tracking algorithm to achieve closed-loop control.

7. The intersection path planning and tracking method based on three-level positioning and two-level path planning mechanism according to claim 1, characterized in that: The secondary path planning module connects the vehicle's current position and the predetermined driving lane of the rear lane through a fifth-order polynomial curve to ensure smooth connection of the vehicles.

8. The intersection path planning and tracking method based on three-level positioning and two-level path planning mechanism according to claim 7, characterized in that: In the secondary path planning module, a path tracking module based on perception data feedback is established, and closed-loop control is achieved using real-time perception data to ensure that the vehicle accurately travels along the planned secondary path.

Images