Cartesian Force Estimation of a 6-DOF Parallel Haptic Device

(1)

IN

DEGREE PROJECT ELECTRICAL ENGINEERING, SECOND CYCLE, 30 CREDITS

,

STOCKHOLM SWEDEN 2019

Cartesian Force Estimation of a 6-DOF Parallel Haptic Device

FANGHONG DONG

(2)

(3)

Sammanfattning

En haptikenhet gör det möjligt att förmedla en känsla av kontakt i en virtuell värld genom att skapa krafter som motverkar en rörelse . Hur denna kraft skapas och kontrolleras är av stor vikt för att få den så verklighetstrogen som möjligt. Om man har en kraftsensor kan den användas till att utforma en kraftreglering med återkoppling, men på bekostnad av en ökad massa och tröghet vid användarens hand. Detta har medfört ett ökat intresse under de senaste åren för att på olika sätt försöka uppskatta den kraft som återkopplas till användaren utan att behöva en kraftsensor.

Målet för detta examensarbete är att utveckla en algoritm för att uppskatta en kontaktkraft i realtid baserat på antagandet att motormomentet är proportionellt beroende av strömmen.

Algoritmen kan sedan användas för att konstruera en sluten reglerloop med kraftåterkoppling för en haptisk enhet.

Forskningsfrågorna som behandlas i detta examensarbete är;

 hur kan vi utforma en algoritm för estimering av kontaktkrafter för haptikenheten TAU

 hur kan vi utforma en experimentell försöksuppställning för mätning av de verkliga kontaktkrafterna från TAU vid kontakt.

 hur kan vi använda resultaten från experimenten för utvärdering av algoritmen För testning och utvärdering av algoritmen har en virtuell värld skapats för att efterlikna en simuleringsmiljö som haptikenheten är tänkt att användas i. En kraftsensor har monterats under det verktyg som användaren håller i när enheten används när ett typiskt ingrepp ska övas i en simulator, t.ex. borrning i en tand. Vid experimenten beräknar algoritmen den uppskattade kontaktkraften som användaren känner baserat på den uppmätta strömmen för de sex motorer som aktiveras av kontakten. Dessa beräknade värden har sedan jämförts med de från

kraftsensorn uppmätta för att avgör om algoritmen är tillräckligt noggrann. Analysen visar att noggrannheten är tillräckligt bra för att vara en lovande ansats till att användas för

kraftuppskattning vid reglering av kontaktkraft för haptikenheten TAU.

Examensarbete Thesis TRITA-ITM-EX 2019:662

Kartesisk kraftuppskattning av en 6-DOF parallellhaptisk enhet

Fanghong Dong

Godkänt

2019-11-22

Examinator

Dejiu Chen

Handledare

Lei Feng

Uppdragsgivare

Fredrik Asplund

Kontaktperson

Fanghong Dong

(4)

Master of Science Thesis TRITA-ITM-EX 2019:662

Cartesian Force Estimation of a 6-DOF Parallel Haptic Device

Fanghong Dong

Approved

2019-11-22

Examiner

Dejiu Chen

Supervisor

Lei Feng

Commissioner

Fredrik Asplund

Contact person

Fanghong Dong

Abstract

The haptic device recreates the sense of touch by applying forces to the user. Since the device is

“rendering” forces to emulate the physical interaction, the force control is essential for haptic devices. While a dedicated force/torque sensor can close the loop of force control, the additional equipment creates extra moving mass and inertia at the tool center point (TCP). Therefore, estimating the Cartesian force at the TCP has continuously been receiving attention over the past decades.

The objective of this thesis project is to develop a real-time force estimation algorithm based on the proportional current-torque relationship with the dynamic modeling of the TAU haptic device. The algorithm can be further used for the force control of the device. The research questions of the thesis are: how to design and develop an algorithm for the TAU that used for Cartesian contact force estimation, how to set up the force estimation test bench and how to evaluate the results of the force estimation algorithm.

In order to achieve the force estimation algorithm, a virtual environment is built to simulate the real-time haptic physics. Then an external force/torque sensor is installed at the TCP to get the measurement of the Cartesian force at the TCP. The force estimation algorithm calculates the Cartesian force at the TCP based on the current measurement of the DC motors at the six joints.

The estimation result of the Cartesian force at the TCP is then compared with the force/torque sensor measurement to determine if the estimation algorithm is sufficiently accurate. The analysis of the estimation accuracy emphasizes the feasibility of Cartesian force estimation on the TAU haptic device.

Keywords:

Haptic device, Cartesian force estimation, force/torque sensor

(5)

(6)

Nomenclature

DOF Degree of freedom ECU Electronic control unit F K Forward kinematic HV Os Haptic virtual objects ODE Ordinary differential equation

P CIExpress Peripheral component interconnect express P KM Parallel kinematic machines

RT I Real-Time interface T CP Tool center point V R Virtual reality

(9)

1 Introduction

The haptic devices can generate a sense of touch to the user by adding forces. It involves the contact of physics between humans and the virtual world simulated by computer through I/O (input/output) devices, for example, robot arms, joysticks, and wired gloves. It is commonly used in virtual reality (VR) applications. For example, the data gloves is a device sensing and transferring interaction between human and computer. It can simulate the movement of the user picking up an object in the virtual environment with the virtual environment graphics generated by the computer. In this process, the user can feel the reactive forces from the object on his hands, rendered by virtual space physics. This makes the feeling of picking up an object rather realistic[1].

Since the device is “rendering” a force to emulate the physical interaction, force control is essential for haptic devices as well as other applications on human- robot interaction. While a dedicated force/torque sensor can close the loop of force feedback, the additional equipment adds extra load to the end-effector and increases the cost. It also creates extra moving mass and inertia at the tool center point (TCP) for a haptic device, which is against the design guidelines of the haptic devices.[2] Thus, the estimation of the Cartesian wrench, including contact forces and torques at TCP, makes a better solution for rendering this Cartesian wrench.

The literature review of the modeling of a parallel manipulator and its force estimation algorithm is presented in the next section.

1.1 State of the Art

In our lab, there is a 6-degree-of-freedom (6-DOFs) desktop parallel manipulator (PM) haptic device named TAU developed by former students. The device aims for the training of surgeons to develop their skills in operating in stiff structures such as bone or tooth. For this typical purpose, software for collision detection and graphical interface has been developed. The design of the device has been thoroughly analyzed and optimized to achieve the best possible performance.

The control algorithm for the current prototypes was also built and tested.

However, the force estimation algorithm was not implemented because of the lack of a force sensor. At this time, with a new force sensor, implementing the Cartesian force estimation feature is feasible.

1.1.1 Modeling of a parallel manipulator

The book by Craig [3] introduces the basic knowledge of industry robot and modeling of the manipulators. The general solution of dynamic and kinematic modeling of a parallel manipulator was introduced by [4]. In this book, the basic principle of modeling a parallel manipulator and relative math methods were introduced, which is suitable to all kinds of the parallel manipulator. Also, in the book [5], a solution of virtually cutting each loop at one of its passive joints

(10)

of the closed chain and decomposing the system into two subsystems is used to get the dynamic modeling of a closed chain parallel manipulator.

In the articles [6] and [7], two numerical solutions of dynamic modeling of the closed chain were introduced, which are excellent examples for building a dynamic model of the parallel manipulator.

1.1.2 Previous Study on TAU

Khan [8] developed the mechanical structure of the TAU device and built its prototype. The six degrees of freedom TAU manipulator was developed for mimicking a milling process in the surgical procedures and optimized to provide high stiffness force/torque capabilities [2]. After a conceptual design, a device prototype was realized, and its performance verified. Then a control strategy is investigated to achieve high transparency and stability of it.

1.1.3 Force Estimation on Robotic Manipulators

A force estimation algorithm is proposed in the article [9], which upgrades the force estimation algorithm by Kalman filters on a UR serial manipulator. In the article [10], a robust estimator is proposed to estimate three-dimensional contact forces applied on a three-link robot manipulator. These force estimation algorithms are all based on the dynamic equation of robotic manipulators and estimate Cartesian contact force by only motor signals. In the diploma thesis [11], the Coulomb friction model and viscous friction model are considered to improve the accuracy of estimated motor torques, which also improves the force estimation accuracy.

1.2 Research Objective and Questions

Since TAU is a complicated 6-DOFs nonlinear parallel haptic device with a human-operated tool on its tool center point, we want to know the force applied by the human to the tool. TAU has three kinematic chains that are not identical to each other. Such asymmetric structure distinguishes the TAU from most well-known PMs and complicates the dynamic modeling process. The force sensor is installed directly on the tool, and the sensor measurement is the ground truth. However, the force sensor is expensive and difficult to install. We want to estimate [12] the human force without the sensor. This can be achieved by the dynamic model [3] of the device and the motor torques.

The objective of this thesis is to develop a real-time force estimation algorithm so that it can be used in force control of the device. Then the estimation result of the external force is compared with the sensor measurement to determine if the estimation algorithm is sufficiently accurate. The research questions are as follow:

• What state-of-the-art knowledge of 6-DOF parallel devices that should be used in the Cartesian contact force estimation algorithm?

(11)

• How to design and develop an algorithm for TAU that used for Cartesian contact force estimation?

• How to install the external force/torque sensor and set up a force estimation test bench?

• How to evaluate the results of the force estimation algorithm?

1.3 Outline

Chapter 2 presents the principles of modeling and control of parallel manipulators, the generalities of the parallel robot and haptic devices are introduced firstly. The kinematic and dynamic modeling methods of parallel manipulators, especially for a closed chain manipulator, are mentioned. Then the external force estimation algorithm based on joint torques and manipulator Jacobian is introduced as the key algorithm of this thesis project. Then the modeling of joint friction torques is given.

Chapter 3 introduces all the equipment used in this test stand, including the TAU manipulator, dSPACE controller, chai3d virtual reality environment, and force/torque sensor. Each part is essential to get the final result of external force estimation.

Chapter 4 describes the implemented force estimation algorithm and estimation results, including using current torque relationship to get joint torques and transforming it into external force on tool center point. The estimated forces are then compared to the measurements from the force sensor with the mean average error present in a table. The results analysis shows the reasons that cause the error.

Chapter 5 summarizes and concludes this thesis project and gives some recommendations for improvement and further work.

1.4 Acknowledgements

I would first like to thank my thesis advisor, associate professor Lei Feng from the School of Mekatronik at KTH. He consistently gives me help and suggestions, steering me in the right direction. I am extremely thankful to him for sharing expertise, sincere and valuable guidance and encouragement extended to me.

I would also like to acknowledge Ph.D. student Yang Wang. I am gratefully indebted to his valuable comments during this project. He also helped me with building 3D printed components for the manipulator.

Finally, I must express my very profound gratitude to my parents for providing me with unfailing support and continuous encouragement throughout my years of study and through the process of researching and writing this thesis. This accomplishment would not have been possible without them. Thank you.

(12)

2 Principles of Modeling and Control of Parallel Manipulators

After a brief introduction to the big picture and the goal of this thesis project, this chapter gives an overview of the industrial manipulators and especially the principles of the parallel manipulators. The generalities on parallel manipulators in Section 2.1 show the development of parallel manipulators in the industry area. Then some different types of parallel robots used in different applications are introduced. Then in Section 2.2 and 2.3, the physical principles of kinematic and dynamic modeling of the parallel robot are introduced, which are essential to the force estimation algorithm. At the end of this chapter, different ways of parallel robot force estimation are explained.

2.1 Generalities on Parallel Robot

Parallel robots, compared to serial robots, have at least two kinematic chains that link between the base of the robot and its end-effector. This fundamental characteristic generates some proprieties of parallel robots, such as closed chains and rather complicated forward kinematics. Also, for parallel robots, the inverse kinematic solution can be unique [4].

The parallel kinematic machines (PKMs) consists of the base, platform, and kinematic chains. With the unique design of parallel kinematic machines, the load on it is shared by not unique but several chains, which makes the machine capable of heavy loading. Compared to the serial manipulator, it can reduce the unwanted flexibility caused by amplification between the base and the end- effector, increase precision, and decrease the power of the motors. On the other hand, the working space of the parallel manipulators is limited by its closed chains, and the determination of the end-effector is more complicated than serial ones. Because of these characteristics, parallel robots are specially used for some industrial applications; one of them is haptic devices.

2.2 Different Types of PKM

There are several categories of PKM architectures. The different types of PKM are categorized according to their different designs and topologies. They can be grouped by degree of freedom (DOF) and types of their platform. Some PKMs are designed to move their platform in a plane while others to move in a three-dimension space. Four types of typical PKMs are introduced in the following paragraph. One, 2-DOF planar prototype, as shown in Figure 1, can move on a square platform. It consists of two translational DOF in a flat plane with three kinematic chains.

(13)

Figure 1: A 2-DOF planar prototype

Two, a large delta-style 3D printer, as shown in Figure 1, can move along the Cartesian plane. By moving the three arms arranged triangle vertically and independently, the end-effector can move in all directions in the Cartesian space.

Figure 2: A large delta-style 3D printer

Three, Figure 2 shows a 6-DOF platform. 6-DOF motion platform solutions are perfectly suitable for flight simulators, driving simulators and others. Figure 3 shows a driving simulator platform.

(14)

Figure 3: A Driving Simulator

Finally, there is a 6-DOF spatial robot designed for industrial assembly applications as shown in Figure 4. It has outstanding move performance with high-speed flexibility and high capacity of payload. It is suitable for picking and packing tasks.

Figure 4: FlexPicker is a high speed robotic for picking and packing

2.3 Haptic Devices

The haptic devices pass the interaction forces between the user’s side and the object’s side. It involves the contact of physics between humans and the virtual world simulated by computer through I/O devices such as robot arms, joysticks, and wired glove. It is commonly used in virtual reality (VR) applications.

Force feedback is one of the important human sense of touch that can be simulated to communicate or recognize objects, which is also the most commonly available haptic interface. The haptic manipulator with its three-dimensional force-feedback allows it to simulate force, torque, and other physical presence of the objects in the virtual world.

(15)

The interaction between humans and haptic virtual objects (HVOs) performed by the sense of touch is generated by the haptic device and computer graphics.

HVOs render multiple combinations of simulated cutaneous with kinesthetic through interactive data transfer. In the next section, some typical haptic applications are introduced.

Novint Falcon, as shown in Figure 5, is a 3-DOF haptic device intended to replace the joystick or mouse in multiple applications. It is a delta type haptic device that has three DOF with force feedback technology.

Figure 5: Novint Falcon

Shadow hand is a robotic system comparable to human hand size and shape, as shown in figure 6. It is available in both variants, electric motor driven model and pneumatic muscle driven model. It reproduces all of the human hand’s degrees of freedom.

Figure 6: Shadow hand compared to human hand

Finally, the latest technology in aerospace named the active side stick unit control system is shown in Figure 7. It can react to vibrations to ensure the aircraft is kept within the defined flight envelope.

(16)

Figure 7: The active side stick unit control system

2.4 Kinematic Modeling of the Parallel Robot

In this section. The basic concepts of the kinematic modeling for a general parallel robot, especially for the tree-structure chain, are introduced.

2.4.1 Homogeneous Coordinates and Transformation Matrix

The notation to describe the relationship between the robot’s frames and objects called homogeneous transformation is introduced in [4]. Let (^jxP,^jyP,^jzP) be the arbitrary point P’s Cartesian coordinates with respect to frame Fj. The origin of the coordinates is point O_j and axes are x_j,y_j,z_j. The homogeneous coordinate of P represented by a column vector is:

jp =







jx_P

jyP jzP

1







(1)

It can also be defined in another frame F_i with coordinates (ⁱx_P,ⁱy_P,ⁱz_P), by the column vector ⁱp = [ⁱx_P ⁱy_P ⁱz_P1]^T. The transform matrix ⁱT_j permits the translation and rotation from frame F_j(O_i, x_i, y_i, z_i) to frame F_i as:.

iT_j ^jp =˜ ^jx_P ^j˜s_P+^jy_P ⁱn˜_j+^jz_P ⁱã_j+ⁱ˜r_j (2) where ⁱsj, ⁱnj and ⁱaj are unit vectors along xj,yj,zj axes respect to the homogeneous coordinates ⁱ˜sj, ⁱñj and ⁱãj. The transform matrix ⁱTj is then:

iT_j = [ⁱs˜_j ⁱn˜_j ⁱ˜a_j ⁱ˜r_j] =

i

Rj irj

0 0 0 1

(3) whereⁱRj is the rotational matrix transform the orientation form frameFj to frame Fi.

(17)

A pure translation along x axis form frame Fj to frame Fi and a rotation of angle α around axis x are as follow:

T rans_xj(a_j) =







1 0 0 a_j

0 1 0 0

0 0 1 0

0 0 0 1







(4)

Rotxj(αj) =







1 0 0 0

0 cos(αj) −sin(αj) 0 0 sin(α_j) cos(α_j) 0

0 0 0 1







(5)

And their corresponding homogeneous transformation matrix is:

T =T ranszj−1(dn) · Rotzj−1(θn)T ransxj(aj)Rot(αj)

=







cθj −sθjcαj sθjcαj ajcθj

sθj cθjcαj −cθjsαj ajsθj

0 sαj cαj dj

0 0 0 1







(6)

where sθj = sinθj, cθj= cosθj

2.4.2 Euler angles

As shown in Figure 8, the Euler angles are the three independent angular parameters used to uniquely determine the position of the rotating object around the fixed point, first proposed by L.Euler [3].

The Euler angles include the nutation angle θ, precession angle ψ, and the spin angle φ. The fixed coordinate system O_xyz is fixed by the point O and the coordinate system O_x⁰_y⁰_z⁰ fixed to the rigid body. The axes Oz and Oz⁰ are the basic axes, and the vertical faces Oxy and Ox⁰y⁰ are basic planes.

Figure 8: Euler angles [13]

(18)

Define the rotation matrix Rz(α), Rx(β), Rz(γ) for the three Euler angles:

Rz(α) =





cosα −sinα 0 sinα cosα 0

0 0 1



 (7)

R_y(β) =





1 0 0

0 cosβ −sinβ 0 sinβ cosβ



 (8)

Rx(γ) =





cosγ −sinγ 0 sinγ cosγ 0

0 0 1



 (9)

The total rotation matrix respect to the ZYX Euler angle is defined as follows:

RAB= Rz(α)Ry(β)Rx(γ)

=





cγcα− sγsαcβ −cγsαcβ− cαsγ sβsα

cγsα+ sγcαsβ cγcαcβ− sαsγ −sβcα

sβsγ cγsβ cβ





=





nx ox ax

ny oy ay

nz oz az





(10)

After solving the position of the end mechanism, the above matrix can be calculated to obtain the Euler angle attitude of the end mechanism.

α = arctan( t13

sin(β), −t23

sin(β)) (11)

β = cos⁻¹(t33) (12)

γ = arctan( t31

sin(β), t32

sin(β)) (13)

2.4.3 Kinematic description of multibody system

When modeling a signal manipulator linkage, a rotary joint or a translation joint with one degree of freedom is usually considered. The links are numbered starting from the base of the manipulator’s arm. Therefore, the fixed base is usually defined as link zero, the first movable joint as the line one, and so on. Designers need to consider the material properties, stiffness and strength, mounting position, shape quality and moment of inertia within every link when designing the robot. A tree-structure open kinematic chain is shown in Figure 9.

(19)

Figure 9: Symbolic representation of a tree-structure open kinematic chain [7]

The connecting rod is regarded as a rigid body when establishing the kinematic equations of the manipulator’s arms. The joint axis i is represented by a vector in space, and the link i is rotated about the joint axis i relative to the link i − 1.

At this time, the length of the common perpendicular line ai−1between the joint axis i − 1 and the joint axis i is defined as the link length. At the same time, the joint axis i − 1 and the joint axis i are projected onto a plane perpendicular to the perpendicular axis of the two joint axes, and the angle of the two projections is measured as α_i−1, defined as the link angle.

Denavit-Hartenberg(DH) Parameters are often used to describe a kinematic chain’s particular relationship with respect to the reference frame. The classic DH parameters have four values representing the convention from the reference frame to the linkage. For more complex kinematic chains such as the tree-structure open kinematic chain, a new concept called modified Denavit-Hartenberg (MDH) allows defining the system parameters of the location relative to anybody within the whole chain system.

2.4.4 Forward Kinematic

First, the direct geometric model of a kinematic chain aims at getting a terminal link B’s translation and angular position with respect to base frame F . For a specific kinematic chain with the tree-structure, there are multiple terminal links. The groups of its sub-chain to the base shall be considered during the calculation [4].

Define a list of size nk containing all frames between the terminal link and the base frame along the sub-chain from Fi to Fk as a^(i:k)= {i . . . a(a(k))a(k)k}.

Then the homogeneous transformation matrix from the terminal frame to the base frame is the product of all transformation matrix as:

iT_k=

nk

Y

j=2

(^a^(i:k)^(j−1)T_a(i:k)(j)) (14)

where a^(i:k)(j) is the jth element of list a^(i:k). With the transformation matrix above, the relative position of frame Fn with respect to F0 can be calculated directly.

(20)

2.4.5 Velocity Analysis

For a serial tree-structure manipulator, the terminal link t_n^T has the velocity with respect to the base frame, it can be determined in terms of joints velocity ˙q [4].

t_n= J_n(q) ˙q (15)

where Jn(q) is the (6 × m) kinematic Jacobin matrix.

For all joints of the total number s, the linear and angular velocities with respect to the base frame of each terminal link are:

tn=

s

X

k=2

V_an(k),n

ω_an(k),n

=

s

X

k=2

δ_an(k)a_an(k)+ ¯δ_an(k)(a_an(k)× rO_{an (k)}O_n) δ¯_an(k)a_an(k)

˙ qaⁿ(k)

(16) where linear and angular velocities denoted as V_an(k), n and ω_an(k), n respectively, rO_{an (k)}O_n denotes the position vector connecting Ok to On. And the Jacobin matrix can be written as:

Jn=δ_la_l+ ¯δ_l(a_l× rOlOn) . . . δ_a(n)a_a(n)+ ¯δ_a(n)(a_a(n)× rO_a(n)On) δ_na_n

¯δlal . . . ¯δ_a₍_n)a_a(n) δ¯nan

(17) Using the inverse Jacobin to get the vector of velocity ˙q

˙

q = ⁱJ_n⁻¹(q)ⁱtn (18)

Especially for a redundant open loop kinematic chain, there is a non-null vector

˙

q0 for the pose of the terminal link:

0 = ⁱJn(q) ˙q0 (19)

Considering the terminal link’s acceleration based on B_n, it can be obtained by differentiating Eq. 15 with respect to time as:

˙t_n=

v˙n

˙ ωn

= J_n(q_n)¨q_n+ ˙J_n(q_n, ˙q_n) ˙q_n (20)

2.5 Dynamics of Parallel Robot

As we all know, the closed-loop structure makes the parallel manipulator hard for dynamic modeling. However, dynamic modeling is important for the movement control of the manipulator. There are multiple ways to get a dynamic model of a closed chain manipulator, such as using Lagrange Euler formula [3], the principle of virtual work, and the principle of Hamilton [4].

The core of the proposed method is projecting both platforms and links dynamic using Jacobian matrices and expressing dynamic models of links in the joint coordinates and platform’s dynamic in the Cartesian coordinate. Then apply the simple dynamic modeling method for serial link manipulator to them.

Finally, the Newton Euler equation gives the final total force and moments on the platform.

(21)

2.5.1 Inverse Dynamic Model

Considering a non-redundant robot, which means the number of active joints of the robot is the same as the number of its degree of freedom n. Denote the platform frame as P

P and the base frame P

0. Then try to get the inverse dynamic model, which results in the joints’ forces and torques as the function of the platform’s trajectory [5].

Denote the joints’ forces and torques as a whole matrix Γ. It composes of inverse dynamic model Γtr and Jacobian matrix of the manipulator:

Γ = G^TΓtr (21)

The inverse dynamic model can be expressed with inertia matrix Atr and vector of Coriolis htr times joints variables (qtr, ˙qtr, ¨qtr) as follow:

Γtr= Atr(qtr)¨qtr+ htr(qtr, ˙qtr) (22) The Jacobian matrix can be expressed with the tree structure variable q_tr with respect to different joins q_a:

G =∂qtr

∂q_a (23)

and since the tree structure variable is not related to time ,the kinematic Jacobian G can also be expressed as:

G =∂ ˙qtr

∂ ˙q_a (24)

Then the total inverse dynamic model including the active joints of the closed chain are:

Γtrdqa= ΓtrT

dqtr (25)

Γtrq˙a= ΓtrT

˙

qtr (26)

Figure 10 shows a tree equivalent structure of the parallel robot with a platform, a base and chains.

(22)

Figure 10: The tree equivalent structure of the parallel robot [7]

The next step is to virtually cut each loop to turn the closed-loop chain into several serial chains. Then the platform’s dynamics can be a function of joints’

variable of all the open links. Then calculate the dynamics of the platform and open-chain separately, using dynamic models of links in the joint coordinates with respect to joints’ variables and the platform’s dynamics with respect to Cartesian variables.

Project the platform’s dynamics on active joint’s space using Jacobian between Cartesian coordinates and joint coordinates. With the platform’s total force F_p velocity vector V_p and kinematic Jacobian J_p of the system, the total dynamic model described by the equation of the parallel structure is as follows:

Γ = JpTFp+

m

X

i=1

(∂ ˙qi

∂ ˙q_a)^THi (27)

The six degrees of freedom platform’s total force expressed by Newton Euler equation is:

F_p= I_S

V˙p− g

˙ ωp

+ωp× (ωp× M Sp) ωp× (Ipωp)

(28) where:

V_p=v_p^T ω_p^TT

(29) M S_p=M Xp M Yp M Zp^T

(30) Differentiate the equation above to get platform’s acceleration:

V˙p= ˙SVr+ S ˙Vr (31)

Using two steps of differentiating, the differential of active joins can be expressed

as: ∂ ˙qi

∂ ˙q_a = Ji−1JviSJr (32)

(23)

Then the compact form of the inverse dynamic model is:

Γ = JrT

S^T[FP +

m

X

i=1

JviT

Ji−THi]Γ = JPT

[FP+

m

X

i=1

JviT

Ji−THi] (33)

where Hi expresses the inverse dynamic model of leg i.

2.5.2 Direct Dynamic Model

The direct dynamic model consists of getting the platform’s acceleration as the function of the homogeneous transformation matrix ⁰Tp, the platform’s velocity and motor torques Γ:

V˙p= f (⁰Tp, Vp, Γ) (34) By differentiating vector of Cartesian velocity with respects to time and comput- ing its inverse, the result is ¨q.

˙vi= Jiq¨i+ ˙Jiq˙i (35)

¨

q_i= J_i⁻¹( ˙v_i− ˙J_iq˙_i) = J_i⁻¹( ˙J_viV_p+ J_viV˙_p− ˙J_iq˙_i) (36)

2.6 The Principle of Force Estimation

The principle of force estimation based on the current-torque relationship and the Jacobian matrix is introduced in this section. When a user operates a haptic manipulator, his/her operations produce torques on motors at the joints. The quantitative relation between the motor torques and forces/torques applied on the tool center point is determined by the manipulator’s Jacobian matrix J as follow:

τext= J^T(q)F (37)

where F =F_x F_y F_z T_x T_y T_z is the force/torques applied on the tool center point with respect to its base frame and τ_ext is the joint torques vectors.

The external forces/torques can be estimated by solving the multiplication of the inverse Jacobian matrix and motor torques vector.

F = (Jˆ ^T(q))⁻¹τˆ_ext (38) However, the total torques on joint motors are not only those corresponding to external forces/torques but also the torques maintaining the joint at the desired position or causing a motion denoted by τµ and the torques overcoming the friction of motor combined with gears denoted by τf r. Then the total torques on joint motors can be expressed as the combination of all three torques.

τ = τµ+ τext+ τf r (39)

The core of the external forces/torques estimation algorithm is to get the motor torques using measured motor current and torque constants. Especially

(24)

for the DC motor, estimated torque ˆτ using the current-torque proportional relationship is:

ˆ

τ = k_τ^Tia (40)

where k_τ ∈ Rⁿis the torque constants vector and i_a ∈ Rⁿis the armature current vector corresponding to the motors. Then the previous equation about total torques can be written as:

ˆ

τ = ˆτµ+ ˆτext+ ˆτf r= k^T_τia (41) The next step is to get the estimated value of the torques τµ that keep the joint at the desired position or cause motion. Then get the torques τf r that overcome the friction of motor combined with gears. The estimated external force and torques can be expressed as:

F = (Jˆ ^T(q))⁻¹τˆext= (J^T(q)⁻¹)(k_τ^Tia− τf r− τµ) (42) The manipulator’s dynamic equation in the Cartesian space using its kinetic and potential energy can be obtained by Lagrange equation. This can be used to get the torques for maintaining the joints at the target position τ_µ.

M (q)¨q + ˙M (q) ˙q + ˙q^TM (q) ˙˙ q + G(q) = τ (43) Then τ_{f r}is sometime assumed negligible or modeled by a rather simple Coulomb friction model or viscous friction model.

The whole structure of Cartesian contact force estimation based on motor signals is shown in Figure 11. The external forces/torques and motor torques cause the motion of the robot. The motor torques and robot’s motion signals are sent to the estimation block to get the estimated forces/torques.

Figure 11: Structure of Cartesian contact force estimation based on motor signals

2.7 Coulomb Friction Model

The standard Coulomb friction model gives the relation between friction force and velocity. The function plot of the Coulomb friction model is shown in Figure

(25)

12. The Coulomb friction force is combined with a part with constant value and a part proportional to the velocity. Its direction is opposite to the object’s motion. The function describing Ff ris as follows:

Ff r= Fcsgn(v) + dv(v 6= 0) (44) where the Coulomb friction level F_c is a constant value. It is determined by the normal force F_N and the friction coefficient µ as:

F_c= µF_N (45)

Figure 12: Standard Coulomb friction model

The Coulomb friction model is widely used in engineering area when analyzing static motion without dynamic effects. It is a simple and fast way to determine the friction but also bring rather large error [14].

The Karnopp friction model is modified based on the Coulomb friction model. It includes static and moving friction, which depends on the magnitude comparison between the velocity deadband d_v and stem velocity v. The function plot of Karnopp friction model is shown in Figure 13. If the magnitude of stem velocity is smaller than the velocity deadband, the model considers stem velocity to be null. Then the fiction Ff ris:

Ff r=







F c (Fa> Fc) F a (F_c≥ Fa ≥ −Fc)

−F c (Fa< −Fc)

(46)

where Fa is the applied force or torque. Otherwise, when stem velocity is larger than the velocity deadband, the fiction Ff is:

Ff r= Fcsgn(v) + dv (47)

(26)

Figure 13: Karnopp’s friction model

2.8 Summary

In this chapter, the general principle and terminology of the parallel robot and haptic device are introduced firstly. Then with the kinematic description of the multibody system combined with dynamic modeling of the manipulator, the principles used for solving the parallel manipulator’s movement and force related values are introduced. The last topic is the principle of force estimation, which is the critical algorithm for achieving the goal of this thesis. The modeling of friction force can be achieved in multiple ways, and a simple way is chosen to decrease the complexity. These are all basic principles for further development in the following chapters.

(27)

3 Laboratory Equipment: Manipulator, Force Sen- sor and Virtual Environment

This chapter introduces the equipment studied in this thesis, including the TAU manipulator, dSPACE controller, Chai3d Virtual Reality environment, and FT sensor.

3.1 The TAU manipulator

The TAU1 and TAU2 are six degrees of freedom, asymmetric parallel manipulators first designed by KTH Royal Institute of Technology, School of Industrial Engineering and Management, Department of Machine Design. The TAU manipulator’s parallel structure gives several benefits, especially for haptic rendering.

It has high stiffness and accurate movement. The end-effector and links have low inertia and high transparency [8]. The mechanical structures and control systems are well designed in the previous study. This paper mainly focuses on its application. Figure 14 shows the kinematic structure of TAU2.

Figure 14: Kinematic structure of TAU2 [8]

This novel structure of asymmetric parallel manipulator can be split into a fixed vertical main column, a moving central platform and three chains connected to the base frame and platform. Chain1 and chain2 are partly symmetric, and chain3 is asymmetric at top. The symmetric chains have two more links that strengthen the connection between the platform and upper links. This type of design can increase structural stiffness. Figure 15 shows a picture of the latest TAU prototype.

(28)

Figure 15: Picture of TAU

The specification requirement of the device is based on a brain surgeon application and it has a basic dimensions and capability as follows: the device fits in a space of 250 × 250 × 300[mm], work space of 50 × 50 × 50[mm], rotational work space of ±40 degrees, stiffness minimum 50[N/mm] and force and torque applied on TCP over 50[N ] and 1[N m] respectively.

The geometric parameters designed during modeling process are showed in the Table 1 as follow:

Table 1: Geometric parameters of TAU Parameters values Parameters values

R_p 45mm m_p 0.1 kg

L1 150mm mass of L1 0.05 kg L₂ 200mm mass of L2 0.05 kg

d1 60mm g 9800 mm/s²

3.1.1 Forward Kinematics

Given joint angles [θ_i1, θ_i2], forward kinematics determines the pose of platform [p_x, p_y, p_z, α, β, γ]. The closed form solution of forward kinematics is hard to get because the non-linear equations are complex. Newton Raphson method gives an approximated solution as follow:

Xn+1= Xn− [F⁰(Xn)]⁻¹F (Xn) (48) where Xn is the current pose of the platform and the next pose Xn+1 is determined by F (Xn) and its derivative with respect to Xn. Similarly, the joint angles Fi(Xn) = θi= Fpi(pi) and its derivative is:

θ˙_i= J_sip˙_i (49)

Cartesian Force Estimation of a 6-DOF Parallel Haptic Device

Cartesian Force Estimation of a 6-DOF Parallel Haptic Device

FANGHONG DONG

Sammanfattning

Abstract

Keywords:

Contents

Nomenclature

1 Introduction

2 Principles of Modeling and Control of Parallel Manipulators

3 Laboratory Equipment: Manipulator, Force Sen- sor and Virtual Environment