Development of a Reduced Size Unmanned CarE. Bertolazzi, F. Biral, P. Bosetti, M. De Cecco, R. Oboe, F. Zendri

Department of Mechanical and Structural EngineeringUniverisity of Trento

Via Mesiano 77, I-38050 Trento, ItalyEmail: {enrico.bertolazzi, francesco.biral, paolo.bosetti, mariolino.dececco, roberto.oboe, fabrizio.zendri}@ing.unitn.it

Telephone: (+39)0461-882500Fax: (+39)0461-882599

Abstract— This article describes the overall system design ofa reduced size autonomous vehicle and focuses on the controlstrategy which is based on a Nonlinear Receding Horizon Control(NRHC) algorithm. The NRH planner is called by a high levelmanager, in the outer control loop, to solve a sequence of optimalcontrol problems. The planned motion provide a sequence of ref-erence set points for the faster inner control loop until a new planis available. The lateral motion is controlled through the steeringcommand by tracking the yaw rate reference. The longitudinalmotion is controlled by means of the throttle/braking coupledcommand by tracking the planned forward speed profiles. Thepaper discusses the results of some simulation results. The projectdescription proceeds as follows: Section II describes the systemarchitecture from vehicle and communication to software. SectionIII and IV details the motion planning and control algorithms.In Section V the simulation results regarding the NRH basedcontroller are discussed.

I. INTRODUCTION

In the last years a great effort has been devoted to thedevelopment of autonomous vehicles capable of moving ina high speed range either in semi-structured and unstructuredenvironments. The DARPA Grand- and Urban Challenge [8]are distinctive examples for military missions. However, thesame or similar technology can be used for civil safetyapplications such as the development of Advanced Driver As-sistance Systems, which is within the scope of the Insafes andSaspence sub-projects of the EC funded PReVENT Project.PReVENT’s aims at developing and demonstrating preventivesafety applications and technologies that help drivers to avoidor mitigate an accident through the use of in-vehicle systems,which sense the nature and significance of the danger, whiletaking the drivers state into account.

In the above quoted sub-projects the assessment of themaneuver risk level is evaluated by solving, on-board, asequence of optimal control problems whose objectives arethree top level hierarchical rules implemented into the goalfunction: absolute safety, user acceptance and mobility inorder of importance [15]. The complete vehicle motion andits trajectory are part of the optimal solution and associatedto the risk level they form the optimal plan or referencemaneuver, which take into account all the relevant aspectsfrom the reconstructed scenario. The optimal plan, being the“best” in the given conditions and environment, is a referencethat may be compared to other alternative maneuvers or towhat the driver is doing in the same scenario. Moreover, the

minimum value of the goal function, which is associated tothe optimal plan, is an objective measure of the performance(or risk level) of the maneuver itself.

Field tests have shown that the proposed method yieldnatural and human-like maneuver. Moreover, the underneathformulation and dedicated solver are able to compute theoptimal plans with a rate of 10 Hz for an horizon of 200 mbased on a fully nonlinear vehicle model [14]. Nevertheless,both for liability and legislations issues (excluded pre-crashapplications), the execution of fully autonomous maneuversis not allowed in the sub-projects’ vehicle demonstratorseven if the application is in principle capable. Since 90 %of the road accident are caused by human errors [1] theauthors are convinced those applications will greatly benefit ofembedding the proposed technology not only for warning andsupport system but also in cooperative and autonomous drivingsituations. For these reasons the mechatronic research groupof the University of Trento has started a project to develop aReduce size UnManned BuggY (called RUMBy), which is aplatform to test the proposed decision and autonomous drivingalgorithms.

The ultimate goal of this project is to develop a system,which is able to autonomously plan and execute accurateoptimal maneuvers both in normal and in critical drivingsituations. Additionally, from the comparisons of alternativepossible maneuvers on the basis of the performance indicesthe system should be able to select the one with the lowest risk(e.g. avoid and obstacle or stop the vehicle). From this pointof view the reference maneuver is an innovative tool both totake an holistic unified common driving decision, which putstogether all relevant aspects from the reconstructed scenario,and to yield the control commands that autonomously producethe selected maneuver.

This article describes the overall RUMBy system designand focuses on the Nonlinear Receding Horizon Control(NRHC) algorithm in standard driving scenario. In particular,it provides an analysis of the key factors that affect the controlalgorithm such as terminal constraints, vehicle dynamic model,finite horizon length by means of simulation tests on sim-ple maneuvers. The project explanation proceeds as follows:Section II describes the system architecture from vehicle andcommunication to software. Section III details the motionplanning and Section IV the control algorithms. In Section IV

978-1-4244-1703-2/08/$25.00 ©2008 IEEE 763

the simulation results on basic maneuvers regarding the NRHCare reported and discussed.

II. DESCRIPTION OF SYSTEM ARCHITECTURE

A. Vehicle and Instrumentation

The hardware part of the autonomous vehicle consists ofthe vehicle itself and of the instrumentation. The vehicle hasbeen selected on the basis of the following main requisites:

1) Dynamics: vehicle dynamics should be similar to realcar dynamics.

2) Representativeness: attention will be payed to on-carssystem specifications (use CAN interface).

3) Safety: it is possible to test emergency conditions suchas obstacle avoidance or critical scenarios at a low risklevel.

4) Flexibility: the system should be cheap, rapid prototyp-ing oriented, and with less constraints w.r.t. real cars.

5) Scalability: it is easy to investigate cooperative andplatooning technologies.

6) Lightness: development of compact and low powerconsumption systems is enforced.

Consequently, a radio-controlled, 1:6 scale model of H2Hummer has been chosen as a development platform (seeFig. 1). The vehicle is 680 mm long and 510 mm wide and isequipped with a 26 cm3 two-strokes engine, 4 disk brakes. Thepower train is connected to the engine through a centrifugalclutch. The chosen solution conforms with all the requirementsabove reported, especially with requirements 1, 4, 5, and 6.

The vehicle model has been customized with stiffer shockabsorbers and with an aluminum frame in order to mounton the necessary on-board instrumentation. Anyway, due tothe limited load capacity of the vehicle, only the sensing andthe sensor fusion instrumentation has been mounted on-board,while the heavy and energy-consuming computer needed forthe control and actuation, and for the high level automation(system manager and path planning) is remotely connected tothe vehicle via radio communication channels.

The sensing instrumentation has been designed with the aimof providing an on-board sensor fusion procedure with themeasurement required to reconstruct the whole vehicle state,namely: position, velocity and acceleration vectors, angularpositions and velocities, and angular velocity of wheels. Thesensor fusion system runs on a PC104 embedded computerand collects measurement data from the following devices:

• a differential GPS receiver running at 20–100 Hz (Race-logic VBox III), connected via CAN to PC104;

• an Inertial Navigation System (INS) providing 3 ac-celerometers, 3 gyroscopes and 3 magnetometers (Mi-croStrain 3DM-GX1), connected via serial port to PC104;

• four wheel speed sensors (odometers) based on Hall-effect sensors, custom designed and manufactured, con-nected via serial port to PC104.

The vehicle state data are sent to the manager computervia a custom UDP protocol by means of a Linksys 802.11nwireless router (effective line-of-sight range exceeding 150 m).

Fig. 1. Scaled vehicle model used for experiments

The control loop is closed thanks to the standard digitalremote control (R/C) provided with the Hummer: the managercomputer is connected via a NI-USB6008 D/A board to thetwo R/C inputs that control the throttle and steering servos.This split-loop design — although of some concern aboutloop timings and connection stability — complies with therequirements of lightness (only the lightest and more energy-saving instrumentation is on-board), scalablity (networked datacan be read and processed by several computers on the samenetwork, each with different tasks), representativeness (it usesindustrial sensors and technologies), and safety (the standardradio link with the RC servos can be instantly commutedto manual mode). Moreover, obstacle avoidance test can beperformed without any risk for human drivers.

The resulting architecture is schematized in Fig. 2, whichshows the sensing devices on the top and the split-loop on thebottom side.

B. Vehicle dynamic model

The RUMBy vehicle, from a multibody point of view,basically consists of five main bodies (i.e. the chassis and thefour wheels) and of highly non-linear subsystems: the doublewish-bone suspension struts, the tyres and the power-train.

In this project more than one mathematical model has beenderived to comply with different objectives. To develop andtest algorithms and control loop strategies a three dimensionalmodel that includes wheel spins, suspension characteristicsand tyre non-linearities was developed on the basis of themodel described in [20]. Particular attention was dedicatedto the centrifugal clutch modeling in order to simulate start-stop maneuvers and investigated the effect of the clutch skidon the longitudinal dynamics. Figure 3 shows the model maingeometrical parameters and state variables.

In the optimal control formulation, described in III, a twodimensional model was adopted with non-linear tyre models.

764

Path Planning CPU

USB

Odometers

GPS

INS

PC104

802.11n HUB

Serial

CAN

RC vehicle servos(throttle and steering)

Ethernet

Radio

Contr

ol

UDP

Fig. 2. RUMBy Project Architecture

Fig. 3. Sketch of mathematical model with main state variables. xm , ym

refer to the moving reference frame.

This model assumes a flat road and neglects the the pitch androll motions. The dynamic equations were derived with respectto the longitudinal and lateral velocities [u(t), v(t)] of the rearaxle middle point and with respect to the vehicle yaw rate ψ:

M ax(t) = Sf (t) + Sr (t)− kD u(t)2 − Ff (t)δ(t) (1)M ay(t) = Fr (t) + Ff (t) + Sf (t)δ(t) (2)

izd

dtψ(t) = a

(Sf (t)δ(t) + Ff (t)

)− bFr (t) (3)

where vx(t), vy(t) and ax(t), ay(t) are respectively the

longitudinal and lateral velocities and accelerations of thevehicle centre of gravity, Sf and Sr are the longitudinal forceson front and rear wheels, and Ff and Fr are the lateral forceson front and rear wheels. These quantities are related to thereference point velocities as follows:

vx(t) = u(t) (4)vy(t) = v(t) + ψ(t) b (5)

ax(t) =d

dtvx (t)− ψ(t)vy(t) (6)

ay(t) =d

dtvy(t) + ψ(t)vx (t) (7)

For the lateral forces a relaxation tyre model was adopted.For sake of simplicity, a linearized version is reported below:

σr

u(t)d

dtFr (t) + Fr(t) = Crλr(t)Nr (t) (8)

σf

u(t)d

dtFf (t) + Ff (t) = Cfλf (t)Nf (t) (9)

where Cr and Cf are the tyre sideslip stiffness, σr and σf

are the relaxation lengths, Nr(t) and Nf (t) are tyre loadsand λr(t) , λf (t) are the sideslip angles whose linearizedexpressions are:

λr(t) = b˙ψ(t)

vx (t)− vy(t)

vx (t)(10)

λf (t) = −a˙ψ(t)

vx (t)− vy(t)

vx (t)+ δ(t) (11)

Equations (9-11) show that the front tyre lateral force, as afirst approximation, depends on vehicle chassis sideslip ( vy(t)

vx (t)),

on the yaw rate, and on the steering angle δ(t). Moreover, thelateral forces depend on the vertical loads and in turn on theload transfer between front- to rear axle during accelerationand on the other way round during deceleration.

C. Software Architecture

The software architecture has been designed following thesemain requisites:

1) Safety: emergency maneuver always available2) Modularity: functions available as black boxes with

flexible interfaces3) Concurrency: re-configurability and parallelization4) Neutrality: interoperability of different softwares, pro-

gramming languages and pre-existing packagesDealing with autonomous vehicles means continuously preventand avoid potential crashes, and this is the reason why safetyis the first requirement. The other main requisites — whichare almost self explaining — are of additional relevance ifone consider that the software should be easily extendable,especially during the development phase.

As a consequence of the requirement 2, the software hasbeen designed on a multi-process architecture as shown inFigure 4.

765

The main process — the Manager Unit — receives com-mands by the GUI and holds the state machine, the optimalmaneuver planner, the driver for the vehicle on-board inertialnavigation system (labelled PC104), and the driver for theradio control (labelled RadioControl). The Manager Unit isa single multi-threaded process developed in Ruby languageand dynamically linked with a fast and efficient C++ librarythat implements the optimal maneuver path planner. Details onthe path planning algorithm will be provided in the followingsection. In order to comply with the requirements 2 and 3, theManager Unit has been implemented as a finite state machineimplemented using the statemachine Ruby library [7],which allows to build both Mealy [10] and Moore [9] statemachines. The PC104 and the RadioControl objects are ab-straction layers that provide a high level and neutral accessto the vehicle controls and to the vehicle state. Within theManager Unit, the main loop runs in real time with a constanttime step of 20±0.5 ms and follows the strategy depicted inFigure 5. It is worth noting that the main loop requests thePlanner to interpolate at the present time the latest availableplanned trajectory. If the error position between planned andactual position exceeds a threshold value (0.25 m), then theplanner is requested to compute a new optimal trajectory,which will be used as reference as soon as it will be available.

Fig. 4. Scheme of the Manager software with switchable connection betweenSimulator and real vehicle

In order to comply with the safety requisite the ManagerUnit can be indifferently connected (thanks to a neutralinterface) with the RUMBy car as well as with a softwaresimulator. In fact, until the software is correctly tested andcalibrated, a direct interface with the hardware vehicle involvessome risks. For example, an uncaught software bug or asudden connection failure could cause the vehicle to becomeunresponsive — and therefore dangerous — while travellingat high velocity. Consequently, the Simulator Unit has beendeveloped, which is a UDP server that listen for throttle andsteering commands, updates the state of the vehicle model it

implements, and gives back to the Manager Unit the updatedstate of the simulated vehicle. Note that the vehicle modelimplemented within the Simulator Unit is based on the samereference vehicle model above described. In particular, it has anon linear centrifugal clutch, 8 degrees of freedom, and takesinto account a 2-D planar position plus the vehicle roll angle.Finally, the Simulator Unit can eventually pass the vehiclestate information to a remote Monitor Unit that shows thevehicle state as 3-D car in Simulink Virtual Reality toolbox.

Fig. 5. Manager Unit main loop flow chart

III. PLANNING

To drive a vehicle from a start point to a final one a highlevel route has to be identified, which is in general definedas a navigation problem. The navigation route is generallydescribed as a sequence of connected nodes (or way-points,where multiple choices are possible. Usually the navigationproblem is solved at the manager level an a unique routeis selected along with back up routes to be used during thetravel if the main one turn out to be unfeasible. Then thevehicle has to be driven from one node to the following in themost efficient way according to environment reconstructed bythe sensing and sensor fusion layer. This second problem isknown as path planning (if only kinematics is included) ormotion/maneuver planning if vehicle dynamics is consideredin the calculation. There are many technique to solve thisproblem and calculate the feedback law to supply to thecontrol and actuation layer [11], [16], [17]. Among thesethe optimal control theory provides a general approach tofind the feedforward law to move the vehicle form the initialpoint/node to the final one, since the problem can be easilyformulated as a minimization of a performance index. Thisapproach is not actually viable since the real system suffersfrom disturbances, from mismatch between the model andthe physical system and from the fact that the surroundingenvironment may change. Hence, in order to incorporatesome feedback in the plan a sequence of optimal controlproblems have to be solved each time a new vehicle stateestimation and environment reconstruction is available. This

766

technique is know as Nonlinear Receding Horizon Controland is a kind of optimal control problem for obtaining a state-feedback control law that does not depend explicitly on time.A performance index of a receding-horizon optimal controlproblem has a moving initial time and a moving terminaltime, and the time interval of the performance index is finite.The resultant optimal feedback law depends only on the stateand on trajectory constraints. Since the time interval of theperformance index is finite, the optimal feedback law canbe determined even for a system that cannot be stabilized[13]. The optimal feedback law achieves the best possibleperformance in the sense that the given performance indexis minimized. The set of performance indexes corresponds tothe set of control objectives.

A. Nonlinear Receding Horizon ControlHereafter, the proposed formulation and solution of the

finite horizon optimal control will be explained. The problemis formulated as a minimum time problem, i.e. the followingperformance index has to be minimized:

find u(t) which minimize : tf − t0 =∫ tf

t0

1 dt

where tf is the free final time and t0 is the initial fixed time.The minimization problem is subject to the ordinary differ-ential equations (ODE) that represents the vehicle dynamicmodel (addressed later as internal model)

z′(t) = f [z(t),u(t)]

with the boundary conditions (BC):

b[z(t0), z(tf )] = 0

where z(t0) are the vehicle state estimation, which representsthe feedback term in the scheme, and z(tf ) are the desiredterminal constraints. Additionally some trajectory or physicalmodel constraints on states and controls are defined:

c[z(t),u(t)] ≤ 0

The the vehicle dynamic model z′(t) = f [z(t),u(t)] is thesame described in section II with some further simplification.In fact the forward forces Sr(t) and Sf (t) are evaluated froma unique force S(t) by the formula:

Sr(t)=max(S(t), 0) +12

min(S(t), 0)

Sf (t)=12

min(S(t), 0)

The control variable are

u(t) = [S(t), δ(t)]

while the state variable of the model are

z(t) = [u(t), v(t), ψ(t), x(t), y(t), ψ(t), Fr(t), Ff (t)]T

Introducing the new variable s and the differential equationT ′(s) = 0, the equations are transformed from time to normal-ized abscissa as independent coordinate by the transformation

t = sT (s) + t0

Now the free time tf = T (1) can be eliminated, and thefinite horizon optimal control can be rewritten as follows

find u(s) which minimize : L[z, u] = T (1)

subject to:

ODE z′(s) = g[z(s), u(s)]BC b[z(0), z(1)] = 0

c[z(s), u(s)] ≤ 0

where

z(s)=[z(sT (s) + t0);T (s)]u(s)=u(sT (s) + t0)

g[z, u]=[T (s)g[z, u]; 0]

All inequalities are treated with penalty functions and areadded at the performance index L[z, u] [14].

A two-point boundary-value problem (TPBVP) is obtainedfrom the first-order necessary conditions by the calculus ofvariations [12]. By setting the Hamintonian

H[z,λ,u] = T (1) +∫ 1

0

λ(s)T g[z(s), u(s)] ds

the resulting TPBVP takes the form:

z′(s) = ∂λH, λ′(s) = −∂zH, 0 = ∂uH

and the boundary conditions result in

b(z(0), z(1))=0(∂z(0)b(z(0), z(1)))T µ− λ(0)=0,

(∂z(1)b(z(0), z(1)))T µ + λ(1)=0

The Lagrange multiplier are λ(s) for the ODE and µfor the boundary conditions. In the special case whenb(Z(0), Z(1)) = z(0) − z0 — i.e. when the initial state iscompletely known — the multiplier µ can be eliminated andboundary conditions reduce to

z(0) = z0, λ(1) = 0

By discretizing the resulting Differential Algebraic Equation(DAE) a large highly non linear system is obtained. A fastand robust numerical algorithm for solving in real-time thenon linear system was developed. Further details on adoptedoptimal control formulation and solution are reported in [14].Note that the self consistency of the terminal constraintsand the horizon length are of paramount importance for thestability of the closed loop system [18].

The numerical solver provides the Manager Unit with theplanned reference optimal maneuver, which is a time descrip-tion of the vehicle states from the present time to the finaltime. Each vehicle state variable is provided in form of aspline, which can be interpolated by the Manager Unit at theneeded time, until the reference maneuver becomes invalid —because of the accumulated errors — and is thus replaced byan updated, newly computed reference maneuver. In particular,the Manager Unit interpolates — at each step of the loop in

767

Figure 5 — the two reference variables used by the controlloops i.e. longitudinal speed and yaw rate, as explained in thenext Section.

IV. CONTROL AND ACTUATION

As previously described, RUMBy allows the command ofsteering angle and throttle. These, in turn, must be properlycontrolled, in order to track the reference trajectory providedby the planner, in terms of longitudinal speed and yaw rate.Of course, there is a rather complex coupling between thesetwo system variables, especially for what concerns the yawrate. In fact, while the longitudinal speed is mainly influencedby throttle, changes in yaw rate are influenced not only bythe steering angle, but also by the longitudinal speed andthe yaw rate itself. These three quantities, as shown in theequations describing the vehicle dynamics, have the greatestinfluence on the lateral force, which, in turn, alters the yawrate. Taking into account the above consideration, a simplifiedmodel of the vehicle dynamics has been used to justify theimplementation of a decoupled controller, with yaw rate andlongitudinal speed controlled by acting on steering angle andthrottle, respectively.

Fig. 6. Decoupled steering (top) and throttle (bottom) controls blockschematics

The block diagram of such simplified system is shown inFigure 6, where the yaw rate dynamics is clearly influencedby the non-linear function f(·). This is a static mapping thatrelates the current values of steering angle, longitudinal speedand yaw rate to the lateral force, which, in turn, determinesthe torque around the yaw axis. So, for a given operatingpoint (in terms of yaw rate and longitudinal speed) the yawacceleration is mainly influenced by the steering angle, througha gain that can be obtained by linearizing f(·). As for theinfluence of the longitudinal speed and all other variables, itcan be considered as a disturbance, its effect to be canceledby an external controller. It can be demonstrated that suchdisturbances are mainly in the low frequency range, so a PIcontrol on the steering angle is suitable to achieve the targetof low tracking error of the yaw rate set point provided by theplanner.

For each operating point, a suitable PI controller has beendesigned and all the controller gains obtained stored in a

look-up table, to be used in a gain-scheduling scheme dur-ing operation. A simpler approach is to use the worst-casecontroller, i.e. the controller designed for the largest value off(·). In both cases, the influence of the longitudinal speed onthe yaw rate is regarded as a disturbance, its effect canceledby the PI controller. As for the longitudinal dynamics, it canbe considered as fairly decoupled from the yaw one. Thisleads to the “natural” choice of controlling the longitudinalspeed by acting on the throttle. The only problem arisingin designing the controller is caused by highly non-linearbehavior of the centrifugal clutch, which may induce limitcycles in the system. As a result of the above considerations,the inner loops of the RUMBy system have been implementedas shown in Figure 7, where a PI controller has been used forthe yaw rate, and a PID controller for the longitudinal speed.

Fig. 7. Controls schematic

It is worth noting that both PI and PID should be tunedfor each operating point (in terms of steering angle δ andlongitudinal speed u), but some simplification may lead to asimpler design. In particular, since the NRH planner computesthe trajectories with a minimum time objective, one canassume that the vehicle always runs at the highest possiblespeed. Consequently, all the closed loop gains can be assumedas constants and tuned for the worst case (i.e., high dynamics).

In terms of implementation, both the steering PI and thethrottle PID are controllers with integral tracking and variablegains. Since the controls run within the main loop of Figure 5,set point, process variable and output are updated at the samesampling period of the main loop, which is set to 20 ms.This value has been tuned as a trade off with the dynamicsof the vehicle, the rate of the radio control communicationperformance — which was measured as about 5 ms —, and theaverage task execution time on the available hardware (about10 ms on an Intel Core 2 Duo 2.33 GHz). It is worth notingthat, due to the internals of the R/C communication protocol,there is a linear rate limiter on both the controls, which reducesdown to 0.5 s the time needed for full travel of each control.The same rate limiter is implemented via software within theSimulator Unit.

As for the set points for the inner loops, they are providedby the optimal maneuver algorithm only when the longitudinalspeed is higher than a certain value and the distance from thearrival is larger than5 m. In particular, within the low speedrange (less than 1 m/s), the optimal maneuver algorithm — onwhich the above described planner is based — exhibits bad

768

performances. For this reason, in this range the set point forthe longitudinal speed comes from a look up table and the yawrate set point is zero (i.e. go straight). If precise positioningis needed, the set points can be eventually provided by othercontrollers (e.g. PC-Sliding controller, as introduced in [11]).In the medium-high speed range, instead, the set points forthe yaw rate and longitudinal speed controllers are directlyprovided by the planner.

Finally, when the vehicle approaches the arrival, the max-imum braking command is issued, while the reference to theyaw rate is again set to zero. In practice, two different outerloops are implemented and the switch between the two isdriven by the longitudinal speed and distance from the arrival.This is a typical case of Mode Switching Control (MSC),for which a particular care must me taken to avoid unwantedtransients [19]. This issue has not been addressed in the currentimplementation of the control for RUMBy.

V. RESULTS

At the time of writing, the manager system above described(thus including all the systems reported in Figure 4, excludingthe real vehicle) has only been tested against the simulator,since the hardware vehicle was not yet fully functional.

The first activity has been the tuning of the control loopgains in standard maneuvers throughout the well knownZiegler-Nichols method. This has been performed by sepa-rately tuning the throttle gains with maneuvers only involvinglongitudinal dynamics, and the steering gains by steady stateturning maneuvers at different curvatures and at maximumspeed.

With this configuration — assume the switch shown inFigure 4 as in the reported position there, i.e. no real carconected — the following test has been performed: a set ofminimum time maneuvers has been carried out with differentinitial and final vehicle configurations. These tests helped ina further tuning of the manager/planner parameters.

Hereafter, one of these tests will be discussed. The simulatorvehicle is set to start at the origin with heading North, and theplanner is initialized with a final target position at (100,0) mand heading West (to be reached with zero speed). The planneris requested to calculate the minimum time maneuver, i.e.having the minimum time as the only objective.

The Figure 8 depicts the simulator trajectory with fivesuperposed planned trajectories (dashed lines). Each of themis the result of a new plan that has been computed when theposition error exceeded the given threshold, as described in theprevios sections, and that became available to the ManagerUnit in correspondence of the small circles. It is evidentthat, the actual trajectory gradually diverges w.r.t. the plannedones. This is of course due to the much simpler vehiclemodel used by the planner w.r.t. the one that runs within theSimulator Unit. In particular, the planer does not consider theload transfer between front and rear axles and the consequentunder- or over-steering behavior. Nevertheless, the re-planningstrategy allows to catch up with this issue and gradually drive

the vehicle towards the desired position and orientation witha good accuracy.

Fig. 8. Simulator trajectory when starting from (0, 0) heading North andaiming at (100, 0) heading West. Circles represent the beginning of each newplanned trajectory

The Figure 9 shows the time profiles of the vehicle speedsand of the throttle and steering controls. Regarding the speedprofile, it is worth noting that at the beginning of the maneuverthere is a sudden increase and a following oscillation of theplaned speed, because of the MSC strategy that enables theplaner only when the vehicle speed becomes larger than 1 m/s.Then, the discontinuity associated with the clutch skid causesa oscillation in the longitudinal acceleration, which, in turn,triggers a re-planning and thus the saw-tooth aspect of theblack curve. This also reflects in the oscillation of the throttlecommand between 0 and 5 s. At the end of the maneuver, thefinal constant segment of the planned speed curve is due tothe fact that the Manager Unit switches to a full brake modewhen the distance to the target falls below the typical brakingdistance (in this case, about 5 m). In this full brake mode, theManager Unit also drives the steering command by setting thereference yaw rate to zero, and this causes the well evidentglitch in steering command at about 16 s. In a future version,a more appropriate and MSC-compliant control strategy willbe implemented, for example by gradually adjust the yaw rateset point from the final planning value to zero with a smoothtransition.

VI. CONCLUSIONS

The paper presents the design and the implementation of anautonomous vehicle able to plan and execute accurate optimalmaneuvers both in normal and in critical driving situations.

769

Fig. 9. Speed profiles (top) and controls profiles (bottom) for the drivingtest of Figure 9

A general description of the overall system hardware andsoftware architecture is presented. The vehicle itself is a 1:6scale R/C model powered by two-strokes gasoline engine withcentrifugal clutch and four disk brakes, controlled by twoservo actuators for steering and throttle/brakes. The artificialintelligence (AI) part is a suite of processes acting at managerlevel, path planning level, and control and actuators interfacelevel.

The Manager Unit, which runs in real time with a constanttime step of 20±0.5 ms is implemented as a finite state ma-chine and can switch between different operating modes basedon vehicle state condition as well as on operator commands.

The Planner Unit runs on a slower loop (on state-triggeredvariable time step) and computes the optimal maneuver on thebasis of the Nonlinear Receding Horizon planning strategy andon a simplified vehicle model.

The control level acts within the fast loop, receives theplanned vehicle states by the Planner Unit and controls thesteering angle by tracking the planned yaw rate, and thethrottle by tracking the planned forward speed profile.

At the moment of writing the real vehicle is not fullyfunctional yet — in particular, not all the tests needed tohave a complete identification of its dynamic behavior havebeen completed — so a preliminar set of tests have beenperformed on a custom software simulator that implements thesame reference kinematic/dynamic model of the real vehicle(including non-linearities). The typical results of these testsare reported and discussed, showing that the AI part of theRUMBy system is able to drive the vehicle with a goodaccuracy to the target position, coping at the same time with

the path following errors thanks to the Nonlinear RecedingHorizon re-planning strategy.

Further work will focus on fine tuning of the mode switch-ing control strategy, on the identification of dynamic param-eters of the real vehicle and finally on the test autonomousdrive of the real vehicle.

REFERENCES

[1] European Commission, Directorate-General for Energy and Transport,“European Road Safety Action Programme - Halving the number of roadaccident victims in the EU by 2010: a shared responsbility”, Internet,June 2003.

[2] European Commission, Directorate-General for Energy and Transport,“Press release - Road safety: The Commission wants to save 20 000lives a year on European roads”, Internet, June 2003.

[3] M. Taylor, D. Lynam, and A. Baruya, “The effects of drivers’ speed onthe frequency of road accidents”, TRL Report 421, Crowthorne, 2000.

[4] European Commission, Road Safety Observatory, “2005 Road fatalitiesby transport mode”, Internet, April 2007.

[5] S. Thrun, M. Montemerlo, H. Dahlkamp, et. al., “Stanley: The RobotThat Won The DARPA Grand Challenge”, Internet, 2005.

[6] K. Kitahama and H. Sakai, “Measurement method of normalized corner-ing stiffness”, Soc. Auto. En. Ja , Review 21, pp.213-217, 2000.

[7] M. Martin, “Statemachine - A Ruby Library, Gem, and Rails Plugin”,http://statemachine.rubyforge.org/

[8] http://www.darpa.mil/grandchallenge/index.asp[9] E. F. Moore, “Gedanken-experiments on sequential machines,” in Au-

tomata Studies, Annals of Mathematical Studies, vol. 34, pp. 129–153,Princeton, N.J: Princeton University Press, 1956.

[10] G. Mealy, “A method to synthesizing sequential circuits,” Bell SystemTechnical Journal, vol. 34, pp. 1045–1079, 1955.

[11] M. De Cecco, E. Bertolazzi, G. Miori, R. Oboe “PC-SLIDING forvehicle path planning and control. Design and evaluation of Robustness toParameters change and Measurement Uncertainty”, ICINCO2007 WorldCongress, Angers, 9-12 May, 2007.

[12] A. E. Jr. Bryson, Y. C. Ho, Applied Optimal Control. Hemisphere,Washington, DC, 1975.

[13] T. Ohtsuka, H. A. Fuji, “Real-time Optimization Algorithm for Non-linear Receding-horizon Control”, Automotica, vol. 33, No. 6, pp. 1147–1154, 1997

[14] E. Bertolazzi, F. Biral, M. Da Lio, “Real-time motion planning formultibody systems: Real life application examples”, Multibody SystemDynamics, vol. 17, 2007

[15] F. Biral, M. Da Lio, E. Bertolazzi, “Combining safety margins and userpreferences into a driving criterion for optimal control-based computationof reference maneuvers for an ADAS of the next generation”. In Intel-ligent Vehicles Symposium, in Proceedings IEEE, No. 94, pp. 36–2005.2005.

[16] E. Frazzoli, M. A. Dahleh, E. Feron, “Real-time motion plannig foragile autonomous vehicles”. In AIAA Guidance, Navigation, and ControlConference and Exhibit, No. 4056, 2000.

[17] M. B. Milam, “Real-Time Optimal Trajectory Generation for Con-strained Dynamical Systems”, PhD thesis, California Institute of Tech-nology, 2003.

[18] J. A. Primbs, “Nonlinear optimal control: a receding horizon approach”,PhD thesis, California Institute of Technology, 1999.

[19] R. Oboe, F. Marcassa, “Initial value compensation applied to disturbanceobserver-based servo control in HDD,” Advanced Motion Control, 2002,7th International Workshop on , vol., no., pp. 34-39, 2002

[20] J.D. Demerly, K. Youcef-Tourni, “Nonlinear Analysis of Vehicle Dy-namics (NAVDyn): A Reduced Order Model for Vehicle Handling Anal-ysis”, SAE Technical Paper Series, SAE 2000-01-1621, 2000

770