Preprints of the 18th IFAC World Congress
Milano (Italy) August 28 - September 2, 2011
Hybrid Functional Machine Strategy to
Avoid Singularities in Underwater
Vehicle-Manipulator Systems ⋆
Carlos Henrique Farias dos Santos ∗ Edson De Pieri ∗∗
Robotics Research Group, Western Paraná State University, Foz do
Iguaçu, Paraná, Brazil (e-mail: chf.santos@uol.com.br).
∗∗
Automatic Systems Department, Santa Catarina Federal University,
Florianópolis, Santa Catarina, Brazil (e-mail: edson@das.ufsc.br)
∗
Abstract: In this paper, the problem of singularity avoidance to autonomous underwater
vehicle-manipulator system (UVMS) is addressed. For this purpose, an algorithm which explores
a hybrid approach with a functional machine as one of its states is adopted. This approach
imposes desired motions to the UVMS kinematic chain. The computer simulation results
demonstrate the beneficial characteristics of the proposed method.
Keywords: Functional Machine, Hybrid Systems, Singularity, Underwater Vehicle-Manipulator
Systems.
1. INTRODUCTION
The demand for advanced underwater robot technologies
is growing and will eventually lead to fully autonomous
underwater vehicle-manipulator systems (UVMS’s). This
autonomy is hampered by some difficulties that include
the system’s highly nonlinear dynamics, the poor knowledge of the hydrodynamic coefficients, the occean currents
disturbances and the subject of this work, the inherent
kinematicaly redundancy of such systems. This redundancy is caused by the additional degrees of freedom (dofs)
provided by the vehicle. It should be remarked that in
UVMS it is not always efficient to use vehicle thrusters to
move the system end-effector because of the difficulty of
controlling the vehicle in hovering Yoerger (1990).
In this paper the kinematic control of the UVMS is researched, in which the goal is to find a suitable vehicle/manipulator joints trajectories that correspond to a
desired end-effector trajectory. The output of this kinematic control provides the reference values to the dynamic
control law of the UVMS. The kinematic control independent of the dynamic control is considered as in Antonelli
(2003).
The most common ways to obtain the redundancy resolution are based on pseudoinverse approaches Antonelli
(2003). However, the use of the pseudoinverses introduce
dimensional inconsistencies Davidson (2004). Another approach is the augmented Jacobian, which could introduce
algorithmic singularities Antonelli (2003).
In order to address these issues, an algorithm that imposes
the vehicle velocities is proposed, which are chosen such
that the vehicle is kept at rest until a manipulator singularity vicinity is reached and is moved such that this
⋆ Thanks for financial support from Western Paraná State University, Itaipu Technological Park Cience and Technology and Santa
Catarina Federal University
Copyright by the
International Federation of Automatic Control (IFAC)
singularity is avoided. To this end, we introduce some
qualitative kinematic concepts as fundaments to our algorithm. This algorithm incorporates artificial intelligence
techniques combined with hybrid systems theory to avoid
singularity postures on the UVMS. These postures are
identified by a kinematic performance index which is supervised by an automaton. The automaton structure has
one of the states driven by a functional machine and the
other kinematic states are driven by a reflexive agent. The
switching between these states provides a kinematic behaviour to the UVMS such that its potential singularities
should be avoided.
The remainder of this paper is organized as follows. In
section 2 the kinematics of the UVMS using the concept
of virtual chain employed by the method of Davies is
described, which is used to obtain the inverse kinematics
of the UVMS and we define our problem. Next, section
3 provides preliminary concepts which are necessary to
define the functional machine structure. In section 4, a
hybrid automaton to complement the functional machine
effect is shown. Two simulation results are analysed in
section 5. The paper conclusions are presented in section
6.
2. UVMS KINEMATIC MODEL
In this section, we describe the UVMS kinematic model
using screws Davidson (2004) combined with the Davies’
method to represent its differential motion.
According to Davies (1981), the constraint equation of a
mechanism with movements in a d order screw system is
given by
N(d×Fb ) Ψ(d×1) = 0(d×1)
(1)
where N is the network matrix containing the normalized
screws with signs that depend on the circuit orientation,
5920
Preprints of the 18th IFAC World Congress
Milano (Italy) August 28 - September 2, 2011
Ψ is the magnitude vector and Fb is the gross degree
of freedom, i.e. the sumPof the degrees of freedom of all
mechanism joints (Fb =
fi ), with fi being the degree of
freedom of the ith joint.
The use of the constraint equation, Eq. 1, allows to calculate the passive joint velocities as functions of the active
joint velocities. The magnitude vector Ψ is rearranged in
d secondary or unknown magnitudes and FN primary or
.
known magnitudes Ψ , i.e. Ψ = [Ψ .. Ψ ]. Rearranging
s
p
the network matrix N(d×Fb ) coherently with the magnitude
division, where the secondary network sub-matrix Ns corresponds to the secondary joints and the primary network
sub-matrix Np corresponds to the primary joints.
This equation may be rewritten as Ns Ψs = −Np Ψp and
the joint space kinematics solution is given by
Ψs = −Ns−1 Np Ψp
(2)
In the case of an UVMS, the end effector motion description in a Cartesian frame may be obtained closing
the chain by introducing a virtual chain composed of
three prismatic joints ($e1 , $e2 , $e3 ) and a spherical joint
S($e4 , $e5 , $e6 ) (PPPS) between the base and the end
effector as in Fig.1. To obtain the inverse kinematics, we
specify the vehicle velocity magnitudes. This should be
done considering energy savings that result in keeping the
vehicle at rest. In this way, the primary matrix results
Np = [$̂v1 · · · $̂v6 − $̂rz · · · − $̂px ], where $̂i represents the
normalized screw of twist $. Consequently, the secondary
matrix is Ns = [$̂m1 · · · $̂m6 ]. This secondary matrix could
be inverted if it is full rank, i.e. if the kinematic chain is
not at a singularity.
$ m2
$
$ m3 $m4 m5
$m1
S ($ $ v5 $ v6 )
v4
$
e3
$ v2
$ v1
Y
Z
where J(q) is the Jacobian matrix of the manipulator. This
manipulability measure is easy to compute. However, its
numerical value does not constitute an absolute measure
to the actual closeness of the manipulator to a singularity.
Because of this, in this work we use a normalized index.
We define this index (named γ) as the absolute rate
between the determinant of the secondary matrix Ns ,
described in the Eq. 2 and the determinant value at some
singular vicinity given by qsv . The index results,
det(Ns ) γ=
(4)
det[Ns (qsv )] Equation 4 uses the per unit notation. This notation has
a vast application in electric power systems, where the
transformation constants are located at a reasonable numerical band. Moreover, at this band, the values correction
is quickly approached Fitzgerald (1971).
In terms of the vehicle motion necessity, if the index γ
is higher than unit value it means that the vehicle could
be kept at rest and, when γ is smaller than the unit, it
means that the vehicle needs to be moved. This leads
to a vehicle movement concept which must achieve the γ
recovery commitment when necessary and it is presented
in the next subsection.
According to Santos (2006) the end-effector velocity $E
can be expressed in a suitable coordinate screw system as
$E = $V + $M
(5)
S ($ e4 $ e5 $ e6)
$ e2
X
The closeness of a manipulator singular configuration
could be measured by a performance index as the manipulability measure Yoshikawa (1985) or other indices Park
(2002). The manipulability measure is usally defined as,
ω = |det(J(q)|
(3)
3.2 Kinematic reactivation
$ m6
$ v3
3.1 A normalised kinematical performance index
$ e1
Fig. 1. UVMS’s closed kinematic chain
As pointed out before, keeping the vehicle at rest should
lead the manipulator kinematic chain to reach a singular posture during the task performance. This could
be avoided by moving the vehicle conveniently. However,
given some particular initial conditions, the vehicle movement is not enough to avoid singularity postures. In this
sense, it is necessary to modify the current primary and
secundary UVMS joint arrangement, i.e. to change the
current kinematic state.
3. QUANTITATIVE KINEMATIC CONCEPTS
To obtain a vehicle motion specification in order to avoid
vicinity of singularities and avoid discontinuities, we introduce the kinematically quantitative concepts described in
this section.
where, $V and $M are the vehicle and manipulator velocity
respectively. In order to overcome potential singular configurations we propose that the specified vehicle velocity
is taken proportional to the end effector desired velocity,
that is $V = α$E , where α is a reflex agent that makes
the system react even if the performance index detects a
vicinity of a singularity. This reflex agent is defined as the
inverse of the system performance and is given by α = γ1 .
To reduce the energy consumption, we intend to move the
vehicle only when the manipulator reaches a vicinity of
singularities and keep it at rest in any other situation. This
may introduce a discontinuous behaviour that should be
avoided.
An avoidance of this discontinuity using a functional
machine which includes some system kinematic knowledge
is proposed, more specifically, the kinematic regulation
index (ω̂) described in the next subsection.
3.3 Kinematical regulation
The regulation concept is a usual subject in power electric
systems theory and our goal is to adjust it to kinematic
5921
Preprints of the 18th IFAC World Congress
Milano (Italy) August 28 - September 2, 2011
control applications. For the sake of clarity, first is shown
how the regulation concept is used in electric problems.
For electric generators the regulation is defined as
VN L − VF L
,
VR=
VF L
(6)
where V R, VF L and VN L are the percentage voltage
regulation and the terminal voltage with full load and no
load, respectively. Eq. 6 shows, by a relative way, how
much the voltage of the system changes when a load
is employed, i.e. it informs the percentual voltage that
must be developed by the generator to restore the original
voltage Kosow (1972).
Likewise in the case of electric power systems, we adopt
here analogous concepts that are applied in kinematical systems Santos (2006). We first establish a physical
meaning of the kinematical performance by a regulation
to measure the singularity approach. This measurement
to consider the vicinity of singularities, detected by the
index of kinematical performance γ, as a load applied to
the kinematical system. In this case, γ is reciprocal to
the situation where the kinematical system supports a
singularity load (γ ≡ VF L ). In the other side, the reflexive
agent α is reciprocal to the situation where the kinematical
system its free of singularity loads (α ≡ VN L ). Thus, an
analogy between the electric system regulation and the
kinematical system regulation is give by,
α − γ ,
(7)
ω̂ = γ where ω̂ is the kinematic regulation index. Intuitively, Eq.
7 measures the size of the action to avoid the singular
configuration and it will be used as the weight of the
functional machine designed in the next section.
4. FUNCTIONAL MACHINE
4.1 The functional construction
weight (vector W). The output weights are added to
determine the level of function activation, this added to
a threshold, θp , known as polarization or bias. The output of the
Pn computer unit can be expressed as follows:
s = f ( i=1 wi xi + θp ), where f should be any activation functions, including the hyperbolic tangent and the
sigmoid functions. The polarization θp works as a reactive
agent, which is presented with more details in the next
subsection.
θp
y1
x1
y2
x2
ym
xn
input layer
output layer
hidden layer
Fig. 2. Functional Machine topology
Figure 2 shows a topology of a Functional Machine,
where the linear combination of functions is organized into
layers. The functions that receive inputs from the machine
(x1 . . . xn ) form the input layer, the last function is the
output. The functions between the input layer and the
output form the middle layer.
4.2 Polarization surface
The polarization agent θp governs the vehicle movement
in order to avoid unnecessary motions. The polarization
proposed is given by a finite state machine based on the
polarization surface showed in Fig. 3 Santos (2006).
The functions can be manipulated in a variety of ways
in functional programming. Functions are first-class variables, which means that functions can be parameters or
input values to other functions and can return values or
output of a function Cousineau (1998). Thus, a combination of functions can be done in order to approach a new
role materializing the solution of the problem.
γ
γ
max
1
γ
tol
Therefore, a program can be seen as an implementation
of the function f , which is a mathematical entity that
works like a machine, receiving raw inputs and returning
the final outputs. In this sense, the purpose of this work
is to solve the problem of driving the vehicle through
a functional machine. This machine consists of several
functions, linearly combined, describing the UVMS movement. The monitoring of the rest and motion conditions
is developed by a reactive agent, incorporated into the
functional machine structure.
The basic computational unit of functional machine are
interconnected by connections with variable values called
weights. Essentially, a set of inputs (vector X) is applied
to a function, each representing an output of another
function. Each input is multiplied by a corresponding
Fig. 3. Polarization surface
This figure presents the performance index γ and the
corresponding reflexive agent α time histories, the vicinity
of singularities (γ = 1) and the limits γmax and γtol .
The corresponding finite state machine has three states,
namely: S1 is the rest state, in which the vehicle is kept at
rest while (γ ≥ 1); S2 is the acceleration state, in which
the vehicle is moved while (γ < 1) until the performance
reaches some value higher or equal the unit named γaccel ,
i.e. while (γ < γaccel ); S3 is the deceleration state, in
which the vehicle is decelerated while γ < γmax since
sgn(1 − α) > 0. This last one is a safe condition in which
the signum of (1 − α) is measured to foresee if the vehicle
has to mantain the deceleration or it has to come back
5922
Preprints of the 18th IFAC World Congress
Milano (Italy) August 28 - September 2, 2011
to the acceleration state S2. The finite state machine is
showed in Fig. 4.
A hybrid automaton, is a finite state machine with discrete
states {q1 , q2 , . . . , qn }, in which each discrete state has
associated continous dynamics ẋ = f (qi , x, u), with x ∈
Rn , and external inputs u ∈ U .
YES
S1
γ> 1
NO
S2
NO
γ < γaccel
YES
γ < γmax
5.2 Hybrid Automaton
Definition: A hybrid automaton is a collection M =
(L, X, A, W, E, Inv, Act), where the symbols have the following meanings van der Shaft (2000):
YES
• L : is a finite set, called the set discrete states. They
are the vertices of a graph.
• X : is the continuous state space of the hybrid
automaton in which the continuous state variables x
take their values.
• A : is a finite set of the symbols which serve to label
the edges.
• W : is the continuous communication space in which
the continuous external variables w take their values.
• E : is a finite set of edges called transitions (or
events).
• Inv : is a mapping from the locations L to the set of
subsets of X.
• Act : is a mapping that assigns to each location
l ∈ L a set of differential-algebriaic equations Ft . The
solutions of these differential-algebriaic equations are
called the activities of the location.
NO
YES
sgn( 1 − α) > 0
S3
NO
Fig. 4. Finite state machine
4.3 Functional machine control formulation
Finally, using our proposed mathematical model of the
vehicle activation, equation $V = α$E is replaced by
$V = sα$E ,
(8)
wherePs is the sum of functional machine (FM) outputs
(s =
yi ), α is the reflexive agent and $E is the column
vector of the end-effector velocities. However, over some
bad initial conditions, the vehicle movement is not enough
to avoid singularity postures and to overcome this drawback, a hybrid automaton approach as complementary
strategy is adopted.
−1
ϕ = −[Ns ] [Np ] ϕ
p
s
q
ε
0
5. MODELING SINGULARITY AVOIDANCE AS
HYBRID AUTOMATON
−1
ϕ = −[Ns ] [Np ] ϕ
p
s
ε
q
We present in this section a hybrid automaton model of
singularity avoidance.
−1
ϕ = −[Ns ] [Np ] ϕ
p
s
q
5.1 Singularity Avoidance Models
We assume that each UVMS task is a sequence of movements from the initial kinematic state to one or until three
of the following states:
• Kinematic state 1 : In which the singularity of the
original kinematic state is avoided by other state with
det(Ns ) as a combination of joints that include the
joints of the previous state.
• Kinematic state 2 : In which the singularity of the
original kinematic state is avoided by other state
with det(Ns ) independent of the joints of the previous
state.
• Kinematic state 3 : In which the singularity of the
original kinematic state is avoided by other state
independent of joints, det(Ns ) = 1 for example.
In this paper we consider the singularity avoidance as the
choice of the kinematic state of maximum performance
index, i.e. (γmax ), when the current performance is below
of a given tolerance (γtol ).
1
β
2
Fig. 5. Singularity avoidance system as a hybrid automaton
One possible UVMS singularity avoidance, which will be
simulated in the next section, can be modeled by the
automaton of Fig. 5. The finite set is L = {q0 , q1 , q2 },
which states are defined as follows: q0 is the state in
which the vehicle movement is imposed by the FM, q1
works as a kinematic state 3, in which the vehicle moves
as a secondary joint set and in the state q2 the vehicle
movement is imposed by a polynomial interpolating law
to keep the current position, for the energy savings. The
continuous state A is (Ψs ) ∈ R2 and the continuous
communication space W is γ. The finite set of edges E
is composed of ǫ, β wich defined (γ < γtol ) and (γ > 1),
respectively.
The idea of finding the next kinematic state is to simulate
the motion until a violation of the constraint γtol . If
5923
Preprints of the 18th IFAC World Congress
Milano (Italy) August 28 - September 2, 2011
such an event occurs, a search is made to find the jth
kinematic state which has γmax or an unit performance
(γ = 1) as in kinematic state 3. Once it has been
established, the transition between the kinematic states
is developed from the initial conditions of time, position,
velocity and aceleration, respectively: ξi = [ ti pi vi ai ] in
the kinematic state of maximum performance. However,
it is necessary to define a rule to conduct the current
primary joint. In this work, this strategy applies the
reflexive agent α to the initial position of the ξi . This
condition develops a polynomial interpolating law position
of ξf = [ tf pf vf af ], and since that (vf = af = 0), the
final position is
pf = α j p i ,
(9)
where αj is the reflexive agent of j state αj = 1/γj .
Intuitively, αj works as a maneuver factor of the joint.
The next section presents simulation results of this hybrid
automaton applied to an UVMS.
6. CASE STUDY
We consider an UVMS with six degrees of freedom, i.e. a
manipulator with three dofs mounted on the vehicle.
φ
q
2
φ
In the following, two groups of simulations related with
a UVMS kinematic model will be presented, i.e. only the
kinematic model will be considered and dynamics effects
are not considered. The first mission consists a path of 2πr
in a deformed circle, where its center is located 1.25 meters
to right and 0.5 meters above from end-effector. In the
second mission, a path of 3πr in a deformed circle, where
its center is located 1.25 meters to the right from endeffector is considered. In both cases, simultaneously, the
vehicle aligment fore aft direction with the ocean current
of 0.78 rads is pursued. The duration of the simulations is
60 seconds.
Figure 7 shows that the index γ reaches values above
of γtol , hence the functional machine is enough to avoid
singularities. When this last happens, the vehicle returns
to a primary state which drives the vehicle to keep its
current position. Figure 8 exposes the smooth actuation
of the vehicle, while in Fig. 9 the joints positions and
velocities are smooth and safe of discontinous behaviour.
e
q
3
q
1
ye
y
this case, we admit the boundaries of the polarization
surface as γmax = 1.42 and γtol = 0.65. The vehicle
deceleration begins when the performance is higher than
γaccel = 1.05. In the kinematic
state (q1 ), the primary
vector is composed by, Ψp = ẋe ẏe φ̇e q̇1 q̇3 φ̇ , while
the secondary vector is, Ψs = [ ẋ ẏ q̇2 ] . As stated before,
this choice results in the unitary determinant of the
secundary matrix det[Ns3 ] = 1. The kinematic state (q2 )
has a primary vector composed by the same way of state
(q0 ), but the vehicle movement is imposed by a polynomial
interpolating law to keep the current position, for the
energy savings.
10
x
8
1.5
xe
4
[u]
[m]
6
1
2
0
0.5
γ
α
−2
−4
Fig. 6. Kinematic chain of the UVMS in the XY plane
5
10
20
t(s)
40
60
Fig. 7. UVMS and performance for first mission
2
0.3
1.5
[m],[rad]
The implemented functional machine is constructed with
one input layer with one computational unit, a hidden
layer with two computational units and an output layer
with only one computational unit. FM inputs are: α and
kα − γmin k.
In the original kinematic state (q0 ), the primary vector
is composed
by the end-effector
and vehicles magnitudes,
Ψp = ẋe ẏe φ̇e ẋ ẏ φ̇ while the secondary vector is the
manipulator dofs, Ψs = [ q̇1 q̇2 q̇3 ]. This choice results
in the following determinant of the secondary matrix
det[Ns ] = a1 a2 sin(θ2 ), where a1 and a2 are the first and
the second links lenghts of the manipulator and θ2 is the
angle of the second joint. It can be easily seen that the
maximum value of det[Ns ] occurs when sin(θ2 ) = 1. The
singularity vicinity is established as θsv = 0.78 rad. In
[m]
1
x
y
φ
0.5
0
0
xp
yp
φp
0.2
[m/s],[rad/s]
This vehicle movement is equivalent to the virtual kinematic chain xyφ as the vehicle dofs in the horizontal XY
plane. To close this open chain a virtual kinematic chain
xe ye φe is employed to represent the end-effector movement
as shown in Fig. 6.
0
0
0
20
t(s)
40
0.1
0
−0.1
60
−0.2
0
20
t(s)
40
60
Fig. 8. Vehicle: position and velocity for first mission
Figure 10 shows that γ reaches values below of γtol , hence
the switch to other states is necessary. When this last
happens, the switching to the kinematic state 3, where
the vehicle is secondary until γ takes the unit value again.
Figure 11 exposes the smooth actuation of the vehicle,
5924
Preprints of the 18th IFAC World Congress
Milano (Italy) August 28 - September 2, 2011
ACKNOWLEDGEMENTS
0.3
2
0.2
1
0.1
rad/s
rad
3
0
q1
q2
q3
−1
This work has been supported by ”Western Paraná State
University” (UNIOESTE), ”Itaipu Technological Park
Cience and Technology” and ”Santa Catarina Federal University” (UFSC), Brazil.
0
−0.1
−0.2
−2
q1p
q2p
q3p
−0.3
−3
0
20
t(s)
40
60
−0.4
0
20
40
t(s)
REFERENCES
60
Fig. 9. Joint: position and velocity for first mission
while in Fig. 12 joints positions and velocities are smooth
and safe of discontinuous behaviour.
10
8
1.5
4
[u]
[m]
6
1
2
0
0.5
γ
α
−2
−4
0
[m]
5
0
0
10
20
t(s)
40
60
Fig. 10. UVMS and performance for second mission
5
0.5
x
y
φ
0.3
3
2
1
0
0
xp
yp
φp
0.4
[m/s],[rad/s]
[m],[rad]
4
0.2
0.1
0
−0.1
20
t(s)
40
60
−0.2
0
20
t(s)
40
60
Fig. 11. Vehicle: position and velocity for second mission
3
0.4
Antonelli, G. (2003). Underwater Robots, Motion and
Force Control of Vehicle-Manipulator Systems. Springer
Tracts in Advanced Robotics, Germany.
Cousineau, G.; Mauny, M. (1998). The Functional Approach to Progamming. Cambridge University Press,
U.K.
Davidson, J.K.; Hunt, K. (2004). Robots and screw theory.
Oxford University Press, U.K.
Davies, T. (1981). Kirchhoff’s circulation law applied to
multi-loop kinematic chains. Mechanism and Machine
Theory, 16, 171–183.
Fitzgerald, A.E.; Kingsley Jr., C.K.A. (1971). Electric
Machinery. McGraw-Hill, USA.
Kosow, I. (1972). Electric machinery and transformers.
Prentice-Hall, USA.
Park, T-W, Y.H.S. (2002). A study on singularity avoidance and robust control of redundant robot. Proceedings
of 4 World Congress on Intelligent Control and Automation.
Santos, C.H. dos, G.R.D.P.E. (2006). A reactive neural network to redundancy resolution for underwater
vehicle-manipulator systems. Proceedings of IEEE International Conference on Robotics and Automation.
van der Shaft, A. J.; Schumacher, J.M. (2000). An
Introduction to Hybrid Dynamical Systems. Spriger,
USA.
Yoerger, D.R., C.J..S.J.J. (1990). The influence of thruster
dynamics on underwater vehicle behavior and their
incorporation into control system design. IEEE Journal
of Oceanic Engineering, 15, 167–178.
Yoshikawa, T. (1985). Manipulability of robotic mechanisms. Int. J. Robotics Research, 3–9.
2
0.2
rad/s
rad
1
0
−0.2
−1
q1
q2
q3
−2
−3
0
0
20
t(s)
40
q1p
q2p
q3p
−0.4
60
−0.6
0
20
t(s)
40
60
Fig. 12. Joint: position and velocity for second mission
7. CONCLUSIONS
A singularity avoidance resolution for UVMS’s based in
specifying the velocities joints of its kinematic chain according to a kinematical performance index is presented.
The method is based on a hybrid system model as complemental strategy to an initial functional machine. Simulation results shows that the proposed resolution scheme is
efficient, since the commitments to avoid singular postures
and keep the continuity of the all velocity joints of the
UVMS are achieved.
5925