6D Force and Acceleration Sensor Fusion for Compliant ... - CiteSeerX

Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems October 9 - 15, 2006, Beijing, China

6D Force and Acceleration Sensor Fusion for Compliant Manipulation Control Torsten Kr¨oger, Daniel Kubus, and Friedrich M. Wahl Institute for Robotics and Process Control Technical University of Braunschweig, Germany {t.kroeger, d.kubus, f.wahl}@tu-bs.de

Abstract— This paper focusses on sensor fusion in robotic manipulation: 6D force/torque signals and 6D acceleration signals are used to extract forces and torques caused by inertia. As result, only forces and torques established by environmental contact(s) remain. Beside an improvement of hybrid force/pose control behavior, an additional major benefit is that regular resetting/zeroing of force/torque sensors before free space/contact transitions can be omitted. All essential equations, transformations, and calculations that are required for this 6D fusion approach are derived. To highlight the meaning for practical implementations, numerous experiments with a six-joint Staeubli RX60 industrial manipulator are presented, and the achieved results are discussed.

I. I NTRODUCTION Force control in industrial manipulation is a widely discussed field. D. E. Whitney [1] and M. T. Mason [2] published early works on compliance and force control. Nowadays, the open literature provides three basic approaches: impedance control [3], parallel control [4] and force/position control [5]. These methods have existed since decades and their basic concepts do not change anymore. In spite of that in industrial applications, robotic manipulators are mainly used for pointto-point operations. Control units are not open for sensor integration, such that research institutes often replace the commercial control units by their own ones to perform experiments in control engineering. For a more flexible usage of manipulators, multi-sensor integration is essential and controllers have to be able to handle sensor-guarded and sensor-guided motion commands. Going further by enabling this, programming becomes more complex, such that non-experienced users would hardly be able to utilize the provided functionalities. To bypass this, research institutions work on novel task orientated programming concepts. Bruyninckx delivered significant articles about research in compliant motion specification [6]. Approaches on contact formation modeling during the force-controlled execution of robot tasks with different contact situations were published in [7] (exemplarily). To bring advanced multi-sensor concepts for robotic manipulation into practice is a tough challenge and should be one of the major aims of the research community. To achieve this, the fields of low level joint control, hybrid control (hybrid control in this context does not mean the handling of discrete and continuous signals, but the control of different physical quantities as position, force, velocity,

1-4244-0259-X/06/$20.00 ©2006 IEEE

2626

distance etc.), programming, and planning have to be led together, as exemplarily proposed in [8]. This paper focusses on force sensing and its consequences for the approaches mentioned above. The forces and torques measured by a force sensor are caused by environmental contact and by inertia. Forces and torques caused by inertia arise from acceleration and are composed of gravity, centrifugal forces, coriolis forces, and vis inertiae. Hence, the measured forces/torques f ts~ F = (f ts fx ,f ts fy ,f ts fz ,f ts τx ,f ts τy ,f ts τz )T consist of two parts: f ts ~

F =

f ts ~ iF

f ts ~ eF

+

f ts ~ iF

(1) f ts ~ eF

contains the non-contact forces/torques and represents forces and torques caused by environmental contact. This quantity is supposed to be controlled during compliant manipulation. Works on the usage of acceleration feedback to estimate f tse f~ = (f tse fx ,f tse fy ,f tse fz )T were recently published in [9], [10]. There an observer, which is fed with position, force, and acceleration signals, is used to estimate the contact force. The respective theory includes Kalman filtering and leads to benefits in force/pose control, which was demonstrated with convincing experimental results. In the paper on hand, the usage of an accelerometer measuring in six degrees of freedom (DOF) combined with a 6D force/torque sensor, which are both mounted on the end-effector, is proposed to calculate the forces caused by environmental contact f tse f~ and also the respective torques f tse~τ = (f tse τx ,f tse τy ,f tse τz )T to get f tse F~ . This improves force/pose control behavior and – which is a huge benefit for automatically generated robot programs – any zeroing/resetting of the force/torque sensor before free space/contact transitions can be omitted. Section II presents all required presuppositions, transformations, and equations for the calculation of the desired force/torque vector. How this concept is supposed to be integrated in manipulation control architectures is described by section III and to emphasize the practical relevance, results of numerous experiments are described in section IV. After interpretation of the results, the idea of using filter techniques was born; considerations on this are given in part V. II. C ALCULATING N ON -C ONTACT F ORCES /T ORQUES Fig. 1 shows the respective frame assignment. Both, force/torque sensor and acceleration sensor, are mounted on

object cog

~ = same frame as the measured accelerations as A as as as as as as T ( ax , ay , az , αx , αy , αz ) do. In the next step, we consider the mass distribution in front of the force/torque sensor. According to Fig. 1, the frame as Tcog is introduced. It is the reference frame for the object in front of the force/torque sensor and simultaneously the object’s center of gravity (cog). Its mass distribution leads to the inertia tensor cog I ǫ 3×3 , which refers to this frame.  cog  Ixx −cog Ixy −cog Ixz cog I =  −cog Iyx cog Iyy −cog Iyz  (5) cog −cog Izx −cog Izy Izz

as

Tcog

a-sensor

f/t-sensor

hand

Tas

hand

Tfts

R

hand

Ttask

task hand

robot

Thand

How the tensor elements are calculated can be found in any robotic textbook, e.g. [5]. Since the orientations of hand Tas and as Tcog are not necessarily the same, i.e. as ϕ ~ cog 6= ~0, we propose to calculate the required torques of inertia individually in order to keep the following calculations simple. Therefore, we introduce a simple rotation matrix r first.
r = rpy as ϕzcog , as ϕycog , as ϕxcog = (~rx ~ry ~rz ) (6)

world robot base world

Trobot Fixed transformations Variable transformations Kinematic forward transformation

Fig. 1. Frame assignments for the proposed force/torque calculation scheme.

R

the hand of the manipulator. hand Tf ts ǫ 4×4 is a homogenous transformation matrix determining the pose of the force/torque sensor frame with respect to (w.r.t.) the endeffector. hand Tas is the equivalent one for the accelerometer. A tool (in the following we call it object) is mounted in front of the two sensors, such that the user can specify an individual task frame hand Ttask depending on the current tool; all set points refer to hand Ttask . One supposition is that the measured forces and torques can be transformed into the same coordinate system. Hence, we transform the measured forces and torques from hand Tf ts to hand Tas by ft

f ts

Tas =

Tas ⇐⇒

hand

T−1 f ts ·

f ts ~ as

P

=

hand



R R

Tas

f ts

as

f ts

as

p~ ϕ ~

as

~τ

=

~ the Till here we know the acceleration signals as A, as ~ as cog force/torque signals F , the object’s center of gravity p~ , the torques of inertia J~ w.r.t. hand Tas translationally displaced into as Tcog , and the mass of the object m. Before deriving the equation for the calculation of asi F~ , we consider the simple two dimensional case. m, cogI


−1 rpy ϕzf ts ,as ϕyf ts ,as ϕxf ts · 
f ts ~ f ts as f ts as f ts as f ts T f+ × px , py , pz

y z

cog y

x as cog y

p

as



(3)

as

~τ



(4) 3×3 rpy(ϕx , ϕy , ϕz ) ǫ constitutes a rotation matrix 3 . computed by means of roll-pitch-yaw angles ϕ ~ ǫ Now the measured forces and torques refer to the

R

R

(2)

From f ts Tas we calculate f ts P~ as ǫ 6 , which describes the translational displacement f ts p~ as ǫ 3 and the rotational one f ts ϕ ~ as ǫ 3 (eqn. (3)). This vector can be used to compute the force/torque transformation:
−1 f ts as ~ f = rpy as ϕzf ts ,as ϕyf ts ,as ϕxf ts · f~

R

This matrix is used to calculate the torques of inertia for arbitrary axes on the base of the inertia tensor. Here we require the torques of inertia w.r.t. the axes of hand Tas translationally displaced into as Tcog . These are expressed by J~ ǫ 3 .    T cog  ~rx · I · ~rx Jx J~ =  Jy  =  ~ryT · cog I · ~ry  (7) Jz ~rzT · cog I · ~rz

R

2627

x as cog x

p

Fig. 2. Two dimensional assignment for the calculation of non-contact forces as f~ and torques as ~ τ. i

i

Here

as i fx

can be calculated by

as i fx

= −m ·as ax + m ·as αz ·as pycog

(8)

as i fy

can be computed analogously. By applying the parallelaxis theorem we get asi τz with as i τz

= m ·as ax ·as pycog − m ·as ay ·as pxcog

− Jz + m · as pxcog 2 +as pycog 2 ·as αz

(9)

as i

0 BB BB B ~ =B F BB BB 

−m

0

0

0

−m · bz

m · by

0

−m

0

m · bz

0

−m · bx

0

0

−m

−m · by

m · bx

0

0

m · bz

−m · by

−Jx − m · (b2y + b2z )

0

0

−m · bz

0

m · bx

0

−Jy − m · (b2x + b2z )

0

m · by

−m · bx

0

0

0

−Jz − m · (b2x + b2y )

The calculation of asi F~ for the three dimensional case is analogous to eqn. (8) and eqn. (9) and is given by eqn. (10). Due to a limited column width we substitute ~b =

as

p~

cog

(11)

in eqn. (10). Eying up to eqn. (1), our aim was to calculate f ts ~ f ts ~ as ~ e F . By knowing i F from eqn. (10), we can calculate i F by the following backward transformation:
f ts ~ as ϕzf ts ,as ϕyf ts ,as ϕxf ts ·asi f~ i f = rpy   
f ts f ts as f ts as ~ as cog as f ts as f × p ~ ~ τ = rpy , ϕ , ϕ · ϕ z y x i i
+ rpy as ϕzf ts ,as ϕyf ts ,as ϕxf ts ·asi ~τ (12) Now we have calculated the forces and torques that are T not  f ts ~ f ts f ts ~ f, ~τ . By caused by environmental contact F = i

i

i

simply using eqn. (1), we can use the counter part f tse F~ for force/torque control. In practice of course, force/torque control is not applied in the sensor frame but in the task frame, such that f tse F~ has to be transformed into the task frame by applying eqn. (4) (see also Fig. 3). III. C ONTROL A RCHITECTURE

As mentioned in section I, there are basically three different force/torque control concepts for robotic manipulation. This chapter describes, how the theory of section II can be integrated into a manipulation control architecture supporting implicit hybrid force/torque control. Fig. 3 depicts the relevant parts of the control structure. The user-given selection matrices S0 . . . Sn determine, which DOF is controlled by which controller, where n is the number of installed controllers. I=

n−1 X

Sl

ξii =

             

•

•

•

(13)

2628

0 :



task e Fi >

as

threshold e Fi

~ A

(10)




 

 ∧ sign htaske Fi i = sign taske Fi
∨ htaske Fi i < threshold e Fi ! f ts
∧ Fi < max Fi

else

(14)

considered; htaske Fi i is the user-given force/torque set point for DOF i. Eqn. (14) in plain text means, an element ξii of the available matrix Ξ2 becomes 1, if the direction of the current force/torque taske Fi equals the one of the set point htaske Fi i, the force/torque taske Fi is greater than the threshold value threshold e Fi , and the absolute value of the measured force/torque vector f ts Fi is lower than the maximum permax mitted sensor values Fi . If the absolute force/torque set task point h e Fi i is lower than the threshold value threshold e Fi , the available matrix element ξii is also set to one, in order to prevent uncontrolled motions. At this point we highly benefit from the elimination of non-contact forces and torques as described in section II:

l=0

I represents the identity matrix. In the special case of this paper, we only consider pose (S0 ) and force/torque (S2 ) control. S1 is reserved for velocity control. The elements of the diagonal matrices are sii ǫ [0, 1]. To inhibit force control in free space and to prevent from sensor overload, an available matrix Ξ2 (comp. [11]), which is also a diagonal matrix is embedded. Its elements are defined as can be seen ~ in eqn. (14). The vector threshold e F represents the force/torque threshold values to enable force control; taske Fi contains the transformed forces/torques in the task frame, whereas only forces and torques caused by environmental contact are

    1 :           

1 CC CC CC CC CC A

•

The measured forces/torques are independent from manipulator pose, speed, and acceleration. Resetting/zeroing the force/torque sensor before free space/contact transitions is not required anymore. Force/torque control behavior becomes improved, especially in two cases: 1) Force/torque control executed in non-static environments (e.g. conveyer belts, rotating plates). Vis inertiae forces, centrifugal forces, and coriolis forces are not taken into account anymore. 2) Disturbances caused by gravity during end-effector orientation changes do not occur anymore. Environmental contacts during highly dynamic motions can be detected.

If a DOF is supposed to be force controlled but still operates in free space, the Trajectory Planner 2 heads the DOF for contact. These DOFs are determined by the matrix complement Ξ2 = I − Ξ2 multiplied by the selection matrix S2 . Trajectory Planner 1 assures values for pose controlled DOFs.

Trajectory Planner 1

S0

-

X2

Transformation into task

Forward Kinematic

Transformation into hand

Inverse Kinematic

-

Robot & Environment

Joint Controller

S2 Force Controller

Trajectory Planner 2

Fig. 3.

X2

S2

-

Transformation into task

Computation of forces and torques caused by environmental contact

Transformation into a-sensor

Embedding force and acceleration sensor fusion in an implicit hybrid force/pose control scheme.

task task The sum of all three outputs f orce P~ task , velocity P~ task , and task ~ task contains the end-effector movement task P~ task for pose P the current control cycle. Together with world P~ task we get the new Cartesian position set point world P~ task . After transforming these values into the hand frame and after applying the inverse kinematic model, the respective values in joint space h~qi serve as input vector for the inner joint control loop.

IV. E XPERIMENTAL R ESULTS The following experimental results have been achieved with a Staeubli RX60. Its original control unit has been replaced by an own one, which basically consists of a network of PCs with QNX running as real time operating system. One PC interfaces the power electronics directly and is dedicated for low level joint control while all further PCs are used for hybrid control or user applications, respectively. Here we focus on a second PC, which is equipped with two sensor interface boards, one DSP receiver board for a JR3 force/torque sensor and one for a JR3 accelerometer. Real time signal processing is essential as well as synchronous capturing of force/torque and acceleration signals. A rectangular block with a mass of 1105 g has been mounted as test object onto the force/torque sensor. Its center of gravity is located 25mm in front of the accelerometer. The trajectories for all experiments described here have been computed offline and are jerk-free as can be seen in Fig. 5c, which exemplarily shows the angular acceleration profile for one DOF. All signals have been captured in real time with a sampling rate of 2 kHz and were captured during several test trajectories to verify the quality of this approach. A. Vis Inertiae Fig. 4a shows results of linear motion in one DOF. The top part displays the force f ts fx as recorded by the force sensor. ~ is taken into account, f ts fx can be calculated and When as A i subtracted from f ts fx . The bottom diagram indicates the result f ts e fx . In comparison to the top chart, non-contact forces could be well eliminated. As we will see in all experiments, there is an increased disturbance during the manipulator motion. Looking at its frequency spectrum, a peak at eleven Hertz can be observed. An attempt to relegate these disturbances is described in section V.

2629

B. Centrifugal Forces Fig. 4b summarizes results to verify the behavior during the occurrence of centrifugal forces without environmental contacts. An angular motion of 180◦ in a horizontal plane was executed and the radial force in z-direction f tse fz was measured. During the motion the measured force raises up to 10N and when eying on the resulting force f tse fz , we see that the disturbances have less influence than in the experiment of Fig. 4a; but the force goes up to one Newton instead of remaining at zero. We assume, that this is caused by bad initial sensor zeroing. In all mentioned cases, an accurate initial reset of both sensors is essential. C. Vis Inertiae with Environmental Contact The next experiment (Fig. 4c) is a repetition of the one from Fig. 4a. The motion was programmed backwards and the manipulator comes into contact with some environmental object and knocks it down. This environmental contact can hardly been seen in the signal of f ts fx (algorithmically it cannot be ~ to calculate f ts fx , the contact can be seen), but after using as A e detected very conspicuously at 850 milliseconds. This clearly demonstrates the high benefit. D. Gravitational Forces Another major benefit of using 6D acceleration signals is that forces and torques caused by gravitational forces can be acquired. Before the test motion corresponding to Fig. 5a, the end-effector stood uprightly. A rotational movement of 180◦ around the joint-5-axis of the manipulator changed the orientation of the end-effector, such that the gravitational force changed its sign and that the centrifugal force is neglectable. This should theoretically result in a measured force f ts fz of 21.68N. After the motion, the resulting force f tse fz did not remain at zero, but at approx. two Newton. E. Torques of Inertia Until now, only force calculations have been considered, whereas Fig. 5b illustrates the resulting torque f tse τy after using f ts τy and as αy during the same motion as used for the experiment shown in Fig. 4b.

a) Influence of vis intertiae

b) Influence of centrifugal forces

c) Influence of vis intertiae

15

15

15

fts

fts

fx/N

fz/N

10

12

5

9

0

6

fts

fx/N

10 5 0 -5

-5

3

-10

0

Measured force

-15 15

ax/(m/s2)

-3 3

-10 -15

Measured force 2

az/(m/s )

10

0

10

5

-3

5

0

-6

0

-5

-9

-5

-12

-10

Measured acceleration

-15 15

15

as i x

f /N

5

9

0

6

Measured acceleration

-15 15

as i z

f /N 12

ax/(m/s2)

-10

Measured acceleration

-15

10

Measured force

-20 15

as i x

f /N

10 5 0 -5

-5

3

-10

0

Calculated non-contact force

-15 15

-15

Calculated non-contact force

-3 15

fts e x

f /N

10

-10

fts e z

f /N

12

5

9

0

6

Calculated non-contact force

-20 15

fts e x

f /N

10 5 0 -5

t/ms

-15

1400

1300

1200

1100

900

1000

800

700

600

400

300

200

0

500

Calculated contact force

-20

100

1400

1300

1200

1100

900

500

400

300

200

0

t/ms

1000

Calculated contact force

-3

100

1400

1300

1200

1100

900

1000

800

700

500

400

300

200

0

100

600

Calculated contact force

-15

-10

800

0

700

3

600

-5 -10

t/ms

Fig. 4. Experimental results: a) Simple linear motion without environmental contact. b) Rotational motion of 180◦ to detect centrifugal forces. c) Same as a) but with an environmental contact during motion.

F. Coriolis Forces To complete this series of experiments, we also verified the behavior of f tse F~ during a motion that lets the Coriolis effect arise. Its path is depicted by Fig. 5d, which shows a circular motion in a horizontal plane with a radius decreasing over time. The outer radius r1 was 60 centimeters, the inner one was 30 centimeters. Fig. 5e depicts the tangential force during this motion (solid line). We calculated the force theoretically without considering the Coriolis effect and drew the result also into the diagram of Fig. 5e (dashed line). One can clearly see, that both parts do not match with each other. The difference between both lines represents the Coriolis force. V. F ILTERING When looking at the force/torque and acceleration signals of Fig. 4 and Fig. 5, the idea of using filters to smooth the signals was born. The expected advantage was that disturbances could be eliminated. We implemented several discrete low-pass FIR and IIR filters. In fact, the signal characteristics of f ts F~

2630

~ could be improved and the undesired disturbances and as A could be damped, but the final result of f tse F~ was afflicted with an increased average filter delay and peaks, as can be seen in Fig. 4c, were undesirably damped. The best signal characteristics were achieved with a type II Chebychev filter, but for the application of compensating non-contact forces and torques, the embedding of low-pass filters did not lead to any improvement. VI. S UMMARY, C ONCLUSIONS , AND F UTURE W ORK 6D force/torque and 6D acceleration signals were fused to contact forces and torques occurring during compliant manipulation. All essential equations for this procedure were derived and to verify this approach, numerous experiments were presented. Vis inertiae, centrifugal forces, torques of inertia, gravitational forces, and Coriolis forces were detected and could be used for a very sufficient compensation of measured inertial forces/torques. The major benefits are an improved force/torque control behavior and making sensor zero-

a) Influence of gravitational forces

b) Influence of torques of inertia

c)

25

0,4

500

fts

fz/N

ft

ty/Nm

0,3

20

250

a/(°/s2)

0,2 15

0,1

10

0 -0,1

5

0

-250

-0,2

az/(m/s2)

0

d)

1350

1200

r2

10

-10

1050

r1

20 -5

900

t/ms

ax/(°/s2)

30

750

Measured torque

-0,4 40

600

-500

-0,3

450

Measured force

300

-5 5

150

0

0 -10

-15

-20 -20

Measured acceleration

-25 25

as

fz/N i

-30

Measured torque

-40 0,4

as i

0,3

20

ty/Nm

0,2 15

0,1 0

10

-0,1

5

-0,2

Calculated non-contact torque

-0,4 0,4

t /Nm

0,3 0,2

10

0,1

5

0

0

-0,1

-5

t/ms

t/ms

-10

900

-15

1000

1400

1300

1200

1100

900

1000

800

700

600

400

300

200

500

Calculated contact torque

-0,4

800

-0,3

0

1400

1300

1200

1100

900

1000

800

700

500

400

300

200

0

100

600

Calculated contact force

100

0 -5

700

-0,2

600

5

500

10

400

15

Measured force ftsfx Predicted force without Coriolis effect

15

300

f /N

e) 20

fts fts ety/Nm e y

200

fts e z

20

-0,3

100

Calculated non-contact force

-5 25

0

0

t/ms

Fig. 5. a) Rotational motion of 180◦ used to compensate captured gravity forces/torques. b) Rotational motion of 180◦ without environmental contact for the computation of f ts e τy . c) Acceleration of a jerk-limited trajectory for a rotational DOF. d) Helix-shaped path to detect the Coriolis effect. e) Detection of forces caused by the Coriolis effect: the difference between the solid and the dashed line represents the Coriolis force.

ings/resets unnecessary. In this approach, the mass distribution in front of the end-effector has to be known in advance and has to be given by the user. For further practical improvements based on this approach, additional pose feedback could be used to compute the inertia tensor automatically. The the key motivation and the long-term aim of this work is to bring multi-sensor integration approaches into practice, such that today’s industrial manipulation processes become more and more versatile. R EFERENCES [1] D. E. Whitney. Force feedback control of manipulator fine motion. ASME Journal of Dynamic Systems, Measurment and Control, 98:91– 97, 1977. [2] M. T. Mason. Compliance and force control for computer controlled manipulators. IEEE Trans. on Systems, Man, and Cybernetics, 11:418– 432, June 1981. [3] C. Canudas, B. Siciliano, and G. Bastin. Theory of Robot Control. Springer, 1996. [4] B. Siciliano and L. Villani. Robot Force Control. Kluwer Academic Publishers, 1999.

2631

[5] J. J. Craig. Introduction to Robotics: Mechanics and Control. Prentice Hall, third edition, 2003. [6] H. Bruyninckx and J. De Schutter. Specification of force-controlled actions in the task frame formalism - a synthesis. IEEE Trans. on Robotics and Automation, 12:581–589, August 1996. [7] T. Lefebvre, H. Bruynincks, and J. De Schutter. Online statistical model recognition and state estimation for autonomous compliant motion. IEEE Trans. on Systems, Man, and Cybernetics, Part C: Applications and Reviews, 35(1):16–29, February 2005. [8] B. Finkemeyer, T. Kr¨oger, and F. M. Wahl. Executing assembly tasks specified by manipulation primitive nets. Advanced Robotics, 19(5):591– 611, June 2005. [9] J. G´amez Garc´ı, A. Robertsson, J. G´omez Ortega, and R. Johansson. Sensor fusion of force and acceleration for robot force control. In Proc. of IEEE/RSJ International Conference on Intellegent Robotic Systems, pages 3009–3014, 2004. [10] J. G´amez Garc´ı, A. Robertsson, J. G´omez Ortega, and R. Johansson. Force and acceleration sensor fusion for compliant robot motion control. In Proc. of IEEE International Conference on Robotics and Automation, pages 2720–2725, 2005. [11] T. Kr¨oger, B. Finkemeyer, M. Heuck, and F. M. Wahl. Adaptive implicit hybrid force/pose control of industrial manipulators: Compliant motion experiments. In Proc. of IEEE/RSJ International Conference on Intellegent Robotic Systems, pages 816–821, 2004.