Génération automatique et sécuritaire de trajectoires pour un robot collaboratif

UNIVERSITÉ DE SHERBROOKE

Faculté de génie

Département de génie électrique et de génie informatique

GÉNÉRATION AUTOMATIQUE ET

SÉCURITAIRE DE TRAJECTOIRES

POUR UN ROBOT COLLABORATIF

Mémoire de maitrise

Spécialité : génie électrique

Kévin Dufour

Sherbrooke (Québec) Canada

Octobre 2017

MEMBRES DU JURY

Wael Suleiman

Jean Rouat

Mehdi Benallegue

RÉSUMÉ

Parce que la robotique collaborative vise à libérer les robots des barrières physiques les séparant des opérateurs humains, de nouveaux dés apparaissent autour de la sécurité de ces derniers. S'il est possible de diminuer la dangerosité des robots en amont de leur conception, les logiciels qui les contrôlent doivent impérativement intégrer des mesures de sécurité, an d'être compatibles avec des environnements humains dynamiques. Les algorithmes classiques de planication de trajectoire nécessitant de lourds calculs, il est avantageux de modier la trajectoire en temps réel pour l'adapter à l'environnement dan-gereux.

Dans ce projet de recherche, un algorithme de cinématique inverse, sous forme de problème d'optimisation, est utilisé an de générer la commande du robot à partir d'une trajectoire dénie hors-ligne. L'ajout de contraintes de sécurité à ce problème est particulièrement étudié : dans un premier temps, l'indice de manipulabilité, qui traduit la distance du robot à une conguration singulière, est considéré. Ainsi, il doit être maximisé tout au long de la trajectoire an d'assurer la meilleure mobilité disponible. Dans un deuxième temps, le facteur humain a été intégré par la prise en compte du confort de celui-ci : an de réduire le stress éprouvé par l'opérateur face à un robot aux mouvements imprévisibles, on s'assure de minimiser la distance entre l'eecteur et le regard de l'humain pour garantir une plus grande visibilité de la tâche. Dans les deux cas, nous avons présenté une formulation originale de ces critères an de les intégrer dans le problème d'optimisation. Par ailleurs la contrainte d'évitement d'obstacles a aussi été utilisée, de même que la relaxation de la trajectoire, qui permet au robot de dévier un peu de cette dernière pendant une portion de la durée de la tâche. Enn des tests en simulation et avec le robot réel Baxter de Rethink Robotics ont permis de valider notre approche et de vérier les performances en conditions réelles, en utilisant une caméra RGB-D et un logiciel de détection d'humain en temps réel. Mots-clés : Cinématique, Optimisation et contrôle optimal, Robot redondant

ABSTRACT

AUTOMATIC AND SAFE GENERATION OF TRAJECTORIES FOR A COLLABORATIVE ROBOT

Because collaborative robots are aimed at working in the vicinity of human workers without physical security fences, they bring new challenges about security. Even if robots can be conceived to be less harmful, their software has to integrate security features in order to be suitable for dynamic human environments. Since classical path planning algorithms require heavy calculations, it is interesting to modify the trajectory in real time to adapt it to the dangerous environment.

In this research project, an inverse kinematics solver, in the form of an optimization problem, is used to generate the command of the robot to follow a trajectory dened oine. The addition of security constraints is studied: rst, the manipulability index, which reects the distance of the robot to singular congurations, is considered. Thus, it should be maximized all along the trajectory to ensure the best mobility available. Then the human is integrated by taking into account its comfort: in order to reduce the stress of working near an unpredictable moving robot, the distance between the end-eector and the human gaze is minimized to guarantee a greater visibility of the task. In both cases, we have presented a new formulation of those criteria to integrate them into the optimization problem. Moreover, the collision avoidance constraint is used, as well as the trajectory relaxation, which allows the robot to deviate from its trajectory for a certain amount of time during the task. Finally tests in simulation and with the real Baxter robot from Rethink Robotics validated our approach and the performance has been evaluated in real conditions, using a RGB-D camera and a real time human tracker software.

REMERCIEMENTS

Je souhaiterais témoigner toute ma reconnaissance à mon directeur de recherche, le pro-fesseur Wael Suleiman, pour m'avoir guidé tout au long de cette maîtrise. Je le remercie notamment pour ses précieux conseils et son aide inestimable qui m'ont apporté une grande expérience et qui ont rendu ce projet particulièrement enrichissant.

Je remercie aussi Sonny qui a su se rendre disponible pour répondre à mes questions et qui m'a permis de gagner un temps considérable.

J'adresse enn mes pensées à mes amis de France et du Québec, ainsi qu'à ma famille, qui m'ont accompagné pendant cette période.

TABLE DES MATIÈRES

1 INTRODUCTION 1

2 ÉTAT DE L'ART 3

2.1 Robotique collaborative . . . 3

2.1.1 Une approche novatrice de la robotique... . . 3

2.1.2 ... qui pose un dé sécuritaire majeur . . . 4

2.2 Modication de trajectoire en temps réel . . . 4

2.2.1 Méthodes analytiques . . . 4

2.2.2 Méthode numérique . . . 8

3 INTÉGRATION DE L'INDICE DE MANIPULABILITÉ DANS L'AL-GORITHME DE CINÉMATIQUE INVERSE 11 3.1 Abstract . . . 12

3.2 Introduction . . . 12

3.3 Inverse Kinematics Problem Formulation . . . 14

3.3.1 General Denition . . . 14

3.3.2 Relaxed Problem Formulation . . . 15

3.4 Manipulability Index . . . 16 3.4.1 Theoretical Denition . . . 16 3.4.2 Practical Implementation . . . 17 3.5 Simulation Results . . . 19 3.5.1 First Scenario . . . 20 3.5.2 Second Scenario . . . 21

3.5.3 Study of the Trajectory Relaxation . . . 22

3.6 Conclusion and Future Work . . . 24

4 INTÉGRATION DE L'ATTENTION DE L'HUMAIN DANS L'ALGO-RITHME DE CINÉMATIQUE INVERSE 27 4.1 Abstract . . . 28

4.2 Introduction . . . 28

4.3 Inverse Kinematics Problem Formulation . . . 30

4.4 Human Awareness Constraint . . . 31

4.4.1 Denition of Human Gaze and Distance to End-Eector . . . 31

4.4.2 Integration into the IK problem . . . 32

4.4.3 Trajectory Relaxation . . . 34 4.5 Manipulability Index . . . 35 4.5.1 Linear Approach . . . 36 4.5.2 Quadratic Approach . . . 36 4.6 Results . . . 36 4.6.1 Simulation Results . . . 37

4.6.2 Experiments with the Real Robot . . . 40 ix

x TABLE DES MATIÈRES

4.7 Conclusion and Future Work . . . 41

4.8 Appendices . . . 42

4.8.1 Proof of Eq (4.4): . . . 42

4.8.2 Proof of Eq (4.6) . . . 42 4.8.3 Proof of the Maximization of the Manipulability Index in Eq (4.15) 43

5 CONCLUSION 49

LISTE DES FIGURES

2.1 Exemple de minimum local [32] . . . 5

2.2 Architecture de contrôle d'un robot an d'eectuer une tâche tout en évitant d'éventuels obstacles [32] . . . 6

2.3 Exemple de tunnel élastique évitant la sphère grise [2] . . . 7

2.4 Chemin suivi par un robot se dirigeant vers un objectif en évitant un obs-tacle cubique ouvert formant un cul-de-sac [9] . . . 8

2.5 Paramètres géométriques utilisés dans la méthode d'évitement de collision entre deux objets O1 et O2 [7] . . . 10

3.1 Geometrical parameters for collision avoidance method between two objects O1 and O2 . . . 15

3.2 Prole of constraint stiness . . . 17

3.3 Normalized manipulability index evolution while executing the task in sce-nario 1, in this case only the rst-order approximation of m is used . . . . 20

3.4 Normalized manipulability evolution during task depending on coecient in scenario 1 using second-order approximation of m . . . 21

3.5 Minimal distance between the robot and the obstacle with dierent constraints 22 3.6 Normalized manipulability evolution in obstacle avoidance scenario . . . . 22

3.7 Eect of the obstacle avoidance constraint on the conguration of the robot for a given state . . . 23

3.8 Normalized manipulability evolution while executing the task in scenario 2 23 3.9 Normalized manipulability evolution in scenario 1 while relaxing the con-straints . . . 24

3.10 Reference and computed trajectories in scenario 1 while relaxing the con-straints . . . 24

3.11 Normalized manipulability evolution in scenario 2 while relaxing the con-straints . . . 25

3.12 Reference and computed trajectories in scenario 2 while relaxing the con-straints . . . 25

3.13 Final Pose of the Second Trajectory . . . 26

4.1 Human awareness, expressed as the distance between human gaze and the end-eector, shown in the 3D model of the real scene detected by an RGB-D camera . . . 30

4.2 Geometrical parameters for distance calculation between human gaze and end-eector . . . 32

4.3 Prole of constraint stiness . . . 35

4.4 Position of the human in simulation tests . . . 37

4.5 Human awareness with the linear approach in scenario T2 . . . 38

4.6 Comparison of the impact of linear and quadratic approaches on human awareness for several scenarios . . . 44

xii LISTE DES FIGURES 4.7 Normalized manipulability index evolution when minimizing distance to

human gaze with linear and quadratic approaches in scenario T2 . . . 45

4.8 Distance to human gaze evolution when adding the manipulability constraint in scenario T2, with quadratic approaches . . . 45

4.9 Normalized manipulability index evolution when adding the manipulability constraint in scenario T2, with quadratic approaches . . . 46

4.10 Snapshots of the Baxter robot executing trajectory T2, showing the impact

of the manipulability index optimization on the arm pose . . . 46 4.11 Snapshots of the Baxter robot executing trajectory T2 while optimizing

human awareness and manipulability index . . . 46 4.12 Human Awareness in scenario T2 in real situation . . . 47

LISTE DES TABLEAUX

4.1 Optimal coecients of human awareness constraint . . . 38 4.2 Optimal coecients for manipulability index maximization . . . 40

LISTE DES ACRONYMES

Acronyme Dénition

IK Cinématique inverse (Inverse Kinematics)

QP Optimisation quadratique (Quadratic Programming)

CHAPITRE 1

INTRODUCTION

La robotique collaborative est un domaine qui gagne en popularité avec l'apparition de nouveaux modèles destinés à l'industrie. Cette génération de robot est destinée à travailler à proximité des opérateurs humains sans les protections physiques habituelles an d'assurer un cadre de travail plus exible : les robots ne viennent plus en remplacement des humains, mais plutôt en collaboration avec eux que ce soit pour des tâches d'assistance, d'interaction ou encore de cohabitation avec un robot et un humain eectuant simultanément des tâches sur un même objet.

Ainsi apparaissent de nouveaux dés en termes de sécurité qui devront être résolus avant un potentiel déploiement global dans l'ensemble de l'industrie. En particulier, ces robots doivent être capables de travailler dans des environnements complexes, dynamiques et im-prévisibles dont la partie la plus critique est la présence d'humain. Il est donc nécessaire d'adapter les algorithmes de calculs de trajectoires an d'intégrer ces contraintes. Notam-ment, ils doivent être capables d'eectuer des calculs rapidement an de permettre une exécution en temps réel et un temps d'adaptation susant. Dans ces conditions, les algo-rithmes de planication traditionnels, par exemple basés sur l'échantillonnage aléatoire, ne peuvent que générer une trajectoire initiale, hors-ligne, adaptée à un modèle statique de l'environnement. Il est alors judicieux d'utiliser des techniques de modication en temps réel de la trajectoire, incluant toutes les contraintes de sécurité nécessaires.

Dans ce projet de recherche, nous nous intéressons à la possibilité d'améliorer la sécurité lors de l'exécution d'une tâche par un robot collaboratif dans un environnement dyna-mique. Ainsi nous étudierons l'intégration de critères de sécurité dans un algorithme de cinématique inverse qui permet de calculer la commande du robot. Notre méthode devra d'une part respecter ses limitations physiques, assurer que la trajectoire n'entre pas en collision et considérer la présence d'un humain. D'autre part cette méthode doit pouvoir utiliser les informations issues de l'observation de l'environnement et fonctionner en temps réel.

Ainsi des formulations originales de l'indice de manipulabilité et de l'attention de l'hu-main sont proposées an de les intégrer dans notre algorithme de même qu'une approche d'optimisation multi-objectif permettant de combinant l'ensemble de nos contraintes dans

2 CHAPITRE 1. INTRODUCTION un cadre compatible avec des tâches non répétitives. Par ailleurs des expériences avec le robot réel et un système d'observation et de détection en temps réel valident l'ecacité de la méthode.

Le chapitre 2 présente plus en détail la robotique collaborative et les problématiques as-sociées et comment y répondre par des méthodes de modication de trajectoire. Sont rapportés au chapitre 3 l'article publié à l'occasion de la conférence IEEE/RSJ Interna-tional Conference on Intelligent Robots and Systems (IROS) concernant la contrainte de manipulabilité et au chapitre 4 l'article soumis à la revue IEEE Robotics and Automation Letters qui traite de l'ajout de la contrainte de visibilité de la tâche. Enn le chapitre 5 fait un bilan du travail eectué et donne de nouvelles perspectives d'axe de recherche.

CHAPITRE 2

ÉTAT DE L'ART

2.1 Robotique collaborative

2.1.1 Une approche novatrice de la robotique...

La robotique collaborative est un domaine qui gagne en popularité : le besoin croissant de exibilité et de réactivité face aux changements dans l'industrie incite à associer l'adapta-bilité des humains aux processus automatisés utilisant des robots [21]. Cette nouvelle géné-ration de robots industriels est destinée à travailler dans un environnement humain, libérée des barrières physiques de protection. D'après [3] malgré le fait que la notion de cobot, pour collaborative robot, date de 1999, la plupart des systèmes hybrides humain/machine actuels consistent seulement en des systèmes de compensation de poids pour le port de charges lourdes. De plus [27] précise que les entreprises n'utilisent pas les robots à leur pleine capacité, citant notamment des dicultés dans l'emploi de ces nouvelles techno-logies. Parmi elles, le fait que certains processus ne soient pas facilement automatisables et que les systèmes de sécurité pour l'emploi de robots dans un environnement humain soient encore imparfaits. Les cobots sont la réponse à cette problématique et pourraient permettre de maximiser l'emploi de robots.

Les bénéces sont multiples : ils peuvent être utilisés en soutien d'un ouvrier pour l'aider à accomplir une tâche dicile, telle que le port d'un outil lourd, améliorant l'ergonomie du poste du travail et la sécurité du travailleur [3]. Ils peuvent également interagir phy-siquement avec les humains : échange d'objets, etc. ou travailler sur une pièce en même temps qu'un autre humain. Pour certains robots, tel Baxter de Rethink Robotics, la colla-boration peut s'eectuer sous la forme d'apprentissage : un opérateur montrant au robot ce qu'il doit faire en déplaçant son bras selon la trajectoire à exécuter, sans avoir besoin de passer par une étape de programmation qui est souvent compliquée. Il dispose de plus d'une interface intuitive avec l'opérateur humain sous la forme d'un écran qui peut acher une paire d'yeux dont la direction du regard indique la zone de travail du robot.

4 CHAPITRE 2. ÉTAT DE L'ART

2.1.2 ... qui pose un dé sécuritaire majeur

La principale problématique de ce domaine est de rendre les robots susamment sûrs pour pouvoir les faire travailler avec des humains. Ainsi l'Organisation internationale de normalisation (ISO) a publié le 15 février 2016 sa première norme relative exclusivement à la robotique collaborative, ISO/TS 15066, qui fait suite aux normes ISO 10218-1 et ISO 10218-2 publiées en 2011 relatives à la sécurité dans le domaine robotique et qui abordaient la notion de collaboration [11]. On peut classer les stratégies de sécurité selon deux catégories : au niveau matériel et au niveau de la programmation. Au niveau matériel, on peut modier le robot an de le rendre plus sûr en diminuant le poids des parties mouvantes ou en utilisant des matériaux légers ou souples. On peut aussi avoir recours à la technologie des articulations élastiques, dont est doté le robot Baxter, qui permet de réduire la dangerosité des impacts par rapport à une articulation rigide puisque le matériau exible (ressort, poulie élastique, etc..) absorbe une partie du choc. Les capteurs de force sont aussi utiles pour identier les chocs et réagir promptement [22].

Au niveau logiciel la sécurité peut être assurée par des stratégies d'évitement d'obstacles ou de réactions aux collisions [10] [42] et la tâche est alors divisée en deux étapes. La première consiste en la planication globale, qui prend en compte une modélisation de l'environnement au complet dans une situation statique, an de trouver le meilleur che-min. La plupart de ces méthodes de planication utilisent l'échantillonnage aléatoire de l'environnement et la construction de graphes à l'intérieur [24]. Deux méthodes populaires sont RRT [18] et PRM [24], qui ont pour avantage que la probabilité de trouver une so-lution existante tend vers un si susamment de temps leur est laissé. En revanche, elles ont pour inconvénient de demander beaucoup de temps de calcul : dans [18], la planica-tion prend plusieurs secondes. C'est pourquoi elles sont d'abord employées hors-ligne puis dans une deuxième étape un algorithme fonctionnant en temps réel modie la trajectoire initialement générée an d'éviter les obstacles, qui peuvent éventuellement se déplacer, ou réagir à des collisions ou à des forces extérieures.

2.2 Modication de trajectoire en temps réel

2.2.1 Méthodes analytiques

Après avoir calculé la trajectoire à eectuer à partir de l'environnement statique, nous nous intéressons à la modication en temps réel an de pouvoir intégrer le robot dans un envi-ronnement dynamique. D'après [4], la plupart des méthodes analytiques de modication temps réel sont basées sur la méthode des champs de potentiel articiel et ses variations.

2.2. MODIFICATION DE TRAJECTOIRE EN TEMPS RÉEL 5 Cette méthode a été présentée initialement dans [16] : les obstacles sont modélisés par des forces répulsives (dont la norme dépend de la distance du robot à l'obstacle) et la position à atteindre par une force attractive. Dans cette première description, ces forces sont uti-lisées au niveau des contrôleurs des moteurs an de modier les angles des articulations : selon l'équation (2.1), si on eectue un contrôle en termes de couple alors le couple Γ appliqué au moteur est l'addition de la commande Γmotion nécessaire pour se rapprocher

de l'objectif et du couple Γobstacle nécessaire pour s'éloigner des obstacles. L'inconvénient

est que cette méthode peut aboutir à des minima locaux : la gure 2.1 illustre le cas où la direction d'attraction est colinéaire avec la direction de répulsion, de par la géométrie de l'obstacle et sa position relative à l'objectif. Ainsi le robot se retrouve piégé dans une position qui n'est pas l'objectif à atteindre et il ne lui est pas possible de s'échapper avec cette méthode. En eet se rapprocher de l'objectif signie se rapprocher de l'obstacle et générer une plus grande force répulsive. Au contraire, s'éloigner de l'obstacle implique de s'éloigner également de l'objectif et donc de générer une plus grande force attractive.

Γ = Γmotion+ Γobstacle (2.1)

Figure 2.1 Exemple de minimum local [32]

[32] propose ensuite une architecture, gure 2.2, qui combine la planication hors-ligne de trajectoire avec la méthode des champs de potentiel articiel en introduisant un nouvel élément : la modélisation de la trajectoire comme étant un objet déformable, appelé bande élastique, soumis à des forces internes assurant la cohésion de la bande et soumis aux forces externes représentant les obstacles. Si les minima locaux restent un problème, on a l'avantage de la vision globale de la planication hors-ligne, qui peut être appelée pour replanier une trajectoire dans le cas où le robot serait bloqué dans un tel minimum. Cette technique a été utilisée avec succès sur le robot manipulateur Puma 560.

6 CHAPITRE 2. ÉTAT DE L'ART

Figure 2.2 Architecture de contrôle d'un robot an d'eectuer une tâche tout en évitant d'éventuels obstacles [32]

[2] présente le principe des elastic strips (qui signie aussi bande élastique, mais désigne un autre type que celle dénie en [32]). Le tunnel d'espace libre, ou tunnel élastique, est d'abord déni. Il est obtenu à partir de l'espace balayé par le robot quand il eectue une trajectoire initiale dénie hors-ligne. Cet espace est ensuite augmenté par des sphères en tenant compte de la présence d'obstacles. Un exemple est présenté à la gure 2.3 avec les positions successives d'un bras manipulateur monté sur une base mobile. Un obstacle, représenté par une sphère opaque, est alors placé sur le chemin de la tâche. Le tunnel d'espace libre est ici constitué de l'ensemble des sphères transparentes. Il met en évidence l'espace localement libre d'obstacles.

Les elastic strips sont alors dénies comme un couple constitué de la trajectoire hors-ligne et du tunnel élastique. L'intérêt de ce tunnel est d'augmenter la rapidité de calcul de l'espace libre puisqu'il est limité aux congurations proches de la trajectoire initiale, on ne fait donc pas un calcul sur tout l'espace de conguration. La déformation de la trajectoire se fait de la manière suivante : la trajectoire est discrétisée en un ensemble ni de congurations du robot. Chacune de ses congurations est soumise aux forces issues des champs de potentiel articiel (forces d'attraction et de répulsion) et est donc modiée par la présence d'obstacles. Les nouvelles congurations constituent alors la nouvelle trajectoire. De plus il arrive parfois qu'un obstacle se déplace perpendiculairement à la trajectoire du

2.2. MODIFICATION DE TRAJECTOIRE EN TEMPS RÉEL 7

Figure 2.3 Exemple de tunnel élastique évitant la sphère grise [2]

robot : dans ce cas la déformation n'est plus intéressante, car plus l'objet va s'éloigner, plus la trajectoire va s'allonger. Dans ce cas des forces internes sont dénies, de manière à ce que la trajectoire puisse temporairement se  rompre  le temps de laisser passer l'obstacle puis de se reformer par l'action des forces internes. Par ailleurs si la modication locale échoue, on fait appel de nouveau au planicateur global. Cette stratégie est appliquée dans [23] sur le robot humanoïde ASIMO de Honda de 26 degrés de liberté. La tâche demandée est de déplacer un objet avec un des bras du robot tout en évitant des obstacles statiques et d'autres qui se déplacent. L'expérience a permis de montrer l'ecacité des elastic strips : le bras a parfaitement évité les obstacles présentés et a réagi en 0.3 secondes entre le moment où l'obstacle dépasse la distance de sécurité et le moment où le bras atteint une vitesse de 0.1 m/s. Enn, le robot met moins de 5 secondes pour se stabiliser après que l'obstacle ait cessé son mouvement. Une approche qui permet d'éviter les minima locaux est la méthode des champs de circulation, présentée en [36]. Elle consiste à modéliser un champ de force répulsif autour des obstacles, à la manière d'un champ magnétique par exemple. De cette façon c'est la vitesse du manipulateur qui est réorientée autour de l'obstacle. La convergence vers l'objectif est garantie en utilisant une force attractive, calculée par les champs de potentiel articiel. [9] implémente cette méthode sur un robot à 7 degrés de liberté et teste avec succès le contournement d'obstacle. En particulier, des situations complexes avec des  pièges , qui habituellement bloquent les robots dans un minimum local, sont résolues avec succès. La gure 2.4 montre le cas où le robot est placé face à un cube dont l'entrée est située sur le chemin entre le robot et son objectif. Grâce à la force attractive de l'objectif, le robot entre à l'intérieur du cube puis le champ répulsif du cube guide le robot vers l'extérieur pour le faire converger vers sa destination.

8 CHAPITRE 2. ÉTAT DE L'ART

Figure 2.4 Chemin suivi par un robot se dirigeant vers un objectif en évitant un obstacle cubique ouvert formant un cul-de-sac [9]

2.2.2 Méthode numérique

Une approche plus populaire est la méthode numérique, par la résolution d'un problème d'optimisation. Le principe de cette méthode, appelée résolution du problème de cinéma-tique inverse, est de calculer en temps réel la trajectoire dans l'espace de conguration à partir de la trajectoire cartésienne, déterminée hors-ligne. En fonction des contraintes appliquées, comme l'évitement d'obstacles, la conguration du robot sera modiée. Selon l'objectif xé, la trajectoire peut être suivie strictement ou un écart peut être autorisé an d'avoir un comportement plus sécuritaire. En suivant strictement la trajectoire, seule la redondance éventuelle du robot permettra un changement de pose.

Ce problème peut s'écrire, de manière générale, sous la forme d'un problème d'optimisation quadratique, [44], [41], [40], dont la formulation générale est :

minimize x 1 2x T_{Qx + g}T_x subject to Cx = e ˆ x− ≤ x ≤ ˆx+ b− _{≤ Ax ≤ b}+ (2.2)

Classiquement, la variable à résoudre est :

x = ˙q (2.3) c'est-à-dire la vitesse des articulations du robot. Q est une matrice symétrique semi-dénie positive, C et A sont des matrices et g, e, b− _{et b}+ _{des vecteurs. ˆx}+ _{et ˆx}− _{sont des}

2.2. MODIFICATION DE TRAJECTOIRE EN TEMPS RÉEL 9 donc de dénir les contraintes linéaires imposées à x et la fonction à minimiser. D'autres méthodes peuvent utiliser des variables diérentes, en y ajoutant par exemple la période d'échantillonnage [41], ou en calculant le jerk des articulations (la dérivée troisième de la position angulaire) [40].

Par ailleurs la trajectoire à exécuter peut être introduite de deux façons ; en utilisant la fonction d'objectif pour minimiser l'erreur entre la position du robot et la trajectoire :

minimize

˙

q ∥Jτq˙− ˙τ ∥ 2

(2.4) avec Jτ la matrice Jacobienne de la tâche τ(q), ou en exprimant la trajectoire comme une

contrainte d'égalité [41], [40] :

J ˙q = ˙r (2.5) avec J la matrice Jacobienne du robot à la conguration q et ˙r la trajectoire exprimée en termes de vitesse. À la diérence de l'équation (2.4), la contrainte d'égalité est très forte, car la résolution du problème échoue si celle-ci ne peut pas être respectée. Elle ore donc moins de liberté que la fonction d'objectif (2.4) mais garantit le strict respect de la tâche. Malgré tout, les tâches ont souvent pour but d'atteindre une position nale. Dans ce cas il est intéressant d'autoriser le robot à dévier un peu de sa trajectoire pendant une période xe et de revenir à la n à la trajectoire prévue et à la position désirée. Ce principe, appelé relaxation de la trajectoire, a été déni dans [14] et [40]. Une nouvelle variable à résoudre, δ, est ajoutée au problème, représentant la déviation de la trajectoire :

minimize ˙ q,δ 1 2q˙ T Q ˙q + 1 2δ T Qδδ subject to J ˙q + δ = ˙r ˆ˙ q− _{≤ ˙q ≤ ˆ˙q}+ δ− _{≤ δ ≤ δ}+ (2.6)

Qδ est ici une matrice similaire à Q et δ+ et δ− sont les bornes de δ. Elle permet de

contrôler l'amplitude de cette déviation en imposant une minimisation très forte de la norme de δ au début et à la n de la trajectoire et une minimisation peu importante ailleurs.

10 CHAPITRE 2. ÉTAT DE L'ART Ces approches peuvent alors être utilisées pour éviter des obstacles dans un environnement dangereux. [7] a présenté initialement l'évitement d'obstacles sous forme d'une contrainte d'inégalité linéaire, utilisable dans un problème d'optimisation :

−nT_{J (q, p}

1) ˙q≤ ξ

d_{− d}s

di− ds if d ≤ d

i (2.7)

et dont les paramètres sont dénis dans la gure 2.5. Cette contrainte est dénie entre deux objets : O1 étant une partie du robot et O2 l'obstacle à éviter. Elle signie que lorsque O2

franchit la distance d'inuence di, sa vitesse ralentit pour l'empêcher de violer la distance

de sécurité ds. La variable d est la distance entre les deux objets et ξ un coecient positif

qui inuence le ralentissement du robot. Cette contrainte a notamment été utilisée avec succès dans [44], [40] et [14].

O

1

O

2

p

p

d

Figure 2.5 Paramètres géométriques utilisés dans la méthode d'évitement de collision entre deux objets O1 et O2 [7]

Une limitation de cette méthode est qu'elle est dénie pour des objets convexes. Dans le cas d'objets quelconques, il faut alors procéder à une approximation en les décomposant en objets élémentaires convexes. De plus, dans les scénarios d'évitement d'obstacles, des objets non strictement convexes sont couramment utilisés. Ce sont des objets possédant par exemple des faces planes ou des arêtes, tels des parallélépipèdes ou des cylindres. Ils sont notamment issus de la détection de l'environnement du robot. Ainsi une caméra de type RGB-D eectue une modélisation en trois dimensions des obstacles par une décomposition en cubes. Comme précisé dans [13], lorsqu'on applique la méthode de [7] à deux objets non strictement convexes, des discontinuités de la vitesse du robot peuvent apparaître. Un algorithme est alors présenté dans [13] pour résoudre cette problématique.

Par la suite, de nouveaux objectifs de sécurité seront dénis et leur intégration dans le problème de cinématique inverse déni précédemment sera étudiée.

CHAPITRE 3

INTÉGRATION DE L'INDICE DE

MANIPU-LABILITÉ DANS L'ALGORITHME DE

CI-NÉMATIQUE INVERSE

Auteurs :

 Kévin Dufour : Étudiant à la maîtrise, Université de Sherbrooke, Faculté de génie, Département de génie électrique et de génie informatique.

 Wael Suleiman : Professeur, Laboratoire de robotique intelligente, interactive, in-tégrée et interdisciplinaire (IntRoLab), Université de Sherbrooke, Faculté de génie, Département de génie électrique et de génie informatique.

Date d'acceptation : 14 juin 2017

État de l'acceptation : Version nale publiée. La version présentée ci-dessous comporte des modications.

Conférence : IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

Référence : Dufour, K. et Suleiman, W. (2017). On integrating manipulability index into inverse kinematics solver. Dans IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). p. 69676972.

Titre français : Intégration de l'indice de manipulabilité dans l'algorithme de cinéma-tique inverse

Contribution au document : La contribution de cet article consiste en la proposition d'une méthode de résolution du problème de cinématique inverse intégrant l'indice de manipulabilité. Il présente ainsi des pistes pour l'implémentation pratique de ce nouvel algorithme ainsi que les stratégies possibles pour maximiser cet indice pendant l'éxecution de la tâche.

Résumé français : Cet article présente une méthode de maximisation de l'indice de manipulabilité d'un robot industriel redondant lors de la résolution du problème de ci-nématique inverse, exprimé comme problème d'optimisation. Même si le problème de

12 CHAPITRE 3. INTÉGRATION DE L'INDICE DE MANIPULABILITÉ DANSL'ALGORITHME DE CINÉMATIQUE INVERSE cinématique inverse est un sujet qui a été largement étudié, l'intégration de l'indice de manipulabilité a rarement été considéré. Puisque la relation entre cet index et les va-riables d'articulation du robot n'est pas directe, diérentes formulations ont été testées en utilisant des approximations de ses dérivées. L'évitement d'obstacle a aussi été pris en compte et l'eet de la modication de la trajectoire cartésienne durant l'éxécution de la tâche a été analysé.

Diérent scénarios ont été conduits en simulation et ont prouvés que notre algorithme modié de résolution du problème de cinématique inverse réussit à maximiser l'indice de manipulabilité, même en ajoutant la contrainte d'évitement d'obstacle. De plus, la relaxation de la trajectoire permet une plus grande manipulabilité tout en assurant la continuité du mouvement et en respectant les limitations physiques du robot.

3.1 Abstract

This paper presents a method to maximize the manipulability index of a redundant indus-trial manipulator while solving the Inverse Kinematics (IK) problem as an optimization problem. Even though the IK problem is a widely studied topic, the integration of the ma-nipulability index into IK has rarely been taken into account. As the relation between this index and the joint variables of the robot is not straightforward, we have tested dierent formulations using approximated derivatives. Obstacle avoidance has also been considered and the eect of modifying the Cartesian trajectory during the execution of a task has been thoroughly analyzed.

Dierent scenarios have been conducted in simulation and have proven that our modied inverse kinematics solver is ecient to maximize the manipulability index, even with the additional constraint of obstacle avoidance. Moreover, the relaxation of the trajectory constraints leads to a greater manipulability while ensuring the motion smoothness and satisfying the robot physical limitations.

3.2 Introduction

The eld of collaborative robots is widely expanding as the manufacturers realize the opportunity of exibility that this kind of robot oers [21]. A major concern, however, is the safety of the human operators of those robots, as they are designed to operate without a security fence or even in physical interaction with humans. There are dierent ways of improving the safety: I)- modifying the physical properties of the robot such as the material, the weight [1] or the type of joint actuator, II)- adding a monitoring system [22]

3.2. INTRODUCTION 13 in order to make it more human-friendly, III)- integrating some safety constraints into the planning and execution of the task.

Regarding the motion planning phase of a motion, methods such as RRT [26] or PRM [15] are among the very popular ones. These methods try to nd the optimal path while considering dierent criteria. Their main drawback is the prohibitive time of computation that restrains their main use to oine planning, however a recent research [28] on the design of specialized processors to reduce the computation time has shown promising results.

However, when trying to deal with a dynamic environment, it is interesting to generate a preliminary trajectory by motion planning algorithms to benet from their global planning properties, and then modify it in real-time, by inverse kinematics methods for instance, during the execution. The robot in this case could react to the presence of moving obsta-cles, such as a human or another robot, while satisfying some constraints, for example the joint limits or a constraint on the velocity or acceleration of the end-eector.

A well-known approach to solve IK problems is the reformulation as an optimization problem [6, 41]. An objective function depending on the optimization variable, often the joint velocity, is minimized or maximized subject to dierent linear equality and inequality constraints. Because of its general formulation, it is easy to add dierent constraints to meet the safety requirements, as long as it is possible to solve the optimization problem in real-time.

Since the robot is executing a task in a dynamic environment, an important feature is the ability of reacting correctly to unforeseen events. This ability is reected by the manipulability index, related to the singularities of the Jacobian matrix [43]. When this index becomes small and tends to zero, it means that the robot is close to a singularity and its ability to move away from that pose will be reduced. It is necessary, therefore, to maximize the manipulability index, which can be done by integrating it into the objective function of the IK optimization problem.

The main contribution of this paper is proposing a method to solve the IK problem while integrating the manipulability index, and mainly giving insights into the practical imple-mentation of the new solver as well as the possible strategies to maximize the manipula-bility index during the task execution.

This paper is organized as follows: Section 3.3 presents the formulation of inverse kine-matics as an optimization problem with respect to joint limits, obstacle avoidance and trajectory relaxation. In Section 3.4, a new objective function to maximize the

manip-14 CHAPITRE 3. INTÉGRATION DE L'INDICE DE MANIPULABILITÉ DANSL'ALGORITHME DE CINÉMATIQUE INVERSE ulability index is proposed. The method is validated through simulation experiments in Section 3.5.

3.3 Inverse Kinematics Problem Formulation

3.3.1 General Denition

The inverse kinematics problem is generally written in the following form, called quadratic programming (QP) problem: min ˙ q 1 2q˙ T Q ˙q (3.1) subject to J ˙q = ˙r (3.2) ˆ˙ q− _{≤ ˙q ≤ ˆ˙q}+ (3.3) b− ≤ A ˙q ≤ b+ _(3.4)

where ˙q ∈ Rn is the joint velocity, Q a diagonal and positive semi-denite weighting

matrix, J the Jacobian matrix, A, b+_{, b}− _{the matrix and vectors dening the linear}

inequality constraints. ˙r is derived from the Cartesian trajectory r generated by an oine planner.

Equation (3.3) expresses in a single form the joint range and velocity limits using the generalized boundaries ˆ˙q−, ˆ˙q+ dened by the velocity damper formula [40], that formula combines the joint position and velocity so that the velocity smoothly decreases when approaching the joint boundaries and does not violate the limits.

The inequality constraint (3.4) can be used for collision avoidance by using the velocity damper denition, introduced initially in [7]: when the distance d between two convex objects, one being a part of the robot (O1) and the other an obstacle (O2), becomes

smaller than a constant di, the following formula is applied:

−nT_{J (q, p}

1) ˙q≤ ξ

d_{− d}s

di− ds if d ≤ d

i (3.5)

where n is the unit vector between the closest points p1 and p2 on the two objects, as

described in Fig. 3.1, ξ is a positive coecient and ds is a security distance that cannot be

crossed. J(q, p1)is the Jacobian matrix of the robot expressed for the conguration q and

3.3. INVERSE KINEMATICS PROBLEM FORMULATION 15 between the robot and a set of obstacles satisfying d ≤ di, the constraint (3.4) can then

be rewritten as:

A ˙q _{≤ b}+ _(3.6)

where A ∈ Rk×n _{and b}+ _{∈ R}k_{, such as their j}th _{line is dened by:}

Aj =−nTjJj(q, p1) b+_j = ξdj − ds di− ds (3.7)

O

1

O

2

Figure 3.1 Geometrical parameters for collision avoidance method between two objects O1 and O2

It should be pointed out that this problem can be adapted to consider the acceleration or even jerk limits of the joints by replacing the optimization variable to the relevant derivative of q and rewriting the constraints consequently [40].

3.3.2 Relaxed Problem Formulation

The particularity of a redundant robot is that the solution of the IK problem is not unique, thus the best solution among those admissible is chosen by the optimization algorithm. However, in case of collision avoidance, there might be no solution if the obstacle is in collision with every conguration corresponding to a given pose in the trajectory. It is then interesting to allow the robot to deviate from the pre-dened Cartesian trajectory, if the path taken between the initial and the nal poses is not important. A task like pick-and-place" could be an example.

This can be achieved by adding a slack variable vector δ to the optimization variable, which was previously ˙q and then becomes Z =[q δ˙ ]

T

. Indeed, if we write the Cartesian path constraint (3.2) with the new variable: J ˙q = ˙r − δ, it appears that the trajectory is relaxed and the new one is, in velocity terms, ˙r − δ. The IK problem formulation thus becomes [14], [40]:

16 CHAPITRE 3. INTÉGRATION DE L'INDICE DE MANIPULABILITÉ DANSL'ALGORITHME DE CINÉMATIQUE INVERSE min Z 1 2Z T_Q ZZ (3.8) subject to J Z = ˙r (3.9) AZ ≤ b+ _(3.10) Z−≤ Z ≤ Z+ _(3.11) with Z = [ ˙ q δ ] , _QZ = [ Q 0 0 Qδ ] , _{J =}[J I ] A =[A 0 ] , _Z+=[ˆ˙q + δ+ ] , _Z− =[ˆ˙q − δ− ]

The amplitude of the deviation is controlled by the bounds δ+ _{and δ}− _{of the slack variable}

and its associated weighting matrix Qδ, which is dened as follows:

Qδ(t) = fe(t)In (3.12) where fe(t) = ⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩ f1(t) if 0 ≤ t ≤ Tρ Qlow if Tρ≤ t ≤ Tf − Tρ f2(t) otherwise (3.13) with In∈ Rn×n the identity matrix, f1 and f2 polynomial functions of degree 5 dened to

obtain a prole similar to Fig. 3.2, allowing a smooth, continuous behavior for δ, and as a result for ˙q, by setting their rst and second derivatives to zero at their limits. Their value, Qhigh, is very high at the beginning and at the end of the task so δ is close to 0.

Refer to [14] for a detailed description and explanation.

3.4 Manipulability Index

3.4.1 Theoretical Denition

In order to have a motion that has enough admissible movements to escape unforeseen events, it is important to stay away as far as possible from the robot joint singularities.

3.4. MANIPULABILITY INDEX 17 Qlow Qhigh 0 Tρ Tf − Tρ Tf Stiness of constrain t Time

Figure 3.2 Prole of constraint stiness This ability, named manipulability, is dened in [43] as:

m(q) =√det(J JT_{) = σ}

1σ2· · · σn (3.14)

with J the Jacobian matrix of the robot's end-eector for the conguration q and (σi)1≤i≤n

the singular values of J.

This index is particularly useful because when it increases, the manipulability of the robot does as well, and when it gets close to 0, the robot is close to a singularity. Other denitions, however, do not have this property: usually they only tell if the robot is close to a singularity but they do not give any information about the relative distance to the singularity and then cannot be used to get away from it.

3.4.2 Practical Implementation

By integrating the manipulability index into the IK formulation, the following optimization problem is obtained: min ˙ q,qt 1 2q˙ T_{Q ˙q} − α m(qt) (3.15) subject to J ˙q = ˙r (3.16) ˆ˙ q− _{≤ ˙q ≤ ˆ˙q}+ (3.17) b− ≤ A ˙q ≤ b+ _(3.18)

18 CHAPITRE 3. INTÉGRATION DE L'INDICE DE MANIPULABILITÉ DANSL'ALGORITHME DE CINÉMATIQUE INVERSE where α is a positive coecient, preceded by a negative sign in order to maximize m(qt).

As m(qt)is a nonlinear function in q, Eq (3.15) becomes a nonlinear optimization problem

which is hard to solve within a xed time and therefore the hard real-time constraints might not be satised.

In order to transform Eq (3.15) into a QP problem, the manipulability index is linearized as follows: m(qt) = m(qt−1) +∇mT∆q + ε 1 2∆q T Hm∆q = m(qt−1) + T∇mTq + ε˙ 1 2T 2_q˙T_H mq˙ (3.19) where Hm is the Hessian matrix of m, ∇m is its gradient, T is the control loop sampling

time of the robot and ε is a constant equal to 0 (rst-order approximation) or 1 (second-order approximation).

By replacing (3.19) into (3.15), the following equivalent QP problem is obtained: min ˙ q 1 2q˙ T Q ˙q_{− α} ( T_∇mTq + ε˙ 1 2T 2 ˙ qT Hmq˙ ) (3.20) subject to J ˙q = ˙r (3.21) ˆ˙ q− ≤ ˙q ≤ ˆ˙q+ (3.22) b− _{≤ A ˙q ≤ b}+ _(3.23)

Because of the denition of m (3.14), it is not evident to nd a direct analytical relationship between m and q and by consequence a proper denition for ∇m and Hm. Thus, the

gradient of m is approximated numerically by: (_∇m)i = ∂m ∂qi = m(q + δqiEi)− m(q − δqiEi) 2δqi (3.24) where (∇m)i is the ith element of vector ∇m, and Ei ∈ Rn,

Ei = [0 . . . 0 1

↑ i

3.5. SIMULATION RESULTS 19 and the Hessian matrix by:

(Hm)i,j = ∂2_m ∂qi∂qj = (∇m)j(q + δqiEi)− (∇m)j(q− δqiEi) 2δqi (3.25) where: (_∇m)j(q + δqiEi) = m(q+δqiEi+δqjEj)−m(q+δqiEi−δqjEj) 2δqj (_∇m)j(q− δqiEi) = m(q−δqiEi+δqjEj)−m(q−δqiEi−δqjEj) 2δqj

Using these formulations, it is then easy to compute the objective function since only the manipulability index of dierent congurations is computed, which indirectly implies, according to (3.14), Jacobian matrix calculations.

3.5 Simulation Results

The optimization problem can be written in dierent ways depending on the desired constraints. In order to evaluate the performance of each denition, several scenarios have been carried out, showing the eciency of the dierent approaches. In the rst scenario, the manipulability index is already high, and we aim at proving that we can improve it nonetheless while precisely tracking the desired Cartesian trajectory all along and even avoiding an obstacle. The second scenario focuses on the case of a trajectory getting close to a singularity and the eect of our algorithm to get the robot away from that singularity thanks to the robot redundancy. Finally, we will allow the relaxation of the desired trajectory in both scenarios and present its benets.

The dierent approaches have been tested in simulation using a Baxter research robot from Rethink Robotics. A preliminary test has been conducted oine to measure the evolution of the manipulability index of the robot depending on the joint positions and to nd an approximation of its maximal value. Thus, all the manipulability index shown in this section will be expressed relatively to that maximal value. The matrix Q in (3.20) is equal to the identity matrix, and T = 0.01s. The optimization problem is solved by qpOASES [8], which has been proven to be ecient and real-time compatible.

20 CHAPITRE 3. INTÉGRATION DE L'INDICE DE MANIPULABILITÉ DANSL'ALGORITHME DE CINÉMATIQUE INVERSE

3.5.1 First Scenario

In this rst scenario, the robot executes a simple cubic trajectory in the Cartesian space along the vertical axis. In this case, the slack variable δ (the relaxation variable) is not considered and the robot should strictly follow the trajectory.

First, the order of approximation of m will be studied by comparing the rst-order (ε = 0), Fig. 3.3, and the second-order (ε = 1), Fig. 3.4, for dierent values of the coecient α, α = 0 meaning that the manipulability index is not maximized.

Fig. 3.3 shows the impact of α and that there is an optimal value for this coecient: a maximum normalized value of m, 0.91, is reached for α = 5000. However increasing α beyond 5000 makes the manipulability prole decrease, as, for instance, the resulting prole for α = 25000 shows, this behavior can help nd the optimal coecient for a given trajectory. Moreover when α becomes too high, the QP problem (3.20) becomes infeasible. Fig. 3.4 compares the results obtained when considering the second-order approximation to the optimal result found with the rst-order approximation, it shows that considering the second-order approximation does not improve the optimal result obtained by only considering the rst-order approximation. Moreover, the calculation of the Hessian implies the evaluation of 4×n×n Jacobian matrices which seriously increases the total calculation time. For that reason, the manipulability index will thereafter be approximated by only its rst-order formulation (ε = 0).

0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 0 1 2 3 4 5 6 Normalized Manipulabilit y Time (s) α = 0 ε = 0, α = 1000 ε = 0, α = 5000 ε = 0, α = 25000

Figure 3.3 Normalized manipulability index evolution while executing the task in scenario 1, in this case only the rst-order approximation of m is used

The case of collision avoidance is then addressed, as shown in Fig. 3.7 a spherical obstacle that is located close to the elbow during the movement has been added.

3.5. SIMULATION RESULTS 21 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 0 1 2 3 4 5 6 Normalized Manipulabilit y Time (s) α = 0 ε = 1, α = 1000 ε = 1, α = 5000 ε = 0, α = 5000

Figure 3.4 Normalized manipulability evolution during task depending on co-ecient in scenario 1 using second-order approximation of m

The minimal distance between the robot and the obstacle is presented in Fig. 3.5 for dierent cases considering alternatively the obstacle constraint and/or the manipulability optimization. It shows that the robot successfully avoids the obstacle and does not cross the security distance ds while still be able to reach the goal. However, Fig. 3.6 also points

that the maximization of the manipulability index and avoiding the collision with the obstacle are two opposite constraints, it shows that when the arm is far enough from the obstacle, the manipulability increases but when the robot becomes too close to the obstacle, the distance constraint has the priority and the manipulability index has the same value as for the non-optimized case.

Fig. 3.7 illustrates the eect of the collision avoidance constraint on the conguration of the robot for a given time during the motion: the elbow, which becomes too close to the obstacle represented by a green sphere, is pushed away to satisfy the security distance. It should be noticed that the end-eector has the same pose in both congurations as the Cartesian trajectory constraint is always satised.

3.5.2 Second Scenario

In this scenario, a trajectory is generated oine to represent a critical situation as the manipulability index decreases during the motion and comes close to a singularity. Fig. 3.8 shows the evolution of the manipulability without optimization and the eect of our algorithm depending on the coecient α. It shows that our method is generally able to increase the normalized manipulability but has a very slight impact near the singularity.

22 CHAPITRE 3. INTÉGRATION DE L'INDICE DE MANIPULABILITÉ DANSL'ALGORITHME DE CINÉMATIQUE INVERSE 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0 1 2 3 4 5 6 Minimal Distance to Obstacle (m) Time (s) inuence distance security distance

with obstacle constraint, α = 0 with obstacle constraint, α = 5000 without obstacle constraint, α = 5000 without obstacle constraint, α = 0

Figure 3.5 Minimal distance between the robot and the obstacle with dierent constraints 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9 0.95 0 1 2 3 4 5 6 Normalized Manipulabilit y Time (s) with constraint, α = 0 with constraint, α = 5000 without constraint, α = 5000 without constraint, α = 0

Figure 3.6 Normalized manipulability evolution in obstacle avoidance scenario In this case, the improvement of manipulability index is limited, this is mainly because only the redundancy is exploited and thus restricted by the robot physical limits. However, this limitation can be overcome by using the trajectory relaxation as shown in the following subsection.

3.5.3 Study of the Trajectory Relaxation

Both previous scenarios have been tested using the trajectory relaxation. For the rst scenario, Fig. 3.9 shows that the relaxation method allows an increase of about 0.05 of the normalized manipulability index maximum to a value of 0.96. Moreover, the manip-ulability increases much faster since it reaches 0.80 in 0.87s instead of 1.72s without the slack variable. Fig. 3.10 shows the reference Cartesian trajectory of the end-eector and

3.5. SIMULATION RESULTS 23

(a) Without obstacle constraint (b) With obstacle constraint

Figure 3.7 Eect of the obstacle avoidance constraint on the conguration of the robot for a given state

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 1 2 3 4 5 6 7 8 9 Normalized Manipulabilit y Time (s) α = 0 α = 50000 α = 150000

Figure 3.8 Normalized manipulability evolution while executing the task in scenario 2

the modied one, pointing out the small deviations and showing that the robot resumes the task at the end to successfully reach the goal pose for the end-eector.

For the second scenario, Fig. 3.11 shows a very important improvement of the manip-ulability index thanks to the relaxation. Thus, near the singularity, the manipmanip-ulability index becomes close to 0.93 for α = 90000 and to 0.82 for α = 15000, which is a sig-nicant improvement compared to the value obtained in Section 3.5.2, proving that the robot succeeds to completely escape from the singular conguration. Fig. 3.12 presents the inuence of α on the relaxation of the trajectory, and as it can be seen increasing α increases the deviations. However, even limited deviations as with α = 15000 leads to a high prole of manipulability.

24 CHAPITRE 3. INTÉGRATION DE L'INDICE DE MANIPULABILITÉ DANSL'ALGORITHME DE CINÉMATIQUE INVERSE 0.450.5 0.550.6 0.650.7 0.750.8 0.850.9 0.951 0 1 2 3 4 5 6 Normalized Manipulabilit y Time (s) α = 0, without relaxation α = 5000, without relaxation α = 8000, with relaxation

Figure 3.9 Normalized manipulability evolution in scenario 1 while relaxing the constraints -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 0 1 2 3 4 5 6 Cartesian Tra jectory (m) Time (s) X Y Z Reference Resulting

Figure 3.10 Reference and computed trajectories in scenario 1 while relaxing the constraints

Fig. 3.13 illustrates the dierence between the nal congurations in scenario 2 in two cases: I)- without relaxation nor manipulability optimization, II)- with relaxation and manipulability optimization, showing that the conguration of the robot has been changed to a more convenient one regarding the manipulability, while having the same end-eector pose.

3.6 Conclusion and Future Work

In this paper, we presented a new method to integrate the manipulability index maximiza-tion into an optimizamaximiza-tion-based IK solver. When considering the rst-order approximamaximiza-tion of the manipulability index, our algorithm does not require heavy calculations and is

real-3.6. CONCLUSION AND FUTURE WORK 25 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 1 2 3 4 5 6 7 8 9 Normalized Manipulabilit y Time (s) α = 0, without relaxation α = 150000, without relaxation α = 15000, with relaxation α = 90000, with relaxation

Figure 3.11 Normalized manipulability evolution in scenario 2 while relaxing the constraints -1.5 -1 -0.5 0 0.5 1 1.5 0 1 2 3 4 5 6 7 8 9 Cartesian Tra jectory (m) Time (s) Reference X Y Z Resulting, α = 15000 Resulting, α = 90000

Figure 3.12 Reference and computed trajectories in scenario 2 while relaxing the constraints

time compatible, it can be combined with other constraints without leading to infeasibility. Moreover, an implementation of our method while considering the trajectory relaxation has proven to give excellent results in maximizing the manipulability index with a reason-able deviation in the desired trajectory of the end-eector.

However, it should be noted that with this approach the optimal value of coecient α, which allows the maximization of manipulability index, varies depending on the trajec-tory. For repetitive tasks the optimal value of α can be determined oine, however our experiments with the Baxter robot pointed out that the manipulability index has been improved whatever the value of α in the interval ]0,250

26 CHAPITRE 3. INTÉGRATION DE L'INDICE DE MANIPULABILITÉ DANSL'ALGORITHME DE CINÉMATIQUE INVERSE

(a) Without relaxation (b) With relaxation nor manipulability optimization and manipulability optimization

Figure 3.13 Final Pose of the Second Trajectory

Future work will focus on integrating other security-related constraints such as the avoid-ance of moving obstacles, taking into account human awareness and dealing with the presence of a human beside the robot. Moreover, we are also interested in optimizing the calculation time to ensure the real-time property as well as testing our algorithm in dierent scenarios with the real Baxter robot.

CHAPITRE 4

INTÉGRATION DE L'ATTENTION DE

L'HU-MAIN DANS L'ALGORITHME DE

CINÉMA-TIQUE INVERSE

Auteurs :

 Kévin Dufour : Étudiant à la maîtrise, Université de Sherbrooke, Faculté de génie, Département de génie électrique et de génie informatique.

 Wael Suleiman : Professeur, Laboratoire de robotique intelligente, interactive, in-tégrée et interdisciplinaire (IntRoLab), Université de Sherbrooke, Faculté de génie, Département de génie électrique et de génie informatique.

Date de soumission : 9 septembre 2017. La version présentée ci-dessous comporte des modications.

Titre original : On Incorporating Human Awareness into Inverse Kinematics Solver Titre français : Intégration de l'attention de l'humain dans l'algorithme de cinématique inverse

Revue : IEEE Robotics and Automation Letters

Contribution au document : Cet article propose une nouvelle formulation de la contrainte de l'attention de l'humain comme distance entre l'eecteur du robot et le regard ainsi que la dérivée analytique de cette distance par rapport aux variables d'articulation. De plus l'intégration de ce critère dans le problème de cinématique inverse est étudiée an d'améliorer la visibilité de la tâche et de rendre celle-ci plus confortable pour un humain opérant près du robot. L'impact de cette intégration sur la trajectoire et sa performance sont aussi étudiés et une méthode pour améliorer la qualité de la trajectoire en maximisant l'indice de manipulabilité est donnée.

Résumé français : Dans cet article, une nouvelle formulation de l'attention de l'humain est proposée an d'améliorer le confort d'un humain se tenant près d'un robot collaboratif en utilisant la distance entre le regarde de l'humain et l'eecteur du robot. L'intégration de cette nouvelle contrainte dans le problème de cinématique inverse est étudié et des solutions

28 CHAPITRE 4. INTÉGRATION DE L'ATTENTION DE L'HUMAIN DANSL'ALGORITHME DE CINÉMATIQUE INVERSE ecaces sont proposées. De plus, an de permettre au robot de réagir rapidement en case d'évênements imprévus, l'ajout de l'indice de manipulabilité dans le problème est aussi étudié, ainsi que son impact.

La méthode proposée est alors testée en simulation dont les résultats sont vériés avec le vrai robot de recherche Baxter. Ces expériences ont démontrées l'ecacité de la méthode à améliorer la visibilité de la tâche tout en évitant les congurations singulières. Enn, les expériences réelles ont révélées la robustesse de la méthode par rapport aux incertitudes telles que l'imprécision dans la détection de la direction du regard humain.

4.1 Abstract

In this paper, we propose a new formulation for human awareness to improve the comfort of a human standing near a collaborative robot, this formulation is based on the distance between the human gaze and the end-eector. The integration of this new constraint into the Inverse Kinematics (IK) problem is thoroughly studied and ecient solutions are proposed. Moreover, to allow the robot to react rapidly in the case of unforeseen events, adding the manipulability index to the IK problem is also studied and its impact is analyzed.

The proposed method is then extensively tested in simulation and veried on the real Baxter research robot, these experiments proved the method eciency to improve the task visibility while avoiding singular congurations. Moreover, real experiments revealed the method robustness to uncertainties such as imprecision of detecting human gaze direction.

4.2 Introduction

Collaborative robotics brings new challenges as the robots leave the secured environment of industrial factories, where they were separated from humans by fences, and become deployed near workers. Among all the security constraints associated with the use of robots in human environment, the psychological eects should not be neglected. The robots can have unnatural movements that can be easily seen as threatening: for instance, a mobile robot changing rapidly its trajectory to avoid a human moving around would be stressful [20].

Therefore, integrating the psychological eects is a necessity, this is because the robot should reduce the stress and discomfort induced by its presence in an environment shared with humans. This is mainly primordial for workers as stress is a long-term health issue

4.2. INTRODUCTION 29 [25]. Thus, the rst priority is no longer to complete the task in the same way as in a conventional industrial environment, but rather to totally ensure human safety and to generate a socially acceptable motion by taking into account human comfort [38], those criteria should be integrated into the planning and execution process. There are several ways to increase the reassuring eects of a robot, for instance by:

 Communicating its intention [38].

 Reducing its velocity depending on the distance with the human [25], [35].

 Using the potential occupancy space of a kinematic model of human body as a security constraint [33].

 Predicting and anticipating human movements [20], [30].

The visibility of the robot is also a key factor, the studies in [29, 37] pointed out that the robot should carry out its task in the human eld of view, and avoid any hidden zone where the human has no visibility or suddenly appearing from behind an obstacle. However, the distance between the human and the robot decreases the importance of this factor. The information of the human gaze is also of great importance as it has been proven that 75% [37] of humans prefer an object to be in front of them and fully visible when delivered by a robot, thus improving the visibility leads to a greater comfort for the human by reducing the surprising eect.

In this paper, the human awareness refers to the human visibility of the task being exe-cuted by the robot, it can be evaluated with the information about the human gaze [37], the attentional area, dened by an angle of 180◦ _{in front of the human, or the attentional}

eld of view: a cone with an angle in a range of [10◦

− 30◦_{] [38]. The evaluation of the}

human gaze information can be done visually with cameras or by using a brain-machine-interface [29]. Using the head orientation to nd the gaze direction is a popular approach as it doesn't require high-resolution cameras or heavy computations [12], [17]. However, in [31], the authors suggested that tracking eye movements gives better results.

The main contributions of the paper are two-fold:

 Proposing a new formulation of human awareness as the distance between the end-eector and the human gaze as well as the analytical derivative of that distance with respect to the joint angles. The details of this contribution is given in Section 4.4.1.  Incorporating the human awareness criterion into an Inverse Kinematics (IK) prob-lem, the main objective is to enhance the visibility of the task and making it more comfortable for a human operating near the robot. Moreover, the impact of this incorporation on the trajectory and its performance is also studied and a way to

30 CHAPITRE 4. INTÉGRATION DE L'ATTENTION DE L'HUMAIN DANSL'ALGORITHME DE CINÉMATIQUE INVERSE

Figure 4.1 Human awareness, expressed as the distance between human gaze and the end-eector, shown in the 3D model of the real scene detected by an RGB-D camera

improve the quality of the trajectory by maximizing the manipulability index is proposed. This contribution is detailed in Sections 4.4.2 and 4.5.

The proposed method has been thoroughly validated in simulation and on the real Baxter robot in Section 4.6.

4.3 Inverse Kinematics Problem Formulation

In order to accomplish a task dened by the user, IK methods convert a Cartesian pose into joint positions. These methods can be analytical, but in general they are solved nu-merically, especially when considering inequality constraints [41]. The general IK problem is then formulated as Quadratic Programming (QP) problem:

minimize x 1 2x T Qx + gTx subject to Cx = e ˆ x− _{≤ x ≤ ˆ}x+ b− ≤ Ax ≤ b+ (4.1)

where x ∈ Rn _{is the optimization variable, Q is a diagonal and positive semidenite}

weighting matrix, g is a vector, A, C, b+_{, b}−_{, e are the matrices and vectors dening the}

linear constraints.

This formulation can be used to dene a kinematic task of a robot following a trajectory and subject to velocity and position constraints at a sampling time T . Then x becomes the joint velocity ˙q ∈ Rn_{, Q could be the identity matrix, ˆx}+ _{and ˆx}− _{combine the joint}

4.4. HUMAN AWARENESS CONSTRAINT 31 C is the Jacobian matrix J(q) and e is the end-eector desired velocity

˙

r =[vx vy vz wx wy wz

]T

, whose components are dened such that [41]: [ vx vy vz ]T = ∆P T T (4.2) ⎡ ⎢ ⎣ 0 _−wz wy wz 0 −wx −wy wx 0 ⎤ ⎥ ⎦= 1 T [ ∆ReRTe ] (4.3) with ∆P is the dierence between the Cartesian position to reach at the time T and the actual one, and Re is the actual rotation matrix. It should be noted that the Rodrigues'

formula [34] can be used to calculate the rotational exponential matrix and nd the angular velocity. Finally, A, b+ _{and b}− _{can be used for collision avoidance as described in [7].}

4.4 Human Awareness Constraint

4.4.1 Denition of Human Gaze and Distance to End-Eector

In order to evaluate the human visibility of the robot task, we dene the human awareness criterion as the distance between the end-eector and human gaze. Our objective is to minimize that distance in order to make the robot arm remains in the human eld of view as much as possible and increase the human comfort.

Assuming that the gaze is dened by two points such as (G2 − G1) is the human gaze

vector and G∗ _{is the projection of the end-eector position P}

e on the human gaze, as

shown on Fig. 4.2, it is dened by (details are in Appendix 4.8.1): G∗ = G1+

(Pe− G1)T · (G2− G1)

∥ G2− G1 ∥2

(G2− G1) (4.4)

The distance dh between the end-eector and human gaze is dened as follows:

dh(q) =∥ Pe− G∗ ∥= (Pe− G∗)T ·

Pe− G∗

∥ (Pe− G∗)∥

32 CHAPITRE 4. INTÉGRATION DE L'ATTENTION DE L'HUMAIN DANSL'ALGORITHME DE CINÉMATIQUE INVERSE Moreover, the gradient of dh (∇dh) can be computed analytically by the following formula

(details are in Appendix 4.8.2): (_∇dh)T = [ ∂dh ∂q1 · · · ∂dh ∂qn ] =nT · ( I3− (G2− G1)(G2− G1)T ∥ G2− G1 ∥2 ) Jp (4.6)

where I3 ∈ R3×3 is the identity matrix, and Jp ∈ R3×n consists of the rst three lines of

the Jacobian matrix of the robot J(q) (corresponding to the linear velocity). Pe

G2

G1

G∗

n

Figure 4.2 Geometrical parameters for distance calculation between human gaze and end-eector

4.4.2 Integration into the IK problem

We will introduce two approaches to integrate the human awareness into the QP prob-lem (4.1), which are the linear and quadratic approaches. Our objective is to compare the performance of both approaches through several scenarios in order to point out the advantages and disadvantages of each approach.

Linear Approach

By integrating the human awareness into the IK formulation (4.1), the following optimiza-tion problem is obtained:

minimize ˙ q,q 1 2q˙ T_{Q ˙q + β d} h(q) subject to J ˙q = ˙r ˆ˙ q− _{≤ ˙q ≤ ˆ˙q}+ (4.7) where β is a positive coecient.

4.4. HUMAN AWARENESS CONSTRAINT 33 As dh(q) is a nonlinear function in q, Eq (4.7) is a nonlinear optimization problem which

is hard to solve within a xed time and therefore the hard real time constraints might not be satised.

In order to transform Eq (4.7) into a QP problem, dh(qt) is rst linearized as follows:

d(qt) = dh(qt−1) + (∇dh)T∆q

= dh(qt−1) + T (∇dh)Tq˙

(4.8) where T is the control loop sampling time of the robot.

By using (4.8), Eq (4.7) can be transformed into the following equivalent QP problem: minimize ˙ q,δh 1 2q˙ T_{Q ˙q + β δ} h subject to J ˙q = ˙r ˆ˙ q− _{≤ ˙q ≤ ˆ˙q}+ T (_∇dh)T q˙≤ δh (4.9)

The above optimization problem can be transformed into a classical QP problem as follows: minimize Z∈Rn+1 1 2Z T QZ + GT Z subject to J Z = ˙r AZ ≤ 0 Z− ≤ Z ≤ Z+ (4.10) with Z = [ ˙ q δh ] , _{Q =} [ Q 0 0 0 ] , _{J =}[J 0 ] , _{G =} [ 0 β ] A =[T (_∇dh)T −1 ] , _Z+ =[ˆ˙q + δ_h+ ] , _Z− =[ˆ˙q − δ−_h ] δ_h+ and δ_h− are the bounds of the slack variable, for instance:

34 CHAPITRE 4. INTÉGRATION DE L'ATTENTION DE L'HUMAIN DANSL'ALGORITHME DE CINÉMATIQUE INVERSE Quadratic Approach

Eq (4.7) can also be transformed into a quadratic form as follows: minimize ˙ q,δh 1 2q˙ T_{Q ˙q +} 1 2β δ 2 h subject to J ˙q = ˙r ˆ˙ q− ≤ ˙q ≤ ˆ˙q+ dh+ T (∇dh)T q = δ˙ h (4.11)

Note that minimizing δ2

h is equivalent to minimizing dh, this is because dh ≥ 0.

In a similar way, the above optimization problem can be transformed into a classical QP problem as follows: minimize Z∈Rn+1 1 2Z T QZ subject to J Z = e Z− ≤ Z ≤ Z+ (4.12) with Z = [ ˙ q δh ] , _{Q =} [ Q 0 0 β ] , _{J =} [ J 0 T (_∇dh)T −1 ] e = [ ˙ r −dh ] , _Z+ =[ˆ˙q + δ+_h ] , _Z− =[ˆ˙q − 0 ]

4.4.3 Trajectory Relaxation

Since the goal of the human awareness constraint is to minimize the distance between the human gaze and the end-eector of the robot, the Cartesian trajectory should be therefore modied and the equality constraints in (4.9) and (4.11) cannot be fully satised. It is then necessary to allow the relaxation of the robot trajectory to be able to deviate and minimize that distance.