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

Abstract

The invention relates to an intersection path planning and tracking method based on a three-level positioning and two-level path planning mechanism, and belongs to the technical field of automatic driving. According to the method, no extra sensor needs to be added, only depending on a vehicle-mounted low-precision GPS system, a vehicle-mounted automatic driving camera and a lightweight intersection pre-built map of an existing L2-level automatic driving vehicle, accurate positioning of the vehicle at the intersection is achieved through a three-level positioning module, and a two-level path planning strategy is designed; and the steering operation can be safely and efficiently finished when the vehicle passes through the intersection. According to the method, the accuracy of vehicle positioning can still be guaranteed under the condition that the computing power is limited, timely updating of path planning is achieved, the availability and safety of the L2-level automatic driving vehicle in a complex intersection scene are remarkably improved, and meanwhile the technical advantages of low cost and high efficiency are kept.

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 lines are often missing in scenes such as intersections, which reduces the integrity of data obtained by the perception system and affects its reliability. In this case, the risk of failure of core algorithms such as trajectory prediction and decision planning increases significantly, directly affecting the normal operation of the autonomous driving system. Existing L2 autonomous driving functions (such as lane centering control systems LCC) usually automatically exit autonomous driving mode when encountering missing lane lines, requiring the driver to take over the vehicle immediately. This not only limits the adaptability of the assisted driving system, but also reduces the user's driving experience and satisfaction.

[0003] In order to improve the performance of the autonomous driving system in the scenario of missing lane lines, pre-built maps, as a powerful auxiliary tool for the perception system, can provide key prior information of the traffic environment, such as lane lines, zebra crossings and other important road signs. This prior information can provide support for trajectory prediction and decision planning, make up for the lack of perception data, reduce the risk of algorithm failure, and ensure the continuous and effective operation of L2 autonomous driving 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 L2 autonomous driving vehicles in the scenario of missing lane lines. 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 the existing intersection automatic driving method, the present invention does not require additional sensors and does not increase the cost of the vehicle. It can effectively solve the problem of the existing L2 level automatic driving vehicle automatically exiting when turning at an intersection without lane lines. Even in the case of limited computing power, 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, use the vehicle's real-time perception data to extract the scene features of the current road, match it with the pre-built map, and calculate the precise planar positioning of the vehicle at the intersection;

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

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

[0012] Establish a primary path planning module: When the vehicle approaches an intersection, it plans a connection 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, it is combined with the pre-built vector map to plan a connecting path from the vehicle's current position to the lane after turning.

[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 by rigid body transformation and minimizing mean square distance or maximizing overlapping area.

[0015] Furthermore, the primary positioning module uses the low-precision GPS positioning information to match the intersection vector map, determines the intersection that 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 a pre-built map.

[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 primary path planning module, a path tracking module based on dynamic model feedback is established, and virtual vehicle posture information is generated through track calculation and fed back to the tracking algorithm to achieve closed-loop control.

[0019] Furthermore, the secondary path planning module connects the current posture of the vehicle with the predetermined driving lane of the rear lane after turning through a quintic 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 implemented 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 reduces the overall cost.

[0023] (2) At present, L2 autonomous driving vehicles often automatically exit the autonomous driving mode when passing through an intersection without lane lines due to the lack of accurate navigation information, which seriously limits its application scope on urban roads. The present invention makes up for this shortcoming by optimizing the path planning and tracking algorithm, so that L2 autonomous driving vehicles can continue to maintain the autonomous driving state in complex intersection scenarios.

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

[0025] (4) The present invention designs a two-level path planning strategy. When the vehicle approaches an intersection, the first-level path planning is performed based on the precise positioning information and the pre-built map; when the vehicle is about to pass the intersection, the second-level path planning is performed based on the re-positioning information and the pre-built map. This method ensures timely update of 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, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can 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 in conjunction with the accompanying drawings, wherein:

[0029] Figure 1 A framework diagram of a path planning and tracking method for an intersection 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 This is a schematic diagram of the matching of lane lines and road boundaries;

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

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

[0035] Figure 7 It is a schematic diagram of 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 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 only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

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

[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 the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position 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 carried by the L2 autonomous driving car, 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, the vehicle's real-time perception data is used to extract scene features of the current road (such as lane lines, road boundaries, zebra crossings, and lane markings, etc.), and matched with the pre-built map to calculate the precise planar positioning of the vehicle at the intersection.

[0045] Level 3 positioning module (repositioning): When the vehicle is about to complete a turn at an 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's repositioning.

[0046] Through this three-level positioning mechanism, the precise position information of the vehicle at 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 module: When a vehicle approaches an intersection, it plans a connection route from the vehicle's current position to the intersection's turning path based on precise positioning information and pre-built intersection vector maps. It performs dead reckoning based on the vehicle's dynamic model to ensure that the vehicle safely passes through the intersection along the planned path.

[0049] Secondary path planning module: When the vehicle is about to complete the intersection turn, based on the repositioning information, it will again combine the pre-built vector map to plan the connection path from the vehicle's current position to the lane after turning. By establishing an error model between the vehicle positioning and the secondary path, the vehicle is guided to continue to complete the autonomous driving operation according to the updated path.

[0050] Data acquisition and processing module: responsible for obtaining positioning information and perception data from the vehicle's low-precision GPS system and vehicle camera, and vectorizing the perception data to extract key features of the road scene (such as lane lines, road boundaries, zebra crossings, and lane markings, etc.). Data first flows from the data acquisition and processing module to the primary positioning module, and then flows to the secondary positioning module, the primary path planning module, the tertiary positioning module, and the secondary path planning module in turn, forming a closed-loop data flow to ensure the accuracy of vehicle positioning at intersections and the effectiveness of path planning.

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

[0052] 1. Data acquisition and processing:

[0053] Obtain 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 the vehicle enters the 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 (i.e., the precise positioning operation in the secondary positioning module) is achieved.

[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] The lane lines of the scene features are discretized to obtain a set of discretized lane lines:

[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 a 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 target is the road boundary of the vector map and the scene feature, and the mean square distance between the vector map and the lane line of the scene feature is minimized:

[0084]

[0085] 3.2 Matching of zebra crossings and lane markings

[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 the vector map can be expressed as:

[0088]

[0089] in, A collection of polygons that make up the zebra crossing of a 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 rigid body transformation to obtain the transformed vertex as

[0100]

[0101] Among them, (x, y) is the translation vector, and θ is the rotation vector. The polygons are

[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 by for exist Search for the lane with the same meaning as index(camera,j) and is close to The polygon with the closest center

[0104]

[0105] The optimization goal is to maximize the overlap area between the zebra crossing of the vector map and the lane marking of the scene feature. 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. First-level 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 position information (including position and direction angle) of the vehicle is obtained through the precise positioning module. Since the actual position of the current vehicle may have position differences and direction angle differences with the starting point of the turning path in the pre-built map, a connection path connecting the current vehicle position and the starting point of the turning path in the map must be planned.

[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, the current posture of the vehicle is calculated to The fifth-degree polynomial curve of :

[0117]

[0118] in, The path set is formed for all quintic polynomial connection 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 connection path at the final planning location. 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 the vehicle is tracking the primary path, the camera cannot obtain information such as lane lines at the intersection, and the vehicle's real posture information cannot be fed back, making it difficult for the tracking algorithm to form a closed-loop control. To solve this problem, a vehicle dynamics model is established, and virtual vehicle posture information is generated through dead reckoning and fed back to the tracking algorithm. In this way, although there is a lack of real perception data, the tracking algorithm can still form a closed loop based on the virtual vehicle posture information to ensure that the vehicle continues to perform the autonomous driving task along the planned primary path.

[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. Therefore, the real vehicle posture feedback data can be obtained at this time. However, since the positioning of the vehicle is calculated by the dynamic model in the path tracking based on the dynamic model feedback, there must be a certain error. At this time, it is necessary to reposition it through the three-level positioning module.

[0124] The third-level positioning module extracts the scene features of the road after the turn (such as lane lines, road boundaries, zebra crossings, lane markings, etc.) based on the vehicle's real-time perception data, and matches it with the pre-built intersection vector map to accurately calculate the vehicle's planar position after the turn. The relocalization process is basically the same as the second-level positioning module, so it will not be repeated here.

[0125] 7. Secondary path planning:

[0126] When a vehicle is about to exit an intersection, Figure 8 As shown, based on the relocation 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 predetermined driving lane of the road after the turn.

[0127] The connection path is calculated as follows:

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

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

[0130] When the vehicle is tracking the secondary path, the camera can directly obtain road sign information such as lane lines, and the vehicle's real posture data can be fed back to the tracking algorithm in a timely manner. In this way, the tracking algorithm can perform closed-loop control based on real-time perception data to ensure that the vehicle accurately drives along the planned secondary path and completes the autonomous driving operation.

[0131] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. 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 solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should 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, use the vehicle's real-time perception data to extract the scene features of the current road, match it with the pre-built map, and calculate the precise planar positioning of the vehicle at the intersection; Establish a three-level positioning module: When the vehicle is about to complete the turn at the intersection, based on the vehicle's real-time perception data, extract the scene features of the road after the turn, and match them with the pre-built map to complete the vehicle's relocation; Establish a two-level path planning module, including: Establish a primary path planning module: When a vehicle approaches an intersection, a connection route is planned 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, it is combined with the pre-built vector map to plan a connecting path from the vehicle's current position to the lane after turning.

2. The intersection path planning and tracking method based on three-level positioning and two-level path planning mechanism according to claim 1 is characterized in that: The scene features include lane lines, road boundaries, zebra crossings and lane markings. The scene features 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 in that: 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 method for intersection path planning and tracking based on three-level positioning and two-level path planning mechanism according to claim 1, characterized in that: 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 in that: The primary path planning module connects the current position 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.

6. The method for intersection path planning and tracking based on three-level positioning and two-level path planning mechanism according to claim 5, characterized in that: In the primary 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 method for intersection path planning and tracking 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 current position of the vehicle with the predetermined driving lane of the rear lane through a quintic 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 is characterized by: In the secondary path planning module, a path tracking module based on perception data feedback is established, and closed-loop control is implemented using real-time perception data to ensure that the vehicle accurately travels along the planned secondary path.

Images