Design and implementation of the low-level control ... - IEEE Xplore

Design and implementation of the low-level control system of an All-Terrain Mobile Robot Luca Bascetta, GianAntonio Magnani, Paolo Rocco and Andrea Maria Zanchettin Dipartimento di Elettronica e Informazione Politecnico di Milano Piazza L. Da Vinci, 32, 20133, Milano, ITALY {bascetta, magnani, rocco, zanchettin}@elet.polimi.it

Abstract—This paper presents a first step towards the design and development of the control system of an All-Terrain Mobile Robot. Different requirements are taken into account with the aim of setting up an experimental test-bed to support research on autonomous vehicles and on electronic devices to enhance the safety in presence of human riders. Since the vehicle should remain drivable, both the design of the hw/sw architecture and the choice of a proper perception and actuation system are particularly critical. The selected perception and actuation system is described here, pointing out the choices that were made in order to satisfy those requirements. The description of the steering and the throttle control systems together with the choice of the control laws are presented. Finally, the tuning of such control laws and some experimental results are reported in order to show the possibility to perform manoeuvres with the same dynamics of a human rider.

I. I NTRODUCTION The popularity of the research on wheeled mobile robots has been recently increasing, due to their possible use in different outdoor environments. Planetary explorations, search and rescue missions in hazardous areas [1], surveillance, humanitarian demining [2], as well as agriculture works such as pruning vine and fruit trees, represent possible applications for autonomous vehicles in natural environments. Differently from the case of indoor mobile robotics, where only flat terrains are considered, outdoor robotics deals with all possible natural terrains. The unstructured environment and the terrain roughness, including dynamic obstacles [3], and poorly traversable terrains, make the development of an autonomous vehicle a challenging problem. The aim of our research is to develop an All-Terrain Mobile Robot (ATMR), equipped with a robotic manipulator, suitable for a wide range of different outdoor operations. The ATMR should be able to operate in any natural environment with a high level of autonomy. The advantage of using All-Terrain Vehicles (ATVs) is represented by their good traversability potential for poorly traversable terrains and by the short time spent for reaching the goal, as well as by the possibility to operate in unsafe environments. On the other hand, the main disadvantage of ATVs is their low stability margin due to dynamic constraints, roll-over and excessive side slip [4]. In fact, ATVs are highly unstable, especially during fast turns and uphill/downhill riding, and a roll/tip-over can often occour. To overcome those problems the development of some

active control systems [5], and in particular an Anti-Rollover System (ARS) [6], would certainly enhance their drivability and becomes necessary once the vehicle is teleoperated or autonomous. The design and development of an All-Terrain Mobile Robot is a challenging task, especially when a high level of autonomy is required. Indeed, due to the complex tasks the robot is supposed to perform, the design of the entire control architecture is anything but trivial [7]: different kind of requirements come from software engineering (e.g. modularity or maintainability), control theory (e.g. stability, robustness, hard real-time-ness) and mobile robotics (e.g. path planning, obstacle avoidance). The hw/sw architecture should fulfill them all in the simplest way. A natural way to achieve those requirements is to design a multi-layered software architecture, in order to map higher levels of algorithmic abstraction to the top layers of the architecture. The control level that will act as interface from these high level tasks (actions planning, goals prioritizations, etc.) and the vehicle itself will be called “virtual rider”. The aim of the virtual rider is to interpret commands from planner and execute them avoiding dangerous manoeuvres that could result in instability. Together with the virtual rider algorithm, a low level control software will be necessary in order to execute simple commands such as steering or braking. This paper presents the first step towards the implementation of a safe autonomous ATMR and concerns the development of the lowest layer of the control architecture. The ATMR will be used as an experimental setup for various research projects. One of them deals with the enhancement of the drivability and safety of commercial ATVs, in order to make the vehicle safe and easily rideable for elderly or disabled people, and improve off-road vehicle stability, making the vehicle suitable for farming tasks on uneven terrains [8]. Thus, it is important that the vehicle, that is supposed to be teleoperated and autonomous as well, should remain drivable by a human rider. This fact estabilishes a constraint on the actuation system design because the reversibility must be guaranteed. The remaining of the paper is organized as follows. In Sections II and III a brief description of the vehicle and an overview of the system architecture are presented. In Section IV actuation and sensor systems, together with the control system design, are presented. Finally, some experimental results are shown in Section V.

II. V EHICLE DESCRIPTION The vehicle studied in this research (see Fig. 1) is a YAMAHA G RIZZLY 700, a commercial fuel powered AllTerrain Vehicle (ATV) equipped with an electric power steering (EPS). The EPS unit consists of a permanent magnet DC motor mounted on the steering column through a screw-pinion transmission. Consider that this unit was designed in order to assist the rider manoeuver with a contribution to the steering torque that is limited to the 35% of the maximum steering torque. The G RIZZLY 700 is a utility ATV and is thus specifically designed for agriculture work. As a result it is equipped with a front and a rear rack having a total load capacity of 130 Kg, and with a tow hook. The main characteristics of the vehicle and the EPS unit are listed in Tables I and II, respectively. A multibody model of this vehicle, in the object oriented Modelica language has been derived in a preliminary study and is described in [9]. Main characteristics of the vehicle Engine type 686cc, 4-stroke, liquid-cooled, 4 valves Drive train 2WD, 4WD, locked 4WD Transmission V-belt with all-wheel engine braking dual hydraulic disc (both f/r) Brakes Suspensions independent double wishbone (both f/r) Steering System Ackermann Dimensions (LxWxH) 2.065 x 1.180 x 1.240 m Weight 296 Kg (empty tank) Table I V EHICLE CHARACTERISTICS

EPS characteristics motor type PM DC motor pole pairs, p 2 0.4 Ω stator resistance, R stator self-inductance, L 0.3 mH torque ratio, Kt 0.045 Nm/A screw-pinion gear ratio, n 34 Table II C HARACTERISTICS OF THE EPS SYSTEM

III. C ONTROL SYSTEM ARCHITECTURE In order to make the vehicle teleoperated, or even autonomous, an on-board hw/sw control platform has to be added. While the design of the whole architecture is still under development, a functional diagram that shows the main components of the control system and their relationships is shown in Fig. 2. The architecture can be divided into three different layers. The top level is a high level planner responsible for the task acquisition and for the medium-long range navigation and planning functionalities. The virtual rider is an intermediate level and is responsible for short range navigation, planning and vehicle stabilization. It has to ensure vehicle integrity with respect to rollover/tipover instabilities, obstacles and terrain traps, etc., replacing the typical low-level riding skills of a human. The lower level, which this paper refers to, represents an interface between the vehicle commands and the virtual rider. Such level interacts with the vehicle measuring the steering angle, throttle ratio, vehicle speed, etc., and acting on the steering column, the throttle leverage and/or the brake pedal through a sensor and actuation system which is described in Section IV. To implement such a complex architecture that includes high level and low level tasks, the former characterised by an heavy computational load but slower sampling frequencies, the latter being simpler but needing a faster time response, a multilayered and multiprocessor hw/sw architecture is required. In this way, one can separate complex (localization and navigation on rough terrains, obstacle avoidance, sensor fusion, etc.) from simple tasks (motion control and servo actuation) and faster from slower ones. The hw/sw architecture should be as modular as possible, in order to be simply reconfigurable and upgradeable. Indeed, the different computational complexity of the tasks calls for different layers of the control system, thus a multiprocessor architecture is an obvious choice. On the other hand, the navigation control system requires the complete knowledge of the state of the vehicle (in terms of what the sensors are perceiving) to take the best decision autonomously. Thus a very large amount of data must be shared between the system’s layers. The selected hardware architecture (Fig. 3) consists of: •

•

Figure 1.

The Yamaha Grizzly 700 ATV

a low level CPU with several I/O modules to perform the control of the steering angle, the throttle position, the pressure of the hydraulic braking circuit, etc. An industrial PLC provided by B&R AUTOMATION (X20 CPU: Celeron 650, 64 MB DRAM, 1 MB SRAM, maximum bus frequency 2 kHz) was selected for its dependability and robustness. Indeed, the choice of a PLC is a good compromise between the hard real-time requirement and the possibility of high level programming. a high level PC to implement the high level algorithms: the so called “virtual rider” (vision, terrain perception, localization and mapping, obstacle and rollover avoidance, etc.), the medium-long range navigation an planning, etc.

Figure 2.

Functional diagram of the controller architecture

For this purpose, an Industrial PC (2.16 GHz Intel Core Duo T7400, 2 MB L2 cache, 1024 MB DDR2 RAM, 5 PCI slots) provided by B&R AUTOMATION was selected. An E THERNET P OWERLINK communication link was selected to connect the two CPUs in order to cope with the hard-real time nature of the application (see [10] for further details on the P OWERLINK protocol).

(a) Architecture

IV. S ERVO LEVEL CONTROL SYSTEM In this section the actuation and the measurement systems for the steering angle and the throttle control are described. The braking system has been already designed and developed. However, it was not yet possibile to carry out an adeguate number of experiments. For this reason, the braking control system is not dealt with in this paper. Remember that, the actuation systems added to autonomously control the vehicle has to be designed in order to keep it ridable by humans as well. A. Steering system In order to preserve the vehicle manual drivability, the steering feedback controller acts directly on the DC motor of the vehicle EPS. A common controlled H-bridge (having a switching frequency of 20 kHz) and a current transducer (LEM) have been mounted in order to modulate the motor input voltage and measure the motor current, respectively. Moreover, an incremental encoder with 8000 ppr (pulse per revolute) was connected at the end of the steering column to measure the steering angle. A block diagram of the steering control system is shown in Fig. 4: a common cascaded control is adopted, composed of an inner and large bandwidth current loop and an outer angle control loop. The steering angle control algorithm is implemented on the B&R AUTOMATION X20 CPU and runs with a frequency of 1 kHz, while the current control algorithm, which needs a faster sampling rate, is allocated to a dedicated DSP with a sample frequency of 5 kHz.

(b) Encoder on the steering column

(c) Screw-pinion gear, EPS

Figure 4. Steering system architecture and details of the perception/actuation system

A mixed digital/analog communication link connects the two hardware platforms. The analog channel is actually used to complete the cascaded control scheme, the digital link allows to share the inner controller status (e.g. voltage saturations). 1) Current controller: The current controller implements a common proportional-integral (PI) control law, according to the following expression: Z t

v = K pi ei + Kii

0

ei (τ) dτ

(1)

where v is the applied voltage, K pi and Kii are the proportional and integral gains, respectively, and ei = ei (t) = ire f (t) − i (t) is the current error, ire f and i being the reference and the actual currents, respectively. Consider the electrical dynamics describing the relationship

Figure 3.

Hardware architecture

between the applied voltage v and the motor current i: v = Ri + Ldi/dt + E where R and L are the winding resistence and self-inductance, respectively, and E is the back-EMF term. According to the current control law in (1) the PI controller has been tuned in order to obtain a fast current response and a satisfactory current ripple rejection. 2) Position controller: As previously stated, an angular position loop is cascaded to the current one. The position controller is a standard proportional-integral-derivative (PID) law:
Z t d cθ re f − θ re f (2) i = K ps eθ + Kis eθ (τ) dτ + Kds dt 0 where K ps , Kis and Kds are the proportional, integral and derivative gains, respectively, 0 < c ≤ 1 is the weight on the set-point, and eθ = eθ (t) = θ re f (t) − θ (t) is the angular position error, θ re f and θ being the desired and the actual steering angles, respectively. The derivative term has been implemented with a discrete fixed-time ( fs = 1/Ts = 100 Hz) derivative D (z) =

−i ∑N−1 i=0 ai z Ts

steering torque. Therefore, a simple mass-spring-damper can be a rough but appropriate approximation of the dynamical behaviour between the steering torque and the steering angle. For such systems a PID control law is necessary to achieve a well-damped response and zero steady-state error. The steering angle controller has been tuned empirically, since an accurate model of the tyre/road interaction was not available. The four gains in (2) have been assigned in order to achieve a smooth response without overshoot (K ps = 300, Kis = 800, Kds = 2, c = 0.1, S.I. units). According with the selected tuning, the time history of a ramp response of the steering angle, with the vehicle running at approximately 5 Km/h, is shown in Fig. 5.

(3)

where z−1 is the unit delay, N is the order of the filter and the coefficients ai are chosen according to a Least Square Fit (LSF) Filter (see [11]). Some words can be spent to justify the choice of a PID controller for the steering angle instead of the common P/PI cascaded control. The cascaded position/velocity control is suitable for standard servomechanisms whose dynamics can be essentially described through a low pass filter (between the torque and the velocity) and an integrator (between the velocity and the position). Considering the common description of tyre/road interaction in terms of lumped forces, see for instance [12], one can see that the forces that are related to the steering angle can be equivalently described in terms of a momentum acting on the steering column. In the literature, such momentum is usually referred to as “self-aligning torque” (SAT). The relation between the SAT and the steering angle is strongly nonlinear and always depends on the vehicle dynamics. For small steering angles and small vehicle velocities, however, a simple proportional relationship can be assumed between the SAT and the steering angle. Thus, one can refer to the SAT like an equivalent spring that cause an external torque that is opposed to the

Figure 5.

Closed loop steering angle ramp: reference (black), actual (gray)

B. Throttle system The throttle system includes the throttle valve, a return spring, a pulley and a bowden cable connecting the pulley to the throttle leverage positioned on the handlebar. A stepper motor (data listed in Table III) was mounted directly on the leverage axis (see Fig. 6). Since the behaviour of a throttle valve is usually complicated and strongly nonlinear, see for instance [13] and [14], the choice of a stepper motor results in a good compromise between the controllability and the need to accurately model the behaviour of the valve, which is not our main intent. An off-the-shelf rotative potentiometer mounted directly on the throttle valve was used as feedback. A block diagram of the throttle system is shown in Fig. 6. A PLC module is responsible for the current regulation of the stepper motor. In particular, it works as a feedforward velocity controller with a sampling rate of 2 kHz, i.e. the

(a) Throttle actuation system

(b) Leverage Figure 6.

(c) Stepper motor

Throttle system architecture

same of the bus frequency. During the bus-cycle, such module generates a sequence of S current pulses that generates an angular displacement of S µsteps. Every bus-cycle the motor velocity is modulated by selecting the value of S. This way it is thus possibile to modulate the motor velocity. A cascaded position control loop is used to control the valve opening. Since the velocity is controlled, the best position control is a simple proportional (P) controller: Ω = K pv ev

(4)

necessary to limit the reference velocity of the stepper motor according to the current limitations of the drive module, the quantizer block represents the finite resolution of the speed command. Since Ω is the velocity reference expressed in µstep/bus − cycle, the gain a converts that value into S.I. units, i.e. in rad/s and the deadzone block is necessary to accurately reproduce the significant backslash between the motor position and the valve opening (as it will be clarified in the following). On the other hand, the gain n takes into account the ratio between the leverage position, in rad/s, and the throttle opening h in the range [0, 1]. Simple experiments have been done in order to identify the unknown parameters of the model. Then a series of openand closed-loop step responses have been carried out both on the experimental setup and on the model. The results of the validation are shown in Figs. 8 and 9. In particular Fig. 8 refers to an open-loop velocity step of 10 µstep/bus − cycle given at time 7 s and released, i.e. set to zero, after 0.2 s. The delay between the velocity command and the response of the throttle position is due to backlash. The consistency, in terms of slope and delay, between the model and the experimental response ensures that the parameters were correctly indentified. In Fig. 9 the closed loop step response (K pv = 100 µstep/bus− cycle) is shown. The proportional gain of the throttle position regulator, see (4), was choosen (K pv = 250 µstep/bus − cycle) in order to obtain a satisfactory cut-off frequency and a reasonable phase margin according to the model presented before.

where ev = ev (t) = hre f (t) − h (t) is the error on the valve position, hre f and h being the desired and the actual angular opening positions (normalized to 1), respectively. An analytStepper motor characteristics motor steps, k 200 2.4 Ω stator resistance, R stator self-inductance, L 8.8 mH stall torque, τstall 2.2 Nm µstep size, ∆ 7 · 10−3 deg Table III C HARACTERISTICS OF THE STEPPER MOTOR Figure 8.

Open loop velocity step: model (black), actual (gray)

V. E XPERIMENTAL RESULTS

Figure 7.

Model of the throttling system - block diagram

ical model of the throttle system has been developed in order to support the tuning of the controller, see the block diagram in Fig. 7. The actual control law (i.e. the relationship between ev and Ω) is quite different from the one in (4). However, the modifications are quite obvious: the saturation element is

To verify the overall servo level control system a simple experimental test was performed. First, a rider drove the vehicle in a smooth 30 m long path at about 10 Km/h. During the experiment, the control system was simply acquiring the steering angle and the throttle position. Then, the acquired data was used as reference for the servo level control system, which tried to reproduce the rider manoeuvres. The time histories of the steering angle and the throttle valve position are shown in Figs. 10 and 11, respectively.

Figure 9. (gray)

Closed loop step: setpoint (dashed black), model (black), actual

that were made to keep the vehicle drivable by a human rider as well. Some experimental results have been presented emphasizing the possibility to replicate the typical driving dynamics of a human rider. Further research concerns the development of an accurate model for the steering system, i.e. a simple law to describe the tyre/road interaction, in order to achieve a more robust tuning of the steering controller. In fact, the experimental test was done on dry asphalt and no additional tests were done to verify the robustness with respect to different road surfaces (e.g. grass, sand, mud, etc.). Finally, research is being carried out in order to complete the actuated braking system. ACKNOWLEDGMENTS This work has been partly supported by A ERO S EKUR S . P. A . and B&R AUTOMATION. R EFERENCES

Figure 10.

Figure 11.

Steering angle: reference (black), actual (gray dashed)

Throttle position: reference (black), actual (gray dashed)

As one can see, the servo level control system achieves good performance in terms of tracking of these two variables. Therefore, the steering and throttle control is able to lead the vehicle with the same dynamics of a human rider (see [15], [16]). Notice that, in this paper only the hardware choices, the design of the servo level controller and its tuning are dealt with. Therefore, only the effectiveness of the control laws and their tuning ware verified through the experimental tests. In other words, there was no claim to have the trajectory of the vehicle repeatable because, according to the functional architecture in Fig. 2, such kind of skill is responsibility of the higher levels of the controller. VI. C ONCLUSIONS The control system architecture for an All-Terrain Mobile Robot has been discussed, pointing out the hardware choices

[1] A. Garcia Cerezo, A. Mandow, J. Martinez, J. Gomez de Gabriel, J. Morales, A. Cruz, A. Reina, and J. Seron, “Development of ALACRANE: A mobile robotic assistance for exploration and rescue missions,” in International Workshop on Safety, Security and Rescue Robotics, Rome, September 2007. [2] P. Debenest, E. Fukushima, and S. Hirose, “Proposal for automation of humanitarian demining with buggy robots,” in International Conference on Intelligent Robots and Systems, IROS 2003, Las Vegas, 2003. [3] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” International Journal of Robotics Research, 1986. [4] R. Lenain, B. Thuilot, C. Cariou, and P. Martinet, “Backstepping observer dedicated to tire cornering stiffness estimation: Application to an all terrain vehicle and a farm tractor,” in International Conference on Intelligent Robotics Systems, IROS 2007, San Diego, October/November 2007. [5] J. van der Burg and P. Blazevic, “Anti-lock braking and traction control concept for all-terrain robotic vehicles,” in International Conference on Robotics and Automation, ICRA 1997, Albuquerque, April 1997. [6] N. Bouton, R. Lenain, B. Thuilot, and J. Fauroux, “A rollover indicator based on the prediction of the load transfer in presence of sliding: application to an all terrain vehicle,” in International Conference on Robotics and Automation, ICRA 2007, Rome, April 2007. [7] H. Utz, S. Sablatnög, S. Enderle, and G. Kraetzschmar, “Miro- middleware for mobile robot applications,” IEEE Transaction on Robotics and Automation, 2002. [8] R. Lenain, B. Thuilot, C. Cariou, and P. Martinet, “A new nonlinear control for vehicle in sliding conditions: application to automatic guidance of farm vehicles using RTK GPS,” in International Conference on Robotics and Automation, ICRA 2004, New Orleans, April/May 2004. [9] F. Donida, G. Ferretti, G. Magnani, and M. Zampini, “Object-oriented modelling and simulation of an ATV,” in International Conference on Noise and Vibration Engineering, ISMA 2008, Leuven, September 2008. [10] “http://www.ethernet-powerlink.org/.” [11] P. Carpenter, R. Brown, J. Heinen, and S. Schneider, “On algorithms for velocity estimation using discrete positione ncoders,” in International Conference on Industrial Electronics, Control, and Instrumentation, IECON 1995, Orlando, November 1995. [12] H. Pacejka, Tyre and vehicle dynamics, 2nd ed. [13] A. F. Contreras, I. P. Quiroz, and C. Canudas de Wit, “Further results on modelling and identification of an electronic throttle body,” in Mediterranean Conference on Control and Automation, MED 2002, Lisbon, July 2002. [14] C. Canudas de Wit, I. Kolmanovsky, and J. Sun, “Adaptive pulse control of electronic throttle,” in American Control Conference, Arlington, June 2001. [15] D. McRuer and E. Krendel, “Mathematical models of human pilot behaviour,” System Technology, Inc., Tech. Rep., 1974. [16] F. Forouhar, “All-terrain vehicles frequency domain response analysis and rider behaviour,” in International Conference on Robotics and Automation, ICRA 1997, Hartford, October 1997.