Dynamic Modelling of the Compliant KUKA-DLR Lightweight Robot

Walter A. Galvao da Veiga*Supervision of Professor Jose M. Gutierrez Sa da Costa and Professor Jorge M. Mateus Martins

*Department of Mechanical Engineering, Instituto Superior Tecnico,University of Lisbon (ULisbon) Av. Rovisco Pais, 1049-001

Lisboa, Portugal; e-mail: waltergalvaoveiga@gmail.comMay, 2014

Abstract

This paper addresses the development of a model that reproduces the dynamic behaviour of a redundant, 7 degrees offreedom robotic manipulator, namely the Kuka Lightweight Robot IV, in the Robotic Surgery Laboratory of the InstitutoSuperior Tecnico. For this purpose, the control architecture behind the Lightweight Robot (LWR) is presented, as wellas, the joint and the Cartesian level impedance control aspects. Then, the manipulator forward and inverse kinematicmodels are addressed, in which the inverse kinematics relies on the Closed Loop Inverse Kinematic method (CLIK).Redundancy resolution methods are used to ensure that the joint angle values remain bounded considering their physicallimits. The joint level model is the first presented, followed by the Cartesian level model. The redundancy inherent tothe Cartesian model is compensated by a null space controller, developed by employing the impedance superpositionmethod. Finally, the effect of possible faults occurring in the system are simulated using the derived model.

Keywords: lightweight robot, flexible joints, impedance control, compliant behaviour, kinematic redundancy,closed-loop inverse kinematics.

1. Introduction

Robotic manipulators are general purpose machines forindustry, developed under the premise that higher positio-ning accuracy (repeatability and absolute accuracy), speed,durability, and robustness can be achieved. Therefore, to-day’s industrial robots are used especially in well structuredenvironments, in which the position and shape of the partsto be manipulated are well known and in which collisionswith the environment can be estimated and excluded in ad-vance.

In contrast, the KUKA lightweight robotic arm, develo-ped by the German aerospace center (DLR), was designedfor applications involving interaction with humans in un-structured environments. In such applications, as in healthcare, high absolute positioning accuracy cannot be exploiteddue to limited accuracy of position information about thesurrounding environment, while its side-effects in design(high stiffness and mass) are clearly undesired. The ma-nipulator’s low mass, compared to its payload, also comesas a requirement of such applications, in order to enablemobility and minimize the injury risks. The main require-ments for the electronic design result from the high numberof sensors, such as joint torque sensors, redundant positionsensing, and wrist force-torque sensing.

Within this new robot concept, a strong emphasis is set onthe design of control laws which can provide robust perfor-mance, with respect to positioning and model uncertainty,as well as active safety for the human and the robot duringtheir interaction. A well established framework to managethis task is given by impedance control [6]. Impedance con-trol, consists in establishing a virtual mass-damper-springrelationship, between the Cartesian position and the Carte-sian force. Furthermore, the same relationship can be es-tablished in the manipulator joint space, relating the jointsposition and the joints torque.

The present article concerns the study of the KUKA LWRdynamic behaviour, more precisely, the development of amodel that reproduces the manipulator dynamic behaviour.The article focuses on the development of the Cartesianlevel model. The model main feature, consists in mappingthe desired Cartesian behaviour to the desired behaviour atthe joints level. Therein, the manipulator Jacobian and itstranspose play a key role. The manipulator stiffness map-ping, from the Cartesian to the joint level, is accomplishedexploiting the vector calculus of Taylor Series.

To compensate the redundancy inherent to the Cartesianmodel, the impedance superposition method is addressed,exploiting two distinguished projection matrices, these be-

Figure 1: Virtual model of the lightweight robot.

ing a static and a dynamic consistent method. The modelwas developed and verified under SimulinkTM environ-ment. Therefore, for the better understanding of the mani-pulator dynamic motion, it is included a virtual manipulatormodel, depicted in figure 1.

2. Overview of the KUKA Lightweight Robotic Ma-nipulator

The KUKA lightweight robots were designed with the pri-mary goal to resembles the human arm, i.e. a kinematic re-dundant manipulator, with seven degrees of freedom (DOF),a load-to-weight ratio of approximately 1:1, a total weightless than 15 Kg, and a high dynamic performance, guaran-teed by a full state measurement at all joints, using straingauge-based torque-sensing, motor position sensing, basedon magneto-resistive encoders and link-side position sensing,based on potentiometers. The torque sensors are mountedon a Harmonic Drive gear and therefore measures the jointtorque acting on the links. The robot joints are seriallyconnected with a central computer, via an optical fibre bussystem, using the standardized real-time SERCOS protocol[1, 14].

The joint level control is computed at 3 kHz sampling rateon a signal processor in each joint, while the robot dynamicsand the Cartesian control are computed in a 1 kHz cycle ona central computer [5], and a flexible joints model is assumedfor the robotic arm [15, 2, 11].

At the joint level, a decentralized state feedback controlleris implemented by using the entire joint state in the feedbackloop, namely the motor position θ and velocity θ as well asthe joint torque τ and its derivative τ [11]. The motor posi-tion and joint torque are available from measurements, whiletheir derivatives are computed numerically. With these foursignals it is possible to implement joint controllers with fullstate feedback (fig. 2).

By appropriate scaling of the feedback gains, the con-troller structure can be used to implement position, torqueor impedance control. The desired torque for the controlleris computed according to the expected robot dynamics, e.g.if the robot is not moving, it corresponds to the gravitytorque. Therefore, the robot operates in the so called ”zero

Figure 2: Structure of the joint level controller [1].

gravity mode”, in which the motors compensate the robot’sown weight [5].

The control approach is passive based [15, 2, 11], meaning,it relies on energy to guarantee the overall system stability.The preference for passivity based control is consequence ofthe fact that the mechanical properties of the manipulatedobjects and of the contacted environment are not knownprecisely.

The feedback terms turn out to have very intuitive phy-sical interpretations. Torque feedback reduces the apparentinertia of the motors, as well as the joints friction. Motorposition feedback is equivalent to a physical spring, whilevelocity feedback produces energy dissipation (viscous fric-tion). These interpretations allow the passivity, and thus,stability analysis aforementioned, and also enables a consis-tent generalization to the Cartesian impedance control [1].

With the physical interpretation of torque and positionfeedback aforementioned, it is intuitive to design a Carte-sian controller, using the joint level torque controller, forreduction of motor inertia and friction, and by replacing thejoint level stiffness with a Cartesian spatial spring [15, 2, 11].To implement the virtual spring, the motor position is uti-lized, in order to preserve the system passivity. However, thedesired tip position and the stiffness is specified in terms oflink position q. Therefore, a statically equivalent estimationq(θ) for q is computed, based only on the motor position θand the joints stiffness. The forward kinematics, the Jaco-bian, the gravitational torques, and the impedance law arethen computed, based on this value, leading to a passivestructure as shown in figure 3.

3. Kinematic Model of the 7 DOF Manipulator

3.1. Forward Kinematics

The redundant, 7 DOF LWR manipulator is composed ofseven rotational joints. For this 7 DOF manipulator, thecoordinate frames are assigned in figure 4. Frame zero, de-fined as the reference frame, is fixed to the base, and frame7 is the end-effector frame.

The corresponding Denavit-Hartenberg (DH) parametersare presented in table 1. The manipulator overall transfor-mation matrix, can be derived by each known homogeneous

2

Figure 3: Structure of the Cartesian impedance controller [1].

transformation matrix Ti−1i [17, 4]. Therefore, it will not be

difficult to obtain the manipulator position and orientation,through forward kinematics.

Figure 4: The kinematic model.

3.2. Inverse Kinematics (IKin)

The goal of the IKin problem is to find a feasible jointspace variable q, for a given task space variable X. Thedifferential kinematic equation, in terms of the (geometric)Jacobian, establishes a linear mapping between joint spacevelocities and task space velocities, and it can be utilized tosolve for the joint velocities using kinematic equations. Thedifferential kinematic equation has the following form:

X =

[p

ω

]= J(q)q . (1)

Due to the non-square Jacobian matrix for the redundantmanipulator, the inverse solution is obtained by using theJacobian pseudo-inverse matrix J†, and the inverse solutioncan then be written as:

q = J†(q)X , (2)

J†(q) = JT(JJT)−1 . (3)

Table 1: Denavit-Hartenberg parameters.

Joints di (m) qi (rad)aiαi(rad) qmin qmax

1 3.105× 10−1 q1 0 π/2 -170 170

2 0 q2 0 −π/2 -120 120

3 4.0× 10−1 q3 0 π/2 -170 170

4 0 q4 0 −π/2 -120 120

5 3.9× 10−1 q5 0 π/2 -170 170

6 0 q6 0 −π/2 -120 120

7 7.8× 10−2 q7 0 0 -170 170

For a kinematically redundant manipulator, a non-emptynull space exists due to the excess of joint space variablesrelative to the task space (n > m), which can be utilised toset up systematic procedures for an effective handling of theredundant DOFs. The null space is the set of joint spacevelocities that yield null task space velocities, at the currentrobot configuration. A common method to deal with thenull space was formulated by Liegeois [9]:

q = J†(q)X + (I− J†(q)J(q))q0 ; (4)

where q0 represents the set of null space velocities and(I− J†(q)J(q)) allows the projection of q0 into the Jacobinnull space.

Open-loop solution of the joint variables through numeri-cal integration, unavoidably leads to solution drift and thento task space errors. To overcome this drawback, a CLIKalgorithm is usually used, which is based on the task spaceerror e, between the desired and actual end-effector positionand orientation. At the velocity level the pseudo-inverse so-lution results in a minimum-norm velocity solution, and thegeneralized CLIK algorithm can be expressed by [17]:

q = J†(q)(Xd + Ke) ; (5)

where K is a symmetric and positive definite matrix. Thechoice of K guarantees that the error uniformly convergesto zero, i.e. the error tends to zero with a convergencerate which depends on the eigenvalues of the K matrix [17].Therefore, the general inverse solution of a kinematicallyredundant manipulator at the velocity level, based on theCLIK algorithm, will have the following form [17]:

q = J†(q)(Xd + Ke) + (In − J†(q)J(q))q0 . (6)

The error e, between the manipulator desired and actualposition and orientation is given by:

ep = pd − p(q) , (7)

eO,quat = η(q)εd − ηdε(q)− S(εd)ε(q) ; (8)

being ep the position error and eO,quat the orientation error.For the orientation error, the unit quaternion Q= {η, ε}representation was adopted [17], and S(·) represents a skew-symmetric operator.

3

Figure 5: Closed-Loop Inverse Kinematics scheme.

3.3. Null Space Optimization

Redundancy plays an important role in the kinematic con-trol, as redundant joints allow a manipulator to avoid jointlimits, singularities or obstacles. The redundancy is alsoutilized to minimize joint velocities or actuator torques, incase of following a desired end-effector trajectory. The vari-able q0, in equation 6, represents a set of velocities in themanipulator null space. Therefore, the contribution of q0 isto generate null space motions of the structure, that do notalter the task space position or orientation, but allow themanipulator to reach postures, which are more dexterousfor the execution of the given task. A typical choice of thenull space joint velocity vector, is given by [17, 9]:

q0 = k0

(∂w(q)

∂q

)T; (9)

being k0 a constant (k0 > 0), w(q) a scalar objective func-

tion of the joint variables and ∂w(q)∂q the vector function re-

presenting the gradient of w(q). At Bruno Siciliano et. al[17] or Kapoor et al. [3] it’s discuss different ways to ex-plore the objective function. However, in this paper, onlythe joint limit avoidance is considered as a optimization cri-teria. Therefore, the objective function can be expressed by[17]:

w(q) = − 1

2n

n∑i=1

(qi − qi

qiM − qim

)2

; (10)

being qiM and qim, respectively, the upper and lower jointlimits, and qi the middle value of the joint range. The CLIKalgorithm considering the joint limits avoidance, as a per-formance criterion, is represented in block scheme form infigure 5.

4. Model Implementation

In this section the Cartesian and joint level models aresummarized. Both in the Cartesian level model, as in thejoint level model, the representative variables of the sys-tem non-linear dynamic motion (Coriolis, centrifugal andgravity) are not considered, i.e. it is assumed that thesedynamics are compensated. Both models allow one to spec-ify the desired level of compliance, through the impedance

Figure 6: Representative model of the i-th joint of the manipulator.

variables. Therefore, depending on the commanded jointstiffness, the manipulator controller, can be parametrizedas a position controller (kdes → kmax), or a torque controller(kdes → 0).

4.1. The Joint Level Model

The impedance control algorithm behind the LWR sys-tem, provides the manipulator joints to behave as a secondorder mass-damper-spring system:

τext = Iθ∆q + Dθ∆q + Kθ∆q ; (11)

where Iθ, Dθ and Kθ represent, respectively, the inertial, thedamping and the stiffness desired matrices, at the joint level.Hence, each joint of the manipulator can be represented bythe block diagram shown in figure 6. The closed-loop modelof the manipulator i-th joint is given by:

qiqrefi

=

Kθi/Iθis2 + Dθi/Iθi s+ Kθi/Iθi

(τexti = 0) , (12)

qiτexti

= (1/Kθi )

Kθi/Iθis2 + Dθi/Iθi s+ Kθi/Iθi

(qrefi = 0) . (13)

That is, the manipulator joints behave as a mass-damper-spring system, both when the manipulator is driven by anexternal force or moment, or when following a given refe-rence. Equation 12 and 13 represent a second order transferfunctions. Parameters of such representations may be eva-luated, considering the following equality:

ω2ni

s2 + 2ξiωnis+ ω2ni

=

Kθi/Iθis2 + Dθi/Iθi s+ Kθi/Iθi

; (14)

That is, the joint level impedance parameters may be eva-luated as:

Iθi = Kθi/ω2ni

, (15)

Dθi = 2ξiωniIθi = 2ξiKθi/ωni ; (16)

where ωni is the natural frequency of the i-th joint, to bedetermined from the actual robotic manipulator using ap-propriate methods of identification, while ξi is the dampingfactor of the i-th joint. Hence, the global joint model al-lows one to specify the desired damping factor and stiffness

4

at each joint, as to assign a certain reference, in terms ofangular position, or apply a certain torque to each joint.

4.2. The Cartesian Level ModelThe main purpose of the Cartesian level impedance con-

trol process, represented by equation 17, is to promote thebehaviour of a second order mass-damper-spring system, be-tween the manipulator end-effector position X, and the vec-tor of external forces and moments fext:

fext = Mx∆x + Dx∆x + Kx∆x ; (17)

being Mx, Dx and Kx positive definite matrices, which re-spectively, represent the virtual inertial, damping and stiff-ness matrices, defined at the Cartesian level.

To derive the manipulator model, where the parametersare assigned at the Cartesian level, it is required a mappingof these variables to the respective joint variables, wherethe manipulator Jacobian matrix and its transpose take aleading role (fig. 7).

Figure 7: Cartesian level to joint level mapping.

Considering the linear velocity mapping through the ma-nipulator Jacobian (J : q 7→ X), described by equation 1,the following relation holds:

∆X = X0 −X , (18)

∆X = −X = −Jq , (19)

∆X = −X = −Jq− Jq ; (20)

where is considered X0 = cte, for simplicity. Substituting19 and 20 into 17, and taking into account that the ex-ternal forces acting on the manipulator can be mappedwith the Jacobian transpose to the respective joint torques(JT : F 7→ τ ), one has:

τ = JTfext = JT(−Mx(Jq + Jq)−DxJq + Kx∆X)

= −(JTMxJ)q− (JTMxJ)q− (JTDxJ)q + JTKx∆X . (21)

The terms JTMxJ and JTDxJ can be characterized as themapping of the virtual mass and damping matrix desired atthe Cartesian level (Mx, Dx), to the respective inertial anddamping matrix at the joint level (Iθ, Dθ):

Iθ = JTMxJ , (22)

Dθ = JTDxJ . (23)

The term JTMxJ is the Coriolis term, and it will be denotedas I.

To complete the mapping from the Cartesian space tothe joint space, it remains to handle the stiffness termJTKx∆X. To this end, it is exploited the vector calculusto Taylor series is exploited.

It can be shown that the multi-variable Taylor series maybe formulated in the following way [7, 10]:

f (x1, · · · , xn) = f |p +∑i

∂f

∂xi

∣∣∣∣p

(xi − pi) +

1

2

∑i,j

∂2f

∂xi∂xj

∣∣∣∣p

(xi − pi)(xi − pi) + · · · (24)

(i, j = 1, · · · , n.) .

where f (x1, · · · , xn) represents a multi-variable function tobe expanded, p is referred to, as the expansion point and xthe evaluation point. The first sum at the right hand side ofequation 24, referred to as the gradient of f(x), contains thevector of the first partial derivatives of f(x), with respect tox:

∇f (x1, · · · , xn) =

∂f∂x1...∂f∂xn

. (25)

The second sum, which contains the matrix of second partialderivatives of the function f(x), with respect to x, is referredto as the Hessian matrix of f(x):

[∇2f ] =∂2f

∂xi∂xj=

∂2f

∂x1∂x1· · · ∂2f

∂x1∂xn...

. . ....

∂2f∂xn∂x1

· · · ∂2f∂xn∂xn

. (26)

Therefore, symbolic notation may be finally applied to ex-press equation 24:

f (x) = f (p) + ∇f T |p(x-p) +1

2(x-p)T [∇2f ]P (x-p) + · · · . (27)

The functions to be expanded by the Taylor series, whichis dependent of the joint variable, is given by:

J(q)TKx∆X(q) =

f 1(q)

...

f n(q)

, (28)

i.e. one has a number k of functions, each n-dimensional:

f k(q) =m∑i=1

m∑j=1

JtkiKij∆Xj (k = 1, · · · , n.) . (29)

where n represents the number of the manipulator DOF, mthe number of DOF required to place and orientate the ma-nipulator in the operational space. Each of the k functionscan then be expanded, applying equation 27.

One can easily verify that the first term, of the expansionof each of the k functions is null. The second term of the

5

expansions, i.e. the gradient of f k(q), is given by:

∇f k(q) =

∑mi=1

∑mj=1(JtkiKij

∂∆Xj∂q1

+∂Jtki∂q1

Kij∆Xj)...∑m

i=1

∑mj=1(JtkiKij

∂∆Xj∂qn

+∂Jtki∂qn

Kij∆Xj)

(30)

(k = 1, · · · , n.) .

Given the definition of a robotic manipulator Jacobian [17]:

J(q) =∂k(q)

∂q; (31)

Equation 30 can be rearranged in the following matrix form:

∇f k(q) = JTKxJ +[

∂JT

∂q1Kx∆X · · · ∂JT

∂qnKx∆X

]= JTKxJ +

∂JT

∂qKx∆X . (32)

Equation 32 can be characterized as the mapping of thestiffness, desired at the Cartesian level, in the respectivestiffness at the joint level:

Kθ = JTKxJ +∂JT

∂qKx∆X . (33)

The third term of the Taylor expansion can be obtained,calculating the second partial derivatives of the functionf k(q), with respect to q:

[∇2f k] =

∂2f k∂q1∂q1

· · · ∂2f k∂q1∂qn

.... . .

...∂2f k∂qn∂q1

· · · ∂2f k∂qn∂qn

(k = 1, · · · , n.) . (34)

It can be demonstrated [16] that the elements of the Hessianmatrix shown above can be obtained by:

ekij =∂2Jt

(k,:)

∂qj∂qiKx∆X + 2

∂Jt(k,:)

∂qiKxJ(:,j) + Jt

(k,:)Kx∂J(:,i)

∂qj

({i, j, k} = 1, · · · , n) . (35)

Obtaining the Higher order terms (HOT) from the Taylorexpansion, proved important for the proper functioning ofthe derived Cartesian model. However, obtain these termsmathematically, as it was done for the first three, wouldlead to an excessive and fatiguing amount of calculation.Therefore, the HOT can be obtained by:

HOT = JTKx∆X−(

Kθ∆q +1

2

[∆qT∇2f∆q

]). (36)

One can now represent the Taylor series expansion of stiff-ness term:

JTKx∆X = Kθ∆q +1

2

[∆qT∇2f∆q

]+ HOT . (37)

Thus equation 21, can be rewritten in the following way:

τ = −Iθq− Iq−Dθq + Kθ∆q +1

2

[∆qT∇2f∆q

]+ HOT . (38)

⊗

����

��

⊗ −

��

�

�

��

��∙�

���

��∙�

QQQQdddd

QQQQ

∫∫� �

��∙�

�� � ��

�����

���

��

��

!�"

! �∙�

#$%" '()*$"

+

− +

-

-

+

� = /01 2−3 − /45� − �5� + �5∆ + 1�∆"78�9:∆; �5∆ + 1

� ∆"78�9�∆-, ��, �:∆

�"����

�")�

�")��

�"���

�5 = �"��� + !�"

! ��∆-

8�9�∆-, ��, �

∆- =++(+

'()*$"

Figure 8: Cartesian level Model.

The dynamics of the manipulated joints can be finally re-presented by:

q = I−1θ

(−τ − Iq−Dθq + Kθ∆q +

1

2

[∆qT∇2f∆q

]+ HOT

). (39)

The joint variable q may be obtained by successive integra-tion of equation 39, which in turn is utilised to calculate theforward kinematics, the Jacobian matrix and the impedancevariables at the manipulator joint level. Figure 8 indicatea block scheme of the derived Cartesian level model. Asfor the joint model derived, one may specify the desiredstiffness and damping factor at the Cartesian level by con-sidering Mx = Kx

ω2n

and Dx = 2ξKx

ωn. The orientation error

is calculated exploiting the unit quaternion representation.The desired reference to the joints q0, has to be consistentwith the reference at the Cartesian space X0,R0. Hence q0

is obtained by de CLIK algorithm presented in section 3.

5. Null Space Stiffness

In this section it is assumed that the desired null spacebehaviour can be characterized in the joint space by a de-sired positive definite stiffness matrix Kn, damping matrixDn and inertial matrix In, as well as a desired equilibriumpoint qn. From these, the desired null space acceleration iscomputed according to a joint level PD-controller:

q0 = I−1n (−Dnq−Kn(q− qn)) . (40)

In order to prevent interference with the Cartesianimpedance behaviour, the desired joint null space accelera-tion, has to be projected into the manipulator’s null space,

6

−0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 0.2−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

y (m)

z (m

)

Given ReferenceActual Position

Figure 9: Reference and Tracking.

by a properly chosen matrix N(q):

q = qc + qn , (41)

qn = N(q)q0 . (42)

where qc comes from the Cartesian model described above.In the following, two distinguished methods to obtain theprojection matrices are addressed.

5.1. Static Null Space Projection

Let V (q) denote a full rank left annihilator of JT(q), i.e.V(q)JT(q) = 0. Then a projection matrix of the form:

N(q) = V(q)TV(q) , (43)

may be used to project the desired acceleration into thenull space of the manipulator’s Jacobian. The matrix V(q)may be computed by a singular value decomposition of theJacobian matrix [12]. Although, in this paper the followingstatic projection matrix will be considered [17]:

N1(q) = Ident − J†J . (44)

where Ident corresponds to an identity matrix.

5.2. Dynamically Consistent Projection

Equation 43 describes a projection matrix, based only onkinematic movement. A sufficient condition for a null spacemapping to be consistent with the dynamic motion of themanipulator is given by:

J(q)M(q)−1N(q) = 0 . (45)

This condition can be fulfilled with a mapping of the form[12]:

N(q) = M(q)V(q)TV(q) ; (46)

in which the statical null space projection matrix is pre-multiplied, and thus scaled, by the manipulator’s mass ma-trix. In this document the following dynamic consistent pro-jection matrix will be considered [8]:

N2(q) = Ident − JTMxJI−1θ . (47)

Figure 10: Tracking errors.

6. Results

6.1. Kinematic Model Validation

The validation of the derived forward kinematics is fairlysimple and straightforward, therefore, it will not be hereinpresented. On the other hand, the validation of inverse kine-matics is much more attractive to evaluate. Table 1 showsthe corresponding data to the manipulator joint limits. Tovalidate the proposed inverse kinematic algorithm, it is in-tended that the manipulator perform in the Cartesian space(YZ plane), during 500 seconds and while maintaining it’sorientation fixed, the maximum number of circumferencespossible, whose radius are 0.2 meters. In the absence of theredundancy resolution algorithm, as can be seen from figure9, the manipulator follows with very good accuracy the pre-scribed reference. The position tracking error is of the orderof 10−3, while the orientation tracking error is practicallynull (figure 10). However, it can be seen from figure 11 thatthe joints 2, 5 and 7 exceed their limits.

Once the redundancy resolution algorithm is introducedthe joint angle values are consistently maintained undertheir limits, as can be seen from figure 12. Both for the posi-tion, as for the orientation, the tracking error is maintainedlow. During the final simulations it was used the follow-ing values for the respective position and orientation gains:

0 50 100 150 200 250 300 350 400 450 500

−80−60−40

Join

t 1

Joint Angles variation (degree)

0 50 100 150 200 250 300 350 400 450 50080

100120140160

Join

t 2

0 50 100 150 200 250 300 350 400 450 50050

100

Join

t 3

0 50 100 150 200 250 300 350 400 450 500

80

100

Join

t 4

0 50 100 150 200 250 300 350 400 450 500

0200400

Join

t 5

0 50 100 150 200 250 300 350 400 450 500−60−40−20

02040

Join

t 6

0 50 100 150 200 250 300 350 400 450 500

−600−400−200

Join

t 7

Time (s)

Figure 11: Joint angular position.

7

0 50 100 150 200 250 300 350 400 450 500

−40−20

020

Join

t 1Joint Angles variation (degree)

0 50 100 150 200 250 300 350 400 450 50050

100

Join

t 2

0 50 100 150 200 250 300 350 400 450 5000

204060

Join

t 3

0 50 100 150 200 250 300 350 400 450 50080

100

Join

t 4

0 50 100 150 200 250 300 350 400 450 500

−200

20

Join

t 5

0 50 100 150 200 250 300 350 400 450 500−90−80−70

Join

t 6

0 50 100 150 200 250 300 350 400 450 500

−40−20

0

Join

t 7

Time (s)

Figure 12: Joint angular position (considering redundancy resolution).

Kp = diag{100, 100, 100} e Ko = diag{500, 500, 500}.Where diag(·) represents a diagonal matrix. The constantk0 needed to calculate the null space velocity, described bythe equation 9, was fixed at 14 after consecutive iterations.

6.2. Joint and Cartesian Model Validation

The validation of the models derived, both for the Carte-sian level as for the joints level, goes through compare thebehaviour of the derived models, to the behaviour of therobotic arm. To this end, in order to identify the manipula-tor bandwidth, it’s performed a set of tests to the manipu-lator, applying the logarithmic decrement method [13].

The logarithmic decrement method, consists to evaluatethe response of a damped system when subjected to freevibrations. Once the damped response is available, themethod allows to compute the signal parameters accordingto the following expressions [13]:

ξ =ln x0xn√

4π2n2 + (ln x0xn

)2, (48)

Ta =Tn − T0

n, (49)

wa =2π

Ta, (50)

wn =wa√1− ξ2

. (51)

where x0 and xn are respectively the first and last signalmaxima, spaced by n cycles. Ta is the damped period, ωa isthe damped natural frequency and ωn the undamped naturalfrequency.

Figure 13 illustrates the responses of the joint model, forthe two joints 1 and 3, and the Cartesian model, in the x andy directions, compared to the Kuka robotic manipulator res-ponses. To evaluate the models performance, it’s applied tothe obtained signals, the root mean square (RMS), and thevariance accounted for (VAF). Table 2 shows the bandwidths

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8Joint 1 position (Normalized data)

Time [s]

Am

plit

ude

Robot data

Model data

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8Joint 3 position (Normalized data)

Time [s]

Am

plit

ude

Robot data

Model data

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20

0.2

0.4

0.6

0.8

1

1.2

1.4Manipulator position in the x direction (Normalized data)

Time [s]

Am

plit

ude

Robot data

Model data

0 0.5 1 1.5 2 2.50

0.2

0.4

0.6

0.8

1

Manipulator position in the y direction (Normalized data)

Time [s]

Am

plit

ude

Robot data

Model data

Figure 13: Models performance.

ωn(rad/s) EQM V AF (%)

Joint 1 15.25 0.0489 97.18

Joint 2 11.1 0.0408 98.56

Joint 3 18.6 0.0527 97.14

Joint 4 19.5 0.0427 98.48

Joint 5 32 0.0519 95.70

Joint 6 31 0.0595 95.10

Joint 7 12.6 0.1922 86.77

X 7.6 0.0104 99.86

Y 8.2 0.0230 99.11

Z 10.6 0.0172 99.44

Table 2: Models performance

identified for each joint of the manipulator, the bandwidthof the Cartesian space, as well as the performance measure-ments.

The performance measurements obtained and presentedin Table 2 are quite satisfactory, since they are related tothe comparison of signals from a robotic system composedof highly non-linear elements, where there is a degree ofuncertainty associated with their behaviours, to a idealizedcomputational system. The measured bandwidths presentedare for a certain configuration of the manipulator. However,it could be seen throughout the testing, that the bandwidthof the manipulator is a function not only of the stiffness butalso of the manipulator configuration, i.e. the bandwidth isa function of the stiffness and a certain parameter which isdependent of the manipulator joint angular positions. How-ever, the identification of this parameter was not possible,

8

Abrupt Faults Time of occurrence

δKθ +/- 25 Nm/rad t0= 20s

δDθ +/- 24 Nms/rad t0= 15s

Table 3: Abrupt faults.

since it is not available for evaluation.

6.3. Fault analyses

Robotic systems may be classified as high-risk systems inthe presence of faults, since they often operate in the vicinityof humans, as is the case for surgical manipulators, wherethe presence of faults is critical and intolerable. Therefore,it’s important to develop algorithms which make possible thedetection and accommodation of such faults. In this sectionit is intended to evaluate the influence of possible occurringfaults in the manipulator, taking advantage of the derivedCartesian model. It was considered two distinguished typesof faults, being these abrupt and incipient faults. Althoughonly the abrupt case will be presented.

Abrupt faults are faults that occur at a specific point intime and with a given amplitude, while incipient faults arefaults which evolve over time making their effect on the sys-tem more predominant. In robotic systems, faults usuallyoccur in a given actuator or a given sensor. To simulate suchfaults, one may actuate in the joint parameter Kθ or Dθ ofthe Cartesian derived model. Thus, the simulation of faultsin the system goes through add a given percentage amountδ, to the matrices that represent the stiffness and dampingof the joints.

To evaluate the influence of faults, it is intended that therobotic manipulator, describe in the Cartesian space a setof circumferences, and therefore one may concluded aboutthe influence of the faults, evaluating both the manipula-tor Cartesian coordinates, as the torque variation. Table 3shows the intensity of the addressed abrupt faults.

The intensity of abrupt faults considered, correspond toan increase or decrease of about 10% at the current jointstiffness, and about 50% of the current damping value. It’sidealized that the faults occur individually in time. For thecase here presented, it’s considered that the fault occurs inthe joint 3. Figure 14 shows the reference and the trajec-tory carried out by the manipulator, when subjected to theabrupt fault δKθ and +/−δDθ in the joint 3. It is clear thatthe considered faults have an impact in the system perfor-mance. Faults in the stiffness, both when there is a increaseor a decrease in the optimal stiffness, cause an immediatedeviation of the end effector position. Damping faults arerelevant when the damping is less than ideal, i.e. an increasein the damping makes the system over-damped, which by it-self, isn’t critical. On the other hand, if the damping is lessthan ideal, the system enters into vibration, and can desta-bilize.

−0.25 −0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 0.2 0.25−0.25

−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

0.25

Influence of faults in the stiffness (−δKθ)

y (m)

z (m

)

Given ReferenceActual Position

−0.3 −0.2 −0.1 0 0.1 0.2 0.3−0.25

−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

0.25

Influence of faults in the stiffness (−δDθ)

y (m)

z (m

)

Given ReferenceActual Position

−0.25 −0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 0.2 0.25−0.25

−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

0.25

Influence of faults in the stiffness (+δKθ)

y (m)

z (m

)

Given ReferenceActual Position

−0.25 −0.2 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 0.2 0.25−0.25

−0.2

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

0.25

Influence of faults in the stiffness (+δDθ)

y (m)

z (m

)

Given ReferenceActual Position

Figure 14: Reference vs. tracking.

Figure 15 illustrates the difference between the torques onthe joints prior to, and after to, the occurrence of the faults.It can be seen that the difference between the torques, re-lated to −δDθ faults, takes greater significance some timeafter the occurrence of the fault, causing vibrations in thestructure mentioned above. In other cases, the influence ofthe faults are immediate and uniform.

7. Conclusion

In this work, it was developed models representing thedynamic behaviour of the Kuka surgical robotic manipula-tor. The development of the models involved the mappingof the desired dynamic at the Cartesian level, into the de-sired one, at the joint level. To this end, the Jacobian of themanipulator had a preponderant role. It was developed aCLIK algorithm, in which the manipulator redundancy wasexploited in order to keep the joints position under theirlimits. It was obtained satisfactory results for the CLIK al-gorithm, in that it was possible to keep all the manipulatorjoints limited. The developed null space controller, in whichthe impedance superposition method was exploited, leadedto a consistent behaviour of the manipulator, for both theconsidered projection matrices. Both the derived models, toreproduce the dynamic behaviour of the robotic manipula-tion, revealed good results. The models are currently beingused in the development of control algorithms to detect andaccommodate faults and to explore diferente control schemesassociated to the Kuka robotic manipulator.

9

0 10 20 30 40 50 60−100

0100

Influence of faults in the stiffness (−δKθ)

0 10 20 30 40 50 60−2

02

0 10 20 30 40 50 60−200

0200

0 10 20 30 40 50 60−2

02

0 10 20 30 40 50 60−50

050

0 10 20 30 40 50 60−50

050

0 10 20 30 40 50 60−100

0100

Time (s)

∆ τ

[Nm

]

0 10 20 30 40 50 60−2000

02000

Influence of faults in the damping (−δDθ)

0 10 20 30 40 50 60−5000

05000

0 10 20 30 40 50 60−5000

05000

0 10 20 30 40 50 60−5000

05000

0 10 20 30 40 50 60−5000

05000

0 10 20 30 40 50 60−5000

05000

0 10 20 30 40 50 60−2000

02000

Time (s)

∆ τ

[Nm

]

0 10 20 30 40 50 60−100

0100

Influence of faults in the stiffness (+δKθ)

0 10 20 30 40 50 60−2

02

0 10 20 30 40 50 60−200

0200

0 10 20 30 40 50 60−2

02

0 10 20 30 40 50 60−50

050

0 10 20 30 40 50 60−50

050

0 10 20 30 40 50 60−100

0100

Time (s)

∆ τ

[Nm

]

0 10 20 30 40 50 60−2

02

Influence of faults in the damping (+δDθ)

0 10 20 30 40 50 60−0.1

00.1

0 10 20 30 40 50 60−5

05

0 10 20 30 40 50 60−5

05

x 10−3

0 10 20 30 40 50 60−2

02

0 10 20 30 40 50 60−1

01

0 10 20 30 40 50 60−2

02

Time (s)

∆ τ

[Nm

]

Figure 15: Variation in torque.

References

[1] A. Stemmer , T. Wimbock A. Albu-Schaffer , S. Had-dadin , Ch. Ott and G. Hirzinger. The DLR LightweightRobot – Design and Control Concepts for Robots in Hu-man Environments. An International Journal, 34:376– 385, 2007.

[2] Christian Ott, Alin Albu-Schaaffer and Gerd Hirzinger.A passivity based cartesian impedance controller forflexible joint robots - part 2: Full state feedback,impedance design and experiments. IEEE InternationalConference of Robotics and Automation, 2004.

[3] C. Kapoor, M. Cetin, and D. Tesar. Performance basedredundancy resolution with multiple criteria. Procee-dings of ASME Design Engineering Technical Confe-rence, September 1998. Atlanta, GA, USA.

[4] J.J. Craig. Introduction to robotics mechanics and con-trol. Pearson Prentice Hall, 1989.

[5] Alin Albu-Schaaffer, Gerd Hirzinger. Cartesianimpedance control techniques for torque controlledlight-weight robots. IEEE International Conference ofRobotics and Automation, pages 657–663, 2002.

[6] N. Hogan. Impedance control: An approach to manip-ulation, part 1 - theory, party 2- implementation, party3- applications. Journal of Dynamic Systems, Measure-ment, and Control, 107:1–24, 1985.

[7] Bronstein, I.N., Semendjajew, K.A. Taschenbuch dermathematik. 1981. Verlag Harri Deutsch, Thun undFrankfurt.

[8] O. Khatib. A unified approach for motion and forcecontrol of robot manipulators: The operational spaceformulation. IEEE Journal of Robotics and Automa-tion, 3(1):1115—-1120, 1987.

[9] A. Liegeois. Automatic supervisory control of theconfiguration and behavior of multibody mechanisms.IEEE Trans. Systems, Man, and Cybernetics, 1977.

[10] University of Applied Sciences Landshut. Ap-plication of the Vector Calculus to Taylor Se-ries. https://people.fh-landshut.de/~maurer/

numeth/node44.html, 2008.

[11] Alin Albu-Schaaffer, Christian Ott and Gerd Hirzinger.A unified passivity based control framework for po-sition, torque and impedance control of flexible jointrobots. International Journal of Robotics Research,26(1):23–39, 2007.

[12] Christian Ott. Cartesian Impedance Control of Redun-dante and Flexible-Joint Robots, volume 49 of Springertracts in advanced robotics (En ligne), chapter four.Springer, 2008.

[13] A. Piersol and T. Paez. Harris’ Shock and VibrationHandbook, chapter 2. McGraw-Hill Inc., New York,2010.

[14] Gunter Schreiber, Ralf Koeppe, Alin Albu-Schaffer,Alexander Beyer, Oliver Eiberger, Sami Haddadin, An-dreas Stemmer, Gerhard Grunwald, Gerhard HirzingerRainer Bischoff, Johannes Kurth. The KUKA-DLRlightweight robot arm – a new reference platform forrobotics research and manufacturing. presented at theJoint Conf. ISR (41st Int. Symp. Robot.), jun 2010.Munich.

[15] Christian Ott, Alin Albu-Schaaffer, Andreas Kugi, Ste-fano Stramigioli and Gerd Hirzinger. A passivity basedcartesian impedance controller for flexible joint robots- part 1: Torque feedback and gravity compensation.2004.

[16] Walter G. Veiga. Modelacao dinamica do robo ma-nipulador complacente kuka-dlr lwr. Master’s thesis,Instituto Superior Tecnico, 2014.

[17] Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani andGiuseppe Oriolo. Robotics: Modeling, Planning andControl. Springer, 2009.

10