Trajectory Based Autonomous Vehicle Following using a ... · This paper outlines a project to improve an existing robotic driver to allow it to follow another vehicle. The robotic

Trajectory Based Autonomous Vehicle Followingusing a Robotic Driver

Martin SpencerUniversity of Auckland,

New [email protected]

Daniel JonesUniversity of Auckland,

New [email protected]

Martin KraehlingHamburg University of Technology,

[email protected]

Dr. Karl StolDepartment of Mechanical Engineering,

University of [email protected]

Abstract

An autonomous vehicle following system in-cluding control approaches is presented in thispaper. An existing robotic driver is used tocontrol a standard passenger vehicle such thatno modifications to the car are necessary. Onlyinformation about the relative position of thelead vehicle and the motion of the following ve-hicle is required, and methods are presented toconstruct a reference trajectory to enable ac-curate following. A laser scanner is used todetect the lead vehicle and the following vehi-cle’s ego-motion is sensed using an IMU andwheel encoder. An algorithm was developedand tested to locate the lead vehicle with RMSposition and orientation errors of 65mm and5.8◦ respectively. Several trajectory-based lat-eral controllers were tested in simulation andthen experimentally, with the best controllerhaving an RMS lateral deviation of 37cm fromthe path of the lead vehicle. A new trajectory-based spacing controller was tested in simula-tion which allows the following vehicle to repro-duce the velocity of the lead vehicle.

1 Introduction

Autonomous vehicles offer many benefits by performingmonotonous and time consuming tasks in the place ofhumans, reducing costs and reducing the risk to humanlife in dangerous environments. They have many appli-cations such as in automated highways, vehicle followingcruise control systems, military convoys and agriculture.

This paper outlines a project to improve an existingrobotic driver to allow it to follow another vehicle. Therobotic driver was originally constructed at the Univer-sity of Auckland to enter the 2005 DARPA Grand Chal-lenge and has since been used in dynamometer research[Namik et al., 2006]. Recently, Wong et al. [2008] addedsensors to allow detection of the lead vehicle and motionof the following vehicle, and simulated various vehicleidentification and lateral control strategies. The roboticdriver was designed as a retrofit solution and is able tofit in most standard passenger cars (Figure 1), with nomodifications to the vehicle required. The steering wheeland pedals are actuated by the robot in the same way asa human driver. As a retrofit solution, the robotic driverpresents an inexpensive and rapid method to automatemost vehicles. The current robot design requires a vehi-cle with automatic transmission and power steering.

Figure 1: The robotic driver fitted in a vehicle

Australasian Conference on Robotics and Automation (ACRA), December 2-4, 2009, Sydney, Australia

The aim of this project is to allow the robotic driverto safely and autonomously follow the trajectory of alead vehicle. It is assumed that the task can be decou-pled into lateral control, to enable the following vehicleto adhere to the path taken by the lead vehicle, and lon-gitudinal control, to reproduce the velocity of the leadvehicle with a constant time headway. This approachhas been used by many previous authors, such as Sotelo[2003]. No information about the absolute position ofthe vehicle, such as that provided by GPS, landmarks orexternal communication is used: vehicle following is ap-proached in a purely relative sense, by making use onlyof the relative position of the lead car and the motion ofthe following car to construct a series of waypoints rep-resenting the path of the lead vehicle over time, in a sim-ilar way to the approach in Fujioka and Omae [1998] andGehrig and Stein [1998]. Both of these papers demon-strated that following the trajectory of the lead vehiclerather than the vehicle itself allows more accurate track-ing.

Wong et al. [2008] developed and tested three meth-ods for determining the relative position of the lead ve-hicle, which use reflections from the rear and side of thecar with no modification necessary. A new algorithm ispresented in this paper and compared with these threemethods.

In this paper, several algorithms for following the tra-jectory of the lead vehicle are tested in simulation. Thebicycle approximation is used to model the steering ofthe vehicle, which has been used by many previous au-thors with good results, for example Sotelo [2003] andLinderoth et al. [2008]. A great deal of literature hasbeen published on methods for following a trajectory.Fujioka and Omae [1998] and Gehrig and Stein [1998]

discuss algorithms for path tracking which involve select-ing a lookahead point on the path, which are relevant tothis work. Other lateral controllers include those devel-oped for the DARPA Grand Challenge such as those byThrun and others [2007] and Linderoth et al. [2008].

Much research into longitudinal control for vehicle fol-lowing has been carried out in the context of vehicleplatooning and automated highways. These often aimat string stability of multiple vehicles following one leadcar in a row or optimising traffic flow. Different constantand variable time headway approaches were successfullyused by Chien et al. [1994], Swaroop et al. [2001] andZhang and Ioannou [2005]. A PID controller was used byChien, Ioannou and Lai [1994] to maintain the desiredspacing. A nonlinear controller is presented by Alain Gi-rault [2003] which does not take a spacing error, but aratio of actual and desired distance. The present workconsiders only one lead vehicle.

Object avoidance is not considered in this project andit is assumed that the lead vehicle is following a safe path.

Real-world tests are conducted only on flat terrain andin clear weather. The speed of both vehicles is limited to30km/h and the target headway time is 3.0s, chosen toallow a safe margin of error during preliminary testing.Due to the limited angular range of the laser scanner,the radius of curvature of the lead vehicle is constrainedto be not less than 18m. The lead vehicle is a standardpassenger car.

The remainder of this paper is organised as follows:Section 2 outlines the robotic driver system includinghardware and software, Section 3 describes the methodsused to determine the relative position of the lead ve-hicle and to create a trajectory, Section 4 describes thelateral controllers that were implemented and tested insimulation, and in Section 5 longitudinal control of thevehicle is explained. Concluding remarks are given inSection 6.

2 Robotic Driver System

Hardware The hardware of the system has been ex-plained by Wong et al. [2008]; a brief summary is pre-sented here.

All electrical power to the robotic driver is providedby the car battery of the vehicle. The robotic driverhas two feet to actuate the brake and accelerator pedals.They are connected via a rack and pinion mechanism,enabling a single DC motor to actuate both pedals. Theresponse time of the pedal actuator has been shown tobe similar to that of a human driver [Wong et al., 2008].

The vehicle steering wheel is turned using a frictionwheel connected to a DC motor. The response of thesteering actuator is significantly slower than that of ahuman driver; during testing it was found to take up to9s to turn from full lock to full lock. This has implica-tions for the lateral controller.

Motion Mind motor controller boards from SolutionsCubed LLC are used to control the pedal and steeringactuators. On-board PID controllers regulate the mo-tor positions using feedback from rotary encoders. Thedesired setpoints are received via serial connection withthe control computer.

An emergency stop mechanism is implemented usingpneumatics, enabling the entire pedal drive mechanismto be forced downwards on to the brake.

A joystick is used to control the robot manually duringinitial testing; this allows the vehicle to be driven by ahuman with the robot in the driver’s seat for collectingperformance data and checking the system.

Sensors A Sick LD MRS laser scanner (a prototypeversion of the Ibeo LUX) provides information aboutthe location of the lead vehicle. The laser scanner hasa working azimuth range of 94◦ and up to 200m, whichis more than adequate for the conditions in this project.

2


Additionally, the Sick LD MRS scans four horizontal lay-ers independently, making the system robust to smallchanges in the relative height of the lead vehicle, andregisters multiple laser beam echoes for each target, en-abling the laser scanner to function from behind the ve-hicle windscreen.

An Xsens Mti-G Inertial Measurement Unit (IMU) isused to measure the yaw rate of the following vehicle.This IMU includes an integrated GPS with a Kalmanfilter. For this project the GPS was not used because it isonly accurate to 5m which was not considered sufficientfor vehicle following, and the aim of the project was toachieve vehicle following in a purely relative sense.

A rotary encoder measures the velocity of the followingvehicle, providing greater accuracy than the IMU.

Software and Control Figure 2 gives an overview ofthe control structure of the system. The open sourcePlayer/Stage software [Vaughan et al., 2009] was usedto interface with the hardware and for performing simu-lations. Player handles all low-level communication withthe hardware and provides a standard set of interfaces forclient programs to connect to. In previous work [Wonget al., 2008], several Player drivers were developed tocommunicate with each of the hardware components ofthe robotic driver.

IMU

Wheel Encoder

LaserScanner

Robotic Driver

Player Interfaces

Player Drivers

Lead VehicleIdentification

Longitudinal and LateralControl

TrajectoryDeterminationClient

Program

Figure 2: Control overview

The client program for high-level control of the roboticdriver was written in C++ using the client libraries in-cluded with Player. There are three primary indepen-dent modules to the program: the main control loop,the data logger and the graphical user interface (GUI).The main control loop reads all sensor data from the

Player server, performs control calculations and sendscommands to the actuators; it was written as a finitestate machine, where each state is a particular mode ofoperation. This structure allows for well-defined condi-tions for changing between any two states. The loopoperates at a control frequency of 10Hz which is slightlyslower than the rate at which the laser scanner, the slow-est sensor, updates (12.5Hz). The data logger writes allimportant information to a log file, and the GUI allowsfor easy interaction with the program.

3 Lead Vehicle Tracking

Tracking of the lead vehicle is divided into two tasks:determining the position and orientation of the lead ve-hicle relative to the following vehicle, and the integrationof this information with the following vehicle’s velocityand yaw rate to calculate the trajectory followed by thelead vehicle.

Determination of Lead Vehicle’s Relative Posi-tion The lead vehicle’s relative position and orienta-tion are calculated from the laser scanner data. Gener-ally the lead vehicle’s point cloud can be simplified toa single line segment, or two adjacent line segments atright angles, as shown in Figure 3. For this task, we de-fine the lead vehicle’s relative position to be the centre ofthe lead vehicle’s rear axle, relative to the laser scanner.

yrel

xrel

Θrel

Follower

Leader

yf

l

x{O}

{F}

Figure 3: Lead vehicle identification schematic

Wong et al. [2008] investigated three algorithms todetermine lead vehicle position: Box Classification (us-ing least-mean-squares to fit a line segment to the datapoints representing the rear side of the lead vehicle),Hough Transform, and Hough Classification (a combina-tion of both techniques). However, it was not concludedwhich was most accurate.

3


There are several disadvantages with these techniques.All three are dependent on the previous lead vehicle’sposition, which makes them susceptible to temporarylaser data dropouts and require careful consideration ofinitial values. These algorithms do not detect when in-accurate results are produced, so the system does notrecognise when it is following an incorrect trajectory. Fi-nally, these algorithms have a tendency to produce largeorientation errors.

A new algorithm was tested which eliminates thesedisadvantages, and reduces the overall position and ori-entation error. First, the point cloud data is passed intoan image kernel filter, locating possible lead vehicle po-sitions. The lead vehicle will be surrounded by emptyspace, therefore a 7x7 Laplacian image kernel with neg-ative outer coefficients was selected to accentuate regionswith high concentrations of data points surrounded byempty spaces. Image kernel filtering was implemented asan alternative method of excluding irrelevant laser data,eliminating the dependency on the previous position ofthe lead vehicle.

For each region output by the image kernel filter, anangular histogram of the nearby laser points is created.The angular histogram will generally have two peaks 90◦

apart, representing the back and sides of the vehicle.These two peaks are combined to find a peak angle rang-ing from 0◦ to 90◦. The true vehicle orientation will beeither at this angle, or shifted by 90◦.

Finally, a rectangle the size of the lead vehicle is placedto enclose the laser points representing the vehicle, de-termining the correct lead vehicle’s orientation and po-sition. In addition, there are physical constraints onthe variation of vehicle position and orientation betweenlaser scanner frames. These constraints will force im-possible vehicle positions to be ignored, unless they arepersistent across multiple frames, in which case we as-sume the lead vehicle’s previous position was incorrect.

A real-world driving test was conducted to quantifythe accuracy of all four algorithms. A sample view andlaser scan are shown in Figure 4. For this test the twovehicles were driven manually in oval and figure-eightpatterns without sharp turns, ensuring the lead vehiclewas within range of the laser scanner at all times. Thepath was chosen so that other vehicles and obstacles suchas trees were too far away to interfere with vehicle iden-tification. Some authors [White and Tomizuka, 2002;Fujioka and Omae, 1998] have used reflective markerson the rear of the lead car to enable easy detection, butfor this test only a flat white board was attached to therear of the lead car to ensure sufficient reflections.

A ground-truth analysis was conducted using 65 of thelaser scanner frames collected from the real-world driv-ing test outlined above. These frames were intention-ally selected to ensure the first three algorithms would

Figure 4: View from following vehicle (top) and corre-sponding laser data visualisation (bottom) from a real-world test

not select entirely incorrect vehicle positions. For eachframe, the ground-truth vehicle position was determinedby hand by looking at the laser range data, and com-pared against the output of each algorithm. The errorsin position and orientation are summarised in Table 1below.

RMS Error

Algorithm Position (mm) Orientation (◦)

Box Class. 71 20.5Hough Trans. 77 7.9Hough Class. 94 13.5New Alg. 65 5.8

Table 1: RMS error in measured position and orientationof lead vehicle

The new algorithm has increased accuracy comparedto the previous three. Note that these results only quan-tify accuracy in frames which produce reasonable resultsin all four algorithms; this test does not quantify therobustness of the new algorithm to laser data dropouts,incorrect initial values etc. The robustness and accu-racy of the new algorithm makes it a viable method fordetermining lead vehicle position.

There are some situations in which all four techniqueswould fail. It is assumed that the lead vehicle orientationwill never exceed the range 90◦ to −90◦. Additionally, it

4


is assumed that there will be a single vehicle in the laserview range, so if another vehicle appears the results willbe unpredictable. Possible improvements to these tech-niques include detection of multiple vehicles and otherobjects such as obstacles and pedestrians.

Determination of Lead Vehicle Trajectory Thecalculated relative lead vehicle positions are used to cre-ate a trajectory over time. This is achieved by trans-forming each position into an arbitrary ground-basedreference frame, where the position of the following ve-hicle in this reference frame is updated each iteration bythe measured velocity and yaw rate, as in the approachtaken by Gehrig and Stein [1998]. In this way a series ofwaypoints are created which can then be followed.

We write the position of the following car as(xf , yf , θf ), and the lead car as (xl, yl, θl). In the rel-ative frame the x axis is forwards, the y axis is to theleft and θ (orientation) is positive anticlockwise. Pointsin the following frame are denoted with a superscript F ,while points in the ground-based frame have a super-script O. These reference frames are shown in Figure3. For each iteration k, the estimated position of thefollowing vehicle is updated as follows:

ωf =θf,k + θf,k−1

2, vf =

vf,k + vf,k−1

2,

Oxf,kOyf,kOθf,k

=

Oxf,k−1Oyf,k−1Oθf,k−1

+T

cos(Oθf,k + ωfT2 ) 0

sin(Oθf,k + ωfT2 ) 0

0 1

[vfωf

](1)

where θf is the yaw rate and vf is the forward velocity ofthe following vehicle, and T is the sampling time, 0.1s.Next, the lead car’s current position is transformed tothe ground-based reference frame and added to the listof waypoints:Oxl,k

Oyl,kOθl,k

=

Oxf,kOyf,kOθf,k

+M

Fxl,kF yl,kF θl,k

, (2)

where

M =

cos(Oθf,k) − sin(Oθf,k) 0

sin(Oθf,k) cos(Oθf,k) 00 0 1

. (3)

Any waypoint in the list can be transformed back intothe following frame by reversing equation (2).

The resulting list of waypoints, ignoring the orienta-tions,

X =

Fxl,1

F yl,1...

...Fxl,n

F yl,n

(4)

is then smoothed by fitting a two dimensional third or-der parametric polynomial vector function to all n way-points, using a standard least-squares approach. Thepath of the lead vehicle is then given by

s(t) =

[Fx(t)

Fy(t)

]=

[αTx

αTy

]·

t3

t2

t1

(5)

where αx, αy ∈ R4 are the vectors of path coefficients.For t ∈ [t1, tn] the polynomial returns an interpolationof the true path of the lead vehicle, and times outsidethis interval are not used. At this stage, the closest pointon the polynomial to the origin, P0 = s(t0), is found byiterating over values in the range [t1, tn]. All waypointsfor which t0 − ti > 4, i > 0 are then removed from thelist. This means that waypoints are kept for 4s after thefollowing vehicle has driven past them.

The velocity and acceleration at a point on the leadvehicle’s trajectory are calculated as follows:

v(t) =

[Fx(t)

Fy(t)

]=

[αTx

αTy

]·

3t2

2t10

(6)

a(t) =

[Fx(t)

Fy(t)

]=

[αTx

αTy

]·

6t200

(7)

The velocity vector is always tangent to the path,whereas the acceleration vector may have a componentin the longitudinal as well as lateral directions:

along(t) =v · vT

vT · v· a(t) (8)

alat(t) = a(t)− along(t) (9)

The curvature of the path (the reciprocal of the turn ra-dius, R) is a scalar, given by the normalized inner prod-uct

κ(t) =1

R(t)=

aT (t) · alat(t)||v(t)|| · ||a(t)|| · ||alat(t)||

. (10)

This method of smoothing the waypoints offers severaladvantages over simply using the raw waypoints. A con-tinuous time solution for the path that the lead vehiclefollows is obtained, allowing position, velocity and ac-celeration to be easily calculated at any time, and al-lowing handling of the situation where the laser scanneris unable to see the lead vehicle for short periods. Themethod also reduces noise in the waypoints. The maindisadvantage is a loss of accuracy in some cases due tothe smoothing, however we have observed that this is

5


negligible for the turns permitted by the scope of thispaper. The polynomial fit also ignores the estimatedorientation of the lead car; including this informationmay provide an advantage and could be the focus of fu-ture work. An example from a real-world test is shownin Figure 5.

−4 −2 0 2 4 6 8 10−2

0

2

4

Forward Distance (m)

Late

ral D

ista

nce

(m)

Figure 5: Sample frame, showing waypoints and the es-timated path

A test was carried out to estimate how accurately thewaypoints represent the true path travelled by the leadvehicle. Two cars were driven by human drivers at a lowspeed of approximately 10km/h along a straight line, ata distance of 20 ± 0.5m (Figure 6). This was repeatedwith the following car directly behind the lead car, andwith the following car driving with a ±6m lateral offsetfrom the lead car. The positions of the waypoints asthe following car drove past them were then comparedwith the true path. Over 9 tests it was found that theabsolute difference was up to 1.2m. This is more thanhalf the width of a car, so this level of accuracy was notconsidered acceptable. Methods for achieving a targetaccuracy of ±0.5m are being investigated, such as devel-oping a mounting system to allow the laser scanner tobe more accurately aligned with the vehicle.

Figure 6: Straight line test of trajectory estimation

4 Lateral Control

The method of creating waypoints transforms the prob-lem from vehicle following into that of following a pre-defined trajectory. Several algorithms were investigated

and tested in simulation for this. The algorithms all pro-duce a desired curvature, which is the input to the steer-ing controller. The notation used to describe points onthe path is shown in Figure 7. P0 is the position vectorto the closest point on the trajectory, and de = ||Plat||.Plat is the lookahead point, and is the input to the lat-eral controllers. Wit et al. [2004] describe the issues sur-rounding the choice of this point: increasing the looka-head distance tends to increase stability but also causethe lateral controller to cut corners. In this work, thelookahead point is calculated as s(t0 + th), where th isthe lookahead time.

The bicycle steering model is used. The centre of therear axle of the vehicle is used as the control point andside slip at the angles is ignored.

Φ

l

des(t)

plat

plong

xFyF

p0=s(t0)

RlatR0

Figure 7: Coordinate system and bicycle steering

Five lateral controllers were tested in simulation.1. Pure Pursuit [Coulter, 1992], a widely used method

in which a circle is fitted passing through the vehicle andPlat:

κctrl =2 · Platy

P 2latx

+ P 2laty

(11)

2. A simplified version of the third algorithm de-scribed by Fujioka and Omae [1998] was tested. Weneglect both side slip and acceleration of the followingvehicle, so the algorithm reduces to Pure Pursuit with aterm proportional to the current tracking error added tothe control output:

κctrl =2 · Platy

P 2latx

+ P 2laty

+K1 · de (12)

3. Vector Pursuit [Wit et al., 2004]

4. An algorithm which calculates the curvature of thetrajectory at the lookahead point, κPlat

and adds a termproportional to the current tracking error to this to ob-tain a desired curvature:

κctrl = κPlat+K3 · de (13)

5. A new algorithm similar to the third method, ex-cept that a predictor is used to compensate for the time

6


delay in the steering actuator response. The predictoris an estimated model of the vehicle which includes thesteering dynamics with no time delay, so that the currentposition of the model is an estimate of the true vehicleposition 0.3s in the future. The desired curvature isthen calculated in the same manner as (13), using thepredicted vehicle position. It was found that adding aderivative term to the control output improved the per-formance for all four controllers. For the first three al-gorithms equation (14) was used, and for the other twoequation (15) was used. The value of 0.8 for the gainscheduling in equation (15) was determined by tuningthe controllers in simulation at different speeds, with theother gains constant, and fitting a curve to the optimalKd values.

κdes = κctrl +Kd ·Platy,t − Platy,t−1

T(14)

κdes = κctrl +Kd

V 0.8·Platy,t − Platy,t−1

T(15)

Once a desired curvature has been obtained, com-mands must be sent to the steering actuator to achievethis curvature. A simple feedback loop, similar to theapproach taken by [Fujioka and Omae, 1998], is used forthis, whereby a desired change in steering wheel position(ST ) is calculated from the current error in curvature:

∆ST =Kst

V(κdes −

θ

V) (16)

The choice of gain Kst will affect the speed of re-sponse. When tuning controllers in simulation at dif-ferent speeds, it was found that the optimal Kst tendedto decrease in inverse proportion to the speed, so Kst isgain scheduled. The inverse relationship means an upperlimit must be imposed on ∆ST at low speeds. The resultof this method is that the relationship between steeringwheel angle and front wheel angle does not need to beknown exactly, so the controller can be easily adaptedto different vehicles.

Simulation Results The simulations carried out totest the lateral controllers included realistic parametersfor the steering dynamics. During drive-by-wire testswith the robot in a real vehicle, it was found that thesteering actuator response is well characterised by a firstorder model with a saturation on the rate of change, inseries with a pure time delay. The estimated parameterswere a time constant of 0.55s, a delay of 0.3s, and asaturation equivalent to 7.3s to turn the steering wheelfrom full lock to full lock. It was also found that therelationship between steering wheel position (measuredin degrees) and curvature is approximately linear acrossmost of the range, with a gain of 3.44×10−4m−1. Thesecharacteristics were used for the simulation.

Using the bicycle model of the vehicle a simple sim-ulation environment was written as a plugin driver forPlayer, updating at a rate of 50Hz. Speed and steeringangle commands are sent to the driver and the positionof the vehicle over time is calculated. Stage (the 2Drobot simulation environment for Player) was not usedfor this because the overhead associated with running afull Stage world meant that it was difficult to accuratelycontrol the update rate and to obtain repeatable results.

The model of the robot and vehicle was written asanother plugin driver for Player, which receives actuatorcommands from the client program and calculates thevehicle speed and steering angle.

Simulations were carried out over a variety of speedsfrom 5 to 30km/h and a headway time of 3.0s, with thelead vehicle following the path shown in Figure 8. In thefirst test, the velocity of the lead car was varied and thesystem parameters were unchanged. In the second test,the gain between vehicle curvature and steering actuatorposition was varied from half to twice the true value.In the third test the steering actuator time delay wasvaried from 0 to 0.6s. For the last two tests the leadcar’s velocity was constant at 20km/h.

0 50 100 150 200 250 300 350

0

50

100

xO (m)

yO

(m)

R25

R20

R35

R100

R50

Figure 8: Path followed by lead vehicle in simulation

The controller gains were tuned manually by findingoptimal values over several velocities. For the first threealgorithms th = 1.5s was found to give good results. Forthe curvature methods th = 0.3s was used.

5 10 15 20 25 300

0.05

0.1

0.15

0.2

Lead Vehicle Speed (km/h)

RM

S L

ater

al T

rack

ing

Err

or (

m) RMS Tracking Error with Varying Lead Vehicle Speed

Pure PursuitFujioka 3Vector PursuitCurvatureCurvPrediction

Figure 9: Simulation results with varying velocity

Figure 9 shows that all the controllers are stable acrossthe range of velocities considered in this paper, with

7


0 0.1 0.2 0.3 0.4 0.5 0.60

0.05

0.1

0.15

0.2

0.25

0.3

Steering Actuator Delay (s)

RMSLateralTrackingError(m) RMS Tracking Error with Varying Steering Actuator Delay


Nominalvalue

2 3 4 5 6 x 10−40

0.1

0.2

0.3

0.4

0.5

Gain Between Steering Actuator Position and Curvature (1/m)

RMSLateralTrackingError(m) RMS Tracking Error with Varying Vehicle Steering Gain


Nominalvalue

Figure 10: Simulation results, showing robustness to pa-rameter uncertainty

the curvature prediction method giving the best perfor-mance. As can be seen in Figure 10, the controllers showsimilar robustness to parameter uncertainty, except foractuator delays greater than 0.5s. However this is notexpected to occur.

Experimental Results Real-world tests were con-ducted to quantify the performance of the overall system.A figure eight path (Figure 11), with two circles of radius20m and 25m, was marked out with 25 reflectors. Thelateral error from the marked path of both the lead carand the following car were determined by placing videocameras above the towbar of each car, facing downwards,and continuously recording. The lateral deviation of thefollowing vehicle’s path from the lead vehicle’s path wasthen calculated as the difference between both vehicle’slateral error at each marker.

20m25m

52m1

2

3

45

6

7

8

20

21

2223

24

25

19

18

1716

15

14

13

12 11

10

9

Figure 11: Path used to test the lateral controller

A total of three tests were conducted. Each test con-sisted of two full laps around the figure eight starting andfinishing at marker #1. The lead vehicle was driven atapproximately 15km/h and the following vehicle’s speedwas controlled via joystick, with the aim of keeping a fol-lowing distance of roughly 20m. The inherent inaccuracyin this method may have affected the results. The threecontrollers tested were Pure Pursuit, Pure Pursuit witha lateral offset term (Fujioka 3), and the new CurvaturePrediction method.

The results of the tests are summarised in Table 2.The Curvature Prediction method gave the best resultsout of the three, in terms of both maximum and RMStracking error, which is consistent with the simulation.However, the tracking errors are significantly larger thanany seen in simulation and the controllers showed sometendency to oscillate about the path, which did not occurin simulation. This could be due to unmodelled effectssuch as sensor noise and wheel slip, the fact that thecontroller gains have not been tuned for the real system,and the fact that the steering rate of the lead vehiclewas not controlled so it may have been too fast for thesteering actuator to follow.

Lateral Deviation (m)

Controller RMS Max

Pure Pursuit 0.47 1.28Fujioka 3 0.44 1.46CurvPrediction 0.37 0.77

Table 2: Experimental lateral deviation results

5 Longitudinal Control

The Control Approach The objective of the longitu-dinal control is to maintain a safe inter-vehicle spacing.A minimum bound in spacing is required to avoid colli-sion between the two cars whereas a maximum bound isrequired to ensure robust operation of the laser scanner.Also the control approach has to be robust, consideringnonlinearities and varying system parameters as well asdelays and discontinuities partly introduced by the self-imposed condition of portability. Because of its robustnature the control approach is therefore chosen to be acascaded PID control as shown in Figure 12. The outercontrol loop takes a reference spacing and a referencespeed to calculate a desired speed. The inner controlloop takes this desired speed and calculates an appro-priate pedal position. Due to the decoupled longitudinaland lateral control a special emphasis is put on repro-ducing the speed trajectory of the lead vehicle.

8


lead

vehicle

PID PIDfollowing

vehicle

velocity

cont

rolle

r

peda

l pos

ition

velocity

erro

r

spac

ing

cont

rolle

r

reference velocity

ref spacing deviation

Trajectory Estimateve

locity

position

+

-

+

Figure 12: Longitudinal Control Structure

Modelling A vehicle model was designed to allow forcontroller design and simulation based on the suggestedmodel structure by Cho and Hedrick [1989]. The modeltakes the desired pedal position as its input and returnsthe vehicle speed as its output. As the pedals are con-trolled through only one linear motion actuator theycan not be activated separately. This makes the vehi-cle model a SISO system.

The vehicle model was fitted to different sets of ref-erence data. The reference data was collected throughdrive-by-wire tests. The simulated speed was comparedwith the reference data through the following equation:

f = 100 ·(

1− ||v − vsim||||v − v||

)[%] (17)

where v is the speed of the real car, v is the mean valueof that speed and vsim is the simulated speed returnedby the model. The simulated speed matches a referencedata with at least f = 63.8% for different random drivecycles. Due to changing parameters of the real vehicle itis hard to find a more precise model.

Control Performance A PID controller was tunedfor speed control representing the inner loop of the cas-caded controller. Experimental results show a rise timeof tr = 1.4s for a step in desired velocity from v = 0m/sto v = 5m/s. The speed keeps oscillating around the ref-erence value with an amplitude of about 0.8m/s. Thismight be caused by discontinuities and delays in the sys-tem as well as the uneven ground of the test area.

The most widely used approach for controlling inter-vehicle spacing is the constant time headway (CTH) ap-proach [Chien et al., 1994; Girault, 2003]. This approachcalculates a desired velocity

xdes =d− d0

∆t(18)

using the actual spacing between the two cars d and theconstant lookahead time ∆t. d0 in this case is a safetydistance that the vehicles will maintain at low speeds.However, this approach leads to an instant accelerationif the lead vehicle starts accelerating.

The CTH method was adapted for this application sothat both vehicles pass a certain point on the path withthe same speed. This results in a new interpretation ofthe constant time headway, not based on the actual ve-hicle speed, but on the time delay with which the twocars pass a point on the trajectory. The advantage ofthis is that the following vehicle is able to reproduce thevelocity of the lead vehicle with a certain headway timedelay. This way the assumption of decoupled longitudi-nal and lateral control can be justified by assuming theappropriate speed to curvature ratio and safe driving ofthe lead vehicle.

The reference speed, vref , is the speed of the lead vehi-cle a certain time headway, tlong, ago. This is calculatedby setting t to be the current time minus tlong in equa-tion (6). A second control input takes the distance errorδp along the path from the closest point to the followingvehicle, P0, to the point Plong on the path where thelead vehicle was at time tlong ago. The distance is calcu-lated by integrating along the polynomial. The controlequation is then given by

vdes = vref + kp · δp + ki

∫ t

0

δpdt+ kd · δp. (19)

Figure 13 shows the advantage of the new spacing con-trol approach in simulation. The lead vehicle’s velocityprofile along the path is reproduced more accurately bythe new approach, allowing the following vehicle to keepa safe speed if the lead vehicle slows down to go around acorner, such as at the section marked by the grey stripe.The constant time headway approach leads to a velocitywhich is 40% to 50% faster than the lead vehicle in thisregion whereas the new approach tracks the desired ve-locity to within 15%. Experimental tests for the spacingcontrol are ongoing.

6 Conclusions

A robust algorithm has been developed for tracking thelead vehicle using a laser range finder. The algorithmhas been tested in real-world experiments, with RMSposition error of 65mm and orientation error of 5.8◦.

A technique to convert relative lead vehicle positionsinto a trajectory over time has been developed andtested, with a lateral error of up to 1.2m in a real-worldtest.

Various lateral control schemes have been imple-mented and tested in simulation and then experimentallyat speeds of approximately 15km/h. The best controllerperformed with an RMS lateral deviation of 37cm and amaximum deviation of 77cm.

A new trajectory-based spacing control approach wasdeveloped and successfully tested in simulation, allowinglateral and longitudinal control to be decoupled.

9


0 10 20 30 40 500

5

10velocity[m/s]

t ime [s]

leadCTHnew

0 50 100 150 200 2500

5

10

velocity[m/s]

path length [m]

leadCTHnew

Figure 13: Velocity over time and over distance alongthe path

Future tests will combine lateral and longitudinal con-trol to achieve full vehicle following. The On-Board Di-agnostics (OBD) interface could also be explored to pro-vide ego-motion.

Acknowledgments

The authors would like to thank the Energy and FuelsResearch Unit at the University of Auckland for accessto the dynamometer lab and the vehicle, as well as theDynamics and Control lab for technical support.

References[Chien et al., 1994] C. C. Chien, P. Ioannou, and M. C.

Lai. Entrainment and vehicle following controllersdesign for autonomous intelligent vehicles. Proceed-ings of the American Control Conference, pages 6–10,1994.

[Cho and Hedrick, 1989] D. Cho and J. Karl Hedrick.Automotive powertrain modeling for control. Jour-nal of Dynamic Systems, Measurement, and Control,111:p. 568–576, 1989.

[Coulter, 1992] R. C. Coulter. Implementation of thepure pursuit path tracking algorithm. TechnicalReport CMU-RI-TR-92-01, The Robotics Institute,Carnegie Mellon University, 1992.

[Fujioka and Omae, 1998] T. Fujioka and M. Omae. Ve-hicle following control in lateral direction for platoon-ing. Vehicle System Dynamics, 29:p. 422–437, 1998.

[Gehrig and Stein, 1998] S. K. Gehrig and F. J. Stein. Atrajectory-based approach for the lateral control of carfollowing systems. IEEE International Conference onSystems, Man, and Cybernetics, 4:p. 3596–3601, 1998.

[Girault, 2003] Alain Girault. A hybrid controller forautonomous vehicles driving on automated highways.2003.

[Linderoth et al., 2008] M. Linderoth, K. Soltesz, andR. M. Murray. Nonlinear lateral control strategy fornonholonomic vehicles. Proceedings of the AmericanControl Conference, 2008.

[Namik et al., 2006] H. Namik, T. Inamura, and K. Stol.Development of a robotic driver for vehicle dy-namometer testing. ACRA, Auckland, New Zealand,2006.

[Sotelo, 2003] M. A. Sotelo. Lateral control strategyfor autonomous steering of ackermann-like vehicles.Robotics and Autonomous Systems, 45(3):p. 223–233,2003.

[Swaroop et al., 2001] Darbha Swaroop, Karl Hedrick,and S.B. Choi. Direct adaptive longitudinal controlof vehicle platoons. IEEE Transactions on VehicularTechnology, 50(1):150–161, 2001.

[Thrun and others, 2007] S. Thrun et al. Stanley: therobot that won the DARPA Grand Challenge, pages 1–43. Springer Tracts in Advanced Robotics. Springer,2007.

[Vaughan et al., 2009] Richard T. Vaughan, AndrewHoward, and Brian P. Gerkey. The player project.http://playerstage.sourceforge.net/, 2009.

[White and Tomizuka, 2002] R. White andM. Tomizuka. Estimating relative position andyaw with laser scanning radar using probabilisticdata association. Proceedings of the American ControlConference, 2002.

[Wit et al., 2004] J. Wit, C.D. Crane, and D. Arm-strong. Autonomous ground vehicle path tracking.Journal of Robotic Systems, 21(8):439–449, 2004.

[Wong et al., 2008] N. Wong, C. Chambers, K. Stol, andR. Halkyard. Autonomous vehicle following usinga robotic driver. 15th International Conference onMechatronics and Machine Vision in Practice, pages115–120, 2008.

[Zhang and Ioannou, 2005] Jianlong Zhang and PetrosIoannou. Adaptive vehicle following control systemwith variable time headways. Proceedings of the 44thIEEE Conference on Decision and Control, pages3880–3885, 2005.

10


Documents

Trajectory Based Autonomous Vehicle Following using a ... · This paper outlines a project to improve an existing robotic driver to allow it to follow another vehicle. The robotic