ESTIMATION OF ROAD CENTERLINE CURVATURE FROM RAW GPS DATA

Development and wide use of route guidance systems lead to the need for suitable digital maps that can be used for some advanced applications. Sufficient accuracy of road geometry with emphasis on road centerline positions and curvature is crucial. In this paper is presented a method for finding road centerline curvature from raw GPS data. The approach consists of a few processing steps. First it is necessary to fit raw data of each road section using B-splines, and generate equidistant vertices of polyline of the fitted curve. Then follows the appliance of stereographic projection of chosen polyline segments onto the unit sphere. Using the least square method, the plane that best fits the points on the unit sphere is found and the circle that is the intersection of the plane and the unit sphere. Stereographic projection of this circle back to the equatorial plane gives the corresponding circular arc and curvature. The method is also applicable in higher dimensions. The 3D case is numerically presented and results show that the proposed procedure is efficient and yields accurate results.


Introduction
Precise digital road geometry is a fundamental for development of various applications regarding intelligent transport systems, traffic safety and other traffic-related areas (Barai 2003).
The accuracy of available digital maps suffices for some useful applications, such as navigation and route guidance systems. However, there are other applications, where a combination of enhanced digital road maps and precise positioning systems is necessary. Some possible applications for which detailed road geometry is needed are Rollover Warning and Control system, Lane Departure Warning (LDW) and lane-level navigation. Another application areas that can benefit from detailed road geometry data are analysis of road geometry on driving conditions and visibility (Pellegrino 2009;Vorobjovas 2011) and consistency analysis of highway elements and their influence on operating speed and traffic safety (Cafiso et al. 2010;D' Attoma 2010;Dell' Acqua, Russo 2010;Discetti 2010;Perco 2008;Zuriaga et al. 2010;Šliupas 2009).
There are several approaches to obtain the data. One widely used approach is obtaining the required data with specially equipped cars. The other is the use of the 2D aerial images, photointerpretation and appropriate vectorization. Due to the complexity of such images, the procedure is practically impossible to automate. Another option is statistical approach, where a large quantity of possibly noisy data from global positioning systems (GPS) for a fleet of vehicles is combined. The data are obtained from vehicles that commute on their usual business. This approach can be rather effective and less expensive. The same is true for the price of differential (DGPS) devices. This way one can construct maps that have higher accuracy, are easy to maintain, and are cheaper.
Accurate determination of the road centerline is necessary for deployment of advanced applications, such as LDW for lateral control. These accuracy requirements were integrated within the NextMAP project that deals with the economical and technical aspects of digitally defined road maps. The quality of geometry includes adequate topology of the road network, accuracy of the centerline and last but not least, frequency of the map update. The combination of all these elements guarantees suitability for most demanding applications.
A specially equipped car, as for example Photobus (Gontran et al. 2005), combines accurate positioning through GPS/IMU measurements with a vertically oriented CCD or CMOS camera. The system performs synchronization of GPS data with imagery and allows precise determination of the road centerline. The geodetic coordinates must be transformed to a local reference frame. This data allows the acquisition of the horizontal and vertical components of the road geometry.
The purpose of presented research is to develop and verify an efficient method for curve fitting, which can be used in modeling and analysis of the road centerline axis geometry. To model and efficiently analyze the road geometry, cubic splines were chosen as interpolating functions. Cubic splines are piecewise interpolated curves by 3 rd -degree polynomials between n adjustment points. Consequently, they take into account all trajectory behavior on each piece of interval and provide continuity conditions, velocity and acceleration on each adjustment point. Due to these characteristics, such curves are wellsuited for approximating road axes. The next task is to segment given digital curve, representing the road axis, into arcs. It is well known that long straight stretches of road can increase the chance of accidents, as they dull the driver's attention or stimulate him or her to speed up. On the other hand, most turns generate a decrease of speed and can sometimes surprise the driver, especially in case of bad weather conditions. The Advanced Driver Assistance Systems (ADAS) function, the Curve Speed Warning (CSW), or Rollover Warning, can inform users if they are traveling too fast to successfully pass an upcoming turn. Since roadway design guidelines link the radius of turn curvature with the max speed for that turn, information on curvature radii together with other data can make CSW reliable and useful.

Overview of published methods
Curve-fitting is an important technique in the processing and analyzing digitaly given curves. Known procedures can be grouped into interpolation and approximation, depending on whether the resulting curve passes through all of the data points or not. To fit a digitaly given curve it has to be divided the into segments. Such digital segment is fitted with a appropriate analytic curve, which can be a line segment, a circular arc, or a high order curve.
The simplest approach, polygonal approximation (Perez, Vidal 1994;Pikaz, Averbuch 1996;Ray, Ray 1993, fits each digital segment with a straight line segment by finding the breakpoints and connecting them with straight line segments. Better representation can be obtained by using circular arcs for segment fitting (Pei, Horng 1995;Pei, Horng 1996). Many methods are based on a combination of line segments and circular arcs (Horng, Li 2001;Ichoku et al. 1996;Rosin, West 1989). A significant effort has also been devoted to fitting arcs and bi-arcs through points in 2D (Yang, Du 1996). None of the above methods can be easily extended to 3D cases. Three-dimensional curves play a fundamental role in many 3D applications, one of them is also road centerline in engineering praxis. For this purpose, non-linear curves have been used (Farin 1997;Laurent et al. 1991).
Curvature is one of the most important pieces of information about shape contours, but in spite of its importance, no definitive numerical method for curvature estimation has been found. While the curvature of continuous curves can be precisely evaluated by using closed-form expressions, the problem of estimating curvature of spatially sampled digital curves is not straightforward. The main problem with such "digital curvature" estimation approaches is that digitaly given curves do not even have a curvature in a strict sense, for they are no more than a set of isolated points. Some pre-processing steps are needed before "curvature" can be estimated. The importance of knowing this element has motivated a series of researches (Coeurjolly et al. 2001;Estrozi et al. 1999;Fairney, Fairney 1994;Imran et al. 2006;Lewiner et al. 2004;Mokhtarian, Mackworth 1992;Worring, Smeulders 1993).
The procedure for road centerline determination should also estimate the corresponding road curvature. Using a spline-fitting approach, the curvature can be calculated through the 1 st and 2 nd derivative (Schroedl et al. 2004). It is usually assumed that curvature is continuous across a road segment, which is why the degree of the polynomial used for interpolation must be at least three.

The proposed method
Here, a brief description of our approach is presented, which is different from procedures used usually.
The raw GPS data, where points are not equidistant, are smoothed by splines, and new equidistant sample data are generated. These data are used in further computations. The next step is segmentation of digitally given curve into circular arcs. For this purpose some points (10 to 20) were selected and search for the best fitting circular arc was performed. In each further step one additional point is added while observing the error, which is defined as the max distance of chosen points to the arc. When this error exceeds the prescribed error (e.g. 1 m), the construction of the arc is stopped and construction of a new arc starts.
The arc construction is performed in two steps. The abovementioned points are projected stereographically onto a sphere and find the plane whose intersection with the sphere best fits the data points. The intersection of the plane with the sphere is a circle, which is then stereographically projected back onto the XY plane. The result is the corresponding circular arc.
All steps of the presented procedure are numerically very stable and there is no numerical problem when estimating a radius of curvature of nearly linear segments of a digital curve. As a consequence, the proposed method is extremely robust. A detailed outline of the proposed procedure is given below.
The GPS coordinates of road centerline are not equidistant; distances between adjacent points vary from few to several 10 m. That is why the digital curve has to be smoothened. Let (1) and let T be the matrix whose rows are coordinates of points T 1 , T 2 , T 3 , T 4 . Then (2) is a cubic polynomial mapping, represented with a matrix product of three matrices of dimensions 1×4, 4×4 and 4×2. While the parameter t takes all values from the interval [0, 1], the point B(t) lies on the arc, starting near point T 2 and ending near point T 3 . Then it is necessary to repeat the procedure with points T 2 , T 3 , T 4 , T 5. Both polynomial arcs are smoothly (C 2 ) joined near point T 3 (Bartels et al. 1987). The whole polygonal line can be approximated in that way, except the 1 st and the last point. The method works on polygonal lines in arbitrary Euclidean space R n . The example in R 2 is illustrated in Fig. 1.

Fig. 1. Splines
For the purpose of the problem, a polygonal line whose vertices are equidistant has to be generated. Let S 2 be the unit sphere with center in the coordinate origin and north pole at N(0, 0, 1). The line through the North pole N and an arbitrary point (u, v, 0) on the equatorial plane intersects the sphere in (x, y, z), if for some real t the following is true: (3) Taking into account that the vector (x, y, z) has unit length, it follows: (4) and (5) Point (u,v,0) in the equatorial plane maps into point T(x, y, z) on the sphere, whose coordinates are (Fig. 2) The inverse relationship between point T(x, y, z) on the sphere and point (u, v, 0) on the equatorial plane is (7) The lines and circles in the equatorial plane map onto circles on the sphere. The circles passing through the north pole correspond to straight lines on the equatorial plane. To see that one can take an arbitrary circle or line lying on the equatorial (u, v) plane, it can be written as: (8) and express it in coordinates on the sphere (9) The above expression simplifies into the Eq of the plane (10) intersecting the sphere in a circle (Fig. 3).
The stereographic projection maps the circle resulting from the intersection of the unit sphere with the plane (11)   (12) and center at (13) For the part L of the above generated polyline a circular arc or line approximation has to be find in the following way. First the stereographically project plane vertices of L onto a sphere must be done. The least square method to fit these 3D space points with the plane Ψ, intersecting the unit sphere in the circle K 1 is used. Its projection back to the equatorial plane gives the desired circular arc or line K 2 .
For given points there exists such a plane that the sum of squares of distances of given points from the plane is min.
First the standard least square problem must be solved, so that a linear affine function is found -which minimizes (14) Then an orthonormal basis with its last base vector orthogonal to the plane is found. The procedure is repeated in the new base. The convergence is extremely fast and only a few steps are needed.
The Mathematica Code of the above procedure is as follows in Fig. 4.
For testing purposes, an artificial digital curve has been constructed with two circular arcs of radius R = 300 m and R = 200 m and a connecting straight line segment (Fig. 5). The curve points are between 20 m and 35 m apart. The coordinates of the testing curve have been randomly uniformly perturbed inside a square of 2 m by 2 m around each point. This perturbation approx corresponds to the errors in position obtained by a GPS device. The procedure described above yielded two circular arcs of R = 299 m and R = 200 m respectively and a circular arc with R = 64 205 m, which corresponds to the straight line segment connecting both arcs. Taking into account rather large random perturbations, the results show good robustness of the proposed method.
The 2D construction of circular arc and line approx from field road data are presented in Figs 6 and 7. In Fig. 6, one can find an example of an urban road with elements consisting of parts with small radii and short straight line segments, while Fig. 7 explains the meaning of additional information:  Fig. 7. Circular arcs -road: blue points -GPS data; red points -1 m equidistant points -splines; yellow points -end points of circular arcs; blue numbers -radii of arcs; black numbersstationing points of segmentation

Conclusions
In this paper, the method for finding road centreline curvature from raw GPS data is presented. The approach consists of a few processing steps. First raw data of each road section using B-splines is fitted then the equidistant vertex of polyline of the fitted curve is generated. Using the least square method, the plane that best fits the points on the unit sphere, and find the circle that is the intersection of the plane and the unit sphere is found.
Stereographic projection of this circle back to the equatorial plane gives the corresponding circular arc and curvature.
The proposed method has been applied on a real 3D data model of the whole Slovenian state road network (more than 6000 km), and the performance is very satis-performance is very satis-is very satisfactory. Furthemore, the approach is simple and straightforward. Last but not least, it also works in higher dimensions, which is not the case with any other known methods. Estimation of clothoide transition lines still has to be explored.