A simple and efficient control scheme to reverse a ...

1

A simple and efficient control scheme to reverse a tractor-trailer system on a trajectory C´edric Pradalier and Kane Usher CSIRO ICT Centre, Autonomous Systems Laboratory Brisbane, Australia Email: [email protected]

Abstract— Tractor-trailer reversing is a problem frequently considered in the literature, usually with fairly complex non-linear control theory based approaches. Our prior work in this area has included the development of a very simple and efficient trajectory controller based on the notion of controlling the hitch-angle of the trailer rather than the steering angle of the tractor. Extensive experimental results conducted on our test platform, the CSIRO Autonomous Tractor have demonstrated the effectiveness of this approach, even in the face of significant additional system dynamics such as second-order effects in the steering loop. This paper brings together these results and furthermore provides a theoretical analysis of the stability and convergence properties of this control approach.

I. I NTRODUCTION Reversing of tractor-trailer systems is a common task in both recreational and industrial settings. In the recreational domain, examples of tractor-trailer reversing tasks include parking of transportation trailers and the reversing of boat trailers onto boat ramps. A previously cited [] example is the growing grey-nomad phenomenum in Australia in which retirees embark on a journey around the country, towing a caravan to accomodate them in their travels. They are frequently required to reverse the caravan into parking spots throughout this journey and as many of these drivers have had little experience in controlling such a system, can experience some difficulty in doing so. This is one example in which a a driver-aid or automation system could be of significant benefit. In industrial settings, examples of tractor-trailer systems include agricultural systems, transportation, and cargo handling. In the main, drivers in industry are well trained and experience little difficulty in achieving a desired tractor-trailer reversing trajectory. However, a growing number of these

tasks are being automated for which a robust and reliable control system is essential. Most approaches address the problem of reversing a tractor-trailer system through the development of relatively complex, non-linear controllers. Our approach is based on closing an inner-loop on the angle between the tractor and trailer using a simple Proportional Integral (PI) controller, and then closing an outer loop on the trajectory using a path tracker developed for articulated vehicles. Our previous work has described the development and experimental validation of this trajectory control algorithm [], including comparisons with human operators which showed that our system performed at a level equivalent to an operator of medium skill, even though our system was significantly hindered in terms of the pose estimates available to the controller and the relatively poor response times of the underlying low-level control system. This paper brings together these results and provides a more thorough theoretical analysis of the stabilizing properties of this controller. The next section outlines the literature available in the field; Section III describes the platform and kinematics of the system considered in this paper; Section V-B outlines our approach to hitch-angle stabilization; Section V-A describes the method used to stabilize the system to a trajectory; Section VII describes comprehensive experiments validating the approach; and Section VIII concludes the paper. II. BACKGROUND The control of tractor-trailer systems has received much attention in the scientific and patent literature. This problem has clear industrial applications but it also interesting due to its inherent non-linear nature.

2

In the scientific literature, this problem has been approached in three ways: through a change of variables which brings the system into a standard form, optimal control methods, and learning-based approaches. The change of variables approach relies on a transformation which brings the system into a chained (or similar) form, from which standard non-linear control theory can be applied, see for example [3], [17], [22]. These techniques have the advantage of being theoretically generalizable to any number of trailers. Their main disadvantage, with respect to the work presented in this paper, is their complexity and high sensitivity to numerical approximation. This complexity makes it difficult to implement these methods reliably, and also poses difficulties in the tuning of these systems — after the change of variables, many of the control parameters no longer have a physical meaning making the design of a tuning strategy challenging and somewhat non-intuitive. As for numerical stability, the chained-form-based conversion for instance requires several trigonometric operators and inversions, and the resulting control often requires inversion of matrices which can become singular at some point of the navigation. The optimal control based methods use optimization schemes to derive a sequence of demands which will control the tractor-trailer system onto the desired trajectory or path, see for example [2], [6]. In simple terms, these methods use a vehicle model and a simulation process to compute the control commands that will lead to the best tracking of the trajectory. This requires accurate models of the vehicle’s behaviour and a heavy reliance on computational resources. Deviations from the model, which in real-world implementations are inevitable, lead to errors, and these methods are also open-loop meaning that re-planning is necessary to deal with errors in localization. The learning-based controllers seek to ease the computational burdens of the previous methods by providing a mapping between the current vehicle state, the desired state and the required inputs to reach the desired state, see for example [10]. Essentially, a simulated model of the tractor-trailer system is used to try many different possible methods and parameters. ‘Learning’ occurs by searching the parameter space for the best set of methods/parameters which are then encoded into, for example, a neural network or look-up table, which maps the current to desired configuration. The main drawback of these techniques is the learning itself: if learning occurs from a model of the system,

then errors in the model are clearly problematic; if learning occurs on the real vehicle, then there are clearly safety issues since it is not possible to predict the behaviour of the vehicle in the learning phase. Also, even if the best set of parameters performs well in practice, it is hard to guarantee its performance, which can be problematic in an application where reliability is an issue. In addition to the scientific literature, there are a number of patents which relate to reversing of tractor-trailer systems, see for example [4], [8], [9], [15]. However, this literature focusses on reversing along straight trajectories, mechanical devices to aid reversing and on driver-assist type systems which provide methods to sense the position/hitchangle of a trailer and to display this information to the driver. Before discussing our approach, it is worth noting some of the advantages and limitations of our approach as compared to those available in the literature. Firstly, our method is relatively simple and has proven to be very robust. This is important if we are to see migration of this technology into a commercial setting as such a system should be straighforward to set-up and tune. Our approach, as will be shown, is also very robust to the response time of the underlying low-level control system, unlike much of the existing work. However, a key limitation of this method in its current form is that it is applicable to systems of one trailer only whereas the methods in the scientific literature are generally extendable to multi-trailer systems, at least theoretically. As already noted, the dynamics of the platform have a significant influence on the success of tractor-trailer reversing control schemes. The next section outlines the kinematic and dynamic aspects of the tractor-trailer systems used in this experimental program. III. E XPERIMENTAL PLATFORM The platform used in these experiments is the CSIRO Autonomous Tractor (AT), as shown in Fig. 1. It is an Ackerman steered, ride-on mower which has been retro-fitted with an array of actuators, sensors, and a computer system enabling the implementation and testing of control and navigation algorithms. For full details of the vehicle’s design, refer to [21]. The trailer hitch-angle is sensed using a set of string-pot encoders. Unlike much of the existing research, which presumes that the hitch-point lies over the centre of the rear axle, the hitch-point

3

following equations: x˙ = v cos(θ1 ) y˙ = v sin(θ1 ) v tan(φ) θ˙1 = L v ˙ [L1 sin(φ) cos(ψ) + ψ = − L2 L cos(φ) L2 sin(φ) + L cos(φ) sin(ψ)]

1: The Autonomous Tractor with its trailer.

on this platform is offset from the rear-axle. This is kinematically more similar to the majority of tractor-trailer systems in general use and represents a further distinction of this work from the existing literature. A. Kinematic model The geometry of our tractor-trailer system is depicted in Fig. 2. The lengths referred to in this figure are defined as: L the distance between the front and rear axles of the tractor (1.2 m); L1 the distance between the rear axle of the trailer and the hitch joint (0.45 m); and L2 the distance between the trailer axle and the hitch joint (1.2 m).

P=(x,y) H Q

F

φ

L P L1 Q

ψ

θ1

H

L2 θ2

y x

2: Kinematic model of a tractor-trailer system The system states are described by: (x, y) the Cartesian coordinate of point P in the global frame; θ1 and θ2 the heading of the tractor and trailer respectively; and ψ the hitch-angle (note that θ2 = θ1 + ψ). The system is controlled via two inputs: v the linear velocity of point P (v = kP˙ k); and φ the steering angle of the front wheels. With this notation, the system can be described by the

(3)

(4)

The first three lines of this system are the standard kinematics for a car-like vehicle, see e.g. [5] for a derivation. The method for proving the expression for ψ˙ is outlined in [13]. B. System dynamics Many control algorithms for car-like vehicles fail to consider the realities of implementation on a vehicle, for example steering angle limitations and the dynamics of the actuators which drive the vehicle. In this work, models of the vehicle’s dynamic response to demands are used, including non-linear effects such as input saturation. 1) Steering: An approximate model of the steering dynamics was experimentally identified from the response of the AT’s steering loop to unit step changes in desired steering angle (provided from the computer). The response was determined to be approximately second-order of the form (in the Laplace domain): φ(s) φ∗ (s)

F

(1) (2)

=

ωn2 s2 + 2ζωn + ωn2

(5)

The parameters ωn and ζ where found to vary due to the complexity of the interactions between the terrain and the wheels on different surfaces and also varied with the vehicle’s translational speed. For the purposes of these experiments, parameter values of ω = 0.72 and ζ = 0.8 were found to model the system adequately. The steering angle is also limited to ±0.523 rad (±30◦ ). Figure 3a shows a plot of the actual and modelled response of the vehicle to a unit step input. 2) Velocity: The velocity loop was empirically determined to have a first-order response which is represented in the Laplace domain as: v(s) Kv = v ∗ (s) τv s + 1

(6)

Figure 3b illustrates the AT’s response to a unit step change in velocity while travelling on level ground (concrete). Also shown is the response of the first-order model where Kv = 1 and τv = 1.33.

4

Steering Angle Step Response (vehicle speed = 0.5ms−1)

Comparison of predicted and actual trajectories 5 model real

0.5 experiment model

0.4

4

ωn = 0.72

0.3

3

0.1 0

2

−0.1

y (m)

steering angle (rad)

ζ = 0.78 0.2

−0.2

1

−0.3 −0.4

0

−0.5 0

2

4

6

8 10 time (sec)

12

14

16

18

−1

(a) Step response of the steering loop.

−2

Velocity Loop Step Response

1

experiment model

2

3

4

5 x (m)

6

7

8

9

1

4: Real and simulated response of the vehicle to a series of control inputs for a trajectory similar to a ‘swerve’ around an obstacle. Model data is plotted as a dashed line and experimental data is plotted as a solid line.

−1

velocity (ms )

0.8

0.6

0.4

0.2

0

0

1

2

3

4

5 time (sec)

6

7

8

9

10

(b) Unit step response of the speed loop.

3: Step response of the steering and velocity loops. Model data is plotted as a dashed line and experimental data is plotted as a solid line.

Note the ‘quantisation’ of the experimental velocity measurements. The parameters (Kv and τv ) will again vary slightly due to differences in the velocity response on sloping terrain, different surfaces and under different loading conditions. Additionally, the forward speed of the AT is limited to 3 m/s while the reverse speed is limited to 1.5 m/s. C. Trajectory simulation The kinematic and dynamic models developed earlier can be used to predict the motion of the vehicle given its current pose and the control inputs. Figure 4 shows a comparison of the real position of the vehicle (measured using the vehicle odometry which is relatively drift-free over the short distance considered here) against the position predicted by the system model. As will be described later, this ability to predict the vehicle’s path can be used to ‘test’ particular trajectories for potential collisions.

testing to the real vehicle. This section describes a selection of algorithms from the literature, highlighting the problems of these approaches for a ‘realistic’ tractor trailer system in which there are significant underlying dynamics in the low-level control system. A. Differential flatness Our system is designed as a general 1-trailer system in Rouchon’s work [16]. It has been proved to be differentially flat. From this property, we could use an approach similar to the work of Hermosillo et al. [7], [11]. In summary, this relies on converting the trajectory of the tractor trailer system into the trajectory of its flat output (y1 , y2 ): y1 = x + L2 cos(θ + ψ) (7) L2 sin(θ + ψ) − L1 sin(θ) + L(ψ) p 2 L1 + L22 − 2L1 L2 cos ψ (8) y2 = y + L2 sin(θ + ψ) L2 cos(θ + ψ) − L1 cos(θ) + L(ψ) p 2 L1 + L22 − 2L1 L2 cos ψ

with

IV. P ROBLEMS WITH EXISTING METHODS Given that we can accurately simulate the motion of this particular tractor-trailer system, we can implement and evaluate the effectivness of existing controllers from the literature. Our software architecture also allows us to seamlessly migrate this

2π−ψ

cos σ dσ L + L 1 2 − 2L1 L2 cos σ π (9) Still following from Hermosillo’s work [7], [11], a stabilizing control would be:

L(ψ) = L1 L2

(3)

Z

yi=1,2 = (yi⋆ )(3) +

√

2 X j=0

(j)

ki,j (yi

− (yi⋆ )(j) ) (10)

5

Where (y1⋆ , y2⋆ ) is the flat output corresponding to the state on the reference trajectory, and (.)( p) is the time derivative operator of order p. This technique requires to estimate the two first derivatives of the state’s flat output, then integrate the resulting control in order to deduce the actual steering and speed from the third derivative of the flat output, all of this while using and inverting equations 7 and 8. Even if theoretically stable, all these derivations and integrations make this method extremely sensitive to noise in the state estimation. In a real implementation, the state itself can be quite noisy, making the second derivate of its flat output close to meaningless. Our experience in implementing the previous control law confirmed the predicted sensitivity. Moreover, the tuning of the parameters ki,j was extremely hard, due to their sheer number, to their coupled effect on all the vehicle state and to the absence of physical meaning. B. Chained form representation The chained form representation for a general n-trailer system can be found from the work of Tilbury and Sordalen [19], [20] and Altafini [1]. To obtain this form, we first use Altafini’s result that show that a car-like trailer pulling a off-centered single trailer, can be modelled as a standard 3trailer system (that is one tractor pulling three trailer). From this, the work of Sordalen gives the transformation of this system into a chained form representation. The resulting formulas are dauntingly complex, and, as a consequence, will be hard to implement correctly, debug, test and maintain. Furthermore, since the chained-form creates very strong coupling between all the angles of the system, designing a controller and a tuning strategy will be as hard as, or even harder than, when using the flatness property of this system.

than can be prohibitively long if the trajectory is more than a few meters long. Then, this method does not provide a way to prioritize the control: when steering a trailer, it is essential to keep the trailer angle stable and controllable first since entering a jack-knife situation is irrecoverable. Unfortunately, this method does not provide any mecanism to deal with this constraint.

D. Stabilisation using a virtual tractor The last method in this review of existing tractor trailer controller comes from the HILARE robot at LAAS [18]. The core idea is that there exists a unique one-to-one mapping between the speed vector of a point of the tractor and the speed vector of a point of the trailer (see section III-A). Knowing this relation, it is possible to exchange virtually the role of the tractor and trailer. Knowing the distance from the trailer to the reference trajectory, a simple controler – such as pure pursuit [] – can give a desirable speed and rotation speed for the trailer. Then, this control vector can be mapped to a control vector for the tractor and applied. Experimentally, this method is able to stabilise a model of our system, provided that we assume no limits on the steering angle speed (see fig. 5). When introducing speed steering limit, even unrealistic with respect to the real system, the performance degrades around 110 degree/second, and the stabilisation is no longer possible at 100 degree/second. Also since this control law was designed for HILARE, which is able to turn on the spot, this control law sometimes require a null translation speed and a non-zero rotation speed, which is not achievable on a car-like vehicle. This is what stops the system in the bottom graph of fig. 5. V. O UR APPROACH

C. Trajectory stabilisation using pre-computed gains Walsh et al. [22] proposes a control law which is, in theory, exponentially stabilizing the system onto a reference trajectory. In principle, this control law estimates a gain for a linear control law on each point of the reference trajectory. This gains is computed using the shape of the trajectory in the next ∆t seconds. In our experimentations, this approach presented two main difficulties. First, the precomputation of the gains involves complex integral estimations,

Our approach to stabilizing a tractor-trailer system to a trajectory is outlined in previous papers [13] and here we provide only a brief overview. The method is based on the idea of closing an innerloop around the trailer hitch-angle, and then treating the vehicle as a traditional articulated vehicle for which there are provably stable algorithms for stabilizing to a trajectory. We begin the discussion with the outer loop which stabilizes the vehicle to the trajectory, and then discuss in more detail the inner, hitch-angle control loop and its stability properties.

2 0

1.5 -2 1

0.5

-6 0

-8

-0.5

-10

Tractor trailer stabilisation (red and blue)

0

2

4

6

8

10

12

14

16

Steering angle (magenta)

-4

-1 18

No limit on steering angle speed

use a trajectory control law which is largely inspired from [14] where a Load Haul Dump (LHD) vehicle is considered. LHDs are used in underground metalliferous (non-coal) mining operations for the transport of ore. They are four-wheeled, centre-articulated vehicles, steered by controlling the hitch joint angle. In principle, these vehicles are similar to the tractor-trailer system considered here differing only in that the hitch-angle is controlled directly rather than via steered wheels.

2 0

Current configuration 1.5

-2 1 -4

ξy

ξθ 0.5

-6 0

-8

-0.5

Steering angle (magenta)

Tractor trailer stabilisation (red and blue)

6

Curvature κ

Curvature κpath

Reference configuration

ξx

ξκ = κ − κpath

(a) Trajectory error definitions. -10 0

2

4

6

8

10

12

14

16

Vpath

-1 18

o

2

ξx

1.5

ξy

V⋆

+

K

+

0

-2

ξθ

1 -4 0.5

-6 0

-8

-0.5

-10 0

2

4

6

8

10

12

14

16

Steering angle (magenta)

Tractor trailer stabilisation (red and blue)

Steering angle speed limited to 110 per second

ψ⋆

Articulated Vehicle Path Tracker

ξκ ψ

+

−

Hitch Angle Servoing

φ⋆

-1 18

o

Steering angle speed limited to 100 per second 5: Stabilisation of a tractor trailer to a straight trajectory using the HILARE controller, while varying the steering angle speed (simulation)

A. Trajectory stabilization To stabilize the vehicle to a trajectory, that, is a sequence of vehicle states indexed by time, we use a path tracking controller derived for an articulated vehicle, and apply an additional controller on the vehicle speed to ensure the vehicle progresses along the trajectory. The first important aspect of a tractor-trailer system is that its dynamics are asymmetric. When driving forward, the trailer angle is naturally exponentially stable. When reversing, it is naturally unstable. Consequently we use different control laws for both situations. When driving forward, we use a standard trajectory-tracking control law, such as pure pursuit, and we ignore the trailer. When reversing, we

(b) Block diagram for the reversing control law.

6: The trajectory controller and the definition of path errors. The control law presented in [14] aims at stabilizing the vehicle on a path, i.e. a 2D curve in the plane. It relies on three error measurements, as depicted in Fig. 6a: ξy the lateral error, ξθ the heading error, and ξκ the curvature error. In the original work, the authors controlled the hitchangle rate of the LHD using these measurements. Here we control the hitch-angle and the control law is defined as: ψ = Ky ξy + Kθ ξθ + Kκ ξκ

(11)

where Ky , Kθ and Kκ are tuning parameters. The resulting hitch-angle is then fed to the hitch-angle stabilization law described in Section V-B. Speed is controlled using a standard pursuit approach to maintain the vehicle’s progression along the trajectory and switching logic is used to deal with any changes of direction, as occurs when the onset of jack-knifing is detected or there is a cusp in

7

Time differential of the Lyapunov function V = 0.5(ψ−ψd(ψ*))2

the trajectory (see [13] for details). The trajectorytracking system is described in block diagram form in Fig. 6b.

0.05

We now turn to the critical element in our approach which is the hitch-angle stabiliation loop. The technique used here has been discussed in previous work and is only outline here. However, a more detailed fomral analysis of the properties of this loop will be presented, from which a nonlinear control law which allows operation over a broader hitch-angle range arises. In short, a simple Proportional-Integral controller is enough to achieve the hitch-angle stabilization task: Z t φ = Kp (ψ ⋆ − ψ) + KI (ψ ⋆ − ψ)du (12)

dV/dt

0

B. Hitch-angle stabilisation

−0.1

−0.15 1 0.5

ψ d (ψ ⋆ ) =

KP L1 − L + KP L2 ⋆ ψ KP (L1 + L2 )

(13)

Using the above relations, we can modify our control law to obtain theoretical convergence on ψ⋆ : Z t (ψ ⋆ − ψ)du (14) φ = Kp (ψ d (ψ ⋆ ) − ψ) + KI 0

In this equation, the integral term is not essential for the stability of the control. It is added to account for minor inaccuracies remaining after the proportional control. Such inaccuracies can result from the linearisation leading to ψ d (ψ ⋆ ), or from errors in the vehicle model (L, L1 , and L2 ). 1) Stability analysis: First we assign a a Lyapunov function to the system which is essentially an ‘energy’ function. This function must be positive semi-definite for all values of the system state. We choose: 1 (15) V = (ψ d (ψ ⋆ ) − ψ)2 2

1 0.5

0 0

−0.5

−0.5 −1

ψ (rad)

−1

ψ*(rad)

(a) Behaviour of V˙ . Time differential of the Lyapunov function V = 0.5(ψ−ψd(ψ*))2, (dV/dt >0)

0.025

0.02

0

0.015 dV/dt

where ψ ⋆ is the demanded hitch-angle, and Kp and KI are the proportional and integral gains. Our previous analysis of this controller showed that for non-zero operating points, the control demand needs to be modified in order to compensate for the steady-state error introduced by nonlinearities at these points. Of course, the Integral term in the controller will compensate for these effects but this modification speeds up the system response The expression to modify the demanded hitch-angle ψ d that will make the system converge to a desired angle ψ ⋆ is:

−0.05

0.01

0.005

0 1

1 0.5

0.5 0

0 −0.5

−0.5 −1

−1

ψ (rad)

ψ*(rad)

(b) Behaviour of V˙ , highlighting points of instability (i.e. V˙ > 0).

7: The closed loop behavior of the the time derivative of the Lyapunov fuction, V˙ . The stability of the system can be determined by analysing V˙ = dV dt , i.e.: V˙

= ψ d (ψ ⋆ ) · ψ˙d (ψ ⋆ ) − ψ˙ · ψ d (ψ ⋆ ) −ψ˙d (ψ ⋆ ) · ψ + ψ · ψ˙ ˙ − ψ d (ψ ⋆ )) = ψ(ψ

(16)

where it is presumed that ψ˙d (ψ ⋆ ) = 0. Inserting the control law of Equation 14 (the proportional component only) and the expression for ψ˙ (Equation 4 into the above relation leads to:   v tan(Kp (ψ d (ψ ⋆ )−ψ )) L1 cos ψ + 1 − V˙ = − L L2 v L2

sin(ψ)

(17)

after some rearrangement. Figure 7a shows a plot of the value of this function across the range of inputs ψ ⋆ = [− π2 , π2 ] and the output state ψ = [− π2 , π2 ] where KP = 1 has been substituted. Figure 7b highlights the instability points. These plots indicate that the system is stable for ‘most’ of the operating region. Exceptions include

8

Rearranging this equation leads to the controller: # " v sin ψ ⋆ L K(ψ − ψ ) − L2 (19) φ = arctan L1 v L2 cos ψ + 1 This version of the hitch-angle controller theoretically ensures convergence across the range of hitch-angle from −π/2 to π/2. Additionally, this controller does not require a modification of the demanded hitch-angle ψ ⋆ for non-zero operating points. However, like the controller of Equation 14, this controller is also sensitive to errors in the system’s geometric parameters due to the presence of these parameters in Equation 19. Thus, an integral term is still required to ensure convergence without a steady-state error. In practice, due to the restricted operating range in hitch-angle, the perfomance of this controller is about equivalent to the linear controller (Equation 14). !!!Sensitivity analysis - will do early next week (not nice)!!!

b) Tuning of Kx , Ky and Kθ :: These parameters can be tuned by subjecting the system to lateral steps: from a starting position with a null hitch angle, the system must stabilise itself to a straight trajectory with a lateral offset (1 meter for instance). This kind of trajectory has been used in fig. 9. The initial values of these parameters can be set so has to generate the maximum hitch angle for a given error. For instance, we want to use the maximum hitch angle when the lateral error |ξy | reaches 1.5 m or the heading error |ξθ reaches 30o . During these experimentation Kκ should be set to zero. c) Tuning of Kκ :: Kκ only has an influence when driving on curved trajectories. As a consequence, it should be tuned by controlling the system to a circular trajectory with a feasible curvature. Using only Ky and Kθ , the system should be able to track a circle reasonnably closely. Kκ adds some look-ahead by keeping the hitch angle above zero even if the other errors are null. In practice Kκ should be less important than the other gains since changing the curvature is, in effect, how we control the trajectory of our system. VII. R ESULTS This section highlight some of the significant experimental results achieved while running this controller on our real tractor trailer system. 0.4 reference Measured hitch angle Steering angle 0.3

0.2

0.1 Angle (rad)

the ‘ripples’ as indicated in the plot but these are avoided in practice by restriciting the operating regime even further, a point made necessary by the significant dynamics of the steering loop (e.g. it takes approximately four seconds to get a lock-tolock steering command). 2) Alternative, non-linear controller: The Lyapunov function analysis from the previous section can be used to derive an alternative, nonlinear hitch-angle controller that is stable across the whole operating regime and does not require ‘heuristic’ modifications to the control demand (as in Equation 13). First we note that the system is stable for V˙ ≤ 0 (where V˙ is from Equation 16) which can be achieved by setting: ψ˙ = −K(ψ − ψ ⋆ ) (18)

0

-0.1

-0.2

-0.3

-0.4 0

20

40

60

80

100

120

140

Time (s)

Linear hitch angle controller (eq. 13) 0.4 reference Measured hitch angle Steering angle 0.3

0.2

0.1 Angle (rad)

VI. T UNING STRATEGY One of the interest of this controller is its simplicity, making it easy to implement and tune. The tuning strategy we use consists in three steps: a) Tuning of the hitch angle controller:: This requires to tune the parameters KP and KI in equation 12. A standard PID tuning technique can be used. In practice, we first start with KP = 1 and KI = 0, we initialise the system with a non-zero hitch angle, and control it to zero while reversing at constant speed. We then increase KP until oscillation appears. When KP is optimal, we can try increasing KI . The required setup is similar, except we need to control the system to a nonzero hitch angle. This procedure should also give practical bounds on the controllable hitch angles.

0

-0.1

-0.2

-0.3

-0.4 0

20

40

60

80

100

120

140

Time (s)

Non-linear hitch angle controller (eq. 19)

8: Performance of the hitch angle controller, implemented using the exact non-linear formula, or the approximated linearisation.

2 0

1.5 -2 1

0.5

0

-8

-0.5

-10 0

2

4

6

8

10

12

14

16

Steering angle (magenta)

-4

-6

-1 18

2 0

1.5 -2 1 -4 0.5

-6 0

-8

-0.5

-10 0

2

4

6

8

10

12

14

16

Steering angle (magenta)

Tractor trailer stabilisation (red and blue)

No limit on steering angle speed

-1 18

2 0

1.5 -2 1

0.5

-6 0

-8

-0.5

-10 0

2

4

6

8

10

12

14

Steering angle speed limited to

15o

16

Steering angle (magenta)

-4

-1 18

per second 2

0

1.5 -2 1 -4 0.5

-6 0

-8

-0.5

-10 0

2

4

6

8

10

12

14

16

Steering angle (magenta)

Tractor trailer stabilisation (red and blue)

Steering angle speed limited to 20o per second

Tractor trailer stabilisation (red and blue)

Fig. 8 shows experimental tests obtained controlling only the hitch angle, while the tractor was moving at a constant speed −0.3 m/s. The reference hitch angle follows a sine functions, with a 240 s period and a 0.35 rad. A full period could not be tested due to the limited space available in our test arena. On this graph we can observe that the linear and non-linear controller gives very similar performance, in terms of accuracy of the tracking and control of the oscillations. The oscillations of the hitch angle around the nominal trajectory seem to be due to some limit cycle caused by the limited steering speed of our vehicle. Simulation tests where we artificially decreased the maximal steering speed produced a similar behaviour (see fig. 9) around 15o per second, which is close to the capabilities of the real vehicle. Fig. 9 also shows the resilience of our controller to very limited steering speed. From an unlimited steering rate to a maximum of 20 deg/s, the controller was able to correct a 1 m lateral error with a very smooth path. With a maximum steering rate of 15 deg/s, some limit cycle starts to appear but the system still converges. A limit of 10 deg/s prevent the controller to stabilise the trailer on the reference path. This would correspond to a time of steering close to 8 s to cover the steering range. All these test where done in simulation at −0.3 m/s. They are to be compared with fig. 5 where the maximum acceptable steering rate was about 100 deg/s. In the set of experiments described in fig. 10, our objective was to demonstrate the performance of our system in a real-world situation. To this end, we designed a path across our research centre. This path is approximately 170 m long and was planned manually using hand-selected way points and interpolating splines. This approach guarantees a smooth and continuous trajectory. For these experiments, the system relied on an external localization estimates coming from a particle filter. This kind of localisation has the advantage of being very resilient to observation noises, but its output cannot be garanteed to be locally smooth. The resulting path is depicted in Fig. 10a. It is composed of two segments: a short forward motion to help align the system with the trajectory, and a long reversing segment of approximately 160 m. The main constraints when designing this trajectory were to keep the vehicle close to the middle of the road, and to minimize the path curvature. Fig. 10b-10d show the trajectories realized by our controller on three seperate runs. More accurately, the trajectories represent the path of the

Tractor trailer stabilisation (red and blue)

9

-1 18

Steering angle speed limited to 10o per second (jack-knife)

9: Stabilisation of a tractor trailer to a straight trajectory using our approach, while varying the steering angle speed (simulation)

10

y position (m)

20

0

-20

0

20

40

60

80

100

120

60

80

100

120

60

80

100

120

60

80

100

120

x position (m)

(a) Reference

y position (m)

20

0

-20

0

20

40 x position (m)

(b) Run A

y position (m)

20

0

-20

0

20

40 x position (m)

(c) Run B

y position (m)

20

0

-20

0

20

40 x position (m)

(d) Run C

10: Reversing trajectories of the autonomous system on three independent runs.

centre of the tractor’s rear axle. Examples of localization jumps (corrections) can be seen at the top end of all trajectories, and around (55, 0) in Run B and C. It is important to note that our system stays stable around the reference trajectory, even when the localization estimation is very unstable. In some situations, e.g. close to (−10, 10) in run A and C, or close to (75, 0) in run B, the localization estimate oscillates between both sides of the reference trajectory. If this occurs at the wrong frequency, it can bring the system to a jack-knife situation. In all cases, the preliminary signs of this event where detected by the system, and a short forward motion was initiated to realign the vehicle

with the reference trajectory. Given the noisiness of the localization estimate, an evaluation of the system based on the mean tracking error is somewhat meaningless. Instead we decided to compare our system performance with human drivers. Detailed results can be found in [12]. In a nutshell, our controller performance in terms of stabilisation of the hitch angle was comparable to a professional human driver, albeit with a much lower speed(0.3 m/s instead of 1.1 m/s). This is mainly due to the fact that humans have a much bigger range of steering speed and acceleration, permitting them to keep the system stable at a much higher speed. Also, humans don’t

11

have to deal with a noisy localisation system. Obstacles/Bollard Reversing Planned Path Forward Planned Path

5m

Event

1

1

4

3

2

Planned trajectory 145

Planned Tracked

6

and straight segments of the path (events 5, 8 and 9) introduce a discontinuity which the system struggles to deal with but nevertheless recovers from. Localization discontinuities (events 6 and 7) are handled much more gracefully than the path discontinuities. From these experiments, it is clear that in order to obtain very accurate tracking, we would need a path planner taking into account the strong constraints of this system, in particular, the small maximum trackable curvature and the need for smooth curvature profiles. However, the trajectory control performed extremely well within these constraints. For even more accuracy, a very accurate and smooth localisation system would also be required.

140

8

135

VIII. C ONCLUSION

9 130

125

120

115

5

7

110

105 -35

-30

-25

-20

-15

-10

-5

0

Executed tracjectory 11: Performance for a poorly planned trajectory which does not account for curvature limitations or path discontinuities. events are denoted by the boxed numbers. In a second set of experiments, we stretch the system by introducing a more complex path as depicted in fig. 11. We consider this trajectory “badly planned” since it does not take into account the limitations of the vehicle, especially the limited turning radius and the very strong limitation on the hitch-angle rate. In addition, the trajectory contains path discontinuities in which straight lines join circular segments — such discontinuities are challenging for the control law since they require a very fast change of hitch-angle. Our objective here is to show the performance of our system at the limits of its nominal specification. External localisation is also used for this experiment. Fig. 11 illustrates how the planned path is tracked by our tractor-trailer system. From this experiment, we note that on straight line segments the system converges reasonably fast. On the curved segments, the curvature is beyond the vehicle capabilities and the tracked trajectory is offset from the planned one. Transitions between the curved

This article introduced a new control scheme for a tractor-trailer system. This scheme is based on a two layer control loop: first a hitch-angle stabilisation loop controls the angle between tractor and trailer, then a path tracking control loop, initially designed for an articulated mining vehicle, is adapted to our tractor-trailer system. The main advantage of this approach over traditional methods is its simplicity of implementation. Only a few parameters need to be tuned, they all have a clear physical meaning and we provide an easy-to-follow strategy to tune them independently. Finally, this control law has been implemented on a real vehicle and experiments were conducted on challenging trajectories. Given the limited dynamic performance of our platform (slow response time, loose components, low speed actuation), the control law exhibited excellent convergence and stability properties. ACKNOWLEDGMENTS This work was funded by the CSIRO ICT Centre under the ROVER and Dependable Field Robotics projects. The authors would like to thank the ASL team for their support of this work. Special thanks go to Polly Alexander, Stephen Brosnan, Peter Corke, Elliot Duff, Paul Flick, Leslie Overs, Ashley Tews, John Whitham and Graeme Winstanley who all contributed to the development of our experimental autonomous tractor. R EFERENCES [1] C. Altafini. Some properties of the general n-trailer. International Journal Control, 74(4):409–424, 2001.

12

[2] C. Altafini, A. Speranzon, and B. Wahlberg. A feedback control scheme for reversing a truck and trailer vehicle. IEEE Transactions on Robotics and Automation, 2001. [3] P. Bolzern, R.M. DeSantis, and A. Locatelli. An inputoutput linearization approach to the control of an nbody articulated vehicle. Journal of Dynamic Systems, Measurement, and Control, 2001. [4] D.N. Carter and J.R. Lormor. Vehicle steering aid system, 2004. GB patent GB2398050. [5] Alessandro De Luca and Giuseppe Oriolo. Modelling and control of nonholonomic mechanical systems. In Kinematics and Dynamics of Multi-Body Systems. 1995. [6] A.W. Divelbiss and J.T. Wen. Trajectory tracking control of a car-trailer system. IEEE Transactions on Control Systems Technology, 1997. [7] J. Hermosillo and S. Sekhavat. Feedback control of a bi-steerable car using flatness: Application to trajectory tracking. In Proc. of the American Control Conference, 2003. [8] R.G. Kandall. Vehicular steering system for reverse paths, 1993. US patent US5247442. [9] S. Karl. Automatic reverse steering system for passenger vehicles with trailer, 1987. DE patent DE3538338. [10] J.R. Koza. A genetic approach to finding a controller to back up a tractor-trailer truck. In Proceedings of the 1992 American Control Conference, 1992. [11] C. Pradalier, J. Hermosillo, C. Koike, C. Braillon, P. Bessi`ere, and C. Laugier. The cycab: a car-like robot navigating autonomously and safely among pedestrians. Robotics and Autonomous Systems, 50(1):51–68, 2005. [12] C. Pradalier and K. Usher. Experiments in autonomous reversing of a tractor trailer system. In Proc. of Int. Conf. on Field and Service Robotics, Chamony, France, 2007. under review. [13] C. Pradalier and K. Usher. A simple and efficient control scheme to reverse a tractor-trailer system on a trajectory. In Proc. of IEEE Int. Conf. on Robotics and Automation, Roma, Italy, 2007. [14] P. Ridley and P. Corke. Load haul dump vehicle kinematics and control. Journal of Dyn. Sys, Measurement and Control, 2003. [15] S.D. Robert. Trailer backing up device and method, 2004. US patent US2004215374. [16] P. Rouchon, M. Fliess, J. Lvine, and Ph. Martin. Flatness and motion planning: the car with n-trailers. In Proc. of European Control Conference, 1993. Groningen, pages 1518–1522, 1993. [17] M. Sampei, T. Itoh Tamura, and M. T. Nakamichi. Path tracking control of trailer-like mobile robot. In Proc. of IEEE/RSJ In. Workshop on Intelligent Robots and Systems ’91, 1991. [18] S. Sekhavat, F. Lamiraux, J.P. Laumond, G. Bauzil, and A. Ferrand. Motion planning and control for hilare pulling a trailer: experimental issues. In Proc. of IEEE Int. Conf. on Robotics and Automation, 1997. [19] O. Sørdalen. Conversion of the kinematics of a car with n trailers into chained form. In Proc. IEEE Int. Conf. on Robotics and Automation, pages 802–819, 1993. [20] D. Tilbury, O. Sørdalen, L. Bushnell, and S. Sastry. A multisteering trailer system: conversion into chained form using dynamic feedback. IEEE Trans. on Robotics and Automation, 1995. [21] K. Usher. Visual homing for a car-like vehicle. PhD thesis, Queensland University of Technology, Brisbane, Australia, June 2005. [22] G. Walsh, D. Tilbury, S. Sastry, R. Murray, and J.P. Laumond. Stabilization of trajectories for a systems with nonholonomic constraints. IEEE Transactions on Robotic and Automation, 1994.