Lane curvature radius calculation method and system based on high-precision map

Abstract

The invention relates to the field of automatic driving, and discloses a lane curvature radius calculation method and system based on a high-precision map, and the method comprises the steps: obtaining a plurality of shape point coordinates of a lane edge line of a lane where a vehicle is located in real time through a high-precision map sensor; determining projection coordinates of the vehicle projected on the lane edge line based on the plurality of shape point coordinates and the position coordinates of the vehicle; calculating a plurality of shape point projection distances based on the projection coordinates and the plurality of shape point coordinates; constructing a lane curve model based on the plurality of shape point projection distances and the plurality of shape point coordinates, and carrying out shape point sampling based on the lane curve model to obtain a plurality of sampling coordinates; and calculating the lane curvature radius of the lane where the vehicle is located based on the plurality of sampling coordinates. Therefore, the curvature radius of the lane is accurately calculated through the lane curve model constructed based on the lane side line shape point data provided by the high-precision map sensor, the curvature radius parameter of the road in front of the vehicle is foreseeably sensed, and behavior planning in automatic driving is facilitated.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a method and system for calculating the radius of curvature of a lane based on a high-precision map. Background Art

[0002] In some scenarios of autonomous driving (such as driving on a curve), it is necessary to obtain the radius of curvature of the road. In the prior art, the data source for the radius of curvature of the road is mainly high-precision maps. For example, the vehicle yaw angle and lateral acceleration are obtained through sensors to estimate the radius of curvature of the current vehicle position. However, this solution lacks the estimation of the road curvature in the direction of the vehicle's travel, and there is uncertainty in the road curvature when the vehicle changes lanes and avoids the vehicle, so the error will be relatively large. For another example, the current curvature parameter is corrected according to the two or more trajectory shapes of the target road section, but this solution is a method for self-correction of the existing curvature radius on the map, and it is impossible to correct and process the parameters of the curvature radius from the source. It can be seen that the calculation accuracy of the curvature radius of the lane in the prior art needs to be improved. Therefore, how to accurately calculate the lane curvature radius and predictively perceive the curvature radius parameters of the road in front of the vehicle to assist in behavior planning in autonomous driving has become a problem that needs to be solved urgently.

[0003] The above contents are only used to assist in understanding the technical solution of the present invention and do not constitute an admission that the above contents are prior art. Summary of the invention

[0004] The main purpose of the present invention is to provide a lane curvature radius calculation method and system based on a high-precision map, aiming to solve the technical problem that the existing technology cannot accurately calculate the lane curvature radius and realize predictive perception of the curvature radius parameters of the road ahead of the vehicle to assist in behavior planning in autonomous driving.

[0005] To achieve the above object, the present invention provides a method for calculating lane curvature radius based on a high-precision map, the method comprising the following steps:

[0006] The coordinates of multiple points on the lane edge line of the lane where the vehicle is located are obtained in real time through a high-precision map sensor;

[0007] Determine the projection coordinates of the vehicle projected on the lane edge line based on the plurality of shape point coordinates and the position coordinates of the vehicle;

[0008] Calculate multiple shape point projection distances based on the projection coordinates and multiple shape point coordinates;

[0009] Constructing a lane curve model based on multiple shape point projection distances and multiple shape point coordinates, and performing shape point sampling based on the lane curve model to obtain a plurality of sampling coordinates;

[0010] The lane curvature radius of the lane where the vehicle is located is calculated based on a number of sampling coordinates.

[0011] Optionally, the step of acquiring in real time through a high-precision map sensor the coordinates of multiple shape points of a lane edge line of a lane where the vehicle is located comprises:

[0012] The coordinates of multiple points on the lane edge line of the lane where the vehicle is located are obtained in real time through a high-precision map sensor;

[0013] Calculate the angle between each shape point coordinate and the vector formed by the coordinates of the adjacent front and back shape points;

[0014] The multiple shape point coordinates are filtered according to whether the angle is greater than a preset angle to obtain the filtered multiple shape point coordinates.

[0015] Optionally, the step of calculating the projection distances of multiple shape points based on the projection coordinates and the coordinates of multiple shape points includes:

[0016] Taking the projection coordinates as the coordinate origin, the vehicle's forward direction as the positive direction, and the direction of the area the vehicle has traveled as the reverse direction, the distances from multiple shape point coordinates to the coordinate origin are calculated to obtain multiple shape point projection distances.

[0017] Optionally, the step of constructing a lane curve model based on a plurality of shape point projection distances and a plurality of shape point coordinates, and performing shape point sampling based on the lane curve model to obtain a plurality of sampling coordinates includes:

[0018] Constructing a lane curve model based on a plurality of shape point projection distances and a plurality of shape point coordinates, wherein the lane curve model includes a lane curve model on the left side of the lane where the vehicle is located and a lane curve model on the right side of the lane where the vehicle is located;

[0019] The lane curve model on the left side of the lane where the vehicle is located is:

[0020]

[0021]

[0022] The lane curve model on the right side of the lane where the vehicle is located is:

[0023]

[0024]

[0025] In the formula, (X l ,Y l ) is the coordinate of the point on the lane edge line on the left side of the lane where the vehicle is located, S l is the projection distance of the point on the left side of the lane where the vehicle is located, (Xr ,Y r ) is the coordinate of the point on the lane edge line on the right side of the lane where the vehicle is located, S r is the projection distance of the shape point on the right side of the lane where the vehicle is located, and D0, D1, D2, D3, D4, D5, D′0, D′1, D′2, D′3, D′4, and D′5 are lane model parameters solved based on the relationship between multiple shape point coordinates and multiple shape point projection distances;

[0026] The step of sampling shape points based on the lane curve model to obtain a plurality of sampling coordinates includes:

[0027] Select several shape points with preset sampling intervals and project distance S l , and project each point S l Substituting the lane curve model on the left side of the lane where the vehicle is located into the lane curve model to perform shape point sampling to obtain sampling coordinates of the lane edge lines on the left side of the lane where the vehicle is located;

[0028] Select several shape point projection distances S at the preset sampling interval r , and project each point S r Substitute the curve into the lane curve model on the right side of the lane where the vehicle is located to perform shape point sampling to obtain sampling coordinates of several lane edge lines on the right side of the lane where the vehicle is located.

[0029] Optionally, the step of calculating the lane curvature radius of the lane where the vehicle is located based on a plurality of sampling coordinates includes:

[0030] Based on a number of sampling coordinates, the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located are respectively calculated by the following formula, and the lane curvature radius of the lane where the vehicle is located is calculated based on the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located;

[0031]

[0032]

[0033]

[0034]

[0035]

[0036] In the formula, R l is the lane curvature radius of the left lane where the vehicle is located, (X0, Y0), (X1, Y1), (X2, Y2) are the adjacent sampling coordinates of the lane edge line on the left side of the lane where the vehicle is located, In the triangle formed by (X0, Y0), (X1, Y1), (X2, Y2), A is the angle between the side formed by (X1, Y1), (X0, Y0) and the side formed by (X1, Y1), (X2, Y2). The length of the corresponding side, R r is the lane curvature radius of the right lane where the vehicle is located, (X0′, Y0′), (X1′, Y1′), (X2′, Y2′) are the adjacent sampling coordinates of the lane edge line on the right side of the lane where the vehicle is located, In the triangle formed by (X0′, Y0′), (X1′, Y1′), (X2′, Y2′), A′ is the angle between the side formed by (X1′, Y1′), (X0′, Y0′) and the side formed by (X1′, Y1′), (X2′, Y2′). The length of the corresponding side, R is the lane curvature radius of the lane where the vehicle is located.

[0037] In addition, to achieve the above purpose, the present invention also proposes a lane curvature radius calculation system based on a high-precision map, and the lane curvature radius calculation system based on a high-precision map includes:

[0038] A shape point coordinate acquisition module is used to acquire multiple shape point coordinates of the lane edge line of the lane where the vehicle is located in real time through a high-precision map sensor;

[0039] A projection coordinate acquisition module, used to determine the projection coordinates of the vehicle projected on the lane edge line based on the coordinates of the multiple shape points and the position coordinates of the vehicle;

[0040] A shape point distance calculation module, used for calculating a plurality of shape point projection distances based on the projection coordinates and a plurality of shape point coordinates;

[0041] A lane model building module, used to build a lane curve model based on multiple shape point projection distances and multiple shape point coordinates, and perform shape point sampling based on the lane curve model to obtain a number of sampling coordinates;

[0042] The curvature radius calculation module is used to calculate the lane curvature radius of the lane where the vehicle is located based on a number of sampling coordinates.

[0043] Optionally, the shape point coordinate acquisition module is further used to acquire multiple shape point coordinates of the lane edge line of the lane where the vehicle is located in real time through a high-precision map sensor;

[0044] The shape point coordinate acquisition module is also used to calculate the angle between each shape point coordinate and the vector formed by the adjacent front and rear shape point coordinates;

[0045] The shape point coordinate acquisition module is also used to filter the multiple shape point coordinates according to whether the angle is greater than a preset angle to obtain the filtered multiple shape point coordinates.

[0046] Optionally, the shape point distance calculation module is also used to calculate the distances from multiple shape point coordinates to the coordinate origin with the projection coordinates as the coordinate origin, the vehicle's forward direction as the positive direction, and the direction of the area through which the vehicle has traveled as the reverse direction to obtain multiple shape point projection distances.

[0047] Optionally, the lane model building module is further used to build a lane curve model based on multiple shape point projection distances and multiple shape point coordinates, and the lane curve model includes a lane curve model on the left side of the lane where the vehicle is located and a lane curve model on the right side of the lane where the vehicle is located;

[0048] The lane curve model on the left side of the lane where the vehicle is located is:

[0049]

[0050]

[0051] The lane curve model on the right side of the lane where the vehicle is located is:

[0052]

[0053]

[0054] In the formula, (X l ,Y l ) is the coordinate of the point on the lane edge line on the left side of the lane where the vehicle is located, S l is the projection distance of the point on the left side of the lane where the vehicle is located, (X r ,Y r ) is the coordinate of the point on the lane edge line on the right side of the lane where the vehicle is located, S r is the projection distance of the shape point on the right side of the lane where the vehicle is located, and D0, D1, D2, D3, D4, D5, D′0, D′1, D′2, D′3, D′4, and D′5 are lane model parameters solved based on the relationship between multiple shape point coordinates and multiple shape point projection distances;

[0055] The lane model building module is also used to select a number of shape point projection distances S at a preset sampling interval. l , and project each point S l Substituting the lane curve model on the left side of the lane where the vehicle is located into the lane curve model to perform shape point sampling to obtain sampling coordinates of the lane edge lines on the left side of the lane where the vehicle is located;

[0056] The lane model building module is also used to select a number of shape point projection distances S at the preset sampling interval. r , and project each point S r Substitute the curve into the lane curve model on the right side of the lane where the vehicle is located to perform shape point sampling to obtain sampling coordinates of several lane edge lines on the right side of the lane where the vehicle is located.

[0057] Optionally, the curvature radius calculation module is further used to calculate the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located respectively based on a number of sampling coordinates by the following formula, and calculate the lane curvature radius of the lane where the vehicle is located based on the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located;

[0058]

[0059]

[0060]

[0061]

[0062]

[0063] In the formula, R l is the lane curvature radius of the left lane where the vehicle is located, (X0, Y0), (X1, Y1), (X2, Y2) are the adjacent sampling coordinates of the lane edge line on the left side of the lane where the vehicle is located, In the triangle formed by (X0, Y0), (X1, Y1), (X2, Y2), A is the angle between the side formed by (X1, Y1), (X0, Y0) and the side formed by (X1, Y1), (X2, Y2). The length of the corresponding side, R r is the lane curvature radius of the right lane where the vehicle is located, (X0′, Y0′), (X1′, Y1′), (X2′, Y2′) are the adjacent sampling coordinates of the lane edge line on the right side of the lane where the vehicle is located, In the triangle formed by (X0′, Y0′), (X1′, Y1′), (X2′, Y2′), A′ is the angle between the side formed by (X1′, Y1′), (X0′, Y0′) and the side formed by (X1′, Y1′), (X2′, Y2′). The length of the corresponding side, R is the lane curvature radius of the lane where the vehicle is located.

[0064] In the present invention, a plurality of shape point coordinates of the lane edge line of the lane where the vehicle is located are obtained in real time through a high-precision map sensor; the projection coordinates of the vehicle projected on the lane edge line are determined based on the plurality of shape point coordinates and the position coordinates of the vehicle; the projection distances of the plurality of shape points are calculated based on the projection coordinates and the plurality of shape point coordinates; a lane curve model is constructed based on the plurality of shape point projection distances and the plurality of shape point coordinates, and shape point sampling is performed based on the lane curve model to obtain a plurality of sampling coordinates; the lane curvature radius of the lane where the vehicle is located is calculated based on the plurality of sampling coordinates. Thus, by constructing a lane curve model based on the shape point data of the lane edge line provided by the high-precision map sensor, and then accurately calculating the lane curvature radius based on the lane curve model, the curvature radius parameters of the road ahead of the vehicle can be predictively perceived, so as to facilitate behavior planning in automatic driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 It is a flowchart of a first embodiment of a method for calculating lane curvature radius based on a high-precision map according to the present invention;

[0066] Figure 2 A schematic diagram of the angle between each shape point coordinate and the vectors formed by the coordinates of the adjacent front and rear shape points involved in the first embodiment of the lane curvature radius calculation method based on a high-precision map of the present invention;

[0067] Figure 3 A schematic diagram of projection coordinates of a vehicle projected onto a lane edge line according to a first embodiment of a method for calculating a lane curvature radius based on a high-precision map of the present invention;

[0068] Figure 4 A schematic diagram of the projection distance of a shape point involved in the first embodiment of the method for calculating the lane curvature radius based on a high-precision map of the present invention;

[0069] Figure 5 Schematic diagram of shape point sampling involved in the first embodiment of the lane curvature radius calculation method based on a high-precision map of the present invention;

[0070] Figure 6 Schematic diagram of sampling coordinate relationship involved in the first embodiment of the lane curvature radius calculation method based on high-precision map of the present invention;

[0071] Figure 7 This is a structural block diagram of the first embodiment of the lane curvature radius calculation system based on a high-precision map of the present invention.

[0072] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0073] 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 present invention.

[0074] The embodiment of the present invention provides a method for calculating the lane curvature radius based on a high-precision map. Figure 1 , Figure 1 This is a flow chart of the first embodiment of the lane curvature radius calculation method based on a high-precision map of the present invention.

[0075] In this embodiment, the lane curvature radius calculation method based on the high-precision map includes the following steps:

[0076] Step S10: acquiring coordinates of multiple points on the lane edge line of the lane where the vehicle is located in real time through a high-precision map sensor;

[0077] In this embodiment, the high-precision map sensor can be used to obtain multiple shape point coordinates of the lane edge line of the lane where the vehicle is located in real time. The high-precision map sensor can be understood as the sensor used for high-precision map (HDMap) collection, such as the Global Navigation Satellite System (GNSS) receiver, inertial measurement unit (IMU), laser radar and other sensors. The shape point coordinates can be understood as the coordinates corresponding to the shape point data (i.e., the point data used to mark the shape and position of the lane edge line provided by the high-precision map). In a specific implementation, ∑(X l ,Y l ) represents the coordinates of the point on the lane edge line on the left side of the lane where the vehicle is located, ∑(X r ,Y r ) represents the coordinates of the point on the lane edge line on the right side of the lane where the vehicle is located, (P x ,P y ) represents the position coordinates of the vehicle.

[0078] It should be noted that, in theory, the farther the lane edge linear point data is, the better. However, due to the overall performance of the high-precision map sensor, the farther the lane linear point data is obtained, the more hardware performance is required. Therefore, in order to balance performance and accuracy, this embodiment takes the linear point data 200 meters in front of the vehicle and 20 meters behind the vehicle for processing.

[0079] Furthermore, in order to reduce the situation where the road curvature radius result is abnormal due to abnormal high-precision map point data, so as to accurately reflect the curvature radius state of the vehicle's driving road, the angle between each shape point coordinate and the vector formed by the corresponding front and rear shape point coordinates can be calculated, and then the multiple shape point coordinates are filtered according to whether the angle is greater than the preset angle to obtain multiple filtered shape point coordinates. In a specific implementation, shape point data greater than the preset angle can be filtered out to ensure the smoothness of the lane line shape point data. Among them, the preset angle can be set according to actual needs, such as 10°, and this embodiment does not limit this.

[0080] See also Figure 2 , Figure 2 Schematic diagram of the angle between each shape point coordinate and the vector formed by the adjacent front and rear shape point coordinates involved in the first embodiment of the lane curvature radius calculation method based on high-precision map of the present invention. Figure 2 In the filtering process of the obtained shape point data, the shape point data can be traversed, and the angle between the two vectors is calculated by the vector formed by the current shape point (X1, Y1) and the adjacent front and back shape points (X0, Y0) and (X2, Y2). When the angle between the vectors When the angle is greater than a preset angle (such as 10°), the shape point data with an angle greater than 10° is filtered out to ensure the smoothness of the lane line shape point data.

[0081] Step S20: determining the projection coordinates of the vehicle projected on the lane edge line based on the coordinates of the plurality of shape points and the position coordinates of the vehicle;

[0082] In this embodiment, the projection coordinates of the vehicle projected on the lane edge line can be determined based on the filtered coordinates of the multiple shape points and the position coordinates of the vehicle. Figure 3 , Figure 3 Schematic diagram of the projection coordinates of the vehicle projected onto the lane edge line involved in the first embodiment of the lane curvature radius calculation method based on a high-precision map of the present invention. Figure 3 In the above example, two continuous shape point coordinates (X1, Y1) and (X2, Y2) can be selected from the filtered multiple shape points for point-to-line projection, so as to obtain the vehicle position coordinates (P x , P y ) Determine the projection coordinates (P') of the vehicle projection between two consecutive point coordinates (X1, Y1) and (X2, Y2) x , P' y ).

[0083] Step S30: Calculating projection distances of multiple shape points based on the projection coordinates and multiple shape point coordinates;

[0084] In this embodiment, multiple shape point projection distances can be calculated based on the projection coordinates and the filtered multiple shape point coordinates. In a specific implementation, the projection coordinates can be used as the coordinate origin, the vehicle forward direction can be used as the positive direction, and the direction of the area where the vehicle has traveled can be used as the reverse direction. The distances from the multiple shape point coordinates to the coordinate origin can be calculated to obtain the multiple shape point projection distances.

[0085] See also Figure 4 , Figure 4 Schematic diagram of the projection distance of the shape point involved in the first embodiment of the lane curvature radius calculation method based on the high-precision map of the present invention. Figure 4 In the projection coordinates (P' x , P' y ) is the coordinate origin, the projection distance of the shape point is recorded as S, the forward direction of the vehicle head is taken as the positive direction, S>0, the direction of the area where the vehicle head has traveled is taken as the reverse direction, S<0, so as to realize the relationship construction of the extracted lane line shape point data. S can be represented by the shape point coordinates, such as (X2, Y2) corresponds to S1, (X3, Y3) corresponds to S1+S2.

[0086] Step S40: constructing a lane curve model based on a plurality of shape point projection distances and a plurality of shape point coordinates, and performing shape point sampling based on the lane curve model to obtain a plurality of sampling coordinates;

[0087] In this embodiment, a lane curve model can be constructed based on the projection distances of multiple shape points and the filtered coordinates of multiple shape points, and the lane curve model includes a lane curve model on the left side of the lane where the vehicle is located and a lane curve model on the right side of the lane where the vehicle is located. Among them, the lane curve model on the left side of the lane where the vehicle is located is:

[0088]

[0089]

[0090] The lane curve model on the right side of the lane where the vehicle is located is:

[0091]

[0092]

[0093] In the formula, (X l , Y l ) is the coordinate of the point on the lane edge line on the left side of the lane where the vehicle is located, S l is the projection distance of the point on the left side of the lane where the vehicle is located, (X r , Y r ) is the coordinate of the point on the lane edge line on the right side of the lane where the vehicle is located, S ris the projection distance of the shape point on the right side of the lane where the vehicle is located, and D0, D1, D2, D3, D4, D5, D′0, D′1, D′2, D′3, D′4, and D′5 are all lane model parameters solved based on the relationship between multiple shape point coordinates and multiple shape point projection distances.

[0094] It is easy to understand that according to the acquired shape point coordinates and the corresponding shape point projection distance, the left lane edge line can be obtained as ∑(S l , X l ) and ∑(S l , Y l ), a fifth-order polynomial is used to construct the above mapping relationship. Similarly, for the right lane edge line, ∑(S r , X r ) and ∑(S r , Y r ), a fifth-order polynomial is used to construct the above mapping relationship according to the corresponding relationship.

[0095] In a specific implementation, when solving the above quintic polynomial, the obtained filtered multiple groups ∑(S l , X l ) and ∑(S l , Y l ) into the quintic polynomial and solve it using the least squares method to obtain two sets of parameters F→(S1, X1) corresponding to ∑D i ,F→(S l , Y l ) corresponds to ∑D′ i , that is, solve for D0, D1, D2, D3, D4, D5, D′0, D′1, D′2, D′3, D′4, D′5.

[0096] Furthermore, when performing shape point sampling based on the lane curve model, a plurality of shape point projection distances S can be selected at a preset sampling interval. l , and project each point S l Substitute the shape point sampling into the lane curve model on the left side of the lane where the vehicle is located to obtain the sampling coordinates of the lane edge line on the left side of the lane where the vehicle is located; select a number of shape point projection distances S at the preset sampling interval r , and project each point S r Substitute the lane curve model on the right side of the lane where the vehicle is located into the shape point sampling to obtain the sampling coordinates of the lane edge lines on the right side of the lane where the vehicle is located. The preset sampling interval can be set according to actual needs, and this embodiment does not limit this. In a specific implementation, when selecting the shape point projection distance S l and the projection distance S of the shape pointr When, it can be selected according to actual needs, for example, it can be selected based on the distance between the position to be measured and the coordinate origin, and this embodiment does not limit this.

[0097] For example, see Figure 5 , Figure 5 This is a schematic diagram of shape point sampling involved in the first embodiment of the lane curvature radius calculation method based on a high-precision map of the present invention. Figure 5 After the lane curve fitting is performed, the shape point projection distance parameter S for shape point sampling on the lane curve can be set. p As an independent variable parameter (S p It can be set according to actual needs. For example, if you need to know the curvature radius of the lane 200 meters ahead on the map, S p The sampling interval can be 0.1 m, so as to obtain the projection distances of three points for point sampling (S p -0.1), (S p ), (S p +0.1)), and substitute it into the quintic polynomial corresponding to the lane curve model (i.e., substitute the independent variable in the quintic polynomial), where S in the lane curve model on the left side of the lane where the vehicle is located is substituted. l When , three sampling coordinates ((X0, Y0), (X1, Y1), (X2, Y2)) can be obtained. Similarly, when the lane curve model on the right side of the lane where the vehicle is located is substituted into S r When , three sampling coordinates ((X0′, Y0′), (X1′, Y1′), (X2′, Y2′)) can be obtained accordingly.

[0098] Step S50: Calculate the lane curvature radius of the lane where the vehicle is located based on a number of sampling coordinates.

[0099] Furthermore, based on the three acquired sampling coordinates, namely ((X0, Y0), (X1, Y1), (X2, Y2)), the lane curvature radius represented by these three sampling coordinates can be calculated according to the sine theorem. Thus, S is obtained based on the shape point data of the high-precision map. p The corresponding lane curvature radius. Figure 6 , Figure 6 Schematic diagram of the sampling coordinate relationship involved in the first embodiment of the lane curvature radius calculation method based on a high-precision map of the present invention. Figure 6 In the example, a triangle is constructed based on ((X0, Y0), (X1, Y1), (X2, Y2)) to facilitate the calculation of the lane curvature radius on the left side of the lane where the vehicle is located. In this triangle, is the angle between the side formed by (X1, Y1) and (X0, Y0) and the side formed by (X1, Y1) and (X2, Y2), and A is the angle The length of the corresponding side is the length of the side formed by (X0, Y0) and (X2, Y2). Similarly, when calculating the curvature radius of the right lane edge line, a triangle that is convenient for calculating the lane curvature radius on the right side of the lane where the vehicle is located can be constructed based on the sampling coordinates ((X0′, Y0′), (X1′, Y1′), (X2′, Y2′)) corresponding to the right lane edge line. In this triangle, is the angle between the side formed by (X1′, Y1′), (X0′, Y0′) and the side formed by (X1′, Y1′), (X2′, Y2′), and A′ is the angle The length of the corresponding side, that is, the length of the side formed by (X0′, Y0′) and (X2′, Y2′).

[0100] In a specific implementation, the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located can be calculated respectively based on a number of sampling coordinates by the following formula, and the lane curvature radius of the lane where the vehicle is located is calculated based on the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located;

[0101]

[0102]

[0103]

[0104]

[0105]

[0106] In the formula, R l is the lane curvature radius of the left lane where the vehicle is located, (X0, Y0), (X1, Y1), (X2, Y2) are the adjacent sampling coordinates of the lane edge line on the left side of the lane where the vehicle is located, In the triangle formed by (X0, Y0), (X1, Y1), (X2, Y2), A is the angle between the side formed by (X1, Y1), (X0, Y0) and the side formed by (X1, Y1), (X2, Y2). The length of the corresponding side, R r is the lane curvature radius of the right lane where the vehicle is located, (X0′, Y0′), (X1′, Y1′), (X2′, Y2′) are the adjacent sampling coordinates of the lane edge line on the right side of the lane where the vehicle is located, In the triangle formed by (X0′, Y0′), (X1′, Y1′), (X2′, Y2′), A′ is the angle between the side formed by (X1′, Y1′), (X0′, Y0′) and the side formed by (X1′, Y1′), (X2′, K2′), The length of the corresponding side, R is the lane curvature radius of the lane where the vehicle is located.

[0107] In this embodiment, the lane curvature radius is calculated based on the lane edge shape point data provided by the high-precision map sensor, so that the autonomous driving system can perceive the curvature radius of the lane where the current vehicle is located in real time, and by obtaining the lane edge shape point data of the lane in front of the vehicle, the curvature radius parameter of the lane in front of the vehicle can be predictably perceived, so that the autonomous driving system can perceive the road state in advance and make targeted behavior planning in advance. In addition, after obtaining the lane edge shape point data provided by the high-precision map sensor, this embodiment uses the above method to filter and smooth the acquired lane edge shape point data, so as to effectively reduce the abnormal curvature result caused by abnormal shape point data, thereby ensuring the accuracy of the curvature radius parameter, so as to accurately reflect the curvature radius state of the lane where the vehicle is located.

[0108] In this embodiment, a high-precision map sensor is used to obtain multiple shape point coordinates of the lane edge line of the lane where the vehicle is located in real time; the projection coordinates of the vehicle projected on the lane edge line are determined based on the multiple shape point coordinates and the vehicle's position coordinates; multiple shape point projection distances are calculated based on the projection coordinates and the multiple shape point coordinates; a lane curve model is constructed based on the multiple shape point projection distances and the multiple shape point coordinates, and shape point sampling is performed based on the lane curve model to obtain a number of sampling coordinates; the lane curvature radius of the lane where the vehicle is located is calculated based on the several sampling coordinates. Thus, by constructing a lane curve model based on the lane edge line shape point data provided by the high-precision map sensor, and then accurately calculating the lane curvature radius based on the lane curve model, the curvature radius parameters of the road ahead of the vehicle can be predictively perceived to facilitate behavior planning in autonomous driving.

[0109] Reference Figure 7 , Figure 7 This is a structural block diagram of the first embodiment of the lane curvature radius calculation system based on a high-precision map of the present invention.

[0110] like Figure 7 As shown, the lane curvature radius calculation system based on the high-precision map proposed in the embodiment of the present invention includes:

[0111] A shape point coordinate acquisition module 10 is used to acquire multiple shape point coordinates of the lane edge line of the lane where the vehicle is located in real time through a high-precision map sensor;

[0112] A projection coordinate acquisition module 20, for determining the projection coordinates of the vehicle projected on the lane edge line based on the coordinates of the plurality of shape points and the position coordinates of the vehicle;

[0113] A shape point distance calculation module 30, used for calculating a plurality of shape point projection distances based on the projection coordinates and a plurality of shape point coordinates;

[0114] A lane model building module 40, for building a lane curve model based on a plurality of shape point projection distances and a plurality of shape point coordinates, and performing shape point sampling based on the lane curve model to obtain a plurality of sampling coordinates;

[0115] The curvature radius calculation module 50 is used to calculate the lane curvature radius of the lane where the vehicle is located based on a number of sampling coordinates.

[0116] Optionally, the shape point coordinate acquisition module 10 is further used to acquire multiple shape point coordinates of the lane edge line of the lane where the vehicle is located in real time through a high-precision map sensor;

[0117] The shape point coordinate acquisition module 10 is also used to calculate the angle between each shape point coordinate and the vector formed by the adjacent front and rear shape point coordinates;

[0118] The shape point coordinate acquisition module 10 is further used to filter the multiple shape point coordinates according to whether the angle is greater than a preset angle to obtain the filtered multiple shape point coordinates.

[0119] Optionally, the shape point distance calculation module 30 is also used to calculate the distances from multiple shape point coordinates to the coordinate origin with the projection coordinates as the coordinate origin, the vehicle's forward direction as the positive direction, and the direction of the area the vehicle has traveled as the reverse direction to obtain multiple shape point projection distances.

[0120] Optionally, the lane model building module is further used to build a lane curve model based on multiple shape point projection distances and multiple shape point coordinates, and the lane curve model includes a lane curve model on the left side of the lane where the vehicle is located and a lane curve model on the right side of the lane where the vehicle is located;

[0121] The lane curve model on the left side of the lane where the vehicle is located is:

[0122]

[0123]

[0124] The lane curve model on the right side of the lane where the vehicle is located is:

[0125]

[0126]

[0127] In the formula, (X l ,Y l ) is the coordinate of the point on the lane edge line on the left side of the lane where the vehicle is located, S l is the projection distance of the point on the left side of the lane where the vehicle is located, (X r ,Y r ) is the coordinate of the point on the lane edge line on the right side of the lane where the vehicle is located, S r is the projection distance of the shape point on the right side of the lane where the vehicle is located, and D0, D1, D2, D3, D4, D5, D′0, D′1, D′2, D′3, D′4, and D′5 are lane model parameters solved based on the relationship between multiple shape point coordinates and multiple shape point projection distances;

[0128] The lane model building module is also used to select a number of shape point projection distances S at a preset sampling interval. l , and project each point S l Substituting the lane curve model on the left side of the lane where the vehicle is located into the lane curve model to perform shape point sampling to obtain sampling coordinates of the lane edge lines on the left side of the lane where the vehicle is located;

[0129] The lane model building module is also used to select a number of shape point projection distances S at the preset sampling interval. r , and project each point S r Substitute the curve into the lane curve model on the right side of the lane where the vehicle is located to perform shape point sampling to obtain sampling coordinates of several lane edge lines on the right side of the lane where the vehicle is located.

[0130] Optionally, the curvature radius calculation module 50 is further used to calculate the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located respectively based on a number of sampling coordinates by the following formula, and calculate the lane curvature radius of the lane where the vehicle is located based on the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located;

[0131]

[0132]

[0133]

[0134]

[0135]

[0136] In the formula, R lis the lane curvature radius of the left lane where the vehicle is located, (X0, Y0), (X1, Y1), (X2, Y2) are the adjacent sampling coordinates of the lane edge line on the left side of the lane where the vehicle is located, In the triangle formed by (X0, Y0), (X1, Y1), (X2, Y2), A is the angle between the side formed by (X1, Y1), (X0, Y0) and the side formed by (X1, Y1), (X2, Y2). The length of the corresponding side, R r is the lane curvature radius of the right lane where the vehicle is located, (X0′, Y0′), (X1′, Y1′), (X2′, Y2′) are the adjacent sampling coordinates of the lane edge line on the right side of the lane where the vehicle is located, In the triangle formed by (X0′, Y0′), (X1′, Y1′), (X2′, Y2′), A′ is the angle between the side formed by (X1′, Y1′), (X0′, Y0′) and the side formed by (X1′, Y1′), (X2′, Y2′). The length of the corresponding side, R is the lane curvature radius of the lane where the vehicle is located.

[0137] In this embodiment, a high-precision map sensor is used to obtain multiple shape point coordinates of the lane edge line of the lane where the vehicle is located in real time; the projection coordinates of the vehicle projected on the lane edge line are determined based on the multiple shape point coordinates and the vehicle's position coordinates; multiple shape point projection distances are calculated based on the projection coordinates and the multiple shape point coordinates; a lane curve model is constructed based on the multiple shape point projection distances and the multiple shape point coordinates, and shape point sampling is performed based on the lane curve model to obtain a number of sampling coordinates; the lane curvature radius of the lane where the vehicle is located is calculated based on the several sampling coordinates. Thus, by constructing a lane curve model based on the lane edge line shape point data provided by the high-precision map sensor, and then accurately calculating the lane curvature radius based on the lane curve model, the curvature radius parameters of the road ahead of the vehicle can be predictively perceived to facilitate behavior planning in autonomous driving.

[0138] Other embodiments or specific implementation methods of the lane curvature radius calculation system based on a high-precision map of the present invention can refer to the above-mentioned method embodiments and will not be repeated here.

[0139] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or system including the element.

[0140] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0141] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course can also be implemented by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as a read-only memory / random access memory, a magnetic disk, or an optical disk), and includes a number of instructions for a terminal device (which can be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0142] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for calculating lane curvature radius based on a high-precision map, characterized in that: The lane curvature radius calculation method based on the high-precision map includes the following steps: The coordinates of multiple points on the lane edge line of the lane where the vehicle is located are obtained in real time through a high-precision map sensor; Determine the projection coordinates of the vehicle projected on the lane edge line based on the plurality of shape point coordinates and the position coordinates of the vehicle; Calculate multiple shape point projection distances based on the projection coordinates and multiple shape point coordinates; Constructing a lane curve model based on multiple shape point projection distances and multiple shape point coordinates, and performing shape point sampling based on the lane curve model to obtain a plurality of sampling coordinates; The lane curvature radius of the lane where the vehicle is located is calculated based on a number of sampling coordinates.

2. The method for calculating lane curvature radius based on a high-precision map according to claim 1, characterized in that: The step of acquiring the coordinates of multiple points of the lane edge line of the lane where the vehicle is located in real time by means of a high-precision map sensor comprises: The coordinates of multiple points on the lane edge line of the lane where the vehicle is located are obtained in real time through a high-precision map sensor; Calculate the angle between each shape point coordinate and the vector formed by the coordinates of the adjacent front and back shape points; The multiple shape point coordinates are filtered according to whether the angle is greater than a preset angle to obtain the filtered multiple shape point coordinates.

3. The method for calculating lane curvature radius based on a high-precision map according to claim 1, characterized in that: The step of calculating the projection distances of multiple shape points based on the projection coordinates and the coordinates of multiple shape points comprises: Taking the projection coordinates as the coordinate origin, the vehicle's forward direction as the positive direction, and the direction of the area the vehicle has traveled as the reverse direction, the distances from multiple shape point coordinates to the coordinate origin are calculated to obtain multiple shape point projection distances.

4. The method for calculating lane curvature radius based on a high-precision map according to claim 1, characterized in that: The step of constructing a lane curve model based on a plurality of shape point projection distances and a plurality of shape point coordinates, and performing shape point sampling based on the lane curve model to obtain a plurality of sampling coordinates includes: Constructing a lane curve model based on a plurality of shape point projection distances and a plurality of shape point coordinates, wherein the lane curve model includes a lane curve model on the left side of the lane where the vehicle is located and a lane curve model on the right side of the lane where the vehicle is located; The lane curve model on the left side of the lane where the vehicle is located is: The lane curve model on the right side of the lane where the vehicle is located is: In the formula, (X l ,Y l ) is the coordinate of the point on the lane edge line on the left side of the lane where the vehicle is located, S l is the projection distance of the point on the left side of the lane where the vehicle is located, (X r ,Y r ) is the coordinate of the point on the lane edge line on the right side of the lane where the vehicle is located, S r is the projection distance of the point on the right side of the lane where the vehicle is located, D0, D1, D2, D3, D4, D5, D0 ′ 、S1 ′ 、D2 ′ 、D3 ′ 、D4 ′ 、D5 ′ They are all lane model parameters solved based on the relationship between multiple shape point coordinates and multiple shape point projection distances; The step of sampling shape points based on the lane curve model to obtain a plurality of sampling coordinates includes: Select several shape points with preset sampling intervals and project distance S l , and project each point S l Substituting the lane curve model on the left side of the lane where the vehicle is located into the lane curve model to perform shape point sampling to obtain sampling coordinates of the lane edge lines on the left side of the lane where the vehicle is located; Select several shape point projection distances S at the preset sampling interval r , and project each point S r Substitute the curve into the lane curve model on the right side of the lane where the vehicle is located to perform shape point sampling to obtain sampling coordinates of several lane edge lines on the right side of the lane where the vehicle is located.

5. The method for calculating lane curvature radius based on a high-precision map according to claim 1, characterized in that: The step of calculating the lane curvature radius of the lane where the vehicle is located based on a plurality of sampling coordinates includes: Based on a number of sampling coordinates, the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located are respectively calculated by the following formula, and the lane curvature radius of the lane where the vehicle is located is calculated based on the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located; In the formula, R l is the lane curvature radius on the left side of the lane where the vehicle is located, (X0, Y0), (X1, Y1), (X2, Y2) are the adjacent sampling coordinates of the lane edge line on the left side of the lane where the vehicle is located, In the triangle formed by (X0, Y0), (X1, Y1), (X2, Y2), A is the angle between the side formed by (X1, Y1), (X0, Y0) and the side formed by (X1, Y1), (X2, Y2). The length of the corresponding side, R r is the lane curvature radius of the right lane where the vehicle is located, (X0′, Y0′), (X1′, Y1′), (X2′, Y2′) are the adjacent sampling coordinates of the lane edge line on the right side of the lane where the vehicle is located, In the triangle formed by (X0′, Y0′), (X1′, Y1′), (X2′, Y2′), A′ is the angle between the side formed by (X1′, Y1′), (X0′, Y0′) and the side formed by (X1′, Y1′), (X2′, Y2′). The length of the corresponding side, R is the lane curvature radius of the lane where the vehicle is located.

6. A lane curvature radius calculation system based on a high-precision map, characterized in that: The lane curvature radius calculation system based on the high-precision map includes: A shape point coordinate acquisition module is used to acquire multiple shape point coordinates of the lane edge line of the lane where the vehicle is located in real time through a high-precision map sensor; A projection coordinate acquisition module, used to determine the projection coordinates of the vehicle projected on the lane edge line based on the coordinates of the multiple shape points and the position coordinates of the vehicle; A shape point distance calculation module, used for calculating a plurality of shape point projection distances based on the projection coordinates and a plurality of shape point coordinates; A lane model building module, used to build a lane curve model based on multiple shape point projection distances and multiple shape point coordinates, and perform shape point sampling based on the lane curve model to obtain a number of sampling coordinates; The curvature radius calculation module is used to calculate the lane curvature radius of the lane where the vehicle is located based on a number of sampling coordinates.

7. The lane curvature radius calculation system based on a high-precision map according to claim 6, characterized in that: The shape point coordinate acquisition module is also used to acquire multiple shape point coordinates of the lane edge line of the lane where the vehicle is located in real time through a high-precision map sensor; The shape point coordinate acquisition module is also used to calculate the angle between each shape point coordinate and the vector formed by the adjacent front and rear shape point coordinates; The shape point coordinate acquisition module is also used to filter the multiple shape point coordinates according to whether the angle is greater than a preset angle to obtain the filtered multiple shape point coordinates.

8. The lane curvature radius calculation system based on a high-precision map as claimed in claim 6, characterized in that: The shape point distance calculation module is also used to calculate the distances from multiple shape point coordinates to the coordinate origin with the projection coordinates as the coordinate origin, the vehicle's forward direction as the positive direction, and the direction of the area the vehicle has traveled as the reverse direction to obtain multiple shape point projection distances.

9. The lane curvature radius calculation system based on a high-precision map according to claim 6, characterized in that: The lane model building module is further used to build a lane curve model based on multiple shape point projection distances and multiple shape point coordinates, and the lane curve model includes a lane curve model on the left side of the lane where the vehicle is located and a lane curve model on the right side of the lane where the vehicle is located; The lane curve model on the left side of the lane where the vehicle is located is: The lane curve model on the right side of the lane where the vehicle is located is: In the formula, (X l ,Y l ) is the coordinate of the point on the lane edge line on the left side of the lane where the vehicle is located, S l is the projection distance of the point on the left side of the lane where the vehicle is located, (X r ,Y r ) is the coordinate of the point on the lane edge line on the right side of the lane where the vehicle is located, S r is the projection distance of the point on the right side of the lane where the vehicle is located, D0, D1, D2, D3, D4, D5, D0 ′ 、D1 ′ 、D2 ′ 、D3 ′ 、D4 ′ 、D5 ′ They are all lane model parameters solved based on the relationship between multiple shape point coordinates and multiple shape point projection distances; The lane model building module is also used to select a number of shape point projection distances S at a preset sampling interval. l , and project each point S l Substituting the lane curve model on the left side of the lane where the vehicle is located into the lane curve model to perform shape point sampling to obtain sampling coordinates of the lane edge lines on the left side of the lane where the vehicle is located; The lane model building module is also used to select a number of shape point projection distances S at the preset sampling interval. r , and project each point S r Substitute the curve into the lane curve model on the right side of the lane where the vehicle is located to perform shape point sampling to obtain sampling coordinates of several lane edge lines on the right side of the lane where the vehicle is located.

10. The lane curvature radius calculation system based on a high-precision map according to claim 6, characterized in that: The curvature radius calculation module is further used to calculate the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located respectively based on a number of sampling coordinates by the following formula, and calculate the lane curvature radius of the lane where the vehicle is located based on the lane curvature radius of the left side of the lane where the vehicle is located and the lane curvature radius of the right side of the lane where the vehicle is located; In the formula, R l is the lane curvature radius on the left side of the lane where the vehicle is located, (X0, Y0), (X1, Y1), (X2, Y2) are the adjacent sampling coordinates of the lane edge line on the left side of the lane where the vehicle is located, In the triangle formed by (X0, Y0), (X1, Y1), (X2, Y2), A is the angle between the side formed by (X1, Y1), (X0, Y0) and the side formed by (X1, Y1), (X2, Y2). The length of the corresponding side, R r is the lane curvature radius of the right lane where the vehicle is located, (X0′, Y0′), (X1′, Y1′), (X2′, Y2′) are the adjacent sampling coordinates of the lane edge line on the right side of the lane where the vehicle is located, In the triangle formed by (X0′, Y0′), (X1′, Y1′), (X2′, Y2′), A′ is the angle between the side formed by (X1′, Y1′), (X0′, Y0′) and the side formed by (X1′, Y1′), (X2′, Y2′). The length of the corresponding side, R is the lane curvature radius of the lane where the vehicle is located.

Images