Advanced Strategies For Robot Manipulators
Advanced Strategies For Robot Manipulators
SCIYO
Advanced Strategies for Robot Manipulators
Edited by Seyed Ehsan Shafiei
Published by Sciyo
Janeza Trdine 9, 51000 Rijeka, Croatia
All chapters are Open Access articles distributed under the Creative Commons Non Commercial Share
Alike Attribution 3.0 license, which permits to copy, distribute, transmit, and adapt the work in any
medium, so long as the original work is properly cited. After this work has been published by Sciyo,
authors have the right to republish it, in whole or part, in any publication of which they are the author,
and to make other personal use of the work. Any republication, referencing or personal use of the work
must explicitly identify the original source.
Statements and opinions expressed in the chapters are these of the individual contributors and
not necessarily those of the editors or publisher. No responsibility is accepted for the accuracy of
information contained in the published articles. The publisher assumes no responsibility for any
damage or injury to persons or property arising out of the use of any materials, instructions, methods
or ideas contained in the book.
Amongst the robotic systems, robot manipulators have proven themselves to be of increasing
importance and are widely adopted to substitute humans in repetitive and/or hazardous
tasks. Modern manipulators have a complicated design and need to do more precise, crucial
and critical tasks. So, the simple traditional control methods cannot be efficient, and advanced
control strategies with considering special constraints need to be established. In spite of the
fact that groundbreaking researches have been carried out in this realm until now, there are
still many novel aspects which have to be explored.
This book consists of a set of materials that introduces various strategies related to robot
manipulators. Although the topics provided here are not in a rational order, they can be divided
into three major subjects such as design and modelling, control strategies and applications of robot
manipulators. These subjects cover different approaches like dynamic modelling, redundant
manipulators, micro-manipulator, parallel manipulator, nonlinear control, intelligent control
and many other valuable matters that are addressed here by different authors through 19
chapters.
Editor
1. Introduction
1.1 Overview
In this book chapter a comparative assessment of modelling and control of mechanical
manipulator is considered. First, kinematic and dynamic modelling of wide range of
mechanical manipulators comprising flexible link, flexible joint and mobile manipulators are
considered. Then, open-loop optimal control problem is formulated to control of the
obtained system. Finally, some applications of method including motion planning and
maximum payload determination are illustrated through the computer simulations.
1.3 Motivation
Unfortunately, reviewing of the recent literature on modelling and optimization of flexible
and mobile manipulators shows that a very scant attention has been paid to study of model
that describes both link and joint flexibility, particularly for mobile manipulators. The main
motivation for this study is to present a comprehensive modelling and optimal control of
flexible link-joint mechanical mobile manipulators. It can provide an inclusive reference for
other researchers with comparative assessment view in the future studies.
2 Advanced Strategies for Robot Manipulators
Mobile manipulators have recently received considerable attention with wide range of
applications mainly due to their extended workspace and their ability to reach targets that
are initially outside of the manipulator reach. A comprehensive literature survey on mobile
manipulator systems can be found (Bloch, 2003). A host of issues related to mobile
manipulators have been studied in the past two decade. These include for example:
dynamic and static stability (Papadopoulos & Rey, 1996), force development and application
(Papadopoulos & Gonthier, 1999), maximum payload determination (Korayem & Ghariblu,
2004). However, a vast number of research publications that deal with the mobile
manipulators focus on techniques for trajectory planning of such robots (Korayem & Rahimi
Nohooji, 2008).
Motion planning for mobile manipulators is concerned with obtaining open-loop or close-
loop controls. It steers a platform and its accompanying manipulator from an initial state to
a final one, without violating the nonholonomic constraints (Sheng & Qun, 2006). In most
studies of trajectory planning for mobile manipulators the end effector trajectory is specified
and the optimal motion planning of the base is considered (Mohri et al., 2001), or integrated
motion planning of the base and the end effector is carried out (Papadopoulos, et al., 2002).
However, because of designing limitation or environmental obstacle in majority of practical
application of mobile manipulators especially in repetitive applications, the platform must
follow a specified pose trajectory. In this case, designer must control the joint motions to
achieve the best dynamic coordination that optimize the defined cost function such as
energy consumption, actuating torques, traveling time or bounding the velocity
magnitudes. Applications for such systems abound in mining, construction or in industrial
factories.
Optimal control problems can be solved with direct and indirect techniques. In the direct
method at first the control and state variables are discretized and the optimal control
problem is transcribed into a large, constrained and often sparse nonlinear programming
problem, then, the resulting nonlinear programming problem is treated by standard
algorithm like interior point methods (Wachter & Biegler, 2006). Famous realizations of
direct methods are direct shooting methods (Bock & Plitt, 1984) or direct collocation
methods (Hargraves & Paris, 1987). However, direct methods are not yield to exact results.
They are exhaustively time consuming and quite inefficient due to the large number of
parameters involved. Consequently, when the solution of highly complex problems such as
the structural analysis of optimal control problems in robotics is required, the indirect
method is a more suitable candidate. This method is widely used as an accurate and
powerful tool in analyzing of the nonlinear systems. The indirect method is characterized by
a ''first optimize, then discretize'' strategy. Hence, the problem of optimal control is first
transformed into a piecewise defined multipoint boundary value problem, which contains
the full mathematical information about the respective optimal control problem. In the
following step, this boundary value problem is discretized to achieve the numerical solution
(Sentinella & Casalino, 2006). It is well known that this technique is conceptually fertile, and
has given rise to far-reaching mathematical developments in the wide ranges of optimal
dynamic motion planning problems. For example, it is employed in the path planning of
flexible manipulators (Rahimi et.al, 2009), for the actuated kinematic chains (Bessonnet &
Chessé, 2005) and for a large multibody system (Bertolazzi et al., 2005). A survey on this
method is found in (Callies & Rentrop, 2008).
4 Advanced Strategies for Robot Manipulators
1.5 Layout
The balance of the remaining of the chapter is organized as follows. Section 2 provides
background information about kinematic and dynamic analysis of the flexible mobile
robotic manipulators. Hence, assumed mode and finite element methods are introduced and
formulated to dynamic modelling of flexible link manipulators. Then, the flexible model is
completed by adding the joint flexibility. After that, formulation is extended to comprise the
mobile manipulators. Section 3 consists of a brief review of converting the problem from
optimal control to optimization procedure with implementing of Pontryagin’s minimum
principle. some application examples with the two links flexible mobile manipulator is
detailed in this section. Finally, the concluding remarks with a brief summary of the chapter
is presented in the last section.
2. System modelling
2.1 Kinematic analysis
A mobile manipulator consisting of differentially driven vehicle with n flexible links and n
flexible revolute joints is expressed in this section (Fig. 1). The links are cascaded in a serial
fashion and are actuated by rotors and hubs with individual motors. The flexible joints are
dynamically simplified as a linear torsional springs that works as a connector between the
rotors and the links. A concentrated payload of mass mp is connected to the distal link.
Payload
θn
rpayload
...
Link-n
θi
rn ( FJ) n
Link-1
...
( FJ) i +1
θ2 Link-i
ri ( FJ) i
θ0 (FJ)2
Y
r0 r1 θ1
( FJ)1
Z
Fig. 1. A schematic view of a multiple flexible links – joints mobile manipulator
The following assumptions are made for the development of a dynamic model of the
system.
• Each link is assumed to be long and slender.
• The motion of each link and its deformation is supposed to be in the horizontal plane.
• Links are considered to have constant cross-sectional area and uniform material
properties.
The Comparative Assessment of Modelling and Control of Mechanical Manipulator 5
ni
vi ( xi , t ) = ∑ ϕij ( xi )eij (t ) i = 1, . . . , n (1)
j =1
In addition, mode shape functions with clamped-pinned boundary conditions are given by
ϕi ( xi ) = Ai sin( Bi . xi )
cosh( Bi .Li )
Ai = (5)
cos( Bi .Li )
Bi .Li : 3.14 6.28 9.42 12.56.
Choosing the appropriate set of assumed modes as a boundary condition may be quite
valuable for robot to fit in a suitable application. Ideally, the optimum set of assumed modes is
that closest to natural modes of the system. Natural modes depend on several factors within
the robotic system ensemble including size of hub inertia and size of payload mass. For large
joint gearing inertia and relatively small payload mass, the link may be considered clamped at
the joint. Conversely, for smaller joint gearing inertia and larger payload mass both ends of the
link may be considered pinned. The ultimate choice requires an assessment based on the
actual robot structure and anticipated range of payloads together with its natural modes.
Although assume mode method has been widely used, there are several ways to choose link
boundary conditions and mode eigen-functions. This drawback may increase drastically
when finding modes for links with non-regular cross sections and multi-link manipulators is
objected. In addition, using the AMM to derive the equations of motion of the flexible
manipulators, only the first several modes are usually retained by truncation and the higher
modes are neglected.
2.2.1.2 Finite element method
The finite element method is broadly used to derive dynamic equations of elastic robotic
arms. Researcher usually used the Euler–Bernoulli beam element with multiple nodes and
Lagrange shape function to achieve the reasonable finite element model. The node number
can be selected according to requirement on precision. But, increasing the node number may
enlarge the stiffness matrix and it cause to long and complex equations. Hence, choosing the
proper node number is very important in the finite element analyzing.
The overall finite element approach involves treating each link of the manipulator as an
assemblage of n elements of length Li. For each of these elements the kinetic energy Tij and
potential energyVij, are computed in terms of a selected system of generalized coordinate q
and their rate of change q . Note that subscript ij refer to the j th element of link i.
In summary the kinetic energy Tij and potential energy Vij are computed by the following
equation:
1 li ⎡ ∂riT ∂ri ⎤
2 ∫0
Tij = mi ⎢ ⋅ ⎥dxij (6)
⎣ ∂t ∂t ⎦
And
2 ∫0
dx
⎥ ij + EI i ⎢ 2 ⎥
dxij
0
⎢⎣ yij ⎥⎦ ⎢⎣ ∂xij ⎥⎦
In above equation, the potential energy is consisted of two parts. One part is due to gravity
(Vgij) and another is related to elasticity of links (Veij). ri, mi, li and EIi are the position, mass,
length and the flexural rigidity of ith element respectively. xij and yij are specified the
distances along body- fixed system OijXijYij from common junction between elements ‘i(j-1)’
8 Advanced Strategies for Robot Manipulators
⎡ cos(θ 1 ) − sin(θ 1 )⎤
and ‘ij’ of link i. T01 = ⎢ ⎥ is transformation matrix from body-fixed system
⎣ cos(θ 1 ) sin(θ 1 ) ⎦
attached to link 1 to inertial system of coordinates and θ1 is it’s correlated joint angle. These
energies of elements are then combined to obtain the total kinetic energy T, and potential
energy V, for the each link. Knowledge of the kinetic and potential energies is tantamount to
specify the Lagrangian £ of the system, given by £=T-V. Using of finite element method in
modelling of the robotics system are details in (Usoro, 1986).
As it can be seen, modelling of flexural vibrations of robotic elements using finite element is
a well-established technique. So, researchers can handle nonlinear conditions with this
method. However, in order to solve a large set of differential equations derived by the finite
element method, a lot of boundary conditions have to be considered, which are, in most
situations, uncertain for flexible manipulators. Also, although significant advantages of FEM
over analytical solution techniques such as easy to handle with which nonlinear conditions,
this approach seems more complex over AMM. The main reason is that use of the finite
element model to approximate flexibility usually gives rise to an overestimated stiffness
matrix. Moreover, because of the large number of equations, the numerical simulation time
may be exhausting for the finite element models.
2.2.1.3 Numerical simulations
The dynamic equations of the flexible robotic arms are verified in this section by
undertaking a computer simulation. Hence, the case of harmonic motion of a nonlinear
model of flexible robotic arms is selected to simulation. In this simulation, the robot is
hanged freely and it influenced only under gravity effect. The physical parameters of the
system used in this simulation study were L1 = L2 = 1 m , I 1 = I 2 = 5 × 10 −9 m 4 , m1 = m2 = 5 kg
and E1 = E2 = 2 × 10 11 N / m2 . Simulating both FEM and AMM (pinned-pinned and clamped-
pinned) models and comparing them with the rigid links in this simulation shows the
oscillatory behavior of nonlinear robotic system advisably.
Now, considering the equations describe in the last section for FEM and AMM, also, using
Lagrangian formulation, the set of equation of motion for each method is derived in
compact form as
M ( q ) q + H (q , q ) = U (8)
where M is the inertia matrix, H is the vector of Coriolis and centrifugal forces in addition to
the gravity effects vector and U is the generalized force vector inserted into the actuator.
Open loop system response of changing the initial condition from normal equilibrium
position to the relative angle between the first and second link of this system (θ2) to the
deviation of 5 degree is studied in this simulation (Fig. 2).
The responses of the system are presented in Figs. 3-5. Figures show the difference between
rigid and flexible robotic arms also between the FEM and AMM with both pinned- pinned
and clamped- pinned boundary conditions.
Figs. 3 and 4 show the angular positions and angular velocities of joints. It is obvious from
figures that the link elasticity appears in velocity graph more and more than the position
graph. Also, these figures restate the issue that the FEM model displays the nonlinearity of
the system properly.
The corresponding amplitudes of vibration modes in the AMM are shown in Fig. 5. It is
clear that link flexibility significantly affects the link vibrations. In addition, pictures shows
that these effects are appeared more when clamped – pinned boundary condition is
The Comparative Assessment of Modelling and Control of Mechanical Manipulator 9
considered. Figures are plotted in this section clearly show a good agreement between the
obtained results in this study and those presented in (Usoro, 1986).
X
θ1 (0) = −90
Link - 1
Link - 2
θ2 (0) = 5
-0.02 0.084
-1.6
-1.5235
-0.04 0.083
-1.62
-1.524 -0.06 0.082
-1.64
-0.08 0.081
-1.5245 2.6 2.62 2.64 2.66
2.18 2.2 2.22 2.24
-1.66 -0.1
0 1 2 3 4 5 0 1 2 3 4 5
Time (s) Time (s)
(a) (b)
Fig. 3. Angular position of joints: (a) joint 1; (b) joint 2.
0.3 0.8
FEM FEM
Angular Velocity-Joint 2 (rad/s)
Angular Velocity-Joint 1 (rad/s)
-4 -5
x 10 x 10
1 5
Mode Shape - Link 1 (m)
0 0
-0.5
P-P P-P
C-P
C-P
-1
0 1 2 3 4 5 -5
0 1 2 3 4 5
Time (s)
Time (s)
(a) (b)
So by adding the joint flexibility with considering the elastic mechanical coupling between
the i th joint and link is modeled as a linear torsional spring with constant stiffness coefficient
ki , the set of equation of motion comprising mobile base with both link and joint flexibility
can be rearranged into the following form:
where K=diag[ k1 , k2 ,..., km ] is a diagonal stiffness matrix which models the joint elasticity,
J=diag[ J 1 , J 2 ,..., J m ] is the diagonal matrix representing motor inertia.
A simulation is performed to investigate the effect of joint flexibility on the response of
model by adding the elasticity at each joint as a linear spring. The case study with clamped-
pinned boundary condition is modeled for that issue. Simulation is done at the overall time
5 seconds. Parameter values of joints are k1 = k2 = 1500 N.m and J 1 = J 2 = 2 kg. m2 .
As shown in Fig. 6 the joint flexibility has considerable consequences on the robot behavior
and link parameters have significant deviations from rotor’s one. Hence, it can be conclude
that the joint flexibility, considerably influences the performance of robotic arms and it can
The Comparative Assessment of Modelling and Control of Mechanical Manipulator 11
-89 5
-89.5 3
2
4.9
-90 1
-89.08
4.8
-89.09
0
-90.5
-1 4.7
-89.1
-2 4.6
-89.11
-91
-3
Link 4.5
-89.12 Link
Motor 3 3.05 3.1 3.15 2.3 2.4 2.5
-4 Motor
-91.5
0 1 2 3 4 5 -5
Time (s) 0 1 2 3 4 5
Time (s)
(a)
0.15
Angular Velocity - First Link & Motor (rad/s)
Link 0.5
Motor Link
0.4 Motor
0.1
0.3
0.2
0.05
0.1
0
0
-0.1
-0.2
-0.05
-0.3
-0.4
-0.1
0 1 2 3 4 5
-0.5
Time (s) 0 1 2 3 4 5
Time (s)
(b)
Fig. 6. Effect of joint flexibility in (a) Position and (b) Velocity of joints
The system dynamics can also be decomposed into two parts: one is corresponding to
redundant set of variables, qr and the remained set of them, qnrf . That is,
⎡ Mr , r Mr , nrf ⎤ ⎡ qr ⎤ ⎡ C r + Gr ⎤ ⎡ U r ⎤
⎢M + = (11)
⎣ r , nrf Mnrf , nrf ⎥⎦ ⎢⎣qnrf ⎥⎦ ⎢⎣C nrf + Gnrf ⎥⎦ ⎢⎣U nrf ⎥⎦
where by considering the second row in order to path optimization procedure leads to
Using redundancy resolution qr will be obtained as a known vector in terms of the time (t).
Therefore A is obtained as a function of time and qnrf and B as a function of time, qr and
qnrf .
By defining the state vector as
X = [ X1 X 2 ] = ⎡⎣qnrf
T T
qnrf ⎤⎦ , (13)
⎡X ⎤ ⎡ X2 ⎤ ⎡ F1 ⎤
X = ⎢ 1⎥ = ⎢ ⎥=⎢ ⎥, (14)
⎣ X 2 ⎦ ⎣ N ( X ) + D( X )U ⎦ ⎣F2 ⎦
3. Optimal control
3.1 Defining the optimal control problem
Pontryagin's minimum principle provides an excellent tool to calculate optimal trajectories
by deriving a two-point boundary value problem. Let the trajectory generation problem be
defined here as determining a feasible specification of motion, which will cause the robot to
move from a given initial state to a given final state. The method presented in this article
adapts in a straightforward manner to the generation of such dynamic profiles.
There are known that nonlinear system dynamics stated as Eq. (14) be expressed in the term
of states (X), controls (U) and time (t) as
X = f ( X ,U , t ) (15)
tf
where the function L may be specified in quite varied manners. There are initial and
terminal constraints on the states:
X (t0 ) = X 0 X (t f ) = X f (17)
There may also be certain pragmatic constraints (reflecting such concerns as limited actuator
power) on the inputs. For example:
U (t ) ≤ Umax (t ) (18)
H ( X ,U , Ψ , mp , t ) = L( X ,U , mp , t ) + Ψ T f ( X ,U , t ) (19)
T
where Ψ(t ) = ⎡⎣ψ 1 (t )T ψ 2 (t )T ⎤⎦ is the nonzero costate time vector-function.
Finally, according to the aforementioned principle, stating the costate vector-equation
Ψ T = −∂H ∂X (20)
∂H ∂U = 0 (21)
X = ∂H ∂Ψ , (22)
leads to transform the problem of optimal control into a non-linear multi-point boundary
value problem.
Consequently, for a specified payload value, substituting obtained computed control
equations from Eqs. (21) and Eq. (18) into Eqs. (20) and (22), sixteen nonlinear ordinary
differential equations are obtained which with sixteen boundary conditions given in Eq. (17),
constructs a Two Point Boundary Value Problem(TPBVP). Such a problem is solvable with
available commands in different software such as MATLAB and MATEMATHICA.
3.2 Application
3.2.1 Developing for two-link flexible mobile manipulator
3.2.1.1 Equations of motion
In this section, a mobile manipulator consists of a mobile platform with two flexible links /
joints manipulator as depicted in Fig. 7 is considered to analysis. For study on the complete
model, first, a mobile manipulator with two flexible links is considered to derive the
dynamic equations, then, with applying the joint flexibility by modelling the elasticity at
each joint as a linear torsinal spring the model is developed for integrated link and joint
flexible mobile manipulator.
To model the equations of motion of the system, assumed mode method is used. For this
purpose, the total energy associated with the system must be computed to determine the
Lagrangian function.
14 Advanced Strategies for Robot Manipulators
E(xe , ye)
F(xf , yf)
mp
r2 v2
G(xb , yb)
v1 θ2
k2
θ1 θ4
L0 J2
k1
θ0
θ3
J1
Base Path
r1
X0
Fig. 7. Two links mobile manipulator with flexible links and joints
The total kinetic energy of the system (T) is given by
T = TL + TB + TM , (23)
L
2
1 i
TL = ∑ ρ i ∫ riT ( xi ) ri ( xi )dxi , (24)
i =1 2 0
where ri is the position vector that describes an arbitrary point along the i th deflected link
with respect to the global co-ordinate frame ( X 0Y0 ) and ρ i is the linear mass density for the
i th link.
By defining rb and rm as position vectors of the base and the payload respectively, the
associated kinetic energies are obtained as:
1
TM = mprm2
2 , (25)
1 1
TB = mb rb2 + I bωb2
2 2
where I b and ωb are the moment of inertia and the angular velocity of base, respectively.
Note that the moment of inertia of the end effector has been neglected.
The Comparative Assessment of Modelling and Control of Mechanical Manipulator 15
Next, the potential energy associated with the flexibility of the links due to the link
deformation is obtained as:
L
2
1 i ⎛ d2v ⎞
UL = ∑ ∫ (EI )i ⎜ 2i ⎟dxi , (26)
i =1 2 0 ⎝ dxi ⎠
where (EI )i is the flexural rigidity of the i th link and vi ( xi , t ) is the bending deflection of the
i th link at a point xi , (0 ≤ xi ≤ li ) . Now, by determining the gravity energy as:
2 Li
U g = ∑ ∫ ρ i g xi dxi , (27)
i =1 0
and adding this energy to those obtained in Eq. (26) the total potential energy of the system
is obtained as U = U L + U g . Finally, by constructing the Lagrangian as L = T – U and using
the Lagrangian equation, the equations of motion for two-link flexible mobile manipulator
can be obtained as Eq. (8). Hence, the overall generalized co-ordinate vector of the system
can be written as: q = [qb qr q f ] = [ x f y f θ 0 θ 1 θ 2 e1 e2 ] , where qb = [ x f y f θ 0 ] is
the base generalized coordinates vector, qr = [θ 1 θ 2 ] is the link angles vector and
q f = [ e1 e2 ] is the vector of link modal displacements.
There is one nonholonomic constraint for the mobile base that undertakes the robot
movement only in the direction normal to the axis of the driving wheels:
Now, by predefining the base trajectory, the system dynamics can be decomposed into two
parts: one is corresponding to redundant set of variables, qr and the remained set of them,
qnrf . That is
⎡ Mr , r Mr , nrf ⎤ ⎡ qr ⎤ ⎡ H r ⎤ ⎡ U r ⎤
⎢M + = , (29)
⎣ r , nrf Mnrf , nrf ⎥⎦ ⎢⎣qnrf ⎥⎦ ⎢⎣ H nrf ⎥⎦ ⎢⎣U nrf ⎥⎦
Now, by remaining the second row of above equation, the non-redundant part of system
equations is considered to path optimization procedure.
For developing the system to encounter the flexible joints manipulator, adding the actuator
positions and their dynamic equations is required. Hence, the set of system dynamic
equation is rearranged as explain in Eq. (10). This overall system is clearly established the
equations that involve the flexible nature of both links and joints.
These enhanced dynamic equations that involve dynamic of the two-link flexible mobile
manipulator are considered in trajectory planning problem in the presenting study.
3.2.1.2 Stating an optimal control solution
Optimal control approach provides an excellent tool to calculate optimal trajectory with
high accuracy for robots that include, in this case, two link flexible mobile manipulators.
Let the trajectory generation problem be defined here as determining a feasible specification
of motion which will cause the robot to move from a given initial posture (state) to a given
final posture (state) while minimize a performance criterion such as integral quadratic norm
of actuating torques or velocities, which leads to minimize energy consumption or bounding
the velocity magnitude.
For this reason, as it can be seen in Fig. 7 the state vectors can be defined as:
⎡θ 1 (t )⎤ ⎡ x1 (t )⎤ ⎡θ (t )⎤ ⎡ x2 (t )⎤
X1 = ⎢ ⎥ =⎢ ⎥ X2 = ⎢ 1 ⎥ = ⎢ ⎥
⎣θ 2 (t )⎦ ⎣ x3 (t )⎦ ⎣θ 2 (t )⎦ ⎣ x 4 (t )⎦
⎡ e1 (t )⎤ ⎡ x5 (t )⎤ ⎡ e1 ( t ) ⎤ ⎡ x 6 ( t ) ⎤
X3 = ⎢ ⎥ =⎢ ⎥ X4 = ⎢ ⎥=⎢ ⎥ (31)
⎣ e2 ( t ) ⎦ ⎣ x 7 ( t ) ⎦ ⎣ e2 (t )⎦ ⎣ x8 (t ) ⎦
⎡θ 3 (t )⎤ ⎡ x9 (t ) ⎤ ⎡θ 3 (t )⎤ ⎡ x10 (t )⎤
X5 = ⎢ ⎥ = ⎢ x ( t )⎥ X 6 = ⎢ ⎥=⎢ ⎥.
θ ( t )
⎣ 4 ⎦ ⎣ 11 ⎦ ⎣θ 4 (t )⎦ ⎣ x12 (t )⎦
where θ1 and θ2 are angular positions of links, e1 and e2 are links modal displacements, and θ3
and θ4 are angular positions of motors. The boundary condition can be expressed as:
x2 i − 1 = x2 i ;
. (34)
x2 i = f i ; i = 1...6
In order to derive the equations associated with optimality conditions, penalty matrices can
be selected as follows:
W = diag( w1 , w2 , w3 , w4 , w5 , w6 )
(35)
R = diag(r1 , r2 ) .
The Comparative Assessment of Modelling and Control of Mechanical Manipulator 17
An important remark must be done here is that the study is planned a trajectory in the joint
space rather than in the operating space. It means the control system acts on the manipulator
joints rather than on the end effector. Trajectory planning in the joint space would allow
avoiding the problems arising with kinematic singularities and manipulator redundancy.
Moreover, it would be easier to adjust the trajectory according to the design requirements if
working in the joint space. By controlling manipulator joints can achieve the best dynamic
coordination of joint motions, while minimizing the actuating inputs together with bounding
the velocity magnitudes. It causes to ensure soft and efficient functioning while improving the
manipulator working performances. For that reason, the objective function is formed as:
1⎛ 6
⎞
L= ⎜ r1 u 12 +r2 u 22 + ∑ w i x 2i2 ⎟ . (36)
2⎝ i =1 ⎠
Subsequently, with defining the auxiliary costate vector as:
Ψ = ⎡⎣ψ 1 ψ 2 ... ψ 12 ⎤⎦ = ⎡⎣ x13 x14 ... x24 ⎤⎦ results to the Hamiltonian function as:
1⎛ 2 6
2 ⎞
12
H= ⎜ r1u1 +r2 u2 + ∑ w i x 2i ⎟ + ∑ x12 + i xi ,
2
(37)
2⎝ i =1 ⎠ i =1
Consequently, with differentiating the Hamiltonian function with respect to states, the
costate equations are obtained as follow
∂H
ψi = − , i = 1, ,12 . (38)
∂xi
Also, differentiating the Hamiltonian with respect to control and setting the derivative equal
to zero, yields the following control equations:
∂H ∂H
= r1u1 + x23 J 1 = 0 ; = r2 u2 + x24 J 2 = 0 (39)
∂u1 ∂u1
where by solving them, the expression for control values in the admissible interval,
ui − < ui < ui + ; i = 1, 2 can be obtained as follow:
Then, by considering the constraint on control input, the optimal control can be expressed as
follows:
where the final bound of control for each motor is obtained as:
u 1+ = τ 1 − S 1 x 11 ; u 1− = − τ 1 − S 1 x 11
(42)
u = τ 2 − S 2 x 12
+
2 ; u 2− = − τ 2 − S 2 x 12
where S i = (τ i / ωmi ) , τ i and ωmi are the stall torque and maximum no-load speed of i th
motor respectively.
Finally, 24 nonlinear ordinary differential equations are obtained by substituting Eq.(33) into
Eqs. (38) and (34), which with 24 boundary conditions given in Eq.(32) construct a two point
boundary value problem (TPBVP).
There are numerous influential and efficient commands for solving such problems that are
available in different software such as MATLAB, MATEMATHICA or FORTRAN. These
commands by employing capable methods such as finite difference, collocation and
shooting method solve the problem. In this study, BVP4C command in MATLAB® which is
based on the collocation method is used to solve the aforesaid problem. This numerical
technique have been detailed by (Shampine et al.).
3.2.1.3 Required parameters
In all simulations the mobile base is initially at point ( x fi = 0.5m, y fi = 0.5m, θ fi = 0) and
moves along a straight-line path to final position ( x ff = 1.5m, y ff = 1m). The necessary
parameters used in the simulations are summarized in the Table 1.
x 1 ( 0) = x 9 ( 0) = 120 , x 3 ( 0) = x 11 ( 0) = 90 ;
(43)
x 1 ( f ) = x 9 ( f ) = 30 , x 3 ( f ) = x 11 ( f ) = 30 ;
Also, in all simulations, the penalty matrix of control efforts R assumes to be R=diag[0.01].
Note that in all simulations, the payload is calculated with the accuracy of 0.1 Kg.
0.5 1
case 1 case 1
Angular Velocity-Joint 1 (rad/s)
-0.5 -0.5
-1
-1
-1.5
-1.5 -2
0 0.5 1 1.5 2 2.5 0 0.5 1 1.5 2 2.5
Time (s) Time (s)
case 4
10 upper bound 10
lower bound
0 0
case 1
-10 -10
case 2
case 3
-20 -20 case 4
upper bound
lower bound
-30 -30
0 0.5 1 1.5 2 2.5 0 0.5 1 1.5 2 2.5
Time (s) Time (s)
2.5
2
y(m)
1.5
0.5
-1 -0.5 0 0.5 1 1.5 2 2.5
x(m)
2.5
2 case 1
case 2
y(m)
case 3
1.5 case 4
0.5
-1 -0.5 0 0.5 1 1.5 2 2.5
x(m)
Fig. 11. End effector trajectory in XY plane
Fig. 8 shows the angular velocities of joints. The computed torques are plotted in Fig. 9. As
shown in figures increasing W causes reducing the maximum velocity magnitude while the
torques are growing. This issue is predictable, since, in the cost functional defined in the Eq.
(16) increasing W causes to rise the role of velocity in path planning an it can decreases the
proportion of R in such process. Furthermore, it can be found from figures, in order to attain
a smoother path with smaller amount of velocity, more effort must be applied. Also, it is
obvious that all the obtained graphs are satisfied the system cost function in Eq. (16). hence,
they specify optimal trajectories of the system motion. Therefore, in the proposed method
designer is able to choose most appropriate path among various optimal paths according to
designing requirements. Robot configuration and end effector trajectory are depected in
Figs. 10 and 11 respectively.
3.2.2.2 Motion planning for different payloads
In this case W is assumed to be constant at W=1. Then, the robot path planning problem will
be investigated by increasing the payload mass until maximum allowable load will be
determined. This maximum payload is obtained as 8.4 kg (case 4). The obtained angular
positions, angular velocities and torque curves graphs for a range of mP given in Table 2 are
shown in Fig.s 12 - 14. It can be found that, increasing the mP results to enlarge the velocity
values as a consequence various optimal paths have been attained. As shown in figures,
increasing the payload increases the required torque until the maximum payload. So that for
the last case the torque curves lay on their limits. Hence, it is the most possible values of the
torques and increasing the payload can lead to violate the boundary conditions. Finally, end
effector trajectories in the Cartesian space are depicting in Fig. 15.
Case 1 2 3 4
mp 1 3 7 8.4
Table 2. The values of mp used in the simulation.
mpmax =8.4 kg is the maximum allowable payload for the selected penalty matrices while
choosing the other penalty matrices, results in other optimal trajectories. To demonstrate
that issue, simulations are carried out for different values of W given in Table 3.
The Comparative Assessment of Modelling and Control of Mechanical Manipulator 21
120 180
case 1 case 1
Angular Position-Joint 1 (degree)
0 40
-20 20
0 0.5 1 1.5 2 2.5 0 0.5 1 1.5 2 2.5
Time (s) Time (s)
2 4
case 1
Angular Velocity-Joint 1 (rad/s)
1 case 2
2 case 3
case 4
0
0
-1
-2
-2
case 1
case 2 -4
-3 case 3
case 4
-4 -6
0 0.5 1 1.5 2 2.5 0 0.5 1 1.5 2 2.5
Time (s) Time (s)
50 60
case 1 case 1
case 2 case 2
40
case 3 case 3
Torque Motor 1 (N-m)
case 4 case 4
upper bound 20 upper bound
lower bound lower bound
0 0
-20
-40
-50 -60
0 0.5 1 1.5 2 2.5 0 0.5 1 1.5 2 2.5
Time (s) Time (s)
2.5
y(m) 2
1.5
case 1
1 case 2
case 3
case 4
0.5
-1 -0.5 0 0.5 1 1.5 2 2.5
x(m)
Fig. 15. End effector trajectory in XY plane
Case 1 2 3 4
W 1 400 600 800
m p max 8.4 7.9 7.5 6.3
2.5
2
y(m)
1.5
case 1
case 2
1
case 3
case 4
0.5
-1 -0.5 0 0.5 1 1.5 2 2.5
x(m)
Fig. 16. End effector trajectory in XY plane
The Comparative Assessment of Modelling and Control of Mechanical Manipulator 23
2 2
y(m)
y(m)
1.5 1.5
1 1
0.5 0.5
-1 0 1 2 3 -1 0 1 2 3
x(m) x(m)
20
10
0
0
-10
4. Conclusion
In this chapter, modelling and control of mechanical manipulator had been studied. First,
kinematic and dynamic modelling of flexible link, flexible joint and mobile manipulators
have been considered. Then, optimal control of a flexible mobile manipulator in point-to-
point motion had been formulated based on the open-loop optimal control approach. The
first objective of the chapter is to state the dynamic optimization problem under a quite
generalized form in order to be applied to a variety of situations with any guess objective
functions for the optimality solution. The second objective is consisting in developing the
method for optimizing the applicable case studies, which results.
Using assumed mode and finite element methods oscillatory behavior of he mobile robotic
manipulators had been described. The model equations had been verified for a two-link
manipulator, and the model responses had been discussed. Then, joint flexibility had been
added to the system and obtained model had been simulated. After that, an efficient
solution on the basis of TPBVP solution had been proposed to path optimization –
maximum payload determination in order to achieve the predefined objective. The solving
strategy makes it possible to get any guess objective functions for the optimality solution.
Attaining the minimum effort trajectory along with bounding the obtained velocity
magnitude had been chosen at the application example. The obtained results illustrate the
24 Advanced Strategies for Robot Manipulators
power and efficiency of the method to overcome the highly nonlinearity nature of the
optimization problem which with other methods, it may be very difficult or impossible.
Highlighting the main contribution of the chapter can be presented as:
• The proposed approach can be adapted to any general serial manipulator including
both non-redundant and redundant systems with link flexibility and base mobility.
• In this approach the nonholonomic constraints do not appear in TPBVP directly, unlike
the method given in (Mohri et al. 2001; Furuno et al. 2003).
• This approach allows completely nonlinear states and control constraints treated
without any simplifications.
• The obtained results illustrate the power and efficiency of the method to overcome the
high nonlinearity nature of the optimization problem, which with other methods, it
may be very difficult or impossible.
• In this method, boundary conditions are satisfied exactly, while the results obtained by
methods such as Iterative Linear Programming (ILP) have a considerable error in final
time (Ghariblu & Korayem, 2006).
• In this method, designer is able to choose the most appropriate path among various
optimal paths by considering the proper penalty matrices.
The optimal trajectory and corresponding input control obtained using this method can be
used as a reference signal and feed forward command in the closed-loop control of such
manipulators.
5. References
Bertolazzi E.; Biral F. & Da Lio M. (2005). Symbolic–numeric indirect method for solving
optimal control problems for large multibody systems, Multibody System Dynamics,
Vol. 13, No. 2, pp. 233-252
Bessonnet G. & Chessé S. (2005). Optimal dynamics of actuated kinematic chains, Part 2:
Problem statements and computational aspects, European J. of Mechanics A/Solids,
Vol. 24, pp. 472–490
Bloch A. M. (2003). Nonholonomic mechanics and control. Springer, New York
Bock H. G. & Plitt K. J. (1984). A multiple shooting algorithm for direct solution of optimal
control problems, Proc. 9th IFAC World Congress, pp. 242–247
Callies R. & Rentrop P. (2008). Optimal control of rigid-link manipulators by indirect
methods, GAMM-Mitt., Vol 31, No. 1, pp. 27 – 58
Chen W. (2001). Dynamic modelling of multi-link flexible robotic manipulators, Computers
and Structures, Vol. 79, (2), pp. 183–195
Furuno S.; Yamamoto M. & Mohri A. (2003). Trajectory planning of mobile manipulator
with stability considerations, Proc. IEEE Int. Conf. on Robotics and Automation, pp.
3403-3408
Gariblu H. & Korayem M. H. (2006). Trajectory Optimization of Flexible Mobile
Manipulators, Robotica, Vol. 24, No. 3, pp. 333-335
Green A. & Sasiadek J.Z. (2004). Dynamics and Trajectory Tracking Control of a Two-Link
Robot Manipulator, Journal of Vibration and Control, Vol. 10, No. 10, pp. 1415–1440
Hargraves C. R. & Paris S. W. (1987). Direct trajectory optimization using nonlinear
programming and collocation, AIAA J. Guidance, Vol. 10, No. 4, pp. 338–342, 1987.
The Comparative Assessment of Modelling and Control of Mechanical Manipulator 25
Usoro P.B.; Nadira R., Mahil S. S. (1986). A finite element/Lagrange approach to modelling
lightweight flexible manipulators, J. of Dynamics Systems, Measurement, and Control,
Vol. 108, pp.198–205
Wachter A. & Biegler L. T. (2006). On the implementation of an interior-point filter line-
search algorithm for large-scale nonlinear programming, Mathematical
Programming, Vol. 106, No. 1, pp. 25–57
Yue S., Tso S. K. & Xu W. L. (2001). Maximum dynamic payload trajectory for flexible robot
manipulators with kinematic redundancy, Mechanism and Machine Theory 36, 785–
800
Zhang C. X. & Yu Y. Q. (2004). Dynamic analysis of planar cooperative manipulators with
link flexibility, ASME Journal of Dynamic Systems, Measurement, and Control, Vol.
126, pp. 442–448
2
1. Introduction
Modern industrial robots are mostly (human) arm-inspired mechanisms with serially
arranged discrete links. When it comes to industrial environment where the workspace is
structured and predefined this kind of structure is fine. This type of robots are placed in
carefully controlled environments and kept away from human and their world.
When it comes to robots that must interact with the natural world, it needs to be able to
solve the same problems that animals do. The rigid structure of traditional robots limit their
ability to maneuver and in small spaces and congested environments, and to adapt to
variations in their environmental contact conditions. For improving the adaptability and
versatility of robots, recently there has been interest and research in “soft” robots. In
particular, several research groups are investigating robots based on continuous body
“continuum” structure. If a robot’s body is soft and/or continuously bendable it might
emulate a snake or an eel with an undulating locomotion (Walker & Carreras, 2006).
An ideal tentacle manipulator is a non-conventional robotic arm with an infinite mobility. It
has the capability of takeing sophisticated shapes and of achieving any position and
orientation in a 3D space. Behavior similar to biological trunks, tentacles, or snakes may be
exhibited by continuum or hyper-redundant robot manipulators (Walker et al., 2005). Hence
these manipulators are extremely dexterous, compliant, and are capable of dynamic
adaptive manipulation in unstructured environments, continuum robot manipulators do not
have rigid joints unlike traditional rigid-link robot manipulators. The movement of the
continuum robot mechanisms is generated by bending continuously along their length to
produce a sequence of smooth curves. This contrasts with discrete robot devices, which
generate movement at independent joints separated by supporting links.
The snake-arm robots and elephant’s trunk robots are also described as continuum robots,
although these descriptions are restrictive in their definitions and cannot be applied to all
snake-arm robots (Hirose, 1993). A continuum robot is a continuously curving manipulator,
much like the arm of an octopus (Cowan & Walker, 2008). An elephant’s trunk robot is a
good descriptor of a continuum robot (Hutchinson, S.; Hager et al., 1996). The elephant’s
trunk robot has been generally associated with an arm manipulation – an entire arm used to
grasp and manipulate objects, the same way that an elephant would pick up a ball. As the
best term for this class of robots has not been agreed upon, this is still an emerging issue.
Snake-arm robots are often used in association with another device meant to introduce the
snake-arm into the confined space.
However, the development of high-performance control algorithms for these manipulators
is quite a challenge, due to their unique design and the high degree of uncertainty in their
28 Advanced Strategies for Robot Manipulators
dynamic models. The great number of parameters, theoretically an infinite one, makes very
difficult the use of classical control methods and the conventional transducers for position
and orientation.” must be moved after the paragraph “An ideal tentacle manipulator is a
non-conventional robotic arm with an infinite mobility. It has the capability of takeing
sophisticated shapes and of achieving any position and orientation in a 3D space. These
systems are also known as hyper redundant manipulators and, over the past several years,
there has been a rapid expanding interest in their study and construction.
An ideal tentacle manipulator is a non-conventional robotic arm with an infinite mobility. It
has the capability of takeing sophisticated shapes and of achieving any position and
orientation in a 3D space. These systems are also known as hyper redundant manipulators
and, over the past several years, there has been a rapid expanding interest in their study and
construction.
The control of these systems is very complicated and a great number of researchers tried to
offer solutions for this difficult problem. In (Hemami, 1984); (Suzumori et al., 1991) it
analyses the control by cables or tendons meant to transmit forces to the elements of the arm
in order to closely approximate the arm as a truly continuous backbone. Also, Mochiyama
has investigated the problem of controlling the shape of an HDOF rigid-link robot with two-
degree-of-freedom joints using spatial curves (Mochiyama & Kobayashi, 1999). Important
results were obtained by Chirikjian (Chirikjian, 1993) who laid the foundations for the
kinematic theory of hyper redundant robots. His results are based on a “backbone curve”
that captures the robot’s macroscopic geometric features.
The inverse kinematic problem is reduced to determining the time varying backbone curve
behaviour (Takegaki & Arimoto, 1981). New methods for determining “optimal” hyper-
redundant manipulator configurations based on a continuous formulation of kinematics are
developed. In (Gravagne & Walker, 2001), Gravagne analysed the kinematic model of
“hyper-redundant” robots, known as “continuum” robots. Robinson and Davies (Robinson
& Davies, 1999) present the “state of art” of continuum robots, outline their areas of
application and introduce some control issues. The great number of parameters,
theoretically an infinite one, makes very difficult the use of classical control methods and the
conventional transducers for position and orientation.
The lack of no discrete joints is a serious and difficult issue in the determination of the
robot’s shape. A solution for this problem is the vision based control of the robot, kinematics
and dynamics.
The research group from the Faculty of Automation, Computers and Electronics, University
of Craiova, Romania, started working in research field of hyper redundant robots over 25
years ago. The experiments started on a family of TEROB robots which used cables and DC
motors. The kinematics and dynamics models, as well as the different control methods
developed by the research group were tested on these robots. Starting with 2008, the
research group designed a new experimental platform for hyper redundant robots. This new
robot is actuated by stepper motors. The rotation of these motors rotates the cables which by
correlated screwing and unscrewing of their ends determine their shortening or prolonging,
and by consequence, the tentacle curvature (Blessing & Walker, 2004). Segments were
cylindrical in the initial prototype, and cone-shaped in actual prototype. The backbone of
the tentacle is an elastic cable made out of steel, which sustains the entire structure and
allows the bending. Depending on which cable shortens or prolongs, the tentacle bends in
different planes, each one making different angles (rotations) respective to the initial
coordinate frame attached to the manipulator segment – i.e. allowing the movement in 3D.
Due to the mechanical design it can be assumed that the individual cable torsion,
Hyper Redundant Manipulators 29
respectively entire manipulator torsion can be neglected. Even if these phenomena would
appear, the structure control is not based on the stepper motors angles, but on the
information given by the robotic vision system which is able to offer the real spatial
positions and orientations of the tentacle segments.
2. Kinematics
In order to control a hyper-redundant robot it has to develop a method to compute the
positions for each one of his segments (Immega & Antonelli, 1995). By consequence, given a
desired curvature S*(x, tf) as sequence of semi circles, identify how to move the structure, to
obtain s(x, t) such that
lim t →t f s( x , t ) = S * ( x , t f ) (1)
where x is the column vector of the shape description and tf is the final time (see Fig. 2).
R3
Y
S*
R1 R2
X
To describe the tentacle’s shape we will consider two angles (α, θ) for each segment, where θ
is the rotation angle around Z-axis and α is the rotation angle around the Y-axis (see Fig. 2).
To describe the movement we can use the roto-translation matrix considering θ = 2β as
shown in Fig. 3.
2β
θ
centre
In 3D space we cannot write immediately the dependence that exists between two segments.
This relation can be obtained through the pre-multiplication of generic roto-translation
matrix. One of the possible combinations to express the coordinate of the next segment
related to the frame coordinate of the previous segment is the following:
where Rzi (θ i ) and Ryi (α i ) are the fundamental roto-translation matrix having 4x4 elements
in 3-D space, and Try(Vi) is a 4x4 elements matrix of pure translation in 3-D space and where
Vi is the vector describing the translation between two segments expressed in coordinate of
i-th reference system.e main problem remains to obtain an imposed shape for the tentacle
arm. In order to control the robot, we need to obtain the relation between the position of the
wires and the position of the segment.
Here, a decoupled approach is used for the robot control scheme. Thus the segments are
controlled separately, without considering the interaction between them. Considering the
segments of the tentacle separately, then (α, θ)i is the asigned coordinate of i-th segment.
Having as purpose to command the robot to reach the position (α, θ)i the following relation
is useful:
LCB
R= ∀θ ≠ 0 (4)
θ
Hyper Redundant Manipulators 31
where R represents the curvature’s radius of the central bone and LCB is a constant, equal to
the length of the central bone.
Once we have θ and α together as parameters of the desired shape, and after we obtained R,
we can compute the corresponding lengths of the wires. Depending on the types of wires
and on the structure of the tentacle, we must choose the way to compute the length of each
wire.
For the hard wire, made from the same material as the central bone, and by consequence
having the same elasticity, referring to Fig. 4, we can write:
Hard wires X1
Soft wires X
R2,R3
R1
θ
Z1 Y
Lw3 Lw1 X2 Z
Lw2
Y1
Z2
Y2
⎧ Lw 1 = R1 ⋅ θ
⎪
⎨Lw 2 = R2 ⋅ θ (5)
⎪L = R3 ⋅ θ
⎩ w3
For the soft wires, we can write:
⎧
⎪L w1 = [R1 ⋅θ ]⋅ sin(θ / i )
⎪ θ /i
⎪ sin(θ / i )
⎨Lw 2 = [R2 ⋅ θ ]⋅ (6)
⎪ θ /i
⎪L sin(θ / i )
⎪ w3 = [R3 ⋅ θ ]⋅
⎩ θ /i
where Lwn is the length of the n-th wire and Ri is the radius of the curvature of the real i-th
wire.
Farther it can be written:
Rn = ( R − ΔR ) ⋅ cos(α n ) (7)
where ΔR is constant equal to the distance between the center and the wires and αn is:
32 Advanced Strategies for Robot Manipulators
⎧α 1 = −α
⎪ (8)
⎨α 2 = 120° − α
⎪
⎩α 3 = 240° − α
Obviously the equations (5) and (6), become the same for i → ∞.
In order to reach the desired shape in a finite time tf, we should choose the appropriate law
for the time variation of the displacements and speed for the three wires, going from the
home position to the final position. For each instant, the wires must be moved in order to
avoid elongation or compression of it self.
The reference systems for each segment are oriented with the X-axes passing through the
first wire. That means that the angles considered between the wires and the desired
directions are as in the equation (8).
We can obtain the correlation between these angles and the bending direction of the
segment. E.g. if the direction is α =2/3π, that means we intend to bend the tentacle in the
direction of the second wire with the imposed value of θ degrees. In this case, if we will
move the second wire of ΔLw2, we should move the first and third wires with ΔLw2/2 and
with the apropiate speed in order to maintain this relation during the movement.
Once we know the angle α, we can obtain the value ΔRi = ΔR ⋅ cos (α i ) , defining the
displacements of the wires.
The algorithm that we are using, assigns the speed of the wires proportional to ΔRi in order
to go from the home position (θ =0, α =0) to the position (α, θ)i with a constant speed of the
motors.
In fact, given the final time tf and the starting time ti, after we obtained the displacement of
the wires we impose the speed in order to reach the desired position in (tf-ti) seconds.
So the speed is:
L wi ( t f ) − LCB
L wi = (9)
(t f − ti )
Our structure does not have encoders. Counting the impulses given to the motors, we can
evaluate the lengths [Lw1, Lw2, Lw3]. We use these values in order to obtain (α ,θ)i. The
algorithm’s steps are the following.
For the n-th rigid wire:
Considering the equation (8) and (10), evaluating these for all the wires we can obtain:
⎧ 3
⎪ ∑ cos(α i ) = 0
⎪ i=1
⎪⎪ 1 3
(11)
⎨ ∑ Ri = R
⎪ 3 i =1
⎪1 3
⎪ ∑ i = 1 L wi = L
⎪⎩ 3
Considering again the equation (10) for the first and second wires, we can write:
2 Lw 1 − Lw 2
θ= ⋅ (13)
ΔR 3cos (α ) − 3 sin (α )
Lw 3 = Lw 1 +
(
2 ⋅ ( Lw 1 − Lw 2 ) ⋅ 3cos (α ) − 3 sin (α ) ) (14)
3 cos (α ) − 3 sin (α )
α = atan2 ( 3 ( Lw 2 − Lw 3 ) , 2 Lw 1 − Lw 2 − Lw 3 ) (15)
where atan2 is an extension of arctan(y/x) on more quadrant having the following form:
⎧atan( y / x ) + π if x < 0, y ≥ 0
⎪ atan( y / x ) − π if x < 0, y < 0
⎪
⎪⎪ atan( y / x ) if x>0
⎨ π (16)
⎪ if x = 0, y > 0
⎪ 2
⎪ −π
if x = 0, y < 0
⎪⎩ 2
The same methodology can be applied for a tronconical robot. The following paragraphs
will show how the equations change. The geometry of one segment for the 2D case is
described in Fig. 6. The curvature’s angle θ of the segment is considered as the input
parameter, while the lengths L1 and L2 of the control wires are the outputs.
H
R= (17)
θ
where H is the height of the segment. The following lengths are obtained from Fig. 5, based
on the segment curvature:
where D1 and D2 are the diameters of the segment end discs. Based on the Carnot theorem,
the lengths A1 and A2 are then obtained:
The control wires curvature radius R1 and R2 are given by the relations (20):
Lw 1 = R1 ⋅ θ = A1 ⋅ θ / 2 ⋅ sin θ2
(21)
Lw 2 = R2 ⋅ θ = A2 ⋅ θ / 2 ⋅ sin θ2
For the 3D case, a virtual wire is considered, which gives the α direction of the curvature.
Considering one virtual wire in the direction of the desired curvature having length
calculated as follows. Firstly the following lengths are computed:
⎧α 1 = −α
⎪
⎨α 2 = 120° − α (23)
⎪α = 240° − α
⎩ 3
Based on (19) and (20) the curvature radiuses R1, R2 and R3 of the three control wires are
then obtained. Finally the lengths of the control wires are computed with (24):
Lw 1 = R1 ⋅ θ
L w 2 = R2 ⋅ θ (24)
L w 3 = R3 ⋅ θ
Apart from the system presented we can obtain two useful relations:
⎧ 3
⎪ ∑ cos(α i ) = 0
⎪ i =1
⎨ (25)
⎪1 3 L
⎪⎩ 3 ∑ i = 1 wi = L
The second equation of (25), can be utilized to estimate the virtual compression or the
extension of the central bone. We call that virtual compression because before we compress
the central bone, the robot will twist to find the shape to guaranty the wrong length of the
wires.
3. Dynamics
3.1 Theoretical model
The essence of the tentacle model is a 3-dimensional backbone curve C that is parametrically
described by a vector r ( s ) ∈ R 3 and an associated frame ϕ ( s ) ∈ R 3× 3 whose columns create
the frame bases (Fig. 7a) (Ivănescu et al., 2006).
φl
ez φ
s Z
r(s)
e0 z
ex
P ey
0
φ q(s)
r(s) Y
e0y
θ(s)
e0x X
(a) (b)
The independent parameter s is related to the arc-length from the origin of the curve C, a
variable parameter, where
N
l = ∑ ( l0 i + Δli ) (26)
i =1
or
l = l0 + u (27)
where l0 represents the length of the N elements of the arm in the initial position and
N
u = ∑ Δli (28)
i =1
r = r (s ) (29)
when s ∈ [0 , l]. For a dynamic motion, the time variable will be introduced, r = r (s , t ) .
We used a parameterization of the curve C based upon two "continuous angles" θ (s ) and
q (s ) and length variable u (Fig. 4).
At each point r (s , t ) , the robot’s orientation is given by a right-handed orthonormal basis
{
vector e x , e y , e z } and its origin coincides with point r (s , t ) , where the vector ex is tangent
and ez is orthogonal to the curve C. The position vector on curve C is given by
where
s
x(s , t ) = ∫ sin θ (s′ , t )cos q (s′ , t )ds′ (31)
0
s
y (s , t ) = ∫ cos θ (s′ , t )cos q (s′ , t )ds′ (32)
0
s
z(s , t ) = ∫ sin q (s′ , t )ds′ (33)
0
with s′ ∈ [0 , s]. We can adopt the following interpretation: at any point s the relations (31)-
(33) determine the current position and Φ (s ) determines the robot’s orientation, and the
robot’s shape is defined by the behaviour of functions θ (s ) and q (s ) . The robot “grows”
from the origin by integrating to get r (s , t ) , s ∈ [0 , l0 + u] . The velocity components are
Hyper Redundant Manipulators 37
(
vx = ∫ −q′ sin q′ sin θ ′ + θ ′ cos q′ cosθ ′ ds′ ) (34)
0
( )
s
vy = ∫ − q′ sin q′ cosθ ′ − θ ′ cos q′ cosθ ′ ds′ (35)
0
s
v z = ∫ q cos q ′ds′ (36)
0
vu = u (37)
For an element dm, kinetic and gravitational potential (Douskaia, 1998) energy will be
dT =
1
2
(
dm vx2 + vy2 + vz2 + vu2 ) (38)
dV = dm ⋅ g ⋅ z (39)
Where
dm = ρds (40)
From (13)-(15) we obtain
1 l ⎛⎜ ⎛⎜ s
2
T= ( ⎞
ρ ∫ ⎜ ⎜ ∫ − q sin q ′ sin θ ′ + θ ′ cos q ′ cos θ ′ ds′ ⎟⎟ +
2 0 ⎜⎝ 0
) (41)
⎝ ⎠
2
⎛s
( ⎞
+ ⎜⎜ ∫ − q ′ sin q ′ cos θ ′ − θ ′ cos q ′ sin θ ′ ds′ ⎟⎟ + )
⎝0 ⎠
⎛s ⎞
2 ⎞ l
⎜ ∫ q ′ cos q ′ds′ ⎟ ⎟ ds + 1 ρ u 2 ds
⎜ ⎟ ⎟⎟ 2 0
∫
⎝0 ⎠ ⎠
l s
V = ρg ∫ ∫ sin q ′ds′ds (42)
00
The elastic potential energy will be approximated by two components, one determined by
the bending of the element
Veb = k
d2 N 2
∑ qi + θi2
4 i =1
( ) (43)
1 2
Vea = ku (44)
2
38 Advanced Strategies for Robot Manipulators
where we assumed that each element has a constant curvature and a uniform equivalent
elasticity coefficient k, assumed constant on all the length of the arm.
The total elastic potential energy will be
We will consider Fθ (s , t ), Fq (s , t ) the distributed forces on the length that determine motion
and orientation in the θ - plane, q - plane and Fu (t ) , the force that determines axial motion,
assumed constant along the length of the arm.
∂ω (t , s ) δH
= (46)
∂t δν (t , s )
∂ν (t , s ) δH
=− + F (t , s ) (47)
∂t δω (t , s )
where ω and ν are the generalized coordinates and momentum densities, respectively, and
δ ( ⋅) / δ ( ⋅) denotes a functional partial derivative.
The state of this system at any fixed time t is specified by the set (ω (t , s ),ν (t , s )) , where
ω = [θ q u]T . The set of all functions of s ∈ Ω that ω , ν can take on at any time is state
function space Γ (Ω ). We will consider that Γ (Ω ) ⊂ L2 (Ω ).
The control forces have the distributed components along the arm, Fθ (t , s ), Fq (t , s ), s ∈ [0 , l]
and a lumped component Fu (t ).
A practical form of dynamic model expressed only as a function of generalised coordinates
is derived by using Lagrange equations developed for infinite dimensional systems,
∂ ⎛ δT ⎞ δT δV δVe
⎜ ⎟− + + = Fθ (48)
∂t ⎜⎝ δθ (t , s ) ⎟⎠ δθ (t , s ) δθ (t , s ) δθ (t , s )
∂ ⎛ δT ⎞ δT δV δVe
⎜ ⎟− + + = Fq (49)
∂t ⎜⎝ δq (t , s ) ⎟⎠ δq (t , s ) δq (t , s ) δq (t , s )
∂ ⎛ ∂T ⎞ ∂T ∂V ∂Ve
⎜ ⎟− + + = Fu (50)
∂t ⎝ ∂u ⎠ ∂u ∂u ∂u
where ∂ / ∂ (⋅), δ / δ (⋅) denote classical and functional partial derivatives (in Gateaux sense]),
respectively.
Hyper Redundant Manipulators 39
In Appendix 1 the dynamic model of this ideal spatial tentacle manipulator will be
developed and the difficulties to obtain a control law will be easily inferred.
The great number of parameters - theoretically an infinite number of parameters - the
complexity of the dynamic model make the application of the classical algorithms meant to
obtain the control law very difficult. In much of the literature concerned with the control of
these systems, the complexity of the problem is emphasized and various methods that
compensate all nonlinear terms in dynamics in real time are developed in order to reduce
the complexity of control systems. Also, simplified procedures are introduced or the
difficult components are neglected in order to generate a particular law for position or
motion control. In all these cases, these methods require a large amount of complicated
calculation so that it is difficult to implement these methods with usual level controllers. In
addition, the reliability of these methods may be lost when a small error in computation or a
small change in the system's parameters occurs.
ω0 = ω (0 , s ) = [θ0 , q0 , l0 ]T (51)
ν 0 = ν (0 , s ) = [0 , 0 , 0 ]T (52)
θ 0 = θ (0 , s ), q 0 = q (0 , s ), s ∈ [0 , l0 ] (53)
l0 = l(0 ) (54)
C 0 : (θ 0 ( s ) , q0 ( s ) , l0 ) , s ∈ [ 0, l0 ] (55)
The desired point in Γ (Ω ) is represented by a desired position of the arm, the curve C d ,
ωd = [θ d , qd , ld ]T ν d = [0 , 0 , 0 ]T
,
(56)
C d : (θ d (s ), q d (s ), ld ), s ∈ [0 , ld ]
The system motion (48)-(5) corresponding to a given initial state (ω0 , ν 0 ) defines a trajectory
in the state function space Γ (Ω ) . The control problem of the manipulator means the motion
control by the forces Fθ , Fq , Fu from the initial position C 0 to the desired position C d . From
the viewpoint of mechanics, the desired position (ω d , ν d ) is asymptotically stable if the
potential function of the system has a minimum at (ω , ν )(s ) = (ω d , ν d )(s ), s ∈ [0 , l] and the
40 Advanced Strategies for Robot Manipulators
system is completely damped. As a control problem in this paper the results of will be
extended for the infinite dynamic systems.
We will consider the control forces,
δV δVe δΠ
Fθ (t , s ) = + − Fθd − (57)
δθ (t , s ) δθ (t , s ) δθ (t , s )
∂V ∂Ve ∂Π
Fu (t ) = + − Fud − (58)
∂u(t ) ∂u(t ) ∂u(t )
The first two terms compensate the gravitational and elastic potential, the third components
assure the damping control and the last terms define the new artificial potential introduced
in order to assure the motion to the desired position. The minimum points of this potential
must be identical with desired positions of the manipulator, as attractors of its motion. For
example, the potential Π can be selected as a functional of generalised coordinates,
( )
l
Π (θ , q , u ) = ∫ (θ − θ d (s ))2 + (q − q d (s ))2 ds + (l0 + u − ld )2 (59)
0
The control law (57)-(59) modifies the system potential and the Lagrange equation (48)-(50)
(Masoud & Masoud, 2000) become
∂ ⎛ δT ⎞ δT δΠ
⎜ ⎟− + = Fθ d (60)
∂t ⎜⎝ δθ (t , s ) ⎟⎠ δθ (t , s ) δθ (t , s )
∂ ⎛ δT ⎞ δT δΠ
⎜ ⎟− + = Fq d (61)
∂t ⎜⎝ δq (t , s ) ⎟⎠ δq (t , s ) δq (t , s )
∂ ⎛ ∂T ⎞ ∂T ∂Π
⎜ ⎟− + = Fu d (62)
∂t ⎝ ∂u ⎠ ∂u ∂u
The force components Fθ d , Fqd , Fud represent the damping components of the control and
have the form
l
Fθ d (s , t ) = − ∫ Kθ (s , s′)θ (s′ , t )ds′ (63)
0
l
Fq d (s , t ) = − ∫ K q (s , s′)q (s′ , t )ds′ (64)
0
where Kθ (s , s′), K q (s , s′) are positive definite specified spatial weighting functions on
(Ω × Ω ) and K u is a positive constant. For practical reasons, the derivative components of
the control have the form
Hyper Redundant Manipulators 41
{
Π (θ , q , u ) = max Π 1 (θ , q , u ), Π 2 (θ , q , u ) } (68)
lim Π 2 (θ , q , u ) = ∞ (69)
d →0
where d is the distance between the current state (θ , q , u ) and the boundary ∂B .
3.5 Appendix 1
We will consider a spatial tentacle model, an ideal system, neglecting friction and structural
damping. We assume a uniformly distributed mass with a linear density ρ [kg/m].
We will use the notations:
[ ]
q = q(s , t ), s ∈ [0 , l ], t ∈ 0 , t f [ ]
θ = θ (s , t ), s ∈ [0 , l ], t ∈ 0 , t f
∂q (s , t )
[ ]
q ′ = q(s′ , t ), s′ ∈ [0 , s ], t ∈ 0 , t f q=
∂t
[ ]
, s ∈ [0 , l ], t ∈ 0 , t f
∂q (s′ , t ) ∂ 2 q (s′ , t )
q′ =
∂t
[ ]
, s′ ∈ [0 , s], t ∈ 0 , t f q′ =
∂t 2
[ ]
, s′ ∈ [0 , s], t ∈ 0 , t f
∂ 2 q (s′′ , t )
q ′′ =
∂t 2
[ ]
, s′′ ∈ [0 , s ], t ∈ 0 , t f
......................................
Fq = Fq (s , t ), s ∈ [0 , l ], t ∈ 0 , t f [ ] Fu = Fu (t ), t ∈ 0 , t f[ ]
42 Advanced Strategies for Robot Manipulators
s s
ρ ∫ ∫ (q′(sin q′ sin q′′ cos(q′ − q′′) + cos q′ cos q′′) − θ ′ cos q′ sin q′′ sin(θ ′′ − θ ′) +
00
+ q′2 (cos q′ sin q′′ cos(θ ′ − θ ′′) − sin q′ cos q′′) + θ ′2 cos q′ sin q′′ cos(θ ′ − θ ′′) − q′q′′ sin(q′′ − q′))ds′ds′′ +
s
1 2
+ ρg ∫ cos q′ds′ + kd q = Fq
0 2
s s
ρ ∫ ∫ (q′ sin q′ cos q′′ sin(θ ′′ − θ ′) + θ ′ cos q′ cos q′′ cos(θ ′′ − θ ′) − q′2 cos q′ cos q′′ sin(θ ′′ − θ ′) +
00
)
+ θ ′ cos q′ cos q′′ sin(θ ′′ − θ ′) − θ ′q′ sin q′ cos q′′ cos(θ ′′ − θ ′) ds′ds′′ +
1 2
2
kd θ = Fθ
1
ρu + ρu 2 + ku = Fu
2
PS 2 =[ x S1 , yS2 ] (71)
Geometric optics are used to model the mapping between the robot Cartesian space and the
screen coordinate systems. We assume that the quantization and the lens distortion effects
Hyper Redundant Manipulators 43
are negligible. The description of the point P=[ x, y, z]T in the robot coordinate frame is given
in terms of screen coordinate frames as
⎡ x s1 ⎤ λ1 ⎧⎪⎡ x ⎤ ⎡ o11 ⎤ ⎫⎪ ⎡c x1 ⎤
⎢y ⎥ = α1 ⋅ ⋅ R( φ ) ⋅ ⎨⎢ ⎥ − ⎢ ⎥ ⎬ + ⎢c ⎥ (73)
⎣ s1 ⎦ λ1 − (D1 + x ) ⎪⎩⎣ y ⎦ ⎣o12 ⎦ ⎪⎭ ⎣⎢ y 1 ⎦⎥
⎡ zs 2 ⎤ λ2 ⎧⎪⎡ z ⎤ ⎡ o21 ⎤ ⎫⎪ ⎡c z 2 ⎤
⎢y ⎥ = α 2 ⋅ ⋅ R(φ ) ⋅ ⎨⎢ ⎥ − ⎢ ⎥ ⎬ + ⎢c ⎥ (74)
⎣ s2 ⎦ λ 2 − (D2 + x ) ⎪⎩⎣ y ⎦ ⎣o22 ⎦ ⎪⎭ ⎣⎢ y 2 ⎦⎥
for the ZS2 OS2 YS2 frame, where [ c x1 , c y1 ]T and [ c z2 , c y 2 ]T the image centers, α 1 and α 2 are
the scale factors of the length units in the front image planes given in pixel/m, R( ψ ) and
R( φ ) are the rotation matrices generated by clockwise rotating the cameras about their
optical axes by ψ and φ radians, respectively, and [O11, O12]T and [O21, O22]T represent the
distances between the optical axes and the XOY and ZOY planes, respectively.
Z s2 Os 2 Xs 2
Z D
O Y
X D
Ys1
Os 1
X s1
ψ
Fig. 8. Camera system
In Fig. 9 the frames corresponding to the screen images of the two cameras are presented.
From the relations (73), (74), we obtain
⎡ Δx s1 ⎤ λ1 ⎡ Δx ⎤
⎢Δy ⎥ = α 1 ⋅ ⋅⎢ ⎥ (75)
⎣ s1 ⎦ λ1 − (D 1 + x ) ⎣Δy ⎦
⎡ Δzs2 ⎤ λ2 ⎡ Δz ⎤
⎢Δy ⎥ = α 2 ⋅ ⋅⎢ ⎥ (76)
⎣ s2 ⎦ λ 2 − (D 2 + x ) ⎣Δy ⎦
44 Advanced Strategies for Robot Manipulators
Δx s1 Δx
tgθ s = = = tgθ (77)
Δy s1 Δy
hence
Δz s 2 Δz
for the plane ZS1 OS1 YS1 and tgq s = =
Δy s 2 Δy
X s1 Cs1 Zs 2
Csd2
Fθs
Cs
x s1 Fq s 2
Csd1 zs 2 qs
Ps 2
O s1 y s1 Ys1 Os 2 ys 2 Ys 2
1
tgq s ( s'' ) = tgq( s ) ⋅ , s ∈ [ 0 , l ] , s''∈ [ 0 , l'' ] (79)
cos θ ( s )
where, s' , s'' and l' , l'' represent the projections of the variable s and the length l in the two
planes, respectively. The projection of the forces on the two planes can be easily inferred and
the relations (77)-(79),
Fθ s = Fθ (80)
C : (θ d ( s ), q d ( s )) , s ∈ [ o , l ] (82)
or, in the two image coordinate frames ZS1 OS1 YS1 and ZS2 OS2 YS2 , by the projection of the
curve C,
Hyper Redundant Manipulators 45
eθ ( t , s ) = θ ( t , s ) − θ d ( s ) , s ∈ [ 0 , l ] (85)
eq ( t , s ) = q( t , s ) − q d ( s ) , s ∈ [ 0 , l ] (86)
or, in the image coordinate frames, by s'∈ [ 0 , l' ] , s''∈ [ 0 , l'' ]
The global control system is presented in Fig. 10. The control problem of this system is a
direct visual servocontrol but we do not use the clasical concept of the position control
where the error between the robot end-effector and target is minimized (Grosso et all., 1996).
θd θ sd eθ s θs
qd q sd eqs qs
Desired Z
Projection Control
law Y
Values O
+ -
X
θs
qs
Image feature
Parameter extractor
computation
Proposition: The closed-loop hyper redundant arm system is stable if the control law is
•
Fθ ( s , t ) = − kθ1 ( s ) ⋅ eθ s ( s' , t ) − kθ2 ( s ) ⋅ eθs ( s' , t ) (89)
[
Fq ( s , t ) = − kq1 ( s ) ⋅ tg −1 (cosθ s ( s' , t )) ⋅ tgqs ( s'' , t ) − q d ( s ) ] (90)
where s'∈ [ 0 , l' ] , s''∈ [ 0 , l'' ] and kθ1 ( s ), kθ2 ( s ), k q1 ( s ) are positive coefficients of the control law
for all s ∈ [ 0 , l ] . The parameter of the control law (88), (89), can be inferred from the image
feature extraction of the •
two planes. The parameters eθ s can be directly calculated from
equation (85-88) and eθ s can be indirectly computed. Also θ s , qs and q sd are evaluated
directly from the trajectory projections. We remark that the control law represents a robust
control, independent of the camera parameters. No intrinsec camera parameters are
assumed known.
4.3 Appendix 2
We will consider a spatial tentacle model, an ideal system, neglecting friction and structural
damping. We assume a uniformly distributed mass with linear density ρ [ kg / m ] . We will
consider a non-extensible arm with constant length.
We will use the notations:
• ∂q( s , t )
q = q( s , t ) , q'= q'( s' , t ) , q = ,
∂t
⎛ ⎞
d ⎜ δT ⎟ δT δV
⎜ • ⎟
− + =F (91)
dt ⎜ δ q ⎟ δq δq
⎝ ⎠
where δ (.) / δ (.) denotes a functional partial (variational) Gateaux derivate (Wang, 1965), as
shown before, that is defined as the variation of the functional Ω with respect to the
function θ at a point s ∈ [ 0 , l ] . From (41-42) it results,
l s ••
⎛ ••
ρ ⋅ ∫ ∫ ⎜ q'⋅ (sin q'⋅ sin q''⋅ cos( q'−q'' ) + cos q'⋅ cos q'') − θ '⋅ cos q'⋅ sin q''⋅ sin(θ ''−θ ' ) +
0 0⎝
• •
+ q'2 ⋅(cos q'⋅ sin q''⋅ cos(θ ''−θ ' )) − sin q ⋅ cos q'' ) + θ '2 ⋅ cos q'⋅ sin q''⋅ cos(θ ''−θ ' ) − (92)
s
• • ⎞
− q'⋅ q''⋅ sin( q''−q' ) ⎟ ⋅ ds'⋅ds''+ ρ ⋅ g ⋅ ∫ cos q'⋅ds'= Fq
⎠ 0
Hyper Redundant Manipulators 47
l s
⎛ •• ••
ρ ⋅ ∫ ∫ ⎜ q'sin q'⋅ cos''⋅ sin( q'−q'' ) + θ '⋅ cos q'⋅ cos''⋅ cos(θ ''−θ ' ) +
00⎝
• •
+ q'2 ⋅(cos q'⋅ cos q''⋅ sin(θ ''−θ ' )) + θ '⋅ cos q'⋅ cos q''⋅ sin( θ ''−θ ' ) − (93)
• • ⎞
− θ '⋅ q'⋅ cos q''⋅ cos(θ ''−θ ' ) ⎟ ⋅ ds'⋅ds''= Fθ
⎠
1 l 1
V * ( t ) = T( t ) + V( t ) + ⋅ kθ ( s ) ⋅ eθ2 ( s , t ) ⋅ ds
2 0∫
(94)
where T, V represent the kinetic and potential energies of the system. V*(t) is pozitive
definited because the terms that represent the energy T and V are always T ( t ) ≥ 0 , V ( t ) ≥ 0 .
For the steady desired position, we have
• l
⎛ • • ⎞
V * ( t ) = ∫ ⎜ Fθ ( s , t ) ⋅ eθ ( s , t ) + F q ( s , t ) ⋅ e q ( s , t ) + kθ1 (θ ) ⋅ eθ ( s , t ) ⎟ ⋅ ds (95)
0 ⎝ ⎠
If we use the control low defined by the relations (89)-(90), where the parameters of motion
are evaluated from (78)-(79), (85)-(88), we will have,
• l 2
⎛• ⎞
V * ( t ) = − ∫ kθ1 ⋅ ⎜ eθ ( s , t ) ⎟ ⋅ ds (96)
0 ⎝ ⎠
•
V* (t) ≤ 0 (97)
Q.E.D.
The derivative of the error in the control laws (89), (90) can be computed by an iteration
procedure. The coordinate x s1 on the projection C s1 can be evaluated by the relation
i
x s1 ( i ⋅ Δs' ) = ∑ sinθ s1 ( j ⋅Δs' ) ⋅ Δs' (98)
j =1
i
Δx s1 ( i ⋅ Δs' ) = ∑ cos θ s1 ( j ⋅Δs' ) ⋅ Δθ ( j ⋅ Δs' ) ⋅ Δs' (99)
j =1
π
Assuming that θ si ( si ) ≠ , we obtain
2
1
i=1 Δθ ( Δs') =
Δs'
( Δxs1 ( Δs') cosθs1 ( Δs'))
i=2 Δθ ( 2 ⋅ Δs') = ( Δx1 ( 2 ⋅ Δs') / Δs'− cos θ s1 ( Δs') ⋅ Δθ ( Δs') ) cosθ s1 ( 2 ⋅ Δs')
(100)
⎛ m −1 ⎞ 1
i=m Δθ (m ⋅ Δs') = ⎜⎜ Δx1 (m ⋅ Δs') / Δs'− − ∑ cos θ s1 ( j ⋅ Δs') ⋅ Δθ ( j ⋅ Δs') ⎟⋅
⎟ cos( m ⋅ Δs' )
⎝ j =1 ⎠
48 Advanced Strategies for Robot Manipulators
π
If θ si (si ) = , a similar procedure for y s1 (i ⋅ Δs') can be used.
2
4.4 Camera calibration
The term “camera calibration” in the context of this paper refers to positioning and orienting
the two cameras at imposed values (Fig. 11) (Tanasie et al., 2009).). This calibration is
performed only at the beginning, after that the cameras remain still. First, a zoom that
maximizes the image resolution of the working space used by the manipulator is performed.
Second, positioning of the two cameras brings the manipulator in the middle of the two
images. Third, a pan / tilt orientation is performed (as descried later in the paper). At this
step the manipulator is moved in a test position that allows free of (or minimum) errors
calibration. The test images are compared to the images generated by the graphic simulator
(ideal images) which represent references for the calibration operation.
θd θs
qd qs Z Image of the Real
Desired Contro real
test Y parameters
l law O manipulator
projection
values X - Pan/tilt/zoom
control law
Σ
+
Image of the
Grapfic simulated Ideal
simulator manipulator
parameters
of each segment. To ease the presentation, the term segment will be used in all that follows
referring to the median segment (arched or un-arched). For the arched segment, its median
arc remains constant. In this paper the term O-X angle will be used to denote the angle that
the chord made by an arched element of the robot makes with the O-X axis of a selected
reference system.
The inputs for the simulator are: robot configuration; robot initial position; rontrol laws for
each of the segments of the hyper redundant robot. The robot configuration consists of the
number of segments the hyper redundant robot has, the length of each segment and the
angles that the cords make with the O-X axis. The arching angles are computed from these
angles. An arching angle is defined as the angle made by the cord (determined by the ends
of the arched segment) and the original un-arched segment. For the direct kinematics
problem, the control of the robot simulation is accomplished by giving the O-X angles for
each of the segments in their final position and the output of the simulation is the hyper
redundant robot’s end-effector final position in the operation space. In order to compute the
final position of the end-effector and the hyper redundant robot’s behavior during its
motion, a few elements must be computed: the relation between the arching angle and the
angle at center determined by the arched segment (this angle determines the length of the
arc); the cord length; the relation between an O-X angle and an arching angle; the final
arching angles – recurrent set.
The computation of the relation between the arching angle and the angle at center
determined by the arched segment is determined by the following axiom: For camera
calibration a direct kinematics model was used, thus the rotation angles for each segment
are given. For a robot that has only rotation joints, the O-X angle increases (or decreases,
depending on the selected positive direction) for each segment with the sum of rotation
angles of each of the previous segments (including the current segment). This is true
because the orthogonal system attached to the ith segment is obtained from its initial
position and applying all the anterior transformations. For a hyper redundant robot the
problems are different. The arching angle is double the sum of each previous arching angle
plus the current arching angle, because the un-arched segment is a prolongation of the
previous segment.
In order to simulate the circular arched segments a series of intermediate points (that are
connected by lines) between the segment origins must be determined. The Catmull-Rom
interpolation algorithm was used for this simulator because it was need an interpolation
algorithm that passes through the control points. Catmull-Rom splines are a family of cubic
interpolating splines formulated such that the tangent at each point pi is calculated using
the previous and next point on the splines, τ (pi +1 − pi −1 ) .
Camera calibration is the essential procedure for all such applications: positioning and
orienting the cameras in order to support the accuracy of the image features extraction.
Calibration for a pan/tilt/zoom camera shape is achieved by means of an engineered
environment and a graphic simulation module.
Term “camera calibration” in the context of this paper refers to positioning and orienting the
two cameras at imposed values. This calibration is performed only at the beginning, after
that the cameras remain still. The general control method is an image based visual servoing
one instead of position based. Camera calibration based on intrinsic parameters (classic
50 Advanced Strategies for Robot Manipulators
sense, not the one used in this paper) is not necessary. Calibration operation assures that the
two cameras’ axes are orthogonal.
Taking into account the presented structure of the tentacle - vision system, in order to apply
the tested visual servoing algorithm, the two cameras must be positioned and oriented as:
both focus on the robot, their axes are orthogonal, both have the same zoom factor.
Two different algorithms were implemented: one uses a cylindrical etalon, other uses the
graphical simulator.
For the first algorithm, special starting conditions were imposed in order to support the
image processing tasks: white background, dark grey cylinder, red vertical equidistant (90
degrees) axes, friendly initial camera's positions and orientations, zoom x1 (Fig. 12).
15 6
10 4
5 2
0 0
0 0.5 1 1.5 0 0.5 1 1.5
α α
1.5
1
1.4
0
1.3 yp i
FRx1 ( f , α ) − FRx0 ( f , α )
yn i
FRx2 ( f , α ) − FRx0 ( f , α )
1.2 −1
1.1
−2
1
0 0.5 1 1.5
α −3
−2 −1 0 1 2
xp i , xni
Fig. 15. a. Ratio distances variation b. Rectangle transformation and distance variation under
zoom influence
Force sensors
Z
Force sensors
0 Y
X
dT =
1
2
( )
dm v x2 + v y2 + v z2 , dV = dm ⋅ g ⋅ z (101)
d2 N 2
Ve = k ∑ ( qi + θi2 )
4 i =1
(102)
We will consider Fθ (s , t ) , Fq (s , t ) the distributed forces on the length of the arm that
determine motion and orientation in the θ -plane, q -plane. The mechanical work is:
( )
l t
L = ∫ ∫ Fθ (s ,τ )θ (s ,τ ) + Fq (s ,τ )q (s ,τ ) dτds (103)
00
( )
l t
L = ∫ ∫ Fθ (s ,τ )θ (s ,τ ) + Fq (s ,τ )q (s ,τ ) dτds (104)
00
where T (t ) and T (0 ) , V ∗ (t ) and V ∗ (0 ) are the total kinetic energy and total potential
energy of the system at time t and 0, respectively.
In this chapter, the manipulator model is considered as a distributed parameter system
defined on a variable spatial domain Ω = [0 , L ] and the spatial coordinate s.
From (101-103), the distributed parameter model becomes,
SS
ρ ∫ ∫ (q′(sin q′ sin q′′ cos(q′ − q′′) + cos q′ cos q′′) −θ ′ cos q′ sin q′′ sin(θ ′′ − θ ′) +
00
( )
+ (q′)2 (cos q′ sin q′′ cos(θ ′ − θ ′′) − sin q′ cos q′′) + θ ′ cos q′ sin q′′ cos(θ ′ − θ ′′) −
2
(105)
S
− q′q′′ sin(q′′ − q′))ds′ds′′ + ρg ∫ cos q′ds′ + k ∗q = Fq
0
SS
ρ ∫ ∫ (q′ sin q′ cos q′′ sin(θ ′′ − θ ′) +θ ′ cos q′ cos q′′ cos(θ ′′ − θ ′) − (q′)2 cos q′ cos q′′ sin(θ ′′ − θ ′) +
00 (106)
( ) )
+ θ ′ cos q′ cos q′′ sin(θ ′′ − θ ′)− θ ′q′ sin q′ cos q′′ cos(θ ′′ − θ ′) ds′ds′′ + k θ = Fθ
2 ∗
The control forces have the distributed components along the arm, Fθ (s , t ) , Fq (s , t ) ,
s ∈ [0 , L ] that are determined by the lumped torques,
⎧ N
⎪⎪Fθ (s , t ) = ∑ δ (s − il )τ θ i (t )
i =1
⎨ N (107)
⎪Fq (s , t ) = ∑ δ (s − il )τ q (t )
⎪⎩ i =1
i
54 Advanced Strategies for Robot Manipulators
(
τ θi (t ) = pθ1i − pθ2i S ⋅ d 8 ) (108)
( )
τ q i (t ) = pq1i − pq2i S ⋅ d 8 i = 1,2 ,… , N
, (109)
In (107)-(108), pθ1i , pθ2i , pq1i , pq2i represent the fluid pressure in the two chamber pairs, θ , q
and S, d are section area and diameter of the cylinder, respectively (Fig. 17).
τθ Li
ERF
1
pθ i pθ i 2
r r
u θ 1i uθ 2i
Fig. 17. The cylinder driving
The pressure control of the chambers is described by the equations:
dpθki
a ki (θ ) = uθki (110)
dt
dpqik
bki (q ) = uqki
dt , k = 1,2 , i = 1,2 ,… , N (111)
where aki (θ ) , bki (q ) are determined by the fluid parameters and the geometry of the
chambers and
The control problem of a grasping function by coiling is constituted from two subproblems:
the position control of the arm around the object-load and the force control of grasping
(Chiaverini et al., (1996). We consider that the initial state of the system is given by
ω0 = ω (0 , s ) = [θ 0 , q0 ]T (113)
C 0 : (θ 0 (s ), q0 (s )) , s ∈ [0 , L ] (114)
Hyper Redundant Manipulators 55
Initial
Position (C0) 0b s
load
Cb
Fid
Grasping
Desired elements X
Position (Cd)
(a) (b)
Fig. 18. (a) The grasping position; (b) The grasping parameters
The desired point is represented by a desired position, the curve Cd that coils the load,
ωd = [θ d , q d ]T (115)
C d : (θ d (s ), q d (s )) , s ∈ [0 , L ] (116)
In a grasping function by coiling, only the last m elements (m < N) are used. Let lg be the
n
active grasping length, where l g = ∑ li . We define by e p (t ) the position error
i =m
L
e p (t ) = ∫ ((θ (s , t ) − θ b (s )) + (q(s , t ) − q b (s )))ds (117)
L −l g
It is difficult to measure practically the angles θ , q for all s ∈ [0 , L ] . These angles can be
evaluated at the terminal point of each element. In this case, the relation (117) becomes
N
e p (t ) = ∑ ((θ i (t ) − θ bi ) + (qi (t ) − qbi )) (118)
i =m
The error can also be expressed with respect to the global desired position Cd
N
e p (t ) = ∑ ((θ i (t ) − θ di ) + (q i (t ) − q di )) (119)
i =1
The position control of the arm means the motion control from the initial position C0 to the
desired position Cd in order to minimize the error. An area reaching control problem is
discussed. The desired area is specified by the inequality function:
f (δr ) ≤ 0 (120)
⎛ ∂VPT ⎞
τ θ i (t ) = − kθ i eθ i (t ) − kθ∗i eθ2i (t ) − max ⎜⎜ 0 , ⋅ k Pθ a ∗ (θ i , q i )⎟⎟ (121)
⎝ ∂r i
⎠
56 Advanced Strategies for Robot Manipulators
Theorem 1. The closed-loop control system for the desired reaching area problem is stable if
the control forces are
(
τ θ i (t ) = − kθ i eθ i (t ) − kθ∗i eθ2i (t ) − max 0 , ∂VPT ∂r ⋅ kPθ a∗ (θ i , qi )
i
) (122)
(
τ θ i (t ) = − kq i eq i (t ) − kq∗i eq2i (t ) − max 0 , ∂VPT ∂r ⋅ kPq a∗ (θ i , qi )
i
) (123)
Theorem 2. The closed-loop control system of the position (107)-(108), (110)-(111) is stable if
the fluid pressures control law in the chambers of the elements given by:
(
uθ ji ( t ) = − a ji (θ ) kθj i1 eθ i ( t ) + kθj i2 eθ i ( t ) ) (124)
(
uqji ( t ) = −b ji (θ ) kqij 1 eqi ( t ) + kqij 2 eqi ( t ) ) (125)
( )
pqi1 ( 0 ) − pqi2 ( 0 ) = kqi11 − kqi21 eqi ( 0 ) (127)
eθ i ( 0 ) = 0 (128)
and the coefficients kθ i , kqi , kθmni , kqimn are positive and verify the conditions
The grasping by coiling of the continuum terminal elements offers a very good solution in
the fore of uncertainty on the geometry of the contact surface. The contact between an
element and the load is presented in Fig. 19. It is assumed that the grasping is determined
by the chambers in θ -plane. The relation between the fluid pressure and the grasping forces
can be inferred for a steady state from,
l
∂ 2θ ( s ) l s
d
∫k ds + ∫ f ( s ) Tθ ( s ) ∫ T Tθ ( s ) ds = ( p1 − p2 ) S (132)
0 ∂s 2 0 0 8
mi Δθ i + c i Δθ i + H i (θ id + Δθ i , θ id , q d ) − H (θ id , q d ) = di ( f i − Fei ) (133)
Hyper Redundant Manipulators 57
si+1
Grasping
Δ element
si
fi
kL
Load
Transducer
The aim of explicit force control is to exert a desired force Fid . If the contact with load is
modelled as a linear spring with constant stiffness kL , the environment force can be
modelled as Fei = kL Δθ i . The error of the force control may be introduced as
mi c ⎛h ⎞ ⎛h ⎞
e fi + i e fi + ⎜ i + di ⎟ e fi = di f i − ⎜ i + di ⎟Fid (136)
kL kL ⎝k ⎠ ⎝ k ⎠
Theorem 3. The closed force control system is asymptotic stable if the control law is
fi =
1
k L di
(( ) )
hi + kL di + miσ 2 e fi − (hi − kL di )Fid , c i > miσ (137)
58 Advanced Strategies for Robot Manipulators
6. Conclusion
The research group from the Faculty of Automation, Computers and Electronics, University
of Craiova, Romania, started working in research field of hyper redundant robots over 25
years ago. The experiments used cables and DC motors or stepper motors. The rotation of
these motors rotates the cables which by correlated screwing and unscrewing of their ends
determine their shortening or prolonging, and by consequence, the tentacle curvature.
The inverse kinematics problem is reduced to determining the time varying backbone curve
behaviour. New methods for determining “optimal” hyper-redundant manipulator
configurations based on a continuous formulation of kinematics are developed.
The difficulty of the dynamic control is determined by integral-partial-differential models
with high nonlinearities that characterize the dynamic of these systems. First, the dynamic
model of the system was inferred. The method of artificial potential was used for these
infinite dimensional systems. In order to avoid the difficulties associated with the dynamic
model, the control law was based only on the gravitational potential and a new artificial
potential.
The control system is an image – based visual servo control. Servoing was based on
binocular vision, a continuous measure of the arm parameters derived from the real-time
computation of the binocular optical flow over the two images, and is compared with the
desired position of the arm. The method is based on the particular structure of the system
defined as a “backbone with two continuous angles”. The control of the system is based on
the control of the two angles. The error angle was used to calculate the spatial error and a
control law was synthesized. The general control method is an image based visual servoing
one instead of position based. By consequence, camera calibration based on intrinsic
parameters is not necessary („calibration“ in the classic sense of the term, not the one used
in this paper). The term “camera calibration” in the context of this paper refers to
positioning and orienting the two cameras at imposed values. This calibration is performed
only at the beginning, after that the cameras remain still.
A new application investigates the control problem of a class of hyper-redundant arms with
continuum elements that performs the grasping function by coiling. The control problem of
a grasping function by coiling is constituted from two subproblems: the position control of
the arm around the object-load and the force control of grasping.
7. Acknowledgement
The research presented in this paper was supported by the Romanian National University
Research Council CNCSIS through the IDEI Research Grant ID93 and by FP6 MARTN
through FREESUBNET Project no. 36186.
8. References
Blessing, B.; & Walker, I.D. (2004). Novel Continuum Robots with Variable- Length Sections,
Proc. 3rd IFAC Symp. on Mechatronic Systems, Sydney, Australia, pp. 55-60.
Boccolato, G.; Dinulescu, I.; Predescu, A.; Manta, F.; Dumitru, S.; & Cojocaru, D.; (2010). 3D
Control for a Tronconic Tentacle, 12th International Conference on Computer Modelling
and Simulation, p380-386, ISBN 978-0-7695-4016-0,
Hyper Redundant Manipulators 59
Singh, S.K. & Popa, D.O. (2005) An Analysis and Some Fundamental Problems in
Adaptive Control of Force, IEEE Trans. on Robotics and Automation, Vol. 11 No 6, pp
912-922.
Suzumori, K.; Iikura, S.; & Tanaka, H. (1991). Develop. of flexible microactuator and its appl.
to robot mech, IEEE Intl. Conf. on Rob. and Autom., Sacramento, pp. 1564 - 1573.
Takegaki, T.; & Arimoto, S. (1981). A new feedback methods for dynamic control of
manipulators, Journal of Dynamic Systems, Measurement and Control, pp. 119-125.
Tanasie, R.T.; Ivănescu, M. & Cojocaru, D. (2009). Camera Positioning and Orienting for
Hyperredundant Robots Visual Servoing Applications, Journal of Control
Engineering and Applied Informatics, ISSN 1454-8658, Vol 11, No 1, p19-26.
Walker, I.D., Dawsona, D.M. & a.o. (2005), Continuum Robot Arms Inspired by
Cephalopods, DARPA Contr. N66001-C-8043, http://www.ces.clemson.edu.
Walker, I.D. & Carreras, C. (2006) Extension versus Bending for Continuum Robots, Internl.
Journal of Advanced Robotic Systems, Vol. 3, No.2, ISSN 1729-8806, pp. 171-178.
Wang, P.C. (1965). Control of distributed parameter systems, Advance in Control Systems,
Academic Press.
3
1. Introduction
Neurosurgery is a part of the surgical field that focused in taking care of the diseases related
to human’s central peripheral nervous system and also their central spinal cord [20]. The
term surgery refers to the operation of peripheral nervous system as well as the spinal cord,
brain, blood vessel connected to it, spine, spinal cord, and also nerves that control our senses
and body’s movement [29]. There are lots of neuro diseases, which among them were brain
tumors, head trauma, stroke, thalamic astrocytomas, and spinal cord trauma. These
diseases, if not thrown away, will results the patient in body disorder, health problem, and
of course, death. To put an end to these disorders, appropriate treatment is mandatory.
Those diseases need to be cured and removed. Surgery, or specifically neurosurgery, is one
of the effective methods to treat it.
Neurosurgery comes with risks. Any operation dealing with brain or the spinal cord can
cause paralysis, brain damage, infection, psychosis, or even death if a mistake happens.
These operations are also likely to cause mental impairment as of any surgical procedure
dealing with the brain. Therefore, it is vital for neurosurgeon to make sure that this kind of
surgery is performed in an almost perfect condition to minimize any risks or poor results as
the consequences from it. Traditionally, starting from scalp removing, drilling and removing
the skull, handling the lump, until sewing the skull and scalp back at its original location;
surgeons put their efforts with their own hands and bare eyes. Tools and equipments did
improved, for example with the usage of apparatus such as top-mount microscope and
magnetic resonance imaging (MRI) machine. However, they still need to manipulate the
surgical tools, the closest tools to the human brain, such as the knife and biopsy needles
with naked hands. As a results, it will surely introduced limits to the tools manipulation.
This is where robots can do a lot better. A very precise robotic device that can perform
manipulation at much smaller or micro scale, plus the capability of the surgeon himself, will
produce much superior results. These robotic devices are termed surgical micro-
manipulator.
This chapter presents readers with information regarding the design of a micro-manipulator
purposely for neurosurgical application. It also shares beneficial facts and particulars
regarding current progress about micro-manipulator research around the globe. This
chapter is organized as the following: Section 2 provides details justification of designing a
robotic hand in an operating room based on the constraints for a neurosurgical procedures.
Section 3 will discuss design considerations for a micro-manipulator for neurosurgery. This
62 Advanced Strategies for Robot Manipulators
includes the important hardware and software elements that contributed to the build-up of
a micromanipulator. Section 4 briefly shares on the design and uniqueness of one of the
recent and successful micro-manipulator for neurosurgical application. Section 5 will finally
conclude this article.
(a) (b)
Fig. 1. (a) Graphical illustration of brain tumor. A primary brain tumor is a mass created by
the growth or uncontrolled proliferation of cells in the brain. (b) Spinal tumor
2. Why robotics
can be very precise and reliable because robot can filter the handshakes and keep the
operation steady.
Another reason surgeons need to use such a system is that it can provide them with a
minimally invasive surgery (MIS). This provides less trauma for the patient after the surgery
and of course, a shorter recovery period. Moreover, human involvement is also a concern. In
today's operating rooms, you'll find two or three surgeons, an anesthesiologist and several
nurses, all needed for even the simplest of surgeries. Most surgeries require nearly a dozen
people in the room. As with all automation, surgical robots will eventually eliminate the
need for some personnel. Taking a glimpse into the future, surgery may require only one
surgeon, an anesthesiologist and one or two nurses. In this nearly empty operating room,
the doctor its at a computer console, either in or outside the operating room, using the
surgical robot to accomplish what it once took a crowd of people to perform.
The first use of robot in a neurosurgical procedure is in 1985, according to [30]. Researches
from Department of Radiology, Memorial Medical Center employed a PUMA
(Programmable Universal Machine for Assembly) robot in the operating room. Even though
the task of the robot at that time is only to hold and manipulate biopsy cannulae, it marked
the start of a robot’s manipulator cooperation inside the operation room. Since then, various
researches in various aspect of neurosurgery have been explored. Those included the
micromanipulators design [22], vision and imaging scheme, sensors design [16], haptic
technology [9], magnetic resonance imaging (MRI) compatibility equipments, telesurgery
system [15], as well as controller technique and planning.
(a) (b)
Fig. 3. (a) A micro-manipulator for surgery, deisgned by Francesco Cepolina, [4] & [7]
(b)NeuroMaster, a stereotactic neurosurgery robot system by Beihang University, [13] & [18]
64 Advanced Strategies for Robot Manipulators
necessarily that the tool and manipulator must be small, but the whole system itself must be
able to integrate and produce very precise micro-manipulation with a very minimal error in
all the 3- axis’s direction. This includes the sensors parts, vision and imaging system and
also the controller technique.
4. Discussion
The buildup of a neurosurgical type micro-manipulator usually consists of few important
parts or elements, both hardwares and softwares. These elements were essential to ensure
the specification and performance of the robot itself achieved the function of a
micromanipulator. This description may act like a general guideline in developing the
micromanipulator because some of the micro-manipulator might not have all the elements,
because each of them has different specifications and design.
4.1 Modeling
Modeling is a process of using mathematical description to simulate real physical events
[17]. It allows complex systems to be understood and thus their behavior can be predicted
and simulated. In modeling, usually some details will be ignored or assumed, due to the
shortcomings occurred in the process. Micro-manipulator is a very expensive system to
build.
Micro-Manipulator for Neurosurgical Application 65
Thus, it needs to be modeled before it is fabricated. From the model, we can investigate
much information, such as the kinematic and dynamic behavior of the model, the
workspace of the model, materials to build the model, suitable actuator to achieve the
design objectives and the controller technique that is most efficient to the system. There are
lots of software that can be use to model a micro-manipulator system, including Robotics
Toolbox® and SimMechanics from Matlab, AutoCAD, SolidWorks and Rhinoceros®.
One common technique to model a manipulator system is to use Denavit-Hartenberg (DH)
method [12]. From the DH model, either the classical or the modified version of it, we can
simulate and further investigate the behavior of the micro-manipulator, both the kinematic
and dynamic. Kinematics relates the motion behavior of the robot without regards to the
forces that causes it, whereas dynamic considers the effect of internal and external forces or
torques applied to the micro-manipulator. With the information from both the kinematic
and dynamic behavior, we can have a good knowledge on how the micro-manipulator
moves, which path it follows, how many micro-Newton of forces applied at the patient’s
head, how precise the robot is, as well as the speed of the robot movements. Those are
among the vital information needed by the surgeon during his usage of the micro-
manipulator during a surgery. In addition, we can always estimate the workspace of the
robot. Workspace is the region where the end effector of the micro-manipulator can possibly
reach, which a surgeon needs to know prior to an operation to estimate the tools
arrangements and movements.
Equation 1 below represents the transformation matrices associated with modified DH
method. The parameters ‘α’ and ‘θ’ represents the angular behavior of the
micromanipulator’s links, while the ‘a’ and ‘d’ parameters represents the prismatic aspect.
⎡ cosθ i − sin θ i 0 α i −1 ⎤
⎢ ⎥
sin θ i × cos α i − 1 cosθ i × cos α i − 1 − sin α i − 1 − sin α i − 1 × di ⎥
i-1
Ti = ⎢ (1)
⎢ sin θ i × sin α i − 1 cosθi × sin α i − 1 cosα i − 1 cos α i − 1 × di ⎥
⎢ ⎥
⎣⎢ 0 0 0 1 ⎦⎥
where;
τ = torques vector
M = inertia matrix
C = Coriolis and Centrifugal matrix (these are types of internal forces)
N = gravity terms and other forces act on the joints (all external forces defines here)
There are two categories of trajectory planning techniques. They are joint space technique
and Cartesian space technique. Joint space technique is suitable for point-to-point motion,
where the motion planning is done at the joint level. This technique describes the time
function of all the joints’ variables including the speed and acceleration. Equation 3 below
shows an example of a five-degree or quintic polynomial equations with its joints’ variables,
Ci. The position of the end effector was computed by using forward kinematics. This is just
one of many smooth functions that can be used to interpolate a trajectory.
Cartesian space technique is a method that most suited with continuous path type of
motion, and therefore suit neurosurgical application better than the previous technique.
While joint space method focus on the joint position, Cartesian space method controls the
end effector itself, with respect to the base of the robot. By using inverse kinematics, the
joints variables were computed.
In general, to set a trajectory, we must define the starting and end points, as well as the
mathematical function that the joints and end effector will undertake during its movement.
The time taken to complete the trajectory is also important as it will affect the speed and
smoothness of the manipulation. This is important in designing a micro-manipulator since
we need to specify each and every point on the route of the end-effector and the joints as
well. Failing to do this will let us lose control of the surgical tools.
where;
c0 = θ0
c = θ
1 0
θ
0
c2 =
2
20θ f − 20θ 0 − (8θf + 12θ0 )t f − (3θ0 − θf )t 2f
c3 =
2t 3f
30θ0 − 30θ f + (14θf + 16θ0 )t f − (3θ0 − 2θf )t 2f
c4 =
2t 4f
12θ f − 12θ 0 − (6θf + 6θ0 )t f − (θ0 − θf )t 2f
c5 =
2t 5f
4.3 Actuator
This is an equipment that allows a robot to move by conversion of different energy types
such as electrical or mechanical processes [26]. This includes human muscles, propellers,
and hydraulic cylinders. Actuators are very important because it’s the main mechanism to
make the robot being in specified motion. For micro-manipulator, type of actuator that is
widely used is the electrically controlled motors. Electrical motors are favored because it is
much more precise and accurate in terms of their generated motion, as compared to the
hydraulic and pneumatic-actuated motor.
Accuracy to the highest level is very important for a micro-manipulator design. Thus, the
selection of a suitable actuator is very important. In [1], Adha Cahyadi et al use piezo
Micro-Manipulator for Neurosurgical Application 67
electric actuator in their micro-manipulator design. With the special capability of piezo
material, it can produce a very fine displacement, down till micrometers. This shows the
importance of selecting the right actuator. It really depends on the micro-manipulator
design. Moreover, suitable controller implementation technique can also contribute well to
manage the output of an actuator.
4.4 Sensors
Sensors are very important devices that allow the analog world to communicate with the
digital environment. By definition, it converts physical signals such as heat, light, sound,
rotary motion, and force into electrical signal [24]. The resulting electrical signal will be send
to the controller and the required calculation or assigned resulting action will be taken. A
precise rotary encoder for example, can provide the system with the exact location of each
joints and the end effector. Then by using various types of control method, their locations
can be corrected if it does slightly differ from the desired one.
In micro-manipulator design, there are many types sensor that is very useful to be
incorporated with. Among them is vision or imaging sensor and force or haptic devices.
Imaging is a key element for a robotic neurosurgery. It can be used during the registration
process before the surgery to organize the surgery through coordinate relationship between
the robot frame and the patient head’s frame [13]. In addition, MRI images can also help the
surgeon. Using images from the MRI and special software, the patient head can be redraw
in the computer, and allows the surgeon to use it for a rehearsal before the real surgery [8].
From the MRI images as well, the location of the tumor can be captured and locked for
operation. This can act as a simulation tool for the surgeon, as well as it can help the surgeon
during the registration process.
On the other hand, the term haptic is referring to the tactile or sense of touch information
that is required during a surgery [11]. By using the tele-operated surgery system, this kind
of information is not there with the surgeon. Thus, haptic or force feedback sensor must be
put at the slave robot so that the surgeon who is manipulating the master robot can have
better control and awareness of the operation undertaken. Excessive force applied on the
head of a patient might well damage important tissues or nerves.
4.5 End-effector
It is a device or tool specifically designed and attached to the last link of the robot or to the
robot wrist [25]. It enables the robot to perform its intended tasks, for example cutting the
skull or taking samples inside a body. The end-effector is loosely comparable to a human's
hand. Its size is depending on the tasks assigned and also the working area. For a MIS
operation, the end effector must be small enough to get to the body through the specified
path or hole created.
(a) (b)
Fig. 4. (a) NeuroArm workstation. (b) NeuroArm surgical robot.
Micro-Manipulator for Neurosurgical Application 69
The master and the surgeon workstation are placed in a room adjacent to the operation
room, while the slave or the robot itself was put inside the operation room. The master and
slave are bit different in design, where the master was only a 6-dof controller with 3-dof
positional force feedback. The workstation also consists of high-resolution binoculars where
the surgeon can have direct access to the surgical binoculars, haptic hand-controllers,
microphone to communicate with the operation room’s personnel, and four monitors that
display the information needed to know by the surgeon of the surgery that is going on. The
master manipulator, as shown in Figure 6 has high-fidelity haptic capability, so the surgeon
that is using the master controller will know the force currently being exerted to the patient
at real time. In addition, tremor filter was installed in the system, to improve accuracy and
precision as well as the stamina of the surgeon. This also enhances the surgical ability of the
robot. The force exerted during operation can be limited. And for security purpose, if the
robot fails itself, intrinsic braking system will automatically freeze the robot. Besides, the
robot’s actuators are functioning at low torque and low force in order to reduce any risks of
injury. It can also move on a slow pace as 1mm/s, and can go up to 200mm/s, depending on
the needs.
the MRI is not significantly affected by the surgical tools. Even though some of the
procedure is still being done by the surgeon himself, like burr holes and cranial exposure,
NeuroArm has started creating its milestones in neurosurgery. With tested accuracy of
30micron, it is so great that in will surely enhance surgical capability. In addition to that,
NeuroArm has successfully performed a neurosurgical operation in May 2008. The
operation is to remove a tumor from a 21-year old’s women brain in United States of
America.
6. Conclusion
Researches on various aspects of neurosurgical operation were still going on. The purpose is
to improve the existing system and also human lives. While surgical robots offer some
advantages over the human hand, we are still a long way from the day when autonomous
robots will operate on people without human interaction. With advances in our technology,
it is not an impossible thing to be done. However, besides the excitement in this research
field, the safety of the patient inside the operation theatre must be the highest priority.
7. References
[1] Adha Cahyadi and Yoshio Yamamoto, Hysteretic Modelling of Piezoelectric Actuator
Attached on Flexure Hinge Mechanism, IEEE International Conference on
Intelligent Robots and Systems, Beijing, China, 9-15 October 2006
[2] Alexander D. Greer, Perry M. Newhook, and Garnette R. Sutherland, Human–Machine
Interface for Robotic Surgery and Stereotaxy, IEEE/ASME Transactions on
Mechatronics, Vol. 13, No. 3, JUNE 2008
[3] Da Liu and Tianmiao Wang, A Workflow for Robot Assisted Neurosurgery, IEEE
International Conference on Intelligent Robots and Systems, Beijing, China, 9-16
October 2006
[4] Damien Sallé, Francesco Cepolina and Philippe Bidaud, Surgery grippers for Minimally
Invasive Heart Surgery, Proceedings of IEEE International Conference on
Intelligent Manipulation and Grasping, Genova, Italy, 1-2 July 2004
[5] da Vinci Surgical System, http://en.wikipedia.org/wiki/Da_Vinci_Surgical_System
Micro-Manipulator for Neurosurgical Application 71
[6] Deon F. Louw, Tim Fielding, and Paul B. McBeth et al, Surgical Robotics: A Review and
Neurosurgical Prototype Development, Neurosurgery, Vol. 54, No. 3, March 2004
[7] F. Cepolina, Development of Micro Tools for Surgical Applications, Ph.D. Thesis,
Universita’ Degli Studi De Genova/ Uiversite Piere Et marie Currie: Paris 2005
[8] Garnette R. Sutherland, Isabelle Latour, and Alexander D. Greer, Integrating an Image-
Guided Robot with Intraoperative MRI, IEEE Engineering in Medicine and Biology
Megazine, May/June 2008
[9] Ho-seok Song, Ki-young Kim and Jung-ju Lee, Development of the Dexterous
Manipulator and the Force Sensor for Minimally Invasive Surgery, IEEE
International Conference on Autonomous Robots and Agents, Wellington, New
Zealand, 10-12 February 2009
[10] How Robotic Surgery Will Work,
http://science.howstuffworks.com/roboticsurgery.htm/printable
[11] James C. Gwilliam, Mohsen Mahvash and Balazs Vagvolgyi et al, Effects of Haptic and
Graphical Force Feedback on Teleoperated Palpation, IEEE International
Conference of Robotics and Automation, Kobe, Japan, 12-17 May 2009
[12] John J. Craig, Introduction to Robotics Mechanics and Control, 3rd edition, 2005,
Pearson Prentice Hall
[13] Junchuan Liu, Yuru Zhang, Zengmin Tian, Tianmiao Wang and Hongguang Xing,
NeuroMaster: A Robot System for Neurosurgery, IEEE International Conference on
Robotics and Automation, 2004
[14] Junchuan Liu, Yuru Zhang and Zhen Li, The Application Accuracy of NeuroMaster:A
Robot System for Stereotactic Neurosurgery, Proceedings of the 2nd IEEE/ASME
International Conference on Mechatronic and Embedded Systems and
Applications, 2006
[15] Koji Ikuta, Keiichi Yamamoto and Keiji Sasaki, Development of RemoteMicrosurgery
Robot and New Surgical Procedure for Deep and Narrow Space, IEEE International
Conference on Robotics and Automation, Taipei, Taiwan, 14-19 September 2003
[16] M. Tanimoto, F. Arail and T. Fukuda et al, Micro Force Sensor for Intravascular
Neurosurgery and In Vivo Experiment, The Eleventh Annual International
Workshop on Micro Electro Mechanical Systems, 1998
[17] Mathematical Model, http://en.wikipedia.org/wiki/Mathematical_model
[18] Meng Cai, Wang Tianmiao, Chou Wusheng and Zhang Yuru, A Neurosurgical Robotic
System under Image-Guidance, IEEE International Conference on Industrial
Informatics, 2006
[19] NeuroArm, http://www.neuroarm.org/project.php
[20] Neurosurgery, http://en.wikipedia.org/wiki/Neurosurgery
[21] Paul B. McBeth, Deon F. Louw, Peter R. Rizun and Garnette R. Sutherland, Robotics in
Neurosurgery, The American Journal or Surgery 188 (Suppl to October 2004)
[22] Peter R. Rizun, Paul B. McBeth, Deon F. Louw and Garnette R. Sutherland, Robot-
Assisted Neurosurgery, http://www.sagepublications.com
[23] R. K. Mittal and I. J. Nagrath, Robotics and Control; 2005, Tata McGraw Hill
[24] Robot and Robotic Glossary,
http://www.kcrobotics.com/robot_information/robot_glossary.php
[25] Robot End Effector, http://en.wikipedia.org/wiki/End_effector
72 Advanced Strategies for Robot Manipulators
1. Introduction
A fully decoupled parallel manipulator is a mechanism in which one output kinematic joint,
degree of freedom, is affected by only one active or input kinematic pair, the perfect
mechanism from a kinematic point of view due to the possibility to generate linear input–
output kinematic constraint equations. Parallel manipulators with fewer than six–degrees–
of–freedom frequently referred as limited–dof or defective parallel manipulators were the
first class of parallel manipulators to be considered in that trend. Kong & Gosselin (2002a)
introduced a class of translational fully decoupled parallel manipulators called Tripteron
family. Carricato & Parenti–Castelli (2004) invented a two–degrees–of–freedom parallel
wrist in which two interconnected linkages independently actuate one of the two angles
associated to the orientation of the moving platform. Recently, Briot & Bonev (2009)
proposed a fully decoupled translational parallel manipulator, called Pantopteron, for
simple pick–and–place operations. Certainly, there is a significative number of contributions
dealing with the study of limited–dof fully decoupled parallel manipulators, see for instance
Carricato & Parenti–Castelli (2002), Kong & Gosselin (2002b, 2002c), Gosselin et al., (2004),
Gogu (2005), Li et al., (2005), Ruggiu (2009) and so on. On the other hand, a fully decoupled
six–degrees–of–freedom parallel manipulator is maybe, still in our days, an unrealistic task.
In fact, the dream that in a Gough–Stewart platform one degree of freedom shall be affected
by only one active kinematic joint is a far away reality, if sensors are not considered. In
order to diminish such drawback, the term fully can be removed from the original concept
meaning that a decoupled parallel manipulator is a mechanism in which the position and
orientation, pose, of the moving platform with respect to the fixed platform can be
computed separately. The decoupled motion can be achieved by introducing geometric
conditions, e.g. Wohlhart (1994) studied a Gough–Stewart platform in which three of the six
limbs share a common spherical joint over the moving platform, other topologies with
uncoupled rotations and translations were investigated by Innocenti & Parenti–Castelli
(1991), Zabalza et al., (2002), Yang et al., (2004), Takeda (2005) and so on. Despite the
indisputable recent valuable advances in this subject, the development of decoupled parallel
manipulators with simplified architectures preserving the well–known benefits of parallel
manipulators such as higher stiffness and payload/capacity is a rather complicated task. At
this point, and mainly due to the lack of an efficient mathematical resource to approach the
forward kinematics of a general Gough–Stewart platform capable to determine the actual
configuration of the manipulator, without using sensors, one can take into account that if
74 Advanced Strategies for Robot Manipulators
there is not essential the fully decoupled motion, then the development of partially
decoupled parallel manipulators is a viable option to apply the benefits of mechanisms with
nearly parallel kinematic structures, see for instance Briot et al., (2009), Altuzarra et al.,
(2010). It is interesting to note that mechanisms with mixed motions can be included in the
class known as partially decoupled parallel manipulators.
In this chapter a new family of partially decoupled parallel manipulators endowed with an
extra active kinematic joint is introduced. One member of this new family of robot
manipulators is selected with the purpose to illustrate the methodology of kinematic
analysis chosen to characterize the angular and linear kinematic properties, up to the
acceleration analysis, of it. The forward position analysis of the robot, a challenging task for
most parallel manipulators, is carried–out in a semi–closed form solution applying
recursively the Sylvester dialytic elimination method that allows to determine all the
feasible locations that the output platform can reach with respect to the fixed platform given
a set of generalized coordinates. On the other hand, the velocity and acceleration analyses of
the robot are approached by means of the theory of screws. With this mathematical tool,
simple and compact expressions for computing the velocity and reduced acceleration states
of the output platform are obtained taking advantage of the properties of reciprocal screws,
via the Klein form of the Lie algebra e (3). Finally, the robot is simulated as a virtual five–
degrees–of–freedom parallel kinematic machine using special commercially available
software like ADAMS©.
Fig. 2. D1, a member of the DeLiA robot family and its geometric scheme
where n is the number of links, j is the number of kinematic pairs, and fi is the number of
freedoms of the i–th kinematic pair. For the robot D1 n = 21, j = 6R+9P+3U+9S = 27,
∑
j
i =1
fi = 48 and therefore F = 6, which is a wrong result. In fact, Dai et al. (2006) proved that
in a 3–RPS parallel manipulator a basis representing the motions of the moving platform,
with respect to the fixed platform, consists of three elements, two non–parallel coplanar
rotations and one translation along an axis perpendicular to the plane formed by the
spherical joints. According to this basis, the coupler platform of the robot D1 cannot rotate
with respect to the fixed platform along an axis perpendicular to the plane formed by the
spherical joints attached at the coupler platform. It is straightforward to demonstrate that
such argument is valid too for the end– effector–platform and the coupler platform, in other
words, the end–effector– platform has a rotation restricted with respect to the coupler
platform due to the passive 3–RPS parallel manipulator connecting both platforms and the
coupler platform has a rotation restricted with respect to the fixed platform due to the 3–
RPS active parallel manipulator. With these considerations in mind, although the computed
degrees of freedom of the robot at hand is six, the end–effector–platform does not accept
arbitrary orientations with respect to the fixed platform, and therefore D1 is in reality a five–
degrees–of–freedom redundant robot.
4. Finite kinematics
In this section the position analysis of the proposed robot is presented.
DeLiA: A New Family of Redundant Robot Manipulators 77
( S 1 − R1 ) • ( R 2 − R 3 ) = 0 ⎫
⎪ (2)
( S 2 − R 2 ) • ( R 3 − R1 ) = 0 ⎬
( S3 − R3 ) • ( R2 − R1 ) = 0 ⎭⎪
where the dot (•) denotes the usual inner product of the three–dimensional vectorial
algebra. Furthermore, three closure equations can be written as
( Si − Ri ) • ( Si − Ri ) = q i2 i = 1, 2, 3 (3)
Finally, according to the triangle ΔS1S2S3 three compatibility equations are given by
2
( Si − S j ) • ( Si − S j ) = S i S j i , j = 1, 2, 3 mod(3) (4)
Expressions (2), (3) and (4) are solved by applying the Sylvester dialytic elimination method
(Tsai, 1999; Gallardo et al., 2007). Once the coordinates of the points Si(i = 1, 2, 3) are
calculated, the center of the triangle ΔS1S2S3, vector AρB, results in
A
ρ B = ( S1 + S2 + S3 ) / 3 (5)
Finally, the pose of the coupler platform with respect to the fixed platform is summarized in
the 4 × 4 homogeneous transformation matrix ATB:
⎡ ARB A
ρB⎤
A
TB = ⎢ ⎥ (6)
⎣ O 1× 3 1 ⎦
where ARB is the rotation matrix which is computed by means of the coordinates of the points
Si(i = 1, 2, 3), for details see Gallardo–Alvarado et al. (2008). Following a similar procedure, the
homogeneous transformation matrix of the end–effector–platform with respect to the fixed
platform, ATC, is computed. To this end, consider the following closure equations
( S 4 − R 4 ) • ( R 5 − R6 ) = 0 ⎫
( S 5 − R 5 ) • ( R6 − R 4 ) = 0 ⎪
⎪
( S 6 − R6 ) • ( R 5 − R 4 ) = 0 ⎪ (7)
⎬
( Si + 3 − U i ) • ( Si + 3 − U i ) = q i2+ 3 i = 1, 2, 3 ⎪
2
⎪
( Si − S j ) • ( S i − S j ) = S i S j i , j = 4, 5, 6 mod(3) ⎪⎭
78 Advanced Strategies for Robot Manipulators
where the vectors Ri(i = 4, 5, 6) are computed by using the matrix ATB. Furthermore, the
homogeneous transformation matrix between the end–effector– platform and the fixed
platforms, BTC, can be calculated from
A
TC = A T B B T C (8)
⎡Si ⎤ A C ⎡si ⎤
⎢ 1 ⎥ = T ⎢1⎥ (9)
⎣ ⎦ ⎣ ⎦
where si(i = 4, 5, 6) is the i–th point but expressed according to the moving reference frame
xyz. Furthermore, the unknown vectors Ri(i = 4, 5, 6) and Si(i = 1, 2, 3) can be computed
according to the following closure equations
(S6 − R6 ) • ( R5 − R4 ) = 0 ⎫
(S5 − R5 ) • ( R6 − R4 ) = 0 ⎪
⎪
(S4 − R4 ) • ( R6 − R5 ) = 0 ⎪
⎪
(S1 − R1 ) • ( R2 − R3 ) = 0 ⎪
(S2 − R2 ) • ( R3 − R1 ) = 0 ⎪
⎪
(S3 − R3 ) • ( R2 − R1 ) = 0 ⎬ (10)
⎪
S1 + S2 + S3 = R4 + R5 + R6 ⎪
2 ⎪
(Si − S j ) • (Si − S j ) = Si S j i , j = 1, 2, 3 mod(3) ⎪
2 ⎪
( Ri − R j ) • ( Ri − R j ) = Ri R j i , j = 4, 5,6 mod(3)⎪
2 ⎪
(Si − Ri + 3 ) • (Si − Ri + 3 ) = Si Ri + 3 i = 1, 2, 3 ⎭
Finally, the limb lengths qi(i = 1, 2, . . . , 6) result in
qi2 = (Si − Ri ) • ( Si − Ri ) ⎫⎪
⎬ (11)
qi2+ 3 = (Si + 3 − U i ) • (Si + 3 − U i ) i = 1, 2, 3 ⎪⎭
5. Infinitesimal kinematics
In this section the velocity and acceleration analyses of the proposed robot are approached
by means of the theory of screws. For detailed information of the kinematic analysis of
closed chains and parallel manipulators, using such mathematical resource, the reader is
referred to (Rico & Duffy, 2000; Gallardo et al., 2003). In particular screw theory is an
DeLiA: A New Family of Redundant Robot Manipulators 79
sO = sˆ h + sˆ × rO /P (12)
where h is the pitch of the screw and rO/P is a vector directed from a point P, fixed to the
screw axis, to point O. Note that if the pitch of the screw goes to infinity, then the screw
represents a prismatic joint and it is represented by $ = (0, ŝ ). Any lower kinematic pair can
be represented either by a screw or a group of screws. A cylindrical joint can be simulated
by the combination of one revolute joint and one prismatic joint, whereas a spherical joint
results of the action of three revolute joints whose axes, usually mutually orthogonal,
intersect a common point.
Screw theory, which is isomorphic to the Lie algebra e(3) also referred as motor algebra, is
the set of elements of the form $ = ( ŝ , sO) with the following operations.
Let $1 = ( ŝ 1, sO1), $2 = ( ŝ 2, sO2), and $3 = ( ŝ 3, sO3) be elements of the Lie algebra e(3) with λ1,
λ2, λ3 ∈ ℜ. Then
1. Addition, $1 + $2 = ( ŝ 1 + ŝ 2, sO1 + sO2)
2. Multiplication by a scalar, λ$1 = (λ ŝ 1, λsO1)
3. Lie product or dual motor product, [$1 $2] = ( ŝ 1× ŝ 2, ŝ 1×sO2− ŝ 2×sO1).
The Lie product exhibits interesting properties like
a. Nilpotent, [$1 $1] = (0, 0)
b. Non–commutative, [$1 $2] = −[$2 $1]
c. Distributive
4. Jacobi identity, [$1 [$2 $3]] + [$3 [$1 $2]] + [$2 [$3 $1]] = (0, 0)
Furthermore, the Lie algebra e(3) is endowed with two symmetric bilinear forms
1. The Killing form, ($1; $2) = ŝ 1 • ŝ 2
2. The Klein form, {$1; $2} = ŝ 1 • sO2 + ŝ 2 • sO1
It is said that the screws $1 and $2 are reciprocal if {$1; $2} = 0, an interesting property that
allows to simplify the forward infinitesimal kinematics of parallel manipulators.
Screw theory is a powerful mathematical tool modeling the kinematics of rigid bodies.
The velocity state n VOm of a rigid body m as it is observed from another body n or reference
frame is a twist about screw (Ball, 1900), indeed n VOm = ωn$m, given by
80 Advanced Strategies for Robot Manipulators
n
⎡ nω m ⎤
VOm = ⎢ n m ⎥ (13)
⎣ vO ⎦
where nωm and n VOm are, respectively, the angular and linear velocities of the body under
study and O is a point of the body m that is instantaneously coincident with the origin of the
reference frame n, point O is also known as the reference pole. Furthermore, in an open
kinematic chain, e.g. a serial manipulator, the velocity state of the end–effector, labeled body
m, with respect to the base link, labeled body n, can be written as a linear combination of the
involved infinitesimal screws associated to the kinematic pairs as follows
⎡ ω
n m
⎤
n
AOm =n ωm n $ m = ⎢ n m n m n m ⎥ (15)
⎣ aO − ω × vO ⎦
where ω and n aOm are the angular and linear accelerations of body m with respect to body n
taking O as the reference pole. Furthermore, in a serial manipulator the reduced acceleration
state of the end-effector with respect to the base link is given by
+[ n + 1 ωn + 2 n + 1 $ n + 2 + n + 2 ωn + 3 n + 2 $ n + 3 + … + m − 1 ωm m − 1 $ m ]
+ … + [ m − 2 ωm − 1 m − 2 $ m − 1 m−1 ωm m − 1 $ m ] (17)
is the Lie screw or complementary six-dimensional vector of the reduced acceleration state.
It is worth mentioning that eventhough its compactness, Eq. (16) contains all the terms
involved in the acceleration analysis of a rigid body. In fact, e.g. Eq. (16) contains the terms
of the acceleration of Coriolis and one not need to make a distinction of it. Furthermore, Eq.
(16) can be easily translated into computer codes approaching the kinematic analysis of
robot manipulators. This expression was introduced by the first time by Rico–Martínez &
Duffy (1996) and its correctness was validated by the author of this work with the
publication of several papers in well known journals. Before the pioneering contribution of
Rico–Martínez & Duffy (1996) the screw theory was confined to the so–called first order
analysis (velocity analysis). Its introduction almost fifteen years ago open the possibility to
extend the screw theory to the so–called higher order kinematic analyses.
Spherical + Spherical, these spherical joints require each one more of the usual three
infinitesimal screws indicating concurrent revolute joints. Furthermore, before do any
further, in order to solve the inverse velocity and acceleration analyses, it is necessary the
introduction of auxiliary screws with the purpose to satisfy an algebraic requirement, with
this consideration in mind the revolute joints are modeled as cylindrical joints, in
which the corresponding translational velocities are equal to zero. In other words,
i i
0 ω 1 =6 ω 7 = 0(i = 1, 2, 3) .
where A VOB is the velocity state of the coupler platform with respect to the fixed platform,
and B VOC is the velocity state of the end–effector–platform with respect to the coupler
platform. Furthermore, these kinematic states can be written in screw form as
A C 13 14 15 15 16 16 17 17 18 ⎫
J = [ 12 $ i ,13 $ 14 i , $i , $i , $i , $i ]
i
⎪
B C 7 9 ⎪
J i = [ 6 $ i ,7 $ 8i ,8 $ i ,9 $ 10 10 11 11 12
i , $i , $i ] ⎬ (20)
A B 0 1 1 2 2 3 3 4 4 5 5 6
⎪
Ji = [ $ i , $ i , $ i , $ i , $ i , $ i ] ⎪
⎭
whereas Ω i∈ { A Ω iB ,B Ω Ci , A Ω Ci } are matrices containing the joint rate velocities. In fact:
ω18i ]T ⎫
i
Ω Ci = [ 12 ω 13 13ω14 14ω 15 15ω16 16ω17
A i i i i
17
⎪
i ⎪
Ω Ci = [ 6 ω 7 7ω8i 8ω 9 9ω10
i
10ω11 11ω12 ]
B i i i T
⎬ (21)
i ⎪
A
Ω iB= [ 0 ω 1 1 ω2i 2ω i3 3ω4i 4ω5i 5ω6i ]T ⎪
⎭
The inverse velocity analysis consists of finding the joint rate velocities of the robot given a
prescribed velocity state of the end–effector–platform with respect to the fixed platform,
A C
VO . This analysis is solved directly by means of expressions (18) and (19), however the
loss freedom of the end–effector–platform must be taken into proper account in order to
obtain the desired velocity state. Furthermore, it is important to emphasize that the
Jacobians A J iB , B JCi and A JCi must be invertible, otherwise the robot is at singular
configuration.
On the other hand, the forward velocity analysis consists of finding the velocity state A VOC ,
given the active joint rate velocities of the robot. It is interesting to note that due to the
decoupled architecture, the velocity state A VOB depends only of the three active joints
2 ω 3 ( i = 1, 2, 3) . Furthermore, since
i 3 4
$ i and 4 $ i5 are reciprocal to the remaining screws
representing the revolute joints in the same limbs, the application of the Klein form, {∗; ∗},
between the velocity state A VOB and these reciprocal screws, the reduction of terms leads to
J 1 Δ AVOB = [0 2ω 3 0 2ω 3 0 2ω 3 ]T
T 1 2 3
(22)
where
J 1 = [ 3 $ 14 ,4 $ 15 ,3 $ 24 , 4 $ 25 ,3 $ 34 , 4 $ 35 ] (23)
is the active Jacobian matrix between the middle and fixed platforms and
⎡O I ⎤
Δ=⎢ ⎥ (24)
⎣ I O⎦
is an operator of polarity defined by the 3 × 3 identity matrix I and the 3 × 3 zero matrix O.
Hence, the velocity state A VOB is obtained directly from the input/output velocity equation (22).
In order to compute the velocity state A VOC please note that the screws 16 $ i17 (i = 1, 2, 3) are
reciprocal to the remaining screws, in the same limb, representing the revolute joints of the
UPS–type limbs. Thus, after applying the Klein form between these screws and the velocity
state A VOC , the reduction of terms yields
DeLiA: A New Family of Redundant Robot Manipulators 83
i ; VO } =14 ω 15 i = 1, 2, 3
i
{ 16 $ 17 A C
(25)
9 10
Similarly, the application of the Klein form of the screws $ ( i = 1, 2, 3) to both sides of Eq.
i
(18) allows to write
{ 9 $ i10 ; A VOC } = { 9 $ 10 A B
i ; VO } i = 1, 2, 3 (26)
Finally, casting in a matrix–vector form Eqs. (25) and (26) one obtains
⎡ 14 ω 115 ⎤
⎢ ⎥
⎢ 14 ω 15 ⎥
2
⎢ ⎥
J 2 Δ AVOC = ⎢ 14 ω 15 ⎥
3
T
(27)
⎢{ 9 $ 10 ; A V B } ⎥
⎢ 9 110 A OB ⎥
⎢{ $ 2 ; VO } ⎥
⎢{ 9 $ 10 ; A V B } ⎥
⎣ 3 O ⎦
where
J 2 = [ 16 $ 117 ,16 $ 17 16 17 9 10 9 10 9 10
2 , $3 , $1 , $2 , $3 ] (28)
is the active Jacobian matrix between the output and fixed platforms.
Therefore the velocity state A VOC can be computed directly from the input/ output velocity
equation (27). Please note that the forward velocity analysis requires that the active Jacobian
matrices J1 and J 2 must be invertible, otherwise the manipulator is at a singular
configuration.
Equation (29) is called a zero–torsion condition and indicates that one element of the angular
velocity AωC can be written as a linear combination of its remaining components. With this
consideration in mind the velocity state A VOC can be considered, by using a proper reference
frame, as a five–dimensional vector which implies that it is possible to write, according to
Eqs. (22) and (27), A VOC in terms of first order coefficients (Gallardo–Alvarado & Rico–
Martínez, 2001) as
A
VOC = GQ + Q * (30)
where Q is a 5 × 1 matrix containing five of the six generalized or active joint rate velocities
which is affected by the 5 × 5 matrix G whose elements are the corresponding first order
coefficients of the chosen active joints while Q∗ is a 5×1 matrix formed with the remaining
active joint multiplied by its corresponding first order coefficients. Given a prescribed
84 Advanced Strategies for Robot Manipulators
velocity state A VOC , expression (30) indicates that the user can select five of the six active
joints and the remaining one can be used in order to avoid/escape from possible
singularities, if any. Furthermore, the extra active joint can be used with the purpose to
optimize trajectories. This feature is one of the main benefits of the robot D1.
where B AOC is the accelerator of the coupler platform with respect to the fixed platform, B AOC
is the accelerator of the end–effector–platform with respect to the coupler platform.
Furthermore, these accelerators can be written in screw form as follows
16 ⎡ 17 ⎤⎫
A
L Ci = ∑⎢ j ω j + 1 i j $ ij + 1 ∑ k ω k + 1 i k $ ik + 1 ⎥ ⎪
⎣
j =12 k= j+1 ⎦⎪
⎪
⎡
10 11 ⎤ ⎪
B
L i = ∑ ⎢ j ω j + 1 i j $ ij + 1
C
∑ k k+1 i ⎥ ⎬
ω i k k +1
$ (33)
j =6 ⎣ k= j+1 ⎦ ⎪
4 ⎡ ⎪
5 ⎤⎪
A
L iB = ∑ ⎢ j ω j + 1 i j $ ij + 1 ∑ k ω k + 1i $ i
k k +1
⎥⎪
j =0 ⎣ k= j+1 ⎦⎭
The inverse acceleration analysis consists of finding the joint rate accelerations of the robot
given a prescribed accelerator A AOC . This analysis is carried–out by means of expressions
(31) and (32).
On the other hand the forward acceleration analysis consists of finding the accelerator of the
end–effector–platform with respect to the fixed platform, A AOC , given the active joint rate
accelerations of the robot. This analysis is very close to the presented to solve the forward
velocity analysis, therefore only the obtained expressions are included here.
The accelerator A AOB can be computed upon the input/output acceleration expression
⎡ { 3 $ 14 ; A L 1B } ⎤
⎢ 1 4 5 A B ⎥
⎢ 2ω 3 + { $1 ; L 1 }⎥
⎢ {3 $ 4 ;A L B } ⎥
J1 Δ A AOB = ⎢ 2 4 5 A B ⎥
T 2 2
(34)
⎢ 2 ω + { $1 ; L 2 }⎥
⎢ 33 4 A B ⎥
⎢ { $3 ; L 3 } ⎥
⎢ 3 4 5 A B ⎥
⎣ 2ω 3 + { $3 ; L 3 }⎦
DeLiA: A New Family of Redundant Robot Manipulators 85
A
whereas the reduced acceleration state AOC can be obtained from the input/ output
acceleration relationship
⎡ 14 ω 15 + { $ 1 ; L 1 }
1 16 17 A C
⎤
⎢ ⎥
14 ω 15 + { $ 1 ; L 2 }
2 16 17 A C
⎢ ⎥
⎢ ⎥
14 ω 15 + { $ 1 ; L 3 }
3 16 17 A C
J 2 Δ A AOC = ⎢ ⎥
T
(35)
⎢{ $ ; A + [ V V ] + L } ⎥
9 10 A B A B B C B C
⎢ 9 10 A B A B B C B C ⎥
1 O O O 1
⎢{ $ 2 ; AO + [ VO VO ] + L 2 } ⎥
⎢{ 9 $ 10 ; A AB + [ A V B BV C ] + B L C } ⎥
⎣ 3 O O O 3 ⎦
Finally, please note that the computation of the accelerators A AOB and A AOC , by means
respectively of expression (34) and (35), does not require the values of the passive joint rate
accelerations of the robot. Furthermore, once these reduced acceleration states are
calculated, the accelerator B AOC is obtained using expression (31).
⎧ R1 = (0.3,0,0) ⎫
⎪ R = ( −0.125,0, −0.216) ⎪
⎪ 2 ⎪
⎪ R3 = ( −0.125,0,0.216) ⎪
⎪ ⎪
⎪U 1 = (0.5, −0.25,0) ⎪
⎪U = ( −0.215, −0.25, −0.337)⎪
⎨ 2 ⎬
⎪U 3 = ( −0.215, −0.25,0.337) ⎪
⎪ ⎪
⎪S1S2 = S1S3 = S3S2 = 0.173 ⎪
⎪ ⎪
⎪S4S5 = S4S6 = S5S6 = 0.311 ⎪
⎪S R = S R = S R = 0.125 ⎪
⎩ 1 4 2 5 3 6 ⎭
whereas in the home position the coordinates of the spherical joints Si(i = 1, 2, . . . , 6) and of
the tool tip T are given by
The inverse kinematics of the robot is proved simulating the D1 robot as a parallel kinematic
machine tool. To this end, two tasks are assigned to the tool tip:
• The robot will drill three holes
• The robot will mill a hexagon
In order to achieve these tasks, only five of the six available generalized coordinates are
required, therefore one of them, e.g. q3, should be locked. After, the required instantaneous
variations of the generalized coordinates satisfying such operations are provided in Fig. 4.
0.25
0.2
0.15
Length (meter)
0.1
0.05
0.0
-0.05
-0.1
0.0 10.0 20.0 30.0 40.0
Time (sec)
0.2
0.15
Length (meter)
0.1
0.05
0.0
0.0 10.0 20.0 30.0 40.0
Time (sec)
With these data the most representative results of the simulation are provided in Fig. 5.
0.15
0.1
Length (meter)
0.05
0.0
-0.05
-0.1
0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0
Time (sec)
-0.9 0.05
0.0
-0.95
Length (meter)
Length (meter)
-0.05
-1.0
-0.1
-1.05
-0.15
-1.1 -0.2
0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0
Time (sec) Time (sec)
1.0 0.15
0.1
0.5
0.05
0.0
0.0
-0.05
-0.1
-0.5
-0.15
-1.0 -0.2
0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0
Time (sec) Time (sec)
1.0 0.35
0.25
0.0 0.15
0.05
0.0
-1.0 -0.05
-0.15
-2.0 -0.25
-0.35
-3.0 -0.45
0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0 0.0 1.0 2.0 3.0 4.0 5.0 6.0 7.0
Time (sec) Time (sec)
Fig. 5. Forward kinematics, six active joints to realize five degrees of freedom
88 Advanced Strategies for Robot Manipulators
Finally, of course a virtual prototype is not the final word about the correctness performance
of a proposed manipulator, but it is an advisable option before the construction of a real
prototype.
7. Conclusions
In this work a new class of redundant robot manipulators called DeLiA is introduced. The
main features of the proposed robots are:
• Symmetry
• Decoupled architecture. Only three of the six active limbs connect the end–effector–
platform to the fixed platform
• The forward position analysis, a challenging task of most parallel manipulators, is
carried–out by solving two sets of non–linear equations
• The proposed robot does not require of special conditions of mechanical assembly like
intersection of screws or similar
• The six active limbs are mounted on the fixed platform, simplifying the kinematics and
control of the robot
• Redundancy, the robot is endowed with an extra degree of freedom which can be used
with the purpose to avoid/escape from singular configurations, as well to optimize
trajectories. Any of the six active joints can play this role
Finally, D1, a member of the DeLiA robot family, is simulated as a five-degrees- of-freedom
parallel kinematic machine tool with the aid of commercially available software like
ADAMS©.
8. Acknowledgement
This work was supported by National Council of Science and Technology of México,
Conacyt.
9. References
Altuzarra, O.; Loizaga, M.; Pinto, C. & Petuya, V. (2010). Synthesis of partially decoupled
multi–level manipulators with lower mobility, Mechanism and Machine Theory, Vol.
45, No. 1, 106–118.
Ball, R.S. (1999, Reprinted 1998). A Treatise on the Theory of Screws, Cambridge University
Press, Cambridge.
Bohez, E.L.J. (2002). Five–axis milling machine tool kinematic chain design and analysis,
International Journal of Machine Tools and Manufacture, Vol. 42, No. 4, 505–520.
Bonev, I.A. (2002). Geometric Analysis of Parallel Mechanisms, thèse de doctorat, Université
Laval, Canada.
Bonev, I. (2003). The true origins of parallel robots. Available at Parallel Mic Center,
ParalleMIC online review, available at
http://www.parallemic.org/Reviews/Review007.html
Briot, S.; Arakelian, V. & Guégan, S. (2009). PAMINSA: a new family of partially decoupled
parallel manipulators, Mechanism and Machine Theory, Vol. 44, No. 2, 425444.
Briot, S. & Bonev, C.M. (2009). Pantopteron: a new fully decoupled 3DOF translational
parallel robot for pick–and–place applications, ASME Journal of Mechanisms and
Robotics, Vol. 1, May, paper 021001, 1–9.
DeLiA: A New Family of Redundant Robot Manipulators 89
Issues and Future Research Directions for Parallel Mechanisms and Manipulators, pp.
2532, Quebec City, Quebec, Canada.
Kong, X. & Gosselin, C.M. (2002b). Type synthesis of linear translational parallel
manipulators, In: Advances in Robot Kinematics Theory and Applications, Lenarčič, J. &
Thomas, F. (Eds.), Kluwer Academic Publishers, 411420.
Kong, X. & Gosselin, C.M. (2002c). Kinematics and singularity analysis of 3– CRR 3–DOF
translational parallel manipulators, The International Journal of Robotics Research, Vol.
21, No. 9, 791798.
Li, Q. & Huang, Z. (2003). Type synthesis of 5–DOF parallel manipulators, Proceedings of the
2003 IEEE International Conference on Robotics & Automation, pp. 1203–1208, Taipei,
Taiwan.
Li, W.M.; Gao, F. & Zhang, J.J. (2005). R–CUBE, a decoupled parallel manipulator only with
revolute joints, Mechanism and Machine Theory, Vol. 40, No. 4, 467–473.
Piccina, O.; Bayle, B.; Maurin B. & de Mathelin, M. (2009). Kinematic modeling of a 5–DOF
parallel mechanism for semi–spherical workspace, Mechanism and Machine Theory,
Vol. 44, No. 8, 1485–1496.
Pollard, W.L.G. (1940). Spray painting machine, US Patent No. 2,213,108, August 26.
Rico–Martínez, J.M. & Duffy, J. (1996). An application of screw algebra to the acceleration
analysis of serial chains, Mechanism and Machine Theory, Vol. 31, No. 4, 445–457.
Rico, J.M. & Duffy, J. (2000). Forward and inverse accceleration analyses of in–parallel
manipulators, ASME Journal of Mechanical Design, Vol. 122, No. 3, 299–303.
Ruggiu, M. (2009). Kinematic analysis of a fully decoupled translational parallel
manipulator, Robotica, Vol. 27, 961–969.
Stewart, D. (1965). A platform with six degrees of freedom, Proceedings Institution of
Mechanical Engineers Part I, Vol. 180, No. 15, 371–386.
Takeda, Y.; Kamiyama, K.; Maki, Y.; Higuchi, M. & Sugimoto, K. (2005). Development of
position–orientation decoupled spatial in–parallel actuated mechanisms with six
degrees of freedom, Journal of Robotics and Mechatronics, Vol. 17, No. 1, 59–68.
Tsai, L.W. (1999). Robot Analysis, John Wiley & Sons, New York.
Vlachos, K. & Papadopoulos, E. (2005). Endpoint–Side optimization of a five degrees–of–
freedom haptic mechanism, Proceedings of the 13th Mediterranean Conference on
Control and Automation Limassol, pp. 27–29 Cyprus.
Wohlhart, K. (1994). Displacement analysis of the general spherical Stewart platform,
Mechanism and Machine Theory, Vol. 29, No. 4, 581–589.
Yang, G.; Chen, I–M.; Chen, W. & Lin, W. (2004). Kinematic design of a six–dof parallel–
kinematics machine with decoupled–motion architecture, IEEE Transactions on
Robotics, Vol. 20, No. 5, 876–884.
Zabalza, I.; Ros, J.; Gil, J.J.; Pintor, J.M. & Jiménez, J.M. (2002). Tri–Scott. A new kinematic
structure for a 6–dof decoupled parallel manipulator, Workshop on Fundamental
Issues and Future Research Directions for Parallel Mechanisms and Manipulators, pp. 12–
15, Québec, Canada.
Zheng, K.–J.; Gao, J.–S. & Zhao, Y.S. (2005). Path control algorithms of a novel 5–DOF
parallel machine tool, Mechatronics and Automation, 2005 IEEE International
Conference, pp. 1381–1385, Niagara Falls, Ont., Canada.
Zhu, S.J.; Huang, Z. & Zhao, M.Y. (2008). Feasible human–spine motion simulators based on
parallel manipulators, In: Parallel Manipulators, Towards New Applications, Wu, H.,
ed., I–Tech Education and Publishing, Vienna, Austria.
5
1. Introduction
Being an inherently open loop unstable mechanical system with highly nonlinear dynamics
and with the number of actuators less than the number of degrees of freedom, the inverted
pendulum system is a perfect benchmark for the design of a wide range of classical and
contemporary control techniques. There are a number of different versions of the inverted
pendulum systems offering a variety of control challenges. The most common types are the
single inverted pendulum on a cart (Ohsumi & Izumikawa, 1995; Åström & Furuta, 2000;
Yoshida, 1999), the double inverted pendulum on a cart (Zhong & Rock, 2001), the double
inverted pendulum with an actuator at the first joint only (Pendubot) (Spong, 1996;
Graichen & Zeitz, 2005; Fantoni et al., 2000), the double inverted pendulum with an actuator
at the second joint only (Acrobot) (Spong, 1994; 1995; Hauser & Murray, 1990), the rotational
single-arm pendulum (Furuta et al., 1991; 1992) and the rotational two-arm pendulum
(Yamakita & Furuta, 1999). Beyond non-mobile inverted pendulum robots, wheeled
inverted pendulum robots or commonly known as balancing robots (e.g., Segway
(Browning et al., 2005), Quasimoro (Salerno & Angeles, 2003), and Joe (Grasser et al., 2002))
have induced much interests by researchers.
The control techniques involved in various types of inverted pendulum systems are also
numerous, ranging from simple conventional controllers to advanced control techniques
based on modern nonlinear control theory. A vast range of contributions exists for the
stabilization of different types of inverted pendulums (Mori et al., 1976; Chaturvedi et al.,
2008; Angeli, 2001). Besides the stabilization aspect, the swing-up of various types of single
and double inverted pendulum(s) is also addressed in the literature. Examples include
classic single pendulum on a cart (Åström et al., 2008; Åström & Furuta, 2000), Acrobot and
Pendubot (Fantoni et al., 2000; Spong, 1994; 1995; Graichen et al., 2007; Brown & Passino,
1997) and the rotary double inverted pendulum (Yamakita et al., 1993; 1995). In addition to
the stabilization and swing-up of different kinds of inverted pendulum robots, trajectory
tracking of these underactuated systems has gained attention by researches (Cho & Jung,
2003; Chanchareon et al., 2006; Hung et al., 1997; Magana & Holzapfel, 1998). There are two
major approaches to construct the trajectory tracking controller for such nonlinear systems.
The first one is based on system inversion (Devasia et al., 1996; Wang & Chen, 2006) and the
92 Advanced Strategies for Robot Manipulators
second approach is based on output regulation theory (Isidori & Byrnes, 1990; Qian & Lin,
2002; Hirschorn & Aranda-Bricaire, 1998). Extensive controller developments have also been
achieved by researchers for mobile inverted pendulum robots over the last decade (Salerno
& Angeles, 2007; Pathak et al., 2005; Tsuchiya et al., 1999).
This chapter studies a novel underactuated wheeled manipulator (WAcrobot) comprising an
underactuated 2-DOF planar manipulator or an unstable double inverted pendulum
(Acrobot) combined with a balancing robot. The WAcrobot has two independent driving
wheels in same axis, and two gyro type sensors to determine the inclination angular velocity
of two arms and rotary encoders to know wheels and arms rotation individually. Due to its
configuration with two coaxial wheels, each wheel is coupled to a geared dc motor. The
manipulator is able to do stationary U-turns while keeping balance and manipulating. Such
manipulator is of interest because it has a small foot-print and can turn on dime. The design,
dynamic modeling and tracking control of this novel mobile manipulator is discussed in this
chapter for the first time. This chapter aims at achieving three different types of trajectory
tracking control tasks for a) wheels, b) first or second arm and c) wheels and one of the arms
simultaneously, while the WAcrobot stabilization is guaranteed by the system internal
equilibria calculation. The tracking controller is designed using the Gain Scheduling method
that is based on the idea of the linearisation of the system equations around certain
operating points and design of a linear controller for each region of operation (Lawrence &
Rugh, 1993; Shamma & Athans, 1990a)]. For the design of the linear controller, we consider
the Linear Quadratic Regulator (LQR) model to stabilize the WAcrobot around any point
over the equilibrium manifold. We verified the effectiveness of the designed control system
via numerical simulation visualized by graphical simulation to illustrate the physical
response of the WAcrobot.
In the following sections of this chapter the dynamic model of the wheeled manipulator
(WAcrobot) is firstly presented. Then the equilibrium manifold of the WAcrobot is
investigated. After that the stabilization controller based on LQR technique is proposed.
Then by employing Gain Scheduling method, for any given trajectory of wheels and/or
arm(s), the trajectories of the rest of DOF of the WAcrobot is determined such that during
the trajectory tracking the WAcrobot system is stabilized. Numerical and graphical
simulations for three types of tracking control tasks are given to show the effectiveness of
the proposed scheme.
2. Dynamics of WAcrobot
The mechanism of the WAcrobot is shown in Figure 1 schematically. The WAcrobot
(Wheeled Acrobot) is an underactuated mechanical system consisting of an underactuated
planar manipulator (Acrobot), a double inverted pendulum robot with an actuator at the
second joint only (Figure 1-a), which is combined with a balancing robot (Figure 1-b) or
equipped with two actuated wheels and has the capability to be as an underactuated
wheeled manipulator. The mathematical model of the WAcrobot can be derived using the
Euler-Lagrange equation. The form of the Euler-Lagrangian equation used here is:
d ⎡ ∂L ⎤ ∂L
⎢ ⎥− =τ (1)
dt ⎣ ∂q ⎦ ∂q
Dynamic Modelling, Tracking Control and Simulation Results
of a Novel Underactuated Wheeled Manipulator (WAcrobot) 93
Active Joint
Passive Joint
b
θ3
θ1
θ2
((m1 + m2 + m3 )l12 + I 1 )θ1 + l1 (lc 3m3cos(θ 2 + θ 3 ) + cos(θ 2 )(m2 lc 2 + m3l2 ))θ2 = τ1 (4)
+(m2 lc22 + m3 (lc23 + l22 ) + I 2 + I 3 + 2 m3l2 lc 3cos(θ 3 ))θ2 − m3l2 lc 3sin(θ 3 )θ32
where θ = [θ1 θ2 θ3] ∈R is the generalized coordinate vector, M(θ )∈R is the symmetric
T 3 3×3
positive definite inertia matrix, C(θ, θ ) θ ∈R3 contains Coriolis and centrifugal terms,
G(θ )∈R3 contains gravitational terms and τ = [τ1 0 τ2]T is the input generalized force vector.
Furthermore,
⎡ M 11 M12 M 13 ⎤
⎢ ⎥ (8)
M (q ) = ⎢ M 21 M 22 M 23 ⎥ ,
⎢⎣ M 31 M 32 M 33 ⎥⎦
where
C (θ ,θ)θ = [ H 1 H 2 H 3 ]T (9)
where
and
where
G1 = 0
3. Tracking control
The tracking controller of the WAcrobot is designed using the Gain Scheduling method
based on the linearisation of the system equations around certain equilibrium points in a
first stage followed by the design of a linear controller for each region of tracking operation
in a second stage. For the design of the linear controller, we consider the Linear Quadratic
Regulator (LQR) model to stabilize the WAcrobot around any operating point over the
equilibrium manifold.
G(θ eq ) = τ eq (12)
If the outputs that are required to track a trajectory include the first arm, it follows from
Equations (7), (10), (11) and (12) that:
96 Advanced Strategies for Robot Manipulators
1.5
1.0
θ +θ3 (rad)
0.5
C=0.1
0.0
-0.5
2
-1.0
-1.5 C=1.0
-3 -2 -1 0 1 2 3
θ (rad)
2
τ 2 = (m3l2 + m2 lc 2 ) gsin(θ 2 )
eq eq
(13)
m3l2 + m2 lc 2
θ 3 + θ 2 = arcsin[ −( )sin(θ 2 eq )] (14)
eq eq
m3lc 3
Since the value of the absolute angular position of the second arm with respect to the
vertical direction θ 3a = θ 3 + θ 2 cannot be imaginary, the condition for the existence of the
eq eq eq
equilibrium from Equation (14) may be written:
m3l2 + m2 lc 2
C= ≤1 (15)
m3lc 3
Figure 2 illustrates equilibrium set points derived from Equation (14) according to different
values of parameter C from Equation (15). It demonstrates how value of parameter C affects
θ 3a corresponding to any given θ 2 in which the WAcrobot is stabilized. By decreasing
eq
eq
decreases and to decrease parameter C, the second arm should be long and heavy which is
not suitable. On the other hand, for any given angular position of the first arm, if the second
arm is long and heavy, it needs smaller angular changes to stabilize the WAcrobot and vice
versa. Therefore there needs to be a trade-off between the ranges of the rotational motions of
arms and the volume and weight of the WAcrobot. In particular, for any given m3, if the
specification of the first arm (m2, l2 and lc2) are given, Equation (15) is only true if lc3 ≥ (m3l2 +
m2lc2)/m3 and for any given lc3, it is only true if m3 ≥ m2lc2/(lc3 –l2). Considering l2 = 2lc2 and l3 =
2lc3, we can simplify Equation (15) as:
l3 m
≥ (2 + 2 ) (16)
l2 m3
Dynamic Modelling, Tracking Control and Simulation Results
of a Novel Underactuated Wheeled Manipulator (WAcrobot) 97
From the other point of view, if the trajectory tracking of the second arm is desired, it
follows from Equations (7), (10), (11) and (12) that:
τ 1eq = 0
m3lc 3
θ 2 eq = arcsin[ −( )sin(θ 3a )] (18)
m3l2 + m2 lc 2 eq
Since the value of the angular position of the first arm ( θ 2 ) cannot be imaginary, the
eq
condition for the existence of the equilibrium from Equation (18) is:
m3lc 3
C −1 = ≤1 (19)
m3l2 + m2 lc 2
l3 m
≤ (2 + 2 ) (20)
l2 m3
3.2 Stabilization
The balancing controller is designed using the well known Linear Quadratic Regulator
(LQR) method based on the linearised plant model around any equilibrium point. The LQR
is a controller for state variable feedback in such a way that u = –Kx is the input so that the
value of K is obtained from minimization of the cost function J = ∫ ∞
0 (x’Qx + u’Ru)dt where
matrix Q and R are positive semidefinite matrix and symmetric positive definite matrix that
penalize the state error and the control effort, respectively.
various technical methods are available in each step. The first step involves finding
θ 3a or θ 2 in all operating points, for each desired θ 2 or θ 3a , using Equations 14 or 18
eq eq eq eq
respectively. The second step is the calculation of the joint torque required to keep the
WAcrobot at desired θ 2 (or θ 3a ) and calculated θ 3a (or θ 2 ). The third step is the
eq eq eq eq
computation of a linear parameter varying model for the plant. The most common approach
is to linearise the nonlinear plant around a selection of equilibrium points. This results in a
family of operating points. The fourth step is to design a family of controllers for the
linearised models in each operating point. Because of the linearised model, linear controller
design methods such as LQR can be used to stabilize the system around the operating point.
The fifth step is the actual Gain Scheduling. Gain Scheduling involves the implementation of
the family of linear controllers such that the controller coefficients are scheduled according
to the current value of the scheduling variables which are θ 2 or θ 3a . The last step is the
eq eq
R = diag([0.1, 0.1])
It must be noted that in order to have better sense of motion, the angular position and
velocity of the second arm are considered as absolute states and are plotted with respect to
the vertical direction not relative to the first arm.
Dynamic Modelling, Tracking Control and Simulation Results
of a Novel Underactuated Wheeled Manipulator (WAcrobot) 99
θ1 actual -1.000
-10.00 -2.000
-15.00
-20.00 -3.000
-25.00 -4.000 dθ
θ1/dt desired
-30.00 -5.000 dθ
θ1/dt actual
-35.00 -6.000
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
0.080 0.250
dθ/dt (rad/s)
0.060 0.200 dθ
θ2/dt desired
0.040 0.150
θ (rad)
dθ
θ2/dt actual
0.100
0.020 0.050
0.000 0.000
-0.020 -0.050
-0.040 θ2 desired -0.100
θ2 actual -0.150
-0.060 -0.200
-0.080 -0.250
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
0.025 0.060
dθ/dt (rad/s)
0.020 θ3 desired
0.015 0.040
θ (rad)
θ3 actual
0.010 0.020
0.005
0.000 0.000
-0.005
-0.010 -0.020 dθ
θ3/dt desired
-0.015 -0.040 dθ
θ3/dt actual
-0.020
-0.025 -0.060
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
0.050 0.060
θ torque (N.m)
θ torque (N.m)
0.040 T2
0.030 0.040
0.020 θ 0.020 θ
0.010
0.000 0.000
-0.010
-0.020 -0.020
-0.030 T1 -0.040
-0.040
-0.050 -0.060
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
time (s) time (s)
Fig. 3. The simulation responses of positions, velocities and torques for the wheels tracking
100 Advanced Strategies for Robot Manipulators
0.020 0.040
θ position (m)
θ speed (m/s)
0.010 0.020
0.005 θ 0.010 θ
0.000 0.000
-0.005 -0.010
-0.010 -0.020
-0.015 -0.030 tracking error
-0.020 -0.040
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
time (s) time (s)
Fig. 4. Tracking errors for the linear position and velocity of the wheels for wheels tracking
Fig. 5. Superimposed snapshot of the visualized simulation of the wheels tracking task while
the arms are in inverted position (left) and are not in inverted position (right)
1.5
T2 desired
θ1 desired
1.0 θ2 desired
θ (rad) and torque (N.m)
θ3 desired
0.5
0.0
-0.5
-1.0
0 1 2 3 4 5 6 7 8 9
time (s)
Fig. 6. The desired and calculated trajectories for the wheels, arms and time-varying torque
0.015
position (m)
speed (m/s)
0.003 0.010
0.002 0.005
0.001 0.000
0.000 -0.005
-0.001 -0.010
-0.002 -0.015
-0.003 -0.020
-0.004 tracking error -0.025 tracking error
-0.005 -0.030
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9
0.150 0.500
dθ/dt (rad/s)
θ2 tracking error 0.400 dθ
θ2/dt tracking error
0.100 0.300
θ (rad)
0.050 0.200
0.100
0.000 0.000
-0.050 -0.100
-0.200
-0.100 -0.300
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9
0.040 0.100
θ dθ/dt (rad/s)
0.030 0.080 dθ
θ3/dt tracking error
0.020 0.060
θ (rad)
0.010 θ 0.040 θ
0.000 0.020
0.000
-0.010 -0.020
-0.020 -0.040
-0.030 θ3 tracking error
-0.060
-0.040 -0.080
θ
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9
time (s) time (s)
Fig. 7. Tracking errors of the positions and velocities of the wheels and arms for arms
tracking
Simulation results are shown in Figure 8. In this figure the simulation responses of angular
positions and velocities of wheels and arms as well as applied torques to actuated degrees of
freedom are demonstrated, respectively. To show the correlation between the computational
analysis results and the WAcrobot physical response, the graphical simulation is prepared
and is shown in Figure 9.
0.060 0.300
dθ/dt (rad/s)
0.040 0.200
0.020 0.100
θ (rad)
0.000 0.000
-0.020 -0.100
-0.200
-0.040 θ1 desired -0.300 dθ
θ1/dt desired
-0.060 -0.400
-0.080 θ1 actual dθ
θ1/dt actual
-0.500
-0.100 -0.600
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9
1.400 1.000
dθ/dt (rad/s)
θ2 actual dθ
θ2/dt actual
0.400
0.800 0.200
0.600 0.000
0.400 -0.200
0.200 -0.400
-0.600
0.000 -0.800
-0.200 -1.000
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9
0.200 0.600
dθ/dt (rad/s)
0.000 0.400
θ (rad)
-0.200 0.200
-0.400 0.000
-0.600 -0.200
-0.800 θ3 desired -0.400 dθ
θ3/dt desired
-1.000 θ3 actual -0.600 dθ
θ3/dt actual
-1.200 -0.800
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9
0.100 0.100
θ torque (N.m)
θ torque (N.m)
T1 0.000
0.050 -0.100
0.000 θ -0.200 θ
-0.300
-0.050 -0.400
-0.100 -0.500
-0.600 T2
-0.150 -0.700
0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9
time (s) time (s)
Fig. 8. The simulation responses of positions, velocities and torques for the first arm tracking
102 Advanced Strategies for Robot Manipulators
Fig. 9. Snapshots of the visualized simulation for the arms tracking task
θ*1 desired
1.0
θ2 desired
θ3 desired
0.5
0.0
-0.5
-1.0
-1.5
0 1 2 3 4 5 6 7 8 9 10
time (s)
Fig. 10. The desired and calculated trajectories for the wheels, arms and time-varying torque
# Description
b d
a LQR Gains
c Switching
b
Block
WAcrobot's
c nonlinear
Block
f Reference
d Generator
Blocks
Equilibrium
e Calculation
Block
a e f
Monitoring
Block
Fig. 11. The Simulink® block diagram of the WAcrobot system with tracking controller
The WAcrobot system with gain scheduling tracking controller simulated in Simulink® is
illustrated in Figure 11. Figure 12 shows the simulation results of the wheels and arm
tracking task. In this figure the simulation responses of the angular positions and velocities
Dynamic Modelling, Tracking Control and Simulation Results
of a Novel Underactuated Wheeled Manipulator (WAcrobot) 103
of wheels and arms as well as applied torques to actuated DOFs are shown. Figure 13
respectively displays the tracking errors of the linear and angular positions and velocities of
the wheels and arms from top to bottom. Figure 14 shows a superimposed snapshot of the
visualized simulation of the WAcrobot while wheels are tracking the specified trajectory
and the first arm is tracking another specified trajectory simultaneously. The simulation
results illustrate the effectiveness of the proposed control methodology and the developed
theory.
5.00 1.000
dθ/dt (rad/s)
0.00 θ1 desired 0.000
-5.00 -1.000
θ (rad)
θ1 actual
-10.00 -2.000
-15.00 -3.000
-4.000
-20.00 -5.000 dθ
θ1/dt desired
-25.00 -6.000
-30.00 dθ
θ1/dt actual
-7.000
-35.00 -8.000
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
1.600 0.800
dθ/dt (rad/s)
1.400 θ2 desired 0.600
1.200 0.400
θ (rad)
θ2 actual
1.000 0.200
0.800 0.000
0.600 -0.200
-0.400
0.400 -0.600 dθ
θ2/dt desired
0.200 -0.800 dθ
θ2/dt actual
0.000 -1.000
-0.200 -1.200
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
0.200 0.600
dθ/dt (rad/s)
0.000 0.400
θ (rad)
-0.200 0.200
0.000
-0.400
-0.200
-0.600 θ3 desired -0.400 dθ
θ3/dt desired
-0.800 θ3 actual -0.600 dθ
θ3/dt actual
-1.000 -0.800
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
0.100 0.200
θ torque (N.m)
θ torque (N.m)
0.000
0.050 -0.200
0.000 θ -0.400 θ
-0.600
-0.050 -0.800
-0.100 -1.000
T1 -1.200 T2
-0.150 -1.400
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
time (s) time (s)
Fig. 12. The simulation responses of positions, velocities and torques for the manipulator
0.025 0.06
position (m)
0.015 0.04
0.010 0.02
0.005
0.000 0.00
-0.005
-0.010 -0.02
-0.015 -0.04
-0.020 tracking error
-0.025 -0.06
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
0.25 0.50
dθ/dt (rad/s)
0.15 0.20
0.10 0.10
0.05 0.00
-0.00 -0.10
-0.20
-0.05 -0.30
-0.10 -0.40
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
0.04 0.08
θ dθ/dt (rad/s)
0.01 θ 0.02 θ
0.00 0.00
-0.01 -0.02
-0.02 -0.04
-0.03 -0.06 dθ
θ3/dt tracking error
-0.04 -0.08
θ
0 1 2 3 4 5 6 7 8 9 10 0 1 2 3 4 5 6 7 8 9 10
time (s) time (s)
Fig. 13. Tracking errors of the positions and velocities of the wheels and arms
104 Advanced Strategies for Robot Manipulators
Fig. 14. Superimposed snapshot of the graphical simulation of the wheels and arms tracking
4. Conclusion
In this chapter the WAcrobot, a novel underactuated manipulator which is the combination
of a well-known double inverted pendulum (Acrobot) and a wheeled inverted pendulum,
was proposed and the tracking control algorithm of this mobile manipulator was
investigated. The balancing controller is designed using the well known Linear Quadratic
Regulator (LQR) method and the tracking controller was designed on the basis of the Gain
Scheduling control strategy. Three different types of trajectory tracking tasks were
investigated including tracking of a) wheels, b) first or second arm and c) wheels and first or
second arm simultaneously.
This chapter also provided numerical and graphical simulation results to validate the
obtained theoretical results and to demonstrate the correlation between the numerical
results and the WAcrobot physical response. Simulation results illustrated good
performance results for different tracking controls designed based on the Gain Scheduling
method.
Research into the control of this novel robotic system is just in the beginning and there are a
number of research problems that remain to be addressed. It would be desirable to develop
the theory of robust and adaptive controller for swing-up control problem of the WAcrobot.
5. References
Angeli, D. (2001). Almost global stabilization of the inverted pendulum via continuous state
feedback, Automatica 37(7): 1103–1108.
Åström, K., Aracil, J. & Gordillo, F. (2008). A family of smooth controllers for swinging up a
pendulum, Automatica 44(7): 1841–1848.
Åström, K. & Furuta, K. (2000). Swinging up a pendulum by energy control, Automatica 36:
287–295.
Baumann, W. & Rugh, W. (1986). Feedback control of nonlinear systems by extended
linearization, IEEE Transactions on Automatic Control 31(1): 40–46.
Bortoff, S. & Spong, M. (1992). Pseudolinearization of the acrobot using spline functions,
Proceedings of the 31st IEEE Conference on Decision and Control, pp. 593–598.
Brown, S. & Passino, K. (1997). Intelligent control for an acrobot, Journal of Intelligent and
Robotic Systems 18(3): 209–248.
Dynamic Modelling, Tracking Control and Simulation Results
of a Novel Underactuated Wheeled Manipulator (WAcrobot) 105
Browning, B., Searock, J., Rybski, P. & Veloso, M. (2005). Turning segways into soccer
robots, Industrial Robot: An International Journal 32(2): 149–156.
Chanchareon, R., Sangveraphunsiri, V. & Chantranuwathana, S. (2006). Tracking Control of
an Inverted Pendulum Using Computed Feedback Linearization Technique,
Proceedings of the IEEE Conference on Robotics, Automation and Mechatronics, pp. 1–6.
Chaturvedi, N., McClamroch, N. & Bernstein, D. (2008). Stabilization of a 3D axially
symmetric pendulum, Automatica 44(9): 2258–2265.
Cho, H. & Jung, S. (2003). Balancing and position tracking control of an inverted pendulum
on a xy plane using decentralized neural networks, Proceedings of the IEEE/ASME
International Conference on Advanced Intelligent Mechatronics (AIM 2003), Vol. 1.
Cloutier, J., D’Souza, C. & Mracek, C. (1996). Nonlinear regulation and nonlinear H-infinity
control via the state-dependent Riccati equation technique. I- Theory, Proceedings of
the International Conference on Nonlinear Problems in Aviation and Aerospace, pp. 117–
130.
Devasia, S., Chen, D. & Paden, B. (1996). Nonlinear inversion-based output tracking, IEEE
Transactions on Automatic Control 41(7): 930–942.
Dorato, P., Cerone, V. & Abdallah, C. (1994). Linear-Quadratic Control: An Introduction, Simon
& Schuster.
Fantoni, I., Lozano, R. & Spong, M. (2000). Energy based control of the pendubot, IEEE
Transactions on Automatic Control 45(4): 725–729.
Furuta, K., Yamakita, M. & Kobayashi, S. (1991). Swing up control of inverted pendulum,
Proceedings of the International Conference on Industrial Electronics, Control and
Instrumentation (IECON’91), pp. 2193–2198.
Furuta, K., Yamakita, M. & Kobayashi, S. (1992). Swing-up control of inverted pendulum
using pseudo-state feedback, Proceedings of the Institution of Mechanical Engineers
206: 263–9.
Graichen, K., Treuer, M. & Zeitz, M. (2007). Swing-up of the double pendulum on a cart by
feedforward and feedback control with experimental validation, Automatica 43(1):
63–71.
Graichen, K. & Zeitz, M. (2005). Nonlinear feedforward and feedback tracking control with
input constraints solving the pendubot swing-up problem, Preprints of 16th IFAC
world congress, Prague, CZ.
Grasser, F., D’ Arrigo, A., Colombi, S. & Rufer, A. (2002). JOE: a mobile, inverted pendulum,
IEEE Transactions on industrial electronics 49(1): 107–114.
Hauser, J. & Murray, R. (1990). Nonlinear controllers for non-integrable systems: The
acrobat example, American Control Conference, pp. 669–671.
Hirschorn, R. & Aranda-Bricaire, E. (1998). Global approximate output tracking for
nonlinear systems, IEEE Transactions on Automatic Control 43(10): 1389–1398.
Hung, T., Yeh, M. & Lu, H. (1997). A PI-like fuzzy controller implementation for the
inverted pendulumsystem, Proceedings of the IEEE International Conference on
Intelligent Processing Systems (ICIPS’97), Vol. 1.
Isidori, A. & Byrnes, C. (1990). Output regulation of nonlinear systems, IEEE Transactions on
Automatic Control 35(2): 131–140.
Langson, W. (1997). Optimal and suboptimal control of a class of nonlinear systems, PhD thesis,
University of Illinois.
Lawrence, D. & Rugh, W. (1993). Gain scheduling dynamic linear controllers for a nonlinear
plant, Proceedings of the 32nd IEEE Conference on Decision and Control, pp. 1024–1029.
106 Advanced Strategies for Robot Manipulators
Magana, M. & Holzapfel, F. (1998). Fuzzy-logic control of an inverted pendulum with vision
feedback, IEEE Transactions on Education 41(2): 165–170.
Mori, S., Nishihara, H. & Furuta, K. (1976). Control of unstable mechanical system Control
of pendulum, International Journal of Control 23(5): 673–692.
Ohsumi, A. & Izumikawa, T. (1995). Nonlinear control of swing-up and stabilization of an
invertedpendulum, Proceedings of the 34th IEEE Conference on Decision and Control,
Vol. 4.
Pathak, K., Franch, J. & Agrawal, S. (2005). Velocity and position control of a wheeled
inverted pendulum by partial feedback linearization, IEEE Transactions on Robotics
21(3): 505– 513.
Qian, C. & Lin,W. (2002). Practical output tracking of nonlinear systems with uncontrollable-
unstable linearization, IEEE Transactions on Automatic Control 47(1): 21–36.
Salerno, A. & Angeles, J. (2003). On the nonlinear controllability of a quasiholonomic mobile
robot, Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA’03), Vol. 3.
Salerno, A. & Angeles, J. (2007). A new family of two-wheeled mobile robots: Modeling and
controllability, IEEE Transactions on Robotics 23(1): 169–173.
Shamma, J. & Athans, M. (1990a). Analysis of gain scheduled control for nonlinear plants,
IEEE Transactions on Automatic Control 35(8): 898–907.
Shamma, J. & Athans, M. (1990b). Analysis of nonlinear gain-scheduled control systems,
IEEE Transactions on Automatic Control 35(8): 898–907.
Spong, M. (1994). Swing up control of the acrobot, Proceedings of the IEEE International
Conference on Robotics and Automation, Vol. 3, pp. 2356–2361.
Spong, M. (1995). The swing up control problem for the acrobot, IEEE Control Systems
Magazine 15(1): 49–55.
Spong, M. (1996). The control of underactuated mechanical systems, Proceedings of the First
international conference on mechatronics, pp. 26–29.
Spong, M. & Block, D. (1995). The Pendubot: a mechatronic system for control research
andeducation, Proceedings of the 34th IEEE Conference on Decision and Control, Vol. 1.
Tsuchiya, K., Urakubo, T. & Tsujita, K. (1999). A motion control of a two-wheeled mobile
robot, Proceedings of the IEEE International Conference on Systems, Man, and
Cybernetics (SMC’99), Vol. 5.
Wang, X. & Chen, D. (2006). Output tracking control of a one-link flexible manipulator via
causal inversion, IEEE Transactions on Control Systems Technology 14(1): 141–148.
WJ, R. & Shamma, J. (2000). Research on gain scheduling, Automatica 36(10): 1401–1425.
Yamakita, M. & Furuta, K. (1999). Toward robust state transfer control of titech double
pendulum, Proceedings of the Åström Symposium on Control, pp. 73–269.
Yamakita, M., Iwashiro, M., Sugahara, Y. & Furuta, K. (1995). Robust swing up control of
double pendulum, Proceedings of the American Control Conference, Vol. 1.
Yamakita, M., Nonaka, K. & Furuta, K. (1993). Swing up control of double pendulum,
Proceedings of the American Control Conference, Vol. 3, pp. 2229–2229.
Yoshida, K. (1999). Swing-up control of an inverted pendulum by energy-based methods,
Proceedings of the American control conference, Vol. 6, pp. 4045–4047.
Zhong, W. & Rock, H. (2001). Energy and passivity based control of the double inverted
pendulum on a cart, Proceedings of the 2001 IEEE international conference on control
applications, pp. 896–900.
6
1. Introduction
This article studies several classes of linear actuators based on parallel topology featuring
lower mobility.
Translation actuator design represents a very important issue in manipulator design in areas
like machine tools for example and more recently hexapods. Actual designs are usually
limited to low accelerations actually limited to 2 g. Moreover, alignment problems are
difficult to circumvent and usually lead to non-uniform friction in the translation motion
refered as hard spots. Despite important breakthroughs, linear motors are still limited to
accelerations of 5g and they are plagued by problems such as surrounding magnetisation
and limited torque. As for any parallel mechanisms, the proposed architectures do provide
for a more rigid linkage. Their rigidity advantage leads to larger actuator bandwidth,
thereby allowing for increased accelerations which result in larger forces being applicable to
the extremity while keeping overall mass very low. The main disadvantage will be their
transverse emcumbrance which will be minimized through mechanism networking.
Two diamond and one rhombus configurations have been designed, analyzed, constructed
and compared verifying their ability for very fast accelerations. Their kinematics are
investigated allowing to write the forward and inverse problems for position, velocity and
accelerations where closed-form solutions are determined. Motion limitations and
singularity analysis are also provided from which configuration recommendations can be
derived. These actuators will then be easily controllable despite their non-linear nature.
In parallel manipulators, the prismatic pairs are usually encountered as the linear actuators
for several architectures such as the planar 3RPR, the general Gough platform, (Gough &
Whitehall, 1962; Fichter, Kerr and Rees-Jones) and the Kanuk (Rolland, 1999) for examples.
These prismatic actuators, may they be guided or not, do play a very important role in
robotics design and their performance has been an issue. According to the author’s
obsevration on several high speed milling projects, these actuators have been hampering the
advent of high speed milling by being unable to provide for adequate accelerations in low
inertia and high rigidity packages.
108 Advanced Strategies for Robot Manipulators
or Gough platforms, the actuators have especially to generate straight lines without any
guiding rails.
This question is not new and it actually comes from the title taken from the book written by
Kempe, where he describes plane linkages which were designed to constrain mechanical
linkages to move in a straight line (Kempe, 1877).
m = Σji − λn (1)
where Σji is the sum of all degree-of-freedoms introduced by joints and λ = 3 is the available
DOF of the planar space in which the actuator is evolving.
Finally, the number of closed loops in the system is n. This number can be multiplied and
shall be a natural number n ∈ {1,2,3, . . .}
This mobility calculation holds fo any four-bar mechanism including the free ones, i-e not
being attached to the base.
If properly designed and dimensionned, four-bar linkages can become straight-line motion
generators as will be seen in the next section on kinematics. This is one of the contribution of
this work.
P
P
P
These three mechanisms do provide for straight-line motion at the cost of complex linkages
which do occupy very valuable space. This makes them less likely to be applied on robots.
3. Kinematics analysis
A mechanism is defined as a group of rigid bodies connected to each other by rigid
kinematics pairs to transmit force and motion. (Soylemez, 1999).
Kinematics synthesis is defined as the design of a mechanism to yield a predetermined set of
motion with specific characteristics.
We shall favor dimensional synthesis of function generation implementing an analytical
method. The function is simply a linear function describing a straight-line positioned
parallel to one reference frame axis.
The method will implement a loop-closure equation particularily expressed for the general
four bar linkage at first. The first step consists in establishing the fixed base coordinate system.
B B
A A
r r
3 3
r r
r 4 r 4
2 2
O O
O 4 2 O
2 4
(r1 + r2 + r3 + r4 = 0) (2)
This last equation is rewritten using the complex algebra formulation which is available in
the textbooks, (Uicker, Pennock and Shigley):
j θ1 jθ 2 jθ 3 jθ 4
r1 e + r2 e + r3 e − r4 e =0 (3)
where θ1, θ2, θ3 and θ4 are respectively the fixed base, crank, coupler and follower angles
respective to the horizontal X axis.
If we set the x axis to be colinear with O2O4, if we wish to isolate point B under study, then
the equation system becomes:
Kinematics Synthesis of a New Generation of Rapid Linear Actuators for High Velocity Robotics
with Improved Performance Based on Parallel Architecture 115
jθ 3 jθ 4 jθ 2
r3 e = r4 e − r1 − r2 e (4)
Complex algebra contains two parts directly related to 2D geometry. We project to the x and
y coordinate axes, in order to obtain the two algebraic equations. The real part corresponds
to the X coordinates and the imaginary part to the Y coordinates. Thus, the equation system
can be converted into two distinct equations in trigonometric format.
For the real or horizontal part:
When O2O4 is made colinear with the X axis, as far as r1 is concerned, there remains only one
real part leading to some useful simplification.
The general four bar linkage can be configured in floating format where the O4 joint is
detached from the fixed base, leaving one joint attached through a pivot connected to the
base. Then, a relative moving reference frame can be attached on O2 and pointing towards
O4. This change results in the same kinematic equations.
Since, the same equation holds and we can solve the system:
⎛ B + − A 2 + B2 − C 2 ⎞
(θ 4 ) = 2arctan ⎜⎜ ⎟
⎟
(7)
⎝ A+C ⎠
where the A,B,C parameters are:
r1
A= − cos (θ 2 )
r2
B = − sin (θ 2 ) (8)
r cos (θ 2 ) 1 r1 2 + r2 2 − r3 2 + r4 2
C= 1 −
r4 2 r2 r4
To determine the position of joint center B in terms of the relative reference frame O:
Then, the norm of the vector OB gives the distance between O and B:
(r + r4 cos (θ 4 ) ) + r4 2 ( sin (θ 4 ) )
2 2
x = O2 B = 1 (10)
This explicit equation gives the solution to the forward kinematics problem. An expression
spanning several lines if expanded and which cannot be shown here when the expression of
θ4, equation 7, is substitued in it. This last equation gives the distance between O and B, the
output of the system in relation to the angle θ2, the input of the system as produced by the
rotary motor. The problem can be defined as: Given the angle θ2, calculate the distance x
between O and B.
116 Advanced Strategies for Robot Manipulators
The four-bar can be referred as one of the simplest parallel manipulator forms, featuring one
DOF in the planar space (λ = 3). One family of the lowest mobility parallel mechanisms.
The important issue is the one of the path obtained by point B which is described by a
coupler curve not being a straight line in the four-bar general case.
However, in the floating case, if applied as an actuator, the general four-bar can be made to
react like a linear actuator. The drawbacks are in its complex algebraic formulation and non-
regular shape making it prone for collisions.
A B
r
3
r B
r 3
2
A
r
4
r r
2 4
O O O O
2 4 2 4
The follower follows exactly the crank. This results in the equivalence of the input and
following angles: θ4 = θ2.
If we set R and r as the link lengths respectively, then to determine the position of joint
center B in terms of the relative reference frame O2; an simple expression is derived from the
general four-bar one:
Then, the norm of the vector O2B gives the distance between O2 and B:
⎛ x2 − R2 − r 2 ⎞
θ = arccos ⎜ ⎟ (13)
⎝ 2 Rr ⎠
Detaching joint O4 from the fixed base, the parallelogram becomes a semi-free linkage which
can be considered as one prismatic actuator.
⎛θ ⎞
x = 2 r cos ⎜ ⎟ (14)
⎝2⎠
The Inverse kinematics problem is expressed as:
O B
C
Fig. 9. The Rhombus detailed configuration
118 Advanced Strategies for Robot Manipulators
⎛ x ⎞
θ = 2arccos ⎜ ⎟ (15)
⎝ 2r ⎠
Simple derivation will lead to differential kinematics.
The forward differential kinematics is expressed by the following equation:
θ
v = −rω sin( ) (16)
2
dθ
where ω =
dt
We take the following geometric property:
⎛θ ⎞ x
cos ⎜ ⎟ = (17)
⎝2⎠ 2
We apply Pythagore’s theorem:
⎛θ ⎞ 1 x2
sin ⎜ ⎟ = 4− 2 (18)
⎝2⎠ 2 r
Then, the FDP can be rewritten in terms of the length x:
r x2
v = −ω 4− 2 (19)
2 r
Inversion of equation 19 lead to the inverse differential kinematics problem being expressed
as:
v
ω=− (20)
r sin( θ2 )
v x
ω=− 1− (21)
r 2r
Further derivation will give the extremity acceleration where the FDDP can be expressed as:
⎛θ ⎞ 1 ⎛θ ⎞
a = −rα sin ⎜ ⎟ − r ω 2 cos ⎜ ⎟ (22)
⎝2⎠ 2 ⎝2⎠
Substituting equation 17 into the former lead to the following expression of the FDDP:
1 x2 1
a = − rα 4 − 2 − ω 2 x (23)
2 r 4
The IDDP:
Kinematics Synthesis of a New Generation of Rapid Linear Actuators for High Velocity Robotics
with Improved Performance Based on Parallel Architecture 119
1 ⎛θ ⎞
− a − r ω 2 cos ⎜ ⎟
2 ⎝2⎠
α= (24)
⎛θ ⎞
r sin ⎜ ⎟
⎝2⎠
Substituting equation 21, equation 17 and equation 18 into the former, we obtain:
−3/2
1 1 ⎛x⎞
α = −2 ar −1 − vx 2 r −4 ⎜ ⎟ (25)
x 2 2 ⎝r⎠
4− 2
r
O B
C
Fig. 10. The diamond shape four-bar
( ( ))
2
⎛ ⎛θ ⎞⎞
2
r sin θ2
x = r 1 − ⎜ sin ⎜ ⎟ ⎟ + R 1 − (26)
⎝ ⎝ 2 ⎠⎠ R
x = R 2 − r 2 − 2 r cos 21 θ ( ) (27)
The inverse kinematics problem requires the distance or position x as input which completes
the two triangle lengths into the diamond shape. Hence, the cosinus laws on general
triangles can be applied to solve the IKP:
⎛ 1 R2 − r 2 − x 2 ⎞
θ = 2arccos ⎜ ⎟ (28)
⎝2 r ⎠
To obtain the differential kinematics models, the kinematics models are differentiated.
FDP:
v=
1 ( )
r 2 sin 21 θ ω
(29)
2 R 2 − r 2 − 2 r cos 1 θ
2 ( )
Differentiation of the IKP leads to the following IDP expression:
4 R 2 − r 2 − 2 r cos( 21 θ )
ω = v∗ (30)
r 4 − 4 cos( 21 θ )2
After testing several approach for obtaining the differential model leading to accelerations,
it was observed that starting with the inverse problem leads to more compact expressions:
The IDDP is obtained by differentiating the IDP:
4 8x 2 ( R 2 − r 2 − x 2 )
α = a( A1 + A2) where A1 = , A2 = − 3/2
(31)
( R 2 − r 2 − x 2 )2 ⎛ ( R 2 − r 2 − x 2 )2 ⎞
r 4− r3 ⎜ 4 − ⎟
r2 ⎝ r2 ⎠
Inverting the IDDP produces the FDDP but it cannot be shown in the most compact form.
The Kite configuration models are definitely more elaborate and complex than for the
rhombus configuration without necessarily leading to any kinematics advantages.
(2)
x1 = 2 r cos θ (32)
This problem can be solved just like solving the original single rhombus FKP.
The distance traveled by the second moving central joint (FKP) is:
(2)
x2 = 2 x1 = 4 r cos θ (33)
The impact of adding the second rhombus is doubling the distance or position reach.
The distance traveled by the third moving central joint or the solution of the FKP of a triple
rhombus is:
(2)
x3 = 3 x1 = 6 r cos θ (34)
The result of the four-bar rhombus repetition is the linear motion amplification by that
repetition number.
To obtain the inverse kinematics problem, one can proceed with inversion of the FKP.
The double rhombus angular position of the actuator can then be deduced:
122 Advanced Strategies for Robot Manipulators
⎛1x⎞
θ = 2arccos ⎜ ⎟ (36)
⎝4r⎠
This equation can then also be extrapolated to a repetition of n identical rhombuses.
⎛ 1 x⎞
θ = 2arccos ⎜ ⎟ (37)
⎝ 2n r ⎠
The forward differential model is obtained by derivation of the forward kinematics model.
For a double rhombus configuration, the relative speed of the second central joint is equal to
the absolute speed of the first central joint:
v2/r = v1 (38)
v2 = v1 + v2 /r (39)
v 2 = 2 v1 (40)
where
Hence, the actual speed of the second extremity or the final end-effector becomes:
The impact of adding the second rhombus is doubling the end-effector velocity.
The same result would be obtained by derivation of the equation for x2.
We now calculate the velocity of the third moving central joint which corresponds to the
solution of the FDP of a triple rhombus.
The inverse differential model can be obtained in two ways, either by derivation of the
inverse kinematics model or inversion of the forward differential model.
By inversion of the FDP, the double rhombus angular position of the actuator can then be
deduced:
1
ω = vr −1 (45)
x2
4−
r2
For the triple rhombus, we extrapolate:
Kinematics Synthesis of a New Generation of Rapid Linear Actuators for High Velocity Robotics
with Improved Performance Based on Parallel Architecture 123
2 1
ω=v (46)
3r x2
4− 2
r
For a linkage with the repetition of n rhombuses, we obtain the following equation:
2 1
ω=v (47)
nr x2
4− 2
r
To determine the accelerations, we will again differentiate the former differential models.
We calculate derivation of the equation for v2 for the second rhombus; it results in doubling
the end-effector acceleration.
The FDDP for the case where we are doubling the rhombus leads to:
2
1
2
( )
a2 = 2 a1 = −2r α sin θ − r ω 2 cos θ
2 ( ) (48)
2
1
2
( )
a3 = 3 a1 = −3r α sin θ − r ω 2 cos θ
2 ( ) (49)
2
1
2
( )
an = na1 = −nr α sin θ − r ω 2 cos θ
2 ( ) (50)
Multying n times the rhombus linkage results in multiplying the acceleration likewise.
The IDDP, inverse model for a double rhombus, through derivation of the IDP or inversion
of the FDDP, the calculation returns this equation:
− 23
1 1 ⎛ 1 x2 ⎞
α = 2 ar −1 − v 2 xr −4 ⎜ 1 − ⎟ (51)
x 2 8 ⎝ 16 r 2 ⎠
16 − 2
r
For three rhombuses, the angular acceleration can then be determined:
− 23
1 1 ⎛ 1 x2 ⎞
α = 2 ar −1 − 7 v 2 xr −4 ⎜ 1 − ⎟ (52)
x 2 2 ⎝ 36 r 2 ⎠
36 − 2
r
We have then extrapolated for a linear actuator constructed with n rhombuses:
− 23
a 1 ⎛ 1 x2 ⎞
α =2 2 −3 −4
− v xn r ⎜ 1 − ⎟ (53)
nr x2 ⎝ 4 n2 r 2 ⎠
4−
n2 r 2
124 Advanced Strategies for Robot Manipulators
4. Kinematics performance
4.1 Singularity analysis
4.1.1 General four bar linkage
For the general four bar linkage, singularities can be found when A + C = 0 using the values
of equation 8. The solution to this equation results in:
⎛ 1 −2 r1 r4 + r1 2 + r2 2 − r3 2 + r4 2 ⎞
θ 2 = arccos ⎜⎜ ⎟⎟ (54)
⎝2 r2 ( − r4 + r1 ) ⎠
( )
sin θ2 = 0 (55)
x2
4− =0 (56)
r2
Hence, the singular position x = 2r corresponds to the same posture as θ = 0.
From a geometric point of view, links have no material existence (no mass) and they can
occupy the same position in space. In reality, the masses do not allow such cases and
therefore the singularity will be alleviated by bar width as will be explained later in the
design section. The IDDP models bring singularities. Observation of the denominator allows
us to determine that the singular configurations are just the same as the one studied for the
IDP since the equations feature the same denominators under the power.
types of four-bars. This would surely represent one reason to apply this mechanism as a
linear actuator.
If R < r, then the mechanism cannot reach an input angle of 180 degrees since this would
mean 2R > 2r in contradiction with stated configurations. Hence, the system will block into
position θmax < 180° unable to go further. The angular range will be limited to [0, θmax] where:
This posture also yield a singularity which can also enforce mechanism blockage. Hence,
this type will not be retained.
4.2 Workspace
The second important performance criterion for robotic design is usually the workspace. In the
case of single DOF device, this narrows down to a simple range which we wish to maximize.
The mechanism can also reach a minimum length which is a far more difficult problem to
determine depending upon the configuration and relative link lengths. This is where
Graschoff’s formulas could help solve this problem. Despite the fact that link lengths value
could be found leading to a coupler curve being a straight line, this constitutes another
reason to avoid the general four-bar mechanisms.
Taking the FKP, equation 14, the maximum value is obtained when cos( θ2 ) = 1 and the
minimum value will be when cos( θ2 ) = −1. Hence, xmax = 2r and the related angle is then θ =
0. Moroever, xmin = −2r and the related angle is then θ = −2π. These values imply that the
pure geometric nature of the kinematics analysis allows the mechanism to reverse by going
unto itself. Hence, the minimum can be seen on the left or negative side of the reference
frame and the maximum is located on the right or positive side.
With considerations of the linkage dimensions, the geometric analysis can be augmented by
taking into account the linkage width.
Firstly, two linkages cannot occupy the same space, therefore, the rhombus linkage
configuration will have a pair of opposite linkages below and one pair above. This lead to
physical constraints equations. This property can also be translated into geometric
information.
These opposite links are parallel pairs which will eventually touch each other alongside at
two mechanism rotations. These postures could be considered as folded ones. The first
corresponds to the minimum rotation and the second to the maximum rotation.
Let the rhombus linkage be constructed by four bars of identical width w.
The minimal rotation angle increases to:
⎛w⎞
θ min = 2arcsin ⎜ ⎟ (60)
⎝r⎠
Taking into account that the kinematics chain cannot reverse by going unto itself, the
maximal rotation angle reduces to:
⎛w⎞
θ max = 2arccos ⎜ ⎟ (62)
⎝r⎠
The final range of the linear actuator is then the interval determined by: [2 arcsin( wr ,
2 arccos( wr ].
X X
O O
B
B
X max
X min
The extreme positions can be determined from these values. Minimal length is going to
occur at maximal angular displacement.
⎛θ ⎞
xmin = 2 r cos ⎜ ⎟ (63)
⎝ 2 max ⎠
Substituting θmax into the equation, we get:
(
xmin = 2 r cos 2arccos ( w
r) ) (64)
⎛θ ⎞
xmax = 2 r cos ⎜ ⎟ (65)
⎝ 2 min ⎠
Substituting θmin into the equation, we get:
(
xmax = 2 r cos 2arcsin ( w
r) ) (66)
θ min = 2arcsin ( w
r) (68)
Taking into account that the kinematics chain cannot reverse by going unto itself, the
maximal rotation angle reduces to the case where the long bars touch each other in the
negative sense of the reference frame. We have to take then the angle outside the triangle
formed by these long bars:
⎛w⎞
θ max = 2π − 2arcsin ⎜ ⎟ (70)
⎝R⎠
The extreme positions can be determined from these values. In the case of maximal position,
the two geometric triangles formed by the long and short links add up:
Minimal length is going to occur at maximal angular displacement.
w ⎛θ ⎞
xmin = R 2 − ( )2 − r cos ⎜ ⎟ (71)
2 ⎝ 2 max ⎠
Maximal length is going to occur at minimal angular displacement.
w ⎛θ ⎞
xmax = R 2 − ( )2 + r cos ⎜ ⎟ (72)
2 ⎝ 2 max ⎠
128 Advanced Strategies for Robot Manipulators
E max
X O
O
B
B
E min
4.3 Encumbrance
4.3.1 The rhombus configuration
The proposed linear actuators are based on four-bar linkage where encumbrance becomes
an issue considering that the mechanism spread sideways making them subject to collisions
if other actuators would be located in the vicinity such as it is often the case with parallel
manipulators.
Ecumbrance is defined as the distance from one side of the mechanism to the other side
taking into account the linkage width.
A Rhombus would have minimum encumbrance of Emin = 2w when the angle is at θmin. This
characteristic is relatively unimportant compared to the maximum encumbrance occuring at
the maximum input angle posture θmax:
Emax = 2 r 2 − w 2 + w (73)
Emax = 2 r + w (74)
As can be observed, the kite encumbrance only really depends on the dimension of the
shortest links.
(
xmin = 4 r cos 2arccos ( w
r) ) (75)
Kinematics Synthesis of a New Generation of Rapid Linear Actuators for High Velocity Robotics
with Improved Performance Based on Parallel Architecture 129
(
xmax = 4 r cos 2arcsin ( w
r) ) (76)
(
xmax = 6 r cos 2arcsin ( w
r) ) (77)
And the maximum position is calculated using:
(
xmax = 6 r cos 2arcsin ( w
r) ) (78)
(
xmax = n 2 r cos 2arcsin ( w
r) ) (79)
And the maximum position is calculated using:
(
xmax = n 2 r cos 2arcsin ( w
r) ) (80)
Xmax
e= (81)
Emax
For the repeated rhombus, this occus when θ = θmin and the encumbrance ratio becomes:
(
e = n 2 r cos 2arcsin ( w )
r ) 2 r −w +w
2 2
(82)
Hence, the motion to encumbrance ratio is increasing proportionaly with the rhombus repetition.
5. Design examples
5.1 Initial prototypes
A first group of prototypes were constructed and tested using a Meccano set while the
author was working at the Ecole Nationale des Arts et Metiers in Metz. This resulted in the
constuction of a planar parallel manipulator as seen in figure 14. The DC motor was the
typical Meccano 36 VDC.
130 Advanced Strategies for Robot Manipulators
5.2.1 Configuration
Here are the mechanism geometric parameters. They were constructed with two
standardized bars. The short bars have length r = 10 cm and width w = 3 cm. The long bars
have length R = 20 cm with same width.
The geared electrical motors were selected to provide maximum rotation speed of 120 RPM.
Hence, ωmax = 4π radians/s.
acceleration changes non-linearily with the angular velocity. It changes almost linearily with
the angular position. It reaches very high values when the linkage reaches very close to the
maximum position.
(c) Acceleration
6. Conclusion
As can be seen by the results obtained by calculations as well by experiments on the
prototypes, four-bar mechanisms can be rearranged in rhombus and kite configurations
which lead to very performant prismatic pairs allowing to design very fast linear actuators.
Moreover, to improve performance and to reduce emcumbrance, networking the rhombus
four-bars can lead to very good results.
In the author’s knowledge, this is the first time that four-bars were envisaged to be applied
as linear actuators.
The next step will be to analyze their dynamics design integrating force analysis.
Then, to design a large scale parallel robot prototype will help investigate their worthiness
towards the design a very high speed milling machine.
Finaly, several optimization problems may arise to determine proper linkage sizing.
7. References
Ceccarelli M. (1997) A new 3 d.o.f. spatial parallel mechanism. Mechanism and Machine
Theory, vol.32(no.8), pp 896-902.
DaVinci, L. (1493) Tratado de Estatica y Mechanica en Italiano. CodexMadrid 1, National
Library Madrid.
Euclid (2002) Euclid’s Elements - All thirteen books in one volume. Green Lion Press. Based on
Heath’s translation, Greek original from c. 300 BC
Fichter, E.F., Kerr, D.R. and Rees-Jones, J. (2009) The Gough-Stewart platform parallel
manipulator: a retrospective appreciation. Proceedings of the Institution of Mechanical
Engineers, Part C: Journal of Mechanical Engineering Science, Volume 223, Number 1,
pp. 243-281.
Gogu, G. (2004) Chebychev-Grübler-Kutzbach’s criterion for mobility calculation of
multiloop mechanisms revisited via theory of linear transformations. European
Journal of Mechanics - A/Solids. Volume 24, Issue 3, May-June 2005, Pages 427-441
Gough, V. E. and Whitehall, S. G. (1962) Universal tyre testing machine. Proceedings of the 9th
International Automobile Technical Congress, discussion, Federation Internationale des
Societes dIngenieurs des Techniques de lAutomobile (FISITA) (Ed. G. Eley),
(IMechE 1, London, UK), pp. 117137.
Kempe A.B. (1877) How to draw a straight line; a lecture on linkages. Macmillan and Co,
London
Moon, F.C. (2007) The Machines of Leonardo Da Vinci and Franz Reuleaux. Volume on
Kinematics of Machines from the Renaissance to the 20th Century, Series on
History of Mechanism and Machine Science, Springer Netherlands, 417 pages
Reuleaux, F. (1876) Kinematics of Machinery: Outlines of a Theory of Machines. Macmillan and
Co., London.
Rolland, L. (1998) Conception de mécanismes, élaboration des principes demobilit´e.
Technical report number 98-02, ISR, EPFL, Lausanne.
Rolland, L. (1999) The Manta and the Kanuk novel 4-dof parallel mechanisms for industrial
handling. Proceedings of the ASME International Mechanical Engineering Congress,
Nashville, 14-19 Novembre 1999.
134 Advanced Strategies for Robot Manipulators
Scheiner, C. (1631) Pantographice, seu ars delineandi res quaslibet per parallelogrammum lineare
seu cavum, mechanicum, mobile. Romae: Ex typographia Ludouici Grignani,
sumptibus Hermanni Scheus, vol. 12, 108 pages.
Soylemez E. (1999) Mechanisms. METU Press, Ankara, 350 pages.
Uicker, J.J., Pennock, G.R. and Shigley, 2003, J.E. Theory of Machines and Mechanisms, third
edition. Oxford University Press, New-York, 2003, 734 pages.
7
1. Introduction
1.1 Robot manipulators
Robot manipulators are well-known as nonlinear systems including strong coupling
between their dynamics (Craig, 1996). These characteristics, in company with: 1) structured
uncertainties caused by model imprecision of link parameters, payload variation, etc., and 2)
unstructured uncertainties produced by un-modeled dynamics –such as nonlinear friction and
external disturbances– make the motion control of rigid-link manipulators a complicated
problem (Spong & Vidiasagar, 1989). Practice trajectory control is required in many of the
sophisticated applications of manipulators (e.g. machining, welding, complex assembly). On
the other hand, robot manipulators have to face various uncertainties in their dynamics and
they are required to handle various tools and, hence, the dynamic parameters of the robots
vary during operation. Thus, it is difficult to initiate an appropriate mathematical model for
employing model-based control strategies.
In general, the intelligent control approaches can attenuate the effects of structured
parametric uncertainty and unstructured disturbance by using their powerful learning
ability without a detailed knowledge of the controlled plant in the design processes. On the
other hand, many intelligent control algorithms could have been found for the robot control
system without including the actuator dynamics, while, actuator dynamics carry out a
significant role in the complete robot dynamics and ignoring them may cause detrimental
effects, especially in the case of high-velocity moment, highly varying loads, friction, and
actuator saturations (Chang et al., 2008), (Chang & Yen, 2009). Since the electrical actuators
are highly controllable in comparison with the other one, they are more convenient for
driving manipulators. Also, in practical applications, the voltages or currents of the
electrical actuators are accessible for applying control commands and consequently, torque-
based control design confronts implementation problems when one intends to apply the
torque control commands directly to actuators. Additionally, one constraint in the robot
controller designs is saturation nonlinearity of actuators which is less considered in control
design of robot manipulators.
M (q )q + C (q , q )q + G( q ) + τ d = τ (1)
where M (q ) ∈ R n×n is the completed inertia matrix, the vectors q , q , q ∈ R n are the position,
velocity and angular acceleration of the robot joints, respectively. Moreover, the matrix
C (q , q ) ∈ R n×n is the matrix of Coriolis and centrifugal forces and G(q ) ∈ R n is the gravity
vector. Also, τ d ∈ R n denotes the vector of disturbance and un-modeled dynamics, and
finally, τ is the torque vector. In the following, two conventional properties of the robot
manipulators are considered.
Property 2.1. The inertia matrix M (q ) is symmetric and positive definite, M T = M .
Property 2.2. The matrix of ( M − 2C ) is skew-symmetric, i.e. for any vector of X , we have
X T ( M − 2C )X = 0 .
e = qd − q (2)
In order to apply the SMC, the sliding surface is considered as relation (3) which contains
the integral part in addition to the derivative term:
t
s = e + λ1 e + λ2 ∫ edt (3)
0
where λi is diagonal positive definite matrix. Therefore, s = 0 is a stable sliding surface and
e → 0 as t → ∞ . The robot dynamical equations can be rewritten based on the sliding
surface (in term of filtered error) as:
Ms = −Cs + f + τ d − τ (4)
Where
t
f = M (q d + λ1 e + λ2 e ) + C (q d + λ1 e + λ2 ∫ edt ) + G (5)
0
τ = fˆ + K vs + K sgn(s ) (6)
where
t
∫
fˆ = Mˆ (q d + λ1e + λ 2 e) + Cˆ (q d + λ1e + λ 2 edt ) + Gˆ
0
(7)
t
is an estimation of f and K v s = K v e + K v λ e + K v λ ∫ edt is the outer PID tracking loop, and
0
K v , K are diagonal positive definite matrices and are defined such that the stability
conditions are guaranteed. The sgn(s) is also the sign function.
We have also:
t
f = M(qd + λ1 e + λ2 e ) + C (qd + λ1 e + λ2 ∫ edt ) + G ≤ F (8)
0
where f = f − fˆ , M = M − M
ˆ , C = C − Cˆ ,and G = G − Gˆ . Vector F can also be selected as
the following relation:
t
F = M(qd + λ1 e + λ2 e ) + C (qd + λ1 e + λ2 ∫ edt + G (9)
0
In order to govern the system states ( e , e ) to reach the sliding surface s = 0 in a limited time
and to remain there, the control law should be designed such that the following sliding
condition is satisfied (Slotin & Li, 1991):
1 d T
⎡s Ms ⎦⎤ < −η (sT s )1/2 , η > 0 (10)
2 dt ⎣
This aim is fulfilled in the following lemma.
Lemma 2.1. In the SMC design of a system with dynamical equation (1) and sliding surface
(3), if the control input τ is selected as (6), by considering F as (9) and
K = diag(K 11 , K 22 ,… , K nn ) with the following components:
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 139
1 T
V= s Ms (12)
2
Since M is positive definite, for s ≠ 0 we have V > 0 and by taking derivative from relation
(12) and regarding the symmetric property of M, it can be written:
1 T
V= s Ms + sT Ms (13)
2
1 T
V= s Ms − sT Cs + sT ( f + τ d − τ ) = sT ( f + τ d − τ ) (14)
2
n
V = sT ( f + τ d − fˆ − K v s − K sgn(s )) = sT ( f + τ d − K v s ) − ∑ K ii si (15)
i =1
Since the following inequality (16) is valid and by regarding the relation (11), we have:
F + K v s + TD ≥ f + τ d − K vs (16)
K ii ≥ [ f + τ d − K v s]i + ηi (17)
n
V ≤ −∑ηi si (18)
i =1
This indicates that V is a Lyapunov function and the sliding condition (10) has been
satisfied.
The use of sign function in the control law leads to high oscillations in control torque which
is undesired phenomenon and is called chattering. To overcome this drawback, there are
some solutions that one of them is using the following saturation function instead of sign
function in the discontinuous part of the control law:
⎧1 s ≥ϕ
⎪
⎛s⎞ ⎪s
sat ⎜ ⎟ = ⎨ −κ < s < ϕ (19)
⎝ϕ ⎠ ⎪ϕ
⎪⎩−1 s ≤ −ϕ
140 Advanced Strategies for Robot Manipulators
By this, there is a boundary layer ϕ around the sliding surface such that once the state
trajectory reaches this layer, then it will be remaining there.
K = N ⋅ K fuzz (20)
K v = N v ⋅ K fuzz (21)
These factors can be selected by trial and error such that the stability condition (17) is
satisfied.
NB NSZEPS PB N Z P
1 1
0.8 0.8
Degree of membership
Degree of membership
0.6 0.6
0.4 0.4
0.2 0.2
0 0
(a) (b)
Fig. 1. The membership functions, (a) input s , (b) input s
The maximum values of K and Kv are limited according to the system actuators power, and the
minimum value of K should not be less than the provided amount in relation (17). The fuzzy
rule base has been given in table 1 in which the following abbreviations have been used: NB:
Negative Big; NS: Negative Small; Z: Zero; PS: Positive Small; PB: Positive Big; M: Medium.
For example, when s is negative small (NS) and s is positive (P), then K fuzz is small (S).
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 141
S M B
1
0.8
Degree of membership
0.6
0.4
0.2
s
NB NS Z PS PB
s
N B B M S B
Z B M S M B
P B S M B B
Table 1. The fuzzy rule base for tuning K fuzz
Simulation example 2.1. In order to show the effectiveness of the proposed control law, it is
applied to a two-link robot with the following parameters:
⎡α + β + 2γ cos q 2 β + γ cos q2 ⎤
M(q ) = ⎢ ⎥ (22)
⎣ β + γ cos q 2 β ⎦
⎡ −γ q 2 sin q 2 −γ ( q1 + q 2 )sin q 2 ⎤
C (q , q ) = ⎢ ⎥ (23)
⎣ γ q1 sin q 2 0 ⎦
where m10 = 4 and m20 = 2 , and M̂ , Ĉ , and Ĝ are estimated. The desired state trajectory is:
⎡ 1 − cos π t ⎤
qd = ⎢ ⎥ (26)
⎣ 2 cos π t ⎦
142 Advanced Strategies for Robot Manipulators
⎡ 0.5sin 2π t ⎤
τd = ⎢ ⎥ (27)
⎣ 0.5sin 2π t ⎦
⎡0.5 ⎤
which leads to TD = ⎢ ⎥ .
⎣0.5 ⎦
The design parameters are determined as follow:
⎡15 0 ⎤ ⎡ 40 0 ⎤
λ1 = ⎢ ⎥ , λ2 = ⎢ 0 40 ⎥ (28)
⎣ 0 15 ⎦ ⎣ ⎦
Values of ϕ and η are selected as ϕ = 0.167 and η = [ 0.1 0.1] . Moreover, the factors N
T
⎡ 50 0 ⎤ ⎡5 0 ⎤
N=⎢ ⎥ , N v = ⎢0 10 ⎥ (29)
⎣ 0 5 ⎦ ⎣ ⎦
In order to show the improvement due to the proposed method, the simulation results of
applying this method are compared with the related results of the conventional SMC. The
tracking error and control law in the case of conventional SMC have been shown in Fig. 3
and Fig. 4, respectively. The corresponding graphs for the case of applying fuzzy SMC-PID
are also provided in Fig. 5 and 6.
0.15
Error1(rad)
0.1
0.05
0
-0.05
0 2 4 6 8 10
time(sec)
2
1.5
Error2(rad)
1
0.5
0
-0.5
0 2 4 6 8 10
time(sec)
150
100
input1(N.m)
50
-50
0 2 4 6 8 10
time(sec)
100
input2(N.m)
50
-50
0 2 4 6 8 10
time(sec)
0.15
0.1
Error1(rad)
0.05
0
-0.05
0 2 4 6 8 10
time(sec)
2
1.5
Error2 (rad)
1
0.5
0
-0.5
0 2 4 6 8 10
time(sec)
200
input1 (N.m)
100
-100
0 2 4 6 8 10
time(sec)
100
50
input2 (N.m)
-50
-100
0 2 4 6 8 10
time(sec)
0.01
Error1(rad)
0.005
-0.005
-0.01
0 2 4 6 8 10
time(sec)
-3
x 10
5
Error2(rad)
-5
0 2 4 6 8 10
time(sec)
Fig. 7. The enlargement of the tracking errors in the case of using conventional SMC
-4
x 10
5
Error1 (rad)
-5
0 2 4 6 8 10
time(sec)
-3
x 10
1
Error2 (rad)
0.5
0
-0.5
-1
0 2 4 6 8 10
time(sec)
Fig. 8. The enlargement of the tracking errors in the case of using Fuzzy SMC-PID
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 145
where F(q ,τ ) ∈ R n stands for the friction vector which is as follows (Cai & Song, 1994):
⎧ r if x>r
⎪
sat ( x ; r ) = ⎨ x if −r ≤ x ≤ r
⎪
⎩−r if x < −r
sup { gi ( q ) } ≤ gi , i = 1, ,n (32)
q ∈R n
where gi stands for the i-th element of G( q ) and gi is finite nonnegative constant. Assume
that the maximum torque that joint actuator can supply is τ max . Therefore:
τ i ≤ τ imax , i = 1, ,n (33)
In robot modeling, one can well determine the terms M (q ) and G( q ) but it is difficult in
most cases obtaining the parameters of C (q , q ) and F(q ,τ ) exactly. So, in present section, the
matrix C is considered as follows:
C = Cˆ + ΔC (35)
where Ĉ denotes estimation of C , and ΔC is bounded estimation error which has the
following relation:
ΔC i , j ≤ 0.1 C i , j (36)
where C i , j stands for elements of the matrix C . Also the vector F is supposed as an external
disturbance with the following unknown upper bound:
F ≤ Fup (37)
q = qd − q (38)
Here, the set-point tracking problem refers to define the control law such that error e would
be driven toward the inside of an arbitrary small region around zero with maintaining the
torques within the constraints (33). In succeeding subsections, this aim will be attained.
s = e + λe (39)
qr = q d − λ e (40)
s = q − qr (41)
τ = τˆ − K sgn(s) (42)
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 147
such that
ˆ +G
τˆ = Mqr + Cq (43)
r
and
K i ≥ ΔCqr + Γ i (44)
then the sliding condition (10) is satisfied. In the last inequality, Ki denotes the element of
sliding gain vector K and Γ is design parameter vector which must be selected such
that Γ i ≥ Fup + ηi .
Proof: Consider the following Lyapunov function candidate:
1 T
V= s Ms (45)
2
Since M is positive definite, for s ≠ 0 we have V > 0 and by taking time derivative of the
relation (45) and regarding the symmetric property of M, it can be written:
1
V = sT Ms + sT Ms (46)
2
from (40), gives:
1
V = sT ( Mq − Mqr ) + sT Ms (47)
2
n
V = sT ( ΔCqr + F ) − ∑ K i si (49)
i =1
n
V ≤ −∑ηi si (50)
i =1
This indicates that V is a Lyapunov function and the sliding condition (10) has been
satisfied.
Note that, in general, the sign function is replaced by saturation function as sat ( s / ϕ ) ,
where ϕ denotes boundary layer thickness.
x1
NB NS ZE PS PB
x2
NB NB NB NS ZE ZE
NS NB NB NS ZE ZE
ZE NS NS ZE PS PS
PS ZE ZE PS PB PB
PB ZE ZE PS PB PB
Table 2. The fuzzy rule base for obtaining output y
Thus, one can compute the output y in terms of inputs as follows (Wang, 1997):
⎛ 2 ⎞
∑∑ y ⎜ j∩= 1 μ Al j ( x j ) ⎟
l1 l2
⎝ j ⎠
y ( x ) = ϕ ( x1 , x 2 ) = 1 2
l l
(52)
⎛ 2
⎞
∑∑ ⎜ ∩ μ Al j ( x j ) ⎟
l1 l2 ⎝ j = 1 j ⎠
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 149
Special properties of this input-output mapping y( x ) for x1, x2 are given in (Santibanez et
al., 2005).
Lemma 2.3. For the system with dynamical equation (30), if one chooses the following
control law,
τ = ϕ ( q , q ) + G(q ) (53)
where q is defined as (38) and q = qd − q is velocity error vector, then the closed-loop system
shown in Fig. 11 becomes stable.
Proof: the stability analysis is based on the study performed in (Calcev 1998) and is fully
discussed in (Santibanez et al., 2005), so it is omitted here. Note that for constant set-point
we have q d = 0 , hence q = −q .
Fig. 11. Closed-loop system in the case of fuzzy controller (Santibanez et al., 2005)
⎧⎪ τˆ − K sgn( s ) when q e ≥ α
τ =⎨ (54)
y (
⎪⎩ e eq , q ) + G( q ) when qe < α
where α is strictly positive small parameter which can be determined adaptively or set to a
constant value. So, while the magnitude of error is greater than or equal to α , SMC drives
the system states, errors in our case, toward sliding surface and as soon as the magnitude of
error becomes less than α , then the SFC which is designed independent of initial
conditions, controls the system. Since the SMC shows faster transient response, the response
of the system controlled by (54) is faster than the case of SFC. Additionally, in spite of the
torque boundedness, since the SFC controls the system in the steady state, the proposed
controller (54) has less set-point tracking error. Also, since near the sliding surface the
proposed controller switch from SMC to SFC, therefore, the chattering is avoided here.
150 Advanced Strategies for Robot Manipulators
Simulation example 2.2. In order to show the effectiveness of the proposed control law, it is
applied to a two-link direct drive robot arm with the following parameters (Santibanez et
al., 2005):
⎡ 2.351 + 0.168 cos( q 2 ) 0.102 + 0.084 cos( q 2 )⎤
M(q ) = ⎢ ⎥
⎣0.102 + 0.084 cos(q 2 ) 0.102 ⎦
⎡ −0.084 sin(q 2 )q 2 −0.084 sin(q 2 )(q1 + q 2 )⎤
Cˆ (q , q ) = ⎢ ⎥
⎣ 0.084 sin(q 2 )q1 0 ⎦
⎡ 3.921sin(q1 ) + 0.186 sin(q1 + q 2 )⎤ (55)
G( q ) = 9.81 ⎢ ⎥
⎣ 0.186 sin(q1 + q 2 ) ⎦
⎡ 2.288q1 + 8.049 sgn(q1 ) + ⎡⎣1 − sgn(q1 ) ⎤⎦ sat(τ 1 ;9.7) ⎤
F(q ) = ⎢ ⎥
⎢⎣0.186q 2 + 1.734 sgn(q 2 ) + ⎡⎣1 − sgn(q 2 ) ⎤⎦ sat(τ 2 ;1.87)⎥⎦
C = Cˆ + ΔC
According to the actuators manufacturer, the direct drive motors are able to supply torques
within the following bounds:
τ 1 ≤ τ 1max = 150[Nm]
(56)
τ 2 ≤ τ 2max = 15[Nm]
The desired set-point is,
qd = [π −π ]
T
(57)
which is applied as a step function at time zero. The SMC design parameters are as below:
⎡10 0 ⎤ ⎡140 ⎤
λ=⎢
0 10 ⎥ , Γ = ⎢ 8 ⎥ and φ = 5 (58)
⎣ ⎦ ⎣ ⎦
For SFC case, according to Fig. 9 and Fig. 11, px j = { − p2 j , − p1 j , p0 j , p1 j , p2 j } is fuzzy partition of
the input universe of discourse and py = { − y 2 , − y1 , y 0 , y 1 , y 2 } is for output universe of
discourse. Now, SFC design parameters are given by following equations (Santibanez et al.,
2005):
pq1 = { −180, − 4,0, 4,180}
pq2 = { −180, − 2,0, 2,180}
pq1 = { −360, − 270,0, 270, 360}
(59)
pq2 = { −360, − 270,0, 270, 360}
py1 = { −109, − 90,0,90,109}
py2 = { −13, − 9,0,9,13}
For our proposed controller (54), the constant α = 0.3 is supposed. Additionally, to show the
improvement achieved from applying the proposed method of this section (incorporating
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 151
SMC and SFC), the simulation results of applying this method are compared with the
related results of the SMC case and SFC case, separately. The error vector and control law in
the case of conventional SMC have been shown in Fig. 12 and Fig. 13, respectively.
4
1
Error (rad)
-1
-2
-3
-4
0 0.5 1 1.5 2 2.5 3 3.5 4
Time(sec)
150
100
Input torque (Nm)
50
-50
-100
0 0.5 1 1.5 2 2.5 3 3.5 4
Time(sec)
As it can be seen from these results, the proposed incorporating SMC and SFC controller has
faster response and less tracking error in comparison with SMC and also the error vector
converges toward zero faster than SFC.
In order to show the robustness of the proposed method, the inertia and torque
perturbations are considered as following. The elements of inertia matrix are supposed to
increase fifty percent after 2 sec. It can be a weight that added to the mass of 2nd link. Also,
disturbance torque is considered with the following equation.
τ d = [ 3sin 2π t 3sin 2π ]
T
(60)
1
Error (rad)
-1
-2
-3
-4
0 0.5 1 1.5 2 2.5 3 3.5 4
Time(sec)
80
60
40
Input torques (Nm)
20
-20
-40
-60
-80
-100
0 0.5 1 1.5 2 2.5 3 3.5 4
Time (sec)
In this case, the vector of joint errors is shown in Fig. 18. The errors are as good as previous
case. Fig. 19 illustrates the control torques which are not change significantly, and because of
existing perturbations, they alter trivially after 2 sec. these two recent results verify the
robustness of the presented approach.
1
Error (rad)
-1
-2
-3
-4
0 0.5 1 1.5 2 2.5 3 3.5 4
Time(sec)
Fig. 16. Error vector in the case of incorporating SMC and SFC
150
100
Input torques (Nm)
50
-50
-100
0 0.5 1 1.5 2 2.5 3 3.5 4
Time(sec)
Fig. 17. The control torques in the case of incorporating SMC and SFC
154 Advanced Strategies for Robot Manipulators
1
Error (rad)
-1
-2
-3
-4
0 0.5 1 1.5 2 2.5 3 3.5 4
Time (sec)
Fig. 18. Error vector in the case of torque and inertia perturbations
150
100
Input torques (Nm)
50
-50
-100
0 0.5 1 1.5 2 2.5 3 3.5 4
Time (sec)
Fig. 19. The control torques in the case of torque and inertia perturbations
system. A robust neural-fuzzy-network controller was designed in (Wai & Chen 2006) for
the position control of an n-link robot manipulator including actuator dynamics. Although,
their control scheme does not require compensating auxiliary control design, but the
employed network is more complicated and uses excess number of neurons. In addition, the
second derivative of position angle is required as a part of controller inputs. Capisani et al.,
(Capisani et al., 2009) presented an inverse dynamic-based second-order sliding mode
controller to perform motion control of robot manipulators, but this method involves the
higher order derivatives of the state variables.
In this section, the motion tracking control of multiple-link robot manipulators actuated by
permanent magnet DC motors is addressed. Sliding-mode-PID tracking controller is
designed such that all the states and signals of the closed loop system remain bounded in
the presence of unknown parameters and uncertainties. Also, neural network universal
approximation property is employed for compensating uncertainties. Furthermore, the
proposed controller contains an outer PID-loop that enhances the approximation
performance during the initial period of weight adaptations, and provides designing a
simple NN with lower amount of layers and neurons. Adaptation laws are applied to adjust
the NN weights on-line. In order to avoid high gain control, the gain factor of robustifying
term is designed adaptively (Shafiei & Soltanpour, 2010).
di dθ
V = Ri + L + Kb (61)
dt dt
J mθ + Bmθ + τ m = τ (62)
τ = K mi (63)
where V is the armature voltage of the motor, R and L are armature equivalent resistance
and inductance, respectively, K b is the back electromotive force constant, i is the armature
current and θ denotes the rotor position, J m is the total moment of inertia, Bm is the
damping coefficient, τ m and τ represent the generated motor torque and the load torque,
respectively, and K m is the diagonal matrix of motor torque constant.
The dynamical equation of an n-link robot manipulator is in the standard form of (30) and is
rewritten here.
q = grθ (65)
156 Advanced Strategies for Robot Manipulators
and
τ m = grτ (66)
where g r is the diagonal matrix of reduction ratio. In the following a practical constraint is
considered.
Constraint 3.1. The maximum voltage that joint actuator can supply is V max . So, we have:
Vi ≤ Vimax , i = 1, ,n
It should be noted that, the applicable control input for driving robot arm is the armature
voltage of the motors, here. So, by using equations (61)-(66) and neglecting the inductance
L , because of its tiny amount, the following equation is achieved.
U = Dq + H + d (68)
Remark 3.1. By noting that the parameters, R , K m , J m and g r are positive definite diagonal
matrices, the matrix D is symmetric and positive definite.
Remark 3.2. From relations (69) and (71), and property 2.2, the matrix ( D − 2Vm ) is skew-
symmetric too.
e = qd − q (73)
A key step in designing sliding mode controller is to introduce a proper sliding surface so
that tracking errors and output deviations can be reduced to a satisfactory level (Eker, 2006).
Accordingly, the sliding surface is considered as (74), containing the integral part in
addition to the derivative term.
t
s = e + λ1 e + λ2 ∫ edt (74)
0
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 157
where λi is diagonal positive definite matrix. Hence, s = 0 is a stable sliding surface and
e → 0 as t → ∞ . Only defining the sliding surface as (74) is not adequate to claim that SMC-
PID is designed, but the control effort must contain the independent PID part. For this
purpose, the robot dynamic equations can be rewritten based on the sliding surface (in term
of filtered error) as follows:
Ds = −Vm s + f − U (75)
where
where D , Vm and d are given by (69), (71) and (72) respectively, and
T
x = ⎡⎣qTd sT qT ⎤⎦ (77)
Note that the input vector of s includes linear combination of e and e , (i.e. e + λ1e ) which
they comprise qd , q and qd , q , too, respectively. The input dimension of the two-layer
NN designed here is less than that of given by (Lewis et al., 1996), and thus the proposed
method is more desirable from an implementation point of view. Sliding mode control
strategy consists of designing a two-part controller.
USMC = U eq + U s (78)
with U eq is equivalent control part which is applied to cancel the uncertain nonlinear
function f , and U s specifies robust control term. Considering unknown parameter,
uncertainties and disturbances indicates that the function f is not accessible. Briefly
speaking, neural networks incorporate to reconstruct the U eq part by approximating the
function f , here. According to universal approximation property of neural networks
(Lewis et al., 1998), there is a two-layer NN with sufficient number of neurons, and sigmoid
or RBF activation function for hidden layer and linear activation function for output layer
(see Fig. 20) such that:
f ( x ) = W Tσ (V T x ) + ε (79)
where x ∈ R N2 is the input vector computed by (77), V ∈ R N 2×N2 and W ∈ R N 2×N 2 represents
the NN weights for hidden and output layers, respectively, σ (⋅) denotes activation function
of the hidden layer and ε is NN approximation error. Choosing activation function is
arbitrary provided that the function satisfies an approximation property and it and its
derivative are bounded (Lewis et al., 1998), consequently the sigmoid activation function is
considered, here.
1
σ ( z) = (80)
1 + e− z
Succeeding section explains complete controller design and investigates stability content.
158 Advanced Strategies for Robot Manipulators
fˆ ( x) = Wˆ T σ (Vˆ T x) (81)
~ ~
V = V − Vˆ , W = W − Wˆ (82)
σ~ = σ (V T x) − σ (Vˆ T x) = σ − σˆ (83)
~ ~
σ (V T x) = σ (Vˆ T x) + σ ′(Vˆ T x)V T x + Oh (V T x) (84)
dσ ( z )
σ ′( z ) ≡ (85)
dz z = zˆ
~ ~ ~
σ~ = σ ′(Vˆ T x)V T x + Oh (V T x) = σˆ ′V T x + Oh (86)
Now, one can obtain overall error between optimum function f and its estimation fˆ as:
f − fˆ = W Tσ (V T x ) + ε − W ˆ Tσ (Vˆ T x ) = W Tσ (V T x ) + W ˆ Tσ (Vˆ T x ) + ε
ˆ T [σ ′(Vˆ T x )V T x + O ] + ε
= W T [σ (Vˆ T x ) + σ ′(Vˆ T x )V T x + O ] + W
h h
(87)
ˆ Tσˆ ′V T x + W Tσˆ ′V T x + W T O + ε
= W σ (Vˆ T x ) − W Tσ ′(Vˆ T x )Vˆ T x + W
T
h
where
~
ε N = W T σˆ ′ V T x + W T Oh + ε (88)
Design of the control system is provided in the following theorem and is illustrated in Fig.
21 schematically.
Theorem 3.1. Robot manipulator including actuator dynamics represented by equation (68)
is considered, and the sliding surface is defined by (74). If the control input U is designed
as (90) together with adaptation laws of NN controller as (91)-(93), then the asymptotic
stability of the dynamical system is guaranteed.
U = K v s + fˆ + Kˆ sgn( s ) (90)
Vˆ = β xs T Wˆ T σˆ ′ (92)
Kˆ = γ s T sgn( s ) (93)
1 T 1 1 1 T
VL =
2
s Ds +
2α
(
tr W T W +
2β
)
tr V TV +
2γ
K K( ) (94)
~
where tr (⋅) denotes the trace operator and K = K − Kˆ . Differentiating of the relation (94)
gives
1
2
1
α
1
β ( 1
)
VL = sT Ds + sT Ds + tr W T W + tr V TV + K T K
γ ( ) (95)
160 Advanced Strategies for Robot Manipulators
By substituting (90) in to the first part of (95) and by using (87) one can obtain
Some useful relations for manipulating last tow equations are provided in the following.
(
⎧sT W Tσˆ = tr W Tσˆ sT
⎪
)
⎪ T T ˆT
(
⎨s W σˆ ′V x = tr W σˆ ′Vˆ xs
T T T
)
⎪
⎪ sT W
⎩ (
ˆ Tσˆ ′V T x = tr V T xsT W
ˆ Tσˆ ′
)
Replacing (96) in (95) and using above relations, produce
1 ⎡ 1 ⎤
VL = −sT K v s + sT (D − 2Vm )s + tr ⎢ W T ( W + σˆ sT − σˆ ′Vˆ T xsT )⎥
2 ⎣ α ⎦
(97)
⎡ 1 ˆ Tσˆ ′)⎤ + sT ε − Ks
ˆ T sgn(s ) + 1 KK
+ tr ⎢V T ( V + xsT W ⎥ N
⎣ β ⎦ γ
adaptive laws (91) and (92) are taken in to account, then we have
m
2
VL ≤ −K vmin s + ε N ( s1 + s 2 + + sm ) − K ∑ si ≤ −K vmin s ≤ 0
2
(99)
i =1
where K vmin is minimum singular value of K v . Since VL ≤ 0 , the stability in the sense of
Lyapunov is guaranteed which implies that the parameters s , W , V and K (and
t
consequently Ŵ , V̂ , K̂ ) are bounded. In addition, lim ∫ −VL dτ < ∞ and −VL is bounded,
t →∞ 0
hence Barbalat’s Lemma (Khalil, 2001) indicates that lim( −VL ) = 0 . Note that
t →∞
2
( −VL ) ≥ K vmin s ≥ 0 , as a result s → 0 as t → ∞ . Therefore, the proposed control system is
asymptotically stable.
Remark 3.3. The PID term in the above control effort, makes Lyapunov derivative more
negative, so it makes the transient response faster and also ensures the performance
efficiency during the initial period of weights adaptations.
Remark 3.4. In practical systems, however, it is impossible to achieve infinitely fast switching
control, because of finite time delays for the control computation and limitation of physical
actuators. For that reason, the sign function is replaced by saturation function here, and the
stability matter is investigated analytically.
The saturation function is selected as
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 161
⎧ s
s ≤ϕ
⎛ s ⎞ ⎪⎪ ϕ
sat ⎜ ⎟ = ⎨ (100)
⎝ ϕ ⎠ ⎪sgn( s ) s ≥ϕ
⎪⎩ ϕ
where ϕ is a thin boundary layer such that 0 < ϕ ≤ 1 . The adaptive law (93) must be
replaced by Kˆ = γ sT sat( s ϕ ) ; So, the equation (98) is changed to
m
+ sm ) − K ∑ si < 0
2
VL ≤ −K vmin s + ε N ( s1 + s2 + (102)
i =1
b. if s ≤ ϕ , then
K m
∑s
2
VL ≤ −K vmin s + ε N ( s1 + s2 + + sm ) − i <0 (103)
φ i =1
K
Note that, since 0 < ϕ < 1 , therefore ≥ K > ε N . Both situations imply that VL < 0 , and
ϕ
consequently, the control system remains stable after replacing saturation function.
Remark 3.5. The sliding gain K̂ is chosen dynamically and its dynamic depends on sliding
surface. When the states go far from the sliding manifold, the absolute value of K̂ increases
to force them back to sliding manifold, and when the states are close to the sliding manifold,
the absolute value of K̂ decreases accordingly. This feature beside the replacing saturation
function, act as what is heuristically designed by fuzzy system in (Ataei & Shafiei, 2008).
Furthermore, the system stability is addressed here.
162 Advanced Strategies for Robot Manipulators
Simulation example 3.1. In order to show the effectiveness of the proposed control method,
it is applied to a two-link elbow robot driven by permanent magnet DC motors with the
following parameters:
For example, one can use 12V DC servo motors for actuating joints. In practice, also, a servo
control card is required which should include multi-channels of digital/analog (D/A) and
encoder interface circuits.
Two-link elbow robot Permanent-magnet DC motors
m1 = 3.55 kg m2 = 0.75 kg J m1 = 3.7 × 10 −5 kg.m2 J m 2 = 1.47 × 10 −4 kg.m2
l1 = 205 mm l2 = 210 mm Bm1 = 1.3 × 10 −5 N.m/s Bm 2 = 2 × 10 −5 N.m/s
lc1 = 154.8 mm lc 2 = 105 mm R1 = 2.8 Ω R2 = 4.8 Ω
K m1 = 0.21 Nm/A K m 2 = 0.23 Nm/A L1 = 3 mH L2 = 2.4 mH
g r1 = 1 60 g r 2 = 1 30 K b1 = 2.42 × 10 s/rad.V
−4
K b 2 = 2.18 × 10 −4 s/rad.V
Also, the friction term is considered here as (Wai & Chen, 2006):
qd = [sin t sin t ]
T
(108)
The design parameters are given in Table 4. The gain matrices λ1 and λ2 are selected such
that the roots of the characteristic polynomial e + λ1e + λ2 e = 0 lie strictly in the open left half
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 163
of the complex plane when the system is in sliding mode ( s = 0 ). The neural network
designed here has four neurons as hidden layer and two neurons as output layer, and its
weights are totally initialized at zero.
Remark 3.6. For a two-layer NN designed here with the input vector given by (77), we have
N1 = 6, N2 = 4 and N3 = 2, for a two-link manipulator. Accordingly, the numbers of adaptive
weights are 24 and 8 for input-to-hidden layer weights and output layer weights,
respectively. So, only 32 weight parameters must be adaptively updated here while using
the NN given in (Lewis et al., 1996), with N1 = 10, N2 = 10 and N3 = 2, this number increases
to 120. If the network size is chosen to large, the improvement of control performance is
limited and the computation burden for the CPU is significantly increased.
The gain matrix Kv which acts as the gain of the PID term is determined large enough to
improve transient response in the initial period of weight adaptations. On the other hand,
choosing Kv to a large extent increases the overall controller gain and may exceed the
permissible voltages of the actuators that are regarded in constraint 3.1. So, there is a trade
off between fast response and practical limitations.
⎡10 0 ⎤ ⎡24 0 ⎤ ⎡1 60 0 ⎤ ⎡5 0 ⎤
λ1 = ⎢ ⎥ λ2 = ⎢ ⎥ gr = ⎢ ⎥ Kv = ⎢ ⎥
⎣ 0 10⎦ ⎣ 0 24⎦ ⎣ 0 1 30⎦ ⎣0 5 ⎦
α =5 β =5 γ =2 ϕ = 0.05
Table 4. Design parameters
The mass variation of second link, the external disturbance and the friction are the major
factors that affect the control performance of the robotic system. In the reminder of this section,
two simulation cases are carried out to show the improvement due to the NNSM_PID control
method proposed in this section. In both cases, the simulation results of applying presented
method are compared with the related results of the fuzzy sliding mode_PID (FSM_PID)
control method proposed in (Ataei & Shafiei, 2008). In the first case, the disturbance (106) and
mass variation are injected and in the second case, the friction term is exerted too. The mass
variation condition is that 1 kg weight is added to the mass of 2nd link (i.e. m2 = 1.75 kg). For the
FSM_PID case, the control law is as following (Ataei & Shafiei, 2008):
U f = K vf s + fˆ f + K f sgn( s) (109)
K vf = N vf K fuzzy (110)
K f = N f K fuzzy (111)
where, Uf is the control input, Kfuzzy is of fuzzy system output and Nvf and Nf are the scaling
gain of the fuzzy system output. Here, it is assumed that only manipulator parameters could
be estimated and actuator parameters are still unknown. So, fˆ f is chosen as (Ataei &
Shafiei, 2008):
t
∫
fˆ f = Mˆ (qd + λ1e + λ2 e) + Cˆ (qd + λ1e + λ2 edt ) + Gˆ
0
(112)
⎡ 3.2 0 ⎤ ⎡0.8 0 ⎤
N vf = ⎢ ⎥ , N f = ⎢ 0 0.7 ⎥ (113)
⎣ 0 3.5 ⎦ ⎣ ⎦
Simulation 1─ In this case, the friction term is neglected, mass variation occurs at 3 sec and
external disturbance is injected at 6 sec. The desired trajectory is depicted in Fig. 22. The
vectors of tracking errors of FSM_PID and NNSM_PID are shown in Fig. 23 (a) and (b),
respectively. Both diagrams of Fig. 23 are plotted in the same scaled axes to achieve fairly
comparison. The FSM_PID controller does not meet the tracking purpose in the unknown
actuator parameters and mass variation conditions. On the contrary, the method proposed
in this section provides swift and precise tracking responses. Fig. 24 displays the control
efforts (i.e. input armature voltages of motors). The FSM_PID associated control commands
are jagged to some extent, while, the NNSM_PID case produces smooth control commands
with slowly variation and lower voltage amplitude. Lower voltage commands are more
protected toward actuator saturations. The NN outputs are shown in Fig. 25 and it indicates
that the designed neural network can approximate nonlinear terms with unknown
parameters, smoothly and boundedly.
Simulation 2─ With the purpose of showing robustness of our designed controller against
uncertainties and un-modeled dynamics, the friction term (107) is added here. The vectors of
tracking errors of FSM_PID and NNSM_PID are shown in Fig. 26 (a) and (b), respectively.
However, the response of the FSM_PID case is further undesirable in this condition, on the
other hand, the NNSM_PID control remains robust and its response is satisfactory, as well
as previous simulation case. Control efforts of this case are demonstrated in Fig. 27. Because
of exerting friction term, the input voltage commands are higher than previous case but the
NNSM_PID control commands are still smooth and vary slowly. The NN output is shown
in Fig. 28. Finally, as can be seen from Fig. 29, matrix norm of the adaptive weights, Ŵ and
Vˆ , have bounded value, less than 3, that it verifies what was claimed in the Theorem 3.1
about boundedness of these signals.
(a)
(b)
Fig. 23. (sim1) Tracking error of joints, (a) FSM_PID (b) NNSM_PID
166 Advanced Strategies for Robot Manipulators
(a)
(b)
(a)
(b)
Fig. 26. (sim2) Tracking error of joints (a) FSM_PID (b) NNSM_PID
Sliding Mode Control of Robot Manipulators via Intelligent Approaches 169
(a)
(b)
4. Conclusion
This chapter addressed sliding mode control (SMC) of n-link robot manipulators by using of
intelligent methods including fuzzy logic and neural network strategies. In this regard, three
control strategies were investigated. In the first case, design of a sliding mode control with a
PID loop for robot manipulator was presented in which the gain of both SMC and PID was
tuned on-line by using fuzzy approach. The proposed methodology in fact tries to use the
advantages of the SMC, PID and Fuzzy controllers simultaneously, i. e., the robustness
against the model uncertainty and external disturbances, quick response, and on-line
automatic gain tuning, respectively. Finally, the simulation results of applying the proposed
methodology to a two-link robot were provided and compared with corresponding results
of the conventional SMC which show the improvements of results in the case of using the
proposed method. In the second case, a new combination of sliding mode control and fuzzy
control is proposed which is called incorporating sliding mode and fuzzy controller. Three
practical aspects of robot manipulator control are considered there, such as restriction on
input torque magnitude due to saturation of actuators, friction and modeling uncertainty. In
spite of these features, the designed controller can improve the sliding mode and fuzzy
controller performance in the tracking error and faster transient points of view, respectively.
As previous case, the simulation results of applying the proposed methodology and other
two methodologies to a two-link direct drive robot arm were provided. Comparing these
results demonstrate the success of the proposed method.
Whenever, fast and high-precision position control is required for a system which has high
nonlinearity and unknown parameters, and also, suffers from uncertainties and
disturbances, such as robot manipulators, in that case, necessity of designing a developed
controller that is robust and has self-learning ability is appeared. For this purpose, an
efficient combination of sliding mode control, PID control and neural network control for
position tracking of robot manipulators driven by permanent magnet DC motors was
addressed in the third case. SMC is robust against uncertainties, but it is extremely
dependent on model and uses unnecessary high control gain; So, NN control approach is
employed to approximate major part of the model. A PID part was added to make the
response faster, and to assure the reaching of sliding surface during initial period of weight
adaptations. Moreover, four practical aspects of robot manipulator control such as actuator
dynamics, restriction on input armature voltage of actuators due to saturation of them,
friction and uncertainties were considered. In spite of these features, the controller was
designed based on Lyapunov stability theory and it could carry out the position control
with fast transient and high-precision response, successfully. Finally, two-step simulation
was performed and its results confirmed the success of presented approach. However, the
presented design was performed in the joint space of robot manipulator and kinematic
uncertainty was not considered. For the future work, one can expand this method to work
space design with uncertain kinematics.
5. References
Ataei, M. & Shafiei, S. E. (2008). Sliding Mode PID Controller Design for Robot Manipulators
by Using Fuzzy Tuning Approach, Proceedings of the 27th Chinese Control Conference,
July 16-18 2008, Kunming, Yunnan, China, pp. 170-174.
Cai, L. & Song, G. (1994). Joint Stick-Slip Friction Compensation of Robot Manipulators by
using Smooth Robust Controllers, Journal of Robotic Systems, Vol. 11, No. 6, pp. 451-
470.
172 Advanced Strategies for Robot Manipulators
Calcev, G. (1998). Some Remarks on the Stability of Mamdani Fuzzy Control Systems, IEEE
Transactions on Fuzzy Systems, Vol. 6, No. 4., pp. 436-442.
Capisani, L. M.; Ferrara, A. & Magnani, L. (2009). Design and experimental validation of a
second-order sliding-mode motion controller for robot manipulators, International
Journal of Control, vol. 82, no. 2, pp. 365-377.
Chang, Y. C.; Yen, H. M. & Wu, M. F. (2008). An intelligent robust tracking control for
electrically driven robot systems, International Journal of Systems Science, vol. 39, no.
5, pp. 497-511.
Chang, Y. C. & Yen, H. M. (2009). Robust tracking control for a class of uncertain electrically
driven robots, IET Control Theory and Applications, vol. 3, no. 5, pp. 519-532.
Craig, J. J. (1986). Introduction to Robotics, Addison& Wesley, Inc.
Eker, I. (2006). Sliding mode control with PID sliding surface and experimental application
to an electromechanical plant, ISA Transaction., vol. 45, no. 1, pp. 109-118.
Hung, J. Y.; Gao, W. & Hung, J. C. (1993). Variable structure control: A survey, IEEE
Transactions on Industrial Electronics, vol. 40, pp. 2-21.
Kaynak, O.; Erbatur, K. & Ertuģrul, M. (2001). The Fusion of Computationally Intelligent
Methodologies and Sliding-Mode Control: A Survey, IEEE Transactions on Industrial
Electronics, vol. 48, no. 1, pp. 4-17.
Khalil, K. H. (2001). Nonlinear Systems, Third edition, Prentice Hall Inc, New York, USA.
Lee, C. C. (1990). Fuzzy Logic in Control Systems: Fuzzy Logic Controller-Part I and II, IEEE
Transanction on System, Man and Cybernetics, Vol. 20, No. 2, 404-435.
Lewis, F. L.; Yesidirek, A. & Liu, K. (1996). Multilayer Neural-Net Robot Controller with
Guaranteed Tracking Performance, IEEE Transactions on Neural Networks, vol. 7, no.
2.
Lewis, F. L.; Jagannathan, S. & Yesildirek, A. (1998). Neural Network Control of Robot
Manipulators and Nonlinear Systems, Taylor & Francis.
Santibanez, V.; Kelly, R. & Liama, L.A. (2005). A Novel Global Asymptotic Stable Set-Point
Fuzzy Controller with Bounded Torques for Robot Manipulators, IEEE Transactions
on Fuzzy Systems, Vol. 13, No. 3, pp. 362-372.
Shafiei, S. E. & Sepasi, S. (2010). Incorporating Sliding Mode and Fuzzy Controller with
Bounded Torques for Set-Point Tracking of Robot Manipulators, Scheduled for
publishing in the Journal of Electronics and Electrical Engineering, T125 Automation,
Robotics, No. 10(106).
Shafiei, S. E. & Soltanpour, M. R. (2010). Neural Network Sliding-Model-PID Controller
Design for Electrically Driven Robot Manipulators, Scheduled for publishing in the
International journal of Innovative Computing, Information and Control, vol. 6, No. 12.
Slotin, J. J. E. & Li, W. (1991). Applied Nonlinear Control. Englewood Cliffs, NJ: Prentice-Hall,
New York, USA.
Spong, M. W. & Vidiasagar, M. (1989) Robot Dynamics and Control, Wiley, New York, USA.
Utkin, V. I. (1978). Sliding Modes and their Application in Variable Structure Systems, MIR
Publishers, Moscow.
Wai, R. J. & Chen, P. C. (2006). Robust Neural-Fuzzy-Network Control for Robot
Manipulator Including Actuator Dynamics, IEEE Transactions on Industrial
Electronics, vol. 53, no. 4, pp. 1328-1349.
Wang, L. X. (1997). A Course in Fuzzy Systems and Control, Prentice Hall, NJ, New York, USA.
Zhang, M.; Yu, Z.; Huan, H. & Zhou, Y. (2008). The Sliding Mode Variable Structure Control
Based on Composite Reaching Law of Active Magnetic Bearing, ICIC Express
Letters, vol.2, no.1, pp.59-63.
8
1. Introduction
Currently, the Stewart Platform is used in different engineering applications (machine tool
technology, underwater research, entertainment, medical applications surgery, and others)
due to its low mechatronic cost implementation as an alternative to conventional robots. The
current trend of using parallel manipulators has created the need for developing open
supervision and control architectures. This chapter presents the mathematical analysis,
simulation, supervision and control implementation of a six degree of freedom (DOF)
parallel manipulator known as the Stewart platform. The related studies are critically
examined to ascertain the research trends in the field. An analytical study of the kinematics,
dynamics and control of this manipulator covers the derivation of closed form expressions
for the inverse Jacobian matrix of the mechanism and its time derivative, the evaluation of a
numerical iterative scheme for forward kinematics on-line solving, the effects of various
configurations of the unpowered joints due to angular velocities and accelerations of the
links, and finally the Newton-Euler formulation for deriving the rigid body dynamic
equations.
The contents of this chapter are organized as follows:
• Section II presents the features of a Stewart Platform manipulator, describing its spatial
motion and applications.
• Section III covers the mathematical description, with the kinematics and dynamics
modelling, and the actuator control using a mechatronic prototyping approach.
• Section IV details the control structure, and compares two different control strategies:
the PID joint control structure and the Generalized Predictive Control (GPC). Both
controllers structured in the polynomial RST form, as a generic framework for
numerical control laws satisfying open architecture requirements.
• Section V describes the supervision and control architecture, particularly the spatial
tracking error is analyzed for both controllers.
174 Advanced Strategies for Robot Manipulators
• Section VI provides time domain simulation results and performance comparison for
several scenarios (linear and circular displacements, translational or rotational
movements), using reconfigurable computing applied to a Stewart-Gough platform.
• Section VII presents the supervisory control and hardware interface implemented in a
LabviewTM environment.
• Finally, section VII presents the conclusions and contributions.
3. Mathematical description
The mathematical model has to respond to a desired trajectory by actuating forces in order
to properly move the mobile plate to the targeted position and orientation. For obtaining the
Supervision and Control Strategies of a 6 DOF Parallel Manipulator Using a Mechatronic Approach 175
X i = f (Li ) (1)
⎡cϕ cθ −cϕ sθ sψ − sϕ cψ cϕ sθ cψ + sϕ sψ ⎤
⎢ ⎥
T (ψ ,θ , ϕ ) = rot( x ,ϕ )rot( y ,θ )rot( z ,ψ ) = ⎢ sϕ cθ −sϕ sθ sψ + cϕ cψ sϕ sθ cψ − cϕ sψ ⎥ (2)
⎢⎣ − sθ cθ sψ cθ cψ ⎥⎦
⎡ − nZ ⎤ ⎡ ny ⎤ ⎡ sφ a x − cφ a y ⎤
where, θ = ATAN 2 ⎢ ⎥ , φ = ATAN 2 ⎢ ⎥ , ψ = ATAN 2 ⎢ ⎥ and
⎢⎣ cφ n x + sφ n y⎥⎦ ⎣ nx ⎦ ⎢⎣ − sφ s x + cφ s y ⎥⎦
[
n= nx ny ] [
nz , s = s x sy ] [ ]
s z , a = a x a y a z : are the orthonormal vectors that describe
the platform's orientation.
This transformation matrix allows changing each actuator's position into a new
configuration in order to define the kinematics model as shown in Fig.2 (Kim, Chungt &
Youmt, 1997)(Li & Salcudean, 1997).
The points that define the upper base motion are located at the extremities of the six linear
actuators fixed at the lower base of the platform. When assuming that the actuators have
reached their final position and orientation, the problem is calculating the coordinates of the
center of mass on the superior base, and the RPY orientation angles (roll, pitch and yaw).
The relative positions can be calculated from the position and orientation analysis (using the
transformation matrix), leading to new ones within the platform’s workspace.
The position vector for the actuator of the upper/lower base, Pi , Ps , is determined in
relation to the fixed reference system at the center of mass of the inferior part as described in
(3). The parameters α , β , δ , ε , a , b , d , e are reported in Fig.2, where h represents the position
of the center of mass of the upper base in its initial configuration, and each line of Pi , Ps
represents the lower ( A1 " A6 ) and superior ( B1 " B6 ) coordinated extremities of
the actuators.
⎡ Ai + ε Di − δ 0⎤ ⎡ As + e Ds − d h⎤
⎢ ⎥ ⎢ ⎥
⎢ − Ai + ε Di − δ 0⎥ ⎢ − As + e Ds − d h⎥
⎢− A + ε + Ci Di − δ + C i 0⎥ ⎢ − As + e + C s Ds − d + C s h⎥
Pi = ⎢ i ⎥ P =⎢ ⎥ (3)
⎢ −Bi + ε Di + δ 0 ⎥ s ⎢ −Bs + e Ds + d h⎥
⎢ B +ε −Di + δ 0⎥ ⎢ B +e −Ds + d h⎥
⎢ i
⎥ ⎢ s
⎥
⎢⎣ Ai − ε − C i Di − δ + C i 0 ⎥⎦ ⎢⎣ As − e − C s Ds − d + C s h ⎥⎦
X i = T (ψ ,θ ,ϕ ) X iT (4)
From the known position of the upper base, the coordinates of its extremities are calculated
using the previous equations resulting in new ones, whose norm corresponds to the new
size of the actuator. If X0 is the reference point, then the difference between the current sizes
and the target ones is the distension that must be imposed to each actuator in order to reach
its new position as presented in (5)
ΔL = X iT − X 0 − X i − X 0 (5)
Thus, the distance between the extremities is calculated using the transformation matrix and
the known coordinates. The kinematic model of the platform receives the translation
information in vector form and the rotation from a matrix with the RPY angles.
This analysis allows calculating each axes lengths so that the platform moves to the target
position, so the required of each linear actuator k connected to the upper mobile base before
and after movement is described in Eqs. 6 and 7.
Supervision and Control Strategies of a 6 DOF Parallel Manipulator Using a Mechatronic Approach 177
3
L= ∑(P
j =1
s
kj
− Pikj )2 with k = 1," ,6 (6)
3
L + ΔL = ∑ (T
j =1
j
−1
(ψ ,θ ,φ )Pskj − Pikj )2 (7)
iπ − ap
Ai =[rp cos(αi ), rp sen(αi ),0]T =[Aix , Aiy , Aiz ]T αi = for i=1,3,5 αi =αi-1 + ap for i=2,4,6 (8)
2
And the links of the base by:
iπ − ab
Bi = [rb cos(βi ), rb sen(βi ),0]T = [Bix , Biy , Biz ]T βi = for i=1,3,5 βi = βi-1 + ab for i=2,4,6 (9)
2
Where rp: radius of platform; rb: radius of base; ap: angle of platform and ab: angle of base
l=f (x ) (10)
Where, l=(l1,l2,l3,l4,l5,l6) is the linear position of the joints, x=(X, Y, Z, ψ, θ, φ) is the position
vector of the platform, X,Y,Z the cartesian position and ψ, θ, φ represents the orientation of
178 Advanced Strategies for Robot Manipulators
the platform. The reference systems are fixed to A(u,v,w) and B(x,y,z) at the base, as shown
in Fig. 4.
The angular motions are expressed as Euler angle rotations in respect to x-axis, y-axis, and
z-axis, i.e. roll, pitch and yaw, in sequence.
⎡cψ cφ cψ sφ sθ − sψ cθ cψ sφ cθ + sψ sφ cθ ⎤
⎢ ⎥
B
RA = ⎢cψ cφ sψ sφ sθ + cψ cθ sψ sφ cθ − cψ sθ ⎥ (12)
⎢⎣ −sφ cφ cθ cφ cθ ⎥⎦
The vector-loop equation for the ith actuator of the manipulator is as follows:
li = A RB Ai + x − Bi (13)
By substituting the terms for each actuator, (14) describes the platform motion in relation to
its base.
( )
li 2 = X 2 + Y 2 + Z 2 + rp 2 + rb 2 + 2 r11 Aix + r12 Aiy ( X − Bix )
(14)
( )(
+ 2 r21 Aix + r22 Aiy Y − Biy ) + 2 (r
31 Aix + r23 Aiy )(Y − B ) + 2Z ( r
iy 31 ) (
Aix + r23 Aiy − 2 XBix + YBiy )
3.4 Dynamics study
The dynamic equations are derived for the Stewart Platform with a universal joint at the
base and a spherical joint at the top of each leg. For this study, it is assumed that there is no
rotation allowed on any leg about its own axis, so the kinematics and dynamics for each one
considers and calculates the constraining force over the spherical joint at its top.
Finally, the kinematics and dynamics of the platform are considered so the spherical joint
forces from all the six legs complete the dynamic equations.
The motion control can be implemented on every joint considering the movements of each
actuator (Guo & Li, 2006). Considering the coupling effects and to solve the trajectory
Supervision and Control Strategies of a 6 DOF Parallel Manipulator Using a Mechatronic Approach 179
problem, the dynamic control takes the inputs of the system so the drive of each joint moves
its links to the target position with the required speed.
The dynamic model of a 6-DOF platform can be calculated with the Euler-Lagrange
formulation that expresses the generalized torque (Jaramillo et al, 2006)(Liu, Li and Li,
2000).The dynamic model is described by a set of differential equations called dynamic
equations of motion as shown in (15).
where τ i ( t ) is the generalized torque vector, Li ( t ) the generalized frame vector (linear
joints), J i ( t ) the inertial matrix, Fi ( t ) the non-linear forces (for example centrifugal) matrix,
Γi the gravity force matrix.
d i( t ) d θ (t )
u(t ) = Lmot + Rmot i(t ) + KE m
dt dt
2 (16)
d θ m (t ) d θ (t )
Tm (t ) = J eq + Beq m = K i(t )
2 dt T
dt
where Tm ( t ) is the torque, θ m ( t ) the angular position of the motor axis, i ( t ) the current,
Lmot , Rmot respectively the inductance, resistance, J eq , Beq the inertia, friction of the axis load
calculated on the motor side.
4. Control structure
A simulation environment allows implementing and testing advanced axis control
strategies, such as Predictive Control, which is a well known structure for providing
improved tracking performance. The purpose of the control structure is to obtain a model of
the system that predicts the future system's behaviour, calculates the minimization of a
quadratic cost function over a finite future horizon using future predicted errors. It also
elaborates a sequence of future control values; only the first value is applied both on the
system and the model, finally the repetition of the whole procedure at the next sampling
period happens accordingly to the preceding horizon strategy (Li & Salcudean, 1997)
(Nadimi, Bak & Izadi, 2006)(Remillard & Boukas, 2007)(Su et al, 2004).
4.1 Model
The Controlled Autoregressive Integrated Moving Average Model (CARIMA) form is used
as numerical model for the system so the steady state error is cancelled due to a step input
or disturbance by introducing an integral term in the controller (Clarke, Mohtadi & Tuffs,
180 Advanced Strategies for Robot Manipulators
1987). The predictive control law uses an external input-output representation form, given
by the polynomial relation:
ξ (k)
A(q −1 )y( k ) = B(q −1 )u( k − 1) + (17)
Δ( q −1 )
where u is the control signal applied to the system, y the output of the system, Δ(q-1) =1 - q-1
the difference operator, A and B polynomials in the backward shift operator q-1, of
respective order na and nb, ξ an uncorrelated zero-mean white noise.
where Fj, Gj, Hj and Jj, unknown polynomials, corresponding to the expression of the past
and of the future, are derived solving Diophantine equations, with unique solutions
controller (Clarke, Mohtadi & Tuffs, 1987).
N2 Nu
∑ ( yˆ ( k + j ) − w( k + j )) + λ ∑ Δu( k + j − 1)2
2
J= (19)
j = N1 j =1
Assuming that Δu (t + j ) = 0 for j ≥ N u . Four tuning parameters are required: N1, the
minimum prediction horizon, N2 the maximum prediction horizon, Nu the control horizon
and λ the control-weighting factor.
ith:
⎡ ⎤
if(q −1 ) = ⎢F (q −1 ) " F (q −1 )⎥ ′
N
⎣ 1
N
2 ⎦ u = ⎡⎣ Δu(t ) " Δu(t + N u − 1)⎤⎦ ′
(21)
⎡ ⎤ yˆ = ⎡⎣ yˆ (t + N 1 ) " yˆ (t + N 2 ))⎤⎦
ih(q −1 ) = ⎢ H (q −1 ) " H (q −1 )⎥ ′
N N
⎣ 1 2 ⎦
Supervision and Control Strategies of a 6 DOF Parallel Manipulator Using a Mechatronic Approach 181
The future control sequence is then obtained by minimizing the criterion (23) (Clarke,
Mohtadi and Tuffs, 1987):
−1
⎡ ⎤
M = Q G' , N u × ( N 2 − N 1 + 1) , Q = ⎢ G'G + λ I , w = ⎡⎣ w(t + N 1 ) " w(t + N 2 )⎤⎦ (24)
⎣ Nu ⎥⎦
1
T (q) DAC Process ADC
w + ΔS (q −1 )
u
- y
Polynomial RST
controller
R(q −1 )
(a) Continuous PID joint control (b) Discrete PID in RST form
( ) ( )
degree R(q −1 ) = degree R(q −1 )
degree (T (q )) = N
2
The GPC has shown to be an effective strategy in many fields of applications, with good
time-domain and frequency properties (small overshoot, improved tracking accuracy and
disturbance rejection ability, good stability and robustness margins), is able to cope with
important parameters variations.
5. Simulation
The modelling of the Parallel Manipulator leads to the design of a simulator adopting
electric and mechanical libraries blocks using Simulink (Gosselin, Lavoie & Toutant, 1992).
The main elements of the robotics joints are brushless DC motor drives, axis inertia, gears
and control blocks. Other elements of the manipulator (including loads) are represented by
three nonlinear models, one for each motor drive. The control system itself consists,
essentially, in a cascade of control loops (for each axis). The inner speed and torque control
loops are part of the drive model where only the position loop is explicitly modelled. In fact,
Supervision and Control Strategies of a 6 DOF Parallel Manipulator Using a Mechatronic Approach 183
the position control of the manipulator can be implemented through the control feedback of
each isolated joint (Cappel, 1967).
The developed simulator also includes a path generation module, providing the joints with
axis trajectories as reference signal for controlling each of the parts (Jaramillo et al, 2006).
Finally, a graphic interface is developed, showing the results of joint motion obtained
through typical trajectories. The simulation software was implemented using Matlab ® and
programmed with the equations of the Stewart Platform manipulator. This interface allows
the input of the dynamic simulation parameters: mass and inertia of the mobile platform,
actuator parameters and the gains of the PID controller. Fig. 8 shows a screen capture of the
developed interface.
Joint N1 N2 Nu λ
1 1 8 1 92
2 1 8 1 107.3
3 1 8 1 126
Table 2. GPC tuning parameters for each joint.
Fig. 10. Top and bottom implemented base geometries and parameters
The constant workspace volume of the manipulator is also evaluated (Fig. 15). This useful
characteristic helps to plan new workspace trajectories with constant orientation.
To = [ 0.1 0 0.395 ] m
The initial position of the 3D platform is presented in Fig. 16a, where a 50N constant force is
applied on each arm for 0.5 s resulting in the position of the platform shown in Fig. 16b.
During this movement the change of position, linear and angular velocities of the center of
gravity is calculated and presented in Fig.17.
0.5 0.8
0.4 0.6
0.3
0.4
0.2
0.2
0.1
0
0
-0.1 -0.2
0.5 0.5
0.6
0.4
0 0 0.2
0.6
0.4 0
0.2
0 -0.2
-0.5 -0.2 -0.5
-0.4 -0.4
-0.5 -2
0 0.2 0.4 0.6 0.8 0 0.2 0.4 0.6 0.8
-2 -5
-4 -10
0 0.2 0.4 0.6 0.8 0 0.2 0.4 0.6 0.8
Fig. 17. Linear and angular displacement of of the center of gravity of the Stewart Platform.
The maximum joint space error is 0.1 mm in all the actuators resulting in a maximum
motion of 30 mm or 0.333%.
strategies associated with the sensors and actuators data. The strategies in this level can be
implemented under a rapid prototyping framework like FPGA, as described in Fig. 23.
P[n] = Kp ⋅ e[ n] , (46)
192 Advanced Strategies for Robot Manipulators
Kp ⋅ Ts
I[n] = I[n − 1] + ( e[n] − e[n − 1]) (47)
2 * Ti
D[n] =
( pTs − 2) ⋅ D[n − 1] + 2 ⋅ Kp ⋅ Td ⋅ (e[n] − e[n − 1]) (48)
( pTs + 2) Ts ⋅ ( pTs + 2)
where: U[n] is the current control signal resultant, P[n] the current proportional control
signal, I[n] the current integral control signal, D[n] is composed of the proportional,
derivative and integral parameters (Kp, Td and Ti) where Ts is the sampling time,
respectively. Also, e[n] the current error sample, and finally, e[n-1] the previous error
sample. A register error block stores values of e[n] and e[n-1], and makes shift operations
(e[n-1] = e[n] and u[n-1] = u[n]). An output register block stores u[n] and u[n-1].
PW
MG
EN
ER
AT
O R
Po
Inte wer
r fa
ce
PW
(16 M
bits
)
PID
CO
NT
R OLL
E R (3
16 2b
bits Pa its )
Co ram e
nfig ter
Reg uratio
iste n
r
16
bit s Dife
r
Equ enc e
Co a tio
Re ntrol n
gist
ers
E
De t rror
e
(16 cting
EN bits
CO )
DE
R I NT
ER
FA C
E
Ou
t
Velo put
Sa m city
ple
No
Velo
c Win is e
Me ity d ow
te r
Fre
q ue
Per ncy /
i
Sw od
itch
PO
W
CIR ER
CU
IT
ENC
D OD
MO C ER
TO
R
gain parameters. PWM and Power Interface converts the binary word supplied by PID
controller in a pattern of digital signs to control the PWM potency block.
The considered reconfiguration in the interface and logical block design eases testing,
implementation and future updating, due to this, the development of systems based on
reconfigurable computing present well-suited features for developing this kind of problem.
The synchronized control of the actuator system can be easily achieved through the same
PLD.
the trajectory (joint coordinate). For this purpose the kinematics model of the platform was
used with six linear joints. Fig. 26a shows the joints movements of each linear actuator and
their displacement (45 degrees, approximately) of one point of the upper base of this
platform obtained through the inverse kinematics model (Fig. 26b). Fig. 26c shows results of
the proposed simulation, obtained with PID axis controllers implemented through FPGA,
considering general sea movements and LABVIEWTM experimental platform.
a) b)
c)
7. Conclusions
This chapter presents the study of kinematics, dynamics and supervision and control of a
Stewart-Gough platform, under a reconfigurable architecture concept, considering the
division of the system in small functional blocks. This implementation consisted in merging
knowledge acquired in multiple areas, and appears as a very promising design strategy for
a better reconfiguration capability and portability.
Supervision and Control Strategies of a 6 DOF Parallel Manipulator Using a Mechatronic Approach 195
This platform also becomes a powerful benchmark for many research activities, such as the
validation of controllers and supervision strategies, model generation and data transmission
protocols, among others. For example, the implementation of predictive controllers on this
prototype may enable the test of this advanced control strategy under severe conditions of
use.
To simplify tests, implementation and future modifications, the use of rapid prototyping
functions in the implementation of the interfaces and other logical blocks is emphasized in
the proposed prototype. The control block, for example, can benefit of the characteristics of
low consumption, high-speed operations, integration capacity, flexibility and simple
programming. Some promising aspects of this architecture are:
• Flexibility, as there is a large variety of possible configurations in the implementation of
solutions for several problems,
• It is a powerful tool for prototype design, allowing simple solution to control the
several sensors and actuators usually present in this kind of projects,
• Possibility of modification of control strategies during operation of the platform,
• The open architecture of this platform enables the use for educational and researches
activities.
8. References
Altera Corporation. http://www.altera.com, 2008.
Bessala, J.; Philippe, B. & Ben Ouezdou, F. (1996). Analytical Study of Stewart Platforms
Workspaces. Proceedings of the 1996 IEEE International Conference on Robotics and
Automation. pp. 3179-3184.
Cappel, K. (1967). Motion simulator. Patent No. 3,295,224 , US Patent No. 3,295,224.
Clarke D. W., Mohtadi C., Tuffs P.S. (1987) Generalized Predictive Control. Part I. The Basic
Algorithm. Part II. Extensions and Interpretation, Automatica. Vol.23(2). pp. 137-160.
Dasgupta, B. & Mruthyunjaya, T. (1998). Newton-Euler formulation for the inverse
dynamics of the Stewart platform manipulator. Mechanism and Machine Theory Vol.
33(8). pp. 135-1152.
Fasse, E. & Gosselin, C. (1998). On the spatial impedance control of Gough-Stewart
platforms. IEEE International Conference on Robotics and Automation. pp. 1749-1754.
Ghobakhloo, A.; Eghtesad, M. & Azadi, M. (2006). Position Control of a Stewart-Gough
Platform using Inverse dynamics Method with full dynamics. International
Workshop on Advanced Motion Control, AMC. Vol. 1. pp. 50-55.
Gosselin C.; Lavoie, E. & Toutant, P. (1992). An efficient algorithm for the graphical
representation of the threedimensional workspace of parallel manipulators.
Proceeding of the 22nd ASME Mechanisms Conference. pp. 323-328.
Gosselin, C. M.; Perreault, L. & Vaillancourt, C. (1999). Simulation and Computer-Aided
Kinematic Desing of Tree-Degree-of-Freedom Spherical Parallel Manipulators.
Jornal of Robotic Systems. Vol.12(12). pp. 857-869.
Gough, V.E. e Whitehall, S. (1962). Universal tyre test machine. Proceedings of the FISITA
Ninth International Technical Congress. pp. 117-137.
Guo, H. B. & Li, H. R. (2006). Dynamic analysis and simulation of a six degree of freedom
Stewart platform manipulator. Proceedings of the Institution of Mechanical Engineers.
Part C, Journal of Mechanical Engineering Science. Vol. 220(1). pp. 61-72.
196 Advanced Strategies for Robot Manipulators
1. Introduction
Flexible manipulators that are lightweight and mechanically flexible are useful for operations
in various fields, e.g. space development programs, robotic assistants to humans and so
forth. However, the derivation of their exact mathematical model and synthesis of the
accurate positioning controller is exceptionally difficult because of the mechanical flexibility.
On the other hand, the mechanical flexibility is conducive to the safety in collision between
the manipulator and the obstacles. However, it is not positive safety measures that
reduction of the influence due to the impact force of collision for the flexible manipulator
depends only on its mechanical flexibility. In order to develop flexible manipulators so that
they work safely with persons cooperatively, we need to introduce the active collision
detection and suspension control algorithms to the flexible manipulators.
The functional requirements of safety in the operations of flexible manipulators are as
follows: i) to avoid collisions with obstacles placed or moving in the work space; ii) to detect
collisions when unlooked-for obstacles contact with the flexible arm of the manipulator and
to suspend the motion as immediately as possible; iii) to plan a new path so as to avoid the
place of obstacles.
There are several researches on collision detection methods without extra sensors (A. Garcia
& Somolinos, 2003), (M. Kaneko & Tsuji, 1998), (T. Matsumoto & Kosuge, 2000). Moorehead
and Wang proposed (Moorehead & Wang, 1996) a collision detection method using strain
gauges to determine the intensity and position of external force due to collision with a
flexible cantilevered beam. The estimation of the contact position in their approach was
achieved by the mechanical relation between positions of the two strain gauges and the
bending moments measured by the sensors. Payo et al. (I. Payo & Cortazar, 2009) is
produced the method of collision detection and suspend control of the very lightweight
single-link flexible arm based on coupling torque feedback. They used the variation of the
control torque.
The authors have focused our attention on the second item mentioned above. We already
developed a method of collision detection for the single-link flexible manipulator using the
innovation process of the Kalman filter (Sawada, 2002a), (Sawada, 2002b), (Sawada, 2002c),
(Sawada, 2004 (in Japanese)), (Kondo & Sawada, 2008). Our approach requires no particular
198 Advanced Strategies for Robot Manipulators
sensors for measuring the contact events between the flexible arm and the obstacles. This
collision detection method is based on the observation data for vibration control of the
flexible manipulator. The mathematical model of the flexible manipulators is expressed by
nonlinear partial differential equations and ordinary differential equations, which is
regarded as the infinite-dimensional system. The Kalman filter is constructed for the
linearized finite-dimensional model corresponding to the mathematical model of the
manipulator.
This chapter describes a method of collision detection and suspend control for parallel-
structured flexible manipulators subject to random disturbance using unscented Kalman
filter (UKF), which is one of the nonlinear filters. The features of the parallel-structured
flexible manipulator are that it holds sufficient rigidity along the vertical axis and mechanical
flexibility along the displacement axis of the arm (Sawada & Watanabe, 2007). The exact
mathematical model of the parallel-structured flexible manipulator is described by quite
complex nonlinear partial and ordinary differential equations, because the manipulator
consists of two flexible beams which are disposed parallel. In this chapter, the parallel-
structured flexible manipulators are approximately modeled by a flexible arm consisting of
a flexible beam with the same boundary conditions as the parallel-structured one.
The approximated model of the flexible manipulator is also a nonlinear system. In order to
construct the state estimate for the flexible manipulator, we employed the unscented
Kalman filter as the nonlinear state estimator (S. Julier & Durrant-Whyte, 2000), (S.J. Julier &
Durrant-Whyte, 1997), (Y.-S. Chen & Wakui, 1989). The UKFs are based on the Monte Carlo
method, which have been developed by Simon Julier (S. Julier & Durrant-Whyte, 2000), (S.J.
Julier & Durrant-Whyte, 1997) in order to improve the accuracy of the extended Kalman
filters. The UKF generates a population of so-called sigma-points on the basis of the current
mean and covariance of the state vector. The mean and covariance of the state are calculated
using these sigma-points, which means that the algorithm is not necessary to evaluate the
Jacobians.
Collision between the parallel-structured flexible manipulator and an undesirable obstacle
can be detected using the innovation process of the UKF based on the measured data of
strain sensors pasted on the side of the manipulator. The UKF is constructed for the
nonlinear state space model corresponding to the parallel-structured flexible manipulator
without the impact force term due to the collision. The collision detection function is defined
by the strength of the innovation process. The detection algorithm decides that the collision
occurs if the collision detection function exceeds a preassigned threshold.
The controller for the manipulator has the following two objectives: i) to rotate the flexible
arm from the initial position to the desired position; ii) to safely suspend the rotation of the
arm when the collision is detected.
1 1
m p (t ) + ( J 0 + J 1 )θ 2 (t ),
2
TR (t ) = (3)
2 2
where &·& denotes the Euclid norm. Similarly, the kinetic energy of the flexible beam is
given by
A
TF (t ) = ∫ Tˆ (t , x )dx , (4)
0
1
Tˆ (t , x ) := ρ S r(t , x ) .
2
(5)
2
The total kinetic energy of the arm is expressed by the sum of the kinetic energies of the
rigid and flexible parts, i.e.,
T (t ) = TR (t ) + TF (t ). (6)
where Vˆ (t , x ) is the density function of the potential energy of the flexible part given by
1 ⎧ ∂ 2 u(t , x ) ⎫
Vˆ (t , x ) = EI ⎨ ⎬. (8)
2 ⎩ ∂x 2 ⎭
Collision Detection and Control of Parallel-Structured Flexible Manipulators
Based on Unscented Kalman Filter 201
where t1 and t2 are arbitrary times; s(t) represents the Lagrange multiplier which is
equivalent to the external force generated by the collision with the obstacle; and δWnc(t)
denotes the virtual work due to the nonconservative forces, e.g. internal damping forces of
the beams, the control torque and external disturbances. ψ(t) describes the geometric
constrained condition between the unlooked-for obstacle and the flexible beam, i.e.
Let us assume that |ϕ0 – θ(t)| is sufficiently small. We can regard tan{ϕ0 – θ(t)} ≅ ϕ0 – θ(t).
Then, (10) can be rewritten into
A
ψ(t ) = ∫ u(t , x )δ ( x − xc )dx − xc {ϕ0 − θ (t )} ≡ 0. (11)
0
δWnc(t) is expressed by
A ⎛ ∂ 3u(t , x ) ⎞ A
− ∫ cD ⎜ ⎟ δ u′′dx + ∫0 g f γ (t , x )δ udxi , (12)
⎝ ∂x ∂t ⎠
0 2
where the prime denotes the differentiation with respect to x; μ denotes the damping
coefficient corresponding to the damping force acting at the shaft of the servomotor; τ (t) the
control torque; γθ(t) the random disturbance acting at the rotation of the arm; γ(t, x) the
distributed random disturbance along the beam due to the aerodynamic resistance; and
gθ and gf are constants.
As the generalized coordinates, we consider the following variables: θ(t), θ(t ) , u(t, x),
u (t , x ) , u’(t, x), u’(t, x), u’’(t, x), u ′′(t , x ), u(t, ℓ), u (t , A ) . Substituting (7), (8) and (11) into (10);
in addition, performing a large amount of calculations, we have the following nonlinear
differential equations as the mathematical model of the approximated dynamical model
corresponding to the parallel-structured single-link flexible manipulator:
⎧ ∂ 2 u(t , x ) ⎫
+ s(t )δ ( x − xc ) − ⎨m(A + h )e(t ) + m − me 2 (t )u(t , x )⎬ δ ( x − A ), (13)
⎩ ∂t 2
⎭
where γ(t, x) the distributed random disturbance modeled by the white Gaussian noise; g1
and h are constants; δ(·) denotes the Dirac’s delta function; and s(t) the magnitude of
collision input. Assuming that the collision occurs momentarily, the magnitude of collision
is assumed to be expressed by s(t) := s0δ(t – tc), where s0 and tc are all unknown. The initial
and boundary conditions of (13) are
202 Advanced Strategies for Robot Manipulators
∂u(0, x )
I .C . : u(0, x ) = =0 (14)
∂t
A
+ ∫ g xγ (t , x )dx − τ (t ) − gθ γ θ (t ) = 0.
0 1 (16)
The observation data is obtained by means of P strain sensors pasted at x = ξj, (j = 1, … ,P)
and a potentiometer installed at the shaft of the hub, i.e.
y 0 (t ) = c0 e(t ) + e0 β 0 (t ) (17)
ξ j + bs ∂ 2 u(t , x )
y j (t ) = c j ∫ dx + e j β j (t ), (18)
ξj ∂x 2
where cj and ej are constants; and βj(t), (j =0, 1, … ,P) represents the observation noise which
is modeled by the white Gaussian noise. In order to use the finite-dimensional controller
and state estimator, the dynamics of the arm described by (13) and (16) are converted into
the stochastic finite-dimensional state space model via the modal expansion technique,
N
u(t , x ) = ∑uk (t )φk ( x ), (19)
k =1
where {uk(t)}k=1,… ,N denote the modal displacements; N the large positive number; φk(x) the
eigenfunction (mode function) of the following eigenvalue problem with respect to the
operator A = {(EI)/(ρS)}(d4/dx4):
Introducing the state vector defined by v(t) = [u1(t),… ,uN(t), u 1 (t), … , u N (t), e(t), e (t)]T, the
state space model of the approximated flexible arm can be described by the following
stochastic differential equation:
where γ (t) = [γ1(t), … ,γ N(t),γθ(t)]T; γ k(t) = ∫ A0 γ (t, x)φk(x)dx; β(t) := [β0(t), β1(t), … , βP(t)]T; E{γ (t)
γ T(τ)g} = Wδ(t – τ); E{β(t)βT(τ)} = Vδ(t – τ) (E{·}: mathematical expectation).
v f (t ) = A( v f )v f (t ) + B( v f ) f (t ) + G( v f )γ (t ), (23)
v f ( k + 1) = F( v f ( k ),τ ( k ), γ ( k )) (24)
y( k ) = Cv( k ) + Eβ ( k ), (25)
where k denotes the time-step; Δt the time interval; and F(·) the nonlinear function defined by
Δt
F( v f ( k ),τ ( k ), γ ( k )) = v f ( k ) + { H 1 ( v f ( k ),τ ( k ), γ ( k )) + 2 H 2 ( v f ( k ),τ ( k ), γ ( k ))
6
X0 = vˆ f ( k|k ) (27)
κ
W0 = (28)
2N + 2 + κ
204 Advanced Strategies for Robot Manipulators
1
Wi = (30)
2(2N + 2 + κ )
Xi + 2 N + 2 = vˆ f ( k| k ) − { (2 N + 2 + κ )P( k| k )} i (31)
1
Wi + 2 N + 2 = , ( i = 1," , 2 N + 2) (32)
2(2N + 2 + κ )
where κ denotes the integer scaling parameter; Wi the weight coefficient that is
associated with the i-th point and { (2 N + 2 + κ )P( k| k )} i represents the i-th column
of the matrix U satisfying M = UUT if M := (2N + 2 + κ)P(k|k). In this paper, the
matrix U is calculated via the incomplete Cholesky decomposition (Saad, 1996).
Step 2. Transform each point through the nonlinear function F(·), and the predicted mean,
covariance and observation, the innovation covariance Pyy(k + 1|k) and the cross
correlation matrix Pxy(k + 1|k) are computed as follows:
2(2 N + 2)
vˆ f ( k + 1|k ) = ∑
i =0
WiX i ( k + 1| k ) (34)
2(2 N + 2)
P( k + 1| k ) = ∑
i =0
Wi {Xi ( k + 1|k ) − vˆ f ( k + 1|k )}
Yi ( k + 1| k ) = CXi ( k + 1| k ) (36)
2(2 N + 2)
yˆ i ( k + 1|k ) = ∑
i =0
Wi Yi ( k + 1|k ) (37)
2(2 N + 2)
Pyy ( k + 1|k ) = ∑
i =0
Wi {Yi ( k + 1|k ) − yˆ ( k + 1|k )}
Step 3. The state estimate and covariance are given through updating the prediction by the
linear update rule which is specified by the weights chosen to minimize the mean
squared error of the estimate. The update rule is
Collision Detection and Control of Parallel-Structured Flexible Manipulators
Based on Unscented Kalman Filter 205
where vˆ f ( k|k ) is the estimate of vf (k) which is calculated by the UKF mentioned in the
previous Section. Substituting (25) into (43), we have
μ ( k ) = Cz( k ) + Eβ ( k ), (44)
where z(k) is the estimation error defined by z(k) := v(k) – vˆ f ( k|k ) . If the collision does not
occurr, the state vector v(k) is equal to vf (k). However, if the collision occurrs, v(k) is equal to
the state vector of collision model. In this case, z(k) becomes large because of the collision
input. In order to detect the collision, the following scalar function (collision detection
function) is introduced:
r ( k ) = μ T ( k )μ ( k ). (45)
If the collision detection function r(k) exceeds a threshold ε, then the collision has occurred.
In fact, it is assumed that the estimation error when the collision has occurred is separated as
z( k ) = z( k ) + α ( k ), (46)
where z( k ) is the estimation error of the UKF based on the collision-free system; and α(k)
the estimation error caused by the collision input. Substituting (46) into (44), the innovation
process μ(k) is rewritten into
μ ( k ) = Cz( k ) + Cα ( k ) + Eβ ( k ). (47)
206 Advanced Strategies for Robot Manipulators
From equations (45) and (47), the mathematical expectation of the collision detection
function r(k) is evaluated by
where Pz ( k ) := E { z T ( k )z( k )} . The first two terms in the right-hand side of (48) is the bias
depending on the observation and the system noises. The third term is caused by the
collision input which is the deterministic process. If the third term becomes sufficiently
large, then r(k) becomes also large at the time when the collision occurs.
which has no system noise term. If the flexible manipulator is well controlled, the error state
vector vf (t) is assumed to sufficiently be small, i.e. &vf (t)& 1. In this work, we consider that
the matrices A(vf (t)) and B(vf (t)) can be approximated as A(vf (t)) ≅ A(0) and B(vf (t)) ≅ B(0).
Using these approximations, the error system is rewritten into the following equation:
v f (t ) = Ae v f (t ) + Be τ (t ), (50)
The objectives of the sliding mode controller are to control the tip position, to suppress the
random vibration of the whole manipulator, and to suspend the motion of the manipulator
when a collision is detected. The control torque τ(t) is generated by the sliding mode
controller. The output of the controller can be separated into two parts, i.e.:
τ (t ) = f eq (t ) + f nl (t ), (52)
where feq(t) represents the term of linear control input called the equivalent control input in
the sliding mode; and fnl(t) the term of nonlinear control input in the reaching mode.
The switching function σ(t) is given by
σ (t ) = Sv f (t ), (53)
where S represents the gradient of the switching plane. In the sliding mode, the switching
function σ(t) holds the following conditions:
σ (t ) = 0 (54)
Collision Detection and Control of Parallel-Structured Flexible Manipulators
Based on Unscented Kalman Filter 207
σ (t ) = 0. (55)
The equivalent control input is obtained using (50), (53) and (55) as:
Substituting (56) into (50), the equivalent control system can be described by
v (t ),
v f (t ) = { Ae − Be (SBe )−1 SAe } v f (t ) ≡ A (57)
e f
:= A − B (SB )−1 SA .
where Ae e e e e
In order to find the switching plane, we consider the cost functional defined by
t A
J = ∫ { ∫ [q1 {u (s , x )} 2 + q2 {u(s , x )} 2
0 0
where qi (i = 1, … 6) are positive constants. Substituting the solution of the system described
by the equation (13) given by (19) into this, it is rewritten into the following expression:
T
J = ∫ v Tf (t )Qv f (t )dt , (59)
0
The gradient of the switching plane S must be decided so that the eigenvalues of A
e
becomes stable. There are a method to choose S as a feedback gain of the optimal control.
Namely, S is determined as follows (Y.-S. Chen & Wakui (1989)):
S = BeT P (61)
The nonlinear control input in reaching mode is considered. The sliding mode control is
regarded as variable structure control as a required condition. So, using the switching
function σ(t), the term of nonlinear control input fnl(t) is defined by
where F is the nonlinear controller gain; and sgn(·) the signum function. Therefore, the
control input f (t) is given by
208 Advanced Strategies for Robot Manipulators
In order to achieve σ(t)→0 (t→∞), the Lyapunov function for the switching function is
chosen as
1
V (t ) = σ T (t )σ (t ). (65)
2
The time derivative of the the Lyapunov function defined by (65) is described with (50) and
(64) by
If V (t ) < 0, the switching function converges to zero. Hence, the nonlinear control gain F
must satisfy the following condition:
σ (t ) σ (t )
sgn(σ (t )) = ≈ , (68)
& σ (t ) & & σ (t ) & +δ
σ (t )
τ (t ) = −(SBe )−1 SAe v f (t ) − F . (69)
& σ (t ) & +δ
Because of using the unscented Kalman filter, it is necessary that the switching function σ(t)
and the controller input τ(t) given by the sliding mode controller with the UKF are
converted into the discretized version given by
σ ( k ) = Svˆ f ( k ) (70)
σ (k)
τ ( k ) = −(SBe )−1 SAe vˆ f ( k ) − F . (71)
& σ ( k ) & +δ
When the collision occurs, it is necessary that the flexible manipulator is suspended because
of absorbing the impact of collision. The proposed flexible manipulator is controlled by
tracking the reference trajectory using the sliding mode controller. For suspending the
motion of the manipulator, we consider that the reference trajectory (position control) is
changed into a steady position when the collision occurs. The angle position of flexible
manipulator at the time tc when the collision occurs is given by
θ ( tc ) = θ c , (72)
Collision Detection and Control of Parallel-Structured Flexible Manipulators
Based on Unscented Kalman Filter 209
After the collision occurred, the reference trajectory is changed into the trajectory given by
the following equation:
θ d (t ) ≡ θ c . (73)
The desired position becomes the position that the flexible manipulator collides with the
obstacle. Then, the flexible manipulator can be suspended at this position.
6. Simulation studies
In this section, several numerical results are presented. The flexible beam is assumed to be
made with the phosphor bronze. The physical parameters and the coefficients of the flexible
manipulator are listed in Table 1. The observation data is supposed to be measured by
piezoelectric sensors with their length bs = 3×10–2[m] and width 1.2×10–2 [m] pasted at
ξ1 = 3×10–3[m] and potentiometers installed at each hub. The parameters of the observation
system were set as c0 = 10, c1 = 1, e0 = e1 = 1. The parameter for the UKF κ was set as κ = 1. The
covariance matrices for the system and observation noises were given by W = 10–5 × I2N+2,
V = 10–8 × I2. The number of modes of the system was set as N = 2. The time partition in
numerical simulation was set as Δt = 1 × 10–3[s].
The initial condition of state vector were set as u(0, x)=0[m], u (0, x)=0[m/s θ ],
˙θ(0)=0[rad/s] and θ(0) = 0[rad]. The initial condition of the state vector of the control error
system was also set as zero. The weight coefficients of cost functional for the hyperplane S
were set as the values; q1 = 100, q2 = 100, q3 = 100, q4 = 5, q5 = 4500, q6 = 200. The nonlinear
controller gain was F = 4, δ = 10 and the simulation study was carried out for 5 seconds.
Parameters Values
ℓ1 0.3[m]
E 1.1×105[MPa]
S 2×10–5 [m2]
ρ 8.8×103[kg/m2]
cD 4.84×107[N·s/m2]
J0 5 [kg·m2]
J1 0.08[kg·m2]
m 0.61[kg]
h 0.026[m]
g1 0.05
g2 0.4
Table 1. Physical parameters of the flexible manipulator.
data of the strain measured by the piezoelectric sensor y1(t) and the displacement of the tip
mass u(t,ℓ), respectively. Furthermore, Fig.5 also depicts the estimate of the tip-mass
displacement uˆ (t , A ) which is calculated by uˆ (t , A ) = ∑ uˆ i (t )φi (A ) . The estimation error of
N
i =1
the tip-displacement based on the noisy observation data (see Figure 4) is adequately small for
suppressing the vibration of the tip-mass. Fig.6 shows the response of the control torque τ(t).
Angle
1.6
True value
1.4
Estimate
1.2
1
θ( t) [rad/s]
0.8
0.6
0.4
0.2
-0.2
0 1 2 3 4 5
Time t[s]
Fig. 3. Behavior of the rotational angle θ(t) and its estimate θˆ(t ) obtained using the UKF in
the collision-free case. The solid line and the dashed line depict the true state of the angle
θ(t) and its estimate θˆ(t ) , respectively
-4 Observation (Strain)
x 10
4
1
y1 ( t )
-1
-2
0 1 2 3 4 5
Time t[s]
Fig. 4. Observation data of the strain measured by the piezoelectric sensor, y1(t) in the
collision-free case.
Collision Detection and Control of Parallel-Structured Flexible Manipulators
Based on Unscented Kalman Filter 211
True value
1
Estimate
u( t, ℓ) , û( t, ℓ) [m]
0
-1
-2
-3
-4
-5
0 1 2 3 4 5
Time t[s]
Fig. 5. Displacement of the tip mass u(t, ℓ) and its estimate û (t, ℓ).
Control Input
1.2
0.8
τ ( t) [N·m]
0.6
0.4
0.2
-0.2
0 1 2 3 4 5
Time t[s]
Fig. 6. Control torque τ(t) generated by the sliding mode controller in the collision-free case.
-11
x 10 Collision Detection Function
1
0.9
0.8
0.7
0.6
0.5
r ( t)
0.4
0.3
0.2
0.1
0
0 1 2 3 4 5
Time t[s]
Fig. 7. Behavior of the collision detection function r(t) generated by the innovation process of
the UKF in the collision case. The collision occurs at tc = 1.24[s].
-4 Observation (Strain)
x 10
5
0
y1 ( t )
-1
-2
-3
-4
-5
0 1 2 3 4 5
Time t[s]
Fig. 8. Observation data of the strain measured by the piezoelectric sensor, y1(t) in the
collision case.
Collision Detection and Control of Parallel-Structured Flexible Manipulators
Based on Unscented Kalman Filter 213
Angle
1.4
1.2
0.8
0.6
0.4
0.2
- 0.2
0 1 2 3 4 5
Fig. 9. Behavior of the rotational angle θ(t) obtained using the UKF in the collision-free and
the collision cases. The solid line and the dashed line depict the angle θ(t) in the collision
and collision-free cases, respectively.
6
u( t, ℓ) , û( t, ℓ) [m]
-2
-4
-6
0 1 2 3 4 5
Time t[s]
Fig. 10. Displacement of the tip mass u(t, ℓ) and its estimate uˆ (t , A ).
214 Advanced Strategies for Robot Manipulators
Control Input
1
0.5
0
τ ( t) [N·m]
-0.5
-1
-1.5
-2
0 1 2 3 4 5
Time t[s]
Fig. 11. Control torque τ(t) generated by the sliding mode controller in the collision case.
observation data of the strain is very noisy. From Figs. 7 and 8, we can see that the collision
detection function can detect the week collision based on the noisy observation data.
The results of the suspend control are shown in Figs. 9 and 10. The rotation of the
manipulator was interrupted when the collision has been detected using the collision
detection function (see Figure 9). After r(t) exceeded the preassigned threshold ε = 1 × 10–12,
the desired position has been changed from θd to θ(tc). The displacement of the tip-mass
u(t, ℓ) shows the vibration with large amplitude after the collision was detected (Fig. 10). The
control torque is depicted in Fig. 11. This figure explains that the control torque when the
motion of the manipulator was suspended requires the torque of large value.
7. Conclusions
This chapter has presented the new collision detection method and the suspend control of
parallel-structured single-link flexible manipulators using the unscented Kalman filter and
the sliding mode control. The main result is that the collision detection was achieved using
the innovation process of the UKF which is one of the nonlinear filters. Furthermore, the
high performance suspend control has been constructed using the sliding mode controller
based on the UKF. The proposed approach brings an advantage that the system model
requires no linearization. In our previous work, the linearized mathematical model was
required because of using the Kalman filter and the LQG controller for collision detection
Collision Detection and Control of Parallel-Structured Flexible Manipulators
Based on Unscented Kalman Filter 215
and control. The proposed collision detection method can be applied to the multi-link
flexible manipulators, which have strong nonlinearity.
8. References
García, A.; Feliu, V. & Somolions, J. A. (2003). Experimental testing of a gauge based
collision detection mechanism for a new three-degree-of-freedom flexible robot,
Journal of Robotics Systems Vol. 20(No. 6): 271–284.
Payo, I.; Feiu V. & Cortazar, O. D. (2009). Force control of very lightweight single-link
flexible arm based on coupling torque feedback, Mechatronics Vol. 19:
334–337.
Kondo, J. & Sawada, Y. (2008). Collision detection and suspend control of parallel-structured
single-link flexible arms, Proceedings of SICE Annual Conference 2008, Tokyo pp.
3250– 3255.
Kaneko, M.; Kanayama, N. & Tsuji, T. (1998). Active antenna for contact sensing, IEEE
Transactions on Robotics and Automation Vol. 14(No. 2): 278–291.
Moorehead, S. & Wang, D. (1996). Active antenna for contact sensing, Proceedings of. IEEE
International Conference on Robotics and Automation pp. 804–809.
Julier, S.; Uhlmann, J. & Durrant-Whyte, H. F. (2000). A new method for the nonlinear
transformation of means and covariances in filters and estimators, Transaction on
Automatic Control Vol. 45(No. 3): 477–482.
Saad, Y. (1996). Iterative Methods for Sparse Linear Systems, PWS Publishing
Company.
Sawada, Y. (2002a). Collision detection for a flexible cantilever-beam subject to random
disturbance based on innovation process, Proceedins of IEEE International Conference
on Control Applications pp. 1171–1176.
Sawada, Y. (2002b). Collision estimation for a flexible cantilevered-beam subject to random
disturbance, Proceedins of 34th ISCIE Int. Symp. on Stochastic Systems Theory and Its
Application pp. 183–188.
Sawada, Y. (2002c). Detection of collisions for a flexible beam subject to random disturbance,
Proceedins of 41st SICE Annual Conference pp. 268–273.
Sawada, Y. (2004 (in Japanese)). On-line collision detection for flexible cantilevered beams
using innovation process, Transaction of The Institute of Systems, Control and
Information Engineers pp. 349–357.
Sawada, Y. &Watanabe, T. (2007). Lqg control of a parallel-structured single-link
flexible arm, Proceedings of 51st Annual Conference of ISCIE
pp. 372–373.
Julier, S.; Uhlmann, J. & Durrant-Whyte, H. F. (1997). A new extension of the kalman filter to
nonlinear systems, Proceedings of AeroSense: 11th International Symposium
Aerospace/Defense Sensing, Simulation Control.
Matsumoto, T. & Kosuge, K. (2000). Collision detection of manipulator based on adaptive
control law, Proceedings of 2001 IEEE/ASME International Conference on Advanced
Intelligent Mechanics pp. 177–182.
Utkin, V. I. (1992). Sliding modes in optimization and control problems, Springer,
New York.
216 Advanced Strategies for Robot Manipulators
Chen, Y.-S.; Ikeda, H.; Mita, T. & Wakui, S. (1989). Trajectory control of robot arm using
sliding mode control and experimented results, Journal of the Robotics Society of Japan
Vol. 7(No. 6): 706–711.
10
1. Introduction
Industrial robots are naturally equipped with classical PID controllers, which theoretically
assure semi–global asymptotic stability of the closed–loop system equilibrium for the
regulation case (see, e.g., Arimoto & Miyazaki (1984), Arimoto et al., (1990), Kelly (1995b),
Ortega et al., (1995), Alvarez-Ramirez et al., (2000), Kelly et al., (2005), Meza et al., (2007)).
Uniform ultimate boundedness of the closed–loop solutions can be concluded when the
desired position is a function of time (some stability analyzes for this case can be found in
the works of Kawamura et al. (1988), Wen & Murphy (1990), Qu & Dorsey (1991), Rocco
(1996), Cervantes & Alvarez-Ramirez (2001), Choi & Chung (2004), and Camarillo et al.,
(2008)), but to the authors’ knowledge, so far there is not a proof of global regulation for
such controller.
In the search of a practical globally stable PID regulator, some nonlinear control structures
based on the classical PID controller, which assure global asymptotic stability of the closed–
loop system, have emerged. Some works that deal with global nonlinear PID regulators
based on Lyapunov theory and passivity theory have been reported by Arimoto (1995),
Kelly (1998), Santibañez & Kelly (1998a), and Meza & Santibañez (1999). Recently, a
particular case of the class of nonlinear PID global regulators originally proposed in
(Santibañez & Kelly, 1998a) was presented by Sun et al., (2009).
On the other hand, it is well known that saturation phenomena in robot control systems are
intrinsically present when the actuators are driven by sufficiently large control signals. If
these physical constraints are not considered in the controller design they may lead to a lack
of the stability properties.
Even though no one of the controllers mentioned above considers the influence of the
saturation phenomena, there are some works that have been reported to solve this
saturation problem in PD-like controllers for the case of regulation tasks (Kelly &
Santibañez, 1996; Colbaugh et al., 1997a; Loria et al., 1997; Santibañez & Kelly, 1997; 1998b).
Solutions without considering velocity measurements and with gravity compensation are
treated in (Loria et al., 1997). A full–state (position and velocity) feedback solution with
adaptive gravity compensation is presented in (Zergeroglu et al., 2000). More recently, new
schemes dealing with this regulation problem of robot manipulators with bounded inputs
have been presented by Zavala & Santibañez (2006), Zavala & Santibañez (2007), Dixon
(2007), Alvarez-Ramirez et al., (2003), and Alvarez–Ramirez et al., (2008). An adaptive
218 Advanced Strategies for Robot Manipulators
Fig. 1. Practical nonlinear PID controller with bounded torques for robot manipulators.
COMPUTER DRIVER
Fig. 2. Variant of the practical PID controller with bounded torques for robot manipulators.
loop system. This result guarantees that exponential stability of the classical PID linear
regulator in industrial robots is preserved even though the saturation phenomena due to the
electronic devices and/or the actuators are present.
The remainder of this chapter is organized as follows: Section 2 states the dynamic model of
a serial n–link rigid robot manipulator in open–loop, some of its properties, as well as some
considerations, assumptions and definitions that are useful throughout the analysis. The
proposed control scheme is presented in Section 3. Section 4 shows the singularly perturbed
system to analyze. Section 5 states the stability analysis and proves that the control objective
is achieved. Section 6 is devoted to the real–time experimental evaluation carried out on the
PA-10 robot arm. The conclusions of the work are presented in Section 7.
Throughout this chapter, we use the notation λmin{A(x)} and λmax{A(x)} to indicate the
smallest and largest eigenvalues, respectively, of a symmetric positive definite bounded
matrix A(x), for any x ∈ Rn. Also, we define λmin{A} as the greatest lower bound (infimum) of
λmin{A(x)}, for all x ∈ Rn, that is, λmin{A} = infx∈Rn λmin{A(x)}. Similarly, we define λmax{A} as
the least upper bound (supremum) of λmax{A(x)}, for all x ∈ Rn, that is, λmax{A} = supx∈Rn
λmax{A(x)}. The norm of vector x is defined as x = x T x and that of matrix A(x) is defined
as the corresponding induced norm A( x ) = λmax { A( x )T A( x )} .
2. Preliminaries
2.1 Robot dynamics
The dynamics of a serial n–link rigid robot, without the effect of friction, can be written as
(Spong & Vidyasagar, 1989):
220 Advanced Strategies for Robot Manipulators
M ( q )q + C ( q , q )q + g ( q ) = τ (1)
where q, q , q ∈ Rn are the vectors of joint positions, velocities and accelerations, respectively,
τ ∈ Rn is the vector of applied torques, M(q) ∈ Rn×n is the symmetric positive–definite inertia
matrix, C(q, q ) ∈ Rn×n is the matrix of centripetal and Coriolis torques, and g(q) ∈ Rn is the
vector of gravitational torques obtained as the gradient of the robot potential energy U(q), i.e.
∂U( q )
g( q ) = . (2)
∂q
We assume that all the joints of the robot are of the revolute type.
⎡1 ⎤
q T ⎢ M ( q ) − C ( q , q )⎥ q = 0 ∀ q , q∈R n .
⎣2 ⎦
◊
Property 2. The gravitational torque vector g(q) is bounded for all q ∈ Rn. This means that
there exist finite constants γ i ≥ 0 such that (Craig, 1998):
sup gi ( q ) ≤ γ i i = 1, 2, , n, (3)
q ∈R n
where gi(q) stands for the i-th element of g(q). Equivalently, there exists a constant k′ such
that &g(q)& ≤ k′ , for all q ∈ Rn. Furthermore, there exists a positive constant kg such that
∂ g( q )
≤ kg ,
∂q
for all q ∈ Rn, and &g(x) − g(y)& ≤ kg&x − y&, for all x,y ∈ Rn. Moreover, a simple way to
compute kg is:
⎛ ∂g ( q ) ⎞
k g ≥ n ⎜ max i ⎟ where i = 1, 2,… n and j = 1, 2,… n. (4)
⎜ i , j , q ∂q j ⎟
⎝ ⎠
A less restrictive constant k g can be computed by:
i
⎛ ∂g ( q ) ⎞
k g ≥ n ⎜ max i ⎟ where i = 1, 2,… n and j = 1, 2,… n. (5)
i ⎜ j , q ∂q j ⎟
⎝ ⎠
◊
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 221
T
⎡ ∂f ( z ) ⎤
⎢ ⎥
⎢ ∂z1 z= ξ ⎥
⎢ ⎥
⎢ ∂f ( z ) ⎥
⎢ ⎥
f ( x ) − f ( y ) = ⎢ ∂z2 z= ξ ⎥ [ x − y ] (6)
⎢ ⎥
⎢ ⎥
⎢ ∂f ( z ) ⎥
⎢ ⎥
⎢ ∂zn ⎥
z= ξ ⎦
⎣
where ξ ∈ Rn is a vector suitably chosen on the line segment which joins vectors x and y. ◊
Theorem 2. [Kelly et al. (2005), p.385] Consider the continuous vectorial function f : Rn → Rm. If
f i (z1,z2, . . . ,zn) has continuous partial derivatives for i = 1, . . . , m, then, for each pair of vectors
x,y ∈ Rn and each ω ∈ Rm there exists ξ ∈ Rn such that:
∂f ( z )
[ f ( x ) − f ( y )]T ω = ω T ( x − y ), (7)
∂z z= ξ
⎡ sat( x1 ; k1 ) ⎤ ⎡ x1 ⎤ ⎡ k1 ⎤
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
sat( x ; k ) x k
sat( x ; k ) = ⎢
2 2 ⎥
, x = ⎢ 2⎥, k = ⎢ 2⎥,
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢⎣sat( xn ; kn )⎥⎦ ⎢⎣ xn ⎥⎦ ⎢⎣ kn ⎥⎦
with ki being the i–th saturation limit, i = 1,2, . . . ,n, and each element of sat(x;k) is defined as:
⎧ xi if |xi |≤ ki
⎪
sat( xi ; ki ) = ⎨ ki if xi > ki
⎪
⎩− ki if xi < − ki
◊
222 Advanced Strategies for Robot Manipulators
Furthermore, the control scheme proposed in this chapter involves special saturation
functions which fit in the following definition.
Definition 2. [Zavala & Santibañez (2006)] Given positive constants l and m, with l < m, a
function Sat(x; l,m) : R→R: x 6 Sat(x; l,m) is said to be a strictly increasing linear saturation
function for (l,m) if it is locally Lipschitz, strictly increasing, C2 differentiable and satisfies:
1. Sat(x; l,m) = x when |x| ≤ l
2. |Sat(x; l,m)| < m for all x ∈ R. ◊
For instance, the following saturation function is a special case of the linear saturation given
in Definition 2:
⎧ ⎛ x+l ⎞
⎪−l+(m−l )tanh ⎜ ⎟ if x < −l
⎪⎪ ⎝ m−l⎠
Sat( x ; l , m)=⎨ x if |x|≤ l (8)
⎪
⎪ l+(m−l )tanh ⎜⎛ x − l ⎟⎞ if x > l
⎪⎩ ⎝m−l⎠
n saturation functions of the form (8) can be joined together in an n × 1 saturation function
vector denoted by Sat(x; l,m), i.e.,
⎡ Sat( x1 ; l1 , m1 ) ⎤
⎢ ⎥
Sat( x2 ; l2 , m2 ) ⎥
Sat( x ; l , m) = ⎢ ,
⎢ ⎥
⎢ ⎥
⎣⎢ Sat( xn ; ln , mn )⎦⎥
where x, l, m ∈Rn, that is,
⎡ x1 ⎤ ⎡ l1 ⎤ ⎡ m1 ⎤
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
x l m
x = ⎢ 2 ⎥ , l = ⎢ 2 ⎥ , m = ⎢ 2 ⎥.
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣⎢ xn ⎦⎥ ⎣⎢ln ⎦⎥ ⎣⎢mn ⎦⎥
Consider the robot dynamic model (1). Assume that each joint actuator is able to supply a
known maximum torque τ imax so that:
where τ i stands for the i–th entry of vector τ. In other words, if ui represents the control
signal (controller output) before the actuator, related to the i-th joint, then
⎛ ui ⎞
τ i = τ imaxsat ⎜ ⎟, (10)
⎜ τ max ⎟
⎝ i ⎠
for i = 1, . . . ,n, where sat(·) is the standard hard saturation function. We also assume:
Assumption 1. The maximum torque τ imax of each actuator satisfies the following condition:
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 223
t
w* = Kip
∫
0
q dr (13)
where Kpv, Kpp and Kip are diagonal positive definite matrices. This control law is formed by
two loops: an outer joint–position proportional–integral PI loop and an inner joint–velocity
proportional P loop, and considers the saturation effects existing in the output of the control
stage (see Figure 2), where Sat[Kpv [Sat(K pp q + w∗ , l∗pi , m∗pi ) − q ] ; lp,mp] is a vector where each
element is a saturation function as in Definition 2 for some (lp,mp), where lp and mp are
vectors with elements lpi and mpi, respectively, and i = 1,2, . . . ,n. The control law (12) can be
rewritten as:
τ = Sat[Sat( K p q + w ; l pi , m pi ) − K v q ; l p , m p ] (14)
t
w = Ki
∫ 0
q dr (15)
where
◊
224 Advanced Strategies for Robot Manipulators
Remark: In practice, the saturation constraints of the electronic devices and the actuators
are, in fact, hard saturations like those in Definition 1. However, with the end of carrying
out the stability analysis, they can be aproximated by linear saturation functions like those
defined in Definition 2, with l < m, and l arbitrarily close to m.
In order to simplify the notation, henceforth, we will omit, in the argument, the limits of the
saturation functions.
⎡ q ⎤ ⎡ −q ⎤
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
d ⎢ ⎥ ⎢
q = M( q )−1[Sat[Sat(K p q + w ) − K v q ] − C( q , q )q − g ( q )]⎥ (18)
dt ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢ w⎥ ⎢ ⎥
⎣ ⎦ ⎣⎢ Ki q ⎦⎥
which is an autonomous differential equation with a unique equilibrium point given by
[ q T q T wT]T = [0T 0T g(qd)T ]T ∈ R3n, where we have used Assumption 2, and (3), to get that
Sat(Sat(w)) − g(qd) = 0 implies w = g(qd). In order to move the equilibrium point of (18) to the
origin, we apply the change of variables x = w − g(qd). Now the new closed–loop system is
given by:
⎡q ⎤ ⎡ −q ⎤
⎢ ⎥ ⎢ ⎥
⎢ ⎥
d ⎢ ⎥ ⎢
⎢ q ⎥ = M( q )−1[Sat[Sat(K p q + x + g( q d )) − K v q ] − C ( q , q )q − g( q )]⎥ . (19)
dt ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢x⎥ ⎢ ⎥
⎣ ⎦ ⎢⎣ K i q ⎦⎥
The previous closed–loop system can be studied as a singularly perturbed system. To this
end, system (19) can be described as two first–order differential equations as follows:
d
x = Ki q (20)
dt
d ⎡q ⎤ ⎡ −q ⎤
⎢ ⎥=⎢ −1 ⎥. (21)
dt ⎣ q ⎦ ⎢⎣ M( q ) [Sat[Sat( K p q + x + g( q d )) − K vq ] − C ( q , q )q − g ( q )]⎥⎦
Moreover, by choosing the integral gain matrix as Ki = ε Ki* , where Ki* is a diagonal
positive–definite matrix and ε > 0 is a small parameter, and letting t′ = εt be a new time–
scale ( t′ is a slow time compared to t), we can rewrite (20)–(21) as
d
x = K i* q (22)
dt′
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 225
d ⎡q ⎤ ⎡ −q ⎤
ε ⎢ ⎥ = ⎢ −1 ⎥ (23)
dt′ ⎣ q ⎦ ⎢⎣ M( q ) [Sat[Sat(K p q + x + g ( q d )) − K v q ] − C ( q ,q )q − g ( q )]⎥⎦
where, in the forthcoming analysis, and in accordance with the singular perturbation theory,
x in (23) will be treated as a fixed parameter, due to its slow variation.
q = 0, (24)
K p q + x + g ( q d ) − g ( q ) = 0. (26)
Now, the Contraction Mapping Theorem (Kelly et al., 2005; Khalil, 2002), guarantees that
(26) has a unique solution q = h1(x) ∈ Rn provided that
kp > k g (27)
i i
⎡ q ⎤ ⎡ h 1( x )⎤ 2n
⎢ ⎥=⎢ ⎥ = h ( x ) ∈R . (28)
q
⎣ ⎦ ⎣ 0 ⎦
Consequently, we have that:
x = h −1( q ) = −K p q − g( q d ) + g ( q ) (29)
⎡ y 1(t′) ⎤ ⎡ q (t′) − h 1( x )⎤
⎢ ⎥=⎢ ⎥ (30)
⎣ y 2( t ′ ) ⎦ ⎣ q (t′) ⎦
which implies that q = y1 + h1(x). Then, (22)–(23) can be now represented by the new
variables as a singularly perturbed system given by
226 Advanced Strategies for Robot Manipulators
d
x = K i* [ y 1+ h 1( x )] (31)
dt′
⎡ ⎡ ∂h (x) ⎤ ⎤
⎡ y1⎤ ⎢ − y 2 − ε ⎢ 1 ⎥ K i* [ y 1+ h 1( x )] ⎥
⎢ ⎥ ⎢ ⎣ ∂x ⎦ ⎥
d ⎢ ⎥ ⎢ ⎥
ε = ⎢ ⎥. (32)
dt′ ⎢ ⎥
⎢ ⎥ ⎢ M( q − y − h ( x ))−1[Sat[Sat[K ( y − h ( x )) + x + g( q )] − K y ]⎥
⎢⎣ y 2 ⎥⎦ ⎢ d 1 1 p 1 1 d v 2 ⎥
⎢⎣ −C ( q d − y 1− h 1( x ), y 2 ) y 2 − g ( q d − y 1− h 1( x ))] ⎥⎦
5. Stability analysis
According to the theory of singularly perturbed systems (Khalil, 2002), the origin of (22)–
(23) is asymptotically stable if and only if the origin of (31)–(32) is asymptotically stable. It is
important to remember that x is a fixed parameter in (23) and (32), this is because t′ and x
are varying slowly since, in the t time scale, they are given by (Khalil, 2002):
t ′ = t0 + ε t , x = x (t0 + ε t ), (33)
being t0 the initial time. The setting of ε = 0 freezes these variables at t′ = t0 and x = x(t0 )
(initial conditions).
By simplicity, we divide the stability analysis in two parts:
• First, we will prove asymptotic stability and local exponential stability of the origin of a
saturated PD controller with desired gravity compensation plus a constant vector x,
which can be seen as a constant control input.
• Second, based on a theorem of singularly perturbed systems, we will prove that the
origin of (22)–(23) is locally exponentially stable.
τ = Sat[Sat(K p q + x + g ( q d )) − K v q ]. (34)
d ⎡q⎤ ⎡ −q ⎤
⎢ ⎥=⎢ −1 ⎥ (35)
dt ⎣ q ⎦ ⎢ M( q ) [Sat[Sat( K p q + x + g ( q d )) − K v q ] − C ( q , q )q − g ( q )]⎥
⎣ ⎦
whose equilibrium points are the solutions of the nonlinear equations (24)-(25) and they
T T
have already been proven to have a unique solution ⎡q T q T ⎤ = ⎡ h 1( x )T 0T ⎤ , provided
⎣ ⎦ ⎣ ⎦
that k p > k g is satisfied.
i i
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 227
1 T
W ( q ,q ) = q M( q )q + W1 ( q ) (36)
2
where
n qi
W1 ( q ) = ∑∫ i =1
0
Sat[Sat( k p ri + x i + gi ( q d ))]dri + U ( q d − q )
i
n h1 ( x )
− ∑∫
i =1
0
i Sat[Sat( k p ri
i
+ x i + gi ( q d ))]dri − U ( q d − h 1( x )).
By following similar steps to those given by Zavala & Santibañez (2007) (see Appendix B)
we prove that (36) is a positive definite and radially unbounded function, provided that
k p > k g . The time derivative of W ( q , q ) along the trajectories of (35), and after some
i i
algebraic simplifications, results in:
Finally, by using the following property of linear saturation functions (Santibañez et al.,
2010):
2
W ( q , q ) ≤ − Sat[Sat(K pq + x + g ( q d )) − K v q ] − Sat[Sat(K p q + x + g ( q d ))] ≤ 0.
Ω = { q , q ∈ R n: W ( q , q ) = 0} = { q = 0, q ∈ R n}.
Notice that, from (35),
q (t ) ≡ 0 ⇒ q (t ) ≡ 0 ⇒ Sat[Sat(K p q + x + g ( q d ))] − g( q d − q ) ≡ 0.
Sat[Sat(K p q + x + g ( q d ))] − g( q d − q ) ≡ 0 ⇒ q ≡ h 1( x ).
228 Advanced Strategies for Robot Manipulators
Therefore, from LaSalle’s Invariance Principle we conclude that the equilibrium point
T T
⎡q T q T ⎤ = ⎡ h 1( x )T 0T ⎤ ∈ R 2 n of (35) is globally asymptotically stable.
⎣ ⎦ ⎣ ⎦
y = A( y ) y + B( y ) f ( y ), (38)
where y ∈ Rm, A(y) and B(y) are m×m nonlinear functions of y, and f (y) is a m×1 nonlinear
function of y. Assume that f (0) = 0; hence, y = 0 ∈ Rm is an equilibrium point of the system
(38). Then, the linearized system of (38) around the equilibrium y = 0 is given by:
⎡ ∂ f (0) ⎤
y = ⎢ A(0) + B(0) ⎥ y. (39)
⎣ ∂y ⎦
◊
In order to prove that the equilibrium point of the closed–loop system (35) is locally
exponentially stable, we consider a local linearization of the closed–loop system around the
T T
equilibrium point ⎡q T q T ⎤ = ⎡ h 1( x )T 0T ⎤ ∈ R 2 n (Khalil, 2002). In the neighborhood of
⎣ ⎦ ⎣ ⎦
this equilibrium point, the closed–loop system (35) can be represented by:
M( q )q + C ( q , q )q + g( q ) − K p q + K v q − x − g( q d ) = 0. (40)
M ( q d − y 1− h 1( x )) y 2 + C ( q d − y 1− h 1( x ), y 2 ) y 2
+ g ( q d − y 1− h 1( x )) − K p [ y 1+ h 1( x )] + K v y 2 − x − g( q d ) = 0
whose unique equilibrium is the origin, provided that (27) is satisfied. The previous
equation can be written as:
y = A( y ) y + B( y ) f ( y ). (41)
where,
d ⎡ y1⎤
y = ⎢ ⎥
dt ⎣ y 2 ⎦
⎡0 −I ⎤
A( y ) = ⎢ −1 ⎥
⎣⎢0 − M( q d − y 1− h 1( x )) [K v + C ( q d − y 1− h 1( x ), y 2 )]⎦⎥
⎡0 0 ⎤
B( y ) = ⎢ −1 ⎥
⎣⎢ 0 M ( q d − y 1− h 1( x )) ⎦⎥
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 229
⎡ 0 ⎤
f (y) = ⎢ ⎥.
K [ y + h ( x )] + x + g ( q ) − g ( q − y − h ( x ))
⎣⎢ p 1 1 d d 1 1 ⎦⎥
According to Lemma 1, the linearized system from (41), around the equilibrium y = 0, has
the form (39), with:
⎡0 −I ⎤
A(0) = ⎢ −1 ⎥
⎢⎣0 −M ( q d − h 1( x )) K v ⎥⎦
⎡0 0 ⎤
B(0) = ⎢ −1 ⎥
⎣⎢ 0 M( q d − h 1( x )) ⎦⎥
∂ f (0) ⎡0 0⎤
= ⎢ * ⎥
∂y ⎣⎢ K 0 ⎦⎥
which can be compacted in:
d ⎡ y1⎤ ⎡ 0 −I ⎤⎡ y1⎤
⎢ ⎥=⎢ ⎥⎢ ⎥ (42)
dt ⎣ y 2 ⎦ ⎣⎢ M ( q d − h 1( x ))−1 K * −1
− M( q d − h 1( x )) K v ⎦⎥ ⎣ y 2 ⎦
J
*
where K is given by:
∂g ( q d − y1 − h1 ( x ))
K * = Kp − .
∂y 1
1 T 1
WL ( y 1 , y 2 ) = y 2 M( q d − h 1( x )) y 2 + y T1 K * y 1 (43)
2 2
which is a positive definite function. The time derivative along the trajectories of (42) is:
W ( y 1, y 2 ) = y T2 M( q d − h 1( x )) y 2 + y T1 K * y 1
= y T2 [K * y 1− K v y 2 ] − y T1 K * y 2= − y T2 K v y 2
which is a negative semidefinite function. By using the LaSalle’s Invariance Principle we can
conclude global asymptotic stability of the closed–loop system (42). To this end, let us define
Ω as:
M ( q d − h 1( x ))−1 K * y 1≡ 0 ⇒ y 1≡ 0.
Therefore, from LaSalle’s Invariance Principle we conclude that the origin of the linear
system (42) is globally asymptotically stable. This implies that the eigenvalues of J in (42) are
located in the left–hand side of the complex plane (see Theorem 4.5 in Khalil (2002)), and
hence, the origin of the linear system (42) is exponentially stable (see e.g. Theorem 4.11 in
Khalil (2002) that shows that, for linear systems, uniform asymptotic stability of the origin is
equivalent to exponential stability). According to this, exponential stability of the origin for
the linear system (42) implies the local exponential stability of the origin for the nonlinear
system (41) (see e.g. Theorem 4.13 in Khalil (2002)).
Finally, we can conclude that the equilibrium point of the nonlinear system (35) is locally
exponentially stable. So we have proven the following:
Proposition 1. Under Assumption 2, and (27), the control law (34) guarantees global
asymptotic stability and local exponential stability of the closed–loop system (35) with
|τ i (t )|≤ τ imax for all i = 1,2, ...,n and t ≥ 0.
x = f (t′ , x , z , ε ) (46)
ε z = g (t′, x , z , ε ). (47)
Assume that the following are satisfied for all ( t′, x,ε) ∈ [0,∞) × Br × [0, ε], with Br = {x ∈ Rn :
&x& ≤ r}:
a. f (t′,0,0, ε ) = 0 and g(t′,0,0, ε ) = 0.
b. The equation 0 = g(t′, x , z ,0) has an isolated root z = h( t′, x ) such that h( t′,0 ) = 0.
c. The functions f, g, h and their partial derivatives up to the second order are bounded for z −
h( t′, x ) ∈ Bρ, with Bρ = {y ∈ R2n : &y& ≤ ρ}.
d. The origin of the reduced system
is exponentially stable.
e. The origin of the boundary–layer system
dy
= g(t′, x , y + h (t′, x ),0) (49)
dt
is exponentially stable, uniformly in ( t′ ,x).
Then, there exists ε* > 0 such that, for all ε < ε *, the origin of (46)–(47) is exponentially stable. ◊
We are now ready to present our main contribution.
Proposition 2. Consider the robot dynamics (1) in closed–loop with the practical saturated
PID control law (12). Under Assumption 2, and (27), the origin of the closed–loop system
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 231
(22)–(23) is locally exponentially stable, and therefore, the equilibrium point of (18) is locally
exponentially stable. Besides |τi (t)| ≤ τ imax for all i = 1,2, ...,n and t ≥ 0. ◊
Proof. Notice that (46)–(47) correspond to (22)–(23), respectively, with
f (t′ , x , z , ε ) = K i* q
⎡ −q ⎤
g (t′ , x , z , ε ) = ⎢ −1 ⎥
⎢⎣ M( q ) [Sat[Sat(K p q + x + g( q d )) − K v q ] − C( q , q ) q − g( q )]⎥⎦
⎡q ⎤ 2n
z = ⎢ ⎥∈ R .
q
⎣ ⎦
In order to complete the stability analysis, we are going to check each item of the Theorem 3.
a) By substituting x = q = q = 0 in (22)–(23), it is straightforward to verify this assumption.
b) This item is easily fulfilled by noting that the root of g(t′, x , z , ε ) has been obtained in
Section 4.2, where it was proven that, for each x ∈ Rn, the unique root of (23) is z = h(x) =
[h1(x)T 0T]T ∈ R2n, provided that (27) is satisfied. On the other hand, we know from (28) that
q = h1(x), and therefore, when x = 0 we have that q = h1(0); then, from (29), 0 = h 1−1 (q ) =
−[KpKpc q + g(qd) − g(qd − q )] which under assumption (27) has a unique solution q = 0.
Hence, h(0) = [h1(0)T 0T]T = [0T 0T]T and assumption b) is verified.
c) This is straightforward given that the right–hand side of (22)–(23) is C2.
d) By substituting the isolated root z = h(x) and ε = 0 in (22), that is q = h1(x) and q = 0, we
obtain the so–called reduced system, which is given by:
d
x = K i* h 1( x ) (50)
dt′
whose unique equilibrium point results from h1(x) = 0 and is given by x = h 1−1 (0) = 0
provided that (27) is satisfied. Comparing the reduced system (50) with the terms used in
Theorem 3, we have x = f (t′, x , h(t , x ),0) = Ki* h1 ( x ).
On the other hand, to analyze the origin of the reduced system (50), let us define the
quadratic Lyapunov function candidate
1 T * −1
V( x ) = x (Ki ) x (51)
2
which satisfies
1 1
λmax {(K i* )−1 } x 2
≥ V(x ) ≥ λmin {(K i* )−1 } x 2
(52)
2 2
and hence, it is a positive definite and radially unbounded function. The time derivative
along the trajectories of (50) is given by:
V ( x ) = x T (K i* )−1 x = x T h 1( x ). (53)
232 Advanced Strategies for Robot Manipulators
x = −K p h( x ) − g( q d ) + g( q d − h( x )), (54)
substituting in (53) we have
h T1 x = h 1( x )T [ −K p h( x ) − g ( q d ) + g ( q d − h ( x ))]
= − h 1( x )T K p h 1( x ) + h 1( x )T [ − g( q d ) + g( q d − h 1( x ))]
⎡ ∂g( z ) ⎤
≤ − h 1( x )T ⎢K p + ⎥ h 1( x )
⎢ ∂z z = ξ ⎥⎦
⎣
where we use Theorem 2, and
∂g( z )
Kp + (55)
∂z z= ξ
is a positive definite matrix provided that
n
∂gi ( q )
kp >
i ∑max
j =1
q ∂q j
for i = 1,… , n. (56)
⎡ ∂g( z ) ⎤ ⎧ ∂g( z ) ⎪⎫
⎥ h 1( x ) ≤ −λmin ⎪⎨K p +
2
V ( x ) ≤ − h 1( x )T ⎢ K p + ⎬ h 1( x ) (57)
⎢ ∂z z = ξ ⎥⎦ ⎪⎩ ∂z z= ξ ⎪
⎣ ⎭
Notice that, due to h1(0) = 0, the time derivative (53) is a negative definite function and we
can conclude global asymptotic stability of the origin of (50).
Moreover, we have that:
2
x = xTx
= [ −K p h 1( x ) − g ( q d ) + g ( q d − h 1( x ))]T [ −K p h 1( x ) − g( q d ) + g( q d − h 1( x ))]
= h 1( x )T K p2 h 1( x ) + 2 h 1( x )T K p [ − g ( q d ) + g ( q d − h 1( x ))]
+[ − g ( q d ) + g( q d − h 1( x ))]T [ − g ( q d ) + g ( q d − h 1( x ))]
2
≤ [λmax {K p } 2 + 2 k g λmax {K p } + k g2 ] h 1( x )
2
= [λmax {K p } + k g ]2 h 1( x ) .
Then
2 1 2
h 1( x ) ≥ 2
x , (58)
[λmax {K p } + k g ]
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 233
⎧⎪ ∂g( z ) ⎫⎪
−λmin ⎨K p + ⎬
∂z
V( x ) ≤ ⎩⎪ z= ξ ⎭⎪
x
2
. (59)
[λmax { K p } + k g ]2
Therefore, from (52) and (59), we can conclude that x = 0 is a globally exponentially stable
equilibrium point for the reduced system (50) provided that (27) is satisfied (see Theorem
4.10, Khalil (2002)). So we have verified the assumption d) of Theorem 3.
dy dy
e) By setting ε = 0 and considering that ε = in (32), we obtain the boundary–layer
dt′ dt
system:
⎡ y1 ⎤ ⎡ − y2 ⎤
⎢ ⎥ ⎢ ⎥
d
dt ⎢ ⎥ ⎢
1 ⎡
⎢
⎣ ⎣ (
⎢ ⎥ = ⎢ M( q d − y1− h 1( x )) Sat ⎢Sat K p( y 1+ h 1( x ) )+ g( q d ) + x −K v y 2 ⎥
− ⎡ ⎤
⎥
⎦⎥ ) (60)
⎢ y2 ⎥ ⎢ −C ( q − y − h ( x ), y ) y − g ( q − y − h ( x )) ⎤ ⎥
⎣ ⎦ ⎣ d 1 1 2 2 d 1 1 ⎦ ⎦
dy g ( t , x , y + h ( t , x ),0)
dt
where, according to (33), x is frozen at x = x(t0 ), which corresponds to the robotic system
under the Saturated PD Controller with Desired Gravity Compensation plus a constant
vector x, whose unique equilibrium point is the origin, provided that (27) is satisfied.
The stability analysis of (60) has already been carried out in the previous subsection, where
we concluded, in accordance with Proposition 1, that the origin of (60) is asymptotically
stable and locally exponentially stable, uniformly in x. The uniformity in x is given
straightforward with the asymptotic stability of the origin of (60) because it is an
autonomous system. This checks the assumption e). Finally, we conclude, in accordance
with Theorem 3, that the equilibrium point of the closed–loop system (18) is locally
exponentially stable for a sufficiently small ε. Under Assumption 2 the constraints (9) are
trivially satisfied. This completes the proof. ◊
6. Experimental results
6.1 The PA10 robot system
The Mitsubishi PA10 arm is an industrial robot manipulator which completely changes the
vision of conventional industrial robots. Its name is an acronym of Portable General-Purpose
Intelligent Arm. There exist two versions (Higuchi et al., 2003): the PA10-6C and the PA10-
7C, where the suffix digit indicates the number of degrees of freedom of the arm. This work
focuses on the study of the PA10-7CE model, which is the enhanced version of the PA10-7C.
The PA10-7CE robot is a 7–dof redundant manipulator with revolute joints. Figure 3 shows
a diagram of the PA10 arm, indicating the positive rotation direction and the respective
names of each of the joints. The PA10 arm is an open architecture robot; it means that it
possesses (Oonishi, 1999):
234 Advanced Strategies for Robot Manipulators
Axis 6 (W1)
Axis 7 (W2)
Axis 5 (E2)
Axis 4 (E1)
Axis 3 (S3)
Axis 1 (S1)
Axis 2 (S2)
T
g ( q ) = ⎡⎣ g1 (q ) g2 ( q ) … gn ( q )⎤⎦
where
g1 ( q ) = 0
g7 ( q ) = 0
The following expressions recall how the parameters of interest can be found:
∂gi ( q ) ∂g ( q )
k g ≥ n max , k g ≥ n max i ,
i, j, q ∂q j i j, q ∂q j
γ i ≥ sup| gi (q )|, k′ ≥ γ 12 + γ 22 + … + γ n2 .
q
The numerical values of the parameters for the PA10-7CE are shown in Table 1. The table
also shows the torque and velocity saturation limits of each joint, which are employed to
select the corresponding limits of the saturation functions in the controller.
⎣ ( )
τ = Sat ⎡⎢K pdSat K pc q ; l p , m p − K pd q + w ; l pi , m pi ⎤⎥
⎦
(61)
t
w = Kid
∫ ⎡⎣Sat(K
0
pc q ( r ); l p , m p ) − q (r )⎤ dr
⎦
where Kpd, Kpc, Kid ∈ Rn×n are diagonal positive definite matrices, and we take α = 1 (see
Fig. 1).
Each of the experiments consisted in taking the robot from the vertical home position
T
(where q = 0) to the following desired position: qd = ⎡⎣ − π2 π3 π2 π3 π2 − π2 π2 ⎤⎦ rad.
Kpv 90.0 150.0 35.0 85.0 10.0 6.0 12.0 [Nm s/rad]
Table 2. Values of the control parameters selected for the Sat(Sat(PI)+P) scheme
q̃1 [rad]
q̃2 [rad]
0 ....... ........ ........ ........ ........ .....................................................................................................................................................................................................................................................................................................................
..
..
..
..
..
π/3 ....
..
...
.. ..
.. . ..
. ..
.. ..
.. ..
.. ..
.. ..
.. ..
..
− π/4 ...
... ..
..
π/6
...
.. ..
..
.. ..
..
.. ..
.. ..
.. .. ...
. ...
.. ..
.. ...
..
.. ...
.. ...
..
− π/2 .. . ...
..
....... ........ ........ ............................................................................................................................................................................................................................................................................................................................................................
0
0 2 4 6 8 10
0 2 4 6 8 10
t [s] t [s]
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
q̃7 [rad]
π/2 ...
..
..
...
..
..
...
..
..
...
..
...
...
..
π/4
...
..
..
...
..
...
..
..
..
...
...
..
...
..
..
....... ..........................................................................................................................................................................................................................................................................................................................................................................................
0
0 2 4 6 8 10
t [s]
τ [Nm]
1 ....... 200 τ2 [Nm]
.. ...
....... ........ ........ ........ ........ ............... ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ...
0 .. ... . .. . .... . ... . . . .. . . .. . . .. .. . . . .. . .. . . . ..
......... ....... . .......................................................................................... ....................................................................................................................................................... 150 .......
.
.... ... .. . .....
....
... ......
... ..
− 20 ..... .......
................ ..... .... ....... ...................
.. 100 ... ....
. ..
...... . ....... ...... ............................. . . ...... ... . . .. ..
... .. . . ...
..... . 50 ..
.... ... ...
..... ....
........
− 40 .... ..
0
... .
....... ............. ................. ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ...
.... .. .. ... . .......
.... ..
..
... .... .... .
− 60
..
.. − 50 ... .. ......
... ........... ................................................................................................................................................................................................................................................................................
.... .... .... .
... ..
.... − 100 .
..... ....
....
.......
− 80 ....
− 150 ......
.
.... ....
.....
...
− 100 − 200
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
.....
60 ..
...
70 ...
... ..... 40
...
.
...
. .
... ......... ...
... ... ..
50
...
...
........
.
.
20 ......... .
............ .
... ... ... ..
....... ........................... .... ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ...
...
...
.... ....
.
...... ..............
.. . 0 ... ..... ..
.... ...... . ... . .
... . . . ... .... . ... ....................................... .. .....................................................................................................................................................................................................................................................
30 ...
... ..
.. ...
..
.. ....... ..... ....
...
− 20 ... .... .... ......... ........
.... ... ..
.. ... . ... .
... ........... .
............. .. ... .............................................................................................................................................................................................................................................................................
... .. − 40 .... ...
.
.. ..
... ... ..... ....
10 .......
.
.....
− 60 ... ...
.... ....
....
....... ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ... − 80
− 10 − 100
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
15 τ5 [Nm] 15 τ6 [Nm]
.... ...
... ...
... .. ...
10 .... ... ...
............... ...
...... .
10
............................... ..
. .. ..
.... ........ .
5 ............
.........
. 5 ....
.......
.
.........
.............. . .. . .. . ........
.. .... . ..... .. .
....... ..................... ......... .................................................................................................................................................................................................................................................................................................................................................................................................................................................................
0 ....
.............
0 ....... ........................................................................................................................................................................................................................................................................................................................................................................................................................................................................................................
...
....
......... .....
.......
−5 ... −5 ....
..... ..
. ..
..... ...
.... ...
− 10 ..... − 10 ..... ....
............
.
.... .
.
.. .....
− 15 − 15
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
15 τ7 [Nm]
......
...
...
10 ...
...
.. .
.................
. ...
...
5 ...
...
...
.. .......................................................................................................................................................................................................................................................................................................................
.
....... .... ............ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ...
0 .....
......
......
.
.........
−5 .........
.
........
.
.........
− 10 ........
.
.......
...
− 15
0 2 4 6 8 10
t [s]
Table 3. Values of the control parameters selected for the Sat(Sat(P)+PI) scheme
7. Conclusions
In this chapter we have proposed an alternative to the saturated nonlinear PID controller
previously presented by Santibañez et al. (2010) which, also, results from the practical
implementation of the classical PID controller, by considering the natural saturations of the
electronics in the control computer, servo drivers, and actuators. The stability analysis of the
closed–loop system is carried out by using the singular perturbation theory. Based on auxiliary
Lyapunov functions, we prove local exponential stability of the equilibrium point of the closed–
loop system. It is also guaranteed that, regardless of the initial conditions, the delivered actuator
torques evolve inside the permitted limits. Experimental results confirm the proposed analysis.
Furthermore, the theoretical result explains why the classical linear PID regulator used in
industrial robot manipulators preserves the exponential stability in spite of entering the
saturation zones inherent to the electronic control devices and the actuator torque constraints.
8. Acknowledgement
This work is partially supported by PROMEP, DGEST, and CONACYT (grant 60230), Mexico.
9. References
Aguiñaga-Ruiz, E.; Zavala-Rio, A.; Santibañez, V. & Reyes, F. (2009). Global trajectory
tracking through static feedback for robot manipulators with bounded inputs. IEEE
Transactions on Control Systems Technology, Vol. 17, No. 4, pp. 934-944.
Alvarez-Ramirez, J.; Cervantes, I. & Kelly, R. (2000). PID regulation of robot manipulators:
Stability and performance. Systems and Control Letters, Vol. 41, pp. 73-83.
Alvarez-Ramirez, J.; Kelly, R. & Cervantes, I. (2003). Semiglobal stability of saturated linear
PID control for robot manipulators. Automatica, Vol. 39, pp. 989-995.
Alvarez-Ramirez, J.; Santibañez, V. & Campa, R. (2008). Stability of robot manipulators
under saturated PID compensation. IEEE Transactions on Control Systems Technology,
Vol. 16, No. 6, pp. 1333-1341.
Arimoto, S. (1995). Fundamental problems of robot control: Part I, Innovations in the realm
of robot servo–loops. Robotica, Vol. 13, pp. 19–27.
240 Advanced Strategies for Robot Manipulators
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
q̃7 [rad]
π/2 ...
...
...
...
..
..
..
..
..
..
..
...
...
...
π/4 ...
...
...
...
....
....
....
....
.....
......
......
.......
.........
...........
..............
......................
...
....... ........ ........ ........ ........ ........ ........ ........ ........ ........ ...........................................................................................................................................................................................................................................
0
0 2 4 6 8 10
t [s]
10 τ1 [Nm]
200 .....
.... τ [Nm] 2
..........
....... ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ... ........
.........
150 ......
........
...
− 10 ...
......................... . ...
...
..
..
.... .
....... ................................................. ........ . .
. . ................................................................. ..... 100 ......
...
.. . . . .................................................................................. ...
................. ... .. .........
.. ..
...... . .... ............................... ....... ....
.
...
............ .... . . .. . ... .
...................
..
50 .........
.........
. .
... ................
− 30 ... ...... .
....... ........................................ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ...
..
... 0 ... ...............................
.... . . . ...
.... . ...
.. ... ............
.. .. ............................................................................... .. ..
− 50 .........
..
.
.. ................................................................................................................ ............................................. ......................... .......... .............. ........ .. .... . ... . . . .. . . . . .. . . . .
. . ...... ...... .............. ...... ................................................................................................................................................... ................. .............................. .... ...............
..... . . . .... ... .. . .... .. .. .. .. .. .. ..... ....... .................................... ...... ................ ....................................................
...... . . .. .. . .. .. .. . . .. ... ..
.
− 50 − 100
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
τ3 [Nm] τ4 [Nm]
90 90 ....
.. ..
....
...
... 70 ...
70 ...
...
...
..
...
...
...
...
50 ...
...
... ..
50 ...
... 30 ...
.....
... ...... .....
........
... . .. ... .. .
... ............... .. .... . .... . . .. . ... . . ..... . ........ .. ...
... ................... ....... ............ ............................................ . ...................................................................................... . ...................................... . ....................................... ............................................................... 10 ................................
30 ...
. .. .
........... . ........ . . . . .......
................................... ................ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ...
...................... ... ........ ............
. ... ................... ..... . .... ... ... .
. ..
− 10 .... .......................................................................................................................................................................... .............................. .. . . ..... .
. . ..... ..... . ... . ... .. .. ................ ........................................................................................................................................................................................
10 .... ...................................... . . . .. .. . . . .
...........................
....... ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ... − 30 .... ... ..
..... ...
.
− 10 − 50
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
15 τ5 [Nm] 15 τ6 [Nm]
............ ....... .. ......
................... .. ........
................................................ ................... ....
10 ................ ......... ... ..
......................................................................... ..........................
.................................................................................
10
. . ...... . . .. ... .. .
........................................................................................................................ ...........
.
. .. . . ...... . .
.................................................................................................................. . .. ..................................
5 .......................................................................... ............................. ..... ......... ..
................................................................................................................................................................................................................... ................... .... ........... ... ..... ...... .... . ............................. .... ....... . .......... .. . .
5
.. . . .. .... . . . . . . ... ..... . .. . ..... . . .. .. . .............. ........... ...... ........................................................................................................................................
..................................................................................................... ... ....... ....................................... ........................................ .... ............... ....... . .. ................. ........ .......... ..... . ... . .. .
...... .................................. .... . .......... . .
.............................................................................. ........... ........ ................ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ...
0 ... ... .......... ............... ... .... ........
. .. ......... . ........ .. ...... .....
... 0 ....... ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ...
− 15 − 15
0 2 4 6 8 10 0 2 4 6 8 10
t [s] t [s]
15 τ7 [Nm]
...
...
..
10 ...
...
...
...
..
...
5 .......
.................. . ..
... . ................................ .. ....... .. .. . . .. .... . .. .
.. ... . . .. .. ......... . ......................... ...................................................................................................................................................................................................
0 ....... ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ........ ...
−5
− 10
− 15
0 2 4 6 8 10
t [s]
Arimoto, S. & Miyazaki, F. (1984). Stability and robustness of PID feedback control for robot
manipulators of sensory capability. In: Robotics Researches: First International
Symposium, M. Brady and R.P. Paul (Eds.), pp. 783-799, MIT Press.
Arimoto, S.; Naniwa, T. & Suzuki, H. (1990). Asymptotic stability and robustness of PID local
feedback for position control of robot manipulators. Proceedings of the International
Conference on Automation Robotics and Computer Vision, Singapore, June 1990.
Camarillo, K.; Campa, R.; Santibañez, V. & Moreno J. (2008). Operational space control of
industrial robots using their own joint velocity PI controllers: Stability analysis and
experiments. Robotica, Vol 26, pp. 729-738.
Cervantes, I. & Alvarez-Ramirez, J. (2001). On the PID tracking control of robot
manipulators. Systems and Control Letters, Vol. 42, pp. 37-46.
Choi, Y. & Chung, W. K. (2004). PID trajectory tracking control for mechanical systems.
Springer–Verlag, 2004.
Colbaugh, R.; Barany, E. & Glass, K. (1997) Global regulation of uncertain manipulators
using bounded controls. Proceedings of the IEEE International Conference on Robotics
and Automation, Albuquerque, NM, April 1997.
Craig, J. J. (1998). Adaptive Control of Mechanical Manipulators, Addison–Wesley, 1998.
Dixon, W. E. (2007). Adaptive regulation of amplitude limited robot manipulators with
uncertain kinematics and dynamics. IEEE Transactions on Automatic Control, Vol. 52,
No. 3, pp. 488–493.
Dixon, W. E.; de Queiroz, M. S.; Zhang, F. & Dawson, D. M. (1999). Tracking control of robot
manipulators with bounded torque inputs. Robotica, Vol. 17, pp. 121–129.
Gorez, R. (1999). Globally stable PID-like control of mechanical systems. Systems and Control
Letters, Vol. 38, pp. 61–72.
Hernandez-Guzman, V.; Santibañez V. & Silva-Ortigoza R. (2008). A new tuning procedure
for PID control of rigid robots. Advanced Robotics, Vol. 22, pp. 1007–1023.
Higuchi, M.; Kawamura, T.; Kaikogi, T.; Murata, T. & Kawaguchi, M. (2003). Mitsubishi clean
room robot. Mitsubishi Heavy Industries, Ltd., Technical Review, Vol. 40, No. 5, 2003.
Horn, R. A. & Johnson, C. R. (1985). Matrix Analysis, Cambridge University Press.
Kawamura, S.; Miyasaki, F. & Arimoto, S. (1988). Is a local linear PD feedback control law
effective for trajectory tracking of robot motion?. Proceedings of the IEEE Conference
on Robotics and Automation, Philadelphia, PA., March 1988.
Kelly, R. (1995a). Regulation of robotic manipulators: Stability analysis via the Lyapunov’s
first method. Technical report, CICESE, Ensenada, Mexico.
Kelly R. (1995b). A tuning procedure for stable PID control of robot manipulators. Robotica,
Vol. 13, No. 2, pp. 141-148.
Kelly R. (1998). Global positioning of robot manipulators via PD control plus a class of
nonlinear integral actions. IEEE Transactions on Automatic Control, Vol. 43, No. 7, pp.
934–938.
Kelly R. & Moreno, J. (2001). Learning PID structures in an introductory course of automatic
control. IEEE Transactions on Education, Vol. 44, No. 4, pp. 373-376.
Kelly, R. & Santibañez, V. (1996). A class of global regulators with bounded control actions
for robot manipulators. Proceedings of the IEEE Conference on Decision and Control,
Kobe, Japan, December 1996.
Kelly, R.; Santibañez, V. & Loría, A. (2005) Control of Robot Manipulators in Joint Space,
Springer–Verlag, 2005.
Khalil, H. (2002). Nonlinear Systems, Prentice Hall, 2002.
Koditschek, D. (1984). Natural motion for robot arms. Proceedings of the IEEE Conference on
Decision and Control, Las Vegas, NV, December 1984.
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 243
Santibañez, V. & Kelly, R. (2001). Global asymptotic stability of bounded output feedback
tracking control for robot manipulators, Proceedings of the IEEE International
Conference on Decision and Control, Orlando, FL, December 2001.
Santibañez, V.; Kelly, R.; Zavala-Rio, A. & Parada, P. (2008). A new saturated nonlinear PID
global regulator for robot manipulators, Proceedings of the 17th IFAC World Congress,
Seoul, Korea, July 2008.
Spong, M. & Vidyasagar, M. (1989). Robot Dynamics and Control, John Wiley and Sons, 1989.
Sun, D.; Hu, S.; Shao, X. & Liu, C. (2009), Global stability of a saturated nonlinear PID
controller for robot manipulators. IEEE Transactions on Control Systems Technology,
Vol. 17, No. 4, pp. 892–899.
Teel, A. R. (1992). Global stabilization and restricted tracking for multiple integrators with
bounded controls. Systems and Control Letters, Vol. 18, No. 3, pp. 165–171.
Wen, J. T. & Murphy, S. (1990). PID control for robot manipulators, CIRSSE Document 54,
Rensselaer Polytechnic Institute, May 1990.
Zavala-Rio, A.; Aguinaga-Ruiz, E. & Santibanez, V. (2010). Global trajectory tracking
through output feedback for robot manipulators with bounded inputs. Asian
Journal of Control. To appear.
Zavala-Rio, A. & Santibañez, V. (2006) Simple extensions of the PD–with–gravity–
compensation control law for robot manipulators with bounded inputs, IEEE
Transactions on Control Systems Technology, Vol. 14, No. 5, pp. 958-965.
Zavala-Rio, A. & Santibañez, V. (2007) A natural saturating extension of the PD–with–
desired–gravity compensation control law for robot manipulators with bounded
inputs, IEEE Transactions on Robotics, Vol. 23, No.2, pp. 386-391.
Zergeroglu, E.; Dixon, W.; Behal, A. & Dawson, D. (2000). Adaptive set–point control of
robotic manipulators with amplitude–limited control inputs, Robotica, Vol. 18, pp.
171–181.
Appendix A
In this section we prove that (26) has a unique solution q = h(x) ∈Rn, provided that
⎛ ∂g ( q ) ⎞
k p > k g ≥ n ⎜ max i ⎟ where i = 1, 2,… n and j = 1, 2,… n.
i i ⎜ q , j ∂q j ⎟
⎝ ⎠
To this end, notice that we can rewrite (26) as
⎡ g1 ( q ) − g 1 ( q d ) − x 1 ⎤
⎢ k p1 ⎥
⎢ ⎥
⎡ 1⎤ ⎢
q ⎥
⎢ ⎥ ⎢ 2 g ( q ) − g (
2 d q ) − x 2
q ⎥
⎢ ⎥ ⎢
2 k p2 ⎥
q =⎢ ⎥=⎢ ⎥ = f ( q , q d ). (62)
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
⎢q ⎥ ⎢ ⎥
⎣ n⎦
g
⎢ n ( q ) − g (
n d q ) − x n⎥
⎢ k pn ⎥
⎣ ⎦
If f ( q , qd) satisfies the Contraction Mapping Theorem (Kelly et al., 2005; Khalil, 2002), then
(62) has a unique solution q *. Considering this, we have
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 245
⎡ g1 ( q d − v ) − g1 ( q d ) − x1 − g1 ( q d − w ) + g1 ( q d ) + x1 ⎤
⎢ k p1 ⎥
⎢ ⎥
⎢ g ( q − v) − g ( q ) − x − g ( q − w) + g ( q ) + x ⎥
2 d 2 d 2 2 d 2 d 2
⎢ ⎥
f (v, q d) − f (w, q d) = ⎢ k p2 ⎥
⎢ ⎥
⎢ ⎥
⎢ gn ( q d − v ) − gn ( q d ) − xn − gn ( q d − w ) + gn ( q d ) + xn ⎥
⎢ ⎥
⎢⎣ k pn ⎥⎦
⎡ g1 ( q d − v ) − g1 ( q d − w ) ⎤
⎢ k p1 ⎥
⎢ ⎥
⎢ g ( q − v) − g ( q − w) ⎥
⎢ 2 d 2 d
⎥
= ⎢ k p2 ⎥ (63)
⎢ ⎥
⎢ ⎥
⎢ gn ( q d − v ) − gn ( q d − w ) ⎥
⎢ ⎥
k pn
⎣⎢ ⎦⎥
Using Theorem 1, we can rewrite gi(qd − v) − gi(qd − w) as
∂gi ( z ) ∂g i ( z ) ∂g i ( z )
g i ( q d − v ) − gi ( q d − w ) = ( w 1 − v1 ) + ( w 2 − v2 ) + … + ( wn − vn ) (64)
∂z1 z =ξ i
∂z 2 z =ξ i
∂zn z =ξ i
where ξi is a vector on the line segment that joins vectors w and v, and, by substituting in
(63), we obtain
⎡ g1 ( q d − v ) − g 1 ( q d − w ) ⎤
⎢ k p1 ⎥
⎢ ⎥
⎢ g ( q − v) − g ( q − w) ⎥
2 d 2 d
⎢ ⎥
f ( v ,q d ) − f ( w ,q d ) = ⎢ k p2 ⎥
⎢ ⎥
⎢ ⎥
⎢ gn ( q d − v ) − g n ( q d − w ) ⎥
⎢ ⎥
⎢⎣ kpn ⎥⎦
⎡ ∂g 1 ( z ) ∂g 1 ( z ) ∂g 1 ( z ) ⎤
⎢ ( w 1 − v1 ) + ( w 2 − v2 ) + … + ( w n − vn ) ⎥
⎢ ∂z1 z =ξ 1
∂z 2 z =ξ 1
∂ zn
z =ξ 1 ⎥
⎢ ⎥
⎢ k p1 ⎥
⎢ ⎥
⎢ ∂g 2 ( z ) ∂g ( z ) ∂g ( z ) ⎥
⎢ ∂z ( w 1 − v1 ) + 2 ( w2 − v2 ) + … + 2 ( wn − vn ) ⎥
∂ z ∂ z
⎢ 1 z =ξ 2 2 z =ξ 2 n z =ξ 2 ⎥
= ⎢ ⎥
⎢ k p2 ⎥
⎢ ⎥
⎢ ⎥
⎢ ∂g ( z ) ∂g n ( z ) ∂g n ( z ) ⎥
⎢ n ( w 1 − v1 ) + ( w2 − v2 ) + … + ( w n − vn ) ⎥
⎢ ∂z1 z =ξ n
∂ z 2 z =ξ n
∂ z n z =ξ n ⎥
⎢ ⎥
⎢⎣ k pn ⎥⎦
246 Advanced Strategies for Robot Manipulators
= A[ w − v ]
≤ A w−v
where
If &A& < 1, then f ( q , qd) fulfills the Contraction Mapping Theorem. Now notice that
A = λmax { AT A} . (66)
∂gi ( z )
By defining = Δgij , we have that
∂zj
z =ξi
⎡ 2
Δg11 Δg2 Δg2 Δg12 Δg11 Δg21Δg22 Δgn1Δgn2 Δg11Δg1n Δgn1Δgnn ⎤
Δg21Δg2n
⎢ + 21 + … + n21 + +…+ … + +…+ ⎥
⎢ kp12 kp22 kpn kp12 kp22 kpn2 kp12 kp22 kpn2 ⎥
⎢ 2 2 ⎥
⎢ Δg12Δg11 + Δg21Δg22 + … + Δgn1Δgn2 Δg12 Δg22 Δgn22 Δg12 Δg1n Δg22Δg2n Δgn2Δgn2 ⎥
+ +…+ … + +…+
= ⎢ kp12 kp22 kpn2 kp12 kp22 kpn2 kp12 2
kp2 kpn ⎥⎥
2
⎢
⎢ ⎥
⎢ ⎥
⎢ Δg11Δg1n Δg21Δg2n Δg Δg Δg12 Δg1n Δg22 Δg2n Δgn2 Δgn2 Δg12n Δg22n 2
Δgnn ⎥
⎢ 2
+ 2
+ … + n1 2 nn + +…+ … + +…+ ⎥
⎣ kp1 kp 2 kpn kp12 kp22 kpn2 2
kp1 2
kp2 2
kpn ⎦
Considering (5) and (27), we have that each element in ATA fulfills
1
| AT A(i , j )|< . (67)
n
Now, knowing that the eigenvalues of any matrix B, where bij denotes its ij-th element, fulfill
(Horn & Johnson, 1985):
On Saturated PID Controllers for Industrial Robots: The PA10 Robot Arm as Case of Study 247
⎡ ⎤
|λk |≤ n ⎢ max{|bij |} ⎥ ∀ k = 1,… , n
⎣⎢ i , j ⎦⎥
we obtain that
⎡ ⎤ ⎡1⎤
λk { AT A} ≤ λmax { AT A} ≤ n ⎢max{| AT A(i , j )|} ⎥ < n ⎢ ⎥ = 1
⎣⎢ i, j n ⎦⎥ ⎣ ⎦
T
and consequently we have that A = λmax { A A} < 1 = 1. Therefore, we get &f (v, qd)−
f (w, qd)& ≤ &A&&w −v& where &A& is strictly smaller than the unity. Hence, we have that (26)
has a unique solution q = h(x) ∈ Rn provided that:
⎛ ∂g ( q ) ⎞⎟
k p > n ⎜ max i where i = 1, 2,… n and j = 1, 2,… n.
i ⎜ q , j ∂q j ⎟
⎝ ⎠
Appendix B
The positive definiteness and radial unboundedness analysis of W( q , q ) is dealt in this
appendix. The Lyapunov function candidate W( q , q ) can be written as:
1 T
W ( q ,q ) = q M( q )q + W1 ( q )
2
with
n qi
W1 ( q ) = ∑∫
i =1
0
⎡ Sat ⎡ Sat( k r +x +g ( q ))⎤−g ( r )⎤ dr
⎢⎣ ⎣⎢ pi i i i d ⎥ i i ⎥ i
⎦ ⎦
n h1i ( x )
−∑∫ i =1
0
⎡ Sat⎡ Sat( k r +x +g ( q ))⎤−g (r )⎤dr .
⎣⎢ ⎣⎢
pi i i i d ⎥ i i ⎥ i
⎦ ⎦
(68)
where
g1 (r1 ) = g1 ( qd − r1 , qd ,… , q d )
1 2 n
g2 ( r2 ) = g2 (qd − q1 , qd − r2 ,… , q d )
1 2 n
gn (rn ) = gn ( qd − q1 , qd − q 2 ,… , q d − rn )
1 2 n
Notice that the positive definiteness and the radial unboundedness of W1( q , q ) implies the
positive definiteness and the radial unboundedness of W( q , q ).
Let us define a region β1 where the saturation functions of the P and PI parts of the
controller work in their linear section, such that:
Notice that, in this region we have that Sat[Sat( k pi qi + xi + gi(qd))] = k pi k pc qi + xi + gi(qd). For
i
this case, we will show that W1( q ) is a strictly convex function with a unique minimum
248 Advanced Strategies for Robot Manipulators
point at q = h1(x). To this end, we evaluate W1( q ) at q = h1(x) and obtain its gradient and
Hessian:
a) W1 ( q )|q = h ( x ) can be written as:
i
n qi
W1 ( q ) q = h
1(x)
= ∑∫
i =1
0
⎡
⎣⎢
k p ri +xi +gi ( q d )−gi (ri )⎤dri
i ⎦⎥
q = h1 ( x )
n h1i ( x )
−∑∫ i =1
0
⎡ k r +x +g ( q )−g ( r )⎤dr .
⎢⎣ pi i i i d i i ⎥⎦ i
= 0
b) The gradient of W1( q ) with respect to q is given by:
∂W1 ( q )
= K p q + x + g ( q d ) − g( q d − q ) = 0. (69)
∂q
Under assumption (27), and by using the Contraction Mapping Theorem, (69) has a
unique solution q = h1(x), that is, a unique critical point.
c) The Hessian of W1( q ) with respect to q is given by:
∂ 2 W1 ( q ) ∂ g( q d − q )
= Kp − =0
∂q 2 ∂q
which is a positive definite function for all q ∈ Rn provided that (Hernandez-Guzman
et al., 2008):
n
∂gi ( q )
kp >
i ∑maxj =1
q ∂qi
(70)
∂W1 ( q )
∂q ⎣ (
= Sat ⎡⎢Sat K p q + x + g ( q d ) ⎤⎥ − g( q d − q )
⎦ ) (71)
which, under Assumption 2, will have a unique critical point for all qi ∈R with i = 1,2, . . .
,n, and hence, the minimum point of W1( q ) results to be a global minimum point q = h1(x).
In order to prove radially unboundedness of W1( q ), it is possible to prove that outside of
the region β1 the function W1( q ) can be lower bounded by straight lines of the type
Wβ = kβ 1 qi − h1 ( x ) − ci
i i i
where kβ 1i and ci are suitable constants. So, | qi − h1i (x)| →∞ implies Wβi → ∞ for i = 1,2, .
. . ,n; therefore W( q )→ ∞ as & q & → ∞, which proves that W1( q ) is radially unbounded.
11
1. Introduction
Robot manipulators, in general, are required to have the same number of actuators as the
number of joints to obtain full control. In the case of under-actuated robots, this condition is
not satisfied which make the behavior of that class of robots very difficult to be predicted.
Under-actuated robots can be a better design choice for robots in space and other industrial
applications, their advantages over fully actuated robots led to many studies to predict their
behavior (Yu et al., 1998; Berkemeier & Fearing, 1999; Spong, 1995; Ono et al., 2001;
Nakanishi et al., 2000; Funda et al., 1996; Luca et al., 2000; Luca & Oriolo, 2002; Arai & Tachi,
1991; Mukherjee & Chen, 1993;Yu et al., 1993;Bergerman et al., 1995; Mahindrakar et al.,
2006; Muscato, 2006; Begovich et al., 2002). As a first advantage, a light-weight and low
power consumption manipulator can be made. This feature is required in low cost
automation and space robots. Second, they can easily overcome actuator failure due to
unexpected accident. The under-actuated manipulator could be the model of the direct drive
manipulator that has some failed joints; such fault-tolerant behavior is highly desirable for
robots in remote or hazardous environments (Yu et al., 1998). Other interesting applications
include the Acrobot (Berkemeier & Fearing, 1999; Spong, 1995), the gymnast robots (Ono et
al., 2001), the brachiating robots (Nakanishi et al., 2000), and surgical robots (Funda et al.,
1996).
The mathematical complexity and wide variety of applications have kept under-actuated
robots an area of open research. (Luca et al., 2000; Luca & Oriolo, 2002) have investigated the
behavior of a 2R manipulator moving in a horizontal plane with a single actuator at the first
joint, neglecting joint friction which is not easy to achieve in real world as it involves high
manufacturing cost. Trying to overcome that problem, some researchers have implemented
additional equipments such as breaks at the passive joint (Arai & Tachi, 1991; Mukherjee &
Chen, 1993; Yu et al., 1993; Bergerman et al., 1995). In this case, the brake can generate
torque that means after all that kind of systems is considered some kind of actuator. So, it
will be difficult to consider that robot as an under-actuated manipulator.
Motivated by this problem, (Yu et al., 1998) have investigated the dynamic characteristics of
a two-link manipulator in view of global motion including joint friction by proposing a
mathematical model; they have found that the manipulator can be positioned if the friction
250 Advanced Strategies for Robot Manipulators
acts on the passive joint. In this case, any additional equipment such as brakes is not needed
in positioning all the joints to desired position. Their results were verified using numerical
simulation. Later on, (Mahindrakar et al., 2006) have presented a mathematical model for a
two-link under-actuated manipulator wherein the motion of the system was confined to a
horizontal plane; their proposed dynamic model takes into account the frictional forces
acting on the joints. Results obtained were also verified through numerical simulation.
Many attempts to solve the problem have been found in the literature. Yet, solutions
proposed are still lack of generality and systematization. To overcome this problem,
artificial intelligence was introduced for prediction and making robot systems able to
attribute more intelligence and high degree of autonomy.
Appling fuzzy logic to under-actuated robots (as an artificial intelligence method), there
were few studies in recent past (Muscato, 2006; Begovich et al., 2002).
Although the results presented were promising, these results cannot be generalized to other
systems, because they only came from practical considerations. Besides, despite the fact that
unlike most learning control algorithms, multiple trials are not necessary for the robot to
learn the desired trajectory. A major drawback was that Fuzzy Logic based approaches only
remembers the most recent data points introduced (Graca & Gu, 1993). Gleaning the
learning abilities of genetic algorithms GA (as another method of artificial intelligence) to
solve the problem was an alternative. Blending of GA with fuzzy rules, in order to capture
the hidden nonlinearities of the system will be useful in developing any learning techniques.
(Lee & Zak, 2002) have presented the design criterion of a GA based neural fuzzy controller
for an anti-break system. As it has been seen, each of the previously mentioned techniques
has their own drawbacks. To overcome this problem researchers have recommended neural
networks so that it would remember the trajectories as it traversed them (Graca & Gu, 1993).
Artificial neural networks (ANNs) have been widely used for their extreme flexibility due to
its learning ability and the capability of non-linear function approximation. Their ability to
learn by example makes them very flexible and powerful. ANNs while implemented on
computers are not programmed to perform specific tasks. Instead, they are trained with
respect to data sets until they learn the patterns presented to them. Once they are trained,
new patterns may be presented to them for prediction or classification (Kalogirou, 2001;
Hasan et al., 2006). Therefore, ANNs have been intensively used for solving regression and
classification problems in many fields. A number of realistic approaches have been
proposed and justified for applications to robotic systems (Balakrishnan et al., 2000; Kim et
al., 2002; Köker, 2005; Hasan et al., 2007; Al-Assadi et al., 2007; Siqueira & Terra, 2009).
In real world application, no physical property such as the friction coefficient can be exactly
derived. Besides, there are always kinematics uncertainties presence in the real world such
as ill-defined linkage parameters and backlashes in gear trains (Hasan et al., 2009; Hasan et
al., 2010). In this paper, and to overcome whichever uncertainty presented in the real world,
data were recorded experimentally from sensors fixed on each joint for a horizontal two-link
under-actuated robot.
The developed learning algorithm is based on weight adaptation of the network, by
minimizing the tracking error after each iteration process. This scheme does not require any
prior knowledge of the dynamic model of the system being controlled. The basic idea of this
concept is the use of the ANNs to learn the characteristics of the robot system rather than to
specify an explicit robot system model, so, every uncertainty in the system will be counted
for. Experimental trajectory tracking has shown the ability of the proposed approach to
Real-Time-Position Prediction Algorithm for Under-actuated
Robot Manipulator Using of Artificial Neural Network 251
overcome the disadvantages of using some schemes like the Fuzzy Learning for example
that only remembers the most recent data sets introduced, as the literature has shown.
Link 2
q2
r2
Link 1
•• •
M(q ) q + h( q , q ) = τ , (1)
• ••
Where q and q are the generalized velocities and accelerations respectively. M (q ) is the
inertia matrix, which is symmetric and positive definite. The centripetal and Coriolis terms
•
are collected in the vector h(q , q ) . The vector h contains terms purely quadratic in the
velocities; gravity terms are absent since it assumed that the manipulator moves in a
horizontal plane.
Define the following constants:
252 Advanced Strategies for Robot Manipulators
The equations of motion accounting for the Coulomb plus viscous friction at the joints become:
•• •• • •
m11 q 1 + m12 q 2 + h1 = τ − SGN (q 1 )F1 − b1 q 1 , (2)
•• •• • •
m21 q 1 + m22 q 2 + h2 = −SGN (q 2 )F2 − b2 q 2 , (3)
Where,
m11 = c1 + c 2 + 2c 3Cosq 2 , m12 = c 2 + c 3Cosq2 ,
m21 = m12 , m22 = c 2 ,
• • •2 •2
h1 = −c 3 (2 q 1 q 2 + q 2 )Sinq2 , h2 = c 3 q 1 Sinq2 .
•
The Fi , bi q i , i = 1, 2 represent the Coulomb and viscous friction forces respectively. The set-
valued signum function is defined as:
⎧
⎪ {1} if x > 0,
⎪
SGN ( x ) ⎨ { −1} if x < 0, (4)
⎪
⎪[ −1,1] if x = 0.
⎩
The above shown function suffers from the fact that the solution does not give a clear
indication on how to select an appropriate solution from the several possible solutions for a
particular arm configuration.
3. Experiment procedure
In this section, the real time implementation of the experimentally collecting data procedure
is discussed. Different methods for collecting data have been found in the literature. Using a
pre-specified model, using a trajectory planning method or using a simulation program for
this purpose are examples for some of these methods. However, there are always kinematics
uncertainties presences in the real world such as ill-defined linkage parameters, links
flexibility and backlashes in gear train, in this approach, data were recorded directly from
sensors fixed on each joint, so every uncertainty in the dynamics of the system will be
counted for.
The manipulator used is shown in Figure 2, which is actuated only at the first joint. The
actuator used is a DC motor connected to the first link through a gearbox with a reduction
ratio of 100:1, while the second joint is passive.
Each of the joints have an encoder attached to it, in order to measure the rotation angle and
there are torque sensors between the motor output shaft and the robot joint to measure the
torque being supplied by the motor. Joints encoders are connected to a computer equipped
with MATLAB software through a data acquisition card. The robot arms were made of an
aluminum square section beam to ensure a resisting to bending lightweight arm. Length of
arms are l1 = 40 cm and l2 = 30 cm respectively. The control circuit is made up of computer
Real-Time-Position Prediction Algorithm for Under-actuated
Robot Manipulator Using of Artificial Neural Network 253
Fig. 2. The robot system used showing the computer, the data acquisition card and the robot
arms
with the MATLAB software connected to the robot through a data acquisition card that
acquires the motion data of the two links. Input signal is generated by the MATLAB
software and transferred to the motor using the electrical board, and the robot response is
recorded using the MATLAB software.
A Sinusoidal excitation signal was applied to the actuator causing different torque to the
joints and the dynamic coupling effect was moving the passive joint correspondently. As a
standard signal generated by the MATLAB, Sinusoidal excitation signal, was chosen in
order to cause a robot motion that covers the whole working cell rather than being a
specified signal to perform a pre-defined trajectory.
When the excitation signal is given, the motion of the active joint and the corresponding
response of the passive joint that can be seen in Figures 3 and 4 respectively were recorded
in order to be used in the training process of the ANN.
Fig. 3. Trajectory of the active joint when the excitation signal was applied
Wij
Wj
k
Torque
Angular Displacement
Angular Displacement
First Joint
Second Joint
Time
A C
Input Layer Output Layer
B
Hidden Layer
O j = f (sumB ) (6)
1
f (O j ) = −O j
(7)
1+ e
As a nonlinear activation function.
However, any input-output function that possesses a bounded derivative can be used in
place of the sigmoid function.
If there is a fixed, finite set of input-output pairs, the total error in the performance of the
network with a particular set of weights can be computed by comparing the actual and the
desired output vectors for each presentation of an input vector.
Error at any output unit eK in the layer C can be calculated by: -
eK = dK − OK (8)
Where dK is the desired output for that unit in layer C and OK is the actual output produced
by the network .the total error E at the output can be calculated by: -
256 Advanced Strategies for Robot Manipulators
1
E= ∑ ( dK − OK )2
2 K
(9)
ΔW JK = ηδ K OJ (10)
Where η is the learning rate parameter, and the error δK at an output layer unit K is given by: -
δ K = OK (1 − OK )( dK − OK ) (11)
δ J = OJ (1 − OJ )∑ δ K WJK (12)
K
Using the generalize delta rule to adjust weights leading to the hidden units is back
propagating the error-adjustment, which allows for adjustment of weights leading to the
hidden layer neurons in addition to the usual adjustments to the weights leading to the
output layer neurons.
A back propagation network trains with two step procedure, the activity from the input
pattern flows forward through the network and the error signal flows backwards to adjust
the weights using the following equations: -
W JK = W JK + ηδ K OJ (14)
Until for each input vector the output vector produced by the network is the same as (or
sufficiently close to) the desired output vector (Kalogirou, 2001; Hasan et al., 2006). Number
of hidden neurons and the learning factor are determined by trial and error.
5. Results
A supervised feed forward ANN was designed using C programming language to learn the
system behavior over its workspace. The network consists of input, output and one hidden
layer, the input vector for the network consists of the angular displacement, the torque
applied at the active joint (first joint) and the time interval, while the output vector was the
angular position of the passive joint (second joint). As can be seen in Figure 5, every neuron
in the network is fully connected with each other, sigmoid transfer function was used to be
Real-Time-Position Prediction Algorithm for Under-actuated
Robot Manipulator Using of Artificial Neural Network 257
the activation function, and generalized backpropagation delta learning rule (GDR)
algorithm was used in the training process. All control datasets values had been scaled
individually so that the overall difference in the dataset was maximized.
Training data were divided into 50 input–output sets, which covered the entire work cell of
the manipulator. To build the control knowledge, a training process was carried out using
the experimentally obtained data. The network was trained by presenting several target
points that the network had to learn, number of neurons in the hidden layer was set to 25
with a constant learning factor of 0.9 by trial and error. Figure 6 shows the building
knowledge process for the system.
To verify the success of the algorithm, the predicted values of the passive joint were
compared to the experimentally collected data. The average absolute error was 4.9% after
100,000 Iterations. Figure 7 graphically shows the trajectory tracking of the passive joint,
Results obtained show that the design network is capable of learning and predicting the
position of the passive joint successfully.
60
50
40
Error %
30
20
10
0
0 10 20 30 40 50 60 70 80 90 100
Iteration X 1000
200 Desired
Predicted
150
Angular Position ( Deg. )
100
50
0
0 10 20 30 40 50
-50
Time ( Sec. )
Fig. 7. Predicted trajectory tracking of the passive joint
Results obtained have shown the ability of the network to predict the trajectory of the
passive joint, that is positioned by the dynamic coupling of the active joint, overcoming the
disadvantages of using some schemes like the Fuzzy Learning for example that only
remembers the most recent data sets introduced.
Backpropagation algorithm was used as a learning algorithm with sigmoid transfer function
as an activation function in all neurons, For further research, we recommend that a different
learning algorithm, different activation function and/or different number of hidden layers
to be used in order to achieve, if possible, a better response in terms of precision and
iteration.
7. References
Al-Assadi, H.M.A.A., Hamouda, A.M.S., Ismail, N. and Aris, I., An Adaptive Learning
Algorithm for Controlling a Two-Degree-Of-Freedom Serial Ball-and-Socket
Actuator. Proceedings of the IMechE Part I: Journal of Systems and Control
Engineering, 2007,221(7): 1001-1006.
Arai, H. and Tachi, S., Position Control of a Manipulator With Passive Joints Using Dynamic
Coupling. IEEE Trans. On Robotics and Automation, 1991,7(4): 528-534.
Balakrishnan, S., Popplewell, N. and Thomlinson, M., Intelligent Robotic Assembly.
International Journal of Computers and Industrial Engineering, 2000, 38: 467-478.
Begovich, O., Sanchez, E.N., and Maldonado, M., Takagi–Sugeno fuzzy scheme for real-time
trajectory tracking of an under-actuated robot. IEEE Transactions on Control
Systems Technology, January 2002, 10(1): 14–20.
Bergerman, M., Lee, C. and Xu, Y., Experimental Study of an Underactuated Manipulator.
Proc. 1995 IEEE/RSJ Int. Conf. On Intelligent Robotics and Systems, 1995,2:317-322.
Berkemeier, M. D. and Fearing, R. S., Tracking fast inverted trajectories of the underactuated
Real-Time-Position Prediction Algorithm for Under-actuated
Robot Manipulator Using of Artificial Neural Network 259
acrobot, IEEE Transactions on Robotics and Automation, Aug. 1999, 15(4): 740 –
750.
Funda, J., Taylor, R., Eldridge, B., Gomory, S. and Gruben, K., Constrained Cartesian motion
control for teleoperated surgical robots. IEEE Transactions on Robotics and
Automation, June 1996, 12(3): 453 – 465.
Graca, R.A. and Gu, Y., A Fuzzy Learning Algorithm for Kinematics Control of a Robotic
System. Proceeding of the 32nd Conference on Decision and Control. San Antonio,
Texas. December, 1993: 1274-1279.
Hasan, A. T., Hamouda, A.M.S., Ismail, N, Aris, I. and Marhaban, M.H., Trajectory Tracking
for a Serial Robot Manipulator Passing Through Singular Configurations Based on
the Adaptive Kinematics Jacobian Method. Proceedings of the IMechE Part I:
Journal of Systems and Control Engineering, 2009, 223(3): 393-415.
Hasan, A. T., Ismail, N, Hamouda, A.M.S., Aris, I., Marhaban, M.H. and Al-Assadi,
H.M.A.A., Artificial Neural Network-Based Kinematics Jacobian Solution for Serial
Manipulator Passing Through Singular Configurations. International Journal of
Advanced in Engineering Software, 2010, 41:359-367.
Hasan, A.T., Hamouda, A.M.S., Ismail, N., and Al-Assadi, H.M.A.A., An adaptive-learning
algorithm to solve the inverse kinematics problem of a 6 D.O.F serial robot
manipulator. Journal of Advances in Engineering Software, 2006, 37(7): 432-438.
Hasan, A.T., Hamouda, A.M.S., Ismail, N., and Al-Assadi, H.M.A.A., A New Adaptive
Learning Algorithm for Robot Manipulator Control. Proceeding of the IMechE, Part
I: Journal of System and Control Engineering, 2007, 221(4): 663-672.
Kim, I.S., Son, J.S., Park, C.E. Lee, C.W., and Prasad, Y. K.D.V., A Study on Prediction of
Bead Height in Robotic Arc Welding Using a Neural Network. International
Journal of Materials Processing Technology, 2002,130–131: 229–234.
Köker, R., Reliability-based approach to the inverse kinematics solution of robots using
Elman’s networks. International Journal of Engineering Applications of Artificial
Intelligence, 2005, 18: 685-693.
Lee, Y. and Zak, S. H., Designing a genetic neural antilock-break system controller. IEEE
Transactions on Evolutionary Computation, 2002, 6(2): 198 – 211.
Luca, A.D. and Oriolo, G., Trajectory Planning and Control for Planar Robots With Passive
Last Joint. International Journal of Robotics Research, 2002, 21:575-590.
Luca, A.D., Mattone, R. and Oriolo, G., Stabilization of an Underactuated Planar 2R
Manipulator. International Journal of Robust and Nonlinear Control, 2000,24:181-
198.
Mahindrakar, A.D., Rao, S. and Banavar, R.N., Point-to Point Control of a 2R Planar
Horizontal Underactuated Manipulator. International Journal of Mechanism and
Machine Theory, 2006, 41:838-844.
Mukherjee, R. and Chen, D., Control of Free-Flying Underactuated Space Manipulators To
Equilibrium Manifolds. IEEE Trans. On Robotics and Automation, 1993, 9(5): 561-
570.
Muscato, G., Fuzzy Control of an Underactuated Robot With a Fuzzy Microcontroller.
International Journal of Microprocessors and Microsystems, 1999,23:385-391.
Nakanishi, J. , Fukuda, T. and Koditschek, D., A brachiating robot controller. IEEE
Transactions on Robotics and Automation, April 2000,16(2): 109 – 123.
260 Advanced Strategies for Robot Manipulators
Ono, K., Yamamoto, K. and Imadu, A. Control of giant swing motion of a two-link
horizontal bar gymnastic robot. Advanced Robotics, 2001, 15(4): 449 – 465.
Siqueira, A. A. G. and Terra, M. H., Neural Network-Based H ∞ Control for Fully Actuated
and Underactuated Cooperative Manipulator. International Journal of Control
Engineering Practice, 2009, 17:418-425.
Kalogirou, S. A., Artificial Neural Networks In Renewable Energy Systems Applications: a
review. International Journal of Renewable and Sustainable Energy Reviews. 2001,
5: 373-401.
Spong, M. W., The swing up control problem for the acrobat. IEEE Control Systems
Magazine, Feb. 1995, 15(1): 49 – 55.
Yu, K-H, Shito, Y. and Inooka, H., Position Control of an Underactuated Manipulator Using
Joint Friction. International Journal of Non-Linear Mechanics, 1998, 33(4): 607- 614.
Yu, K-H., Takahashi, T. and Inooka, H. ,Dynamics and Motion Control of a Two-Link Robot
Manipulator With a Passive Joint. Proc. 1995 IEEE/RSJ Int. Conf. On Intelligent
Robots and Systems, 1995, 2:311-316.
12
1. Introduction
Dynamical systems are often nonlinear in nature. It motives people to explore various
theoretical nonlinear analysis and control design tools, of which constructive nonlinear
design methods are the most celebrated ones. However, applying a constructive tool faces
up a big hurdle that the tool deals only with a certain dynamical structure, often not
possessed by the natural dynamics. Nonlinear constructive control designs heavily relies on
the identification of a particular structure via coordinate transformation and control
transformation. To be realistic, these theoretical tools are not general to all of the nonlinear
systems. Here, a challenging benchmark example–a four degrees of freedom inverted
pendulum under the influence of a planar force–is considered that is nonlinear, multiple
input and multiple output, underactuated and unstable. The benchmark is also of practical
interests because it is an abstract of several applications. Three challenging control objectives
are envisaged for the first time in the literature in order to how to apply various cutting-
edge theoretical nonlinear control tools. In fact, the key step of all of the nonlinear designs is
to identify spectral structures– certain “normal” forms. From this aspect, a sequence of
preliminary designs will accompany the existing tools to construct nonlinear controllers,
which is quite different from the linear control designs.
Define a Lagrangian L = K – V where K and V are respectively the kinetic energy and the
potential energy of the benchmark. Applying the Euler-Lagrangian equations
d dL dL
− =Q (2)
dt dq dq
D(q ) ⋅ {
q} + C(q , q ) ⋅ { q i } + G(q ) = Q , (3)
where D(q) is the matrix of inertia, C(q, q ) is the centrifugal and Coriolis matrix and G(q) is
the gravitational matrix. Equation 3 is taken as the mathematical model of the benchmark.
Three models with respect to three sets of generalized coordinates are derived (see Fig. 1)
M.1 The model in q = (x,y, θ,φ) in (Liu, 2006) – θ and φ are the procession and nutation angles
respectively; the model has singular points at φ = . . . , 0,π,2π, . . . but the model is ideal
for the objective of swing-up (e.g., (Albouy & Praly, 2000)); the upper space is defined
by U = {(x,y, θ,φ, x , y , θ , φ ) ∈ R8|– π/2 < φ < π/2};
M.2 The model in q = (x,y, δ, ε) in (Liu et al., 2008a) – δ and ε denote the heading and bank
angles respectively; the model has singular points at δ = π/2,3π/2, . . . and/or ε =
π/2,3π/2, . . . that does not affect the control objectives here; special structures have
been derived from this model (see S.1 and S.2 in the sequel); the upper space is defined
by U = {(x,y, δ, ε, x , y , δ , ε ) ∈ R8| – π/2 < δ < π/2 and – π/2 < ε < π/2};
M.3 The model in q = (x,y,X,Y) in (Liu et al., 2008b) – X and Y are the projection of the center
of mass in the horizontal plane; the model can only represent the case that the
pendulum is either above the horizontal plane or below the plane but it is sufficient to
the control objectives in this paper; the description of the model is technically simpler
than the above two but we cannot ensure that it also implies particular structures as
those derived from M.2; the upper space is defined by U = {(x,y,X,Y, x , y , X , Y ) ∈
R8| X 2 + Y 2 < L} (L is the length of the center of mass to the pivot).
Generally, Equation 3 can be written in a state space form
η = f (η , F , v f ) (4)
where η (q, q ) ∈ U denotes the state vector and Equation 4 is called the nominal dynamics
as vf ≡ 0.
contained in a domain of attraction. Then, the closed loop system is said to yield a
“semi-global” stability region.
PF.2 Exact output tracking – Let (xd(t),yd(t)) for t ∈ (–∞,∞) be a sufficiently smooth desired
curvature in the globally fixed frame with respect to the time variable t. Derive a
feedback control law for F such that the pivot position, denoted by triplet (t,x(t),y(t)), of
the pendulum starting from a set of initial conditions (t0, x(t0),y(t0)) converges to
(t,xd(t),yd(t)) asymptotically, i.e., x(t) – xd(t) →0, y(t) – yd(t) →0 as t → ∞. Meanwhile, the
pendulum is kept in U.
PF.3 Way-point tracking – Let p = {p1, p2..., pn} with pi = (xri,yri ) for i = 1, 2, ..., n be a given
sequence of points on the plane x – y of the globally fixed frame. Associated with each
pi, consider the closed ball Nμi (pi) with center pi and radius μi > 0. Derive a feedback
control law for F such that the pivot (x,y) of the pendulum converges to pn after visiting
the ordered sequence of neighborhood Nμi (pi) for i = 1, 2, ..., (n – 1) while keeping the
pendulum in the upper space U.
solve the associated PDEs. The stable inversion technique (see (Devasia, 1996)) trades
the requirement of solving these general PDES for a specific trajectory. Both tools can
deal with the unstable “zero” dynamics that cannot be dealt with by the conventional
inversion technique.
T.7 Hybrid control1–There is no ultimate definition. It refers to a control system that mixes
discrete parts (e.g., a controller, a supervisor) and continuous parts (e.g., a continuous
plant).
ζ i = Ai ζ i + gi (ξi , u)
for i = 1, 2, 3, 4 (5)
ξ = f (ξ , u),
i i i
1 It does not mean a particular tool or method but a broad class of mixed tools and methods.
268 Advanced Strategies for Robot Manipulators
ω = Aω + Bξ1 + ϕη ( ξ1 , ξ2 , ζ 1 , ζ 2 )
ξ 1 = ξ2 (6)
ξ 2 = u1
ϑ = Aϑ + Bζ 1 + ϕϑ (ξ1 , ξ2 , ζ 1 , ζ 2 )
ζ 1 = ζ 2 (7)
ζ 2 = u2
ω = Auω + ψη (ω ,ϑ , u1 , u2 )
ξ 1 = ξ2 (10)
ξ 2 = u1
ϑ = Asϑ + ψϑ (ω ,ϑ , u1 , u2 )
ζ 1 = ζ 2 (11)
ζ 2 = u2
NC.1 The high-low gain controller (see (Liu et al., 2008a) for PF.1 is designed on the basis of S.1
1
where L ∈ R4×4 is a linear high gain matrix, σ i + 1 λi ⋅ sat( ( Ki + 1ξi + 1 + Γi + 1 vi + 1 )) with
λi
vi+1 = σi+2 (v5 does not necessary to be given as the design is complete, Ki+1 and λi are
associated gain matrices and saturation levels). Nested saturating method (Teel, 1996)
in T.3 is used to design a low gain control part σi+1 at the aid of a linear control design
method–LQR. The controller yields a closed loop system with a “global” stability
region. The design implies the existence of appropriate λi that is related to the domain
of attraction yielded by a linear controller. Practically, λi is found by trails and errors.
ISS (see (Sontag, 2005)) in T.2 is a key analysis tool in both the design and the redesign.
NC.2 The decentralized controller in (Liu et al., 2008c) for PF.1 is designed on the basis of S.2
where L.,. and K.,. are positive scalars, ε ∈ (0,1) is time scaling parameters. The resultant
closed loop system is a hidden singularly perturbed system that can be transformed
into a standard singular perturbation form (slow) x = f ( x , y ), (fast) ε y = h( x , y , ε). A
“strong” Lyapunov function comes with the design and the total stability of the system
is ensured. A “semi-global” stability region (it increases as ε decreases) is yielded by the
closed loop system. The design is heavily relying on T.4 (see (Kokotović, 1986)).
NC.3 The controller via controlled Lagrangians in (Block et al., 2001) and (Liu et al., 2007) (a
complete version) for PF.1 is based on S.3
F ⇐ Lc (14)
u1 =
ξ1 d + K 1 (ω1 − ω1dω2 − ω2 d
ξ1 − ξ1 d ξ 2 − ξ 2 d )
(15)
u2 =
ζ 1d + K 2 (ϑ1 − ϑ1dϑ2 − ϑ2 d
ζ 1 − ζ 1d ζ 2 − ζ 2 d )
On Nonlinear Control Perspectives of a Challenging Benchmark 271
ξ1d ,
where {ξ1d, ζ1d|ξ2d, ζ2d, ζ 1d ,ω1d,ω2d,ϑ1d,ϑ2d} are obtained based on the stable
inversion tool (Devasia, 1996) in T.6 with respect to a desired output trajectory. K. are
ξ ,
linear feedback gain matrices obtained by a linear controller design–LQR. ( ζ 1d ) is
1d
a guidance controller (a feedforward part) and the rest is a feedback minimizing the
tracking errors and rejecting exogenous disturbances. For an achievable desired
trajectory that is c2(–∞,∞), the output (the translational variables ξ1 and ζ1 – the original x
and y) of the closed loop system tracks exactly the desired trajectory while keeping the
pendulum upward.
NC.5 The hybrid controller in (Liu & Yang, 2010) for PF.3 is in the category of T.7. The result is
relying on NC.1 or NC.2 and an event driven piecewise constant signal σ[t0,∞) →Zn that
is continuous from the right at every point and is defined recursively by
σ = α ( χ , ψ ,σ − ), t ≥ t0 (16)
where χ and ψ are metrics on the current tracking errors with respect to the
neighborhood Nμi of ith way-point, σ–(τ ) is equal to the limit from the left of σ(τ ) as τ →
t based on an event that determines the discrete value i in a set {1, 2, ...,n}. The controller
yields either “global” or “semi-global” stability region to the closed loop system inherit
from NC.1 or NC.2. The ordered sequence of way-points are guaranteed but the timing
to a way-point is uncertain.
6. References
Albouy, X. & Praly, L. (2000). On the use of dynamic invariants and forwarding for
swinging up a spherical inverted pendulum, in Proceedings of 39th Conference on
Decision & Control, Sydney, Australia, pp. 1667–1672.
Bloch, A.; Chang, D.; Leonard, N. & Marsden, J. (2001). Controlled Lagragians and the
stabilization of mechanical systems II potential shaping, IEEE Transactions on
Automatic Control, Vol. 41, pp. 1556–1571.
Devasia, D.; Chen, D. & Paden, B. (1996). Nonliear inversion-based output tracking, IEEE
Transactions on Automatic Control, Vol. 41, pp. 930–942.
Isidori, A. (1995). Nonlinear Control System (3rd edition), Springer.
Kokotović; P. Khalil, H. & O’Reilly, J. (1986). Singular Perturbation Methods in Control Analysis
and Design, Academic Press Inc..
272 Advanced Strategies for Robot Manipulators
Krstić M.; Kanellakopoulos, L. & Kokotović, P. (1995). Nonlinear and Adaptive Control Design,
John Wiley & Sons.
Liu, G. (2006). Modeling, Stabilizing Control and Trajectory Tracking of a Spherical Inverted
Pendulu. Ph.D Thesis, The University of Melbourne.
Liu, G.; Challa, I. & Yu, L.(2007). Revisit controlled Lagrangians for spherical inverted
pendulum , International Journal of Mathematics and Computers in Simulation, Vol. 1,
No. 1, pp. 209–214.
Liu, G.; Mareels, I. & Nešić, D. (2008). Decentralized control design of interconnected chains
of integrators a case study, Automatica, Vol. 44, No. 8, pp. 2171-2178.
Liu, G.; D. Nešić & I. Mareels (2008). Nonlinear stable-inversion based output tracking for
the spherical inverted pendulum, International Journal of Control, Vol. 81, No.7, pp.
1035–1053.
Liu, G.; D. Nešić & I. Mareels (2008). Nonlinear stable-inversion based output tracking for
the spherical inverted pendulum, International Journal of Control, Vol. 81, No.1, pp.
116–133.
Liu, G. & Yang, R. (2010). Minimizing operating points of way point tracking of an unstable
nonlinear plant, Asian Journal of Control, Vol. 12, No. 1, pp. 84–88.
Mazenc, F. & Praly, L. (1996). Adding integrations, saturated controls, and stabilization for
feedforward systems, IEEE Transactions on Automatic Control, Vol.41, pp. 1559–1577.
Ortega, R.; Spong, W.; Gomez-Estern, F. & Blankenstein, G. (2002). Stabilization of a class of
underactuated mechanical systems via interconnection and damping assignment.
IEEE Transactions on Automatic Control, Vol. 47, pp. 1218–1233.
Sepulchre, R.; Janković, M. & Kokotović, P. (1997). Constructive Nonlinear Control, Springer,
pp. 979–984.
Sepulchre, R.; Janković M. & Kokotović, P. (1997). Integrator forwarding a new recursive
nonlinear robust design. Automatica, Vol. 393, pp. 979–984.
Sontag, E. (1990). Further facts about input to state stabilization. IEEE Transactions on
Automatic Control, Vol. 35, pp. 473–476.
Sontag, E. (2005). Input to state stability Basic concepts and results, Springer Lecture Notes in
Mathematics, Springer.
Teel, A. (1996). A nonlinear small gain theorem for the analysis of control systems with
saturation. IEEE Transactions on Automatic Control, Vol. 41, pp. 1256–1270.
Utkin, V. (1992). Sliding modes in control optimization, Springer-Verlag.
13
1. Introduction
This chapter presents a unified approach to robust control of a variety of flexible mechanical
systems, which are not only systems having flexible structure themselves such as a robotic
manipulator with a flexible structure and a crane system, but also systems not having
flexible structure but handling flexible objects such as a liquid container system and a
fishery robot. So far, a lot of research efforts have been devoted to solve control problems of
such flexible systems, one of the most typical problems among which is the problem of
flexible robotic manipulators, e.g., [Sharon & Hardt (1984); Spong (1987); Wang &
Vidyasagar (1990); Torres et al., (1994); Magee & Book (1995); Nenchev et al., (1996);
Nenchev et al., (1997)]. As other types of applications, the problems of a crane system [Kang
et al. (1999)] and of a liquid container system [Yano & Terashima (2001); Yano et al., (2001)]
have been investigated. The common control problem for flexible systems can be stated as
“how to achieve required motion control with suppressing undesirable oscillation due to its flexibility”.
From the control methodology point of view, let us review those previous works. For so-
called micro-macro manipulators associated with large flexible space robots, [Torres et al
(1994)] and [Nenchev et al., (1996); Nenchev et al., (1997)] have proposed path-planning
based control methods using a coupling map and a reaction null-space respectively, which
utilize the geometric redundancy. The control methods in [Sharon & Hardt (1984)] for a
micro-macro manipulator and in [Kang et al., (1999)] for a crane system rely on the endpoint
direct feedback, which require sensors to measure the endpoint. In [Wang & Vidyasagar
(1990)], a passivity-based control method has been proposed for a single flexible link, and in
[Spong (1987)] an exact-linearization method and an integral manifold method have been
presented for a flexible-joint manipulator. The method in [Magee & Book (1995)] is based on
input signal filtering where the underlying concept is pole-zero cancellation. [Ueda &
Yoshikawa (2004)] has applied a mode-shape compensator based on acceleration feedback
to a flexible-base manipulator. For a liquid container system, H∞ control in [Yano &
Terashima (2001)] and a notch-type filter based control, that is, equivalent to pole-zero
cancellation, in [Yano et al., (2001)] are utilized respectively. In general, most other works
have focused on individual systems and hence their control methods are not directly
available for various flexible systems. For example, the path-planning methods in [Torres et
274 Advanced Strategies for Robot Manipulators
al., (1994); Nenchev et al., (1996); Nenchev et al., (1997)] cannot be applied to non-redundant
systems. The direct endpoint feedback might be difficult in such a case as of a large space
robot where it is difficult to employ sensors to directly measure the endpoint.
In a stark contrast with those works, we have been tackling with a unified control design
method which can be applied to various flexible mechanical systems in a uniform and
systematic manner. The proposed method exploits a problem setting framework which is
referred to as “generic problem setting” in the modeling phase and then, in the control
design phase, H∞ control powered by PD control. In the sense of control methodology, the
underlying concept is pole-zero cancellation similarly with [Magee & Book (1995); Yano et
al., (2001)], however the control design approach is totally different from ones in those
works. On the other hand, although [Yano & Terashima (2001)] has employed H∞ control, its
usage is different from ours as explained later, and further the pole-zero cancellation is not
the case in [Yano & Terashima (2001)]. In our control design method, the point to be
emphasized is that PD control plays very important roles in facilitating the generic problem
setting and the H∞ control design, and most importantly in enhancing the robustness of the
control system. Then, the advantageous features of our control design method are:
1. The method can be applied to various flexible systems in a uniform, systematic, and
simple manner where the frequency-domain perspective will be provided;
2. The robustness can easily be enhanced by appropriately choosing the PD control gains;
3. Due to the nature based on pole-zero cancellation, any oscillation sensors will not be
required, which is considerably important in the practical sense.
In [Toda (2004)], we have first introduced the fundamental idea and demonstrated control
simulations using linear system and weakly nonlinear system examples. Then, in [Toda
(2007)], robust control has been explicitly considered and a rather strongly nonlinear system
example has been tackled. Now, in this article the control design method and the previous
achievements are summarized, moreover a multiple-input-multiple-output (MIMO) system
and the optimality with respect to PD control are examined while those points have not
been considered in [Toda (2004); Toda (2007)].
The remainder of this chapter is organized as follows. Section 2 presents the generic problem
setting and an illustrative MIMO system example. Section 3 introduces the control design
method and discusses its features in some detail. Then, Section 4 demonstrates control
simulations using the MIMO system example. Finally, Section 5 gives some concluding remarks.
f1 f2 fn
k1 k2 kn
m1 m2 mn
d1 d2 dn
q1 q2 qn
Fig. 1. Schematic diagram of the generic problem setting.
q1 + m2 (
m1 q1 +
q2 ) + d1q 1 + k1q1 = f1
(1)
q1 +
m2 ( q2 ) + d2 q 2 + k2 q2 = 0.
On the other hand, let us consider a single-link flexible-base linear manipulator. In this case,
conversely, the first component is merely an oscillatory component while the second one is
to be directly controlled via the actuator. The dynamical model including PD control to the
actuator can be described as follows,
q1 + m2 (
m1 q1 +
q2 ) + d1q 1 + k1q1 = 0
(2)
q1 +
m2 ( q2 ) + d2 q 2 + k2 q2 = f2 .
As seen from the above discussion, by assigning a component to be directly controlled via
the corresponding actuator or an oscillatory component to each mass, this chain model can
represent various flexible systems. This problem setting framework based on the chain
model is referred to as “generic problem setting”. Then, the control problem is how to
control positions of the directly controlled components with suppressing oscillations of the
oscillatory components. It should be noted that with the proposed control method any
sensors for the oscillatory components will not be required except such cases where, in the
steady state, deformation due to the flexibility and the gravity would become a problem. In
cases of nonlinear and/or uncertain systems, through some linearization procedures such as
nonlinear state feedback and linear approximation around the equilibrium, the system is
modeled as a linear model with parametric uncertainties and/or disturbances. Furthermore,
by applying PD control to the nonlinear system, one can make the linear dynamics
dominant, therefore can facilitate the generic problem setting.
q4
q2 k4
q3
k2
q1
link
actuator
+ C ( q , q ) + Dq + Kq = f
M ( q )q (3)
where M(q) is the inertia matrix, C(q, q ) is the centripetal and Coriolis term,
D = diag[d1,d2,d3,d4] is the damping diagonal matrix, K = diag[k1, k2, k3, k4] is the stiffness
diagonal matrix, and f = [ f1, 0, f3, 0]t is the control torque vector excluding the PD control
scheme. Specifically, each element of M(q), Mij is as follows:
M11 = m1 + m2 + m3 + m4 + 2 R cos(q3 + q 4 )
M12 = M 21 = M 22 = m2 + m3 + m4 + 2 R cos(q3 + q 4 )
M13 = M 23 = M 31 = M 32 = m3 + m4 + R cos(q3 + q 4 )
(4)
M14 = M 24 = M 41 = M 42 = m4 + R cos(q3 + q 4 )
M 33 = m3 + m4
M 34 = M 43 = M 44 = m4
dynamical model employed for the control design and simulations in the sequel are shown
in Table 1, which are set by considering the experimental apparatus at hand.
3. Control design
Here we introduce our control design method which is applied to the obtained model in the
generic problem setting. In the design procedure, first one should determine the PD control
gains, then proceed to the H∞ control design aiming to shape the associated sensitivity
functions. However, in this section, for ease of exposition we first present the H∞ control
design and after that discuss the PD control scheme in some detail.
z2 z3 r
W2 W3
Pj qj
fi +e
Pi qi - W1
z2
P
G
C
Fig. 3. Augmented plant for H∞ control design.
278 Advanced Strategies for Robot Manipulators
where Pi and Pj are coupled systems each other. The sensitivity functions taken into account
are the transfer function S1 from the reference commands r to the tracking control errors e, S2
from r to the control inputs fi, and S3 from r to the oscillatory component displacements qj. In
the example given in Section 2.2, qi are q1, q3, and qj are q2, q4 respectively.
Note that S3 plays a key role in this problem and, in terms of H∞ control design, makes our
method differ from the others such as [Yano & Terashima (2001)] which does not consider S3
but only the standard mixed sensitivity problem. By explicitly employing S3, the resultant
H∞ controller will automatically contain the corresponding zeros to the oscillatory poles of
the plant and thus pole-zero cancellation will occur in the closed-loop system which leads to
suppression of oscillation. Due to this nature of pole-zero cancellation, the control system
will not require any sensors to measure the states of the oscillatory components qj.
The respective weighting functions for the sensitivity functions in the example are
⎡ 1 ⎤
0
20 ⎢ s + 0.0001 ⎥
W1 (s ) = ⎢ ⎥ (6)
7 ⎢ 1 ⎥
0
⎢⎣ s + 0.0001 ⎥⎦
⎡ s + 0.1 ⎤
0
3 ⎢ s + 100 ⎥
W2 (s ) = ⎢ ⎥ (7)
7 ⎢ s + 0.1 ⎥
0
⎣⎢ s + 100 ⎦⎥
20 ⎡ 1 0 ⎤
W3 (s) = ⎢ ⎥. (8)
7 ⎣0 1⎦
W1 is only a quasi-integrator intended for step tracking control. W2 is a high-pass filter which
will be determined by the actuator capability. W3 for S3 is only a constant gain. These functions
are very simple, and in particular W1 and W3 might not depend on problems. Therefore, the
designer will only need to care the constants 20/7, 3/7, 20/7 to adjust the balance among the
functions. This simplicity is one of the important advantages of the proposed method.
Then, by constructing the augmented plant G as in Fig. 3, an H∞ controller C will be
synthesized such that the H∞ norm of the closed-loop system Trz from r to z = [z1, z2, z3]t, that
is, &Trz&∞ is minimum. In this example, the resultant &Trz&∞ was 1.
If one may wish to explicitly consider the model uncertainties in the control design, μ-
synthesis [Packard & Doyle (1993); Zhou et al., (1995)] can be applied instead of merely H∞
control design. The interested readers may consult [Toda (2007)] for the specific approach in
the same framework.
In addition, to improve the transient performance of the obtained control system, a low-pass
filter is employed for step reference commands. In this example, the reference command
filter is
⎡ 100 ⎤
⎢ s 2 + 36 s + 100 0 ⎥
Pr = ⎢ ⎥. (9)
⎢ 100 ⎥
0
⎢⎣ s 2 + 36 s + 100 ⎥⎦
A Unified Approach to Robust Control of Flexible Mechanical Systems
Using H∞ Control Powered by PD Control 279
3.2 PD control
3.2.1 Roles of PD control
Next, let us discuss the PD control scheme exploited for this problem. One role of the PD
control scheme is, as mentioned in Section 2.1, of facilitating the generic problem setting by
making the linear dynamics dominant. And as the second role, the scheme serves to
facilitate the H∞ control design, that is, by eliminating the poles on the imaginary axis and
turning the problem into so-called the standard H∞ control problem [Doyle et al. (1989);
Zhou et al., (1995)]. However, a more important role is of enhancing the robustness with
respect to the oscillation suppression capability, which is deeply connected with the pole-
zero cancellation mechanism of the H∞ controller.
In the case of a completely linear system with neither model uncertainties nor perturbations,
the pole-zero cancellation will never fail, and hence the constant oscillation suppression
performance can be acquired. However, otherwise, that is, in cases of a nonlinear system
and/or with model uncertainties, the pole-zero cancellation will fail since the oscillatory
poles of the plant vary. In such a case, the damping property of the plant will become
critical. Specifically, when the minimum among the damping factors of the plant poles is too
small, the oscillation suppression performance can largely degrade in case of failure of the
pole-zero cancellation. Here the damping factor ζ of a stable pole s, whose real part
Re(s) ≤ 0, is defined as
Re(s )
ζ := - (10)
s
(a)
C
10
0
S1
gain S3
−1 0 1 2 3
10 10 10 10 10
frequency (rad/s)
(b)
C
10
0
S1
S3
gain
−1 0 1 2 3
10 10 10 10 10
frequency (rad/s)
(a) Frequency responses of C, S1, and S3 with the minimal damping factor of 8 × 10-4.
(a) nominal case (b) perturbed case.
(a)
C
10
0
S1
S3
gain
−1 0 1 2 3
10 10 10 10 10
frequency (rad/s)
(b)
C
10
0
S1
S3
gain
−1 0 1 2 3
10 10 10 10 10
frequency (rad/s)
(b) Frequency responses of C, S1, and S3 with the minimal damping factor of 6 × 10-2.
(a) nominal case (b) perturbed case.
where di’s and ki’s are bounded as 2.18e-5 ≤ d1 ≤ 2.18e1, 1.52e-6·≤ d3 ≤ 1.52, 2.18e-6 ≤ k1 ≤
2.18e2, 1.52e-7 ≤ k3 ≤ 1.52e1, respectively. Further, to demonstrate the above point 3, the
other cost function η2 taking only ζmin into account
for similarly bounded di’s and ki’s has been also considered. In the next section, these
optimization strategies will be discussed based on control simulations.
4. Control simulations
In this article, to prove that the proposed control method can be applied to even MIMO
systems, and to demonstrate the above discussions on the optimality with respect to the PD
gains, we here present control simulations. According to the last section, four cases of PD
gains are considered, which includes the cases of the respective optimal gains due to η1 and
η2, and additional two non-optimal cases. The respective ζmin and PD gains are shown in
Table 2. Comparing Cases 1 and 2 in Table 2, it is noticed that the same ζmin and similar D
gains can be obtained, however that the P gains in Case 2 are considerably larger than those
in Case 1, which indeed reflects the cost functions in (11) and (12).
3. two types of references 0→π/3 rad and 0→π/2 rad for both r1 and r3, with the step time
of 1 s are applied.
The simulation results are shown in Figs. 5–7 respectively. First we shall see the two optimal
cases. In Figs. 5 and 6, the upper figures show each displacement on large scale graphs while
the lower ones do each tracking control error to the final goal on fine scale ones. Comparing
Case 1 of η1 and Case2 of η2, that is, with the same ζmin of 0.40, on large scale graphs those
results are almost the same and reveal the good performances for both tracking control and
oscillation suppression. On fine scale graphs, they are still very similar, however the
oscillations of the oscillatory components e2 and e4 in Case 2 are slightly larger than those in
Case 1, and slight overshoots of e3 can be seen at around 3 s in Case 2, which might be due to
the largenesses of k1 and k3.
Next, let us see the non-optimal cases in Fig. 7. In the figure, the upper figure shows the
results of Case 3 with the small ζmin of 0.06, while the lower one does those of Case 4 with the
large, in fact, maximal ζmin of 1.00 on fine scale graphs respectively. As seen from the figures,
as pointed out before, the results of Case 3 reveal poor oscillation suppression performances,
while the results of Case 4 reveal a slightly slow response in e3 and a slight steady error in e1,
which thus has demonstrated P1 and P2 in the last section.
Consequently, the main goal of extending our proposed method to MIMO systems has
successfully been achieved, that is, it has been confirmed that the proposed method is
effective and feasible for even MIMO systems. Additionally, discussions on the optimality
with respect to the PD control gains have been given in some detail. The obtained control
system based on the cost function η1 has revealed good performances in both tracking
control and oscillation suppression, which therefore can be one of the promising candidates
for the optimality, although it has not yet been conclusive that η1 can be useful for other
examples.
5. Conclusions
In this article, we have presented the control design method based on H∞ control and PD
control aiming at a uniform approach to motion control of various flexible mechanical
systems. In particular, with a special emphasis on MIMO systems and the optimal PD
gains, we have introduced and demonstrated the concept of the generic problem setting in
the modeling phase, the physics behind our control method, that is, how the PD control
scheme elaborately powers the H∞ control system, the promising candidate of cost
function for the optimal PD gains, and the control simulations which have supported all
the discussions.
Here, again we emphasize the advantageous features of the proposed approach:
1. A variety of flexible mechanical systems can be systematically dealt with in a uniform
and simple manner where the frequency-domain perspective will be provided;
2. The robustness can be easily enhanced by appropriately choosing the PD control
gains;
3. Due to the nature based on pole-zero cancellation, any oscillation sensors will not be
required, which is considerably important in the practical sense.
Consequently, we have shown that our methodology is easy to use and effective indeed and
further will possibly evolve in the sense of optimality.
A Unified Approach to Robust Control of Flexible Mechanical Systems
Using H∞ Control Powered by PD Control 283
0 to π/3 rad
displacements (rad) 1.5
q1
1
q2
0.5 q3
0 q4
−0.5
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/2 rad
2
displacements (rad)
q1
1 q2
q3
0
q4
−1
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/3 rad
0.1
e1
errors (rad)
0.05
0
e2
e3
−0.05
e4
−0.1
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/2 rad
0.1
e1
errors (rad)
0.05
0
e2
e3
−0.05
e4
−0.1
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/3 rad
displacements (rad) 1.5
q1
1
q2
0.5 q3
0 q4
−0.5
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/2 rad
2
displacements (rad)
q1
1 q2
q3
0
q4
−1
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/3 rad
0.1
e1
errors (rad)
0.05
0
e2
e3
−0.05
e4
−0.1
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/2 rad
0.1
e1
errors (rad)
0.05
0
e2
e3
−0.05
e4
−0.1
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/3 rad
0.1
e1
errors (rad)
0.05
0
e2
e3
−0.05
e4
−0.1
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/2 rad
0.1
e1
errors (rad)
0.05
0
e2
e3
−0.05
e4
−0.1
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/3 rad
0.1
e1
errors (rad)
0.05
0
e2
e3
−0.05
e4
−0.1
0 1 2 3 4 5 6 7 8 9 10
time (s)
0 to π/2 rad
0.1
e1
errors (rad)
0.05
0
e2
e3
−0.05
e4
−0.1
0 1 2 3 4 5 6 7 8 9 10
time (s)
6. References
Doyle, J. C.; Glover, K.; Khargonekar, P. P. & Francis, B. A. (1989). State-space solutions to
standard H2 and H∞ control problems. IEEE Transactions on Automatic Control, Vol.
34, No. 8, August-1989, 831–847.
Kang, Z.; Fuji, S.; Zhou, C. & Ogata, K. (1999). Adaptive control of a planar gantry crane by
the switching of controllers. Transactions of the Society of Instrument and Control
Engineering, Vol. 35, No. 2, Feb-1999, 253–261.
Magee, D. P. & Book, W. J. (1995). Filtering micro-manipulator wrist commands to prevent
flexible base motion. Proceedings of American Control Conference 1995, 924–928.
Nenchev, D. N.; Yoshida, K. & Uchiyama, M. (1996). Reaction null-space based control of a
flexible structure mounted manipulator systems. Proceedings of IEEE International
Conference on Decision and Control 1996, 4118–4123.
Nenchev, D. N.; Yoshida, K.; Vichitkulsawat, P.; Konno, A. & Uchiyama, M. (1997).
Experiments on reaction null-space based decoupled control of a flexible structure
mounted manipulator system. Proceedings of IEEE International Conference on
Robotics and Automation 1997, 2528–2534.
Packard, A. & Doyle, J. C. (1993). The complex structured singular value. Automatica, Vol. 29,
No. 1, 1993, 71–110.
Sharon, A. & Hardt, D. (1984). Enhancement of robot accuracy using end-point feedback and
a macro-micro manipulator system. Proceedings of American Control Conference 1984,
1836–1842.
Spong, M. W. (1987). Modeling and control of elastic joint robots. ASME Journal of Dynamic
Systems Measurement and Control, Vol. 109, Dec-1987, 310–319.
Toda, M. (2004). A unified approach to control of mechanical systems with a flexible
structure. Proceedings of International Symposium on Robotics and Automation 2004,
313–319.
Toda, M. (2007). A unified approach to robust control of flexible mechanical systems.
Proceedings of IEEE Conference on Decision and Control 2007, 5787–5793.
Torres, M. A.; Dubowsky, S. & Pisoni, A. C. (1994). Path-planning for elastically-mounted
space manipulators: experimental evaluation of the coupling map. Proceedings of
IEEE Internatinal Conference on Robotics and Automation 1994, 2227–2233.
Ueda, U. & Yoshikawa, T. (2004). Mode-shape compensator for improving robustness of
manipulator mounted on flexible base. IEEE Transactions on Robotics and
Automation, Vol. 20, No. 2, April-2004, 256–268.
Wang, D. & Vidyasagar, M. (1990). Passive control of a single flexible link. Proceedings of
IEEE International Conference on Robotics and Automation 1990, 1432–1437.
Yano, K. & Terashima, K. (2001). Robust liquid container transfer control for complete
sloshing suppression. IEEE Transctions on Control and Systems Technology, Vol. 9, No.
3, May-2001, 483–493.
Yano, K; S. Higashikawa, S. & Terashima, K. (2001). Liquid container transfer control on 3D
transfer path by hybrid shaped approach. Proceedings of IEEE International
Conference on Control Applications 2001, 1168–1173.
Zhou, K.; Doyle, J. C. & Glover, K. (1995).Robust Control and Optimal Control, Prentice-Hall,
New Jersey.
14
Malaysia
1. Introduction
In real time applications, the trajectory which has to be followed and the task that has to be
performed during motion planning of multi-axis non-linear mechanical systems, such as
robot manipulators are of great importance. Due to the non-linear transformation between
the task space and the joint space coordinates, singularities and uncertainties in the arm
configuration occur, the unplanned occurrence of such problems drive the end-effector out
of the desired path which may cause collision of the robot arm with objects located in its
work cell (Köker, 2005; Antonelli et al., 2003).
Depending on different tasks operation requirements and circumstances, motion control
algorithms can be developed either at the kinematics level or at the dynamic level (Graca &
Gu, 1993; Karilk & Aydin, 2000). To develop a dynamic control algorithm, torque limits of
the joint actuators are to be handled, two typical approaches were introduced which are the
Computed-torque and Resolved-acceleration approach, both approaches are based on the
inverse dynamic model of the robot system (Asada & Soltin,1986; Sopng & Vinyasagar,1998;
Faiz & Agrawal ,2000). A problem with these algorithms is the remarkable computational
load required to handle the dynamics of a full-sized manipulator, which is seldom
affordable by current industrial control units. In addition, implementation of torque-based
control laws requires replacement of the low-level joint servos typically available in
industrial robots with custom control loops.
Aimed at overcoming the above drawbacks, a different approach to path tracking based on
the kinematics control was proposed. In detail, kinematics control consists in an inverse
kinematics transformation which sends to the joint servos the reference values
corresponding to an assigned end-effector trajectory; as a first advantage, this allows simple
interfacing with the standard control architecture of industrial robots. In the framework of
kinematics-based methods for path tracking, the counterpart of the physically meaning joint
torque limits is played by acceleration constraints and the use of full dynamic models can be
avoided; this typically leads to computationally light algorithms that allow real-time
implementation on standard numerical hardware even for robot arms of many Degrees of
288 Advanced Strategies for Robot Manipulators
Kuroe and colleges (Kuroe et al., 1994) have proposed a learning method of a neural
network such that the network represents the relations of both the positions and velocities
from the Cartesian coordinate to the joint space coordinate. They’ve derived a learning
algorithm for arbitrary connected recurrent networks by introducing adjoint neural
networks for the original neural networks (Network inversion method). On-line training has
been performed for a 2 DOF robot.
It was essentially an on-line learning process (Graca & Gu, 1993) have developed a Fuzzy
Learning Control algorithm. Based on the robotic differential motion procedure, the
Jacobian inverse has treated as a fuzzy matrix and has learned through the fuzzy regression
process. It was significant that the fuzzy learning control algorithm neither requires an exact
kinematics model of a robotic manipulator, nor a fuzzy inference engine as is typically done
in conventional fuzzy control. Despite the fact that unlike most learning control algorithms,
multiple trials are not necessary for the robot to “learn” the desired trajectory. A major
drawback was that it only remembers the most recent data points introduced, the
researchers have recommended neural networks so that it would remember the trajectories
as it traversed them.
Studying the trajectory tracking of a serial manipulator by using ANNs has two problems,
one of these is the selection of the appropriate type of network and the other is the
generating of suitable training data set (Funahashi, 1998; Hasan et al, 2007). Researchers
have applied different methods for gathering training data, while some of them have used
the kinematics equations (Karilk & Aydin, 2000; Bingual et al., 2005), others have used the
network inversion method (Kuroe et al., 1994); Köker, 2005), while the cubic trajectory
planning was also used (Köker et al., 2004), a simulation program has also been used for this
purpose (Driscoll, 2000). However, there are always kinematics uncertainties presence in the
real world such as ill-defined linkage parameters, links flexibility and backlashes in gear
train.
The proposed solution of the kinematics Jacobian in this approach, involves the
determination of the end-effectors coordinates and their rate of change as a function of given
positions and speed of the axes of motion, although this is very difficult in practice (Hornic,
1991), training data were recorded experimentally from sensors fixed on each joint and the
Euler (RPY) representation was used to represent the orientation (as was recommended by
Karilk and Aydin (Karilk & Aydin, 2000), as they have used the robot model to get the
training data and used the homogeneous transformation matrix representation to represent
the orientation). On the other hand, two different network’s configurations were trained and
compared to examine the effect of the orientation on the Inverse Kinematics solution of
serial robots. Finally, the obtained results from the testing phase of the best network were
verified experimentally using a six DOF serial robot manipulator.
x = f (q ) (1)
If the Cartesian coordinates x were given, joint coordinates q can be obtained as:
q = f −1 ( x ) (2)
•
If a Cartesian linear velocity is denoted by V , the joint velocity vector q has the following
relation:
•
V = Jq (3)
•
q = J −1V (4)
In differential motion control, the desired trajectory is subdivided into sampling points
separated by a time interval Δt between two terminal points of the path. Assuming that at
time ti the joint positions take on the value q(ti ) , the required q at time (ti + Δt ) is
conventionally updated by using:
•
q(ti + Δt ) = q(ti ) + q Δt (5)
Equation (6) is a kinematics control law used to update the joint position q and is evaluated
on each sampling interval. The resulting q(ti + Δt ) is then sent to the individual joint motor
servo-controllers, each of which will independently drive the motor so that the robotic
manipulator can be maneuvered to follow the desired trajectory (Graca & Gu, 1993).
Using ANN to solve relation (2), researchers applied two approaches. In (Ogawa et al., 2005;
Hasan et al., 2006; Köker et al.,2004) only the Cartesian coordinates has been inverted,
mapping from the joint space to the Cartesian space is uniquely decided when the end
effector’s position is calculated using direct kinematics, as shown in figure 1(a). However,
the transformation from the Cartesian to the joint space is not uniquely decided in the
inverse kinematics as shown in figure 1(b).
When coupling of the position and orientation e.g., (Köker,2005; Karilk & Aydin, 2000)
Denavit and Hartenberg (Denavit & Hertenberg, 1955) proposed a matrix method of
systematically establishing a coordinate system to each link of an articulated chain as shown
in figure 2 to describe both translational and rotational relationships between adjacent links
(Fu et al., 1987; Köker, 2005).
In this method each of the manipulator links is modelled, this modelling describes the “A”
homogeneous transformation matrix, which uses four link parameters. The forward
kinematics solution can be obtained as:
An Improved Adaptive Kinematics Jacobian Trajectory Tracking
of a Serial Robot Passing Through Singular Configurations 291
⎡ Rotation Position ⎤
⎢ matrix |
vector ⎥ ⎡ nx sx ax px ⎤
⎢ ⎥ ⎢n sy ay py ⎥⎥
AEND − EFFECTOR = T6 = A1 . A2 .A3 .A4 .A5 .A6 = ⎢ −−−− | −−−− ⎥ =⎢ y (7)
⎢ ⎥ ⎢ nz sz az pz ⎥
⎢ Perspective | Scaling ⎥ ⎢0 0 0 1⎦
⎥
⎢transformation ⎥ ⎣
⎣ ⎦
Fig. 2. Schematic diagram for a general 6 DOF serial robot showing the wrist mechanism
These equations describe the orientation according to the RPY representation (Karilk &
Aydin, 2000). To find the IK solution, however, joints angels are found according to the
manipulator’s end position, described with respect to the world coordinate system.
IK solution can be shown as a function:
IK ( X , Y , Z ,ϕ x ,ϕ y ,ϕ z ) = (θ 1 ,θ 2 ,θ 3 ,θ 4 ,θ 5 ,θ 6 ) (12)
Traditional methods for solving the IK problem are inadequate if the structure of the robot is
complex, besides; these methods suffer from the fact that the solution does not give a clear
indication on how to select an appropriate solution from the several possible solutions for a
particular arm configuration, users often needs to rely on their intuition to choose the right
answer (Fu et al., 1987; Hasan et al., 2006).
On the other hand, solving Eq. (4) for the joint velocities (Inverting the Jacobian matrix),
results in the singularity problem. The manipulator singularity resolution problem has
attracted many research interests, and various approaches have been proposed to tackle the
problem. Techniques of coping with kinematics singularities can be divided into four
groups: avoiding singular configurations, robust inverses, a normal form approach and
extended Jacobian techniques.
The first approach to cope with singularities is to keep a current configuration far away
from singular configurations. Unfortunately, it causes severe restrictions on the
configuration space as well as the workspace because the singular configurations split the
configuration space into separate components. To avoid ill conditioning of the Jacobian
matrix, robust inverses are used. Instead inverting the original Jacobian matrix at
singularity, a disturbed well-conditioned Jacobian matrix is inverted. The main drawback
using this approach is that robust inverse methods increase errors in following a desired
path.
An Improved Adaptive Kinematics Jacobian Trajectory Tracking
of a Serial Robot Passing Through Singular Configurations 293
The normal form technique, with the use of diffeomorphisms in joint and task spaces,
expresses original kinematics around singularity in the simplest normal form. Then, a piece
of the path to follow corresponding to the singular configuration mapped into the task space
is moved from the task to the joint space and trajectory planning is performed there. Far
away from singularities the basic Newton algorithm is used to generate a trajectory. Finally,
trajectory pieces are joined.
For most singularities the normal form approach enables to detect their types. It provides for
a smooth passing through singular configurations. The main disadvantage of the normal
form approach is a significant computational load in deriving the diffeomorphisms.
Finally, The extended Jacobian technique, supplements original kinematics with auxiliary
functions. Then, extended Jacobian is formulated to be well conditioned.
For nonredundant manipulators with square Jacobian matrices the extended Jacobian forms
a non-square matrix and its generalized (Moore-Penrose) inversion is computationally
expensive (Dulęba & Sasiadek, 2000).
Therefore, to analyze the singular conditions of a manipulator and develop effective
algorithms to resolve the inverse kinematics problem at or in the vicinity of singularities are
of great importance.
to stimulate some important aspects of the real biological neuron. An ANN is a group of
interconnected artificial neurons usually referred to as “node” interacting with one another
in a concerted manner; Figure 4 illustrates how information is processed through a single
node. The node receives weighted activation of other nodes through its incoming
connections. First, these are added up (summation). The result is then passed through an
activation function and the outcome is the activation of the node. The activation function
can be a threshold function that passes information only if the combined activity level
reaches a certain value, or it could be a continues function of the combined input, the most
common to use is a sigmoid function for this purpose. For each of the outgoing connections,
this activation value is multiplied by the specific weight and transferred to the next node
(Kalogirou, 2001; Hasan, 2006).
Also, it is used in many situations of learning. A set of input and output patterns called a
training set is required for this learning mode. Figure 6-b shows the block diagram of
unsupervised learning. In unsupervised learning, the desired response is not known; thus,
explicit error information cannot be used to improve network’s behaviour. Since no
information is available as to correctness or incorrectness of responses, learning must
somehow be accomplished based on observations of responses to inputs that we have mar-
ginal or no knowledge about (Zurada, 1992).
The fundamental idea underlying the design of a network is that the information entering
the input layer is mapped as an internal representation in the units of the hidden layer(s)
and the outputs are generated by this internal representation rather than by the input vector.
Given that there are enough hidden neurons, input vectors can always be encoded in a form
so that the appropriate output vector can be generated from any input vector (Santosh et al.,
1993).
O j = f ( sumB ) (14)
Where f is the non-linear activation function, it is a common practice to choose the sigmoid
function given by:
1
f (O j ) = −O j
(15)
1+ e
as the nonlinear activation function. However, any input-output function that possesses a
bounded derivative can be used in place of the sigmoid function. If there is a fixed, finite set
An Improved Adaptive Kinematics Jacobian Trajectory Tracking
of a Serial Robot Passing Through Singular Configurations 297
of input-output pairs, the total error in the performance of the network with a particular set
of weights can be computed by comparing the actual and the desired output vectors for
each presentation of an input vector. The error at any output unit eK in the layer C can be
calculated by:
eK = dK − OK (16)
Where dK is the desired output for that unit in layer C and OK is the actual output produced
by the network .the total error E at the output can be calculated by:
1
E= ∑ ( dK − OK )2
2 K
(17)
Learning comprises changing weights so as to minimize the error function and to minimize
E by the gradient descent method. It is necessary to compute the partial derivative of E with
respect to each weight in the network. Equations (13) and (14) describe the forward pass
through the network where units in each layer have there states determined by the inputs
they received from units of lower layer. The backward pass through the network that
involves “back propagation “ of weight error derivatives from the output layer back to the
input layer is more complicated. For the sigmoid activation function given in equation (15),
the so-called delta-rule for iterative convergence towards a solution maybe stated in general
as:
ΔWJK = ηδ K OJ (18)
Where η is the learning rate parameter, and the error δ K at an output layer unit K is given
by:
δ K = OK (1 − OK )( dK − OK ) (19)
δ J = OJ (1 − OJ )∑ δ K WJK (20)
K
Using the generalize delta rule to adjust weights leading to the hidden units is back
propagating the error-adjustment, which allows for adjustment of weights leading to the
hidden layer neurons in addition to the usual adjustments to the weights leading to the
output layer neurons. A back propagation network trains with two step procedures as it is
shown in figure 7, the activity from the input pattern flows forward through the network
and the error signal flows backwards to adjust the weights using the following equations:
Until for each input vector the output vector produced by the network is the same as (or
sufficiently close to) the desired output vector (Santosh et al., 1993).
298 Advanced Strategies for Robot Manipulators
ANNs while implemented on computers are not programmed to perform specific tasks.
Instead, they are trained with respect to data sets until they learn the patterns presented to
them. Once they are trained, new patterns may be presented to them for prediction or
classification (Kalogirou, 2001).
4. Experiment design
Trajectory planning was performed for every 1-second interval using cubic trajectory
planning method to generate the angular position and velocity for each joint, and then these
generated data were fed to the robot’s controller to generate the corresponding Cartesian
position, orientation and linear velocity of the end-effector, which were recorded
experimentally from sensors fixed on the robot joints.
In trajectory planning of a manipulator, it is interested in getting the robot from an initial
position to a target position with free of obstacles path. Cubic trajectory planning method
has been used in order to find a function for each joint between the initial position, θ0, and
final position, θf of each joint.
It is necessary to have at least four-limit value on the θ(t) function that belongs to each joint,
where θ(t) denotes the angular position at time t.
Two limit values of the function are the initial and final position of the joint, where:
θ (0) = θ 0 (23)
θ (t f ) = θ f (24)
Additional two limit values, the angular velocity will be zero at the beginning and the target
position of the joint, where:
•
θ (0) = 0 (25)
•
θ (t f ) = 0 (26)
Based on the constrains of typical joint trajectory listed above, a third order polynomial
function can be used to satisfy these four conditions; since a cubic polynomial has four
coefficients.
These conditions can determine the cubic path, where a cubic trajectory equation can be
written as:
•
θ (t ) = a1 + 2 a2t + 3 a3t 2 (28)
••
θ ( t ) = 2 a 2 + 6 a3 t (29)
Substituting the constrain conditions in the above equations results in four equations with
four unknowns:
An Improved Adaptive Kinematics Jacobian Trajectory Tracking
of a Serial Robot Passing Through Singular Configurations 299
θ 0 = a0 ,
θ f = a0 + a1t f + a2t 2f + a3t 3f ,
0 = a0 , (30)
0 = a1 + 2 a2t f + 3a3t 2f
a0 = θ 0 ,
a1 = 0,
3
a2 = (θ f − θ 0 ), (31)
t 2f
−2
a3 = (θ f − θ 0 )
t 3f
Angular position and velocity can be calculated by substituting the coefficients driven in Eq.
(31) into the cubic trajectory Equations (27) and (28) respectively (Köker et al.,2004), which
yield:
3 2
θ i (t ) = θ i 0 + 2
(θ if − θ i 0 )t 2 − 3 (θ if − θ i 0 )t 3 , (32)
tf tf
• 6 6
θ i (t ) = (θ if − θ i 0 )t − 3 (θ if − θ i 0 )t 2
t 2f tf (33)
i = 1, 2,..........., n Where n is the joint number
Joint angles generated ranged from amongst all the possible joint angles that do not exceed
the physical limits of each joint; Table 1 shows the range of angles for each joint used in this
study.
5. ANN implementation
To avoid modeling kinematics and the determination of the inverse of the Jacobian matrix,
the ANN technique has been used.
Two different configurations of supervised feed-forward ANNs were designed using
C programming language, each of which consists of input, output, and one hidden layer.
Every neuron in each network was fully connected with each other. Sigmoid transfer
function was chosen to be the activation function, and the generalized backpropagation
GDR algorithm was used in the training process.
Off-line training was implemented, every input and output values are usually scaled
individually such that overall variance in the data set is maximized, this is necessary as it
leads to faster learning, all the vectors were scaled to reflect continuous values ranges
from -1 to 1.
FANUC M-710i robot was used in this study, which is a serial robot manipulator consisting
of axes and arms driven by servomotors. The place at which arm is connected is a joint, or
an axis. This type of robot has three main axes; the basic configuration of the robot depends
on whether each main axis functions as a linear axis or rotation axis. The wrist axes are used
to move an end effecter (tool) mounted on the wrist flange. The wrist itself can be wagged
about one wrist axis and the end effecter rotated about the other wrist axis, this highly non-
linear structure makes this robot very useful in typical industrial applications such as the
material handling, assembly of parts and painting.
90 4 - 12 Network Configuration
80
70
60
Error %
50
40
30
20
10
0
0 50 100 150 200 250 300
Iteration X 1000
Fig. 9. The learning curve for the angular position of Joint 1 as a sample
An Improved Adaptive Kinematics Jacobian Trajectory Tracking
of a Serial Robot Passing Through Singular Configurations 303
Network Configuration
4 - 12 7 - 12
θ 7.898% 2.27%
Joint 1
ω 8.67% 1.8%
θ 12.432% 0.907%
Joint 2
ω 39.75% 2.183%
θ 2.607% 1.033%
Joint 3
ω 5.03% 1.775%
θ 9.82% 2.015%
Joint 4
ω 10.4% 2.342%
θ 8.47% 4.435%
Joint 5
ω 19.94% 1.558%
θ 10.86% 1.143%
Joint 6
ω 5.735% 1.528%
Table 2. Training error percentages of each of the six joints compared for each other in both
network configurations
180 Desired
120
100
80
60
40
20
0
0 50 100 150 200 250 300 350 400
Time ( Sec. )
Fig. 10. The response of both network configurations compared to each other during
training (The angular position of the firs joint as an example)
As a result for the training stage, 7-12 network configuration has shown a better response
than the 4-12 network configuration, in terms of precision and iteration (as can be seen
through Table 2). Therefore, it has been chosen to apply the testing data.
To drive the robot to follow a desired path, it will be necessary to divide this path into small
segments, and to move the robot through all intermediate points. To accomplish this task, at
each intermediate location, the robot’s trajectory equations are solved, a set of joint variables
304 Advanced Strategies for Robot Manipulators
is calculated, and the controller is directed to drive the robot to the next segment. When all
segments are completed, the robot will be at the end point as desired. Figure 10 shows a
sample of angular position and velocity for each joint during training (other joints have a
similar behavior).
6. Conclusions
In this approach, ANN technique has been used. The Jacobian inverse is now learned
through training the network based only on observation of the input–output relationship
unlike most other control schemes, which depends on the robot system model
The proposed technique does not require any prior knowledge of the kinematics model of
the system being controlled, the basic idea of this concept is the use of the ANN to learn the
characteristics of the robot system rather than to specify explicit robot system model.
Two different ANN configurations were used in this study. Training results have shown a
better response (in terms of precision and iteration) for the configuration where the orientation
of the tool is considered as an input to the network, which makes it useful in applications
where a relatively accurate, minimally complex, and cheaper configuration is required.
As a conclusion, this study shows that ANNs are applicable to the Kinematics Jacobian
solution of serial robots. Since one of the most important issues in using ANNs is the
selection of the appropriate type of network, for future research, we suggest that different
An Improved Adaptive Kinematics Jacobian Trajectory Tracking
of a Serial Robot Passing Through Singular Configurations 305
types of networks (different topology, different activation function, different learning mode)
to be used in order to get, if possible, more accurate trajectory tracking.
Fig. 14. Experimental trajectory tracking for the Roll orientation angle
An Improved Adaptive Kinematics Jacobian Trajectory Tracking
of a Serial Robot Passing Through Singular Configurations 307
Fig. 15. Experimental trajectory tracking for the Pitch orientation angle
Fig. 16. Experimental trajectory tracking for the Yaw orientation angle
308 Advanced Strategies for Robot Manipulators
7. References
Al-Assadi, H.M.A.A.; Hamouda, A.M.S.; Ismail, N. and Aris, I. (2007) .An adaptive learning
algorithm for controlling a two-degree-of-freedom serial ball-and-socket actuator.
Proceedings of the I MECH E Part I Journal of Systems & Control Engineering,
Vol.221, No. 7, pp.1001-1006.
Albala, H. & Angeles, J. (1979). Numerical solution to the input–output displacement
equation of the general 7R spatial mechanism. Proceedings of the Fifth world congress
on theory of machines and mechanisms, pp 8–11.
Antonelli, G.; Chiaverini, S. and Fusco, G. (2003) .A new on-line algorithm for inverse
kinematics of robot manipulators ensuring path-tracking capability under joint
limits. IEEE Transaction on Robotics and Automation, Vol.19, No.1, pp. 162-167.
Asada, H. & Soltin, J-J. E., Robot analysis and control. John Wiley and Sons Inc., New York.
1986.
Balestrino, A., De Maria, G. and Sciavicco, L., Robust control of robotic manipulators.
International Proceedings of the 9th IFAC World Congr. Budapest, Hungary .1984;
6:80-85.
Bingual, Z., Ertunc, H.M. and Oysu, C., Comparison of Inverse Kinematics Solutions Using
Neural Network for 6R Robot Manipulator with Offset. 2005 ICSC congress on
Computational Intelligence.
Daniel, M. and Raul, G., Hierarchical Kinematics analysis of robots. Mech Mach Theory.
2003; 33: 497–518.
Denavit, J., and Hertenberg, R.S.A. Kinematics Notation for lower Pair Mechanism Based on
Matrices. Applied mechanics 1955; 77: 215-221.
Driscoll, J.A., Comparison of neural network architectures for the modeling of robot inverse
kinematics. Proceedings of the IEEE, south astcon. 2000:44-51.
D'Souza, A., Vijayakumar, S., and Schaal, S. (2001) ‘Learning Inverse Kinematics’.
Proceedings of the 2001 IEEE/ RSJ International Conference on Intelligent Robots and
Systems Maui, Haw- USA, pp.298-303.
Duffy, J. and Rooney, J., A foundation for a unified theory of analysis of spatial mechanism.
Eng Ind. 1975; 97(4): 1159–64.
Dulęba, I., and Sasiadek, J.Z. (2000) ‘Modified Jacobian method of transversal passing
through singularities of nonredundant manipulators’. Proceedings of the American
Control Conference Chicago, Illinois June, pp.2839-2843.
Faiz, N. and Agrawal, S. K., Trajectory planning of robots with dynamics and inequalities. In
Proc. IEEE Int. Conf. Robotics and Automation, San Francisco, CA.2000: 3977–3983.
Fu, K.S., Gonzalez, R.C. and Lee, C.S.G. Robotics Control, Vision, and Intelligence. McGraw-
Hill book Co. Singapore, international edition. 1987.
Funahashi, K.I., On the approximate realization of continuous mapping by neural networks.
Neural Networks.1998; 2(3): 183-192.
Graca, R.A. and Gu, Y., A fuzzy learning algorithm for kinematics control of a robotic
system. Proceeding of the 32nd conference on decision and control. San Antonio,
Texas. December 1993:1274-1279.
An Improved Adaptive Kinematics Jacobian Trajectory Tracking
of a Serial Robot Passing Through Singular Configurations 309
Hasan, A.T., Hamouda, A.M.S., Ismail, N., and Al-Assadi, H.M.A.A. (2006) ‘An adaptive-
learning algorithm to solve the inverse kinematics problem of a 6 D.O.F serial robot
manipulator’. Int. J. Advances in Engineering Software, Vol.37, pp. 432-438.
Hasan, A.T., Hamouda, A.M.S., Ismail, N., and Al-Assadi, H.M.A.A., A new adaptive
learning algorithm for robot manipulator control. Proceeding of the I Mech E, Part
I: Journal of System and Control Engineering .2007; 221(4): 663-672.
Haykin S. Neural Networks. A Comprehensive Foundation. New York: Macmillan, 1994.
Hornik, K., Approximation capabilities of multi-layer feed forward networks. IEEE Trans.
Neural Networks. 1991; 4(2): 251-257.
Karilk, B., Aydin, S., An improved approach to the solution of inverse kinematics problems
for robot manipulators. Journal of Engineering applications of artificial intelligence.
2000; 13: 159-164.
Köker, R. (2005) ‘Reliability-based approach to the inverse kinematics solution of robots
using Elman’s networks’. Int. J. Engineering Applications of Artificial Inttelegence,
Vol.18, pp. 685-693.
Köker, R., Öz, C., Çakar.T. and Ekiz, H., A study of neural network based inverse kinematics
solution for a three-joint robot. Robotics and Autonomous Systems. 2004; 49: 227–
234.
Kuroe, Y., Nakai, Y. and Mori, T., A new Neural Network Learning on Inverse Kinematics of
Robot Manipulators. International Conference on Neural Networks, IEEE world
congress on computational Intelligence. 1994; 5: 2819-2824.
Nakamura, Y. and Hanafusa, H., Inverse kinematic solutions with singularity robustness for
robot manipulator control. Journal of Dynamic Systems Measurements
Control.1986; 108: 163–171.
Ogawa, T., Matsuura, H., and Kanada, H. (2005) ‘A Solution of Inverse Kinematics of Robot
Arm Using Network Inversion’. Proceedings of the International Conference on
Computational Intelligence for Modelling, Control and Automation.
Santosh, A. Devendra P. Garg. Training back propagation and CMAC neural networks for
control of a SCARA robot. Engineering Applications of Artificial Intelligence.
Vol.6.No.2. pp.105-115. 1993.
Sopng, M.W., and Vinyasagar, M., Robot Dynamics and control. John Wiley and Sons Inc.,
New York. 1998.
Soteris, A.Kalogirou. Artificial Neural Networks In Renewable Energy Systems
Applications: a review. Renewable and Sustainable Energy Reviews. Vol. 5:pp.373-
401. 2001.
Tsai, L.W. and Morgan, A., Solving the Kinematics of the most general six and five degree of
freedom manipulators by continuation methods. Mech Transm Autom Des 1985;
107:189–200.
Wampler, C. W. and Leifer, L. J., Applications of damped least-squares methods to resolved-
rate and resolved-acceleration control of manipulators, Journal of Dynamic Systems
Measurements Control.1988; 110: 31–38.
Wampler, C. W., Manipulator inverse kinematic solutions based on vector formulations and
damped least-squares methods. IEEE Transaction Syst., Man, Cybernetics. 1986; 16:
93–101.
310 Advanced Strategies for Robot Manipulators
Whitney. E., Resolved motion rate control of manipulators and human prostheses, IEEE
Transaction Man–Mach. Systems.1969; 10:47–53.
Yang, A.T., Displacement analysis of spatial five-link mechanism using (3 × 3) matrices with
dual-number element. Eng Ind. 1969; 9(1): 152–7.
Zurda, M. J. (1992). Introduction to Artificial Neural System Network. West Publishing
Companies, ISBN 0-314-93397-3, St. Paul, MN, USA.
15
1. Introduction
Servomotors are used in a variety of applications in industrial electronics and robotics that
includes precision positioning as well as speed control. Basically, any motor can be used in a
servo system (Kissell, 2002). There are two types of motors: (a) DC motors, and (b) AC
motors. DC motors have better starting torque than AC motors although they are more
expensive than AC motors. Servomotors use feedback controller to control the speed or the
position, or both. The basic continuous feedback control is PID controller. The PID controller
posses good performance but is not adaptive enough. This is appealing when the load is
changed, where the original controller generally cannot maintain the design performance
and thus should be re-designed for the new system conditions (Shieh & Li, 1998).
The pioneering work dealing with expert knowledge that can be well applied to the control
of systems with uncertained, nonlinear dynamics is credited to Zadeh (Zadeh, 1968) who
proposed fuzzy control theory to overcome the weakness of conventional controllers, and
investigated by which owns good robustness (Yu et. al., 2004). Fuzzy systems are capable of
handling complex, non-linear and sometimes mathematically intangible dynamic systems
using simple solutions. Fuzzy logic uses human-like but systematic properties of converting
linguistic control rules based on expert knowledge into automatic control strategies. But, the
response of a fuzzy logic controller is slower than a PID controller. It has been reported in a
number of papers that hybrid of PID or PI, with fuzzy logic in control system can overcome
the set-back of fuzzy logic controller, see (Yeh & Tsao, 1994), (He et. al., 1993), (Ga & Feng,
2005) and (Jee & Koren, 2004). In fuzzy systems, the numerical input values should be first
converted into the corresponding fuzzy representations by using ‘fuzzifiers’. The fuzzy
output are then provided by a fuzzy model, which could be a set of fuzzy logic rules, fuzzy
relations or even a simple fuzzy table, with or without deep fuzzy reasoning. Finally, the
fuzzy output can be converted back into their relevant numerical (crisp) output through
’defuzzifiers’ (Lu, 1996). Basic configuration of fuzzy systems with fuzzifier and defuzzifier
is shown in Figure 1.
Singleton fuzzifier maps a real-valued point x * ∈ U into a fuzzy singleton A’ in U, which
l
has membership value 1 at x* and 0 at all other points in U (Wang, 1997). If y is the center
of the l'th fuzzy set and wl is its height, then the center of average defuzzifier determines y*
as (Wang, 1997).
312 Advanced Strategies for Robot Manipulators
Fig. 1. Basic configuration of fuzzy systems with fuzzifier and defuzzifier (Wang, 1997)
∑
M l
y wl
y* = l =1
(1)
∑
M
l=1
wl
A study on induction machine speed control proved that PID controller’s parameters can be
tuned on-line by an adaptive mechanism based on a fuzzy logic (Bousserhanel et. al., 2006).
It is expected that hybrid of PID and fuzzy logic in control system can overcome the
problem of fuzzy logic controller.
Servomotor controllers need optimization to give a good performance as desired.
Sometimes, it requires time and has big risk in the optimization process. To investigate this
issue, a detail study on servomotor control is conducted. A real plant, constituting of a DC
motor and its controller has been built and this system is modeled and simulated to allow
detail analysis about its control system. The system identification problem is to estimate a
model of a system based on the observed input-output data (Takami & Mahmoudi, 2007).
The expected output of identification is an s-domain model of a real system. System
identification technique is used to get a transfer function of the plant and is used to build a
virtual controller, which is basically the software equivalent of the real controller at the
abstraction level. The virtual controller is optimized, and then the optimized parameters are
applied to the real controller in the real control system.
Therefore, this work discusses the modeling, simulation and hardware implementation of a
DC servomotor controller built using MATLAB/Simulink, and the analysis of controller’s
performance, namely a PID controller, PI controller, fuzzy logic controller, fuzzy-logic-based
self tuning PI controller, and a fuzzy-logic-based self tuning PID controller on the system. In
this work the aim is to improve the controller’s performance using hybrid fuzzy and PID.
Figure 2 outlines the background and purpose of this work.
devices are DAC, differential amplifier, and the IGBT inverter circuit. The measuring devices
provide status of the output responses of the speed and position where the information about
the speed and position is fed through signal conditioning circuit and anti-aliasing filter for
analysis and calculation of the control signal. The speed and position requirements
proportional to the manipulated variable of the controller’s output are fed to a computer.
3. Experimental procedures
3.1 The identification process
The typical identification process consists of stages where the model structure is iteratively
selected, computed and updated for the best model in the structure, and finally the
evaluation of the model’s properties (Takami & Mahmoudi, 2007). The steps can be itemized
as follows:
a. Design an experiment and collect input-output data from the process to be identified.
b. Examine the data. Polish the data by removing trends and outliers, and select useful
portions of the original data. Apply filters to the data to enhance important frequency
ranges.
c. Select and define a model structure (a set of candidate system descriptions), within
which a model is to be found.
d. Compute the best model in the model structure according to the input-output data and
a given criterion for goodness of fit.
e. Examine the properties of the model obtained. If the model is good enough, then stop;
otherwise go back to step c to try another model structure. Attempt other estimation
methods (step 4), or work further on the input-output data (steps a and b).
data=iddata(workspace_output,workspace_input,0.01);
Using MATLAB command, the general command to get the process model is as follows:
model=pem(data, 'PnDZU');
where:
P : (required) for process model
n : 0,1,2, or 3 (required) for the number of poles
D : (optional) to include a time-delay term
Z : (optional) to include a process zero (numerator term)
U : (optional) to indicate possible complex-valued (underdamped) poles
and Y is the mean value of real output, then the fitness value can be obtained from the
formula below, see (Montanari et. al., 2007):
(
⎛ 1 − NORM Y − Yˆ
i _ FIT = ⎜
) ⎞⎟ ∗ 100 (2)
⎝
(
⎜ NORM Y − Y ) ⎟
⎠
K P = 0.45 * K Pu
1 (2a)
KI =
0.83 * Tu
for the PI controller, and
K P = 0.6 * K Pu
Tu
KD = (2b)
8
2
KI =
Tu
for the PID controller.
The fuzzy sets and their corresponding membership functions for input (error, and change
of error) are shown in Figure 8, and for output are shown in Figure 9.
Fig. 10. Block diagram of fuzzy-logic-based self-tuning PI for the speed controller [11]
The fuzzy sets and their corresponding membership functions for input (error, eω ( k ) and
change of error, Δeω ( k ) ) and output (h) are shown in Figure 11. The rules for FLBPI and
FLBPID (KP and KI) are shown in Table II. The value of KD is constant based on Ziegler-
Nichols method in PID tuning.
Fig. 11. Fuzzy sets and their corresponding membership functions, (Mannan et. al., 2004).
Error
Change of Error
N Z P
N S B S
Z S B S
P S B S
N: Negative, Z: Zero, P: Positive, S: Small, B: Big
Table II. Fuzzy Rules Base for KP and KI in Hybrid (Mannan et. al., 2004).
The values of KP, KI, and KD for hybrid controllers are obtained from the following formula:
K p = h.K Pm ; K I = h 2 .K Im ; K D = K Dm (3)
Proportional based controller is used for the position control since the requirement is such
that the motor would rotate in one direction. The proportional controller for position (KPP) is
tuned using experimental method with no overshoot criteria.
Integral of Absolute value of Error (IAE) is used as a performance index for overall speed
control (IAEv) in which the formula is as follows, see (Marlin, 2000):
90
IAEv = ∫ SP(t ) − PV (t ) dt
0
(4)
Development of Fuzzy-logic-based Self Tuning PI Controller for Servomotor 321
Integral of Time Absolute value of Error (ITAE) is used as a performance index for position
control (ITAEp), the first 8-second of starting speed control (ITAEvp), and the 9-second of
loading speed control (ITAEvpl) which the formulas are as follows, see (Marlin, 2000):
8
ITAEvp = t ∫ SP(t ) − PV (t ) dt (5a)
0
23
ITAEvpl = t ∫ SP(t ) − PV (t ) dt (5b)
14
90
ITAEp = t ∫ SP(t ) − PV (t ) dt (5c)
0
and the fitness value is obtained from the basic formula as follows, see (Xiu & Ren, 2004):
ITAE − ITAE (i )
f (i ) = (6)
∑ ITAE
i
max − ITAE ( i )
Based on Eq (6), the fitness values for overall speed (fv), first 8-second starting speed (fvp), 9-
second loading speed (fvpl) and position (fp) are as follows:
4. Experimental results
The comparison on the effectiveness of modeling the DC motor based on the fitness value is
presented in Table III.
Selecting the best process model in Table III, the transfer function (s-domain model) of the
plant based on the best model is as follows:
322 Advanced Strategies for Robot Manipulators
Fig. 12. Graphical comparison of actual (real time) and estimation (s-model)
The relevant parameters were obtained during the simulation (offline optimization) using
the transfer function as shown in Eq (8). The comparison on the effectiveness of
implementing FLBPI as compared to FLBPID, FLC, PID, and PI controller based on the
performance metrics for real time experiment is presented in Table IV.
The ability to understand the influence of the different controllers on the servomotor speed
and achieving its target position is one of the important aspects of this study. It is noted that
the speed response with the FLBPID-type controller has a relatively good settling time, but
Development of Fuzzy-logic-based Self Tuning PI Controller for Servomotor 323
the fitness values for the motor during starting and loading is not good enough due to the
fact that the noise is a bit larger as compared to FLBPI, albeit the fitness value of the overall
speed for the FLBPI and the FLBPID are similar. A closer observation at the starting and
loading performances during the real time experimentations, FLBPI performances is
relatively better as compared to the PI, PID, FLBPID or FLC for speed control. A similar
performance is also observed in the fitness value of position for real time experiment, where
the FLBPI is relatively better as compared to the PI, PID, FLBPI or FLC for position control.
PERFORMANCE
NO. PI PID FLC FLBPI FLBPID
INDEX
1 Overshoot (%) 0.0000 0.0000 11.4299 9.5200 0.0000
2 Settling Time (sec) 3.4900 3.6200 8.9400 2.1700 1.1100
3 #SSEP (%) 0.3914 0.0334 0.5290 0.0896 0.0083
4 Undershoot (%) 38.4279 36.5108 41.5249 41.2515 40.7199
5 IAE v all 1.15E+03 1.22E+03 1.34E+03 7.12E+02 7.13E+02
6 ITAE v start 2.54E+02 1.84E+02 5.17E+02 1.45E+02 1.49E+02
7 ITAE v load 2.28E+03 1.59E+03 2.42E+03 1.54E+03 1.62E+03
8 ITAE p 1.40E+03 1.30E+03 1.49E+03 1.29E+03 1.30E+03
9 fv all 1.2137 0.7666 0.0000 4.0140 4.0057
10 fv start 1.9715 2.4915 0.0000 2.7820 2.7550
11 fv load 0.0528 0.3132 0.0000 0.3321 0.3019
12 fp 1.3433 2.8358 0.0000 2.9851 2.8358
#SSEP: Steady State Error of Position
Table IV. Performance Index Comparison for Real time experiment
Comparing the conventional and hybrid controllers based on fitness values in real time
experiments, the PID performs better than the PI for speed and position control in the case
for conventional controllers. Interestingly, for hybrid controllers, the FLBPI is better than the
FLBPID for speed and position control, whilst the FLBPI is better than the PID for speed and
position control. This shows that the performance has improved when having the hybrid
fuzzy and PI as a controller. In the standalone condition and using default parameters, the
FLC is not as good as its competitors for both speed and position controls.
Plots for the speed control in response to a set-point specified in both simulation and real
time experiment for FLBPI controller are presented in Figure 13 and for the position control
are presented in Figure 14. Comparing the simulation and real time experiment results as
shown in Figures 13 and 14, the performances are not exactly the same but the output
patterns are similar and the results in real time experiments are validly representing the real
system’s performance. This shows that identification (s-modeling) is an estimation of real
hardware plant.
Plots for the speed control in response to a set-point specified in the real time experiment for
FLBPI compared to PID controller are presented in Figure 15 and for the position control are
as presented in Figure 16. A sudden change in load requirements that happens at t=15
seconds causes the speed to fall and then rises and stabilized at the original level within a
reasonable range of time. This demonstrates the action of the controllers to regulate the
speed.
324 Advanced Strategies for Robot Manipulators
Fig. 13. Speed control of DC servomotor using FLBPI in real time experiment vs simulation
Fig. 14. Position control of DC servomotor using FLBPI in real time vs simulation
Plots for the speed control in response to a set-point specified in the real time experiment for
FLBPI compared to FLC are presented in Figure 17 and for the position control are
presented in Figure 18. A sudden change in load requirements that happens at t=15 seconds
causes the speed to fall and then rises and stabilized at the original level within a reasonable
range of time. Interestingly, it can be seen the delay in speed and position when using FLC
alone as the controller. This demonstrates the action of the controllers to regulate the speed,
and the effect of the PI in improving the response time of the hybrid controller.
Development of Fuzzy-logic-based Self Tuning PI Controller for Servomotor 325
Fig. 15. Speed control of DC servomotor using FLBPI vs PID in real time experiment
Fig. 16. Position control of DC servomotor using FLBPI vs PID in real time experiment
326 Advanced Strategies for Robot Manipulators
Fig. 17. Speed control of DC servomotor using FLBPI vs FLC in real time experiment
Fig. 18. Position control of DC servomotor using FLBPI vs FLC in real time experiment
5. Conclusions
This work discusses the modeling of a DC servomotor from gray box identification and
performance evaluations of real time experiment using a fuzzy-logic-based self tuning PI
controller as compared to fuzzy-logic-based self tuning PID controller, fuzzy logic
controller, PID controller and PI controller on the DC servomotor system. Here, the s-model
transfer function of a DC servomotor is identified as a third order transfer function without
Development of Fuzzy-logic-based Self Tuning PI Controller for Servomotor 327
zero. This identification is useful in offline optimization of a DC servomotor control, and the
performance of s-modeled and real DC servomotor are similar.
Two control modes are applied in sequential to the plant: speed control in the position
control loop. The open loop characteristic of a DC servomotor is sampled at 0.01 sec interval
by a DAQ based on Simulink platform. Some controllers are applied to both s-modeled and
real DC servomotor. It has been demonstrated that defining the fuzzy rules for the fuzzy
logic-based self-tuning PI/PID controller is a much simpler task than for fuzzy logic
controller. Based on the real time experiment, hybrid controllers are better than
conventional controllers and fuzzy logic-based self-tuning PI controller tends to be the
better choice for implementation in the hybrid controller.
6. References
Bousserhanel, I.K., Hazzabl, A., Rahli, M., Kamli, M. and Mazari, B. (2006). Adaptive PI
Controller using Fuzzy System Optimized by Genetic Algorithm for Induction
Motor Control, presented at CIEP-IEEE, Puebla, Mexico.
Fulford, C., Maggiore, M. and Apkarian, J. (2009). Control of a 5DOF Magnetically Levitated
Positioning Stage, IEEE Transaction on Control System Technology, vol. 17 no. 4,
pp. 844-852.
Ga, X. and Feng, Z.J. (2005). Design study of an adaptive fuzzy-PD controller for pneumatic
servo system, Control Engineering Practice, vol. 13, issue 1, pp 55-65.
He, S.Z., Tan S., and Xu, F.L (1993). Fuzzy self-tuning of PID controllers, Fuzzy Sets and
Systems, vol. 56, pp. 37-46.
Jee, S. and Koren, Y. (2004). Adapative fuzzy-logic controller for feed drive of a CNC
machine tool, Mechatronics, vol: 14, pp. 299-326.
Kissell, T. (2002). Motor Control Technology for Industrial Maintenance. New Jersey 07458:
Prentice-Hall, Inc., Upper Saddle River.
Lacevic, B., Velagic, J. and Osmic, N. (2007). Design of Fuzzy Logic Based Mobile Robot
Position Controller Using Genetic Algorithm," presented at International
Conference on Advanced Intelligent Mechatronics, IEEE/ASME 2007, vol., no.,
pp.1-6.
Lu, Y.Z. (1996). Industrial Intelligent Control, Fundamentals and Applications. West Sussex
PO19 1UD, England: John Wiley&Sons Ltd.
Mannan, M.A., et al. (2004). Fuzzy-Logic-Based Self-Tuning PI Controller for Speed Control
of Indirect Field-Oriented Induction Motor Drive, in SICE Annual Conference.
Sapporo: Hokkaido Institute of Technology.
Marlin, T.E. (2000). Process Control: Designing Processes and Control Systems for Dynamic
Performance, 2nd International ed. Singapore: McGraw-Hill Book Companies, Inc.
Montanari, M., Peresada, S.M., Rossi, C. and Tilli, A. (2007). Speed Sensorless Control of
Induction Motors Based on a Reduced-Order Adaptive Observer," IEEE
Transaction on Control System Technology, vol. 15 no. 6, pp. 1049-1064.
Shieh, M. Y. and Li, T.H.S. (1998). Design and implementation of integrated fuzzy logic
controller for servomotor system, Mechatronics, vol. 8, pp. 217-240.
Takami, K.M. and Mahmoudi, J. (2007). Identification of a Best Thermal Formula and Model
for Oil and Winding of Power Transformers Using Prediction Methods, presented
at The 48th Scandinavian Conference on Simulation and Modeling (SIMS 2007), pp.
182-188.
328 Advanced Strategies for Robot Manipulators
Wang, L.X. (1997). A Course in Fuzzy Systems and Control. New Jersey 07458: Prentice-
Hall, Inc, A Division of Simon and Schuster Upper Saddle River, New Jersey 07458.
Xiu, Z. and Ren, G. (2004). Optimization Design of TS-PID Fuzzy Controllers Based on
Genetic Algorithms," presented at 5th World Congress on Intelligent Control and
Automation, Hangzhou, P.R. China.
Yeh, E.C., and Tsao, Y.J. (1994). A fuzzy preview control scheme of active suspension for
rough road, Inc. Journal of Vehicle Design, vol. 15, pp. 166-180.
Yu, G.R., Hwang, R.C. and Lin, C.P. (2004). Optimal Fuzzy Control of the Spindle Motor in
a CD-ROM Drive Using Genetic Algorithms, presented at Asian Control
Conference, vol. 5, pp. 51-57.
Zadeh, L.A. (1988). Fuzzy Logic, Computer, vol. 21 no. 24, pp. 83-93.
16
1. Introduction
State estimation and control over sensor networks is a problem met in several applications
such as surveillance and condition monitoring of large-scale systems, multi-robot systems
and cooperating UAVs. In sensor networks the simplest kind of architecture is centralized.
Distributed sensors send measurement data to a central processing unit which provides the
state estimate for the monitored system. Such an approach has several weaknesses: (i) it
lacks fault tolerance: if the central processing unit is subject to a fault then state estimation
becomes impossible, (ii) communication overhead often prohibits proper functioning in case
of a large number of distributed measurement units. On the other hand decentralized
architectures are based on the communication between neighboring measurement units.
This assures scalability for the network since the number of messages received or sent by
each measurement unit is independent of the total number of measurement units in the
system. It has been shown that scalable decentralized state estimation can be achieved for
linear Gaussian models, when the measurements are linear functions of the state and the
associated process and measurement noise models follow a Gaussian distribution (Nettleton
et al. 2003). A solution to decentralized sensor fusion over sensor networks with the use of
distributed Kalman Filtering has been proposed in (Olfati-Saber 2006), (Watanabe &
Tzafestas 1992), (Olfati-Saber 2005), (Gan & Harris 2001), (Gao et al. 2009). Distributed state
estimation in the case of non-Gaussian models has been studied in (Rosencrantz et al. 2003)
where decentralized sensor fusion with the use of distributed particle filters has been
proposed in several other research works (Mahler 2007), (Makarenko & Durrant-Whyte
2006), (Deming & Perlovsky 2007).
In this paper autonomous navigation of UAVs will be examined and a solution to this
problem will be first attempted with the use of the Extended Information Filter and the
Unscented Kalman filter (Shima et al. 2007), (Lee et al. 2008), (Lee et al. 2008), (Vercauteren
& Wang 2005). Comparatively, autonomous UAV navigation with the use of the Distributed
Particle Filter will be studied. This problem belongs to the wider area of multi-source multi-
target tracking (Coué et al. 2006), (Hue et al. 2002), (Ing & Coates 2005), (Coué et al. 2003),
(Morelande & D. Mušicki 2005). Subproblems to be solved for succeeding autonomous
navigation of the UAVs are: (i) implementation of sensor fusion with the use of distributed
filtering. In this approach the goal is to consistently combine the local particle distribution
with the communicated particle distribution coming from particle filters running on nearby
330 Advanced Strategies for Robot Manipulators
measurement stations (Caballero et al. 2008). It is assumed that each local measurement
station runs its own local filter and communicates information to other measurement
stations close to it. The motivation for using particle filters is that they can represent almost
arbitrary probability distributions, thus becoming well-suited to accommodate the types of
uncertainty and nonlinearities that arise in the distributed estimation (Rigatos 2009a),
(Rigatos 2009b) (ii) nonlinear control of the UAVs based on the state estimates provided by
the particle filtering algorithm. Various approaches have been proposed for the UAV
navigation using nonlinear feedback control (Ren & Beard 2004), (Beard et al. 2002), (Singh
& Fuller 2001). The paper proposes flatness-based control for the UAV models. Flatness-
based control theory is based on the concept of differential flatness and has been
successfully applied to several nonlinear dynamical systems. Flatness-based control for a
UAV helicopter-like model has been developed in (Léchevin & Rabbath 2006), assuming
that the UAV performs manoeuvres at a constant altitude.
The paper proposes first the Extended Information Filter (EIF) and the Unscented
Information Filter (UIF) as possible approaches for fusing the state estimates provided by
the local monitoring stations, under the assumption of Gaussian noises. The EIF and UIF
estimated state vector is in turn used by a flatness-based controller that makes the UAV
follow the desirable trajectory. The Extended Information Filter is a generalization of the
Information Filter in which the local filters do not exchange raw measurements but send to
an aggregation filter their local information matrices (local inverse covariance matrices) and
their associated local information state vectors (products of the local information matrices
with the local state vectors) (Shima et al. 2007), (Lee et al. 2008). In the case of the Unscented
Information Filter there is no linearization of the UAVs observation equation. However the
application of the Information Filter algorithm is possible through an implicit linearization
which is performed by approximating the Jacobian matrix of the system’s output equation
by the product of the inverse of the state vector’s covariance matrix (which can be also
associated to the Fisher Information matrix) with the cross-correlation covariance matrix
between the system’s state vector and the system’s output (Lee et al. 2008)], (Vercauteren &
Wang 2005). Again, the local information matrices and the local information state vectors are
transferred to an aggregation filter which produces the global estimation of the system’s
state vector.
Next, the Distributed Particle Filter (DPF) is proposed for fusing the state estimates
provided by the local monitoring stations (local filters). The motivation for using DPF is that
it is well-suited to accommodate non-Gaussian measurements. A difficulty in implementing
distributed particle filtering is that particles from one particle set (which correspond to a
local particle filter) do not have the same support (do not cover the same area and points on
the samples space) as particles from another particle set (which are associated with another
particle filter) (Ong et al. 2008), (Ong et al. 2006). This can be resolved by transforming the
particles sets into Gaussian mixtures, and defining the global probability distribution on the
common support set of the probability density functions associated with the local filters. The
state vector which is estimated with the use of the DPF is used again by a flatness-based
controller to make each UAV follow a desirable flight path.
The structure of the chapter is as follows: in Section 2 the Distributed Extended Kalman
Filter (Extended Information Filter) is studied. In Section 3, the Distributed Unscented
Kalman Filter (Unscented Information Filter) is analyzed and its use for distributed sensor
fusion and state estimation is explained. In Section 4 Distributed Particle Filtering for sensor
fusion-based state estimation will be analyzed. In Section 5 nonlinear control will be
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 331
x( k + 1) = φ ( x( k )) + L( k )u( k ) + w( k )
(1)
z( k ) = γ ( x( k )) + v( k )
where x∈Rm×1 is the system’s state vector and z∈Rp×1 is the system’s output, while w(k) and
v(k) are uncorrelated, zero-mean, Gaussian zero-mean noise processes with co- variance
matrices Q(k) and R(k) respectively. The operators φ(x) and γ(x) are φ (x) = [φ1(x), φ2(x),
···,φm(x)]T, and γ (x) = [γ1(x), γ2(x), ··· , γp(x)]T, respectively. It is assumed that φ and γ are
sufficiently smooth in x so that each one has a valid series Taylor expansion.
Following a linearization procedure, φ is expanded into Taylor series about x̂ :
φ ( x( k )) = φ ( xˆ ( k )) + Jφ ( xˆ ( k ))[ x( k ) − xˆ ( k )] + (2)
γ ( x( k )) = γ ( xˆ − ( k )) + Jγ [ x( k ) − xˆ − ( k )] + (4)
where xˆ − ( k ) is the estimation of the state vector x(k) before measurement at the k-th instant
to be receivec and xˆ ( k ) is the updated estimation of the state vector after measurement at
the k-th instant has been received. The Jacobian Jγ(x) is
332 Advanced Strategies for Robot Manipulators
⎛ ∂γ 1 ∂γ 1 ∂γ 1 ⎞
⎜ ∂x ∂x2 ∂xm ⎟⎟
⎜ 1
⎜ ∂γ 2 ∂γ 2 ∂γ 2 ⎟
∂γ ⎜ ⎟
Jγ ( x ) = |x = xˆ − ( k ) = ⎜ ∂x1 ∂x2 ∂xm ⎟ (5)
∂x ⎜ ⎟
⎜ ⎟
⎜ ∂γ p ∂γ p ∂γ p ⎟
⎜ ∂x ∂x2 ∂xm ⎟⎠
⎝ 1
The resulting expressions create first order approximations of φ and γ. Thus the linearized
version of the system is obtained:
x( k + 1) = φ ( xˆ ( k )) + Jφ ( xˆ ( k ))[ x( k ) − xˆ ( k )] + w( k )
(6)
z( k ) = γ ( xˆ − ( k )) + Jγ ( xˆ − ( k ))[ x( k ) − xˆ − ( k )] + v( k )
Now, the EKF recursion is as follows: First the time update is considered: by xˆ ( k ) the
estimation of the state vector at instant k is denoted. Given initial conditions xˆ (0) and P−(0)
the recursion proceeds as:
• Measurement update. Acquire z(k) and compute:
P − ( k + 1) = Jφ ( xˆ ( k ))P( k ) JφT ( xˆ ( k )) + Q( k )
(8)
xˆ − ( k + 1) = φ ( xˆ ( k )) + L( k )u( k )
Y ( k ) = P −1 ( k ) = I ( K )
(9)
yˆ ( k ) = P −−1 xˆ ( k ) = Y ( k )xˆ ( k )
The update equation for the Information Matrix and the Information state vector are given by
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 333
Y ( k ) = P − ( k )−1 + JγT ( k )R −1 ( k ) Jγ ( k )
(10)
= Y − ( k ) + I( k )
yˆ ( k ) = yˆ − ( k ) + JγT R( k )−1[ z( k ) − γ ( x( k )) + Jγ xˆ − ( k )]
(11)
= yˆ − ( k ) + i( k )
where
The predicted information state vector and Information matrix are obtained from
yˆ − ( k ) = P − ( k )−1 xˆ − ( k )
(13)
Y − ( k ) = P − ( k )−1 = [ Jφ ( k )P − ( k ) Jφ ( k )T + Q( k )]−1
The Extended Information Filter is next formulated for the case that multiple local sensor
measurements and local estimates are used to increase the accuracy and reliability of the
estimation. It is assumed that an observation vector zi(k) is available for N different sensor
sites i = 1, 2, ··· ,N and each sensor observes a common state according to the local
observation model, expressed by
zi ( k ) = γ ( x( k )) + v i ( k ), i = 1, 2, ,N (14)
where the local noise vector vi(k)~N(0,Ri) is assumed to be white Gaussian and uncorrelated
between sensors. The variance of a composite observation noise vector vk is expressed in
terms of the block diagonal matrix
334 Advanced Strategies for Robot Manipulators
N ^_
i( k ) = ∑ Jγi T ( k )R i ( k )−1[ zi ( k ) − γ k ( x( k )) + Jγi ( k ) x ( k )]
i =1
(16)
N
I ( k ) = ∑ Jγ ( k )R ( k ) Jγ ( k )
iT i −1 i
i =1
Using Eq. (16) the update equations for fusing the local state estimates become
N
yˆ ( k ) = yˆ − ( k ) + ∑ Jγi T ( k )R i ( k )−1[ zi ( k ) − γ k ( x( k )) + Jγi ( k )xˆ _ ( k )]
i =1
(17)
N
Y ( k ) = Y ( k ) + ∑ J γ ( k )R ( k ) J γ ( k )
− iT i −1 i
i =1
It is noted that in the Extended Information Filter an aggregation (master) fusion filter
produces a global estimate by using the local sensor information provided by each local filter.
Fig. 2. Fusion of the distributed state estimates with the use of the Extended Information Filter
As in the case of the Extended Kalman Filter the local filters which constitute the Extended
Information Filter can be written in terms of time update and a measurement update equation.
Measurement update: Acquire z(k) and compute
y − ( k + 1) = P − ( k + 1)−1 xˆ − ( k + 1) (21)
It is noted that the local estimates are suboptimal and also conditionally independent given
their own measurements. The global estimate and the associated error covariance for the
aggregate fusion filter can be rewritten in terms of the computed estimates and covariances
from the local filters using the relations
336 Advanced Strategies for Robot Manipulators
For the general case of N local filters i = 1, ··· , N, the distributed filtering architecture is
described by the following equations
N
P( k )−1 = P − ( k )−1 + ∑[ Pi ( k )−1 − Pi− ( k )−1 ]
i =1
N
(24)
xˆ ( k ) = P( k )[ P ( k )−1 xˆ − ( k ) + ∑( Pi ( k )−1 xˆ i ( k ) − Pi− ( k )−1 xˆ i− ( k ))]
−
i =1
It is noted that the global state update equation in the above distributed filter can be written
in terms of the information state vector and of the information matrix
N
yˆ ( k ) = yˆ − ( k ) + ∑( yˆ i ( k ) − yˆ i− ( k ))
i =1
N
(25)
Yˆ ( k ) = Yˆ ( k ) + ∑(Yˆi ( k ) − Yˆi− ( k ))
−
i =1
The local filters provide their own local estimates and repeat the cycle at step k + 1. In turn
the global filter can predict its global estimate and repeat the cycle at the next time step k + 1
when the new state x̂ (k + 1) and the new global covariance matrix P(k + 1) are calculated.
From Eq. (24) it can be seen that if a local filter (processing station) fails, then the local
covariance matrices and the local state estimates provided by the rest of the filters will
enable an accurate computation of the system’s state vector.
x( k + 1) = φ ( x( k )) + L( k )U ( k ) + w( k )
(26)
y( k ) = γ ( x( k )) + v( k )
where x(k)∈Rn is the system’s state vector, y(k)∈Rm is the measurement, w(k)∈Rn is a
Gaussian process noise w(k)~N(0,Q(k)), and v(k)∈Rm is a Gaussian measurement noise
v(k)~N(0,R(k)). The mean and covariance of the initial state x(0) are m(0) and P(0),
respectively.
Some basic operations performed in the UKF algorithm (Unscented Transform) are
summarized as follows:
1. Denoting the current state mean as x̂ , a set of 2n+1 sigma points is taken from the
columns of the n×n matrix ( n + λ )Pxx as follows:
x 0 = xˆ
x i = xˆ + [ ( n + λ )Pxx ]i , i = 1, ,n (27)
x = xˆ − [ (n + λ )Pxx ]i , i = n + 1,
i
, 2n
λ λ
W0( m ) = W0( c ) =
(n + λ ) (n + λ ) + (1 − α 2 + b )
(28)
1 1
Wi( m ) = , i = 1, , 2n Wi( c ) =
2(n + λ ) 2(n + λ )
338 Advanced Strategies for Robot Manipulators
where i = 1, 2, ··· ,2n and λ = α 2(n + κ) − n is a scaling parameter, while α, β and κ are
constant parameters. Matrix Pxx is the covariance matrix of the state x.
2. Transform each of the sigma points as
2n
Pxz = ∑W
i =0
i
(c )
( x i − xˆ )( zi − zˆ )T (31)
The matrix square root of positive definite matrix Pxx means a matrix A = Pxx such that
Pxx = AAT and a possible way for calculation is SVD.
Next the basic stages of the Unscented Kalman Filter are given:
As in the case of the Extended Kalman Filter and the Particle Filter, the Unscented Kalman
Filter also consists of prediction stage (time update) and correction stage (measurement
update) (Julier et al. 2004), (Särrkä 2007).
Time update: Compute the predicted state mean x̂ −(k) and the predicted covariance Pxx−(k) as
Measurement update: Obtain the new output measurement zk and compute the predicted
mean ẑ (k) and covariance of the measurement Pzz(k), and the cross covariance of the state
and measurement Pxz(k)
Then compute the filter gain K(k), the state mean x̂ (k) and the covariance Pxx(k), conditional
to the measurement y(k)
K (k ) = Pxz (k ) Pzz−1 (k )
xˆ (k ) = xˆ − (k ) + K (k )[ z (k ) − zˆ (k )] (34)
The filter starts from the initial mean m(0) and covariance Pxx(0). The stages of state vector
estimation with the use of the Unscented Kalman Filter algorithm are depicted in Fig. 6.
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 339
⎛ xˆ − ( k ) ⎞ ⎛ P− (k) 0 ⎞
xˆ α − ( k ) = ⎜⎜ − ⎟⎟ , Pα − ( k ) = ⎜⎜ ⎟ (35)
⎝w ˆ (k)⎠ ⎝ 0 Q ( k ) ⎟⎠
−
As in the case of local (lumped) Unscented Kalman Filters, a set of weighted sigma points
Xαi − ( k ) is generated as
Xα− ,0 ( k ) = xˆ α − ( k )
Xα− , i ( k ) = xˆ α − ( k ) + [ (nα + λ )Pα− ( k − 1)]i , i = 1, ,n (36)
Xα , i ( k ) = xˆα ( k ) + [ (nα + λ )Pα ( k − 1)]i , i = n + 1,
− − −
, 2n
λ λ
W0( m ) = W0( c ) =
nα + λ ( nα + λ ) + (1 − α 2 + β )
(37)
1 1
Wi( m ) = , i = 1, , 2 nα Wi(C ) = , i = 1, , 2 nα
2( nα + λ ) 2( nα + λ )
where β is again a constant parameter. The equations of the prediction stage (measurement
update) of the information filter, i.e. the calculation of the information matrix and the
information state vector of Eq. (13) now become
2 nα
yˆ − ( k ) = Y − ( k ) ∑Wim Xix ( k )
i =0 (38)
− − −1
Y (k) = P (k)
where Xix are the predicted state vectors when using the sigma point vectors Xiw in the
state equation Xix ( k + 1) = φ ( Xiw − ( k )) + L( k )U ( k ) . The predicted state covariance matrix is
computed as
2 nα
As noted, the equations of the Extended Information Filter (EIF) are based on the linearized
dynamic model of the system and on the inverse of the covariance matrix of the state vector.
However, in the equations of the Unscented Kalman Filter (UKF) there is no linearization of
the system dynamics, thus the UKF cannot be included directly into the EIF equations. In-
stead, it is assumed that the nonlinear measurement equation of the system given in Eq. (1)
can be mapped into a linear function of its statistical mean and covariance, which makes
possible to use the information update equations of the EIF. Denoting Yi(k) = γ( Xix (k)) (i.e.
the output of the system calculated through the propagation of the i-th sigma point Xi
through the system’s nonlinear equation) the observation covariance and its cross-
covariance are approximated by
−
PYY ( k ) = E[( z( k ) − zˆ _ ( k ))( z( k ) − zˆ _ ( k ))T ]
(40)
≅ Jγ ( k )P − ( k ) Jγ ( k )T
−
PXY ( k ) = E[( x( k ) − xˆ _ ( k ))( z( k ) − zˆ _ ( k ))T ]
(41)
≅ P − ( k ) Jγ ( k )T
where z(k) = γ(x(k)) and Jγ(k) is the Jacobian of the output equation γ(x(k)). Next, mul-
tiplying the predicted covariance and its inverse term on the right side of the information
matrix Eq. (12) and replacing P(k)Jγ(k)T with PXY−
( k ) (k) gives the following representation of
the information matrix (Lee et al. 2008), (Lee et al. 2008), (Vercauteren &Wang 2005)
I ( k ) = Jγ ( k )T R( k )−1 Jγ ( k )
= P − ( k )−1 P − ( k ) Jγ ( k )T R( k )−1 Jγ− ( k )P − ( k )T ( P − ( k )−1 )T (42)
− −1 −1 − −1 T
= P ( k ) PXY ( k )R( k ) PXY ( k ) ( P ( k ) ) T
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 341
where P−(k)−1 is calculated according to Eq. (39) and the cross-correlation matrix PXY(k) is
calculated from
2 nα
−
PXY (k) = ∑W
i =0
i
(c )
[ X ix ( k ) − xˆ − ( k )][Yi ( k ) − zˆ − ( k )]T (43)
i( k ) = J γT ( k ) R( k )−1 [ z( k ) − γ ( x( k )) + J γT ( k )xˆ _ ( k )]
= P − ( k )−1 P − ( k ) J γT ( k ) R( k )−1 ⋅
(44)
⋅ [ z( k ) − γ ( x( k )) + J γT ( k )xˆ _ ( k )( P − ( k ))T ( P − ( k )−1 )T xˆ _ ( k )]
= P − ( k )−1 PXY
−
( k ) R( k )−1 [ z( k ) − γ ( x( k )) + PXY
−
( k )( P − ( k )−1 )T xˆ _ ( k )]
H ( k )T = P − ( k )−1 PXY
−
(k) (45)
In terms of the measurement matrix H(k) the information contributions equations are
written as
i( k ) = H T ( k ) R( k )−1 [ z( k ) − γ ( x( k )) + H ( k )xˆ − ( k )]
(46)
I ( k ) = H T ( k ) R ( k ) −1 H ( k )
The above procedure leads to an implicit linearization in which the nonlinear measurement
equation of the system given in Eq. (1) is approximated by the statistical error variance and
its mean
z( k ) = h( x( k )) H ( k )x ( k ) + u ( k ) (47)
zi ( k ) = H i ( k )x( k ) + ui ( k ) + vi ( k ) (48)
where the noise vector vi(k) is taken to be white Gaussian and uncorrelated between sensors.
The variance of the composite observation noise vector vk of all sensors is written in terms of
the block diagonal matrix R(k) = diag[R1(k)T, ··· ,RN(k)T]T. Then one can define the local
information matrix Ii(k) and the local information state vector ii(k) at the i-th sensor, as follows
342 Advanced Strategies for Robot Manipulators
ii ( k ) = H iT ( k ) Ri ( k )−1 [ zi ( k ) − γ i ( x( k )) + H i ( k )xˆ − ( k )]
(49)
I i ( k ) = H iT ( k )Ri ( k )−1 H i ( k )
Since the information contribution terms have group diagonal structure in terms of the
innovation and measurement matrix, the update equations for the multiple state estimation
and data fusion are written as a linear combination of the local information contribution terms
N
yˆ ( k ) = yˆ − ( k ) + ∑ii ( k )
i =1
(50)
N
Y ( k ) = Y ( k ) + ∑I i ( k )
−
i=1
Then using Eq. (38) one can find the mean state vector for the multiple sensor estimation
problem.
As in the case of the Unscented Kalman Filter, the Unscented Information Filter running at
the i-th measurement processing unit can be written in terms of measurement update and time
update equations:
Measurement update: Acquire measurement z(k) and compute
Y ( k ) = P − ( k )−1 + H T ( k )R −1 ( k )H ( k )
(51)
or Y ( k ) = Y − ( k ) + I ( k ) where I ( k ) = H T ( k )R −1 ( k )H ( k )
yˆ ( k ) = yˆ − ( k ) + H T ( k )R −1 ( k )[ z( k ) − γ ( xˆ ( k )) + H ( k )xˆ − ( k )]
(52)
or yˆ ( k ) = yˆ − ( k ) + i( k )
Time update: Compute
Y − ( k + 1) = ( P − ( k + 1))−1
2 nα (53)
where P − ( k + 1) = ∑W
i =0
i
(c )
[ X ix ( k + 1) − xˆ − ( k + 1)][ X ix ( k + 1) − xˆ − ( k + 1)]T
2 nα
yˆ ( k + 1) = Y ( k + 1) ∑ W i( m ) X ix ( k + 1)
i=0
(54)
where X ( k + 1) = φ ( X ( k )) + L( k )U ( k )
x
i
w
i
Using Eq. (55), each local information state contribution ii and its associated information
matrix Ii at the i-th filter are rewritten in terms of the computed estimates and covariances of
the local filters
and also in terms of the information state vector and of the information state covariance matrix
N
yˆ ( k ) = yˆ − ( k ) + ∑( yˆ i ( k ) − yˆ i− ( k ))
i =1
N
(58)
Y ( k ) = Y ( k ) + ∑[Yi ( k ) − Yi− ( k )]
−
i =1
344 Advanced Strategies for Robot Manipulators
State estimation fusion based on the Unscented Information Filter (UIF) is fault tolerant.
From Eq. (57) it can be seen that if a local filter (processing station) fails, then the local
covariance matrices and local estimates provided by the rest of the filters will enable a
reliable calculation of the system’s state vector. Moreover, it is and computationally efficient
comparing to centralized filters and results in enhanced estimation accuracy.
N ξ
have an equal weighted contribution to the approximation of p(x). A more general approach
would be if weight factors were assigned to the points ξi, which will also satisfy the
∑
N
normality condition i =1
w i = 1 . In the latter case
N
p( x ) p N ( x ) = ∑w iδ i ( x ) (59)
ξ
i =1
If p(ξi) is known then the probability P(x) can be approximated using the discrete values of
the p.d.f. p(ξi) = wi. If sampling over the p.d.f. p(x) is unavailable, then one can use a p.d.f.
p( x ) with similar support set, i.e. p( x ) = 0 ⇒ p( x ) = 0 . Then it holds
p( x )
E(φ ( x )) = ∫φ ( x )p( x )dx = ∫φ ( x )p( x ) dx . If the N samples of p( x ) are available at the points
p( x )
p(ξ i )
ξ1 ξ N , i.e. p(ξ )i = δ ξ i ( x ) and the weight coefficients wi are defined as w i = , then it is
p(ξ i )
easily shown that
N ⎧⎪ ξ 1:N ∼ p( x )
E(φ ( x ))
i =1
∑w φ (ξ
i
), where ⎨ i i
i i
⎪⎩ w = p( x ) / p( x )
(60)
The meaning of Eq. (60) is as follows: assume that the p.d.f. p(x) is unknown (target
distribution), however the p.d.f. p( x ) (importance law) is available. Then, it is sufficient to
sample on p( x ) and find the associated weight coefficients wi so as to calculate E(φ(x)).
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 345
N
p( x( k − 1)|Z − ) = ∑wki − 1δ ( x( k − 1)) (61)
ξ ki − 1
i =1
while from Bayes formula it holds p(x(k)|Z−) = ∫p(x(k)|x(k − 1))p(x(k − 1)|Z−)dx. Using also
Eq. (61) one finally obtains
N
p( x( k )|Z − ) = ∑wki − 1δ ( x( k ))
ξi
i =1 k− (62)
with ξ i − ∼ p( x( k )|x( k − 1) = ξ ki − 1 )
k
The meaning of Eq. (62) is as follows: the state equation of the system is executed N times,
starting from the N previous values of the state vectors x( k − 1) = ξ ki − 1
xˆ ( k + 1) = φ ( xˆ ( k )) + L( k )u( k ) + w( k )
(63)
z( k ) = γ ( xˆ ( k )) + v( k )
Thus estimations of the current value of the state vector xˆ ( k ) are obtained, and
consequently the mean value of the state vector will be given from Eq. (62). This means that
the value of the state vector which is calculated in the prediction stage is the result of the
weighted averaging of the state vectors which were calculated after running the state
equation, starting from the N previous values of the state vectors ξ ki − 1 .
Substituting Eq. (62) into Eq. (64) and after intermediate calculations one finally obtains
N
p( x( k )|Z ) = ∑wki δ ( x( k ))
ξi
i =1 k−
wi − p( z( k )|x( k ) = ξ i − ) (65)
where wki = N
k k
∑w
j =1
j
k−
p( z( k )|x( k ) = ξ j− )
k
346 Advanced Strategies for Robot Manipulators
Eq. (65) denotes the corrected value for the state vector. The recursion of the Particle Filter
proceeds in a way similar to the update of the Kalman Filter or the Extended Kalman Filter,
i.e.:
• Measurement update: Acquire z(k) and compute
wki = N
k k
and ξ ki = ξ i −
k
∑w
j =1
i
k−
p( z( k )|x( k ) = ξ − )
k
i
N
p( x( k + 1)|Z ) = ∑wki δ i ( x( k ))
i =1
ξk (67)
where ξ ∼ p( x( k + 1)|x( k ) = ξ ki )
i
k
The stages of state vector estimation with the use of the Particle Filtering algorithm are
depicted in Fig. 6.
density functions of the local Particle Filters into a common probability distribution of the
system’s state vector. Without loss of generality fusion between two estimates which are
provided by two different probabilistic estimators (particle filters) is assumed. This amounts
to a multiplication and a division operation to remove the common information, and is
given by (Ong et al. 2008), (Ong et al. 2006)
where ZA is the sequence of measurements associated with the i-th processing unit and ZB is
the sequence of measurements associated with the j-th measurement unit. In the
implementation of distributed particle filtering, the following issues arise:
1. Particles from one particle set (which correspond to a local particle filter) do not have
the same support (do not cover the same area and points on the samples space) as
particles from another particle set (which are associated with another particle filter).
Therefore a point-to-point application of Eq. (68) is not possible.
2. The communication of particles representation (i.e. local particle sets and associated
weight sets) requires significantly more bandwidth compared to other representations,
such as Gaussian mixtures.
Fusion of the estimates provided by the local particle filters (located at different processing
units) can be performed through the following stages. First, the discrete particle set of
Particle Filter A (Particle Filter B) is transformed into a continuous distribution by placing a
Gaussian kernel over each sample (Fig. 7) (Musso et al. 2001)
N N
∑w
i =1
A δ ( x(Ai ) ) → p A ( x ) ( ∑wB( i )δ ( xB( i ) ) → pB ( x ))
(i )
i =1
(70)
The common information appearing in the processing units A and B should not be taken
into account in the joint probability distribution which is created after fusing the local
probability densities of A and B. This means that in the joint p.d.f. one should sample with
importance weights calculated according to Eq. (68). The objective is then to create an
importance sampling approximation for the joint distribution that will be in accordance to
Eq. (68). A solution to this can be obtained through Monte Carlo sampling and suitable
selection of the so-called ”proposal distribution” (Ong et al. 2008), (Ong et al. 2006)]
According to the above, for the joint distribution the idea behind Monte Carlo sampling is to
draw N i.i.d samples from the associated probability density function p(x), such that the
target density is approximated by a point-mass function of the form
N
p( x ) ∑w
i =1
(i )
k δ ( x(ki ) ) (71)
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 349
1 N
IN ( f ) = ∑ f (x( i ) )
N i =1
(73)
where x(i) p(X) and IN( f )→I( f ) for N→∞. since, the true probability distribution p(x) is
hard to sample from, the concept of importance sampling is to select a proposal distribution
p( x ) in place of p(x), with the assumption that p( x ) includes the support space of p(x). Then
the expectation of function f (x), previously given in Eq. (72), is now calculated as
p( x )
I( f ) = ∫ f (x) p( x )dx = ∫ f ( x )w( x )p( x )dx (74)
p( x )
p( x )
w( x ) = (75)
p( x )
Then the Monte-Carlo estimation of the mean value of function f (x) becomes
N
I N ( f ) = ∑ f ( x( i ) )w( x( i ) ) (76)
i =1
p A ( x( i ) )
p( x( i ) ) = (77)
pB ( x ( i ) )
In that case the important weights of the fused probability density functions become
pA ( x( i ) )
w( x( i ) ) = (78)
pB ( x ( i ) ) p ( x ( i ) )
The next step is to decide what will be the form of the proposal distribution p( x ) . A first
option is to take p( x ) to be a uniform distribution, with a support that covers both of the
support sets of the distributions A and B.
p( x ) = U( x ) (79)
Then the sample weights p( x( i ) ) are all equal at a constant of value C. Hence the importance
weights are
pA ( x( i ) )
w( x( i ) ) = (80)
pB ( x( i ) )C
Another suitable proposal distribution that takes more into account the new information re
ceived (described as the probability distribution of the second processing unit) is given by
p ( x ) = pB ( x ) (81)
p A ( x( i ) )
w( x( i ) ) = (82)
p B ( x ( i ) )2
x = vcos(θ ), y = vsin(θ ), θ = u1
(83)
v = u2 , h = 0
where (x,y) is the desired inertial position of the UAV, θ is the UAV’s heading, v is the
UAV’s velocity, h is the UAV’s attitude, and u1, u2 are constrained by the dynamic capability
of the UAVs namely the heading rate constraint and the acceleration constraint respectively.
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 351
0.04
pA(x) 0.02
0
0 2 4 6 8 10
x
0.04
pB(x)
0.02
0
0 2 4 6 8 10
x
0.04
p(x)
0.02
0
0 2 4 6 8 10
x
Fig. 8. Fusion of the probability density functions produced by the local particle filters
An inertial measurement unit (IMU) of a UAV usually consists of a three axis gyroscope and
a three axis accelerometer. A vision sensor can be also mounted underneath the body of the
UAV and is used to extract points of interest in the environment. The UAV also carries a
barometric pressure sensor for aiding of the platform attitude estimation. A GPS sensor, can
be also mounted on the board. The sensor data is filtered and fused to obtain estimates of
the desired entities such as platform and feature position (Vissière et al. 2008).
Si ( w , w , w , , w i ), i = 1, 2, ,q (84)
The quantity w denotes the system variable while wi, i = 1, 2, ··· , q are its derivatives (these
and can be for instance the elements of the system’s state vector). The system of Eq. (1) is
said to be differentially flat if there exists a collection of m functions y = (y1, ··· ,ym) of the
system variables wi, i = 1, ··· , s and of their time-derivatives, i.e.
α
yi = φ ( w , w , w , , w i ), i = 1, ,m (85)
such that the following two conditions are satisfied (Fliess et al. 1999), (Rigatos 2008):
1. There does not exist any differential relation of the form
R( y , y , , yβ ) = 0 (86)
352 Advanced Strategies for Robot Manipulators
which implies that the derivatives of the flat output are not coupled in the sense of an
ODE, or equivalently it can be said that the flat output is differentially independent.
2. All system variables, i.e. the components of w (elements of the system’s state vectors)
can be expressed using only the flat output y and its time derivatives
γ
wi = ψi ( y , y , , y i ), i = 1, ,s (87)
v
x = vcos(θ ), y = vsin(θ ), θ = tan(φ ) (88)
l
where using the analogous of the unicycle robot v is the velocity of the UAV, l is the UAV’s
length, θ is the UAV’s orientation (angle between the transversal axis of the UAV and axis
OX), and φ is a steering angle. The flat output is the cartesian position of the UAV’s center of
gravity, denoted as η = (x,y) , while the other model parameters can be written as:
⎛ cos(θ ) ⎞ η
v=± η ⎜ ⎟= tan(φ ) = ldet(ηη ) / v 3 (89)
⎝ sin(θ ) ⎠ v
These formulas show simply that θ is the tangent angle of the curve traced by P and tan(φ) is
the associated curvature. With reference to a generic driftless nonlinear system
q , q ∈ Rn , w ∈ Rm (90)
ξ = α (q , ξ ) + b(q , ξ )u
(91)
w = c(q , ξ ) + d(q , ξ )u
with state ξ ∈ Rv and input u ∈ Rm, such that the closed-loop system of Eq. (90) and Eq. (91)
is equivalent under a state transformation z = T(q, ξ) to a linear system. The starting point is
the definition of a m-dimensional output η = h(q) to which a desired behavior can be
assigned. One then proceeds by successively differentiating the output until the input
appears in a non-singular way. If the sum of the output differentiation orders equals the
dimension n + v of the extended state space, full input-state-output linearization is obtained
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 353
(In this case η is also called a flat output). The closed-loop system is then equivalent to a set
of decoupled input-output chains of integrators from ui to ηi. The exact linearization
procedure is illustrated for the unicycle model of Eq. (21). As flat output the coordinates of
the center of gravity of the vehicle is considered η = (x,y). Differentiation with respect to
time then yields (Oriolo et al. 2002), (Rigatos 2008)
⎛x⎞ ⎛ cos(θ ) 0 ⎞ ⎛ v ⎞
η =⎜ ⎟=⎜ ⎟⋅⎜ ⎟ (92)
⎝y⎠ ⎝ sin(θ ) 0 ⎠ ⎝ ω ⎠
showing that only v affects η , while the angular velocity ω cannot be recovered from this
first-order differential information. To proceed, one needs to add an integrator (whose state
is denoted by ξ) on the linear velocity input
⎛ cos(θ ) ⎞
v = ξ , ξ =α ⇒η = ξ ⎜ ⎟ (93)
⎝ sin(θ ) ⎠
where α denotes the linear acceleration of the UAV. Differentiating further one obtains
⎛ α ⎞ ⎛ cos(θ ) −ξ sin(θ ) ⎞ ⎛ u1 ⎞
⎜ ⎟=⎜ ⎟⋅⎜ ⎟ (95)
⎝ ω ⎠ ⎝ sin(θ ) ξ cos(θ ) ⎠ ⎝ u2 ⎠
η is denoted as
⎛ η1 ⎞ ⎛ u1 ⎞
η =⎜ ⎟=⎜ ⎟=u (96)
⎝η 2 ⎠ ⎝ u2 ⎠
which means that the desirable linear acceleration and the desirable angular velocity can be
expressed using the transformed control inputs u1 and u2. Then, the resulting dynamic
compensator is (return to the initial control inputs v and ω)
ξ = u1cos(θ ) + u2 sin(θ )
v=ξ (97)
u 2 cos(θ ) − u1 sin(θ )
ω=
ξ
Being ξ∈R, it is n + v = 3 + 1 = 4, equal to the output differentiation order in Eq. (29). In the
new coordinates
z1 = x
z2 = y
(98)
z 3 = x = ξ cos(θ )
z 4 = y = ξ sin(θ )
354 Advanced Strategies for Robot Manipulators
The extended system is thus fully linearized and described by the chains of integrators, in
Eq. (29), and can be rewritten as
z1 = u1
(99)
z2 = u2
The dynamic compensator of Eq. (97) has a potential singularity at ξ = v = 0, i.e. when the
UAV is not moving, which is a case never met when the UAV is in flight. It is noted
however, that the occurrence of such a singularity is structural for non-holonomic systems.
In general, this difficulty must be obviously taken into account when designing control laws
on the equivalent linear model.
A nonlinear controller for output trajectory tracking, based on dynamic feedback
linearization, is easily derived. Assume that the UAV must follow a smooth trajectory
1
(xd(t),yd(t)) which is persistent, i.e. for which the nominal velocity vd = ( xd2 + y d2 ) 2 along the
trajectory never goes to zeros (and thus singularities are avoided). On the equivalent and
decoupled system of Eq. (32), one can easily design an exponentially stabilizing feedback for
the desired trajectory, which has the form
u1 = xd + k p ( xd − x ) + kd ( xd − x )
1 1
(100)
u2 = y d + k p ( y d − y ) + kd ( y d − y )
1 1
and which results in the following error dynamics for the closed-loop system
ex + k d ex + k p ex = 0
1 1
(101)
ey + kd ey + k p ey = 0
2 2
where ex = x − xd and ey = y − yd. The proportional-derivative (PD) gains are chosen as kp1 > 0
and kd1 > 0 for i = 1, 2. Knowing the control inputs u1, u2, for the linearized system one can
calculate the control inputs v and ω applied to the UAV, using Eq. (91). The above result is
valid, provided that the dynamic feedback compensator does not meet the singularity. In
the general case of design of flatness-based controllers, the following theorem assures the
avoidance of singularities in the proposed control law (Oriolo et al. 2002):
Theorem: Let λ11, λ12 and λ21, λ22, be respectively the eigenvalues of two equations of the
error dynamics, given in Eq. (91). Assume that, for i = 1,2 it is λ11 < λ12 < 0 (negative real
eigenvalues), and that λi2 is sufficiently small. If
⎛ xd (t ) ⎞ ⎛ εx ⎞
0
min t ≥ 0 ⎜ ⎟ ≥ ⎜⎜ 0 ⎟⎟ (102)
⎝ y d (t ) ⎠ ⎝ εy ⎠
with εx0 = εx (0) ≠ 0 and εy0 = εy (0) ≠ 0 , then the singularity ξ = 0 is never met.
6. Simulation tests
6.1 Autonomous UAV navigation with Extended Information Filtering
It was assumed that m = 2 helicopter models were monitored by n = 2 different ground
stations. At each ground station an Extended Kalman Filter was used to track each UAV. By
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 355
fusing the measurements provided by the sensors mounted on each UAV, each local EKF
was able to produce an estimation of a UAV’s motion. Next, the state estimates obtained by
the pair local EKFs associated with each UAV were fused with the use of the Extended
Information Filter. This fusion-based state estimation scheme is depicted in Fig. 2. As
explained in Section 2 the weighting of the state estimates of the local EKFs was performed
using the local information matrices. The distributed fitering architecture is shown in Fig. 9.
The IMU system provides measurements or the UAV’s position [x,y] and the UAV’s
orientation angle θ over a sampling period T. These sensors are used to obtain an estimation
of the displacement and the angular velocity of the UAV v(t) and ω(t), respectively. The IMU
sensors can introduce incremental errors, which result in an erroneous estimation of the
orientation θ. To improve the accuracy of the UAV’s localization, measurements from the
GPS (or visual sensors) can be used. On the other hand, the GPS on this own is not always
reliable since its signal can be intermittent. Therefore, to succeed accurate localization of the
UAV it is necessary to fuse the GPS measurements with the IMU measurements of the UAV
or with measurements from visual sensors (visual odometry).
The inertial coordinates system OXY is defined. Furthermore the coordinates system O′X′Y′
is considered (Fig. 10). O′X′Y′ results from OXY if it is rotated by an angle θ. The coordinates
of the center of symmetry of the UAV with respect to OXY are (x,y), while the coordinates of
356 Advanced Strategies for Robot Manipulators
For manoeuvres at constant altitude the GPS measurement (or the visual sensor
measurement) can be considered as the measurement of the distance from a reference
surface P j. A reference surface P j in the UAVs 2D flight area can be represented by Prj and
Pnj , where (i) Prj is the normal distance of the plane from the origin O, (ii) Pnj is the angle
between the normal line to the plane and the x-direction.
The GPS sensor (or visual sensor i) is at position xi(k),yi(k) with respect to the inertial
coordinates system OXY and its orientation is θi(k). Using the above notation, the distance of
the GPS (or visual sensor i), from the plane P j is represented by Prj , Pnj (see Fig. 10):
Assuming a constant sampling period Δtk = T the measurement equation is z(k + 1) = γ(x(k))
+ v(k), where z(k) is the vector containing GPS (or visual sensor) and IMU measures and v(k)
is a white noise sequence ~N(0,R(kT)).
By definition of the measurement vector one has that the output function is γ(x(k)) =
[x(k),y(k), θ(k),d1(k)]T. The UAV state is [x(k),y(k), θ(k)]T and the control input is denoted by
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 357
U(k) = [v(k),ω(k)]T. To obtain the Extended Kalman Filter (EKF), the kinematic model of the
UAV is linearized about the estimates xˆ ( k ) and xˆ − ( k ) the control input U(k − 1) is applied.
The measurement update of the EKF is
P − ( k + 1) = Jφ ( xˆ ( k ))P( k ) JφT ( xˆ ( k )) + Q( k )
xˆ − ( k + 1) = φ ( xˆ ( k )) + L( k )U ( k )
⎛ Tcos(θ ( k )) 0 ⎞ ⎛ 1 0 − v( k )sin(θ )T ⎞
⎜ ⎟ ⎜ ⎟
where L( k ) = ⎜ Tsin(θ ( k )) 0 ⎟ and Jφ ( xˆ ( k )) = ⎜ 0 1 − v( k )cos(θ )T ⎟ (106)
⎜ 0 T ⎟⎠ ⎜0 0 1 ⎟
⎝ ⎝ ⎠
while Q(k) = diag[σ2(k),σ2(k),σ2(k)], with σ2(k) chosen to be 10−3 and φ ( xˆ ( k )) = [ xˆ ( k ), yˆ ( k ),θˆ( k )]T ,
γ ( xˆ ( k )) = [ xˆ ( k ), yˆ ( k ),θˆ( k ), d( k )]T , i.e.
⎛ xˆ ( k ) ⎞
⎜ ˆ ⎟
y ( k )
γ ( xˆ ( k )) = ⎜⎜ ⎟
⎟ (107)
θˆ( k )
⎜ j ⎟
⎜ P − x ( k ))cos( P ) − y ( k )sin( P ) ⎟
j j
⎝ r i n i n ⎠
⎛ 1 0 0 ⎞
⎜ ⎟
0 1 0
JγT ( xˆ − ( k )) = ⎜ ⎟ (108)
⎜ 0 0 1 ⎟
⎜⎜ ⎟⎟
j j
′ j
′
⎝ −cos( Pn ) −sin( Pn ) { xi cos(θ − Pn ) − yi sin(θ − Pn )} ⎠
j
The UAV is steered by a dynamic feedback linearization control algorithm which is based
the flatness-based control analyzed in Section 5:
u1 = xd + K p ( xd − x ) + K d ( xd − x )
1 1
u2 = y d + K p ( y d − y ) + K d ( y d − y )
2 2
Under the control law of Eq. (109) the dynamics of the tracking error finally becomes
ex + K d ex + K p ex = 0
1 1
(110)
e y + K d ex + K p e y = 0
2 2
where ex = x − xd and ey = y − yd. The proportional-derivative (PD) gains are chosen as Kp1
and Kd1, for i = 1, 2.
15 15
10 10
5 5
Y
0 0
-5 -5
-10 -10
-15 -15
-15 -10 -5 0 5 10 15 20 -15 -10 -5 0 5 10 15 20
X X
(a) (b)
Fig. 11. Autonomous navigation of the multi-UAV system when the UAVs state vector is
estimated with the use of the Extended Information Filter (a) tracking of circular reference
trajectory (b) tracking of a curve-shaped reference trajectory
Results on the performance of the Extended Information Filter in estimating the state vectors
of multiple UAVs when observed by distributed processing units is given in Fig. 11. Using
distributed EKFs and fusion through the Extended Information Filter is more robust
comparing to the centralized EKF since (i) if a local processing unit is subject to a fault then
state estimation becomes is still possible and can be used for accurate localization of the
UAV, as well as for tracking of desirable flight paths, (ii) communication overhead remains
low even in the case of a large number of distributed measurement units, because the
greatest part of state estimation is performed locally and only information matrices and state
vectors are communicated between the local processing units, (iii) the aggregation
performed on the local EKF also compensates for deviations in state estimates of local filters
(which can be due to linearization errors).
p( x( k )|Z ) = ∑ i =1wki δ
N
The measurement update of the PF is ( x( k )) with
ξi−
k
w i − p( z( k )|x( k ) = ξ i − )
wki = k k
where the measurement equation is given by zˆ( k ) = z( k ) + v( k )
∑ j =1wkj p( z( k )|x( k ) = ξ j− )
N
k
with z(k) = [x(k), y(k), θ(k), d(k)]T, and v(k) =measurement noise.
The time update of the PF is p( x( k + 1)|Z ) = ∑ i =1wki δ i ( x( k )) where ξ ki ∼ p( x( k + 1)|x( k ) = ξ i − )
N
ξk k
and the state equation is xˆ − = φ ( x( k )) + L( k )U ( k ) , where φ(x(k)), L(k), and U(k) are defined in
subsection 6.1. At each run of the time update of the PF, the state vector estimation xˆ − ( k + 1)
is calculated N times, starting each time from a different value of the state vector ξ ki .
Although the Distributed Particle Filter can function under any noise distribution in the
simulation experiments the measurement noise was assumed to be Gaussian. The obtained
results are given in Fig. 12.
15 15
10 10
5 5
Y
0 0
-5 -5
-10 -10
-15 -15
-15 -10 -5 0 5 10 15 20 -15 -10 -5 0 5 10 15 20
X X
(a) (b)
Fig. 12. Autonomous navigation of the multi-UAV system when the UAVs state vector is
estimated with the use of the Distributed Particle Filter (a) tracking of circular reference
trajectory (b) tracking of a curve-shaped reference trajectory
In the simulation experiments it was observed that the Distributed Particle Filter, for
N = 1000 particles, succeeded more accurate state estimation (smaller variance) than the EIF
and consequently enables better tracking of the desirable trajectories by the UAVs. This
improved performance of the DPF over the EIF is due to the fact that the local EKFs that
constitute the EIF introduce cumulative errors due to the EKF linearization assumption
(truncation of higher order terms in the Taylor expansion of Eq. (2) and Eq. (4)). Comparing
to the Extended Information Filter, the Distributed Particle Filter demands more
computation resources and its computation cycle is longer. However, the computation cycle
of PF can be drastically reduced on a computing machine with a fast processor or with
360 Advanced Strategies for Robot Manipulators
parallel processors (Míguez 2007). Other significant issues that should be taken into account
in the design of the Distributed Particle Filter are the consistency of the fusion performed
between the probability density functions of the local filters and the communication
overhead between the local filters.
The simulation results presented in Fig. 12 show the efficiency of the Distributed Particle
Filtering in providing accurate localization for the multi-UAV system, as well as for
implementing state estimation-based control schemes. The advantages of using Distributed
Particle Filtering are summarized as follows: (i) there is robust state estimation which is not
constrained by the assumption of Gaussian noises. The fusion performed between the local
probability density functions enables to remove outlier particles thus resulting in an
aggregate state distribution that confines with accuracy the real state vector of each UAV. If
a local processing unit (local filter) fails the reliability of the aggregate state estimation will
be preserved (ii) computation load can be better managed comparing to a centralized
particle filtering architecture. The greatest part of the necessary computations is performed
at the local filters. Moreover the advantage of communicating state posteriors over raw
observations is bandwidth efficiency, which is particularly useful for control over a wireless
sensor network.
7. Conclusions
The paper has examined the problem of localization and autonomous navigation of a multi-
UAV system based on distributed filtering over sensor networks. Particular emphasis was
paid to distributed particle filtering since this decentralized state estimation approach is not
constrained by the assumption of noise Gaussian distribution. It was considered that m
UAV (helicopter) models are monitored by n different ground stations. The overall concept
was that at each monitoring station a filter should be used to track each UAV by fusing
measurements which are provided by various UAV sensors, while by fusing the state
estimates from the distributed local filters an aggregate state estimate for each UAV should
be obtained.
The paper proposed first the Extended Information Filter (EIF) and the Unscented
Information Filter (UIF) as possible approaches for fusing the state estimates obtained by the
local monitoring stations, under the assumption of Gaussian noises. It was shown that the
EIF and UIF estimated state vector can be used by a flatness-based controller that makes the
UAV follow the desirable trajectory. The Extended Information Filter is a generalization of
the Information Filter in which the local filters do not exchange raw measurements but send
to an aggregation filter their local information matrices (inverse covariance matrices which
can be also associated to the Fisher Information matrices) and their associated local
information state vectors (products of the local Information matrices with the local state
vectors). In case of nonlinear system dynamics, such as the considered UAV models, the
calculation of the information matrices and information state vectors requires the
linearization of the local observation equations in the system’s state space description and
consequently the computation of Jacobian matrices is needed.
In the case of the Unscented Information Filter there is no linearization of the UAVs
observation equation. However the application of the Information Filter algorithm is
possible through an implicit linearization which is performed by approximating the
Jacobian matrix of the system’s output equation by the product of the inverse of the state
vector’s covariance matrix (Fisher information matrix) with the cross-covariance matrix
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 361
between the system’s state vector and the system’s output. Again, the local information
matrices and the local information state vectors are transferred to an aggregation filter
which produces the global estimation of the system’s state vector.
Next, the Distributed Particle Filter (DPF) was proposed for fusing the state estimates pro-
vided by the local monitoring stations (local filters). The motivation for using DPF was that
it is well-suited to accommodate non-Gaussian measurements. A difficulty in implementing
distributed particle filtering is that particles from one particle set (which correspond to a
local particle filter) do not have the same support (do not cover the same area and points on
the samples space) as particles from another particle set (which are associated with another
particle filter). This can be resolved by transforming the particles set into Gaussian mixtures,
and defining the global probability distribution on the common support set of the
probability density functions associated with the local filters. Suitable importance
resampling is proposed so as to derive the weights of the joint distribution after removing
the common information contained in the probability density functions of the local filters.
The state vector which is estimated with the use of the DPF was again used by the flatness-
based controller to make each UAV follow a desirable flight path.
Comparing to centralized state estimation and control the proposed distributed state
estimation and control schemes have significant advantages: (i) they are fault tolerant: if a
local processing unit is subject to a fault then state estimation is still possible and accurate,
(ii) the computation load is distributed between local processing units and since there is no
need to exchange a large amount of information, the associated communication bandwidth
is low. In the case of the Extended Information Filter and of the Unscented Information
Filter the information transmitted between the local processing units takes the form of the
information covariance matrices and the information state vectors. In the case of Distributed
Particle Filtering the information transmitted between the local processing units takes the
form of Gaussian mixtures. The performance of the Extended Information Filter and of the
Distributed Particle Filter was evaluated through simulation experiments in the case of a 2-
UAV model monitored and remotely navigated by two local stations.
Comparing the DPF to the EIF through simulation experiments it was observed that the
Distributed Particle Filter, succeeded more accurate state estimation (smaller variance) than
the EIF and consequently enabled better tracking of the desirable trajectories by the UAVs.
This improved performance of the DPF over the EIF is explained according to to the fact that
the local EKFs that constitute the EIF introduce cumulative errors due to the EKF
linearization assumption. It was also observed that the Distributed Particle Filter demands
more computation resources than the Extended Information Filter and that its computation
cycle is longer. However, the computation cycle of the DPF can be drastically reduced on a
computing machine with a fast processor or with parallel processors. Other issues that
should be taken into account in the design of the Distributed Particle Filter are the
consistency of the fusion performed between the probability density functions of the local
filters and the communication overhead between the local filters.
8. References
[Beard et al. 2002] Beard, R.W.; McLain, T.W., Goodrich, M., & Anderson, E.P. (2002).
Coordinated Target Assignment and Intercept for Unmanned Air Vehicles. IEEE
Transactions on Robotics and Automation, Vol. 18, No. 6, pp. 911-922, 2002.
362 Advanced Strategies for Robot Manipulators
[Caballero et al. 2008] Caballero, F.; Merino, L., Ferruz, J., & Ollero, A. (2008). A particle
filtering method for Wireless Sensor Network localization with an aerial robot
beacon. Proc: IEEE International Conference on Robotics and Automation 2006, pp. 2860-
2865, 2008.
[Coué et al. 2003] Coué, C.; Pradalier, C. & Laugier, C. (2003). Bayesian Programming
MultiTarget Tracking: an Automotive Application. Int. Conf. on Field and Service
Robotics, Lake Yamanaka (Japan), July 2003.
[Coué et al. 2006] Coué, C; Pradalier, C., Laugier, C., Fraichard, T. & Bessiére, P. (2006).
Bayesian Occupancy Filtering for Multitarget Tracking: An Automotive
Application. The International Journal of Robotics Research, Vol. 25, No. 1, pp. 19-30,
2006.
[Deming & Perlovsky 2007] Deming, R.W. & Perlovsky, L.I. (2007). Concurrent multi-target
localization, data association, and navigation for a swarm of flying sensors.
Information Fusion, Elsevier, vol.8, no.3, pp. 316-330, 2007.
[Fliess et al. 1999] Fliess, M. & Mounier, H. (1999). Tracking control and π-freeness of infinite
dimensional linear systems, Dynamical Systems, Control, Coding and Computer Vision,
(G. Picci and D.S. Gilliam, Eds.), Vol. 258, pp. 41-68, Birkhaüser, 1999.
[Gan & Harris 2001] Gan Q. & Harris, C.J. (2001). Comparison of two measurement fusion
methods for Kalman-filter-based multisensor data fusion. IEEE Transactions on
Aerospace and Electronic Systems, Vol. 37, No.1, pp. 273-280, 2001.
[Gao et al. 2009] Gao, S; Zhong, Y. Zhang, X., & Shirinzadeh, B. (2009). Multi-sensor optimal
data fusion for INS/GPS/SAR integrated navigation system. Aerospace Science and
Technology, Elsevier, Vol. 13, pp. 232-237, 2009.
[Hue et al. 2002] Hue, C.; Le Cadre, J.P. & P´erez, P. (2002). Tracking Multiple Objects with
Particle Filtering. IEEE Transactions on Aerospace and Electronic Systems, Vol. 38.
No.3, pp. 791-812, 2002.
[Ing & Coates 2005] Ing, J. & Coates, M.G. (2005). Parallel particle filters for tracking in
wireless sensor networks. IEEE Workshop on Signal Processing Advances in Wireless
Communications, SPAWC 2005, art. no. 1506277, pp. 935-939, 2005.
[Julier et al. 2000] Julier, S.; Uhlmann, J. & Durrant-Whyte, H.F. (2000). A new method for
the nonlinear transformations of means and covariances in filters and estimators.
IEEE Transactions on Automatic Control, Vol.45, No.3, pp. 477-482, 2000.
[Julier et al. 2004] Julier, S.J. & Uhlmann, J.K. (2004). Unscented Filtering and Nonlinear
Estimation, Proceedings of the IEEE, Vol.92, pp. 401-422, 2004.
[Léchevin & Rabbath 2006] Léchevin, N. & Rabbath, C.A. (2006). Sampled-data control of a
class of nonlinear flat systems with application to unicycle trajectory tracking.
ASME Journal of Dynamical Systems, Measurement and Control, vol. 128, No.3, pp.
722-728, 2006.
[Lee et al. 2008] Lee D.J. (2008). Unscented Information Filtering for Distributed Estimation
and multiple sensor fusion. AIAA Guidance, Navigation and Control Conference and
Exhibit, Aug. 2008, Hawai, USA.
[Lee et al. 2008] Lee, D.J. (2008). Nonlinear estimation and multiple sensor fusion using
unscented information filtering. IEEE Signal Processing Letters, Vol. 15, pp. 861-864,
2008.
[Mahler 2007] Mahler, R.P.S. (2003). Statistical Multisource-Multitarget Information Fusion.
Artech House Inc. 2007.
Distributed Particle Filtering over Sensor Networks for Autonomous Navigation of UAVs 363
[Rigatos 2009a] Rigatos, G.G. (2009). Particle Filtering for State Estimation in Nonlinear
Industrial Systems. IEEE Transactions on Instrumentation and Measurement, Vol. 58,
No. 11, pp. 3885-3900, 2009.
[Rigatos 2009b] Rigatos, G.G. (2009). Sigma-point Kalman Filters and Particle Filters for
integrated navigation of unmanned aerial vehicles. Intl. Workshop on Robotics for
Risky Interventions and Environmental Surveillance, RISE 2009, Brussels, Belgium, Jan.
2009.
[Rigatos & Zhang 2009] Rigatos, G. & Zhang, Q. (2009). Fuzzy model validation using the
local statistical approach. Fuzzy Sets and Systems, Elsevier, Vol. 60, No.7, pp. 882-
904, 2009.
[Rosencrantz et al. 2003] Rosencrantz, M.; Gordon, G., Thrun, S. (2003). Decentralized data
fusion with distributed particle filtering. Proceedings of the Conference of Uncertainty
in AI (UAI), Acapulco, Mexico, 2003.
[Särrkä 2007] Särrkä, S. (2007). On Unscented Kalman Filtering for state estimation of
continuous-time nonlinear systems. IEEE Transactions on Automatic Control, Vol.52,
No.9, pp.1631-1641, 2007.
[Shima et al. 2007] Shima, T.; Rasmussen, S.J. & Chandler, P. (2007). UAV team decision and
control using efficient collaborative estimation. Journal of Dynamic Systems,
Measurement and Control, Transactions of the ASME, Vol. 129, No 5, pp. 609-619,
2007.
[Singh & Fuller 2001] Singh, L. & Fuller, J. (2001). Trajectory generation for a UAV in urban
terrain using nonlinear MPC. Proceedings of American Control Conference, pp. 2301-
2308, 2001.
[Thrun et al. 2005] Thrun, S.; Burgard, M. & Fox, D. (2005). Probabilistic Robotics, MIT Press,
2005.
[Vercauteren &Wang 2005] Vercauteren, T. &Wang, X. (2005). Decentralized Sigma-Point
Information Filters for Target Tracking in Collaborative Sensor Networks. IEEE
Transactions on Signal Processing, Vol.53, No.8, pp. 2997-3009, 2005.
[Villagra et al. 2007] Villagra, J.; d’Andrea-Novel, B., Mounier, H. & Pengov, M. (2007).
Flatness-based vehicle steering control strategy with SDRE feedback gains tuned
via a sensitivity approach. emphIEEE Transactions on Control Systems Technology,
Vol. 15, pp. 554-565, 2007.
[Vissière et al. 2008] Vissière, D.; Bristeau, P.-J., Martin, A.P. & N. Petit (2008). Experimental
autonomous flight of a small-scaled helicopter using accurate dynamics model and
low-cost sensors. Proceedings of the 17thWorld Congress The International Federation of
Automatic Control Seoul, Korea, July 2008.
[Watanabe & Tzafestas 1992] Watanabe, K. & Tzafestas, S.G. (1992). Filtering, Smoothing
and Control in Discrete-Time Stochastic Distributed-Sensor Networks, In: Stochastic
Large- Scale Engineering Systems (S.G. Tzafestas and K. Watanabe Eds), pp. 229-252,
Marcel Dekker, 1992.
[Zhang et al. 2005] Zhang, Campillo, Q.F., C´erou, F. & F. Legland (2005). Nonlinear fault
detection and isolation based on bootstrap particle filters, Proc. of the 44th IEEE
Conference on Decision and Control, and European Control Conference, Seville Spain,
Dec. 2005.
17
Japan
1. Introduction
Laparoscopic surgery is a technique where surgical tools and a laparoscope are inserted into
the patient’s body through small holes in the abdomen, and the surgeon carries out the
surgery while viewing the images from the laparoscope on a TV monitor (see Fig. 1(left)).
Laparoscopic surgery has grown rapidly in popularity in recent years, not only because it is
less invasive and produces less visible scarring, but also because of its benefits in terms of
healthcare economy, such as shorter patient stays. The most important characteristic of this
technique is that the surgeon performs the operation while watching the video image from
the laparoscope on a monitor instead of looking directly at the site of the operation. Thus, an
important factor affecting the safety and smoothness of the operation is the way in which
the video images are presented in a field of view suitable for the surgical operation.
Manipulation of the laparoscope is not only needed for orienting the laparoscope towards
the parts requiring surgery, but also for making fine adjustments to ensure that the field of
view, viewing distance and so on are suitable for the surgical operation being performed. A
camera assistant operates the laparoscope according to the surgeon’s instructions, but must
also make independent decisions on how to operate the laparoscope in line with the
surgeon’s intentions as the surgery progresses. Consequently even the camera assistant that
operates the laparoscope must have the same level of experience in laparoscopic surgery as
the surgeon. However, not many surgeons are skilled in the special techniques of
laparoscopic surgery. It is therefore not uncommon for camera assistants to be
inexperienced and unable to maintain a suitable field of view, thus hindering the progress of
the operation. To address this problem, laparoscope manipulating robots are expected as a
substitute for the human camera assistant and have already been made commercially
available (see Fig. 1(right)). However, there are several problems to be solved:
1. Hardware problems: A large apparatus sometimes interferes with the surgeon. The
setting and repositioning is awkward. Furthermore, the initial and maintenance costs
are expensive.
366 Advanced Strategies for Robot Manipulators
2. Related works
Laparoscope manipulators have been developed in the last fifteen years and there are at
least 27 kinds of laparoscope controlling robots which are commercialized or published in
refereed articles as of September 2009 (Taniguchi et al. (2010)). Some of them have already
been made commercially available and are in widespread use. These include AESOP made
in the US by Computer Motion Inc. (now known as Intuitive Surgical Inc.) (Sackier & Wang
(1994)), EndoAssist made in the UK by Armstrong Healthcare Ltd. (now known as
Prosurgics Inc.) (Aiono et al. (2002)), LapMan made in Belgium by Medsys s.a. (Polet &
Donnez (2004)), and Naviot made in Japan by Hitachi Co.,Ltd. (Tanoue et al. (2006)).
Although these commercialized manipulators have various merits such as stable view and
reduction of need for medical staff, several problems have been noted. First, the bulky
manipulator and the supporting arm often interfere with the surgical procedures. Second,
the setting and detaching of the robot is frequently awkward, causing an extension of the
time required for the operation. Furthermore, the initial and maintenance costs are
expensive. In addition to such hardware problems, they usually must be controlled by the
operating surgeon himself/herself using a human-machine interface such as an instrument-
mounted joystick, foot pedal, voice controller, or head/face motion-activated system. This is
an additional task that distracts the surgeon’s attention from the main region of interest and
may result in frustration and longer surgery time.
To free the surgeon from the task of controlling the view and to automatically offer an
optimal and stable view during laparoscopic surgery, several automatic camera positioning
systems have been devised (Casals et al. (1996), Wei et al. (1997), Wang et al. (1998),
Nishikawa et al. (2003), Ko & Kwon (2004), Nishikawa et al. (2006)). These systems visually
extract the shape and/or position of the surgical instrument from the laparoscopic images in
Design and Control of a Compact Laparoscope Manipulator: A Biologically Inspired Approach 367
real time, and automatically manipulate the laparoscope to always center the tip of the
instrument in the displayed image. Such systems are based on the simple idea that the
projected position of the distal end of the surgical tool corresponds to the surgeon’s region
of interest in a laparoscopic image. Besides centering on the most interesting area, there is an
additional and important factor that defines a good image of the surgical scene—zooming
ratio (Nishikawa et al. (2008)) — that corresponds to the depth of insertion of the
laparoscope along its longitudinal axis. The pioneering studies of fully automatic camera
positioning systems defined the zooming ratio as a “uniform” function of the estimated
distance between the tip of the tool and the laparoscope (Wei et al. (1997)) or the area ratio
between the visible tool and the whole image (Casals et al. (1996)). Although these
approaches may completely remove the surgeon’s camera control burden, they may not
provide the specific view that the surgeon wants, because the most appropriate zooming
ratio varies widely during surgery. The best zooming ratio depends on both the surgical
procedure/phase and the habits/preferences of the operating surgeon. For this reason, most
of the instrument tracking systems recently developed (Wang et al. (1998), Nishikawa et al.
(2003), Ko & Kwon (2004), Nishikawa et al. (2006)) have abandoned the idea of systematic
control of zooming parameters; instead, the surgeon is required to define the parameters
preoperatively or adjust them intraoperatively through conventional human-machine
interfaces, which again means an extra control burden for the surgeon.
(a) (b)
Fig. 2. Compact and lightweight laparoscope manipulator, named P-arm. (a) The P-arm is
composed of a Stewart-Gough platform equipped with six linear hydraulic actuators. (b)
The P-arm can hold a general laparoscope and can be supported by the conventional
instrument holder.
The actuators are attached to the manipulator using a permanent magnet. Therefore, when
excess force is applied to the manipulator, the actuator is readily dislocated so it does not
injure the patient. In addition, even when two of the six actuators are dislocated, the
manipulator works safely as discussed above. The dislocated actuators can be easily
reattached to the manipulator. Furthermore, in the case of an emergency, the robot can be
stopped promptly by the emergency stop system, which is controlled by a circuit
independent of the operating system.
3.3 Results
The robot was evaluated by performing the following three types of operations using a
living swine: a laparoscopic cholecystectomy, a laparoscopic anterior resection of the
rectum, a laparoscopic distal gastrectomy (Sekimoto et al. (2009)). As a result, it worked
steadily for all the operations, without interfering with the surgeon’s work (see Fig. 3). Also,
it contributed to shortening the setting and detaching time. The setting times were 66, 93,
104 seconds and the detaching times were 24 and 17 seconds, respectively. Wagner reported
the setting time of 2 minutes for AESOP and 5.3 minutes for EndoAssist (Wagner et al.
(2006)). Compared with these results, the P-arm was considered to be superior. The facility
of the system is essential for the robot to be accepted by surgeons.
assistant changed the field of view when the magnitude of the acceleration of the tip of the
left-hand instrument was large, and employed the following equation as the activity:
N −1
1
activity =
N
∑ activity
n=0
i −n (2)
N −1
1
activity i =
N
∑p
n=0
i −n (3)
⎪⎧0 if vi − vi − 1 > K
pi = ⎨ (4)
⎪⎩ 1 if vi − vi − 1 ≤ K
where i means time, N indicates the positive number for calculating the moving average. vi
indicates the magnitude of the velocity of the tip of the left-hand instrument at time i, and
K(> 0) is a threshold value.
⎡ ⎤ G G
G G ⎢ i− j B ⎥ rj − x
f (x) = ∑ ⎢C × activity j ×
G G2 ⎥⋅ G G
rj − x
(5)
i− j
j ≤ i , C × activity j > M ⎢ rj − x + A ⎥
⎣ ⎦
N −1
G 1 G
x( t ) t =0 =
N
∑ ri −n (6)
n=0
where i means the present time, N indicates the positive number for calculating the moving
G
average. The vector rj represents the position of the tip of the right-hand instrument at time
j. A, B, and C are all the positive constants: the parameters A and B respectively set the range
and power of attractors, and the parameter C(<1) indicates a forgetting factor. M(>0) is a
threshold for ignoring weak attractors. The term B acts like the Gaussian function
G G 2
rj − x + A
G
whose center is rj .
4.3 Results
We implemented this bio-inspired method on our robotic laparoscope positioner described
in section 3. Fig. 4 shows the overview of our automatic laparoscope positioning system. The
position/pose of the three tools: the right and left instruments and the laparoscope can be
obtained simultaneously by the commercial 3D tracking system, Polaris Accedo (NDI
Corporation) (See Blasinski et al. (2007) and Nishikawa et al. (2008) for the details). Then
372 Advanced Strategies for Robot Manipulators
Fig. 4. Overview of automatic laparoscope positioning system. The proposed system uses
“fluctuation” to determine and update in real-time the desired position of the tip of the
G
righthand instrument, x , during surgery.
G G
both ri (the position of the tip of the right-hand instrument) and vi (the velocity of the tip of
the left-hand instrument)
G G are estimated in terms of laparoscopic camera coordinates, and
activity and f ( x ) are calculated from Eqs. 2 and 5 respectively. As a result, we can
determine and update also in real-time the desired position of the tip of the right-hand
G G
G
instrument, x , during surgery, by substituting the resulting values: activity and f ( x ) into
Eq. 1 and solving the Eq. 1 numerically (e.g., by the Runge-Kutta method) under the initial
condition given by Eq. 6.
To validate the proposed system, a number of in-vitro laparoscopic cholecystectomy tests
were performed. For each test, a swine liver with a gallbladder was placed in the training
box and the gallbladder was removed by an operating surgeon with the support of the
laparoscope robot P-arm controlled by Eq. 1. As a result, our system successfully and
automatically controlled the position of a laparoscope during all the operations (Figs. 5–10).
5. Concluding remarks
A compact and lightweight laparoscope manipulator was developed. Also, a novel method
for controlling the position of a laparoscope was inspired by biological systems dynamics.
Our approach opens potential applications to skill transfer and adaptive behavior in
medicine.
Design and Control of a Compact Laparoscope Manipulator: A Biologically Inspired Approach 373
Fig. 10. View of an in vitro experiment (laparoscopic cholecystectomy) using a swine liver
with a gallbladder (6/6). (left) a surgeon and the laparoscope robot P-arm, (mid) image from
the laparoscope, (right) visualization of attractors as the contour map on the image plane.
Design and Control of a Compact Laparoscope Manipulator: A Biologically Inspired Approach 379
6. Acknowledgements
This research was supported by “Special Coordination Funds for Promoting Science and
Technology: Yuragi Project” of the Ministry of Education, Culture, Sports, Science and
Technology, Japan, and Grant-in-Aid for Scientific Research (A) of the Japan Society for the
Promotion of Science (Representative Researcher: Atsushi Nishikawa, Project Number
19206047). We cordially appreciate the cooperation of Mr. Takeharu Kobayashi, Mr. Kouhei
Kazuhara, Mr. Takaharu Ichihara, and Mr. Naoto Kurashita of Daiken Medical Co. Ltd.
Also, we would like to express our sincere gratitude that the surgeons of Osaka University
Hospital agreed to help our research.
7. References
Aiono, S.; Gilbert, J. M.; Soin, B.; Finlay, P. A. & Gordan, A. (2002). Controlled trial of the
introduction of a robotic camera assistant (Endo Assist) for laparoscopic
cholecystectomy. Surgical Endoscopy, Vol. 16, No. 9, September 2002, 1267–1270,
ISSN 0930-2794 (Print) 1432-2218 (Online)
Blasinski, H.; Nishikawa, A. & Miyazaki, F. (2007). The application of adaptive filters for
motion prediction in visually tracked laparoscopic surgery. Proceedings of the 2007
IEEE International Conference on Robotics and Biomimetics (ROBIO2007), pp. 360–365,
Sanya, China, December 2007, IEEE Press
Casals, A.; Amat, J. & Laporte, E. (1996). Automatic guidance of an assistant robot in
laparoscopic surgery. Proceedings of the 1996 IEEE International Conference on Robotics
and Automation, pp. 895–900, Minneapolis, USA, April 1996, IEEE Press
Fukuyori, I.; Nakamura, Y.; Matsumoto, Y. & Ishiguro, H. (2008). Flexible control
mechanism for multi-DOF robotic arm based on biological fluctuation. In: From
Animals to Animats 10, 22-31, Springer, ISBN 978-3-540-69133-4
Kashiwagi, A.; Urabe, I.; Kaneko, K. & Yomo, T. (2006). Adaptive response of a gene
network to environmental changes by fitness-induced attractor selection. PLoS
ONE, Vol. 1, No. 1, December 2006, e49
Ko, S. Y. & Kwon, D. S. (2004). A surgical knowledge based interaction method for a
laparoscopic assistant robot. Proceedings of the 13th IEEE International Workshop on
Robot and Human Interactive Communication (ROMAN 2004), pp. 313–318, Kurashiki,
Japan, September 2004, IEEE Press
Kobayashi, E.; Masamune, K.; Sakuma, I.; Dohi, T. & Hashimoto, D. (1999). A new safe
laparoscopic manipulator system with a five-bar linkage mechanism and an optical
zoom. Computer Aided Surgery, Vol. 4, No. 4, 1999, 182–192
Nishikawa, A.; Asano, S.; Fujita, R.; Yamaguchi, S.; Yohda, T.; Miyazaki, F.; Sekimoto, M.;
Yasui, M.; Takiguchi, S.; Miyake, Y. & Monden, M. (2003). Selective use of face
gesture interface and instrument tracking system for control of a robotic
laparoscope positioner. In: Medical Image Computing and Computer Assisted
Intervention –MICCAI2003, 973-974, Springer, ISBN 978-3-540-20464-0
Nishikawa, A.; Ito, K.; Nakagoe, H.; Taniguchi, K.; Sekimoto, M.; Takiguchi, S.; Seki, Y.;
Yasui, M.; Okada, K.; Monden, M. & Miyazaki, F. (2006). Automatic positioning of
a laparoscope by preoperative workspace planning and intraoperative 3D
instrument tracking. MICCAI2006 Workshop Proceedings, Workshop on Medical
Robotics: Systems and Technology towards Open Architecture, pp. 82–91, Copenhagen,
Denmark, October 2006
380 Advanced Strategies for Robot Manipulators
Nishikawa, A.; Nakagoe, H.; Taniguchi, K.; Yamada, Y.; Sekimoto, M.; Takiguchi, S.;
Monden, M. & Miyazaki, F. (2008). How does the camera assistant decide the
zooming ratio of laparoscopic images? Analysis and implementation. In: Medical
Image Computing and Computer Assisted Intervention –MICCAI2008, 611-618,
Springer, ISBN 978-3-540- 85989-5
Nishikawa, A.; Yamada, Y.; Taniguchi, K. & Miyazaki, F. (2009). Automatic endoscope
positioning algorithm based on biological fluctuation. Proceedings of the 5th Asian
Conference on Computer Aided Surgery (ACCAS2009), p. 125, Changhua, Taiwan, July
2009
Nishikawa, A.; Yamada, Y.; Toda, S.; Sekimoto, M.; Miyoshi, N.; Takiguchi, S.; Doki, Y.;
Mori, M. & Miyazaki, F. (2009). Analysis of Decisions by Camera Assistants on the
Field of View for Laparoscopic Surgery and Its Application to Automatic
Positioning of a Laparoscope. Proceedings of the 21st International Conference of the
Society for Medical Innovation and Technology (SMIT2009), pp. 99–100, Sinaia,
Romania, October 2009
Pisla, D.; Plitea, N. & Vaida, C. (2008). Kinematic modeling and workspace generation for a
new parallel robot used in minimally invasive surgery. In: Advances in Robot
Kinematics: Analysis and Design, Lenarcic, J & Wenger, P, (Ed.), 459–468, Springer,
ISBN 978-1-4020-8599-4 (Print) 978-1-4020-8600-7 (Online)
Polet, R. & Donnez, J. (2004). Gynecologic laparoscopic surgery with a palm-controlled
laparoscope holder. The Journal of the American Association of Gynecologic
Laparoscopists, Vol. 11, No. 1, February 2004, 73–78
Sackier, J. M. & Wang, Y. (1994). Robotically assisted laparoscopic surgery. From concept to
development. Surgical Endoscopy, Vol. 8, No. 1, January 1994, 63–66, ISSN 0930-2794
(Print) 1432-2218 (Online)
Sekimoto, M.; Nishikawa, A.; Taniguchi, K.; Takiguchi, S.; Miyazaki, F.; Doki, Y. & Mori, M.
(2009). Development of a compact laparoscope manipulator (P-arm). Surgical
Endoscopy, Vol. 23, No. 11, November 2009, 2596–2604, ISSN 0930-2794 (Print) 1432-
2218 (Online)
Taniguchi, K.; Nishikawa, A.; Sekimoto, M.; Kobayashi, T.; Kazuhara, K.; Ichihara, T.;
Kurashita, N.; Takiguchi, S.; Doki, Y.; Mori, M. & Miyazaki, F. (2010). Classification,
design and evaluation of endoscope robots. In: Robot Surgery, Seung Hyuk Baik,
(Ed.), 1–24, INTECH, ISBN 978-953-7619-77-0
Tanoue, K.; Yasunaga, T.; Kobayashi, E.; Miyamoto, S.; Sakuma, I.; Dohi, T.; Konishi, K.;
Yamaguchi, S.; Kinjo, N.; Takenaka, K.; Maehara, Y. & Hashizume, M. (2006).
Laparoscopic cholecystectomy using a newly developed laparoscope manipulator
for 10 patients with cholelithiasis. Surgical Endoscopy, Vol. 20, No. 5, May 2006, 753–
756, ISSN 0930-2794 (Print) 1432-2218 (Online)
Wagner, A.; Varkarakis, I. M.; Link, R. E.; Sullivan, W. & Su, L. M. (2006). Comparison of
surgical performance during laparoscopic radical prostatectomy of two robotic
camera holders, EndoAssist and AESOP: a pilot study. Urology, Vol. 68, 2006, 70–74
Wang, Y. F.; Uecker, D. R. &Wang, Y. (1998). A new framework for vision-enabled and
robotically assisted minimally invasive surgery. Computerized Medical Imaging and
Graphics, Vol. 22, No. 6, December 1998, 429–437.
Wei, G. Q.; Arbter, K. & Hirzinger, G. (1997). Real-time visual servoing for laparoscopic
surgery. Controlling robot motion with color image segmentation. IEEE Engineering
in Medicine and Biology Magazine, Vol. 16, No. 1, January-February 1997, 40–45, ISSN
0739-5175
18
1. Introduction
So far, the robotic applications has been dominated by proprietary based hardware and
software devices developed for industrial applications with a large volume manufacturing,
like the automotive and electronics industries. Then, the main goal of the automation
technologies has been an optimized robot design for precise assembly tasks, resulting in
complex systems with a reduced flexibility.
Traditional robotic applications have a fixed configuration, with the advantages of high
accuracy and a well studied kinematics. However, since recent years, the number of service
robots in our daily life environments is increasing. There are many new applications, i.e.
teleoperation, human-robot-collaborative works, etc. that require reconfigurable hardware
and expansibility to accomplish new working modes in not-structured scenarios and not-
intensive manufacturing tasks. However, these systems must meet diverse user
requirements and integrate different hardware and software systems developed for a
particular proprietary platform. As a result, many different researchers have solved similar
issues with non-interchangeable products, working from scratch each time, adapting the
traditional industrial robots platform for the new applications.
Today, a new robotic system is an integration of different processors and hardware
platforms manufactured by different vendors, controlled by software modules developed
using different programming languages and different communication protocols. In
addition, as robotic manipulator is expected to accomplish more complex tasks, it needs the
integration of multiple sensors working with different time bases and bandwidths (Gamez
et al., 2009; Luo et al., 2002), and new capabilities are needed that traditional control
technology of current industrial robots is not offering. In order to solve these problems
different open robotics platforms have been presented.
• Reusability is an issue that should be addressed from the beginning of the development
process, identifying common problems that could be solved with reusable solutions and
shared within the robotics community.
• Extensibility to change or add several component (of hardware or software) to the
system from different vendors.
• Adaptability/dynamic reconfigurability, providing mechanism of easy adaptation of its
parameters according to the application requirements.
• Interoperability refers to the ability to support interchange of information between
robotic modules designed by different vendors, providing effective communication and
working in a coordinated manner. In particular it relies on the network and
communication protocols that must provide effective real-time communication among
distributed components, independently of the system specific particularities.
Diverse approaches have been proposed to achieve these capabilities. Some solutions use
Matlab/Simulink and Real Time Workshop to generate control applications for robotic
systems with a proprietary operating systems (Gamez et al., 2007), but with the
disadvantage of a limited interoperability. Today, most of the research and robotic
applications developed based on proprietary hardware used a layered software architecture.
This approach typically includes a standard based middleware to provide integration,
efficient communication, interoperability, abstraction of software components, also
providing portability. At the top level different reusable software components are used. In
the low level layer, the hardware is controlled by drivers developed to run on a proprietary
RTOS. However, since last decade, developers have a growing interest on developing open
source applications based on Linux RTOS. Thus, vendors are offering commercial-grade
Linux operating systems (Saravanan et al., 2009; Gamez et al. 2009).
Another approach based on hardware modularity can support integration of new
components from various vendors. The corresponding software must provide a well
defined interface to provide easily integration between interconnected devices, and the
capabilities of extensibility and modification (Xuemei & Liangzhong, 2007).
As the hardware is always vendor-dependent, the integration of different devices may be
difficult due to incompatibility reasons. To overcome this problem, some hardware
standards have been proposed, however this method is considered too restrictive to achieve
reusability of existing hardware (Hong et al. 2001).
the participation of the Japan Robot Association (JARA). One of the objectives of the
project is to simplify the construction of customized robot combining selected RT-
components. In recent years, the Object Management Group (OMG) (OMG, 2008)
started a standardization process for these RT-components to achieve interoperability,
interconnectivity and integration of components from different manufacturers.
• In Korea, the Open Platform for Robotic Services (OPRoS) (Park & Han, 2009) is another
open software project promoted to unify different robots platforms. The framework
includes standardized components, an integrated development environment (IDE) and
a simulation and testing environment. OPRoS supports CORBA and the Universal Plug
and Play (UPnP) (Ahn et al., 2006) standards for modular integration. The operational
scheme employs a server-client model to interact with the robot system as a target
robot, and external servers for heavy computation.
• The Coupled Layered Architecture for Robotic Autonomy (CLARAty) (Nesnas et al.,
2003) was initiated in the NASA to provide a software framework to develop advanced
robotic technologies for robotic platforms employed in other NASA programs. Unlike
others architectures, CLARAty is a two-level architecture were the system
decomposition allows for intelligent behavior at low levels while the structure of the
different levels is maintained. In this scheme, the high level Decision Layer sends
command to the Functional Layer and in a client-server model, and the Functional
Layer provides different levels of abstractions to achieve adaptation of the reusable
components to the hardware of different robots. Also, the Decision Layer provides a
unified representation of activity plans based on a declarative model.
• For the Mobile and Autonomous Robotics Integration Environment (MARIE), the main
goal was to provide a common component-based middleware to reuse and interconnect
different programming environments (Cote et al., 2006). The framework followed a one-
to-many interaction model between different components to coordinate the interaction
within a virtual shared space, and allowing each component to use its own
communication protocol.
• MIRO (Utz et al., 2002) is a CORBA based middleware organized in three layers: a
device layer provides object-oriented interface abstraction for the hardware, and a
service layer provides CORBA interface services between the device layer and the top
layer. This layer provides reusability and easy integration in an object oriented
framework.
• In recent years, several RT-Linux based open projects are developed: RTOC (Xu & Jia,
2006) is a RT-Linux based architecture based upon the OSACA model (OSACA, 1996)
that can be ported to not PC-based platforms. In its layered model, a database stores
universal application modules for control, path planning, etc. Other Linux based
platforms use ST-RTL to generate control applications from Simulink models
(Ostrovrsnik et al., 2003). Xenomai (Xenomai, 2010) is another Linux-based Real Time
operating system used to develop robot control systems using open source and
standardized communications protocols (Sarker et al. 2006).
The remainder of this paper is organized as follows. Firstly, a brief explanation of the
necessity of these platforms is introduced in Section 2. Later, Section 3 describes the
hardware structure. In this section the main characteristics of both hardware configurations
are presented. In Sec. 4, the software structure is presented, while Sec. 5 presents
experimental results which validate the performance of the proposed architecture. Finally,
Discussions and Conclusions are presented in Section 6 and 7, respectively.
384 Advanced Strategies for Robot Manipulators
moving in either free or constraint space, the interaction forces and moments at the contact
point, and also the noncontact ones, are measured by this sensor (Gamez et al., 2004).
Furthermore, the magnitude of these dynamics disturbances cannot be ignored when large
accelerations and fast motions are considered (Khatib & Burdick, 1986), when the
manipulator carries out tasks with heavy tools (Johansson & Robertsson, 2003), or when the
environment is not perfectly known (not allowing the use of switching strategies that
compensate for the free space phase).
To solve this problem, the integration of different sensors such as a force/torque and a
acceleration sensor could be use to solve this problem (Gamez et al., 2008; Kroger et al.,
2007); however, fusion of data from multiple sensors into a robust and consistent model
meets some difficulties such as measurements with different time bases (Luo et al., 2002) or
noise and incompleteness of sensor data (Larsson et al., 1996). Another problem could be to
easily connect these sensors, which are from diverse manufacturers, to the hardware setup
(Gamez et al. 2009).
Thus, observing these problems, it can be guessed why a complex dynamic system, such as
robotic manipulator, is demanding new and highly sophisticated capabilities that traditional
control technology of current industrial robots is not offering (Wills et al., 2001).
context switching, timeliness, support for open standard. The external sensors used to
model the environment, and thus to make the robot capable of interacting with it, are: a ATI
wrist force/torque (f/t) sensor (MINI SI80-4) where the f/t strain gauge signals are
conditioned using an intermediate module, called supply board, and later transmitted
through a DAQ acquisition board which processes the strain gauge information and offers it
through the PCI slot. A 3D accelerometer, which was attached to the end-effector of the
manipulator and a 3D gyroscope of CFX Technology (an UCG-TX model). These two last
sensors were also read by the same acquisition board.
CS8 Controller
Teach
Pendant DAQ (PCI)
CPU Pentium
32 MB RAM
Force and
acc. sensors
+
Servo Vision Sensors
USB Board Controllers
Haptic (original
Device PC I/O
Ethernet units)
Ethernet
MASTER PC
Haptic Device
DAQ (PCI)
Parallel
Port
UP TO DATE
COMPUTER Force and
Vision Sensors acceleration
sensors
CS8
IEEE 1394 CONTROLLER
Ethernet
4. Software structure
In this section, we describe a component-based control software architecture developed in
order to get a robust and easy-to-maintain experimental robotic platform. Two fundamental
388 Advanced Strategies for Robot Manipulators
goals were established for the architecture: first, it should standardize functions that are
common across sensors and open robotic platforms; second, the architecture should enable
design by composition. Since the most interesting configuration is the new one, we limit this
section to its the description. Further information about the old software configuration can
be found in (Gamez et al. 2010).
The end-user layer describes the task to be carried out in terms of final positions,
orientations and velocities of the robot end-effector. Different components have been
developed and they are used depending if the task to be carried out is in open space, with
constraint motion or with both. In addition, another component has been designed which is
in charge of controlling the haptic device. The inputs to the components of this layer can be
the reference position-orientation of the robot TCP, the desired contact forces exerted by the
manipulator to the environment or even vision features. Currently, these inputs can only be
defined off-line, not taking the most significant advantage of on-line programming, that is,
the robot can be programmed in accordance with the actual position of equipment and
pieces of these modules.
The high-level layer is compound basically of two components with the functions of a path
planner. This planner generates trajectory set points for the robot, according to motion
command which it receives from task specification. The commands these components offer
to their lower layer can be either the joints trajectory or the Cartesian trajectory of the robot
end-effector. It is necessary to point out that both the original task planning and the original
trajectory generator developed by Stäubli were not used in this platform due to proprietary
reasons. For our applications, the components designed for the special-purpose planner
calculate the joint coordinates from the Cartesian references solving the inverse kinematics
on line (Gamez, 2006). In this sense, a second component has been developed to reduce the
computational cost of the previous block if necessary. Specifically, it consists of the
decomposition of the robot geometric structure into two subsystems: one for position and
one for orientation. This option offers an analytic solution that simplifies the singularities
problem. Furthermore, a number of restrictions have been imposed to prevent special
singularities such as shoulder and wrist ones. Although the developed trajectory is not
robust, the resultant workspace is acceptable for most of the practical cases. Currently, these
components are used from the former configuration and, in the future, we expect to
improve them using more sophisticated trajectory generators than can be found, for instant,
in (Bruyninckx, 2001).
For the middle-level layer, and from an engineering point of view, we note that tailoring the
motion control requires control engineering competence while application support does not
(Nilsson & Johansson, 1999). Although is therefore reasonable and appropriate to define two
different sub-layers for these types of programming: application control layer, (movement
constraints, tool mass, etc.) and control layer (to configure the control loop, tunes the gains,
etc.), this level is built, on the one hand, using manipulator control components. On the
other hand, other kind of components that are used in this layer are the virtual sensor
components. These elements allowed the application of sensor fusion strategies in a
structured way. Both components are designed with Simulink and the Real Time Toolboox
of Matlab.
Using the property that any Simulink control model is an interconnection of signals
(reference commands, position feedback, velocity feedback, torque feedback, sensors
feedback) and mathematical operations, a generic block has been designed with a
predefined number of inputs and outputs. Inside each block, one can implement different
control algorithms, or sensor fusion strategies, combining a high-performance language for
technical computing with a fast prototyping of the robotic platform since all the inputs and
outputs are readable and writable.
390 Advanced Strategies for Robot Manipulators
In the lowest layer dedicated to the sensor, each one is modeled by a component that
contains basically two parts: one is for data structure building and the other is for sensory
data sharing. One of the function of the sensor components is to process the information of a
specific sensor and to provide a unified sensory data bank manager. The main advantage of
this manager is that it can directly offer the calibrated sensor data. Furthermore, sensor data
must be shared with every necessary function in the software architecture. Another
important function of the sensor components, and on the rest of components, is to stamp a
time when a set of measurements, or interaction, is obtained. It helps to obtain a history of
the events.
4.2 Middleware
In our case, we have to different software contexts: this one placed at the controller PC and
the second one running on the master PC. In the controller PC, where the component of the
joints control sub-layer is running, to guarantee that the shared memory constraints are
fulfilled, the system has to protect itself from invalid memory accesses that otherwise could
compromise the system.
In this case, to avoid this problem, between the component and the monitoring task, the
tasks are synchronized following a structure "top to bottom" where the maximum priority is
given to the joints control task. The operating system running on this PC was VxWorks
(Wind-River, 2005).
Another problem was to synchronize different components that are placed in a master PC
with interconnections with external systems and a Real Time Linux operating system. The
solution selected was to choose XENOMAI (Xenomai, 2010) with the RTNet (Real Time
Open Software Architecture for Advanced Control of Robotic Manipulators 391
Network) package. For our case, the synchronization scheme is not based on a master clock
(as it was in the former configuration, where the it followed a "top to bottom" structure).
Each component has its own clock, updating data with their respective bandwidth.
Currently, we are implementing a middleware using concepts similar to those ones defined
in OROCOS project (Bruyninckx, 2001). In a middle-long term, our intention is to obtain a
user-friendly API that allows fast and easy prototyping. Figure 7 shows the block diagram
of the hardware and software communications differentiating between the master PC and
the controller PC. It can be guessed from this figure that each component communicates
with other ones mainly through shared memory, or through Ethernet depending on where it
is placed.
5. Experimental validation
Different experiments have been carried out to validate the performance of the proposed
architecture, noting that these results are obtained from the old hardware configuration.
They consisted in the application of a compliant motion controller where the environment
392 Advanced Strategies for Robot Manipulators
information was obtained fusing different sensors. In particular, for the case shown in this
paper, the sensors used were a force/torque sensor, an accelerometer and the joint sensors.
The objective of this integration was to develop a force observer capable of estimating,
accurately and from the f/t sensor measurements -which reflect the contact forces, the
inertial ones and the gravity forces- , the contact force exerted by a manipulator to its
environment (Gamez et al., 2008).
To carry out this test, some of the components that were described before in the previous
section were used.
Fig. 8. Force measurement from the wrist sensor ATI (upper-left), force observer output
(upper-right), accelerometer output (lower-left) and real and measured position of the robot
tip for y-axis (lower-right).
The results obtained applying the force/position controller, where the information of the
force observer is used, are presented in Fig. 8. The experiment consisted of a movement in
the axis z of three phases: an initial movement in free space (from t =5s to t = 6.2s), a contact
transition (from t =6.2s to t = 6.4s) and a movement in constrained space (from t =6.4s to t =
9s). Apart from the force compensation shown in Fig. 8, it can be also compared how the
observer eliminates the inertial effects and the noise introduced by the sensors. Note the
time lag between the filtered signal and the original one. It was because the selection of the
gains made the poles of observer to be quite near the unit-circle (Gamez, 2006). The force
control loop applied was an impedance controller (Hogan, 1985).
Open Software Architecture for Advanced Control of Robotic Manipulators 393
6. Discussion
The construction of the open robotic system developed in the framework of this work was
necessary because, as it is well known, industrial manipulators do not offer, with an
appropriate bandwidth, the possibility of integrating either advanced control algorithms or
new sensors into the software-hardware architecture. This fact forced the research
community to extend an industrial manipulator architecture in order to get a completely
open one in both senses: hardware and software.
The proposed platform was designed considering a multi-layer structure that simplified the
integration of external functionality in several ways. The first one consisted of offering
different interfaces where the user was capable of reading all the parameters and variables,
besides modifying the commanded signals, with a considerable bandwidth. The second one
pretended to avoid the limitation of the industrial robots where the current methodology is
to control exclusively the position without considering high level strategies for task decision
making, or without taking into account new sensors that could improve the environment
modelling.
Certainly, a pending aspect of this platform is the the man-machine interaction. New
solutions in the area of software technology have to be included in order to create a more
friendly-interface that permits to modify easily the requirements of the system, especially
for the experiment generation. Perhaps, creating pseudo intelligent task interpreters, as a
experimental interface, will play an important role.
Furthermore, it has to be pointed out that the design solutions have been driven following a
trade-off between mass products (paying attention to the cost) and standardization
requirements (leading edge technology).
On the other hand, the proposed architecture was based on consolidated open robotic
platforms, specially on those ones developed in Lund University (Sweden) and Leuven
University (Belgium). These platforms have been developed during several decades and
have accumulated a great deal of experience, representing an excellent paradigm for initial
developments. In addition, a narrow collaboration with the company of the manipulator
robot has existed, what allowed access to internal functions and hardware what would be
impossible in other conditions.
To conclude this section, the development of this kind of platforms does not only prove to
be useful for testing advance control algorithms, but also it is necessary to emphasize the
necessity of building such systems since, from a robotic research point of view, and mainly,
from a robotic manufacturers overview, it helps to increase the development speed opening
up the systems for third party.
7. Conclusions
This work describes an experimental platform that allows the implementation of model-
based and sensor-based control algorithms in robotic manipulator. Particulary, this new
system allows to easily integrate new sensors and advance control algorithms in an Stäubli
industrial 6-dof robot using a component-based software methodology.
Based on a component-based development approach, two possible configurations were
described, explaining why the original structure was modified migrating to a new one
where the Master PC was different to the controller PC. It is also explained how the fact of
using this paradigm allowed an easy reconfiguration of the robotic platform, demonstrating
394 Advanced Strategies for Robot Manipulators
that the use of components -i. e. Sensor components- , that are independent of the context,
allowed as well an important restructuration of the new robotic architecture.
The resulting architecture has been designed, among other objectives, to allow different
sensors to be easily switched and rewired depending on the new sensor fusion or control
strategy that must be tested. Together with the component-based development approach, a
software structure of layers has been proposed to facilitate the design, configuration and
testing of new control algorithms and sensor fusion techniques. This structure allows
systems of components, with standardized interfaces, to be connected while abstracting
away implementation details of components.
Eventually, a number of experiments were performed to validate the performance of the
proposed architecture and its capacity of allowing a fast and easy implementation of
advance control algorithms in non-structured environments.
8. References
Ahn, S. C., Lee, J.-W., Lim, K.-W., Ko, H., Kwon, Y.-M. & Kim, H.-G. (2006). Requirements to
UPnP for Robot Middleware, Proc. of the 2006 IEEE/RSJ Int. Conf. on Intelligent
Robots and Systems (IROS), Beijing, pp. 4716 -4721.
Ando, N., Suehiro, T., Kitagaki, K. & Kotoku, T. (2006). RT (Robot Technology)-Component
and its Standarization, SICE-ICASE International Joint Conference, 2006, pp. 2633-
2638.
Bruyninckx, H. (2001). Open robot control software: the OROCOS project. Proc. of the 2001
IEEE Int. Conf. on Robotics and Automation (ICRA), pp. 2523–2528.
Chishiro, H., Fujita, Y., Takeda, A., Kojima, Y., Funaoka, K., Kato, S. & Yamasaki, N. (2009).
Extended RT-Component Framework for RT-Middleware, IEEE International
Symposium on Object/Component/Service-Oriented Real-Time Distributed Computing,
2009 (ISORC), pp. 161-168.
Cote, C., Brosseau, Y., Letourneau, D., Raievsky, C. & Michaud, F. (2006). Robotic Software
Integration Using MARIE, International Journal of Advanced Robotic Systems, 2006, pp.
055-060.
Gamez, J., Robertsson, A., Gomez, J. & Johansson, R. (2004). Sensor fusion of force and
acceleration for robot force control. Int. Conf. Intelligent Robots and Systems (IROS
2004), 2004, pp. 3009–3014.
Gamez, J., Robertsson, A., Gomez, J. & Johansson, R. (2005). Force and acceleration sensor
fusion for compliant robot motion control. IEEE Int. Conf. on Robotics and
Automation (ICRA2005), 2005, pp. 2709 - 2714.
Gamez, J. Sensor Fusion of Force. (2006). Acceleration and Position for Compliant Robot
Motion Control. Phd thesis, Jaen University, Spain, 2006.
Gamez, J., Gomez, J. Nieto, L. & Sanchez Garcia, A. (2007). Design and validation of an open
architecture for an industrial robot control, IEEE International Symposium on
Industrial Electronics (IEEE ISIE 2007), 2007, pp. 2004–2009.
Gamez, J., Robertsson, A., Gomez, J. & Johansson, R. Sensor fusion for compliant robot
motion control. IEEE Trans. on Robotics, 2008, pp. 430–441.
Gamez, J., Gomez, J., Sanchez, A. & Satorres, S. (2009). Robotic software architecture for
multisensor fusion system. IEEE Trans. on Industrial Electronics, 2009, pp. 766–777.
Groover, M. P. (2008). Automation, Production Systems and Computer-Integrated
Manufacturing. Pearson Education, Upper Saddle River, New Jersey, USA, 2008.
Open Software Architecture for Advanced Control of Robotic Manipulators 395
Utz, H., Sablatnög, S., Enderle, S. & Kraetzschmar, G. (2002). MIRO - Middleware for mobile
robot applications, IEEE Transactions on Robotics and Automation, 2002, pp. 493 – 497.
Wills, L., Kannan, S., Sander, S., Guler, M., Heck, B., Prasad, J., Schrage, D. & Vachtsevanos,
G. (2001). An Open Platform for Reconfigurable Control, IEEE Control Systems
Magazine, 2001, pp. 49 – 64.
Wind-River. VxWorks: Reference Manual. Wind River Systems, 2005.
Wind-River Linux. http://www.windriver.com/products/linux/, 2010.
Xenomai: Real-Time Framework for Linux. http://www.xenomai.org/, 2010.
Xu, H. & Jia, P. (2006). RTOC: A RT-Linux Based Open Robot Controller, IEEE/RSJ
International Conference on Intelligent Robots and Systems, 2006, pp. 1644 – 1649.
Xuemei, L. & Liangzhong, J. (2007). Study on control system architecture of modular robot.
Proc. of the 2007 IEEE Int. Conf. on Robotics and Biometrics, 2007, pp. 508 – 512.
19
China
1. Introduction
During the past two decades, parallel manipulator system has become one of the research
attentions in robotics. This popularity has been motivated by the fact that parallel
manipulators possess some specific advantages over serial manipulators, i.e., higher rigidity
and load-carrying capacity, better dynamic performance and a simpler inverse position
kinematics, etc. Among various manipulators, the best-known is the Gough-Stewart
Platform (GSP) that was introduced as a tire performance (Gough 1956-57) and an aircraft
simulator (Stewart 1965).
One of the important problems in robot kinematics is special configuration or singularity. As
to parallel manipulators, in such configurations, the end-effector keeps at least one remnant
freedom while all the actuators are locked. This transitorily puts the end-effector out of
control. Meanwhile, the articular forces may go to infinity and cause mechanical damages.
Determination of the special configurations of the six-DOF Gough-Stewart parallel
manipulators is a very important problem. It is one of the main concerns in the analysis and
design of manipulators. The singularity analysis of parallel manipulators has attracted a
great deal of attention in the past two decades. Hunt (1983) first discovered a special
configuration for this manipulator that occurs when the moving triangle-platform is
coplanar with two legs meeting at a vertex of the triangle, and all the six segments
associated with six prismatic actuators intersect a common line. Fichter (1986) discovered a
singularity of the parallel manipulator. That occurs when the moving platform rotates ψ
=±π/2 around Z-axis, whatever the position of the moving platform is. That mechanism has
a triangular mobile platform and a hexagonal base platform. It may be named a 3/6-GSP.
Huang and Qu (1987) and Huang, Kong and Fang (1997) also studied the singularity of the
parallel manipulator, whose moving and basic platforms are both semi-regular hexagons
(6/6-GSP). It also occurs whenψ= ±π/2. Merlet (1988, 1989) studied the singularity of the
six-DOF 3/6-GSP more systematically based on Grassman line geometry. He discovered
many new singularities including 3c, 4b, 4d, 5a and 5b. 3c occurs when four lines of the six
legs intersect at a common point; 4b occurs when five lines are concurrent with two skew
lines; 4d occurs when all the five lines are in one plane or pass through one common point
398 Advanced Strategies for Robot Manipulators
in that plane; 5a is in general complex; 5b occurs when the six segments cross the same line.
Based on line geometry, wrench singularity analyses for platform devices have been
presented by Collins & Long (1995), and Hao & McCarthy (1998). Gosselin and Angeles
(1990) pointed that singularities of closed-loop mechanisms can be classified into three
different groups based on the Jacobian matrices. This classification was further discussed by
Zlatanov, Fenton and Benhabib (1994, 1995). Zlatanov, Bonev and Gosselin (2002) discussed
constraint singularities. Ma and Angeles (1991) studied architecture singularities of parallel
manipulators. Kong (1998) also discussed architecture singularities of the general GSP.
McAree and Daniel (1999) discussed the singularity and motion property of a 3/3-parallel
manipulator. Karger and Husty (1998), Karger (2001) described the singular positions and
self-motions of a special class of planar parallel manipulators where the platform is similar
to the base one. It is shown that it has no self-motions unless it is architecturally singular.
Kong (1998), Kong and Gosselin (2002) also studied self-motion. Chan and Ebert-Uphoff
(2000) studied the nature of the kinematic deficiency in a singular configuration by
calculating the nullspace of the Jacobian matrix. Di Gregorio (2004) studied the SX-YS-ZS
Structures and Singularity.
Many researches dealt only with isolated singular points in space. However, in the practical
configuration space of parallel manipulators the singularity configuration should be a
continuous singularity curve or even be high-dimension surface. One of the main concerns
is further to find out its singularity loci and their graphical representations, as well as the
structure and property of the singularity loci. That is of great significance in a context of
analysis and design since it allows one to obtain a complete picture of the location of the
singular configurations in the workspace. For a given practical application, it is therefore
easy to decide whether the singularities can be avoided. Sefrioui and Gosselin (1994, 1995)
studied singularity loci of planar and spherical parallel mechanisms. Wang and Gosselin
(1996, 1997) used the numerical method to study the singularity loci of spatial four- and five-
DOF parallel manipulators. Collins and McCarthy (1997, 1998) studied singularity loci of the
planar 3-RPR parallel manipulator, and 2-2-2 and 3-2-1 platforms and obtained cubic
singularity surfaces. For the six-DOF Gough-Stewart Platform, however, the singularity
expression generally is quite complicated, and difficult to analyze. Recently, Wang (1998)
presented a method to analyze the singularity of a special form of the GSP and derived
corresponding analytical singularity conditions. Di Gregorio (2001, 2002) also discussed the
singularity loci of 3/6 and 6/6 fully-parallel manipulators. In particular, Mayer St-Onge and
Gosselin (2000) analyzed the Jacobian matrix of general Stewart manipulators by two
different new approaches. They derived a simpler explicit expression from the Jacobian
matrix, and pointed out that the singularity locus of the general Gough-Stewart manipulator
should be a polynomial expression of degree three. They also gave the graphical
representations of the singularity loci.
For practical application, we want to obtain a simpler algebra expression of the singularity
loci, their accurate graphical representations and know whether it consists of some typical
geometrical figures. But this is very difficult for the Gough-Stewart manipulator. Huang et
al. (1999, 2003) studied the singularity kinematics principle of parallel manipulators, and
proved a new kinematics sufficient and necessary condition to determine the singularity.
Using this method he discovered the characters of singularity locus of the 3/6-Gough-
Stewart platform firstly. It shows that the singularity locus of the 3/6-Gough-Stewart
platform is resolvable and consists of two typical geometrical graphs, a plane and a
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 399
hyperbolic paraboloid, for the special orientations: φ=±30°, ±90°, or ±150°. However, the
singularity locus expression of degree three is irresolvable, and the locus graph in infinite
parallel principal sections includes a parabola, four pairs of intersecting straight lines and
infinite hyperbolas for the general orientations: φ≠±30°, ±90°, and ±150°.
For the singularity loci of the 6/6-GSP which is a more general structure form and widely
used in practice, its graphical representations of the singularity loci for different orientations
are quite various and complex. Huang and Cao (2005) analyzed the singularity loci both in
3-D space and in the principal-section on which the moving platform lies. The singularity
locus equation of this class of Gough–Stewart manipulators in three-dimensional space is
also irresolvable, and the curves in infinite parallel principal sections of the singularity loci
also contains one parabola, four pairs of intersecting straight lines, and infinite hyperbolas.
We also found out an incredible phenomenon, in that special configuration six lines
associated with the six extensible links of the 6/6-Gough-Stewart manipulator can intersect
the same common line and the remnant instantaneous motion of the manipulator is a pure
rotation.
All the above-mentioned analyses are only about positional singularity when the orientation
of the moving platform is specified and invariable. On the other hand, there is a need to
further find out the orientation-singularity space when the position of the moving platform
is specified and invariable. Some researchers began to study the issue, such as Pernkopf and
Husty (2002); Cao, Huang & Ge (2006). Of course, for this topic there is still much work to be
done in depth.
a1 P + a2Q + a3 R + a4 L + a5 M + a6 N = 0 (1)
$m = ( a1 a2 a3 ; a4 a5 a6 ) (2)
Its pitch is
a1 a4 + a2 a5 + a3 a6
hm = (3)
a12 + a22 + a32
$ = (L M N ; P Q R ) (4)
and we have
LP + MQ + NR = 0 (5)
where $ denotes a line vector. The infinite line vectors satisfying Eq. (1) composed a line
complex.
400 Advanced Strategies for Robot Manipulators
$m
Fig. 2 The velocity relationship of three non collinear points in a moving body
Theorem 2: When three velocity directions of three points in a rigid body are given, then
three normal planes of the three velocities are determined. If the intersecting point of the
three planes lies in the plane determined by the three points, the three velocities can
determine a twist; otherwise, the given velocities are improper and cannot determine a twist
of that body.
The thinking of the velocity analysis in the proof of Theorem 2 itself is also useful for
singularity study of the 3/6-GSP.
The 3/6-GSP is a typical manipulator which many authors paid attention to. The 3/6-GSP is
represented schematically in Fig.3. It consists of a mobile platform B1B3B5, equilateral
triangle; a base platform C1…C6, semi-regular hexagon; and they are connected via six
extensible prismatic actuators.
When all the legs of 3/6-GSP are locked, the three normal planes of three velocities VB1, VB3
and VB5 are respectively B1C1C2, B3C3C4 and B5C5C6 (Fig.3). According to Theorem 2, we may
educe the following deduction to determine the singularity of 3/6-GSP. Let us firstly define
a “Star-frame C-B1B3B5” in the moving platform. It is constructed by using three ray lines
passing three points, B1, B2 and B3, of the triangle B1B3B5 and intersecting at a common point
C called the center of Star-Frame.
Theorem 3: A necessary and sufficient condition that the three velocities of three points in a
rigid body can express that the body has a possible twist motion is that the intersecting
point of three normal planes of the three velocities lies in the plane determined by the three
points.
y
X' B1
B3 P
C
B1
B51
B3 C0
P
Y'
B5 X x
A1 V
Y A3 O O θ O
2
A5 U
Fig. 4. θ- Oblique plane for the orientation (90°, θ, 0)
The coordinates of points P, C, B31 and V with respect to O2-xyz are
P : ( x , y , 0)
C : (0, y , 0)
Rb 3 (6)
B31 : ( x −
, y+ Rb , 0)
2 2
3 Z0
V : (− , 0, 0)
3 sin θ
Considering O2C0=O2A3, we can obtain
namely
cosθ Z
3 Ra cos( β 0 / 2) − Z0 − Y0 = − 0 (8)
sin θ sin θ
In the right-angled triangle ΔO1O2C0, we obtain
cosθ
Y0 − u = −Z0 (9)
sin θ
Solving Eqs. (8) and (9) for Y0 and Z0, we obtain
Provided that the coordinates of an arbitrary point in line B31Vare ( xx , y y , 0) , its equation is
written as
404 Advanced Strategies for Robot Manipulators
3 R
yy − y − Rb xx − x + b
2 = 2 (11)
3 3 Z0 R
−y − Rb − −x+ b
2 3 sin θ 2
Since point C lies in line B31V, substitute the coordinates of point C ( xx = 0 and y y = y ) into
Eq. (9) and simplify as
Rb ZR
xy − y− 0 b =0 (12)
2 2 sin θ
Substituting Eq. (10) into Eq. (11) and eliminating Z0 , yield
Rb (3 Ra cos( β 0 / 2) − u)Rb
xy − y+ =0 (13)
2 2
Eq. (13) denotes a hyperbola and is independent of the Euler angle θ. The coordinates of its
center are (Rb/2,0), and its vertical and horizontal asymptotes are x= Rb/2, y=0
This is an important conclusion, as we have known that the singularity equation of GSP in 3-
dimension space is a polynomial expression of degree three. However, equation (13) is only
a quadratic equation in the special θ-plane. Eq. (13) only contains variables x and y, so it
denotes the positions of point p when the mechanism is singular. The equation is termed the
equation of the singularity curve in θ-plane.
When orientation of the mobile is given by three Euler angles (-90°, θ, 0), the singularity
equation can also be obtained in θ-plane with respect to the frame O2-xyz, the same as in Fig. 4.
Rb (3 Ra cos( β 0 / 2) − u)Rb
xy + y− =0 (14)
2 2
(a) For the orientation ( ±900 θ 0) (b) For the orientation ( ±900 θ 600 )
Fig. 5. The singularity curve in θ- plane
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 405
⎡ cos φ osθ cosψ − sin φ sinψ − cos φ cosθ sinψ − sin φ cosψ cos φ sin θ X ⎤
⎢sin φ cosθ cosψ + cos φ sinψ − sin φ cosθ sinψ + cos φ cosψ sin φ sin θ Y ⎥
[ T ] = ⎢⎢ − sin θ cosψ sin θ sinψ cosθ Z ⎥
⎥ (15)
⎢ ⎥
⎢⎣0 0 0 1 ⎥⎦
where(X, Y, Z) are the coordinates of point p with respect to the fixed frame. The coordinates
of point Bi in the mobile with respect to the fixed frame are
⎧Bix ⎫ ⎧Bix′ ⎫
⎪B ⎪ ⎪B′ ⎪
⎪ iy ⎪ ⎪ iy ⎪
⎨ ⎬ = [ T ] ⎨ ⎬ , i = 1, 2, 3 (16)
B
⎪ iz ⎪ ⎪Biz′ ⎪
⎪1 ⎪ ⎪1 ⎪
⎩ ⎭ ⎩ ⎭
x − B1 x y − B1 y z − B1 z
C 1 x − B1 x C 1 y − B1 y C 1 z − B1 z = 0 (17)
C 2 x − B1 x C 2 y − B1 y C 2 z − B1 z
where x, y and z are the coordinates of moving point in plane B1C1C2 with respect to the
fixed frame. Substituting coordinates of points B1, C1 and C2 into the above equation, we
obtain
Z y −Y z = 0 (18)
Note that, the equations of these planes are on the same condition that point P (X, Y, Z) is
located at some point and the orientation is denoted by three Euler angles (90°, θ, 0).
Solving Eqs. (18), (19) and (20) for x, y and z, then substituting them into Eq. (21) and
eliminating x, y and z, we obtain
Eq. (23) denotes that singularity locus of point P is a plane containing line C1C2 or A5A1,
namely, X-axis. As the plane and plane B1B3B5 denoted by Eq. (23) have the same normal
vector, and when plane B1B3B5 translates and coincides with plane expressed by Eq.(23), the
configuration is singular. The case belongs to the Hunt’s singularity and is the first special-
linear-complex singularity explained in Case 5. Eq. (23) shows that the mechanism is
singular, wherever point P locates in the plane.
The conicoid equation is
When θ is constant, Eq. (24) denotes a hyperbolic paraboloid and we will explain later.
Eq. (28) also represents a hyperbolic paraboloid.
⎡ −c − d 0 X⎤
⎢bd − bc a Y ⎥⎥
[ T ] = ⎢⎢ − ad ac b Z⎥
(25)
⎢ ⎥
⎣⎢0 0 0 1⎦⎥
where
a y + b z − aY − b Z = 0 (30)
Solving Eqs. (27), (28) and (29) for x , y and z , and then substituting them into Eq. (30), the
singularity equation is
Eq. (31) shows that the singular loci include a plane and a conicoid. The plane equation is
the same as Eq. (25). It also represents that in this case all the six lines cross a common line.
This case belongs to the first special-linear-complex singularity. The quadratic equation is
e Z 2 − f XZ + gYZ + h X − iY + j Z + k = 0 (32)
Eq. (32) is a singularity equation with respect to the fixed frame O-XYZ. When the mobile
shown in Fig. 5 rotates an angle ψ about Z″-axis again, its orientation is (90°, θ, ψ).
The plane in which the mobile lies is still θ-plane. After the coordinate transformation, the
equation of the singularity curve in θ-plane with respect to the frame O2-xyz is
f h
0 0 −
2 2
g i
0 0 −
2 = Rb sin θ cos 3ψ ≥ 0
2 6 2
Δ= 2 (34)
f g j 4
− e
2 2 2
h i j
− k
2 2 2
408 Advanced Strategies for Robot Manipulators
f
0 0 −
2
g
D= 0 0 =0 (35)
2
f g
− e
2 2
The following cases are discussed according to its invariants, in which D is always zero
whatever θ and ψ are.
1. If θ≠0, ψ≠±30°, ±90°, and ±150°, then D=0, Δ>0, the singular locus denoted by Eq. (32) is
a hyperbolic paraboloid. Generally, the six lines 1, 2, …, 6 belong to a general linear
complex when point P locates at the surface.
2. If θ=0, Eq. (32) can be written as
4(sinψ )Z 3 = 0 (37)
a. When ψ=0 and Z≠0, namely, the orientation is (90°, 0, 0), Eq. (37) is an identical
equation and the mechanism is singular whatever the position of point P in three-
dimensional space is. This is the Fichter’s singular configuration and all the six
lines belong to a general linear complex.
b. When Z=0, the moving platform and the base are coplanar. The mechanism is also
singular whatever Euler angle ψ is. The mechanism holds three remnant freedoms
when all the legs are locked. In this case, there exist the first and the second special-
linear-complex singularities.
3. If θ≠0, ψ=±30°, ±90°, or ±150°, then D=0, Δ=0 and J≠0, and the conicoid degenerates into
a pair of intersecting planes. For instance, when ψ=±30°, two equations are
2 Z − Rb sin θ = 0 (38)
When ψ=-30°, ±90°, or ±150, the conicoid also degenerates into two planes. The singularity
cases are similar to the above.
3.2.2.3. Analysis of Other Singularities
The singularities discussed above are all for the orientations, (±90°, θ, ψ), of the mobile. In
these cases, the intersecting lines between the oblique moving plane and the basic one are
parallel to line C1C2 or A1A5, one of the three sides of the triangle A1A3A5.
The similar singularities with a plane equation and a quadratic one can also occur when the
orientations are as follows
1. The Euler angles are
(–150°, θ, ψ) or (30°, θ, ψ)
All the intersecting lines between the oblique mobile and the base are parallel to line C3C4 or
A1A3.
2. The Euler angles are
(150°, θ, ψ) or (–30°, θ, ψ)
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 409
All the intersecting lines between the oblique mobile and the base are parallel to line C5C6 or
A3A5.
For the two cases, the singularity equation can also resolve into two parts: one is a plane
equation containing the corresponding side CiCj, another is a hyperbolic paraboloid
equation, too. When ψ =±30°, ±90°, or ±150°, the hyperbolic paraboloid also degenerates into
two planes.
However, when the orientation is
(φ, θ, ±30°), (φ, θ, ±90°) or (φ, θ, ±150°)
in which φ and θ, can be arbitrary values, the singularity locus also consists of two parts:
One is a plane; another is also a hyperbolic paraboloid. When point P translates in the plane,
two of three points B1, B3 and B5 lie in the basic plane.
(a) The orientation (90° 45° 0) (b) The orientation (90° 30° 60°)
(c) The orientation (90° 45° 30°) (d) The orientation (45° 25° 30°)
Fig. 6. The singularity loci for 3/6―Stewart parallel manipulator
410 Advanced Strategies for Robot Manipulators
The readers may wonder that the singularity loci are so huge and completed and ask how
can the GSP work? In practice, if you notice the position of the origin point of the O-XYZ
system in Figures, and the magnitudes of the parameters, Ra = 2 and Rm=1m, you can find
that the workspace of the manipulator is smaller relative the singularity loci shown in
figures. You can easily design the manipulator making its workspace locate over the
singularity loci and avoiding singularity.
X'
B3
Y'
P B1 x X
B5 ψ
y
W
Y
A3 A1 V
O
A5
U
Fig. 7. The general case
where (x′, y′, z′)denotes coordinates of the moving point on plane B1C1C2 in the fixed frame.
This gives
Similarly, equations of three planes B3C3C4, B5C5C6 and B1B3B5 can be obtained as well.
According to Theorem 3, solving the linear equation system of the four planes for
intersecting point C, the singularity locus equation for general orientations is as follows
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 411
f1 Z 3 + f2 XZ 2 + f3 YZ 2 + f4 X 2 Z + f5 Y 2 Z + f6 XYZ + f7 Z 2 + f8 X 2
(42)
+ f9 Y 2 + f10 XY + f11 XZ + f12 YZ + f13 Z + f14 X + f15 Y + f16 = 0
where (X, Y, Z)are the coordinates of center point P. It is a polynomial expression of degree
three. The equation is still very complicated and difficult to further analyze, but it is very
simple in the following special cases.
When φ≠±30°, ±90°, and ±150°and ψ is one of the values ±30°, ±90°, or ±150°, Eq. (42)
degenerates into a plane and a hyperbolic paraboloid as well. For example, whenψ=90°, the
singularity equation is
( 2Z + R b sin θ )( a 11 X 2 + a 22 Y 2 + a 33 Z 2 + 2a 23 YZ + 2a 31 ZX + 2a12 XY +
(43)
2a 14 X + 2a 24 Y + 2a 34 Z + a 44 ) = 0
where these coefficients are listed in the Appendix 2. Eq. (43) indicates a plane and a
hyperbolic paraboloid. The first factor forms a plane equation
2Z + R b sin θ = 0 (44)
which is parallel to the basic plane. When point P lies in the plane, the mechanism is
singular for orientation (φ, θ, 90°), because points B3 and B5 lie in the basic plane. This is
similar to Case 6. All the six lines cross the same line C1C2.
corresponding sides of the basic triangle. From Deduction 2, every position of the planar
mechanism corresponds to a special configuration of the original GSP. So we call it a
“singularity-equivalent-mechanism”. Thus the position solution of the planar mechanism
expresses the singularity of the original mechanism.
4.2.1.2 Forward Position Analysis of the Singularity -Equivalent-Mechanism
The frames are set as the same as in Fig. 5 and Fig. 10. The coordinates of point P in frame
O2-xy are (x, y). ψ indicates the orientation of the triangle B1B3B5 in θ,-plane. In order to
obtain the locus equation of point P, firstly we can set three equations of three lines passing
through the three vertices, and substitute the coordinates of points B1, B3 and B5 into the
equations, then (x, y)and ψ can be obtained.
y
X'
B3
PRPRP C
Y '
P ψ B1 L1
B5
L3 L2
α β
x
U O2 V
Y = ( tan α )( X + a / 2 ) (45)
Y = ( tan β )( X − a / 2 ) (46)
and
a tan a tan β
Y=− (47)
tan α − tan β
Solving Eqs. (52), (53) and (54) yields
Rb cosψ J 1 − ( 3 Rb sinψ + a )J 3
x= (48)
2( tan α − tan β )
(tanβ + tanα )
tanψ = (50)
3tanα - 3tanβ − 2tanα tanβ
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 413
where J 1 = tan α − tan β − 2 3 , J 2 = tan α − tan β − 2 3 tan α tan β , J 3 = tan α + tan β , Eqs. (48),
(49) and (50) denote direct kinematics of the mechanism.
4. 2. 1. 3. Singularity Equation in the θ- plane
Once the orientation (90°, θ, ψ)of the mobile platform is specified, in Fig.10, Euler angle ψ is
an invariant. So it only needs to choose one input in this case. From Eq. (65) one obtains
where a = 2(3 Ra cos( β 0 / 2) − u) / 3 . Eq. (52) denotes a hyperbola. Especially, when ψ=±90°,
Eq. (52) degenerates into a pair of intersecting straight lines respectively. Two of the four
equations are
y − R b /2 = 0 , y + R b /2 = 0 (53)
In both cases, two points B3 and B5 lie in line UV. So that four lines are coplanar with the
base plane. This is the singularity of Case 6. The similar situation is for ψ=30°,ψ=-150°,
ψ=-30° and ψ=150°.
X'
B3
PRPRP
Y' C
ψ
P
B1
α
L2 L1 x
B5
L3 W
β V
U
Fig. 9. The singularity-equivalent-mechanism for general case
y = (-R b tan α sinψ − 3R b tan α cosψ + 3R b tan α tan β cosψ + 2u tan α tan β
(55)
− 3R b tan α tan β sinψ -2R b tan β sinψ − 2w tan α tan β )/(2 tan β − 2 tan α )
where u indicates the distance from point U to V, and w the distance from V to W.
Substituting Eq. (56) into Eqs. (55) and (54), and eliminating ψ, the relations between (x, y)
and the inputs α, β can be obtained. This is direct kinematics of the equivalent mechanism.
4.2.2.3 Singularity Equation in the θ- plane
Under a general case, Euler angle φ can be any value with the exception of ±30°, ±90°, or
±150°. From Eq (56) one obtains
2 3w tan α
tan β = (57)
-2 3w tan α tanψ + 3u tan α tanψ -3u tanψ + 3u tan α + 3u
In the case of some specified ψ, there are the same three particular situations that is B1 and
B5, B1 and B3, or B3 and B5 lie in the line UV, respectively. The singularity loci are three pairs
of intersecting straight lines.
In order to use the above-mentioned formulas, u and w in Eq. (57) should be calculated in
advance. They depend on their relative positions in UV, as shown in Fig. 10.
The distance w between V and W is
3 Ra cos( β 0 / 2) − 3xV
w = WV = (68)
cos φ
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 415
Y
C
A3
y
φ x
A5 O γ
X
θ A1 W
φ V
U
Fig. 10. The Intersecting Line UW of two planes
The distance u between U and V is
2 3x V
u = UV = (59)
( 3 + cot φ ) sin φ
The sign of w is positive when point W is on the right side of V, and it is negative when W is
on the left side of V. It is similar for the sign of u.
For a given xv, the singularity equation in θ-plane can be obtained by eliminating the
parameter α
bxy + cy 2 + dx + ey + f = 0 (60)
0 b/2 d/2
1
D = b/2 c e/2 = − ( b 2 f + d 2 c − bde ) (61)
4
d/2 e/2 f
and
0 b/2 1
δ= = − b2 < 0 (62)
b/2 c 4
Generally, D≠0 and δ<0 for a general value of xv, so Eq. (60) indicates a set of hyperbolas.
Y Y
y Y' y
Y'
'
A3 X
Q B3 A3 Q B3
B1 X'
p
B1
x p x
θ φ θ φ
γ B5 γ
O X A5 X
A5 A1 (V, W) O B5(A1,V,W)
U U
[ y − Rb sin(ψ + 60° )][( − 3 sin(ψ ) + cos(ψ ))x + ( 3 cos(ψ ) + sin(ψ ))y + Rb ] = 0 (63)
One of them is
Case 2. UV passes through point A3. In this case xv=0, two points U and V coincide with
point A3. Eq. (60) degenerates into a pair of intersecting straight lines either
The first part of Eq. (65) indicates a straight line parallel to x-axis. Similarly when B1
coincides with point A3, the singularity of this point is the first special-linear-complex
singularity and the instantaneous motion is a pure rotation. When B1 does not coincide with
A3, the singularities of points lying in this straight line are the general-linear-complex
singularity and its instantaneous motion is a twist with hm≠0.
The second part of Eq. (62) denotes another straight line. The singularities of points lying in
this straight line are all the general-linear-complex singularity.
Case 3. UV passes point A5. In this case
two points U and W coincide with point A5. Eq. (60) degenerates into a pair of intersecting
straight lines.
The first factor indicates a straight line parallel to the x-axis. Similarly when B3 coincides
with A5, the singularity of this point is the first special-linear-complex singularity. When B3
does not coincide with A5, the singularities of points lying in this straight line are the
general-linear-complex singularity.
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 417
Similarly the second factor of Eq. (66) denotes another straight line. The singularities of
points lying in this straight line are all the general-linear-complex singularity.
Case 4. When
For the first straight line when β0=90°, (φ, θ, ψ)=(60°, 30°, 0), and the coordinates of point P6
are x = Rb /2, y = (2 2 Ra - Rb )/2 3 , point B5 lies in the intersecting line of two normal
planes B1A1A5 and B3A1A3. Therefore, the six lines associated with the six extensible links of
the 3/6-GSP intersect a common line B5A1. It is the first special-linear-complex singularity.
The instantaneous motion is a pure rotation about line B5A1. The singularities of points lying
in the first line with the exception of the above-mentioned point and the points lying in the
second line are all belong to the general-linear-complex singularity.
Case 5. When
cy 2 + dx + ey + f = 0 (69)
According to the analysis mentioned above, it is shown that the singularity expression in θ-
plane is not cubic but always quadratic. This indicates the θ-plane is a very special cross
section of the singularity surface, so the special θ-plane can be called the principal section.
Generally speaking, the singularity loci of the 3/6-GSP for the most general orientations are
different from those for some special orientations. The singularity loci in infinite parallel
principal sections are all quadratic equations. The structure of the singularity loci in the
principal sections of the cubic singularity surface includes a parabola, four pairs of
intersecting straight lines and infinity of hyperbolas. The singularity loci in three-
dimensional space are illustrated in Fig. 12.
In addition, it should be pointed out that once the mechanism is singular at the orientation
(φ, θ, ψ), any orientation with different variable θ is singular as well (Huang at el. 2003).
(a) for orientation (600 ,450 ,450) (b) with a principal section xv=-4
(c) for orientation (600 ,600 ,450) (d) with a principal section xv=-4
Fig. 12. The singularity loci in three-dimensional space for the general orientations
⎛ S S2 S3 S4 S 5 S6 ⎞
[J] = [$1 $2 $6 ] = ⎜ 1
T
$3 $4 $5 ⎟
⎝ SO 1 SO 2 SO 3 SO 4 SO 5 SO 6 ⎠
⎛ (B1 − C 1 ) (B2 − C 2 ) (B3 − C 3 ) (B4 − C 4 ) (B5 − C 5 ) (B6 − C 6 ) ⎞
⎜ B −C (70)
B2 − C 2 B3 − C 3 B4 − C 4 B5 − C 5 B6 − C 6 ⎟⎟
=⎜
1 1
⎜ (C 1 × B1 ) (C 2 × B2 ) (C 3 × B3 ) (C 4 × B4 ) (C 5 × B5 ) (C 6 × B6 ) ⎟
⎜⎜ ⎟
⎝ B1 − C 1 B2 − C 2 B3 − C 3 B4 − C 4 B5 − C 5 B6 − C 6 ⎠⎟
where vectors, Bi, Ci (i=1, 2, …, 6), respectively denote the vertex vectors of the moving and
base platforms with respect to the fixed frame,Fig. 15; $i (i=1, 2, …, 6)is a line vector of the
corresponding extensible link, and its Plücker coordinates are as follows $i=(Si; SOi)=(Li, Mi,
Ni; Pi, Qi, Ri)where the subscript i (i=1, 2, …, 6) indicates the ith limb connected by two
vertices Bi, Ci of the moving and base platforms of the manipulator. Si is a unit vector
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 419
specifying the direction of line vector $i, and SOi is a vector indicating the position of the line
vector together with Si.
f 1Z 3 + f 2 XZ 2 + f 3YZ 2 + f 4 X 2 Z + f 5Y 2 Z + f 6 XYZ + f 7 Z 2 + f 8 X 2 +
(71)
f 9Y 2 + f 10 XY + f 11 XZ + f 12YZ + f 13Z + f 14 X + f 15Y + f 16 = 0
Eq. (71) represents the constant-orientation singularity locus of this class of the Gough-
Stewart manipulators in the Cartesian space for a constant orientation (φ, θ, ψ). It is a
polynomial expression of degree three in the moving platform position parameters XYZ.
420 Advanced Strategies for Robot Manipulators
Coefficients of Eq. (71), fi (i=1, 2, …, 15, 16), are all functions of geometric parameters, Ra, Rb
and β0, and orientation parameters, (φ, θ, ψ), of the manipulator.
Graphical representations of the constant-orientation singularity locus of the manipulator
for different orientations are given to illustrate the result, as shown in Fig. 14. Geometric
parameters used here are given as Rb=2, Ra=1.5, β0=π/2.
(a) for orientation (90°, 60°, 30°) (b) for orientation (-90°, 30°, 60°)
(c) for orientation (60°, 30°, 45°) (d) for orientation (45°, 30°, 45°)
Fig. 14. Singularity loci for different orientations
From Figure 14, it can be clearly seen that the singularity loci for different orientations are
quite different, and they are complex and various. Among them, the most complicated
graph of the singularity loci looks like a trifoliate surface, whose two branches are of the
shape of a horn with one hole (Figure 14 (c) and (d)).
shows the position of the manipulator for orientation (φ, θ, ψ). The oblique plane is θ-plane
on which the moving platform lies.
Y
A3
C5 C4
X'
y
X ψ
U O
C6 C3
π /2 +φ Y' P
A5 A1 x
C1 W C2
V
Fig. 15. The position of the manipulator for orientation (φ, θ, ψ)
When θ≠0, the moving platform is not parallel to the base one. θ-plane intersects the base
plane at a line UWV, where points U, W, V are intersecting points between θ-plane and
three sides, A3A5, A1A5, and A3A1, of the base hexagon, as shown in Fig. 15. We set another
moving reference frame V-xy in θ-plane, and the coordinates of point P in this moving frame
V-xy are denoted by (x, y).
Equations of three lines, A3A5, A1A5, and A3A1, in the fixed frame O-XYZ can be easily
written. Owing to space limitations, we do not present these equations here. As point V, i.e.,
origin of the moving frame V-xy, lies on line A3A1, and it can be assumed that the
coordinates of point V with respect to the fixed frame O-XYZ are
V : ( XV , YV , 0)
where XV is a variable indicating the position of θ-plane, i.e., the position of the moving
platform for any given geometric and orientation parameters, and YV can be established by
the following expression
Therefore, coordinates of points U and W can be easily obtained. The coordinates of point P
designated by (X, Y, Z) with respect to the fixed frame and (x, y) in the moving frame V-xy
satisfy the following expression
Substituting Eq. (81) into Eq. (78) and after some rearrangements and factorizations, the
singularity locus equation of the manipulator in θ-plane can be written as follows
422 Advanced Strategies for Robot Manipulators
Since θ≠0, the singularity locus equation of the manipulator with respect to θ-plane becomes
ax 2 + 2 bxy + cy 2 + 2 dx + 2 ey + f = 0 (76)
It can be proved that the coefficient c is always equal to zero, so Eq. (76) is a quadratic
polynomial expression with respect to x and y, and the maximum degree of variable x is 2
and y is 1. Coefficients a, b, d, e, f of Eq. (76) are all functions of geometric parameters Ra, Rb
and β0, Euler angles (φ, ψ) and XV. They are all independent of Euler angle θ. Generally, the
intersecting curve between a cubic surface and a plane is also a cubic expression that may
also contain a closed-loop curve. For example, when Ra=2, Rb=1.5, β0=π/2, (φ, θ, ψ)=(π/3,
π/6, π/4), intersecting curves between the corresponding singularity locus surface and the
following two planes, Z=-Y/3 and Z=-4(X-14)/45, are respectively presented as follows
(a) with the plane Z = −Y / 3 (b) with the plane Z = −4( X − 14) / 45
a b
δ= = ac − b 2 = −b 2 (77)
b c
a b d
D= b c e = −( a e 2 + f b 2 − 2b d e ) (78)
d e f
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 423
For any given geometric parameters and orientation parameters, generally, D≠0 and δ<0 for
general values of XV, so Eq. (76) indicates a set of hyperbolas shown in Fig. 19. Geometric
parameters and orientation parameters used in these examples are given as Ra=2, Rb=1.5,
β0=π/2, (φ, θ, ψ)=(π/3, π/6, 0).
Further research shows that for any given geometric parameters, Ra, Rb and β0, and
orientation parameters, (φ, θ, ψ), of the manipulator, D is a quartic expression while δ is a
quadratic expression with respect to the single variable XV. Generally, there are four real
roots when D=0 and δ≠0, and, in each of the four cases, Eq. (76) degenerates into two
intersecting lines. For the same reason, there is one real root of multiplicity two when δ=0
and D=0; in this case, Eq. (83) degenerates into a parabola. In order to demonstrate the
aforementioned theoretical results, a 6/6-GSP will be studied, whose geometric parameters
and the orientation parameters are given as follows: Ra=2, Rb=1.5, β0=π/2, (φ, θ, ψ)=(π/3,
π/6, 0). Please note that the following calculations are all based on these parameters.
(a) for orientation (60°, 30°, 0°), XV = 0 (b) for orientation (60°, 30°, 0°), XV = −1
Fig. 17. Singularity loci in parallel principal-planes for general values of XV
XV 2 = ( 6 − 2 ) / 2 (80)
8x − 3( 6 − 2 ) = 0 (82)
which is a line parallel to y-axis. Meanwhile, it can be proved that point B1 is located on the
base plane. Similarly, when B1 coincides with C4, the singularity of this point is of the first
special-linear-complex singularity. Similarly, when B1 does not coincide with C4,
singularities corresponding to points lying in line of Eq. (82) are of the general-linear-
complex singularity.
The second part of Eq. (81) denotes another line. Singularities corresponding to points lying
in this line are all of the general-linear-complex singularity.
5.3.2.3 Third Case of Two Intersecting Lines
When XV 3 = 3( 6 + 2 ) / 2 , D=0 and δ≠0, Eq. (76) degenerates into two intersecting lines
24 x − (15 2 + 5 6 ) = 0 (85)
which is a line parallel to y-axis of frame V-xy. In particular, there are three special points at
which all the segments associated with the six extensible links of the manipulator intersect
one common line, respectively
5.3.2.5 One Case of a Parabola
When XV 5 = (7 6 + 3 2 ) / 6 , δ=0and D≠0, Eq. (76) degenerates into a parabola, as shown in
Fig. 23
(528 6 − 912 2 )x 2 + (1423 3 − 2472)x + (504 − 288 3)y + 513 6 − 909 2 = 0 (86)
The manipulator is always singular corresponding to points lying in the parabola. Similarly,
there are three special points at which all the segments associated with the six extensible
links of the manipulator intersect one common line.
Based on the analyses described above, it can be concluded that the singularity loci of this
class of the 6/6-Gough-Stewart manipulators in parallel principal-sections are always
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 425
quadratic expressions that generally include infinite hyperbolas. However, for four parallel
locations of the principal-section, the quadratic expression degenerates into two intersecting
lines respectively, and in one location the quadratic expression is a parabola when θ≠0.
Z 3 cos(φ + ψ ) = 0 (87)
1. When Z=0, the moving and the base platforms are coincident. In this special
configuration, the manipulator has three DOFs: two rotational freedoms and one
translational freedom.
2. When (φ+ψ)=±90°, for the 6/6-Gough-Stewart manipulator, it is the singularity
proposed by Fichter (1986).
As we have discussed, the singularity loci of this class of the 6/6-Gough-Stewart
manipulators in parallel principal-sections include infinite hyperbolas, four cases of two
intersecting lines, and one case of a parabola when θ=0.
From analytic geometry, there are five different types of quadric surface with hyperbola
sections: hyperbolic cylinder, hyperbolic paraboloid, hyperboloid of one sheet, hyperboloid
of two sheets, and conic surface. However, none of these can contain infinite hyperbolas,
one case of a parabola, and four cases of two intersecting lines simultaneously. Therefore,
the singularity locus equation of this class of 6/6-Gough-Stewart manipulators considered
in three-dimensional space is a special irresolvable polynomial expression of degree three,
whose cross-sections in parallel principal-sections contain one case of a parabola, four cases
of two intersecting lines and infinite hyperbolas.
6. Conslusion
6.1 A necessary and sufficient condition that the three velocities of three non-collinear points
in a rigid body can reflect that the body has a possible twist motion is that the intersecting
point of three normal planes of three velocities lies in the plane determined by the three
points. This is also the necessary and sufficient condition of occuring singularity for a
parallel mechanism, when all actuators are locked.
6.2 Based on the singularity kinematics principle and Singularity-Equivalent-Mechanism
method, the structure and property of the singularity surface of 3/6-Gough-Stewart
platform for all different orientations (φ, θ, ψ)can be finally concluded as follows
426 Advanced Strategies for Robot Manipulators
6.2.1 When θ=0, the mobile platform and the base one are parallel with each other. The
special configuration of the mechanism is (90°, 0, 0)which is proposed by Fichter (1986). The
mechanism is singular whatever the position of the mobile platform is. It belongs to the
general-linear-complex singularity.
6.2.2 When θ=0, Z=0, the mobile platform and the base one are coincident. The mechanism is
singular whatever the other two Euler angles are. The mechanism has three DOF: two
rotational freedoms and one translational freedom. They belong to the first or the second
special-linear-complex singularity, respectively (Huang; Chen; Li 2003).
6.2.3 When θ≠0, φ≠±30°, ±90°, and ±150° and ψ≠±30°, ±90°, and ±150°, the singularity loci for
this orientation include a plane (Hunt plane) and a hyperbolic paraboloid (Huang; Chen; Li
2003). The intersecting line UV between the mobile platform and the base one is parallel to
some side of the basic triangle A1A3A5.
6.2.4 When θ≠0, φ≠±30°, ±90°, and ±150° and ψ=±30°, ±90°, or ±150°, the singularity loci are
three intersecting planes (Huang; Chen; Li 2003).
6.2.5 When θ≠0, φ≠±30°, ±90°, and ±150° and ψ=±30°, ±90°, or ±150°, the singularity loci for
this orientation also include a plane and a hyperbolic paraboloid (Huang; Chen; Li 2003).
6.2.6 When θ=0, neither φ nor ψ is equal to any one of the angles, ±30°, ±90°, ±150°. This is
the most general case for 3/6-GSP. The singularity equation for this orientation is a special
irresolvable polynomial expression of degree three, and the structure of the singularity loci
in infinite parallel principal sections includes a parabola, four pairs of intersecting straight
lines and infinity of hyperbolas (Huang and Cao 2006).
6.2.7 There is only one instantaneous freedom forming a twist with hm≠0, appearing when
3/6-GSP is singular for infinite general orientations. But there are seven special situations in
which the instantaneous motion is a pure rotation with hm=0. However, under the case θ=0,
Z=0 the instantaneous motion is a pure translation with hm=∞.
6.2.8 According to the singularity-equivalent-mechanism, the singularity can occur at any
point all over the θ-plane. That is to say, the singularity can occur everywhere in the
workspace of the mechanism. But the singularity orientation of the platform in different
points may be different.
6.2.9 The planar section parallel to the mobile platform is named principal section of the
singularity surface. The singularity expressions in infinite parallel principal sections are
always quadratic.
6.3 The structure and property of the singularity loci of this class of 6/6-Gough-Stewart
manipulators for all different orientations can be finally concluded as follows (Huang and
Cao 2005)
6.3.1 The singularity locus equation of degree three, is of a special irresolvable polynomial
expression whose cross-sections in parallel principal-sections contain one parabola, four
pairs of intersecting lines, and infinite hyperbolas.
6.3.2 The graphical representations of the singularity locus of this class of the 6/6-Gough-
Stewart manipulators are quite complex and various for different orientations. The most
complex graphic of the singularity loci looks like a trifoliate surface with two holes.
6.3.3 We find an incredible phenomenon, for this class of the 6/6-Gough-Stewart
manipulators, there are also some special singularity cases where six lines associated with
the six extensible links of the manipulator can intersect one common line and the remnant
motion of the manipulator is a pure rotational motion. Even for the same orientation of the
manipulator, there are two or more positions of the manipulator at which the six lines of the
manipulator all intersect one common line simultaneously.
Structure and Property of the Singularity Loci of Gough-Stewart Manipulator 427
6.3.4 When θ=0, Z=0, the manipulator also has three remnant DOFs: two rotational freedoms
and one translational freedom.
7. Acknowledgment
The research work reported here is supported by the NSFC under Grant No. 59885006 and
50275129, 50905075 and Supported by the Self-determined Research Program of Jiangnan
University under Grant No. JUSRP10908.
8. References
Ball, R. S. (1900). Theory of Screw. Cambridge:Cambridge University Press, 1900.
Cao, Y.; Huang, Z. & Ge, Q. J. (2005). Orientation-Singularity and Orientation Capability Analysis of The
Stewart-Gough Manipulator, ASME 2005 paper DETC2005-84556, 2005.
Chan, V. K. & Ebert-Uphoff, I. (2000). Investigation of the deficiencies of parallel manipulators in singular
configurations through the Jacobian nullspace. Proc. IEEE Int. Conf. on Robotics and Automation, Seoul,
Korea, 2000.
Collins, C. L. & Long, G. L. (1995). The Singularity Analysis of an In-Parallel Hand Controller for Force-
Reflected Teleoperation," IEEE Transactions on Robotics and Automation, Vol. 11, No. 5, October, 1995.
Collins, C. L. & McCarthy, J. M. (1997). The singularity loci of two triangular parallel manipulator. IEEE ICAR
97’, Monterey, CA, July 7:9, pp. 473-478, 1997.
Collins, C. L. & McCarthy, J. M. (1998). The quaric singularity surfaces of planar platforms in the Clifford
algebra of the projective plane, Mechanism and Machine Theory, 33:7, pp.931-944, 1998.
Di Gregorio, R.(2001). Analytic Formulation of the 6-3 Fully-Parallel Manipulator’s Singularity Determination,”
Robotica, 19, pp.663-667, 2001.
Di Gregorio, R. (2002). Singularity-Locus Expression of a Class of Parallel Mechanisms, Robotica, 20, pp. 323-
328, 2002.
Di Gregorio, R. (2004). Properties of the SX-YS-ZS Structures and Singularity Determination in Parallel
Manipulators which Generate Those Structures. Proceedings of DETC'04: ASME 2004 Design Engineering
Technical Conferences and Computers and Information in Engineering Conference September 28 – October
2, 2004, Salt Lake City, Utah USA, 2004.
Ebert-Uphoff, I.; Lee, J.-K. & Lipkin, H. (2000). Characteristic Tetrahedron of Wrench Singularities for Parallel
Manipulators with Three Legs. Proc. of A Symposium Commemorating the Legacy, Works, and Life of Sir
Robert Stawell Ball Upon the 100th Anniversary of “A Treatise on the Theory of Screws” University of
Cambridge, Trinity College, 2000.
Fichter, E. F. (1986). A stewart platform-based manipulator: General theory and practical construction, Int. J. Rob.
Res. 5, pp.157-182, 1986.
Gough, V. E. (1956-57). Contribution to discussion to papers on research in automobile stability and control and
in tyre performance. Proc. Auto. Div. Instn mech. Engrs, p.392. by Cornell staff, pp. 392-397, 1956-57.
Gosselin, C. & Angeles, J. (1990). Singularity analysis of closed-loop kinematic chains, IEEE Trans. Rob. Autom.
6, pp. 281-190, 1990
Hao, F. & McCarthy, J. M. (1998). Conditions for Line-Based Singularities in Spatial Platform Manipulator,
Journal of Robotic Systems, 15(1), pp. 43-55, 1998.
Huang Z. and Qu Y. Y. (1987). The analysis of the special configuration of the spatial parallel manipulators, The
5th National Mechanism Conference, Lu Shan, China, pp. 1-7, 1987.
Huang, Z.; Kong, L.F. & Fang, Y.F. (1997). Theory and Control of Parallel Robotic Mechanisms Manipulator.
Beijing, China. Publisher of Mechanical Industry, 1997.
Huang, Z.; Zhao, Y.; Wang, J. & Yu, J. J. (1999). Kinematic principle and geometrical Condition of general-
linear-complex special configuration of parallel manipulators, Mechanism Machine Theory, 34, pp. 1171-
1186, 1999.
Huang, Z.; Chen, L.H. & Li, Y. W. (2003). The Singularity Principle and Property of Stewart manipulator,
Journal of Robotic Systems, 20:4, pp.163-176, 2003.
Huang, Z. & Cao, Y. (2005). Property Identification of the Singularity Loci of a Class of Gough- Stewart
Manipulators, The International Journal of Robotics Research, 24:8, pp. 675-685, 2005
428 Advanced Strategies for Robot Manipulators
Huang, Z.; Cao, Y.; Li Y. W.,& Chen L.H. (2006). Structure and Property of the Singularity Loci of the 3/6-
Stewart-Gough Platform for General Orientations, Robotica, 2006, 24, pp. 75-84, 2006.
Hunt, K.H.,(1978). Kinematic Geometry of Mechanisms. Oxford, UK: Oxford University Press, 1978.
Hunt, K.H. (1983). Structural kinematics of in-parallel-actuated robot-arms, ASME J. Mech. Transmissions Autom.
Design, 105, pp. 705-712, 1983.
Karger, A. & Husty, M., (1998). Architecture singular parallel manipulators, Proceedings of ARK’98 (J. Lenarcic
and M. Husty eds), pp. 445-454, 1998.
Karger, A. (2001). Singularities and self-motions of equiform platforms. Mechanism and Machine Theory, 36:7,
pp. 801-805, 2001.
Kong, X W. (1998). Generation of 6-SPS Parallel Manipulators,” Proc.of the 1998 ASME Design Engineering
Technical Conference,Atlanta, USA, 98DETC/MECH-5952, 1998.
Kong, X. & Gosselin, C. M., (2001). Uncertainty Singularity Analysis of Parallel Manipulators Based on the
Instability Analysis of Structures, The International Journal of Robotics Research, Vol 20 :11, pp. 847-856,
2001.
Kong, X. & Gosselin, C.M. (2002). Generation of Architecturally Singular 6-SPS Parallel Manipulators with
Linearly Related Planar Platforms. Electronic Journal of Computational Kinematics (EJCK), May 2002.
1(1), Paper No. 7, 2002
Ma, O. & Angeles, J.,(1991). Architecture Singularity of Platform Manipulators, IEEE Int.Conf.on Robotics and
Automation. Sacramento, USA , pp. 1542-1547, 1991.
Mayer St-Onge, B. & Gosselin, C. (2000). Singularity analysis and representation of the general Gough-Stewart
platform, Int. J. Rob. Res. Vol.19, No. 3, pp, 271-288, 2000.
McAree, P.R. & Daniel, R. W.(1999). An Explanation of never-special Assembly Changing Motions for 3–3
Parallel Manipulators, Int. J. of. Robotics Research, 18:6, pp. 556–574, 1999.
Merlet, J. P.( 1988). Parallel manipulator part 2: Singular configurations and grassmann geometry. Technical
report, INRIA, Sophia Antipolis, France, 1988.
Merlet J.P. (1989). Singular configurations of parallel manipulators and Grassmann geometry, Int. J. Rob. Res. 8,
pp. 45-56, 1989.
Parenti-Castelli, V. & Innocenti, C., (1990). Direct displacement analysis for some classes of spatial parallel
mechanisms, Proc. of the 8th CISM-IFToMM Symp. on Theory and Practice of Robots and Manipulators,
Cracow, Poland, pp. 126-133, 1990.
Pernkopf, F. & Husty, M. L. (2002). Sigularity Analysis of Spatial Stewart-Gough Platforms with Planar Base and
Platform. Proceedings of DETC.02 ASME 2002 Design Engineering Technical Conferences and Computer
and Information in Engineering Conference, Montreal, Canada, September 29 - October 2, 2002,
DETC2002/MECH-34267.
Sefrioui, J. & Gosselin, C. (1994). Étude et représentation des lieux de singularité des manipulateurs parallèles
sphériques à trois degrés de liberté avec actionneurs prismatiques, Mechanism Machine Theory, 29, pp. 559-
579, 1994.
Sefrioui, J. & Gosselin, C. (1995). On the quadratic nature of the singularity curves of planar three-degree-of-
freedom parallel manipulators, Mechanism Machine Theory, 30, pp. 533-551, 1995.
Sugimoto, K. & Duffy, J.(1981). Special Configuration of Industrial Robots, Proc. of the 11th ISIR, pp: 309-316,
1981.
Stewart, D. (1965). A platform with six degrees of freedom, Proc. Inst. Mech. Eng, 180, pp. 371-378, 1965.
Wang, G. Z. (1998). Singulrity analysis of a class of the 6-6 Stewart platforms. Proc. of DETC’98 1998 ASME
Design Engineering Technical Conference, Atlanta, Georgia, USA, 1998.
Wang, J. & Gosselin, C.M. (1996). Kinematic analysis and singularity loci of spatial four-degree-of-freedom
parallel manipulators. Proc. of the 1996 ASME Design Engineering Technical Conference and Computers in
Engineering Conference, California, USA, 1996.
Wang, J. & Gosselin, C. (1997). Kinematic analysis and singularity representation of spatial five-degree-of-
freedom parallel mechanisms, J Robotic Syst, 14, pp. 851-869, 1997.
Zlatanov, D., Bonev, I.A. & Gosselin, C.M., (2002), Constraint singularities of parallel mechanisms, Proceedings
of the 2002 IEEE International conference on Robotics and Automation, Washington, DC, USA, pp. 496–
502, 2002.
Zlatanov, D.; Fenton, R.G. & Benhabib, B.(1994). Singularity analysis of mechanisms and robots via a motion-
space model of the instantaneous kinematics, Proc. of 1994 IEEE Int. Conf. On Rob. And Auto 2 pp. 980-
991, 1994.
Zlatanov, D, Fenton, RG & Benhabib, B. (1995). A Unifying Framework for Classification and Interpretation of
Mechanism Singularities. ASME J Mechanical Design, 117, pp. 566-572, 1995.