ACCURATE CURVATURE ESTIMATION ALGORITHM FOR PATH PLANNING OF AUTONOMOUS DRIVING VEHICLE

[0001] 1. Field of the Invention

[0002] This invention relates generally to a system and method for identifying the curvature of a roadway using map database shape points and, more particularly, to a system and method for identifying roadway curvature in a semi-autonomous or autonomously driven vehicle using map database shape points and a multi-point polynomial curve fitting algorithm.

[0003] 2. Discussion of the Related Art

[0004] The operation of modern vehicles is becoming more autonomous, i.e., vehicles are able to provide driving control with less and less driver intervention. Cruise control systems have been on vehicles for a number of years where the vehicle operator can set a particular speed of the vehicle, and the vehicle will maintain at that speed without the driver operating the throttle. Adaptive cruise control systems have been recently developed in the art where not only does the system maintain the set speed, but also will automatically slow the vehicle down in the event that a slower moving preceding vehicle is detected using various sensors, such as radar and cameras. Certain modern vehicles also provide autonomous parking where the vehicle will automatically provide the steering control for parking the vehicle. Some vehicle systems intervene if the driver makes harsh steering changes that may affect vehicle stability. Some vehicle systems attempt to maintain the vehicle near the center of a lane on the road. Further, fully autonomous vehicles have been demonstrated that can drive in simulated urban traffic up to 30 mph, observing all of the rules of the road.

[0005] As vehicle systems improve, they will become more autonomous with the goal being a complete autonomously driven vehicle. For example, future vehicles probably will employ autonomous systems for lane changing, passing, turns away from traffic, turns into traffic, etc. Smooth maneuvering and automated lane centering and lane changing control is important for driver and passenger comfort in autonomously driven vehicles. However, as a result of sensor and actuator latency, measured vehicle states may be different from actual vehicle states. This difference may cause improper path generation, which will affect lane changing harshness.

[0006] U.S. Pat. No. 8,170,739 issued May 1, 2012, titled, Path Generation Algorithm for Automated Lane Centering and Lane Changing Control System, assigned to the assignee of this application and herein incorporated by reference, discloses a system for providing path generation for automated lane center and/or lane changing purposes. The system employs one or more forward-looking cameras that detect lane markings in front of the vehicle for identifying a travel lane on which the vehicle is traveling. The road marking information detected by the cameras is used to determine a center lane of the vehicle that can be used to identify the curvature of the roadway, the heading angle of the vehicle, location of the vehicle, etc. A desired path generation processor receives the signals from the camera, vehicle state information and a steering angle of the vehicle, and a request for a vehicle lane change. The system also includes a path prediction processor that predicts the vehicle path based on the vehicle state information including vehicle longitudinal speed, vehicle lateral speed, vehicle yaw-rate and vehicle steering angle. The desired path information and the predicted path information are compared to generate an error signal that is sent to a lane change controller that provides a steering angle signal to turn the vehicle and reduce the error signal, where the path generation processor employs a fifth-order polynomial equation to determine the desired path of the vehicle based on the input signals.

[0007] The systems discussed above typically require knowledge of the roadway curvature to provide smooth vehicle path planning. It is known in the art to use a GPS receiver and associated map database on a vehicle for identifying the curvature of the roadway on which the vehicle is traveling. The GPS receiver identifies the location of the vehicle by latitude and longitude on the Earth and the map database uses that location to reconcile what roadway the vehicle is on and provides a number of shape points identifying the location of the roadway also in latitude and longitude around the vehicle. The available shape points in the map database are typically spaced apart depending on the curvature of the roadway, where the shape points are more closely spaced together for greater curved roads. The shape points can be connected by a line, and by fitting the line to a curve equation, the curvature of the roadway can be determined, where the curvature of the line travels through each of the shape points identified by database. However, because there are errors in the survey of the roadway, the map database shape points may not be exactly at the center of the roadway and may be somewhat off the roadway, which creates errors in the curve that is generated by the shape points.

[0008] Because the shape point locations have errors, the curvature of the line between the shape points may not accurately identify the proper radius of curvature of the road at any particular location. Because the outliers in the shape points in the map database are significant enough to cause the radius of curvature of the road to be significantly inaccurate, known techniques for identifying roadway curvature using map database shape points cannot be reliably employed.

[0009] The present disclosure describes a system and method for identifying roadway curvature in a vehicle system that uses shape points from a map database. The method includes determining a range of interest for determining the roadway curvature and collecting the shape points from the map database from a current position of the vehicle to an end of the range of interest that define the location of the roadway. The method converts the shape points from World Geodetic System 84 (WGS84) coordinates to UTM coordinates, and then fits a set of single polynomial equations to define a curve using the converted shape points. The method determines whether the single polynomial equations exceed a predetermined curvature accuracy threshold, and if so, fits multiple sets of polynomial equations to multiple contiguous roadway segments over the range of interest using the converted shape points. The method then determines the roadway curvature at any roadway location using solutions to the multiple polynomial equations.

[0010] Additional features of the present invention will become apparent from the following description and appended claims, taken in conjunction with the accompanying drawings.

[0016] The following discussion of the embodiments of the invention directed to a system and method for determining roadway curvature using map database shape points is merely exemplary in nature, and is in no way intended to limit the invention or its applications or uses. For example, as discussed herein, the system and method of the invention have particular application for a semi-autonomous or autonomously driven vehicle. However, as will be appreciated by those skilled in the art, the invention may have non-automotive applications.

[0017] FIG. 1 is an illustration of a vehicle 10 including a controller 12 that performs all of the necessary processes, calculations, algorithms, etc. discussed herein to provide path prediction, generation and control of the vehicle 10 driven in a semi-autonomous or autonomous manner, where the controller 12 can be any number of independent or combined processors, electronic control units (ECUs), devices, etc. The controller 12 receives signals from one or more forward-looking vision cameras 14 that detect lane markings and objects on or in the roadway or otherwise in front of the vehicle 10, where the camera 14 can be any suitable detection device for this purpose, such as a charge-coupled device (CCD) camera, complementary metal oxide semiconductor (CMOS) video image sensors, etc. The vehicle 10 also includes a number of sensors, represented generally as sensor 16, that provides vehicle motion information including, but not limited to, vehicle speed, yaw-rate, steering angle, lateral and longitudinal acceleration, lateral and longitudinal speed, etc. The vehicle 10 further includes a GPS receiver 18 that receives GPS signals identifying the location of the vehicle 10, which are provided to a map database 20 that provides an indication to the controller 12 of the location of the vehicle 10 on the roadway, for example, as spaced apart roadway shape points. Based on the desired path of the vehicle 10 and whether the vehicle 10 is being semi-autonomously or autonomously driven, the controller 12 may provide output signals to a vehicle brake system 22, a vehicle throttle 24 and/or a vehicle steering system 26.

[0018] FIG. 2 is an illustration 40 showing a vehicle 42 that is being autonomously driven on a lane 46 of a roadway 44 along a center of the lane 46. The illustration 40 shows a number of shape points 50, represented by (P₁, P₂, . . . , P_N), along the roadway 44, where the shape points 50 are provided by the map database 20 to represent the roadway 44, and where the shape points 50 generally follow the center of the lane 46. However, as discussed above, as a result of errors in the roadway surveying process, some of the shape points 50 may not be accurately at the center of the lane 46, and may not even be in the lane 46, referred to herein as outlier shape points.

[0019] As will be discussed in detail below, the present invention uses the shape points 50 to determine or estimate roadway curvature generally shown by line 52, where the number N of the shape points 50 that are used depends on many factors, such as the speed of the vehicle 42, curvature of the roadway 44, etc. The roadway curvature is determined for a particular range of interest, which may vary depending on what the curvature is being used for. For example, lane centering, lane changes, evasive maneuvers, etc. may require a long curvature preview range, where the range of interest can be a few kilometers, and where the longer the range, the more processing time is required. Further, the range of interest includes the roadway curvature behind the vehicle 42 as well as ahead of the vehicle 42, where a typical range may be 150 m behind the vehicle 42 and 300 m in front of the vehicle 42.

[0020] FIG. 3 is a flow chart diagram 60 showing a general overview of a process for determining roadway curvature using the map database shape points 50. At box 62, the algorithm collects the map database shape points 50, where each shape point 50 is represented as a longitude (Lon) and a latitude (Lat) in World Geodetic System 84 (WGS84):

$\begin{array}{cc}\begin{array}{c}\begin{array}{c}{\mathrm{Lon}}_1\\ ⋮\end{array}\\ {\mathrm{Lon}}_N\end{array}\begin{array}{c}\begin{array}{c}{\mathrm{Lat}}_1\\ ⋮\end{array}\\ {\mathrm{Lat}}_N\end{array}& \left(1\right)\end{array}$

[0021] The longitudinal and latitudinal identification of each of the shape points 50 is in three-dimensions, i.e., has an x, y and z designation, where the (Lat, Lon) coordinate system gives locations on the surface of the Earth. The algorithm is only interested in the curvature of the roadway 44 in the x and y coordinates, and as such, the algorithm converts the longitude and latitude of each point 50 into Universal Transverse Mercator (UTM) coordinates (x, y) at box 64. As is well understood by those skilled in the art, UTM is a projected coordinate system that uses a two-dimensional Cartesian coordinate system. The x,y coordinates in the UTM are the global coordinates where x represents the east direction and y represents the north direction.

[0022] The algorithm then attempts to fit those shape points 50 within the range of interest to a single curve, represented by the line 52, for each x and y shape point 50 as a function of arc length s at box 66, where the shape points (P₁, P₂, . . . , P_N) are in UTM coordinates within the predetermined range of interest, and where each shape point P_i has a (x, y) coordinate value, i.e., P_i=(x_i, y_i). In one embodiment, the algorithm attempts to find a single third-order polynomial curve that best fits all of the N collected shape points 50 as:

{circumflex over (x)}(s)=p₁₃·s³+p₁₂·s²+p₁₁·s+p₁₀,  (2)

ŷ(s)=q₁₃·s³+q₁₂·s²+q₁₁·s+q₁₀,  (3)

where s is the arc length, i.e., the distance between any two of the shape points 50, and is defined by:

s_i+1=s_i+√{square root over ((x_i+1−x_i)²+(y_i+1−y_i)²)}{square root over ((x_i+1−x_i)²+(y_i+1−y_i)²)}, for i=1, . . . , N−1  (4)

and where s₁=0 and the coefficients are unknowns in equations (2) and (3) and are determined by minimizing the cost function J as:

$\begin{array}{cc}J=\frac{1}{2}{\sum }_i^N\left[{\left(x_i-x_1-{\hat{x}}_i\right)}^2+{\left(y_i-y_1-{\hat{y}}_i\right)}^2\right].& \left(5\right)\end{array}$

[0023] Based on this analysis, there will be N−1 equations and eight unknowns. By minimizing the cost function J in equation (5), the solution of the equations can be found as:

$\begin{array}{cc}{\left[\begin{array}{cc}{p}_{13}& q_{13}\\ p_{12}& q_{12}\\ p_{11}& q_{11}\\ p_{10}& q_{10}\end{array}\right]\left[\begin{array}{cccc}{\sum }_{j=1}^{N_i}s_j6& {\sum }_{j=1}^{N_i}s_j5& {\sum }_{j=1}^{N_i}s_j4& {\sum }_{j=1}^{N_i}s_j3\\ {\sum }_{j=1}^{N_i}s_j5& {\sum }_{j=1}^{N_i}s_j4& {\sum }_{j=1}^{N_i}s_j3& {\sum }_{j=1}^{N_i}s_j2\\ {\sum }_{j=1}^{N_i}s_j4& {\sum }_{j=1}^{N_i}s_j3& {\sum }_{j=1}^{N_i}s_j2& {\sum }_{j=1}^{N_i}s_j\\ {\sum }_{j=1}^{N_i}s_j3& {\sum }_{j=1}^{N_i}s_j2& {\sum }_{j=1}^{N_i}s_j& {\sum }_{j=1}^{N_i}s_j1\end{array}\right]}^{-1}\times \hspace{1em}\left[\begin{array}{cc}{\sum }_{j=1}^{N_i}x_j\times s_j3& {\sum }_{j=1}^{N_i}x_j\times s_j3\\ {\sum }_{j=1}^{N_i}x_j\times s_j2& {\sum }_{j=1}^{N_i}x_j\times s_j2\\ {\sum }_{j=1}^{N_i}x_j\times s_j& {\sum }_{j=1}^{N_i}x_j\times s_j\\ {\sum }_{j=1}^{N_i}x_j& {\sum }_{j=1}^{N_i}y_j\end{array}\right]& \left(6\right)\end{array}$

[0024] Once the coefficients p₁₃, p₁₂, p₁₁, p₁₀, q₁₃, q₁₂, q₁₁ and q₁₀ are determined, then the curve line can be determined therefrom.

[0025] The single curve fitting process discussed above has limitations in that a single polynomial has poor performance when the road curvature changes often or quickly or the range of interest is too long, where the curve fitting process will not match the shape points 50 accurately enough. In order to overcome this limitation, the present invention provides a multi-segment curve fitting process at box 68, where the range of interest is divided into multiple segments to improve curvature performance. The algorithm needs to determine where the first segment ends that is determined by the single-polynomial curve fitting process and where the next segment begins in a smooth transition.

[0026] FIG. 4 is an illustration 80 similar to the illustration 40, where like elements are identified by the same reference number that shows a first road curvature segment identified by the line 52 and ending at shape point P_N1, where the first segment ends when the cost function J exceeds a predetermined threshold J>J_thre. A second segment represented by line 82 begins at the end point P_N1 at the end of the first segment line 52. In order to provide a smooth transition between the first segment and the second segment, an overlap section 84 is defined at the end of the first segment line 52 and includes a number of virtual shape points 86. Particularly, when determining the second segment, the algorithm may define the virtual shape points 86, which are not provided by the map database 20, but are determined from the first segment calculated by equations (2) and (3), because the overlap section 84 between the first and second segments may not be long enough, i.e., 50 m or so, to include map database shape points, thus requiring the virtual shape points 86. Since the first segment is already determined, the virtual shape points 86 at the end of the first segment are calculated and used for the second segment. The length of the overlap section 84 and the number of the virtual points 86 are calibrated parameters depending on the particular application.

[0027] As above, for the second segment curve fitting process, the algorithm selects a new set of shape points 50 starting from P_N1 with a predetermined number N2 of the shape points 50 from the map database 20 and the second segment curve is determined by the third-order polynomial equations:

{circumflex over (x)}(s)=p₂₃·s³+p₂₂·s²+p₂₁·s+p₂₀,  (7)

ŷ(s)=q₂₃·s³+q₂₂·s²+q₂₁·s+q₂₀,  (8)

where the coefficients p₂₃, p₂₂, p₂₁, p₂₀, q₂₃, q₂₂, q₂₁ and q₂₀ are unknown and are determined in the same manner as discussed above for the first segment using the cost function:

$\begin{array}{cc}J=\frac{1}{2}{\sum }_i^{N2}\left[{\left(x_i-x_1-{\hat{x}}_i\right)}^2+{\left(y_i-y_1-{\hat{y}}_i\right)}^2\right].& \left(9\right)\end{array}$

[0028] As discussed, in order to provide a smooth transition from the first segment line 52 to the second segment line 82, the algorithm includes a number of the shape points 50 at the end of the first segment line 52 and the virtual shape points 86 to be included as part of the second segment line 82 so that there is an overlap between the first segment and the second segment for a smooth transition therebetween. Therefore, a polynomial equation defining the curvature of the road is provided in multiple segments until the end of the desired range at any particular point in time. Depending on the curvature of the roadway 44, the range of interest, etc., more than two segments may be required, where the end of each segment is determined when the cost function J exceeds a predetermined threshold. Thus, the number of sets of polynomial equations required for the number of segments necessary for accurately determining the roadway curvature is automatically determined, which allows complex curved roads, such as S-curves, to be accurately represented by a single set of polynomial equations. The virtual shape points 86 will be determined at the beginning of each segment using the calculations for the previous segment.

[0029] Once the polynomial curve fitting equation has been calculated for each segment as discussed above, the algorithm determines the curvature κ of the roadway 44 at any particular shape point P_i at box 70 as:

$\begin{array}{cc}{\kappa }_i=\left({\hat{x}}_i^{\prime }{\hat{y}}_i^{″}-{\hat{y}}_i^{\prime }{\hat{x}}_i^{″}\right)/{\left(\sqrt{{\hat{x}}_i^{\prime 2}}+{\hat{y}}_i^{\prime 2}\right)}^3.& \left(10\right)\end{array}$

[0030] The heading angle θ_i of the vehicle 42 at point P_i is determined as:

θ_i=α tan(ŷ_i′/{circumflex over (x)}_i′),  (11)

where:

{circumflex over (x)}_i′=3p₁₃·s_i²+p₁₂·s_i+p₁₁,  (12)

ŷ_i′=3q₁₃·s_i²+q₁₂·s_i+q₁₁,  (13)

{circumflex over (x)}_i″=6p₁₃·s_i+p₁₂·s_i,  (14)

ŷ_i″=6q₁₃·s_i+q₁₂·s_i,  (15)

and where s_i is the arc length to point P_i from the origin point P₁.

[0031] Equations (12)-(15) are used to calculate the curvature κ and the heading angle θ_i within the first segment. If the curvature κ and the heading angle θ_i in the second segments are needed, those equations are:

{circumflex over (x)}_i′=3p₂₃·s_i²+p₂₂·s_i+p₂₁,  (16)

ŷ_i′=3q₂₃·s_i²+q₂₂·s_i+q₂₁,  (17)

{circumflex over (x)}_i″=6p₂₃·s_i+p₂₂·s_i,  (18)

ŷ_i″=6q₂₃·s_i+q₂₂·s_i.  (19)

[0032] Each of the coefficients p and q have been determined by the calculations above for the particular segment, where the first and second derivative of the values x and y are determined as above. Therefore, the algorithm can determine the curvature κ at any distance defined by the arc-length s_i to the point P_i from the location of the vehicle 42 at point P₁. In other words, the roadway 44 is represented by parametric equations, and the curvature κ can be calculated at any point regardless of the existence of shape points. For example, if the curvature κ is needed every 10 m, the curvature κ and the heading angle θ_i can be calculated for s_i=10, 20, 30, 40, etc.

[0033] FIG. 5 is a flow chart diagram 90 providing a more detailed discussion of the curve defining process discussed above. The algorithm starts at box 92 and reads the curve fitting calibration parameters at box 94 including the desired range to determine the road curvature κ and the acceptable curve fitting error thresholds. The algorithm then reads the shape points 50 from the map database 20 within the predetermined range at box 96. The algorithm then performs the coordinate transformation to UTM coordinates at box 98. The single curve fitting process for the first segment is performed at box 100 and the parameter coefficients p and q for that segment are determined at box 102. The algorithm then performs a range check at decision diamond 94 to determine if a single segment curve fitting process covers the entire desired range with enough accuracy, and if so, returns to the box 96 to read the shape points 50 from the map database 20 to update the curve as the vehicle 42 moves along the roadway 44. If the first segment cannot accurately cover the desired range within the cost function threshold, then the algorithm ends the previous curve fitting algorithm at box 106, and adds the virtual points 86 in the overlap section 84 at box 108 to determine the next segment in the multi-curve fitting process and provide a smooth transition between segments. The algorithm then determines the curve fitting algorithm for the next segment at box 110 and calculates the parameter coefficients p and q for that segment for those map database shape points 50 at box 112. The algorithm then returns to the decision diamond 102 to determine if the curvature κ calculated by this segment reaches the end of the desired range, and if not, proceeds to calculating a next segment curvature at the box 104.

[0034] As will be well understood by those skilled in the art, the several and various steps and processes discussed herein to describe the invention may be referring to operations performed by a computer, a processor or other electronic calculating device that manipulate and/or transform data using electrical phenomenon. Those computers and electronic devices may employ various volatile and/or non-volatile memories including non-transitory computer-readable medium with an executable program stored thereon including various code or executable instructions able to be performed by the computer or processor, where the memory and/or computer-readable medium may include all forms and types of memory and other computer-readable media.

[0035] The foregoing discussion disclosed and describes merely exemplary embodiments of the present invention. One skilled in the art will readily recognize from such discussion and from the accompanying drawings and claims that various changes, modifications and variations can be made therein without departing from the spirit and scope of the invention as defined in the following claims.