Development of an active suspension control strategy for a forestry machine with pendulum arms

(1)

Development of an active suspension

control strategy for a forestry machine

with pendulum arms.

JORGE AGUILAR

MORGAN VIKTORSSON

Master of Science Thesis

Stockholm, Sweden 2014

(2)

(3)

Development of an active suspension control strategy

for a forestry machine with pendulum arms.

Jorge Aguilar Morgan Viktorsson

Master of Science Thesis MMK 2014:47 MDA 474 KTH Industrial Engineering and Management

Machine Design SE-100 44 STOCKHOLM

(4)

(5)

.

Master of Science Thesis MMK 2014:47 MDA 474

Development of an active suspension control strategy for a forestry machine with pendulum

arms

Approved: Examiner: Supervisor:

Jan Wikander Björn Möller

Commissioner: Contact person:

Skogforsk Björn Löfgren

.

Abstract

.

The forest industry depends on the cut to lenght method for production. The for-warder is one of the machines used in this process. These machines currently have no suspension, which leads to big problems such as lower productivity and higher soil damage.

A relatively new approach to solve these problems is to use pendulum arms actuated by hydraulic cylinders. This solution turns out to be complex, since there are more actuators than variables controlled. This paper discusses several strategies to control such a system in order to minimize pitch and roll, while decoupling from vertical accelerations.

A solution based on a nonlinear car model is suggested, and an implementation based on this model and using a pole placement technique is developed and compared in simulink with the same model with no suspension.

Results show that this strategy is able to reduce pitch and roll considerably, while offering the possibility of adding a vertical displacement controller with little coupling between each other. The next steps towards implementation are to integrate the dynamics of the actuator, to consider robustness and noise interference, as well as the effects of discretization.

(6)

(7)

.

Examensarbete MMK 2014:47 MTK 474

Utveckling av en styrstrategi för en pendelarmsskotare med aktiv fjädring

Godkänt: Examinator: Handledare:

Jan Wikander Björn Möller

Uppdragsgivare: Kontaktperson:

Skogforsk Björn Löfgren

.

Sammanfattning

.

Skogsindustrin idag använder i stor utsträckning skördar/skotar metoden för att producera. Skotaren är en av de maskiner som används i processen. Dessa maskiner har i dagsläget inngen fjädring, vilket leder till stora problem som exempelvis lägre produktionstakt och markskador.

En relativt ny lösning för att lösa problemet är att använda pendelarmar med hydrauliska cylindrar som hjulupphängning. Lösningen visar sig snabbt vara kom-plex, eftersom det finns fler ställdon än de variabler som skall regleras. I den här rapporten diskuteras flera strategier för att få bukt med problemet och minimera roterande och vertikala rörelser.

En lösning som baseras på en olinjär dynamisk modell tas fram, och en imple-mentation som bygger på en polplacerings teknik utvecklas och jämförs med den olinjära modellen, utan fjädring, i Simulink.

Resultaten tyder på att strategin reducerar de roterande rörelserna avsevärt, medans möjligheten att styra den vertikala rörelsen är stor och påverkar rotationerna minimalt. Nästa steg i utvecklingen av fjädringssystemet är att även ta till hänsyn den hydrauliska cylinderns dynamik, och undersöka hur robust systemet är och ta i beakt det faktum att systemet skall verka i diskret tid.

(8)

(9)

Acknowledgement

We would like to present our sincere thanks to Björn Möller for the help and guidance provided during the semester of this thesis project. We would also like to thank Bengt O. Eriksson for his support and technical knowledge and Björn Löfgren along with the rest of Skogforsk for providing this project.

A big thanks goes out to Federico Baez for discussing relevant topics to both of our thesises, and we also thank the rest of the guys in the room we where seated for the amazing coffee breaks.

Last but not least, we want to thank both of our families for the help and support they have provided, not only during this project but rather throughout all of the years we have spent studying, we truly love all of you.

(10)

(11)

List of Tables

3.1 Different small angle approximations considered. . . 17 3.2 Control parameters used in the leveling controller. . . 30 3.3 Control parameters used in the skyhook controller. . . 31 4.1 The results of Wang (2012) in terms of RMS values for roll, pitch and

vertical displacement and the RCI value of the accelerations. . . . . 39 4.2 The results of the leveling controller in terms of RMS values for roll,

pitch and vertical displacement and the RCI value of the accelera-tions. The change is the value compared to Wang’s result in table 4.1 . . . 40 4.3 The corresponding RCI values of different scaling factors of the hybrid

controller. The dashes indicate the two settings that did not finish their simulations. . . 42 4.4 The results of the hybrid controller in terms of RMS values for roll,

pitch and vertical displacement and the RCI value of the accelerations when a scaling of 50% leveling control and 50% vibration control is applied. The change is the value compared to Wang’s result in table 4.1 43 4.5 RMS comparison for the models with and without control. . . 47 C.1 Table listing the sensors relative for controlling the forwarder. . . 63

(14)

(15)

List of Figures

1.1 A CAD model of the XT28 forwarder. . . 2 1.2 A CAD model of the XT28 forwarder showing the pendulum arm

configuration. . . 2 1.3 The pendulum arm configuration with the hydraulic actuator (A), the

point where the arm connects to the chassi (B) and the placement of the wheel (C). . . 3 2.1 Representation of Roll-Pitch-Yaw angles. Figure taken from Sciavicco

and Siciliano (2000). . . 6 2.2 Sun et al. (2013). Half-car suspension system. . . 7 2.3 Evers et al. (2009) Schematic representation of the control model . . 9 2.4 Chen et al. (2012) Schematic of the skyhook principle applied to a

2 DOF system with a suspended mass (Body) and an unsuspended mass (Tyre) . . . 11 2.5 Schematic of a PID Neural Network with signals and their respective

weight factor. The two neurons on the left represent the input layer, the three in the middle represent the hidden layer and the one on the right represents the output layer. The signals r, y and u are the reference-, system output- and control signal. Figure taken from Di Wu and Huang (2012). . . 13 3.1 Orientation in 3D space. A point A in its initial configuration is

translated to its new location, Anew. . . 16

3.2 Pendulum arm rotation around a chassis point. . . 16 3.3 Calculated pendulum arm angles (left) and the error in percentage

(right) for varying pitch and roll. . . 17 3.4 Calculated pendulum arm angles (left) and the error in percentage

(right) for varying pitch. . . 18 3.5 Calculated pendulum arm angles (left) and the error in percentage

(right) for varying roll. . . 18 3.6 Schematic representation of the geometry of the model. . . 19 3.7 Schematic representation of the dynamic model on the right side. The

(16)

3.8 Illustration of forwarder model with the test track as used by Wang (2012). The left picture’s weels have been modified to comply with

the XT28 and the right picture’s weels are unmodified. . . 22

3.9 Schematic representation of the passive model being developed. . . . 24

3.10 The resulting input signals to the dynamic forwarder model. The bumps are spaced two meters apart. . . 24

3.11 Graphic representation of the test track used for all simulations. . . 25

3.12 Response of the linearized full car model with passive suspension for the road profile in 3.10. From top to bottom, vertical displacement, roll and pitch angles. . . 25

3.13 Response of the linearized full car model with passive suspension for the road profile in 3.10. From top to bottom, vertical, roll and pitch velocities. . . 26

3.14 Response of the linearized full car model with passive suspension for the road profile in 3.10. From top to bottom, vertical, roll and pitch accelerations. . . 26

3.15 System response for the road profile shown in 3.10. From top to bottom, vertical displacement, roll, pitch, and the input road. . . 27

3.16 System response for the road profile shown in 3.10. From top to bottom, vertical velocity, roll derivative, pitch derivative and the input road. . . 28

3.17 Schematic representation of the dynamic model without passive sus-pension. . . 32

3.18 Pole zero map of the vertical displacement. . . 33

3.19 Pole zero map of the roll motion. . . 34

3.20 Pole zero map of the pitch motion. . . 34

3.21 New pole location (blue) versus old (green) for the vertical displace-ment response. . . 35

3.22 New pole location (blue) versus old (green) for roll response. . . 36

3.23 New pole location (blue) versus old (green) for pitch response. . . . 36

4.1 A plot of the measured euler angles of the vehicle when running with Wang’s combined passive and active suspension strategy. . . 40

4.2 A plot of the measured Euler angles of the vehicle when running with the leveling controller only. . . 41

4.3 A plot of the measured euler angles of the vehicle when running with the skyhook controller only. . . 42

4.4 A plot of the measured Euler angles of the vehicle when running with 50% leveling control and 50% vibration control. . . 43

4.5 On top, the resulting output for the individual controller for vertical displacement, roll and pitch respectively. At the middle, the corre-sponding six actuating forces that generate the same response as the individual controllers shown above. The bottom shows the road input, as reference. . . 44

(17)

4.6 In blue, the controlled response. In orange, the model response with-out control for vertical displacement, roll and pitch. At the bottom, the road profile is shown for reference. . . 45 4.7 In blue, the controlled response. In orange, the model response

with-out control for vertical velocity, pitch and roll. At the bottom, the road profile is shown for reference. . . 46 4.8 In blue, the controlled response. In orange, the model response

with-out control for acceleration. At the bottom, the road profile is shown for reference. . . 47 A.1 Simulink model of the forwarder. . . 58

(18)

(19)

CHAPTER 1. INTRODUCTION

Chapter 1 Introduction

This chapter describes the background of the problem, states its purpose and de-limitations, investigates some methods that could be applied and summarizes the outline of the report.

1.1 Background and problem description

In Scandinavian countries, the cut-to-length timber harvesting systems have been used for many years, and currently this procedure is the most important one in use (Chiorescu and Gronlund, 2001). The system is based on two machines only: the harvester for felling and processing, and the forwarder for extraction (Spinelli et al., 2011).

The environmental damage caused by these machines can be noticeable on the remaining trees, growth surface covering and soil (Wijekoon, 2012). Compared to other systems, the cut-to-length system causes more soil damage in each trip, due to increased payload per trip (Akay et al., 2007). The main factor that influences the impact of soils is the nominal ground pressure of the tire. The nominal ground pressure can vary depending on the dynamic wheel load and tire dimensions (Akay et al., 2007).

Another reason for the need of having suspensions in forestry machines is to increase productivity. Leveling of the machine would ease the job performed by the operator, as it would offer a better visibility and the ability to drive faster through rough terrain.

Several approaches can be used to address the issues from lacking a suspension. One of them is to use an active damping control with pendulum arms to distribute the wheel load. The XT28 forwarder is displayed in figures 1.1 and 1.2, and the principle of having the wheels mounted on pendulum arms is shown in figure 1.3. A controller is needed to control the hydraulic actuators moving the wheel in the arc movement.

(20)

Figure 1.1: A CAD model of the XT28 forwarder.

Figure 1.2: A CAD model of the XT28 forwarder showing the pendulum arm

(21)

Figure 1.3: The pendulum arm configuration with the hydraulic actuator (A), the

point where the arm connects to the chassi (B) and the placement of the wheel (C).

A simplified forwarder simulation model based on this concept was developed by Wang (2012) in SimMechanics with the chassis parts simplified to rectangular boxes and the pendulum arms actuated without the use of an hydraulic model.

1.2 Purpose

The purpose of this master thesis is to investigate and develop an active damping control system for a forwarder equipped with six pendulum arms (XT28). The con-troller model performance shall be evaluated with simulations in Matlab/Simulink, and compared towards the results of Wang (2012). The problem description can be summarized as: "Develop an active damping suspension strategy for a pendulum arm forwarder in order to minimize the angular motion in pitch and roll".

1.3 Delimitations

In this master thesis project, the following delimitations are stated:

• The controller shall not be implemented on the real hardware. The reason of this is that the prototype was not finished by the time this thesis started.

(22)

• The controller handling the valve position to make the hydraulic cylinder (vis-ible in figure 1.3) output a desired force will be considered but not developed. • For development and validation the model provided to us, developed by Wang (2012), will be used, as well as a linearized model obtained from Adams. The parameters of the model will not be tampered with, but small modifications will be done and explained further on in this report.

1.4 Method description

A simplified model, developed by Wang (2012), of a forwarder with pendulum arms to connect the chassis and tires was provided in Simulink/SimMechanics. The pa-rameters were based on the information obtained in Autodesk/Inventor software.This model will be used to design and evaluate control strategies.

Several control strategies exist, therefore a study on relevant control methods will be performed, and the most suitable ones will be selected for implementation. A thorough analysis will be performed on the results obtained for each of the controllers. Finally, performance of the simulated controllers will be verified using an ADAMS model of the XT28 forwarder.

1.5 Expected outcome

For this master thesis work, the following outcomes are expected:

• Evaluate and develop different models to analize a forwarder suspension. • Investigate and implement control strategies applied to active suspensions. • Evaluate the performance of the controllers implemented.

1.6 Report outline

In the Theory chapter, different theorems that help control a vehicle suspension will be described.

The Implementation chapter describes the process of modeling and simulation of the control strategies.

All results concerning the implementation of the controller will be stated concisely in the Results chapter.

The Conclusions chapter analyses the results and puts them into context while the Recommendations chapter suggests both further development of the implementation and different approaches.

(23)

CHAPTER 2. FRAME OF REFERENCE

Chapter 2 Frame of reference

This chapter investigates theories needed to understand the different solutions to active vehicle suspension and also prior solutions on how to control active vehicle suspension.

2.1 Theory behind suspension technology

This section brings up topics needed in order to follow the later stages of implementa-tion in both previous implementaimplementa-tion strategies and for the implemented controllers later described in this report.

2.1.1 Rotational Matrices

The following information is taken from Sciavicco and Siciliano (2000). This section will briefly introduce the euler angles and the rotation matrices, which are the basis for building an inverse kinematic model. This model is used to determine what pendulum arm angle must be set to each arm so that the whole forwarder would take a certain pitch and roll angle. This is the basis of a levelling controller. Three parameters are sufficient to describe orientation of a rigid body in space. A minimal representation of orientation can be obtained by using a set of three angles φ = [ϕ ϑ ψ]T.One of these sets are the ZYX angles, also called Roll-Pitch-Yaw angles. These angles are shown in figure 2.1.

(24)

CHAPTER 2. FRAME OF REFERENCE x ψ ϑ y φ z

Figure 2.1: Representation of Roll-Pitch-Yaw angles. Figure taken from Sciavicco

and Siciliano (2000).

The rotation resulting from Roll-Pitch-Yaw angles can be obtained as follows: • Rotate the reference frame by the angle ψ about axis x (yaw).

Rx(ψ) =    1 0 0 0 cos(ψ) −sin(ψ) 0 sin(ψ) cos(ψ)    (2.1.1)

• Rotate the reference frame by the angle ϑ about axis y (pitch).

Ry(ϑ) =    cos(ϑ) 0 sin(ϑ 0 1 0 −sin(ϑ) 0 cos(ϑ)    (2.1.2)

• Rotate the reference frame by the angle ϕ about axis z (roll).

Rz(ϕ) =    cos(ϕ) −sin(ϕ) 0 sin(ϕ) cos(ϕ) 0 0 0 1    (2.1.3)

The resulting frame orientation is obtained by composition of rotations with respect to the fixed frame, and it can be computed via premultiplication of the

(25)

matrices of elementary rotation.

R(φ) = Rz(ϕ)Ry(ϑ)Rx(ψ) =    c(ϕ)c(ϑ) c(ϕ)s(ψ)s(ϑ) − c(ψ)s(ϕ) s(ψ)s(ϕ) + c(ψ)c(ϕ)s(ϑ) c(ϑ)s(ϕ) c(ψ)c(ϕ) + s(ψ)s(ϕ)s(ϑ) c(ψ)s(ϕ)s(ϑ) − c(ϕ)s(ψ) −s(ϑ) c(ϑ)s(ψ) c(ψ)c(ϑ)    (2.1.4)

2.1.2 Half Car Model

Sun et al. (2013) describes the way to implement a nonlinear half car model. This model is a great start to analyze nonlinear dynamic behavior in vehicles, and will be used to generate a model that describes motion in heave, pitch and roll. Figure 2.2 shows the model developed

Figure 2.2: Sun et al. (2013). Half-car suspension system.

In this case, the dynamic equations of the sprung and unsprung masses are given by the following equations:

M ¨zc+ Fdf+ Fdr+ Fsf + Fsr= uz+ F l I ¨ϕ + a(Fdf+ Fsf) − b(Fdr+ Fsr) = uϕ+ Fϕ

mfz¨1− Fsf − Fdf+ Ftf+ Fbf = −u1

mfz¨2− Fsr− Fdr+ Ftr+ Fbr = −u2. (2.1.5)

(26)

spring, the damper and the tire obey:

Fsf = kf 1∆yf Fsr= kr1∆yr Fdr = bc2∆ ˙yr Fdf = bc1∆ ˙yr Ftf = kf 2(z1− zo1) Ftr = kr2(z2− zo2) Fbf = bf 2( ˙z2− ˙zo2) Fbr = br2( ˙z2− ˙zo2). (2.1.6)

Where k_{f 1} and k_r1 are the stiffness coefficients, and b_c1 and b_c2 are the damping coefficients. kf 1, kr1, bf 2 and br2 are the wheel parameters.

2.1.3 Modal Input-Output Decoupling For Controlling Active

Sus-pensions

Evers et al. (2009) propose a control strategy for an active cabin suspension of a commercial vehicle. The suspension analized in this article consists of four actuators on the corners of the cabin. By means of input-output decoupling of the linearized equations of motion the system is transformed into three decoupled single-input-single-output (SISO) loops. Each of these loops is controlled separately. This is how the problem of overactuation is adressed, since there are 3 control variables and 4 actuators, located at each corner of the cabin. This approach will be used to deal with the same problem in a forwarder suspension. Figure 2.3 shows a schematic representation of the model used by Evers et al. (2009).

(27)

Figure 2.3: Evers et al. (2009) Schematic representation of the control model

Following up on this schematic, the following equations of motion are obtained:    mtotz¨c Ixϕ¨ Iyϑ¨   = T F S_{+ T F}A_{+ ω} _(2.1.7) uS = T FS (2.1.8) uA = T FA (2.1.9) where T =    1 1 1 1 hϕs+ bl hϕs− br hϕs+ bl hϕs− br hϑs− lf hϑs− lf hϑs+ br hϑs+ br    (2.1.10) w =    −mg lfϑsFyf− lrϑsF yr + h(Fyf + Fyr) lfϕsFyf− lrϕsF yr − (lfϑs+ h)Fx    (2.1.11) FS =hF_{f l}S F_{f r}S F_rlS F_rrSiT (2.1.12)

(28)

FA=hF_{f l}A F_{f r}A F_rlA F_rrAiT . (2.1.13) Here, w is a disturbance vector, FA is a vector containing the actuator forces and FS contains the forces from the passive suspension elements. Using linear springs with stiffnes cs and linear dampers with constant ds, the modal passive suspension

inputs are given by

us = −csGs    z − zv− h ϕs− ϕv ϑs− ϑv   − dsGs    ˙ z − ˙zv ˙ ϕs− ˙ϕv ˙ ϑs− ˙ϑv    (2.1.14) and Gs =    4 2(b_l− br) 2(lf − lr) 2(b_l− b_r) 2(b2_r+ b2_l) bllr− bllf + brlf − brlr 2(lf − lr) bllr− bllf+ brlf − brlr 2(lf2+ l2r).    (2.1.15)

To decouple heave, pitch and roll controllers, take equation 2.1.9 into account. as T is not a square matrix, FA _{cannot be uniquely determined given a certain u}A_{. One}

way to find a unique transformation is to look for additional constraints, or using the pseudo-inverse (Moore-Penrose inverse). The pseudo-inverse, which is optimal in a least square sense, is given by

T†= TT(T TT)−1 (2.1.16)

and with this we can now determine FA as shown in equation

FA= T†uA, (2.1.17)

here uA is the control signal for heave, pitch and roll.

2.1.4 Ride Comfort Index (RCI)

Evers et al. (2009) uses a measurement, in order to evaluate the vibrations in a vehicle, called the Ride Comfort Index (RCI) which is calculated as

RCI = s 1 T Z T 0

(¨x2_ISO + ¨y2_ISO+ ¨z_ISO2 )dt (2.1.18) where ¨xISO, ¨yISOand ¨zISO are the ISO weighted accelerations in x, y and z directions

and T is the total time. This value represents the perception of comfort for a seated passenger and are considered more comfortable the lower the number. The RCI value will be stated in the results but is not the main goal of the simulations.

2.2 Previous control strategies

This section covers some previously developed solutions on how to control active suspensions in vehicles.

(29)

2.2.1 Skyhook control

The skyhook damper principle is built on having a damper attatched between the body that needs to be damped and a fixed point outside the system referenced to as the sky. The skyhook damper can reduce the resonance frequency of the system significantly (Chen et al., 2012). A schematic of the principle can be seen in figure 2.4.

The damper placed is not an actual part of the vehicle but more a way to model how you would want the vehicle to behave. As mentioned by Hrovat (1997), the skyhook damper is a part of the linear quadratic optimal solution to control 1 DOF quarter car suspension systems.

Figure 2.4: Chen et al. (2012) Schematic of the skyhook principle applied to a 2

DOF system with a suspended mass (Body) and an unsuspended mass (Tyre)

The skyhook principle is also used by Evers et al. (2009) to control a cabin suspension system with four force actuators. There, the skyhook is complemented with a spring and an integrating part in order to not only damp the motions of the cabin but also keep a reference height, pitch and roll.

2.2.2 Saturated Adaptive Robust Controller

Sun et al. (2013) shows an example of making a saturated Adaptive Robust Con-troller (ARC) to control the suspension of a vehicle modeled as a half-car. The implementation is an ARC together with an anti windup filter. The purpouse of an ARC controller is, as Chantranuwathana and Peng (2004) states,

“... to achive guaranteed steady state and transient tracking accuracy (properties of deterministic robust controls) and asymptotic tracking at the absence of disturbances without relying on discontinuous switching or infinite control gains (property of adaptive controls).”

(30)

In their article, Sun et al. (2013) defines a half car model in state-space and then a lemma are introduced to enable the implementation of the ARC. The lemma, which is prooven at a later stage of the article, states that all signals in the system are bounded and that the ARC will make the tracking error converge to zero asymp-totically, i.e., e1, e2 → 0 as t → ∞, which is compliant with the above quotation of

Chantranuwathana and Peng (2004).

When the ARC is defined, Sun et al. (2013) adds an anti-windup block to deal with windup due to saturation. When the signal is not saturated, the controller will strive to minimize the error e = y − r, where r is the reference and y is the system ouput, but when the control signal is saturated, the anti-windup block is enabled.

Chantranuwathana and Peng (2004) describes how to implement a modular ver-sion of the ARC (MARC), and by comparing it to an ARC with similar settings the show that the MARC perform better in terms of parameter estimation convergance. The MARC is a development of the ARC that is separated into a control module and an identifier module so that the identifier can be changed without affecting the properties of the original ARC.

The obvious downpart of theese types of controllers are that they are very coupled to the system they are controlling, and need a model of the controlled system in order to work.

2.2.3 PID Neural Network

As described by Di Wu and Huang (2012), a PID Neural Network (PID NN) is a control method implementing an adaptive algorithm to decrease the output error by changing the weight of signals within the network. The strength of this approach is that it does not need to know the system it is applied to, but it is trained to fit a system via input/output data. Di Wu and Huang (2012) describes how to use the PID NN in order to control an hydraulic motor, but it can be used for controlling other types of systems.

The PID NN consists of three layers: input-, hidden- and output-layers, with two, three and one ’neurons’ respectively, as seen in figure 2.5.

(31)

Figure 2.5: Schematic of a PID Neural Network with signals and their respective

weight factor. The two neurons on the left represent the input layer, the three in the middle represent the hidden layer and the one on the right represents the output layer. The signals r, y and u are the reference-, system output- and control signal. Figure taken from Di Wu and Huang (2012).

Every neuron in the network has both an input function and an output function. The input functions for the input layer neurons are stated as

net1(k) = r(k) (2.2.1)

net2(k) = y(k), (2.2.2)

where k is the current sample, and the output function of the input layer neurons are

xi(k) = ui(k), i = 1, 2. (2.2.3)

The hidden layers input functions are stated as

net0_j(k) =

2

X

i=1

wijxi(k), j = 1, 2, 3 (2.2.4)

and its ouput functions for the P-neuron, the I-neuron and the D-neuron respectively

x0₁(k) = net0₁(k) (2.2.5)

x0₂(k) = net0₂(k − 1) + net0₂(k) (2.2.6)

(32)

For the third and last layer, the output layer, the input and output function are defined as net00(k) = 3 X j=1 w0_jx0_j(k) (2.2.8) and u(k) = net00(k). (2.2.9)

Between every iteration the weights wij and wj are updated to mimimize the

error e = 1 m m X k=1 [r(k) − y(k)]2 (2.2.10)

where m is the number of samples considered. The new weights are then

w0_j(n + 1) = w0_j(n) + η m m X k=1 2[r(k) − y(k)]sgn _{y(k + 1) − y(k)} u(k) − u(k − 1)x 0 j(k) (2.2.11) and w_ij0 (k + 1) = w0_ij(k) + η m m X k=1 2[r(k) − y(k)]sgn _{y(k + 1) − y(k)} u(k) − u(k − 1) sgn u 0 j(k) − u0j(k − 1) net0_j(k) − net0_j(k − 1)w 0 jx0i(k) ! (2.2.12)

(33)

CHAPTER 3. IMPLEMENTATION

Chapter 3 Implementation

This chapter presents the different ways a pendulum arm can be modeled and also the solutions implemented to control an active cabin suspension using pendulum arms.

3.1 Kinematics

In this section our aim is to identify a relation between the pendulum arm angles and the chassi that connects with these pendulums in order to obtain an equation that holds true for any given euler angle of pitch and roll. This is, we want to solve the inverse kinematics of the pendulum arm and the chassis. For a 3D element, six coordinates are needed to fully represent the position of an object. One way of representing this position is with a Cartesian coordinate system and the Euler angles. As seen in the theory chapter, rotation matrices are very handy at describing the orientation of a 3D body. Based on this knowledge, a way to represent the motion of the chassis and pendulum arm such that all wheels will maintain tangential to a tilted plane at all times, is developed. A homogeneous transformation matrix is defined as " R TT 0 1 # (3.1.1)

where R is the rotational matrix and T is a translation, in our case representing the chassis center of geometry height. Figure 3.1 describes the orientation used through this section.

(34)

y

x

z

A Anew

ᶱ

ᶲ

ψ

Figure 3.1: Orientation in 3D space. A point A in its initial configuration is

translated to its new location, Anew.

A point in the chassis will be described in matrix form as A =hAx Ay Az 1

i

where the last 1 is added for consistency with the transformation matrix, and it has no effect in the output. z is defined as the vertical displacement from an arbitrary reference to the chassis. The corresponding transformation matrix is then a rotation and a translation, which yields the equations shown in 3.1.2.

AnewT =   c(ϕ)c(ϑ) c(ϕ)s(ψ)s(ϑ) − c(ψ)s(ϕ) s(ψ)s(ϕ) + c(ψ)c(ϕ)s(ϑ) 0 c(ϑ)s(ϕ) c(ψ)c(ϕ) + s(ψ)s(ϕ)s(ϑ) c(ψ)s(ϕ)s(ϑ) − c(ϕ)s(ψ) 0 −s(ϑ) c(ϑ)s(ψ) c(ψ)c(ϑ) z 0 0 0 1     Ax Ay Az 1   (3.1.2)

A rotation of the pendulum arm can be described as a rotation around the y axis of a distance P , (as it can be seen in figure 3.2 which yields the following equation for a pendulum arm P endulumT =      cos (ϑ_p) 0 sin (ϑ_p) 0 0 1 0 0 − sin (ϑp) 0 cos (ϑp) 0 0 0 0 1           P 0 0 1      =      P cos (ϑp) 0 P sin (ϑp) 1      . (3.1.3) P

ᶱ

P

(35)

After this rotation the pendulum arm in the point connected to the chassis is rotated to its new position with a different value of pitch and roll, and the resulting equation is solved for ϑp, the pendulum arm angle that yields zero at the z coordinate.

Equation 3.1.4 shows the result.

z − sin(ϑ)(Ax+ P cos(ϑp)) + cos(ϑ) cos(ϕ)(Az+ P sin(ϑp)) + Aycos(ϑ) sin(ϕ) = 0

(3.1.4) This solution, however, turns out to be complex, so any implementation is very difficult unless a solver that can handle complex solutions is used. Because of this, different small angle approximations were tried and compared for error differences in the calculated pendulum arm angle, varying both pitch and roll from 0◦ to 45◦at the same time and separately. The table 3.1 shows the different approximations compared.

Table 3.1: Different small angle approximations considered.

Type of approximation Values

Small angle 1 sin(ϑ) = ϑ

cos(ϑ) = 1 −ϑ₂2

Small angle 2 sin(ϑ) = ϑ

cos(ϑ) = 1 Maclaurin expansion sin(ϑ) = ϑ − ϑ₆3

cos(ϑ) = 1 −ϑ₂2 0 10 20 30 40 50 −2 0 2 4

Input angle, pitch and roll (degrees)

Calculated p endulum arm angle (radians)

Angle comparisons, pitch and roll Real solution Small angle 1 Small angle 2 Maclaurin series 0 10 20 30 40 50 −200 −150 −100 −50 0 50

Input angle, pitch and roll (degrees)

P

ercen

tage

err

or

Angle comparisons, pitch and roll

Small angle 1 Small angle 2 Maclaurin series

Figure 3.3: Calculated pendulum arm angles (left) and the error in percentage

(36)

CHAPTER 3. IMPLEMENTATION 0 10 20 30 40 50 0 1 2 3

Input angle, pitch (degrees)

Angle comparisons, pitch Real solution Small angle 1 Small angle 2 Maclaurin series 0 10 20 30 40 50 −20 −10 0 10

Input angle, pitch and roll

P

ercen

tage

error

Angle comparisons, pitch Small angle 1

Small angle 2 Maclaurin series

(right) for varying pitch.

0 10 20 30 40 50

0 0.2 0.4

Input angle, roll (degrees)

Angle comparisons, roll Real solution Small angle 1 Small angle 2 Maclaurin series 0 10 20 30 40 50 −20 −10 0

Input angle, roll (degrees)

P ercen tage error Angle comparisons,roll Small angle 1 Small angle 2 Maclaurin series

(right) for varying roll.

As it can be seen from the figures, it is clear that the approximation "Small angle 2" follows reasonably well in comparison with the complete solution, with an error of 1% up to 11 degrees. Using this approximation to get a closed solution gives us the equation shown in 3.1.5 when the pendulum arm is originally positioned facing forward, and clockwise motion is considered a negative angle.With the inverse

(37)

kinematics a simple controller can be implemented, in which the control would try to set the pendulum arms so that they counteract the disturbances measured in pitch and roll.

ϑp = −

z − sin (ϑ)(Ax+ P ) + Aycos (ϑ) sin (ϕ)

P cos (ϑ) cos (ϕ) (3.1.5)

3.2 Dynamics

Consider a diagram as shown in 3.6 and 3.7 as reference. A model was made to define the chassis dynamics, while considering only the z motion of the pendulum arm instead of an angle. This is done to simplify the model, since considering the pendulum arm motion would increase the model complexity and would add nonlinear behavior to the equations. To model the actuator, it was assumed to be an ideal force with a spring and a damper in parallel simulating a passive suspension. It is assumed that in the future this force can be converted to a torque, which is what the pendulum arm cylinder would produce on the joint with the arm that holds the wheel in place.

x

ᶱ

y

ᶲ

z

ψ lr lm lf bl br Zfl Zfr Zml Zrl Zrr Zmr

(38)

A

Ks Kw Dw Kw Dw Kw Dw Ds Ks Ds Ks Ds

m

tot

m

w5

m

w3

m

w1

z

c

S

o1

S

o3

S

o5

z

1

z

5

z

3

Figure 3.7: Schematic representation of the dynamic model on the right side. The

left side is identical.

The dynamic equations of the chassis are described as

mtotz¨c+ Fs1+ Fs2+ Fs3+ Fs4+ Fs5+ Fs6 = uz+ Fl (3.2.1) Ixϕ + l¨ f(Fs1+ Fs2) − lm(Fs3+ Fs4) − lr(Fs5+ Fs6) = uϕ+ Fϕ (3.2.2) Iyϑ + b¨ l(Fs1+ Fs3+ Fs5) − br(Fs2+ Fs4+ Fs6) = uϑ+ Fϑ (3.2.3)

while the wheel equations are

miz¨i− Fsi+ Fri = −ui (3.2.4)

Fri = Kw(zi− zoi) + Dw( ˙zi− ˙zoi) (3.2.5)

where i = 1, 2, ..., 6 for all the wheels in the forwarder, and uz, uϕ, and uϑ are

defined as

uz = u1+ u2+ u3+ u4+ u5+ u6 (3.2.6)

uϕ = lf(u1+ u2) − lm(u3+ u4) − lr(u5+ u6) (3.2.7)

uϑ= bl(u1+ u3+ u5) − br(u2+ u4+ u6) (3.2.8)

where u₁-u₆ are the forces from the ideal actuator as can be seen in figure 3.7, and have the same direction as z.

To calculate the vertical displacement in a point in the chassis, the kinematic relation is:

(39)

where a corresponds to the distances shown in 3.6 as l_f, l_m and l_r and b is either br or bl depending if the pendulum arm is in the right or left side of the forwarder.

The vertical displacement difference ∆z is defined as:

∆z = zc+ a sin(ϑ) − b cos(ϑ) sin(ϕ) − zi (3.2.10)

where i = 1, 2, ..., 6 for all the wheels in the forwarder. To linearize the model, a small angle approximation is used, assuming that sin(θ) = θ and cos(θ) = 1. If this is the case the following equations for the passive elements in the chassi are obtained:

Fs1= Ks(zc+ lfϑ + blϕ − z1) + Ds( ˙zc+ lfϑ + b˙ lϕ − ˙˙ z1) Fs2= Ks(zc+ lfϑ − brϕ − z2) + Ds( ˙zc+ lfϑ − b˙ rϕ − ˙˙ z2) Fs3= Ks(zc− lmϑ + blϕ − z3) + Ds( ˙zc+ lmϑ + b˙ lϕ − ˙˙ z3) Fs4= Ks(zc− lmϑ − brϕ − z4) + Ds( ˙zc− lmϑ − b˙ rϕ − ˙˙ z4) Fs5= Ks(zc− lrϑ + blϕ − z5) + Ds( ˙zc− lrϑ + b˙ lϕ − ˙˙ z5) Fs6= Ks(zc− lrϑ − brϕ − z6) + Ds( ˙zc− lrϑ − b˙ rϕ − ˙˙ z6) (3.2.11)

Rewriting these equations in a matrix form similar to Evers et al. (2009) yields the following    mtotz¨c Ixϕ¨ Iyϑ¨   = T F A_{− T F}S_{+ ω} _(3.2.12) where T =    1 1 1 1 1 1 bl −br bl −br bl −br lf lf −lm −lm −lr −lr    (3.2.13) FS =h Fs1 Fs2 Fs3 F4 Fs5 Fs6 i (3.2.14) FA=h u1 u2 u3 u4 u5 u6 i (3.2.15) and ω =h F l Fϕ Fϑ i

represents a disturbance vector. Defining T FS in terms of state variables, yields:

T FS = K_sGs    zc ϑ ϕ   + DsGs    ˙ zc ˙ ϑ ˙ ϕ   − KsT          z1 z2 z3 z4 z5 z6          − D_sT          ˙ z1 ˙ z2 ˙ z3 ˙ z4 ˙ z5 ˙ z6          (3.2.16)

(40)

CHAPTER 3. IMPLEMENTATION Matrix G_s is defined as Gs= " ₆ _3(b l− br) 2(lf− lm− lr) 3(bl− br) 3(b2r+ b2l) lfbl− lfbr− lmbl+ lmbr− lrbl+ lrbr 2(lf− lm− lr) lfbl− lfbr− lmbl+ lmbr− lrbl+ lrbr 2(l_f2+ l2m+ l2r) # (3.2.17) Finally, to decouple the controller, the actuating element TFA are redefined in terms of the pitch, roll and heave and then calculate the pseudoinverse to deal with the problem of overactuation. Now it is possible to design three separate controllers.

FA= ˜T−1ua (3.2.18) ua= h uz uϕ uϑ i (3.2.19) ˜ T−1 = T†. (3.2.20)

3.3 Different modeling implementations of the Forwarder

In order to test control strategies, a model of the forwarder dynamics was needed. During the work in this project two different types of models have been used: one based on SimMechanics and one based in the full car model.

3.3.1 SimMechanics model

As stated before, a model developed by Wang (2012) was acquired in the beginning of the thesis work. At the time of writing the model is two years old and some of the parameters is not coherent with a newer Adams model of the system that Baez (2014) was kind enough to provide. Figure 3.8 shows a graphic representation of both the unmodified model and the modified model.

Figure 3.8: Illustration of forwarder model with the test track as used by Wang

(2012). The left picture’s weels have been modified to comply with the XT28 and the right picture’s weels are unmodified.

(41)

• The orientation of the pendulum arms have ben changed to stay coherent with the real machine. The two front pendulums are facing forward while the rest of the pendulums are facing backwards.

• The waist joint have been changed from a prismatic joint to a revolute joint that can only move in a yaw motion.

In addition to this, some delimitations concerning the model have been made and the most obvious one is that only one waist jointis considered, while in the physical machine there will be two. It was deducted that this will not affect the control of the forwarder while it is driving without turning since the waist joints are actuated with two hydraulic cylinders each, with the purpouse to turn the vehicle.

The second delimitation is that the same parameter values used by Wang (2012) are going to be used. This means that the control tested on this model will use the same parameter values (distances, masses and inertias) as the SimMechanics model. In order to use the SimMechanics model, existing suspension had to be bypassed and as the model makes use of a joint actuator in the point where the pendulum arm and the chassi are connected, a torque is set as the only input to the model. A transport delay is applied to the hydraulic actuator, both to break possibly unsolv-able algebraic loops, but also to get a more real control result from the model. The delay is set to a tenth of the control loops period and represents a microcontrollers computational delay.

One of the delimitations of this project states that a model of the actuator will not be developed. The hydraulic actuator is considered ideal, but since it is known that an hydraulic actuator will have physical limitations, the input torque is ran through a low-pass filter with a bandwidth of six hertz.

The XT28 is equipped with a set of sensors, see appendix C, which all are set as the ouput variables. It was found that the model computes the ground forces of the vehicle to calculate friction forces under the wheels, and this force was also set as an output with the purpouse of monitoring it. All the ouputs are run through a zero order hold (zoh) block to simulate the discrete behaviour of the sensors. The simulation period, Ts, that where used throughout the project where: Ts= ₅₀1 = 0.02

s.

Two additional features were added to the model in order to ease the process of testing controllers. A function making use of the monitored force between ground and wheel where developed so that the model does not put any torque on the pendulum arm if it looses contact with the ground. The other feature is that the propulsion torque that the wheels are fed will affect the motion of the pendulum arm (the torque the motor puts on the wheel), but since this information will be available to read, it is countered within the forwarder model.

3.3.2 State Space model

Using the equations described in chapter 3.2, a model was developed in simulink. The first task was to compare a passive suspension, as shown in 3.9

(42)

CHAPTER 3. IMPLEMENTATION Ks Kw Dw Kw Dw Kw Dw Ds Ks Ds Ks Ds mtot mw5 mw3 zc So1 So3 So5 z1 z5 z3 _m w1

Figure 3.9: Schematic representation of the passive model being developed.

Appendix A shows the simulink implementation as a block diagram, while ap-pendix B shows the code used in Matlab. The purpose of this model is to observe the dynamic response, since real hardware implementation is not possible at the mo-ment. In order to be able to run the simulation, a road profile was built according to the bump used by Wang (2012) and that can be seen in figure 3.8 . That is, one bump on each side of the forwarder, but positioned a few meters apart. The right bump is the one that makes contact first. The resulting road profile is shown in figure 3.10, and a visual representation is visible in figure 3.11.

0 2 4 6 8 10 12 14 16 18 20 22 24 0 0.2 0.4 0.6 0.8 1 Time [s] [m]

Wheel input disturbance Front left Front right Middle left Middle right Rear left Rear right

Figure 3.10: The resulting input signals to the dynamic forwarder model. The

(43)

Figure 3.11: Graphic representation of the test track used for all simulations.

With this road input the passive dynamic response was calculated. Figures 3.12 3.13 and 3.14 show the response obtained.

8 10 12 14 16 18 20 22 24 −0.2 −0.1 0 0.1 0.2 [rad] Roll 8 10 12 14 16 18 20 22 24 −0.1 0 0.1 [m] Vertical displacement 8 10 12 14 16 18 20 22 24 −4 −2 0 2 4 · 10 −2 Time [s] [rad] Pitch

Figure 3.12: Response of the linearized full car model with passive suspension for

the road profile in 3.10. From top to bottom, vertical displacement, roll and pitch angles.

(44)

CHAPTER 3. IMPLEMENTATION 8 10 12 14 16 18 20 22 24 −20 −100 10 20 [rad/s] Roll velocity 8 10 12 14 16 18 20 22 24 −1 −0.50 0.5 1 [m/s] Vertical velocity 8 10 12 14 16 18 20 22 24 −0.2 −0.10 0.1 0.2 Time [s] [rad/s] Pitch velocity

the road profile in 3.10. From top to bottom, vertical, roll and pitch velocities.

8 10 12 14 16 18 20 22 24 −4 −20 2 4 [rad/s 2 ] Roll acceleration 8 10 12 14 16 18 20 22 24 −20 −100 10 20 [m/s 2 ] Vertical acceleration 8 10 12 14 16 18 20 22 24 −10−5 0 5 10 Time [s] [rad/s 2 ] Pitch acceleration

(45)

To compare the results obtained and check their validity, the response from a linearized model obtained from Baez (2014) as calculated by ADAMS software was obtained. Even tough the response is different, it can be estabilished that the dy-namics are similar, even though there may be discrepancies in the values assumed. One of the biggest factors that can influence the response is the center of gravity. Since no information was available, it was assumed that the center of gravity lied around the geometric center of the forwarder, assuming that the forwarder can be represented by a box, like in figure 3.6. The following figures show the comparison between the ADAMS model and the one developed, for the heave position, roll, pitch and their derivatives.

0 5 10 15 20 25 −0.1 0 0.1 [m] Vertical displacement 0 5 10 15 20 25 −0.1 0 0.1 [rad] Roll angle 0 5 10 15 20 25 −0.05 0 0.05 Pitch angle [rad] 0 5 10 15 20 25 0 0.5 1 [m] Time [s] Wheel input disturbance Full car model

Linearized ADAMS model

Full car model(roll)

Linearized ADAMS model (roll)

Full car model(pitch)

Linearized ADAMS model (pitch)

Figure 3.15: System response for the road profile shown in 3.10. From top to

(46)

CHAPTER 3. IMPLEMENTATION 0 5 10 15 20 25 −0.4 −0.2 0 0.2 [m] Vertical velocity 0 5 10 15 20 25 −0.2 0 0.2 [rad] Roll velocity 0 5 10 15 20 25 −0.4 −0.2 0 0.2 Pitch velocity [rad] 0 5 10 15 20 25 0 0.5 1 [m] Time [s] Wheel input disturbance Full car model

Linearized ADAMS model

Full car model(roll)

Linearized ADAMS model (roll)

Full car model(pitch)

Linearized ADAMS model (pitch)

Figure 3.16: System response for the road profile shown in 3.10. From top to

bottom, vertical velocity, roll derivative, pitch derivative and the input road.

3.4 Control structure

During the work in this project, some different control types have been implemented and tested against one of the models previously described. This section covers the implementation of a kinematic controller which will make use of the previously de-scribed way of how to calculate a reference angle for each of the six pendulum arm legs, a skyhook controller which will make use of the skyhook principle, a combina-tion of both the kinematic and the dynamic controllers and a controller based on pole placement with a full car model. The first three will be applied to and tested against the simulation model that Wang (2012) developed and the last one will be applied to the dynamic model derived earlier.

(47)

3.4.1 Kinematic leveling controller

In order to make use of the pendulum arm angles calculated in section 3.1, a controller is needed to correct the error between the desired angle and the actual one. To achieve this, a control signal u is calculated as

u = (r − y) · F, (3.4.1)

where F is the controllers transfer function. A PID controller with a filter was composed in continuous time, yielding the transfer function

F (s) = kP + kI

s + skDN

s + N, (3.4.2)

where the N is the filter coefficient.

This controller is then approximated to a discrete time transfer function through the Tustin transformation

s = 2(z − 1) Ts(z + 1)

. (3.4.3)

3.4.2 together with 3.4.3 yields in

F (z) = kP + kITs(z + 1) 2(z − 1) + 2(z − 1)kDN 2(z − 1) + Ts(z + 1)N . (3.4.4) Control parameters

In order to set the control parameters for a PID controller, a method described by Ogata (2001, p. 169-173) was used. The method is built upon the desire to choose a rise time, T_r, and an allowed overshoot percentage, P_overshoot‚ to get the desired control parameters. The first step is to calculate the damping coefficient

ζ = s log(Povershoot)2 π2_{+ log(P} overshoot)2 , (3.4.5)

which corresponds to the damping of the desired poles. Secondly, the desired natural frequency, ωn, is calculated as ωn= ωd p 1 − ζ2 ωd= 1 Tr (π − β) β = arctan   s 1 − ζ2 ζ  . (3.4.6)

(48)

From this, the control parameters k_P and k_D are calculated as kP = ω2n mtot 6 kD = 2ζωn mtot 6 , (3.4.7)

where mtot is the vehicles total weight, which is divided by the number of actuators.

The integral part is tuned manually in order to decrease steady state error without increasing oscillations.

With P_overshoot = 0.06 and T_r = 16T_s, where T_s = 0.02 is the sampling period, the parameters visible in table 3.2 are obtained. T_r was initially set to be ten times the sampling time, which would correspond to a good rule of thumb for the speed of the system, while the maximum overshoot was 0.02. Some basic tests showed that great improvements in the control where gained if the values were modified by hand.

Table 3.2: Control parameters used in the leveling controller.

kP kI kD N

4858 2915 3712 175

3.4.2 Dynamic vibration controller

One concern creating the kinematic control was that it would not act fast enough in order to counter vibrations around the set operating point. This lead to the development of a dynamic approach, where the actuators would be controlled as according to equation 3.2.18.

A skyhook controller was implemented with the same structure as by Evers et al. (2009) so that it would function without passive suspension, whilst also being able to account for it should it be required. The control signal is stated as

ua= csG˜s    z − h ϑ ϕ   − ˜ω− uc, (3.4.8)

where c_sis the passive suspensions spring coefficient, ˜Gsis the same as Gsin equation

3.2.17 but with the diagonal elements replaced by zeros, and where

˜ ω =    −mg hm¨y hm¨x    (3.4.9) uc=    −izR₀t(z − h) dt − (cz− 6cs)(z − h) − dzz˙ −i_ϑRt 0(ϑ) dt − (cϑ− 3cs(b2l + b2r))ϑ − dϑϑ˙ −i_ϕRt 0(ϕ) dt − (cϕ− 2cs(l2f + l2m+ l2r))ϕ − dϕϕ˙   , (3.4.10)

(49)

where i_z, i_ϑ, i_ϕ, c_z, c_ϑ, c_ϕ, d_z, d_ϑ and d_ϕ are control parameters, c_s is the passive suspensions spring constant (cs= 0 would be the case where no passive suspension

is mounted) and b_l, br, lf, lm and lr are defined in figure 3.6.

This approach was not discretisized.

Control parameters

The springs and dampeners are tuned in the same manner as described in the control parameter part of section 3.4.1, with some minor changes. ζ and ω_n are calculated as according to equations 3.4.5 and 3.4.6, and the control parameters are calculated as a spring and a damper in a damped harmonic oscillator

cz= ω2nmtot cϑ= ω2nIϑ cϕ= ω2nIϕ dz= 2ζωnmtot dϑ= 2ζωnIϑ dϕ= 2ζωnIϕ, (3.4.11)

where I_ϑ and I_ϕ are the inertias with the same rotational directions as roll and pitch respectively and mtot still being the total mass of the vehicle. This approach

yields the parameters shown in table 3.3, with P_overshoot = 0.03 and Tr = 12Ts. Tr

was initially set to be ten times the sampling time, which as stated before, would correspond to a good rule of thumb for the speed of the system, while the maximum overshoot was 0.02. The values where modified by hand to decrease RMS and RCI values. The values of the integral part of the controller is manually tuned to reduce steady state error.

Table 3.3: Control parameters used in the skyhook controller.

cz cϑ cϕ dz dϑ dϕ iz iϑ iϕ

60425 6294 251531 35798 3728 149016 24170 2517 100612

3.4.3 Hybrid controller

The goal with the two previously described controllers is to make them robust enough to work independently, but also be able to use them together in order to extract the benefits of each one and make a stronger control algorithm, a thought inspired by Cho et al. (2013). A combination of the controllers was made by simply inputting an weighted averaged torque to the model. The proportions 50% − 50%, 40% − 60%, 60% − 40%, 30% − 70% and 70% − 30% where tested, but the proportions with more vibration than leveling control where unable to finish their simulations.

(50)

3.4.4 Pole placement design based on forwarder model

With the model obtained from chapter 3.2 it is possible to develop a controller based on the poles from the system. One difference with the model described earlier is that the passive elements between the wheel and the chassi is going to be removed, and replaced with an ideal actuator, since there is no passive suspension in the real forwarder. The resulting model is shown in figure 3.17.

Kw Dw Kw Dw Kw Dw

m

tot

m

w5

m

w3

z

c

S

o1

S

o3

S

o5

z

1

z

5

z

3

_m

w1 A A A

Figure 3.17: Schematic representation of the dynamic model without passive

sus-pension.

In state space form the following equation results:

˙

x = Ax + KwT so+ DwT ˙so+ T FA+ F . (3.4.12)

Equations 3.4.13, 3.4.14 , 3.4.15, 3.4.16, 3.4.17 and 3.4.18 show the states and the matrices used. x =hzc z˙c ϕ ϕ˙ ϑ ϑ˙ i (3.4.13) A =           0 1 0 0 0 0 −6Kw mtot −6Dw mtot −C2Kw mtot −C2Dw mtot −C1Kw mtot −C1Dw mtot 0 0 0 1 0 0 −C2Kw Ix −C2Dw Ix −C4Kw Ix −C4Dw Ix −C3Kw Ix −C3Dw Ix 0 0 0 0 0 1 −C1Kw Iy −C1Dw Iy −C3Kw Iy −C3Dw Iy −C5Kr Iy −C5Dw Iy           , (3.4.14) C1= 2(lf − lr− lm) C2= 3(bl− br) C3= (lfbl) − (lfbr) − (lmbl) + (lmbr) − (lrbl) + (lrbr) C4= 3(b2r+ b2l) C5= 2(l2f + l2m+ l2r) (3.4.15)

(51)

CHAPTER 3. IMPLEMENTATION so = h so1 so2 so3 so4 so5 so6 i (3.4.16) ˙ so = h ˙s_o1 ˙s_o2 ˙s_o3 ˙s_o4 ˙s_o5 ˙s_o6i (3.4.17) F =hFl Fϕ Fϑ i (3.4.18)

where s_o and ˙so are the road input and its derivative to the wheel as seen in 3.10,

the T matrix is defined in equation 3.2.13, and FAis defined in equation 3.2.15.The vector F can be used to represent a disturbance, but also it can represent an input for a controller as if it was a single input. Estimating the wheel coefficients as Kw = 1500000 and Dw = Kw/10for the spring element and the damper, a transfer

function was obtained from each of the states to each of the corresponding inputs in vector F . The following pole-zero maps were obtained for vertical displacement, roll and pitch.

−140 −120 −100 −80 −60 −40 −20 0 −150 −100 −50 0 50 100 150 0.08 0.16 0.26 0.36 0.48 0.62 0.78 0.94 0.08 0.16 0.26 0.36 0.48 0.62 0.78 0.94 20 40 60 80 100 120 140 20 40 60 80 100 120 140 Pole−Zero Map

Real Axis (seconds−1)

Imaginary Axis (seconds

−1

)

(52)

CHAPTER 3. IMPLEMENTATION −0.7 −0.6 −0.5 −0.4 −0.3 −0.2 −0.1 0 −4 −3 −2 −1 0 1 2 3 4 0.105 0.15 0.24 0.45 0.5 1 1.5 2 2.5 3 3.5 4 0.5 1 1.5 2 2.5 3 3.5 4 0.014 0.032 0.05 0.075 0.105 0.15 0.24 0.45 0.014 0.032 0.05 0.075 Pole−Zero Map

−1

)

Figure 3.19: Pole zero map of the roll motion.

−18 −16 −14 −12 −10 −8 −6 −4 −2 0 −10 −8 −6 −4 −2 0 2 4 6 8 10 0.16 0.3 0.46 0.6 0.72 0.84 0.92 0.98 0.16 0.3 0.46 0.6 0.72 0.84 0.92 0.98 2 4 6 8 10 12 14 16 18 Pole−Zero Map

−1

)

(53)

Based on these pole locations, the governing dynamics were taken into account to design a PID controller with a low pass filter for the vertical displacement, pitch and roll states. For the pitch and vertical displacement, the governing dynamics are assumed to be the slowest poles where there is no zero that effectively cancels each other. The new pole locations are then calculated to be approximately ten times faster than the open loop response. The equations for these calculations are:

AR + BS = AmAo (3.4.19)

where B_A is the open loop response, and S_R is the controller which for a PID controller with a low pass filter has the form shown in 3.4.20, and Am and Ao are the poles of

the closed loop response and the observer.

S2s2+ S1s + S0

s(s + R0)

(3.4.20)

The following figures show the poles of the new closed loop response with the imple-mented controller: −140 −120 −100 −80 −60 −40 −20 0 −150 −100 −50 0 50 100 150 0.08 0.16 0.26 0.36 0.48 0.62 0.78 0.94 0.08 0.16 0.26 0.36 0.48 0.62 0.78 0.94 20 40 60 80 100 120 140 20 40 60 80 100 120 140 Pole−Zero Map

−1

)

Figure 3.21: New pole location (blue) versus old (green) for the vertical

(54)

CHAPTER 3. IMPLEMENTATION −30 −25 −20 −15 −10 −5 0 −25 −20 −15 −10 −5 0 5 10 15 20 25 0.1 0.22 0.32 0.44 0.58 0.72 0.86 0.96 0.1 0.22 0.32 0.44 0.58 0.72 0.86 0.96 5 10 15 20 25 30 Pole−Zero Map

−1

)

Figure 3.22: New pole location (blue) versus old (green) for roll response.

−100 −90 −80 −70 −60 −50 −40 −30 −20 −10 0 −80 −60 −40 −20 0 20 40 60 80 0.1 0.22 0.34 0.46 0.6 0.74 0.86 0.96 0.1 0.22 0.34 0.46 0.6 0.74 0.86 0.96 20 40 60 80 100 Pole−Zero Map

−1

)

(55)

The control output for the pitch, roll and vertical displacement controllers can be added to the model by multiplying the resulting individual control with the pseu-doinverse of the matrix T . The output of this matrix is then the corresponding value that needs to be set in each individual actuator.

(56)

(57)

CHAPTER 4. RESULTS

Chapter 4 Results

This chapter states the results of the implemented control algorithms.

4.1 Controllers compared to SimMechanics model

This section contains the results of the controllers that have been compared to the model made by Wang (2012). First, the results of Wang’s model will be presented in table 4.1 and figure 4.1. Then the following sections will display the results of the Kinematic Leveling Controller, the Dynamic Vibration Controller and lastly the Hybrid Control approach. The results extracted results from wangs model are mea-sured at the same place as the results for the controllers applied to the SimMechanics model.

Table 4.1: The results of Wang (2012) in terms of RMS values for roll, pitch and

vertical displacement and the RCI value of the accelerations.

Vertical acceleration RMS 1.2021 Roll acceleration RMS 2.0033 Pitch acceleration RMS 0.2859

(58)

CHAPTER 4. RESULTS 0 2 4 6 8 10 12 14 16 −1 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 · 10−2 time [s] [rad] Pitch Roll Yaw

Figure 4.1: A plot of the measured euler angles of the vehicle when running with

Wang’s combined passive and active suspension strategy.

4.1.1 Kinematic leveling controller

The results of the leveling controller alone when verified against the SimMechanics model made by Wang (2012), with the parameters stated in table 3.2 are visible in table 4.2 and figure 4.2. The test track used are the same as used by Wang (2012) and is visible in figure 3.10.

Table 4.2: The results of the leveling controller in terms of RMS values for roll,

pitch and vertical displacement and the RCI value of the accelerations. The change is the value compared to Wang’s result in table 4.1

Value Change

Vertical acceleration RMS 0.7029 −41.5% Roll acceleration RMS 1.3789 −31.2% Pitch acceleration RMS 0.4289 50.0%

(59)

CHAPTER 4. RESULTS 0 2 4 6 8 10 12 14 16 −0.4 −0.2 0 0.2 0.4 time [s] [rad] Pitch Roll Yaw

Figure 4.2: A plot of the measured Euler angles of the vehicle when running with

the leveling controller only.

4.1.2 Dynamic vibration controller

The skyhook controller was unable to complete a full simulation without causing the pendulum arms to rotate further than they are designed to do, causing them to rotate full laps around the vehicle at very high torques. The controller was still tried together with the kinematic controller in the hybrid controller. The best result obtained where the parameters as in table 3.3, and resulted in the Euler angles shown in figure 4.3. The test track used are the same as used by Wang (2012) and is visible in figure 3.10.

Development of an active suspension control strategy for a forestry machine with pendulum arms

Development of an active suspension

control strategy for a forestry machine

with pendulum arms.

JORGE AGUILAR

MORGAN VIKTORSSON

Master of Science Thesis

Stockholm, Sweden 2014

Development of an active suspension control strategy

for a forestry machine with pendulum arms.

Abstract

Sammanfattning

Acknowledgement

Contents

List of Tables

List of Figures

Chapter 1

Introduction

1.1

Background and problem description

1.2

Purpose

1.3

Delimitations

1.4

Method description

1.5

Expected outcome

1.6

Report outline

Chapter 2

Frame of reference

2.1

Theory behind suspension technology

2.2

Previous control strategies

Chapter 3

Implementation

3.1

Kinematics

y

x

z

ᶱ

ᶲ

ᶱ

3.2

Dynamics

x

ᶱ

y

ᶲ

z

A

A

A

m

m

m

m

z

S

S

S

z

z

z

3.3

Different modeling implementations of the Forwarder

3.4

Control structure

m

m

m

z

S

S

S

z

z

_m