Radar origin information correction method in intersection traffic scene based on high-precision map

Abstract

The present invention discloses a method for correcting radar origin information in an intersection traffic scene based on a high-precision map, comprising the following steps: obtaining radar origin information and information of the intersection where the radar origin information is located; screening stop points and establishing a stop point set S; determining an import stop line according to the stop point set S; extracting trajectory data entering the intersection based on the import stop line; performing clustering extraction according to the trajectory data to find the center line of the innermost lane; performing radar coordinate conversion; performing radar heading angle adjustment; performing radar origin position adjustment; and evaluating the accuracy of the correction result after iterative optimization. The present invention automatically repairs the radar origin information in the intersection traffic scene, so that not only the trajectory of the target vehicle at the intersection matches the high-precision map positioning with high precision, but also a basis is provided for vehicle parallel simulation and digital roads.

Description

Technical Field

[0001] The present invention relates to the application field of radar in traffic scenes, and in particular to a method for correcting radar origin information in intersection traffic scenes based on high-precision maps. Background Art

[0002] Nowadays, many cities have installed millimeter-wave radar terminal equipment at intersections, so that they can obtain the real-time vehicle driving trajectory at the intersection. Some scholars have studied the conversion relationship between the radar coordinate system and the geodetic coordinate system, and calculated the conversion error. Some scholars have also studied the radar coordinate system of the road section scene, predicting the deviation between the next position and the actual position based on the current coordinate position of the target vehicle, and then calibrating the origin information of the radar equipment. However, the correction of the radar origin information in the intersection traffic scene based on the high-precision map has not given a specific solution. Accurately, stably and automatically repairing the radar origin information in the intersection traffic scene not only makes the trajectory of the target vehicle at the intersection match the high-precision map positioning with high precision, but also provides a basis for vehicle parallel simulation and digital roads.

[0003] Therefore, this paper uses the trajectory data of the radar coordinate system to identify key points, and compares them with the corresponding key points in the high-precision map of the geodetic coordinate system to obtain the moving position and rotation angle of the radar origin, thereby improving the accuracy and stability of radar origin position identification. Summary of the invention

[0004] The present invention mainly solves the problem of poor accuracy in identifying the radar origin position at an intersection in the existing technology; provides a radar origin information correction method in an intersection traffic scene based on a high-precision map to obtain accurate radar origin information.

[0005] The above technical problem of the present invention is mainly solved by the following technical solution: A method for correcting radar origin information in a traffic scene at an intersection based on a high-precision map comprises the following steps:

[0006] Get radar origin information and intersection information;

[0007] Screen the stop points and establish a stop point set S; determine the import stop line according to the stop point set S;

[0008] Extracting trajectory data entering the intersection based on the import stop line;

[0009] Cluster extraction is performed based on trajectory data to find the center line of the innermost lane;

[0010] Perform radar coordinate conversion;

[0011] Adjust the radar heading angle;

[0012] Adjust the radar origin position;

[0013] After iterative optimization, the accuracy of the correction results is evaluated.

[0014] Preferably, the method for screening the stopping points is: obtaining the trajectory point data detected by the radar, recorded as the data set Q, screening the sample data of two consecutive trajectory points of the same target ID in Q that have not changed, considering the sample as the stopping point, and recording the screened stopping point set as S.

[0015] As a preferred method, the method for determining the import stop line is: starting from the trajectory point with the smallest X coordinate in the stop point set S, one by one determines whether the number of stop points within its two-meter range is greater than num, until a stop point that meets the conditions is found, and then another non-repeated stop point is found within the different Y coordinate ranges of this stop point according to the above rules. If there is no point that meets the conditions in the stop point set S, the amount of data in Q is increased to continue searching until two stop points with the same X coordinate that meet the conditions are found, and a stop line is generated based on the two stop points, and its X coordinate is the position of the stop line, which is recorded as X stop .

[0016] As a preferred method for extracting the trajectory data of entering the intersection, the following method is used: select from Q the trajectory data whose X coordinate is greater than X stop The direction of the trajectory is distinguished according to the change trend of the trajectory X coordinate. If the X coordinate is sorted in ascending order according to time, it is a sample of the trajectory of the exit intersection; if the X coordinate is sorted in descending order according to time, it is a sample of the trajectory of the entrance intersection. The trajectory data of the entrance intersection is screened out and recorded as R.

[0017] As a preferred method, the center line extraction method of the innermost lane is: extract the first 1 / 4 sample data with the smallest Y coordinate from R, divide it into a region Z every M meters according to the X coordinate, cluster the data set of each region, and obtain the cluster center of each region after clustering iteration convergence. For each region Z, if the number of clusters is 1, the center point of the data cluster represents the center point of the innermost lane in the region; if the number of clusters is greater than 1, the point with the smallest Y coordinate value among the cluster centers represents the center point of the innermost lane in the region; the set of the center points of the innermost lanes in each region is recorded as {(cx1,cy1),(cx2,cy2)...(cx n ,cy n )}, a line is formed between two adjacent points, and a total of n-1 center lines are combined, which is the line set L.

[0018] Preferably, the method for radar coordinate conversion is: assuming that the initial longitude and latitude of the radar is (lon, lat) and the heading angle is α; extracting the two endpoints P1 and P2 of the center line of the innermost lane of the import area and the intersection point O (lon1, lat1) of the center line of the innermost lane and the stop line on the high-precision map, and converting the longitude and latitude coordinates of P1, P2, and O into coordinates in the radar coordinate system according to the method of converting the geodetic coordinate system to the radar coordinate system.

[0019] As a preferred method for adjusting the radar heading angle, the angle η between the endpoints N1, N2 and P1, P2 of each line in L is calculated by the cosine theorem, and the average of n-1 angles can be obtained as Update the radar's heading angle: As a preferred method for adjusting the radar origin position, the intersection point O (lon) of the road centerline and the lane stop line is extracted through a high-precision map. O ,lat O ), calculate the intersection of the stop line and the center line of the inner lane in the radar coordinate system, denoted as O′(x O′ ,y O′ ), where x O′ =x stop , According to the method of converting the radar coordinate system to the geodetic coordinate system, the intersection point O′(x O′ ,y O′ ) is converted into longitude and latitude coordinates, denoted as O′(lon O′ ,lat O′ ), update the latitude and longitude of the radar origin:

[0020] lon=lon+(lon O′ -lon O )

[0021] lat=lat+(lat O′ -lat O ).

[0022] Preferably, the accuracy evaluation method of the correction result is: select the vehicle trajectory recognized by multiple radars at the same time, calculate the distance error recognized by different radars at the same time, if the distance error is less than one vehicle body distance, it means that the corrected radar information is accurate, otherwise it is inaccurate.

[0023] The beneficial effect of the present invention is that by automatically repairing the radar origin information in the intersection traffic scene, not only the trajectory of the target vehicle at the intersection is matched with the high-precision map positioning with high precision, but also a basis is provided for vehicle parallel simulation and digitized roads. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1It is a flow chart of the radar origin information correction method of the present invention.

[0025] Figure 2 It is a schematic diagram of the illumination of the radar of the present invention at an intersection.

[0026] Figure 3 This is an example diagram of target import data distribution of the present invention.

[0027] Figure 4 It is the trajectory of the radar origin before correction of the present invention.

[0028] Figure 5 It is the radar origin corrected trajectory of the present invention. DETAILED DESCRIPTION

[0029] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the technical solution in the embodiments of the present invention is further described in detail through the following embodiments and in combination with the accompanying drawings. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the invention.

[0030] Embodiment: A method for correcting radar origin information in a traffic scene at an intersection based on a high-precision map, such as Figure 1 As shown, the following steps are included:

[0031] S1: Get the radar origin information and the intersection information; take the intersection where a radar is located in City A as an example, the radar irradiation range and the radar coordinate system establishment rules are as follows Figure 2 As shown; obtain the radar target trajectory point data within 70 meters around the intersection where the radar is located for 10 minutes. The sample information is shown in Table 1. The distribution of the target import stop line, the divided area lane, the lane center point and the data set Q in the coordinate system is shown in Figure 3 shown.

[0032] S2: Filter the stopping points and establish the stopping point set S; filter the sample data of two consecutive trajectory points of the same target ID in Q, regard the sample as the stopping point, and record the filtered stopping point set as S. Clean the target samples with fewer than 5 trajectory points in S; the distribution of the cleaning stopping point set S is as follows Figure 3 As shown in bold.

[0033] S3: Determine the import stop line according to the stop point set S; select the trajectory point X with the smallest X coordinate from the stop point set S. min (71.35,0.57) starts to determine whether there are multiple other target trajectory samples within the 2-meter range. If there are more than the number of different target trajectories num (num is 5), then the trajectory point X minAs the import stop line point, otherwise, select points greater than 71.35 from the stop point set S in turn to start judging until the import stop line point is found. In order to determine the stop line, it is necessary to find two stop points of the stop line. In the range of [71.35-3.75,71.35+3.75], follow the above rules to find the stop point that matches X min Another non-repeating stop point X′ min (70.85,3.68); If the X coordinate of the import stop line cannot be determined, it is necessary to obtain 10 minutes of radar target trajectory point data, repeat the steps of screening the point information of the suspected intersection parking target trajectory and determining the import stop line until two stop points X that meet the conditions are found. min and X′ min . Generate a stop line based on two stop points, X stop =71.1, such as Figure 3 Marked as stop line.

[0034] Table 1 Sample information of trajectory point data

[0035]

[0036] S4: Extract the trajectory data of the intersection based on the import stop line; select the trajectory data with X coordinate greater than X from Q stop The direction of the trajectory is distinguished according to the change trend of the trajectory X coordinate. If the X coordinate is sorted in ascending order according to time, it is a sample of the trajectory of the exit intersection; if the X coordinate is sorted in descending order according to time, it is a sample of the trajectory of the entrance intersection. The trajectory data of the entrance intersection is screened out and recorded as R.

[0037] S5: Clustering extraction is performed based on trajectory data to find the center line of the innermost lane. The method for extracting the center line of the innermost lane is as follows: extract the first 1 / 4 sample data with the smallest Y coordinate from R, divide it into a region Z every M meters according to the X coordinate, cluster the data set of each region, and obtain the cluster center of each region after clustering iteration convergence. For each region Z, if the number of clusters is 1, the center point of the data cluster represents the center point of the innermost lane in the region; if the number of clusters is greater than 1, the point with the smallest Y coordinate value among the cluster centers represents the center point of the innermost lane in the region; the set of the center points of the innermost lanes in each region is recorded as {(cx1,cy1),(cx2,cy2)...(cx n ,cy n )}, a line is formed between two adjacent points, and a total of n-1 center lines are combined, which is the line set L.

[0038] S6: Perform radar coordinate conversion; the initial origin M of the radar is known to have a latitude and longitude of (a, b) and a heading angle of 83.5076; extract the two endpoints P1 and P2 of the center line of the innermost lane of the import area and the intersection point O (lon O ,lat O ), convert the latitude and longitude coordinates of P1, P2, and O into the coordinates of the radar coordinate system according to the method of converting the geodetic coordinate system to the radar coordinate system; take the origin M as the reference point, the heading angles from point M to points P1, P2, and O are 79.61, 81.34, and 80.27 respectively, and the coordinates of the origin M are (0,0). Substituting them into the formula, the radar coordinates of points P1, P2, and O are (56.62,10.14), (60.55,8.16), and (71.78,5.85) respectively.

[0039] Converting from earth coordinate system to radar coordinate system: Taking point O as an example, the conversion method of points P1 and P2 is the same as that of point O. If the longitude and latitude coordinates of the current reference point M are (lon, lat), the radar heading angle is α, the longitude and latitude coordinates of point O are (lon1, lat1), and the radius of the earth is R, the calculation method of the radar coordinates (x1, y1) of point O in the radar coordinate system is as follows:

[0040] x1=d·cos(β)

[0041] y1=d·sin(β)

[0042] Where d is the distance between MO, β is the angle between point O and the radar X-axis, and θ is the heading angle of point O. The calculation method is as follows:

[0043]

[0044] β=α-θ

[0045]

[0046] The calculation method of longitude difference and latitude difference is as follows:

[0047] Δlon=lon1-lon

[0048] Δlat=lat1-lat.

[0049] S7: Adjust the radar heading angle; calculate the angle η between each line in L and line P1P2 by the cosine theorem, and the average of n-1 angles can be obtained as Update the radar's heading angle: α = 83.5076 + 6.37 = 89.8776.

[0050] S8: Adjust the radar origin position. The method for adjusting the radar origin position is: extract the intersection point O (lon) of the road center line and the lane stop line through the high-precision map O ,lat O ), calculate the intersection of the stop line and the center line of the inner lane in the radar coordinate system, denoted as O′(x O′ ,y O′ ), where x O′ =x stop , According to the method of converting the radar coordinate system to the geodetic coordinate system, the intersection point O′(x O′ ,y O′ ) is converted into longitude and latitude coordinates, denoted as O′(lon O′ ,lat O′ ), update the latitude and longitude of the radar origin:

[0051] lon=lon+(lon O′ -lon O )

[0052] lat=lat+(lat O′ -lat O ).

[0053] Radar coordinate system to geodetic coordinate system: Take point O′(x O′ ,y O′ ) as an example, if the radar coordinates of the current reference point N are (x, y), the longitude and latitude coordinates of the geodetic coordinate system are (lon, lat), the radar heading angle is α, and the radar coordinates of point O′ are (x O′ ,y O′ ), the radius of the earth is R, the longitude and latitude coordinates of point O' in the geodetic coordinate system (lon O′ ,lat O′ ) is calculated as follows:

[0054]

[0055]

[0056] Where θ is the heading angle of point O′, d is the distance between NO′, and the calculation method is as follows:

[0057] θ=α-arctan(y,x)

[0058]

[0059] S9: After iterative optimization, the accuracy of the correction result is evaluated; S7 and S8 are repeated for iterative optimization, where lon min ,lat min ,θmin Take 0.0001, 0.0001, 1 respectively until the optimized value meets lon O′ -lon O <lon min and lat O′ -lat O <lat min and where lon min ,lat min ,θ min They are the minimum threshold of longitude change, the minimum threshold of latitude change, and the minimum threshold of angle change; the iteration ends; the matching effect of the radar trajectory on the high-precision map before and after the radar origin correction is as follows Figure 4 , 5 shown.

[0060] The accuracy evaluation method of the correction result is as follows: select the vehicle tracks recognized by multiple radars at the same time, calculate the distance errors recognized by different radars at the same time, if the distance error is less than one vehicle body distance, it means that the corrected radar information is accurate, otherwise it is inaccurate, collect 1000 identical moving vehicle targets recognized by four radars in the intersection, calculate the position deviation of their track points, and the evaluation results are shown in Table 2.

[0061] Table 2 Evaluation results

[0062] Evaluation indicators Index value The position deviation is less than 2 meters. 86％ The position deviation is less than 5 meters. 90％

[0063] It can be seen from Table 2 that the radar origin corrected by the radar origin information correction method of the present invention is accurate; the present invention automatically repairs the radar origin information in the intersection traffic scene, which not only makes the trajectory of the target vehicle at the intersection match the high-precision map positioning with high precision, but also provides a basis for vehicle parallel simulation and digitized roads.

[0064] The above-described embodiment is only a preferred solution of the present invention and does not limit the present invention in any form. There are other variations and modifications without exceeding the technical solution described in the claims.

Claims

1. A radar origin information correction method in a traffic scene at an intersection based on a high-precision map, characterized in that: The following steps are involved: Get radar origin information and intersection information; Screen the stopping points and establish the stopping point set S; Determine the import stop line according to the stop point set S; Extracting trajectory data entering the intersection based on the import stop line; Cluster extraction is performed based on trajectory data to find the center line of the innermost lane; Perform radar coordinate conversion, assuming that the initial latitude and longitude of the radar is (lon, lat), and the heading angle is α; extract the two endpoints P1 and P2 of the center line of the innermost lane of the import area on the high-precision map, and the intersection point O (lon O ,lat O ), convert the longitude and latitude coordinates of P1, P2, and O into coordinates in the radar coordinate system; Adjust the radar heading angle, calculate the angle η between each centerline endpoint N1, N2 and P1, P2, and get the average of n-1 angles: , update the radar heading angle α = α + ; Adjust the radar origin position and extract the intersection point O(lon) between the center line of the innermost lane and the lane stop line through the high-precision map. O ,lat O ), calculate the intersection point O′(x O′ ,y O′ ), convert the radar coordinates of the intersection point O′ into the longitude and latitude coordinates O′ (lon O′ ,lat O′ ), update the latitude and longitude of the radar origin lon = lon + (lon O′ -lon O ), lat=lat+(lat O′ -lat O ); After iterative optimization, the accuracy of the correction results is evaluated.

2. The radar origin information correction method in the intersection traffic scene based on high-precision map according to claim 1 is characterized in that: The method for screening the stop points is: obtain the trajectory point data detected by the radar, recorded as the data set Q, screen the sample data of two consecutive trajectory points of the same target ID in Q that have not changed, regard the sample as the stop point, and record the screened stop point set as S.

3. The radar origin information correction method in the intersection traffic scene based on high-precision map according to claim 2 is characterized in that: The method for determining the import stop line is as follows: starting from the trajectory point with the smallest X coordinate in the stop point set S, determine one by one whether the number of stop points within its two-meter range is greater than num, until a stop point that meets the conditions is found, and then find another non-repeated stop point within the different Y coordinate ranges of this stop point according to the above rules. If there is no point that meets the conditions in the stop point set S, increase the amount of data in Q and continue to search until two stop points with the same X coordinate that meet the conditions are found, and generate a stop line based on the two stop points. Its X coordinate is the position of the stop line, recorded as x stop .

4. The radar origin information correction method in the intersection traffic scene based on high-precision map according to claim 3 is characterized in that: The method for extracting the trajectory data entering the intersection is to filter out the X coordinates greater than x from Q. stop The data is used to distinguish the direction of the trajectory according to the change trend of the trajectory X coordinate. If the X coordinate increases in time order, it is a sample of the exit trajectory. If the X coordinate is in descending order according to time, it is a trajectory sample entering the intersection. The trajectory data entering the intersection is filtered out and recorded as R.

5. The radar origin information correction method in the intersection traffic scene based on high-precision map according to claim 4 is characterized in that: The method for extracting the center line of the innermost lane is as follows: extract the first 1 / 4 sample data with the smallest Y coordinate from R, divide it into a region Z every M meters according to the X coordinate, cluster the data set of each region, and obtain the cluster center of each region after clustering iteration convergence. For each region Z, if the number of clusters is 1, the center point of the data cluster represents the center point of the innermost lane in the region; if the number of clusters is greater than 1, the point with the smallest Y coordinate value among the cluster centers represents the center point of the innermost lane in the region; the set of the center points of the innermost lanes in each region is recorded as {(cx1,cy1),(cx2,cy2)...(cx n ,cy n )}, a line is formed between two adjacent points, and a total of n-1 center lines are combined, which is the line set L.

6. The method for correcting radar origin information in a high-precision map-based intersection traffic scene according to claim 5 is characterized in that: The method of radar coordinate conversion is as follows: assuming the initial latitude and longitude of the radar as (lon, lat) and the heading angle as α; extracting the two endpoints P1 and P2 of the center line of the innermost lane of the import area and the intersection point O (lon) of the center line of the innermost lane and the stop line on the high-precision map. O ,lat O ), convert the latitude and longitude coordinates of P1, P2, and O into coordinates in the radar coordinate system according to the method of converting the geodetic coordinate system to the radar coordinate system.

7. The method for correcting radar origin information in a high-precision map-based intersection traffic scene according to claim 6 is characterized in that: The radar heading angle adjustment method is: calculate the angle η between the endpoints N1, N2 and P1, P2 of each line in L by the cosine theorem, and the average of n-1 angles can be obtained as Update the radar's heading angle:

8. The method for correcting radar origin information in a high-precision map-based intersection traffic scene according to claim 7 is characterized in that: The method for adjusting the radar origin position is to extract the intersection point O(lon) of the center line of the innermost lane of the road and the lane stop line through the high-precision map. O ,lat O ), calculate the intersection of the stop line and the center line of the inner lane in the radar coordinate system, denoted as O′(x O′ ,y O′ ),in According to the method of converting the radar coordinate system to the geodetic coordinate system, the intersection point O′(x O′ ,y O′ ) is converted into longitude and latitude coordinates, denoted as O′(lon O′ ,lat O′ ), update the latitude and longitude of the radar origin: lon=lon+(lon O′ -lon O ) years=years+(years O′ -years O )。 9. The radar origin information correction method in a high-precision map-based intersection traffic scene according to any one of claims 1 to 8, characterized in that: The accuracy evaluation method of the correction result is as follows: select the vehicle tracks recognized by multiple radars at the same time, calculate the distance errors recognized by different radars at the same time, and if the distance error is less than one vehicle body distance, it means that the corrected radar information is accurate, otherwise it is inaccurate.

Images