4 Master of Science Thesis Stockholm, Sweden 200

Master of Science Thesis Stockholm, Sweden 2004

Hybrid Control System for Reversing a Multibody Vehicle

Homan Bromand

IR-RT-EX-0402

Abstract

This thesis deals with the problem of prototyping a vehicle, made up by a motorized body and two passive trailers using LOGO Mindstorms, and provide a hybrid control system supporting the driver while reversing the vehicle.

The goal of the hybrid system is to switch between different linear controllers, each designed for a specific purpose, to follow a generic path. The approach, when designing the hybrid system, is similar to Dubin’s car problem.

The prototype will be designed to perform as a stand-alone system and be controlled by its own computer.

Information about the instant condition of the vehicle is collected by means of a set of onboard sensors. Two angle sensors are used to measure the relative angles between the different parts of the structure, one rotation sensor is used to measure the steering wheel angle and two rotation sensors are used to transform the wheel’s revolution into orientation of a specific point of the vehicle.

The implementation of the controller consist in a java-program written on a host computer and then downloaded to a robotics command explorer (RCX), equipped with an input-output board that allows to generate the control signal and acquire data from sensors.

i

Acknowledgments

This thesis is the final report to a Master degree in Electronics Engineering at the Royal Institute of Technology in Stockholm, Sweden.

First, I would like to thank Professor Bo Wahlberg from the Automatic Control group for giving me the opportunity to work on this interesting project and giving me great freedom to develop this work. A special thank should go to Henning Schmidt and Alberto Speranzon for frequently answering my questions.

I also wish to acknowledge Manuel Mazzo, Andreas Jakobsson, and Gustav Alm for their help when constructing the vehicle and Kamran Bromand for our long, vivid, and fruitful discussions.

Last but not least I wish to thank my family for their support and guidance during my years at the University.

Contents

1 Mathematical model 3

1.1 Equations and notations ……… 3

1.2 Case of study ……… 6

1.3 State variables ……… 7

1.4 Kinematic model ………... 9

1.5 Model consideration ………. 10

2 Controller design 12

2.1 Linear model………... 12

2.1.1 Equilibrium points ………. 12

2.1.2 Linearization ……… 16

2.2 Linear quadratic regulator ……… 18

2.3 Constraints ……….. 21

3 Hybrid system 25

3.1 Hybrid systems and switching control ……… 25

3.2 Switching controller design ………. 27

3.2.1 Segmentation through arcs and lines………. 27

3.2.2 Segmentation purely through lines ……….. 31

3.2.3 The complete hybrid system ……… 36

4 Prototyping 39

4.1 Introduction ……… 39

4.2 LEGO Mindstorm parts ………. 39

4.2.1 RCX ……… 39

4.2.2 Motors ………. 40

4.2.3 Sensors ……… 41

4.2.4 IR tower ………... 41

4.2.5 IR communication ………... 42

4.3 Programming environment ……….. 42

4.3.1 RCX code ……… 42

4.3.2 Alternative RCX programming ……… 42

4.3.3 LeJOS ……….. 43

4.4 Design ………. 43

4.4.1 States……… 44

4.4.2 Steering angle……… 44

4.4.3 Complete design scheme ……….. 44

4.5 Model consideration ………. 46

iii

5 Prototype performance 47

5.1 Manoeuvre along a straight line ………... 47

5.2 Manoeuvre along a circle ………. 48

6 System error 51

6.1 IR communication error ………... 51

6.2 Steering angle error ………... 51

6.3 Odometry error ………... 52

7 Conclusions 54

A Simulink model 55

B Construction 58

List of Figures

1 The truck-semi trailer prototype ……….. 2

1.1 Generalized n-trailer system ………. 4

1.2 I-th coupling ………. 6

1.3 Truck and trailer system ……… 7

1.4 Cartesian reference ………... 9

2.1 Arc of circle for reverse motion……….. 15

2.2 Eigenvalues for the open and closed loop system (straight line case) ….. 20

2.3 Eigenvalues for the open and closed loop system (arc case) ……… 21

2.4 Example of a bad condition ………... 22

2.5 Simulated response to initial error in θ3……… 23

2.6 Circular trajectory response for the case when r₃ = 1.75 ………. 24

3.1 A hierarchical structure of hybrid systems……….. 26

3.2 A multicontroller architecture ……….... 27

3.3 A proposed path as a combination of lines and arcs ……….. 28

3.4 Circular trajectory response for the case when all states starts at zero…. 29 3.5 Simulation response of the introduced path ……….. 30

3.6 Relative Cartesian reference systems with respect to a fixed……… 31

3.7 Illustrates how a bad condition is handled by the system ……… 33

3.8 Simulated response to initial error in both θ₃ and y₃ ………... 34

3.9 Sim. of the sys. starting in bad cond. and switching after 4 cm………… 35

3.10 Switch through a small circle ………. 36

3.11 The complete hybrid system ……… 37

3.12 Simulated response to switches through a small circle ……….. 38

4.1 The RCX ……….. 40

4.2 Design scheme ………... 45

5.1 Manoeuvre along a circle ……..……… 47

5.2 Manoeuvre along a straight line ……… 48

6.1 Growing “error ellipses” due to odometry ……… 52

6.2 Results from running UMBmark ……… 53

v

A.1 The complete Simulink model ……….. 55

A.2 The lower level of the block Error indicator ………. 56

A.3 The lower level of the block Regulators ……….. 57

A.4 The lower level of the block Calculates the input ……….. 57

B.1 Overview of the odometry equipment ……….. 58

B.2 Overview of the “engine” ……… 59

B.3 Overview of the steering equipment ……….. 59

Preface

Introduction

This thesis can be divided in two parts. In the first part, the path following problem for a backward driving nonholonomic kinematic mobile truck towing trailers is addressed.

The task is to design a hybrid control scheme witch permits to combine several control algorithms and thus get a controller that consist of several subcontrollers, each designed for a special purpose, i.e. controller for backward motion along an arc or a straight line. Then by switching between those subcontrollers it becomes possible to follow generic paths. The system will then be implemented in Simulink in order to illustrate its performance.

The system developed in this work could be a valid support in many different situations, for inctance if there are some obstacles in the way of the reversing vehicle or as in Figure 1, where

Of course, the vehicle in this work is just an application; there are other cases in automotive field in which the result of this work could be useful, e.g, the vehicles which pull several trailers carrying luggage in the airports and other similar vehicles which have to drive inside factories.

The development of different systems to support the limited capabilities of humans in vehicle control such as, dynamic stability control (DSC) and active cruise control becomes more and more common in the automobile industry. The hybrid system developed in this work is done in the same spirit, namely to support the driver.

The second part of this thesis consist in prototyping the truck/trailer system using LEGO Mindstorms (see Figure 1).

The prototype can be seen as a so called autonomous robot, which is a robot that acts as a stand-alone system and is controlled by its own computer. Lego offers a programmable micro controller, called RCX, which makes it possible to build such robots.

Each RCX exposes six ports on its upper surface that can be used to read or control sensors and motors (three each) respectively.

In order to let the RCX generate the control signal for the steering, it is necessary that it receives information about the instant state of the vehicle. This information can be available through a set of onboard sensors and the control signal will be executed through a motor.

The design and implementation process shall result in a functioning system, which can later be used for further experimentation.

1

Figure 1: The truck-semi trailer prototype.

Chapter 1

Mathematical model

In order to design linear controllers for the system, a mathematical model that describes the system behavior is presented in this first chapter.

Since the speed of the reversing vehicle is low, it is assumed that the dynamic effects due to the speed can be neglected. As a result, the system can be described completely from a kinematic point of view, i.e. just by considering the displacement and speed of some points belonging to the system.

This is the approach taken in [1], where the generic n-trailer system, moving in a two- dimensional space subject to non-holonomic constraints is analysed. The three-body configuration in this thesis is a special case of that theory.

Note that vehicle parameters such as vehicle mass, inertia moment etc are not assumed to be known. The controller will thus only use measurements of the position and orientation of the vehicle to decide the steering angle.

1.1 Equations and notations

The n-trailers theory considers a wheeled vehicle system consisting of a high power truck pulling a number of passive trailers, which are allowed to have kingpin hitching.

Kingpin hitching is a kind of coupling in which the trailer is not directly hitched at the centre of the previous axle but at a distance M_i from this point (see Figure 1.1).

The system in this thesis does not have such coupling, i.e. M_i = 0 The assumptions taken when modelling the system are as follows:

· Each trailer is modelled as having only one axle.

· Each trailer is hooked up to the midpoint of the rear axle of the preceding trailer.

· The system is nonholonomic since it is subject to non integrable constraint, i.e.

the rolling without skidding of the wheels.

With these assumptions taken it become possible to write the relation of the angular velocity and longitudinal velocity as recursive dynamic laws.

3

Let θ_i be the orientation angle of the i-th axle as in figure 1.1 and v_i its longitudinal velocity, for i є (1,2,…,n-1).

If Cartesian coordinate system is assumed for orientation, the geometry of the vehicle gives:

θ₁

θ_n-2 θ_n-1

θ_n

θ₀ Y

X

Figure 1.1: Generalized n-trailer system

xi₊₁ = −xi Li₊₁cosθ_i+1 (1.1) yi₊₁ = −yi Li₊₁sinθ_i+1 (1.2) Where:

- ( xi , y_i ) are the Cartesian coordinates of the centre of the i:th axle, - θi is the angle of the i:th body with respect to the x-axis, for i > 0, - θ0 is the angle of the steering wheels with respect to the x-axis,

- Li is the distance between the centre of the i:th axle and the hitching point of the same axle, for i > 1,

Finally, L₁ is the distance between the trucks two axles (compare Figure 1.2).

From the nonholonomic constraint on the point P_i = (x_i, y_i) it follows that the axle can only move along a curve whose tangent vector is perpendicular to the axle itself, which results in the following equations:

xi =vicosθ_i (1.3)

y_i =v_isinθ_i (1.4) Rewriting equations (1.3) and (1.4) for index i+1, they can be written into the one form.

xi₊₁sinθi−yi₊₁cosθi₊₁ =0 (1.5)

By differentiation of (1.1) and (1.2) with respect to time, the recursive equation for the orientation angle θ_i+1 as function of θ_i and v_i, with i є (1, 2, …, n-1) is given by:

(

1

)

1

1

sin i i

i i

i

v L

θ₊ θ θ⁺

+

= −

 (1.6)

Using again equations (1.1) and (1.2) and resorting to trigonometry, the recursive equation for the tangential velocity of the i+1-th axle as a function of θ_i and v_i, with i є (1,2,…,n-1) can be written as:

vi₊1 =vicos

(

θ θi− i₊₁

)

(1.7) Finally, v₁ is defined as the tangential velocity, and ω as the angular velocity of the truck.

5

1.2 Case of study

The system to be built and analysed in this thesis is a particular case of the n-trailer system with n = 2, and the vehicle is made up of 3 bodies with five axles. The truck will be designed to have three axles.

As commonly done in studying these kind of systems, it can be assumed that the behaviour of a couple of axles is equivalent to a single axle positioned in the middle point of the distance between the two original axles. In this case, the number of axles decreases from five to four.

Y

X

(x_i , y_i) = P_i (x_i+1 , y_i+1)

L_i+1

Figure 1.2: i-th coupling

Lⁱ

1.3 State variables

As mentioned in the preface, linear controllers using full-state feedback will be used to make the system performe in the desired manner.

Y

X

L

¹

L

2

θ

₁

θ

₂

θ

₃ L³

Figure 1.3: truck and trailer system

θ₀

7

A system state at a given time t must give all the necessary information needed about the system which makes it possible to unambiguously determine future outputs if future inputs are known. In this case, the position of the structure must be univocally defined from the state space.

One body into a bidimensional space is fully characterized if three quantities are given. These quantities are usually chosen to be, two absolute coordinates of one point of the body with respect to an arbitrarily chosen Cartesian reference, and the angle which the body forms with the x-axis.

The vehicle under examination is made up of three rigid bodies connected by two joints which permit relative rotations. Therefore two more quantities apart from the three discussed above is necessary in order to describe the relative position of the remaining two bodies with respect to the first one. As a consequence, to fully characterize the vehicle into the bidimensional space, also the relative angles are chosen as state variables.

Since the back side of the last trailer is the furthest part from the steering wheel, it is the most critical part of the structure to be controlled. Therefore, the midpoint of the last axle is chosen as the arbitrary point, which gives the other three state variables.

This results in the following state vector:

[

3 3 3 12 23

]

x y T

X

^{= θ θ θ} (1.8)

Where the relative angle θ_ij = θ_i – θ_j.

The fixed Cartesian reference system chosen with respect to the vehicle’s position is illustrated in fig 1.4.

1.4 Kinematic Model

It is now possible to derive the non linear equations which describe the system.

Considering the state vector (1.8) and using the n-trailer theory, following equations are obtained:

X

₃

θ

₁₂

θ

₂₃

(x

₃

, y

₃

)

Y

₃

θ

₃

Figure 1.4: Cartesian reference

9

x₃=v₁cosθ₁₂cosθ₂₃cosθ₃ (1.9a)

y₃ =v₁cosθ₁₂cosθ₂₃sinθ₃ (1.9b)

_{3 1} ^{12 23}

3

cos sin

v L

θ θ

θ = (1.9c)

_{12 1} ¹²

1 2

sin v tan

L L

θ θ = ^⎛⎜ α − ^⎞

⎝ ⎠

 ⎟ (1.9d)

_{23 1 12} ^{12 23}

2 3

sin cos tan

v L L

θ θ = θ ^⎛⎜ θ − ^⎞

⎝ ⎠

 ⎟ (1.9e)

Where α = θ₀ – θ₁ is the angle of the steering wheels with respect to the truck.

1.5 Model considerations

From the non linear equation in the previous section it is obvious that two inputs are present, the steering wheel angle α and the tangential velocity of the truck v1.

It can also be notice that the velocity appears only as a scale factor in all the equations. Which means that the velocity only decides the time it takes to reach the target point and do not have any influence on the dynamic of the system. Of course, this is the case only when the velocity is low compare to the yaw rate, which is assumed for a reversing semi-trailer truck, and there is no limitation on the rate of change of the steering wheel angle α. This implies that the speed can be fixed to a constant value and the system can be considered as if it only had one input, namely the steering angle.

Through implementing equation 1.9(a)–1.9(e) in Simulink and simulate for different values of v₁ with small displacement of the initial states vector from the zero vector, which physically means that the system is not perfectly aligned, it showed out that the sign, or more correctly, the direction of the tangential velocity v₁ of the truck has great impact to the system behaviour. If the velocity is positive, which is due to a forward motion of the truck, the system is stable, while the system becomes unstable for negative velocities. The impact of the velocity is discussed further in chapter 2.

Further, some physical limitation and constrains must be taken under consideration in order to have a good model of the system. Both of the relative angles must satisfy the following constraints

θ₁₂ ≤θ_12max (1.10) θ₂₃ ≤θ_23max (1.11)

These constrains will prevent the system to reach the so called jack-knife condition, which accurse if any of the three rigid bodies would touch any of the other bodies. If any of the above defined constraints is not satisfied, it would be impossible to continue the backward motion.

The physical limitation due to the steering wheel angle gives the following constraint

α α≤ max (1.12) These values, θ_12max, θ_23max, and αmax,, will be decided when prototyping the model, using LEGO Mindstorms in chapter 4. These limitations will also be taken under consideration when modelling the system in Simulink, see Appendix A.

11

Chapter 2

Controller design

Since the model that was driven in the previous chapter showed unstable behaviour for backward motion, it is necessary to design a controller which can stabilizes the system.

As mentioned in the preface, the approach taken is based on designing linear controllers. Therefore, in section 2.1 the system will be linearized along the equilibrium points, due to the straight line and arc of circle motion. In order to find the equilibrium point, the system must first be reduced to obtain a lower dimensional system which retains the mechanical system structure of the original system. When the system is linearized, the controllers will be derived in section 2.2.

Finally, the close loop system will be simulated for some initial errors in the state vector to illustrate the performance.

2.1 Linear model

In this section the equilibrium point of the system will first be discussed, and then, after reducing the system in a proper manner, the system will be linearized about those points.

2.1.1 Equilibrium points

The model derived in the previous chapter is a system of first order nonlinear differential equations, given in the form

x = f

(

x,α

)

(2.1) Where x is the state vector and α is the input of the system.

In an equilibrium point, all the state variables are constant due to some constant input α0.

This can be expressed as

0= f

(

x0,α0

)

(2.2) Where x₀ is the constant state vector.

In the case of study, such equilibrium point does not exist. Since, for any constant value of α, at least one of the state variables x3 or y₃ will change over time, due to the motion of the vehicle.

By taking a closer look into the system, it becomes clear that for any constant steering angle, α₀, that satisfy condition (1.9) it is possible to keep at least some of the state variables constant. This can be divided in two different cases. The first case is due to α0 = 0, in which case it is possible to keep all the state variables constant, except for the first (x₃). This means that the vehicle is following a straight line trajectory. The second case is due to α₀ ≠ 0, in which case it is possible to keep two of the state variables constant, namely the relative angles θ12 and θ23, which corresponds to a circular trajectory.

In the first case, it is possible to remove the equation for x´₃ from 1.9 since x₃ do not appear as an argument in any of the other differential equations.

This leads to the following reduced state vector:

[

3 3 12 23

]

T

l = y θ θ θ

x (2.3)

With the corresponding equilibrium point:

x_l0 = ⎣⎡y3₀ 0 0 0

]

^T (2.4)

Where y₃₀ is an arbitrary value that decides which line, parallel to the x-axis, the vehicle will follow. Among those, the one with

0equal to zero is chosen, which means that the system will be linearized about the line that lies on the x-axis.

y3

In the second case it is possible to remove the equations for x´₃, y´₃ and θ´₃ from (1.9) for the same reason as in the first case.

13

This gives the following reduced state vector:

[

12 23

]

T

c = θ θ

x (2.5)

With the corresponding equilibrium point:

0 120 230

T

c = ⎣⎡θ θ ⎤⎦

x (2.6)

Where θ120 and θ230 is decided based on the radius of the circle that is chosen as the trajectory, and can be calculated for any circle, as shown below.

From trigonometry (see Figure 2.1), the following equations are derived:

₁ ¹ tan 0

r L

= α (2.7)

₁₂₀ ²

2

arctan L θ = ^⎛⎜ r ^⎞

⎝ ⎠⎟ (2.8)

0

23 3

3

arctan L θ = ^⎛⎜ r ^⎞⎟

⎝ ⎠ (2.9)

α₀ θ₁₂₀

Figure 2.1: Arc of circle for reverse motion

Where r₁, r₂ and r₃ are the radius of the circles that each rigid body follows at a constant steering angle depending on the lengths L₁, L₂ and L₃.

15

2.1.2 Linearization

Linearization of (2.1) about an equilibrium point can be presented as:

x=v1⎣⎡A x x

(

− 0

)

+B

(

α α− 0

)

⎦⎤ (2.10) Where the element of the matrix A and B are

( 0, 0)

( )ij ⁱ

j x

f

x _α

= ∂

A ∂ (2.11)

( 0, 0)

( )i ⁱ x

f α α

= ∂

B ∂ (2.12)

The straight line linearization, with the equilibrium point given by (2.4), results in the following linear model

xl =v1

[

A xl l+B_lα

]

(2.13) Where the matrixes A_l and B_l equal

³

2

2 3

0 1 0 0

0 0 0 1

0 0 1 0

0 0 1 1

l

L L

L L

⎡ ⎤

⎢ ⎥

⎢ ⎥

=⎢ − ⎥

⎢ − ⎥

⎣ ⎦

A (2.14)

1

0 0 1

0

l L

⎡ ⎤

⎢ ⎥

⎢ ⎥

=⎢ ⎥

⎢ ⎥

⎣ ⎦

B (2.15)

The arc of circle linearization with the equilibrium point given by θ₁₂⁰ and θ₂₃⁰ results in the following linear model

^xc ⁼v1_⎣^⎡Ac

(

^xc^−xc0

)

^+B

⁽

α α⁻ 0

)

_⎦^⎤ (2.16)

Where the matrixes A_c and B_c equal

( )

0

0 0

0 0 0 0

0

12 2

12 2 12

12 23 12 23

12

2 3 2 3

cos 0

cos 1 tan

tan sin cos cos

sin

c

L

L L L L

θ

θ θ

θ θ θ θ

θ

⎡ ⎤

⎢ − ⎥

⎢ ⎥

= ⎢⎣⎢⎢− ⎛⎝⎜ − ⎟⎞⎠+ + − ⎥⎥⎥⎦

A (2.17)

( )

0 0

12 2 12

2

cos 1 tan

0

c L

θ θ

⎡ + ⎤

⎢ ⎥

= ⎢ ⎥

⎢ ⎥

⎣ ⎦

B (2.18)

The system is now on the form (2.10), and the open loop system poles are given by the eigenvalues of A_l and A_c. Both of these matrixes have poles on the right half plane, which indicates that the systems are unstable, but since the matrixes

S_l = ⎣⎡B_l B A_{l l} B A_l ²_l B A_{l l}³⎤⎦ (2.19) Sc =

[

Bc B Ac _c

]

(2.20)

17

both have full rank, the systems are controllable [5]. Stabilization of the systems is conducted through full state feedback. Using this method, the system poles can be placed arbitrary. An array of negative feedback gains is used, so that the input α is proportional to the given states:

α= −Lx (2.21)

Where L is the gain array. Substituting Equation (2.21) into Equation (2.10),

(2.22)

( )

= −

x A BL x

and the poles are now given by the controlled state matrix A_R:

AR =

(

A BL−

)

(2.23) By choosing a proper gain matrix, the poles of the system can in theory be placed anywhere, and a fast and stable system can be accomplished. But actual performance is limited by the physical hardware, such as the limitation on the steering angle and the relative anglers discussed in the previous chapter.

Therefore, the best result should be achieved by optimisation between response speed and control effort. This can be accomplished using the linear quadratic regulator (LQR) method, which allows for weighting of both the state errors and the control effort. This is discussed in the following section.

2.2 Linear quadratic regulator

To solve the LQR problem means to find the optimal gain matrix L so that the state feedback law (2.21) minimizes the cost function

(

^{1 2}

0

T T

)

J dt

∞

=

∫

x Q x u Q u+ (2.24)

subject to the state dynamics (2.10), where u is the input and x the states. A more detailed description of the LQR technique is found in [5].

Q₁ is a square, symmetric and positively defined matrix and Q₂ becomes a positive scalar due to the fact that the system only has one input. Q₁ and Q₂ are considered as the design variables which through equation (2.24) gives the controller. Each controller is then examined through simulations to see if the relation between state error and input effort is acceptable.

If the input becomes too high, which would result in saturation, the punishment of the input Q₂ must be increased, and if the response is too slow in some of the states, the punishment of those states must be increased in the matrix Q₁.

To make the design of Q₁ easier, it is chosen to be a diagonal matrix

3 3

12 23

0 0 0

0 0

0 0 0

0 0 0

qy

q q

q

θ θ

θ

0

⎡ ⎤

⎢ ⎥

⎢ ⎥

⎢ ⎥

⎢ ⎥

⎢ ⎥

⎣ ⎦

(2.25)

where the indexes indicate which state variable the weight is associated to.

As a starting point all of the weights are chosen to be one. Then by calculating the corresponding controller and simulating the closed loop system for different state- errors as initial condition and investigating the responses due to this error, it is possible to get an idea of how to change the design parameters to achieve an acceptable controller.

This was done in two steps. First, by recalling that the steering angle α, the input of the system, has a limited range αmax, it becomes obvious that the weight Q₂ must be increased, while holding Q₁ fixed, until α is kept within this range. Then, in similar fashion the weight θ₁₂ and θ₂₃ must be increased until the relative anglers’ stays within their limited range, this time holding Q₂ fixed.

When a satisfying gain array is achieved, the final adjustments to get a better controller are done by direct change in the gain array in this second step. Since, it is time demanding to recalculate for each small change in the design parameters. This procedure results in the following gain arrays:

L_l = [0.013, -1.645, -6.012, 8.803] (2.26) L_c = [-4.675, 4.521] (2.27)

Where L_l corresponds to the straight line trajectory and L_c to the circular trajectory. In the latter r3 is chosen to equal 1.75. The poles of each closed loop system are viewed bellow. All the poles are now in the left half plan, meaning that the closed loop systems are stable.

19

−0.1 −0.08 −0.06 −0.04 −0.02 0 0.02 0.04 0.06 0.08 0.1

−0.1

−0.08

−0.06

−0.04

−0.02 0 0.02 0.04 0.06 0.08 0.1

Figure 2.2: Eigenvalues for the open (+), and closed (*) loop system.

−3 −2 −1 0 1 2 3

−3

−2

−1 0 1 2 3

Figure 2.3: Eigenvalues for the open (+), and closed (*) loop system.

Special case whit r3 = 1.75.

2.3 Constraints

The goal of this chapter was to stabilize an unstable system, so there where no specifications on performance. But the main task of this thesis is to follow a path, not only a straight line or a circle; therefore it is essential to have good insight on the performance. That is, to have an idea of how the system responds to different errors.

A brief discussion is followed by some simulations with initial errors in the state vector to provide this insight.

21

The straight line controller is designed to keep the system far from the jack-knife case, for which the physical limitations on the relative anglers does not allow the system to continue the backward motion. This means that the relative anglers are kept small, and so the vehicle is fairly straight at all time. As a consequence, if the vehicle would start at a distance from the trajectory line it would take along time for the vehicle to follow the trajectory line. This is also the case for error in θ₃.

The responses to displacement, or error, in the relative anglers are fast and stable for the case when both errors have the same sign, which is the case most of the time in reality. But if they have different signs, see Figure 2.4, the system becomes unstable even for small errors. This behaviour is due to the fact that for any steering angle, either one or both the relative anglers will continue to grow when they have different signs. Thus in general, the jack-knife condition will be reached, for which there do not exist any input that can bring the system further back without damaging the hitching equipment.

θ

12

α

θ

23

Figure 2.4: Bad condition.

An intuitive solution to this problem is to drive the wehicle forward until the system is able to converge. This switching approach between different controllers can also be used to follow a path, wich is the topic of the next chapter.

Some simulated responses to initial errors in the state vector are viewed in figure 2.5 and 2.6 to illustrate the behaviour. The simulations are done in Simulink, and the model is presented in Appendix A. It should be mentioned that the simulations use the non linear equations from the previous chapter to calculate the states and not the linearized models. The latter was only used to design the controllers.

It is the midpoint of the last axle that is viewed in the x-y graph, and the total length of the vehicle is 1.1 cm. recall also (see Figure 1.4) that the Cartesian reference system is defined in such a way that a forward motion is due to increasing x, i.e. the vehicle starts at the right side of the x-y graph below and moves along negative x-axis.

0 10 20 30 40 50 60 70 80

−4

−2 0 2

t [s]

θ 12 [deg]

−25 −20 −15 −10 −5 0

0.2 0.25 0.3

X [cm]

Y [cm]

0 10 20 30 40 50 60 70 80

−10

−5 0 5

t [s]

α [deg]

0 10 20 30 40 50 60 70 80

−2

−1 0 1

t [s]

θ 23 [deg]

Figure 2.5: Simulated response to initial error in θ₃ for the straight line case. From top to bottom:

trajectory followed by the vehicle, the control effort α and the relative angles θ12 and θ23.

23

As mentioned, the straight line controller is designed to keep the system far from the jack-knife condition. The simulation above shows that the controller does satisfy this demand, but at the expense of a slow response to displacement from the trajectory line.

0 2 4 6 8 10 12

10.5 11 11.5

t [s]

θ 12 [deg]

−8 −6 −4 −2 0 2 4 6 8

0 2 4

X [cm]

Y [cm]

0 2 4 6 8 10 12

12 12.5 13 13.5

t [s]

α [deg]

0 2 4 6 8 10 12

10.5 11 11.5

t [s]

θ 23 [deg]

Figure 2.5: Circular trajectory response for the case when r₃ equals 1.75 cm.

Chapter 3 Hybrid system

In this chapter a switching controller will be presented, which can derive the system backward along some predefined path.

In the previous chapter it has been shown that the vehicle is able to follow two kinds of backward trajectories, namely, straight line and arc of circle trajectories. Therefore, it is proposed that any path that is possible for the vehicle to follow, due to the physical constraints, such as the limitation of the relative anglers, can be approximated by a set of straight lines and arc of circles with different radius.

This implies that the control law must be adjust to the specific segment of the path that is being covered, which results in the switching controller that will be presented Before modelling the controller, hybrid systems and switching control will be discussed in the first section.

3.1 Hybrid systems and switching control

Hybrid systems are generally understood as reactive systems that intermix continuous and discrete components. Where the continuous components are described by differential or difference equations and the discrete components are described by finite state machines, temporal logic and if-then-else rules.

The discrete part of the system makes the decision for the entire system to switch to another set of control rules if conditions are favourable. The continuous part workes as a result according to the new rules. Thus, the system is designed to control and supervise the behaviour of the continuous components.

A hierarchical structure arises when a logical control unit governs a collection of different continuous systems by issuing logic decision. This leads to the system framework shown in Figure 3.1, which clearly illustrates this architecture.

25

The top layer, which is a discrete event system, can as mentioned above use different type of description language. The one used in this work is a set of, if-then-else rules, and the continuous system is the physical system with different LQ controllers.

The interface plays the role of facilitating communication between the two different layers by means of translating signals between them.

To conclude, by using a hybrid control scheme it is possible to combine several sub controllers, each designed for a specific purpose. The purpose in this work is to drive the vehicle along different trajectories. An example of a multicontrol architecture is shown in Figure 3.2.

Discrete event system

Interface

Continuous system

Figure 3.1 A hierarchial structure of hybrid systems

The basic idea here is that the process output y is fed back to a set of controllers C_i, which provides different inputs U_i to the system. Which input signal used finally is decided by the supervisor, U = U_m.

For a more detailed description of hybrid systems see [6, 7].

3.2 Switching controller design

In order to design a realistic switching controller, two different types of switching approaches to follow a path will be proposed and examined through simulations to get an idea of the performance before the final hybrid system is designed.

The first approach consist in approximating the path by a set of straight lines and arcs with different radius, as it was proposed in the introduction, while the second consist in approximating the path purely by straight lines.

Each segment of the approximated path is then covered by the vehicle using a controller, designed for that specific segment.

3.2.1 Segmentation through arcs and lines

In order to se how well the system performs, i.e. how exact it follows some trajectory, a path has been introduced in Figure 3.3, which is a combination of straight lines and arc

C

₁

u

₁

C

₂

C

_i

u

₂

u

_i

Supervisor

Process

m

u

_m

y

Figure 3.2 A multicontroller architecture

27

of circles whit two radius.

The switching controller in Figure 3.2 has been implemented in Simulink (Appendix A). This particular path required a switching controller with five subcontrollers, namely, the straight line controller and four arc controllers. Two arc controllers are needed for each radius, due to the clockwise and anti-clockwise motion of the vehicle.

For simplicity, the model is assumed to be free from noise and errors when the hybrid system is designed. Errors are discussed in chapter 5.

The supervisor switches between the regulators based on the current position of the midpoint of the last axle. Before simulating for the path in Figure 3.3, a closer inspection of the arc regulator is needed.

The same controller that was used for the simulation in Figure 2.6, which is designed to follow a circle with a radius of 1.75 cm, is used to simulate for the case when all states are set to zero as the simulation starts. This initial condition can be seen as if the vehicle just has switched from a straight line segment to an arc segment.

Y

X

Switchpoint

r₂ r₂ r₂

r₁

r₁ r₁

Figure 3.3: A proposed path as a combination of lines and arcs.

0 2 4 6 8 10 12

−10 0 10 20

t [s]

θ 12 [deg]

−8 −6 −4 −2 0 2 4 6 8

0 2 4

X [cm]

Y [cm]

0 2 4 6 8 10 12

−10 0 10 20

t [s]

α [deg]

0 2 4 6 8 10 12

0 5 10 15

t [s]

θ 23 [deg]

Figure 3.4: Simulate for the case when all states are set to zero as the simulation starts.

As it can be seen in the figure 3.4, the circular trajectory has been dislocated a short distance from the origin, which was to be seen as a switching point. By recalling equation 2.5, it becomes clear the displacement is due to the fact that the arc regulator only controls the relative anglers, and not the absolute position of the vehicle. So where the trajectory circle appears in a fixt Cartesian reference depends only on the initial condition.

By first simulate the regulators with some appropriate initial errors, it becomes possible to take this displacement into account when the switching controller is designed. This was done by changing the switching instruction given to the supervisor according to the displacement, and resulted in the following simulation:

29

−50 −45 −40 −35 −30 −25 −20 −15 −10 −5 0

−5 0 5

X [cm]

Y [cm]

0 50 100 150

−50 0 50

t [s]

α [deg]

0 50 100 150

−20 0 20

t [s]

θ 23 [deg]

0 50 100 150

−20 0 20

t [s]

θ 12 [deg]

Figure 3.5: Simulation of the path, introduced in Figure 3.2

This approach gets rather lengthy since for each switch point, the system must first be simulated to se the displacement and then modified accordingly.

A better approach is to approximate the path purely by straight lines, since, the regulator that was designed to follow a line do control the absolute position parameter y₃. This approach is the topic in the following subsection.

3.2.2 Segmentation purely through lines

By recalling the equilibrium point of the straight line linearization (2.4), it becomes clear that the system do not have any equilibrium point for lines which forms an angle with the x-axis, since the value of y₃ and θ₃ would constantly increase or decrease. As a consequence, it is not possible to find a linear controller for those lines.

To overcome this problem, some relative Cartesian reference systems with respect to a fixed one are introduced.

Where the first Cartesian reference (see Figure 3.6) is considered to be the fixed one. i.e.

the point (x, y) represent the global position of the vehicle, and (xⁱ, yⁱ) the position in the i:th Cartesian reference system.

The origin of the fixed system is seen as the starting point of the path, the point (x₀ⁱ, y₀ⁱ) is the origin of the i:th Cartesian reference expressed in the fixed one, and (φⁱ) is the angular, which the x-axis of the i:th system forms with the fixed systems x-axis of the fixed one. This gives the following relation between the global and relative state vector:

y⁰₃

x⁰₃ y¹

3

x¹

3

φ¹₀ (y¹₀ , x¹₀)

(yⁱ₀ , xⁱ₀) φⁱ₀ yi

3

x_i

3

Figure 3.6: Relative Cartesian reference systems with respect to a fixed.

31

0 0

3 3 3

3 3 3

3 3 12 12 23 23

cos sin 0 0 0

sin cos 0 0 0

0 0 1 0 0

0 0 0 1 0 0

0 0 0 0 1 0

i i i i

i i i

i i

i i

i

x x x

y y y

ϕ ϕ

ϕ ϕ

θ θ ϕ

θ θ θ θ

⎡ ⎤

⎡ ⎤ ⎡ ⎤ ⎡ ⎤

⎢ ⎥

⎢ ⎥ ⎢− ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥

⎢ ⎥ ⎢= ⎥ ⎢ ⎥−

⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥

⎢ ⎥ ⎢ ⎥ ⎢ ⎥

⎢ ⎥

⎢ ⎥ ⎢⎣ ⎥ ⎢ ⎥⎦⎣ ⎦

⎣ ⎦ ⎣ ⎦

(3.1)

The relative anglers’ θ₁₂ and θ₂₃ are of course unchanged, since those are the anglers the trailers form with the truck.

It is now possible to use the same controller for each approximated line segment of the path, simply by changing the state vector as in equation (3.1).

Making sudden changes to the states can lead to instability, it is therefore necessary to introduce the subsystem mentioned in the previous chapter, namely the one that switches to forward controller when the system is close to reach a bad condition.

The bad conditions here are either the case when one of the relative anglers becomes too large or when the relative anglers have different signs, in both case the so called jack-knife condition could be reached.

In order to design the subsystem, some questions needs to be answered, namely, when to switch to the forward controller, how long to continue the forward motion and how to find back to the line where the bad condition occurred.

These questions can be answered by introducing some appropriate thresholds for when to switch to the forward controller and for how long the system should continue the forward motion.

By recalling the limitation on the relative anglers from chapter one, and leave some margin, ε1, a good choice of the thresholds for when the relative anglers are considered to be too large can easily be achieved.

θ₁₂ ≥θ_12max − ε₁ (3.2a) θ₂₃ ≥θ_23max − ε₁ (3.2b) For the case when the relative anglers have different sign, the threshold, ε2, can be obtained trough simulation.

^sign

( )

θ₁₂ ≠^sign

( )

θ₂₃ (3.3a) θ₁₂ +θ₂₃ ≥ ε₂ (3.3b) When these conditions are met, the systems condition is considered to be bad, and thus the system will switch to the forward controller.

The threshold, ε3, for which the system switches back to the backward controller, is chosen in such a way that when it is met, the system can continue the backward motion rather easy.

θ₁₂ +θ₂₃ ≤ ε₃ (3.4)

It should be mentioned that the system with the forward controller is much faster then the backward controller. Therefore, the vehicle does not need to travel any large distances to bring the system to the desired condition (3.4).

Figure 3.7 illustrates how a bad condition is handled by the subsystem.

If the bad condition occurs in the i:th line segment, the switching controller simply switch to the forward controller and calculate the states trough equation (3.1) for the i:th line, as in Figure 3.7. The system continues the forward motion until condition (3.4) is met, and then switches back to the backward controller, still using equation (3.1) for i:th line to calculate the states.

In the previous chapter, the straight line controller was designed to keep the relative anglers small, due to the hard constraints on the relative anglers. These hard constraints are no longer as critical since it now exists a subsystem that prevents the system to reach them. As a consequence the controller can be modified to be more suitable for the switching controller. This modification is done to increase the speed of the system. The new controller is achieved through changing the design variables Q₁ and Q₂ in such a way that the state variables y₃ and θ₃ becomes more critical (see Figure 3.8). This results in the following gain array:

L_l = [0.550, -2.022, -5.863, 8.609] (3.5) Figure 3.9 shows how the hybrid system respons to switches, and also how the subsystem designed to handle bad conditions performs. Recall that the vehicles total length is 1 cm

y^i-1₃

x^i-1₃ y₃ⁱ

xⁱ

3

Figure 3.7 illustrates how a bad condition is handled by the subsystem.

Bad condition accures

Forward motion i:th linesegment

33

0 10 20 30 40 50 60 70 80

−10

−5 0 5

t [s]

θ 12 [deg]

−250 −20 −15 −10 −5 0

0.1 0.2 0.3

X [cm]

Y [cm]

0 10 20 30 40 50 60 70 80

−40

−20 0 20

t [s]

α [deg]

0 10 20 30 40 50 60 70 80

−5 0 5

t [s]

θ 23 [deg]

Figure 3.8: Simulated response to initial error in both θ₃ and y₃ with the modified controller.

0 10 20 30 40 50 60

−40

−20 0 20

t [s]

θ 12 [deg]

−12 −10 −8 −6 −4 −2 0 2

−1 0 1 2

X [cm]

Y [cm]

0 10 20 30 40 50 60

−20 0 20

t [s]

α [deg]

0 10 20 30 40 50 60

−10 0 10 20

t [s]

θ 23 [deg]

Figure 3.9: The system starts in a bad condition and is instructed to switch to a line that forms an angle of 35 degrees with the x-axis after 4 cm. The star shows the starting point.

This second approach gives several advantages comparing to the first. The position of the midpoint of the last axle is being controlled at all time and thus, no knowledge about displacements at each switch, due to initial errors, is needed in advance.

It becomes possible to follow lines which form an angle with the x-axis (φⁱ ), even though it is still not possible to switch between lines which forms an angle larger than 25 degree with each other. It should be mention that this problem can not be overcome by making quick changes between lines, since it takes some time for the system to be align to each new line segment.

The path segmentation becomes easier, since we no longer need a lot of circles to describe the path, and as a consequence, it is not necessary to derive several controllers to follow a generic path, the straight line controller is enough.

35

It becomes possible to develop a simple and effective fail-soft operation that secures the system from crashing at the moment it experience a serious error, i.e. the bad conditions discussed above.

The big disadvantage, using the second approach is that the system looses much of the mobility it had in the first approach, that is the capability of manoeuvre in small spaces.

In the following subsection, it is proposed that by combining these two approaches, a functional and more realistic switch controller can be achieved.

3.2.3 The complete hybrid system

The approaches discussed above are combined by first finding the smallest circle that the vehicle can perform on, and then switch to that circle at each line-intersection see Figure 3.10. This approach is Similar to Dubin´s car problem [8].

In this way, each switch between two lines is done in two steps. First, at a distance (dⁱ) from the intersection point, the system switches from straight line controller to the arc controller. This distance is proportional to the angle the two lines form (φⁱ - φ^i-1), compare Figure 3.10a to 3.10b, and is given by:

r₁ r₁

dⁱ Switchpoint dⁱ

i:th linesegment

(i-1):th linesegment

Figure 3.10: switch through a small circle.

(3.10 a) (3.10 b)

1 2

i i

d =aϕ +a (3.6)

Note that the displacement, se Figure 3.8, originating from switches from line to a circle is considered in (3.6) through a2.

In the second step, switch from the circle to the line, the switch occurs when the coordination angle of the last trailer with respect to the global Cartesian references x-axis is equal to the angle i:th line forms with the global Cartesian references x-axis.

θ₃ = −ϕⁱ (3.7)

In this way, the system can switch between line segments, independent on what angle the two line form and the vehicle gets aligned faster to each line segment, see Figure 3.12. The complete hybrid system is presented in figure 3.11.

Figure 3.11: the complete hybrid system.

37