INTEGRATED PATH PLANNING AND TRACKING FOR

AUTONOMOUS CAR-LIKE VEHICLES MANEUVERING

Fernando G

omez-Bravo, Diego A. L

opez

Departamento de Ingenier

ıa Electr

onica, S. I. y Autom

atica, University of Huelva, 21819 La R

abida, Huelva, Spain

Francisco Real, Luis Merino, Jos

e M. Matamoros

Robotics, Vision and Control Group,University of Seville, Camino de los Descubrimientos, 41092 Sevilla, Spain

Keywords:

Autonomous Car-like vehicle, Maneuvering, Motion Planning, Path-tracking.

Abstract:

This paper proposes a new method for control car-like vehicles maneuvering. For this purpose, traditional

planning and tracking algorithms has been modiﬁed in order to follow complex maneuvers in a frame of a

distributed control architecture. Thus, planning and tracking algorithm run in different platforms exchanging

data in order to control the maneuvers performance.

1 INTRODUCTION

Autonomous navigation of car-like vehicles is a well

known topic that has attracted the attention of many

researchers (Paromtchik et al., 1998), (Ollero et al.,

1999), (Gomez-Bravo et al., 2001), (Wada et al.,

2003), (Cuesta et al., 2004), (Daily and Bevly, 2004).

Navigation in cluttered scenarios usually involves

maneuvering that require stopping the vehicle and

changing the sign of the vehicle velocity to avoid col-

lisions. The non-holonomic constraints of the car-like

vehicles play an important role. Path tracking (Ollero

et al., 1994), (Ollero et al., 1996) and path planning

techniques (Latombe, 1991), (Laumont et al., 1994),

(LaValle, 2006) have been largely approached in mo-

bile robot literature. However, generating and track-

ing car-like maneuvers are not frequently reported

(Cheng et al., 2001), (Wada et al., 2003), (Cuesta

et al., 2004). This paper presents a new integrated

architecture particularly designed for planning and

tracking maneuvers. Furthermore, this strategy can

be also applied when complex maneuvers are not re-

quired.

Real-time path planning usually requires signiﬁ-

cant computational resources that could overload the

limited on-board processing capability. Then, a suit-

able alternative is to implement the planner on exter-

nal dedicated servers that could provide this capabil-

ity to one or several networked autonomous vehicles.

These servers could be also networked with sensors in

the environment providing information for navigation

(Gomez-Bravo et al., 2007).

In this paper a distributed implementation inte-

grating both maneuvers planning and tracking tech-

niques for autonomous car-like maneuvering is pre-

sented. The novelty of this method is the adaptation

of the planning and tracking method so that both can

work in different computer, establishing a communi-

cation process between the planner and the tracker

system in order to control effectively the maneuver

performance.

On the one hand, the planning method applied in

this paper is based on the Rapidly Exploring Random

Trees algorithm (RRT) (LaValle, 1998), (Bruce and

Veloso, 2002) (LaValle, 2006). This technique can

be easily extended to non-holonomic vehicles pro-

viding simple solutions for car-like maneuvers gen-

eration (LaValle and Kuffner, 1999), (Gomez-Bravo

et al., 2008). In the present approach, besides ob-

taining path for maneuvering in complex scenarios,

the planner is capable of dividing the originally com-

puted maneuver into different sections in order to fa-

cilitate the tracking task and allowing to modify eas-

ily the original path if changes in the initial map are

detected. On the other hand, the path tracking tech-

nique is a modiﬁed version of the well known pure

pursuit geometric approach (Ollero, 2001). The orig-

inal tracking strategy has been modiﬁed so that this

method can be applied for maneuvers tracking. More-

over, and different from previous approaches, in the

457

Gómez-Bravo F., A. López D., Real F., Merino L. and M. Matamoros J. (2009).

INTEGRATED PATH PLANNING AND TRACKING FOR AUTONOMOUS CAR-LIKE VEHICLES MANEUVERING.

In Proceedings of the 6th International Conference on Informatics in Control, Automation and Robotics - Robotics and Automation, pages 457-464

DOI: 10.5220/0002219504570464

 SciTePress

architecture presented in this paper the planner de-

termines the maneuver performance by establishing

some of the tracker parameters.

The paper is organized as follows: in the next sec-

tion the basis of the car-like vehicles maneuvering is

introduced and the application of the RRT to maneu-

vers generation is also presented. Section 3 is devoted

to describe the adaptation of the path tracking algo-

rithm for maneuvering. In section 4 the proposed im-

plementation is presented. Finally, in section 5, ex-

perimental results, obtained with a real autonomous

car-like vehicle when maneuvering in an outdoor sce-

nario, are presented. The paper ends with the conclu-

sions and the references.

2 CAR-LIKE VEHICLES

MANEUVERING

2.1 Car-Like Vehicles

Car-Like vehicles are non-holonomic systems charac-

terized by kinematics constraints resulting in noninte-

grable differential equations that should be taken into

account for motion planning and obstacle avoidance.

x

max

Figure 1: Car-like vehicle.

Usually, the kinematics model of car-like vehicles

is expressed as:





˙x

˙y









cos(θ) 0

sin(θ) 0

0 1







v(t)

tanφ(t)



(1)

where (x, y) is the position of the rear reference point,

and θ is the vehicle’s heading, both in a global ref-

erence frame, v(t) is the linear velocity, φ(t) is the

steering angle, that deﬁnes the curvature of the path,

and l is the distance between the rear and front wheels

(see Fig. 1).

Typically, the values of φ(t) are constrained by

the value φ

max

, accomplishing the relation

min

tanφ

max

(2)

where R

min

is the minimum curvature radius.

Each wheel of the vehicle presents a kinematics

constraint which prevents the robot from moving to

the orthogonal direction of the wheels longitudinal

axe. Then, the kinematics of these vehicles is usu-

ally characterized by the following differential non-

holonomic equation

˙x · sinθ − ˙y · cosθ = 0. (3)

In the next section, the basis of the method for

coping with this constraint, when planning trajecto-

ries, are presented.

2.2 Continuous Navigation vs.

Maneuvering

There are different approaches to car-like vehicles

motion planning (Laumont et al., 1994), (Cheng

et al., 2001), (Gomez-Bravo et al., 2008). Path plan-

ning techniques provides trajectories based on differ-

ent searching techniques (Latombe, 1991), (LaValle,

2006). Particularly, the randomized methods have

attracted considerable attention (Barraquand and

Latombe, 1991), (Amato and Wu, 1996), (Cheng

et al., 2001), (Bruce and Veloso, 2002). Due to

the non-holonomic nature of these vehicles, many

of these methods require further processing in order

to obtain a path accomplishing the kinematics con-

straints. Thus, derivable and continuous curvature tra-

jectories are frequently provided by these techniques.

However, when cluttered scenarios are involved, com-

plex maneuvers may be needed, and path gener-

ation should also include inversor con figurations,

(Latombe, 1991), i.e conﬁgurations where the sign of

the vehicle velocity changes. In these cases, discon-

tinuous curvature trajectories can also be considered

as admissible paths, although the kinematics con-

straints still have to be accomplished.

2.3 Planning Maneuvers

General strategies for maneuvers generation have not

been frequently addressed, (Latombe, 1991), (Lau-

mont et al., 1994), (LaValle, 2006). Recent ap-

proaches have focused attention in car-like parking

maneuvers (Paromtchik et al., 1998), (Gomez-Bravo

et al., 2001), (Cuesta et al., 2004).

The method presented in this paper is based

on a planner previously presented in (Gomez-Bravo

ICINCO 2009 - 6th International Conference on Informatics in Control, Automation and Robotics

458

et al., 2007) and (Gomez-Bravo et al., 2008). This

method takes the advantage of a randomized genera-

tion process, the original Bidirectional RRT algorithm

(LaValle, 1998), (Bruce and Veloso, 2002) adapted to

non-holonomic vehicles. Firstly, RRT is used with-

out considering any kinematics constraints providing

a data structure (two trees with free collision conﬁg-

urations) connecting the initial with the goal conﬁgu-

ration (see Fig. 2).

Initial Configuration

Goal Configuration

Figure 2: RRT connecting trees.

In the next step, the tree provided by the algorithm

is turned into a sequence of feasible admissible paths

by means of a set of connecting path previously de-

signed (see Fig. 3). Each of these connecting paths are

built from a set of basic canonical maneuvers. Exam-

ples of these canonical maneuvers for car-like vehi-

cles have been previously reported in (Gomez-Bravo

et al., 2001), (Cuesta et al., 2004) and (Gomez-Bravo

et al., 2008).

Figure 3: Final path.

As it is shown in (Gomez-Bravo et al., 2008), by

means of this procedure, any two conﬁgurations can

be connected by an admissible path. It is remarkable

that, with this methods, the planner generates trajec-

tories for both strategies: traditional continuous nav-

igation, without stopping, or maneuvering navigation

when necessary. That is, the planner includes inver-

sors on the trajectory when a cluttered environment

requires the vehicle to stop several times.

3 MANEUVERING NAVIGATION

CONTROL

In this section the basis of the tracking method are il-

lustrated. The particular problems of performing ma-

neuvers are also addressed. Finally, the convenient

modiﬁcations of the path following method for exe-

cuting complex maneuvers are presented.

3.1 Continuous Path-tracking

Different approaches have been proposed for path fol-

lowing, where the vehicle position is estimated by an

Extended Kalman Filter (EKF) (Grewal and Andrews,

1993) using Global Positioning System (GPS) (Daily

and Bevly, 2004) and odometry.

In the approach presented in this paper, an accu-

rate path-tracking strategy is accomplished by apply-

ing a modiﬁed version of the “Pure-pursuit” algorithm

(Ollero, 2001). In this algorithm a point of the refer-

ence path is selected at each time instant so that the

steering angle is obtained according to the expression:

φ = arctan(l ·

) (4)

where l is deﬁned in Fig. 1 and L

is a parameter

called the look-ahead, which represents the distance

between the target point and the vehicle’s current po-

sition, and 4 is the lateral distance between the robot

and the target point (see Fig. 4).

∆

, y

)

, y

)

Path

Current robot’s position

Target point

Figure 4: Circular trajectory.

With this driving strategy the vehicle will follow

a circular arc from its current position to the target

point. Usually the point of the path is selected by the

following procedure: a) ﬁrstly, the nearest point, (x

), to the vehicle is obtained; b) secondly, from (x

) the target point (x

, y

) is found as the one which is

at the distance L

, from the current vehicle’s position

in the direction of the desired motion, (see Fig. 5). In

INTEGRATED PATH PLANNING AND TRACKING FOR AUTONOMOUS CAR-LIKE VEHICLES MANEUVERING

459

this way at each time instant, from an estimated vehi-

cle conﬁguration, a new point of the path is selected

and the vehicle navigates following the desired direc-

tion.

∆

, y

)

, y

)

Path

Current robot’s position

Target point

Figure 5: Pure-pursuit.

There could be different criteria for selecting the

value of L

according to the velocity and the shape of

the path. In this approach the selection of the velocity

and the look-ahead value is determined by the planner

taking into account the characteristics of the planned

trajectory.

3.2 Maneuvers Path-tracking

Traditionally, continuous navigation is based on a se-

quential procedure: ﬁrst the planner provides a path

connecting the starting point with the goal conﬁgura-

tion and ﬁnally the tracking algorithm is applied so

that the vehicle follows the trajectory until the vehi-

cle arrives to the end of the path. In this way, path-

following algorithms are usually designed so that the

tracking error decrease as the vehicle follows the ref-

erence path. However, errors use to increase when

the vehicle arrive to the goal conﬁguration. Clearly,

stopping the vehicle involves slipping phenomenon,

particularly when the vehicle navigates outdoor on ir-

regular terrains. Obviously, this is a drawback, spe-

cially when the vehicle is performing maneuvers as

the vehicle has to stop several times. Thus, if a trajec-

tory to be followed presents inversors, the traditional

procedure need to be improved.

The present approach is based on developing two

independent modules, the planner and the tracker, that

take the responsibility of planning and tracking the

maneuver respectively. These two modules will inter-

act so that they manage the maneuver performance.

The planner, once a initial trajectory has been gen-

erated, will divide it in path sections, separated by

inversors. Each path section will have associated a

kinematics proﬁle and a look-ahead determined by the

planner. The tracker will receive sequentially each of

the path sections and will apply the tracking algorithm

until the inversor conﬁguration is reached. When the

vehicle is stopped over a inversor the tracker will ask

the planner for the next section of the path. This pro-

cedure will be repeated until the whole maneuver is

performed.

Clearly, stopping the vehicle over the inversor is a

very difﬁcult task. A simple approach for managing

the stopping process consist on decreasing, the vehi-

cle velocity as it is closed to the inversor. The tracker

will stop the vehicle when the position error is lower

than a threshold. However a problem could appear

when this strategy is applied. If the vehicle conﬁgu-

ration and the inversor fall in a circular arc which ra-

dius is shorter than minimum curvature radius, R

min

the vehicle will be trapped in a circular trajectory in

which the position error never decrease (see Fig. 6).

Inversor

min

Path

Figure 6: Pure pursuit problem.

Thus, the tracking algorithm will stop following

that section of the path if this situation is detected,

and asks the planner for the next section to follow.

4 DISTRIBUTED

IMPLEMENTATION

Motion control of autonomous vehicles requires real

time attention to different task. For instance, getting

the GPS lecture, reading the proximity sensors, run-

ning the path tracking algorithm etc, are procedures

that need a tailored use of the time into the control

loop. Nevertheless, path planning usually presents a

high computational cost. Then, implementing these

algorithms and the vehicle control program into the

same computer machine could negatively affect the

ICINCO 2009 - 6th International Conference on Informatics in Control, Automation and Robotics

460

distribution of the time control loop. In order to avoid

this problem, in this approach, the planning algorithm

is executed on a computer different to the one contain-

ing the autonomous vehicle control program. Thus a

distributed strategy has been implemented by using an

auxiliary server. This computer has been conﬁgured

so that planning task are executed. Moreover other

external interactions are also performed (collecting

information from external sensors or attending human

interface for instance). The novelty of this approach

is based on the relation between the vehicle control

program and the services running into the auxiliary

server. As was mentioned in the previous section,

there are parameters of the tracking algorithm (veloc-

ity and look-ahead) that are computed in the auxiliary

server from the path provided by the planning algo-

rithm.

The proposed implementation is shown in Fig. 7.

The auxiliary server communicates with the au-

tonomous vehicle by means of WIFI devices, using

the TCP protocol. In this way the YARP protocols

and services have been used. Considering the OSI

Reference Model the YARP protocols could be placed

on the Session Layer. This procedure allow transmit-

ting object data deﬁned in the C/C++ format. These

protocols provide several net ports independent from

the operative system. Thus, it represents a ﬂexible so-

lution for implementing the dialog between the nav-

igation control algorithm and the process running at

the auxiliary server, being independent from the com-

puter platform.

The global planner program has two process run-

ning in parallel. The ﬁrst one is the planner itself, i.e,

the responsible of providing admissible paths. The

second one, establishes and maintains the communi-

cation between the planner and the tracker through

the YARP port. By means of this process the plan-

ner receives continuously the vehicle position and the

goal point over the current path section. If the vehicle

navigates too far from reference path or the scenario

changes, the planner decides whether the current path

section is computed again or not. As was described

before, the path is divided into sections separated by

the inversor conﬁgurations, and the vehicle follows

sequentially the path sections. Thus, in case of any

possible eventuality (new obstacles on the map, large

position error or the vehicle being trapped by a circu-

lar trajectory), the planner modiﬁes the affected path

section. Only if these changes involve the current path

section, the vehicle has to stop. Otherwise, the modi-

ﬁcations are transmitted to the autonomous vehicle in

a sequential procedure.

Additionally, the planner determines the value of

the look-ahead and velocity associated to each sec-

tion of the path. It establishes these values according

to their characteristic, taking into account the curva-

ture and the length. An heuristic procedure for setting

the vehicle velocity has been implemented. Thus a

long and straight path section can be followed with a

high linear velocity whereas a short and curved sec-

tion requires a low velocity value. Likewise, different

criteria for selecting the Look-ahead according to the

velocity and the shape of the path can be found in

(Ollero et al., 1994) and (Ollero et al., 1996).

VEHICLE

Figure 7: Distributed implementation.

5 EXPERIMENTAL RESULTS

The experiments presented in this section have been

performed with ROMEO-4R, an autonomous car-like

vehicle build at the Sevilla University (Ollero et al.,

1999). The navigation control program runs into an

industrial PC-Pentium III under Linux (Debian 2.6).

The control program has been implemented in C++

and attends different concurrent process. Thus an

EKF has been implemented so that GPS and odom-

etry data are combined to obtain a robust pose es-

timation. At the same time, the path tracking algo-

rithm is executed, applying the techniques described

above, being continuously connected whit the plan-

ner program at the auxiliary server. Likewise, the au-

tonomous vehicle also attends different type of prox-

imity sensor (ultrasonics, LIDAR etc) that can be used

for the improvement of the local obstacle avoidance.

This technique is not addressed in the paper. The

auxiliary server is implemented in a laptop computer,

with a 1.8 Ghz AMD processor under Windows XP.

INTEGRATED PATH PLANNING AND TRACKING FOR AUTONOMOUS CAR-LIKE VEHICLES MANEUVERING

461

The planner and all the services running in the aux-

iliary server have been implemented in Java. Addi-

tionally, a connection with an external wireless sen-

sor network has been also developed (Gomez-Bravo

et al., 2007).

Inversor

End of the path

0 20 40 60 80 100 120 140 160

−0.6

−0.4

−0.2

0.2

0.4

0.6

0.8

1.2

1.4

m−1 , m/s

Curvature

Velocity

Figure 8: Experiment 1: a) Vehicle trajectory and b) Vehicle

velocity and curvature.

Fig. 8 a) shows an experiment in which the path

presents three sections and two inversors. In this ex-

periment the vehicle fell trapped at the end of the path,

it was specially conﬁgured in other to illustrate this

type of situation. Fig. 8 b) presents the evolution of

the velocity and curvature. Note that the velocity kept

constant value as the vehicle navigated around the in-

versor and the curvature got the maximum value that

represents the minimum curvature radius.

Inversor

End of the path

Figure 9: Experiment 2: a) Vehicle trajectory and b) Vehicle

velocity and curvature.

In Fig. 9 a) a new experiment is shown. Due to

the irregularities of the terrain, the vehicle motion is

affected by perturbations that make the vehicle trajec-

tory being different to the reference path. However,

the vehicle ﬁnally achieved the ﬁnal desired conﬁg-

uration. Fig. 9 b) presents the evolution of the ve-

locity and the curvature. Observe how, velocity was

decreased along the curved section and was increased

when a straight section was followed.

Finally in Fig. 10 some pictures of ROMEO 4R

performing a maneuver are shown. These pictures

were recorded during the experiment presented in

Fig. 9

ICINCO 2009 - 6th International Conference on Informatics in Control, Automation and Robotics

462

e) f)

Figure 10: ROMEO 4R performing the second maneuver.

6 CONCLUSIONS

This paper presents a new approach for autonomous

car-like vehicles maneuvering. The method allows

navigation of robots with non-holonomic constraints

in cluttered scenarios, providing, when necessary,

continuous paths or complex maneuvers, where the

vehicle has to change the sign of the velocity. This ap-

proach allows to distribute the computational task for

computing the path and tracking the maneuver. Thus,

a new implementation has been proposed so that the

planning and the tracking algorithm exchange contin-

uously data in order to enhance the maneuver perfor-

mance. The method has been validated in real experi-

ment with the autonomous car-like vehicle ROMEO-

4R built at the Sevilla University.

ACKNOWLEDGEMENTS

This work has been supported by the National Spanish

Research Program, project DPI2008-03847, and the

Project URUS funded by the European Commission

under grant IST-045062.

REFERENCES

Amato, N. M. and Wu, Y. (1996). A randomized roadmap

method for path and manipulation planning. IEEE Int.

Conf. Robot. and Autom, pages 113–120.

Barraquand, J. and Latombe, J. C. (1991). Robot motion

planning: A distributed representation approach. Int.

J. Robot, pages 167–194.

Bruce, J. and Veloso, M. (2002). Real-time randomized

path planning for robot navigation. International

Conference on Intelligent Robots and Systems, pages

2383–2388.

Cheng, P., Shen, Z., and LaValle, S. M. (2001). Rrt-

based trajectory design for autonomous automobiles

and spacecraft. Archives of Control Sciences, pages

167–194.

Cuesta, F., Gomez-Bravo, F., and Ollero, A. (2004). Parking

manoeuvres of industrial-like electrical vehicles with

and without trailer. IEEE Trans. on Industrial Elec-

tronics, pages 257–269.

Daily, R. and Bevly, D. (2004). The use of gps for vehicle

stability control. IEEE Trans. on Ind. Electron, pages

270–277.

Gomez-Bravo, F., Cuesta, F., and Ollero, A. (2001). Parallel

and diagonal parking in nonholonomic autonomous

vehicles. Engineering Aplication of Artiﬁcial In-

teligence, pages 419–434.

Gomez-Bravo, F., Ollero, A., Cuesta, F., and Lopez, D.

(2007). Rrt-d: A motion planning approach for au-

tonomous vehicles based on wireless sensor network

information. 6th IFAC Symposium on Intelligent Au-

tonomousVehicles, page C.D.

Gomez-Bravo, F., Ollero, A., Cuesta, F., and Lopez, D.

(2008). A new approach for car-like robots manoeu-

vering based on rrt. Robotica, pages 10–14.

Grewal, M. S. and Andrews, A. P. (1993). Kalman Filtering

Theory and Practice. Prentice-Hall.

Latombe, J. C. (1991). Robot Motion Planning. Kluwer

Academic Pulisher.

Laumont, J., Jacobs, P., Taix, M., and Murray, M. (1994).

A motion planner for nonholonomic mobile robots.

IEEE Trans. on Robotics and Autom, pages 577–593.

LaValle, S. M. (1998). Rapidly-exploring random trees: A

new tool for path planning. In TR 98-11.

LaValle, S. M. (2006). Planning algorithms. Cambridge

University Press.

LaValle, S. M. and Kuffner, J. J. (1999). Randomized kino-

dynamic planning. Proc. IEEE Int. Conf. on Robotics

and Automation, pages 473–479.

Ollero, A. (2001). Manipuladores y Robots Moviles. Mar-

combo Boixareu.

Ollero, A., Arrue, B. C., Ferruz, J., Heredia, G., Cuesta, F.,

Lopez-Pichaco, F., and Nogales, C. (1999). Control

and perception componenets for autonomous vehicle

guidance. application to the romeo vehicles. Control

Eng. Practice, pages 1291–1299.

INTEGRATED PATH PLANNING AND TRACKING FOR AUTONOMOUS CAR-LIKE VEHICLES MANEUVERING

463

Ollero, A., Garcia-Cerezo, A., and Martinez, J. (1994).

Fuzzy supervisory path tacking of autonomous vehi-

cles. Control Engineering Practice, pages 313–319.

Ollero, A., Garcia-Cerezo, A., and Martinez, J. (1996). De-

sign of a robust high performance fuzzy path tracker

for autonomous vehicles. Journal of Systems Science,

pages 799–806.

Paromtchik, I. E., Damm, M., and Matioukhina, L. I.

(1998). Autonomous maneuvers of a nonholonomic

vehicle. Intelligent Autonomous Systems. Interna-

tional Scientiﬁc Issue, pages 38–45.

Wada, M., Yoon, K. S., and Hashimoto, H. (2003). Devel-

opment of advanced parking assistance system. IEEE

Trans. on Ind. Electron, pages 4–17.

ICINCO 2009 - 6th International Conference on Informatics in Control, Automation and Robotics

464