2017 13th International Conference on Computational Intelligence and Security

An Algorithm of Curved Path Tracking with Prediction Model for Autonomous

Vehicle

Jun Yang Hong Bao

Beijing Key Laboratory of Information Service Beijing Key Laboratory of Information Service
Engineering, Beijing Union University Engineering, Beijing Union University
Beijing, China Beijing, China
e-mail: buu_yangjun@sina.com e-mail: xxtbaohong@buu.edu.cn

Nan Ma Zuxing Xuan

Beijing Key Laboratory of Information Service Beijing Key Laboratory of Information Service
Engineering, Beijing Union University Engineering, Beijing Union University
Beijing, China Beijing, China
e-mail: xxtmanan@buu.edu.cn e-mail: zuxingxuan@163.com

Abstract—The Stanley method is a popular geometric steering The main functions of autonomous vehicles are mapping,
controller. However, the Stanley method is not as robust to localization, perception, navigation, and control. Path tracking
disturbances. The path tracking of the Stanley method is prone is one of the key components of autonomous vehicle
to large deviations in the curved path. In this paper, a predictive navigation system. The main purpose of path tracking is to
method is proposed to improve the curve tracking effect of the compute control commands, taking into account the
Stanley method. The predictive method of curved path tracking associated motion constraints, so that the vehicle is able to
not only the relative position of the closest point of the path, but follow the previously planned path.
also the direction of the fixation point. Firstly, we use the inertial The geometric path tracking algorithm uses simple
navigation system of autonomous vehicle to collect waypoints.
geometric relations to propose steering control laws. These
Considering that the GPS signal is susceptible to environmental
techniques use forward looking distances to measure errors
disturbances, we use cubic B-spline interpolation to correct the
waypoints and get an accurate road map. Secondly, we calculate before vehicles, and can be extended from simple arc
the position of the preview point by introducing the logarithmic calculations to more complex geometric theorems, such as the
relationship between the vehicle speed and the fixation point. vector tracking method [3]. Compared with model based path
Then, the preview point is used as the target for Stanley path tracking algorithm, geometric path tracking algorithm is
tracking. The results in a variety of environments show the relatively simple, more robust to path curvature, and tends to
effectiveness of the proposed method. work better at lower speed driving. However, compared with
geometric methods, model-based algorithms must consider
Keywords-component; curved path tracking; autonomous the effects of nonlinear vehicle dynamics on higher speed
vehicle; road map; tracking accuracy, usually better.
The Stanley method is a popular geometric steering
controller that first introduced Stanford University into the
I. INTRODUCTION DARPA City challenge [4]. Nonlinear control law calculation
Autonomous vehicles provide additional security, of steering command based on Stanley method, the nonlinear
increased productivity, greater accessibility, better road control rules are considered from the front of the vehicle to
efficiency and the potential for positive impact on the measure the vehicle cross track of path error and error path
environment [1]. relative to the vehicle driving. It has been proved that the
The research of autonomous vehicles has made significant algorithm converges to zero cross orbit error in an exponential
progress in recent years due to the increase in available manner.
computing power and the reduced cost of sensing and However, the Stanley method is less robust to disturbances
computing technologies. Recent competition, such as the 2007 and tends to oscillate more strongly. The Stanley method also
DARPA City Challenge [2], has accelerated the field of requires continuous curvature paths instead of path points,
independent design and development.

Corresponding author.

0-7695-6341-4/17/31.00 ©2017 IEEE 405

DOI 10.1109/CIS.2017.00094
which makes it susceptible to discretization effects on related Another method is to use spline curves. Many studies use
problems [5]. various types of spline curves to represent road geometry. For
In this paper, we build a high-precision road map to obtain example, the cubic B-spline is a representative method for
road curvature constraints by modeling road geometry. At the road modeling, with the advantage that local modifications of
same time, the logarithmic relationship between the vehicle the curve do not affect the overall shape of the curve. In
speed and the fixation point [6] is introduced, and the preview addition, many efficient B-spline approximation algorithms
point of the path is calculated according to the dynamic developed for computer aided design (CAD) make B-spline
vehicle speed and is used as the target for Stanley path curves easy [12]. In this paper, we use cubic B-spline curve
tracking. We show that the method can effectively reduce the fitting to perform road modeling.
lateral deviation when tracking the path through independent
driving experiments on a variety of roads. III. ROAD MODELING
This paper is organized as follows. Section II presents a In this paper, the planned road networks data model is a
review of related works. Section III introduces the road polyline model based on discrete points. The basic elements
geometry data acquisition and map generation. The curved of the model are waypoints and road polylines. The waypoint
path tracking algorithm will be reviewed in section IV. is a characteristic point that describes the structure of the road
Section V provides the experimental results and Section VI network, including the starting point, the end point, the
concludes this paper. curvature change point, the road intersection and some
special location points of the road, on the center line of the
II. RELATED WORK
road composed of the same lane. The polyline is connected
The accuracy of the map, storage efficiency and by the waypoints in turn, and the same lanes in the same road
availability depend on the combination of data acquisition and are indicated by a broken line. The different lanes of different
road modeling methods used. The purpose of data collection roads or different directions are expressed by different
is to obtain accurate data to represent the actual geometry of polylines.
the road. The main purpose of road modeling are to effectively
represent the road geometry in terms of storage efficiency and A. Data Acquisition
availability while maintaining a certain degree of accuracy [7]. In order to obtain the accurate vehicle pose data, a high-
Various approaches have been attempted to obtain precision vehicle positioning system is recommended. In this
accurate road geometry data. For example, aerial / satellite paper, we use a sensor fusion system that integrates a RTK-
based methods have been widely used in traditional digital GPS and a high precision inertial navigation system (INS).
maps and GIS. High resolution aerial camera images have Figure. 1 describes the Beijing Union University autonomous
been obtained from satellites or aircraft, and road geometry is vehicle C70 used for the probe vehicle of map generation
extracted by manual work or by means of an image processing algorithm.
device [8]. In addition, elevation information of the road
cannot be obtained because the image does not contain depth
information. Therefore, the accuracy of image-based methods
is limited to the instrument level.
Many studies use vehicle based detection methods to
obtain more accurate road geometry [9]. In this method, a
probe vehicle equipped with various sensors explores the road
and collects sensor data to obtain road geometry information.
Among the various sensor configurations possible, the
Figure 1. Autonomous vehicle equipped with a GPS+INS positioning
kinematics based GPS method is the most widely used system, system.
including real-time kinematics (RTK) and post-processing
kinematics (PPK) GPS. For each lane, we use the GPS device on the autonomous
It is important to have proper road geometry vehicle to collect the center waypoint on the center line.
representation to ensure storage efficiency and availability as Therefore, we use ܶ௚௣௦ to indicate the travel path of the
well as mapping accuracy. Various road geometry models autonomous vehicle.
were previously proposed [10], but the previous models did ܶ௚௣௦ = ሼ‫ݔ‬௜ ǡ ‫ݕ‬௜ ȁ݅ ൌ ͳǡʹǡ ǥ ǡ ݊ሽ (1)
not take into account three requirements simultaneously. For
Where ‫ݔ‬௜ and ‫ݕ‬௜ are the global positions of the waypoints.
example, polygons are widely used in traditional digital road
maps and various intelligent vehicle applications. B. Map Generation With Smoothing
Various mathematical curve models have been proposed Considering that the original road network data is sparse
to provide more accurate and more efficient road and unevenly distributed, it is necessary to interpolate the data
representations. For example, the curve is the best way to to the road network. Without changing the geometry of the
represent the geometry of a road, because traditionally a path road, interpolation can increase the number of waypoints and
is designed by a set of clothoids [11]. However, the gyro curve improve the accuracy of road map. The spline curve
is not sufficient for intelligent vehicle applications because it composed of continuous curve segments has been widely used
includes transcendental functions. in the construction of road model.

406
 The B-spline curve ‫ܥ‬ሺ‫ݐ‬ሻ is described using the parametric to oscillate during travel. When the vehicle travels to the
spline equations, as shown in (2), i.e., corners, the road tracking effect of the Stanley method is
‫ܥ‬ሺ‫ݐ‬ሻ ൌ σ௡௝ୀ଴ ܰ௝ǡ௣ ሺ‫ݐ‬ሻܾ௝ ൫ܶ௣ିଵ ൑ ‫ ݐ‬൑ ܶ௡ାଵ ൯ (2) delayed, resulting in an increase in the lateral error of the
Where ‫ ݌‬is the order of the spline,ܾ௝ ሺ݆ ൌ Ͳǡ ǥ ǡ ݊ሻ are the vehicle. In order to solve this problem, the predictive method
control points, and ‫ ݐ‬is the parameter of the spline. The knot of curved path tracking is proposed to improve the curve
tracking effect of the Stanley method.
vector ܶ is described as ܶ ൌ ൣܶ଴ ǡ ܶଵ ǡ ǥ ǡ ܶ௡ା௣ିଵ ǡ ܶ௡ା௣ ൧, where
Firstly, we get the center waypoint after the road map has
the elements set of ܶ is a non-decreasing sequence of real been interpolated. It is assumed that the distance between the
value. To fix both the endpoints of the spline curve to the start road map waypoints is ߝ = 1m. In order to ensure smooth road,
and end of control points, ܾ଴ and ܾ௡ , the first and last set of we require interpolation interval ‫ = ݌‬0.1m.
knots are repeated ‫ ݌‬times with the same value as follows: ᇱ ௜
ܶ௚௣௦ ൌ ݂൫ܶ௚௣௦ ǡ ͲǤͳǡͳ൯ ൌ ൛ܵ௪௔௬ ȁ݅ ൌ Ͳǡͳǡʹǡ ǥ ǡ ݉ൟ (7)
ܶ଴ ൌ ‫ ڮ‬ൌ ܶ௣ିଵ ൌ ‫ݐ‬௦௧௔௥௧ ǡ ܶ௡ାଵ ൌ ‫ ڮ‬ൌ ܶ௡ା௣ ൌ ‫ݐ‬௘௡ௗ
At the ‫ ݐ‬moment, the nearest point of an autonomous
Where ‫ݐ‬௦௧௔௥௧ and ‫ݐ‬௘௡ௗ are the parameter values of the start ௧ ௧
vehicle relative to the path is ܲ௣௥௢௝௘௖௧ . For each ܲ௣௥௢௝௘௖௧ , we
and endpoint of the spline, respectively. ௝
ᇱ
ܰ௝ǡ௣ ሺ‫ݐ‬ሻ is the basis function of the curve with order ‫ ݌‬and can find the nearest center waypoint in ܶ௚௣௦ , which is ܵ௪௔௬ .
can be described with the following equation: There is a relationship between the speed and the point of
ͳ݂݅ܶ௜ ൑ ‫ ݐ‬൑ ܶ௜ାଵ  fixation [6]:
ܰ௜ǡ଴ ሺ‫ݐ‬ሻ ൌ ቄ (3) ‫ݒ‬௦௧ ൌ െʹ͵ͷǤ͵ͳʹ ൅ ͷʹǤͺͷ͸݈݂݊௣௧ (8)
Ͳ‫݁ݏ݅ݓݎ݄݁ݐ݋‬
‫ ݐ‬െ ܶ௝ ܶ௜ା௝ିଵ െ ‫ݐ‬ ೡ೟ೞ శమయఱǤయభమ
ܰ௜ǡ௝ ሺ‫ݐ‬ሻ ൌ ܰ ሺ‫ݐ‬ሻ ൅ ܰ ሺ‫ݐ‬ሻ ݂௣௧ ൌ ݁‫ ݌ݔ‬ఱమǤఴలఱ (9)
ܶ௜ା௝ െ ܶ௜ ௜ǡ௝ିଵ ܶ௜ା௝ିଵ െ ܶ௜ାଵ ௜ାଵǡ௝ିଵ ௧
Where ‫ݒ‬௦ is the vehicle speed of the autonomous vehicle,
Where ݆ ൌ ͳǡʹǡ ǥ ǡ ‫݌‬.
and ݂௣௧ is the distance between the fixation point and the car at
In this paper, we use cubic B-spline fitting on the original ఫ ෣௞
road network data ܶ௚௣௦ . ܶ௚௣௦ ᇱ
is a set of navigation data time‫ݐ‬. Curve length (ܵ௪௔௬ ܵ௪௔௬ ) was estimated by summing
generated by interpolation and smoothing; ‫ ݌‬is the interval of up the Euclidean distances between the segments that form the
interpolation; ߝ is the interval of navigation data. curve.
ఫ ෣௞ మ
ᇱ
ܶ௚௣௦ ൌ ݂൫ܶ௚௣௦ ǡ ‫݌‬ǡ ߝ൯ (4) ܵ௪௔௬ ܵ௪௔௬ ൌ σ௞ିଵ ଶ ௧
௜ୀ௝ ඥሺ‫ݔ‬௜ െ ‫ݔ‬௜ାଵ ሻ ൅ ሺ‫ݕ‬௜ െ ‫ݕ‬௜ାଵ ሻ  ൎ ݂௣ (10)
Where Ͳ ൑ Œ ൏  ൑ . From (10), we can get the center
IV. GEOMETRIC PATH TRACKING waypoint ܵ௪௔௬ ௞
corresponding to the path of the fixation point
In this section, the Stanley path tracking method is firstly ௧
ܲ௙௜௫௔௧௜௢௡ .
introduced. Then the predictive method of curved path Then, we look at the minimum angle between the two
tracking is presented. adjacent waypoint vectors ሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬԦ ௡ ܵ ௡ାଵ and the vector
ܵ௪௔௬ ௪௔௬
A. The Stanley Method ሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬԦ
ܲ௧ ܲ௧ in ܶ ᇱ , denote by ߠ ௠௜௡ . The
௣௥௢ఫ௘௖௧ ௙ప௫௔௧ప௢௡ ௚௣௦ ௣௥௘௩௜௘௪
The Stanley method is Stanford University's own vehicle ௡
corresponding ܵ௪௔௬ ௠௜௡
of ߠ௣௥௘௩௜௘௪ is the path prediction point
into the DARPA Challenge at Stanley's path tracking method. ௧
ܲ௣௥௘௩௜௘௪ .
The Stanley method is a nonlinear feedback function of the ௠௜௡
ߠ௣௥௘௩௜௘௪ ൌ
cross-track error ݁௙௔ measured from the front axle center to ሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬԦ
೟
௉೛ೝ೚ണ೐೎೟ ௉೟ ሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬԦ
ήௌ ೙ ೙శభ
ೢೌ೤ ௌೢೌ೤
the closest path point ܲ௣௥௢௝௘௖௧ ൫‫ܥ‬௫ ǡ ‫ܥ‬௬ ൯ , which can show ೑ഢೣೌ೟ഢ೚೙
‹ ቊܽ‫ ݏ݋ܿܿݎ‬ቆ ሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬԦ ቇቋ (11)
೟ ೟ ሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬሬԦ
೙ ೙శభ
exponential convergence. The common positioning control ቚ௉೛ೝ೚ണ೐೎೟௉೑ഢೣೌ೟ഢ೚೙ ቚ‫כ‬ቚௌ ೢೌ೤ ௌೢೌ೤ ቚ
point with the steering wheel allows the intuitive control law, Where Ͳ ൑ ݊ ൑ ݉.
where the first term simply sets the steering angle Ɂ equal to Finally, the heading ߠ of the autonomous vehicle is
the azimuth error to simply keep the wheel aligned with the acquired by vehicle inertial navigation system. ߠ௘ᇱ is the error
given path. between the heading of ego-vehicle and the direction of the
ߠ௘ ൌ ߠ െ ߠ௣ (5) preview point, as described in (12), i.e.,
Where ߠ is the course of the vehicle and ߠ௣ is the course ߠ௘ᇱ ൌ ߠ െ ߠ௣௥௘௩௜௘௪ (12)
ᇱ
of the ܲ௣௥௢௝௘௖௧ path. The resulting steering control law is as We take ߠ௘ into equation 6, and then we can find steering
follows: angle of the front wheel ߜ ᇱ ሺ‫ݐ‬ሻ after correcting with preview
௞௘೑ೌ ሺ௧ሻ point.
Ɂሺ‫ݐ‬ሻ ൌ ߠ௘ ሺ‫ݐ‬ሻ ൅ ܽ‫ ݊ܽݐܿݎ‬ቀ ቁ (6)
௩ೣ ሺ௧ሻ
Where ݇ is a gain value. Obviously, the desired effect is V. EXPERIMENTAL RESULT
achieved by this control rule: as the ݁௙௔ increases, the wheels In this section, we test the path tracking algorithm on
are further directed towards the path. different roads. This is to illustrate how the tracking accuracy
is improved after introducing the preview point in the Stanley
B. Predictive Method of Curved Path Tracking method.
The Stanley method specifies that ܲ௣௥௢௝௘௖௧ is the closest Both of the actual paths have different length and curve
point to the vehicle's front axle and path. However, in practice, characteristics. Our experimental platform is the Beijing
selecting a near point is likely to cause the autonomous vehicle Union University autonomous vehicle C70, as shown in

407
Figure 1.The data of the paths are based only on GPS modified algorithm can reduce the lateral errors in curved path
information captured by GPS and RTK-GPS receivers. tracking.
A visual representation of the considered paths can be seen
in Figure 2. ACKNOWLEDGMENT
We would like to acknowledge all of our team members,
whose contributions were essential for our autonomous
vehicle. The work was supported by the Project of the Major
Programs, funded by National Natural Science Foundation of
China (Grant No. 91420202) and the Importation and
Development of High-Caliber Talents Project of Beijing
Municipal Institutions under project IDHT: 20140508.
(a) PATH-1 (b) PATH-2
Figure 2. The autonomous car turns left at the intersection. The toatal REFERENCES
distance of this path is about 234m. (b) The autonomous car turns right at
the intersection. The total distance of this path is about 215m. [1] S. Pendleton, T. Uthaicharoenpong, Z. J. Chong, G. M. J. Fu, B. Qin, W.
Liu, et al., "Autonomous golf cars for public trial of mobility-on-
demand service," 2015 IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS) , 2015, pp. 1164-1171.
TABLE I. ߠ௘ UNDER 15KM/H IN
[2] C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. Clark, et al.,
"Autonomous driving in urban environments: Boss and the urban
ࣂࢋ (degree)
challenge," Journal of Field Robotics, vol. 25, 2008, pp. 425-466.
Max Min MSD [3] J. S. Wit, "Vector pursuit path tracking for autonomous ground
Stanley Method(PATH-1) 2.03486 -17.460201 4.118019 vehicles," FLORIDA UNIV GAINESVILLE CENTER FOR
INTELLIGENT MACHINES AND ROBOTICS, 2000.
Stanley Method(PATH-2) 18.730621 -0.536976 4.486799 [4] G. M. Hoffmann, C. J. Tomlin, M. Montemerlo, and S. Thrun,
"Autonomous automobile trajectory tracking for off-road driving:
Our Method(PATH-1) 0.132582 -17.448857 4.094828 Controller design, experimental validation and racing," American
Control Conference, 2007. ACC'07, 2007, pp. 2296-2301.
Our Method(PATH-2) 13.764967 -0.322346 4.362918
[5] H. Andersen, Z. J. Chong, Y. H. Eng, S. Pendleton, and M. H. Ang,
"Geometric path tracking algorithm for autonomous driving in
pedestrian environment," 2016 IEEE International Conference on
Advanced Intelligent Mechatronics (AIM) , 2016, pp. 1669-1674.
[6] Q. Guan, H. Bao, and Z. Xuan, "The research of prediction model on
TABLE II. LATERAL ERRORS UNDER 15KM/H
intelligent vehicle based on driver’s perception," Cluster Computing,
Lateral Errors (m) 2017, pp. 1-13.
[7] G.-P. Gwon, W.-S. Hur, S.-W. Kim, and S.-W. Seo, "Generation of a
Max Min Average precise and efficient lane-level road map for intelligent vehicle
Stanley Method(PATH-1) 0.3 -0.5 -0.139206 systems," IEEE Transactions on Vehicular Technology, vol. 66, 2017,
pp. 4517-4533.
Stanley Method(PATH-2) 0.1 -1.0 -0.204382 [8] Y.-W. Seo, C. Urmson, and D. Wettergreen, "Exploiting publicly
available cartographic resources for aerial image analysis," Proceedings
Our Method(PATH-1) 0.4 -0.1 0.073256 of the 20th International Conference on Advances in Geographic
Information Systems, 2012, pp. 109-118.
Our Method(PATH-2) 0.2 -0.3 -0.025217
[9] D. Bétaille and R. Toledo-Moreo, "Creating enhanced maps for lane-
level vehicle navigation," IEEE Transactions on Intelligent
In all of the above experiments, we set the gain ݇ in the Transportation Systems, vol. 11, 2010, pp. 786-798.
[10] S. Brummer, F. Janda, G. Maier, and A. Schindler, "Evaluation of a
Stanley method to 3.5, which is an empirical value. The mapping strategy based on smooth arc splines for different road types,"
positive ߠ௘ indicates that the vehicle leans to the left, the 2013 16th International IEEE Conference on Intelligent Transportation
negative ߠ௘ indicates that the vehicle leans to the right. Table Systems-(ITSC), 2013, pp. 160-165.
1 shows that the angular oscillation of the Stanley method is [11] V. Gikas and J. Stratakos, "A novel geodetic engineering method for
higher than the improved Stanley method. Table 2 shows that accurate and automated road/railway centerline geometry extraction
based on the bearing diagram and fractal behavior," IEEE transactions
our method is superior to the Stanley method in terms of on intelligent transportation systems, vol. 13, 2012, pp. 115-126.
lateral errors. [12] H. Park and J.-H. Lee, "B-spline curve fitting based on adaptive curve
refinement using dominant points," Computer-Aided Design, vol. 39,
VI. CONCLUSIONS 2007, pp. 439-451.
This paper reviewed the construction of the road map first,
then discussed the predictive model of curve path tracking in
detail, emphasizing three main problems, namely, roadside
acquisition, path interpolation and improved the Stanley
method based on the preview point.
Two different paths were considered to test the adaptation
of the system. The experimental results showed that the

408