Control Implementation and Co-simulation of A 6-DOF TAU Haptic Device Yang Zhang

Control Implementation and Co-simulation

of A 6-DOF TAU Haptic Device

Yang Zhang

Master of Science Thesis TRITA-ITM-EX 2020:522 KTH Industrial Engineering and Management

Machine Design SE-100 44 STOCKHOLM

Examensarbete TRITA-ITM-EX 2020:522

Reglering, implementering och samsimulering

av en 6-DOF TAU haptisk enhet

Yang Zhang Godkänt 2021-04-26 Examinator Kjell Andersson Handledare Lei Feng Uppdragsgivare

Kungliga Tekniska Högskolan

Kontaktperson Lei Feng

Sammanfattning

Inom forskningsområdet virtuell verklighet definieras termen hatisk återgivning (haptic rendering) som processen för beräkning och generering av interaktiva krafter mellan det virtuella objektet och användaren. En av de största utmaningarna med haptisk återgivning är att stabilt simulera känslan av beröring av styv material för användare. Traditionella algoritmer fungerar när det gäller att simulera känslan av beröring av mjuk material, men när algoritmerna används för att simulera kontakt med materialer med stor styvhet kan det orsaka instabilitet hos haptiska enheter. Sådana instabilitet, bland annat svängning hos enheten, kan förstöra den virtuella miljöns exakthet och till och med skada användare.

Denna uppsats försöker ta itu med det ovanstående problemet genom att föreslå en ny designmetod. Metoden består av tre huvudsteg: modellering och linearisering med hjälp av ADAMS, design av LQR-positionskontroll, och verifiering med samsimulering (co-simulation). I det första steget skapas systemets simuleringsmodell med hjälp av ADAMS/View. Sedan linjäriseras denna icke-linjära ADAMS-multikroppsdynamikmodell. Modellen exporteras som linjära tillståndsmatriser med hjälp av ADAMS/Linear. I det andra steget designas en LQR-positionskontroll med hjälp av Matlab Simulink baserat på de exporterade matriserna tidigare för att simulera interaktioner med styv material, vilket skiljer sig från de traditionella kraftkontrollalgoritmer (force-control algorithms). I det sista steget utförs verifieringen av positionskontrollens prestanda genom att ställa in samsimulering (co-simulation) mellan ADAMS och Simulink.

En testkörning av denna föreslagna metod har utförs på TAU-enheten som tidigare utvecklades av KTH institutionen för maskinkonstruktion. TAU är en asymmetrisk parallellsrobot med sex frihetsgrader för att simulera kirurgiska ingrepp som borrning av hårda vävnader i ben och tänder. Resultaten visar att den linjära modellen som exporteras från ADAMS är tillräckligt korrekt, för den föreslagna positionskontrollen kan framställa en virtuell vägg med styvhet vid 105 N/m.

Master of Science Thesis TRITA-ITM-EX 2020:522

Control Implementation and Co-simulation of A 6-DOF TAU Haptic Device

Yang Zhang Approved 2021-04-26 Examiner Kjell Andersson Supervisor Lei Feng Commissioner

KTH Royal Institute of Technology

Contact person Lei Feng

Abstract

In the research area of virtual reality, the term haptic rendering is defined as the process of computing and generating the interaction force between the virtual object and the operator. One of the major challenges of haptic rendering is the stably rendering contact with a stiff object. Traditional haptic rendering algorithms performs well when rendering contact with soft objects. But when it is used to simulate contact with objects with high stiffness, the algorithm may cause unstable response of haptic devices. Such unstable behavior (e.g., oscillation of the device) can destroy the fidelity of the virtual environment and even hurt the user.

To address the above stability issues, a new design approach has been proposed in this paper. The proposed approach consists of three main process steps: modeling and linearization in ADAMS, LQR position controller design, verification with co-simulation. In the first step, a simulation model of the system is firstly created in ADAMS/View. Then this nonlinear ADAMS multi-body dynamics model is linearized and exported as a set of linear state space matrices with the help of ADAMS/Linear. In the second step, different from the traditional force-control algorithms, LQR position controller is developed in Matlab Simulink based on the exported matrices to emulate interactions with stiff objects. At last, the verification of control performance is carried out by setting up co-simulation between ADAMS and Simulink.

A case study implementation of this proposed method was performed on the TAU device which was previously developed by Machine Design department at KTH. TAU is an asymmetrical parallel robot with six degrees of freedom for the simulation of surgical procedures like drilling and milling of hard tissues of bones and teeth. The results show that the linear model exported from ADAMS is sufficiently accurate and the proposed controller can render a virtual wall with stiffness at the level of 105 N/m.

FOREWORD

Foremost, I would like to express my sincere gratitude to my supervisor, Professor Lei Feng, who led me into the world of dynamics and motion control. By taking the courses he offered, I realized that the use of electronics and software can greatly enhance the performance of mechanical products and had been drawn to the area of mechatronics design. His guidance helped me in all the time of research and writing of this thesis. I would also like to show my gratitude to Professor Kjell Andersson. He consistently gives me help and suggestions in ADAMS multi-body dynamics simulation. Without his guidance, it would be impossible for me to complete the dynamins simulation of such a complex mechanical system.

I would like to extend my sincere thanks to Ph.D. student Yang Wang for his support and guidance. He is always happy to share his latest research results and is always willing to give guidance and suggestions to my thesis project. Every meeting with him inspired me a lot and helped me to find a better way of approaching the problem.

Last but not the least, my thanks would go to my beloved family for their continuous support and encouragement. There are many difficulties in my study abroad experience, but my parents have always been helping me out of difficulties and supporting without a word of complaint. The compilation of this project would not have been possible without them.

Yang Zhang Sweden, September 2020

NOMENCLATURE

Notations and Abbreviations that were used in this master thesis were compiled and presented in this section.

Notations

Symbol

Description

K Total Kinetic Energy of System

L Total Potential Energy of System

M Mass Matrix

q Angular Position of Active Joints

X Workspace Cartesian Coordinates

𝜏 Active Joint Torques

Φ The Set of Equations of constraints Φ_𝑞 The gradient of constraints

Abbreviations

DAE Differential Algebraic Equation

DOF Degree of Freedom

LQR Linear Quadratic Regulator

LTI Linear Time Invariant

MIMO Multiple-input Multiple-output System

MBS Multi-body Simulation

ODE Ordinary Differential Equation

SI2 Stabilized Index Two Formulation

SISO Single-input Single-output System

TCP Tool Center Point

TABLE OF CONTENTS

2.1.1 Traditional haptic rendering algorithm ... 15

2.1.2 Problems with traditional algorithms ... 16

2.1.3 New position-control based approach to haptic rendering ... 16

2.2 Generalities on parallel robot ... 17

2.3 Dynamic models of parallel robots ... 19

2.4 Control techniques for robotic manipulators ... 20

2.4.1 Decentralized control ... 20

2.4.2 Centralized control ... 20

3 IMPLEMENTATION ... 23

3.1 Requirements specification ... 23

3.2.4 User-applied force ... 28

3.2.5 Measurements ... 29

3.3 Linearization in ADAMS ... 30

3.3.1 ADAMS linearization techniques ... 30

3.3.2 ADAMS linearization procedure ... 31

3.4 Discrete LQR controller design ... 33

3.4.1 Changing coordinates in state-space model ... 33

3.4.2 Zero order hold (ZOH) ... 34

3.4.3 Linear-quadratic regulator (LQR) design ... 34

3.5 Co-simulation between ADAMS and Simulink ... 35

3.5.1 Defining State Variables ... 36

3.5.2 Solver settings ... 37

3.5.3 Exporting ADAMS control Plant ... 40

3.5.4 Building co-simulation model in Simulink ... 41

4 RESULTS ... 43

4.1 Evaluation of the linearization in ADAMS ... 43

4.1.1 Linear model extracted from ADAMS ... 43

4.1.2 Model Validation ... 45

4.2 Co-simulation between ADAMS and Simulink ... 47

4.2.1 Virtual wall modeling ... 48

4.2.2 Simulation methodology ... 48

4.2.3 Simulation results ... 48

5 DISCUSSION AND CONCLUSIONS ... 53

5.1 Discussion ... 53

5.1.1 Modeling and linearization in ADAMS ... 53

5.1.2 LQR controller design ... 54

5.1.3 Co-simulation between ADAMS and Simulink ... 54

5.2 Conclusions ... 55

6 RECOMMENDATIONS AND FUTURE WORK ... 57

7 REFERENCES ... 59

APPENDIX A: GANTT CHART ... 61

1 INTRODUCTION

This chapter describes the background, the purpose, the limitations and the outline of the thesis project.

1.1 Background

With the rapid development of applications like virtual reality (VR) and tele-robotics, the research field of haptic device is getting more and more attention. In the area of virtual reality, haptic device works as a force-reflecting device which allows the human operator to touch, feel, manipulate simulated objects in a virtual environment and reflects feedback forces and torques to the operator. The main purpose of haptic device is to generate fidelity touch interactions that makes the user feel as if they are interacting with objects in the real world. Another popular application area of haptic devices is the area of tele-robotics. In the telerobotic system, the haptic device works as an interface between the operator and the remote world. It sends the command signal generated by the operator to remote the slave robot via wireless network and reflects forces and torques felt by the slave robot to the operator. In this way, many tasks that are too dangerous or impossible to complete for humans can be entrusted to the salve robot.

Figure 1. TAU parallel robot.

One of the research area of Machine Design department at KTH is to study haptic device that could achieve manipulation capabilities and force/torque feedback in six degrees of freedom (6-DoF) during simulation of surgical procedures like drilling and milling of hard tissues of bones and teeth [1]. After analyzing and comparing kinematic and dynamic properties of various proposed concepts, an asymmetrical parallel kinematic structure called TAU was selected as the optimal solution and prototype of the TAU parallel manipulator has been developed as Figure 1 shows. After the mechanical structure of haptic device has been determined, the next step is to integrate the TAU device with computer hardware and software. With the help of computer-controlled algorithm, the TAU device could generate fidelity touch interactions which could make user feel the interact with the simulated objects in virtual environment. The lab’s current research focuses on implementing different control algorithms to achieve high-performance force and torque feedback for stiff interactions (such as interactions with bones and teeth) while maintaining the stable behavior of the system.

1.2 To Continue

In most commercial haptic simulation software like CHAI3D, virtual objects with complex geometries are often modeled using a polygonal mesh. Figure 2 shows an example of a dolphin’s curved surface represented by the flat surfaces of triangular elements. By using this kind of modeling method, during the usage of haptic device the touch interaction of virtual objects with complex shapes can be simplified as the touch interaction with a collection of flat surfaces. In our project, the TAU parallel robot is designed for haptic simulation of surgical procedures in hard tissue such as bone structures and dental milling. Since the haptic rendering of hard tissues can be simplified as the haptic rendering of a set of stiff flat surfaces, this thesis aims to study the representative case of rendering an ideal virtual surface with very high stiffness. Three challenging problems in this field and the objectives of this project can be summarized as follows:

Figure 2. Example of polygon mesh.

The first challenge is the complexity of deriving dynamic model of TAU parallel manipulator suitable for controller design. In the model-based control system analysis and design, the first step is to derive the mathematical model of the dynamic system. In contrast to the open-chain serial manipulators, dynamic analysis of parallel manipulators presents an inherent complexity due to their closed-loop structure and their kinematic constraints [2]. There are a number of methods like Newton–Euler and Lagrange formulation can be used to attain dynamic equations for a parallel manipulator. However, all of these methods involve tedious derivation procedure by hand calculation. Since ADAMS can automatically formulate and solve the equations of motion for dynamic simulation in background once the ADAMS simulation model is created, we want to investigate the possibility of extracting the state space model which is efficient for real-time control directly from ADAMS to avoid hand calculation errors and increase modelling efficiency. The second problem is that although the traditional haptic rendering algorithms performs well for rendering contact with soft objects, when it comes to stiff object interaction rendering unstable behavior of device arises. A recent solution to this instability problem is to use position control scheme rather than traditional force-control scheme [3]. In this project, we want to apply this new position-control based method to the TAU device and investigate how much stiffness this new approach can achieve.

The last problem is related to the low efficiency of verifying the performance of the mechatronics system. In the typical design process of a mechatronics system, although the mechanical designers and controls designers work on the same system, they have to use different analysis software to

design process prior to the availability of physical prototypes. In this project, ADAMS is selected for multibody dynamics simulation and Matlab Simulink is selected for control system design. We want to investigate the possibility of co-simulation between two software.

1.3 Research questions

As above mentioned, the goal of this thesis is to extract state space model from ADAMS and design position-control system based on exported state space model to achieve haptic rendering of stiff objects. Focus is also on setting up co-simulation between ADAMS and Matlab Simulink to check the control performance of control system. In order to achieve the objectives of this thesis, the following research questions need to be answered.

 Could state space model be extracted from ADAMS and what is the accuracy of the extracted state space model?

 Could the problem of rendering stiff interaction be solved by using position controller design methodologies and how much stiffness can this approach achieve?

 Could co-simulation between MSC/ADAMS and Simulink be set up to increase the performance and flexibility of the simulation?

1.4 Delimitations

It is good practice to define and describe limitations of the project.

 This project only studies the haptic rendering of a stiff virtual wall. The rendering of virtual objects with complex geometries are not in the scope of this thesis.

 Surface property-dependent force-rendering algorithms which aim at conveying surface properties like texture and friction are not in the scope of this thesis project. Only geometry-dependent force-rendering algorithms are investigated.

 Analysis of flexible multibody systems is not in the scope of this thesis. Only rigid-body dynamics are considered.

 Controller is designed based on linear time invariant (LTI) model.

1.5 Outline

This thesis report contains a total of six chapters. In the first chapter, the overall situation and objectives of the thesis work is introduced. The theory and tools relating to haptic rendering, parallel robot, multi-body dynamics modelling and MIMO control techniques are presented in the second chapter. A new design approach for the development of control system of TAU device for better haptic rendering of stiff virtual objects is explained in the third chapter. The proposed approach consists of three main process steps: modelling and linearization in ADAMS, LQR position controller design, verification with co-simulation. The case study implementation on TAU device for high stiffness virtual wall rendering along with the simulation results are shown in the fourth chapter. Then conclusions are drawn and potential future research are suggested in the fifth and sixth chapters of this report.

2 FRAME OF REFERENCE

The reference frame is a summary of the existing knowledge and former performed research on the subject. This chapter presents the theoretical reference frame that is necessary for the performed design.

2.1 Haptic rendering

Virtual reality (VR) aims at creating a virtual environment where users can directly interact with virtual objects and perceive the effects of their actions in real time. To increase immersion perception of Virtual Reality, a variety of technologies have been employed to activate all five senses of human (vision, hearing, smell, taste, and touch) in simulated environment. Since the main operation of user during the usage of VR equipment is to interact with various objects in virtual world, the quest for generating and rendering accurate touch interactions in real time is getting more and more attention. In the research area of virtual reality, the term haptic rendering is defined as the process of computing and generating the interaction force between the virtual object and the operator. This section firstly introduces traditional haptic rendering algorithms and points out shortcomings of traditional algorithms and then introduces the concept of a new approach to haptic rendering based on position control theory.

2.1.1 Traditional haptic rendering algorithm

Figure 3. Schematic diagram of the traditional haptic rendering loop.

As the above schematic diagram of the traditional haptic rendering loop shows, there are six necessary steps to achieve accurate haptic rendering. When human operator operates haptic device to interact with the virtual world, sensors sense motion variables of haptic device in joint space. The collected information in joint space from sensors are then converted to Cartesian space through forward kinematics. The converted position and velocity in Cartesian space are utilized to represent the state of avatar in virtual environment. Here, an avatar refers to the virtual representation of the haptic interface through which the user physically interacts with the virtual environment [4]. In other words, the operator operates the haptic device in real world to control the avatar’s position inside the virtual environment. It is the virtual avatar that user utilizes to interact with objects in virtual environment. The collision detection algorithm plays an important role in haptic rendering loop. This algorithm detects if collision between objects and avatar occurs and reports necessary information about the collision (i.e. where, when, what extent of collision). For popular commercial haptic simulation software like Chai 3D, axis-aligned bounding boxes

(AABB) and oriented bounding box (OBB) collision detection algorithms are often used to achieve computationally-and memory-efficient collision detection. Once a collision is detected, force-response algorithm is called to compute the interaction force between avatars and virtual objects. Since the action and reaction forces between collision objects are represented in Cartesian space of the virtual environment, these forces should be converted to actuator forces or torques in joint spaces by pre-multiplying the transpose of Jacobina matrix. In this way, the desired actuator torques, normally motor torques, are obtained. The calculated torques can be regarded as the reference signals to the control algorithms. With appropriate open loop or closed loop force control algorithms, the interaction force between avatar and object can be applied to operator by haptic device.

2.1.2 Problems with traditional algorithms

For haptic rendering algorithms, it is essential to render realistic contact forces while maintaining stable behavior of haptic devices. Any unstable behavior of the device (e.g., oscillation of the device) can destroy the fidelity of the virtual reality and even hurt the user. When haptic rendering algorithms discussed above are applied to render contact with soft objects, it performs well. However, when it comes the scenario of stiff object interaction rendering, unstable behavior of haptic device arises. The reasons for this instability are explained as follows. The control system of haptic device is a closed-loop, discrete-time system while the operation of the user is in continues time, which means the sampled avatar position will always lag behind the avatar’s actual continuous-time position [4]. Based on Gillespie [5]’s research, extra energy emerges from the difference between discrete and continues processes and this phenomenon is termed as “energy leak”. It is this energy leak that causes the instability of haptic device when rendering stiff interaction. Currently, there are two methods to solve this problem. The first one is increase sampling frequency. Due to limited hardware capacity, the required high sampling rates cannot always be met. The second method is to introduce damping to dissipate the leaked energy. Two measures can be employed to increase damping. The first one is to increase the mechanical damping of haptic device. However, when user operates the device moving freely in workspace without interacting with any object, user will clearly feel the effects of damping of the device, which violates the design criterion of haptic device. Here the design criterion is that except for forces generated during the interaction with the virtual objects, the user should not feel the presence of the haptic device. Colgate [6] proposed the concept of “virtual coupling” to solve the above problem. The limitation of virtual coupling method is that it cannot render very high level of stiffness.

2.1.3 New position-control based approach to haptic rendering

Given the problem of traditional haptic rendering algorithm in the scenario of interacting with a stiff virtual object, Wang et al. [3] proposed a new haptic rendering method based on position controller. The Yang Wang’s article indicates that for the problem of rendering very high or even infinite stiffness, focus should be more on avoiding penetration into the object rather than minimizing the error of the rendered force. As the schematic diagram in Figure 4 shows, there are five necessary steps for this new approach.

Figure 4. Schematic diagram of the new haptic rendering loop.

When the human operator operates haptic device to interact with the virtual world, sensors sense position and velocity of the haptic device in the joint space. The collected joint space information from sensors is then converted to Cartesian space through forward kinematics. The converted position and velocity in Cartesian space can be utilized to represent the state of avatar in virtual environment. The collision detection algorithm is called to detect if collision between objects and avatar occurs and reports information of collision. If collision occurs, instead of calling force-response algorithm as the traditional algorithm to calculate interaction force between avatars and virtual objects, the newly proposed approach firstly converts the position of collision point expressed in Cartesian space to joint space through inverse kinematics and then uses converted collision position as the reference signals to the position controller. With carefully designed position control algorithms, the outputs of the position controller are the estimated interaction forces expressed in joint space. These calculated interaction forces are also the required actuator forces or torques for haptic rendering.

2.2

Generalities on parallel robot

Unlike serial robots (Figure 5 left) which are designed as an open-loop kinematic chain composed of a serial of links connected by motor-actuated joints, a parallel robot is a closed-loop kinematic chain mechanism where the moving platform (end-effector is often installed on this moving platform) is linked to the base by at least two independent kinematic chains. The typical structure of parallel is shown in Figure 5 right. Compared with series robots, parallel robots have two main disadvantages. One is the relatively small workspace. The other is the complicated forward kinematics analysis of parallel manipulators. The solution of forward kinematics equations usually involves systems of nonlinear equations which are highly coupled and in general have no closed form and unique solution [2]. Iterative or recursive methods are always used to solve this kind of problem. On the other side, due to its unique structure, parallel robots have various advantages and can be summarized as follows.

Figure 5. Schematics of serial and parallel robot.

First of all, since the moving platform of the parallel robot is supported by at least two kinematic chains, load applied on the platform can be distributed and shared by several kinematic chains of the system. Compared with the open-loop structure of the serial robot, the parallel robot has the advantages of high structure stiffness, high stability and high load carrying capacity. Secondly, since serial robot consists of a serial of rigid links connected by joints, the position error on the end-effector is caused by the accumulation and amplification of the errors of the joints. For serial robot, small positioning errors in joint actuators would lead to large positioning error in the end-effector. Therefore, the serial robot has the disadvantages of large positioning error and low accuracy. With relatively shorter kinematic chain of each limb, parallel robot could achieve very high positioning accuracy. Thirdly, the actuation motors and transmission system of the serial robot are mounted on the movable robot arm, which increases the inertia of the system and deteriorates the dynamic performance. With all the actuators installed on or near the fixed base, it possible for parallel robot to reduce the mass of the movable links and increase the performance of system dynamics. At last, unlike serial manipulators, inverse kinematic analysis of parallel robots is very simple. Online calculation of robot’s controller always involves mapping the position and orientation of end-effector to joint space through inverse kinematics in real time. For the serial robot, some special designs of structure or simplification of inverse kinematic model have to be made to reduce the burden of online computation. For the parallel robot, joint variables can be easily computed via closed-form solution of mathematical model.

Because of above unique advantages, in recent years, a great deal of research on parallel robots has been carried out worldwide, and a large number of parallel mechanism systems have been built for various applications. As Figure 6 shows, the major industrial applications of these devices are surgical instruments used in medical environment, pick-and-place in food industry, motion simulators and haptic device [7].

Figure 6. Industrial applications of parallel robot (upper left: medical environment, upper right: pick-and place device, bottom left: motion simulator, bottom right: haptic device).

2.3

Dynamic models of parallel robots

Dynamic analysis of robot studies the relationship between force and motion of robot mechanisms. In contrast to the open-chain serial manipulators, dynamic analysis of parallel manipulators presents an inherent complexity due to their closed-loop structure [2]. To conduct model-based control system analysis and design, control engineers are interested in deriving the inverse dynamic model of parallel robot which computes the required forces and torques of actuators based on the motion of robot in the following explicit form [8]:

𝑀(𝑋)𝑋̈ + 𝐶(𝑋, 𝑋̇)𝑋̇ + 𝐺(𝑋) = 𝐹 (1)

Where 𝑋 is a vector of the generalized coordinates, 𝑀(𝑋) denotes the system mass matrix, 𝐶(𝑋, 𝑋̇) denotes the Coriolis and centrifugal matrix, 𝐺(𝑋) denotes the gravity vector, and F denotes the generalized force/torque.

Several approaches have been proposed for the derivation of dynamic model of parallel manipulators, including Newton Euler formulation [9, 10] and d’Alembert principle of virtual work [11, 12]. However, the inverse dynamic model derived from above methods is implicit and non-detailed which presents the inverse dynamics in a non-structured way [8]. Unlike explicit model in equation (1), this kind of implicit dynamic model makes the controller design difficult to implement. To overcome this problem, for parallel robot the Lagrangian formalism is often used to derive and analyze the equations of motion.

The general equation for Lagrange dynamic formulation is expressed as: 𝑑 𝑑𝑡( 𝜕𝐿 𝜕𝑋̇) − 𝜕𝐿 𝜕𝑋= 𝐹 (2)

Where L=K-P, K is the total kinetic energy of the system, P is the total potential energy of the system, 𝑋 is a vector the generalized coordinates and F denotes the generalized force/torque. As the total kinetic energy can be defined using the mass matrix:

𝐾 = 1 2𝑋̇

𝑇_{𝑀(𝑋)𝑋̇ (3)}

Substitute equation (3) into equation (2). The Lagrange equations can be written in an explicit form: 𝑀(𝑋)𝑋̈ + 𝑀̇(𝑋)𝑋̇ −1 2 𝜕 𝜕𝑋(𝑋̇ 𝑇_{𝑀(𝑋)𝑋̇) +}𝜕𝑃(𝑋) 𝜕𝑋 = 𝐹 (4)

2.4

Control techniques for robotic manipulators

A robotic manipulator with multiple degrees-of-freedom is usually formulated as a multi-input-multi-output (MIMO) and nonlinear model. In general, control strategies for robot can be divided into two categories. The first one is the decentralized control and the other approach is centralized MIMO control approach.

2.4.1 Decentralized control

Decentralized control has been widely implemented in industry. This kind of control strategy focuses on analysing and designing multivariable controller for multivariable plant based on the framework of SISO (single-input- single-output) system. In decentralized control every input signal of the process is determined only by a feedback from one output and there are as many independent controllers as the number of the process inputs. For instance, instead of treating the parallel robot with multiple degrees-of-freedom as a multi-input-multi-output (MIMO) system, a single PD or PID controller is tuned for each motor on active joint. One of the biggest problems of decentralized control is that the coupling of system dynamics always exists, which means each input determined be controller may affect several outputs of the system. This kind of interactions between the control loops make decentralized control strategy difficult to implement. Relative Gain Array (RGA) is one of the useful tools to determine appropriate input-output pairing. With carefully chosen input-output pairings, the undesirable interactions can be minimized and the closed-loop performance and stability of the decentralized control system can be improved. The biggest advantage of decentralized control is that the implementation is very straightforward and the controller can be designed without knowing the detailed knowledge of complex dynamic model of the system. The PD or PID decentralized controllers are usually tuned by trial and error in simulation or through experiments on actual physical device. However, without dynamics model of controlled plant to perform model-based controller design, the tuning process of controller might be time consuming, highly relying on designer’s experience and the designed controller

combined with optimization theory, resulting in the now famous state space optimal control theory. The principle of optimal control can be summarized as follows: for a controlled system, find an optimal solution from a class of allowed control schemes to drive the initial state of the system to the target state while the specific performance criterion (cost function) is optimal. From the practical point of view, Linear Quadratic Regulator (LQR) and H-infinity (𝐻_∞) control are two commonly used control strategies. In this section only LQR controller will be discussed in detail. A Linear-Quadratic Regulator (LQR) is a state feedback optimal controller and its control law is derived based on the principle of dynamic programming. As a technique for sequential optimization, the essential feature of dynamic programming is to transform a complex optimization problem into a sequence of simpler subproblems. With the help of dynamic programming, the infinite-horizon linear quadratic optimal control can be summarized as follows. Given a linear discrete system in state space format:

𝑥_𝑡+1 = 𝐴𝑥_𝑡+ 𝐵𝑢_𝑡 (5)

The controller design problem is formulated as to minimize the following infinite-horizon quadratic cost function of the states and the control signals:

𝐽∞ = ∑ 𝑥𝑡𝑇 ∞

𝑡=0

𝑄𝑥𝑡+ 𝑢𝑡𝑇𝑅𝑢𝑡 (6)

Where 𝑄1 is positive semidefinite matrix and 𝑄2 is positive matrix.

A static linear feedback (i.e. stationary LQ-controller) 𝑢𝑡 = −𝐿𝑥𝑡 can by calculated by

𝐿 = (𝑅 + 𝐵𝑇𝑃𝐵)−1𝐵𝑇𝑃𝐴 (7)

Where P satisfies the algebraic Riccati equation:

𝑃 = 𝑄 + 𝐴𝑇𝑃𝐴 − 𝐴𝑇𝑃𝐵(𝑅 + 𝐵𝑇𝑃𝐵)−1𝐵𝑇𝑃𝐴 (8) Unlike traditional pole placement method to directly modify the eigenvalues of the closed-loop system to achieve desired control performance, for linear quadratic optimal control strategy, by choosing appropriate relative values of weight matrices 𝑄₁ and 𝑄₂, a trade-off between achieving efficient regulation or energy-efficient control can be easily made.

3 IMPLEMENTATION

In this chapter the working process is described. In this chapter, a new design approach for the development of control system of TAU device for better haptic rendering of stiff virtual objects is explained in detail. The proposed approach consists of three main process steps: modeling and linearization in ADAMS, LQR position controller design, verification with co-simulation.

3.1 Requirements specification

3.1.1 Device Specifications

The TAU haptic device has been developed by Machine Design department at KTH for the studies of simulation of surgical procedures like drilling and milling of hard tissues of bones and teeth [2]. The device has asymmetric parallel structure with six degrees of freedom, which consist of an I-column fixed to the ground, a moving platform and three kinematic chains: chain1, chain 2 and chain 3. Chain 1 and chain 2 are symmetrical while chain 3 is asymmetrical [13].

Figure 7. Structure of TAU parallel robot.

Figure 7 shows the detailed CAD model of the TAU parallel robot. Three motors (motor 1,3 and 5) are mounted on the I-column in a line to provide actuation torques to rotate revolving platform 1,2 and 3 around the I-column. Motors 2, 4 and 6 are installed on the revolving platforms to drive active upper arms A1, A2 and A3 rotate around specific axis, respectively.

For symmetric chain 1 and chain 2, two passive lower arms P1 and P2 are connected to active upper arms A1 and A2 through universal joints and connected to moving platform by spherical joints. Besides, extra passive links in chain 1 and 2 are introduced to increase the overall structural

stiffness of the device. As for asymmetric chain 3, a passive revolute joint connects active arm A3 to passive arm P3 and a spherical joint connects P3 to the moving platform.

Figure 8. Kinematic structure of TAU parallel robot.

Figure 8 shows the kinematic structure of TAU device and design parameters of the final design are listed in Table 1.

Table 1. Design parameters

Parameter Value

Radius of the platform 55 mm

𝐿₁ 138 mm

𝐿₂ 180 mm

𝐿3 91.5 mm

Length of column I 274.5 mm

3.1.2 Control requirements

The TAU parallel robot is designed to facilitate haptic interaction with stiff tissues for medical training. However, as discussed in Section 2.1.2, stability issues arise when simulating interactions with stiff objects if traditional force-control algorithms are implemented. To overcome this instability problem, a position-controller-based haptic rendering approach proposed Wang et al. [3] is employed in this project. The principle of this position-controller-based method has been described in detail in Section 2.1.3. For the position controller, the control goal is to keep the TCP at a given point in the workspace and reject disturbance on the TCP. Here we define user-applied

Table 2. Design parameters

Overshoot < 2%

Rise time < 0.05 s

Settling time < 0.1 s

Joint position deviation < 0.001 rad

TCP position deviation < 1 mm in all xyz axis directions TCP orientation deviation < 0.001 rad in all xyz axes

3.2 Modelling in ADAMS

ADAMS is the most widely used multibody dynamics and motion analysis software in the world. After the virtual prototypes of mechanical systems are created in ADAMS simulation environment using extensive pre-defined libraries, the software checks model and automatically formulates and solves the equations of motion for kinematic, static, quasi-static, or dynamic simulations. Due to its various advantages, ADAMS was selected as the Multibody System (MBS) simulation software in this project. This section shows the procedure of creating virtual prototype of TAU parallel robot in ADAMS/View.

3.2.1 Unit system

The unit system that was followed throughout the modelling and simulation procedure was MMKS with length in millimetre, mass in kilogram and time in second.

3.2.2 Constrains

According to the theory of multibody dynamics, constraints remove the degrees of freedom of the system by adding algebraic constraint equations to the equations of governing system. When a constraint is created in ADAMS to restrict relative movement between parts, a coordinate system called I marker is created to be fixed on the first part while the other coordinate system called J marker is placed on the second part, the relative movement of two parts is simplified as the relative movement of two marker [14]. In this way, constraints in a physical mechanical system can be transferred into an ideal mathematical model. With the help of various kinds of constraints provided by ADAMS, designers could remove different types and number of DOF and make parts move with respect to one another in a prescribed way. The following part describes how to use idealized joints to connect components of TAU haptic device.

3.2.2.1 Active joints

An active joint is a joint controlled by an actuator. There are totally six active joints in TAU parallel manipulator as Figure 9 shows. For all active joints, revolute joints are defined as Figure 10. A revolute joint removes all three translational DOF and two of the rotational DOF between two parts. With the help of this single-degree-of-freedom joint, relative motion between two parts can only occur about the common rotational axis.

Figure 9. Positions of six active joints.

Figure 10. Definition of active joint in ADAMS (joint 2). 3.2.2.2 Passive joints

A passive joint does not have any actuator. As passive joints are neither sensed nor controlled, the coordinate of a passive joint is a function of the coordinates of active joints as well as the mechanical construction of the robot. For the TAU parallel robot, a passive lower arm is connected to an active upper arm through passive universal joint. As Figure 11 shows, since the CAD model of a universal joint consists of a cross-shaped intermediate member called the spider, instead of directly using pre-defined universal joint in ADAMS library to constrain relative motion between

and a rotational joint as Figure 12 shows. To define this constraint, three revolute joints are used and the rotational axis of three rotation joints are perpendicular to each other.

Figure 11. Modeling of universal joint in ADAMS.

Figure 12. Modeling of spherical joint in ADAMS. 3.2.2.3 Fix joints

Fix Joints in ADAMS are joints that lock all DOF between two bodies. Firstly, to fix three motors on the main column and other three motors on moving platforms, six fix joints were used. In addition, due to the fact that robotic arms are assembled by components of different material and there is no relative movement between these parts, fix joints were used to define the connection relationship of the parts in the assembly.

3.2.3 Motor torques

To model motor torque applied on the active joint, rotational action-reaction SFORCE (signal component force) was applied at the center of active revolute joint. In the ADAMS, if a rotational single-component torque is defined, ADAMS would create an I marker fixed on action body and a J marker on reaction body. The z-axis of the I marker and J marker are parallel and pointed in the same direction. Since pure torques are independent of location, these two markers only specify the two parts affected by the torque and the z-axis of the marker specifies the axis about which the torque is applied [14]. Take the definition of motor torque 2 as an example, in this case, the torque should act between the active arm 1 and the moving platform 1 where the motor 2 are mounted. As Figure 13 shows, take active arm 1 as action body and the moving platform (including DC motor) as reaction body, I marker is created to be fixed on active arm 1 and J marker is created on moving platform. The effects of the torque could be represented as the action marker rotate relative to reaction marker around the z axis.

Figure 13. Definition of motor torque in ADAMS.

3.2.4 User-applied force

This project studies the scenario of rendering a stiff wall whose stiffness is very large. To model the force imitating the human operator pressing the tool center point of platform against the virtual wall, space fixed single component force was applied on the center of the moving platform in ADAMS. For this kind of force, the action force is applied on part while the reaction force is applied to the ground (i.e. reaction force has no effect on the model). Besides, as Figure 14 shows the direction of the force never changes during the simulation while the action body moves, which is consistent with the situation where user pushes the wall. The definition of user-applied force is shown in Figure 15.

Figure 14. Direction-invariant characteristics of space fixed force.

Figure 15. Definition of user-applied force in ADAMS.

3.2.5 Measurements

ADAMS Measure functions can be used to monitor and analyze the response of system. For the physical TAU device, the angular positions and angular velocities of all six active joints are measured by encoders in DC motors. To monitor these values in ADAMS simulation environment, the AZ function was used to measure the joint angular position and the WZ function was used to monitor the joint angular velocity. Figure 16 shows the measurement setting of active joint 2. Besides, as the project studies the scenario of rendering a stiff wall, the position and orientation of moving platform when the user-applied force is applied at TCP are also of interest. Thus, ADAMS functions DX, DY, DZ and ROLL, PITCH, YAW were chosen to measure the position and orientation of moving platform during the simulation.

Figure 16. Measurements of joint’s angular position and velocity.

3.3 Linearization in ADAMS

The TAU parallel manipulator is a nonlinear, complex and multivariable system. To design control system for such a non-linear system with classic control tools like pole placement technique or linear quadratic (LQ) control technique, it is common to linearize the nonlinear system around the operating point. Since the linearized model is an approximation of the system dynamics around the chosen operating condition, the controller is then designed based on the linear model. However, as Lee et al. [15] proposed, in general the dynamic equations of a haptic device are given as higher degree nonlinear equations and it is impossible to obtain satisfactory linear system with a general linearization method. To solve this problem, ADAMS/linear is used in this project to linearize the nonlinear multibody system with the help of its build-in linearization function. In this section, first of all the linearization techniques of ADAMS are discussed briefly. After that the concrete procedure of generation of linear model with the help of ADAMS/linear is presented.

3.3.1.1 ADAMS FORTRAN solver

ADAMS FORTRAN based solver uses the following technique to generate a linearized dynamic model from a nonlinear model in Lagrange Equations of the First Kind. Firstly, the system model is created in ADAMS, then ADAMS FORTRAN solver uses the state-space reduction (SSR) approach to reduce the differential-algebraic equations of the system to a set of ordinary differential equations. The obtained ordinary differential equations are then linearized to get a linear approximation of the system dynamics around certain operating condition. The detailed information about linearization technique based on ADAMS FORTRAN is presented in [16]. According to [16], since ADAMS FORTRAN solver uses system's Jacobian matrix to reduce differential-algebraic equations to ordinary differential equations, it is crucial that the Jacobian matrix be time invariant. In this case, the application of FORTRAN solver’s linearization technique has following limitations. First of all, the nonlinear model should be linearized only under special circumstances such as during states of steady motion or while it is in a state of static equilibrium [16]. Secondly, nonlinear dynamic model with nonholonomic constraints cannot be linearized. Despite the above limitations, linearization method based on FORTRAN solver is still suitable for our project for the following reasons. Firstly, due to the relatively small workspace of TAU device (50 × 50 × 50 𝑚𝑚3), our project aims at studying dynamics behavior of the nonlinear system in the neighborhood of the centroid of cube workspace, which means the ADAMS model are linearized in a state of static equilibrium at the centroid point. Secondly, links of TAU parallel robot are connected by revolute joints, universal joints and spherical joints. All of the joints are holonomic joints.

3.3.1.2 ADAMS C++ solver

Unlike FORTRAN based solver that perform differential-algebraic equations to ordinary differential equations transformation followed by a linearization, ADAMS C++ based solver uses a different method to linearize the non-linear dynamic model. The approach does not explicitly reduce the problem to second-order differential form; instead, it augments the set of equations of motion and computes the quantities of interest as the solution of a multiple right-side sensitivity linear system [17].

Compared with FORTRAN based solver, with the help of the more general algorithm, ADAMS C++ solver could handle linearization under all operating conditions (i.e. both static and dynamic). Besides, this method is suitable for dynamic system with both holonomic and nonholonomic constraints.

3.3.2 ADAMS linearization procedure

Before any linear analysis in ADAMS, inputs and outputs of TAU parallel robot must be specified in ADAMS. ADAMS uses state variables to specify the inputs and outputs of the ADAMS model [18]. The TAU device is actuated by 6 DC motors. Thus, six input state variables representing motor torques were defined as Figure 17 left shows. As the angles and velocities of 6 active joint are measured by encoders of DC motors, the measurements created in section 3.2.5 were used to define 12 output variables as Figure 17 right shows.

Figure 17. Definition of state variables.

3.3.2.2 ADAMS kinematic simulation and static simulation

As stated before, the linearized model is an approximation of the actual system’s behavior in the vicinity of a certain linearization point. To study the dynamic behavior of the TAU device with a relatively small workspace (50 × 50 × 50 𝑚𝑚3), the center of the cubic workspace is chosen as the operating point to perform the linearization. The required active joint angels to make the tool center point (TCP) of the moving platform stay at the center of the cubic workspace and to make the orientation of a moving platform parallel to global XOY plane were calculated by ADAMS and are given below.

Table 3. Active joint angles at operating point

Joint 1 Joint 2 Joint 3 Joint 4 Joint 5 Joint 6

-49.6425° 0.0000° 49.6425° 0.0000° 0.0000° 24.0000°

Given the values of active joints, ADAMS kinematic simulation is conducted to calculate the required active joint torques to keep the TAU manipulator stay at the equilibrium position. The calculated joint torques are shown in Table 4.

Table 4. Motor torques at operating point

Motor 1 Motor 2 Motor 3 Motor 4 Motor 5 Motor 6

329.2573 𝑁 ∙ 𝑚𝑚 -196.7850 𝑁 ∙ 𝑚𝑚 -329.2573 𝑁 ∙ 𝑚𝑚 -0.4797 𝑁 ∙ 𝑚𝑚 0.0000 𝑁 ∙ 𝑚𝑚 849.9554 𝑁 ∙ 𝑚𝑚

Before the linearization operation, ADAMS static simulation was performed first to give ADAMS/linear necessary information about the linearization point. By default, ADAMS/linear assumes the function expressions of all input state variables are zero (Figure 17). To perform static analysis, in the function expression box of the joint torque each input state variable should be added to the corresponding equilibrium torque as Figure 18 shows. Besides, the input state variable is scaled by 1000 to convert the unit of joint torque from 𝑁 ∙ 𝑚𝑚 to 𝑁 ∙ 𝑚.

Figure 18. Definition of motor torque for linearization 3.3.2.3 Model exportation

After the static simulation is finished, the next step is to select ADAMS Linear Mode option. In the pop-up dialogue, specify the plant input and plant output using the created state variables and select Matlab as the format of the exported state space model. After the linearization operation, seven files are created by ADAMS. Four files contain the A, B, C and D-matrices, two files contain information about the inputs and outputs and one file contains information about the internal state variables of the system [18].

3.4 Discrete LQR controller design

3.4.1 Changing coordinates in state-space model

The exported linear model from ADAMS is in the following state space form: {𝑥̇ = 𝐴𝑥 + 𝐵𝑢

𝑦 = 𝐶𝑥 (9)

Where 𝑢 = [𝜏₁, 𝜏₂, 𝜏₃, 𝜏₄, 𝜏₅, 𝜏₆]𝑇 _{are motor torques applied on active joints, 𝑦 = [𝑞̇, 𝑞]}𝑇 _{in which} 𝑞 = [𝑞1, 𝑞2, 𝑞3, 𝑞4, 𝑞5, 𝑞6]𝑇 are the active joint angles.

In ADAMS/Linear, plant state vector x is automatically determined by ADAMS solver to get the best numerical conditioning of the state matrices. Due to different linearization schemes, Adams FORTRAN solver uses a subset of global displacements and Euler angles of part coordinate systems to build the linearized model while ADAMS Solver C++ chooses a subset of global displacements and global rotations for the state space representation. However, in this project the automatically selected state vector is not a good choice for controller design. For TAU parallel robot, the encoders in the DC motors are used to monitor the active joint angles and velocities. Thus, it is necessary to transform the automatically generated state variables to variables related to joint angles and velocities.

Since the characteristic equation of the system is invariant when new states are introduced through the nonsingular transformation matrix, a new state vector that equals to output vector y can be obtained through the transformation and the new state space model is shown below:

{𝑥̃̇ = 𝐴̃𝑥̃ + 𝐵̃𝑢

Where 𝐴̃ = 𝐶𝐴𝐶−1, 𝐵̃ = 𝐶𝐵, 𝐶̃ = 𝐼12×12, 𝑥̃ = 𝑦 = [ 𝑞̇, 𝑞]𝑇 in which 𝑞 = [𝑞1, 𝑞2, 𝑞3, 𝑞4, 𝑞5, 𝑞6]𝑇 are the active joint angles, 𝑢 = [𝜏1, 𝜏2, 𝜏3, 𝜏4, 𝜏5, 𝜏6]𝑇 are motor torques applied on active joints.

3.4.2 Zero order hold (ZOH)

Figure 19. Computer-controlled system

The control plant generated from ADAMS/Linear in equation (10) is in continuous time. For a computer-controlled system where computer receives measurements from the process at discrete times and generates new control signals at discrete times, a discrete-time representation of the continuous-time state-space model should be found. Typically, in a computer-controller system, a continuous-time physical process is connected to a computer via A-D and D-A converters as Figure 19 shows. With the assumption that the D-A converter holds the analog signal constant until a new conversion is commanded [19], Zero-Order Hold (ZOH) method could be applied to continuous time state space model and the converted discrete state space model is given below.

{𝑥𝑡+1 = Φ𝑥𝑡+ Γ𝑢𝑡 𝑦𝑡 = 𝐶𝑥𝑡 (11) Where Φ = 𝑒𝐴̃ℎ (12) Γ = ∫ 𝑒𝐴̃𝑠 ℎ 0 𝑑𝑠𝐵̃ (13)

Here, h is the period of periodic sampling.

3.4.3 Linear-quadratic regulator (LQR) design

An integral action is usually added to standard LQR controller to eliminate steady state error and achieve error-free reference tracking. For a discrete linear system, the expression of integral action is given by:

𝑖𝑡+1 = 𝑖𝑡+ ℎ(𝑟𝑡− 𝑦𝑡) = 𝑖𝑡+ ℎ(𝑟𝑡− 𝐶𝑥𝑡) (14) Where h is the period of periodic sampling. For this project, the sampling frequency is 1000Hz. And 𝑟𝑡 is the reference signal.

𝑢𝑡 = −𝐿𝑥 − 𝑙𝑖𝑖𝑡 (16) Since the extended system state consists of both the system state and the integral state, the gains L and 𝑙_𝑖 should be selected to minimize the cost

𝐽 = ∑ (∆𝑥𝑡 ∆𝑖_𝑡) 𝑇 𝑄 (∆𝑥𝑡 ∆𝑖_𝑡) + ∆𝑢𝑡𝑇 ∞ 𝑡=0 𝑅∆𝑢_𝑡 (17)

By changing the relative magnitude between the elements in the weighting matrices Q and R, a compromise could be made between speed of the response and the size of the control signal. An initial guess for the weight parameters is obtained by following Bryson’s rule. According to Bryson’s rule, the weights should be picked inversely proportional to the square of the maximum allowed magnitude of the corresponding input or output [20]:

𝑞_𝑖~ 1 𝑦_𝑖2 𝑟_𝑖~ 1

𝑢_𝑖2

Where 𝑞_𝑖 and 𝑟_𝑖 are elements in the weighting matrices Q and R and 𝑦_𝑖 and 𝑢_𝑖 is the maximum allowed magnitude of signal.

Based on the initial guess, the relative weights were found by trial and error to meet the control requirement listed in Table 2. After the iterative process, the final choice of the weight matrices are listed below.

𝑄 = ( 𝐼_6×6 0_6×6 0_6×6 06×6 106× 𝐼6×6 06×6 0_6×6 0_6×6 108 × 𝐼_6×6 ) 𝑅 = 𝐼_6×6

3.5 Co-simulation between ADAMS and Simulink

With the popularization and development of computer technology, the use of computer software for computer-aided design and analysis has become an important part of product development. A variety of commercial software has been developed in different fields. However, these kinds of software usually work independently of each other and lack the ability to work together. As a result, some problems in the design can only be discovered during verification and testing process on the physical prototype, which will lead to a decrease in work efficiency and cause a lot of waste of resources. For instance, in the typical design process of a mechatronics system, although the mechanical designers and controls designers work on the same system, they have to use different analysis software to build their own models. For mechanical designers, multi-body simulation (MBS) software is employed to analyze dynamic behavior of the system while for controls designers, software containing various build-in control strategies and analysis tools like Matlab Simulink are used for control system development. The first time the two designs are brought together is during physical prototype testing. If a problem occurs during the interaction between the controls design and the mechanical design, the engineers from two different fields have to rework and go through the entire verification process again [14].

Figure 20. Co-simulation between ADASM and Simulink.

In this project, the control performance of the LQR controller which was designed based on the linear model needs to be tested on the non-linear virtual prototype created in ADAMS. To achieve this goal, co-simulation between ADAMS and Simulink has to be developed. Figure 20 describes how ADAMS and Simulink communicate during simulation. The principle of co-simulation between ADAMS and Simulink is to let the ADAMS solves the mechanical system equations and the Simulink solves control system equations. ADAMS and Simulink exchange data in real-time to complete the simulation of the mechatronics system. Since ADAMS’s solver is better solving complex differential algebraic equation of multibody dynamics and Simulink is good for control system analysis, an accurate result of multi-domain simulation can be obtained. This section describes how to set up the co-simulation.

3.5.1 Defining State Variables

During co-simulation, ADAMS model works as a subsystem of the whole mechatronic system. As ADAMS state variables act as the parameters that Simulink and Adams exchange with each other, before ADAMS model can be read in Simulink, state variables must be defined for all the inputs and outputs of the system [14].

In this project, co-simulation should be performed to check the control performance of LQR controller under the disturbance force applied by human operator. Thus, 7 input states variables were defined. They were 6 motor torques applied on active joints and 1 user-applied disturbance force. As Figure 21 shows, in order to transfer the values calculated by Simulink to ADAMS through input state variable, ADAMS function VARVAL() was used.

Besides, 18 output state variables were defined. They were 6 joint angles, 6 joint velocities and 6 variables representing position and orientation of moving platform. The measurements created in section 3.2.5 were used to define these output variables as Figure 17 shows.

3.5.2 Solver settings

In ADAMS, after the model is created, based on the Lagrange formulation, the equations of motion are automatically formulated as following differential algebraic equations (DAE):

𝑀𝑞̈ + Φ𝑞𝑇𝜆 − 𝐴𝑇𝐹(𝑞, 𝑞̇) = 0 (18)

Φ(𝑞, 𝑡) = 0 (19)

Where

M is the mass matrix of the system

𝑞 is the set of coordinates used to represent displacements Φ is the set of equations of constraint

Φ𝑞 is the gradient of the constraints

𝐴𝑇𝐹(𝑞, 𝑞̇) is vector of generalized forces corresponding to the set of coordinates 𝑞

To solve above differential algebraic equations by using standard numerical integration method, second-order differential equations should be transformed to first-order equations by introducing velocities as new variables [16]. The resulting first-order differential combined with algebraic equations are then integrated by ADAMS build-in numerical integrator.

In general, Index 3 formulation is utilized to transform the second-order differential equations to first-order equations. By substituting the new variable 𝑢 = 𝑞̇ into the above equations, equations (18) and (19) are converted to first-order form:

𝑀𝑢̇ + Φ_𝑞𝑇𝜆 − 𝐴𝑇_{𝐹 = 0 (20)}

𝑢 − 𝑞̇ = 0 (21)

Φ = 0 (22)

The corresponding Jacobian matrix of above equations can be calculated as [18]:

( 𝑀 ℎ𝛽₀ 𝑀𝑢̇ + Φ𝑞𝑞 𝑇_{𝜆 − 𝐴}𝑇_{𝐹 = 0 Φ} 𝑞𝑇 𝐼 − 𝐼 ℎ𝛽₀ 0 0 Φ_𝑞 0 ) (23)

This Jacobian matrix becomes ill-conditioned as the integration step size h decreases, and becomes singular as h moves closer to 0, which means Index 3 formulations become unstable at small step sizes. In our haptic project, to provide an accurate force feedback, the haptic rendering loop should be kept operating at 1000 Hz, which means that ADAMS and Simulink should exchange data every 0.001s during co-simulation. In this case, the unstable behavior of I3 formulations under small step sizes would cause problem to the project. Besides, as equations (20-22) have an Index of 3, integration error of velocity cannot be monitored. The LQR controller in our project has been

derived under the assumption of knowledge of the full state vector, which means joint angles and velocities should be measured accurately by the control system. Failure to provide accurate velocity values will give an unreliable simulation results when checking the performance of controller during co-simulation.

To overcome the above problems, SI2 (Stabilized Index Two) Formulation is chosen in this project. The equations of a mechanical system after index reduction by SI2 Formulation are formulated as [14]:

𝑀𝑢̇ + Φ_𝑞𝑇𝜆 − 𝐴𝑇_{𝐹 = 0 (24)}

𝑢 − 𝑞̇ + Φ_𝑞𝑇𝜇 = 0 (25)

Φ̇(𝑞, 𝑦, 𝑡) = 0 (26)

Φ(q, t) = 0 (27)

By adding time derivatives of the constraint, the integrators in ADAMS can monitor integration error on both angular displacement and angular velocities of TAU device. The property of delivering accurate angular displacement as well as velocity makes SI2 Formulation suitable for our project. Another benefit of choosing this formulation is that Jacobian matrix of above equations will not become ill-conditioned as integration step size moves closer to 0. This benefit could make the simulation results of haptic rendering at 1000 Hz robust and stable.

3.5.2.2 GSTIFF (Gear) integrator

To integrate the obtained first-order equations of motion, the most widely-used GSTIFF (Gear) integrator was chosen in this project [21]. The integrator repeats the prediction-correction-error estimation process until it reaches the pre-defined end time of simulation.

The first task of this integrator is to predict values of the system state variables. The predictor formula for this method is [16]:

𝑦𝑛+1 = −ℎ𝛽0𝑦̇𝑛+1+ ∑(𝛼𝑗𝑦𝑛−𝑗+1+ ℎ𝛽𝑗𝑦̇𝑛−𝑗+1) 𝑘

𝑗=1

(28)

Where

h is integration step size

𝛼, 𝛽 are gear integration constants

The above equation fits a polynomial through the past values of each system state. For GSTIFF (Gear) integrator, the maximum order of polynomial is six. In our project, the corresponding parameter KMAX was set to be six.

was set to be 0.001s. According the help document of ADAMS [14], set the minimum integration time step to be 1 × 10−6_{∙Hmax and initial time step to be} 1

20∙Hmax. In order to make sure the error in all solution variables less than corrector error tolerance when convergence is reached, the corrector algorithm was selected to be CORRECTOR=original.

In the third stage, the error estimation stage, according to help document of ADAMS [14], after the corrector has converged to a solution, the GSTIFF (Gear) integrator estimates the local integration error in the solution by calculating a function combining predicted value, corrected value, the step size, and the order of the integrator. The strategy of determining integration tolerance can be summarized as: at first, the simulation results of default integration accuracy and accuracy 10 times tighter should be compared. If noticeable differences between two simulation results occur, tighten the integration tolerance by a factor of 10 again and make another comparison. Repeat this process until there is no significant difference in the results. After several simulations, the integration tolerance was chosen to be 1 × 10−5_.

The overall settings of ADAMS solver are shown in figure below.

3.5.3 Exporting ADAMS control Plant

Figure 23. ADAMS controls plant export.

After the state variables are defined, the system can be exported as block diagram which can be used in Simulink. The settings of the ADAMS Control Plant Import dialogue are shown in Figure 23. And the ADAMS model expressed in Simulink block diagram format is shown in Figure 24.

3.5.4 Building co-simulation model in Simulink

Figure 25. Co-simulation environment.

The above figure shows the co-simulation environment that was set up. The exported ADAMS subsystem (the orange ADAMS block) was dragged into a Simulink file and connected with other blocks relating to the control system. The settings of co-simulation are shown in Figure 26. Here, discrete mode was selected to let Adams solves the mechanical system equations and Simulink solves the control system equations. Both Plant Input interpolation order and Plant Output extrapolation order were set to be 0 instead of 1. This setting applies a zero-order hold to the Plant Input and output values rather than first-order hold to simulate computer-controlled system at discrete time. What is more, communication interval was settled to be 0.001s, which is the same frequency as the sampling frequency of the haptic rendering.

4 RESULTS

In the results chapter the results that are obtained with the process described in the previous chapter are compiled, and analyzed and compared with the previous work at our lab.

4.1 Evaluation of the linearization in ADAMS

One of the objectives of this project is to extract the linear time invariant (LTI) state space model of TAU parallel robot from ADAMS to avoid the complicated task of deriving the model based on mechanics principles by hand calculation. The first part of this section shows the state space model extracted from ADAMS and the final part shows the model validation procedure and validation results.

4.1.1 Linear model extracted from ADAMS

In this project, both ADAMS FORTRAN based solver and C++ based solver were used to export state space model from ADAMS. Since two solvers used different condensation schemes to linearize the nonlinear dynamic model of the TAU parallel robot about the equilibrium position, a comparison between linearization results should be made to check if similar results are obtained. When the linearization procedure was finished, seven files were created by ADAMS. Four files contained the A, B, C and D matrices, two files contained information about the inputs and outputs and one file contains information about the state variables [18]. From the file containing information about the state variables what kinds of state variables are used to form the state space model can be found. The contents of two files about the selected state variables obtained from two different solvers are shown below.