Development and Testing of an Autonomous Driving ...

Proceedings of IMECE2008 2008 ASME International Mechanical Engineering Congress and Exposition October 31-November 6, 2008, Boston, Massachusetts, USA Proceedings of IMECE 2008 2008 ASME International Mechanical Engineering Congress and Exposition October 31 – November 6, 2008, Boston, Massachusetts, USA

IMECE2008-68487 IMECE2008-68487

DEVELOPMENT AND TESTING OF AN AUTONOMOUS DRIVING MODULE FOR CRITICAL DRIVING CONDITIONS

Francesco Biral∗ Department of Mechanical and Structural Engineering University of Trento Trento, 38050, Italy Email: [email protected]

Enrico Bertolazzi Department of Mechanical and Structural Engineering University of Trento Trento, 38050, Italy

Daniele Bortoluzzi Department of Mechanical and Structural Engineering University of Trento Trento, 38050, Italy

Paolo Bosetti Department of Mechanical and Structural Engineering University of Trento Trento, 38050, Italy

ABSTRACT In the last years a great effort has been devoted to the development of autonomous vehicles able to drive in a high range of speeds in semi-structured and unstructured environments. This article presents and discusses the software framework for Hardware-In-the-Loop (HIL) and Software-In-the-Loop (SIL) analysis that has been designed for developing and testing of control laws and mission functionalities of semi-autonomous and autonomous vehicles. The ultimate goal of this project is to develop a robotic system, named RUMBy, able to autonomously plan and execute accurate optimal manoeuvres both in normal and in critical driving situations and to be used as a test platform for advanced decision and autonomous driving algorithms. RUMBy’s hardware is a 1:6 scale gasoline engine R/C car with onboard telemetry and control systems. RUMBy’s software consists of three main modules: the manager module that coordinates the other modules and take high level decision; the motion planner module which is based on a Nonlinear Receding Horizon Control (NRHC) algorithm; the actuation module that produces the driving command for the vehicle. The article describes the details of RUMBy architecture and discusses its modular configuration that easily allows HIL and SIL tests.

∗ Address

all correspondence to this author.

INTRODUCTION Much of the research effort in the automotive field is dedicated to increase the vehicle safety both in normal and critical driving conditions. The active safety systems (such as ABS, ESP, etc.) have made available on the market vehicles that are easier to control. However, easier vehicles have the counter effect of encouraging to drive closer to the vehicle limits, therefore affecting the intended safety benefits. For this reason a new generation of systems are being developed to assist the driver not only when he is facing the critical condition but also to warn him of incoming dangers (e.g. obstacle in the vehicle lane) or dangerous behaviour (e.g. approaching a curve too fast) in advance as earlier as possible. Those systems are based on a set of sensors (the radar/lidar, video cameras, vehicle to vehicle communication) combined with fusion algorithms that reconstruct the scenario around the vehicle. The level of risk of the manoeuvre the driver is performing is assessed in relation to the scenario and vehicle state. Based on the risk rate, a proper warning to prevent the possible critical situations/accidents or some autonomous corrective actions are taken to reduce the impact effect. Consequently, although introduction of fully autonomous vehicles on the market has not to be expected for the near future, the next step in the road map to the autonomous drive is the development of systems 1

c 2008 by ASME Copyright

range manoeuvre planning, all the subsystems 1–4 are present on the RUMBy platform. More specifically, all the algorithms needed by subsystems 3 and 4 have been custom developed by the authors. The whole system has been designed and developed by following a modularity approach as extensively as possible. As a consequence, the RUMBy systems —both in regards the hardware platform and of the software framework— helps and ensures the gradual design of each subsystems through softwarein-the-loop and hardware-in-the-loop simulations as well as real field experiments. In order to further clarify these statements, let illustrate the example of obstacle avoidance manoeuvres. It is evident that the testing of such manoeuvres —when performed by real— presents significative issues about safety and possible damages to persons and things. The modular approach adopted in RUMBy allows to easily substitute “real” obstacles with “virtual” ones, obtaining a viable and reliable test of planner/controller/vehicle system with a greatly reduced risk of damage. Rather analogously, the same modularity allows to reduce the development costs: for example, the vaste majority of autonomous drive system currently under development heavily rely on on-board cameras and artificial vision systems that provide —amongst other— the localization of the vehicle with respect to road margins. On RUMBy, this system has been substituted with a “virtual camera” that, on the basis of a high resolution GPS and of a well known road environment, can provide the same kind of information to the manoeuvre planning kernel. The following subsections will describe the implementation details of the RUMBy hardware and software subsystems.

that collaborate with the driver: support him/her in the execution of the intended manoeuvre, suggest a safer alternative when necessary and autonomously executing it in situations of extreme danger. The development of such systems has to fight with many issues, which are mainly related to the unexpected behaviour that complex planning AI algorithms could show up when interacting with a real and complex mechanical system. The development is made easier if a software/hardware platform is available that lets researchers to design each module starting from simple model representation and to progressively improve it with more detailed versions and eventually with their real counterpart. The mechatronic research group of the University of Trento —following its previous experience within the 6th Framework Programme PReVENT [1–4]— started a project to develop a system be able to autonomously plan and execute accurate optimal manoeuvres both in normal and in critical driving situations. Additionally, from the comparisons of alternative possible manoeuvres on the basis of performance indices, the system is able to select the one with the lowest risk (e.g. avoid an obstacle or stop the vehicle). At the heart of this project there is a novel approach in manoeuvre planning that is based on the fast solution of an optimal control problem (no linearization are necessary as in [5]), which provides the reference manoeuvre that the vehicle should perform in order to get to the target. This reference manoeuvre method (RMM) is an innovative tool both to take au unified common driving decision and to yield the control commands that autonomously produce the selected manoeuvre. To facilitate the development and testing of the RMM, a mechatronic system has been designed, which includes a Reduce size UnManned BuggY (called RUMBy), which allows to carry out real experiments of autonomous emergency manoeuvres with a limited budget and no danger for humans. This article describes the overall RUMBy system design and focuses on the software framework and on the planner and the control algorithms.

Hardware: Vehicle and Instrumentation The hardware part of the autonomous vehicle consists of the vehicle itself and of the instrumentation. The vehicle has been selected on the basis of the following main requisites: 1. Dynamics: vehicle dynamics should be similar to real car dynamics. 2. Representativeness: attention will be payed to on-cars system specifications (use CAN interface). 3. Safety: it is possible to test emergency conditions such as obstacle avoidance or critical scenarios at a low risk level. 4. Flexibility: the system should be cheap, rapid prototyping oriented, and with less constraints w.r.t. real cars. 5. Scalability: it is easy to investigate cooperative and platooning technologies. 6. Lightness: development of compact and low power consumption systems is enforced.

DESCRIPTION OF SYSTEM ARCHITECTURE Generally speaking, an autonomous vehicle is composed by five main systems: 1. the vehicle itself, which should be large enough in order to carry the needed equipment and that must provide a bywire interface to all the driving subsystems (steering, throttle, brakes); 2. the sensors, which have to provide a description of the vehicle state and of the environment as complete as possible; 3. the algorithms for sensor fusion and scenario reconstruction; 4. the algorithms for manoeuvre planing and vehicle control; 5. the algorithms for navigation and path planning.

Consequently, a radio-controlled, 1:6 scale model of H2 Hummer has been chosen as a development platform (see Fig. 1). The vehicle is 680 mm long and 510 mm wide and is equipped

With the sole exception of the last subsystem, which only deals with long-range navigation and which has no effect on short2

c 2008 by ASME Copyright

on-board Odometers

serial

GPS

CAN

INS

serial

Ethernet

Throttle and steering motors

Radio

USB

with a 26 cm3

two-strokes engine, 4 disk brakes. The power train is connected to the engine through a centrifugal clutch. The chosen solution conforms with all the requirements above reported, especially with requirements 1, 4, 5, and 6. The vehicle model has been customized with stiffer shock absorbers and with an aluminum frame in order to mount on the necessary on-board instrumentation. Anyway, due to the limited load capacity of the vehicle, only the sensing and the sensor fusion instrumentations have been installed on-board, while the heavy and energy-consuming computer needed for the control and actuation, and for the high level automation (system manager and path planning) is remotely connected to the vehicle via radio communication channels. This choice does not jeopardize the project objective since the intelligence (planning-control) can be transfered on-board in a next step. The sensing instrumentation has been designed with the aim of providing an on-board sensor fusion procedure with the measurement required to reconstruct the whole vehicle state, namely: position, velocity and acceleration vectors, angular positions and velocities, and angular velocity of wheels. The sensor fusion system implements a Kalman filter [6] that runs on a PC104 embedded computer and collects measurement data from the following devices:

UDP

base station

VEHICLE MODEL USED FOR EXPERIMENT.

802.11n HUB

serial

Programmable Microcontroller

R/C reciver

Figure 1.

PC104

Figure 2.

Path Planning CPU

SYSTEM ARCHITECTURE

via a custom UDP protocol by means of a Linksys 802.11n wireless router (effective line-of-sight range exceeding 150 m). Regarding the actuation part, two different solution are implemented. In the first solution, the control loop is closed thanks to the standard digital remote control (R/C) provided with the Hummer: the manager computer is connected via a NI-USB6008 D/A board to the two R/C inputs that control the throttle and steering servos. This means that the control and actuation functions must reside within the manager computer. In the second solution, the throttle and steering servomotors can be driven manually (by means of a standard R/C joystick) or can be switched in closed loop control (for autonomous drive). A programmable microcontroller1 that is in serial communication with the PC104 takes care of the mode switching, which can be toggled either by pushing a button on R/C joystick or by way of the on-board PC104. In case of autonomous drive mode, the steering and throttle commands are routed by the manager computer through the PC104 and through the microcontroller to the servomotors. The two solutions result to be somehow complementary: in the first case, the design and the implementation is much simpler and complex control strategies can be exploited thanks to the larger computational power available on the manager computer, but the on-board PC104 cannot directly access the servos. This is the solution adopted for early tests.

1. a differential GPS receiver running at 20–100 Hz (Racelogic VBox III), connected via CAN to PC104; 2. an Inertial Navigation System (INS) providing 3 accelerometers, 3 gyroscopes and 3 magnetometers (MicroStrain 3DM-GX1), connected via serial port to PC104; 3. four wheel speed sensors (odometers) based on Hall-effect sensors, custom designed and manufactured, connected via serial port to PC104. Both the raw measurements and the Kalman-filtered data that describe the vehicle state are sent to the manager computer

1 Arduino Diecimila, a programmable board based on Atmel ATMega168 microcontroller, http://arduino.cc

3

c 2008 by ASME Copyright

In the second case —not yet fully implemented—, the PC104 has a direct access to the servos and thus on-board control techniques and emergency-management manoeuvres can be investigated. Moreover, an immediate mode-switching function can be easily implemented, which is helpful in testing drive assistance algorithms and makes this solution suitable for testing advanced configurations. The resulting architecture is schematized in Fig. 2, which shows the sensing devices on the top and the split-loop on the bottom side.

Throttle

Control GUI

NIDAQ

Steering

R/C Joystick Radio

USB

UDP:3141

UDP:14000

StateMachine

Controls

Communication

UDP:12000

UDP:13000 UDP:12000

Telemetry

Simulator Unit

VehicleInterface

Manager Unit

Software architecture The software architecture has been designed following these main requisites:

Symbols:

1. Safety: emergency manoeuvre always available 2. Modularity: functions available as black boxes with flexible interfaces 3. Concurrency: re-configurability and parallelization 4. Neutrality: interoperability of different softwares, programming languages and pre-existing packages

Simulator / Real vehicle switch

Ruby Objects

Simulink

Devices

Other (C/C++)

Figure 3.

Simulator

UDP:15000

Planner (C++ wrapper)

RUMBY

Visualizer

Monitor Unit

SOFTWARE AND HARDWARE ARCHITECTURE WITH

SWITCHABLE CONNECTION BETWEEN SIMULATOR AND REAL VEHICLE. DASHED LINES REPRESENT AN ALTERNATIVE CONNECTION

Dealing with autonomous vehicles means continuously prevent and avoid potential crashes, and this is the reason why safety is the first requirement. The other main requisites —which are almost self explaining— are of additional relevance if one considers that the software should be easily extendable, especially during the development phase. As a consequence of the requirement 2, the software has been designed on a multi-process architecture as shown in Fig. 3. The main process —the Manager Unit— is a multithreaded process developed in Ruby language that receives commands by a custom graphical interface (GUI). It holds the optimal manoeuvre planner and the interface with vehicle systems, which, in turn, is made of the driver for the vehicle on-board inertial navigation system (labelled “Telemetry”), the control system (labelled “Controls”), and the commands communication system (labelled “Communication”). The Communication and the Telemetry objects are abstraction layers that provide a neutral, high level access to the vehicle commands and to the vehicle state, respectively. The Planner object is wrapper for an efficient C++ library that implements the optimal manoeuvre planner, whose details will be provided in the following section. In order to comply with the requirements 2 and 3, the Manager Unit has been implemented as a finite state machine. The state machine runs a main loop with a constant time step (usually of 20±0.5 ms). Within this main loop, the state machine can change state either by receiving a command from the GUI or as a consequence of a change in vehicle state variables. For example, at the beginning the system is in the ‘idle state’ (all commands in neutral state, telemetry enabled). When the GUI sends the command to switch in ‘accelerate state’, the vehi-

cle accelerates at full throttle until it reaches a threshold speed (say 2 m/s), then the controller spontaneously switches to the ‘planning state’ performing the requested manoeuvre. In planning state, the main loop follows the strategy depicted in Fig. 4. It is worth noting that the main loop requests the Planner to interpolate at the present time the latest available planned trajectory. If the error position between planned and actual position exceeds a threshold value (0.25 m), then the planner is requested to compute a new optimal trajectory, which will be used as reference as soon as it will be available. Finally —when the distance to the target position is less than the braking distance at the current speed— the state machine switches to the ‘halt state’, which disables the planning and performs a full stop of the vehicle. In order to avoid command glitches during state transitions, command values (i.e. throttle and steering) are gradually adjusted to the new values over a given transition time (set to 0.5 s) using the convex sum: v = v0 (1 − α) + v1 α

(1)

where v0 and v1 are the values of command v at the end of the previous state and at the current time, respectively, and α linearly ranges from 0 up to 1 during the transition time. It is worth noting that, until the software is correctly tested and calibrated, a direct interface with the hardware vehicle involves some risks. For example, an uncaught software bug or a sudden connection failure could cause the vehicle to become unresponsive —and therefore dangerous— while travelling at high 4

c 2008 by ASME Copyright

Figure 5.

VEHICLE MODEL WITH MAIN STATE VARIABLES AND GE-

OMETRICAL PARAMETERS Figure 4.

MANAGER UNIT MAIN LOOP FLOW CHART

scribed in [9] and [10]. Model parameters are measured in the laboratory with custom testing/measurement machines or estimated from logged data acquired during field tests. A validation campaign is carried out for each model. Due to its importance in the control performance a simplified model of the centrifugal clutch of actual vehicle is implemented to make it possible start and stop manoeuvres. Finally, the engine torque characteristic is identified versus forward engine shaft RPM and radio control throttle position.

velocity. Consequently, in order to comply with the safety requisite, the Simulator Unit is introduced, which is a UDP server that listen for throttle and steering commands, updates the state of the vehicle model it implements, and gives back to the Manager Unit the updated state of the simulated vehicle. Note that the vehicle model implemented within the Simulator Unit is based on the same reference vehicle model above described. In particular, it has a non linear centrifugal clutch, 8 degrees of freedom, and takes into account a 2-D planar position plus the vehicle roll angle. Finally, the Simulator Unit can eventually pass the vehicle state information to a remote Monitor Unit that shows the vehicle state as 3-D car in Simulink Virtual Reality toolbox. Fig. 3 makes evident the system modularity. The Manager Unit, in fact, can indifferently drive the real vehicle or the simulated one, and the choice simply requires a switch of IP addresses. Moreover, as already pointed out in Fig. 2, the Manager Unit can be alternatively configured in order to calculate control commands and directly act on the vehicle servos (the loop routed through the R/C joystick), or to send references to the on-board PC104, wich directly acts on servos (dashed route in Fig. 2).

Motion Planner: Nonlinear Receding Horizon Control The motion plan —subject to physical and trajectory constraints— can be formulated as an optimal control problem. The optimal control theory provides a general approach to find the feedforward law to move the vehicle from an initial point to the final one [11]. The quality of the planned motion depends on the mismatch between the model and the physical system, on the changes that occur in the surrounding environment, and disturbances that may deviate the vehicle from the desired path or affect the motion and environment reconstruction. These detrimental effects can be limited by a feedback design, where a sequence of optimal control problems is routinely solved on the basis of updated vehicle state estimation. This technique is know as Nonlinear Receding Horizon Control and is a special kind of optimal control problem for obtaining a state-feedback control law that does not explicitly depend on time. A performance index of a receding-horizon optimal control problem has a moving initial time and a moving terminal time, and the time interval of the performance index is finite. The resultant optimal feedback law only depends on the state and on the trajectory constraints. Since the time interval of the performance index is finite, the optimal feedback law can be determined even for a system that cannot be stabilized [12]. The optimal feedback law achieves the best possible performance in the sense that the given performance index is minimized. In the present case, the optimal control problem has been

PLANNING AND SIMULATION SOFTWARE Vehicle Models The simulator module may be linked to different vehicle models via Ruby wrapper libraries. Each vehicle model is a C++ class with a standard interface, which exploits the associative array of the C++ Standard Template Library to pass model- and solver parameters. The model class contains the vehicle equation of motion (ODE or DAE) and the suitable numerical solver. Equation of motion were symbolically derived with an add-on of c Maple (MBSymba [7]) and automatically translated into C++ code. For each model, wrappers for Simulink (S-Function) and Ruby are automatically generated thanks to the SWIG tool [8]. Implemented models are of different complexity and are de5

c 2008 by ASME Copyright

formulated as weighted sum of minimum time and maximal comfort objectives, both in cartesian coordinates (for off-road applications) and in curvilinear coordinates (related to a road model). Hence, two different modules are available that can be called by the manager according to the scenario where the autonomous manoeuvre has to be performed (i.e. off-road scenario or road infrastructure available). In both modules the adopted vehicle model is the single track or “bicycle” model, which incudes the nonlinear tyre behaviour (represented by the non linear function FNLT in the equations) and force lag. Figure 5 indicates the main state variables and the equations of motion expressed in time domain are as follows: d ˙ v) = (Sr + S f ) − Ff δ − kD u2 u−ψ dt d ˙ u) = (Fr + Ff ) − S f δ m( v+ψ dt d ˙ = (Ff + δ S f )l f − Fr lr izz ψ dt σ d Fr + Fr = FNLT (λr )Nr u dt σ d Ff + Ff = FNLT (λ f )N f u dt m(

Figure 6.

(2) (3)

and steering angle execution two additional equations are used as follows:

(4) (5)

d S = vS , dt

(6)

mgl f hD u2 + h(Sr + S f ) + L L 2 mglr hD u + h(Sr + S f ) − N f (t) = L L ˙ f +v ˙ r +v ψl ψl , λf = − +δ λr = − u u

(7)

(12)

Motion Planner: road scenario. This module addresses the problem to find the optimal motion of a vehicle running on a road. Hence the problem is formulated in the space domain with a curvilinear abscissa ζ used to describe the main line of the road, which, in turn, is described by segments of straight lines and arc of circles as shown in Fig. 6. Discontinuities are possible on curvature κ(ζ), lane width (wle f t (ζ), wright (ζ) ), road surface adherence (µ(ζ)) (i.e. they are function of the curvilinear abscissa and are considered constant or linear variables of ζ on each segment). Eqn. (2-6) and (12) are re–written in the space domain and three additional equations are added to define the vehicle position on the road:

(8) (9)

It is worth noting that the load transfer due to longitudinal acceleration is considered. Moreover the longitudinal forces are computed on the basis of the single input force S as follows: 1 Sr = max(S, 0) + min(S kbr , 0) 2 1 S f = min(S (1 − kbr ), 0) 2

d δ = vδ dt

where vS and vδ are the rate of change of the longitudinal force and steer angle and are the actual inputs that must be minimized in the optimal control problem. Hereafter, the proposed formulation and solution of the finite horizon optimal control will be explained detailed for the two different modules.

The algebraic equations that respectively define the rear and front normal loads and the rear and front sideslip angles are:

Nr (t) =

ROAD MODEL AND CURVILINEAR COORDINATES VARI-

ABLES THAT DESCRIBE VEHICLE POSITION ON THE ROAD

(10) (11)

1 d ζn = v cos(α) + u sin(α) Td dζ 1 d α = dotψ − κ(ζ) Td dζ κ ζn − 1 Td = v sin(α) + u cos(α)

Let us note that only the rear axle applies traction forces while the total braking force is subdivided between front and rear axles according to the braking ratio kbr . The vehicle model is controlled by means of two inputs: the steering angle δ and the longitudinal force S. To include mechanical delays in the force generation 6

(13) (14) (15)

c 2008 by ASME Copyright

●

J=

Z ζ0 +`P

100

●

●

● ●

●

50

Traction force (N)

150

where all the state variables are now function of curvilinear abscissa ζ. Finally, the full vector of state variables is: z(ζ) = ˙ [u(ζ), v(ζ), ψ(ζ), ζn (ζ), α(ζ), Fr (ζ), Ff (ζ)]T and the control variables are: u(ζ) = [vS (ζ), vδ (ζ)] The performance index (or target function) to be minimized is a weighted sum of the minimum time Td and of a two dimensional function fa of the lateral (ay ) and longitudinal (ax ) acceleration of the centre of mass of the vehicle Eqn. 16. Function fa defines the pattern of comfort or safe accelerations and are related to each driver preferences [1]. w1 ∈ [0, 1] and w2 ∈ [0, 1] define the ratio of the above two objectives.

● ●

0

● ●● ● ●

●

0

5

10

15

Speed (m/s)

Figure 7.

EXAMPLE OF MAXIMUM TRACTION FORCE AT THE REAR

WHEELS AS A FUNCTION OF FORWARD SPEED

w1 Td (ζ) + w2 fa (ax (ζ), ay (ζ))dζ

(16)

ζ0

[13]. A two-point boundary-value problem (TPBVP) is obtained from the first-order necessary conditions by the calculus of variations [11]. By discretizing the resulting Differential Algebraic Equation (DAE) a large highly non linear system is obtained. A fast and robust numerical algorithm for solving in real-time the non linear system was developed. Further details on adopted optimal control formulation and solution are reported in [13].

where `P is the planning distance and ζ0 is the initial fixed position. The minimization problem is subject to the ordinary differential equations (ODE) that represents the vehicle dynamic model described by Eqn. 2-6, and 12: z0 (ζ) = f [z(ζ), u(ζ)]

(17)

Motion Planner: off-road scenario. The motion planner module for the off-road scenario is intended to be used in an unstructured environment where the road geometry is not available. The vehicle position is defined in cartesian coordinates and the system of equations defined by Eqn. (2–6) is incremented by the following three new equations:

with the boundary conditions (BC): b [z(ζ0 ), z(ζ f )] = 0

(18)

where z(ζ0 ) are the vehicle state estimation, which represents the feedback term in the scheme, and z(ζ f ) are the desired terminal constraints, which are set according to safety criteria or driver intention. Additionally some trajectory or physical model constraints on states and controls are defined: c [z(ζ), u(ζ)] ≤ 0

d x = v cos(ψ) + u sin(ψ) dt d y = −v sin(ψ) + u cos(ψ) dt d ˙ ψ=ψ dt

(19)

In particular, the trajectory constraints include limitations on the controls: |vS (ζ)| ≤ vSMax , |vδ (ζ)| ≤ vδMax . Moreover, the maximum traction force is put as a constraint in the optimization problem with the following inequality: S(ζ) ≤ TrMax where TrMax is a function of the forward speed and represents the maximum traction force available at the rear wheels for a given speed (Fig. 7). The constraint requires the longitudinal force S(ζ) to be less than the maximum value however throttling is obtained as part of the optimal solution. The maximum traction force curve is determined with a sequence of experimental tests in stationary conditions. All inequalities are treated with penalty functions and are added at the performance index w1 Td (ζ) + w2 fa (ax (ζ), ay (ζ))

(20) (21) (22)

In this case the problem is re-formulated from time to a normalized abscissa s ∈ [0, 1] as independent coordinate by the transformation: t = s T (s) + t0

(23)

and introducing the differential equation T 0 (s) = 0 to be added to the other ODE equations. Similarly to the road scenario the function to be minimized is a weighted sum of the minimum time 1 and of a cost function of the pattern accepted accelerations fa , therefore the performance index of the finite horizon 7

c 2008 by ASME Copyright

optimal control is now: Z 1

J = w1 T (1) +

0

w2 fa (ax (s), ay (s)) ds

(24)

CONTROL AND ACTUATION The Planner object in Fig. 3 provides the Manager Unit with the planned reference optimal manoeuvre, which is a time description of the vehicle states from the present time to the final time. Each vehicle state variable is provided as a spline, which can be interpolated by the Manager Unit at the needed time, until the reference manoeuvre becomes invalid —because of the accumulated errors— and is thus replaced by an updated, freshly computed reference manoeuvre. In particular, the Manager Unit interpolates —at each step of the loop in Fig. 4— the two reference variables used by the control loops i.e. longitudinal speed and yaw rate, as explained hereafter. As previously described, RUMBy allows the command of steering angle and throttle. These, in turn, must be properly controlled, in order to track the reference trajectory provided by the planner, in terms of longitudinal speed and yaw rate. Of course, there is a rather complex coupling between these two system variables, especially for what concerns the yaw rate. In fact, while the longitudinal speed is mainly influenced by throttle, changes in yaw rate are influenced not only by the steering angle, but also by the longitudinal speed and the yaw rate itself. These three quantities, as shown in the equations describing the vehicle dynamics, have the greatest influence on the lateral force, which, in turn, alters the yaw rate. Taking into account the above consideration, a simplified model of the vehicle dynamics has been used to justify the implementation of a decoupled controller, with yaw rate and longitudinal speed controlled by acting on steering angle and throttle, respectively. The block diagram of such simplified system is shown in upper part of Fig. 8, where the yaw rate dynamics is clearly influenced by the non-linear function f (·). This is a static mapping that relates the current values of steering angle, longitudinal speed and yaw rate to the lateral force, which, in turn, determines the torque around the yaw axis. So, for a given operating point (in terms of yaw rate and longitudinal speed) the yaw acceleration is mainly influenced by the steering angle, through a gain that can be obtained by linearizing f (·). As for the influence of the longitudinal speed and all other variables, it can be considered as a disturbance, its effect to be canceled by an external controller. It can be demonstrated that such disturbances are mainly in the low frequency range, so a PI control on the steering angle is suitable to achieve the target of low tracking error of the yaw rate set point provided by the planner. For each operating point, a suitable PI controller has been designed and all the controller gains obtained stored in a look-up table, to be used in a gain-scheduling scheme during operation. A

Figure 8.

SCHEMATIC BLOCK DIAGRAM OF CONTROL LOOPS FOR

THROTTLE AND STEERING (TOP) AND ITS DECOUPLED VERSION (BOTTOM)

simpler approach is to use the worst-case controller, i.e. the controller designed for the largest value of f (·). In both cases, the influence of the longitudinal speed on the yaw rate is regarded as a disturbance, its effect canceled by the PI controller. As for the longitudinal dynamics, it can be considered as fairly decoupled from the yaw one. This leads to the “natural” choice of controlling the longitudinal speed by acting on the throttle. The only problem arising in designing the controller is caused by highly non-linear behavior of the centrifugal clutch, which may induce limit cycles in the system. As a result of the above considerations, the inner loops of the RUMBy system have been implemented as shown in bottom of Fig. 8, where a PI controller has been used for the yaw rate, and a PID controller for the longitudinal speed. It is worth noting that both PI and PID should be tuned for each operating point, but some simplification may lead to a simpler design. In particular, since the NRH planner computes the trajectories with a minimum time objective, one can assume that the vehicle always runs at the highest possible speed. Consequently, all the closed loop gains can be assumed as constants and tuned for the worst case (i.e., high dynamics). In terms of implementation, both the steering PI and the throttle PID are controllers with integral tracking and variable gains. Since the controls run within the main loop of Fig. 4, set point, process variables, and output are updated at the same sampling period of the main loop, which is set to 20 ms. This value has been tuned as a trade off with the dynamics of the vehicle, the rate of the radio control communication performance —which was measured as about 5 ms—, and the average task execution time on the available hardware (about 10 ms on an Intel Core 2 Duo 2.33 GHz). As for the set points for the inner loops, they are provided 8

c 2008 by ASME Copyright

20

30

40

50

60

●

●

●

● ●

0

Reference Smoothed

20

40

60

80

100

0.5

x (m)

TEST TRAJECTORY IN OFF-ROAD SCENARIO

−0.5

Figure 10.

−1.5

Commands (V)

1.5

Time (s)

0

10

20

30

40

50

After this, autonomous drive manoeuvres have been tested. In one of these test, the vehicle is set to start at the origin with heading North, and the planner is initialized with a final target position at (100,0) m and heading North (to be reached with zero speed). The planner, configured for an off-road scenario, is requested to calculate the minimum time manoeuvre, i.e. having the minimum time as the only objective. Figure 10 depicts the vehicle trajectory with five superposed planned trajectories (dashed lines). Each of them is the result of a new plan that has been computed when the position error exceeded the given threshold, as described in the previos sections, and that became available to the Manager Unit in correspondence of the small circles. It is evident that, the actual trajectory gradually diverges w.r.t. the planned ones. This is of course due to the much simpler vehicle model used by the planner w.r.t. the one that runs within the Simulator Unit. In particular, the planer does not consider the load transfer between front and rear axles and the consequent under- or over-steering behavior. Nevertheless, the re-planning strategy allows to catch up with this issue and gradually drive the vehicle towards the desired position and orientation with a good accuracy. Figure 11 shows the time profiles of the vehicle speeds and of the throttle and steering controls. Regarding the speed profile, it is worth noting that at the beginning of the manoeuvre there is a sudden increase and a following oscillation of the planed speed, because of the MSC strategy that enables the planer only when the vehicle speed becomes greater than 1 m/s. Then, the discontinuity associated with the clutch skid causes a oscillation in the longitudinal acceleration, which, in turn, triggers a re-planning and thus the saw-tooth aspect of the black curve. This also reflects in the oscillation of the throttle command within 0–4 s. At the end of the manoeuvre, the final constant segment of the planned speed curve is due to the fact that the Manager Unit switches to a full brake mode when the distance to the target falls below the typical braking distance (in this case, about 5 m). In this full brake mode, the Manager Unit also drives the steering command by setting the reference yaw rate to zero, and this causes the well evident glitch in steering command at about 13 s.

60

Time (s)

Figure 9.

Trajectory Planned trajectories

−30

10

10 20 30 40

y (m) 0

−10 0

10 6 4 0

2

Speed (m/s)

8

Reference Actual

LONGITUDINAL DYNAMICS TEST FOR TUNING OF

THROTTLE CONTROL GAINS

by the optimal manoeuvre algorithm only when the longitudinal speed is higher than a certain value and the distance from the arrival is larger than 5 m. In particular, within the low speed range (less than 1 m/s), the optimal manoeuvre algorithm —on which the above described planner is based— exhibits bad performances. For this reason, in this range the set point for the longitudinal speed comes from a look up table and the yaw rate set point is zero (i.e. go straight). If precise positioning is needed, the set points can be eventually provided by other controllers more suitable for purely kinematic conditions (e.g. PC-Sliding controller, as introduced in [14]). In the medium-high speed range, instead, the set points for the yaw rate and longitudinal speed controllers are directly provided by the planner. Finally, when the vehicle approaches the arrival, the maximum braking command is issued, while the reference to the yaw rate is again set to zero. In practice, two different outer loops are implemented and the switch between the two is driven by the longitudinal speed and distance from the arrival.

RESULTS The first activity has been the tuning of the control loop gains in standard manoeuvres throughout the well known Ziegler-Nichols method. This has been performed by separately tuning the throttle gains with manoeuvres only involving longitudinal dynamics (see Fig. 9), and the steering gains by steady state turning manoeuvres at different curvatures and at maximum speed. Thanks to RUMBy’s design, these tests have been firstly performed in SIL, then in HIL configuration, thus reducing the risks. 9

c 2008 by ASME Copyright

10 5

[2] 0

Speed (m/s)

15

Planned Actual

0

5

10

[3]

15

Time (s)

0.5

Steer Throttle

−0.5

[5]

−1.5

Command (V)

1.5

[4]

0

5

10

[6]

15

Time (s)

Figure 11.

VELOCITY AND CONTROLS TIME PROFILES

[7] CONCLUSION The paper presents the design and the implementation of an autonomous vehicle able to plan and execute accurate optimal manoeuvres both in normal and in critical driving situations. The design of the mechatronic system has been focused on modularity and re-configurability, thus devoting a big effort in the design of interfaces. The result is a research platform that not only allows a fast switching from HIL to SIL and to fullhardware tests, but also exploits automatic code generation techniques in order to make the implementation within the control and manoeuvre planning software of a symbolic dynamic model written in Maple a matter of few minutes. Further work will focus on implementation of the alternative connection in Fig. 2 (i.e. switching between manual/autonomous drive) and on the automatic identification of dynamic parameters of the vehicle by means of neural networks, mimicking the ability of human drivers to adapt their driving style on the response characteristic of different vehicle or different configurations of the same vehicle (e.g. different mass, different tyre pressures, and so on).

[8] [9]

[10]

[11] [12]

[13]

[14] ACKNOWLEDGMENT Thanks go to Roberto Oboe, Mirko Paoletto, and Fabrizio Zendri for their invaluable help.

vers for an adas of the next generation”. In Intelligent Vehicles Symposium, 2005. Proceedings. IEEE, Vol. 94, pp. 36– 41. Various authors, 2008. INSAFES: PReVENT Horizontal Project. http://www.preventip.org/en/prevent subprojects/horizontal activities/insafes/. Various authors, 2008. PReVENT EC Founded Project. http://www.prevent-ip.org. Various authors, 2008. SASPENCE: PReVENT Vertical Project. http://www.prevent-ip.org/en/prevent subprojects/ safe speed and safe following/saspence/. Falcone, P., Borrelli, F., Asgari, J., Tseng, H. E., and Hrovat, D., 2007. “Predictive active steering control for autonomous vehicle systems”. Control Systems Technology, IEEE Transactions on, 15(3), pp. 566–580. Best, M. C., Gordon, T. J., and Dixon, P. J., 2000. “An extended adaptive kalman filter for real-time state estimation of vehicle handling dynamics”. Vehicle System Dynamics, 34, pp. 57–75. Da Lio, M., and Lot, R., 2004. “A symbolic approach for automatic generation of the equations of motion of multibody systems”. Multibody System Dynamics, 12(26), pp. 147–172. Various authors, 2008. SWIG: Simplified Wrapper and Interface Generator. http://www.swig.org. Shim, T., and Ghike, C., 2007. “Understanding the limitations of different vehicle models for roll dynamics studies”. Vehicle System Dynamics, 45(3), pp. 191–216. Demerly, J., and Youcef-Tourni, K., 2000. “Nonlinear analysis of vehicle dynamics (navdyn): A reduced order model for vehicle handling analysis”. SAE Technical Paper Series(SAE 2000-01-1621). Bryson, A. E. J., and Ho, Y. C., 1975. Applied Optimal Control. Hemisphere, Washington, DC. Ohtsuka, T., and Fuji, H. A., 1997. “Real-time optimization algorithm for nonlinear receding-horizon control”. Automotica, 33(6), pp. 1147–1154. Bertolazzi, E., Biral, F., and Da Lio, M., 2007. “Real-time motion planning for multibody systems: Real life application examples”. Multibody System Dynamics, 17(2-3), pp. 119–139. De Cecco, M., Bertolazzi, E., Miori, G., and Oboe, R., 2007. “PC-SLIDING for vehicle path planning and control. design and evaluation of robustness to parameters change and measurement uncertainty”. In ICINCO2007 World Congress, Angers, 9-12 May.

REFERENCES [1] Biral, F., Da Lio, M., and Bertolazzi, E., 2005. “Combining safety margins and user preferences into a driving criterion for optimal control-based computation of reference maneu10

c 2008 by ASME Copyright