Keywords: Collision avoidance, safety, potential elds, Newton method, Hessian, collaborative robots, hand-guiding, redundancy, robot kinematics, robot dynamics, recursive algorithms, Mass matrix, Christoel symbols

Key contributions in hand-guiding:

1. Introducing the precision hand guiding method. An intuitive substitute for the teach pendant of the robot for performing precise positioning of the end-eector.

2. Introducing a novel solution for performing precision hand guiding of sensitive redundant manipulators, at the end-eector level, while navigating obstacles during contact.

Key contributions in robot dynamics:

1. Development of an algorithm with a minimal O(n 2 ) cost 1 for eciently calculating the joint space inertia matrix for serially linked robots with high degrees of freedom.

2. Development of a light weight recursive (non-symbolic) algorithm for calculating Christoel symbols of robotic manipulators.

University of Coimbra and École Nationale Supérieure d'Arts et Métiers

Collaborative robotic manipulators are new type of industrial manipulators, that allow the coexistence of humans in the same working area around the robot. Consequently, a human can work safely beside a collaborative manipulator, without having physical barriers or safety fences separating between them. In light of this, we can see that the main features of collaborative robots are centred around two main aspects, safety and collaboration, which are the main topics of this thesis.

On the subject of safe interaction with the robot, we revisit the problem of realtime collision avoidance for robotic manipulators in unstructured and dynamic environments, where humans are working side-by-side with the robot. We propose a method for endowing the manipulator with sensing capabilities, so it can detect the proximity of humans around it. Also, we propose a control algorithm, based on the potential elds method, for providing the robot with human like reexes, which allows it to avoid collision with people nearby. Afterwards, we propose a method for implementing the collision avoidance controller to modify o-line generated paths (necessary for performing industrial tasks). For testing, we implement the proposed method in a real industrial robotic cell, utilising an industry standard collaborative manipulator (KUKA iiwa) in human centred environment, where a human worker (coworker) and the robot are sharing the same working area.

On the subject of collaboration we study the topic of precise positioning of the endeector of the robot in an intuitive manner. We present the precision hand-guiding technique, and we present a lightweight algorithm for performing motion in the null space of redundant manipulators while preserving the accuracy of end-eector's pose.

Afterwards, we exploit the redundancy for performing secondary tasks, where using the torque feedback at the joints of sensitive robots and the force/torque measurements from an external Force/Torque (FT) sensor at the ange of the robot we were able to perform precise hand-guiding at the end-eector level while exploiting redundancy to perform in-contact obstacle navigation.

For developing the aforementioned applications, and for achieving real-time performance, we implemented various light weight numerical methods. Consequently, several ecient algorithms for performing the calculations and the control have been realized in software packages, most of which are provided as open source libraries in public repositories.

Throughout this work, the proposed algorithms were implemented in various programming languages, while the real-time virtual-reality simulations were carried out using the program V-REP (Virtual Robot Experimentation Platform). Using these tools several controllers, algorithms, techniques and simulations were applied and the results achieved were discussed throughout this document. Experimental tests were carried out using industrial-grade hardware, the collaborative manipulator KUKA iiwa7R800, laser scanner, IMUs, force torque sensor and magnetic trackers. Concernant l'aspect de l'interaction sécurisée avec le robot, on revisite le problème d'évitement de collisions en temps réel pour des robots manipulateurs dans des environnements dynamiques et non structurés, où des humains et le robot travaillent côte-à-côte. On propose une méthode pour doter le robot manipulateur de capacités de détection, dans le but de repérer les humains autour de lui. On propose également un algorithme de contrôle basé sur la méthode des champs potentiels pour équiper le robot de réexes comme les humains, qui lui permettrait d'éviter la collision avec les gens aux alentours. Après cela, on présente une méthode d'implantation pratique du contrôle d'évitement de collision permettant la modication hors ligne des chemins générés (nécessaire pour eectuer des tâches industrielles). La méthode proposée est validée experimentalement en exploitant une cellule robotique industrielle, constituée d'un manipulateur collaboratif de type KUKA iiwa dans un environnement principalement humain, où un travailleur humain (collaborateur) et le robot partagent le même espace de travail et potentiellement la même tâche. Concernant l'aspect de la collaboration homme-robot, nous proposons une première contribution portant sur la thématique du positionnement précis de l'eecteur du robot et ceci de manière intuitive pour l'humain. Ainsi, une nouvelle méthodologie de guidage manuel (hand-guiding) permettant la maitrise de la précision du guidage à la main est développée et validée expérimentalement. D'autres fonctionnalités innovantes basées sur l'exploitation de la redondance cinématique du robot utilisé sont développées dans ce travail. Une méthode basée sur un algorithme de faible complexité (temps de calcul) est développée an d'eectuer un mouvement dans le noyau de l'espace redondant des robots tout en préservant la précision de la pose de l'eecteur. Ensuite, la redondance est exploitée pour eectuer des tâches secondaires autorisant un guidage manuel précis de l'eecteur tout en exploitant la redondance, par exemple dans le cas d'un guidage en environnement contraint. Cette dernière méthode utilise les informations de couple de rétroaction au niveau des articulations des robots , ainsi que la mesure de la force et du couple exercée au niveau de l'eecteur par le biais d'un capteur d'eort/couple (F/T) externe.

Le développement des applications précédemment mentionnées a été réalisé en intégrant la contrainte de la faisabilité de calcul temps réel sur un moyen robotisé industriel.

Pour ce faire, plusieurs méthodes numériques à faible complexité algorithmique ont été développées est évaluées. Les logiciels et algorithmes exploités proviennent de librairies open source et les développements proposés ont été rendu également accessibles à tous publics.

Tout au long de ce travail, les algorithmes développés ont été codés dans diérents langages de programmation, tandis que les simulations de réalité virtuel en temps réel sont réalisée en utilisant le programme V-REP (Virtual Robot Experimentation Platform). Une contribution transverse à cette étude réside dans le fait que tous les algorithmes, techniques et simulations développés sont appliqués expérimentalement sur des moyens industriels (robot manipulateur collaboratif KUKA iiwa7R800, laser scanner, IMUs, capteur d'eort force/couple 6 composantes et traqueurs magnétiques de géolocalisation) et qu'une analyse pratique des contraintes d'implantations est conduite pour chacune des nouvelles fonctionnalités.

Mots clés

Évitement des collisions, sécurité, champs potentiels, méthode de Newton, Hessian, robots collaboratifs, guidage manuel, redondance, cinématique du robot, dynamique du robot, algorithmes récursifs, matrice de masse, symboles de Christoel.

Resumo

Manipuladores robóticos colaborativos seguros por Mohammad Safeea Doutoramento em Engenharia Mecânica Universidade de Coimbra e École Nationale Supérieure d'Arts et Métiers Os manipuladores robóticos colaborativos são um novo tipo de manipulador industrial que permite a coexistência de seres humanos na área de trabalho em redor do robô. Consequentemente, um humano pode trabalhar com segurança ao lado de um manipulador colaborativo, partilhando o mesmo espaço e sem barreiras físicas entre eles. As principais características dos robôs colaborativos centram-se em dois aspetos principais, segurança e colaboração, sendo estes os tópicos principais desta tese.

Relativamente à interação segura com manipuladores robóticos colaborativos, revisitamos o problema da prevenção de colisões em tempo real em ambientes não estruturados e dinâmicos, onde humanos e robôs trabalham lado a lado. É proposto um método para dotar o manipulador de recursos de deteção de humanos/obstáculos em seu redor. Além disso, é proposto um algoritmo de controlo baseado no método de campos potenciais que permite ao robô colaborativo evitar colisões com humanos/obstáculos. Posteriormente, propomos um método para o controlador de prevenção de colisões de forma a alterar as trajetórias nominais do robô denidas o-line (necessário para executar tarefas industriais). As metodologias propostas foram implementadas e testadas numa célula robótica real, utilizando um manipulador colaborativo industrial (KUKA iiwa) em tarefas onde robô e humano partilham a mesma área de trabalho.

O tópico da colaboração homem-robô foi estudado considerando o posicionamento preciso do robô de forma intuitiva. É proposto um método de guiamento manual de precisão, assente num algoritmo que permite o movimento do robô no espaço nulo de manipuladores redundantes enquanto preserva a precisão posicional. A redundância é também explorada recorrendo ao uso de medições de torque a partir das articulações do robô e medições de força/torque de um sensor externo de Força/Torque (FT) acoplado à ange do robô. Desta forma conseguimos realizar o guiamento manual do robô ao nível do end-eector enquanto tiramos vantagem da redundância na navegação de obstáculos por contato.

As metodologias acima mencionadas foram implementadas em robôs reais, recorrendo a algoritmos que pela sua natureza de aplicação devem ser computacionalmente leves e ecientes, permitindo assim desempenho em tempo real por parte do sistema robótico. Neste contexto, vários algoritmos relacionados com a implementação dos métodos, cálculos matemáticos e controlo foram desenvolvidos e agrupados em pacotes de software, a maioria dos quais são fornecidos como bibliotecas de código aberto em repositórios públicos.

Os algoritmos propostos foram implementados em várias linguagens de programação, enquanto as que simulações em tempo real foram realizadas usando o programa V-REP (Virtual Robot Experimentation Platform). Usando essas ferramentas vários controladores e algoritmos foram aplicados, sendo os seus resultados discutidos neste documento. Testes experimentais foram realizados usando hardware de nível industrial, nomeadamente o manipulador colaborativo KUKA iiwa7R800, scanner laser, unidades de medição inerciais (IMUs), sensores de força/torque e rastreadores magnéticos.

Palavras-chave

Prevenção de colisões, segurança, campos potenciais, método de Newton, Hessian, robôs colaborativos, guiamento manual, redundância, cinemática, dinâmica, algoritmos recursivos, matriz de massa, símbolos de Christoel. ACKNOWLEDGEMENT I would like to thank my supervisors Professor Pedro Neto, and Professor Richard Bearee for their friendliness, patience, important notes, and the useful guidance that they kindly oered throughout this work. They provided encouragement, and unwavering enthusiasm with my research, their advice and leadership were unequivocally fundamental in the progress of this work. Besides to my supervisors, I would like to thank Dr. Antonio Mourão for his mentorship and advice.

I also express my appreciation to the administrations of both Universities Coimbra and Arts et Métiers, for oering the suitable and rich environment to work in, they provided the required resources which facilitated my work, especially the unrestricted access to the robotics laboratories, with all of their resources, and materials, among others.

A special sense of gratitude to my loving family, particularly my mother, for her faith in me, encouragement and moral support, which provided the needed motivation during times of diculty.

My most special thanks to the Culture and Communication oce of the university of Coimbra, especially to Dr. Teresa Baptista, also to the oce of president Jorge Sampaio for their continuous support particularly the president's advisor Dr. Helena

3.2

Minimum distance between two capsules. . . . . . . . . . . . . . . . . . .

3.3

Region of feasible solutions and level sets of optimization problem (3.5).

3.4

Region of feasible solutions and level sets of modied optimization problem (3.7). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3.5

Execution time comparison for the proposed QR method, the method in [5] and the modied method in [5] as a function of number of linesegments/capsules of the set. Algorithms implemented in C++. . . . . .

3.6

Execution time ratio (Method [5]/QR in red), (Modied method [5]/QR in blue) and (Method [5]/Modied method [5] in green) as a function of number of line-segments/capsules of the set. Algorithms implemented in C++. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3.7

Average relative error between the method in [5] and the proposed QR method as a function of the number of line-segments/capsules of the set.

4.4

Scaling factor for joint limits avoidance. . . . . . . . . . . . . . . . . . .

4.5

Light alert (yellow) turns on when joint ve is near the limit. . . . . . .

4.6

Light alert turns on, when pushing joint four near its limit. . . . . . . .

4.7

Joint two near the zero, white markers are near each others. . . . . . . . 6.12 L'algorithme d'évitement des collisions développé est mis en ÷uvre dans une cellule robotique collaborative pour l'assemblage des cartes de portes de voitures (une vidéo montrant l'expérience est en [6]). . . . . . . . . . metal processing, and even in food industry. Owing to their accuracy, they are also becoming valuable tools for performing precise operations in aerospace industry. In the framework of Industry 4.0, the presence of robots in human centred environments is of vital importance, as it brings to the factory oor the best of both, the cognitive capacity of humans and the special qualities of robots, including their precision, strength and repetitiveness. Such approach will render production lines more productive and exible.

But, industrial robots are currently separated from humans behind fences for safety reasons. In addition, they are still hard to program, even for experienced users. Those drawbacks form a barrier to eective achievement of human-robot cooperation and interaction on the factory oor.

In such a case, it is required to have safer robots with better collaborative capabilities, that are easier to program without requiring high technical knowledge. Which have the ability to avert danger and ensure safety of the human coworker during mutual interaction.

Context and Motivation

In the light of the previous introduction, the proposed research study addresses the issues of safety and collaboration for industrial manipulators. On safety, the study delves into the subject of collision avoidance for robotic manipulators in a humanrobot shared workspace. On collaboration, the study explores the subject of precision hand-guiding for robotic manipulators. In both cases, the aim of the study is to apply the acquired results in industry. The following two subsections elaborate on the importance of both collision avoidance and hand-guiding functionalities, and lay down the arguments on the necessity of researching those subjects.

Collision Avoidance

Providing robots with advanced motor skills and human like reexes will allow them to circumvent obstacles and avoid collisions with the dynamic environment around them.

This is extremely important in order to give robots more autonomy and minimum need for human intervention, especially when robots are operating in changing workspace and dynamic environments.

When it comes to the new industrial revolution, industry 4.0, collision avoidance is vitally important, where it is expected to have humans and robots working with each others, and collaborating together, a quest that can not be achieved without providing the robot with collision avoidance capacities. Usually, the structure of industrial robots is made of metals (retain immense inertia). Due to productivity reasons, they are also required to move with high speeds, conditions that make it unsafe for humans to work side-by-side with a robot moving blindly on a pre-programmed path. Consequently, it has been the norm to have boundaries and safety fences that stand between humans and industrial robots. When it comes to the factories of the future, the idea is to eliminate these boundaries and have humans and robots sharing the same workspace, Figure 6.3. In such scenario robot's control system shall ensure the safety of the human coworker under all circumstances.

Another example on the importance of collision avoidance algorithms is robots required to work in dangerous environments. For example, when performing (robot operated) repairs inside nuclear reactors, or when working in potentially explosive environments. Figure 6.4 demonstrates such concept, where a robotic arm is actuating a valve in an oil infrastructure while being surrounded by gas pipes. This type of research is motivated by the fact that robots are required if humans want to tap into oil and gas reserves in remote regions where extremely harsh environments prevail, for example the reservoirs in the Barents Sea where temperatures can reach as low as -50 • C. In such places working conditions for humans are unfeasible. A study focused on such situation was proposed in [START_REF] Berg | Industrial robot collision handling in harsh environments[END_REF], where the author's work has focused on collision avoidance for industrial robots in gas and oil industry.

Apart from safety, collision avoidance capabilities endow robots with more autonomy. This is especially important when they are operating in remote places and in unknown environments. An example on this is the Curiosity Mars rover, Figure 6.6.

The rover works far away from earth, this makes it impossible to control the asset in real time. As a matter of fact, the distance between the two planets is immense, such that the time required for sending a signal to Mars and then receiving it back on earth could be in between 4 to 24 minutes (depending on the relative position of the two planets while they are rotating around the sun). So to keep the robotic rover away from obstacles, a collision avoidance algorithm called Morphin was developed, and is used to guide the rover through the terrains of Mars. This algorithm maintains a map of the environment, Figure 6.5, and based on this map Morphin recommends safe steering commands to the rover.

In robotics literature, the problem of collision avoidance is handled at one of two levels: global level, addressed through planning, or local level treated at the low level control. The global solutions are high-level solutions that guarantee to nd a collisionfree path from the initial conguration to the nal conguration, if such a path exists.

These algorithms treat the problem in conguration space. In such case the manipulator and the environment need to be remapped to conguration space and a collision-free path is searched in the unoccupied portion of the conguration space. However, these algorithms are very costly in terms of computation, thus their use is not feasible for realtime dynamic applications. On the other hand, the local reactive control is suitable for real-time implementation, since that its mathematical formulation is computationally ecient, so it can be embedded directly into the low level control.

Hand-guiding

Hand-guiding is a representative functionality of collaborative robots. It allows unskilled users to interact and program a robot rapidly in an intuitive manner. Many industrial applications require precise positioning of the end-eector (EEF) during the teaching process. Normally, the teach pendant is used for performing this task.

However, utilizing the teach pendant is not always convenient due to the following drawbacks:

1. When using the teach pendant to position the end-eector in Cartesian space, the user has to keep a track of the orientation of the base frame of the robot, this could become confusing even for experienced users. Not to mention that in some cases, the manipulator is mounted on a mobile platform, which makes the task of keeping a track of robot's base frame even harder.

2. Unlike the hand-guiding, when using the teach pendant the user does not have a feel of the force applied between the end-eector and its surrounding in case of contact. Accidents could happen and the user might over press the end-eector against the surrounding (which might include sensitive instruments) without noticing.

3. The teach pendant convention in describing the orientation is the Euler rotation angles. While it is well suited for computers, this way for describing orientation is not intuitive for humans. Even the experienced roboticist agrees that, in the general case, it is hard for a human to imagine the orientation of an object based on three numbers (Euler angles).

Due to the previous drawbacks, this thesis proposes a more intuitive method for performing precise positioning operation of the end-eector through hand-guiding.

Key Contributions

The thesis contributes into the eld of collaborative robotics in various aspects, including the subjects of collision avoidance for collaborative industrial manipulators, hand-guiding, and mathematical formulation of robot dynamics, Figure 6.7. In the following, the main contributions are listed according to each subject. Key contributions in collision avoidance:

Safety HRC

1. Development of an algorithm for capturing the conguration of the human coworker using laser scanner and IMU sensors. The developed algorithm is implemented in the context of human-robot collision avoidance.

2. Development of an ecient algorithm, based on optimization and QR factorisation, for fast calculation of minimum distances among a set of capsules (hemisphere capped cylinders).

Development of a real-time, potential elds based, collision avoidance algorithm.

This novel algorithm is suitable for application in industrial robotic cells, allowing the robot to adjust the o-line generated paths of the industrial task.

The proposed method also introduces a novel solution that allows the robot to switch between collision avoidance reactive motion and the task in hand while maintaining continuous and smooth movement. For achieving ecient execution, the relationship between the Hessian (of the potential function) described in joint space and the Hessian in Cartesian space is also deduced.

Thesis Outline

The thesis is divided into various chapters and sections. Chapter two gives the background, it also lists the basic principles required for the mathematical modelling of robots, all of which will be used throughout this document.

Chapter three is dedicated to the subject of collision avoidance between humans and industrial manipulators. We start this chapter by listing out the various methods in literature for representing the human and the robot. For this thesis, we choose the geometric primitive representation using capsules. Then, we derive an ecient algorithm for calculating the minimum distance between these capsules. Afterwards, a method based on sensor fusion is presented for capturing the conguration of the human around the robot. This method is then implemented in a collision avoidance algorithm derived from the potential elds method and tested on a real industrial robotic cell, where a human coworker and an industrial robot (KUKA iiwa) are sharing the same work space and tasks. In such a case, the robot is able to sense the presence of the human around it, and reacts on-the-y to avoid collisions while keeping the task target as possible. Finally, we propose coupling Newton method with potential elds collision avoidance algorithm in a second order optimisation framework. A symbolic mathematical formula is deduced for calculating the Hessian and the gradient of the potential eld eciently in the joint space of the robot. The advantages of using such solution for performing collision avoidance is demonstrated through various tests.

Chapter four explores the subject of hand-guiding while preserving the precision at the end-eector level. First, the main method for performing precision hand-guiding is introduced, the method is deduced mathematically and tested on KUKA iiwa robot.

Then the precision hand-guiding (at the EEF level) and the hand-guiding at joints level are compared in terms of precision and level of vibrations. Finally, by applying sensor fusion on force/torque measurements from an external force/torque sensor attached 1 Where n is the number of degrees of freedom of the robot.

at the end-eector and the torque measurements from integrated joint torque sensors, we propose a method that allows the user to hand guide the robot precisely, at the end-eector level, while navigating obstacles in the null space during a contact.

Chapter ve deals with the formulation of robot dynamics equations. Two ecient algorithms are proposed. The rst algorithm is for calculating the joint space inertia matrix of serially linked rigid bodies. It is an ecient algorithm with a minimal second order cost. The second algorithm describes a recursive method for calculating Christoel symbols of robotic manipulators eciently. The proposed algorithms were tested against state of the art methods. Results were reported in terms of number of operations, execution time and numerical error.

Publications and Technical Contributions

The ndings and the intermediate results achieved during the research period were the subject for various publications in (international) peer reviewed journals and conferences, including some of the main robotic congresses. Table 6.7 lists the main publications conducted in the scope of this thesis. Table 6.8 lists coauthored publications that have been conducted in cooperation with colleagues during the research period.

Moreover, implementing the proposed approach led to the development of a set of open source software packages/toolboxes (listed in Table 6.9), some of which, the Kuka Sunrise Toolbox (KST), gained popularity and is being used in various laboratories around the world. A Toolbox used to control KUKA iiwa robots, the 7R800 and the 14R820, from an external computer using MATLAB. SimulinkIIWA [27] A graphical programming interface that allows controlling KUKA iiwa robots, the 7R800 and the 14R820, using Simulink block diagrams. iiwaPy [28] A library that allows controlling KUKA iiwa robots, the 7R800 and the 14R820, using Python 2.7. GDA [29] A collection of MATLAB functions used for calculating robot dynamics (including Joint Space Inertia Matrix (JSIM), Coriolis matrix, centrifugal matrix, time derivative of JSIM), the algorithms implemented for preforming the calculations are derived from the Geometric Dynamic Algorithm (GDA). GDAHJ [30] C++ implementation of GDAHJ algorithm for calculating mass matrix of robots with a minimal second order cost Christoel recursively [31] Recursive algorithm for calculating Christoel symbols eciently implemented in MATLAB Chapter 2

Background

This chapter reviews the basics which are important for the implementation of the methodologies proposed in this thesis. Section 5.2.6 gives a review on the kinematics of robotic manipulators, the transformation matrices and the formula for the Jacobian.

Section 5.2.6 gives a review on the pseudo inverse of the Jacobian and the Damped Least Squares (DLS) method. Section 5.2.6 gives a review on the null space projection matrix for redundant manipulators. Section 5.2.6 gives a brief introduction into the articial potential elds.

Kinematics of Robotic Manipulators

Most of industrial manipulators consist of rigid links coupled serially together by revolute joints. In such a case, robot's conguration is described by its joints angular position vector q ∈ n , where n is the number of degrees of freedom of the robot. On the other hand robots are required to perform tasks in the Cartesian space, specied by the position and orientation of the robot's end-eector (vector x ∈ m ). The forward kinematics function [START_REF] John | Introduction to robotics: mechanics and control[END_REF] introduces a mapping from the robot's conguration q to the end-eector's position/orientation x:

x = f (q) (2.1)
Where f is the manipulator's forward kinematics, a highly non-linear vector function, that gives the position and orientation of the end-eector as a function of joints angles. This function can be calculated using the homogeneous transformation matrices, following Denavit-Hartenberg (DH) convention (rst introduced in [START_REF] Denavit | A kinematic notation for lower pair mechanisms based on matrices[END_REF]). From [START_REF] John | Introduction to robotics: mechanics and control[END_REF] a transformation matrix (using the modied DH parameters) of link i is given by:

T i-1 i =     cθ i -sθ i 0 a i-1 sθ i cα i-1 cθ i cα i-1 -sα i-1 -d i sα i-1 sθ i cα i-1 cθ i cα i-1 cα i-1 d i cα i-1 0 0 0 1     (2.2)
Where i is the joint index, θ i is the rotation angle of joint i around the axis z i , and (d i , α i-1 , a i-1 ) are the modied DH parameters of the link i. Dierentiating (6.17)

gives the equation of dierential kinematics [1], [START_REF] Corke | Robotics, vision and control: fundamental algorithms in MATLAB[END_REF]:

ẋ = J q (2.3)
Where q is the joints velocities vector, ẋ is the end-eector's velocity and J is the manipulator's analytic Jacobian matrix, each element of which:

J ij = δf i δq j (2.4)
However, the analytic Jacobian can also be calculated from the geometric Jacobian by pre-multiplication with a matrix whose value depends on the choice of orientation representation [1].

Pseudo Inverse

For non-redundant manipulators the inverse mapping, from a particular velocity of EEF to joints velocity is given by:

q = J -1 ẋ (2.5)
In the case of redundant manipulators, J is not square (has more columns than rows) and the matrix J -1 does not exist. In such a case, the least norm solution of the inverse mapping is given by the pseudo inverse:

q = J † ẋ (2.6)
Where J † is the Moore-Penrose or pseudo inverse of the Jacobian:

J † = J T (JJ T ) -1 (2.7)
In some congurations the previous equation suers from singularity problem. This happens when the matrix product JJ T losses rank. In such a case the Damped Least Squares [35] can be used:

q ≈ J T (JJ T + λ 2 I) -1 ẋ (2.8)
Where I is the identity matrix (m × m) and λ is a damping factor. For choosing an optimised value of the damping factor the method in [START_REF] Chiaverini | Achieving user-dened accuracy with damped least-squares inverse kinematics[END_REF] can be used.

Null Space Projection

Redundant manipulators possess more degrees of freedom than it is needed to execute a given task. In such a case, the inverse mapping (joints angular velocities that give a particular velocity of EEF) involves innite number of solutions, those solutions are given by the equation:

q = J † ẋ + N qc (2.9)
Where N is the null space projection matrix and qc is an angular velocity command. qc is commonly used to optimize some criteria. From [37], the basic formula of N is given by:

N = I -J T (JJ T ) -1 J (2.10)
Where I is the identity matrix (n × n). From (6.25) it can be noticed that redundant manipulators allow internal motion where the pose (position/orientation) of EEF is kept xed, in such a case ẋ = 0, and (6.25) becomes:

qnull = N qc (2.11)
Where qnull is the vector of the angular velocities in the null space of the robot.

Articial Potential Field

The Articial Potential Field (APF) method introduced by the pioneering work of Khatib [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF] is one of the most popular methods for performing real-time reactive collision avoidance (according to the number of citations). This method was applied successfully for both manipulators and mobile robots. In this method the robot is subjected to two types of potential elds. One represents the attraction forces that pull the robot toward the goal position, while the other represents the repulsion forces that push the robot away from obstacles in the environment. Those forces are dened by the gradient of the potential eld. As a result the robot moves toward the goal while avoiding collisions with obstacles. Owing to its additive property, the total potential eld is the sum of the attraction and the repulsion potentials:

U total = U att + U rep
(2.12) Figure 6.8 demonstrates the articial potential elds concept and illustrates their additive property. The total eld (Figure 6.8 right) is the sum of the attraction eld (Figure 6.8 left), with forces pulling towards the goal, and the repulsion eld (Figure 6.8 middle), with forces pointing away from obstacles. Owing to its computational eciency the method can be implemented in real-time, and can be deployed easily into the low-level control system of the robot. The use of this method had been extended successfully to oine global path planning problems. However, in the context of this study we limit the discussion to real-time collision avoidance implementation.

Chapter 3 Collision Avoidance

Modern factories are becoming more automated. Greater number of robots are being installed on production lines for performing various industrial operations including welding, manipulation and assembly. This is particularly advantageous in repetitive tasks that require precision. However, the presence of humans on the factory oor is still essential for performing complex and cognitive tasks. For safety concerns, robots are separated from humans, where they work behind fences. This physical separation limits the productivity and the exibility of the production lines. Currently, lots of attention and eorts are being directed for removing the barriers between humans and robots on the factory oor. This will bring better productivity by combining the precision and repetitiveness of robots with the cognitive ability of humans. In the framework of Industry 4.0, collision avoidance plays an important role for achieving safety criteria while having humans and machines working side by side. Consequently, there is a requirement for adapting the research results on robot collision avoidance into the factory oor. This study introduces the subject of manipulator's on-line collision avoidance into a real industrial application implementing typical sensors and a commonly used collaborative industrial manipulator (KUKA iiwa). In the proposed method, the human coworker and the robot are represented by capsules, the conguration of those capsules are acquired using external sensors, the minimum distance between the capsules is calculated, when human/obstacles are nearby the concept of hypothetical repulsion and attraction vectors is used. By coupling this concept with a mathematical representation of robot's kinematics we achieved a task level control with collision avoidance capability, where the o-line generated nominal path of the industrial task is modied onthe-y so the robot is able to avoid collision with the coworker safely while being able to full the industrial operation. Tests were carried out successfully on an industrial robotic cell performing typical assembly operations in automotive industry. Results show that the robot moves smoothly and avoids collisions successfully by adjusting the o-line generated nominal paths.

Chapter's breakdown

This chapter is organised into various sections:

• Section 3.1: lists the various methods applied in the literature for representing the geometry of humans and the robots. Due to its computational eciency we opted to represent geometries by capsules, where we present an ecient algorithm for calculating the minimum distance between capsules using QR factorization.

• Section 3.2: describes the method used for capturing the conguration of the capsules representing the human using data acquired from laser scanner and a group of Inertial Measurement Unit sensors (IMUs).

• Section 3.3: describes an implementation of a custom-made collision avoidance algorithm based on the potential eld method. The algorithm is applied on a robotic cell containing a collaborative industrial manipulator (KUKA iiwa),

where the robot has to perform an industrial task while avoiding collisions with a human coworker around it, sharing the same work space and tasks.

• Section 3.4: describes the application of Newton's method on the potential elds for collision avoidance, and discusses the advantages of such implementation.

• Section 3.5: describes a software package developed by the author which allows controlling KUKA iiwa robots from external computer using MATLAB.

Given that the chapter discusses dierent subjects, each in its own section, then for the sake of clarity, the author has opted to list the related work (state of the art) of each subject in the beginning of its respective section. In such a case, each section (of the previously listed) starts by a small introduction, followed by a review of the related work, afterwards the developed methodology is presented. Each of the proposed methods is validated by testing or simulation (sometimes both), the acquired results are reported in the experiments subsection. Finally each section is concluded with a summary.

Geometrical Representation

To implement a collision avoidance algorithm it is of vital importance to have a geometrical representation of the robot and the human/obstacles. Several methods had been proposed in literature for this purpose. In [START_REF] Chang | Collision avoidance of two general robot manipulators by minimum delay time[END_REF] convex polyhedrons are utilized to represent two PUMA 560 manipulators. In [START_REF] Su | Obstacle avoidance control for redundant manipulators using collidability measure[END_REF] the authors opted for ellipses to represent the links of the robot, while obstacles were represented by spheres. Another technique that is computationally ecient represents the robot and obstacles by primitive shapes, as segments of lines with spheres swept onto them, [START_REF] Bosscher | Real-time collision avoidance algorithm for robotic manipulators[END_REF] and [START_REF] Ouyang | Virtual velocity vector-based oine collision-free path planning of industrial robotic manipulator[END_REF]. A similar convention was implemented in [START_REF] Shadpey | Force control and collision avoidance strategies for kinematically redundant manipulators[END_REF], where obstacles and robot are represented by spheres and cylinders. Also, in [START_REF] Fang | Ecient self-collision avoidance based on focus of interest for humanoid robots[END_REF] the authors represented a humanoid robot by cylindrical shapes. While in [START_REF] Balan | Real-time 3d collision avoidance method for safe human and robot coexistence[END_REF] the authors opted to represent the robot and the coworker by a collection of spheres for ecient calculation of the minimum distance.

Another technique, which is more precise for representing the robot and the environment around it, utilizes mesh representation [4]. This type of representation has the disadvantage of being extremely costly in terms of computations while performing minimum distance calculations between the robot and the obstacle. To speed up the computation, researchers have utilized the power of parallel processing, using GPU, to carry out the calculations [4]. Nevertheless, programming a GPU is not an easy task, it requires considerable amount of time and special skills, so it had been avoided for the sake of this study. Thus, the primitive geometry method was our choice, the coworker and the robot are represented by hemisphere capped cylinders (capsules).

Minimum Distance Calculation

The subject of minimum distance calculation is important in many areas, for example in CAD design, computer graphics/games and in simulation. In such situations, minimum distance calculations are used to detect any overlap or collision between elements. This subject is also important in robotics for the problem of path planning and safety in human-robot interaction, where the minimum distance is used as a measure of collision imminence. In this scenario, calculating the minimum distance online is required for time critical applications such as human-robot collision avoidance and the path planning of robots navigating obstacles towards a goal. A novel approach to approximate the minimum distance between robot links and obstacles is proposed in [START_REF] Tarbouriech | On bisection continuous collision checking method: Spherical joints and minimum distance to obstacles[END_REF]. Obstacles are represented by a bounding box, modelled as cylinders and boxes.

Each part of the robot arm is subdivided into an optimal number of spheres that encompass its volume. The minimum distance between the robot and the objects is approximated by the minimum distance between the obstacle bounding box and the spheres. In [START_REF] Kuhn | Fast vision-based minimum distance determination between known and unkown objects[END_REF] a method is proposed to determine the minimum distance between multiple known objects (geometry, position, orientation and conguration for example a robot) and multiple unknown objects within a camera image. The distance is estimated by searching for the largest expansion radius where the projected model does not intersect the object areas classied as unknown. An algorithm for computing the minimum translational distance based on the Gilbert-Johnson-Keerthi algorithm between two spherically extended polytopes is introduced in [START_REF] Enrique | Hough transform for distance computation and collision avoidance[END_REF].

In our study, the implemented collision avoidance algorithm requires the calculation of the minimum distance between robot and surrounding environment (including humans), which are represented by capsules. By using higher number of geometric primitives the accuracy of representation increases. The human body can be represented roughly by a single capsule, Figure 3.1 (B). In this scenario the arms can extend out of the capsule volume for some congurations. proposed for calculating the minimum distance between capsules/line-segments. In [START_REF] Ennen | Ecient collision avoidance for industrial manipulators with overlapping workspaces[END_REF], it is proposed an algebraic method for minimum distance computation between two capsules. Other methods were proposed in [START_REF] Agostino De Santis | The skeleton algorithm for self-collision avoidance of a humanoid manipulator[END_REF][START_REF] Patel | 3 Collision Avoidance for a 7-DOF Redundant Manipulator[END_REF]. A solution for computing the minimum distance between cylinders with at ends was proposed in [START_REF] Biermann | Fast distance computation between cylinders for the design of mold temperature control systems[END_REF]. A well known methodology for computing the segment to segment (capsules) distance which is considered the most ecient method in literature (concerning computation time) is detailed in [5], pages 417-418.

Mathematical Formulation and QR Factorization

In this section the problem of calculating the minimum distance between two capsules is expressed mathematically as an optimization problem. Then the optimization function is reformulated using QR factorization and a geometrical solution is proposed.

Formulation

Owing to their geometry, calculating the minimum distance between two capsules can be reduced to the calculation of the minimum distance between two line-segments at the capsules axes. In Figure 3.2 it is shown two line-segments representing the axes of two capsules and their associated common normal. Each capsule can be dened by two vectors (at the beginning and end of the capsule's axis-segment) and a radius. Let's designate the position vectors dening the end points of the axis-segment of a capsule by p 1 and u 1 (capsule 1), and p 2 and u 2 (capsule 2). Then, we can dene two vectors s 1 = u 1 -p 1 and s 2 = u 2 -p 2 . Two points of interest, one in each axis segment of a capsule, are considered. Those points are represented relative to the base frame by two vectors, r 1 and r 2 :

r 1 = p 1 + s 1 λ 1 (3.1)
r 2 = p 2 + s 2 λ 2 (3.2)
Where λ 1 and λ 2 are scalar parameters. Each parameter has a value in the range from zero to one when the point it represents is conned in between the two ends of the axis segment of the capsule. The problem of calculating the minimum distance between the two capsules renders to a minimization problem:

min(φ) = min(| p 2 + s 2 λ 2 -(p 1 + s 1 λ 1 ) | -ρ 1 -ρ 2 ) (3.3)
Where ρ 1 and ρ 2 are the radii of the capsules. Giving that ρ 1 and ρ 2 are constants, then the minimization problem can be reformulated:

min(| ∆r |) = min(| p 2 + s 2 λ 2 -(p 1 + s 1 λ 1 ) |) (3.4)
Where ∆r = r 2 -r 1 . We can rewrite the optimization function in the following equivalent quadratic form:

min(f ) = min((Ax + y) T (Ax + y)) (3.5)
Where matrix A = s 2 -s 1 and vector y = p 2 -p 1 . The problem can be viewed as minimizing (3.5), subject to the constrains 0 ≤ x 1 ≤ 1 and 0 ≤ x 2 ≤ 1 (x 1 and x 2 are the components of the vector x -also they are equal to parameters λ 1 and λ 2 ).

Figure 3.3 shows the level sets and the region of feasible solutions of the optimization problem (3.5).

QR Factorization

The function f can be reformulated by performing QR factorization on matrix A and xing. Then, the optimization problem in (3.5) is equivalent to:

min(f 1 ) = (Rx + Q T y) T (Rx + Q T y) (3.6)
Where Q is a 3x2 matrix whose column vectors are of unit length and mutually orthogonal, and matrix R is a 2x2 upper triangular. By performing a variable change the optimization problem becomes:

min(f 1 ) = min(u T u) (3.7)
Where u is given by: u=Rx

+ Q T y (3.8)
We want to mention here that the optimization problems, original (before applying QR factorization) and modied (after performing the QR factorization) are equivalent (both problems result in the same minimizing solution x min ) but the functions f and f 1 are not equal (they have dierent values at the minima).

d min = u T min u min + y T y -y T QQ T y -ρ 1 -ρ 2 (3.9)
For a set of k line-segments/capsules it can be noticed that:

1. Minimum distance calculations shall be performed mutually between any two capsules of the set, resulting in O(k 2 ) complexity.

2. QR factorization of matrix A i associated with a subset i of two capsules can be enhanced for eciency since dierent A i have shared columns in their structure.

3. The vector u min is being calculated in two dimensional space, while other algorithms calculate x min in the three dimensional space.

4. Vectors operations in two dimensional space are less costly than operations in three dimensional space. By taking the aforementioned observations into account, the described algorithm can be applied eciently for calculating minimum distance for a set of capsules 1 .

Experiments

The performance was evaluated by comparing the proposed QR method with the method in [5] to compute the segment to segment minimum distance. Three comparison criteria were considered: (1) computational complexity, (2) execution time using C++ and (3) numerical precision. The results for the computational complexity of the algorithms (number of oating point operations addition, multiplication, division and square root for QR) are in Table 3.1. For the second comparison criteria, a set of line-segments was randomly generated and the minimum distance (squared) between each two line segments of the set was calculated using the proposed QR method and the method in [5]. By implementing the algorithms in C++, results indicate that in terms of execution time the proposed QR method performed up to 25% faster than the method in [5], Figure 3.5 and Figure 3.6.

We noticed that method [5] can be modied by promoting some of the operations from O(n 2 ) to O(n). The results of those operations can be stored in memory and used later in the O(n 2 ) part of the algorithm. In such a case, the O(n 2 ) computational complexity of method [5] is reduced as shown in Table 3.1. Nevertheless, the proposed QR method is still more ecient in terms of execution time as shown in Figure 3.5 and Figure 3.6. The modied method in [5] is detailed in the Media materials (attachments) of [12], which includes C++ code in Appendix I (page 22) and a detailed breakdown Figure 3.5: Execution time comparison for the proposed QR method, the method in [5] and the modied method in [5] as a function of number of line-segments/capsules of the set. Algorithms implemented in C++. Table 3.1: Computational complexity for the method in [5], the modied method in [5] Where d qr min is the minimum distance calculated using the proposed QR method and d s min is the minimum distance calculated using the method in [5]. Experimental tests resulted in a maximum value of the relative error of 1.059e -8 , a minimum value of the relative error of 1.11e -16 and an average error of 4.87e -10 . These values demonstrate that the error is negligible. The same test has been repeated for dierent groups of line-segments/capsules with dierent number of elements, Figure 3.7.

Summary

In this section, a novel method based on QR factorization for performing minimum distance calculations for a set of line-segments/capsules is proposed. Capsules demonstrated to be good solution to represent humans and objects in real environment having data from real sensors as input. Experimental results indicate that the proposed solution is more ecient than the existing most ecient method in literature. Such eciency was measured in computational complexity (reduced number of oating point operations), execution time (up to 25% better) and numerical precision (the error is negligible).

Figure 3.7: Average relative error between the method in [5] and the proposed QR method as a function of the number of line-segments/capsules of the set.

Human Pose Estimation

The avoidance motion carried out by a manipulator to prevent collisions with a human nearby relies heavily on his/her pose estimates. In this study, we opted to use Inertial Measurement Units (IMUs), shown in Figure 3.8 (a), coupled with a Sick laser scanner, shown in Figure 3.8 (b), for locating the human, and his/her conguration, around the robot. A total of ve IMUs are used. One is attached to the human's chest, the other four are attached to the arms. This is suitable for our choice of representing the upper part of the human body by ve capsules, one covering the torso and four covering the arms, Figure 3.8 (b). Given that in our robotic cell the robot is mounted on a table, as shown in Figure 3.9, with its motion restricted to a one meter level above the factory oor, there is no danger of collision between the robotic arm and the lower part of the human body, hence our negligence of representing the legs is justied. On the other hand, the robot (KUKA iiwa with 7 DoF) is described by three capsules representing the main links of the robot, Figure 3.8 (c). The conguration of the capsules covering the robot is obtained after calculating forward kinematics applied on the measured joint angles (acquired from the controller of the robot). After capturing the conguration of the capsules covering the human and the robot, the minimum distance is calculated using the method introduced in the previous section. In the following subsections, the methodology used to capture the conguration of the capsules covering the human body is described.

Laser Scanner (Torso Position)

Data from the laser scanner mounted at the level of the coworker's legs, Figure 3.9, are utilized to dene the relative position of the capsule representing the torso. Through TCP/IP connection, the sensor (SICK TiM5xx) provides the radius measurements along a scan-angle spanning 270 degrees, an angular precision of one degree and a maximum measurement radius of 8 meters. The methodology behind the proposed algorithm for calculating the torso position is divided into two stages: 1. Data acquisition and ltering;

2. Calculating the minima and the position of the torso.

Data Acquisition and Filtering

A TCP/IP server is implemented in MATLAB. It acquires the measurements from the laser scanner by decoding TCP/IP messages coming from the sensor. The result is stored in a two dimensional array, which relates the radius measurements with the corresponding scan-angle. Figure 3.10 shows the radius measurements along the scanangle as acquired from the laser sensor corresponding to a scene where a human is standing in the scan eld of the sensor. The radius measurements are clipped to 1400 mm away from the sensor. The contour of the legs appears in the plot as two minimas.

It is noticed that the raw data (represented by a blue continuous line in Figure 3.10)

are not smooth. To smooth out the curves two ltering methods are proposed: (1) temporal ltering and (2) spatial ltering (along the scan-angle).

Temporal Filtering

To lter out the noise in radius measurements from the laser scanner a low-pass lter (LPF) is utilized:

r (θ,t) = αr m (θ,t) + (1 -α)r (θ,t-dt) (3.11)
Where r (θ,t) is the ltered value of the radius at angle θ and time t, and r m (θ,t) is the measurement value of the radius at angle θ and time t, α is a scalar from zero to one (adjusted during experiment for better response), and dt is an update time interval

Spatial Filtering

To smooth out short term uctuations of the radius measurements along the scan-angle, a moving average (MVA) lter is used:

r f (θ,t) = r f (θ-dθ,t) - r (θ-ndθ,t) n + r (θ,t) n (3.12)
Where r f (θ,t) is the value of the MVA at angle θ and time t, dθ is the angular resolution of the scanner, r f (θ-dθ,t) the value of the MVA at angle θ -dθ and time t, and n is the number of averaging steps.

Minimas and the Position of the Torso

Considering the plane of the oor, the xy position of the capsule representing the torso is calculated based on the position of the legs. The polar coordinates of the legs correspond to the minimas in the plot, as shown in Figure 3.11. To calculate the minimas, rst we perform a mirror transform, on plot in Figure 3.11, with respect to the axis of the scan-angle θ. In such a case, the minimas become peaks, which can be calculated by performing a peak analysis on the resulting curve. Consequently, the angle and the radius associated with the rst leg (θ 1 , r 1 ) and the second leg (θ 2 , r 2 ) are acquired. Afterwards, the position vector of the rst leg x 1 in the Cartesian space is calculated:

x 1 = cos(θ 1 ) sin(θ 1 ) (r 1 + ρ l ) (3.13)
3.5 Where ρ l is the radius of the leg. The vector of the Cartesian position for the second leg x 2 is calculated in the same manner. The position of the torso capsule x t can be approximately considered at the middle distance between the two legs:

x t = (x 1 + x 2 ) 2 (3.14)
Figure 3.12 shows a laser scan with a human in the scan eld of the sensor. The scan eld span of 270 degrees is satisfactory given that the sensor is mounted at the corner of the table holding the robot. Using the proposed algorithm, the position of the legs is detected, red and green dots. Using the legs coordinates the position of the capsule covering the torso is approximated, black circle in Figure 3.12.

IMU Sensors (Conguration of the Upper Body)

To capture the conguration of the capsules representing the human upper body ve IMU sensors are used. One is attached to the chest (IMU 1) and the other four sensors are attached to the arms and the forearms (IMU 2, IMU 3, IMU 4 and IMU 5), Figure 3.13. Each capsule is described by two vectors and a radius. The vectors represent the position of the beginning and end of each capsule.

The quaternion measurements provided by the IMU sensors coupled with the geometric information from the human coworker's body (its dimensions) are used to estimate the position of the capsules covering the coworker's limbs in relation to robot base. The procedure for performing the calculations is divided into the following steps:

1. Calibration phase;

2. Calculating rotations of limbs with respect to base frame of the robot;

3. Calculating the position of the limbs' capsules with respect to the base frame of the robot.

Calibration

Each IMU measurement gives its orientation, in quaternion w ref imu , with respect to a pre-dened reference frame 2 . For the collision avoidance algorithm the rotations shall be described in relation to the robot base frame using the quaternion w b imu . To calculate w b imu , the rotation quaternion from the reference frame to the robot base frame w b ref shall be calculated. This is achieved by performing an initial calibration phase. Consequently, the IMUs are placed in a predened orientation with respect to the robot base before the system is initiated. In such a case, the initial rotation quaternion w imu,init b of the IMU frame with respect to robot base is already known.

Orientation of the Limbs

After calculating the orientation of the reference frame with respect to robot base w b ref , the quaternion measurements describing the IMU orientation with respect to base frame of the robot w b imu is calculated from the orientation measurement of the IMU w ref imu , as the following:

w b imu = w b ref w ref imu (3.16)

Position of the Limbs

Five vectors on the coworker's body are considered, Figure 3.13. Due to the symmetry of the human body, the following elaboration and equations are given for the left half of the coworker's body. For the other half, an identical methodology is applied. For the left half of the body three vectors are considered:

1. Vector v 1 : a vector that spans from the chest up to the left shoulder;

2. Vector v 2 : a vector that spans the left arm of the coworker, from the shoulder up to the elbow;

3. Vector v 3 : a vector that spans the length of the left forearm, from the elbow up to the wrist.

The IMUs are mounted rmly on the body of the coworker according to:

• The chest's IMU (IMU 1 in Figure 3.13) is mounted such that the x axis of the IMU is pointing vertically down when the coworker is standing straight up. The z axis of the sensor is in the Sagittal plane, coming out of the chest. The y axis of the sensor is horizontally positioned in the Coronal (Frontal) plane. In such case the coordinates of the vector v imu Where d 1 is the length on coworker's body taken vertically from the chest up to the shoulder and d 2 is the width of the coworker's shoulders divided by two.

• The upper arm's IMU is mounted such that the x axis of the IMU is aligned with the upper arm's length, in such case the coordinates of the vector v imu • The forearm's IMU is mounted such that the x axis of the IMU is aligned with the forearm's length. In such case the coordinates of the vector v imu The previous vectors are rotated back to the base frame of the robot using:

v b i = w b imu i v imu i i w b imu i -1 (3.17)
Where

v imu i i , i = 1, 2, ...5,
p b 1 = v b 1 + p b 0 (3.18)
Where p b 0 is the position of the chest point with respect to the base frame of the robot, given that the coworker is standing up all the time the xy position of the coworker's chest point is the same as the position of the torso acquired from the laser scanner. If we denoted l to the height of the coworker's chest point from the xy plane of the base frame of the robot, then p b 0 is calculated:

p b 0 = x b t l (3.19)
Where x b t is the xy position of the torso of the coworker with respect to the base frame of the robot, calculated by transforming the estimation of the torso position (acquired from laser scanner measurement) x t into the base frame of the robot. Given that the frame of the laser scanner is parallel to the base frame of the robot then the vector x b t is given by:

x b t = x t + c 1 c 2 (3.20)
Where c 1 is the x coordinate of the origin O l of the scanner's-measurement-frame in the base frame of the robot, and c 2 is the y coordinate of the origin O l of the scanner'smeasurement-frame in the base frame of the robot. The origin O l and the dimensions (c 1 , c 2 ) are shown in Figure 3.13. The position vector of the elbow point in the base frame of the robot p b 2 is given by:

p b 2 = v b 2 + p b 1 (3.21)
Accordingly, the position vector of the wrist point in the base frame of the robot p b 3 is:

p b 3 = v b 3 + p b 2 (3.22)

Summary

In this section a method for capturing the conguration of the human upper body is presented. Laser scanner and IMU sensors are used for this purpose. The position of the human torso (approximated by an upright capsule) is acquired by processing measurements from the laser scanner. The conguration of the capsules covering the human arms are acquired by processing the measurements from the IMU sensors. In addition, a method for calibrating the IMU reference frame with respect to robot base frame is presented. The method proposed in this section is important for the application of our collision avoidance algorithm (in the next section).

Collision Avoidance Algorithm

This section introduces the subject of manipulator's on-line collision avoidance into a real industrial application implementing typical sensors and a commonly used collaborative industrial manipulator, KUKA iiwa. In the proposed methodology, the minimum distance and relative velocity between the human and the robot is calculated, when human/obstacles are nearby the concept of hypothetical repulsion and attraction vectors is used. By coupling this concept with a mathematical representation of robot's kinematics, a task level control with collision avoidance capability is achieved. Consequently, the o-line generated nominal path of the industrial task is modied onthe-y so the robot is able to avoid collision with the coworker safely while being able to fulll the industrial operation. To guarantee motion continuity when switching between dierent tasks, the notion of repulsion-vector-reshaping is introduced. Tests on an assembly robotic cell in automotive industry show that the robot moves smoothly and avoids collisions successfully by adjusting the o-line generated nominal paths.

A Review on Collision Avoidance Algorithms

Collision avoidance is a major topic in robotics research, especially in the new context of collaborative robotics. Owing to its importance, numerous studies approaching collision avoidance had been presented. In the following we tried to list the major studies about the subject. In potential eld (PF)-based methods the robot is in a hypothetical vector eld inuenced by two types of forces. Forces of attraction that guide the robot towards the goal, and repulsion forces that repel it away from obstacles.

Subjected to these forces the robot nds its way to the goal while avoiding collisions.

Unlike its predecessors the articial potential eld [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF] was the rst of its kind that treated the problem of collision avoidance at the low-level control, which is best suited for achieving real-time response. Another method that is based on a similar principle is the 3 Dimensional (3D) force eld method [START_REF] Chotiprayanakul | A 3-dimensional force eld method for robot collision avoidance in complex environments[END_REF]. In this method each link of the robot is surrounded by a virtual elliptical volume. When the obstacle penetrates into the ellipsoid, a hypothetical repulsive force is generated, repelling the robot away from it and avoiding collision.

Based on the PF principle, an approach to collision avoidance and trajectory planning was proposed in [START_REF] Bu | Active motion planning and collision avoidance for redundant manipulators[END_REF]. This method requires as an input a preliminary trajectory, generated with another method, repulsion poles are used for representing obstacles and an attraction pole is utilized for guiding the robot toward the goal. The attraction pole moves along the preliminary trajectory and the robot will follow it while being repelled away from obstacles. This method is suitable for generating collision free paths that are as close as possible to a predened trajectory. Recently, in [START_REF] Fabrizio Flacco | A depth space approach to human-robot collision avoidance[END_REF], a depth space approach for collision avoidance between a robot and coworker was presented. The study describes an improved implementation of the articial potential eld method in which an estimation of obstacle's velocity was taken into consideration when computing the repulsion vector. This is an advancement to the original articial potential eld that utilizes only information about minimum distances for calculating the repulsion vectors. Also, the paper proposed a novel approach for estimating obstacle's velocity.

In [START_REF] Agostino De Santis | The skeleton algorithm for self-collision avoidance of a humanoid manipulator[END_REF] the authors presented the skeleton algorithm, a framework for self-collision avoidance for humanoid robots. In this algorithm a skeletal representation of the robot is proposed, based on this representation the points of the robot closest to collision are obtained through minimum distance calculations between dierent segments of the skeletal structure, then repulsion forces are calculated and converted to control commands used to control the robot.

In [START_REF] Wallho | A skill-based approach towards hybrid assembly[END_REF] the authors proposed a control architecture for a collaborative robotic cell that performs assembly works, the cell is provided with collision avoidance module, the robot and the workspace are represented in a virtual environment where the minimum distance is computed and control commands are generated.

While the articial potential eld is inspired by electric eld phenomena, other approaches, inspired by other types of elds, were proposed. The circular elds method, inspired by electromagnetism, was investigated in [START_REF] Singh | Real-time robot motion control with circulatory elds[END_REF][START_REF] Haddadin | Dynamic motion planning for robots in partially unknown environments[END_REF]. One of the advantages oered by this method over the articial potential eld is that it is immune to getting stuck in local minima, a drawback that might appear in the articial potential eld.

Other researchers drew inspiration from uid mechanics. In this context, [START_REF] Soukieh | Obstacle avoidance of nonholonomic unicycle robots based on uid mechanical modeling[END_REF] utilized the stream lines of potential ow for attaining collision free paths. The Circular Theorem from uid dynamics was implemented for computing the stream lines of the potential ow. Similarly, in [START_REF] Palm | Fluid mechanics for path planning and obstacle avoidance of mobile robots[END_REF] collision free paths are attained by superimposing elementary ows. Doublets are used to model cylindrical obstacles inserted in a uniform ow. In such a way, collision free paths are attained after calculating the stream lines of the ow. Analogous to circular elds, the methods based on uid dynamics are also immune to dead-lock (getting stuck in local minima). Nevertheless, almost all studies based on uid mechanics techniques are dedicated to applications of collision avoidance for mobile robots.

In [START_REF] Masoud | Robot navigation using a pressure generated mechanical stress eld: the biharmonic potential approach[END_REF] the authors proposed the application of biharmonic potential functions for collision free path calculation, inspired by the phenomena of stress distribution in materials. In this method, the navigation zone is considered to be a continuous solid with elasticity properties, obstacles are voids inside the solid, and the goal is modelled by a pressurized cavity. The distribution of stresses inside the material is calculated and from this distribution a collision free path is established.

Other researchers have approached robot collision avoidance as an optimization problem. In [START_REF] Bosscher | Real-time collision avoidance algorithm for robotic manipulators[END_REF] the authors utilized the square of a norm of an error vector as objective function. This error was dened from the dierence between the end-eector's velocity and the desired velocity. The constraints are formulated from limits on (1) joints angles,

(2) joints velocities and (3) the likelihood of collision. The optimization problem is solved in real-time using the logarithmic barrier method, and the computed velocity is used to command the robot.

A motion planner that takes into consideration obstacle avoidance is presented in [START_REF] Nishimura | A motion planning method for a hyper multi-joint manipulator using genetic algorithm[END_REF]. This planner employs both the potential eld and a genetic optimization algorithm. In this method the planning process is divided into two procedures. In the rst the articial potential elds method was employed to nd a collision free path for the end-eector. In the second procedure, collision free congurations were calculated using Genetic Optimization techniques. The method was validated by performing simulations on a virtual robot with twelve DoF.

In [START_REF] Lajpah | Kinematic control algorithms for on-line obstacle avoidance for redundant manipulators[END_REF] the authors described an algorithm for collision avoidance for redundant manipulators. The algorithm operates at the kinematics (joint-velocities) level. At rst, the minimum distance between the robot and the obstacle is calculated, if closer than a predened safety distance, a motion component is assigned to the part of the robot closest to the obstacle, repelling the robot away from collision. The novelty in their approach was in the way the repulsion motion was dened, or what they call one dimensional operational space approach. In such approach, the repulsion component is projected on the direction aligned with the closest distance between the robot and the obstacle. This way of dening the repulsion motion gave their algorithm better immunity against singularities. Nonetheless, the proposed method was built around the fact that the end-eector's task is the primary task, or the task with the highest-level priority, while collision avoidance was treated in the null space of the Jacobian associated with the end-eector. This will cause the manipulator to fail in avoiding collision with the obstacle in some situations. For example, when there is a contradiction between the end-eector's task, assigned the highest-level priority, and the collision avoidance task, assigned a lower-level of priority. For solving this drawback the authors proposed to re-invoke a high-level path planning algorithm, so that a new collision free path can be calculated. Another solution for this problem, more ecient and not requiring re-planning, is proposed in [START_REF] Petri£ | Serial and parallel robot manipulators kinematics, dynamics, control and optimization, chap obstacle avoidance for redundant manipulators as control problem[END_REF]. In this solution a coecient is introduced into the control equation, and by changing the value of this coecient the priority level between tasks can be switched smoothly.

In [START_REF] Mitsi | Optimization of robot links motion in inverse kinematics solution considering collision avoidance and joint limits[END_REF] the authors proposed an inverse kinematics solver that takes into consideration joint limits and collision avoidance. The inverse kinematics problem was formulated as a minimization of the square of the error between the goal position and the end-eector position. The minimization is subjected to (1) constraints due to joints limits, and (2) constraints due to restrictions on minimum distance between the obstacle and the robot. Fiacco and McCormick algorithm was used to solve the optimization problem. The method was tested on a 5 DoF robotic manipulator.

Probabilistic RoadMaps (PRM), [START_REF] Kavraki | Probabilistic roadmaps for path planning in high-dimensional conguration spaces[END_REF] is a powerful method for generating collision free paths for robots with high degrees of freedom in non-dynamic environments. This method is composed of two phases. The rst phase is called the learning phase, it is performed oine, and used for stochastic generation of a roadmap of the scene.

The second phase is called the query phase. In this phase, paths are queried, and the roadmap is searched for a feasible path. While PRM is a powerful method, it suers two major problems. The rst of which is that it is unsuitable for real-time implementation. This is due to the high computational cost of the method, particularly for robots with high degrees of freedom. The second drawback comes from the fact that the resulting trajectories generated by this planner are not dynamically optimized for direct implementation on the robot. So much so, PRM is more suited for the oine global planning over real-time collision avoidance implementation. Nevertheless, due to the advancement in processing power of contemporary computers, and despite the high computational cost of PRM, a recent study [START_REF] Kunz | Real-time path planning for a robot arm in changing environments[END_REF] reported success in implementing PRM for real-time collision free path planning in dynamic environments. Based on PRM, in [START_REF] Leven | A framework for real-time path planning in changing environments[END_REF] the authors proposed the dynamic roadmaps, a high level algorithm for planning dynamic paths in changing environment, and more suitable for real-time collision avoidance applications. This method was used successfully in [START_REF] Dittrich | A Modular Cognitive System for Safe Human Robot Collaboration: Design, Implementation and Evaluation[END_REF], where the authors presented a collaborative human-robot system with integrated collision avoidance capability.

In [START_REF] Balan | Real-time 3d collision avoidance method for safe human and robot coexistence[END_REF] the authors presented a collision avoidance system in which the control strategy proposed searches for a motion-direction of the end-eector that guarantees a collision free path between the robot and the coworker. In case this direction exists, the end-eector is commanded to move in that direction. Otherwise the robot is stopped, while waiting for the coworker to move from its way.

Other researchers took alternate approach towards solving collision avoidance, this approach requires a complete knowledge of obstacle's trajectory. Though restrictive, these methods are appealing when performing collision avoidance between dierent robotic manipulators. For example, in multi-robot cells or for dual-arm manipulators,

where the trajectories of the manipulators are known a priori. A study that utilizes this knowledge for achieving collision avoidance between two end-eectors is presented in [START_REF] Yasser | On-line collision avoidance between two robot manipulators using collision map and simple escaping method[END_REF]. In this study the end-eectors are modelled as spheres, using their pre-calculated trajectories, a collision map is constructed. Afterwards, an iterative time shifting algorithm is applied and the time delay required to achieve collision avoidance is calculated.

In other words, collision free operation is achieved by performing time-rescheduling on motion commands before sending them to controllers. This approach was developed further in [START_REF] Yasser | Advanced-collision-map-based on-line collision and deadlock avoidance between two robot manipulators with ptp commands[END_REF], where an Advanced Collision Map is introduced, the method can be applied for collision avoidance between two manipulators rather than their end-eectors only. In [START_REF] Yasser | On-line collision detection of nrobot industrial manipulators using advanced collision map[END_REF] the method was generalized to perform collision avoidance between any number of manipulators.

A study dedicated to collision avoidance between two manipulators for industrial applications is in [START_REF] Zhou | Collision avoidance of two manipulators using rt-middleware[END_REF]. The problem was addressed by dividing the work space of the manipulators into a shared work area, accessible to both manipulators, and an external work area accessible to only one manipulator. The authors added a processing layer into the control structure, in which point-to-point control commands are processed before being sent to the controllers. As consequence, the manipulators are allowed to operate in their own external work area at any time. However, the presence of one of the manipulators inside the shared work area will deny access to the other manipulator, causing it to wait until the shared work area is free from the other manipulator.

For redundant manipulators self induced collisions could occur. This problem is treated in [START_REF] Seto | Self-collision Avoidance Motion Control for Human Robot Cooperation System using RoBE[END_REF], where RoBE (Representation of Body by Elastic Elements) is presented as a method for preventing robot self collisions. In this method each link is covered by a ctitious elastic element, whenever the elements touch, a force is generated and collision avoidance is achieved.

Collision Avoidance for Industrial Manipulators

Industrial robots are traditionally working inside fences, isolated from humans. The ability to have robots sharing the workspace and working side-by-side with human coworkers is a key factor for the materialization of the Industry 4.0 concept. The paradigm for robot usage has changed in the last few years, from an idea in which robots work with complete autonomy to a scenario where robots cognitively collaborate with human beings. This brings together the best of each partner, robot and human, by combining coordination, dexterity and cognitive capabilities of humans with the robots' accuracy, agility and ability to produce repetitive work. For example, robots can help humans in carrying and manipulating sensitive/heavy objects safely [START_REF] Solanes | Human-robot collaboration for safe object transportation using force feedback[END_REF] and positioning them precisely by hand-guiding [16]. In this scenario the robot can play the role of a force magnier while moving compliantly according to the haptic feedback from the human.

Reaching the goal of developing/creating safe collaborative robots will allow a greater presence of robots in our society, with a positive impact in several domains, including industry [22]. Nowadays, industrial collaborative robots, which are not operating inside fences, do not have autonomy to perceive its unstructured and time-varying surrounding environment, nor the ability to avoid collisions with human coworkers in real-time while keeping the task target dened by the o-line generated paths. On the contrary, they stop when a predened minimum separation distance is reached.

Due to this issue, the full potentialities of collaborative robots in industrial environment are not totally explored. The increasing demand by industry for collaborative robot-based solutions makes the need for advanced collision avoidance strategies more visible. To have them working safely alongside humans, robots need to be provided with biological-like reexes, allowing them to circumvent obstacles and avoid collisions. This is extremely important in order to give robots more autonomy and minimum need for human intervention, especially when robots are operating in dynamic environment and interacting/collaborating with human coworkers [START_REF] Nikolaidis | Improved humanrobot team performance through cross-training, an approach inspired by human team training practices[END_REF]. The requirements for safe collaborative robots, including physical human-robot interaction (HRI), are detailed in [START_REF] Sami Haddadin | Requirements for safe robots: Measurements, analysis and new insights[END_REF], where collision avoidance is listed as a factor, among others, which is important for human-robot safety. The new standard ISO 10218 and the technical specication TS 15066 dene the safety requirements for collaborative robots [START_REF] Saenz | Survey of methods for design of collaborative robotics applications-why safety is a barrier to more widespread robotics uptake[END_REF]. Apart from industrial domain and human-robot collaboration, collision avoidance is also being investigated for aerospace applications, including robotic arms mounted on space maneuverable platforms [START_REF] Chu | Path planning and collision avoidance for a multi-arm space maneuverable robot[END_REF] and aerial manipulators mounted on drones [START_REF] Jeon | Collision avoidance of robotic arm of aerial manipulator[END_REF].

The subject of collision avoidance for robotic manipulators has captured the interest of researchers for decades. In the pioneering work of Khatib [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF], a real-time obstacle avoidance approach based on the classical articial potential eld (PF) concept is introduced. In PF-based methods, the robot is in a hypothetical vector eld, and its motion is inuenced by forces of attraction that guide the robot towards the goal and forces of repulsion that repel it away from obstacles. Subjected to these forces the robot nds its way to the goal while avoiding collisions. An improved implementation of the PF method in which an estimation of obstacle's velocity is taken into consideration when computing the repulsion vector is proposed in [START_REF] Flacco | Real-time computation of distance to dynamic obstacles with multiple depth sensors[END_REF]. PF-based robot self-collision avoidance has been studied, as well as the development of collision avoidance techniques for redundant robots [START_REF] Flacco | Control of Redundant Robots Under Hard Joint Constraints: Saturation in the Null Space[END_REF]. A distributed real-time approach to collision avoidance considering not only the robot tool centre point over the objects in the cell, but also the body of the tool mounted on the robot ange is in [START_REF] Fenucci | A real time distributed approach to collision avoidance for industrial manipulators[END_REF]. A passivity-based control scheme for human-robot safe cooperation is proposed in [START_REF] Maria Zanchettin | Passivity-based control of robotic manipulators for safe cooperation with humans[END_REF]. A collision free trajectory generating method for a robot operating in a shared workspace in which a neural network is applied to create the way points required for dynamic obstacles avoidance is proposed in [START_REF] Meziane | Human-robot collaboration while sharing production activities in dynamic environment: Spader system[END_REF]. In [START_REF] Rubio | Industrial robot ecient trajectory generation without collision through the evolution of the optimal trajectory[END_REF], the authors presented a method for calculating collision free optimal trajectories for robotic manipulators with static obstacles. The proposed algorithm takes into consideration the maximum limits of jerk, torques, and power for each actuator. Tests have been carried out in simulation in a PUMA 560 robot. In [START_REF] Perdereau | Real-time control of redundant robotic manipulators for mobile obstacle avoidance[END_REF],

it is presented a collision avoidance algorithm between robotic manipulators and mobile obstacles validated in simulation environment. Using the variation principle, it is proposed a path planner for serial manipulators with high degrees of freedom operating in constrained work spaces where the planner produces monotonically optimal collision free paths [START_REF] Shukla | A direct variational method for planning monotonically optimal paths for redundant manipulators in constrained workspaces[END_REF]. Based on fuzzy rules [START_REF] Palm | Control of a redundant manipulator using fuzzy rules[END_REF], the authors presented a method for resolving internal joint angles in redundant manipulators. The method allows the EEF to follow the desired path, while the internal motion manifold is used to perform other objectives including collision avoidance with surrounding obstacles.

However, the number of existing studies dedicated to on-line human-robot collision avoidance for manipulators in industrial setups is very limited, and when existing, results are presented in simulation environment. Some of these studies, especially the ones with more direct industrial application, approach collision avoidance by stopping the robot or reducing its velocity when a human reaches a given distance threshold [START_REF] Schmidt | Depth camera based collision avoidance via active robot control[END_REF]. An interesting work denes four safety strategies for workspace monitoring and collision detection: the system alerts the operator, stops the robot, moves the robot away, or modies the robot's trajectory from an approaching operator [START_REF] Mohammed | Active collision avoidance for human robot collaboration driven by vision sensors[END_REF].

Proposed Approach

In this thesis, an industrial task is dened by the o-line generated robot paths (task primitives), which can be programmed by a Computer-Aided Design (CAD) software, or by using more sophisticated methods including Programming by Demonstration (PbD) [START_REF] Skoglund | Programming by demonstration of pick-and-place tasks for industrial manipulators using task primitives[END_REF]. Consequently, a safe human robot interaction is achieved through realtime modication of the o-line generated paths, Figure 6.11. In this scenario of shared workspace, the human coworker focuses on the collaborative task he/she is performing rather than the potential danger from the robot. Using external sensors (IMUs, a magnetic tracker and a laser scanner) the pose of the human body is captured and approximated by capsules. The robot is represented by three capsules. The analytical minimum distance between capsules representing the robot and the human(s) is calculated using the method in section 3.1. The human-robot minimum distance and relative velocity are used as inputs for the proposed collision avoidance controller, where hypothetical attraction and repulsion vectors attract the robot towards the goal/target while repelling it away from obstacles. We also introduce the notion of repulsionvector-reshaping to avoid control discontinuity. By coupling these concepts with a mathematical representation of robot's kinematics we can achieve a task level control with smooth collision avoidance capability. The proposed framework is tested in three dierent experiments, including a real use case for assembly in automotive industry using real sensors and a collaborative industrial manipulator. Results indicate that the robot reacts smoothly by modifying its o-line generated paths to avoid collision with the human coworker. Consequently, our study is the rst of its kind (according to our knowledge) that satises all the following points combined:

1. In our study a real industrial robot (not experimental) is used for performing a typical industrial task;

2. The proposed method for performing the collision avoidance is tailored for industrial use, by combining an o-line path of the EEF (important for industrial applications) with an on-line reactive collision avoidance (required for dynamic collision avoidance);

3. In our method a human coworker is moving freely around the robot, the whole conguration of his/her upper body is captured using real sensors. In this regard, our study diers from the previously presented studies, which mainly based on simulations, while the remaining few (that approached human-robot collision avoidance) utilized an experimental robot, and mostly vision systems which suer from occlusion, in addition other studies focused on the collision avoidance itself, without showing results in a real industrial operation;

4. Unlike other studies, we realized that in a real industrial scenario, the control shall switch between dierent operation modes (as shown later in Figure 3.22 and Algorithm 3.4), this can lead to repulsion action discontinuity, we resolved this issue by proposing the repulsion-vector-reshaping (described in Algorithm 3.2).

Problem Specications

Two major problems in on-line human-robot collision avoidance are related with the reliable acquisition of the human pose in unstructured environments and the diculty in achieving smooth continuous robot motion while generating collision avoidance paths.

For capturing the conguration of the human, the method in the previous (Human Pose Estimation) section is implemented. Concerning the diculty in achieving collision free and smooth continuous robot motion, this problem is particularly visible in an industrial setup where the control algorithm switches between dierent controllers depending on the task-in-hand. In summary, several challenges can be pointed out:

1. Accurate denition of humans/obstacles and robot pose in space using geometric primitives, and calculation of the minimum distance between them;

2. Achieving reliable autonomous human-robot collision avoidance in which the robot adapts the o-line generated nominal paths while keeping the task goal/target.

In such a case, instead of stopping or reducing robot's velocity when humans are nearby, the robot has to continue its motion while avoiding the humans/obstacles;

3. The control strategy shall produce continuous motion of robot's reaction when it adjusts the path to avoid collision. This continuity shall be guaranteed even when switching between dierent controllers;

4. Industrial applications require high-performance control in terms of motion accuracy and agility;

5. Collision free robot motion should be possible and reliable in the entire working volume of the robot.

Experiments demonstrated the ability of the proposed solution to achieve on-line human-robot collision avoidance materialized in the following contributions:

1. Reliable and smooth human-robot collision avoidance in which the robot adapts the o-line generated nominal paths (dened in the initial robot program) while keeping the task target. The robot nds a way to get around the human(s)/obstacles when they are nearby. Human-robot minimum distance and relative velocity are used as inputs to the implemented collision avoidance algorithm;

2. Successfully applying on-line collision avoidance on a real industrial collaborative robot performing industrial assembly tasks in collaboration with a human coworker.

Collision Avoidance Strategy

Hypothetical attraction and repulsion vectors attract the robot towards the goal/target (dened by the o-line generated nominal paths) while repelling it away from human(s)/obstacles. By coupling this concept with a mathematical representation of robot's kinematics we can achieve a task level control with collision avoidance capability.

Repulsion

A vector v cp.rep acts on the point of the robot closest to the obstacle (CP) repelling it away from collision. This vector is dened considering a magnitude v rep.mod (calculated from a base repulsion amplitude v rep ) and a direction s:

v cp.rep = v rep.mod s (3.23)
The direction of the repulsion vector s is taken to be aligned with the line segment associated with the minimum distance:

s = r 1 -r 2 |r 1 -r 2 | (3.24)
Where r 1 is the position vector of the point of the robot closest to the obstacle and r 2 is the position vector of the point of the obstacle closest to the robot. For calculating the base repulsion amplitude v rep we propose to superimpose the repulsion due to the minimum distance (v rep1 ) and the repulsion due to the relative velocity between human and robot (v rep2 ), so that v rep = v rep1 + v rep2 . Here, v rep1 is calculated from the minimum distance d min :

v rep1 = k 1 d 0 d min -dcr -1 , if d min -d cr < d 0 0 if d min -d cr ≥ d 0 (3.25)
Where k 1 is a constant, d 0 is an oset distance around the obstacle's capsule, it species the area around the obstacle where the repulsion vector is activated, and d cr is a critical distance below which the robot is not allowed to be near the human. To enhance the responsiveness of the robot, we propose a dynamical reshaping of the size of the area of inuence around the obstacle d 0 , such that the value of d 0 increases when the relative velocity between the robot and the obstacle increases:

d 0 = d 1 -c v v rel v rel < 0 d 1 v rel ≥ 0 (3.26)
Where v rel is the human-robot relative velocity, c v is a constant and d 1 is the minimum value of the area of inuence around the obstacle. For v rep2 we have:

v rep2 = -c k 2 v rel v rel < 0 0 v rel ≥ 0 (3.27)
Where k 2 is a damping constant and c is a coecient that takes into consideration the proximity of the obstacle from the robot:

c =        1 d min ≤ l 1 1+cos(π d min -l 1 l 2 -l 1 ) 2 l 1 < d min < l 2 0 l 2 ≤ d min (3.28)
Where l 1 and l 2 are constant distances that dene the range around the robot where the damping force is activated. The intuition of using c is that obstacles far away from the robot shall not aect robot's motion since that they do not pose any risk of collision.

The modied repulsion magnitude v rep.mod is calculated from v rep according to Algorithm 3.2. For complex industrial collaborative operations, the collision avoidance controller is typically embedded in a state machine, where the collision avoidance functionality is activated/deactivated according to the tasks being performed. In such a case, discontinuity could appear when calculating the repulsion action. As an example, if the controller is switched (from the collision avoidance deactivation to the collision avoidance activation) while the coworker is near the robot, discontinuity appears. In such a case, a high magnitude of the repulsion vector will act on the robot suddenly.

To solve this problem, the repulsion action is proposed to be time dependent, by introducing the concept of repulsion-vector-reshaping coecient γ, such that when the control scheme is switched the repulsion magnitude is allowed to increase monotonically starting from zero up to its stable value. In the Algorithm, now is a function returning 69 Algorithm 3.2 Modied repulsion vector (magnitude). the current time, τ is a time constant that can be calculated from v max /(5a max ), where v max and a max are the maximum curvilinear velocity and acceleration of the EEF used during collision avoidance, respectively.

Algorithm Modified repulsion vector

Attraction

An attraction velocity vector v e.att attached to the EEF guides the robot towards the goal/target, Figure 3.16. This vector is a function of the error e between EEF's position p e and the goal position p g (dened in the o-line generated nominal paths):

e = p e -p g (3.29)
The attraction velocity is calculated from a proportional term (ψ p ) and a quasi-integral term (ψ i ):

v e.att = β(ψ p + ψ i ) (3.30)
Where ψ p is a pure proportional term:

ψ p = -K p e (3.31)
In which K p is the proportional coecient. The quasi-integral term ψ i , Algorithm 3.3, prevents the quasi-integral from accumulating when the human-robot distance is less than a predened safety distance d 0 . In the Algorithm, d min is the human-robot minimum distance and the integral term is calculated numerically using a simple Euler

ψ i = ψ i -K i ∫ t+△t t edt 4:
else 5:

ψ i = ψ i 6:
end if 7: end for scheme (more sophisticated Runge-Kutta methods could also be used). The term β is used to reduce the magnitude of the attraction vector. This term has the eect of detaching the EEF gradually from the goal when the human coworker is closer to the robot:

β =   2 1 + e - d min -dcr d 0 2 -1   (3.32)
Figure 3.17 shows a block diagram illustrating the proposed method for calculating the attraction vector.

Controller

The robot is controlled at the joint velocity level. (9) Algorithm: Integral term of the attraction vector i ψ β , eq (10) + + eq(3.31) eq(3.32)

Figure 3.17: Block diagram showing the proposed method for calculating the attraction vector. Term ψ i is calculated using the Algorithm 3.3, which is used to avoid windup problem.

the overall angular velocities of the joints requires superimposing the angular velocities due to repulsion and attraction. Thus, the angular velocities due to v cp.rep that acts at CP is calculated using the Damped Least Squares (DLS) [35]:

qrep = J T cp (J cp J T cp + λ 2 I) -1 v cp.rep (3.33)
Where qrep is the joint velocities vector due to the repulsion action, J cp is the partial Jacobian associated with CP on the robot, λ is a damping constant, and I is the identity matrix.

The angular velocities due to v e.att that act at the EEF are calculated from: qatt = J T e (J e J T e + λ 2 I) -1 v e.att

(3.34)
Where qatt is the joint velocities vector due to the attraction action and J e is the Jacobian associated with the EEF. Thus, the total angular velocities of the joints sent to the robot:

qtotal = qatt + qrep (3.35)

Experiments

Experiments are conducted in three main tests:

Human and Robot Representation

The human is represented by ve capsules, Figure 3.18, four capsules used to cover the right/left upper arm and forearm, while the fth capsule is used to cover the torso up to the head. The robot (KUKA iiwa with 7 DoF) is represented by three capsules, Figure the velocity of the EEF and its position in Cartesian coordinates along y axis. These quantities are reported as function of time. We can notice from the plot that at the beginning the human arm is in a resting position and the robot is moving with a constant velocity of about 0.26 m/sec, along y direction, towards the human arm.

When the robot EEF approaches the human arm the minimum distance decreases to a minimum of about 5 centimeters. The EEF velocity is constant until a threshold minimum distance is reached. In such scenario the EEF velocity decreases to start circumventing the obstacle and then accelerates to reach a velocity close to the nominal velocity of 0.26 m/s. It can be concluded that the robot manages to avoid collision with the coworker successfully and the collision avoidance controller smoothly reacts to avoid collision while reaching the task target.

Test 2

In test 2 the human approaches the robot from the side while the robot is stopped at a pre-dened home position. As the human approaches the robot the human-robot minimum distance decreases and the robot reacts in an agile-smooth behaviour to avoid collision. At the top of Figure 3.20, the same quantities presented for test 1 show that at the beginning the human starts walking towards the robot, when the minimum distance reaches 0.5 meters, the robot reacts to avoid collision. When the human goes away the robot returns to the initial home position. The robot successfully avoided collision as in the snapshots at the bottom of Figure 3.20 and in the video segment in [START_REF] Safeea | A video segment demonstrating collision avoidance with human coworker[END_REF].

Test 3

Flexible manufacturing, and industrial assembly processes in particular, present several challenges due to the unstructured nature of an industrial environment. Some tasks are more suited to be executed by humans, others by robots, and others by the collaborative work between human and robot. The ability to have humans and robots working sideby-side will bring enormous eciency benets to exible manufacturing. However, this scenario is challenging, due to the requirement of having the robot avoiding collisions with the coworker in real-time, allowing him/her to focus on the manufacturing tasks and not on the potential danger from the robot side. In this context, we tested the proposed system, in test 3, in a collaborative robotic cell for automotive industry where a human coworker approaches the robot to place a sticker in a car door card while the robot is inserting trim clips in the same door card, Figure 3.21 (a video segment showing the experiment is available in [6]). This exible collaborative task allows the coworker to manage his/her working time and sequencing of operations since he/she is free to place the sticker in the door card at any time and devotes attention to other tasks that he/she has to take care of. Meanwhile the robot continues inserting the trim clips in the door card by using its force feedback to compensate for deviations in the door card positioning. When the human coworker approaches the robot it adapts the nominal path to avoid collisions in a smooth way while keeping its task. For this test, the pre-established nominal path is divided in 3 sub-path segments, Figure 3.22 (A).

In segment 1 and 3 (green line) the collision avoidance control is activated (collision avoidance (CA) paths) while in segment 2 (red and blue line) is deactivated. This is because this path is dened to be the working path where the robot is inserting the trim clips at relative reduced velocity. Starting from a given home position coincident with the beginning of segment 1 and the tool centre point (TCP) the system behaves as in Algorithm 3.4. In Figure 3.22 (B) the robot and goal point move along segment 1 so that an error vector is established. If the human is detected in the safety zone the collision avoidance is activated and the goal point stops moving, Figure 3.22 (C).

When the human is not in the safety zone the robot returns back to the goal point that starts moving with the robot, Figure 3.22 (D). When the robot reaches segment 2 the collision avoidance is deactivated, Figure 3.22 (E). All the users indicate that the system does not appear to be dangerous virtue to the collision avoidance motion which is perceived as smooth and natural. if human is not detected in the safety zone then 3:

if CA path then 4:

Goal point moves along path segment

5:

Error vector generated between TCP and the goal point, subfigure (B)

6:

Attraction velocity generated from the error vector The robot TCP reaches the goal point Integral term stops accumulating

Summary

This section presented a novel method for human-robot collision avoidance for collaborative robotics tailored for industrial applications. The collision avoidance controller demonstrated on-line capabilities to avoid collisions while the robot continues working by keeping the task target. The concept of repulsion-vector-reshaping was introduced to guarantee the continuity of the generated motion when switching between controllers. Experiments indicated that the robot reaction to avoid collisions is well perceived by the coworker, smooth, natural and eective.

Collision Avoidance Algorithm Using Newton's Method

In this section the application of Newton method for the problems of collision avoidance and path planning for robotic manipulators is investigated, especially for robots with high Degrees of Freedom (DoF). The proposed algorithm applies on the potential elds method, where the Newton technique is used for performing the optimization.

Furthermore, a computationally ecient symbolic formula for calculating the Hessian with respect to joint angles is deduced, which is essential for achieving real-time performance of the algorithm in high DoF conguration spaces. As compared to classical gradient descent method (with repulsion and attraction vectors) this implementation of Newton's method oers various advantages: it is mathematically elegant, enhances the performance of motion generation, eliminates oscillations, does not require tedious gains tuning, and gives faster convergence to the solution. Finally the method is validated successfully in simulation for 15 DoF planar manipulator.

Introduction

Redundancy is a term used for referring to robots that have more degrees of freedom than necessary to complete a task. Extra degrees of freedom oer better exibility and can be used to achieve multiple objectives besides to the required task [START_REF] Nakamura | Advanced robotics: redundancy and optimization[END_REF],

as such several studies have been dedicated to the problem of controlling redundant robots [START_REF] Sciavicco | Lagrange and newtoneuler dynamic modeling of a gear-driven robot manipulator with inclusion of motor inertia eects[END_REF][START_REF] Flacco | Control of Redundant Robots Under Hard Joint Constraints: Saturation in the Null Space[END_REF]. In cluttered environments with obstacles, redundancy plays an important role, allowing the robot to reach the goal while avoiding obstacles. Yet, for achieving real-time path planning capacity for high DoF manipulators in unstructured environments, it is required to merge the redundancy control frameworks with some notion of attraction/repulsion vector eld [START_REF] Fabrizio Flacco | A depth space approach to human-robot collision avoidance[END_REF][START_REF] Parigi Polverini | Real-time collision avoidance in human-robot interaction based on kinetostatic safety eld[END_REF]. In such a case, task-level priorities are achieved through projections on the null space of the Jacobian [START_REF] Petri£ | Serial and parallel robot manipulators kinematics, dynamics, control and optimization, chap obstacle avoidance for redundant manipulators as control problem[END_REF], or by using more sophisticated projection criteria [START_REF] Flacco | A reverse priority approach to multitask control of redundant robots[END_REF]. This method for formulating collision avoidance is proved to be computationally ecient. Nevertheless, in many cases this implementation requires a predened trajectory of the end-eector (EEF) and the repulsion/attraction vectors are custom engineered with many parameters to tune, as shown in the previous section. Moreover, this method suers from oscillation problems, which become more evident for increasing number of obstacles and for higher DoF robots, this was noticed in our implementation in the previous section, yet a simple (but not very elegant) solution is implemented by ltering. A method with better immunity to local minima problem is the Probabilistic RoadMaps (PRM) proposed in [START_REF] Kavraki | Probabilistic roadmaps for path planning in high-dimensional conguration spaces[END_REF]. This method is composed of two phases. The rst phase is called the learning phase, it is performed for stochastic generation of a roadmap of the scene (robot and its surroundings). The second phase is called the query phase. In this phase, paths are queried, and the roadmap is searched for a feasible path. While PRM is a powerful method, it suers two major problems. The rst of which is due to computational cost, where according to the times reported in [START_REF] Kavraki | Probabilistic roadmaps for path planning in high-dimensional conguration spaces[END_REF] it can be concluded that for high DoF robots PRM is unsuitable for implementation in control loops with fast update rates.

The second drawback comes from the fact that the resulting trajectories generated by this planner are not smooth and require processing before implementation on the robot [START_REF] Kim | Extracting optimal paths from roadmaps for motion planning[END_REF].

In this section, the application of Newton method on the problem of collision avoidance for hyper redundant robotic manipulators is investigate. In such a case, the problem is reformulated as a second order optimization problem. The Newton method is utilized for performing the optimization, as a result generating collision-free and smooth paths. The application of Newton method in robotics oers various advantages. As reported in the optimization literature [START_REF] Boyd | Convex optimization[END_REF] using the Newton method for optimization oers: (1) better numerical stability, (2) faster convergence and (3) eliminates numerical oscillations that might be induced during rst order optimization. In robotics this translates into generating smoother paths and faster execution, as reported in [START_REF] Ren | Modied newton's method applied to potential eld-based navigation for mobile robots[END_REF][START_REF] Ren | Modied newton's method applied to potential eld-based navigation for nonholonomic robots in dynamic environments[END_REF], where the modied Newton method is applied for the problem of mobile robot navigation, where the robot is approximated by a point in a plane and the potential eld is an explicit function of the conguration variables, in such a case calculating the Hessian analytically is possible as noted in [START_REF] Mcisaac | Modied newton's method applied to potential eld navigation[END_REF]. While the Newton method has been described in literature for the problem of mobile robot navigation, this method has not been investigated yet for the problem of collision avoidance in robotic manipulators. This comes from the fact that the resulting potential elds, which are functions of Cartesian space variables, are implicit functions of conguration space variables, with high non-linearity due to the nature of the complex transform function between the spaces [START_REF] Denhavit | A kinematic notation for lower-pair mechanisms based on matrices[END_REF]. This makes nding a closed form solution for the Hessian in joint space a challenging task. In this section, a systematic method for applying Newton technique for robotic manipulators, at kinematic level, is addressed.

We present a formula for ecient calculation of the Hessian matrix, by deducing a relationship between the Hessian in Cartesian space and the resulting Hessian in joint space. This allows to speed up the algorithm and makes it applicable for real-time implementation. In addition to the aforementioned advantages the proposed solution contributes in the following:

• Bigger optimization steps: In Newton method bigger optimization steps can be used, resulting in faster execution rates, as compared with the gradient method;

• Faster convergence: Newton method oers better performance in terms of convergence rate than the gradient descent [START_REF] Mokhtar S Bazaraa | Nonlinear programming: theory and algorithms[END_REF];

• Elegant formulation: The proposed method is mathematically elegant, eliminates the need for tedious tuning of control parameters, and can be applied on hyper redundant manipulators. The method does not require pre-planned trajectory and keeps the number of control parameters to a minimum.

The proposed method can be used either for real-time control of the manipulator to avoid collisions with obstacles, or it can be used for o-line path planning. The application of the presented method extends easily to computer animations and to motion generation for kinematic chains in virtual environments with obstacles.

Mathematical Formulation

For achieving a robot motion that drives the EEF towards the goal, while avoiding collision with obstacles, the potential elds method by Khatib [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF] is considered. In this method an attraction potential eld u att attracts the EEF towards the goal. This potential is expressed in Cartesian space as a function of EEF position. In addition, repulsion potential elds repel the robot away from obstacles. As a result the robot is subjected to a total potential eld u total , given by:

u total = u att + u rep (i,j) (3.36)
Where u rep (i,j) is a repulsion potential eld that repels link i from colliding with obstacle j. The potential u rep (i,j) is expressed in Cartesian space as a function of the minimum distance between link i of the robot and obstacle j. Given that the control algorithm of the robot is running in a loop, each iteration the robot shall move in a way to minimize the total potential function, as a result the robot moves towards the goal while avoiding collisions with obstacles. Traditionally, the notion of attraction and repulsion vectors is used for performing the collision avoidance. This traditional method can be interpreted as minimizing the potential function using the gradient descent technique, where the gradients represent the repulsion and attraction vectors. In contrast, we propose to use Newton method for minimizing the potential function. As shown in Appendix A, taking the second order approximation of the potential function yields the update equation for the joint angles:

q m+1 = q m -α(H + λI) -1 g (3.37)
Where q m , q m+1 are the resulting vectors of the angular positions calculated at timesteps m, m + 1. In real world scenario q m represents the angular positions of the joint angles as acquired from the encoder measurements, α is a scalar representing the minimization step, H is the Hessian of the potential eld with respect to joint angles, g is the gradient of the potential eld with respect to joint angles, I is the identity matrix, and λ is a damping factor. By adopting a similar approach to the already developed techniques in dierential inverse kinematics, the damping factor can be calculated as a function of the minimum singular value [START_REF] Charles | Dexterity measures for the design and control of kinematically redundant manipulators[END_REF], but unlike inverse kinematics methods, in our method the minimum singular value pertains to the singular value decomposition of the Hessian matrix H. For fast execution of the proposed algorithm the Hessian shall be calculated analytically. By taking a second order approximation of the potential function, equation (3.36), it is proven (in Appendix A) that for robotic manipulator the Hessian is given by:

H = J T k ∇ 2 u k J k (3.38)
Where ∇ 2 u k is the Hessian matrix of u k with respect to Cartesian coordinates. Given that u k is an explicit function of Cartesian coordinates for both the attraction and repulsion elds, then it is possible to derive a closed form solution for the matrix ∇ 2 u k , either by hand or using symbolic math software. J k is the partial Jacobian associated with the potential eld u k . In case of the attraction potential u att , then J att is the Jacobian associated with the EEF. In case of a repulsion potential u rep (i,j)

due to the eect of obstacle j on link i, then J (i,j) is the partial Jacobian associated with control point cp (i,j) , where cp (i,j) is the point of link i closest to obstacle j. The gradient (as shown in Appendix A) is given by:

g = J T k ∇u k (3.39)
Where ∇u k is the gradient of potential function u k in Cartesian space. For achieving better numerical robustness and by analogy to dierential kinematics approaches, the alternate Jacobian described in [START_REF] Samuel R Buss | Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods[END_REF] can be utilized for calculating the partial Jacobians. As a result, it can be concluded that the presented mathematical formulation forms a systematic way to integrate several potential elds:

• Attraction potential eld attracting the EEF to the goal position;

• Repulsion potential eld repelling the robot away from obstacles;

• Repulsion potential eld repelling the robot away from self-collision and joint limits.

Experiments

To evaluate the performance of the proposed method, real-time simulations using MATLAB are implemented, where the attraction potential eld u att is chosen to be a quadratic function with a minima at the goal position. In such a case this potential eld drives the EEF towards the goal:

u att = 1 2 k att (x ef -x g ) T (x ef -x g ) (3.40)
Where k att is the attraction constant, x ef is the position of the EEF and x g is the position of the goal point. On the other hand, the repulsion potential eld chosen for the simulations is identical to the one in [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF], where according to the proximity between obstacle j and link i the repulsion potential u rep (i,j) is given by:

u rep (i,j) = 0 ρ (i,j) > ρ 1 krep ρ (i,j) -ρ 0 - krep ρ 1 -ρ 0 ρ (i,j) ≤ ρ 1 (3.41)
Where k rep is a repulsion constant, ρ (i,j) is the minimum distance between obstacle j and link i of the robot, ρ 1 is the distance at which the repulsion potential starts acting on the robot, ρ 0 denes the forbidden area around the obstacle where the robot can not evolve. Similar repulsion elds could be added for joints limits avoidance and for self collision avoidance.

Under the previous potential functions a planar hyper redundant manipulator, with 15 DoF, is navigating through 10 obstacles towards the goal. The goal is marked by a green circle and obstacles are marked by red circles, as shown in Figure 3.23, where the Newton method is applied to perform the collision avoidance and the motion generation. The gure shows a sequence of congurations taken by the robot during its motion towards the goal. In the beginning of the simulation, the robot is in an initial conguration where it is vertically straight, links are fully stretched. Afterwards the robot starts bending and reaching towards the goal while navigating through the obstacles. In the simulation, the Hessian matrix was calculated eciently using equation (3.38), as such an update time lower than 5 milliseconds was achieved on dual core laptop without code optimization.

Test 1

In this test the Newton method applied to the problem of collision avoidance and path planning for high DoF manipulators is compared with the gradient descent method.

For this purpose the following MATLAB simulation is proposed. Two identical planar robots with 15 DoF are navigating towards the same goal point, while avoiding collision with 10 circle-shaped obstacles. In the beginning of the simulation, the two manipulators are coincident with each other, dashed line in Figure 3.24. The blue manipulator is controlled using the Newton method and the red manipulator is controlled using the gradient descent method. For performing the comparison the same goal, obstacles, potential elds are used for both manipulators. The only dierence is in the optimization method used, where for the red robot the gradient method was used to calculate the optimization direction, while for the blue robot the Newton method was used to calculate the optimization direction. For both robots the magnitude of the optimization step α is identical, in other words the update equation for both robots is:

q m+1 = q m -αs (3.42)
Where s is the optimization direction. In the case of the Newton method s is given by:

s = (H + λI) -1 g (H + λI) -1 g (3.43)
In case of the gradient method:

s = g g (3.44)
Figure 3.24 shows the nal congurations taken by the two manipulators. Figure 3.25 shows joints angles for this simulation as recorded in the Newton method (left) and the gradient descent method (right). After performing the simulation the two methods are compared in terms of:

• Quality of the path: The Newton method generated an oscillation free path. On the other hand, the gradient method suered intermittent oscillations. This is evident in Figure 3.26, where the angular position of the rst joint is plotted during the Newton method (left) and during the gradient method (right). It is worth mentioning that the oscillations, or the zig-zag phenomena as known in the optimization community, happens in the gradient method when one component of the gradient keeps ipping direction during the minimization, as such the minimization point keeps overshooting in the vicinity of a cleavage on the optimization surface [START_REF] Boyd | Convex optimization[END_REF], this phenomena does not appear in the Newton method.

• Distance between robot and obstacles: In Newton method the robot keeps bigger minimum distance away from obstacles than in the gradient method where the robot reaches closer to obstacles, despite the fact that identical potential elds, geometries, and optimization steps are used. This is demonstrated in Figure 3.27, where the distances ρ (15,j) -ρ 0 between the last link of the robot (link 15) and the obstacles, j = 1, 2..10, are plotted for Newton method (left), and for the gradient method (right). Numerical values of the minimum distance reached during the simulations are listed in Table 3.2.

Summary

This section addressed the application of Newton method to collision avoidance and path planning problems for high DoF manipulators. The standard articial potential elds is used. The collision avoidance problem is viewed mathematically as an optimization problem. The Newton method is utilized for performing the optimization.

As such, lling a gap in the robotics literature, where the Newton method has been described for application on collision avoidance of mobile robots only owing to the simplicity in calculating its Hessian matrix. For robotic manipulators the Hessian matrix is more complex and computationally expensive, consequently, an ecient formula for speeding up its calculation is presented. To compare the performance of the Newton method against gradient based methods, two tests using simulation were performed. In test 1 the Newton method and the gradient method are compared in terms of quality of the path generated, oscillations, and the minimum distance to obstacles. In test 2 three dierent methods (1) Newton (2) xed step gradient descent (3) gradient descent with momentum are compared in terms of the convergence rate. Qualitatively, the Newton method generates oscillation free paths and resulted in an interesting, snake like, behavior of the robot. Quantitatively the Newton method is superior in terms of convergence rate, 55 iteration for the Newton method, 18073 iterations for the xed step gradient descent and 7928 iterations for the gradient descent with momentum method. Future work will be dedicated to applying the proposed technique for controlling industrial robotic manipulator in human centered environments to achieve real-time collision avoidance with dynamic obstacles.

2. MATLAB supports various debugging and data visualisation tools, which facilitate the process of debugging advanced control algorithms including the collision avoidance algorithm.

3. MATLAB allows interfacing with Vrep (3D simulation environment). This allows simulating and experimenting the collision avoidance algorithm in a virtual environment before implementation on the real robot. This is extremely important for verifying the control algorithm, and oers two main advantages, (1) reducing testing and validation time, (2) reducing the risk of equipment loss and injuries.

4. The used sensors are easily integrated into external computers. Hardware manufacturers oer drivers and Application Programming Interfaces (APIs) for using their sensors on personal computers.

5. MATLAB compatible APIs already exist for interfacing with the used senors.

6. Unlike the robot hardware, upgrading an external computer with better hardware is easily done. Thus allowing the implementation of more complex algorithms without compromising the real-time performance.

7. Using an external computer allows the experimentation with various types of sensors easily.

Given the powerful advantages of using MATLAB, it was decided to bundle the developed interfaces, mostly during the development of the collision avoidance system, in a MATLAB toolbox that allows users to control the robot more easily. The developed toolbox was named the KUKA Sunrise Toolbox (KST). It was published in the IEEE Robotics & Automation Magazine in [11] including proper documentation and tutorials.

Introduction

Robotics is a multidisciplinary subject. It involves various engineering specialities, including: electrical engineering, mechanical engineering, control engineering, computer science and software engineering. Consequently, to develop a robust robotic application, all previous disciplines work hand in hand. To avoid reinventing the wheel and to facilitate the way robots are programmed, various researches have developed robotics libraries and made them available for use. The robotics toolbox for MATLAB [START_REF] Peter I Corke | A robotics toolbox for MATLAB[END_REF][START_REF] Corke | MATLAB toolboxes: robotics and vision for students and teachers[END_REF] is a MATLAB library that implements various robotics algorithms important for calculating the forward/inverse kinematics, Jacobian, robot dynamics and others. The Machine Vision Toolbox [START_REF] Corke | The Machine Vision Toolbox: a MATLAB toolbox for vision and vision-based control[END_REF] is another MATLAB toolbox that is used to integrate vision into robotics applications. Those packages are used for robot simulation and for developing control algorithms, yet they do not interface directly with the robot. Due to the advantages of using an external computer for controlling industrial robots, various software packages have been proposed. Those packages dier in the type of robots they support, the programming languages they implement, in addition to the dierent functionalities they oer. Using the KUKA Control Toolbox (KCT) [START_REF] Chinello | Kuka control toolbox[END_REF] the user is able to control KUKA industrial robots equipped with the KUKA Robot Controller (KRC) from an external computer using MATLAB. JOpenShowVar [START_REF] Sanlippo | Controlling Kuka Industrial Robots: Flexible Communication Interface JOpenShowVar[END_REF][START_REF] Sanlippo | Jopenshowvar: An open-source cross-platform communication interface to kuka robots[END_REF] is another library that allows interfacing KRC controllers from external PC using Java. For KUKA iiwa collaborative robots (equipped with the Sunrise Controller) various packages also exist. From the Robot Operating System (ROS) [START_REF] Quigley | ROS: an open-source robot operating system[END_REF] the user is able to interface with KUKA iiwa using the iiwa_stack [START_REF]ROS Indigo/Kinetic metapackage for the KUKA LBR IIWA[END_REF] and KUKA-IIWA-API [START_REF] Mokaram | A ROS-integrated api for the kuka lbr iiwa collaborative robot[END_REF]. The iiwa_stack is a library that allows controlling iiwa robots from ROS. For communication purposes it implements the ROSJava nodes (third party libraries). In addition, its motion functions are built on top of the Smart.Servo library (from KUKA) and does not implement the robot's native point-to-point motion functions. The KUKA-IIWA-API provides a ROS interface for controlling KUKA iiwa from an external PC, similar to iiwa_stack the interface provides various topics, the user can subscribe or publish to these topics, but in the case of KUKA-IIWA-API the update rate for data feedback is low (10 Hz).

To control the robot using iiwa_stack or KUKA-IIWA-API a familiarity with ROS is required. On the other hand, the KUKA Sunrise Toolbox (KST) allows controlling KUKA iiwa robots from an external computer using MATLAB, such that any person with basic knowledge on MATLAB can start using KUKA collaborative robots without requiring advanced knowledge on how to program industrial manipulators. With more than a hundred dierent functions, KST supports various operation modes including soft-real time control, point-to-point motion calls and non-blocking motion calls, those functions are divided into seven categories detailed in subsection 3.5.6.

The toolbox was designed to be user friendly, so that anyone with basic knowledge about MATLAB can start using KUKA iiwa collaborative manipulators in a minimal amount of time. KST is provided under MIT license, and is freely available in the GitHub repository [26]. The repository also includes a detailed documentation, video tutorials, code snippets and tutorial MATLAB scripts.

Architecture

The KST has a client server architecture, Figure 6.14. For achieving a robust data transmission the toolbox utilises the Transmission Control Protocol/Internet Protocol (TCP/IP). The server, represented by KUKA Sunrise Cabinet in Figure 6.14, is a multi-threaded Java application that runs inside the robot controller. The main thread of the Java application, KST Main in Figure 6.14, is mainly used to run the motion commands. The second thread, KST Server in Figure 6.14, is mainly used for communication layer and also data acquisition using robot's own sensors including: the integrated joints torque sensors, the encoders and the inputs integrated in the ange.

Before starting to utilize the toolbox the user shall synchronize the server application into the robot controller, after being synchronized the user shall run the server, once is running, the toolbox can be used to connect to the robot from MATLAB.

On the other hand, the client part is written using MATLAB scripting language, the various functions oered by the toolbox are wrapped inside the KST.m class. Therefore the user is able to control the robot from MATLAB through this class.

Figure 3.29: Architecture of the toolbox.

Modus Operandi

The KUKA Sunrise Toolbox implements object oriented programming. The main programming class of the toolbox is KST, a wrapper class which includes the various properties and functions (methods) required to control the manipulator from MAT-LAB. To get an access to the controller of the robot, the user shall rst instantiate an object from the KST class by calling its constructor:

> > iiwa = KST(ip,rT,fT,Tef_fl);

Where iiwa is an instance of the KST class, ip is a string variable which contains the IP of the robot, rT is a constant referring to robot type 7R800 or 14R820, fT is a constant referring to type of the ange attached to the robot, and Tef_fl is 4x4 matrix representing the transformation matrix from the end-eector to the frame of the ange (Tef_fl is an optional argument if omitted the KST uses the identity matrix by default).

In In the following subsections each category is presented along with illustrative snippet code examples.

Networking

The KST provides methods to establish and terminate the connection with the robot controller (Table 6.1 of Appendix B). The TCP/IP connection is initialized by typing:

> > iiwa.net_establishConnection();

The TCP/IP connection is terminated by typing:

> > iiwa.net_turnOffServer();

Soft real-time control

The soft real-time motion control is required when performing online motion generation, where the trajectory is generated/adjusted online, for example from external sensor data (see Test 3 in subsection 3.3.6.3). KST allows soft real-time control of the robot. The control modes supported are joints velocity mode and joints/end-eector position control mode. The soft real-time motion in joints position mode is activated by typing the command: > > iiwa.realTime_startDirectServoJoints()

Once started, the target angular positions of the joints can be sent to the robot using:

> > jPos={pi/3,0,0,-pi/2,0,pi/6,pi/2}; > > iiwa.sendJointsPositions(jPos);

Where jPos is a cell array of dimension 1x7 containing the values of the target joints angles in radians (KUKA LBR iiwa is a 7 DoF manipulator). A loop can be implemented for sending a stream of joint angles to the robot, so that the robot will perform the motion between the points on-the-y.

To stop the soft real-time motion control the following method is used: > > iiwa.realTime_stopDirectServoJoints();

The impedance control is activated by: > > iiwa.realTime_startImpedanceJoints( massOfTool,cOMx,cOMy,cOMz, cStifness,rStifness,nStifness);

Where massOfTool is the mass of the tool (in kg), cOMx, cOMy, cOMz are the coordinates of the centre of mass of the tool in the reference frame of the ange (in mm), and cStifness, rStifness, nStifness are the stiness values for Cartesian translation, Cartesian orientation and null space, respectively. The robot joint positions are updated online using:

> > iiwa.sendJointsPositions(jPos);

To stop the impedance functionality: > > iiwa.realTime_stopImpedanceJoints();

The KST simultaneous control-feedback methods allow to control the robot while simultaneously receiving feedback about the robot's internal state, namely torques, joint positions and end-eector position. These functions are possible due to the multithreaded nature of the KST server.

Point-to-point motion

The KST point-to-point (PTP) motion methods allow the robot to move from the current conguration/end-eector pose (position and orientation) to a dened target conguration/pose. The user is not required to implement the path planning algorithms since they are implemented directly by the software. The robot can be controlled in Cartesian space and also in joint space. For example, to move the robot in joint space:

> > jPos={pi/3,0,0,-pi/2,0,pi/6,pi/2}; > > relVel=0.25; > > iiwa.movePTPJointSpace(jPos,relVel);

Where relVel represents the override velocity assuming a value from zero to one. The robot can also be controlled in Cartesian space. For example, to move the end-eector along a straight line:

> > pos={400,0,580,-pi,0,-pi}; > > vel=50; > > iiwa.movePTPLineEEF(pos,vel);

Where pos is the target end-eector pose in Cartesian space, a 1x6 cell array in which the rst three cells are the end-eector x,y and z coordinates (relative to robot base) and the last three cells are the X-Y-Z xed rotation angles representing the orientation of the end-eector in the space. The variable vel represents the linear velocity of the end-eector in mm/sec. The KST also includes methods for arc and ellipse motion.

The aforementioned code snippet utilizes a blocking call. However, the KST also supports non-blocking PTP motion methods (listed in Table 6.4 of Appendix B). This

gives the possibility to perform other computations, like path planning or input sampling, while the robot is moving. Examples of these methods are the nonBlocking_isG-oalReached and the nonBlocking_movePTPArcXY_AC.

In some applications the ability to interrupt robot motion upon the occurrence of some condition is required, for instance if a given torque threshold is reached. The KST allows such operations using the conditional motion functions. An example on using those functions is in the tutorial script KSTclass_Tutorial_ptpConditionalTorques.m.

Setters

KST provides methods for updating robot parameters, including the state of the outputs and the LED light of the ange. For example, the LED light is useful to alert the human about the state of the robot and can be associated to the beginning and end of a given robot task. To turn on the blue LED the following method is used: > > iiwa.setBlueOn();

To turn o the blue led: > > iiwa.setBlueOff();

Getters

The KST provides functionalities to acquire various internal parameters of the robot such as joint angles, end-eector pose, force/moment acting on the end-eector, joints torques and the values of the IO connector inputs. For example, to acquire the joint angles of the robot, the following method is utilized:

> > jPos = iiwa.getJointsPos()

jPos = {[1.017]} {[1.919]} {[0.513]} {[0.621]} {[0.698]} {[0.341]} {[0.175]}
The variable jPos is a 1x7 cell array with the robot joint angles in radians.

General purpose

The KST integrates several methods to calculate forward kinematics, inverse kinematics, partial Jacobian, forward dynamics, inverse dynamics, among others. As an example, to calculate the transform matrix of the end-eector and the Jacobian associated with the end-eector:

> > [eef_transform,J] = iiwa.gen_DirectKinematics(q) eef_Transform = -0.8162 -0.2795 0.5056 0.6229 0.5210 0.0220 0.8532 0.8155 -0.2496 0.9599 0.1277 0.3434 0.0000 0.0000 0.0000 1.0000 J = -0.815 0.001 0.281 0.148 -0.039 -0.211 0.00 0.622 0.002 -0.214 -0.297 0.010 0.142 0.00 0.000 -1.017 -0.123 0.578 0.086 -0.115 0.00 0.000 -0.866 0.469 0.664 0.733 -0.417 0.50 0.000 0.500 0.813 -0.581 0.627 0.112 0.85 1.000 0.000 -0.342 -0.469 0.261 0.902 0.12

Where eef_transform is the end-eector transform matrix, J is the Jacobian associated with the end-eecor, and the variable q is a 7x1 vector of the robot joint angles in radians.

To calculate the partial Jacobian associated to a given point in a given robot link:

> > J = iiwa.gen_partialJacobean(q,ln,Pos);

Where ln is the number of the link for which the partial Jacobian is calculated (an integer number from 1 to 7), Pos is a 3x1 vector representing the position of the point at which the partial Jacobian is calculated (this vector is described in the reference frame of the link specied by ln), and J is the partial Jacobian at the described point.

Precision Hand-Guiding

In the hand-guiding mode the robot moves compliantly according to the force applied by the user. While it is a good tool for rough positioning, end-eector (EEF) precision positioning is still dicult to achieve by hand-guiding. This section presents a novel method for precisely hand-guiding a robot at the EEF level. Using the EEF force/torque measurements the hand-guiding force/moment is calculated after compensating for the weight/inertia of the EEF. The proposed control solution is inspired by the motion properties of a passive mechanical system subjected to Coulomb/viscous friction. As a result, the human can control the linear/angular motion of the decoupled EEF. To ensure that the robot does not exceed its joints limits during the hand-guiding process, a solution to scale down the linear/angular velocity of the EEF near the joints limits is incorporated into the control strategy. Several experimental tests were conducted in an assembly task using a KUKA iiwa manipulator. Performance was evaluated considering the precision in positioning, ability to avoid joint limits and vibrations during hand-guiding. A qualitative and quantitative comparison of the proposed control strategy against the o-the-shelf KUKA iiwa hand-guiding is also presented.

Introduction

Physical human-robot interaction has been studied extensively in the last decade. Research in this eld focused on the ability to interact with a robot physically while preserving safety. This includes hand-guiding, which is a representative functionality of collaborative robots that allows the unskilled user to interact and to program robots in a more intuitive way than using the traditional teach pendant. Existing collaborative robots include hand-guiding functionality with limitations in terms of the precision and accuracy required for many tasks. When precision positioning (position and orientation of the EEF) is required, the teach pendant is still widely used, where it remains the main option for most of robots in operation [START_REF] Fukui | Development of teaching pendant optimized for robot application[END_REF]. However, the use of the teach pendant limits the intuitiveness of the teaching process, also it is time consuming (an important parameter on the factory oor) [START_REF] Hoo | A survey on robot teaching: Categorization and brief review[END_REF]. In addition, the use of the teach pendant requires a mental visualisation of the dierent reference axes of the robot, which lacks intuitiveness and becomes cumbersome as the human moves around the robot [START_REF] Kushida | Human direct teaching of industrial articulated robot arms based on force-free control[END_REF]. In some scenarios, using the teach pendant for robot positioning can lead to undesirable collisions, which may cause damage to sensitive equipment.

Owing to its importance, hand-guiding has been addressed in several studies, where dierent terminologies have been used by researchers for describing the hand-guiding process: manual-guidance [START_REF] Massa | Manual guidance for industrial robot programming[END_REF], force guidance [START_REF] Boulhosa Rodamilans | A comparison of industrial robots interface: force guidance system and teach pendant operation[END_REF], lead-through programming [START_REF] Ragaglia | Accurate sensorless lead-through programming for lightweight robots in structured environments[END_REF],

or walk-through programming [START_REF] Bascetta | Walkthrough programming for robotic manipulators based on admittance control[END_REF]. Due to its advantages, the hand-guiding functionality is gaining popularity in the robotics community, allowing to program a robot in an intuitive manner by moving it compliantly with the applied force.

In this section it is presented a novel method for hand-guiding while preserving the precision at EEF level, an important requirement for plenty of robotized manufacturing operations. By analogy to the motion properties of a passive mechanical system, mass subjected to Coulomb/viscous friction, a control scheme is proposed, which governs the linear/angular motion of the decoupled EEF. To avoid violating the physical limits of the joints, a scaling factor is introduced, it is used to scale down the linear/angular velocity of the EEF near the joints limits. To test the proposed approach, experiments were conducted in robot assisted assembly tasks requiring precision, where a KUKA iiwa robot is used. Performance was evaluated considering the precision in positioning, ability to avoid joints limits and the smoothness of the motion during hand-guiding.

In addition, a qualitative and quantitative comparison of the proposed control strategy against the o-the-shelf KUKA iiwa hand-guiding is presented. Results indicate that the proposed solution allows smooth Cartesian hand-guiding motion, with low level of vibrations, while having the ability to react for avoiding the joints limits.

State of the art

The importance of hand-guiding as a functionality to teach collaborative robots is well known in the robotics community. At the same time, it is still a challenge to have robots and humans sharing the same space and performing collaborative tasks with the required safety levels to minimise the risk of injuries [START_REF] Sami Haddadin | Requirements for safe robots: Measurements, analysis and new insights[END_REF]. Humans can interact physically with a robotic arm by hand-guiding the robot into the desired grasping poses while the robot's conguration is recorded [START_REF] Balasubramanian | Physical human interactive guidance: Identifying grasping principles from human-planned grasps[END_REF]. Robot assistance through handguiding is also used to assist humans in the transport of heavy parts [START_REF] Solanes | Human-robot collaboration for safe object transportation using force feedback[END_REF]. In [START_REF] Sonehara | Study on application of a human-robot collaborative system using hand-guiding in a production line[END_REF] the integration of a human-robot cooperative system (implementing hand-guiding) in a production line was studied including safety, operability and the level of assistance to humans. The assisted gravity compensation method is presented in [START_REF] Emmerich | Assisted gravity compensation to cope with the complexity of kinesthetic teaching on redundant robots[END_REF]. This method facilitates the hand-guiding process, making it more intuitive for unskilled users. A virtual tool method for kinesthetic teaching of robotic manipulators is proposed in [START_REF] Ferretti | Assigning virtual tool dynamics to an industrial robot through an admittance controller[END_REF].

The method builds on an admittance controller that utilizes the feedback from a FT sensor attached at the EEF. In a recent study an operator guides a collaborative robot along a predened geometric path while taking required joint constraints into account and thus implicitly considering the actuator dynamics [START_REF] Hanses | Hand-guiding robots along predened geometric paths under hard joint constraints[END_REF]. Kinesthetic teaching and learning have been combined to enable non-experts to congure and program a redundant robot in the presence of constraints such as conned spaces [START_REF] Wrede | A user study on kinesthetic teaching of redundant robots in task and conguration space[END_REF].

Concerning the use of conventional industrial robots, hand-guiding is normally sensor based. However, sensorless hand-guiding methods have been proposed. In [START_REF] Lee | Torque control based sensorless hand guiding for direct robot teaching[END_REF],

the dynamic model of a robot along with the motor current and friction model is used to determine the user's intention to move the EEF of a robot instead of directly sensing the external force by the user. By gaining access to motors currents measurements, hand-guiding can be achieved without the need to install an external FT sensor on the manipulator [START_REF] Geravand | Human-robot physical interaction and collaboration using an industrial robot with a closed control architecture[END_REF]. In [START_REF] Ragaglia | Accurate sensorless lead-through programming for lightweight robots in structured environments[END_REF] the authors presented a sensorless method for hand-guiding the EEF in a structured surrounding.

The problem of Cartesian impedance control of a redundant robot executing a cooperative task with a human has been addressed in [START_REF] Ficuciello | Variable impedance control of redundant manipulators for intuitive humanrobot physical interaction[END_REF]. Redundancy was used to keep robot's natural behaviour as close as possible to the desired impedance behaviour, by decoupling the EEF equivalent inertia. The authors claim that this allows to easily nd a region in the impedance parameter space where stability is preserved. An important study in the eld uses impedance control of multiple robots to aid a human moving an object [START_REF] Kosuge | Dynamic control for robot-human collaboration[END_REF]. Experiments were conducted considering motion along 1 DoF. In medical domain, robot assisted surgery demands high precision and consequently hand-guiding accuracy. In this scenario, an approach that enables an operator to guide a robot along a predened geometric path while considering joint constraints and implicitly the actuator dynamics is proposed in [START_REF] Hanses | Hand-guiding robots along predened geometric paths under hard joint constraints[END_REF].

However, the subject of precision in hand-guiding for robot assisted assembly operations was not fully covered in the literature. In this section it is presented a solution for precisely hand-guiding a robot at the EEF level applicable for precise assembly operations.

Work principle

Assuming force feedback at the robot EEF (the robot may have joint torque sensors or a force/torque sensor attached to the ange of the robot), three groups of robot hand-guiding motion are considered, Figure 4.1:

1. The rst motion-group used to control the positioning (linear displacements) of the EEF along the x, y and z axes of the robot base;

2. The second motion-group used to control the orientation of the EEF axis in Cartesian space;

3. The third motion group represents the rotation of the EEF around its axis.

Those motion groups are selected because they cover the 6-DoF translation and rotation in Cartesian space, also because they are very intuitive for workers (just imagine how humans insert a key into a keyhole). A video showing the proposed concept is in [START_REF] Safeea | A video segment demonstrating the motion groups for the precise hand-guiding[END_REF].

The robot moves in one of the three motion groups when a force/moment is applied at the EEF. Two terms will be introduced to specify the input force/moment. The rst term is the hand-guiding force for linear positioning, dened as the force applied by the operator at the EEF. The second term is the hand-guiding moment for angular positioning, dened as the moment applied by the operator at the EEF. The hand-guiding force/moment shall be calculated from the force/moment measurements (directly acquired from FT sensor, or indirectly calculated from integrated joint torque sensors).

Those measurements represent the forces/moments due to (1) the EEF weight, (

the inertial force/moment due to the acceleration of the EEF, and (3) the external hand-guiding force/moment applied by the operator. To simplify the calculations, we omit the inertial force/moment due to the acceleration of the EEF because in precise hand-guiding they are relatively small compared to EEF's weight and to the external force/moment. In this context, the force/moment measurements are approximated to be due to the EEF weight and the external hand-guiding force/moment applied by the operator. The components of the hand-guiding force described relative to the robot base frame are used as inputs for the proposed linear motion controller, i.e., for positioning the EEF according to the robot base frame (rst motion group). The components of the hand-guiding moment are used as inputs for the orientation controller (second and third motion groups).

Throughout this section, the following notation is used to describe a vector in a reference frame:

• The superscript b is used for vectors described in base frame of the robot;

• The superscript e is used for vectors described in end-eector frame. It is considered that the origin of the EEF frame is coincident with the point where the hand-guiding force/moment acts. Consequently, the human operator applies the hand-guiding force/moment at the handle, which is coincident with the origin of the EEF reference frame;

• The superscript s is used for vectors described in sensor measurement frame. This frame is dened by the manufacturer of the FT sensor (in case an external sensor is used). However, if a sensitive robot is used, then the sensor measurement frame is virtual, at which the equivalent values of the external forces/moments are calculated from the torque readings at the integrated torque sensors. In our experiment with KUKA iiwa robot, the sensor measurement frame is considered at the frame of the ange;

• The little hat above a vector (ˆ) is used to denote the skew symmetric matrix of that vector.

Hand guiding force

The measured hand-guiding force described in robot base frame is f b = (f x , f y , f z ). This force f b serves as the input for calculating the 1st motion group command. The maximum of the components (f x , f y , f z ) is used as the control command to move the end-eector along one axis at a time. The hand-guiding force (f e x , f e y , f e z ) described in the EEF reference frame is calculated from:

  f e x f e y f e z   = R e s     u s x u s y u s z   -R s b w   (4.1)
Where (u s x , u s y , u s z ) are components of force measurements in sensor frame, R e s is a constant rotation matrix that describes the orientation of the sensor measurement frame in relation to the EEF frame, and the vector w is dened by w = [ 0 0 -w ] T for base mounted robots in the upright position, in which w is the weight of the EEF. R s b is the rotation matrix describing the orientation of robot base frame in relation to sensor frame: Table 4.1: Values and conditions to obtain (a, b, c). a b c Condition

1 0 0 |f x | > |f y | && |f x | > |f z | 0 1 0 |f y | > |f x | && |f y | > |f z | 0 0 1 |f z | > |f x | && |f z | > |f y | R s b = R s e R b e T (4.2)
Where R b e is the rotation matrix of EEF frame in relation to robot's base frame, obtained from the forward kinematics.

The hand-guiding force f b described in base frame is calculated from:

f b = R b e   f e x f e y f e z   (4.3)
The force command f cmd sent to the control algorithm is:

f cmd =   a 0 0 0 b 0 0 0 c   f b (4.4)
Where (a, b, c) values are dened according to the force conditions detailed in Table 4.1.

In addition, to achieve smooth transitioning between dierent directions/conditions, switching from one motion direction to another is only allowed when the velocity of the EEF is zero.

Hand guiding moment

The end-eector orientation (2nd and 3rd motion groups) is calculated from the handguiding moment. In this scenario, the hand-guiding moment m e in the EEF reference frame is calculated:

m e = R e s     τ s x τ s y τ s z   -ps e R s e f e -ps c R s b w   (4.5)
Where τ s x , τ s y , τ s z are the three torques measurements from the force/torque sensor, p s e is the vector describing the position of the EEF in sensor's reference frame, p s c is the vector describing the position of the center-of-mass (COM) of the EEF in the sensor's reference frame.

In the 2nd motion group, the end-eector axis z eef is oriented in space, Figure 4.1. For this type of motion the input to the controller, vector m xy , is calculated from the hand-guiding moment m e . The component of the hand-guiding moment in the xy plane of the end-eector frame, m e xy , is shown in Figure 4.2 where it is calculated: For the 3rd motion group, the end-eector is allowed to rotate around its axis z eef . The input command to the controller is the vector m z . To calculate this vector, the vector m e z , Figure 4.2, shall be calculated rst:

m cmd = m xy if m xy > m z m z if m z ≥ m xy

Controller

The robot is controlled at the EEF level, considering the decoupled end-eector as a mass moving under the eect of Coulomb and viscous friction,

mẍ + b ẋ + f r = f (4.11)
Where ẍ is the linear acceleration of the COM, ẋ is the linear velocity of the COM, m is the mass, b is the damping coecient, f r is Coulomb friction and f is the external force acting on the mass. The equation of angular rotation around an axis passing through the COM is:

I θ + β θ + τ r = τ (4.12)
Where θ is the angular acceleration around rotation axis, θ is the angular velocity, I is the moment of inertia around the rotation axis, β is the damping coecient, τ r is the torque due to Coulomb friction and τ is the external torque.

We consider that the Coulomb and viscous friction eects are much bigger than the eect of the inertial forces due to acceleration/deceleration of the mass. In this context, the inertial terms in the previous equations are omitted and equation (4.11) becomes:

b ẋ + f r = f |f | > |f r | ẋ = 0 otherwise (4.13)
Where |f | is the absolute value of the external force. By applying the same reasoning for the angular motion equation (4.12) becomes:

β θ + τ r = τ |τ | > |τ r | θ = 0 otherwise (4.14)
Where |τ | is the absolute value of the external torque.

Robot control

The linear motion of the end-eector is controlled by:

v = f cmd ( f cmd -|fr|) b f cmd f cmd > |f r | v = 0 otherwise (4.15)
Where v is the linear velocity vector of the end-eector, f cmd is the force vector command issued to the controller, its magnitude is f cmd . |f r | is the sensitivity threshold magnitude (motion is only executed when the force command reaches a value above this threshold), and b is the motion constant that denes the rate of conversion from force measurement to velocity.

The angular motion of the end-eector is controlled by:

= m cmd ( m cmd -|τ r |) β m cmd m cmd > |τ r | = 0 otherwise (4.16)
Where is the angular velocity vector of the end-eector, m cmd is the moment command issued to the controller, its magnitude is m cmd . |τ r | is the sensitivity threshold (the motion is only valid when the input command value is higher than this threshold), and β is a motion constant that denes the rate of conversion from moment measurement to velocity. Finally, from equation (4.15) and (4.16), the linear and angular velocity of the end-eector ẋ is given by: ẋ = v (4.17)

The robot joint velocities qhg , that satisfy the proposed hand-guiding motion, are calculated using the pseudo inverse of the Jacobian:

qhg = J † ẋ (4.18)
Where J † is calculated using the singular value decomposition of the Jacobian matrix:

J = UΣV T (4.19)
Where matrices U, Σ and V are the result of the singular value decomposition of the Jacobian. Thus, the pseudo inverse is given by:

J † = n i=1 1 σ i v i u T i (4.20)
Where n is the number of non-zero diagonal elements of matrix Σ, σ i is the i th nonzero diagonal element of matrix Σ, v i is the i th column of matrix V, and u i is the i th column of matrix U. To achieve better numerical stability, and according to [START_REF] Samuel R Buss | Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods[END_REF], the alternate Jacobian is used.

The robot can be controlled at joint level using the calculated angular velocities of the joints q. Nevertheless, many robots does not support real-time control at the joint velocity level, namely industrial robots, however they are normally provided with realtime position control loop at joint level. In this study for example, we demonstrated the proposed method in a sensitive industrial robot (KUKA LBR iiwa) using its internal Where (a e ,b e ,c e ) values are calculated as described in Table 4.2.

Joints limits

During the precise hand-guiding, the operator might drive the robot near the joint limit. In the case of KUKA iiwa robot, this results in an error causing the manipulator to stop in place. To solve this problem we propose to utilize a scaling factor α min , this scaling factor is used to scale down the linear/angular velocity of the end-eector near the limit. For each joint i a scaling factor is calculated:

α i =                    1-cos π q i -q i,ll q i 2
, (q i,ll + q i > q i > q i,ll ) && ( qi < 0)

1-cos π q i -q i,ul q i 2 , (q i,ul > q i > q i,ul -q i ) && ( qi > 0) 1 otherwise

(4.22)
Figure 4.4 shows a plot of α i with joint angle q i . The value of the scaling factor α min is calculated as the product of the elements α i :

α min = α i (4.23)
For robots controlled at joint angular velocities level, the angular velocity command of the joints qcmd is calculated:

qcmd = α min qhg (4.24)
For industrial robotic manipulators that support real-time control functionality, a stream of joint angular positions is used to command the robot (not the angular velocities, this applies for the KUKA iiwa robot), in such case the previous strategy for scaling down the angular velocities does not apply, and the previous method needs to be modied to suit the control strategy of such robots. In the following the method for joints limits avoidance applied for the precise hand-guiding for the KUKA iiwa robot is explained: in the beginning α min is considered to be one, consequently, the angular positions q 1 = q are calculated using Algorithm 4.1 (previously presented in 4.1.7), the subscript 1 in q 1 is used to denote the rst execution of the Algorithm 4.1. Afterwards, using q 1 the value of α min is calculated using equations (4.22, 4.23). Now in case α min at q 1 is equal to one, then the calculated angular positions q 1 are updated to the controller of the robot. Otherwise if (α min < 1), then the angular positions q 1 are discarded, and the value of α min is used to scale down the velocity of the end-eector ẋ into α min ẋ. Afterwards, the inverse kinematics resolution is re-initiated using the Cartesian velocity α min ẋ instead of ẋ (in such way the user feels that the motion is becoming harder when pushing towards the limit of the joints). Then after the recalculation the resulting positions of the joints q 2 are applied to the internal control loop of the robot (resulting in a scaled down velocity at the EEF). Now in case the coworker kept pushing against the limit, the scaling factor value reaches zero, and the robot stops in place (does not exceed its physical limit). In case the user moved the robot in the opposite direction, away from the limit, the robot moves freely, this is guaranteed due to the angular velocity inequality (after the && operator of expression 4.22). The proposed method should apply for any industrial robot with realtime position control functionalities, in the case of the KUKA iiwa example an alert light, yellow, is turned on once α min is less than one Figures 4.5

Experiments

The experiments were performed using KUKA iiwa 7 R800 robotic manipulator, the proposed algorithm could be implemented in Java using Sunrise.Workbench (the standard development environment for KUKA iiwa robot), in such case the computations run directly inside the controller of the robot. Otherwise, the algorithm could be implemented remotely (run on outside computer). In such a case, client-sever software [11] shall be used to acquire feedback and to stream motion commands to the robot.

In the following experiments a qualitative/quantitative assessment of the performance of the proposed method is reported.

In the rst test an assembly operation requiring precision positioning and ne tuning is demonstrated, where a qualitative assessment of the performance of the proposed algorithm is discussed. In the second test, the performance of the proposed method is measured quantitatively. In the third test a comparison between o-the-shelf handguiding functionality provided by KUKA and the proposed method is presented. Finally, the precision hand-guiding is implemented in a programming by demonstration application, consequently, any user can program the robot easily and with precision, in a time ecient manner and without the need to write any code. The test is shown in Figure 6.17. The robot is used as an assistive third hand for precise assembly operations 1 . Using the hand-guiding application it is required to move parts/instruments (attached at the EEF of the robot) precisely into the xing location.

Afterwards, the instruments are held in place by the robot, allowing the coworker to apply the xtures. Such approach makes less workers occupied in manual tasks on the factory oor, also in hard-to-access locations the proposed method oers better safety by reducing the risk of fallen parts, which might happen if humans are involved. For mounting heavier parts, the proposed method works as a force magnier, where the weight of the instrument is balanced by the force from the robot, reducing the eort required from the coworker. In this test, the precision hand-guiding algorithm was implemented inside the robot controller, in a single thread program, with an update rate of 118 Hz. The control algorithm compensates automatically for the part weight such that when no force is applied by the user, the part is held in place by the robot.

Dierent tests demonstrated that in the 1st motion group the robot position along x, y and z can be precisely adjusted, in the 2nd motion group the axis of the end-eector is oriented smoothly in 3D space, while in the 3rd motion group we can rotate around the end-eector axis with accuracy, Figure 6.17.

Test 2

The precise hand-guiding algorithm was implemented in a multi-threaded program inside the KUKA iiwa 7 R800 controller. The main thread is used to run the proposed control algorithm, the second thread is used to (1) connect to an external computer using a TCP/IP network, (2) acquire the measurements of the force and the position at the end-eector, which are streamed over the network to an external computer using TCP/IP protocol.

In this test the user starts the proposed hand-guiding application on the KUKA iiwa controller, then he/she hand-guides the robot in the x, y and z directions. The force and the Cartesian position measurements were acquired from the robot, then sent to an external computer, where they were recorded along with their corresponding time stamps. the controller is activated, and the robot moves in the corresponding direction, in the time interval [33;37] seconds, the operator applies a force bigger than the limit on the positive y direction, this causes the robot to move in the positive y direction. According to the force, the robot moves with an average velocity of 99.7 mm/second. While the coordinates in the x and z direction are almost xed. From the acquired data we see that the average position in the x direction is 624.16 mm, and the average position in the z direction is 307.47 mm (note that the z coordinate values shown in Figure 4.10 are displaced by -300mm), also we nd that the maximum deviation from the average in the x direction is 0.091 mm Figure 4.11, while the maximum deviation from the average in the z direction is 0.092 mm Figure 4.12.

Test 3

To test the performance of the proposed method against the hand-guiding at the joints level, we compare with KUKA iiwa o-the-shelf hand-guiding, where the following test is proposed. The manipulator KUKA iiwa 7 R 800 is used for positioning operation of a tool (using precise hand-guiding and KUKA hand-guiding), during each positioning operation the following data were recorded:

• x, y, z position and orientation of the end-eector;

• Vibrations level at the end-eector (measured using IMU);

• Joint angles.

Experimental setup

The experimental setup proposed is demonstrated in Figure 4. 13. In this setup a tool is attached to the end-eector, and the objective is to insert the tool in a tool holder using using the proposed method is in [START_REF] Safeea | A video segment demonstrating Test 3 for precision handguiding[END_REF]. Another video segment showing Test 3 while using KUKA o-the-shelf hand guiding is in [START_REF] Safeea | A video segment demonstrating Test 3 for KUKA o-theshelf hand-guiding[END_REF].

Positioning results

During the placement operation of the tool, the x, y and z positions of the EEF were recorded. The test was performed two times, in the rst the precise hand-guiding was used, in the second the o-the-shelf hand-guiding of KUKA iiwa was utilized, position data were collected from robot controller and relayed over a TCP/IP connection to the computer. orientation during displacement (x,y,z) motion, while it is almost impossible to achieve the same while using the KUKA o-the-shelf hand-guiding, giving that KUKA othe-shelf hand-guiding is controlled in joint space, the orientation is as precise as the coworker can be.

Joint angles

Vibrations level at the end-eector

For measuring the vibrations level at the EEF, an accelerometer was rmly attached to it, Figure 4.20. Then, a gravity acceleration compensation was applied as described in Appendix C.

During the hand-guiding positioning operation of the tool (using KUKA and precise hand-guiding), acceleration data were acquired from the accelerometer, and the results were plotted against time during the manipulation operation.

From to put the box back on a specic location on the table sub-gures (f,g,h,i,j). It took the user less than 3 minutes to teach the robot the whole operation. After the teaching process, the robot can perform the taught path by clicking on the corresponding button from the HMI interface.

(c) (d) (e) (f) (g) (h) (i) (j)

Redundancy Resolution & Precision Hand-Guiding

In the previous section, the precision hand-guiding is presented as a tool that allows unskilled users to interact and position the EEF intuitively and precisely. However, the least norm solution was used in generating the motion, such that the redundancy resolution of redundant manipulators was not fully explored. Many industrial applications require precise positioning at the end-eector (EEF) level inside cluttered environments, where manipulator's redundancy is required. This section addresses the subject of precision in hand-guiding at EEF level while using the redundancy for in-contact obstacle navigation. In the presence of a contact with an obstacle, the proposed null space control method actuates in a way that the manipulator slides compliantly with its structure on the body of the obstacle while preserving the precision of the hand-guiding motion at EEF level. Force/torque (FT) data from a FT sensor mounted at the robot ange are the input for EEF precision hand-guiding while the torque data from the joints of the manipulator are used to calculate the contact between robot structure and obstacles. Experimental tests were carried out successfully using a KUKA iiwa industrial manipulator with 7 degrees of freedom (DoF). Where, the EEF is hand-guided on a straight line while the robot is sliding on the obstacle with its structure, results indicate the precision of the proposed method.

Proposed method

The method is described in Figure 4.24. Where using the force and torque measurements from an external FT sensor attached to the ange, and based on the precision hand-guiding algorithm (in the previous section), the user is able to hand-guide a sensitive redundant manipulator precisely at the EEF level. At the same time, using the torques feedback from the integrated torque sensors at the joints of the sensitive manipulator, it is possible to perform automatic redundancy resolution, where upon a contact with an external obstacle (wooden box in Figure 4.24), the robot is able to adjust the internal motion manifold to avoid the obstacle. As a result, the robot slides with its structure on the body of the obstacle while keeping the precision at the EEF level according to the hand-guiding motion.

Control strategy

The inputs to the proposed controller are:

• Force and torque measurements from an external FT sensor attached at the ange of the robot;

• Torque measurements from the integrated torque sensors at the joints of the robot.

Most industrial robots, including the KUKA iiwa 7R800 and 14R820, are controlled at the kinematic level, by streaming the joints reference positions or velocities to the robot.

As such, the proposed control strategy operates at the kinematic level, it includes superimposing the angular velocities qhg , in equation 4.18, that satisfy hand-guiding motion, with the angular velocities in the null space qn that satisfy the in-contact vector τ ex . In this study, we opted to use the KUKA Sunrise Toolbox [11], where we are able to retrieve the components of τ ex using a TCP/IP communication in MATLAB.

Joint torques compensation

In addition to the joint torques due to the external contact forces with the obstacles, the toque vector τ ex includes some extra components caused by the forces/moments due to the (1) hand-guiding, (2) weight of the tool mounted at FT sensor, and (3) the weight of the FT sensor. The eect of those forces/moments on joints torques shall be accounted for. This procedure is referred to as the torques compensation, in which two main factors are recognized:

1. Compensation for the torques due to sensor's weight, which also includes the weight of the adaptor ange used to mount the FT sensor on the robot. The weight of the sensor and its Center-Of-Mass (COM) are previously known. Consequently, the torque compensation is applied directly after calculating the Jacobian associated with COM as described later in the Sensor weight compensation;

2. Compensation for the torques due to external force/moment acting at the FT sensor. This force/moment is measured by the FT sensor, it includes the weight of the tool mounted at the FT sensor and the hand-guiding force/moment applied by the human, as described later in the Hand-guiding-wrench/tool-weight compensation.

Sensor weight compensation: For robot motions with relatively small accelerations, which is the case in precision hand-guiding, the forces/moments due to the inertia of the FT sensor and the mounting ange can be neglected in comparison to their weight. Thus, to compensate the torques generated due to the weight of the sensor and the mounting ange τ ws , the Jacobian J ws associated with their COM is utilized:

τ ws = J T ws 0 0 -w s 0 0 0

T (4.26)
Where w s is the weight of the sensor (including mounting ange). The previous equation is valid for base mounted robots, where the base is mounted horizontally and the z axis is pointing up, otherwise the orientation of the base frame shall be considered.

Hand-guiding-wrench/tool-weight compensation: The torques generated due to external forces/moments acting on the FT sensor shall be compensated for.

Those forces/moments include the hand-guiding wrench (force/moment), the attached tool's weight and the inertial forces due to its acceleration. For calculating the torques τ f s due to external forces/moments, the Jacobian J s associated with the origin of the measurement frame of the FT sensor is considered:

τ f s = J T s R b s 0 0 R b s f f s m f s (4.27)
123 Where f f s and m f s are the force and the moment measured at the FT sensor, respectively. Those force and moment quantities are due to the hand-guiding force/moment plus the weight/inertial force/moment of the tool. The joint torques vector τ f s is due to f f s and m f s , and R b s is the rotation matrix from the measurement frame of the sensor to the base frame of the robot.

Joint torques due to contact with obstacles

After calculating the compensation torques τ ws and τ f s , the torques due to contact with the obstacle τ c can be calculated:

τ c = τ ex -τ f s -τ ws (4.28)
Finally, the torques vector τ c due to external contact forces with an obstacle is used to calculate the motion in the null space, as described in sub-section 4.2.2.4.

Contact controller

Considering that a human user hand-guides the EEF precisely using the measurements from FT sensor, the least squares solution is used for calculating the joints angular velocities vector qhg . This control command generates the required hand-guiding motion for the EEF (with precision).

In the presence of obstacles, and due to contact forces between the structure of the robot and the obstacles, extra torques start to appear at the joints. Those torques, τ c , are calculated after acquiring measurements from torque sensors at the joints of the robot and FT sensor measurements at the EEF, as described in the previous section.

The torques vector τ c is then used as an input to the contact controller. The output of the contact controller is the null space angular velocities qn :

qn = NKτ c (4.29)
Where, N is the null space projection matrix of the robot, and K is a diagonal matrix with constant coecients.

Control command

The total command used to control the robot is the sum of (1) the angular velocity vector for hand-guiding qhg , and (2) the null space angular velocity vector qn , which allows the robot to slide on the obstacle:

q = qhg + qn (4.30)

Robot control

Industrial robots are provided with internal servo control loop at the joint position level, for example the KUKA iiwa robot [START_REF] Kuka | KUKA Sunrise[END_REF]. Consequently, the industrial robot can be controlled by streaming the reference joint angles, which can be calculated by integrating the joints angular velocities of equation (4.30). However, with time, the integration causes numerical errors to accumulate which might aect the accuracy in the resulting Cartesian motion. To solve this problem we propose the following novel solution, after calculating the angular velocities in the null space qn , the following integration is calculated:

q ini = q f + t+ t t qn dt (4.31)
Where, q f is the joints angles feedback from the robot controller and t is the update time interval of the control loop. Then the result q ini is used as an initial value for the precision hand-guiding algorithm in section 4.1. Which allows the robot to move precisely in the Cartesian space while avoiding the obstacle in the null space.

Experiments

The proposed methodology was tested using a KUKA iiwa 7 R800 robot. This is an industrial sensitive collaborative robot with 7 DoF provided with torque sensors integrated into its joints. An external FT sensor, JR3, is attached at the ange of the robot. The robot was controlled from an external computer using KUKA Sunrise Toolbox [11]. Meanwhile, during hand-guiding, robot's structure collides with an obstacle, a box in the way of the robot. The robot adjusts its conguration, by utilizing its redundancy, and slides smoothly on the obstacle with its structure. As a result, the robot is able to keep moving on a straight line along the y direction while navigating the obstacle during the contact. In contrast, for traditional hand-guiding solutions at EEF level the robot is blind to its surrounding, such that it keeps pushing with its structure against the obstacle causing joints-torques/motors-currents to increase triggering an emergency stop.

A video segment showing the test is available in [START_REF] Safeea | A video segment demonstrating the precision hand-guiding with in-contact obstacle navigation method applied to KUKA iiwa 7R800 robot[END_REF]. During the experimental test, various data were recorded including (1) robot joints angular positions, From Figure 4.28 it is noticed that at the beginning of the test the operator applies a hand-guiding force at EEF, mainly along the y direction of the base frame. As a result the EEF starts moving in the positive y direction, Figure 4.29. From the joints torques graph, Figure 4.27, it is noticed that at around 2 seconds from the beginning of the test, a contact between the robot structure and the obstacle is initiated. After the contact the torques acting on the rst and the third joints increase. Due to the proposed null space control, the rate of motion of the rst and third joints also increase. This is evident in the joint angles graph, Figure 4.26, where the rate of change of the angular positions of rst and third joints increase due to null space motion. This allows the EEF to move as desired by the operator, without conict due to the presence of the obstacle. the maximum error of the actual path in the x and z directions are 0.52 and 0.51 mm, respectively. As a comparison, we carried out the same test with KUKA o-theshelf hand-guiding, joint level controlled, we tried to move the EEF in a line parallel to the y direction, the errors were of order of centimeters. Depending on the user, errors as big as 3 cm and 5 cm of EEF position in the x and the z directions have occurred, an expected result since that hand-guiding the robot at the joints level does not guarantee precision at the EEF level (in best case possible the robot can be as precise as the human operator can be).

Summary

In this section it is presented a novel method for precision hand-guiding of redundant manipulators with obstacle avoidance capability. Unlike traditional hand-guiding solutions, we take advantage of robot redundancy to avoid in-contact obstacles between the manipulator's structure and the surrounding environment while achieving precision at EEF level. Torque measurements from robot joints are used together with the measurements from an external FT sensor attached at the ange. The acquired sensory data are then treated for calculating two essential quantities: (1) the torques due to contact forces and (2) the hand-guiding force and moment. Using these data a control scheme is proposed such that the manipulator is able to compliantly slide on obstacles during the contact while precisely hand-guided at the EEF level. Tests were carried out successfully on KUKA iiwa robot. From the results it is concluded that the robot successfully manages to perform the desired hand-guiding path with precision while avoiding excessive contact forces with the surrounding environment. As compared to joint level hand-guiding, the proposed method gives superior precision with order of magnitude.

Chapter 5

Contribution to Robot Dynamics Formulation Dynamics of robots is an important topic since that it is highly involved in their design, simulation and control. Owing to its importance this subject had been studied extensively in the past decades. Thus, several algorithms and methods had been developed

to calculate robot dynamics [START_REF] Featherstone | Robot dynamics: equations and algorithms[END_REF][START_REF] Ca Balafoutis | A survey of ecient computational methods for manipulator inverse dynamics[END_REF]. Nevertheless, this subject remains till this day open for extensive research while every year there are new studies being published, methods and algorithms being proposed. This chapter introduces two main contributions into the mathematical formulation of robot dynamics. The rst pertains to the eciency in calculating the joint space inertia (mass) matrix for robots with High Degrees of Freedom (HDoF). The second describes a recursive algorithm for calculating Christoel symbols eciently.

Chapter's breakdown

This chapter of the document is organised into two separate sections:

• Section 5.1: describes an algorithm with a minimal O(n 2 ) cost 1 for eciently calculating the joint space inertia matrix for robots with HDoF. This algorithm is obviously important in practice for reducing the execution time required to calculate the mass matrix of HDoF robots. However, it is also important theoretically, since it gives the limit on the second order eciency possible of calculating the mass matrix for HDoF robots.

• Section 5.2: describes a light weight recursive (non-symbolic) algorithm for calculating Christoel symbols of robotic manipulators eciently. 1 Where n is the number of degrees of freedom of the robot.

Contribution to Mass Matrix Calculation

Increasingly, robots have more Degrees of Freedom 2 (DoF), imposing a need for calcu- lating more complex dynamics. As a result, better eciency in carrying out dynamics computations is becoming more important. In this section, an ecient method for computing the joint space inertia matrix (JSIM) for serially linked robots with high degrees of freedom is addressed. We call this method the Geometric Dynamics Algorithm for High number of robot Joints (GDAHJ). GDAHJ is non-symbolic, preserves simple formulation, and is convenient for numerical implementation. Results compare favorably with existing methods, achieving better performance over state-of-the-art by

Featherstone when applied to robots with more than 13 DoF.

Introduction

Robot dynamics can be described by one of two formulations:

1. Operational space formulation. In this formulation the dynamics equations are referenced to the manipulator end-eector. In a pioneering study this approach was described and used to control PUMA600 robot [START_REF] Khatib | Dynamic control of manipulator in operational space[END_REF]. It is also applied for the combined application of motion and force control [START_REF] Khatib | A unied approach for motion and force control of robot manipulators: The operational space formulation[END_REF]. Algorithms for ecient robot dynamics calculations based on operational space formulation are presented in [START_REF] Chang | Ecient algorithms for articulated branching mechanisms: dynamic modeling, control, and simulation[END_REF] and [START_REF] Chang | Ecient recursive algorithm for the operational space inertia matrix of branching mechanisms[END_REF].

2. Joint space formulation. This formulation describes the dynamics of robot in joint space. This formulation manifests the eect of the joints' positions, velocities and accelerations on the torques and vice-versa.

The mathematical formulation of the inverse dynamics in joint space [1,[START_REF] John | Introduction to robotics: mechanics and control[END_REF][START_REF] Corke | Robotics, vision and control: fundamental algorithms in MATLAB[END_REF][START_REF] Khalil | Dynamic modeling of robots using recursive newton-euler techniques[END_REF] is given by: τ = A(q)q + C(q, q) q + g (

Where τ is the joints' torques vector, q is the joints' positions vector, q is the vector of joints' angular velocities, q is the vector of joints' angular accelerations, A(q) is joint space inertia matrix of the robot, C(q, q) is the joint space Coriolis matrix of the robot, and g is the vector of joints' torques due to gravity. As described in [START_REF] Mark | Control of robots and manipulators[END_REF], equation

(5.1) can be extended to include contact forces, joints elasticity, friction, actuators inertias and dynamics. A(q) is an n × n matrix, in which n is the number of robot's joints considering that each joint has one degree of freedom, it is symmetric, positive denite and has the property of being a function of only joints' positions. C(q, q) is an n × n matrix, function of joints' positions and velocities, and describes the centrifugal and Coriolis eects on joints' torques.

One of the earliest methods used to deduce the equations of robot dynamics was the one based on Lagrangian formulation. This method is well described in the literature.

A methodology for deducing the dynamics of gear-driven serially linked robot by using Lagrangian formulation is described in [START_REF] Sciavicco | Lagrange and newtoneuler dynamic modeling of a gear-driven robot manipulator with inclusion of motor inertia eects[END_REF]. This study took into consideration the 2 For example Caltech's 30 DoF snake like robot presented in [START_REF] Gregory | Design and experiments with a 30 dof robot[END_REF].

eects of the driving motors. The Lagrangian formulation is widely used as the bases for automatic generation of equations of robot dynamics in symbolic form. Most recent toolboxes for generating equations of robot dynamics using Lagrangian formulation are in [START_REF] Toz | Dynamics simulation toolbox for industrial robot manipulators[END_REF].

The Lagrangian formulation is a straight forward approach that treats the robot as a whole and utilizes its Lagrangian, a function that describes the energy of the mechanical system:

L = T -U (5.2)
Where L is the Lagrangian function, T is the kinetic energy and U is the potential energy. The function described previously is formulated in terms of the generalized coordinates q, dierentiating it gives an expression for the associated generalized forces v:

v = d dt ( δL δ q ) T -( δL δq ) T (5.3)
Even though the Lagrangian formulation can be considered as a straight forward approach, the method requires partial dierentiation. Despite the fact that symbolic manipulation methods have been utilized to perform the dierentiation [START_REF] Charles | Symbolically ecient formulations for computational robot dynamics[END_REF], the method still lacks the eciency in terms of execution-time. This can be clearly noticed when the robot presents a relatively high number of DoF as noted in [START_REF] Khalil | Dynamic modeling of robots using recursive newton-euler techniques[END_REF] and most remarkably in [START_REF] Hoifodt | Dynamic modeling and simulation of robot manipulators: the newton-euler formulation[END_REF], where the author performed comparison of execution-times required to run simulations based on dynamical models derived by Newton-Euler recursive technique and Euler-Lagrange technique. It was reported execution-times dierence of order of magnitude which clearly put the case in favor of the Newton-Euler recursion method.

The formulation of robot-specic dynamics using Kane's dynamical equations is in [START_REF] Thomas | The use of kane's dynamical equations in robotics[END_REF]. In this study the authors argue that using Lagrange method to compute dynamics produces huge equations resulting in slow execution and costly computations, while the Recursive Newton-Euler is a generalized method that might perform unnecessary calculations on specic robot. Thus, a faster execution algorithm with less computational-cost could be achieved if robot-specic equations are carefully deduced.

The study elaborates in step by step manner the methodology for deriving the dynamics equations of Stanford manipulator starting from Kane's dynamical equations.

Nevertheless, the method requires a knowledgeable analyst to take on a pencil and paper in hand and work out the equations of a specic robot. A comprehensive review of Kane's equations and Gibbs-Appell equations is in [START_REF] Baruh | Analytical dynamics[END_REF].

A computationally ecient Newton-Euler recursive method is described in [START_REF] John Ys Luh | On-line computational scheme for mechanical manipulators[END_REF].

This method is performed in two phases: the rst phase (forward propagation) during which the accelerations and velocities of robot links are calculated, and the second phase (backward propagation) where torques and forces are calculated. The method proved to be very ecient for calculating the inverse dynamics. However, the calculations are carried out implicitly such that the inertia matrix cannot be retrieved directly. It is shown in [START_REF] Michael | Ecient dynamic computer simulation of robotic mechanisms[END_REF] that the inertia matrix A(q) can be calculated from the model of the inverse dynamics by assigning a unit value to one element of the joints' accelerations vector and assigning a zero value to the remaining elements, including the joints' velocities and the gravity term. In such scenario, the associated column of the inertia matrix is calculated, and by iterating the procedure through all of the elements of the joints' acceleration vector the inertia matrix is achieved. This method was later renamed Composite-Rigid-Body Algorithm (CRBA), by Featherstone [158].

Using CRBA to calculate the inertia matrix proved to be computationally ecient, especially if the calculations are performed in links-attached local frames. Computer code of the algorithm based on 6D or spatial vectors algebra is available in [158]. A comprehensive review of spatial vectors and Plücker basis is in [START_REF] Featherstone | Plucker basis vectors[END_REF] and in chapter 2 of [START_REF] Featherstone | Rigid body dynamics algorithms[END_REF].

In this section we propose GDAHJ as a method to calculate JSIM for serially linked bodies with relatively high number of DoF, GDAHJ achieves better eciency over state-of-the-art method, the famous CRBA. This increase in eciency is achieved through minimizing the number of operations that have O(n 2 ) computational complexity. In GDAHJ the number of computations associated with the quadratic terms are reduced to the minimum value possible, from 16n 2 in the case of CRBA to 5.5n 2 for GDAHJ.

Theory and principles

The proposed algorithm builds on what we call the frame injection principle illustrated in Figure 5.1, where a frame j attached to joint j (according to the modied Denavit Hartenberg convention [START_REF] John | Introduction to robotics: mechanics and control[END_REF]) transfers to link k a linear acceleration into its center of mass and an inertial moment around its center of mass. In this study we notate them by Γ Ckj and µ Ckj , respectively. This transfer is due to the rotational eect of joint j around its axis of rotation, or the z axis of frame j. This cause and eect relationship between frame j and link k is referred to by the subscript kj in Γ Ckj and µ Ckj , while the subscript C is used to refer to the center of mass of link k. The same subscript notation will hold in this chapter for denoting frame-link interaction of cause-and-eect unless stated otherwise.

Link's acceleration due to the single-frame eect

Each frame j transfers to link k three acceleration vectors tangential acceleration, normal acceleration and Coriolis acceleration. The rst of which is shown in Figure 5.2, it is due to the angular acceleration of frame j:

Γ τ Ckj = ε j × p Ckj (5.4)
Where Γ τ Ckj is the tangential acceleration of the center of mass of link k due to the rotation of frame j, the symbol × is used to denote the cross product and p Ckj is the vector connecting the origin of frame j and the center of mass of link k. ε j is the angular acceleration of joint j:

ε j = qj k j (5.5)
where k j is the unit vector associated with the z axis of joint j and qj is the angular acceleration of that joint.

L k = R k (tr(I k k )1 3 -2I k k )R T k (5.14)
The subscript in L k is to notate that the matrix calculated pertains to link k, tr(I k k ) is the trace of the inertial tensor and 1 3 is the identity matrix.

The third inertial moment transferred from frame j to link k is due to Coriolis eect:

µ cor Ckj = (L k ω j ) × ω r kj (5.15)
Where ω r kj can be calculated from:

ω r kj = k i=j+1 ω i (5.16)
Thus, the total inertial moment transferred to link k around its center of mass due to the rotational eect of frame j is given by:

µ Ckj = µ τ Ckj + µ n Ckj + µ cor

Ckj

(5.17)

Mass Matrix for High DoF Robot

The GDAHJ algorithm calculates the joint space inertia matrix for robots with high DoF quite eciently, this increase in eciency is achieved through minimizing the number of operations that has O(n 2 ) computational complexity to a minimum, according to our knowledge GDAHJ is the most ecient method for high DoF robots ever proposed till now.

Starting from the basic interpretation of JSIM columns, the mathematical equations of GDAHJ algorithm can be deduced. Where as described in section 3.2 of [START_REF] Featherstone | Robot dynamics: equations and algorithms[END_REF], each column j of the JSIM can be interpreted as: the torques acting on the various joints of the robot, due to the unit acceleration of joint j, giving that the angular velocities of all of the joints are equal to zero. In Figure 5.3 we show the free body diagram of one link of the robot, with the inertial moments and inertial forces acting on it.

Following the previous denition of column j of JSIM, we can calculate that column as the following: (1) choose a joint j, and (2) write the balance equation of a link k from the robot. By referring to Figure 5.3, the balance equation of link k:

µ k,j = µ k+1,j + (R k I k k R T k )k j qj + m k pCkk (q j k j × p Ckj )+ lk n i=k+1 m i (q j k j × p Cij ) (5.18)
Where µ k,j is the total moment acting on joint k due to the acceleration of joint j only. l k is the vector connecting the origin of frame k to the proceeding frame's origin, the little hat notation above the vector is used to denote the skew symmetric operator associated with that vector. From the denition given in the previous section of column j of the mass matrix, we substitute qj by its value qj = 1, consequently:

µ k,j = µ k+1,j + (R k I k k R T k )k j + m k pCkk (k j × p Ckj ) + lk n i=k+1 m i (k j × p Cij ) (5.19) While:
p Ckj = p Ck -p j (5.20)

And:

p Cij = p Ci -p j (5.21)
We substitute the values of p Ckj and p Cij into (5.19), and we x:

µ kj = µ k+1,j + R k I k k R T k -m k pCkk pCk -lk n i=k+1 m i pCi k j -m k p Ckk + ( n i=k+1 m i )l k × (k j × p j ) (5.22)
We dene the vector η k by:

η k = m k p Ckk + ( n i=k+1 m i )l k (5.23)
And we dene the matrix operator κ k by:

κ k = -m k pCkk pCk -lk n i=k+1 m i pCi (5.24)
Then we write:

µ k,j = µ k+1,j + R k I k k R T k + κ k k j -(η k ) × (k j × p j ) (5.25)
By performing a recursion on previous equation from link n to link k, and noticing that µ n+1,j = 0 we get:

µ k,j = n i=k R i I i i R T i + κ i k j - n i=k η i × (k j × p j ) (5.26)
To hide the complexity in the previous equation, we denote the terms between parenthesis by:

b k = n i=k η i (5.27)
And

D k = n i=k R i I i i R T i + κ i (5.28)
Substituting (5.27) and (5.28) in (5.26) yields:

µ k,j = D k k j -bk (k j × p j ) (5.29)
For calculating the (k, j) entry of JSIM, A k ,j , we project µ k,j on the z axis of joint k, or in other words we multiply (5.29) by the unit vector k T k :

A k ,j = k T k µ k,j = k T k D k k j -k T k bk (k j × p j ) (5.30)
By noticing that each entry k, j of the JSIM, or k T k µ k,j is a scalar, then we can transpose the previous equation without loss of generality:

A k ,j = k T j D T k k k -k j × p j T bT k k k = k T j D T k k k + k j × p j T bk k k (5.31)
In such a way we have decoupled the dependency between indexes k and j. Moreover, we limited the cross-coupling interaction between joint j and bodies k into a minimum. The previous equation states that the eect of acceleration of each joint j is limited to the terms k T j , and t j = k j × p j . The eect of the articulated bodies from link n to link k is manifested by the terms D T 01 :For k = 1 : n 02 : For j = 1 : k 03 :% calculating A k ,j will require two vector inner products and one 04 :% scalar addition with total cost (3n 2 + 3n)mul + (2.5n 2 + 2.5n)add 05 :

A k ,j = k T j d k + t T j y k 06 :
A j ,k = A k ,j 07 : End 08 :End multiple recursions, while the mass matrix entries can be calculated with minimum quadratic cost using the nested loop in Algorithm 5.1.

The nested loop in Algorithm 5.1 has the minimal quadratic cost. This cost results from two vector-inner products and one scalar addition, with a cost (3n 2 + 3n)mul + (2.5n 2 + 2.5n)add, where mul stands for multiplication and add stands for addition. Thus, the O(n 2 ) computational cost is optimized.

Tests

To prove the validity of the proposed method, GDAHJ, and to assess its execution-time performance, a comparison with well established algorithms was performed, namely with CRBA method.

Table 5.1 shows the computational complexity of the proposed algorithm against state-of-the-art algorithm, measured in the number of oating point operations (additions and multiplications) as function of n, the number of DoF of the robot. The operation count for CRBA reported in Table 5.1 pertains to the most ecient version of this algorithm [START_REF] Featherstone | Rigid body dynamics algorithms[END_REF]. The results reported in Table 5.1 for GDAHJ do not include the number of operations required to perform the forward kinematics of O(n), since that most of robotics applications require forward kinematics calculation, otherwise the cost of the forward kinematics can be added. It can be inferred that GDAHJ performs better than CRBA for articulated bodies (serially connected) that have more than 13 DoF.

To conrm the theoretical results, both algorithms CRBA and GDAHJ were implemented in C++, and a comparison in terms of execution time between the two algorithms was performed, the C++ code with the operation count tables and the results are available online in [30]. Numerical tests were carried out by considering a manipulator in which the mass of each link was generated randomly in the range [0,1]

kg. The inertial tensors of the links were generated as random positive denite matrices in which each element is in the range [0,1] kg.m 2 . Denavit-Hartenberg parameters of each link were generated randomly as well as the conguration of the robot (joint angles). Afterwards, the JSIM of the manipulator was calculated twice, once using CRBA method and another time using GDAHJ method. Figure 5.4 shows the results in terms of execution time and Figure 5.5 shows the results in terms of numerical error of the calculation between the two methods. From the gures we notice that GDAHJ performs better than CRBA in terms of execution time for high DoF robots (more than 13 DoF).

A metric-value was dened to measure the average error, given by:

e = 1 n 2 i,j A CRBA i,j -A GAHJ i,j (5.32)
Where e is the resulting average error, A CRBA i,j and A GAHJ i,j are the i, j elements of the mass matrix calculated using CRBA and GDAHJ algorithms.

Summary

In this section we proposed GDAHJ, a novel algorithm for ecient calculation of JSIM for serially linked robots, the algorithm achieves better eciency over state-of-theart when calculating JSIM for hyper-joint manipulators. This increase in eciency is achieved through minimizing the number of operations associated with the O(n 2 ).

In such a case, the number of computations associated with the quadratic terms are reduced to the minimum value possible, from (16n 2 ) in the case of CRBA to (5.5n 2 )

for the proposed algorithm. At the end comparison between the proposed algorithm against state of the art CRBA was made, the performance of the proposed algorithm was discussed in operation count section of this study. On a theoretical level, this study demonstrates the minimum bound for the O(n 2 ) operations required for calculating JSIM. Future work will focus on reducing the number of operations associated with O(n) of the algorithm.

this fact becomes more noticeable for robots with high number of joints or DoF as noted in [START_REF] Khalil | Dynamic modeling of robots using recursive newton-euler techniques[END_REF] and most remarkably in [START_REF] Hoifodt | Dynamic modeling and simulation of robot manipulators: the newton-euler formulation[END_REF], where the author performed comparison of execution times required to run dynamics simulations based on models derived by Newton-Euler recursive technique and Euler-Lagrange technique. Where it was reported execution times dierence of order of magnitude which put the case in favor of the Newton-Euler recursive methods.

Due to their eciency, researchers developed various recursive algorithms for calculating the inverse dynamics [START_REF] John Ys Luh | On-line computational scheme for mechanical manipulators[END_REF], the forward dynamics [START_REF] Featherstone | A divide-and-conquer articulated-body algorithm for parallel o (log (n)) calculation of rigid-body dynamics. part 1: Basic algorithm[END_REF][START_REF] Featherstone | A divide-and-conquer articulated-body algorithm for parallel o (log (n)) calculation of rigid-body dynamics. part 2: Trees, loops, and accuracy[END_REF], the joint space inertia matrix [START_REF] Featherstone | Ecient factorization of the joint-space inertia matrix for branched kinematic trees[END_REF]14]. Nevertheless, we are not aware of any recursive algorithm for calculating Christoel symbols numerically. As such, in this study we present a method for calculating Christoel symbols recursively. We also analyze the performance of the proposed algorithm in terms of number of operations, execution time and numerical error. MATLAB code of the proposed algorithm, including implementation examples, are available in the web-page [31].

Motivation and Contribution

Calculating Christoel symbols of the rst kind is very important in robot dynamics.

Hence, Christoel symbols have been used for solving various robotics problems. In [START_REF] Gunawardana | On the uniform boundedness of the coriolis/centrifugal terms in the robot equations of motion[END_REF] they are used for calculating the bounds on the Coriolis/Centrifugal matrix.

These bounds play an important role for designing and tuning various proposed robot controllers [START_REF] Kelly | Comments on Adaptive PD controller for robot manipulators[END_REF][START_REF] Tomei | Adaptive PD controller for robot manipulators[END_REF][START_REF] Alvarez-Ramirez | PID regulation of robot manipulators: stability and performance[END_REF][START_REF] Cervantes | On the PID tracking control of robot manipulators[END_REF][START_REF] Qu | Robust tracking control of robots by a linear feedback law[END_REF][START_REF] Ortega | A semiglobally stable output feedback PID regulator for robot manipulator, automatic control[END_REF]. In addition, Christoel symbols have been used in a dynamic neurocontroller of robotic arms [START_REF] Mulero-Martinez | An improved dynamic neurocontroller based on Christoel symbols[END_REF]. They can also be used to calculate a special form of Coriolis matrix that preserves the skew symmetry property [1] (an essential property for various control algorithms). Christoel symbols are also important for planning time optimal trajectories and optimal velocity-prole generation [START_REF] Lipp | Minimum-time speed optimisation over a xed path[END_REF]. In such a case, the geometric path is parametrized using a vector function of a scalar parameter θ (t). So that the inverse dynamics equation (which enters the optimization as dierential constraint) is reformulated to decouple the conguration dependent coecients from the time dependent parameter θ (t), as shown in [START_REF] Lipp | Minimum-time speed optimisation over a xed path[END_REF]:

τ = m (q) θ + c (q) θ2 + d (q) (5.37)
In such a case, Christoel symbols are utilized for calculating the (conguration dependent) coecients c (q) in each conguration on the discretized geometrical path.

In [START_REF] Yin | An ecient algorithm for automatic generation of manipulator dynamic equations[END_REF] Christoel symbols are calculated in symbolic form, based on the Lagrangian formulation of the robot dynamics. This method has become the norm and is presented in standard robotics textbooks [START_REF] Lynch | Modern Robotics: Mechanics, Planning, and Control[END_REF]1,[START_REF] Steven M Lavalle | Planning algorithms[END_REF], including the Handbook of Robotics [START_REF] Siciliano | Springer handbook of robotics[END_REF] in page 44, where the deduction of Christoel symbols is introduced in the Dynamics chapter under the subsection 2.3.2 Lagrange Formulation. Nevertheless, symbolic methods for performing the calculations have major drawbacks, namely:

• The symbolic manipulation of the equations is time consuming, so it has to be performed oine.

• For high DoF, the symbolic equations become very complex resulting in much slower execution times than recursive methods, a fact reported in literature [START_REF] Khalil | Dynamic modeling of robots using recursive newton-euler techniques[END_REF].

Apart form that, a recursive method for calculating Christoel symbols has several advantages over the symbolic:

• Unlike the symbolic methods, recursive methods can be used on-the-y, and do not require an oine preprocessing. This makes recursive methods essential for calculating the Christoel symbols on-the-y in robots that change its kinematic chain and dynamic model, for example by adding or subtracting extra bodies (including Recongurable and Self Assembling robots or when attaching objects to the EEF).

• Since that recursive methods are used on-the-y, the proposed recursive method allows updating the dynamical constants (dynamical model) of the robot on-they. This allows the algorithm to use initial estimates of dynamic constants while learning and tuning them more accurately during operation. This can not be done easily in symbolic methods that require oine code regeneration.

Moreover, the proposed method calculates Christoel symbols based on the robot's transformation matrices and inertial parameters, without requiring partial dierentiation. Apart from the previously listed computational advantages, the proposed method oers more insight into the nature of Christoel symbols from the point of view of Newton mechanics.

Calculating Christoel symbols

For articulated rigid bodies in a weightless environment (no gravitational eld) equation Considering the frame injection principle (described previously in 5.1.2), the right hand side of equation (5.38) is the torque resulting from the sum of three inertial moments:

τ k = τ k j + τ k ji + τ k i (5.40)
Where:

• τ k j is the torque due to the Centrifugal eect resulting from motion of joint j. Thus, it is a function of q2 j ;

• τ k ji is the torque due to the Coriolis eect resulting from motion of joints j and i. Thus, it is a function of the product qj qi ; • τ k i is the torque due to the Centrifugal eect resulting from motion of joint i. Thus, it is a function of q2

i .

From equations (5.39) and (5.40) we nd that:

τ k j = c k jj q2 j (5.41)
τ k ji = 2c k ji qj qi

(5.42)

τ k i = c k ii q2 i (5.43)
In such a case, to calculate c k jj , c k ji and c k ii we assign a unitary value to the angular velocities qj and qi . Then, equations (5.41),(5.42), (5.43) are interpreted as:

• The Christoel symbol c k ji is equal to half of the torque τ k ji which acts on joint k due to Coriolis eect resulting from the unit angular velocities at joints j and i;

• The Christoel symbol c k jj is equal to the torque τ k j which acts on joint k due to Centrifugal eect resulting from the unit angular velocity at joint j;

• The same applies for Christoel symbol c k ii which results from c k jj after a change of index.

To calculate the Christoel symbols c k ji we apply backward recursion on the forces and moments shown in Figure 5.6, where:

• h k ji is half of the inertial moment µ cor Ckj at the center of mass of link k. It is due to Coriolis eect resulting from a unit angular velocity at joints j and i. From equation (5.15) of the frame injection principle, h k ji is given by: From equation (5.9) of the frame injection principle, f k ji is given by:

h k ji = 1 2 µ cor Ckj = 1 2 (L k k j ) × k i (5.44)
f k ji = 1 2 m k Γ cor Ckj = m k k j × (k i × p Cki ) (5.45)
We calculate the Christoel symbols c k ji recursively, by applying a backward recursion on Figure 5.6 for the inertial forces f k ji and the inertial moments h k ji :

F k ji = F k+1 ji + f k ji (5.46) H k ji = H k+1 ji + h k ji + p Ckk × f k ji + l k × F k+1 ji (5.47) c k ji = k T k H k ji (5.48)
Where F k ji is half of the inertial force calculated recursively at joint k due to the unit angular velocity at joints j and i. H k ji is half of the inertial moment calculated recur- sively at joint k due to the unit angular velocity at joints j and i, and the superscript T in k T k is to denote the transpose. By applying a similar approach on normal accelera- tions we can calculate c k ii (c k jj ). It is noticed that the resulting equations for calculating c k ii and c k jj are exactly similar to the ones in the presented algorithm (for calculating c k ji ) only with indices changed.

Tests

To prove the validity of the proposed method for calculating Christoel symbols and to assess its performance, a comparison with symbolic Lagrangian based method was performed. The code is provided in the web-page [31]. The robot used to run the test is a 5 DoF serially linked robot, its structure is described in the le robotStructure_5DOF.mat. This robot is generated using the le generateRandomRobot.m in which the mass of each link was generated randomly in the range [0,1] kg. The inertial tensor of each link was generated as random positive denite matrix in which each element of the matrix is in the range [0,1] kg m 2 . Denavit-Hartenberg (DH) parameters of each link were also generated randomly. Afterwards, using MATLAB, Christoel symbols of the robot were calculated using: 2. An oine generated MATLAB function which contains the symbolic equations generated using Lagrangian method, chri_symbGen5DOF.m. In this case, the optimization option of the code generator was set to true, as to optimize the generated symbolic equations.

3. Using an oine generated MATLAB function which contains the symbolic equations generated using Lagrangian method chri_symbGen5DOFnoOpt.m. In this case, the optimization option of the symbolic equation generator was set to false.

The Christoel symbols of the manipulator were calculated twice, once using symbolic function and another using the proposed method. Table 5. On the other hand, using the proposed algorithm to calculate Christoel symbols for 6 DoF robot requires 13.3e-5 seconds, the le with the test is a01_timeExecution6DOF.m found in [31].

Table 5.3 shows a summary of the computational complexity of the proposed algorithm measured in the number of oating point operations (additions and multiplications) as function of n, the number of DoF of the robot. A detailed breakdown of the computational complexity is found in the le Operation_Count.ods found in [31].

Finally, to measure the numerical accuracy of the calculations, the following metricvalue was dened:

e = 2 n 3 i,j,k c k ji -ĉk ji c k ji + ĉk ji f or each, c k ji = 0 (5.49)
Where e is the relative error, c k ji is Christoel symbol calculated using the proposed method and ĉk ji is Christoel symbol calculated using the symbolic method. From various calculations using randomly generated congurations, the maximum (worst) e value achieved is 2.196e -14, indicating that the error is so small mainly due to numerical rounding errors.

Application Example

An important application for the proposed algorithm is in minimum-time trajectory optimization for industrial manipulators working in exible manufacturing. In such based on robot dynamics as shown in [START_REF] Reynoso-Mora | A convex relaxation for the timeoptimal trajectory planning of robotic manipulators along predetermined geometric paths[END_REF][START_REF] Verscheure | Practical time-optimal trajectory planning for robots: a convex optimization approach[END_REF], where the elements of vector c (q) in equation (5.37) are calculated based on the mass matrix and Christoel symbols: ck = j a kj q j + c k ij q i q j

(5.50)

Where ck is the k th element of the vector c (q) , q j is calculated from:

q j = dq j dθ (5.51)
And q j is calculated from:

q j = d 2 q j dθ 2
(5.52)

A representative scenario is shown in Figure 5.7, where a 7 DoF industrial robot is used to palletize objects in a exible-manufacturing production line. Optimizing the robot motion for achieving minimum-time trajectory is very important for achieving high productivity. However, due to the exible-manufacturing requirements, the robot is required to manipulate various types of objects, with dierent inertial data (inertial tensor, mass, center of mass). Consequently, the inertial data of the object has to be taken into consideration for optimizing the robot motion while moving the object.

This can be done by considering the last link of the robot and the object as a one body when the robot is manipulating it on the planned path.

In such a case, a laser sensor is used to read the bar-code sticker (on the object) which includes the inertial data of the object. When a new object (with dierent inertial data) is present, the control algorithm calculates the equivalent inertial data of the last link coupled with the object. Considering both, the inertia of the last link of the robot and the object. Afterwards, the optimization problem is invoked,

where Christoel symbols are calculated eciently and on-the-y using our algorithm.

In comparison, deducing the equations of Christoel symbols by symbolic manipulation creates a bottleneck in the problem formulation, where generating the symbolic equations is (extremely) time consuming (requires an oine generation phase which is eliminated by our recursive algorithm). This limits the applicability of the traditional method for calculating Christoel symbols for variable inertias in time critical operations. After formulating the problem, the time required to perform the optimization is of order of seconds [START_REF] Verscheure | Practical time-optimal trajectory planning for robots: a convex optimization approach[END_REF].

This application example shows the importance of our algorithm for performing Christoel symbols calculation in a practical application, where our algorithm oers two fundamental advantages:

1. Christoel symbols are calculated on-the-y (without requiring an extremely time consuming oine phase), even when the inertial data are changing;

2. Christoel symbols are calculated more eciently using our recursive algorithm than using the traditional method (symbolic equation generation).

Summary

In this section we proposed recursive algorithm for calculating Christoel symbols eciently for serially linked robots. The algorithm achieves better eciency over Lagrangian based symbolic method. This increase in eciency is achieved by performing backward recursion on forces and moments. As compared to symbolic method, computational testing proves that the proposed algorithm is (1) ecient (faster execution time), (2) precise (negligible numerical error), and most importantly (3) it does not require a time consuming o-line code generation phase.

149 Chapter 6 Conclusion and Future Work

In this thesis three main topics related to collaborative robotic manipulators were discussed, the collision avoidance of collaborative robots, the hand-guiding of sensitive manipulators in addition to the dynamics of serially linked robots.

The subject of collision avoidance for robotic manipulators has been present in the robotics literature for quite some time. However, it can be noticed that the majority of reported studies are implemented in simulation, on the other hand only a few applied collision avoidance in a real setup (mostly using experimental robot and Kinect vision based sensor, without industrial application context). Thus, in our proposed collision avoidance topic we focused on its implementation in real industrial applications (using collaborative industrial robot KUKA iiwa, and industrial sensors). A laser scanner and IMU sensors are used to capture the conguration of the human coworker around the robot (both are approximated by capsules). An ecient algorithm based on QR factorization is used to calculate the minimum distance between the capsules eciently. This minimum distance is used as a collision imminence measure. A repulsion vector based on the minimum distance and the relative velocity between the human and the robot is used to repel the robot away from collision. However, unlike other algorithms in robotics literature, we noticed that for industrial applications the presence of a predened oine generated path is important, and the robot shall be able to switch between dierent tasks smoothly while moving on the path. Thus, a traditional implementation of the collision avoidance motion without further modication might cause discontinuity in the collision avoidance motion, especially when switching between the tasks that does not allow collision avoidance motion (active assembly) to tasks that allow collision avoidance motion (free motion in the work space) while the human is near the robot. To solve this problem we proposed a solution to guarantee the continuity of the motion. Finally, the proposed algorithm is tested successfully on an industrial robotic cell in automotive industry (car door assembly task).

Going back to the published studies (in the robotics literature) on manipulator's collision avoidance, it can be noticed that most potential elds based algorithms dene the attraction and the repulsion vectors as the gradients of the attraction and repul-sion potential elds. However, moving along the gradient to minimize the potential function is not the best option and suers drawbacks (known fact in the optimization literature where such implementation is termed as the greedy algorithm), for example at some critical congurations it suers from intermittent oscillations. This issue is also known in mobile robots literature, where a solution was proposed by applying

the Newton method on the potential elds. This solution is inspired from the optimization literature, where the Newton method is preferred over the gradient when the Hessian is known. After a long research into the subject, we could not nd any study describing the application of the Newton method on PF based collision avoidance for robotic manipulators. We believe this comes from the fact that for mobile robots the potential eld is an explicit function of the conguration space variables, consequently the Hessian is easy to deduce. On the other hand, for robotic manipulators it is not obvious how to calculate its Hessian matrix. In this thesis, a symbolic formula for fast calculation of the Hessian matrix of robotic manipulators is derived, also a systematic method for applying the Newton method to potential elds collision avoidance of robotic manipulators is presented. Where, the Newton method is applied for calculating the direction of the collision avoidance motion in the joint space of the robot.

Simulation tests using hyper-redundant planner manipulator proved the eectiveness of the proposed method.

On the subject of hand-guiding collaborative robotic manipulators we presented the precision hand-guiding method, an intuitive alternative to the teach pendant for performing precise positioning operations. It also allows to use the robot as an assistive third hand, which is appealing for tedious assembly tasks. Consequently, a robot can be hand-guided to lift and hold parts in place, while giving the human coworker an opportunity to apply the xtures (perform the assembly task). Such functionality reduces the risk to workers and properties (falling components), provides precision, allows lifting heavier parts, and increases productivity by keeping less workers occupied in manual tasks on the factory oor. By attaching a force/torque sensor at the ange of the sensitive redundant manipulator, we demonstrated a method that allows to precisely hand-guide the end-eector in the Cartesian space while navigating around surrounding obstacles (upon a contact). Tests were carried out successfully using KUKA iiwa collaborative robot.

On robot dynamics formulation, the thesis presented two recursive algorithms with high computational eciency. The rst is an algorithm with a minimal second order cost for calculating the joint space inertia matrix of serially linked robots. This is important for achieving the lowest computational cost when calculating joint space inertia matrix of hyper-redundant manipulators where the second order term is associated with the highest computational cost. The second algorithm is a recursive non-symbolic method for calculating Christoel symbols of the robotic manipulator.

This method is more ecient (in order of magnitude) than the traditional symbolic method (based on Lagrangian formulation). Various tests were carried out for comparing the performance of the proposed methods against state of the art methods.

Results (in terms of execution time and computational complexity analysis) indicate the superior eciency of the proposed methods.

Future work will be focused in various aspects. For collision avoidance, the proposed method can be extended for mobile manipulators through the implementation of To move the end-eector in an ellipse in the XY, XZ, YZ plane (movePTPEllipseXY, movePTPEllipseXZ, movePTPEllipseYZ) Appendix C

Gravity compensation of IMU measurements

To compensate for gravity eect in the acceleration measurements of the IMU, rst a calibration of the sensor frame in relation to end-eector frame shall be established.

This phase is important because the geometry of the sensor's holder (xture) is not perfect and also due to the uncertainties in the assembly of the sensor on the endeector. As such the rotation matrix R s ef , from EEF frame to sensor frame, shall be established. To do this, a calibration phase is proposed. Since that the relationship between the measured gravity vector g s in the sensor frame, and the gravity vector in end-eector frame g ef :

g s = R s ef g ef (6.8)
And since that:

R s ef = X, Y , Z (6.9)
Where X, Y and Z are the direction vectors along the x, y and z axes (respectively)

of EEF frame described in sensor frame. Based on the previous equations three steps are proposed to calculate the rotation matrix R s ef :

In the rst step, the x axis of the end-eector frame is positioned vertically facing upwards. Afterwards, the measurement of the gravity vector g s using the IMU is acquired, while the robot is static. The relationship between the gravity vector measured in sensor frame and the gravity vector in end-eector frame:

g s = R s ef   -g s 0 0   (6.10)
Where g s is the magnitude of the vector g s . By analyzing the previous equation, the rst column vector X of the matrix R s ef can be calculated by: X = g s g s (6.11) Figure 6.1 shows conguration of the robot and the sensor for the rst calibration step.

In the second step, the y axis of the end-eector is positioned vertically facing upwards, afterwards the measurement of the gravity vector g s using the accelerometer is acquired, while the robot is static. The relationship between the gravity vector measured in sensor frame and the gravity vector in end-eector frame: Where g s is the magnitude of the vector g s . By analyzing the previous equation, an approximation Y 0 of the second column vector Y of the matrix R s ef can be calculated:

g s = R s ef   0 -g s 0   (6.12)
Y 0 = g s -g s (6.13)
The unit vectors X and Y of the matrix R s ef shall be perfectly normal to each others. As such a Gram-Schmidt like procedures shall be applied to guarantee that X and Y are orthogonal, this is important to correct for the small errors coming from (1) the inaccuracy in end-eector positioning which depends on the precision of the robot, (2) the measurement uncertainty which depends on the precision of the inertial measuring unit and (3) the external noise due to vibrations and other interfering eects.

Y = Y 0 -( Y T 0 X) X Y 0 -( Y T 0 X) X (6.14)
Figure 6.2 shows conguration of the robot and the sensor in the second calibration step.

For calculating the third column of the rotation matrix Z, a similar procedure could be carried out. Alternatively, the Z vector can be calculated from the cross product:

Z = X × Y (6.15)
Thus the vibrations Γ s are calculated from the raw measurements of the IMU u s after gravity compensation using the following equation:

Γ s = u s -R s ef R ef b   0 0 -g   (6.16)
Where R ef b is the rotation matrix of the robot base frame to EEF frame. Previous equation applies for upright base mounted robots, as the case in our experiment. Principales contributions en matière de guidage manuel:

1. Présentation de la méthode de guidage manuel de précision. Un substitut intuitif au Teach-Pendant du robot pour eectuer un positionnement précis de l'End-Eector.

2. Introduction d'une nouvelle solution pour le guidage manuel de précision de manipulateurs redondants sensibles, au niveau de l'End-Eector, tout en naviguant sur les obstacles lors du contact.

Principales contributions en matière de dynamique des robots:

Cinématique des manipulateurs robotiques

La plupart des manipulateurs industriels sont constitués de maillons rigides couplés en série par des joints tournants. Dans un tel cas, la conguration du robot est décrite par le vecteur de position angulaire de ses articulations q ∈ n , où n est le nombre de degrés de liberté du robot. D'autre part, les robots doivent eectuer des tâches dans l'espace cartésien, spécié par la position et l'orientation de l'End-Eector du robot (vecteur x ∈ m ). La fonction de cinématique [START_REF] John | Introduction to robotics: mechanics and control[END_REF] introduit une cartographie à partir de la conguration du robot q à la position/orientation de l'End-Eector x:

x = f (q) (6.17)

Où f est la cinématique avant du manipulateur, une fonction vectorielle très non linéaire, qui donne la position et l'orientation de l'End-Eector en fonction des angles des articulations. Cette fonction peut être calculée en utilisant les matrices de transformation homogènes, selon la convention Denavit-Hartenberg (DH) [START_REF] Denavit | A kinematic notation for lower pair mechanisms based on matrices[END_REF]. A partir de [START_REF] John | Introduction to robotics: mechanics and control[END_REF] une matrice de transformation (utilisant les paramètres DH modiés) du lien i est donnée par:

T i-1 i =     cθ i -sθ i 0 a i-1 sθ i cα i-1 cθ i cα i-1 -sα i-1 -d i sα i-1 sθ i cα i-1 cθ i cα i-1 cα i-1 d i cα i-1 0 0 0 1     (6.18)
Où i est l'indice de joint, θ i est l'angle de rotation du joint i autour de l'axe z i , et (d i , α i-1 , a i-1 ) sont les paramètres DH modiés du lien i. La diérenciation (6.17) donne l'équation de la cinématique diérentielle [1], [START_REF] Corke | Robotics, vision and control: fundamental algorithms in MATLAB[END_REF]:

ẋ = J q (6.19)
Où q est le vecteur de vitesse des articulations, ẋ est la vitesse de l'End-Eector et J est la matrice jacobienne analytique du manipulateur, dont chaque élément:

J ij = δf i δq j (6.20)
Cependant, le Jacobien analytique peut également être calculé à partir du Jacobien géométrique par pré-multiplication avec une matrice dont la valeur dépend du choix de la représentation de l'orientation [1].

Pseudo Inverse

Pour les manipulateurs non redondants, la cartographie inverse, d'une vitesse particulière de EEF à la vitesse des articulations est donnée par:

q = J -1 ẋ (6.21)
Dans le cas de manipulateurs redondants, J 'est pas carré (a plus de colonnes que de lignes) et la matrice J -1 n'existe pas. Dans ce cas, la solution de la norme la plus faible de la cartographie inverse est donnée par le pseudo inverse :

q = J † ẋ (6.22)
Où J † est le Moore-Penrose ou pseudo inverse du Jacobien:

J † = J T (JJ T ) -1 (

Projection dans l'espace Nul

Les manipulateurs redondants possèdent plus de degrés de liberté qu'il n'en faut pour exécuter une tâche donnée. Dans un tel cas, la cartographie inverse (les vitesses angulaires des articulations qui donnent une vitesse particulière de EEF) implique un nombre inni de solutions, ces solutions sont données par l'équation:

q = J † ẋ + N qc (6.25)
Où N est la matrice de projection de l'espace nul et qc est une commande de vitesse angulaire. qc est couramment utilisé pour optimiser certains critères. A partir de [37], la formule de N est donnée par: Où qnull est le vecteur des vitesses angulaires dans l'espace nul du robot. 1. L'interfaçage avec les capteurs est plus facile à l'aide d'un ordinateur externe.

N = I -J T (JJ T ) -1 J (
2. La possibilité d'intégrer des simulations 3D tout en contrôlant le robot.

3. Les limites de calcul imposées par la commande du robot.

4. Intégration de divers logiciels de calcul mathématique facilement implémentables sur un ordinateur externe.

Pour des raisons de lisibilité, cette section est organisée en plusieurs sous-sections.

Après la sous-section "Motivation", la sous-section "Introduction" donne un aperçu des logiciels de robotique les plus populaires disponibles dans la communauté de la recherche. La sous-section Architecture donne des détails sur les éléments constitutifs de la ToolBox proposée. Enn, la sous-section Modus Operandi énumère, avec des exemples, les diérentes méthodes proposées par la ToolBox et leur utilisation.

(a) En raison de son importance, le guidage manuel a été abordé dans plusieurs études, où diérentes terminologies ont été utilisées par les chercheurs pour décrire le processus de guidage manuel: "manual-guidance" [START_REF] Massa | Manual guidance for industrial robot programming[END_REF], "force guidance" [START_REF] Boulhosa Rodamilans | A comparison of industrial robots interface: force guidance system and teach pendant operation[END_REF], "lead-through

(b) (c) (d) (e) (f) (g) (h) (i) (j)
programming" [START_REF] Ragaglia | Accurate sensorless lead-through programming for lightweight robots in structured environments[END_REF], or "walk-through programming" [START_REF] Bascetta | Walkthrough programming for robotic manipulators based on admittance control[END_REF]. En raison de ses avantages, la fonctionnalité de guidage manuel gagne en popularité dans la communauté robotique, permettant de programmer un robot de manière intuitive en le déplaçant conformément à la force appliquée.

Cette section présente une nouvelle méthode de guidage manuel tout en préservant la précision au niveau du EEF, une exigence importante pour de nombreuses opérations

  Université de Coimbra et École Nationale Supérieure d'Arts et Métiers Les robots manipulateurs collaboratifs sont de nouveaux types de manipulateurs dans l'industrie, qui permettent la coexistence homme-robot en partageant le même espace de travail. Par principe, un humain peut travailler au côté d'un robot collaboratif en toute sécurité sans devoir utiliser une barrière physique ou une clôture de sécurité pour les séparer. Actuellement, le développement de ces moyens est encore limité par le niveau de collaboration homme-machine qui se résume généralement à du partage d'espace en mode dégradé. Ainsi, nous proposons dans cette étude plusieurs contributions concernant la sécurité et la collaboration an d'étendre le champ d'interaction entre l'humain et le robot, et par conséquent la viabilité économique de ce type d'application, dans un contexte industriel.

  Barroco. Many thanks to my friends and colleagues in the collaborative robotics labs in both Universities. This thesis was possible thanks to the support of the Foundation for Science and Technology (FCT), through grant SFRH/BD/131091/2017. working side-by-side with a human coworker in Aerospace industry, courtesy of ColRobot project [3]. . . . . . . . . . . . . . . . . . 1.2 A robot operating in potentially explosive environment, among pipes carrying gas [4]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Morphin map of Curiosity Mars rover, courtesy of Mars autonomy project. 1.4 The Curiosity rover on Mars (NASA). . . . . . . . . . . . . . . . . . . . 1.5 Graphical representation of thesis contributions into the eld, the contributions are highlighted in blue. . . . . . . . . . . . . . . . . . . . . . . 2.1 Articial potential eld concept. . . . . . . . . . . . . . . . . . . . . . . 3.1 (A) L'Uomo Vitruviano from Leonardo da Vinci and its representation by 10 capsules, (B) human represented by 1 capsule, (C) human represented by 3 capsules, (D) human represented by 5 capsules, (E) robot represented by 2 capsules, (F) robot represented by 3 capsules and (G) human hand and forearm are represented by 21 capsules. . . . . . . . . .

  IMUs attached to the upper body, (b) a human (represented by 5 capsules) standing in the scanning eld of a laser scanner and (c) robot represented by 3 capsules. . . . . . . . . . . . . . . . . . . . . . . . . . .3.9Laser scanner mounted at the level of the coworker's legs in the base of the table holding the robot. If the robotic arm is installed on a mobile platform the solution is similar. . . . . . . . . . . . . . . . . . . . . . . 3.10 Raw data and LPF ltered of radius measurement with scan-angle, close ups show ltered data are smoother in the critical parts of the curves. . 3.11 Filtered radius measurement with scan-angle. Minimas are marked with green and red dots representing the human legs. . . . . . . . . . . . . . 3.12 Human legs detected in the laser scan eld. Red and green dots represent the legs and the black circle represents the projection of the capsule covering the torso. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.13 Minimum distance between two capsules. . . . . . . . . . . . . . . . . . 3.14 Proposed framework for on-line human-robot collision avoidance. . . . . 3.15 Block diagram showing the proposed method for calculating the magnitude of the modied repulsion vector. . . . . . . . . . . . . . . . . . . . . 3.16 The nominal path curve dened o-line, the attraction pole, and the error vector. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.17 Block diagram showing the proposed method for calculating the attraction vector. Term ψ i is calculated using the Algorithm 3.3, which is used to avoid windup problem. . . . . . . . . . . . . . . . . . . . . . . . 3.18 Robot (A) and human (B) represented by capsules. . . . . . . . . . . . . 3.19 Test 1 results. Minimum distance, EEF velocity and position along y axis (top). Snapshot of collision avoidance testing and collision avoidance path in 3D and 2D space (middle and bottom). . . . . . . . . . . . 3.20 Test 2 results. Minimum distance, EEF velocity and position along y axis (top). Snapshot of collision avoidance testing (bottom). . . . . . . . 3.21 Test 3. Collaborative robotic cell for car door card assembly (video segment showing the experiment is in [6]). . . . . . . . . . . . . . . . . 3.22 Pre-established sub-path segments for test 3. This process is detailed in the algorithm 3.4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.23 Sequence in time of collision avoidance simulation for hyper redundant planar manipulator using Newton method. . . . . . . . . . . . . . . . . . 3.24 Test 1: comparison between congurations of the two manipulators after a period time from the beginning of the simulation. . . . . . . . . . . . . 3.25 Test 1: joint angles, Newton method (left), gradient method (right). . . 3.26 Test 1: rst joint angle, Newton method (left), gradient method (right). 3.27 Test 1: minimum distance between last link and obstacles, Newton method (left), gradient method (right). . . . . . . . . . . . . . . . . . . . 3.28 Test 2: time line during simulations of 15 DoF planar manipulator for three dierent optimization methods. . . . . . . . . . . . . . . . . . . . . 3.29 Architecture of the toolbox. . . . . . . . . . . . . . . . . . . . . . . . . . 4.1 Robot motion groups for precision hand-guiding. . . . . . . . . . . . . . 4.2 Hand-guiding moment in EEF reference frame for 2nd motion group (left) and for the 3rd motion group (right). . . . . . . . . . . . . . . . . 4.3 Damper-mass mechanical system. . . . . . . . . . . . . . . . . . . . . . .

4 . 8 9 Test 1 :

 4891 Joint four near the limit, yellow and white markers are nearing each others. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.Precision hand-guiding for assembly operation. . . . . . . . . . . 4.10 Test 2: End-eector position along y and z axes according to applied force. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.11 Test 2: Nominal end-eector path against the real robot end-eector path in plane xy. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.12 Test 2: Nominal end-eector path against the real robot end-eector path in plane zy. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.13 Test 3: Experimental setup. . . . . . . . . . . . . . . . . . . . . . . . . . 4.14 Test 3: Coordinates of the end-eector during the positioning task using the precise hand-guiding function. . . . . . . . . . . . . . . . . . . . . . . 4.15 Test 3: Coordinates of the end-eector during the prescribed task using the KUKA o-the-shelf hand-guiding. . . . . . . . . . . . . . . . . . . . 4.16 Test 3: Orientation of the end-eector in the precise hand-guiding function.115 4.17 Test 3: Orientation of the end-eector during the KUKA o-the-shelf hand-guiding function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.18 Test 3: Joint angles during positioning operation for the precise handguiding. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.19 Test 3: Joint angles during positioning operation for the KUKA o-theshelf hand-guiding. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.20 Test 3: Accelerometer attached at the end-eector. . . . . . . . . . . . . 4.21 Test 3: Acceleration magnitude (after gravity compensation) at endeector during precise/KUKA hand-guiding. . . . . . . . . . . . . . . . .4.22 Programming by demonstration system based on the precision handguiding method, (left) real robot, (middle) real-time simulation of the robot/gripper, the black lines inside the simulation represent the taught path, (right) HMI interface for easy control of the teaching process. . . . 4.23 Teaching an operation to KUKA iiwa using a programming by demonstration application based on the precision hand-guiding. A virtual reality simulation of the robotic cell in Vrep is used to show the coworker a feedback of the programmed path during the teaching process, allowing the user to visualise, modify and verify the taught path while performing the operation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.24 Precision hand-guiding on a straight line (along y axis) of a redundant robot subject to a contact with obstacle. An external FT sensor is attached at the robot ange. . . . . . . . . . . . . . . . . . . . . . . . . . 4.25 Experimental setup. The robot is hand-guided to perform a straight line motion and compliantly avoids the obstacle taking advantage of the redundant axis. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.26 Robot joints angular positions. . . . . . . . . . . . . . . . . . . . . . . . 4.27 External torques measurement at the robot joints. . . . . . . . . . . . . 4.28 Components of hand-guiding force described in base frame of the robot. 4.29 Positional data of the EEF. . . . . . . . . . . . . . . . . . . . . . . . . . 4.30 Actual path in xy plane. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.31 Actual path in zy plane. . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1 Inertial moment µ Ckj and linear acceleration Γ Ckj of center of mass of link k transferred by frame. j . . . . . . . . . . . . . . . . . . . . . . . . 5.2 Tangential, normal and Coriolis accelerations of center of mass of link k transferred by frame j. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.3 Inertial forces and moments acting on link k due to angular acceleration of joint j. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4 Execution time results for GDAHJ vs CRBA. . . . . . . . . . . . . . . . 5.5 Relative error in computation for results achieved using GDAHJ and CRBA. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.6 Backward recursion on moments and forces. . . . . . . . . . . . . . . . . 5.7 Minimum-time trajectory optimization for industrial manipulator performing palletization in exible-manufacturing scenario. . . . . . . . . . 6.1 Calibration phase rst step, x axis of the EEF is positioned vertically facing upwards (front view). . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Calibration phase second step, y axis of the EEF is positioned vertically facing upwards (upper view). . . . . . . . . . . . . . . . . . . . . . . . . 6.3 KUKA iiwa robot travaillant côte à côte avec un collègue humain dans l'industrie aérospatiale, (Projet ColRobot [3]). . . . . . . . . . . . . . . . 6.4 Un robot fonctionnant dans un environnement potentiellement explosif, parmi des tuyaux transportant du gaz [4]. . . . . . . . . . . . . . . . . . 6.5 Carte Morphin du rover Curiosity Mars, gracieuseté du projet d'autonomie de Mars. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.6 Le rover Curiosity sur Mars (NASA). . . . . . . . . . . . . . . . . . . . . 6.7 Représentation graphique des contributions de thèse dans le domaine, les contributions sont surlignées en bleu. . . . . . . . . . . . . . . . . . . 6.8 Concept de champ potentiel articiel. . . . . . . . . . . . . . . . . . . . . 6.9 (A) L'Uomo Vitruviano représenté par 10 capsules, (B) humain représenté par 1 capsule, (C) humain représenté par 3 capsules, (D) humain représenté par 5 capsules, (E) robot représenté par 2 capsules, (F) robot représenté par 3 capsules, (G) la main et l'avant-bras humains sont représentés par 21 capsules. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.10 (a) IMU attachées à la partie supérieure du corps, (b) un humain (représenté par 5 capsules) debout dans le champ de balayage d'un scanner laser et (c) un robot représenté par 3 capsules. . . . . . . . . . . . . . . . . . . . 6.11 Proposition de cadre pour la prévention des collisions homme-robot en temps réel. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Figure 1 . 1 :

 11 Figure 1.1: KUKA iiwa robot working side-by-side with a human coworker in Aerospace industry, courtesy of ColRobot project [3].

Figure 1 . 2 :

 12 Figure 1.2: A robot operating in potentially explosive environment, among pipes carrying gas [4].

Figure 1 . 3 :

 13 Figure 1.3: Morphin map of Curiosity Mars rover, courtesy of Mars autonomy project.

Figure 1 . 4 :

 14 Figure 1.4: The Curiosity rover on Mars (NASA).

Figure 1 . 5 :

 15 Figure 1.5: Graphical representation of thesis contributions into the eld, the contributions are highlighted in blue.

4 .

 4 Development of a new algorithm for generating reactive collision avoidance motion (for robotic manipulators), where the Newton method (from optimization) is applied on the potential eld function for calculating the minimization direction.

Figure 2 . 1 :

 21 Figure 2.1: Articial potential eld concept.

Figure 3 . 1 :

 31 Figure 3.1: (A) L'Uomo Vitruviano from Leonardo da Vinci and its representation by 10 capsules, (B) human represented by 1 capsule, (C) human represented by 3 capsules, (D) human represented by 5 capsules, (E) robot represented by 2 capsules, (F) robot represented by 3 capsules and (G) human hand and forearm are represented by 21 capsules.

Figure 3 . 1 (

 31 C) shows a human represented by 3 capsules. In Figure 3.1 (D) the human is represented by 5 capsules, 2 capsules in each arm and 1 single capsule for the torso and head. A relatively precise representation of the human hand and forearm by 21 capsules is shown in Figure 3.1 (G). A robot can roughly be represented by 2 capsules, Figure 3.1 (E), or by 3 capsules representing the main robot links (KUKA iiwa with 7 DoF), Figure 3.1 (F). The position and orientation of the capsules covering the robot are obtained from forward kinematics calculation at the measured robot joint angles. In literature, various methods were

Figure 3 . 2 :

 32 Figure 3.2: Minimum distance between two capsules.

Figure 3 . 3 :

 33 Figure 3.3: Region of feasible solutions and level sets of optimization problem (3.5).

Figure 3 . 4 :

 34 Figure 3.4: Region of feasible solutions and level sets of modied optimization problem (3.7).

Figure 3 . 6 :

 36 Figure 3.6: Execution time ratio (Method [5]/QR in red), (Modied method [5]/QR in blue) and (Method [5]/Modied method [5] in green) as a function of number of line-segments/capsules of the set. Algorithms implemented in C++.

Figure 3 . 8 :

 38 Figure 3.8: (a) IMUs attached to the upper body, (b) a human (represented by 5 capsules) standing in the scanning eld of a laser scanner and (c) robot represented by 3 capsules.

Figure 3 . 9 :

 39 Figure 3.9: Laser scanner mounted at the level of the coworker's legs in the base of the table holding the robot. If the robotic arm is installed on a mobile platform the solution is similar.

Figure 3 . 10 :

 310 Figure 3.10: Raw data and LPF ltered of radius measurement with scan-angle, close ups show ltered data are smoother in the critical parts of the curves.

Figure 3 . 11 :

 311 Figure 3.11: Filtered radius measurement with scan-angle. Minimas are marked with green and red dots representing the human legs.

Figure 3 . 12 :

 312 Figure 3.12: Human legs detected in the laser scan eld. Red and green dots represent the legs and the black circle represents the projection of the capsule covering the torso.

Figure 3 . 13 :

 313 Figure 3.13: Minimum distance between two capsules.

  By reading the initial IMU measurement w ref imu,init , the quaternion w ref b of the calibration phase, the rotation quaternion from the reference frame of the IMU to the base of the robot w b ref is calculated as the inverse of the quaternion w ref b .

1 1

 1 as described in upper chest's IMU frame are [ -d 1 d 2 0 ] for the left shoulder and [ -d 1 -d 2 0 ] for the right shoulder.

2

 2

2

  as described in upper arm's IMU frame are [ d 3 0 0 ]. d 3 is the length of the upper arm measured from the shoulder to the elbow.

3 3

 3 as described in forearm's IMU frame are [ d 4 0 0 ]. d 4 is the length of the forearm measured from the elbow to the wrist.

  is the vector of human part described in the i th -imu frame. w b imu i is the quaternion describing the rotation of i th -imu in relation to base frame of the robot Eq (3.16) and w b imu i -1 is the complex conjugate of w b imu i . The position vector of the shoulder point in the base frame of the robot p b 1 is calculated:

Figure 3 . 14 :

 314 Figure 3.14: Proposed framework for on-line human-robot collision avoidance.

Figure 3 . 15 :

 315 Figure 3.15: Block diagram showing the proposed method for calculating the magnitude of the modied repulsion vector.

Figure 3 .

 3 Figure 3.15 shows a block diagram illustrating the proposed method for calculating the modied repulsion v rep.mod and its relationship to v rep.1 and v rep.2 .

Figure 3 . 16 :

 316 Figure 3.16: The nominal path curve dened o-line, the attraction pole, and the error vector. Algorithm 3.3 Integral term of the attraction vector. Algorithm Integral term of the attraction vector 1: for each time step △t do 2:

Figure 3 .

 3 Figure 3.18: Robot (A) and human (B) represented by capsules.

3. 18 . 1 Figure 3 .

 1813 Figure 3.19 shows the results for test 1. The human forearm, represented by a capsule, is extended and acts as an obstacle. The robot is moving on a straight line path

Figure 3 . 19 :

 319 Figure 3.19: Test 1 results. Minimum distance, EEF velocity and position along y axis (top). Snapshot of collision avoidance testing and collision avoidance path in 3D and 2D space (middle and bottom).

Figure 3 . 20 :

 320 Figure 3.20: Test 2 results. Minimum distance, EEF velocity and position along y axis (top). Snapshot of collision avoidance testing (bottom).

Algorithm 3 . 4

 34 Collision avoidance -collaborative robotic cell in automotive industry (reads together with Figure 3.22) Algorithm Collision avoidance -automotive sample case 1: for each time step do 2:

7 : 8 :

 78 if goal point reaches the segment end then Goal point stops moving 9:

19 : 1 Figure 3 . 21 :

 191321 Figure 3.21: Test 3. Collaborative robotic cell for car door card assembly (video segment showing the experiment is in [6]).

Figure 3 . 22 :

 322 Figure 3.22: Pre-established sub-path segments for test 3. This process is detailed in the algorithm 3.4.

Figure 3 . 23 :Figure 3 . 24 :• Test 2 :

 3233242 Figure 3.23: Sequence in time of collision avoidance simulation for hyper redundant planar manipulator using Newton method.

Figure 3 . 25 :

 325 Figure 3.25: Test 1: joint angles, Newton method (left), gradient method (right).

Figure 3 . 26 :

 326 Figure 3.26: Test 1: rst joint angle, Newton method (left), gradient method (right).

Figure 3 . 27 :

 327 Figure 3.27: Test 1: minimum distance between last link and obstacles, Newton method (left), gradient method (right).

  the following example an instance of the KST class associated with LBR7R800 robot is created. > > ip='172.31.1.147'; > > rT=KST.LBR7R800; > > fT=KST.Medien_Flansch_elektrisch; > > Tef_fl=eye(4); > > iiwa = KST(ip,rT,fT,Tef_fl);3.5.5 Properties of the KST classThe KST class implements several properties. Those properties are used to store various variables and values that are important for the proper operation of the toolbox. Some of those properties are used for communication purposes, for example to store the IP of the robot or to store the TCP/IP communication object used by the class.Others are used to store the various parameters dening the robot, like the type of the robot, the type of the ange attached to the robot, the Denavit-Hartenberg (DH) parameters of the robot and the inertial data of the robot. These inertial parameters include: the mass of each link, the inertial tensor of each link and the position vector of center of mass of each link. As an example, to access the link-twist angles[START_REF] John | Introduction to robotics: mechanics and control[END_REF] of DH parameters, the user can access the dh_data property as the following: > > alfa=iiwa.dh_data.alfa alfa= {[0]} {[-1.57]} {[1.57]} {[1.57]} {[-1.57]} {[-1.57]} {[1.57]} 3.5.6 Methods of the KST class The KST class implements various methods (Appendix B), according to their functionality those methods are grouped into seven dierent categories: 1. Networking; 2. Soft real-time control; 3. Point-to-point (PTP) motion;

Figure 4 . 1 :

 41 Figure 4.1: Robot motion groups for precision hand-guiding.

Figure 4 . 2 :

 42 Figure 4.2: Hand-guiding moment in EEF reference frame for 2nd motion group (left) and for the 3rd motion group (right).

6 )

 6 The controller command for 2nd motion group is calculated by transforming m e xy to the robot base frame: m xy = R b e m e xy (4.7)

m e z = m e -m e xy ( 4 . 8 )( 4 . 9 )

 4849 The controller command for the 3rd motion group is calculated by transforming m e z to the robot base frame:m z = R b e m e zOnly one motion group (2nd or 3rd motion group) is allowed to be performed once at a time. Depending on the magnitude of m xy and m z , the moment command m cmd sent to the control algorithm is:

Figure 4 . 3 :

 43 Figure 4.3: Damper-mass mechanical system.

  Figure 4.3. The motion equation of center of mass (COM) is dened by:

Table 4 . 2 :Figure 4 . 4 :

 4244 Figure 4.4: Scaling factor for joint limits avoidance.

  and 4.6, this helps giving a visual feedback to the worker, at the same time markers are added on the joints Figures 4.7 and 4.8, by looking at it the coworker is able to realize which joint is nearing the limit.

Figure 4 . 5 :

 45 Figure 4.5: Light alert (yellow) turns on when joint ve is near the limit.

Figure 4 . 6 :

 46 Figure 4.6: Light alert turns on, when pushing joint four near its limit.

Figure 4 . 7 :

 47 Figure 4.7: Joint two near the zero, white markers are near each others.

Figure 4 . 8 :

 48 Figure 4.8: Joint four near the limit, yellow and white markers are nearing each others.

Figure 4 . 9 :

 49 Figure 4.9: Test 1: Precision hand-guiding for assembly operation.

Figure 4 .

 4 Figure 4.10 shows end-eector position data recorded during the test. It is noticed that when the hand-guiding force along one of the axes reaches the predened limit,

Figure 4 . 10 :

 410 Figure 4.10: Test 2: End-eector position along y and z axes according to applied force.

Figure 4 . 12 :

 412 Figure 4.12: Test 2: Nominal end-eector path against the real robot end-eector path in plane zy.

Figure 4 . 13 :

 413 Figure 4.13: Test 3: Experimental setup.

Figure 4 .

 4 14 presents the coordinates of end-eector during the prescribed task using the precise hand-guiding function. While Figure4.15 presents the coordinates of end-eector during the prescribed task using the KUKA o-the-shelf hand-guiding function. From the results we notice that the precise hand-guiding oers smoother motion. An expected result, since that in the precise hand-guiding the robot is controlled at EEF level, so no eort is required by the operator for constraining endeector's orientation during the motion, unlike the KUKA o-the-shelf hand-guiding which operates at joints level.At the same time the orientation of the end-eector was acquired and relayed over the TCP/IP connection to an external computer.

Figure 4 .

 4 16 represents the orientation results of the end-eector (represented by a unit quaternion) for the prescribed task using the precise hand-guiding function. Figure4.17 represents the orientation results of the end-eector during the positioning task using the KUKA o-the-shelf handguiding function. In the precise hand-guiding the robot is able to keep the same

Figure 4 . 14 :

 414 Figure 4.14: Test 3: Coordinates of the end-eector during the positioning task using the precise hand-guiding function.

Figure 4 . 15 :Figure 4 . 16 :

 415416 Figure 4.15: Test 3: Coordinates of the end-eector during the prescribed task using the KUKA o-the-shelf hand-guiding.

Figure 4 .

 4 Figure 4.18 and Figure 4.19 show the joints angles for the positioning during the precise and the KUKA hand-guiding respectively. From the gures we nd that the precise hand-guiding results in smoother angles of the joints over KUKA o-the-shelf handguiding.

Figure 4 .

 4 [START_REF] Safeea | A quest towards safe human robot collaboration[END_REF], we notice that the acceleration levels at the end-eector in the case of the precise hand-guiding are less than those of the KUKA hand-guiding, where the maximum magnitude of acceleration in case of KUKA hand-guiding reached 2.38 m/sec 2 , while in case of the precise hand-guiding 1.2 m/sec 2 .

Figure 4 . 17 :

 417 Figure 4.17: Test 3: Orientation of the end-eector during the KUKA o-the-shelf hand-guiding function.

Figure 4 . 18 :

 418 Figure 4.18: Test 3: Joint angles during positioning operation for the precise handguiding.

Figure 4 . 19 :

 419 Figure 4.19: Test 3: Joint angles during positioning operation for the KUKA o-theshelf hand-guiding.

Figure 4 . 20 :Figure 4 . 21 :

 420421 Figure 4.20: Test 3: Accelerometer attached at the end-eector.

Figure 4 . 22 :

 422 Figure 4.22: Programming by demonstration system based on the precision handguiding method, (left) real robot, (middle) real-time simulation of the robot/gripper, the black lines inside the simulation represent the taught path, (right) HMI interface for easy control of the teaching process.

Figure 4 . 23 :

 423 Figure 4.23: Teaching an operation to KUKA iiwa using a programming by demonstration application based on the precision hand-guiding. A virtual reality simulation of the robotic cell in Vrep is used to show the coworker a feedback of the programmed path during the teaching process, allowing the user to visualise, modify and verify the taught path while performing the operation.

Figure 4 . 25 :

 425 Figure 4.25: Experimental setup. The robot is hand-guided to perform a straight line motion and compliantly avoids the obstacle taking advantage of the redundant axis.

Figure 4 .

 4 Figure 4.25 shows the proposed experimental setup. The robot is hand-guided by applying a force on the FT sensor in the y direction of the robot base frame. In consequence, the tool attached to the robot moves along a straight line in that direction.

  Figure 4.26, (2) external torque measurements at robot joints, Figure 4.27, (3) force measurements

Figure 4 . 26 :

 426 Figure 4.26: Robot joints angular positions.

Figure 4 . 27 :

 427 Figure 4.27: External torques measurement at the robot joints.

Figure 4 . 28 :

 428 Figure 4.28: Components of hand-guiding force described in base frame of the robot.

Figure 4 . 29 :

 429 Figure 4.29: Positional data of the EEF.

Figure 4 . 30 :

 430 Figure 4.30: Actual path in xy plane.

Figure 4 . 31 :

 431 Figure 4.31: Actual path in zy plane.

129

 129

Figure 5 . 1 :

 51 Figure 5.1: Inertial moment µ Ckj and linear acceleration Γ Ckj of center of mass of link k transferred by frame. j

Figure 5 . 3 :

 53 Figure 5.3: Inertial forces and moments acting on link k due to angular acceleration of joint j.

  k k k and bT k k k . The terms d k = D T k k k and y k = bT k k k can be calculated with an O(n) algorithm using Algorithm 5.1 Calculating joint space inertia matrix entries, algorithm is based on eq (5.31).

Figure 5 . 4 :

 54 Figure 5.4: Execution time results for GDAHJ vs CRBA.

Figure 5 . 5 :

 55 Figure 5.5: Relative error in computation for results achieved using GDAHJ and CRBA.

38 )

 38 We propose a scenario where only joints i and j of the articulated rigid bodies are in motion with constant angular velocities, while the other joints are xed. Then, the left hand side of equation(5.38) becomes:

Figure 5 . 6 :

 56 Figure 5.6: Backward recursion on moments and forces.

  2 shows a comparison of achieved results. The proposed recursive method is superior in various aspects, including in terms of execution time (4.6X times faster for a 5 DoF robot). The tests were carried out on a personal computer with Intel(R) Core(TM) i7-6850K CPU @ 3.6 GHz, under Windows 10, running MATLAB 2018a. For a 6 DoF robot, the script has been running for two months without nishing the symbolic equations generation (the automatic optimization of the generated equations is extremely time consuming).

Figure 5 . 7 :

 57 Figure 5.7: Minimum-time trajectory optimization for industrial manipulator performing palletization in exible-manufacturing scenario.

  continuously in joint space realTime_startDirectServoJoints Start the DirectServo for controlling the robot in joint space, position mode realTime_stopDirectServoJoints Stop the DirectServo for controlling the robot in joint space, destination positions and waits for an acknowledgment from the server sendJointsPositionsExTorque Updates the joints' destination positions while returns back the external torques sendJointsPositionsMTorque Updates the joints' destination positions while returns back the measured torques sendJointsPositionsGetActualJpos Updates the joints' destination positions while returns back the actual joint position measurements from encoders realTime_startDirectServoCartesian Start the DirectServo for controlling the robot in Cartesian space realTime_stopDirectServoCartesian Stop the DirectServo for controlling the robot in Cartesian space sendEEfPosition Updates the end-eector destination position and waits for an acknowledgment from the server sendEEfPositionExTorque Updates the end-eector destination position while returns back the external torques sendEEfPositionMTorque Updates the end-eector destination position while returns back the measured torques realTime_startVelControlJoints Start joints' velocity control mode realTime_stopVelControlJoints Stop joints' velocity control mode sendJointsVelocities Send reference velocities for robot joints sendJointsVelocitiesExTorques Updates the joints' destination velocities while returns back the external torques sendJointsVelocitiesMTorques Updates the joints' destination velocities while returns back the measured torques sendJointsVelocitiesGetActualJpos Updates the joints' destination velocities while returns back the actual joint position measurements from encoders sendJointsVelocitiesGetActualEEfPos Updates the joints' destination velocities while returns back the actual end-eector position Table 6.4: List of KST methods for point-to-point motion. Method Description movePTPJointSpace Moves from the current conguration to a new conguration in joint space movePTPLineEEF Moves the end-eector in a straight line from the current pose to a new pose movePTPHomeJointSpace Moves the robot to the home conguration movePTPTransportPositionJoi... Moves the robot to the transportation conguration movePTPLineEefRelBase Moves the end-eector in a straight line path relative to base frame movePTPLineEefRelEef Moves the end-eector in a straight line path relative to end-eector initial frame movePTPCirc1OrientationInter... Moves the end-eector in an arc specied by two frames movePTPArc_AC Moves the end-eector in an arc specied by center, normal, arc's radius and angleTo move the end-eector in an arc in the XY, XZ, YZ plane (movePTPArcXY_AC, movePTPArcXZ_AC, movePTPArcYZ_AC)

  blue LED of the pneumatic ange setBlueOn Turns on the blue LED of the pneumatic ange setPin1Off Sets the output of Pin1 to low level setPin1On Sets the output of Pin1 to high level To change the state of Pin2, Pin11 and Pin12 (setPin2Off, setPin2On, setPin11Off, setPin11On, setPin12Off, setPin12On) getEEF_Force Returns the measured force at the end-eector reference frame getEEF_Moment Returns the measured moments at the end-eector reference frame getEEFCartesianOrientation Returns the orientation (X-Y-Z xed rotations angles) in radians getEEFCartesianPosition Returns the position of the end-eector relative to robot base reference frame getEEFPos Returns the position and orientation of the end-eector relative to robot base reference frame getJointsExternalTorques Returns the robot joint torques due to external forces getJointsMeasuredTorques Returns the robot joint torques measured by the torque sensors getJointsPos Returns the robot joint angles in radians getMeasuredTorqueAtJoint Returns the measured torque at a specic joint getExternalTorqueAtJoint Returns the torque at a specic joint due to external forces getEEFOrientationR Returns the orientation of the end-eector as a rotation matrix getEEFOrientationQuat Returns the orientation of the end-eector as a quaternion getPin3State Returns the state of Pin3 Returns the state of Pin4, Pin10, Pin13 and Pin16 (getPin4State, getPin10State, getPin13State, getPin16State)

Figure 6 . 1 :

 61 Figure 6.1: Calibration phase rst step, x axis of the EEF is positioned vertically facing upwards (front view).

Figure 6 . 2 :

 62 Figure 6.2: Calibration phase second step, y axis of the EEF is positioned vertically facing upwards (upper view).

Chapitre 1 Introduction

 1 De nos jours, les robots sont la pierre angulaire des usines modernes. Leur implication dans l'espace de travail contribue à augmenter les capacités de production, la exibilité et la précision. Ils sont largement utilisés dans la fabrication pour des applications de "pick and place", la peinture, la soudure, la transformation des métaux, et même dans l'industrie alimentaire. En raison de leur précision, ils deviennent également des outils précieux pour eectuer des opérations précises dans l'industrie aérospatiale. Dans le cadre d'Industry 4.0, la présence de robots dans des environnements centrés sur l'homme est d'une importance vitale, car elle apporte au sol de l'usine le meilleur des deux, la capacité cognitive des humains et les qualités spéciales des robots, notamment leur précision, leur force et leur répétitivité. Une telle approche rendra les lignes de production plus productives et plus exibles. Mais, pour des raisons de sécurité, les robots industriels sont actuellement séparés des humains derrière des clôtures. En outre, ils sont encore diciles à programmer, même pour des utilisateurs expérimentés. Ces inconvénients constituent un obstacle à la réalisation eective de la coopération et de l'interaction entre l'homme et le robot dans l'usine.Dans un tel cas, il est nécessaire de disposer de robots plus sûrs, dotés de meilleures capacités de collaboration, plus faciles à programmer sans nécessiter de grandes connaissances techniques. Qui ont la capacité d'éviter le danger et d'assurer la sécurité du collègue humain lors de l'interaction mutuelle.1.1 MotivationÀ la lumière de l'introduction précédente, l'étude de recherche proposée aborde les questions de sécurité et de collaboration pour les manipulateurs industriels. En ce qui concerne la sécurité, l'étude se penche sur la question de l'évitement des collisions pour les manipulateurs robotiques dans un espace de travail partagé entre l'homme et le robot. En ce qui concerne la collaboration, l'étude explore le sujet du guidage manuel de précision pour les manipulateurs robotiques. Dans les deux cas, l'objectif de l'étude est d'appliquer les résultats acquis dans l'industrie. Les deux sous-sections suivantes traitent de l'importance des fonctionnalités d'évitement des collisions et de guidage manuel, et exposent les arguments sur la nécessité d'eectuer des recherches sur ces sujets.

Figure 6 . 3 :

 63 Figure 6.3: KUKA iiwa robot travaillant côte à côte avec un collègue humain dans l'industrie aérospatiale, (Projet ColRobot [3]).

1. 1 . 1

 11 Évitement de collision En dotant les robots de capacités motrices avancées et de réexes semblables à ceux des humains, on leur permettra de contourner les obstacles et d'éviter les collisions avec l'environnement dynamique qui les entoure. Ceci est extrêmement important pour donner aux robots une plus grande autonomie et un besoin minimal d'intervention humaine, en particulier lorsque les robots fonctionnent dans un espace de travail et un environnement dynamique changeants. En ce qui concerne la nouvelle révolution industrielle, l'industrie 4.0, l'évitement des collisions est d'une importance vitale, où il est prévu que les humains et les robots travaillent ensemble et collaborent, une quête qui ne peut être réalisée sans doter le robot de capacités d'évitement des collisions. Habituellement, la structure des robots industriels est faite de métaux (qui conservent une immense inertie). Pour des raisons de productivité, ils doivent également se déplacer à grande vitesse, des conditions qui rendent dangereux pour les humains de travailler côte à côte avec un robot se déplaçant à l'aveuglette sur une trajectoire préprogrammée. C'est pourquoi il est courant d'avoir des limites et des barrières de sécurité entre les humains et les robots industriels. En ce qui concerne les usines du futur, l'idée est d'éliminer ces frontières et de faire en sorte que les humains et les robots partagent le même espace de travail, Figure 6.3. Dans un tel scénario, le système de commande du robot doit garantir la sécurité du collaborateur humain en toutes circonstances. Un autre exemple sur l'importance des algorithmes d'évitement des collisions est celui des robots qui doivent travailler dans des environnements dangereux. Par exemple, lorsqu'ils eectuent des réparations (commandées par un robot) à l'intérieur de réacteurs nucléaires ou lorsqu'ils travaillent dans des environnements potentiellement explosifs. Figure 6.4 démontre un tel concept, où un bras robotique actionne une vanne

Figure 6 . 4 :

 64 Figure 6.4: Un robot fonctionnant dans un environnement potentiellement explosif, parmi des tuyaux transportant du gaz [4].

Figure 6

 6 Figure 6.5: Carte Morphin du rover Curiosity Mars, gracieuseté du projet d'autonomie de Mars.

Figure 6 . 6 :

 66 Figure 6.6: Le rover Curiosity sur Mars (NASA).

6 . 26 )

 626 Où I est la matrice d'identité (n × n). A partir de (6.25) on peut remarquer que les manipulateurs redondants permettent un mouvement interne où la pose (position/orientation) du EEF est maintenue xe, dans un tel cas ẋ = 0, et (6.25) devient: qnull = N qc(6.27)

Figure 6 . 8 :Figure 6 . 8

 6868 Figure 6.8: Concept de champ potentiel articiel.

Figure 6 . 9 :

 69 Figure 6.9: (A) L'Uomo Vitruviano représenté par 10 capsules, (B) humain représenté par 1 capsule, (C) humain représenté par 3 capsules, (D) humain représenté par 5 capsules, (E) robot représenté par 2 capsules, (F) robot représenté par 3 capsules, (G) la main et l'avant-bras humains sont représentés par 21 capsules.

Figure 6 .

 6 Figure 6.10: (a) IMU attachées à la partie supérieure du corps, (b) un humain (représenté par 5 capsules) debout dans le champ de balayage d'un scanner laser et (c) un robot représenté par 3 capsules.

Figure 6 . 11 :

 611 Figure 6.11: Proposition de cadre pour la prévention des collisions homme-robot en temps réel.

Figure 6 .

 6 Figure 6.12: L'algorithme d'évitement des collisions développé est mis en ÷uvre dans une cellule robotique collaborative pour l'assemblage des cartes de portes de voitures(une vidéo montrant l'expérience est en[6]).

Figure 6 . 14 :

 614 Figure 6.14: Architecture de la ToolBox.

Figure 6 . 16 :

 616 Figure 6.16: Enseignement d'une opération à KUKA iiwa à l'aide d'une application de programmation par démonstration basée sur le guidage manuel de précision. Une simulation en réalité virtuelle de la cellule robotique de Vrep est utilisée pour montrer au collaborateur un retour d'information sur la trajectoire programmée pendant le processus d'enseignement, ce qui permet à l'utilisateur de visualiser, de modier et de vérier la trajectoire enseignée tout en eectuant l'opération.

  de fabrication robotisées. Par analogie avec les propriétés de mouvement d'un système mécanique passif, masse soumise à un frottement Coulomb/visqueux, un schéma de contrôle est proposé, qui régit le mouvement linéaire/angulaire de la EEF découplée. Pour éviter de violer les limites physiques des articulations, un facteur d'échelle est introduit, il est utilisé pour réduire la vitesse linéaire/angulaire de la EEF près des limites des articulations. Pour tester l'approche proposée, des expériences ont été menées dans des tâches d'assemblage assistées par robot nécessitant de la précision, où un robot KUKA iiwa est utilisé. Les performances ont été évaluées en tenant compte de la précision du positionnement, de la capacité à éviter les limites des articulations et de la douceur du mouvement pendant le guidage manuel. En outre, une comparaison qualitative et quantitative de la stratégie de contrôle proposée par rapport au guidage manuel KUKA iiwa disponible dans le commerce est présentée. Les résultats indiquent que la solution proposée permet un mouvement de guidage manuel cartésien en douceur, avec un faible niveau de vibrations, tout en ayant la capacité de réagir pour éviter les limites des articulations.

Figure 6 . 17 :

 617 Figure 6.17: Guidage manuel de précision pour une opération d'assemblage précise.

Figure 6 . 18 :Figure 6 . 19 :

 618619 Figure 6.18: Accélérations tangentielles, normales et de Coriolis du centre de masse k.

Figure 6 . 20 : 6

 6206 Figure 6.20: Récursion à rebours sur les moments et les forces pour le calcul des symboles de Christoel.

  6.13 Ligne de temps pendant les simulations du manipulateur planaire de 15 DoF contrôlé par la méthode de Newton, la descente en gradient et la descente en gradient avec élan. . . . . . . . . . . . . . . . . . . . . . . . .15 Guidage manuel de précision sur une ligne droite (le long de l'axe des y) d'un robot redondant soumis à un contact avec un obstacle. Un capteur FT externe est xé à la bride du robot. . . . . . . . . . . . . . . . . . . . Minimum distance calculation. . . . . . . . . . . . . . . . . . . . . . . . Integral term of the attraction vector. . . . . . . . . . . . . . . . . . . .

	6.16 Enseignement d'une opération à KUKA iiwa à l'aide d'une application
	de programmation par démonstration basée sur le guidage manuel de
	précision. Une simulation en réalité virtuelle de la cellule robotique de Vrep est utilisée pour montrer au collaborateur un retour d'information List of Algorithms
	sur la trajectoire programmée pendant le processus d'enseignement, ce
	qui permet à l'utilisateur de visualiser, de modier et de vérier la tra-
	jectoire enseignée tout en eectuant l'opération. . . . . . . . . . . . . . .
	6.17 Guidage manuel de précision pour une opération d'assemblage précise. . 3.1
	6.18 Accélérations tangentielles, normales et de Coriolis du centre de masse k. 3.2 Modied repulsion vector (magnitude). . . . . . . . . . . . . . . . . . . .
	3.3

6.14 Architecture de la ToolBox. . . . . . . . . . . . . . . . . . . . . . . . . . 66.19 Résultats du temps d'exécution pour la méthode proposée et le Composite Rigid Body Algorithm par Featherstone. . . . . . . . . . . . . . . 6.20 Récursion à rebours sur les moments et les forces pour le calcul des symboles de Christoel. . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4 Collision avoidance -collaborative robotic cell in automotive industry (reads together with Figure 3.22) . . . . . . . . . . . . . . . . . . . . . . 4.1 Precision hand-guiding -control algorithm. . . . . . . . . . . . . . . . . 5.1 Calculating joint space inertia matrix entries, algorithm is based on eq (5.31). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Chapter 1 Introduction Nowadays, industrial robots are cornerstones in modern factories. Their involvement in workspace contributes to higher production capabilities, exibility and accuracy. They are used extensively in manufacturing for pick and place applications, painting, welding,

Table 1 .

 1 1: List of main publications Proc. of the 45th Annual Conference of the IEEE Industrial Electronics.

	Book chapter
	(accepted)
	Book chapter
	(accepted)
	Book chapter
	[18]
	Book chapter
	[19]
	Book chapter [20]	M. Safeea, P. Neto and R. Béarée: The Third Hand, Cobots Assisted Precise Assembly. Part of the Lecture Notes in Computer Science book series (LNCS, volume 11650).
	Book chapter
	[21]

M.

Safeea

, P. Neto and R. Béarée: Task execution combined with in-contact obstacle navigation by exploiting torque feedback of sensitive robots. Procedia Manufacturing. M. Safeea, P. Neto and R. Béarée: Model-based hardware in the loop control of collaborative robots. Procedia Manufacturing. M. Safeea, N. Mendes, and P. Neto: Minimum distance calculation for safe human robot interaction. Procedia Manufacturing. M. Safeea, P. Neto and R. Béarée: A Geometric Dynamics Algorithm for Serially Linked Robots. Part of the Lecture Notes in Computer Science book series (LNCS, volume 11649). M. Safeea, P. Neto and R. Béarée: A Quest Towards Safe Human Robot Collaboration. Part of the Lecture Notes in Computer Science book series (LNCS, volume 11650).

Table 1 .

 1

		2: List of coauthored publications
		Procedia Manufacturing.
	Book chapter	N. Mendes, J. Ferrer, J. Vitorino, M. Safeea and P. Neto: Human
	[25]	behavior and hand gesture classication for smart human-robot interaction. Procedia Manufacturing.
		Table 1.3: List of developed software packages
	Package	Description
	KST
	[26]

  and the proposed QR method.

	Method		Route	Computational complexity
	Method in [5]	Least expensive	56 2 n 2 -56 2 n
	Method in [5]	Most expensive	66 2 n 2 -66 2 n
	Proposed QR method	Least expensive	29 2 n 2 -5 2 n
	Proposed QR method	Most expensive	52 2 n 2 -28 2 n
	Method in [5] modied	Least expensive	40 2 n 2 -24 2 n
	Method in [5] modied	Most expensive	50 2 n 2 -34 2 n
	of the operation count in Appendix II (page 36). Considering the third comparison
	criteria, numerical precision, a group of 5000 line-segments/capsules has been generated
	randomly in 3D space. The (x, y, z) coordinates of the start and end point of each
	segment are in the range [-100, 100]. The proposed QR method and the method in
	[5] are used to calculate the minimum distance between each couple of segments from
	the set. For each couple of segments, the relative error is dened as the ratio of the
	absolute value of dierence between calculations using the two methods:
		e =	2|d qr min -d s min | d qr min min + d s	(3.10)

Table 3 .

 3

	2: Test 1: Minimum reach-distance between last link of the manipulator and
	obstacles.
	Method	Minimum distance (m)
	Gradient descent	0.0102
	Newton	0.0480

Table 3 .

 3 3: Test 2: Convergence rate for three dierent optimization methods as applied to collision avoidance and path planning problems for robotic manipulators.

	Method	Iterations
	Newton	55
	Fixed step gradient descent	18073
	gradient descent with momentum	7928

Table 5 .

 5 1: Operation count for proposed method and other methods, mul stands for multiplication and add for addition.

	Method	Matrix	Cost	Reference
	GDAHJ	JSIM	(3n 2 + 88n -3)mul + (2.5n 2 + 95.5n -18)add
	CRBA	JSIM	(10n 2 + 22n -32)mul + (6n 2 + 37n -43)add	[161] and
				[160] EQ.
				10.3
	Symbolic-	JSIM-	( 3 2 n 3 + 35 2 n 2 + 9n -16)mul + ( 7 6 n 3 + 23 2 n 2 + 64 3 n -28)add
	Numeric	Coriolis

Table 5 .

 5 • f k ji is half of the inertial force at the center of mass of link k due to Γ cor Ckj . It is due to Coriolis eect resulting from a unit angular velocity at joints j and i.

	2: Comparison for calculating Christoel symbols of a 5 DoF serially linked
	robot using dierent methods
	Criteria	Lagrangian (Optimized) (Not optimized) method Lagrangian Proposed
	Size of generated le (bytes)	778 732	67 873 609	4 497
	O-line time for function generation	8 days	897 sec	-
	On-line execution time (seconds)	4.6e-04	96	9.9e-5

Table 5 .

 5 3: Computational complexity of the proposed method

	Additions	Multiplications
	12n 3 + 19n 2 + 40n -1 21 2 n 3 + 45 2 n 2 + 49n
	1. The proposed algorithm, implemented in the le christoffelNumerically.m
	(MATALB function).

Table 6 .

 6 3: List of KST methods for soft real-time control.

Table 6 .

 6 5: List of KST methods, setters and getters.

  Dans la littérature robotique, le problème de l'évitement des collisions est traité à l'un des deux niveaux suivants : le niveau global, traité par la planication, ou le niveau local traité par le contrôle de bas niveau. Les solutions globales sont des solutions de haut niveau qui garantissent de trouver un chemin sans collision de la conguration initiale à la conguration nale, si un tel chemin existe. Ces algorithmes traitent le problème dans l'espace de conguration. Dans ce cas, le manipulateur et l'environnement doivent être recalés dans l'espace de conguration et un chemin sans collision est recherché dans la partie inoccupée de l'espace de conguration. Toutefois, ces algorithmes sont très coûteux en termes de calcul, de sorte que leur utilisation n'est pas possible pour les applications dynamiques en temps réel. D'autre part, la commande réactive locale convient à une mise en ÷uvre en temps réel, car sa formulation mathématique est ecace sur le plan du calcul, de sorte qu'elle peut être intégrée directement dans la commande de bas niveau.

	des cellules robotisées industrielles, permettant au robot d'ajuster les trajectoires
	générées hors ligne de la tâche industrielle. La méthode proposée introduit égale-
	ment une nouvelle solution qui permet au robot de passer d'un mouvement réactif
	d'évitement des collisions à la tâche en cours tout en maintenant un mouvement
	continu et régulier.
	4. Développement d'un nouvel algorithme pour générer un mouvement réactif d'évitement
	des collisions (pour les manipulateurs robotiques), où la méthode Newton (issue
	de l'optimisation) est appliquée sur la fonction de champ potentiel pour calculer
	la direction de minimisation. Pour obtenir une exécution ecace, on déduit égale-
	ment la relation entre le Hessien (de la fonction potentielle) décrit dans l'espace
	articulaire et le Hessien dans l'espace cartésien.
	2. Contrairement au guidage manuel, lorsque l'utilisateur utilise le Teach-Pendant,
	il ne ressent pas la force appliquée entre l'End-Eector et son environnement
	en cas de contact. Des accidents pourraient survenir et l'utilisateur pourrait
	presser l'eecteur contre son environnement (qui pourrait inclure des instruments
	sensibles) sans s'en rendre compte.
	Outre la sécurité, les capacités d'évitement des collisions confèrent aux robots une
	plus grande autonomie. Cela est particulièrement important lorsqu'ils opèrent dans 3. La convention du Teach-Pendant pour décrire l'orientation est l'angle de rota-
	des endroits éloignés et dans des environnements inconnus. Le rover Curiosity Mars, tion d'Euler. Bien qu'elle convienne bien aux ordinateurs, cette façon de décrire
	Figure 6.6. Le rover travaille loin de la terre, ce qui rend impossible le contrôle de l'orientation n'est pas intuitive pour les humains. Même le roboticien expéri-
	l'actif en temps réel. En fait, la distance entre les deux planètes est immense, de sorte menté convient que, dans le cas général, il est dicile pour un humain d'imaginer
	que le temps nécessaire pour envoyer un signal à Mars et le recevoir de retour sur terre l'orientation d'un objet à partir de trois nombres (angles d'Euler).
	pourrait être de 4 à 24 minutes (selon la position relative des deux planètes pendant
	leur rotation autour du soleil). Pour maintenir le rover robotique loin des obstacles, un En raison des inconvénients précédents, cette thèse propose une méthode plus intuitive
	algorithme d'évitement des collisions appelé Morphin a donc été développé, et est utilisé pour eectuer une opération de positionnement précis de l'End-Eector par guidage
	manuel.

• C. Dans de tels endroits, les conditions de travail pour les humains sont irréalisables. Une étude axée sur ce type de situation a été proposée dans

[START_REF] Berg | Industrial robot collision handling in harsh environments[END_REF]

, où le travail de l'auteur s'est concentré sur l'évitement des collisions pour les robots industriels dans l'industrie du gaz et du pétrole.

pour guider le rover à travers les terrains de Mars. Cet algorithme maintient une carte de l'environnement, Figure

6

.5, et sur la base de cette carte, Morphin recommande au rover des commandes de direction sûres. 1.1.2 Guidage manuel Le guidage manuel est une fonctionnalité représentative des robots collaboratifs. Elle permet à des utilisateurs non qualiés d'interagir et de programmer rapidement un robot de manière intuitive. De nombreuses applications industrielles nécessitent un positionnement précis de l'End-Eector (EEF) pendant le processus d'enseignement. Normalement, le Teach-Pendant est utilisé pour eectuer cette tâche. Cependant, l'utilisation du Teach-Pendant n'est pas toujours pratique en raison des inconvénients suivants: 1. Lorsque l'utilisateur utilise le Teach-Pendant pour positionner l'End-Eector dans l'espace cartésien, il doit suivre l'orientation du socle du robot, ce qui peut être déroutant même pour les utilisateurs expérimentés. Sans compter que dans certains cas, le manipulateur est monté sur une plate-forme mobile, ce qui rend la tâche de suivre l'orientation de la base du robot encore plus dicile.

Table 6 .

 6 un collègue humain et un robot industriel (KUKA iiwa) partagent le même espace de travail et les mêmes tâches. Dans un tel cas, le robot est capable de détecter la présence de l'homme autour de lui et réagit à la volée pour éviter les collisions tout en gardant la cible de la tâche le plus possible. Enn, nous proposons de coupler la méthode de Newton avec l'algorithme d'évitement des collisions des champs potentiels dans un cadre d'optimisation du second ordre. Une formule mathématique symbolique est déduite pour calculer ecacement le Hessien et le gradient du champ potentiel dans l'espace articulaire du robot. Les avantages de l'utilisation d'une telle solution pour l'évitement des collisions sont démontrés par divers tests. Le chapitre quatre explore le sujet du guidage manuel tout en préservant la précision au niveau de l'End-Eector. Tout d'abord, la principale méthode pour réaliser un guidage manuel de précision est présentée, la méthode est déduite mathématiquement et testée sur le robot KUKA iiwa. Ensuite, le guidage manuel de précision (au niveau de l'EEF) et le guidage manuel au niveau des articulations sont comparés en termes de précision et de niveau de vibrations. Enn, en appliquant la fusion des capteurs aux mesures de force/couple d'un capteur de force/couple externe xé à l'End-Eector et aux mesures de couple des capteurs de couple intégrés aux articulations, nous proposons une méthode qui permet à l'utilisateur de guider la main du robot avec précision, au 7: Liste des principales publications Assembly. Part of the Lecture Notes in Computer Science book series (LNCS, volume 11650).

	1. Développement d'un algorithme à un coût O(n 2 ) minimal 1 pour calculer ecace-
	ment la matrice d'inertie de l'espace articulaire pour les robots reliés en série avec
	des degrés de liberté élevés.
	2. Développement d'un algorithme récursif léger (non symbolique) pour le calcul
	des symboles de Christoel des manipulateurs robotiques.
	1.3 Aperçu de la thèse
	La thèse est divisée en plusieurs chapitres et sections. Le chapitre deux donne le con-
	texte, il énumère également les principes de base nécessaires à la modélisation mathé-
	matique des robots, qui seront tous utilisés tout au long de ce document.
	Le troisième chapitre est consacré à la prévention des collisions entre les humains
	et les manipulateurs industriels. Nous commençons ce chapitre en énumérant les dif-
	férentes méthodes utilisées dans la littérature pour représenter l'homme et le robot.
	Pour cette thèse, nous choisissons la représentation géométrique primitive à l'aide de
	capsules. Ensuite, nous dérivons un algorithme ecace pour calculer la distance min-
	imale entre ces capsules. Ensuite, une méthode basée sur la fusion de capteurs est
	présentée pour capturer la conguration de l'humain autour du robot. Cette méthode

est ensuite mise en ÷uvre dans un algorithme d'évitement des collisions dérivé de la méthode des champs potentiels et testé sur une véritable cellule robotique industrielle,

1

Où n est le nombre de degrés de liberté du robot. où niveau de l'End-Eector, tout en naviguant sur des obstacles dans l'espace nul lors d'un contact. Le chapitre cinq traite de la formulation des équations de la dynamique des robots. Deux algorithmes ecaces sont proposés. Le premier algorithme permet de calculer la matrice d'inertie de l'espace commun des corps rigides reliés en série. Il s'agit d'un algorithme ecace avec un coût minimal du second ordre. Le second algorithme décrit une méthode récursive pour calculer ecacement les symboles de Christoel des manipulateurs robotiques. Les algorithmes proposés ont été testés par rapport à des méthodes de pointe. Les résultats ont été présentés en termes de nombre d'opérations, M. Safeea, P. Neto and R. Béarée: A Quest Towards Safe Human Robot Collaboration. Part of the Lecture Notes in Computer Science book series (LNCS, volume 11650).

Table 6 .

 6 8: Liste des publications des co-auteurs Chapitre 2ContexteCe chapitre passe en revue les bases qui sont importantes pour la mise en ÷uvre des méthodologies proposées dans cette thèse. Section 1.1 donne un aperçu de la cinématique des manipulateurs robotiques, des matrices de transformation et de la formule du jacobien.. Section 1.2 donne un aperçu du pseudo inverse de la méthode jacobienne et de la méthode des Damped Least Squares (DLS). Section 1.3 donne un aperçu de la matrice de projection de l'espace nul pour les manipulateurs redondants. Section 1.4 donne une brève introduction sur les champs de potentiel articiel.

A complete elaboration (including C++ implementation) on applying the proposed method eciently for a set of capsules is available in the le attached to the Media materials of[12].

In w ref imu the superscript ref stands for reference frame, and the subscript imu stands for the frame of the IMU.

This application was developed during the European project Colrbot[3] (Horizon

2020) for the use in satellite assembly operations.

Appendix C: Gravity compensation of IMU measurements . . . . . . . . . . .

Résumé en français

Test 2

Test 2 is performed to asses the convergence rate for Newton method against the xed step gradient descent and the gradient descent with momentum [START_REF] Ning Qian | On the momentum term in gradient descent learning algorithms[END_REF] as applied to the problem of collision avoidance and path planning. The three methods are applied in simulation environment, in all cases the simulation scenes are identical. The distance between the EEF and the goal position is used as the convergence criteria, when the EEF is closer than 0.02 meter from the goal position, the algorithm is terminated and the iterations number is reported. In all three methods the step size α is taken as big as possible, given that the algorithm does not cause the robot to overshoot through any of the obstacles. From the tests the Newton method achieved the best convergence per iteration, followed by the momentum and lastly the xed step gradient method. The convergence rates achieved are as shown in Table 3.3. The dierences in the resulting conguration are also evident as demonstrated in Figure 6.13.

KUKA Sunrise Toolbox

During the author's work on the collision avoidance system, a software-library (Toolbox) for controlling KUKA iiwa collaborative robots from an external computer using MATLAB is created, this was motivated by various reasons (discussed in the following subsection).

For the purpose of readability, this section is organised into various subsections.

After the Motivation, the Introduction subsection gives a survey on the most popular robotic software packages available in the research community. The Architecture subsection gives details on the building blocks of the proposed Toolbox. Finally, the Modus Operandi subsection lists with examples the various methods oered by the Toolbox and their use.

Motivation

Given that one of the objectives of this thesis is to implement the collision avoidance algorithm on a real collaborative robot (KUKA iiwa), extensive time and eort was dedicated for studying the Sunrise.OS 3 . Consequently, various operation modes were tested. During these tests, the author faced various challanges, including:

1. Sensors integration: while the sensors used in the collision avoidance system are easily integrated into personal computers, they are not easily integrated into the robot's controller.

2. Simulation integration: to write programs for KUKA iiwa robots, the Sunrise.Workbench IDE (Integrated Development Environment) is used. Unfortunately, the Sunrise.Workbench does not include 3D simulation capability, which is very important during the development process of the control algorithm (as explained later).

3. Implementing advanced mathematical operations: the Sunrise.Workbench supports Java programming language, which is a powerful language. But the collision avoidance algorithm includes lots of advanced mathematical operations (matrix operations, ltering, integration among others). Consequently, developing the algorithm in Java will require considerable amount of time.

4. Hardware limitations: due to the limitation of the performance of the robot controller, the complexity of the developed algorithms and their performance is limited by the robot's hardware capacity.

Faced with the previous limitations, the author opted to develop the collision avoidance control algorithm on an external computer using MATLAB, which oers the following advantages:

1. MATLAB is suitable for developing the collision avoidance algorithm (mathematically demanding), given its support of advanced mathematics and the existence of powerful toolboxes including the control toolbox and the signal processing toolbox. 3 The operating system of the KUKA iiwa.

Physical interaction

KST provides several physical interaction functionalities, some of which are the handguiding mode and the touch detection. The hand-guiding is activated using: > > iiwa.startHandGuiding();

Once called the hand-guiding functionality is initiated. To perform hand-guiding operation, the user shall press the ange's white button to deactivate the brakes and move the robot. When the white button is released the robot stops in its current conguration. To terminate the hand-guiding mode, the green button should be pressed continuously for more than 1.5 seconds (after 1.5 seconds of pressing the green button the blue LED light starts to icker), release the green button and the hand-guiding mode is terminated.

Summary

According to users' feedback (students, researchers, and industry engineers), the proposed toolbox is a useful and intuitive tool to interface with KUKA Sunrise.OS and, in particular, to speed up the development and implementation of robot applications.

KST functionalities are advantageous for the implementation of advanced robot applications. KST also facilitates integration of external hardware, data processing, and implementation of complex algorithms using existing toolboxes.

Chapter 4

Hand-guiding

The hand-guiding functionality represents an intuitive human-robot interface which allows human coworkers to move and teach the robot easily, without requiring advanced technical skills on how to program industrial manipulators. This functionality also allows to use the robot as an assistive third hand, which is appealing for tedious assembly tasks. Consequently, a robot can be hand-guided to lift and hold parts in place, while giving the human coworker an opportunity to apply the xtures (perform the assembly task). Such functionality reduces the risk to workers and properties (falling components), provides precision, allows lifting heavier parts, and increases productivity by keeping less workers occupied in manual tasks on the factory oor.

Chapter's breakdown

This chapter of the document is organised in two main sections:

• Section 4.1 describes the proposed algorithm for performing precise hand-guiding at the EEF level. This method is evaluated on 7 DoF KUKA iiwa robot.

• Section 4.2 builds upon the ideas presented in section 4.1, where a method is developed that allows hand-guiding sensitive redundant manipulators (at EEF level) in cluttered environments with obstacles. In such a case, the redundancy of the robot is used to navigate obstacles during the contact while preserving the precision of the motion at the EEF in Cartesian space.

Algorithm 4.1 Precision hand-guiding -control algorithm.

Input : q ini joints positions feedback from robot ẋ calculated EEF velocity (4.17)

α scaling factor for joints limits avoidance t time interval for loop update cycle ε vector of acceptable Cartesian errors Output : q goal joitns goal positions 01 : xini = f (q ini ) % F orward kinematics 02 : xg = xini + α ẋ t 03 : x = xini 04 : q = q ini 05 : e = (xg -x) % Calculate error 06 : while (norm(e) > ε) % Loop until error is small enough 07 :

J † = φ(q) % P seudo inverse of the Jacobian 08 : q = q + J † e % Joints positions update 09 :

x = f (q) % F orward kinematics 10 : e = (xg -x) % Calculate error 11 : Loop 12 : q goal = q joint position servo control feature. In this scenario, from equation (4.20), we can deduce a joint level position control according to the following procedure:

1. For each update cycle of the control loop the joint positions q ini are acquired from the robot encoders; 2. At each update cycle, a feedback from the FT sensor is also obtained, from which the command velocity ẋ of the EEF is calculated as described previously;

3. Using the above information the desired joint positions are calculated and afterwards updated to the position control loop of the robot controller. In this study, the Algorithm 4.1 is used to calculate the destination joint positions, which will be updated to the position control loop, provided by the robot manufacturer.

Such update is performed at each control cycle. Algorithm 4.1 details the methodology to achieve a high level of numerical precision, although the nal precision of robot motion will depend on the precision performance of the real-time position control loop of the robot being used.

Referencing motion to the EEF

Using the presented framework the operator can only translate the EEF along the x, y or z axis of the base frame of the robot, one at a time. But in some situations the operator may require to perform an inclined linear motion of the EEF, to solve this, another operation mode is proposed, in this operation mode the operator can reference the motion to the EEF frame, the operator can enter this operation mode by pressing an external button on the robot while the EEF is static.

In this operation mode, the force command is referenced to the end-eector, as such the robot moves in the direction of the maximum component of the hand-guiding

Summary

A novel precision hand-guiding method at the end-eector level for collaborative robots is proposed. Experimental tests demonstrated that with the proposed method it is possible to hand-guide the robot with accuracy, with lower level of vibrations, and Thereby, the method is applicable for any sensitive robot possessing redundancy with respect to its required task.

Joint torques

Once a contact is initiated with the obstacle, extra torques due to contact forces τ ex start to appear in the joints:

Where τ ex is the vector of external torque due to external forces, τ r is the raw torque measurements from the sensors at robot joints, and τ dyn are the torques due to robot's own motion and gravity. The latter includes the torques due to (1) joints angular acceleration, (2) Coriolis/centrifugal eect, (3) friction at joints and (4) torques due to gravity, it is given by the equation:

Where A is the mass matrix of the robot, q is the vector of joints angular acceleration, C is Coriolis matrix of the robot, q is the vector of joints angular velocities, f (q, q) are the torques due to friction in the joints, and g are the torques due to gravitational eect. In such a case, the accuracy in calculating τ dyn depends mainly on the precision of the dynamical model used to describe the manipulator's dynamics.

When it comes to KUKA iiwa robots, the Sunrise Controller calculates internally the vector of the external torques, allowing the user to acquire the numerical value of Concerning the normal acceleration, each frame j transfers to link k a normal acceleration due to its rotation, Figure 5.2:

Where ω j is the angular velocity of link j due to the rotational eect of joint j. It is given by:

We can rewrite the equation of the normal acceleration transferred to link k due to frame j by:

The third acceleration transferred is Coriolis acceleration, Figure 5.2, in which each frame j transfers to center of mass of link k Coriolis acceleration Γ cor Ckj :

Where v r Ckj is the velocity transferred to the center of mass of link k from frames j + 1 up to frame k. The superscript r is used to denote that this is a relative velocity and C to refer to the center of mass of link k, so that v r Ckj can be calculated from:

The total linear acceleration transferred by frame j to the center of mass of link k is given by:

5.1.2.2 Link's inertial moment due to single-frame eect Each frame j transfers to link k three inertial moments, the rst of which is due to angular acceleration of frame j:

Where µ τ Ckj is the moment transferred by frame j into link k due to frame's j angular acceleration, R k is the rotation matrix of frame k in relation to base frame and I k k is 3 × 3 inertial tensor of link k around its center of mass represented in frame k.

The second inertial moment transferred from frame j to link k is due to centrifugal eect:

Where L k is a 3 × 3 matrix that is calculated from:

Contribution to Christoel Symbols Calculation

Christoel symbols of the rst kind are very important in robot dynamics. They are used for tuning various proposed robot controllers, for determining the bounds on Coriolis/Centrifugal matrix, for mathematical formulation of optimal trajectory calculation, among others. In the literature of robot dynamics, Christoel symbols of the rst kind are calculated from Lagrangian dynamics using an oine generated symbolic formula. In this study we present a novel and ecient recursive, non-symbolic, method where Christoel symbols of the rst kind are calculated on-the-y based on the inertial parameters of robot's links and their transformation matrices. The proposed method was analyzed in terms of execution time, computational complexity and numerical error. Results show that the proposed algorithm compares favorably with existing methods.

Introduction

Christoel symbols are important tools in applied sciences, engineering, mathematics and physics. In the latter they appear in rigid body dynamics [START_REF] Sciavicco | Lagrange and newtoneuler dynamic modeling of a gear-driven robot manipulator with inclusion of motor inertia eects[END_REF] and general relativity [START_REF] Charles W Misner | Gravitation[END_REF]. In the area of robotics, Christoel symbols of the rst kind appear when deducing the equation of robot dynamics using the Lagrangian:

Where L is the Lagrangian function, T is the kinetic energy and U is the potential energy, all described in terms of the generalised coordinates q. In such a case, the associated generalised forces τ :

Consequently, the canonical form of the inverse dynamics derived using the Lagrangian is:

Where τ k is the torque at joint k, a kj is the (k, j) element of the mass matrix, qj is the angular acceleration of joint j, qj is the angular velocity of joint j, g k is the torque at joint k due to gravity, and c k ji represents Christoel symbols of the rst kind (later in the document referred to by Christoel symbols). From [START_REF] Liu | A survey of underactuated mechanical systems[END_REF] c k ji is given by:

The Lagrangian formulation can be considered as a straight forward approach.

However, the method requires partial dierentiation, which makes it unpractical to apply manually for complex systems. Thus, symbolic manipulation methods have been utilized to perform the dierentiation by a computer [START_REF] Charles | Symbolically ecient formulations for computational robot dynamics[END_REF]. Nevertheless, the resulting equations generated by a computer lack the eciency in terms of execution time, the potential elds method on both the mobile platform and the robotic manipulator.

In addition, it is noticed that in the proposed collision avoidance method the subject of redundancy was addressed implicitly by using the pseudo inverse of the Jacobian of the redundant manipulator when calculating the angular velocities for both the attraction and repulsion vectors. On the other hand, addressing redundancy explicitly (by performing null space projection) oers extra advantages, which for some situations allow collision avoidance motion while performing the required task successfully. For the precision hand-guiding method, we noticed that when the redundant axis is not xed, a repetitive hand-guiding motion at the EEF does not result in a repetitive motion in the joint space of the robot. This causes a drift in the robot conguration (mainly the redundant axis). While it is easily corrected by readjusting the redundant axis in the null space when required, this solution is not convenient. Therefore, future work will focus on solving this issue. On the subject of robot dynamics, future work will be focused on improving the eciency of the proposed method for calculating Christoel symbols and on reducing the O(n) operations of GDAHJ method for calculating the mass matrix.

Appendix A Hessian & Gradient in Joints Space

In this section, the formulas for the rst and second order derivatives, gradient Equation (3.39) and Hessian Equation (3.38), of the potential eld with respect to manipulator's joint angles are deduced. Let u k (x) be a potential eld, this potential eld is described in Cartesian space, it could be an attraction potential, that attracts the EEF to the goal, or a repulsion potential that repels the robot away from obstacles.

Then the second order approximation of the potential eld u k (x 0 ) near the current position x 0 is given by:

Where:

• u k (x) is the approximate value of the potential function at the Cartesian position x;

• u k (x 0 ) is the value of the potential function at the Cartesian position x 0 ;

• x = xx 0 ;

• ∇u k is the gradient of the potential function u k with respect to x taken at the point x 0 ;

• ∇ 2 u k is the Hessian of the potential function u k with respect to x taken at the point x 0 .

In addition, dierential kinematics gives the following relationship between elemental displacements in Cartesian space and elemental displacements in joint space:

Where J is the Jacobian associated with the point of the manipulator x 0 , and q = qq 0 . By substituting (6.2) in (6.1) and xing:

From the previous equation, the joint space gradient g k and the joint space Hessian H k of an individual potential function u k are deduced. Where the gradient is associated with the rst order term of the approximation:

The Hessian is associated with the second order term of the approximation:

Finally, in case of a manipulator subjected to several potential elds simultaneously, the total potential eld is the sum of all of the potentials, and the total gradient is the sum of the individual gradients:

The total Hessian is the sum of the individual Hessians:

List of Methods for KST

Principales Contributions

La thèse contribue au domaine de la robotique collaborative sous divers aspects, notamment les sujets de l'évitement des collisions pour les manipulateurs industriels collaboratifs, du guidage manuel et de la formulation mathématique de la dynamique des robots, Figure 6.7. Dans ce qui suit, les principales contributions sont énumérées en fonction de chaque sujet.

Principales contributions en matière l'évitement des collisions:

Résumé

Les manipulateurs industriels collaboratifs ouvrent une nouvelle ère dans la fabrication flexible, où les robots et les humains sont capables de coexister et de travailler ensemble. Cependant, divers défis persistent pour parvenir à une collaboration complète entre les robots et les humains en milieu industriel. Dans cette thèse, deux défis principaux -la sécurité et la collaboration -sont abordés pour atteindre cet objectif. Concernant la sécurité, la thèse présente une méthode d'évitement des collisions en temps réel qui permet au robot d'ajuster les chemins générés hors ligne pour une tâche industrielle, tout en évitant les collisions avec les humains à proximité. En outre, la thèse a présenté une nouvelle méthode pour effectuer un mouvement d'évitement de collision réactif, en utilisant la méthode de Newton du second ordre qui offre divers avantages par rapport aux méthodes traditionnelles utilisées dans la littérature. Sur la collaboration, la thèse présente un mode de guidage manuel précis comme alternative au mode de guidage actuel pour effectuer des opérations de positionnement précis de l'effecteur terminal du robot d'une manière simple et intuitive. La thèse présente également de nouvelles contributions à la formulation mathématique de la dynamique des robots, y compris un algorithme récursif pour calculer la matrice de masse des robots sériels avec un coût minimal du second ordre et un algorithme récursif pour calculer efficacement les symboles de Christoffel. Tous les algorithmes présentés sont validés soit en simulation, soit dans un scénario réel.

Mots clés : Évitement des collisions, sécurité, champs potentiels, méthode de Newton, Hessian, robots collaboratifs, guidage manuel, redondance, cinématique du robot, dynamique du robot, algorithmes récursifs, matrice de masse, symboles de Christoffel.

Safe Collaborative Robotic Manipulators Abstract

Collaborative industrial manipulators are ushering a new era in flexible manufacturing, where robots and humans are allowed to coexist and work side by side. However, various challenges still persist in achieving full human robot collaboration on the factory floor. In this thesis two main challenges -safety and collaboration -for achieving that goal are addressed. On safety, the thesis presents a real-time collision avoidance method which allows the robot to adjust the offline generated paths of the industrial task in real-time for avoiding collisions with humans nearby. In addition, the thesis presented a new method for performing the reactive collision avoidance motion using second order Newton method which offers various advantages over the traditional methods in the literature. On collaboration, the thesis presents the precision hand-guiding as an alternative to the teach-pendant for performing precise positioning operations of the robot's end-effector in a simple and intuitive manner. The thesis also presents new contributions into the mathematical formulation of robot dynamics, including a recursive algorithm for calculating the mass matrix of serially linked robots with a minimal second order cost, and a recursive algorithm for calculating Christoffel symbols efficiently. All the presented algorithms are validated either in simulation or in a real-world scenario.

Keywords : Collision avoidance, safety, potential fields, Newton method, Hessian, collaborative manipulators, hand-guiding, redundancy, robot kinematics, robot dynamics, recursive algorithms, Mass matrix, Christoffel symbols.