SIMECS

Models description

SPIDER elettro-mechanical model (open-loop) It is constituted by various modules:

Two-phase DC brushless motors Analogic current controllers Mechanical transmissions 7 d.o.f. rigid multi-body chain

Two-phase DC brushless motor: This type of motor is constituted by a rotor, on which the permanent magnets are mounted, and by a stator, which provides the phase electric windings. Assuming the isotropy of the motor, the equations describing the electrical dynamics are:

+

+

=

2

1

2

1

2

1

2

1

00

00

EE

II

dtd

LL

II

RR

VV

where R is the equivalent resistence of each winding, L the inductance and Ei the induced back electromotive force of each phase. In particular, Ei can be expressed as:

( )ϑω ii KE = where ϑ is the motor angle and the shape functions Ki are:

( ) ( )( ) ( )απϑα

αϑαcos)2/sin(

sin)sin(

2

1

KpKKKpKK

−=−===

In these expressions, p is the number of pole pairs and α is the electric angle. Assuming a constant rotor velocity ω , we simply have tωϑ = and tpωα = . Since current controller assures that: ( ) ( )( ) ( )απϑα

αϑαcos)2/sin(

sin)sin(

2

1

IpIIIpII

−=−===

so the motor torque will be:

( ) ( ) IKKIKIKIIEIEPT

mm ==+=

+== αα

ωωτ 222211 cossin

The Dymola model of the motor describes the electrical dynamics of the two phases, the electro-mechanical conversion (block EMF_2), the equivalent inertia of the rotor, the viscous friction of the motor and it includes the sensor.

The electro-mechanical conversion block offers the interesting possibility of simulating the effect of the most representative torque disturbances (due to the motor dynamics):

ripple caused by phase torques unbalances (frequency ωp2=Ω ) ripple due to shape functions imperfections (frequency ωp4=Ω ) detent torque: it’s present also when current is zero (frequency ωp2=Ω )

The ripple caused by phase torques unbalances may be explained introducing the parameter ∆ , which represents the difference in magnitude between the torques produced by the two phases:

)cos()1()sin(

2

1

tpKKtpKK

T

T

ωω∆−−=

=

)cos()sin(

2

1

tpItpIIωω

−==

so we have: )2cos(21

211)(cos2 tpIKIKtpIKIK TTTTm ωωτ ∆−

∆−=∆−=

The parameter ∆ (“delta” in EMF_2 module) must be specified starting from experimental values. Shape functions imperfections, instead, which are frequently caused by border effects in the magnetic circuits or commutation transient, can be justified recurring to Fourier series:

)2/(2

1

πα

α

−∞

−∞=

∞

−∞=

∑

∑

=

=

jn

nn

jn

nn

eKK

eKK

)2/(

2

1

πα

α

−∞

−∞=

∞

−∞=

∑

∑

=

=

jn

nn

jn

nn

eII

eII

obtaining: ( )[ ] απαατ jn

nn

n l

lnjln

lnjlnm eTeIKeIK ∑∑ ∑

∞

−∞=

∞

−∞=

∞

−∞=

−++ =+= 2/)()(

Starting from this equality we can write:

( ) ll

lnjn

n IKeT ∑∞

−∞=−

−+= 2/1 π , so ( ) ll

lnjn

n IKneT ∑∞

−∞=−

−

=

4cos24/ ππ

The last expression make us understand that non-zero harmonics correspond to kn 42 +≠ , where n is an integer; in reality, since current and BEMF shape functions present the same half-wave symmetry, which is repeated every half period with changed sign, the contribution of the even harmonics is null. Thus, the fundamental harmonic of torque disturbance due to shape functions imperfections corresponds to n=4 and it’s characterized by the frequency

ωp4=Ω . The first BEMF significant harmonic is then the third:

−+−=

+=

23sin)()cos(

)3sin()()sin(

2

1

πωω

ωω

tparmonicKtpKK

tparmonicKtpKK

−==

)cos()sin(

2

1

tpIItpIIωω

so we have:

−−+=

23sin)cos()()3sin()sin()( πωωωωτ tptparmonicKItptparmonicKIKIm ,

which can be written as :

)3cos()cos()()3sin()sin()( tptparmonicKItptparmonicKIKIm ωωωωτ −+= , that is:

)4cos()( tparmonicKIKIm ωτ −= The parameter “armonic”, which indicates the relative amplitude of the third BEMF harmonic, can be set (block EMF_2) making use of experimental data. The cogging disturbance, instead, doesnt’t depend on current and can be simulated with a sine term having a frequency of ωp2=Ω and an amplitude equal to “detent” [Nm].

2-phase DC brushless motor Analogic current controller (NRPI module): The current controller answers to two requirements: it amplifies the control signal (output of the motion control) in order to make it compatible with the elettro-mechanical conversion; moreover, it creates opportune sinusoidal references for the phase currents, according to rotor position (inverter). These goals are reached, in the real robot, through PWM (pulse width modulation) and two indipendent current loops (PI analog regulator), one for each phase. In particular, the current set-points for phase 1 and 2 are:

)2

sin(

)sin(

2

1

πα

α

−=

=

II

II where α is the electric angle and I is the current reference

Each PI phase regulator implements the following control law, expressed by means of Laplace transform:

)(11)( sEsT

KsVi

+=

where K is the proportional gain, iT the integral time, V the control voltage and E the phase current error. The Modelica realization provides two analog PI regulator with anti-windup compensation; besides, for time-scale reason, it substitutes the PWM with a simple linear amplifier. The NRPI module allows to set the value of the current offset, useful to simulate sensor polarization, for example. Making this, the module can riproduce a torque disturbance on the motor (ripple), characterized by a frequency of ωp=Ω , where p is the number of pole pairs and ω is the motor speed (assumed constant). In fact, in presence of current offset:

+−=+=

22

11

)cos()sin(

ItpIIItpII

ωω

so:

[ ][ ] [ ][ ])cos()cos()sin()sin( 21 tpKItpItpKItpIm ωωωωτ −+−++= , which can be written

as: )cos()sin( 21 tpIKtpIKKIm ωωτ −+=

Assuming IoffsetII *21 == we can write:

−+=

4sin

22 πωτ tpKIoffsetKIm .

The SIMECS user can set the value of “off_corr” parameter, which represents the offset related to max current (Imax) and which depends from “offset” through the expression:

max

_I

Ioffsetcorroff =

Remembering all we have seen about ripple, we can analyze the complete disturbance model:

−+

−∆−=

+=

23sin)(

2sin)1(

)3sin()()sin(

2

1

πωπω

ωω

tparmonicKtpKK

tparmonicKtpKK

+−=+=

ItpIIItpII

)cos()sin(

2

1

ωω

so the electro-magnetic torque is:

[ ]

[ ] )2sin(detent)4cos()()3cos()3sin()(

)2cos(2

)cos()1()sin(2

1

tptparmonicKItptparmonicIK

tpKItptpIKKIm

ωωωω

ωωωτ

−−++

+∆

−∆−−+

∆−=

then:

[ ]

[ ] )4cos()()3cos()3sin()(

detent2tan2cosdetent2

)cos()1()sin(2

1

122

tparmonicKItptparmonicIK

KItpKI

tptpIKKIm

ωωω

ω

ωωτ

−++

+

∆−

+

∆

−

+∆−−+

∆−=

−

Therefore, the complete model confirms substantially what we have observed analyzing the single disturbance contributions, producing small coupling effects which can be generally be neglected. The module accepts as input q_m , which is the motor position (measured by the motor sensor), and I_ref , which is the current reference output of the control system. NRPI module has two electrical pins that concur to interface it to the motor.

NRPI module Mechanical Transmission: The transmission boxes used in SPIDER robotic arm (Harmonic Drive) present one or more reduction stages; they’re affected by friction, torsional flexibility and backlash problems, so a good mathematical model of such a gear train cannot neglect these phenomena.

A realistic transmission model has been built using the Rotational objects taken from Modelica Standard Library; the model includes:

1. a continous-nonlinear friction model (LuGre) 2. a model able to simulate mechanical efficiency 3. an equivalent gear train inertia model 4. a model of torsional flexibility, damping and backlash 5. an ideal reducer model 1- Friction Model: Normally, friction is mathematically described as a discontinous function of speed; this description replies exactly the macroscopic aspect of the phenomenon and make possible the simulation of stiction, coulomb and viscous friction. But a more accurate description of friction cannot neglect the microscopic dynamics involved, so the correspondent mathematical model should be continous. Such a model presents these advantages:

friction is a dynamic phenomenon, also when it’s macroscopically static it describes accurately and continously the passage from a microscopic motion

(pre-sliding), where the relative sliding is determining, to a macroscopic one, influenced only by relative speed.

The model adopted is so called LuGre; it represents a good compromise between the precision of description and the numerical complexity. It’s based on the following set of equations:

++=

−+=

−=

−

qDdtdzz

eqg

zqg

qq

dtdz

a

vqcsc

SS

&

&

&

&&

&

10

/

0

)()()(

σστ

τττ

σδ

where sτ is stiction torque, cτ coulomb torque, 0σ and 1σ are two numerical parameters of the model (normally the first should be very big and the second next to zero), Sv is so called Stribeck velocity, D is the viscous factor, Sδ is parameter tipically set to 2, q is the rotational angle and z is the state variable of the model. In practice, z represents the mean deflection of ideal brushes, characterized by a certain stiffness. The parameter assignement requires attention: for example, the bigger will be 0σ , the slower will be the simulation (but more accurate!). 2-Mechanical Efficiency: This model is useful to concentrate in the macro-parameter efficiency all the non-linearities and dissipations neglected by the other models. Use it only if precise value of efficiency are available; sometimes a proper value for efficiency can substitute the effect of damping. 3-Gear Train Inertia: Remember that the equivalent gear inertia value must be referred to the correct side of reducer, because it changes with the square of the reduction ratio.

4-Torsional Flexibility, Damping and Backlash:

Torsional flexibility and damping imply that torque transmitted to load is given by:

[ ])_()_(__ lmellmellmll qredrqDredqrqKredrredr && −+−== ττ , where r_red is the reduction ratio, Kel is gear stiffness, Del gear damping, qm and ql, respectively, motor and load angular position. Also the simulation of backlash is important in robotic applications, because, if excessive, baklashes risk to compomise the positioning accuracy. In order to simulate backlash, numerical solver must be able to manage discrete states, corresponding to the situations of contact (in both directions) and separation. 5-Ideal Reducer Model: It simply increases the torque of a r_red factor and decrease the angular position of the same factor.

The Dymola transmission model of gear train includes also the load position encoder, which is simply implemented as an ideal analog sensor.

Gear train

Mechanical chain: SPIDER mechanical chain model has been realized making use of the Modelica Multi-body objects; the result is a 7 d.o.f rigid multi-body chain, that offers also the possibility of simulating the presence of payload. The “standard” SPIDER mechanical chain is an open loop, because it’s assumed that robot is in free motion condition. Anyway, assuming a flat wall as environment, both elastic or perfectly stiff, it has been possible to manage contact situation (closed loop chain). In Dymola/Modelica a generical multi-body chain is defined starting from the positioning of an inertial system; every rigid body is characterized not only by its inertial properties (mass, position of center of mass, inertia tensor) but it presents also two mechanical flanges, called “a” and “b”, which represent the interfaces that allow bodies to exchange forces and torques. A generic 3D mechanical flange is represented by a cartesian tern; thus it’s possible to build kinematic and dynamic constraints acting from the flange “b” of a body and the flange “a” of another body. For example in a robotic mechanical chain, the typical constraint is represented by the so called Revolute Joint. This kind of constaint forces body to rotate relatively around an axis defined by user, coherently with the angular position output of a Rotational element (the transmission). The operation of linking a Multi-body flange with another causes the two reference systems to become coincident. The complete definition of a 3D rigid body (“Body2”, with two frames) in Modelica/Dymola environment requires the assignment of the following parameters:

r[3] : Position vector from the origin of frame_a to the origin of frame_b. rCM[3]: Position vector from the origin of frame_a to the center of mass. m: mass of body Iij: the six independent component of inertia tensor, evaluated having assumed as pole

the center of mass and coordinate directions the aces of frame_a

Denavit-Hartenberg structure of SPIDER mechanical chain

According to Dymola/Modelica convention, every frame present in the mechanical chain is positioned, in home configuration, as the reference one (ISS_truss_PAS). This is a great difference in respect to DH convention, which always assumes z-axis as rotational axis, orienting the frames in opportune way. The passage from the DH convention to Dymola one obliges to transform some data (the position of the center of mass and the components of the inertia tensor) through rototranslational operators:

=

10003332313

2232212

1131211

tRRRtRRRtRRR

C DymDH

DHCM

DymDH

DymCM rCr ~~ =

TDymDHDH

DymDHDym RIRI ,=

being 1,~ Dym

CMDym

CM rr = The Dymola scheme shows all the components we have spoken about; the interfaces on the left side represent the connections to the gear trains (1D Rotational elements). SPIDER multi-body chain, for free motion operations This multi-body chain can be used to simulate contact/proximity operations, making use of one of these two interaction models:

Elasto-viscous environment:

This kind of model represents a good choice when the resultant compliancy of end-effector and environment (which are equivalent to two springs in series) cannot be neglected. Anyway, the bigger is the stiffness (Ke) the slower is the simulation (the DAE system

becomes very stiff). Assuming that the environment may be represented as a flat wall (in general this is true locally), such a model can be expressed as follows:

( )

=−−−=

0c

seevesseeec

fnvKnrrKf ro

rro

rr

if ( )( )

≥−<−

00

sseee

sseee

nrrKnrrKr

orr

ro

rr

where Ke and Kve are, respectively, environment stiffness and damping, snr is the local normal to surface, eerr and eevr are the position and velocity of end-effector, referred to inertial system, and sr

r is the position of a generic point belonging to the planar surface. Finally, cf is the reaction force normal to the surface, acting on the robot. The two situations described above represent contact and separation; the commutation between them allows to manage collision, for example.

Perfectly stiff environment:

Often, the resultant stiffness of end-effector and environment is so high (more than 10^6 N/m) that only a perfectly stiff environment model can be used. However, this model is useful also when the determination of Ke and Kve is not possible. Differently from elastic model, the assumption of rigid environment doesn’t represent a dynamic law existing between reaction forces and position, but a kinematic constraint (closed loop chain). So a lagrangian approach to the problem is necessary: the problem is the determination of the Lagrange multipliers, i.e. the necessary reaction forces needed to avoid the movements in the constrained directions. This approach, for a rigid n d.o.f. manipulator in absence of gravity, can be synthetized through the following equations:

=−=+

0)(),()(

qCqqnqA cττ&

,

where q is the vector of n joints parameters, A(q) represents the inertia matrix in joints space, ),( qqn & is the centrifugal and Coriolis term, τ is the vector of torques applied to joints, while cτ is formed by the reaction torques. )(qC is the constraint matrix, built using m holomic and time-invariant constraints:

=

=

0),...,(

0),...,(

1

11

nm

n

qqC

qqCM

Defining the jacobian matrix of constraints:

∂∂

∂∂

∂∂

∂∂

=

n

mm

n

q

qC

qC

qC

qC

CL

MLM

L

1

1

1

1

the reaction torques are simply given by:

λτ Tqc C= , where

=

mλ

λλ M

1

is the Lagrange multipliers vector.

Both interaction models give the three components of reaction force (referred to inertial system) as output, so an “ideal force sensor” is already included.

SPIDER Joint Independent Control The seven degree of freedom SPIDER model allows user to simulate the dynamic behaviour of this manipulator when it’s controlled through the traditional joint independent motion control (P_PI regulator on each axis). The robot is supposed to operate in microgravity condition, so gravity acceleration value, in the mechanical chain, is set to zero. Each regulator requires the joint position set-point as input, which can be conveniently provided by the block called TRAJECTORY. SPIDER Joint Independent Control model is formed by the following modules:

SPIDER elettro-mechanical model Seven P_PI cascade controllers Trajectory module

SPIDER Joint Independent Control model P_PI Cascade Controller: The P_PI cascade controller, taking advantage of the presence of two resolvers for each joint (motor and load side), guarantees a very good joint control; it’s formed by an inner loop (PI), projected for motor velocity control, and by an outer loop (P), destined to control joint position. It’s also present a velocity feed-forward in the inner loop (Kff is the gear ratio):

P_PI control scheme The real controller is digital, with 1ms of sampling frequency, and provides saturation limits. Simulation speed requirements, at the present moment, oblige to use an analog version of regulator, which is represented in the following Dymola scheme:

Dymola P_PI control The Dymola model has been equipped of anti-windup compensation. The control module accepts as inputs the load position set-point (“q_ref”), the load and motor angular positions measured by their relative encoder (“q” and “q_m”). Velocity error is computed starting from “q_m” and “q” with numerical derivation, providing a low-pass filter dynamic:

)(1

)( sEsT

sspdEder+

=

where E(s) is the Laplace transform of position error and Tder is the derivative time of the filter.

The output of controller represents the current set-point for NRPI module, and it’s a voltage fraction of Vci, i.e. the power supply voltage range of amplifier. Trajectory module: It implements an interpolator in the joint space, which is able to generate each of the seven joint trajectories according to a trapezoidal profile of speed:

Trapezoidal profile of speed: kinematic law Dymola scheme of joints interpolator Trajectory module requires as inputs, for each of the seven joint, the percentage of Vmax and Amax to be exploited by the kinematic law. Vmax [rad/s], Amax [rad/s^2] and the joints limits too must be specified as parameters.

MODEL DATA Kinematic parameter of the multi-body chain: Parametri D-H Link a α d 1 0 2/π 0.2085m 2 0 2/π 0.165m 3 0 2/π 0.65m 4 0 2/π 0.165m 5 0 2/π 0.6m 6 0 2/π 0 7 0 0 0

Inertial parameters of SPIDER mechanical chain: Link 1:

=

100000010100

2085.0010

DymDHC

D-H Dymola Mass [Kg] 16.535 C.M. position [m] [0,-0.047,0.036] [0.1615,0.036,0] Ixx [Kgm^2] 0.1665 0.105 Iyy [Kgm^2] 0.105 0.098 Izz [Kgm^2] 0.098 0.1665 Ixy [Kgm^2] 0.005 -0.01827 Ixz [Kgm^2] -0.00944 0.005 Iyz [Kgm^2] -0.01827 -0.00944

Joints Limits Joint Stroke 1 +-180° 2 +-180° 3 +-180° 4 +-180° 5 +-180° 6 +-120° 7 +-180°

Frame_b position referred to frame_a (Dymola)

Link rr

[m] 1 [0.2085,0.165,0] 2 [0.65,0,0] 3 [0,0.165,0] 4 [0.6,0,0] 5 [0,0,0] 6 [0,0,0] 7 [0,0,0]

Link 2:

−=

1000000100100100

DymDHC

D-H Dymola Mass [Kg] 13.43 C.M. position [m] [0,-0.008,0.310] [0.310,-0.008,0] Ixx [Kgm^2] 0.34145 0.0239 Iyy [Kgm^2] 0.335 0.335 Izz [Kgm^2] 0.0239 0.34145 Ixy [Kgm^2] 0 -0.02106 Ixz [Kgm^2] 0 0 Iyz [Kgm^2] -0.02106 0 Link 3:

=

1000000101000010

DymDHC

D-H Dymola Mass [Kg] 4.31 C.M. position [m] [0,0,0.037] [0,0.037,0] Ixx [Kgm^2] 0.0132 0.00718 Iyy [Kgm^2] 0.00718 0.0109 Izz [Kgm^2] 0.0109 0.0132 Ixy [Kgm^2] 0 7.195e-4 Ixz [Kgm^2] 0 0 Iyz [Kgm^2] 7.195e-4 0 Link 4:

−=

1000000100100100

DymDHC

D-H Dymola Mass [Kg] 15.25 C.M. position [m] [0,-0.0075,0.267] [0.267,-0.0075,0] Ixx [Kgm^2] 0.428 0.044 Iyy [Kgm^2] 0.395 0.395 Izz [Kgm^2] 0.044 0.428 Ixy [Kgm^2] 0 -0.0587 Ixz [Kgm^2] 0 0 Iyz [Kgm^2] -0.0587 0 Link 5:

=

1000000101000010

DymDHC

D-H Dymola Mass [Kg] 1.0425 C.M. position [m] [0,-0.001,-0.007] [-0.001,-0.007,0] Ixx [Kgm^2] 0.003345 0.001624 Iyy [Kgm^2] 0.001624 0.00369 Izz [Kgm^2] 0.00369 0.003345 Ixy [Kgm^2] 0 -4.095e-4 Ixz [Kgm^2] 0 0 Iyz [Kgm^2] -4.095e-4 0 Link 6:

−=

1000000100100100

DymDHC

D-H Dymola Mass [Kg] 5.787 C.M. position [m] [0,-0.005,0.052] [0.052,-0.005,0] Ixx [Kgm^2] 0.0194 0.00846 Iyy [Kgm^2] 0.0191 0.0191 Izz [Kgm^2] 0.00846 0.0194 Ixy [Kgm^2] 0 -0.00132 Ixz [Kgm^2] 0 0 Iyz [Kgm^2] -0.00132 0

Link 7:

=

1000010000100001

DymDHC

D-H Dymola Mass [Kg] 0.98 C.M. position [m] [0,0,0.137] [0.137,0,0] Ixx [Kgm^2] 9.18e-4 0.00193 Iyy [Kgm^2] 9.18e-4 9.18e-4 Izz [Kgm^2] 0.00193 9.18e-4 Ixy [Kgm^2] 0 0 Ixz [Kgm^2] 0 0 Iyz [Kgm^2] 0 0 Parameters of motors (Etel TSEU MP01-001): Motor performances (except integrated joint 5): DC voltage 28 V Number of phase 2 Number of pole pairs 7 Nominal speed 955 rpm Maximum speed 2800 rpm Nominal power 35 W (at 955rpm) Motor data: Joint J [Kgm^2] Viscous fr.

[Nms/rad] Phase Res. [Ohm]

Phase Induc. [H]

Torque const Kt [Nm/A]

1 1.724e-5 5e-4 1 0.0013 0.11 2 1.724e-5 5e-4 1 0.0013 0.11 3 1.724e-5 5e-4 1 0.0013 0.11 4 1.724e-5 5e-4 1 0.0013 0.11 5 1.65e-4 5e-4 5.7 0.03 0.48 6 1.724e-5 5e-4 1 0.0013 0.11 7 1.724e-5 5e-4 1 0.0013 0.11

Transmissions data (motor side): Joint Tr.ratio J[Kgm^2] Viscous fr

[Nms/rad] Stiffness [Nm/rad]

Damping [Nms/rad]

Stiction torque [Nm]

Coulomb torque [Nm]

1 800 2.17e-5 5e-4 0.192 3.84e-4 0.02 0.01 2 800 2.17e-5 5e-4 0.192 3.84e-4 0.02 0.01 3 480 2.17e-5 5e-4 0.317 6.34e-4 0.02 0.01 4 737.6 2.17e-5 5e-4 0.212 4.24e-4 0.02 0.01 5 160 5.81e-5 5e-4 0.976 1.952e-3 0.02 0.01 6 748.8 2.17e-5 5e-4 0.208 4.16e-4 0.02 0.01 7 508.8 1.247e-5 5e-4 0.0494 9.88e-5 0.02 0.01 Current controller data (NRPI): Joint Vlim [V] Vci [V] Imax [A] Prop.gain K Int.time

[ms] 1 28 28 5 20 1.3 2 28 28 5 20 1.3 3 28 28 5 20 1.3 4 28 28 5 20 1.3 5 28 28 5 30 5.26 6 28 28 5 20 1.3 7 28 28 5 20 1.3 Joint control data (P_PI): Joint Kpp Kpv Kiv Kff 1 1000 0.1 0.22 800 2 1000 0.1 0.22 800 3 1000 0.1 0.22 480 4 1000 0.1 0.22 737.6 5 1000 0.1 0.75 160 6 1000 0.1 0.75 748.8 7 1000 0.1 0.75 508.8

The Operational Space Hybrid Control The philosophy: A generic joint control isn’t naturally compatible with the requirements imposed by constrained motion, which implies at the same time position and force control. Moreover, the space in which the operations of the manipulator are planified, i.e. the operational space, would differ from the space in which the control is realized, making necessary the use of computationally complex kinematic and dynamic transformations. The dynamic models at joints level provides a description of the dynamic interactions between the joints of the robot; however, what truly interests in the project of force control algorithms is the description of the dynamic interaction between end-effector and object manipulated or part in contact. All reasons above have induced the development of operational space control algorithms, based on an unified approach to the dynamics of motion and force exchanged with environment. The basic idea consists in the control of motion and interaction through the use of forces acting directly at end-effector level, forces that are produced exerting opportune joints torques. So, the consequence is that, theorically, every joint should be controlled in torque and not more in position. The operational space: The operational space is defined by the m independent parameter necessary to describe end-effector position and orientation; if we consider a non-redundant manipulator, m=n , where n is the number of d.o.f. of the manipulator. Tipically, the m operational parameters are represented by the three wrist Cartesian coordinates plus the three Euler angles. But in technical applications, rotations are described making use of certain parametrizations (Euler, Cardano, etc.); such parameters don’t constitute a vector, from a geometrical point of view, so the operational forces directed as the rotation parameters are not coinciding with moments, and they vary with the parametrization used. Because in constrained motion operations homogeneity between operational forces and coordinates is fundamental, this homogeneity is assured making use of the linear law existing between the derivates of the angular parameters and the angular speed vector, changing from one parametrization to another; so:

)()()( 0 qJxEqJ = , where J(q) it’s the jacobian matrix. The matrix )(0 qJ is called “basic Jacobian”, it’s independent from parametrization and it allows to write a differential kinematic law between joint speed vector and operational speed.

So, if

=φp

x is the vector of operational coordinates,

=ωp

v&

doesn’t coincide with x& .

For example, using Euler parametrization, we have:

φφφϑϑϕϕϑϕϕ

ω && )(cos01

sinsincos0sincossin0

T=

−= so xEx

TIp

v &&&

)()(0

0φ

φω=

=

= .

The contact/interaction operations previews the motion control along certain direction and force control along direction orthogonal to them. The constrained motion control can be realized passing through the definition of particular generalized task specification matrices, one for motion control and one for force control.

Definition of motion and force control directions In order to define them, let’s consider the case in which no constrained rotations are implied; so, let df be a unite vector, in the frame of reference ),,,( 0000 zyxΟℜ , directed as the force that is to be applied to the end-effector. The positional freedom, if any, of the constrained end-effector will therefore lie in the subspace orthogonal to df . A convenient coordinate frame for the description of tasks involving constrained motion operations is a coordinate frame

),,,( ffff zyxΟℜ obtained from 0ℜ by a rotation transformation described by fS matrix such that vector fz is aligned with df . For tasks where the freedom of motion (displacement) is restricted to a single direction orthogonal to df , one of the axes fxΟ or fyΟ will be selected in alignment with that direction. Let us define, in the coordinate fℜ , the position specification matrix

=Σ

z

y

x

f

σσ

σ

000000

where zyx σσσ ,, are binary numbers assigned the value 1 when a free

motion is specified along the axes fff Ozyx ,,ΟΟ , respectively, and zero otherwise. A non-zero value of zσ implies a full freedom of the end-effector position. The directions of force control are described by the force specification matrix fΣ associated

with fΣ and defined by ff I Σ−=Σ , where I designates the 3x3 identity matrix. Let us now consider the case where the end-effector task involves constrained rotations and applied moments. Let dτ be the vector, in the frame of reference 0ℜ , of moments that are to be applied by the end-effector, and ),,,( ττττ zyxΟℜ be a coordinate frame obtained from 0ℜ by a rotation τS that brings τz into alignment with the moment vector dτ . In τℜ , the rotation

freedom of the end-effector lies in the subspace spanned by ττ yx , . To a task specified in terms of end-effector rotations and applied moments in the coordinate frame τℜ , we associate the rotation and moment specification matrices τΣ and τΣ , defined similarly to fΣ and fΣ . For general tasks that involve end-effector motion (both position and orientation) and contact forces (forces and moments) described in the frame of reference 0ℜ , we define the generalized task specification matrices

ΣΣ

=Ωτττ SS

SST

ffTf

00

and

ΣΣ

=Ωτττ SS

SST

ffTf

00

The end-effector dynamics: The equations describing the dynamics of a rigid n d.o.f. manipulator in absence of gravity and external forces are:

Γ=+ ),()( qqnqqA &&& where A(q) is the inertia matrix, ),( qqn & is the centrifugal and Coriolis term and Γ is the vector of torques applied to joints. Starting from the differential kinematic laws, of the first and second order, respectively

qqJx && )(= and qqqJqqJx &&&&&&& ),()( += , and from the definition of manipulator kinetic energy in

the operational space xxxE TK && )(

21

Λ= , where )(xΛ is the inertia matrix in the operational

space, it can be demonstrated that the same equations may be rewritten as:

Fxxxx =+Λ ),()( &&& µ being 1)()()()( −−=Λ qJqAqJx T , xqJqqJxqqnqJxx T &&&&& 1)(),()(),()(),( −− Λ−=µ , vector of centrifugal and Coriolis forces, Γ= −TqJF )( , vector of operational forces. Considering the basic Jacobian )(0 qJ , we can define:

)()()()( 10 xExxEx T −− Λ=Λ , where 1

000 )()()()( −−=Λ qJqAqJx T , and FxEmf

F T )(0 =

= ,

being f and m the vectors of forces and moments applied to the end-effector. Therefore we can write the equations of the dynamics in terms of linear and angular velocities:

000 ),()( Fvxvx =+Λ µ& Control structure: Considering contact operations, the same equations should be completed as follows:

000 ),()( FFvxvx S =Ω++Λ µ&

where SFΩ represents the reaction force acting on the end-effector. This system, describing the end-effector dynamics in the operational space, is highly nonlinear and strongly coupled, so the main goal of an operational space control is to make the closed loop system linear and decoupled in the directions of motion and force control. The unified approach for the end-effector nonlinear dynamics decoupling, for motion and force control is achieved through the structure:

activemotion FFF +=0 being

Ω+ΩΛ=+ΩΛ=

desiredactiveactive

motionmotion

FFxFvxFxF

*0

0*

0

)(ˆ),(ˆ)(ˆ µ

while *

motionF and *activeF represent the vectorial inputs of the decoupled system; )(ˆ

0 xΛ and ),(ˆ 0 vxµ represent, respectively, the estimates for inertia matrix and centrifugal/Coriolis forces

vector. The torques required at joint level in order to produce the operational forces 0F are

00 )( FqJ T=Γ .

Prof. O. Khatib has demonstrated that, in presence of perfect estimates, i.e. 0000 ˆ,ˆ µµ =Λ=Λ , the nonlinear, decoupling control structure makes the end-effector equivalent to a single unit mass 0mI moving in the 0m -dimensional space. Thus the resultant closed loop system, become linear, is described by the two decoupled systems:

( ) ( )

+Ω=+ΩΩ=Ω

*

*

activedesiredS

motion

FFFvFv

&

&

Operational hybrid control scheme Redundant manipulators: In presence of redundant manipulators the problem is much more complicated: the configuration of a redundant manipulator cannot be specified by a set of parameters that only describes the end-effector position and orientation. An independent set of end-effector

configuration parameters, therefore, does not constitute a generalized coordinate system for a redundant manipulator, and the dynamic behaviour of the entire redundant system cannot be represented by a dinamic model in coordinates only of the end-effector configuration. The inertia matrix in the operational space is then interpreted as a pseudo-kinetic energy matrix. Moreover, the relation FqJ T )(=Γ becomes incomplete, since, for a given configuration, an infinite number of elementary displacements of the redundant mechanism may occur, without altering the end-effector configuration. These displacements correspond to the motion in the null space of the generalized inverse of Jacobian matrix; also a null space associated to the Jacobian matrix transpose exists: when the manipulator is not in static equilibrium condition, an infinity of the torque vectors that can be applied to the joints without influencing the resultant force on the end-effector is present. Therefore, considering the null space of the joint torques, the relationship between forces applied to the end-effector and torques exerted on the joints can be written in the general form

[ ] 0# )()()( Γ−+=Γ qJqJIFqJ TTT

where 0Γ is a vector of arbitrary torques, which is projected into the null space of the transpose of the Jacobian matrix, while )(# qJ T is the generalized inverse of the transpose of the Jacobian matrix. Among all the infinite generalized inverse matrices of the Jacobian, only one is consistent with system dynamics, and it’s given by the expression:

)()()()( 1 qqJqAqJ T Λ= − An alternative to this approach, which may appear too abstract, consists in leading back the manipulator to non-redundancy conditions increasing its degrees of freedom of a number equal to n-m. In practice, if we consider SPIDER robot arm, a further coordinate can be introduced, representing, for example, the geometric angle included between the plane surface conteining the shoulder, the elbow and the wrist of the arm, and the plane surface generated by the wrist-shoulder straight line and the z-axis. This approach, based on the specification of the posture, allows to exploit intuitively the redundancy of robot in order to avoid obstacles, for example. PD operational space motion control: The project of the motion control law is free, but it’s a frequent choice the adoption of a proportional-derivative (PD) regulator. Let us consider the equations of dynamics for a rigid n d.o.f manipulator, in absence of gravity:

Fxxxx =+Λ ),()( &&& µ , being xqJqqJxqqnqJxx T &&&&& 1)(),()(),()(),( −− Λ−=µ . Considering the motion control only, the operational space control structure will imply:

),()( * xxFxF motion &µ+Λ= so, making a comparison, we’ll have *

motionFx =&& . In other words, actuating a perfect compensation (i.e. the estimates of the dynamic parameters are exact and the system is rigid,

is not affected by friction, backlash or other phenomena not considered), the controlled system results linear and decoupled relatively to the input *

motionF ; so, the i-eth component of *

motionF influences only ix . From a mathematical point of view, the behaviour of the closed-loop system is equivalent to a double integrator: the input *

motionF is integrated twice obtaining x& and x as outputs. Adopting the stabilizing PD control law

++=+−−=

dPddd

dPmotion

xKxKxrrxKxKF

&&&

&*

where Kp and Kd are simmetric positive definite matrix, we have:

)()(* xxKxxKxxF dddPdmotion &&&&&& −+−+== so, calling e the positional error vector, we can write 0=++ eKeKe Pd &&& . This means that the asymptotic stability of system is assured, as the convergence to zero of the error too. Kp and Kd matrices can be simply assigned as follows:

=

nm

n

n

PK

2

22

12

...00......000...00...0

ω

ωω

=

nmm

n

n

dK

ωξ

ωξωξ

2...00......000...200...02

22

11

this approach requires the definition of natural frequency and damping for each of the m error dynamics. It can be observed that acceleration compensation is not really necessary, requiring only a zero steady-state error : in fact, in presence of a trapezoidal profile of speed in Cartesian space, the acceleration compensation contribution is zero after the transient. Force control scheme: A suggested force control scheme is shown in the following picture:

Force control scheme

The Force Control module implements the basic force control law; the controlling vector (dimensionally a force) is then projected into the subspace orthogonal to motion control through the well known matrix Ω . A damping term, given by the expression

xKF vfdamper &ΩΛ= 0ˆ

is then subtracted to the control signal, obtaining the resultant activeF controlling vector. This last term becomes very important during impact/contact transient, where the end-effector components of speed in the directions of force control are not negligible, since it allows the fast dissipation of the kinetic energy involved in the impact. As in motion case, the force control law may be freely chosen, but simply physical considerations must be taken into account:

1. A derivative term is strongly contraindicated, because of the relevant noise present at sensors output.

2. A simply proportional control law, like )( cdfdactive FFKFF −Ω+Ω= , cannot be adopted, since the dynamic relationship between desired force and contact force, in the ideal circumstance of rigid manipulator and rigid environment, would be algebraic; this means that the true system behaviour could be determined by the unknown dynamics, implying stability problems.

3. A simple integral control law allows to “cut” the unkown dynamics. Furthermore, it can be demonstrated that an integral force control law assures an increasing stability with the environment stiffness.

These considerations suggest the adoption of an integral or PI (proportional-integral) force control law; in this last case, a careful calibration of regulator is required in order to avoid stability problem.

3 d.o.f. SPIDER model: operational space motion control This semplified version of SPIDER gives to the user the possibility of testing the performances of the operational space motion control, moving the first three joints of the robot (excluding the redundant one). That concurs to move the end-effector of SPIDER, like if it was a material point, along a trajectory of the cartesian space which must be included in the operational envelope of the robot. The trajectory has to be planned in order to avoid kinematic singularities. The model is constituted by:

SPIDER 3 d.o.f. elettro-mechanical model Operational Space Motion Control blocks

Operational Space Motion Control : Dymola implementation The SPIDER 3 d.o.f. elettro-mechanical model has been built using the following objects:

SPIDER mechanical chain (free motion) plus optional payload: for more details see the relative description on page 9.

WristBlock SpiderTorqueServoArray

Here we report the description of the last two elements: WristBlock: This module is used to maintain the revolute joints no 3,5,6,7 (redundant) fixed, allowing the reduction of the number of degree of freedom to three. This is obtained through three rotational constraints, which imply “home” configuration for each of the interested joints. SpiderTorqueServoArray: The module provides three actuated joints (1,2,4), according to an open-loop scheme where the required torques are converted into current set-point. Therefore, SpiderTorqueServoArray accepts torque reference for joint 1,2,4 and it gives back load measured position for the same joints. It’s ready to be connected to SPIDER multi-body chain. Each actuated joint model provides input conversion, current regulator (NRPI module), two-phase DC brushless motor and transmission model, which we have already described. The conversion factor is given by the following expression:

refT

ciref T

IredKrV

Imax_

=

For the meaning of the symbols, see the description of the modules involved; remember that

refI is physically a voltage, so the current maxI flows in motor windings when ciref VI = . Actuated joint model The operational space motion control has been realized starting from the following blocks:

Inertia_Matrix Omega Lambda Jacobian DirectEEKin Motion_Control CartesLinTraj

No gravitational, Coriolis or centrifugal compensation is provided, because SPIDER is designed to operate at 0-g condition, at very low joint speed. Inertia Matrix: The Inertia Matrix module allows the calculation of Jacobian and inertia matrix, relative to operational space, starting from the position values of the actuated joints (q1,q2 and q4). The block gives back the columns of the two matrices as output. The procedure can be subdivided in these steps:

1. Define the rotation and rototranslation matrices )(1i

ii qR − (3x3) and )(1

iii qA − (4x4),

which describe the passage from one D-H tern to the next. Besides, the constant translation matrices i

CGA , linking the position of the origin of tern i to the position of the center of mass of link i, has to be defined too.

2. Using such matrices, compute the position of the origins of the seven D-H terns, the position of the center of mass of each link and, finally, the unite vectors of the seven joint axes projected in the inertial system:

01

12

110111

~)()...(]1,[~ pAqAqApp iCGi

ii

Ti

CGi

CG −−

−−−− ==

01211

011 )()...( zqRqRz i

iii

rr−

−−− =

3. Compute the Jacobian matrix (3x3) through the expressions:

[ ]

=→

−×=−×=−×=

421421

334

112

001

),,()()()(

jpjpjpqqqJppzjpppzjpppzjp

ee

ee

ee

rrr

rrr

rrr

where eepr represents wrist / end-effector position.

4. Compute the inertia matrix, relative to joint space, of the rigid manipulator (the equivalent load inertia takes account also of motor and transmission inertia), using the expression:

( ) ( ) ( ) ( ) ( ) ( )[ ]∑=

+++=7

10000)(

i

miO

Tmimimi

miTmiO

miP

TmiPmi

LiO

TiiLi

iTLiO

LiP

TLiPLi JRIRJJJmJRIRJJJmqA

where:

o Lim is the mass of link i o ]0...0...[ 1

LiPi

LiP

LiP jjJ = with )( 11 −− −×= j

CGLij

LiPj ppzj vrr

o [ ]0...0...1LiOi

LiO

LiO jjJ = with 1−= j

LiOj zj r

o iLiI is the inertia tensor of link i, evaluated assuming like pole the center of

mass and coordinate directions the aces of tern i o mim is the mass of the motor of link i o miR0 is the rotation matrix describing the orientation of the axis of rotor i o miI is the inertia tensor of the motor i, referred to barycentric axes

Since the inertial data take already account of the inertial properties of motor, it’s sufficient considering the moment of inertia of motor and transmission relative to their rotation axis, referred to load side. Thus:

[ ]0...0...1LiOi

LiO

miO jjJ = with

=≠=

−

−

1

1 ,

ii

REDLiOi

jLiOj

zrjijzj

r

r

Remembering that z axis of tern i , according to D-H rules, is directed as the rotation axis of joint i, assuming that all the rotational masses of the servo-mechanism are aligned with this axis, we can assume: 0=Lim 1−= i

OmiO RR

+=

itr

im

mi

III

00000000

5. Knowing the simmetric positive definite matrix A(q), compute the inertia matrix referred to operational space )(xΛ using the expression

( ) ( )11 ~)(~)( −−=Λ JqAJxT

where 1~ −J is a pseudo-inverse based on the least squares fitting, which prevents malfunctions of the simulator when singularities are crossed. It can be calculated using the expression TT JIkJJJ =+− )(~

321 , where k is a

parameter that needs opportune setting; the situation k=0 implies 11~ −− = JJ .

Omega: The module called Omega simply represents the generalized task specification matrix Ω . In a simple motion control scheme, the Ω matrix is inserted only for completeness and it’s set equal to (3x3) identity matrix by default. Lambda: Lambda module performs the product of )(xΛ and *

motionFΩ obtaining the required motionF acting on the end-effector. Jacobian: Jacobian module acts the product of motionF and TJ , transforming the required forces acting on the end-effector into the required torques acting at joint level. DirectEEKin: This block simply implements the direct kinematic algorithm for the position of the end-effector; it accepts q1,q2,q4 as input and it gives back (x,y,z) coordinates. Motion Control: The block Motion Control implements a general PID control law, based on three distinct SISO regulators.

Decoupled Motion Control

CartesLinTraj: CartesLinTraj generates a linear trajectory connecting two cartesian points: (Xi,Yi,Zi) to (Xf,Yf,Zf). The kinematic law is determined by a trapezoidal profile of speed; the percentage of Vmax and Amax to be exploited by the kinematic law, Vmax , Amax must be specified as parameters.

3 d.o.f. SPIDER model: operational space hybrid control As in the operational space motion control simulator, this model applies the operational space control to a reduced 3 d.o.f. version of SPIDER robotic system. The interaction models provided (viscous-elastic and rigid flat surface) allows the simulation of the end-effector creeping on the surface along a desired planar trajectory, while a desired force is exerted on it. Moreover, impact/contact transients can be reproduced. This simple interaction simulation makes possible the understanding of the most typical phenomena involved in contact situations (transmissions of vibrations, friction cycles, contact instability, contact bouncing, etc.). The model is constituted by:

SPIDER 3 d.o.f. elettro-mechanical model Environment model Operational Space Motion Control blocks Operational Space Force Control blocks

Operational Space Hybrid Control : Dymola implementation The operational space force control scheme is mainly constituted by these elements:

Force set-points sources ForceControl Damper

Force set-points sources: The three component Fx,Fy,Fz of desired contact force are generated according to three step functions, but more complex functions may be used. ForceControl: The ForceControl block implements a general PI (proportional-integral) control law, which can be expressed as:

dtFFKFFKFF cdifcdfdactive )()( −Ω+−Ω+Ω= ∫ where dF is the desired contact force and cF is the measured contact force. As in motion control, the system decoupling allows to build such a MIMO (multi-input, multi-output) control structure as an union of three distinct SISO regulators. Decoupled Force Control Damper: Damper module, according to operational space force control architecture, is useful in impact/bouncing situations because it induces fast kinetic energy dissipation, making possible an easy stabilization of the end-effector onto the surface. As we have already seen, it implements the expression:

xKF vfdamper &ΩΛ= 0ˆ

which acts as a subtracting term on the final controlling force. Damper Module

TESTED CONTROL PARAMETERS: Motion Control:

=

=355000355000355

000000

2

2

2

nz

ny

nx

PKω

ωω

)/19( sradn ≅ω

=

=

260002600026

200020002

nzz

nyy

nxx

DKωξ

ωξωξ

)7.0( =ξ

msTder 1= Force Control:

=

=100010001

000000

fz

fy

fx

f

kk

kK

=

=ms

msms

TT

TT

ifz

ify

ifx

if

700007000070

000000

10=vfK , msTder 1=