Curve recognition method based on electronic map

Abstract

The invention relates to the technical field of curve recognition, in particular to a curve recognition method based on an electronic map. The method comprises the following steps: firstly, constructing a map interface through an electronic map API (Application Program Interface), planning a driving path by using the API, determining the driving path from a starting point to a terminal point, and extracting longitude and latitude data of all path points on the driving path; performing duplicate removal processing on the longitude and latitude data of the approach points, converting a longitude and latitude coordinate system into a WGS84 coordinate system according to the longitude and latitude data after duplicate removal processing, and calculating the distance between two adjacent points in the approach points; and finally, judging whether the three approach points are collinear or not by adopting the geometric property of vector cross multiplication so as to judge whether a curve exists in front or not, and calculating the radius of the curve according to the screened curve approach points. According to the method provided by the invention, accurate recognition of the curve is realized without equipment such as a camera, and the problems that the recognition precision of the existing curve recognition scheme is limited by weather conditions and illumination changes and the environmental adaptability is poor are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of curve recognition, and in particular to a curve recognition method based on an electronic map. Background Art

[0002] One of the places where traffic accidents frequently occur is curves. If a car passes a curve at too fast a speed, it may deviate from the lane, run off the road, or skid or roll over. Therefore, accurate identification of curves is a basic prerequisite for ensuring car driving safety and thus reducing traffic accidents.

[0003] Most existing methods for identifying curves rely on cameras and other equipment to collect road images and identify lane lines in the images. This method is easily affected by external factors, such as weather conditions (rain, snow, fog, etc.) and lighting changes (inside and outside tunnels). In addition, due to the diversity of road environments, models trained in the same area or road conditions may not be applicable to other areas, requiring a large amount of data and complex model training to improve generalization capabilities. Existing solutions not only have low recognition rates in extreme weather, but also have high costs.

[0004] In "A road curve recognition method and device thereof" (CN109886131A), the invention obtains a road image ahead during vehicle driving; grayscales the road image; uses the variance of the grayscale values ​​of pixels between two adjacent rows to extract the pixel coordinates of the vanishing point; and according to the set range, saves the pixel points with the largest grayscale value in each row of the lane lines on both sides, and thereby fits the lane line equations of the lane lines on both sides; uses the coordinates of the single pixel point with the largest grayscale value of the lane lines on both sides on the same row to calculate the pixel coordinates of the center point of each row, and fits the center line equation; calculates the coordinates of the intersection of the center line equation calculated at the previous moment with the lane lines on both sides at the current moment; determines the size of the vertical coordinate of the intersection and the vertical coordinate of the vanishing point, and then determines whether there is a curve ahead.

[0005] In "A curve recognition method and system based on multi-scale calculation" (CN114092910A), the invention projects road vector line data into an image based on multiple scales; identifies a curve target frame from the image based on a deep learning method; for the road vector line data, calculates the curvature based on a multi-scale plus sliding window method, and identifies the curve interval based on the calculated curvature; performs fusion processing on the detected curve target frame and the identified curve interval to obtain the identified final curve interval.

[0006] In "A method and terminal device for automatically identifying highway curves" (CN115761676A), by acquiring road information in a preset area, the turning radius of each sampling point on the driving route of the sampled vehicle is calculated based on the road information; based on the turning radius of each sampling point, the first target curve in the preset area is determined.

[0007] In summary, the existing technical solutions mainly adopt image recognition, and the recognition accuracy is limited by weather conditions, lighting changes and poor environmental adaptability, making it difficult to accurately identify curves, thereby reducing the safety of vehicle driving. Summary of the invention

[0008] In view of the above problems, the purpose of the present invention is to provide a curve recognition method based on an electronic map. The curve recognition is performed through the electronic map, and the recognition accuracy is not affected by external factors such as weather conditions and light changes. It has strong environmental adaptability and avoids regional restrictions, making it easier to implement. The technical solution is as follows:

[0009] A method for identifying a curve based on an electronic map comprises the following steps:

[0010] Step 1: Construction of electronic map:

[0011] Build a map interface through the electronic map API, use the API to plan the driving route, determine the driving route between the starting point and the end point, and extract the longitude and latitude data of all the waypoints on the driving route;

[0012] Step 2: Distance calculation based on electronic map data:

[0013] First, the longitude and latitude data of the waypoints are deduplicated, and then the longitude and latitude coordinate system is converted to the WGS84 coordinate system based on the deduplicated longitude and latitude data, and finally the distance between two adjacent points in the waypoints is calculated;

[0014] Step 3: Curve screening and radius calculation:

[0015] The geometric properties of vector cross product are used to determine whether the three approach points are collinear, so as to judge whether there is a curve ahead, and then the curve radius is calculated based on the selected curve approach points.

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

[0017] The curve recognition method based on the electronic map of the present invention does not need to use the aid of devices such as cameras to achieve accurate recognition of curves, and solves the problem that the recognition accuracy of existing curve recognition solutions is limited by weather conditions, lighting changes and poor environmental adaptability.

[0018] The technical solution provided by the present invention can assist vehicle motion control by accurately identifying curves, thereby improving vehicle driving safety, and can be used to optimize the overall performance of the vehicle based on curve identification. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 The figure is a flow chart of the curve recognition method based on electronic map of the present invention.

[0020] Figure 2 De-duplicate the flow chart for waypoints.

[0021] Figure 3 Flowchart for calculating the distance between two points.

[0022] Figure 4 Filter flow chart for curve waypoints.

[0023] Figure 5 Schematic diagram of the implementation method. DETAILED DESCRIPTION

[0024] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0025] The technical solution of the present invention for identifying curves based on electronic maps is as follows: Figure 1 As shown, it is divided into three stages: electronic map construction, distance calculation based on electronic map data, and curve screening and radius calculation. Electronic map construction stage: build a map interface through the electronic map API (Application Programming Interface), use the API to plan the driving route and extract the longitude and latitude data of all waypoints on the route. Distance calculation stage based on electronic map data: remove duplicate waypoints; convert the coordinate system to the WGS84 coordinate system; calculate the distance between two adjacent points in the waypoints. Curve screening and radius calculation stage: use the geometric properties of vector cross product to determine whether the three waypoints are collinear, to determine whether there is a curve ahead, and then use the screened curve waypoints to calculate the curve radius.

[0026] The waypoints refer to all the longitude and latitude coordinate points (Lon, Lat) in the path planning.

[0027] 1. The specific technical solution of the present invention comprises the following steps:

[0028] Construction of S1 electronic map.

[0029] Waypoint data acquisition:

[0030] Use the path planning algorithm in the electronic map API to determine the driving route between the starting point and the end point; use the annotation class function in the electronic map API to extract the longitude and latitude data of all the waypoints on the path and mark them in the map instance.

[0031] In this embodiment, the latitude and longitude coordinates of the starting point and the end point are input, the starting point is input (103.847035, 30.715316), and the end point is input (103.82579, 30.71483). The service class interface function "BMap.DrivingRoute" in Baidu API is called to plan the route and obtain the latitude and longitude data of the waypoints; then the "BMap.Marker" interface function is called to mark these waypoints on the map.

[0032] S2 is a distance calculation based on electronic map data.

[0033] S21 route point removal.

[0034] Create a two-dimensional input array and result array: the input array contains the latitude and longitude data of all the waypoints in the path planning; the result array is used to store the deduplicated waypoint data.

[0035] The process of deduplication of waypoints is as follows Figure 2 As shown. First, initialize c=1, s=t, n=0, c is the deduplication process loop variable, s is the total number of deduplication loops, t is the length of the input array, and n is the length of the result array; when entering the loop, first determine whether the loop variable c exceeds the length of the input array; if it exceeds, end the loop; if it does not exceed, determine whether the current waypoint is repeated, if at least one waypoint that repeats the current waypoint is found after the current waypoint, set the loop variable c=c+1, and again determine whether the current loop variable c exceeds the length of the input array, if it does not exceed, continue to check whether the next waypoint is repeated, if it exceeds, end the loop; if no waypoint that repeats the current waypoint is found after the current waypoint, add the current waypoint to the result array, set the length of the result array n=n+1, set the loop variable c=c+1, and then enter the next loop.

[0036] In this embodiment, the input array has 24 path points, and the result array after deduplication has 23 path points. Figure 2 The flowchart shown in the figure is removed to obtain the longitude and latitude data of the waypoints shown in Table 1 below.

[0037] Table 1 Latitude and longitude data of waypoints

[0038]

[0039]

[0040] S22: Conversion of waypoint coordinates.

[0041] Traverse all the waypoints in the result array, convert them from the BD-09 coordinate system (Baidu coordinate system) to the GCJ-02 coordinate system (the coordinate system of the geographic information system formulated by the China National Administration of Surveying, Mapping and Geoinformation), and then convert them from the GCJ-02 coordinate system to the WGS84 (World Geodetic System 1984, the coordinate system under the Global Positioning System) coordinate system.

[0042] S221 converts the waypoint coordinates from the BD-09 coordinate system to the GCJ-02 coordinate system.

[0043] Adjust the longitude x and latitude y of the waypoint

[0044]

[0045] Where: Lon BD-09 , Lat BD-09 are the longitude and latitude of the waypoint in the BD-09 coordinate system. 0.0065 and 0.006 are the offset constants between the BD-09 coordinate system and the GCJ-02 coordinate system.

[0046] Calculate the coordinates of the waypoints in the GCJ-02 coordinate system:

[0047]

[0048] Where: Lon GCJ-02 , Lat GCJ-02 are the longitude and latitude of the waypoint in the GCJ-02 coordinate system. 0.00002 and 0.00003 are the offset coefficients; 3000 and 180 are also the offset constants of the GCJ-02 coordinate system;

[0049] Using equations (1) and (2), we can obtain the GCJ-02 coordinate data shown in Table 2.

[0050] Table 2 Latitude and longitude in GCJ-02 coordinate system

[0051]

[0052] S222 converts the waypoint coordinates from the GCJ-02 coordinate system to the WGS84 coordinate system.

[0053] Calculate the offset of the longitude and latitude of the waypoint:

[0054]

[0055] Where: ret1 and ret2 are the offsets of the longitude and latitude of the waypoint in the GCJ-02 coordinate system respectively.

[0056] Calculate the coordinates of the waypoints in the WGS84 coordinate system:

[0057]

[0058] Where: Lon WGS84 , Lat WGS84 are the longitude and latitude of the waypoint in the WGS84 coordinate system, a is the major axis of the earth (6378137m), and 0.00669342162296594323 is the square of the eccentricity.

[0059] Using equations (3) and (4), we can obtain the WGS84 coordinate data shown in Table 3.

[0060] Table 3 Latitude and longitude in WGS84 coordinate system

[0061]

[0062]

[0063] S23 point distance calculation.

[0064] Input the result array after coordinate transformation, traverse all the path points in the result array, and calculate the distance s between the kth point and the k+1th point in the result array in turn k,k+1 (k=1,…,n-1).

[0065] s k,k+1 The calculation steps are as follows:

[0066] S231 radians conversion:

[0067] Enter two points (Lon WGS84,k ,Lat WGS84,k )、(Lon WGS84,k+1 ,Lat WGS84,k+1 ), and convert the two points to radians ([λ k ,φ k ]、[λ k+1 ,φ k+1 ]). The conversion formula is:

[0068]

[0069] Where: k ,φ k are the longitude Lon of the kth waypoint WGS84,k , Latitude WGS84,k The corresponding radian value.

[0070] S232 initializes the ellipsoid parameters:

[0071] Initialize the length of the earth's major semi-axis (a), minor semi-axis (b), and flattening (f). The specific values ​​are: a = 6378137m, b = 6356752m, f = 1 / 298.257223563.

[0072] S233 calculates the naturalized latitude:

[0073]

[0074] Where: U k , U k+1 are the naturalized latitudes of the two points respectively.

[0075] S234 calculates the distance between the kth point and the k+1th point:

[0076] S2341 distance calculation parameter initialization:

[0077] Set the longitude difference L = λ k+1 -λ k , the initial setting is λ=L, k=1, and the number of iterations is limited to 100.

[0078] S2342 iteratively calculates λ' and determines Δλ:

[0079] ①When each iteration is completed, determine whether the number of iterations has been exceeded. If it has been exceeded, it means that convergence has not occurred, and NaN (Not a Number, indicating an ambiguous numerical result) is returned.

[0080] ② If the number of iterations is not exceeded, determine whether the accuracy requirement Δλ=|λ'-λ|>10 is met -12 , if not satisfied, assign λ' to λ, calculate the new λ' value, and then repeat steps ①②.

[0081] If the accuracy requirement is met, jump to S2343 to calculate the distance s between the two points. k,k+1 .

[0082] λ'=L+(1-C)·f·sinα·{σ+C·sinσ[cos2σ m +C·cosσ·(-1+2·cos 2 2σ m )]}(7)

[0083] In the formula, sinσ, cosσ, σ, sinα, cos 2 α,cos2σ m , C is the auxiliary variable to be calculated in the iteration, and the formula is as follows.

[0084]

[0085] cosσ=sinUk ·sinU k+1 +cosU k ·cosU k+1 ·cosλ(9)

[0086]

[0087] cos 2 α=1-sin 2 α(12)

[0088]

[0089] S2343 calculate distance s k,k+1 :

[0090] s k,k+1 = b·A·(σ-Δσ)(15)

[0091]

[0092] Where: u 2 , A, B, Δσ are auxiliary parameters. Substituting the calculation results of equations (16) to (19) into equation (15), the distance s between the kth point and the k+1th point can be calculated. k,k+1 .

[0093] Use formula (15) to calculate the distance s between two adjacent points in the waypoints k,k+1 The distance calculation process between two points is as follows: Figure 3 The distance calculation results between two adjacent points of all waypoints are shown in Table 4.

[0094] Table 4 Distance between two adjacent points on the planned path

[0095]

[0096]

[0097] S3 curve screening and radius calculation.

[0098] S31 bend screening.

[0099] S311 obtains waypoint array P = [p(1), ..., p(m), ..., p(n)];

[0100] Among them, p(m) represents the latitude and longitude data of the mth waypoint, and n is the length of the result array.

[0101] S312 initialization of curve screening parameters;

[0102] Set the initial variables i=1 and j=1, and set the tolerance tol=1×10 -8.

[0103] S313 curve waypoint screening;

[0104] Starting from the first waypoint in the obtained waypoint array P, perform collinearity determination with the second and third waypoints. If they are collinear, start the collinearity determination of the second waypoint with the third and fourth waypoints, and repeat this cycle until they are not collinear. Save a j =i,a j is the index value of the waypoint when entering the curve in the waypoint array P. After that, continue to judge whether the next waypoint is collinear with the next two waypoints. If they are not collinear, continue to judge until they are collinear, and save b j =i+1,b j It is the index value of the waypoint in the waypoint array P when exiting the curve.

[0105] The collinearity judgment first constructs two vectors u and v through the three selected path points:

[0106]

[0107] Wherein, Δx1 is the difference between the latitude of the second waypoint and the latitude of the first waypoint among the three waypoints, Δx2 is the difference between the latitude of the third waypoint and the latitude of the second waypoint among the three waypoints, Δy1 is the difference between the longitude of the second waypoint and the longitude of the first waypoint among the three waypoints, and Δy2 is the difference between the longitude of the third waypoint and the longitude of the second waypoint among the three waypoints.

[0108] Then calculate the cross product of the two vectors:

[0109] q=u×v (21)

[0110] Finally, determine whether the absolute value of the cross product |q| is within the tolerance tol. If |q|≤tol, it means that the three points are collinear.

[0111] S314 Curve waypoints saved

[0112] The selected j Waypoint to b j The longitude and latitude data of the jth waypoints are the jth group of curve waypoints, and the process ends when the third to last waypoint is determined.

[0113] The process of selecting curve waypoints is as follows: Figure 4 As shown, the curve path points are screened out as shown in Table 5.

[0114] Table 5 Curve pass points

[0115]

[0116] S32 curve radius calculation

[0117] For each group of selected curve path points, the curve radius is calculated respectively. The steps for calculating the curve radius corresponding to each group of curve path points are as follows.

[0118] S321 Coordinate Conversion

[0119] For each group of curve waypoints screened out, the average longitude and the average latitude are calculated, and a new coordinate system with the mean as the origin is determined. The new coordinates of the waypoints are the offsets relative to the mean, and then the original coordinates are converted to this coordinate system.

[0120]

[0121] Where: p(w) lon and p(w) lat are the longitude and latitude of a set of curve path points, w is the number of curve path points, avg x and avg y are the average longitude and latitude of this set of curve pass points, respectively, x w and w The new coordinates after the offset calculation.

[0122] S322 Radius Calculation

[0123] Written in matrix form:

[0124]

[0125] Right now:

[0126] D·X=E (25)

[0127] Where: (x w ,y w ) is the coordinate of the curve path point, (M, N) is the coordinate of the circle center, and r is the radius of the circle; D, X and E correspond to the matrices of the corresponding positions in equation (24) respectively.

[0128] Find Then, the center (M, N) and radius r are solved.

[0129] Using formulas (24) and (25), it can be calculated that the curve radius value in this embodiment is 1307.6199383278413 m.

[0130] 2. Verification of curve recognition method

[0131] In order to verify the accuracy of the curve radius calculation results, the 91 Satellite Map Assistant was used to verify the calculated results. The fitting radius value of 91 Satellite Map Assistant is 1310.38m. The curve radius calculated based on the electronic map is slightly different from the curve radius fitted by 91 Satellite Map Assistant, and the relative error is 0.024%.

[0132] Figure 5 An example is given to illustrate how the functions of this invention are realized during the actual driving of a car.

[0133] First, call the map API to plan the path according to the input starting point and end point, obtain the latitude and longitude data of the waypoints and mark them in the map instance. Secondly, use the deduplication algorithm to deduplicate the waypoints. Then, traverse the deduplicated waypoints, convert them from the BD-09 coordinate system to the GCJ-02 coordinate system, and then convert them from the GCJ-02 coordinate system to the WGS84 coordinate system. Use the point distance calculation method based on electronic map data to calculate the distance between waypoints. Finally, use the curve screening algorithm based on collinearity judgment to screen out the curve waypoints and complete the calculation of the curve radius.

[0134] Taking the curve radius calculated online as input, the safe speed is estimated by the curve driving safety speed estimation model, which is used for curve motion control to improve the curve driving safety of the vehicle.

Claims

1. A curve recognition method based on an electronic map, characterized in that: The following steps are involved: Step 1: Construction of electronic map: Build a map interface through the electronic map API, use the API to plan the driving route, determine the driving route between the starting point and the end point, and extract the longitude and latitude data of all the waypoints on the driving route; Step 2: Distance calculation based on electronic map data: First, the longitude and latitude data of the waypoints are deduplicated, and then the longitude and latitude coordinate system is converted to the WGS84 coordinate system based on the deduplicated longitude and latitude data, and finally the distance between two adjacent points in the waypoints is calculated; Step 3: Curve screening and radius calculation: The geometric properties of vector cross product are used to determine whether the three approach points are collinear, so as to judge whether there is a curve ahead, and then the curve radius is calculated based on the selected curve approach points.

2. The method for identifying curves based on electronic maps according to claim 1, characterized in that: Step 2 is as follows: Step 2.1: Deduplication of waypoints: Create a two-dimensional input array and result array: the input array contains the latitude and longitude data of all the waypoints in the path planning; the result array is used to store the waypoint data after deduplication; Initialize c=1, s=t, n=0, c is the deduplication process loop variable, s is the total number of deduplication loops, t is the length of the input array, and n is the length of the result array; When entering the loop, first determine whether the loop variable c exceeds the length of the input array; If it exceeds, the loop ends; if it does not exceed, determine whether the current waypoint is repeated. If at least one waypoint that repeats the current waypoint is found after the current waypoint, set the loop variable c=c+1, and determine again whether the current loop variable c exceeds the length of the input array. If it does not exceed, continue to check whether the next waypoint is repeated. If it exceeds, the loop ends; if no waypoint that repeats the current waypoint is found after the current waypoint, add the current waypoint to the result array, set the length of the result array n=n+1, set the loop variable c=c+1, and then proceed to the next loop; Step 2.2: Waypoint coordinate conversion: Traverse all the waypoints in the result array, convert them from the BD-09 coordinate system to the GCJ-02 coordinate system, and then from the GCJ-02 coordinate system to the WGS84 coordinate system; Step 2.3: Point distance calculation: Input the result array after coordinate transformation, traverse all the path points in the result array, and calculate the distance s between the kth path and the k+1th path point in the result array in turn. k,k+1 , k=1,…,n-1.

3. The method for identifying curves based on electronic maps according to claim 2, characterized in that: Step 2.2 is as follows: Step 2.2.1: Convert the waypoint coordinates from the BD-09 coordinate system to the GCJ-02 coordinate system; Adjust the longitude x and latitude y of the waypoint as follows: Where: Lon BD-09 and Lat BD-09 They are the longitude and latitude of the waypoint in the BD-09 coordinate system; Calculate the coordinates of the waypoints in the GCJ-02 coordinate system: Where: Lon GCJ-02 and Lat GCJ-02 are the longitude and latitude of the waypoint in the GCJ-02 coordinate system; Step 2.2.2: Convert the waypoint coordinates from the GCJ-02 coordinate system to the WGS84 coordinate system; Calculate the offset of the longitude and latitude of the waypoint: Where: ret1 and ret2 are the longitude and latitude offsets of the waypoint in the GCJ-02 coordinate system respectively; Calculate the coordinates of the waypoints in the WGS84 coordinate system: Where: Lon WGS84 and Lat WGS84 are the longitude and latitude of the passing point in the WGS84 coordinate system, and a is the major axis of the earth.

4. The method for identifying curves based on electronic maps according to claim 2, characterized in that: Step 2.3 is as follows: Step 2.3.1: Radians conversion: The kth waypoint (Lon WGS84,k ,Lat WGS84,k ) and the k+1th waypoint (Lon WGS84,k+1 ,Lat WGS84,k+1 ) to radians [λ k ,φ k ] and [λ k+1 ,φ k+1 ]; the conversion formula is: Where: k and φ k are the longitude Lon of the kth waypoint WGS84,k and Latitude Lat WGS84,k The corresponding radian value; Step 2.3.2: Initialize ellipsoid parameters: Initialize the length of the earth's major semi-axis a, minor semi-axis b, and flattening f; the specific values ​​are: a = 6378137m, b = 6356752m, f＝1 / 298.257223563； Step 2.3.3: Calculate the naturalized latitude: Where: U k and U k+1 are the naturalized latitudes of the kth and k+1th waypoints, respectively; Step 2.3.4: Calculate the distance between the kth and k+1th waypoints: Step a: Set the longitude difference between the kth and k+1th waypoints L = λ k+1 -λ k , initially set λ = L, where λ represents the radian difference between the longitudes of the kth and k+1th waypoints, and sets an upper limit on the number of iterations; Step b: Iteratively calculate λ' and determine Δλ: ①When each iteration is completed, determine whether the number of iterations exceeds the upper limit. If it exceeds the upper limit, it means that it has not converged and NaN is returned; ② If the number of iterations does not exceed the upper limit, determine whether the accuracy requirement Δλ=|λ'-λ|>10 is met -12 , if not satisfied, assign λ' to λ, calculate a new λ' value, and then repeat steps ① and ②; where λ' is the radian difference corresponding to the longitudes of adjacent waypoints after iterative calculation; Δλ is the absolute value of the difference between λ' and λ; If the accuracy requirement is met, jump to step c to calculate the distance s between the two points k,k+1 ;in, λ'=L+(1-C)·f·sinα·{σ+C·sinσ[cos2σ m +C·cosσ·(-1+2·cos 2 2p m )]} (7) Where: f is the Earth's flattening; sinσ, cosσ, σ, sinα, cos 2 α,cos2σ m , C is the auxiliary variable to be calculated in the iteration, the formula is as follows: cosσ=sinU k ·sinU k+1 +cosU k ·cosU k+1 ·cosλ (9) cos 2 α=1-sin 2 a (12) Step c: Calculate the distance s k,k+1 : s k,k+1 =b·A·(σ-Δσ) (15) Where: u 2 , A, B and Δσ are auxiliary parameters; Substitute the results of equations (16) to (19) into equation (15) to calculate the distance s between the kth point and the k+1th point k,k+1 .

5. The method for identifying curves based on electronic maps according to claim 1, characterized in that: Step 3 is as follows: Step 3.1: Corner screening: Step 3.1.1: Get the waypoint array P = [p(1),…,p(m),…,p(n)]; Among them, p(m) represents the latitude and longitude data of the mth waypoint, and n is the length of the result array; Step 3.1.2: Initialize the curve screening parameters: Set the initial variables i=1 and j=1, and set the tolerance tol; Step 3.1.3: Curve waypoint screening: Starting from the first waypoint in the obtained waypoint array P, perform collinearity determination with the second and third waypoints. If they are collinear, start the collinearity determination of the second waypoint with the third and fourth waypoints, and repeat this cycle until they are not collinear. Save a j =i,a j The index value of the waypoint when entering the curve in the waypoint array P; After that, continue to judge whether the next waypoint is collinear with the next two waypoints. If they are not collinear, continue to judge until they are collinear, and save b j =i+1,b j is the index value of the waypoint in the waypoint array P when exiting the curve; The collinearity judgment first constructs two vectors u and v through the three selected path points: Wherein, Δx1 is the difference between the latitude of the second waypoint and the latitude of the first waypoint among the three waypoints, Δx2 is the difference between the latitude of the third waypoint and the latitude of the second waypoint among the three waypoints, Δy1 is the difference between the longitude of the second waypoint and the longitude of the first waypoint among the three waypoints, and Δy2 is the difference between the longitude of the third waypoint and the longitude of the second waypoint among the three waypoints; Then calculate the cross product of the two vectors: q=u×v (21) Finally, determine whether the absolute value of the cross product |q| is within the tolerance tol. If |q|≤tol, it means that the three points are collinear, otherwise it means that the three points are not collinear; Step 3.1.4: Saving the curve waypoints: The selected j Waypoint to b j The latitude and longitude data of the jth group of waypoints are the waypoints of the curve, and the process ends when the third to last waypoint is judged. Step 3.2: Calculation of curve radius: For each group of selected curve path points, the curve radius is calculated respectively; the curve radius calculation steps corresponding to each group of curve path points are as follows: Step 3.2.1: Coordinate transformation: For each group of selected curve waypoints, calculate the average longitude and latitude, determine a new coordinate system with the mean as the origin, and the new coordinates of the waypoints are the offsets relative to the mean, and then transform the original coordinates into the new coordinate system: Where: p(w) lon and p(w) lat are the longitude and latitude of a set of curve path points, w is the number of curve path points, avg x and avg y are the average longitude and latitude of this set of curve pass points, x w and w The new coordinates after offset calculation; Step 3.2.2: Radius calculation: The new coordinates after the offset calculation are written in matrix form as follows: Right now: D·X=E (25) Where: (x w ,y w ) is the coordinate of the curve path point, (M, N) is the coordinate of the circle center, and r is the radius of the circle; D, X and E are the matrices of the corresponding positions in equation (24); Find Then, the center (M, N) and radius r are solved.

Images