Hyper Redundant Manipulators

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 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 twodegree-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” hyperredundant 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, 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.

Fig. 1. A tentacle arm prototype
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
limt→tf 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).

Fig. 2. The description of the desired shape
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.

Fig. 3. Curvature and relation between θ and β
The generic matrix in 2D that expresses the coordinate of the next segment related to the previous reference system can be written as follow:
⎡ cos(2⋅β) sin(2⋅β) L⋅sin(β)⎤
⎢⎢−sin(2⋅β) cos(2⋅β) L⋅cos(β)⎥⎥ (2)
⎢⎣ 0 0 1 ⎥⎦
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:
Rigeneric := Rzi (θi )⋅Tr Vy( i )⋅Ryi (α θi )⋅Rzi ( i ) (3)
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:

L
R (4) 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:

Fig. 4. Different types of wires.

For the soft wires, we can write: ⎧
⎪Lw1 ⎪
⎪⎨Lw2
⎪
⎪
⎪⎩Lw3
⎧Lw1 = R1 ⋅θ
⎪ w2 = R2 ⋅θ (5)
⎨L
⎪⎩Lw3 = R3 ⋅θ
=
= (6)
= [R3 ⋅θ]⋅ sinθ(θ/ i/ 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:
⎧α1
⎪⎨α2
⎪
⎩α3 =
=
= −α
120°−α
240°−α (8)
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 ΔR Ri = Δ ⋅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 = Lwi(t f ) − LCB (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:

Lwn = LCB −θ⋅ΔR⋅cos(αn) (10)
Considering the equation (8) and (10), evaluating these for all the wires we can obtain:
⎧ 3
⎪ ∑ cos(αi ) = 0
⎪ i=1
⎪⎪⎨ 1 ∑3 R = R (11)
i
⎪ 3 i=1
⎪⎪⎪⎩ 13 ∑i3=1Lwi = L
Considering again the equation (10) for the first and second wires, we can write:
Lw1 + ΔR⋅θ⋅cos(α1) = Lw2 + ΔR⋅θ⋅cos(α2) (12)
Replacing the (8) we obtain θ in function of α:
(13)
ΔR 3cos α − 3sin α
And considering the eq. (10) for the third wire:
Lw Lw (14)
Finally the α angle can be obtained using the function atan2.
α= atan2( 3(Lw2 − Lw3),2Lw1 − Lw2 − Lw3) (15)
where atan2 is an extension of arctan(y/x) on more quadrant having the following form:
⎧atan(y / x)+π
⎪ −π
⎪atan(y / x)
⎪⎪ atan(y / x)
⎨ π
⎪
⎪ −2π
⎪
⎪ if if if if if x < 0, y ≥ 0 x < 0, y < 0 x > 0

x = 0, y > 0 x = 0, y < 0 (16)
⎩ 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.

Fig. 5. Projection of the wire to get the α direction

Fig. 6. The geometry of one segment.
The radius R of the segment curvature is obtained using equation (17):
R = H (17) θ
where H is the height of the segment. The following lengths are obtained from Fig. 5, based on the segment curvature:
L11 = CP4 = R + D12 L12 = CP1 = R + D22
(18)
L21 = CP3 = R − D12 L22 = CP2 = R − D22
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:
(19)
A
The control wires curvature radius R and R are given by the relations (20):
R1 = A1 (20)
Finally, the lengths of the control wires are obtained as in (21):
Lw R A
(21)
Lw R A
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:
L11 = R + D12⋅cos(α1) L12 = R + D22⋅cos(α1)
L21 = R + D12⋅cos(α2) L22 = R + D22⋅cos(α2) (22)
L31 = R + D12⋅cos(α3) L22 = R + D22⋅cos(α3)
where αn is according to Fig. 5:
⎧α1 ⎪
⎨α2
⎪α3 ⎩ =
=
= −α
120° −α
240° −α (23)
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):
Lw1 = R1 ⋅θ
Lw2 = R2 ⋅θ
Lw3 = R3 ⋅θ
Apart from the system presented we can obtain two useful relations: (24)
⎧ 3
⎪⎪∑i=1cos(αi ) = 0
⎨ (25)
⎪⎪⎩ 13 ∑i3=1Lwi = 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( )∈R3 and an associated frame ϕ(s)∈R3 3× whose columns create the frame bases (Fig. 7a) (Ivănescu et al., 2006).

Fig. 7. Tentacle system parameters.
The independent parameter s is related to the arc-length from the origin of the curve C, a variable parameter, where
l = ∑N (l0i + Δ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
u = ∑N Δli (28)
i=1

determines the control variable of the arm length.
The position of a point s on curve C is defined by the position vector,
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 ,e ,ex y 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

r s,t( )=[x s,t( ) y s,t( ) z s,t( )]T (30)
where
x s,t( )= ∫s sinθ(s ,t′ )cosq s ,t ds( ′ ) ′ (31)
0
y s,t( )= ∫s cosθ(s ,t′ )cosq s ,t ds( ′ ) ′ (32)
0
z s,t( )= ∫s sinq 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
vx q q′ ′ ′ q′ ′ ds′ (34)
vy ′ ds′ (35)
s
vz q ds′ ′ (36)
vu = u
For an element dm, kinetic and gravitational potential (Douskaia, 1998) energy will be (37)
dV = dm⋅ g z⋅
Where (39)
dm =ρds
From (13)-(15) we obtain (40)
⎛
T = 12 ρ0∫l ⎜⎜⎜⎝⎜⎛⎜⎝0∫s (− qsinq ′sinθ′ +θ′cosq′cosθ′)ds′⎞⎟⎠⎟2 + (41)
⎛
+ ⎜⎜∫s (− q′sinq′cosθ′ −θ′cosq′sinθ′)ds′⎟⎟⎞2 +
⎝0 ⎠
⎛⎝ 0s ′cosqds′ ′⎞⎟⎟⎠ ⎞⎟⎟⎟⎠ds + 12 ρ0∫l u ds2 2
⎜⎜∫q
l s
V = ρg∫∫sinq dsds′ ′ (42)
dT (38)
0 0
The elastic potential energy will be approximated by two components, one determined by the bending of the element
and the other is given by the axial tension/compression energy component
Vea = 1 ku2 (44)
Veb (43)
2
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
Ve =Veb +Vea (45)
We will consider F s,t , F s,tθ( ) q( ) the distributed forces on the length that determine motion and orientation in the θ- plane, q - plane and F tu( ), the force that determines axial motion, assumed constant along the length of the arm.
3.2 Dynamic model
In this paper, the manipulator model is considered a distributed parameter (Ivanescu, 2002). system defined on a variable spatial domain Ω= [0,l] and the spatial coordinate is denoted by s.
The dynamic model of this manipulator with hyper-redundant configurations can be obtained, in general form, from Hamilton partial differential equations of the distributed parameter model,
∂ω(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 , F t,s , sθ( ) q ( ) ∈[0,l] and a lumped component F t .u( )
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 ⎜⎜⎝δθδ(Tt,s)⎞⎟⎠⎟−δθδ(Tt,s) +δθδ(Vt,s) +δθδ(Vt,se ) = Fθ (48)
∂ ⎛⎜ δT ⎞
∂t ⎜⎝δq t,s( )⎟⎠⎟ −δq t,sδ(T ) + δq t,sδ(V ) + δq t,sδ(Ve ) = Fq (49)
∂ ⎜⎛ ∂T ⎞⎟− ∂T + ∂V + ∂Ve = Fu (50)
∂t ⎝ ∂u⎠ ∂u ∂u ∂u
where ∂ / ∂ ⋅(),δ/δ(⋅) denote classical and functional partial derivatives (in Gateaux sense]), respectively.
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.
3.3 Unconstrained control
The artificial potential is a potential function whose points of minimum are attractors for a dissipative controlled system. It was shown that the control of robot motion to a desired point is possible if the function has a minimum in the desired point. In this section we will extend this result for the infinite dimensional model of the tentacle manipulator with variable length.
We consider that the initial state of the system is given by
ω0 =ω(0,s)= [θ0 , q , l0 0 ]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)
corresponding to the initial position of the manipulator defined by the curve C0
C0 :(θ0 (s), q0 (s), l0 ), s∈[0,l0] (55)
The desired point in Γ(Ω) is represented by a desired position of the arm, the curve Cd ,
ωd = [θd , q , ld d ]T , νd = [0, 0, 0]T
(56) C :d (θd(s , q) d(s , l) d), 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 , F , Fθ q u from the initial position C0 to the desired position Cd . 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 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,
(57)
F tu( ) (58)
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,
(59)
The control law (57)-(59) modifies the system potential and the Lagrange equation (48)-(50)
(Masoud & Masoud, 2000) become
t ⎜⎜⎝δθ(t,s)⎟⎟⎠−δθ(t,s) +δθ(t,s) = Fθd
∂ (60)
∂ ⎛⎜⎜δq t,sδ(T ⎞⎟⎟⎠ −δq t,sδ(T ) +δq t,sδΠ( ) = Fqd
∂t ⎝ ) (61)
∂ ⎛ ∂
⎜ T ⎞⎟− ∂T + ∂Π = Fu (62)
∂ ⎛ δT ⎞ δT δΠ
∂t ⎝ ∂u⎠ ∂u ∂u d
The force components Fθd , Fqd , Fud represent the damping components of the control and have the form
Fθd (s,t)= −∫l Kθ(s,s′) (θs ,t ds′ ) ′ (63)
0
l
Fqd (s,t)= −∫Kq (s,s q s ,t ds′) (′ ) ′ (64)
0
Fud (t)= −K u tu ( ) (65)
where Kθ(s,s , K′) q (s,s′) are positive definite specified spatial weighting functions on (Ω×Ω) and Ku is a positive constant. For practical reasons, the derivative components of the control have the form
Kθ(s,s′)=δ(s −s′)⋅kθ(s) (66)
Kq (s,s′)=δ(s −s′)⋅kq (s) (67)
3.4 Constrained control
Let B be the region of the state (Ceah & Wang, 2005) space where the mechanical system

motion is not admissible, its complement B is the region of admissible movements and ∂B is the boundary of B. The control problem is to determine the potential function Π(θ,q,u) which would determine the motion to the desired position (ωd(s ,) νd(s , s)) ∈[0,l] and it does not penetrate the constrained area B. In terms of the artificial potential, this means that this

functional should have a single stationary point in B and grows without limit when the system penetrates the boundary ∂B.
We will consider the following artificial potential,
Π(θ,q,u)= max{Π1 (θ,q,u ,) Π2 (θ,q,u)} (68)
where Π1 (θ,q,u) is the artificial potential for unconstrained problem and Π2 (θ,q,u) is the potential for constrained control problem.

Π2 (θ,q,u) is a non-negative, continuous functional defined in B and
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, , tl] ∈[0,t f ] θ=θ(s,t , s) ∈[0, , tl] ∈[0,t f ]
q′ = q s ,t , s( ′ ) ′∈[0,s , t] ∈[0,t f ] q
t
∂
q′ = q s ,t( ′ ) , s′∈[0,s , t] ∈[0,t f ] q′
∂t t
q′′ = ∂2q s ,t( 2′′ ) , s′′∈[0,s , t] ∈[0,t f ]
∂t
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Fq = F s,t , sq ( ) ∈[0, , tl] ∈[0,t f ] Fu = F t , tu ( ) ∈[0,t f ]
From (60)-(62), it results,

+ q′2(cosq′sinq′′cos(θ′−θ′′)−sinq′cosq′′)+ θ′2 cosq′sinq′′cos(θ′ −θ′′) −q q′ ′′sin(q′′−q′))ds ds′ ′′+
s
cosq ds′ ′ + kd q2 = Fq
s s
(q′sinq′cosq′′sin(θ′′−θ′)+ θ′cosq′cosq′′cos(θ′′−θ′)− q′2 cosq′cosq′′sin(θ′′−θ′)+ +θ′cosq′cosq′′sin(θ′′−θ′)− θ′ ′qsinq′cosq′′cos(θ′′−θ′))ds ds′ ′′+ kd2θ= Fθ
ρu+ ρu2 + ku = Fu
4. Visual servoing system
4.1 Camera system
In the Appendix 2 the dynamic model of the 3D spatial hyper redundant arm is determinated. Two video cameras provide two images of the whole robot workspace. The two images planes are parallel with XOY and ZOY planes from robot coordinate frame, respectively (Fig. 8). The cameras provide the images of the scene stored in the frame grabber’s video memory being displayed on the computer screens (Hannan & Walker, 2005); (Kelly, 1996). Related to the image planes, two dimensional coordinate frames, called screen coordinate frames or image coordinate systems are defined. Denote XS1 , YS1 and ZS2 , YS2 , respectively, the axes of the two screen coordinate frames provided by the two cameras. The spatial centers for each camera are located at the distances D1 and D2, with respect to the XOY and ZOY planes, respectively. The orientation of the cameras arround the optical axes with respect to the robot coordinate frame, are noted with ψ and φ, respectively. A point P in the coordinate frame is
P=[ x, y, z]T (70)
The description of a point P in the two screen coordinate frames are denoted by
PS2 =[ xS1 , yS2 ] (71)
PS2 =[ zS2 , yS2 ] (72)
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 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
⎡⎣⎢xyss11 ⎤⎥⎦ =α1 ⋅λ1 −(λD1 + x)⋅R( )φ ⋅ ⎧⎪⎨⎩⎪⎡⎢⎣yx⎥⎦⎤ − ⎣⎡⎢oo11⎦⎥⎤⎪⎫⎭⎪⎬+ ⎡⎢⎢⎣ccxy11 ⎤⎥⎦⎥ (73)
1 12
for the ZS1OS1YS1 frame and
⎡⎢⎣yzss22 ⎤⎥⎦ (λ2 )⋅R( )φ ⋅ ⎧⎪⎨⎢⎡z⎤⎥ − ⎢⎡o21⎥⎤⎪⎬⎪⎭⎫ + ⎢⎢⎣⎡ccy2z2 ⎥⎥⎦⎤ (74) =α2 ⋅λ2 − D2 + x ⎩⎪⎣y⎦ ⎣o22⎦
for the ZS2OS2YS2 frame, where [ cx1 , cy1 ]T and [ cz2 , cy2 ]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.

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
⎡⎢Δxs11 ⎤ 1 λ1 −(λD11 + x)⋅⎡⎣ΔΔyx⎤⎥⎦
⎣Δys ⎥⎦ =α ⋅ ⎢ (75)
⎡⎢Δzs2 ⎤ =α2 ⋅ (λ2 )⋅⎡Δz⎤⎥
Δ ⎥ ⎢ (76)
⎣ ys2 ⎦ λ2 − D2 + x ⎣Δy⎦