A Much Advanced and Efficient Lane Detection Algorithm for Intelligent Highwa...
REU spring 2016
1. Path Following Control for Experimental Used Car-like Robots†
Zhenyuan Yuan1
Abstract— This paper presents a method of path following for
a car-like robot in an indoor setting using real-time feedback
from motion capture system. In particular, we implement a
modified version of Proportional Control for path following.
The major difficulty of the problem is due to the car’s non-
holonomic motion, which makes path following very difficult
due to the differential constraint so that the car cannot go
sideway. Path following is eventually realized by taking a linear
combination of the proportional gains over cross-track error
and heading error. The method has been experimentally tested
on a laboratory-scale car-like robot in an indoor environment.
The state feedback for the car is provided by a high-fidelity real-
time motion capture system which can provide high-frequency
feedback on the position, velocity as well orientation of the car.
The finally tuned controlled is tested on several paths (e.g.,
straight paths, paths composed of two parallel lines to simulate
the urban driving scenario of lane switching, etc.).
I. INTRODUCTION
Research on motion planning algorithm is experiencing a
tremendous growth recently in the field of autonomous cars.
These motion planning algorithms, such as Rapidly exploring
Random Tree (RRT) in [1] and the multi-robot motion
algorithm in [2], are planning algorithm that determines the
output according to the state of the robot. After the path
in generated, the vehicles need to follow the designated path
through a close loop so that the whole autonomy is achieved.
Path following for car-like robots becomes a complicated
issue because of the car’s non-holonomic motion, and thus
it is usually implemented through rather complicated al-
gorithm, in which the heavy computation requires, even
though provides accurate regulation, slows down the speed
of processing as well as requires high-fidelity on-board
hardware setup. In our lab, motion planning algorithm is
also the topic under research, and we need to verify the
algorithm through experiments on car-like robots by path
following. In order to keep the experiments cost-effective
while keeping the dynamics of cars, we use 1/10 Radio-
Controlled (RC) car as experimental-used car-like robot and
aims to keeps the cost of other setup as low as possible
so that the platform can be replicated easily for future use.
Thus in this paper, a computational-ease steering controller
is proposed to implement on car-like robots so that the
autonomous platform is kept at low cost but retains good
performance in following the path.
A lot of path following control researches are done in
theory but rarely implemented in experiment. Breaking the
†This work is supported by Networked Robotic Systems Lab from
Department of Mechanical and Nuclear Engineering, University Park.
1Zhenyuan Yuan is with department of Electrical Engineering,
The Pennsylvania State University, University Park, PA. 16802 USA.
zqy5086@psu.edu
path needed to follow into points and computing outputs
for the car with respect to the closest point as in [3] and
[4] theoretically fit our laboratory environment very well,
but the theory had only been simulated in software, and
there are a lot of realistic conditions, such as transmission
delay, computation capacity of the on board processor and
the steering accuracy, had not been taken into account.
Existing path following algorithm like [5], even though is
proved to be valid, is too computationally heavy, which
is unfeasible to be implemented on the microprocessor as
Arduino Uno on the car-like robots in our lab. Algorithm
developed in the experiment of car parking control using a
path controller in [6] seems to be a solution to the problem;
however, this algorithm is an adoption of control algorithm
for a two wheeled mobile robot. It is applicable on car-like
robots for motions on curvature such as parking, but when
it turns to urban driving scenarios such as lane following
switching and right angle turning where the non-holonomic
motion constraint becomes effective, the controller goes
highly unstable as it will be shown in the experiment section.
PID (Proportional-Integral-Derivative) controller was first
introduced in 1936, when Nicolas Minorsky was designing
a control algorithm for the automated steering on the ships
to avoid the disturbance force from distracting the ship from
the desired course [7]. As shown in Fig. 1, PID controller
implement proportional, integral and derivative operations
on an error between the output and the desired input so
that the output can be maintained at the desire level. PID
controller is now usually used in feedback loop and acts as
a method of regulation, such as regulating temperature and
pressure [8]. Similarly, PID controller is suitable for real-
time speed control of electric motors as demonstrated in [9]
and [10]. In [11], PID controller is used in steering control
for car following taking the feedback as relative position
with respect to the leading car. Similarly, PID controller
can also be used to address heading error between the
car and the reference path as well as the cross-track error
so that the computation is much relieved compared to the
transformations in [5].
In this paper, a path is given and a modification of PID
controller is presented as a method of path following for
car-like robots. The general function for PID controller is as
in Fig. 1. But for this project,instead of taking one source
of error, the new version of the controller, inspiring from
the theory in [3], takes a linear combination of proportional
control on two sources of error, heading error and cross-
track error, while remaining economical in computation. In
the rest of the paper, hardware setup and the function of
controller are explained in section II. The non-holonomic
2. Fig. 1.
Fig. 2. PID controller
system is analyzed in section III. In section IV, simulations
of different designs of the controller are displayed. In section
V, experimental validation of the successful controller is
presented, and conclusion is drawn in section VI.
II. HARDWARE SETUP AND THE CONTROLLER
An overview of the project is shown in Fig. 3. The car
used in the project is a 1/10 EXCEED RC 2.4GHz Electric
Infinitive RTR Off Road Truck. The parts original from the
car used in the project is the motor and the steering servo.
The Electronic Speed Control (ESC) is replaced by an off-
the-shelf motor driver. A platform is built upon the car in
order to hold the designed hardwares to satisfy the project
objective. As shown in Fig. 4, the stacks of the boards make
up the center of control for the car. Beginning from the top of
the layer is the circuit for steering control that is connected
to the steering servo. The steering servo is controlled by the
Pulse-Width-Modulation (PWM) signal sent from Arduino,
which is the control signal of the controller so that the output
of the steering can keep the car on the path. The second
layer is the motor driver. The third layer is equipped with
Bluetooth for transmission. The bottom one is the Arduino,
which is the microcontroller of the system that actuate the
car.
The proposed controller has its block diagram shown in
Fig. 2. The designated path specifies the line location as well
as the direction the car should go. Taking these two setpoints
as input, regulation is implemented on the cross-track error
and the heading error, which are the distance between the car
and the path and the difference between the path direction
and the car’s orientation, respectively. Then different gains
are implemented on the two errors to output an appropriate
PWM signal to the steering servo for steering adjustment so
that the can can converge to the designated path.
III. NON-HOLONOMIC MOTION CONSTRAINT
Throughout the whole control problem, the inherent non-
holonomic motion constraint of the car-like robot is proved
to be the most challenging part. The configuration of car-like
robots can be modeled as in Fig. 5, where the rear wheels are
fixed and always pointing to the moving direction. Since the
car-like robot in the experiment is usually run at low speed
as 50 cm/s, the assumptions of non-slipping motion and non-
sideway motion hold very well in the model but become the
most difficult part to overcome in order to converge to the
designated path. As explained in [1], using the same notation
in Fig. 5 the motion of the car is described by the following
equations:
˙x = us cosθ (1a)
˙y = us sinθ (1b)
˙θ =
us
L
tanuφ (1c)
where x,y are the coordinates in the imaginary plane and
θ is polar angle of the plane. These equations show that the
motion of the car is related to the speed and the steering
angle as well as limited by the maximum curvature when
it is turning. Thus, the error of the car in path following is
described by two parameters, the distance from the desired
path and orientation error. This makes it difficult when
applying the traditional PID controller because it is usually
implement on one dimensional error, whereas the error in
this problem is two dimensional. It becomes even more
difficult when the real world disturbance factors such as
communication delay, differential gearing and surface fiction
are introduced into the system.
IV. SIMULATION
The purposed algorithm, taking the combination of cross-
track error and the heading error as the input of the
controller, is simulated in MATLAB before being carried
out in experiment. The main purposes of simulations are
mimicking the behavior of the non-holonomic motion at
different situation, tuning and foreshadowing the gains in
actual experiment. As shown in Fig. 6, the simulations are
run on a designated square in the same dimension of the
testbed, the car is represented by a point but retaining the
car features such as car length and maximum steering angl
while its motion is simulated by the equations (1) and the
designated path is assigned by the four corner points, denoted
Fig. 3. Project overview
3. Fig. 4. Layers of circuit boards on top of the car platform
Fig. 5. Simple Car Model (source: [1])
as (x1,y1),(x2,y2),(x3,y3),(x4,y4), and the equation of the
line is followed by line equation
Ax+By+C = 0
in which
A = yj −yj+1
B = xj+1 −xj
C = xjyj+1 −xj+1yj
j is incremented when the car is getting close to the end point
of the line, in this case is 450 mm, to provide a smooth corner
turning. The calculation of the errors and the drawbacks of
using only either one of them are explained in the following
subsection.
A. CROSS-TRACK ERROR CALCULATION
Cross-track error was used as an input for PID controller,
which is the distance between the car and the designated
path, and the PID controller is adjusting output in order to
minimize this error. The cross-track error e(t)d related to
time is directly calculated as point-to-line distance as
e(t)d = (−1)sign(Ax+by+c)
|Ax+by+c|
√
A2 +B2
Fig. 6. Designated Trajectory in Simulation
Fig. 7. Simulation on cross-track error with optimal condition
in which, x and y are the corresponding coordinates of the
car, and the sign function is used to determined the side the
steering should turn. The steering servo functions within the
PWM signal input range from 1000 to 2000: at 1500 PWM
signal input, the steering stays straight, and the steering turns
right when the input is higher than 1500 and turns left when
it is lower.
The condition is very limited in order for the car to behave
well by taking only the cross-track error as the feedback input
for PID controller. This algorithm only works when the car
is running at low speed under minimal communication delay
and small initial offset. Successful simulation is shown in
Fig. 7, when the car speed is set at 500 mm/s, communication
delay as low as 0.05 s and is started off very close to the
designated path. When the car is run at higher or greater
communication delay in the simulation, the system becomes
as unstable as shown in Fig.8.
B. HEADING ERROR CALCULATION
The major drawback of using only cross-track error for
regulation is that it does not take the orientation into account,
which results in the misalignment between the car’s heading
and the path’s direction even though the car is right on the
track, and therefore the error is not reduced even though
the cross-track error is zero. The heading error is calculated
through the comparison between the heading direction and
the direction of the designated line. The PID controller taking
this error as input is trying to force the car to head in the same
direction as the designated path so the oscillatory motion can
4. Fig. 8. Simulation on cross-track error with nonideal condition
Fig. 9. Simulation on heading error
be avoided. The heading error e(t)h is calculated as
e(t)h = (−1)sign(θ −γ)(1−cos(θ −γ))
where θ is the heading angle of the car in the polar
coordinate system, and γ is the angle of the path. The sign
is used for indicating the side the steering should turn, the
same principle as in last subsection. The term 1−cos(θ −γ)
is for limiting the difference within the range -1 to 1 so that
the tunning process is easier.
As shown in Fig. 9, with only heading error for regulation,
the car cannot be turned back into the line once it is off since
the algorithm only makes the car goes in the same direction
as the path.
C. COMBINATION OF CROSS-TRACK ERROR AND
HEADING ERROR
Obviously, regulation using cross-track error and regu-
lation using heading error are complement to each other.
When merging the two error calculation methods together,
the drawbacks of both methods are complemented by the
advantages of the other. As it turns out, when taking the
linear combination of the two types of error, the car is
regulated to follow the direction of the path as a result of
the heading error, and it tends to converge back to the line
when it is off as the cross-track error is in effect. Simulation
result can seen in Fig.10, and through the simulation, the
new algorithm is more tolerable to the range of speed and
communication delay. The error of the combination e(t) is
calculated as
Fig. 10. Simulation taking the combinatio of both cross-track error and
heading error
e(t) = Ae(t)d +Be(t)h
where A and B are the gains for cross-track error and
heading error, respectively. The gain of the heading error is
set as large as 1970 to keep the car move in a proper direction
while the gain of the cross-track error is set as low as 0.028
so that the car can converge back to the line when it is off
but the unstable behavior as shown in Fig. 8 is negligible.
V. EXPERIMENT
The proposed algorithm is tested in experiments equipped
with indoor motion capture system, Vicon, which provides
position information that can be translated to Cartesian
coordinate to MATLAB[12], as shown in Fig. 11. Through
the same Cartesian coordinate system, designated path in
located through code in MATLAB, which also communicate
position information with Vicon. As shown in the lane-
switching scenario in Fig. 15, which is a good representation
for path following, the path designed for the car to complete
lane-switching motion is in Fig. 12 by MATLAB. The car,
running at low speed, started from the bottom right corner
in Fig. 13 with offset by purpose. As captured by Vicon,
the car trajectories are shown in Fig. 13 and Fig. 14. The
dots that are out of the continuous car path is due to the
noise of Vicon. Fig. 14 is the resulted plot when using only
cross-track error as an input for the controller; it is highly
unstable because of the non-holonomic motion constrain. On
the other hand, the algorithm using the combination of cross-
track error and heading error gives rise to stable control and
smooth performance in Fig. 13. Because of the transmission
defect, the car cannot fully converge to the path even though
the command is instructing to on the left top corner in
Fig. 13, but compared the scale in the figure with the car
dimension (39*30*14 cm3), the deviation is negligible. In the
end, the gains tuned for the controller in actual experiment
are 3050 for heading error and 0.225 for cross-track error.
VI. CONCLUSIONS
The main contribution of this paper is an steering control
algorithm for autonomous car path following in urban sce-
nario. The algorithm was validated in experiments, where the
desired performance is satisfying given the non-holonomic
5. Fig. 11. Vicon motion capture system
Fig. 12. Designated tracjectory in experiment
Fig. 13. Experiment in lane-swtiching scenario
Fig. 14. Car behavior using only cross-track error as input
Fig. 15. Lane-switching scenario
motion constrained. Experiments are held with the car start-
ing off close to the designated path without opposite heading
due to the space limitation as well as the desire to have the
car converge quickly to the path, but these assumptions hold
well since motion planning algorithm would generate path
starting from the car’s current position and heading. This
algorithm is proved to be valid for path following section
under motion planning , so in the coming research on-
board computer (NVIDIA Jetson TK-1) will be embedded
on the car-like robots to execute on-board motion planning
as well as minimize communication defect and integrate with
sensors. In this way, the car, according to the motion planning
algorithm developed in the lab, will be able to generate and
follow a path on its own for a given task, achieving the full
autonomy.
ACKNOWLEDGMENT
This work is supported by Networked Robotic Systems
Lab from Department of Mechanical and Nuclear Engineer-
ing, supported by Dr. Asok Ray. The author would also thank
for the supervision from Dr. Minghui Zhu and the assistance
and guidance provided by graduate student, Nurali Virani
and Devesh Jha.
REFERENCES
[1] S. M. LaValle, Planning algorithms. Cambridge university press, 2006.
[2] D. K. Jha, M. Zhu, and A. Ray, “Game theoretic controller synthesis
for multi-robot motion planning-part ii: Policy-based algorithms,”
IFAC-PapersOnLine, vol. 48, no. 22, pp. 168–173, 2015.
[3] R. DeSantis, “Path-tracking for car-like robots with single and double
steering,” Vehicular Technology, IEEE Transactions on, vol. 44, no. 2,
pp. 366–377, 1995.
6. [4] P. Encarnac¸ao and A. Pascoal, “Combined trajectory tracking and path
following: an application to the coordinated control of autonomous
marine craft,” in Decision and Control, 2001. Proceedings of the 40th
IEEE Conference on, vol. 1, pp. 964–969, IEEE, 2001.
[5] C. Samson, “Control of chained systems application to path following
and time-varying point-stabilization of mobile robots,” Automatic
Control, IEEE Transactions on, vol. 40, no. 1, pp. 64–77, 1995.
[6] K. Lee, D. Kim, W. Chung, H. W. Chang, and P. Yoon, “Car parking
control using a trajectory tracking controller,” in SICE-ICASE, 2006.
International Joint Conference, pp. 2058–2063, IEEE, 2006.
[7] N. Minorsky, “Steering of ships,” 1984.
[8] J. Bechhoefer, “Feedback for physicists: A tutorial essay on control,”
Reviews of Modern Physics, vol. 77, no. 3, p. 783, 2005.
[9] H.-B. Shin and J.-G. Park, “Anti-windup pid controller with integral
state predictor for variable-speed motor drives,” Industrial Electronics,
IEEE Transactions on, vol. 59, no. 3, pp. 1509–1516, 2012.
[10] J. Tang, “Pid controller using the tms320c31 dsk with online parameter
adjustment for real-time dc motor speed and position control,” in In-
dustrial Electronics, 2001. Proceedings. ISIE 2001. IEEE International
Symposium on, vol. 2, pp. 786–791, IEEE, 2001.
[11] T. Sangyam, P. Laohapiengsak, W. Chongcharoen, and
I. Nilkhamhang, “Path tracking of uav using self-tuning pid
controller based on fuzzy logic,” in SICE Annual Conference 2010,
Proceedings of, pp. 1265–1269, IEEE, 2010.
[12] Vicon Tracker User Guide, May 2015.