Modelling, Simulation and Control of A Hydraulic Crane
Modelling, Simulation and Control of A Hydraulic Crane
Modelling, Simulation and Control of A Hydraulic Crane
Fahrzeugmechatronik, FK03
Handledare/ Supervisor/
Aufsichtsperson
Torbjrn Ekevid
(Vxj University)
Examinator/ Examiner/
Prfer
Prof. Dr. Peter
Wolfsteiner
(Munich University of
Applied Sciences)
Internet
http://www.vxu.se/td
Abstract
The objective of this thesis is to develop a model that represents the dynamics of a
hydraulically operated forestry crane. The model was derived with the traditional EulerLagrange formalism and considered the crane mechanics, three double-acting hydraulic
cylinders and the valve control unit. On the basis of the derived model we reproduced the
entire crane model in MATLAB in order to run simulations herewith. This gave us the
possibility to do parameter changes for further studies of the crane in motion.
Another major goal within the thesis work was to estimate cylinder friction of the hydraulic
actuators. We build up a test rig and used a double-acting cylinder for determing its frictional
behaviour. For this, we ran open-loop experiments in order to create velocity-friction maps
that represented the static friction force of the cylinder. In this concern, we varied system
pressure and cylinder load to study their influence on the friction force. By means of the
derived static friction maps we approached the cylinders dynamic friction behaviour and
applied both step and ramp control inputs to examine the spring-damping characteristics of
the microspoic bristles in the contacting area. The dynamic friction experiments have been
exerted in the fashion of the LuGre model. As a result we acquired different nominal friction
parameters that we necessarily used to develope adequate friction models.
A third objective of this thesis was to establish a crane-tip control. Instead of a traditional
control, providing a direct relationship between joystick input and cylinder extension, the focus
was to build up a control for the end-effectors trajectory in a two-dimensional frame. This was
achieved by using inverse kinematics in order to determine the required joint angles that
corresponded to the desired position of the crane-tip.
The work also contains a CD including all developed MATLAB models that have been written
within this project.
Summary
Hydraulic cranes are very popular for carrying out hauling operations of forestry machines. In
chapter 1 we will give an overview of such vehicles followed by the problem definition and a
description of the tools that have been used during the project work.
A small hydraulic crane was provided by Rottne Industri AB to do experimental work in the
laboratory hall of Vxj University. In chapter 2, the laboratory crane and its constituent parts
are described in detail. This involves the mechanical structure, the connected hydraulic
system, sensors and peripheral equipment. Sketches of the crane elements are also
presented in this chapter.
The mechanical model will be derived in chapter 3, beginning with the definition of rotation
matrices followed by a set-up of necessary kinematic chains. Finally the derivation of the
Euler-Lagrange equations allows us for examinig the dynamical behaviour of the crane.
In chapter 4 the hydraulic model will be described. It is divided into two major parts, namely
the mathematical model of the spool valve and the mathematical model of the hydraulic
cylinders. In both cases we will foremost present a static model which will be followed by the
dynamic model in order to derive the corresponding equations of motion.
Friction modeling is done in chapter 5. At the beginning of this chapter we will describe the
main friction phenomenon including the traditional static friction model and the dynamic bristle
interpretation in the fashion of the LuGre model. Open-loop experiments at the laboratory
crane will be carried out to examine the frictional behaviour of the used hydraulic cylinders.
The results provide us the cylinders nominal friction parameters which, in turn allows us to
build up adequate friction models.
The main control principle is described in chapter 6. The traditional control task of a hydraulic
crane is accomplished via two joysticks that control each cylinder separately. We will present
a more convenient method using a sophisticated crane-tip control. In this concern, we will
focus on inverse joint determination combined with traditional PID control.
We used MATLAB for all kind of modeling and simulation during the thesis work. The main
program files are represented in chapter 7 but can also be found on the attached CD in the
appendix.
Acknowledgements
This thesis work was carried out in the department of technology and design (TD) at Vxj
University, Sweden. It was initiated within the research group Heavy Vehicles in colaboration
with Vxj University and Rottne Industri AB.
The author would like to express his gratitude to all those who confirmed the permission and
thus made it possible to complete the thesis work at Vxj University.
Most notably, I would like to convey my thanks to the project supervisor Torbjrn Ekevid at
Vxj University for providing advisory support and professional advice of any kind. I also
want to thank Matz Lennels at Vxj University for giving me advisable help regarding control
principles of Euler-Lagrange systems. Furthermore I am deeply indepted to Anders Hultgren
at Kalmar University for his stimulating suggestions and outstanding encourangement from
the very beginning of this thesis work, especially for the intense and valuable discussions
about cylinder friction on many nights in the laboratory hall. I would also like to thank Rottne
Industri AB whose courtesy of the hydraulic crane enabled us to experience the real-crane
behaviour along friction experiments that have been directly carried out at the labcrane.
Finally I would like to give my special thanks to my professor at Munich University of Applied
Sciences for encouraging me working abroad but also for his help and feedback on numerous
issues of this thesis work
Alexander Heinze
Vxj, 29th September 2007
Symbols
General notation:
v
ry
v
X rYZ
AY
v
r (x)
X
Frames:
I
Inertial frame
e/ 2 e/ 3 e/ 4 e
Joint frames (origin in the center of 1st, 2nd, 3rd and 4th joint)
c1
e/ c 2 e/ c3 e
Cyldiner frames (origin in the center of the opening joint of 1st, 2nd and 3rd
cylinder)
Indices:
x1 , x2 , x3
Piston extension from cylinder center of 1st, 2nd and 3rd cylinder
c1s , c2 s , c3 s
j1 , j2 , j3
cg1 , cg 2 , cg 3
cg t1 , cg t 2
t1
t2
t3
ct
ct d , d , d
Crane-tip
Desired crane-tip postion and corresponding joint angles
Symbols:
x3
T
Ttrans
Kinetic energy
Trot
V
Q
Potential energy
Angular velocity
m
I
J
g
Mass
Inertia tensor
Jacobian matrix
Force vector
M
h
y
Mass matrix
Vector of gyroscopic and active forces
lc
los
loe
lp
lr
lc
1 , 2
r2
r3
rdiag
xs
Spool stroke
Control input
p0
Tank pressure
pS
Supply pressure
pN
Nominal pressure
C0
QN
Nominal flow
Re
Reynolds number
Generalized forces
State vector
ptr
Transition pressure
p A , pB
Natural frequency
Ringing frequency
Damping ratio
k
c
Spring stiffness
Time constant
A1 , A2
F fr
Friction force
Fg
Gravitational force
V0,1 , V0, 2
V1 , V2
dc
dr
qint / qext
Eoil
C1 , C 2
Oil constants
k int / kext
Leakage coefficients
FC
FN
Nominal force
Fv
kv
Damping factor
FS
Static friction
F St
Stribeck friction
Stribeck velocity
Stribeck gradient
0 , 1, 2
0 , 1
k p , ki, kd
Control error
Table of contents
1
BACKGROUND.............................................................................................................. 11
1.1
1.2
1.3
1.4
1.5
1.6
1.7
2.2.3
Sensors .............................................................................................................. 22
2.2.3.1
2.2.3.2
2.2.3.3
2.2.3.4
2.2.4
Hydraulic cylinders................................................................................................. 20
Valve package ....................................................................................................... 20
Hydraulic power unit .............................................................................................. 21
Pressure sensors................................................................................................... 22
Load sensors ......................................................................................................... 23
Angular sensors..................................................................................................... 23
Position sensor ...................................................................................................... 24
Peripheral equipment......................................................................................... 25
2.2.4.1
2.2.4.2
2.2.4.3
2.3
SKETCHES OF THE CRANE ELEMENTS ......................................................................... 26
2.3.1 Link sketches ..................................................................................................... 27
2.3.1.1
2.3.1.2
2.3.1.3
2.3.1.4
2.3.1.5
2.3.2
3
Cylinder sketches............................................................................................... 29
MECHANICAL MODEL.................................................................................................. 31
3.1
KINEMATICS OF THE CRANE ....................................................................................... 31
3.1.1 Rotation matrices ............................................................................................... 33
3.1.1.1
3.1.1.2
3.1.2
Kinematic chains................................................................................................ 40
3.1.2.1
Determination of forward kinematics...................................................................... 40
3.1.2.2
Determination of inverse kinematics ...................................................................... 41
3.1.2.2.1 Joint variable ............................................................................................... 44
3.1.2.2.2
Joint variable
............................................................................................... 45
3.1.2.2.3
Joint variable
x3
............................................................................................. 46
HYDRAULIC MODEL..................................................................................................... 54
4.1
MATHEMATICAL MODEL OF THE SPOOL VALVE ............................................................. 54
4.1.1 Static model of the spool valve .......................................................................... 54
4.1.1.1
4.1.1.2
4.1.1.3
5.1.2
Coulomb friction..................................................................................................... 71
Viscous friction ...................................................................................................... 72
Static friction (Stiction) ........................................................................................... 73
Stribeck friction ...................................................................................................... 74
5.1.2.1
5.1.2.2
5.2
ESTIMATION OF NOMINAL FRICTION PARAMETER ......................................................... 80
5.2.1 Experimental set-up ........................................................................................... 80
5.2.2 Static parameter estimation ............................................................................... 83
5.2.3 Dynamic parameter estimation .......................................................................... 89
5.2.3.1
5.2.3.2
1 .................................................................... 92
5.2.4
6
................................................................... 89
7.2
7.3
7.4
APPENDIX.................................................................................................................... 128
9.1
DATA SHEETS ......................................................................................................... 128
9.2
CD-ROM ................................................................................................................ 133
9.2.1 Content............................................................................................................. 133
9.2.2 CD-Rom attachment ........................................................................................ 133
10
Background
1 Background
1.1 A typical forwarder in use
Swedish economy is characterized by a market heavily orientated towards foreign trade in
which the agricultural sector is of great importance. Within this branch, timber products
account for an essential part of the export market. This emanates from a highly forested land
utilization in Sweden (see Figure 1), recently not least because of improved transportation
and distribution systems. Consequently agricultural technology plays an important role in the
forest-based industry. In the middle part of southern Sweden there exist numerous
manufacturers for forestry machines, all of them offer products to facilitate and speed-up the
machining in wood proceedings. Some of the companies are joining, together with the
Universities of Vxj and Kalmar, a research group named Heavy Vehicles (swed. Tunga
Fordon), whose ambition is to develop know-how and advancement in technology in order to
make the forestry vehicles become more efficient.
One of the members within this research group is Rottne Industri AB, a leading manufacturer
in logging machinery that has his headquarters located in Rottne, nearby the town of Vxj. In
Sweden, Rottne Industri covers approximately 15% of the market for forestry machines with a
production capacity of up to 200 machines per year. The company develops and
manufactures a wide range of forwarders and harvester machineries covering everything from
first thinning to clear felling. All of the machines are equipped with a hydraulic crane for
handling the timber logs. It is a major objective within the sector to continually improve the
efficiency and reliability of the forestry machines, predominantly by introducing more
automated steering control. For research purposes Rottne Industri has provided one of their
forwarders that is now located in the laboratory of the technology department at Vxj
University. On the basis of this prototype of a forwarder, practicable research activities like
sensor feedback and programmable control could be done in the laboratory hall of Vxj
University.
11
Background
Forestry work often consists of monotonous work steps that could be performed automatically
facilitating the human work and thus save time and energy. A main limitation but also an
essential way to enhance the efficiency of forwarders is the control interface of the crane.
Nowadays the crane is manoeuvred manually by well-trained and experienced operators.
They undertake the task of controlling each cylinder separately in order to move the crane-tip
to its desired position. Consequently the operator has to use several levers and buttons
simultaneously which complicates a precise and effective realization of the handling
operation. In respect of this, the forestry industry is particularly interested in a more direct and
easier way of controlling the crane in order to relief the driver from parallel operations. The
answer to this problem is a control mechanism which allows controlling the crane-tip in a
plane Cartesian frame, namely the up-down motion with one lever and the forward-backward
motion using a second lever.
In comparison to harvester machines that are additionally utilized in operations for felling,
delimbing and cutting trees, a typical forwarder is used to haul the log from the stump to a
roadside landing. Beside stability and flexibility, good tracking is one of the major conditions in
order to carry out good log-hauling in a densely wooded working environment. Primarily
forwarders are equipped with a powerful hydraulic system that has made them extremely
popular within the class of heavy articulated vehicles. Since most of the forwarders consist of
many moving parts, they are regarded as high maintenance wheelers. In the following
sections a short description of the constructive aspects and the control principle are given.
12
Background
The machines are characterized by a robust frame design with powerful articulated joints,
wide wheels and a load area with a low center of gravity for providing a good ground
clearance. Hence forwarders have an excellent stability which makes them use the full reach
and lifting capacity of the loader. Commonly they have a high ground pressure which allows
climbing over rocks and stumps as well as travelling through deep snow or wet lands.
Forwarders are basically equipped with a 24 volt electrical system for control and monitoring
operations. The motion of each crane link is quick and easy actuated by hydraulic cylinders
that are connected to a powerful hydraulic system. The system together with its working
principle is specified in chapter 1.3. Generally the manipulator is a forward linkage of three
beams with an additional telescopic extension arm. With this configuration the forwarder has
a broad reach and a flexible operating mode which makes it a particularly suitable vehicle
under exceedingly difficult conditions in the forest.
z0
z1
z2
Shoulder
Elbow
Forearm
1
Body
Base
13
Background
The configuration in Figure 3 allows the gripper for reaching a wide working area, still being
most flexible and accessible. Often robotic systems are also valued in accuracy (attribute of
how close the end-effector can come to a given point) and repeatability (attribute of how close
the end-effector can return to a previously taught point), that are both highly dependent on the
resolution of the signals, the backlash in the joints, the control algorithm and several outer
influences.
The control of a robotic system involves several tasks, beginning from signal perception and
data processing to the actuating operations of the cylinder. A typical robotic paradigm of the
anthropomorphic manipulator is given in Figure 4:
sensors
power supply
computer
controller
mechanical arm
program storage/
network
end-of-arm tooling
input device/
teach pendant
F1
A2
A1
F1
F2
Hydraulic fluid
14
Background
Under equal chamber pressure for example, a hydraulic cylinder with two times the diameter
of another cylinder ( d 2 = 2d1 ) can exert a force that is four times higher since
d 2 4 d1
=
. By making use of this phenomenon, a hydraulically operated
4
4
2
F2 ~ A2 =
crane is able to carry extremely high payloads, which makes it perfectly cut out for log hauling
operations in the forest.
1.5 Goal
One of the major goals of this work is to build up a complete model of the crane dynamics,
including the hydraulic system and real body dimensions. This will be phased in different
steps, beginning with a detailed investigation of the crane parameters, a derivation of the
kinematics and dynamics of the crane links, the cylinders and the valve block. Furthermore
the developed model has to be consolidated afterwards allowing the performance of crane
simulations in MATLAB/Simulink.
Another goal of this work is to simplify the operating control steps and consequently increase
the performance of the crane by using a more advanced control princple. This should be done
via control algorithm that uses the joystick command input for a direct control of the endeffector. A basic PID control will be implemented that combines the inverse crane kinematics
with optimization methods.
A major aim of this work is to capture the friction phenomenon that occurs during the motion
of the hydraulic cylinders. In order to provide friction compensation schemes it is most
essential to estimate the individual friction force to an accurate extent. Therefore we have to
set-up a cylinder test rig for friction evaluation and run several experiments that enhance the
dynamical aspects of the friction force. The evaluated friction maps and nominal friction
15
Background
parameters can then be used for further compensation schemes. It should be pointed out that
the friction maps will be used to create a reference friction model. However, this work does
not offer adaptive friction compensation schemes but this should be a major topic of the
forthcoming works within the project at Vxj University.
16
Background
and its results from practical experiments. Canudas de Wit also applied model-based
adaptive friction compensation on a DC motor in [14]. Here, a proposal of nominal static and
dynamic parameter estimation is given. Its principle method is used in this work later on when
we will evaluate dynamic friction on the hydraulic cylinders of the laboratory crane.
1.7 Tools
During the project work we have used several computer-aided tools which are described in
the following section.
The part-model of the laboratory crane has been created and provided by Rottne Industri.
Dimensions of the bodies and the crane parameters could be read out in Solid Works. The
Solid environment was also utilized to generate the screenshots that are used in this work for
depicting the correlation of the crane linkage.
We used Simulink for modelling the hydraulic system, beginning with Simulink blocks to
capture the cylinders flow dynamics. Later on we added the valve block including sundry
control algorithms. Simulink is an object-oriented tool which made it possible to model
different systems that could be first handled and put together in the end. As a start to the
crane model this method turned out to be most valuable, since its flexibility made us modify
and exchange the blocks rather easily.
Afterwards, the whole crane kinematics has been modelled in MATLAB R2007a that allowed
for solving the derived differential equations. Its m-code can be found in chapter 7 as well as
on the CD in the appendix of this thesis work.
Friction experiments were performed in the laboratory hall at Vxj University. Since there
was no adequate digital control system available in the laboratory, a dSpace system was
provided by courtesy of Kalmar University. Sensor signals were fed into the dSpace hardware
and could be readout via the Control Desk software. The block models for controlling the
valves were created in Simulink. Control Desk was connected to the Simulink environment
and in turn, the block models could be converted into ppc-files that were required to run the
Control Desk software.
17
18
Torque
link
Jib
(3rd link)
st
1 link
Extension
(4th link)
19
applications (>3) in order to intercept the particles that are released from the cylinder
components. These particles could wear the cylinder components, especially the gaskets
inside of the valves and consequently harm the cylinders and distort the straight-forward
manner of the mathematic model.
2.2.2.1
Hydraulic cylinders
Hydraulic cylinders are used in order to apply a force to the crane elements that controls the
joint motions. In general, hydraulic actuators are much more powerful than electrical actuators
of comparable size due to high actuation forces and high power density. Because of their
simple construction and low cost, hydraulic cylinders are widely used. A hydraulic cylinder
basically consists of the cylinder unit, a piston and gaskets that are located at the intersection
of piston and cylinder (see Figure 8).
2.2.2.2
Valve package
20
2.2.2.3
A hydraulic power unit consists of a pump, a tank, filters and valves for pressure restriction.
There are different types of pumps available depending on the technical solution that is used
in order to create the oil flow. The common pump types are gear pumps, radial-piston
distributor pumps and axial piston pumps. For flexible hydraulic applications, an axial piston
pump is used in most cases since these can be operated with a variable displacement that is
the amount of oil that theoretically passes the pump during one rotation. The main advantage
is that the pump can be continuously adjusted from zero flow to maximum flow. Thus, this
type of pump is very energy efficient and suitable at applications where flow varies heavily.
The assembly of a hydraulic unit and the special performance of the pump depends on the oil
flows and the pressures that are requested. For the laboratory crane that implies a capacity of
approximately 40 l/min at 200 bar pressure which corresponds to an effective energy of
approximately 15 kW. The hydraulic power unit as shown in Figure 10 is located at Vxj
University. This unit has a capacity of 60 l/min at 210 bar.
21
2.2.3 Sensors
There are a number of sensors required in order to operate the crane and to carry out
feedback control. The sensors can be used to as direct-input sampling in order to know which
condition the crane has at any time.
In Figure 11 the different types of sensors and their location at the crane are sketched.
f
a
p
a
p
p
f
l
f
load sensor
length sensor
pressure sensor
2.2.3.1
Pressure sensors
Pressures are often measured using the physical principle that the medium that is pressurized
squeezes against a mechanical diaphragm. This diaphragm will deform and thus the
deformation can be related to the pressure inside of the medium. In order to obtain a good
precision, one has to adjust the measuring area to the expected pressures that arise. A
common pressure sensor is shown in Figure 5. Amplifiers are used with pressure sensors in
order to obtain a suitable level of the measuring signal.
22
2.2.3.2
Load sensors
There are several possibilities to measure the resulting force on a cylinder. The laboratory
crane is equipped with load cells that can be connected directly between the cylinder and its
mounting. These sensors have a good precision but come with the drawback of being
comparatively expensive. In Figure 13 the load sensor from HBM is shown.
2.2.3.3
Angular sensors
In order to measure the rotation angles between adjacent links, incremental sensors are
widely used. These should be designed to determine both the direction from the pulse and the
zero level in order to avoid resets at each system start. Figure 14 represents an incremental
sensor that could be appropriate for goniometry.
23
2.2.3.4
Position sensor
Position sensors are used for measuring the cylinder lengths and speeds. In order to obtain a
stable and precise signal, the correct attachment of these sensors is most important. The
crane consists of a thread sensor that operates as a common potentiometer by coiling up a
thread, which is attached to the telescopic beam. When moving the telescopic beam very
quickly, the dynamics in the thread can influence the results, as the spool cannot reel up the
thread fast enough. However, the main advantage is that thread sensors are placed externally
and thus easily within reach in order to check its function. To avoid the need of resetting the
sensor each time the system is powered up, absolute position encoders should be preferably
used. Alternatives encoders can be built-in position sensors, which are located inside of the
cylinder. However, these sensors are fairly expensive and difficult to attach. The used sensor
is shown in Figure 15.
24
dSpace system
In order to convert all sensor signals and at the same time trigger output signals to the valves,
a dSpace system is used to which all cables are connected. Since we have only one dSpace
module available we must prior the signals of capital importance. The remaining signals may
be sampled with external equipments whereas the problem is to synchronize the clock to the
dSpace system.
The University of Kalmar has provided a dSpace system of the type DS 1044. This particular
system is most appropriate to use with the laboratory crane and is shown in Figure 16.
2.2.4.2
Amplifier box
The following section provides an overview of the amplifiers and voltage supply units. An
amplifier box was built at Vxj University1, its components are assembled on DIN-strips (35
mm) inside of a plastic box to provide a flexible connection to other applications. The main
set-up is shown in Figure 17:
25
The intention of using such kind of box is that all sensors can be fed from voltage sources of 5
V, 10 V respectively 24 V. The signal and supply cables to the sensors are connected on the
backside of the box, whereas the output signals to the dSpace system are transmitted on the
box's left side. On the box's right side there are couplings to provide voltage supply to other
applications. A filter is used in order to reduce disturbances that are excited by the main
power supply.
2.2.4.3
Interface box
The laboratory crane is controlled via two joysticks. The control signals from the joysticks
(related to the joystick deflection) are fed into a so called interface box that forwards the input
signals to the input ports of a dSpace system 2. After certain data-processing in the Matlab
environment, the converted control signals will be generated back into the interface box.
Here, the voltage of the incoming signal will be divided in a defined way to suit the input
limitations in Rottnes IPS-system. The IPS-box then steers the electronic valves in the valve
block of the crane. Figure 18 shows the signal flows between the different hardware parts at
the laboratory crane:
Joysticks
x2
Interface box
dSpace
x4
Valve block
IPS
x8
x8
x8
Data processing
& visualization
+24V
PC
Crane
Amplifier box
sensor signals
Figure 18: Peripheral equipment and data flow at the laboratory crane
Thanks to Johan Olofsson, student in the department of Electrical Engineering at Vxj University
26
We begin with link 1 which is represented in the coordinate frame of 1 e (Figure 19). One can
notice that only the vector to the cylinders swivel-joint ( c1e ) and the 2nd joint position ( j 2 ) are
necessary to establish a vector chain to the next link. The fact that the 1 ez -axis is the inertial
frames vertical gives us the possibility to equate frame 1 e and the inertial frame I e .
Furthermore it is not necessary to regard the links center of gravity since the 1st link is not in
motion.
j2
ez
c1e
I
ex
2.3.1.2
The second joint combines the 1st link with the 2nd link in the
orientation of 2 e is determined by its x -axis being parallel to the lower edge of the beam.
cg 2
c2 s
2 z
t1
j3
2 x
c1s
Figure 20: Sketch of the 2nd link
27
Besides the cylinder joints ( c1s and c 2 s ) one has to locate the joint of the 1st torque beam t1
and the joint in which the 3rd link is pivoted, namely j3 . Furthermore it is important to identify
the center of gravity cg 2 in which the translation and gravity energy is assumed to be stored.
2.3.1.3
At the end of the 2nd beam, a closed-loop torque linkage is attached to transmit the forces to
the 3rd beam. In Figure 21 both torque links are shown along with their centers of gravity and
the corresponding joints to the next beam. Notice that t 2 e is fixed in the joint of torque link 1
( jt 2 ) whereby torque link 2 is pivoted to its adjacent link 3 in jt 3 .
t1 z
cg t1
jt 2
t1 x
t2 z
cgt 2
jt 3
t2 x
2.3.1.4
The frame e3 is placed in joint j 3 and is orientated by the longitudinal direction of the 3rd link
(Figure 22). Its necessary coordinates are the center of gravity and the bearing of the 3rd
cylinder, namely joint c3 s .
c 3s
cg 3
e
jt 3
3 z
3 x
28
2.3.1.5
The telescopic beam is located inside of link 3 and thus, its frame
orientation as
e (Figure 23). Vertically it is fixed at half the beams height. Moreover, one
has to identify the coordinates of the center of gravity cg 4 , the end position of the 3rd cylinder
c3e and the crane-tip position ct .
3
c3e
e
cg 4
4 z
ct
e
4 x
x, x&, &x&
los
lc / 2
lr
loe
lp / 2
ez
ex
The cylinder length is determined by the offset to the cylinder joints at both ends l os and l oe ,
as well as the lengths of piston, rod and cylinder frame ( l p , l r and l c ). The only variable
remains the stroke of the piston x , whose initial position is assumed to be in the cylinder
center. That follows: x = 0 at a distance of l os + l c / 2 from the cylinder joint.
29
It is important to note, that all cylinder frames are fixed in the joint in which the cylinder is
attached (opening joint). Furthermore the frames x -axis correlates to the longitudinal
direction of the cylinder. From this it follows that cylinder vectors related to its local frame
have no portion in y - and z -direction, which holds: rvc = rvc ( x ) .
The cylinder vectors are given as
v
c1 rc1
v
c1 rc1 = 0
0
v
c 2 rc 2
v
0
c 2 rc 2 =
0
v
c 3 rc 3
v
c 3 rc 3 = 0
0
(1)
with
v
r = los1 + lc1 / 2 + x1 + l p1 / 2 + lr1 + loe1
(2)
c1 c1
and
v
r ,
c2 c2
v
r
c 2 c3
analogical.
One should keep in mind that the frame of cylinder 1 is fixed at the 2nd link. Besides, the 2nd
cylinder operates backwards as it is fixed to the torque link. That means the joint angle
between the 2nd and 3rd link is decreasing with an increasing cylinder stroke. The position and
orientation of each cylinder frame can be seen in Figure 25.
c2 z
c2 x
c3 z
c1 z
c3 x
c1 x
30
Mechanical model
3 Mechanical model
To represent the basic geometric and dynamic aspects of a robot manipulator, one has to
define the coherence between the different beams, associated to their adjacent link. In this
chapter we will therefore derive a mathematical model of the crane mechanics.
At first we will consider kinematic aspects, including rotational matrices and vector chains.
The latter is done in both forward and reverse kinematics. The forward kinematics has to be
applied when identifying the position of the crane-tip in terms of the joint variables, whereas
the inverse kinematics is used for control purpose. In this regard, one has to identify the joint
variables related to the desired position of the end effector. Before we start to model the
crane mechanics, some simplifications need to be done in order to keep the mathematical
expressions as short as possible. Assumptions are made as follows:
- The whole model is visually represented in a planar frame that is specified by the rotary joint
which is attached to the lower end of the 1st link. It is further used as the inertial frame to
which all kinematic vectors are related. Nevertheless we will use vectors of size 3 in order to
describe the entire kinetic energy caused by rotation.
- The cylinders and its internal components are assumed to be massless and hence store no
kind of energy, neither kinetic nor potential energy. This can be assumed due to the fact that
the cylinder centers vary merely slightly when the cylinder piston is in motion. The reason for
that is the fixed position of the cylinder frame which contributes predominantly to the whole
cylinder mass. Rotational energy can be neglected because the cylinders slope velocities are
almost zero (presented in chapter 3.1.2.2).
- All parts of the crane, in particular the beams of the crane, are assumed to be rigid bodies
and consequently resist any deformation. This denotes that the distance between two points
of a link remains constant, regardless of external forces exerted on it.
3 z
2 z
3 x
2 x
Figure 26: Link-fixed frames (solid line) and Denavit-Hartenberg convention (dashed line)
31
Mechanical model
The dashed frame is used when applying the Denavit-Hartenberg convention. One can notice
that its x -axis intersects the 3rd joint in the origin of frame 3 e . However, since we prefer to
use the local frames as given in the CAD-model, we chose the orientation of frame
that its x -axis is parallel to the lower edge of link 2 (solid line).
e , so
As a start to the mechanical model, it is necessary to identify the joint variables that build up
the generalized coordinates of the mechanical system. The number of joint variables is well
defined by the systems number of degrees of freedom. As our model consists of two rotary
joints and one prismatic joint, we write the vector of generalized coordinates as
q = ,
x3
(3)
where is the angle between the 1st link and the 2nd link, is the angle between the 2nd link
and the 3rd link and x 3 is the cylinder extension of the telescopic beam (see Figure 27).
3 z
2 z
4 z
2 x
3 x
x3
4 x
e z =1 e z
e x =1 e x
32
Mechanical model
In this section we will address the problem of describing the orientation of a coordinate frame
with respect to its adjacent frame. By specifying the geometric relationship between these
frames, it is then possible to determine rotation matrices relative to the initial frame. We
decide on fixing the initial frame to the rotary joint of the 1st link.
In our case, the transformation can be obtained as an elementary rotation of a reference
frame about the y -axis. According to Figure 27 the corresponding rotation matrices in terms
of the generalized coordinates are
A2 = A
y ,
2
2
2
=
0
1
0
0
1
0
=
(4)
A3 = Ay ,
=
0
1
0
0
1
0
=
(5)
A4 = 0
0
0 ,
1
0
1
0
(6)
A4 = I A3 .
When applying kinematic chains in section 3.1.2, we will therefore consider the orientation of
also as the frame for the referrring coordinates of the telescopic beam.
3e
Now, having defined a coordinate frame to each link in terms of the generalized coordinates,
we obtain the transformation matrices with respect to the inertial frame by post-multiplication
33
Mechanical model
0
1
0 =
A2 = I A1 1 A2 = 0 1 0
0
1
0
0 0 1 cos( ) 0 sin( ) cos( ) 0 sin( )
(7)
A3 = I A1 1 A2 2 A3 = 0 1 0
0
1
0
0
1
0
(8)
s ( ) c ( ) c ( ) s ( )
=
0
c ( ) c ( ) s ( ) s ( )
0
1
0
s ( ) s ( ) c ( ) c ( )
0
= I A4 ,
c ( ) s ( ) s ( ) c ( )
One should bear in mind, that only two rotation matrices 1 A2 and
the vector chains of the crane since
3.1.1.2
A1 = 3 A4 = eye(3) .
In order to combine the mechanical model with the hydraulic model it is necessary to
determine the direction of the cylinder force. This force can be seen as an external input to
the crane mechanics that causes the desired rotation of the joint variables. In this regard we
evaluate the cylinders rotation matrices by using a simple trigonometric approach.
We begin with the first joint variable , that is computed as a summation of the variable
angle 0 and the link-fixed angles 1 and 2 (see Figure 28). Notice, that the dashed lines
are parallel to the axes of the corresponding coordinate frame and thus represent its
orientation.
34
Mechanical model
v
r
2 c1s
1
v
r
c1 c1
v
v
r 1 rc1e
1 j2
We apply the law of cosine to the inner triangle in Figure 28 and yield
2 1 r j 2 1 rc1e 2 rc 2 s
0 = a cos
(9)
v
1 r j 2 ( x) 1
1 = a tan v
1 rj 2 ( z ) 1
v
r ( z)
2 rc1s ( x )
2 = a tan 2 vc1s
v
rc1 e ( x )
v
rc1 e ( z )
(10)
(11)
35
Mechanical model
It is important to note that equations 10 and 11 result in negative angles 1 and 2 . In this
regard, we have to subtract 1 and 2 from 0 in order to compute the first joint variable
. Plugging the cylinder vector rc 1 in equation 2 gives us a definition of that describes its
direct relationship to the cylinder stroke x 1 :
2
v
( r rv ) 2 + ( rv ) 2 x + l + l + l + lc1 + l p1
2 c2 s
1 os1 r1 oe1
1 j 2 1 c1e
2
= a cos
1 2
v
v
v
2 1 r j 2 1 rc1e 2 rc 2 s
(12)
As one can notice in Figure 28, the cylinder frame has then been rotated by an angle of
+ 1 +
related to the inertial frame. This leads to the rotation matrix of the 1st cylinder
2
2
0
1
0
0
1
0
,
I Ac1 =
where
(13)
is determined by
2 1 r j 2 1 rc1e c1 rc1
(14)
However, when modelling the crane in MATLAB, tests have shown that processing time is
much lower when using a more convenient approach to the rotation angle. Therefore we
define the cylinder vector in the inertial frame by means of a vector chain (see chapter 3.1.2)
and apply a tangent function to the cylinder vector afterwards in order to obtain c 1 e x related
to I e z :
v
r ( x)
= 1
I rc1 ( z )
1 + 4 = a tan I vc1
(15)
36
Mechanical model
e is determined by a
2
2
=
A
0
1
0
=
0
1
0
1 c1
(16)
Whereas the computing of the transformation of cylinder frame 1 has been fairly simple, it
needs some more effort to evaluate the rotation matrix of the 2nd cylinder. The reason for this
is a more complex relationship between the joint angle and the cylinder extension due to the
fact that a torque link is attached to the end of the 2nd cylinder (see Figure 29).
v
r
t2 t2
v
2 rc1
v
r
t1 t1
3''
3'
1
1
v
r
3 t3
2 t1
v
v
r 2 rc 2 s
2 jt1
We begin with an approach to 1 which is the lower angle inside the closed-loop quadrangle
j3 t1 t 2 jt 3 . Assuming we know the magnitude of from incremental measuring, 1 can
then be determined by
r ( x)
r ( z)
1 = 2 2 + a tan 3 vt 3 + a tan 2 vt 4
3 rt 3 ( z )
with
2 rt 4 ( x)
(17)
v
v
v
r = 2 r j 3 2 rt1 .
2 t4
Once again both a tan -functions are constant and implicate negative signs, regardless of the
joint configuration. Therefore they appear as a positive term at the right hand side of equation
17.
37
Mechanical model
We continue with applying the law of cosine to both triangles that arise when splitting the
quadrangle with its horizontal diagonal rc1rc 3 :
rc1rc 3 =
rc1rc3 =
v
r
2 t4
v 2
v
v
+ 3 rt 3 2 2 rt 4 3 rt 3 cos 1
v2
v 2
v
r + t 2 rt 2 2 t1 rt1
t 1 t1
v
r cos 2
t2 t2
(18)
(19)
Knowing 1 , one can easily compute 2 by equating the right hand sides of equation 18 and
19. It then holds:
v
v
2
r
r
t
1
t
1
t
2
t
2
2 = a cos
(20)
Now we take a look at the left hand side of the inner quadrangle. The diagonal divides 3 into
an upper and lower angle, namely 3 ' and 3 ' ' . We apply the law of sine to the arising
rectangles and obtain:
3 rvt 3
3 ' = a sin
1
r r
c1 c 3
(21)
t 2 rvt 2
c1 c 3
(22)
At first sight it is ambiguous that the a sin -functions in equation 21 and 22 remain positive.
Therefore we refer to equation 17 in order to proof that 1 is possible to increase beyond
180 , which means that 3 ' is likely to become negative. Inserting the cranes parameters
shows that
0.035
0.109 0.081
1 = 2 2 + a tan
+ a tan
= 4.011
0.094
2.231 2.309
(23)
We compute equation 23 and notice that 1 > 180 for < 50 . However, since 3 ' also
becomes negative, we dont have to concern with this special case any further.
38
Mechanical model
The next step gives us the rotation angle of the 1st torque link. It is determined by a
summation of 3 ' , 3 ' ' and its correlating angle to the x -axis of frame 2 e :
v
2 rt 4 ( z )
(24)
In order to compute to the rotation angle of the 2nd torque link, we simply refer to the already
determined angle 2 . It holds
2 = 1 + 2
(25)
Finally, the determined torque angles have to be placed in the rotation matrix with their
correct signs. The torque matrices referred to the 2 e -frame are
cos( 1 )
0
At 1 =
sin( )
1
0
1
0
sin( 1 ) cos( 1 )
0
0
=
cos( 1 ) sin( 1 )
0
1
0
sin( 1 )
cos( 1 )
(26)
cos( 2 )
0
2 At 2 =
sin( )
2
0
1
sin( 2 ) cos( 2 )
0
0
=
cos( 2 ) sin( 2 )
0
1
sin( 2 )
cos( 2 )
(27)
nd
cylinder by a vector chain to its start position and its end position and apply a tangent-function
to obtain the cylinders slope angle related to the 2nd ink:
v
r ( z)
2 rc 2 ( x )
2 = 2 vc 2
(28)
Ac 2
cos( 2 )
0
=
sin( )
2
0
1
0
sin( 2 ) cos( 2 )
0
0
=
cos( 2 ) sin( 2 )
0
1
0
sin( 2 )
cos( 2 )
(29)
We will not explicitly replace the rotation angles in equation 26 and 27 in terms of the piston
stroke as the equations become fairly extensive. The computing in MATLAB shows that
equation 24 consists of more than 50 different terms and thus it is hard to handle in written
form.
39
Mechanical model
3.1.2.1
Direct kinematic equations establish the functional relationship between the joint variables
and the position of the end effector. For us, the problem of the forward kinematics is to specify
the position of the crane-tip in terms of the generalized coordinates q . Afterwards it must be
related to the piston position in order to connect the crane mechanics to the hydraulic model.
The crane-tip position is hereby well defined by the joint variables , and x 3 ,
respectively by the cylinder strokes x1 , x 2 and x 3 . When applying the rotation matrices that
have been derived earlier in chapter 3.1.1, we are then able to express vectors from the
inertial frame to any point of the crane on the basis of a vector chain.
Below, a set of vectors is given, that describe the distance and orientation of the centers of
gravity of all links related to the initial frame I e :
v
v
r = 1 rcg 1
(30)
v
r
(31)
I cg 1
I cg 2
v
v
= 1 r j 2 + I A2 2 rcg 2
v
v
v
v
r =1 r j 2 + I A2 2 r j 3 + I A3 3 rcg 3
(32)
v
v
v
v
v
v
v
rcg 4 = 1 r j 2 + I A2 2 r j 3 + I A3 ( 3 rc 3 s + 3 rc 3 4 rc 3 e + 4 rcg 4 )
(33)
I cg 3
v
v
v
v
v
v
v
r = I r j 2 + I A2 2 r j 3 + I A3 ( 3 rc 3 s + 3 rc 3 4 rc 3 e + 4 rct ) .
I ct
(34)
v
r is also sketched in Figure 30. This vector is of
I ct
particular importance to the crane control, as it will be used as a feedback signal to the control
interface. The main control topic will be discussed separately in chapter 6.
40
Mechanical model
v
r
3 z
2 j3
v
r
2 z
3 c 3s
3 x
2 x
v
( 3 rc 3e )
v
rj 2
ez
ex
v
r
3 c3
v
r
3 ct
3.1.2.2
v
r
I ct
As one could notice in the preceding section, it was fairly easy to compute the vector
coordinates of the crane-tip in terms of the joint variables , and x3 . However, in order to
control the crane, we have to apply the principle of inverse kinematics that deals with the
problem of finding the joint variables related to a given position of the end-effector. Once
knowing the exact joint angle at a certain crane configuration we will develop a systematic
algorithm in order to transform the desired motion specifications assigned to the end-effector
into the corresponding piston strokes.
The main problem of inverse kinematics is that there may exist more than one possible
configuration of the vector of generalized coordinates for a specific position of the endeffector. Besides, the equations to solve are in general non-linear, which might end up in an
admissible solution that is out of the cranes working area. The problem of multiple solutions
rises, if the number of degrees of freedom is higher than the number of variables that are
necessary to describe a given task.
Our first goal is to express the crane-tip position in terms of the generalized coordinates.
There are several ways to derive inverse kinematics equations, e.g. as proposed in [4, 5].
These equations are much too difficult to solve directly. Nevertheless we have to identify the
joint variables in a closed form without iterative search in order to solve the equations rapidly
and thus meet the demands of the sampling rate. Therefore we will apply a geometrical
approach to determine the joint variables with relative certainty.
41
Mechanical model
Unlike the problem of forward kinematics that always consists of one unique solution, there
may be several solutions or even no solution at all when applying the principle of inverse
kinematics. As a start, we set the extension of the telescopic beam to zero, however it will be
varied later on in chapter 6. With two degrees of freedom in our system, there are now only
two crane configurations possible (see Figure 31):
2 x
j3
r2
r3
2 z
j2
d
rdiag
ct
d
ctd
v
rct ,d
0
Figure 32: Determination of inverse kinematics
42
Mechanical model
Introducing d and d as the desired angles related to the joint-to-joint axis, the law of
cosine brings us
r2 2 + r3 2 rdiag 2
d = a cos
2 r2 r3
(35)
with r2 = j 2 j3 and r3 = j3 ct d .
Knowing the desired coordinates of the end-effector, the diagonal that divides the quadrangle
0 j2 j3 ctd can be written as
v
v
rdiag = rct , d r j 3 .
(36)
The desired rotation angle d is then equal to the summation of the upper and lower angle
about the 2nd joint. The two angles can be determined by applying the law of cosine to both
triangles:
r j 2 2 + rdiag 2 rct , d 2
d = a cos
2 r j 2 rdiag
r 2 + rdiag 2 r3 2
.
+ a cos 2
2
r
r
2 diag
(37)
The desired (predetermined) position of the crane-tip and consequently the vector rct ,d will be
computed in chapter 6 in terms of the joystick control input. However, inserting any possible
value for the end-effectors position (within working range) leads to a unique solution.
Finally the slope angles between the frames x -axis and the joint axis have to be considered.
The desired joint variables are then defined as
v
2 rj 3 ( z)
and
d = d a tan v
(
)
r
x
2 j3
(38)
v
v
2 rj3 ( z)
3 rct ( z )
respectively.
d = d + a tan v
a tan v
3 rct ( x)
2 r j 3 ( x)
(39)
With equations 38 and 39 the joint angles are now well-defined at a certain crane position.
Notice, that such a unique solution only exists at a given extension of the telescopic beam.
However, when operating the crane, we will optimize the joint extension by applying the
derived formulas for varying cylinder strokes x3 . The corresponding algorithm will be
developed in chapter 6 by means of a penalty function.
43
Mechanical model
lc1 + l p1
v
v
v
v
v
v
x1 = 2 cos( +1 + 2 ) 1 rj 2 1 rc1e 2 rc2 s + (1 rj 2 1 rc1e )2 + (2 rc2 s )2 los1 + lr1 + loe1 +
2
(40)
This particular relationship between the joint angle and the piston position x 1 is graphed
in Figure 33. The MATLAB code can be found in angle_limits_alpha.m.
When operating the crane it has to be assured that the second link is not rotating beyond an
angle of 180 in order to keep the system stable. Therefore we have to define an upper
angle-limitation of max = 180. However, this configuration would correspond to a cylinder
extension that is out of range and thus impossible to obtain. Because of constraints in the
cylinder dimension, we can extend the piston to a maximum of x max = 217 mm. As one can
notice in Figure 33, this specific threshold corresponds to a maximum joint angle of max =
172. Consequently, we have to apply this angular limitation to the control parameters in
order to restrict x 1 from extending logically beyond the cylinders end. Figure 33 also shows
that the maximum cylinder retraction of x min = -0.155 mm (due to the rod length) leads to a
limitation in the lower angle of min = 45. These geometrical restraints have been approved
and verified with the corresponding configuration at the laboratory crane.
Furthermore it appears that the relationship between and x 1 is almost linear, most notably
in the range of -0.1 m < x 1 0.15 m. In any case the designers try to keep this relation linear
in order to facilitate manual operating of the crane.
44
Mechanical model
v
r
2 c2
v
v
v
= 2 rt 1 + 2 At 1 t 1 rt 1 2 rc 2 s .
(41)
One should notice, that the joint variable is hidden in the rotation matrix
At 1 .
Now we proceed in the same way as we did when computed the stroke of the 1st cylinder - we
v
substitute the cylinder vector 2 rc 2 with the individual cylinder elements (equation 2) and
rewrite equation 41 with focus on the piston length x 2 :
x2 =
l + l p1
v
rc 2 l os 1 + l r 1 + l oe 1 + c 1
2
(42)
In this case, the above-quoted relationship between and x 2 is rather complex since
appears in several terms of
v
rc 2 . Thus we do not give an extensive formulation of x 2 ( )
at this point. Instead we compute this particular relationship with MATLAB again, which yields
in the following figure (m-flie: angle_limits_beta.m):
45
Mechanical model
According to Figure 34, the maximum angle of max = 178 is reached at a retraction stroke
of x 2 min = -0.101 m. This retraction determines the upper angular limitation as it corresponds
to the end position of the cylinder. As one can notice, certain linearity is given below a
cylinder extension of approximately x 2 = 0.150 m. When moving the piston further than this
threshold, the relationship becomes rather non-linear until the cylinder reaches its maximum
extension of x 2 max = 0.210 m at a joint angle of min = 5.
3.1.3 Workspace
The workspace of the crane represents the portion of the environment that the cranes end
effector can access. As we model the crane in a planar frame with no rotation about the z axis, the total workspace is then the total area swept out by the crane-tip as all possible
motions are executed.
In our case, the robot is designed as an anthropomorphic manipulator, because the links are
designated as the body, upper arm, and forearm respectively. Its geometry is realized with
three revolute joints and a prismatic joint, which provides a larger workspace than other
kinematic geometries relative to its size. This feature becomes extremely indispensable with
increasing log size and thus a large accessible working area is of great importance for an
effective forwarder.
The working area is constrained by the geometry of the crane links as well as the operating
range of its joint variables. The mechanical constraints on the joints have been determined in
the previous section and are summarized in Table 1:
Joint variable
Minimum value
Maximum value
45
172
178
x3
-0.315 m
0.315 m
46
Mechanical model
47
Mechanical model
L = T V ,
(43)
d L L
=Q ,
dt q& q
(44)
where Q is the vector of generalized forces associated with q . The terms of the Lagrangian
will be determined in the next sections.
T = T trans + T rot =
1
v
1 v T
T
T
I v i , cg m i I v i , cg +
I i I Ai i I i I Ai I i ,
2
2
i= 2
v i , cg and
(45)
individual centers of gravity ri , cg . The vectors necessary for computing the kinetic energy are
sketched in Figure 36:
48
Mechanical model
m2
v
v2,cg
2 ez
3 z
2 2
2 z
v
v3,cg
3 x
2 x
3 ez
m3
I
3 3
ez
m4
I
4 4
ex
3 ex
v i = J vi , cg ( q ) q&
(46)
i = J i ( q ) q& ,
(47)
where J vi , cg
rotational Jacobian respectively. Notice, that the translatory velocity is only dependent on the
v
r
joint variables and is not time-dependent which follows that
= 0.
t
49
v
v4,cg
Mechanical model
The Jacobian matrix can be obtained by a partial derivation of a vector with respect to the
generalized coordinates. In our case, the Jacobian matrices are determined by:
J vi , cg ( q ) =
J i (q ) =
v
I ri , cg
(48)
Ii
.
q&
(49)
T =
1 T n
T
q& J vi ,cg ( q ) T m i J vi ,cg ( q ) + J i ( q ) T I Ai i I i I Ai J i ( q )
2
i=2
) q&
(50)
V = mg
T
I
v
rcg ,
(51)
v
rcg the vector of the centers of mass in the
V =
mg
i
i=2
where
v
r
I i , cg
(q ) ,
(52)
v
ri ,cg has been derived in chapter 3.1.2 and the mass of each link can be found in the
appendix.
50
Mechanical model
3 z
2 z
m2
2 x
3 x
v
I r2 ,cg
v
r
I 3,cg
ez
m3
m4
v
r
I 4 , cg
I ex
Q =
I rv j , f ( q )
j =1
where
T
I
fj,
(53)
v
r j , f ( q ) denotes the
Q =
(
m
j =1
T
j
J vj , f ( q ) .
(54)
51
Mechanical model
Figure 38 sketches the cylinder forces f m and the respective vectors to the points of force
application:
e
c2 z
f3
e
c2 x
f2
f4
f6
c3 z
v
r
I 4, f
v
I r2 , f
v
r
c3 x
I 3, f
c1 ez
v
r
I 6, f
c1 x
v
r
ez
I 1, f
ex
v
r
I 5, f
f1
f5
bodies and thus corresponds to a spring-damping phenomenon. Anyhow, we will not consider
a spring-damping behaviour of two bodies in contact. Instead we assume that the generalized
force is expressed solely in terms of the resulting cylinder force, caused by the differential
pressure of the adjacent cylinder chambers. The detailed determination of the cylinder forces
will be separately addressed in chapter 4.2.
d (T ( q , q& ) V ( q ) ) (T ( q , q& ) V ( q ) )
= QT .
dt
q
q&
(55)
We begin with the computation of the 1st term of the equations left hand side. In the latter
section, we have computed V as a function of the generalized coordinates q only, and
52
Mechanical model
(V ( q ))
= 0
q&
(56)
It is then obvious that after applying the distributive law, equation 55 yields in
d
dt
T ( q , q& )
q&
T ( q , q& )
q
V (q )
+
q
= Q T .
(57)
According to [10] one can also substitute the time derivative for the partial derivative related to
q which gives us
T (q, q& )
T (q, q& )
T (q, q& ) T (q, q& ) V (q)
q&& +
q& +
+
=Q.
q& q&
q q&
t q& q q
(58)
M (q ) =
q&
T ( q , q& )
,
q&
(59)
T ( q , q& )
V ( q )
T ( q , q& )
T ( q , q& )
q&
+
+ Q
h ( q , q& ) =
q q&
t q&
q
q
(60)
M ( q ) q&& h ( q , q& ) = 0
(61)
q&& = M ( q ) 1 h ( q , q& )
(62)
which allows us to define our first state space set of the mechanical system:
y&1 y2
T
y& = q&& , with y12 = [ y1 y 2 ]
2
(63)
53
Hydraulic model
4 Hydraulic model
In this chapter we will derive a non-linear model of the hydraulic system that is connected to
the laboratory crane. A hydraulic system consists of hydraulic cylinders, a hydraulic pump,
hoses between the different units and valves for controlling the hydraulic flow. This work will
not specifically regard the hydraulics of the pump, but it has been a major topic of ongoing
works that in this concern also deals with the subject of load sensing of excavators and
forestry machines [2].
As a start, the hydraulic system will be distinguished into a valve model and a cylinder model.
Afterwards, both sub-systems will be combined in order to derive the non-linear state space
equations of the entire hydraulic system. Finally in chapter 7, the derived equations are
applied to a MATLAB environment where the overall aim is to create a suitable m-file that
captures the dynamic model of the crane.
Valve configuration
The valves that are used for controlling the double-acting hydraulic cylinders are solely 4/3valves, that is, four ways and three positions. Thus, there are two incoming ports (pump
supply and return tank) and two outgoing ports (A and B) connected to the valve (see Figure
39).
x s (t ), x& s (t ), &x&s (t )
u (t )
54
Hydraulic model
One should bear in mind that port A is always connected to cylinder chamber 1 (blind-end
side), whereas port B is linked with cylinder chamber 2 (rod-end side).
The spools position can be regulated such as the supply port is connected to either port A
( x s > 0) or port B ( x s < 0). The intermediate position ( x s = 0, sketched in Figure 39) is to
freeze the cylinder piston, where all the valve flow through the ports is stopped immediately. A
spring is used at both ends of the spool in order to force the spool back into its initial position.
This will cause a flow stop when the coil is not energized.
The sixth valve, which controls the flow to the cylinder of the telescopic beam, is slightly
different compared to the other valves, as it is a so-called regenerative valve. During the
extraction movement, ports A and B are then both connected to the supply port. From this it
follows, that the pressures in chamber A and chamber B are nearly the same and the piston
will only move due to the larger cross-sectional area of the cylinders blind-end chamber (A).
The resulting cylinder force ( F = pArod ) then determines the piston velocity and will push the
oil from the rod-end of the cylinder (chamber B) back into chamber A. As a conclusion, a
regenerative valve gives rise to rapid extractions with less oil consumption.
The several valve configurations and their system responses are sketched in Table 2:
Spool position x s
Valve nr. /
Cylinder nr. 3
Cylinder response
Joint
variable
Q1
&&
2/1
Q2
Q2
Q1
&&
3/2
Q1
6/3
Q2
&x&3
Q1
&&
2/1
Q2
Valve nr.1 is used in order to control the rotary joint located at link 1 which is not discussed in this work
due to a planar model. Besides, valve nr.4 and nr.5 are not in use.
55
Hydraulic model
Spool position x s
Valve nr. /
Cylinder nr.
Cylinder response
Joint
variable
Q2
Q1
&&
3/2
Q1
6/3
2/1
3/2
6/3
Q2
no valve flow
&x&3
&& = 0
&& = 0
&x&3 = 0
Working principle
Generally the crane valves are controlled electronically, that is by solenoid-operated devices
such as coils. However, the valve block of the laboratory crane is equipped with certain kind
of pre-valves in addition to the main valve so that its working principle can be explained as
follows:
The induced magnetic field provides the transformation of an electrical signal into an
electromagnetic force, which drives the spool of the pre-valve to its desired position (see
Figure 40). In general there are two pre-valves controlling one main valve. One pre-valve
controls the pressure at one end of the main valve, the other pre-valve at the other end. The
main valve spool is centerd by springs. Hence, a flow ( q A ' , q B ' ) is caused into the main
valve chamber and generates a pressure at the spool ends, meaning that the position of the
main spool x s is thereby hydraulically operated. From the spool displacement it follows that a
flow is generated through the valve port caused by the supply pressure p S and the tank
pressure p0 . The output flows of the valve ( q A , q B ) then vary the pressures in both cylinder
chambers.
56
Hydraulic model
pA
qA
x s (t )
x& s (t )
pB
qB
&x&s (t )
qA '
qB '
q L1
qa
qL2
qb
q0
p0
qS
pS
u (t )
57
Hydraulic model
pressure downstream of an orifice is sensed and pump flow is adjusted to maintain a constant
pressure drop (and therefore flow) across the orifice. The red channels are for pressurized oil
and the blue ones indicate the oil that flows back to the tank. The yellow ones are the
channels to the chambers of the cylinder. The green channel is controlling the oil pressure
across the main valve.
The pump that is connected to the valves of the laboratory crane is assumed to produce a
constant pressure. When operating the crane, the pump is adjusted to a supply pressure of
p s = 140 bar. It also seems reasonable that the tank pressure p0 can be put on an
atmosphere pressure level, as the fluid must be allowed to flow freely back into the tank. The
experimental setup for friction evaluation (see chapter 5) also includes a separate hydraulic
cylinder controlled by a traditional industrial proportional control valve.
4.1.1.3
Flow equations
Hydraulic flows can arise when a restriction causes a pressure drop between adjacent
spatiality. In Figure 40 one could identify that the shifted spool determines the restriction size
of the cross-section areas of supply and return port. Consequently q A and q B can be seen
as restriction flows, that are functions of the spool position and the local pressure difference at
the orifice.
The flow through a restriction is generally turbulent and then happens to be proportional to the
squared root of the pressure drop:
q = C0 xs p
(64)
In practice there is some loss of energy at the orifices that can be considered in terms of a
constant discharge coefficient C0 :
C0 =
QN
x s ,max
p N
2
(65)
where QN is the nominal flow and p N the nominal pressure drop respectively. Both
quantities are based on static valve characteristics that are provided in data sheets.
When applying equation 64 to the valves of the laboratory crane, the orifice flows yield in
qtur , A = C0 xs sgn( p S p A ) p S p A
(66)
58
Hydraulic model
One should keep in mind, that equation 66 changes when using the regenerative valve of the
3rd cylinder. In this case the flow from port B becomes
qtur , B = C0 xs sgn( p B p S ) p B p S .
(68)
Laminar flow can arise when the restriction at the orifice is fairly narrow, for example in the
origin of an opening process. Then the flow happens to become directly proportional to the
pressure drop and hence equations 66 and 67 change to:
p pA
qlam , A = C0 x s ( p S p A ) S
ptr
p p0
qlam , B = C 0 x s ( p B p0 ) B
ptr
p p0
qlam , A = C 0 x s ( p A p0 ) A
ptr
qlam , B
p pB
= C0 x s ( p S p B ) S
ptr
(69)
(70)
According to [1] the transition region at which the flow turns from a laminar to a turbulent state
can be estimated in terms of the transition pressure
3 Re tr 2
ptr =
4C0
(71)
The flow is assumed to be laminar when the pressure drop is below transition pressure. The
Reynolds number then indicates the point at which the flow changes to a turbulent state. In
traditional hydraulic applications the critical Reynolds number is generally in the range of
2000 < Re tr < 3000. Plugging Re tr in equation 71, the size of the transition pressure is then
in a range of approximately 2 to 4 bar. Hence, the maximum laminar flow is given at
(72)
(73)
In order to achieve a smooth transition between the two flow states, we harmonize equations
59
Hydraulic model
66/69 and 67/70 respectively, which gives a more practical description of the flow dynamics:
qtrans =
p ptr ,min
ptr ,max ptr ,min
p ptr ,min
qtur + 1
qlam .
(74)
where qtrans is the flow in the transition range ptr ,min < p < ptr ,max .
From the different flow equations we graph the flows dependency on the pressure drop and
the maximum spool opening in MATLAB and yield the chart depicted in Figure 42:
Figure 42: Restriction flow in terms of a merged laminar and turbulent flow
As one can notice in Figure 42, the laminar flow increases constantly with the pressure drop
until a level of 2 bar has been reached. From 2-4 bar equation 74 takes effect until the
turbulent flow dominates and thus the flow gradient decreases. In either case, the size of the
restriction opening is always directly proportional to the valve flow.
Strictly speaking there is some leakage between the spool and the inner body of the valve. It
follows that there has to be a flow ( q L1 and q L 2 ) from the inner spool segment to its adjacent
chambers (see Figure 40). The corresponding flow chart is given in Figure 43.
60
Hydraulic model
qA
pS
qa
qL 2
qB
qL1
pT
pA
qS
pB
qb
qT
q A = qa ( xs , pS , p A ) q L1 ( x g , p A , pT )
q B = qb ( xs , p B , pT ) q L 2 ( x g , p S , p B )
q S = qa ( xs , p S , p A ) + q L 2 ( x g , pS , p B )
(75)
q0 = qb ( xs , p A , pT ) + q L1 ( x g , p B , pT )
We notice that the leakage flow is a function of the gap size x g between the spool and the
valve frame. In general, the gap size is almost minuscule which follows that the leakage flow
is innately low. However, when using a spool configuration that overlaps the ports, leakage
can be further reduced primarily in the non-energized mode. As a matter of negligible leakage
flow and also in order to keep the equations manageable, we will subsequently not include
the leakage effect of the valves to the hydraulic model.
61
Hydraulic model
- Inside the hydraulic hose, there is no pressure drop. However, the rise in pressure at the
cylinder connectors is taken into account specially.
- The pump supplies the valves with a steadily constant pressure whereas the tank pressure
is put on a level of atmosphere pressure.
- Hydraulic friction inside of the hoses will be neglected
The dynamic of a spool valve follows approximately a linear differential equation of second
order described in equation 76:
dx s ( t )
dx ( t )
+c s
+ kx s ( t ) = F ,
2
dt
dt
(76)
where m is the mass of the spool, c the damping factor and k the spring stiffness. The
force F can be seen as an input to the system, e.g. the input current to the valves.
By substituting k = m n and c
2
= 2 km equation 76 becomes
dx s (t )
dx s (t )
2
2
+ 2 n
+ n x s (t ) = n u (t ) , [3]
2
dt
dt
(77)
n 2 =
+ d
(78)
1
1 + 2 d
(79)
0.48 .
As we notice, the right hand side of equation 77 is not equal to zero, which makes the system
become inhomogeneous. That is, the term
n 2u (t )
homogenous system. The action of changing the spool position is then induced by the control
input u (t ) .
62
Hydraulic model
With the initial conditions of the spool valve being x s (0) = 0 , x& s (0) = 0 and
u (0) = 0 , we
s 2 X ( s) + 2 n sX ( s) + n X ( s) = n U ( s ) .
2
(80)
Written equation 80 as a relation between input and output finally yields in the transfer
function of the form
n
X (s)
1
= 2
=
G(s) =
.
2
1 2 2
U ( s ) s + 2 ns + n
s + 2 s +1
2
2
(81)
The system behaviour can now be described qualitatively by the poles, namely if the
denominator of the transfer function is set to zero.
From equation 81 we obtain s1, 2 = n (
for the spool behaviour, dependent on the
than one (
< 1 ) and hence we find two complex values for s . The system is under-damped
d = n 1 2
shown in the Simulink block in Figure 44 and will be extensively discussed in chapter 7:
y& 3 0
y& = 2
n
4
y 34 + 2
.
2 n
n u (t )
1
(82)
We notice that the position of the spool, which will be the input signal for the later described
cylinder model, is solely determined by the control input u (t ) .
63
Hydraulic model
tc
q1
A1 , p1
dc
F2
qint
F fr
lp
lc
x p , x& p , &x&p
F1
A2 , p 2
q2
lr
qext
dr
ps
p0
Fg
Figure 45: Schematic sketch of a double-acting cylinder
The figure on the left indicates the forces and flows at the cylinder whereas the latters build
up the equations of motion which will be derived in the following sections. The figure on the
right gives the cylinder dimensions in terms of the cylinders characteristic parameters. The
exact cylinder dimensions are given in the appendix.
64
Hydraulic model
m&x&p = F1 F2 + Fg F fr ,
(83)
where &x&p is the piston acceleration and m is the mass of the loose parts (basically the mass
of piston and rod). The force F1 is generated by the upper chamber pressure, whereas F2
comes from the lower chamber. Fg is the gravitational force
force to solve.
As a start to the mathematical cylinder model, we begin with a geometrical approach to the
models dimensions. Let us define the initial conditions for the oil volume of both cylinder
chambers as
V 0 ,1 ( x p = 0 ) =
V0,2 ( x p
lc l p
2
A1 + V 0 , p =
l c l p d c 2
+ V 0 , p and
2
4
(84)
lc l p
l c l p ( d c 2 d r 2 )
= 0) =
+ V0, p
A2 + V 0 , p =
2
2
4
(85)
respectively.
Notice that V0, p denotes the dead volume in the pre-chambers to which the hydraulic hoses
are connected. The initial chamber volume is based on the initial piston position, which is the
centering position of the cylinder (see 2.3.2).
The dynamic chamber volumes are then defined by
2
(l l p + 2 x p ) d c
l l p d c 2
d
+ x p c + V0 , p = c
+ V 0 , p and
V1 ( t ) = c
2
4
4
8
(86)
2
2
lc l p (dc 2 dr 2 )
(lc l p 2x p )(dc dr )
(d dr )
V2 (t) =
xp c
+V0, p =
+V0, p
2
4
4
8
(87)
respectively.
65
Hydraulic model
&x&p =
1
( p1 A1 p 2 A2 ) + mg F fr
m
(88)
One can observe, that the only variables in equation 88 are given by the chamber pressures
p1 and p 2 and the friction force F fr . Friction evaluation will be studied extensively in chapter
5, whereas the pressure as a crucial state of the dynamic model is derived in the following
section.
The mass of substances in the hydraulic system will remain constant regardless of the
process that acts in the system. It holds: V& =
m& = const
V& = vA = const
v'
v' '
A' '
A'
Figure 46: Conservation of mass
This conversation of a fluid mass is described in terms of the following continuity equation:
dm1
V&1 = q1 + qint
= oil ( q1 + q int )
dt
(89)
dm2
V&2 = q2 qint qext
= oil (q2 + qint + qext )
dt
(90)
where q int is the internal leakage flow between piston and cylinder frame and q ext is the
external leakage flow at the rod-end side of the cylinder.
66
Hydraulic model
One should keep in mind that q2 becomes negative when the fluid circulates into the 2nd
cylinder chamber (see Figure 45). Furthermore, the compressible fluid makes
oil
a function
oil ( p) = Eoil ( p)
oil ( p )
,
p
(91)
dp 1 V1 oil dV1
+
oil = oil (q1 + q int )
dt E oil
dt
(92)
dp 2 V 2 oil dV 2
+
oil = oil (q 2 + q int + q ext )
dt E oil
dt
(93)
dp1 Eoil
=
dt
V1
dx
q1 + qint A1 p
dt
E
dp 2
= oil
dt
V2
(94)
dx
q 2 + qint + qext A2 p
dt
(95)
Hence, we have determined the rate of change in pressure, which can later be used as a
state derivative to the dynamic model. Notice, that the time derivation of the overall chamber
volume yields merely in A1
dx p
dt
and
A2
dx p
dt
, respectively.
We now focus on the unknown parameters of the equations 94 and 95. According to [3], the
heuristically obtained formula for the bulk modulus of oil elasticity can be written in the form
p
1
Eoil ( p) = Eoil ,max log10 C1
+ C2 ,
2
pmax
(96)
where Eoil ,max denotes the magnitude at maximum oil compressibility and
system pressure respectively. The oil constants
[6] as
C1 = 90 and C2 = 3 .
67
Hydraulic model
There are two possible leakage phenomena when operating a hydraulic cylinder. An internal
leakage flow qint arises at the restriction of the piston and the inner cylinder body. An external
leakage flow qext appears at the opening for the rod-outlet. In both cases, the leakage can be
seen as a flow through an infinitesimal restriction. Thus, the flow happens to be laminar so
that it becomes directly proportional to the pressure difference between the mediums:
Qint = kint ( p2 p1 )
(97)
Qext = kext ( p2 p0 )
(98)
leakage equation
di
+ rc
2
r 3 ,
k=
c
6 L
(99)
d i denotes the inner diameter of the bore hole, rc the radius of the shaft, the static
friction coefficient and L the length of the restriction.
where
It is obvious that the leakage flow tends to zero when using appropriate gaskets inside of the
cylinder. Nevertheless, we include the leakage flow to our model although the friction
coefficient has to be estimated without experimental identification. It is also important to
notice, that the pressure sensors of the cylinders are located in a pre-chamber to which the
hydraulic hose is connected. The diameters of pre-chamber and hose are approximately the
same, but they differ from the diameter of the main cylinder chamber. Consequently the
pressure inside of the cylinder may vary compared to the measured sensor value. This effect
can be represented by Bernoullis principle:
p
v
+ = const
2
(100)
This formula states that an increase in velocity occurs simultaneously with a decrease in
pressure.
It is obvious that the motion of the piston
chamber. The change in velocity is then given by differing diameters of hydraulic hose and
cylinder chamber and can be captured with the continuity equation for the mass flows
& = const )
(m
Ah vh
oil
where
Ac v p
oil
(101)
Ah is the cross-sectional area of the hose and Ac the cross-sectional are of the
cylinder respectively.
68
Hydraulic model
Assuming that the oil density remains constant, the velocity in the connection region changes
to
vh =
Ac
vp .
Ah
(102)
v p Ac
pc = oil 1 + ph .
2 Ah
oil = 1000
(103)
v p = 0.5
m
and an approximate oil density of
s
kg
, the maximum pressure rise is given as a function of the diameters only:
m3
d 4
p = 250Pa c 1
d h
(104)
A maximum diameter ratio of dc /dh =10 at the blind-end side for example leads to a rise in
pressure of pmax 25 bar. It seems absolutely vital that the derived pressure variation
should be taken into account when designing control models of the crane. In this regard, we
have to consider the increase of pressure in the flow equations 66 - 70 and add the pressure
difference of equation 104 to p A and p B respectively.
Now, having derived a set of differential equations, we formulate a state space equation of the
y 56 = [ y 5 y 6 ] = [ p 1 p 2 ] ,
which includes both chamber pressures of the cylinder. Finally we equal y 56 and equations
94 and 95 as representing the time derivatives of the state vector and obtain the following
state space form of the cylinder hydraulics:
y& 5 =
E oil
V1
y& 6 =
dx
q1 + q int A1 p
dt
E oil
V2
dx
q 2 + qint + qext A2 p
dt
(105)
(106)
At this point we do not explicitly insert the flows that have been derived in chapter 4.1 into the
state space form. The case differentiation of the spool valve would end up in a rather
extensive formulation of the state space equations. However, one should bear in mind that it
can be easily done when it comes to model the hydraulic system in MATLAB.
69
70
curves in the following section are mirrored in the origin of the coordinate frame.
Nevertheless, when estimating the cylinder friction, we will run open-loop experiments in both
directions to examine the frictions dependency on the sign of velocity. For the compensation
model, however, we facilitate the model and define the mean value of the evaluated
parameters as being the nominal friction parameters.
Coulomb friction
In general, friction is the force that is exerted in the opposite direction to the motion of a body
whose surface is in contact with another body. The basic idea of friction during motions is that
the force is proportional to the normal force FN and thus it is not affected by the contact area
or the magnitude of velocity v :
F C = F N sign ( v ) for v 0 ,
where
(107)
The friction approximation in equation 107 is termed Coulomb friction and is a function of the
sign of velocity as graphed in Figure 48:
FC
FC
71
5.1.1.2
Viscous friction
Fv =k v v
(108)
sign ( v ) ,
experimental evaluations have shown that the friction force is consistent to the analytical
model with an coefficient of v 1 . 5 . The corresponding gradient of viscous friction is
depicted in Figure 49:
Fv
FC
FC
Fv
72
5.1.1.3
Static friction, often termed as stiction, is the threshold value that describes the friction force
at rest. Unlike Coulomb friction that is not determined at zero velocity, static friction introduces
the force that prevents the sliding of the surfaces of two bodies in contact. In order to
overcome static cohesion, stiction force has to be higher than Coulomb friction. Basically
static friction can be modelled as counteracted to external forces that are below the stiction
threshold, meaning that
F S = F e for v = 0
(109)
FS
Fv
FC
FC
Fv
FS
Figure 50: Coulomb friction, Viscous friction and Static friction
When estimating the friction parameters, we have used several cylinders that hold different
threshold values for the stiction force. In order to evaluate static friction with a good precision,
experiments at very low velocities have to be performed which is rather difficult by reasons of
interfering noise in the position sensor. This gives rise to the need for the dynamic model in
chapter 5.1.2, which mainly considers the bristle deflection of the lubricants at very low
velocities.
73
5.1.1.4
Stribeck friction
According to the work of Richard Stribeck, friction decreases continuously with increasing
velocity when entering the slipping phase. This phenomenon contradicts the discontinuous
behaviour of the stiction force. It rather describes the friction force in the transition between
sticking and slipping, which can be approximately estimated by the model
F St = ( F S F C ) e
in which v
v
v
(110)
F
FSt
FS
Fv
FC
FC
Fv
FSt
FS
Figure 51: Coluomb friction, Viscous friction, Static friction and Stribeck effect
breakaway force beyond which the motion is initiated. The breakaway phase is most
important for friction modelling and offers some anomalies that are not included in the static
friction model but will be captured by a dynamic model later on.
and
by Canudas de Wit and Lischinsky [14]) however both Stribeck coefficients can be evaluated
precisely in an easy manner by means of static friction maps that will be derived in chapter
5.2.2. The influence on the friction force of both Stribeck velocity and Stribeck gradient is
shown in Figure 52 and Figure 53:
74
75
It is obvious that the friction force is steeply falling with decreasing Stribeck velocity and
increasing Stribeck gradient respectively. Superposing the derived models brings us a
complete static friction model that has the form:
F = FC + ( F S FC ) e
F = 0 + 1e
v
v
v
v
+k v v
(111)
sign ( v ) or
+ 2 v
(112)
sign ( v ) respectively,
0 (Coulomb friction),
0 + 1 (Static friction) and
(113)
2 (Viscous friction).
5.1.2.1
Dynamic friction plays an important role when the contacting surfaces are steadily in transient
motion affected by permanent velocity reversals. This emanates from the assumption that
friction is not only determined by velocity but also depends on the rate of changes in the
external force. Figure 54 shows the common relationship between breakaway force and the
rate of force application.
FSt
dF
dt
Figure 54: Relation between breakaway force and rate of force application [16]
76
Figure 54 points out that breakaway force decreases with increasing force rate. Moreover,
friction is dependent on the rate of velocity changes. Therefore descriptive experiments have
been exerted in order to capture the friction behaviour at different accelerated motions (see
Figure 55 below).
5.1.2.2
The Lund-Grenoble model (LuGre) is a widely used dynamic model that describes the
microscopic deflection between the contact surfaces in terms of a damping-spring behaviour.
The dynamic system can be modeled by an elastic spring, where the friction force is related to
the average deflection of the bristles in the sticking phase. For this purpose, we will derive a
differential equation that leads to a new system state, namely the bristle deflection z :
z =
i =1
zi ,
(114)
where k denotes the number of bristles in the contacting area of the two surfaces.
77
zi
Fext
v
Figure 56: Schematic bristle deflection of contacting surfaces in motion
The standard parametrization of the bristle model can be obtained by establishing the
dynamic parameters 0 and 1 which correspond to the bristle deflection z . By adding
linear viscous friction, the dynamic friction force can then be computed as follows:
F = 0 z +
where
dz
+ 2v ,
dt
(115)
bristles.
For small displacements the bristles will behave like a spring-damping system in whose
steady-state the deflection z is a function of the velocity. An appropriate model is given in
[14]:
dz
0z
= v
v ,
dt
g (v )
(116)
where v is the piston velocity and g ( v ) the static friction regardless of viscous friction:
g (v ) =
+ 1e
v
v
(117)
78
We noticed that estimated cylinder friction in the hydraulic cylinder shows a characteristic
which can be divided into three different phases:
Piston
Piston
3
Piston
Cylinder
Cylinder
Cylinder
vp 0
v p z&
vp > 0
79
80
thoroughly. Moreover, we were not sure if the cylinder begins to oscillate when moving the
piston with high velocities. After several tests we found out that heavy cylinder vibrations did
not occur, not even in accelerated motions.
With pressure sensors located in each chamber as well as a length sensor for measuring the
piston extension we were able to determine the resulting friction force according to equation
118:
F fr = p 1 A1 p 2 A 2 + mg + m &x& ,
(118)
where m is the mass of the moving parts (load, piston and rod).
We used a dSpace system that we fed with the input signal of both pressure and length
sensor. With the Control Desk software we were able to capture the signals from the dSpace
system and moreover, the software calculated the friction force continuously. In order to
analyse the signals we saved the data as mat-files that we later imported in MATLAB to
obtain suitable charts.
In order to excite the proper output signal to the valve block we had to build a Simulink block,
specified by the characteristics of the control box to the valves. The block diagram is shown in
Figure 60 which also points out the opposed control signals to port A and B:
-6
Constant 6
(10 -2)/0.5
1
Constant 4
(1-0.75 )/(1-0.5)
Add 3
Gain 4
0.1
1
In 1
(1-0.75 )/(1-0.5)
0
Constant 7
Add 2
Gain 1
Add 5
2V - 10 V AND 0V
1
A
Gain
Constant 2
Gain 5
1
Constant 5
Add 4
0
Constant 3
0.1
0 V AND 2V - 10 V
2
B
Gain 3
-6
Constant 1
(10 -2)/0.5
Add 1
Gain 2
81
The above derived figures approve that friction force at our test cylinder matches the curves
from the previous friction model. The test bench had been configured properly and we were
ready to perform experiments for friction parameters estimation.
F = 0 + 1e
v
v
+ 2 v
sign ( v ) .
(119)
In order to determine the static parameters we had to run open-loop experiments on both
hydraulic cylinders. Most important to evaluate static friction is to keep the velocity constant
due to the effect of frictional lag (see also Figure 55). In this regard we tried to apply a PID
velocity control but it failed due to a time delay in the cranes system response. We then
simply controlled the output voltage and the current to the valves respectively and noticed that
this control method keeps the piston velocity constant as well.
To capture the data loggings, the following signals have been used for static parameter
estimation:
- Pressure sensor in the upper cylinder chamber p 1
- Pressure sensor in the lower cylinder chamber p 2
- Length sensor of piston position x p and its differentiation v p respectively
Friction force as calculated in equation 83 is then a function of both pressure signals and the
cylinder dimensions ( &x& p = 0 ):
F fr = p 1 A1 p 2 A 2 + mg
(120)
The corresponding Simulink models for friction force and piston velocity are shown in Figure
63 and Figure 64.
83
0.67
Out1
In1
xd2
Out1
In1
pressure1
p2
pressure
p1
0.05
Out1
In1
d1
A1
F1
F_fr
Ffr
friction
0.03
Out1
In1
A2
d2
F2
Ar
Out1
In1
Fg
position _unf
position
velocity
acceleration
x_unfiltered
besself
Bad
Link
ADC
DS 1104 ADC_C5
In 1
du /dt
Out1
Derivative xv
du /dt
Derivative va
Analog
Filter Design
84
Pressure 1
Pressure 2
Velocity
Friction force
Figure 65: dSpace data logging with unfiltered (left) and filtered (right) velocity signal (grey)
We began the experiments with approximately ten different piston velocities in the range of
0 . 0001
m
m
to 0 . 1
. Within this range numerous values were collected at very low
s
s
velocity in order to specify the identification of the Stribeck effect. Afterwards we applied the
complementary output to the valves to achieve negative velocities for the retracting motion.
We also evaluated the static friction at zero velocity by finding the maximum output, friction
force respectively at which the piston was kept just about from moving. Hence we were able
to determine the coefficients 0 and 1 immediately.
After a certain delay all sensor signals remained on a constant level. We saved the mat data
files and created a chart in MATLAB that represented the friction force at different velocities.
85
In order to obtain a general friction map we had to find a continuous behaviour between
friction and velocity. For this purpose we drew a curve through the measured values and
applied hereby two different methods.
The first was to simply average the values at each velocity level to eliminate the error due to
noise in the sensor signals. As an output, we yielded several well-defined friction values that
corresponded each to a certain velocity. This form of a friction map sufficiently describes all
the unknown static parameters as graphically estimated in Figure 67.
v = v p ( F )
F = 0 +
1
e
for 2 v p << F
(121)
The Stribeck velocity to solve is then the point of intersection at the specific friction force F .
Another way of finding an analytical expression of the cylinder friction is to use the
optimization toolbox in MATLAB. Therefore we applied the lscurvefit-command to the
experimentally evaluated values. By doing so we were able to determine a non-linear function
that represented the static parameters, solved through least square deviation:
min
0 , 1 , 2 , v
i =1
[F ( v
],
2
) F ( v
(122)
where F ( v p ) includes the measured friction values and F ( v p ) is the non-linear function
solved by the lscurvefit-command in MATLAB.
However, in this case we encountered the problem of unequally distributed values, which
86
means that the majority of our measured values corresponded to low velocities. Consequently
MATLAB solved the least square problem with an inaccurate curve gradient at higher
velocities or insufficient consideration of the Stribeck effect (see Figure 68). Consistently, we
decided to estimate the parameters manually as proposed in Figure 67.
Velocitiy [m/s]
Evaluated data
Curve-fit function
Figure 68: Problem of inaccurate curve gradient when solving the least-square function
Thereafter we substituted the derived parameters in equation 119 in order to verify the
resulted continuous graph with the experimentally obtained curve. The friction maps for both
cylinders are depicted in Figure 69 whereas the static parameters are listed separately in
Table 3:
Cylinder
Rottne (0 kg)
Rottne (115 kg)
Kalmar (0 kg)
Kalmar (115 kg)
160 / 280
170 / 350
105 / 80
140 / 100
180 / 120
450 / 450
350 / 300
310 / 350
8000 / 12000
10500 / 12000
1250 / 1800
1200 / 1800
0.002 / 0.002
0.004 / 0.002
0.01 / 0.015
0.015 / 0.02
87
Rottne cylinder
Kalmar cylinder
Figure 69: Static friction map of Rottne cylinder and Kalmar cylinder
88
From Figure 69 we basically notice two fundamental differences. On the one hand, friction
force at the Rottne cylinder increases more heavily than the Kalmar cylinder does. This might
probably arise from the fact that the Rottne cylinder has been used in real-world application
before and consequently it consists of wear particles that have been piled up as a
consequence of abrasion. Another significant disparity is given by a much higher Stribeck
velocity of the friction force at the Kalmar cylinder. This is reflected in a major specification of
the Stribeck effect within in the friction curve, represented by its minimum at higher velocities.
One should bear in mind that the above friction parameters have been experimentally
evaluated and may further change as dependent on temperature variations, position and
normal forces in contact. Nevertheless, as start to our dynamic model, the derived coefficients
turn out to be suitable for the derivation of static friction maps and further parameter
estimation.
5.2.3.1
Let us use a small ramp function as an input to the valves. The corresponding Simulink model
is depicted in Figure 70:
simout 4
signal 1
0.5
Bad
Link
DAC
input
A
In1
signal total
output 1
Ramp
DAC
Bad
Link
output 2
simout 5
signal 2
Rewriting equation 119 for ramp control allows us to neglect the damping term while
assuming v 0 and z& 0 :
F 0 z u ( t ) ,
(123)
0z
dz
v ,
= v
0 + 1
dt
in which g ( v 0 ) =
(124)
+ 1.
When applying the input ramp function u ( t ) = ct with small c > 0 , equation 124 can be
integrated for v > 0 as follows:
c
z (t ) = x p (t ) x p (0 )
2 ( 0 + 1 )
x (t )t +
x ( ) d
(125)
It is most important that the experiments are performed from rest, that is z ( t 0 ) = 0 , which
means that we initiate the ramp signal when the piston is fully retracted. In this special case
we can assure that the bristles are not deflected by means of unbalanced chamber pressures.
Furthermore, we have to compute z within the time interval [ 0 , T ], whereas T specifies
the time up to the very moment before point of break-away.
Next, the ramp responding curves are depicted together with piston position and friction force
for both hydraulic cylinders.
90
Rottne cylinder
Kalmar cylinder
91
When comparing both ramp responses, we consider a much stronger shearing strain at the
Rottne cylinder with a maximum bristle deflection of approximately 4 mm. The deflection at
the Kalmar cylinder is almost unrecognizable and only measurable by further data processing,
e.g. averaging the data samples and creating a mean curve.
One should bring to mind that ramp control enhances the springing effect of bristles and
though we can easily compute the bristle stiffness by solving equation 123 for 0 . When
averaging the data vectors Z and U
= Z
U =
0 is then determined by
Z TU
. [14]
ZTZ
(126)
Since point of break-away was reached very early at the Kalmar cylinder, we had to use
different input ramps for both cylinders in order to log the datas for nearly the same time
period.
The computed values for the first dynamic coefficient are listed below in Table 4:
Cylinder
Rottne (c=0.01)
Kalmar (c=0.0001)
0 extending
1.2 x 10e5
1.0 x 10e6
0 retracting
1.0 x 10e5
1.2 x 10e6
0 averaged
1.1 x 10e5
1.1 x 10e6
0
According to Table 4 the bristle deflection of the Kalmar cylinder is far less than it is at the
Rottne cylinder. Therefore it is evident that its bristle stiffness and consequently its averaged
coefficient 0 are approximately higher by the power of ten.
5.2.3.2
The estimated coefficients are now being used for computing the second dynamic parameter
1 . Therefore we run open-loop experiments again, but to cover the bristles damping
behaviour we use step inputs instead of a ramp control. The corresponding Simulink block is
shown in Figure 72.
s im out 4
s ignal 1
0 .5
DAC
B ad
Link
input
A
In 1
s ignal total
output 1
S tep
DAC
B ad
Link
output 2
s im out 5
s ignal 2
Within the shearing phase, a step input leads approximately to z& x& p which lets us
simplify equation 119 to
m &x& p + 0 x + ( 1 + 2 ) x& p = F ,
where
and
(127)
parameter to solve.
Notice that due to inconstant piston velocity we have to consider Newtons second law to our
friction model. In this regard, we derive the unfiltered piston position twice and multiply the
result with the mass of the cylinders loos parts that are predominantly its rod, its piston and
the external load.
When running the experiment, it is most crucial to use sufficiently small step inputs in order to
examine the bristle behaviour within the stiction regime. This condition turned out to be almost
unmanageable because of immoderate noise that interfered with the smallish and unfiltered
velocity signal. The following step responses were investigated through MATLAB data
processing. Figure 73 shows the step input response for both cylinders whereas the
estimated parameters for 1 are given in Table 5:
Rottne cylinder
Kalmar cylinder
93
Cylinder
Rottne
Kalmar
ext
-1.53 x 10e4
-1.74 x 10e5
ret
-1.55 x 10e4
-1.73 x 10e5
avg
-1.54 x 10e4
-1.74 x 10e5
Figure 74: Comparison between friction force with variation in pressure supply
94
The nearly identical friction curve of varying loads is depicted in Figure 75:
95
z pos
x pos
vz
vx
z neg
x neg
ez
ex
Figure 77: Possible arrangement for a single joystick control of the crane-tip
96
That means the direction of the speed of the end-effector (the gripper) follows the joystick
command and if you release it, the crane simply stops moving. Hence, the joystick input will
be used as a target-value for the desired speed of the crane-tip and not of the hydraulic
cylinders. The output from the controller then feeds the corresponding current into the valves
in order to keep the cylinders at a necessary speed that is required to fulfill the specified
crane-tip trajectory.
During forestry work, this type of control might also require a second joystick but in
comparison to control methods nowadays, it will be used only for rotational control about the
1st swivel-joint to determine the xz plane. However, this configuration simplifies the learning
task for humans, making it unnecessary to coordinate both hands in a complex pattern.
Moreover this arrangement is likely to reduce strain injuries while increasing the efficiency of
the main work. A first suggestion for this control is given in [9], but still there are no forestry
machines in use that adapted this control principle by now.
The follwing chapter describes a two-dimensional control approach in the common
xz plane. Other solutions, including a full three-dimensional crane-tip control in the
xyz space will be developed in the near future within the department of Applied Physics
and Electronics at Ume University, Sweden.
x joystick
xz plane
0
t0
t1
t2
t3
t4
t
v
v (t 4 )
z joystick
v
v (t3 )
0
t0
t1
t2
t3
v
v (t0 )
v
v (t1 )
v
v (t 2 )
t4
x
sampling
crane-tip position
97
relative speed between the cylinders. This is the main difference to the traditional control that
uses the joystick input for controlling the hydraulic cylinders and thus moving the crane-tip
circularly in proportion to the angular velocity about each joint.
We used a PID control for control purposes. It is described in the following section.
u = k p et + k i et j + k d
j=0
et et 1
T
(128)
with k p being the proportional gain, k I the integral gain and k d the derivative gain. The
control error e t results from the deviation between the real crane-tip coordinates given by
the sensor signals and the desired coordinates that are calculated by the control system. The
index in the control error denotes the sampling time, meaning that et 1 is the error of the
previous sampling to et . The length of time of a sampling period is given by T and finally the
resulting control signal to trigger the valves is termed u .
This control seems to be the most common control but since we have limited control signals
we probably encounter wind-up effects of the integral part, meaning that the term
j=0
et
continues to integrate indefinitely. This might end up in high control inputs even though the
control error is low. Inevitably the discrete control will lead to inadequate results. We can
avoid the wind-up problem by rewriting equation 128 into its differential form:
u = u t u t 1 = k
(e t
e t 1 ) + k i e t + k d (e t + e t 2 )
(129)
98
The present control output is then defined by the sum of the previous control output and the
differential output of equation 129. This allows for limiting the control output to
u t = (u t 1 + u
where
max
min
) max
min
(130)
denotes the range of possible control values within the predefined boundaries.
A possible MATLAB-code for the decribed differential PID-control of a single cylinder is given
in Figure 79:
*********************************************************************
% control cylinder 1
% *********************************************************************
c1.K_p = 1; % proportional gain
c1.K_d = 0.01; % derivative gain
c1.K_i = 0.0001; % integral gain
c1.e_2 = c1.e_1; % error t_-2
c1.e_1 = c1.e; % error t_-1
c1.e = alpha_d - x(1); % error
c1.d_u = c1.K_p * (c1.e - c1.e_1) + c1.K_i * c1.e + c1.K_d * (c1.e - 2 * c1.e_1 +
c1.e_2); % differential control force
c1.u = c1.u + c1.d_u; % control force
c1.u = sign(c1.u) * min(abs([x_s_max; c1.u])); % limit control input
c1.i = 1 * c1.u; % input to spool
99
v z , joy = k u z respectively
5) Determine
the
new
crane-tip
coordinates
by
means
of
equations
and
10) Determine if new configuration is better than previous ones (penalty function)
11) End Loop of inverse kinematics
12) Overwrite control error et 2 with last error et1
13) Overwrite control error et1 with present error et ( e and e )
14) Calculate new control errors from the difference e = d and e =
6.2.3 Optimization
The algorithm in Table 6 is based on the described inverse kinematics problem but also
includes variations in length of the telescopic beam according to a penalty function. This
function is not implemented in the attached MATLAB code but it should be designed in further
studies to vary the extension length of the 3rd cylinder in a loop while calculating the bestangle configuration for the other two cylinders.
The overall aim of a penalty function is to determine the best cylinder configuration, aiming at
limitation of crane-tip coordinates (working range) and avoidance of large changes in joint
angles. This is explicitly done by testing several values of the telescopic extension x3 and
100
p ( , , x3 , d , d , x3,d ) =
w0 outsideran ge + ...
w1 ( d ) 2 + w2 ( d ) 2 + w3 ( x3 x3,d ) 2 + ...
(131)
w4 ( ) 2 + w5 ( ) 2 + w6 ( x3 x3 ) 2
On the right-hand side of equation 131, the first term penalizes the crane-tip coordinates
outside the accessible working range including an arbitrary factor w0 . The factors w1 , w2 and
w3 of the 2nd term limit high joint angles requiring unachievable cylinder speed. Furthermore
one should attempt to keep the pistons in the middle of each cylinder, avoiding the risk of new
crane reconfiguration (due to maximum retraction / extension length) which might end up in
large errors. For this purpose, the notations , and x3 are used as being the middlevalue of the nominal working range. Hence, the terms referred to the factors w4 , w5 and w6
will force the cylinder to strive into their recommended center position.
Equation 131 can be extended at will considering new optimization methods. Future works on
the topic of crane control should implement this penalty function in the present MATLAB code.
joy hor = 0 . Then the simulation was run five seconds in order to investigate the cranes
trajectory beginning at initial conditions for the joint configuration = 100 and = 100 .
Since we did not reset the initial control conditions within these five seconds, this type of
control should force the gripper to follow a path parallel to the x-axis. Consequently it should
be characterized by the same z-coordinates. From Figure 80 and Figure 81 one can clearly
notice the expected upward movement of the crane tip, however we also want to figure out if
the gripper has deviated from its straight vertical path. Therefore we have created a chart that
represents the trajectory of the crane tip in both x- and z-axis (see Figure 82).
101
102
Figure 82: Trajectory path of the crane tip with vertical control
Figure 82 indicates that the crane tip almost follows a linear path in z-axis. Starting at
dz
m
and reaches z = 1.1m
0.25
dt
s
after five seconds. However, the crane-tip also moves in x-direction, most notably in the
beginning of the control task. Obviously the reason is the gravitational force of the crane
referring the first joint. Here the cylinder cant build up the necessary force that quickly. This
time delay should be further reduced by adjusting the control coefficients.
In our second test we ran the simulation with opposite control, namely a horizontal control
task with the control parameters joy ver = 0 and joy hor = 0.5 . The result of the animation
can be seen in Figure 83 and Figure 84. The crane tip moves straight to the left
(approximately with
dx
m
0,3 ) and finally ends up in the same position on the z-axis.
dt
s
However, if we look closer to the trajectory in z-direction, we notice that in between motion,
the crane tip has deviated from its initial z-position z = 0.2 m to z = 0.6 m after three
seconds. In this special case the cylinders can build up the pressure fast enough to force the
crane tip almost in its initial level on the z-axis ( z = 0.3m ). In Figure 85 the motions for both
x-axis and z-axis are depicted.
103
104
Figure 85: Trajectory path of the crane tip with horizontal control
The prescending simulations gave us a useful indication that generally the control task
operates quite well. Motion control was responded by a coresponding movement in the
desired direction. However we have noticed that gravity can lead to a deviation at the
beginning of each motion. Once the cylinders have compensated these forces, motion control
can be achieved in a good way as seen in Figure 85. In forthcoming projects one should
further optimize the control parameters efficiently for its desired application.
105
y& 1 y 2
y& = q&&
2
y4
y& 3
y& = 2 y 2 y + 2 u ( t )
3
n
n 4
n
4
x
E oil
q1 + q int A1 p
&
y5
V1
dt
y& =
xp
E oil
6
V q 2 + q int + q ext A2 dt
2
[ y& 7 ] = [z& ]
106
We have now created the set of space state equations for a sinlge part-model. In the same
fashion we are then able to combine the three part-models to a complete crane model. This
overall model is characterized by 21 states; however the follwing m-file CRANE_MAIN does
not include the friction state as we adapted the static friction model for reasons of better
performance.
% generalized vectors
% *************************************************************************
syms alph bet x3 alph_p bet_p x3_p % generalized coordinates
syms f1 f2 f3 % generalized forces (resulting cylinder forces)
q
= [alph; bet; x3]; % (3)
q_p = [alph_p; bet_p; x3_p];
w_alph = [0;alph_p;0]; % angular velocity alpha
w_bet = [0;bet_p;0]; % angular velocity beta
I_w_alpha = w_alph; % angular velocity alpha in inertia frame
I_w_beta = w_alph + w_bet; % angular velocity beta in inertia frame
% *************************************************************************
% gravitation vector
% *************************************************************************
g = 9.81; % gravitational acceleration [m/s2]
I_g = [0; 0; -g];
% *************************************************************************
% control input
% *************************************************************************
joy.hor = 0; % horizontal axis -0.5 = left, 0 = stop, 0.5 = right [m/s]
joy.ver = 0.5; % vertical axis -0.5 = down, 0 = stop, 0.5 = up [m/s]
joy.t_samp = 5; % sampling interval [sec]
joy.t_low = 0; % initial time minimum [sec]
% *************************************************************************
% crane parameters
% *************************************************************************
[par] = dimension;
l0 = par{1}; % tripod
l1 = par{2}; % link 1
l2 = par{3}; % link 2
l3 = par{4}; % link 3
107
l4 = par{5}; % link 4
cl = par{6}; % torque link
c1 = par{7}; % cylinder 1
c2 = par{8}; % cylinder 2
c3 = par{9}; % cylinder 3
% *************************************************************************
% general properties
% *************************************************************************
% fluid *******************************************************************
rho = 1000; % oil density [kg/m3]
E_max = 1e9; % maximum bulk modulus of oil [Pa]
C_1 = 90; % bulk modulus coefficient 1 [-]
C_2 = 3; % bulk modulus coefficient 2 [-]
% pump
p_s = 140e5; % supply pressure of the pump [Pa]
p_0 = 0; % atmosphere pressure of the tank [Pa]
p_max = 200e5; % maximum pressure [Pa]
% cylinder ****************************************************************
F_c = 100; % coulomb friction [N]
F_s = 750; % static friction (stiction) [N]
v_s = 0.005; % stribeck velocity [m/s]
k_v = 3500; % viscous friction coefficient [Ns/m]
mu = 0.6; % friction coefficient of cylinder gaskets [-]
k = 5e6; % spring stiffness at cylinder floor [N/m] (limitation of cylinder stroke)
% spool *******************************************************************
x_s_max = 0.05; % maximum spool stroke [m]
w_n = 2 * pi * 161.2; % natural frequency of the valve spool [1/s]
B = 0.481; % damping factor of the valve spool[-]
p_tr = 3e5; % (63) transition pressure corresponding to Reynolds transition [Pa]
p_tr_min = 2e5; % (63) lower threshhold for transition region [Pa]
p_tr_max = 4e5; % (63) upper threshhold for transition region [Pa]
Q_N = 0.0001; % nominal flow [m3/s] (valve characteristic)
p_n = 150e5; % nominal pressure drop [Pa] (valve characteristic)
C_0 = Q_N / (x_s_max * sqrt(0.5 * p_n)); % discharge coefficient [sqrt(m5/kg)]
% *************************************************************************
% cylinder 1
% *************************************************************************
c1.A_1 = power(c1.d_c,2) * pi / 4; % area chamber 1 [m2]
c1.A_2 = c1.A_1 - (power(c1.d_r,2) * pi / 4); % area chamber 2 [m2]
c1.A_0 = power(c1.d_0,2) * pi / 4; % area dead zone [m2]
c1.V_0d1 = c1.l_0 * c1.A_0; % dead volume chamber 1 [m3]
c1.V_0d2 = c1.l_0 * c1.A_0; % dead volume chamber 2 [m3]
c1.V_01 = c1.V_0d1 + c1.A_1*(c1.l_c-c1.l_p)/2; % (84) volume of oil in chamber 1 at x
= 0 [m3]
c1.V_02 = c1.V_0d2 + c1.A_2*(c1.l_c-c1.l_p)/2; % (85) volume of oil in chamber 2 at x
= 0 [m3]
c1.l_k = c1.l_c/20; % spring position from cylinder end [m]
c1.k_int = pi * (c1.d_c/2+c1.r_c) * power(c1.r_c,3) / (6*mu*c1.l_p); % (99) internal
leakage factor [m3/kg]
c1.k_ext = pi * (c1.d_r/2+c1.r_c) * power(c1.r_c,3) / (6*mu*c1.l_cw); % (99) external
leakage factor [m3/kg]
% *************************************************************************
% cylinder 2
% *************************************************************************
c2.A_1 = power(c2.d_c,2) * pi / 4; % area chamber 1 [m2]
c2.A_2 = c2.A_1 - (power(c2.d_r,2) * pi / 4); % area chamber 2 [m2]
c2.A_0 = power(c2.d_0,2) * pi / 4; % area dead zone [m2]
c2.V_0d1 = 0.02 * c2.A_0; % dead volume chamber 1 [m3]
c2.V_0d2 = 0.02 * c2.A_0; % dead volume chamber 2 [m3]
c2.V_01 = c2.V_0d1 + c2.A_1 * (c2.l_c - c2.l_p) / 2; % (84) volume of oil in chamber 1
at x = 0 [m3]
c2.V_02 = c2.V_0d2 + c2.A_2 * (c2.l_c - c2.l_p) / 2; % (85) volume of oil in chamber 2
at x = 0 [m3]
c2.l_k = c2.l_c/20; % spring position from floor [m]
c2.k_int = pi * (c2.d_c / 2 + c2.r_c) * power(c2.r_c,3) / (6*mu*c2.l_p); % (99)
internal leakage factor [m3/kg]
c2.k_ext = pi * (c2.d_r / 2 + c2.r_c) * power(c2.r_c,3) / (6*mu*c2.l_cw); % (99)
external leakage factor [m3/kg]
% *************************************************************************
% cylinder 3
108
% *************************************************************************
c3.A_1 = power(c3.d_c,2) * pi / 4; % area chamber 1 [m2]
c3.A_2 = c3.A_1 - (power(c3.d_r,2) * pi / 4); % area chamber 2 [m2]
c3.A_0 = power(c3.d_0,2) * pi / 4; % area dead zone [m2]
c3.V_0d1 = 0.02 * c3.A_0 * pi / 4; % dead volume chamber 1 [m3]
c3.V_0d2 = 0.02 * c3.A_0 * pi / 4; % dead volume chamber 2 [m3]
c3.V_01 = c3.V_0d1 + c3.A_1*(c3.l_c-c3.l_p)/2; % (84) volume of oil in chamber 1 at x
= 0 [m3]
c3.V_02 = c3.V_0d2 + c3.A_2*(c3.l_c-c3.l_p)/2; % (85) volume of oil in chamber 2 at x
= 0 [m3]
c3.l_k = c3.l_c/20; % spring position from floor [m]
c3.k_int = pi * (c3.d_c/2+c3.r_c) * power(c3.r_c,3) / (6*mu*c3.l_p); % (99) internal
leakage factor [m3/kg]
c3.k_ext = pi * (c3.d_r/2+c3.r_c) * power(c3.r_c,3) / (6*mu*c3.l_cw); % (99) external
leakage factor [m3/kg]
c3.l = (c3.l_c + c3.l_p) / 2 + c3.l_r + x3 + c3.o_s + c3.o_e; % (2) total lenght of
cylinder [m]
% *************************************************************************
% transformation matrices
% *************************************************************************
A_IK1 = eye(3); % rotation around z-axis of link 1 (==0)
A_K1K2 = A_y(pi / 2 - alph); % (4) rotation around y-axis of link 2 ! alpha' = pi / 2
- alpha !
A_K2K3 = A_y(pi - bet); % (5) rotation around y-axis of link 3 ! beta' = pi - beta !
A_K2_CL = A_y(-cl.eps1); % (26) rotation from torque link 1 to K2
A_IK2 = A_IK1 * A_K1K2; % (7) rotation matrix of K2 associated to initial frame
A_IK3 = A_IK1 * A_K1K2 * A_K2K3; % (8) rotation matrix of K2 associated to initial
frame
A_ICL = A_IK1 * A_K1K2 * A_K2_CL; % rotation matrix of torque frame associatred to
initial frame
% *************************************************************************
=
=
=
=
=
=
=
=
=
A_IK1
A_IK2
A_IK3
A_IK3
*
*
*
*
l1.cg;
l2.cg;
l3.cg;
l4.cg;
%
%
%
%
center
center
center
center
of
of
of
of
gravity
gravity
gravity
gravity
link
link
link
link
1
2
3
4
109
% piston stroke 1
% *************************************************************************
c1.r = A_IK1' * (I_r_0_22 - I_r_0_13); % cylinder vector in K1
c1.l = sqrt(c1.r(1) * c1.r(1) + c1.r(2) * c1.r(2) + c1.r(3) * c1.r(3)); % (1) total
length of cylinder (joint-2-joint)
c1.phi = atan(c1.r(1) / c1.r(3)); % (15) angle between cylinder and link
c1.phi = simplify(c1.phi);
c1.fhandle_phi = sym_2_fun(c1.phi,'alph');
c1.x = c1.l - c1.l_r - c1.l_c / 2 - c1.l_p / 2 - c1.o_s - c1.o_e; % (2)/(40) extension
[m]
c1.x = simplify(c1.x);
c1.fhandle_x = sym_2_fun(c1.x,'alph');
c1.v = jacobian(c1.x,q) * q_p; % velocity [m/s]
c1.v = simplify(c1.v);
c1.fhandle_v = sym_2_fun(c1.v,'alph, alph_p');
% *************************************************************************
% piston stroke 2
% *************************************************************************
c2.r = l2.p4 + A_K2_CL * cl.r1 - l2.p3; % (41) cylinder vector in K2
c2.l = sqrt(c2.r(1) * c2.r(1) + c2.r(2) * c2.r(2) + c2.r(3) * c2.r(3)); % (1) total
length of cylinder (joint-2-joint)
c2.phi = atan(c2.r(3) / c2.r(1)); % (28) angle between cylinder and link (phi gets
negativ)
c2.phi = simplify(c2.phi);
c2.fhandle_phi = sym_2_fun(c2.phi,'alph, bet');
c2.x = c2.l - c2.l_r - c2.l_c / 2 - c2.l_p / 2 - c2.o_s - c2.o_e; % (2)/(42) extension
[m]
c2.x = simplify(c2.x);
c2.fhandle_x = sym_2_fun(c2.x,'alph, bet');
c2.v = jacobian(c2.x,q) * q_p; % velocity [m/s]
c2.v = simplify(c2.v);
c2.fhandle_v = sym_2_fun(c2.v,'alph, alph_p, bet, bet_p');
% *************************************************************************
110
%
%
T
%
% potential energy
% *************************************************************************
V = l1.m * I_g' * I_r_0_1cg +...
l2.m * I_g' * I_r_0_2cg +...
l3.m * I_g' * I_r_0_3cg +...
l4.m * I_g' * I_r_0_4cg; % (52)
% *************************************************************************
% actuator forces
% *************************************************************************
A_Ic1 = A_y(pi / 2 + c1.phi); % (16) rotation matrix from cylinder 1 to earth frame
A_Ic2 = A_IK2 * A_y(-c2.phi); % (29) rotation matrix from cylinder 2 to earth frame
A_Ic3 = A_IK3; % rotation matrix from cylinder 3 to earth frame
I_f_a1 = A_Ic1 * [f1;0;0]; % cylidner force vector 1
I_f_a2 = A_Ic2 * [f2;0;0]; % cylidner force vector 2
I_f_a3 = A_Ic3 * [f3;0;0]; % cylidner force vector 3
% *************************************************************************
% non-potential forces
% *************************************************************************
Q = jacobian(I_r_0_13,q).' * I_f_a1 - jacobian(I_r_0_22,q).' * I_f_a1 +...
jacobian(I_r_0_cl,q).' * I_f_a2 - jacobian(I_r_0_23,q).' * I_f_a2 +...
jacobian(I_r_0_42,q).' * I_f_a3 - jacobian(I_r_0_33,q).' * I_f_a3; % (54)
% *************************************************************************
111
% ode call
% *************************************************************************
[t,x] =
ode23(@odefunction,t_start:t_step:t_end,x0,opt,fhandle_M,fhandle_h,c1.fhandle_x,c2.fha
ndle_x,c1.fhandle_v,c2.fhandle_v);
% *************************************************************************
% plot call
% *************************************************************************
PLOT_CHARTS(t,x,c1,c2,c3,cl,l0,l1,l2,l3,l4,t_step,t_end)
% *************************************************************************
*
*
*
*
l1.p2 +
l2.p5 +
l3.p3 +
[c3.o_s
...
...
...
+ c3.l_c / 2 + x(i,13) + c3.l_p / 2 + c3.l_r +
* (l4.p1 - l4.p2);
end
figure
subplot(2,1,1)
plot(t',ct(1,:))
grid
xlabel('time [sec]')
ylabel({'trajectory of crane tip';'in x-direction [m]'})
subplot(2,1,2)
plot(t',ct(3,:))
grid
xlabel('time [sec]')
ylabel({'trajectory of crane tip';'in z-direction [m]'})
% *************************************************************************
end
% *************************************************************************
% --------------------------END OF MAIN-----------------------------------% *************************************************************************
function
dxdt=odefunction(t,x,M_function,h_function,x1_function,x2_function,v1_function,v2_func
tion)
global c1 c2 c3 l1 l2 l3 l4 % cylinder and link properties
global p_s p_0 C_0 k E_max p_max x_s_max C_1 C_2 F_c F_s v_s k_v w_n B p_tr p_tr_min
p_tr_max rho % general properites
global joy % control input
global alpha_d beta_d x3_d % computation of generalized coordinates
% crane control
112
% *************************************************************************
if (t == 0) || ((t - joy.t_low) > joy.t_samp) % sampling criterion
joy.t_low = t; % reset of lower time
% position of the crane tip
A_IK1 = eye(3);
A_IK2 = A_IK1 * A_y(pi / 2 - x(1));
A_IK3 = A_IK1 * A_y(pi / 2 - x(1)) * A_y(pi - x(7));
ct = A_IK1 * l1.p2 + ... % link 1
A_IK2 * l2.p5 + ... % link 2
A_IK3 * l3.p3 + ... % link 3
A_IK3 * [c3.o_s + c3.l_c / 2 + x(13) + c3.l_p / 2 + c3.l_r + c3.o_e;0;0] +
... % cylinder 3
A_IK3 * (l4.p1 - l4.p2); % tele beam
% inverse kinematics
joy.delta = [joy.hor * joy.t_samp; 0; joy.ver * joy.t_samp]; % joystick input
ct_d = ct + joy.delta; % desired position of the crane tip [m]
x3_d = 0; % telescopic beam extension (will be varied ...)
l3.ct = l3.p3 + [c3.o_s + c3.l_c / 2 + x3_d + c3.l_p / 2 + c3.l_r + c3.o_e;0;0] +
(l4.p1 - l4.p2); % K3-vector from joint 3 to crane tip [m]
r1 = norm(l2.p5); % length of r1 [m]
r2 = norm(l3.ct); % length of r2 [m]
r_diag = ct_d - l1.p2; % (36) diagonal vector [m]
beta_d = acos((r1 * r1 + r2 * r2 - r_diag' * r_diag) / (2 * r1 * r2)); % (35)
desired angle beta related to joint axis [rad]
alpha_d = acos((l1.p2' * l1.p2 + r_diag' * r_diag - ct_d' * ct_d) / (2 *
norm(l1.p2) * norm(r_diag))) + acos((r1 * r1 + r_diag' * r_diag - r2 * r2) / (2 * r1 *
norm(r_diag))); % (37) desired angle alpha related to joint axis [rad]
alpha_d = alpha_d - atan(l2.p5(3) / l2.p5(1)); % (38) desired angle alpha related
to frame axis [rad]
beta_d = beta_d + atan(l2.p5(3) / l2.p5(1)) - atan(l3.ct(3) / l3.ct(1)); % (39)
desired angle beta related to frame axis [rad]
% text
disp({['sampling time = ',num2str(joy.t_low)]})
disp({['ct (x) = ',num2str(ct(1))],['ct_d (x) = ',num2str(ct_d(1))]})
disp({['ct (z) = ',num2str(ct(3))],['ct_d (z) = ',num2str(ct_d(3))]})
disp({['alpha = ',num2str(x(1) * 180 / pi)],['alpha_d = ',num2str(alpha_d * 180 /
pi)]})
disp({['beta = ',num2str(x(7) * 180 / pi)],['beta_d = ',num2str(beta_d * 180 /
pi)]})
disp(' ')
end
% *************************************************************************
% *************************************************************************
% control cylinder 1
% *************************************************************************
c1.K_p = 1; % proportional gain
c1.K_d = 0.01; % derivative gain
c1.K_i = 0.0001; % integral gain
c1.e_2 = c1.e_1; % error t_-2
c1.e_1 = c1.e; % error t_-1
c1.e = alpha_d - x(1); % error
c1.d_u = c1.K_p * (c1.e - c1.e_1) + c1.K_i * c1.e + c1.K_d * (c1.e - 2 * c1.e_1 +
c1.e_2); % differential control force
c1.u = c1.u + c1.d_u; % control force
c1.u = sign(c1.u) * min(abs([x_s_max; c1.u])); % limit control input
c1.i = 1 * c1.u; % input to spool
% *************************************************************************
% control cylinder 2
% *************************************************************************
c2.K_p = 1; % proportional gain
c2.K_d = 0.01; % derivative gain
c2.K_i = 0.005; % integral gain
c2.e_2 = c2.e_1; % error t_-2
c2.e_1 = c2.e; % error t_-1
c2.e = beta_d - x(7); % error
113
% *************************************************************************
% chamber flow cylinder 1
% *************************************************************************
c1.dp1 = rho / 2 * (power((c1.A_1 / c1.A_0),2) - 1) * power(v1_real,2); % (104)
additional pressure chamber 1 (Bernoulli)
c1.dp2 = rho / 2 * (power((c1.A_2 / c1.A_0),2) - 1) * power(v1_real,2); % (104)
additional pressure chamber 2 (Bernoulli)
if x(5) >= 0 % positive flow x(5) = 0 redundant
delta_p1 = p_s - (x(3) - c1.dp1);
delta_p2 = (x(4) - c1.dp2) - p_0;
elseif x(5) < 0 % negative flow
delta_p1 = (x(3) - c1.dp1) - p_0;
delta_p2 = p_s - (x(4) - c1.dp2);
end
if abs(delta_p1) >= p_tr_max % (66/67) turbulent flow
c1.Q_1 = C_0 * x(5) * sign(delta_p1) * sqrt(abs(delta_p1)); % flow chamber 1
p_1==p_s
elseif abs(delta_p1) < p_tr_min % (69/70) laminar flow
c1.Q_1 = C_0 * x(5) * delta_p1 * (abs(delta_p1) / p_tr) / (2 * sqrt(p_tr)); %
factor '(3...' is between 2e-3 and 4e-3
else % (74) transition range
c1.Q_1 = abs((abs(delta_p1) - p_tr_min) / (p_tr_max - p_tr_min)) * ...
C_0 * x(5) * sign(delta_p1) * sqrt(abs(delta_p1)) + ...
(1 - abs((abs(delta_p1) - p_tr_min) / (p_tr_max - p_tr_min))) * ...
C_0 * x(5) * delta_p1 * (abs(delta_p1) / p_tr) / (2 * sqrt(p_tr)); %
(69) transition flow
end
if abs(delta_p2) >= p_tr % (58/59) turbulent flow
c1.Q_2 = C_0 * x(5) * sign(delta_p2) * sqrt(abs(delta_p2)); % flow chamber 2
p_2==p_0
elseif abs(delta_p2) < p_tr % laminar flow
c1.Q_2 = C_0 * x(5) * delta_p2 * (abs(delta_p2) / p_tr) / (2 * sqrt(p_tr));
else % (74) transition range
c1.Q_2 = abs((abs(delta_p2) - p_tr_min) / (p_tr_max - p_tr_min)) * ...
C_0 * x(5) * sign(delta_p2) * sqrt(abs(delta_p2)) + ...
(1 - abs((abs(delta_p2) - p_tr_min) / (p_tr_max - p_tr_min))) * ...
C_0 * x(5) * delta_p2 * (abs(delta_p2) / p_tr) / (2 * sqrt(p_tr)); %
(69) transition flow
end
c1.Q_int = c1.k_int * (x(3) - x(4)); % (97) internal leakage
c1.Q_ext = c1.k_ext * x(4); % (98) external leakage
% *************************************************************************
% chamber flow cylinder 2
% *************************************************************************
c2.dp1 = rho / 2 * (power((c2.A_1 / c2.A_0),2) - 1) * power(v2_real,2); % (104)
additional pressure chamber 1 (Bernoulli)
c2.dp2 = rho / 2 * (power((c2.A_2 / c2.A_0),2) - 1) * power(v2_real,2); % (104)
additional pressure chamber 2 (Bernoulli)
if x(11) >= 0 % positive flow x(11) = 0 redundant
delta_p1 = p_s - (x(9) - c2.dp1);
delta_p2 = (x(10) - c2.dp2) - p_0;
elseif x(11) < 0 % negative flow
114
% *************************************************************************
% compressibility of oil cylinder 1
115
% *************************************************************************
c1.E_1 = 0.5 * E_max * log10(C_1 * (abs(x(3)) / p_max) + C_2); % (96) bulk modulus of
oil [Pa]
c1.E_2 = 0.5 * E_max * log10(C_1 * (abs(x(4)) / p_max) + C_2); % (96) bulk modulus of
oil [Pa]
% *************************************************************************
% compressibility of oil cylinder 2
% *************************************************************************
c2.E_1 = 0.5 * E_max * log10(C_1 * (abs(x(9)) / p_max) + C_2); % (96) bulk modulus of
oil [Pa]
c2.E_2 = 0.5 * E_max * log10(C_1 * (abs(x(10)) / p_max) + C_2); % (96) bulk modulus of
oil [Pa]
% *************************************************************************
% compressibility of oil cylinder 3
% *************************************************************************
c3.E_1 = 0.5 * E_max * log10(C_1 * (abs(x(15)) / p_max) + C_2); % (96) bulk modulus
of oil [Pa]
c3.E_2 = 0.5 * E_max * log10(C_1 * (abs(x(16)) / p_max) + C_2); % (96) bulk modulus
of oil [Pa]
% *************************************************************************
% *************************************************************************
% chamber volumes cylinder 1
% *************************************************************************
c1.V_1 = c1.V_01 + c1.A_1 * x1_real; % (86)
c1.V_2 = c1.V_02 - c1.A_2 * x1_real; % (87)
% *************************************************************************
% chamber volumes cylinder 2
% *************************************************************************
c2.V_1 = c2.V_01 + c2.A_1 * x2_real; % (86)
c2.V_2 = c2.V_02 - c2.A_2 * x2_real; % (87)
% *************************************************************************
% chamber volumes cylinder 3
% *************************************************************************
c3.V_1 = c3.V_01 + c3.A_1 * x(13); % (86)
c3.V_2 = c3.V_02 - c3.A_2 * x(13); % (87)
% *************************************************************************
% *************************************************************************
% friction force cylinder 1
% *************************************************************************
c1.F_f = k_v * v1_real + sign(v1_real) * (F_c + F_s * exp(-abs(v1_real) / v_s)); %
friction force [N]
% *************************************************************************
% friction force cylinder 2
% *************************************************************************
c2.F_f = k_v * v2_real + sign(v2_real) * (F_c + F_s * exp(-abs(v2_real) / v_s)); %
friction force [N]
% *************************************************************************
% friction force cylinder 3
% *************************************************************************
c3.F_f = k_v * x(14) + sign(x(14)) * (F_c + F_s * exp(-abs(x(14)) / v_s)); %
friction force [N]
% *************************************************************************
% *************************************************************************
% spring force cylinder 1
% *************************************************************************
if x1_real > (c1.l_c / 2) - c1.l_k - (c1.l_p / 2);
c1.F_k = k * ((c1.l_c / 2) - x1_real - (c1.l_p / 2) - c1.l_k);
elseif x1_real < -(c1.l_c / 2) + c1.l_k + (c1.l_p / 2);
c1.F_k = k * (-c1.l_c / 2 - x1_real + c1.l_p / 2 + c1.l_k);
else
c1.F_k = 0;
end
% *************************************************************************
% spring force cylinder 2
% *************************************************************************
if x2_real > (c2.l_c / 2) - c2.l_k - (c2.l_p / 2);
c2.F_k = k * ((c2.l_c / 2) - x2_real - (c2.l_p / 2) - c2.l_k);
elseif x2_real < -(c2.l_c / 2) + c2.l_k + (c2.l_p / 2);
c2.F_k = k * (-c2.l_c / 2 - x2_real + c2.l_p / 2 + c2.l_k);
else
116
c2.F_k = 0;
end
% *************************************************************************
% spring force cylinder 3
% *************************************************************************
if x(13) > (c3.l_c / 2) - c3.l_k - (c3.l_p / 2);
c3.F_k = k * ((c3.l_c / 2) - x(13) - (c3.l_p / 2) - c3.l_k);
elseif x(13) < -(c3.l_c / 2) + c3.l_k + (c3.l_p / 2);
c3.F_k = k * (-c3.l_c / 2 - x(13) + c3.l_p / 2 + c3.l_k);
else
c3.F_k = 0;
end
% *************************************************************************
% Lagrange formulation
% *************************************************************************
M_real = M_function(x(1),x(7),x(13),f1,f2,f3);
h_real = h_function(x(1),x(2),x(7),x(8),x(13),x(14),f1,f2,f3);
q_pp = M_real\h_real; % (62)
% *************************************************************************
117
%
%
%
%
%
%
%
%
%
%
% tripod
% *************************************************************************
l0.p1 = [0;0;0]; % origin [m] (initial frame)
l0.p2 = [0;0;-1.21]; % floor [m]
% *************************************************************************
% link 1 (rotary)
% *************************************************************************
l1.p1 = [0;0;0]; % origin [m] (initial frame)
l1.p2 = [0;0;0.73]; % joint link 2 [m]
l1.p3 = [0.115;0;0.033]; % end cylinder 1 [m]
l1.cg = [0;0;0.73/2]; % center of gravity [m]
l1.m = 50; % mass [kg]
l1.I = [0 0 0;
0 0 0;
0 0 0]; % inertia tensor [kg m4]
% *************************************************************************
% link 2 (boom)
% *************************************************************************
l2.p1 = [0;0;0]; % joint link 1 [m]
l2.p2 = [0.199;0;-0.061]; % start cylinder 1 [m]
l2.p3 = [1.519;0;0.214]; % start cylinder 2 [m]
l2.p4 = [2.231;0;0.109]; % closed loop [m]
l2.p5 = [2.309;0;0.081]; % joint link 3 [m]
l2.cg = [1.085;0;0.082]; % center of gravity [m]
l2.m = 68.2; % mass [kg]
l2.I = [0.724 0
6.793;
0
118.644
0;
6.793 0
118.1]; % intertia tensor [kg m4]
l2.p4_p5 = l2.p5 - l2.p4; % vector between closed loop and joint link 3 [m]
l2.phi = atan(l2.p4_p5(3) / l2.p4_p5(1)); % angle between vector p4/p5 and x-axis(K2)
[rad] !negative!
% *************************************************************************
% link 3 (jib)
% *************************************************************************
l3.p1 = [0;0;0]; % joint link 2 [m]
l3.p2 = [-0.035;0;0.094]; % closed loop [m]
l3.p3 = [0.255;0;0.207]; % start cylinder 3 [m]
l3.p4 = [0.98;0;0]; % x-end of link 3 [m]
l3.cg = [0.403;0;0.097]; % center of gravity [m]
l3.m = 27.54; % mass [kg]
l3.I = [0.367 0
1.124;
0
7.9 0;
1.124 0
7.638]; % inertia tensor [kg m4]
l3.phi = atan(l3.p2(1) / l3.p2(3)); % angle between x-axis(K3) and l3_p2 [rad]
!negative!
% *************************************************************************
% link 4 (extension)
% *************************************************************************
l4.p1 = [1.136;0;-0.051]; % crane tip [m]
l4.p2 = [1.058;0;0.107]; % end cylinder 3 [m]
l4.cg = [0.678;0;0.004]; % center of gravity [m]
l4.m = 10.11; % mass [kg]
l4.I = [0.019 0
0.044
118
0
5.942 0
0.044 0
5.935]; % interia tensor [kg m4]
% *************************************************************************
% torque links in K2
% *************************************************************************
cl.r1 = [0.182;0;0]; % vector torque link 1 [m]
cl.r2 = [0.194;0;0]; % vector torque link 2 [m]
% *************************************************************************
% specification of cylinder 1
% *************************************************************************
c1.o_s = 0.05; % offset start [m]
c1.o_e = 0.01; % offset end [m]
c1.l_c = 0.43; % length cylinder [m]
c1.l_p = 0.1; % length piston [m]
c1.l_r = 0.31; % length rod [m]
c1.l_0 = 0.02; % length dead zone [m]
c1.l_cw = 0.01; % thickness of cylinder body [m]
c1.d_c = 0.1; % diameter cylinder [m]
c1.d_r = 0.04; % diameter rod [m]
c1.d_0 = 0.01; % diameter dead zone [m]
c1.r_c = 0.0001; % leakage radius [m]
c1.e = 0; % initial control error (deviation of norm)
c1.e_i = 0; % initial integral error at t
c1.e_1 = 0; % initial integral error at t_-1
c1.e_2 = 0; % initial integral error at t_-2
c1.u = 0; % initial control input
% *************************************************************************
% specification of cylinder 2
% *************************************************************************
c2.o_s = 0.035; % offset start [m]
c2.o_e = 0.095; % offset end [m]
c2.l_c = 0.445; % length cylinder body [m]
c2.l_p = 0.1; % length piston [m]
c2.l_r = 0.31; % length rod [m]
c2.l_cw = 0.01; % thickness of cylinder [m]
c2.d_c = 0.09; % diameter cylinder [m]
c2.d_r = 0.045; % diameter rod [m]
c2.d_0 = 0.01; % diameter dead zone [m]
c2.r_c = 0.0001; % leakage radius [m]
c2.e = 0; % initial control error (deviation of norm)
c2.e_i = 0; % initial integral error at t
c2.e_1 = 0; % initial integral error at t_-1
c2.e_2 = 0; % initial integral error at t_-2
c2.u = 0; % initial control input
% *************************************************************************
% specification of cylinder 3
% *************************************************************************
c3.o_s = 0.04; % offset start [m]
c3.o_e = 0.04; % offset end [m]
c3.l_c = 0.71; % length cylinder [m]
c3.l_p = 0.08; % length piston [m]
c3.l_r = 0.71; % length rod [m]
c3.l_cw = 0.01; % thickness of cylinder body [m]
c3.d_c = 0.05; % diameter cylinder [m]
c3.d_r = 0.03; % diameter rod [m]
c3.d_0 = 0.01; % diameter dead zone [m]
c3.r_c = 0.0001; % leakage radius [m]
c3.e = 0; % initial error (deviation of norm)
c3.e_i = 0; % initial integral error at t
c3.e_1 = 0; % initial integral error at t_-1
c3.e_2 = 0; % initial integral error at t_-2
c3.u = 0; % initial control input
% *************************************************************************
119
x3 ) , its derivatives (
T
stroke x p along with its velocity v p . At the 3rd cylinder, x p and v p are obviously the same
as the generalized coordinates x3 and v3 respectively. Finally, at the end of the plot call, the
animation of motion of the crane will be simulated (
120
121
In the second section the call command for the crane animation is listed. The entire crane is
drawn by line-representation of all crane elements. Finally the vector of generalized
coordinates is used to determine the position and direction of all elements for sequential
processing of the animation (see Figure 89)
function PLOT_CHARTS(t,x,c1,c2,c3,cl,l0,l1,l2,l3,l4,t_step,t_end)
% cylinder 1
% *************************************************************************
figure;
% chart 1 - alpha
subplot(3,2,1);
plot(t,x(:,1)*180/pi);
xlabel('time [sec]')
ylabel('\alpha [deg]')
grid on
% chart 2 - w_alpha
subplot(3,2,2);
plot(t,x(:,2)*180/pi);
xlabel('time [sec]')
ylabel('\omega_\alpha [deg/sec]')
grid on
% chart 3 - pressure 1
subplot(3,2,3);
122
plot(t,x(:,3));
xlabel('time [sec]')
ylabel('p_1 [Pa]')
grid on
% chart 4 - pressure 2
subplot(3,2,4);
plot(t,x(:,4));
xlabel('time [sec]')
ylabel('p_2 [Pa]')
grid on
% chart 5 - spool stroke
subplot(3,2,5);
plot(t,x(:,5));
xlabel('time [sec]')
ylabel('x_s [m]')
grid on
% chart 6 - spool velocity
subplot(3,2,6);
plot(t,x(:,6));
xlabel('time [sec]')
ylabel('v_s [m/s]')
grid on
% cylinder 2
% *************************************************************************
figure
% chart 1 - beta
subplot(3,2,1);
plot(t,x(:,7)*180/pi);
xlabel('time [sec]')
ylabel('\beta [deg]')
grid on
% chart 2 - beta_p
subplot(3,2,2);
plot(t,x(:,8)*180/pi);
xlabel('time [sec]')
ylabel('\omega_\beta [deg/sec]')
grid on
% chart 3 - pressure 1
subplot(3,2,3);
plot(t,x(:,9));
xlabel('time [sec]')
ylabel('p_1 [Pa]')
grid on
% chart 4 - pressure 2
subplot(3,2,4);
plot(t,x(:,10));
xlabel('time [sec]')
ylabel('p_2 [Pa]')
grid on
% chart 5 - spool stroke
subplot(3,2,5);
plot(t,x(:,11));
xlabel('time [sec]')
ylabel('x_s [m]')
grid on
% chart 6 - spool velocity
subplot(3,2,6);
plot(t,x(:,12));
xlabel('time [sec]')
ylabel('v_s [m/s]')
grid on
% cylinder 3
% *************************************************************************
123
figure
% chart 1 - cylinder stroke
subplot(3,2,1);
plot(t,x(:,13));
xlabel('time [sec]')
ylabel('x_p [m]')
grid on
% chart 2 - cylinder velocity
subplot(3,2,2);
plot(t,x(:,14));
xlabel('time [sec]')
ylabel('v_p [m/s]')
grid on
% chart 3 - pressure 1
subplot(3,2,3);
plot(t,x(:,15));
xlabel('time [sec]')
ylabel('p_1 [Pa]')
grid on
% chart 4 - pressure 2
subplot(3,2,4);
plot(t,x(:,16));
xlabel('time [sec]')
ylabel('p_2 [Pa]')
grid on
% chart 5 - spool stroke
subplot(3,2,5);
plot(t,x(:,17));
xlabel('time [sec]')
ylabel('x_s [m]')
grid on
% chart 6 - spool velocity
subplot(3,2,6);
plot(t,x(:,18));
xlabel('time [sec]')
ylabel('v_s [m/s]')
grid on
% *************************************************************************
% animation
% *************************************************************************
figure
% cylinder 1 specs
cylinder1 = drahtmodell_rechteck_xz(c1.l_c/2,c1.d_c/2);
piston1 = drahtmodell_rechteck_xz(c1.l_p/2,c1.d_c/2);
rod1 = drahtmodell_rechteck_xz(c1.l_r/2,c1.d_r/2);
% cylinder 2 specs
cylinder2 = drahtmodell_rechteck_xz(c2.l_c/2,c2.d_c/2);
piston2 = drahtmodell_rechteck_xz(c2.l_p/2,c2.d_c/2);
rod2 = drahtmodell_rechteck_xz(c2.l_r/2,c2.d_r/2);
% cylinder 3 specs
cylinder3 = drahtmodell_rechteck_xz(c3.l_c/2,c3.d_c/2);
piston3 = drahtmodell_rechteck_xz(c3.l_p/2,c3.d_c/2);
rod3 = drahtmodell_rechteck_xz(c3.l_r/2,c3.d_r/2);
% cylinder mount
c1_mount_s = {[0 0 0
c1.o_s 0 0]};
c1_mount_e = {[0 0 0
-c1.o_e 0 0]};
c2_mount_s = {[0 0 0
c2.o_s 0 0]};
124
c2_mount_e = {[0 0 0
-c2.o_e 0 0]};
c3_mount_s = {[0 0 0
c3.o_s 0 0]};
c3_mount_e = {[0 0 0
c3.o_e 0 0]};
% crane specs
link_0 = {[l0.p1';
l0.p2']};
link_1 = {[l1.p1';
l1.p2'];
[0 0 l1.p3(3);
l1.p3']};
link_2 = {[l2.p1';
l2.p5(1)
[l2.p2';
l2.p2(1)
[l2.p3';
l2.p3(1)
[l2.p4';
l2.p4(1)
[l2.p5';
l2.p5(1)
0 0];
0 0];
0 0]
0 0];
0 0]};
link_3 = {[l3.p4';
l3.p2(1) 0 0];
[l3.p2';
l3.p2(1) 0 0];
[l3.p3';
l3.p3(1) 0 0]};
link_4 = {[0 0 0;
l4.p1(1) 0 0;
l4.p1'];
[l4.p2'
l4.p2(1) 0 0]};
cl_1 =
{[0 0 0;
cl.r1']};
cl_2 =
{[0 0 0;
cl.r2']};
for i=1:length(t)
l_x1 = c1.fhandle_x(x(i,1));
l_x2 = c2.fhandle_x(x(i,1),x(i,7));
A_IK1 = eye(3);
A_IK2 = A_IK1 * A_y(pi / 2 - x(i,1));
A_IK3 = A_IK1 * A_y(pi / 2 - x(i,1)) * A_y(pi - x(i,7));
A_Ic1 = A_y(pi / 2 + c1.fhandle_phi(x(i,1))); % angle cylinder 1 related to inertia
frame
A_Ic2 = A_IK2 * A_y(-c2.fhandle_phi(x(i,1),x(i,7))); % angle cylinder 2 / link 2
A_It1 = A_IK2 * A_y(- cl.fhandle_eps1(x(i,1),x(i,7))); % angle torque 1 related to
inertia frame
A_It2 = A_IK2 * A_y(- cl.fhandle_eps1(x(i,1),x(i,7)) - cl.fhandle_teta2(x(i,1),x(i,7))
+ pi); % angle torque 2 related to inertia frame
c1.center_c = A_IK1 * l1.p2 + A_IK2 * l2.p2 + A_Ic1 * [c1.o_s + c1.l_c / 2;0;0];
c1.center_p = c1.center_c + A_Ic1 * [l_x1;0;0];
c1.center_r = c1.center_p + A_Ic1 * [(c1.l_p + c1.l_r) / 2;0;0];
c2.center_c = A_IK1 * l1.p2 + A_IK2 * l2.p3 + A_Ic2 * [c2.o_s + c2.l_c / 2;0;0];
c2.center_p = c2.center_c + A_Ic2 * [l_x2;0;0];
c2.center_r = c2.center_p + A_Ic2 * [(c2.l_p + c2.l_r) / 2;0;0];
125
c3.center_c = A_IK1 * l1.p2 + A_IK2 * l2.p5 + A_IK3 * l3.p3 + A_IK3 * [c3.o_s + c3.l_c
/ 2;0;0];
c3.center_p = c3.center_c + A_IK3 * [x(i,13);0;0];
c3.center_r = c3.center_p + A_IK3 * [(c3.l_p + c3.l_r) / 2;0;0];
x_00(i,:) = [0 0 0 1 0 0 0 1 0 0 0 1];
x_01(i,:) = [x_00(i,1:3) + (l1.p1)' A_IK1(1,:) A_IK1(2,:) A_IK1(3,:)];
x_02(i,:) = [x_01(i,1:3) + (A_IK1 * l1.p2)' A_IK2(1,:) A_IK2(2,:) A_IK2(3,:)];
x_03(i,:) = [x_02(i,1:3) + (A_IK2 * l2.p5)' A_IK3(1,:) A_IK3(2,:) A_IK3(3,:)];
x_04(i,:) = [x_rod_3(i,1:3) + (A_IK3 * ([c3.l_r / 2 + c3.o_e;0;0] - l4.p2))'
A_IK3(1,:) A_IK3(2,:) A_IK3(3,:)];
x_cl_1(i,:) = [x_02(i,1:3) + (A_IK2 * l2.p4)' A_It1(1,:) A_It1(2,:) A_It1(3,:)];
x_cl_2(i,:) = [x_cl_1(i,1:3) + (A_It1 * cl.r1)' A_It2(1,:) A_It2(2,:) A_It2(3,:)];
x_c_1_mount_s(i,:) = [x_02(i,1:3) + (A_IK2 * l2.p2)' A_Ic1(1,:) A_Ic1(2,:)
A_Ic1(3,:)];
x_c_1_mount_e(i,:) = [x_01(i,1:3) + (A_IK1 * l1.p3)' A_Ic1(1,:) A_Ic1(2,:)
A_Ic1(3,:)];
x_c_2_mount_s(i,:) = [x_02(i,1:3) + (A_IK2 * l2.p3)' A_Ic2(1,:) A_Ic2(2,:)
A_Ic2(3,:)];
x_c_2_mount_e(i,:) = [x_cl_2(i,1:3) A_Ic2(1,:) A_Ic2(2,:) A_Ic2(3,:)];
x_c_3_mount_s(i,:) = [x_03(i,1:3) + (A_IK3 * l3.p3)' A_IK3(1,:) A_IK3(2,:)
A_IK3(3,:)];
x_c_3_mount_e(i,:) = [x_rod_3(i,1:3) + (A_IK3 * [c3.l_r / 2;0;0])' A_IK3(1,:)
A_IK3(2,:) A_IK3(3,:)];
end
CRANE_ANIMATION({x_00 link_0 4;...
x_01 link_1 4;...
x_02 link_2 4;...
x_03 link_3 4;...
x_04 link_4 4;...
x_cl_1 cl_1 5;...
x_cl_2 cl_2 5;...
x_c_1 cylinder1 6;...
x_piston_1 piston1 5;...
x_rod_1 rod1 6;...
x_c_2 cylinder2 6;...
x_piston_2 piston2 5;...
x_rod_2 rod2 6;...
x_c_3 cylinder3 6;...
x_piston_3 piston3 5;...
x_rod_3 rod3 6;...
x_c_1_mount_s c1_mount_s 6;...
x_c_1_mount_e c1_mount_e 6;...
x_c_2_mount_s c2_mount_s 6;...
x_c_2_mount_e c2_mount_e 6;...
x_c_3_mount_s c3_mount_s 6;...
x_c_3_mount_e c3_mount_e 6},t_step,t_end,[0,0]);
% *************************************************************************
End
126
127
Appendix
9 Appendix
9.1 Data sheets
128
Appendix
Figure 91: Data sheet of HBM load sensor U9B (50 kN)
129
Appendix
Figure 92: Data sheet of HBM load sensor U9B (10 kN)
130
Appendix
131
Appendix
132
Appendix
9.2 CD-Rom
9.2.1 Content
THESIS
- Thesis_Modelling_Simulation_And_Control_Of_A_Hydraulic_Crane.doc: Word-document of the thesis work
- Thesis_Modelling_Simulation_And_Control_Of_A_Hydraulic_Crane.pdf: PDF-document of the thesis work
MATLAB
- A_x.m / A_y.m / A_z.m: Rotational matrix about x-/ y-/ and z-axis
- ANGLE_LIMIT_ALPHA.m: Correlation between joint variable alpha and piston stroke of cylinder 1
- CRANE_ANIMATION.m: Animation of the crane
- CRANE_MAIN.m: Main functionf for crane simulation
- CYLINDER.m: Test file for cylinder simulation
- CYLINDER_SIMULINK.mdl: Corresponding test model for cylinder simulation
- DIMENSION.m: Crane dimensions
- PLOT_CHARTS.m: Charts of cylinder sepcification
- TRANSITION_FLOW.m: Transition phase of laminar and turbulent pressure
- WORKING_AREA.m: Operating range of the crane
CAD
- 38652rev2.sldasm: Solid Assembly file of the laboratory crane
FRICTION
- CYLINDER KALMAR: MATLAB- and dSPACE- files of the Kalmar cylinder
- CYLINDER ROTTNE: MATLAB- and dSPACE- files of the Rottne cylinder
133
Bibliography
10 Bibliography
[1]
U. Mettin, P.X. Miranda la Hera. Modelling and Control Design for a Hydraulic Forestry Crane.
Department of Applied Physics and Electronics, Ume University, Sweden, 2005.
[2]
[3]
B. ulc, J.A. Jan. Non Linear Modelling and Control of Hydraulic Actuators. Acta Polytechnica
Vol.42 No.3/2002, pages 41 47, Prague, Czech Republic, 2002.
[4]
M.W. Spong, S. Hutchinson, M. Vidyasagar. Robot Modeling And Control. John Wiley & Sons,
Inc., New Jersey, USA, 2006.
[5]
[6]
[7]
[8]
[9]
[10] Prof. Dr. P. Wolfsteiner. Mehrkrpersysteme. Fakultt 03, Fachhochschule Mnchen, Germany,
2003.
[11] H.J. Bartsch. Taschenbuch mathematischer Formeln. Fachbuchverlag Leipzig, Germany, 2004.
[12] LPA. Kranstyrning SMST. Rottne Industri AB, Sweden, 2006.
[13] http://www.physik.uni-bielefeld.de/theory/cm/research/projects/friction
[14] C. Canudas de Wit, P. Lischinsky. Adaptive friction compensation with partially known dynamic
friction model. International journal of adaptive control and signal processing, Vol.11, pages
65 80, Grenoble, France, 1997.
134
Bibliography
[15] http://www.rottne.com/prod/bilder/stor-smv-rapid.jpg
[16] H. Olsson, K.J. strom, C. Canudas de Wit, M. Grfvert, P. Lischinsky. Friction Models and
Friction Compensation. European Journal of Control 1998 (4), pages 176-195, UK, 1998.
[17] B. Armstrong-Hlouvry, P. Dupont, C. Canudas de Wit. A survey of models, analysis tools and
compensation methods for the control of machines with friction. Automatica 1994 30(7), pages
1083 1138, UK, 1994.
135